Metric Planning: Overview

Transcription

1 Also called quantitative planning Tries to find optimal path to goal Metric Planning: Overview As opposed to some path as per qualitative approach Usually plans path as series of subgoals (waypoints) Optimal/best implies there exists a metric of comparison Optimal path algorithms computationally expensive Want to make as efficient as possible This approach requires 2 components: 1. Representation Data structure to represent world Must be usable by planning algorithm Should only represent what s necessary for planning 2. Algorithm 2 general types: (a) Ones that treat planning as graph problem (b) Ones that treat planning as a color-fill algorithm Hybrid model must address problem of interleaving deliberation and reaction 1

2 Metric Planning: CSpace Intro Configuration Space (CSpace): Data structure that represents world Usd to situate robot and obstacles in world Degrees of freedom: parameters needed to situate robot in space Need 6 in 3D: 1. 3 Cartesian coordinates (x, y, z) 2. 3 Euler angles (φ, θ, γ: pitch, yaw, roll) Want to use as few as possible to minimize storage 2 (x, y) often sufficient Holonomic robot: one that can turn in place Can be considered round Orientation not a factor in this case Simplifies DOF There are a number of CSpace representations: 1. Meadow map (hybrid vertex-graph free-space model) 2. Generalized Voronoi graph (GVG) 3. Regular grid 4. Quadtree (Octree) Each represents free (unoccupied) space differently 2

3 Metric Planning: CSpace Representations - Meadow Maps Generates CSpace from existing detailed map Expand boundaries of obstacles by radius of robot Can then treat robot as a point without need to worry about collisions Based on convex polygons Can travel from any point on perimeter to any other point on perimeter of convex poly without ever leaving poly If poly encloses free space, robot traveling such a path will have no collisions 3 general steps: 1. Make basic unit/ pixel size = size of robot 3

4 Metric Planning: CSpace Representations - Meadow Maps (2) 2. Create polygons Connect interesting features (corners, door frames,...) Identify useful polygons for partition 3. Create path Need to id points on perimeters of polys that connect polys Collection of these points and edges between result in a graph of free space One possibility: Choose midpoint of perimeter edges that are shared between 2 polys Given a start node and goal node, search graph for path Nodes on path become waypoints (subgoals) 4

5 Metric Planning: CSpace Representations - Meadow Maps (3) Problems: 1. Path is jagged Use path relaxation to alleviate 2. Polygon id computationally complex 3. Path not based on features that can be sensed, but on map artifacts i.e., robot must use self-localization 4. Hard to update CSpace if original map inaccurate 5

6 Metric Planning: CSpace Representations - Generalized Voronoi Graph (GVG) Path follows lines that are equidistant from all obstacles Such lines called Voronoi edges Points of convergence called Voronoi vertices Often correspond to features that can be sensed in environment Collection of Voronoi edges and vertices create a graph of free space By following edges, robot guaranteed not to collide with obstacle Can be created as robot navigates - do not necessarily need a priori map 6

7 Metric Planning: CSpace Representations - Regular (Occupancy) Grid World divided into regular rectangular cells If any part of cell occupied, entire cell is considered occupied Consider center of each cell a node Collection of nodes and edges between create a graph of free space 4-connected cell links to neighbors thru sides 8-connected cell links to neighbors thru sides and vertices Problems: 1. Digitation bias: Cell is considered filled if only a small fraction is actually occupied Results in wasted space Solution: Make cells small 2. Small cells require more storage 7

8 Metric Planning: CSpace Representations - Quadtree (Octree) Developed to solve problems of regular grid approach Grid uses large cells If grid partially occupied, subdivide grid into 4 equal subcells Recursively apply subdivision to partially filled subcells Centers of free cells represented as graph nodes Adjacent free nodes connected by graph edges Minimizes wasted free space, maximizes storage efficiency 8

9 Metric Planning: A Algorithm Given a graph, want to find optimal path from start to goal node Do not want algorithm to visit every node in graph: computationally expensive One solution is to use branch-and-bound algorithms: Those that only consider a path that looks promising When path becomes less promising (more expensive) than others, temporarily abandon it and pursue the more attractive ones A algorithm Basis of A Searches a state space for an optimal path State space is entire graph: all nodes and edges between them From a given node, need to decide which leaving edge is best one to take Decision based on following equation: f(n) = g(n) + h(n), where g(n) is cost of getting from start state to node n h(n) is cost of getting from node n to goal state f(n) is total cost of going from start to goal state via node n Note that h(n) and g(n) are known values Hence, selecting the node n with minimal f(n) as next node to visit from start to goal node, will generate optimal (minimal cost) path Problem: Since assumes h(n) known, can only have been determined by visiting all nodes in the graph, which is what want to avoid 9

10 Metric Planning: A Algorithm Same basic algorithm as A, but uses different evaluation function: f (n) = g (n) + h (n), where * means value is estimated g (n) g(n) since always know actual cost of getting from start to node n h (n) is heuristic function - rule of thumb for estimating cost to goal f(n) is total cost of going from start to goal state via node n Since f (n) is an estimate, how can optimal path be guaranteed? If h (n) h(n), path guaranteed optimal Any algorithm for which this relation is true is called admissible (hence algorithm A) 10

11 Metric Planning: A Algorithm (2) Example: Edges are labeled with the costs of moving between nodes. The following table lists the h values for each node. Node h A 15 B 13 C 9 D 15 E 13 F 12 G 1 H 7 I 9 J 0 Initial state is A Goal state is J 11

12 Partial plan generation: 1. Queue: A(15) Pop A f(b) = = 16 f(c) = = 18 f(d) = = Queue: B(16)D(17)C(18) Pop B f(e) = = 24 f(f ) = = Queue: D(17)C(18)F(19)E(24) Pop D f(h) = = 13 f(i) = = 18 Metric Planning: A Algorithm (3) 4. Queue: H(13)D(17)C(18)I(18)F(19)E(24) Pop H f(c) = = 16 Found a shorter path to C - update queue 5. Queue: C(16)D(17)I(18)F(19)E(24) Pop C f(g) = = Queue: G(16)D(17)I(18)F(19)E(24) etc. 12

13 Metric Planning: A Algorithm (4) For planning where optimal path is shortest, can let h (n) = Euclidean distance between node n and goal Note that since using a metric map, will know coordinates of all nodes Points of interest: Edges cannot have negative costs Algorithm must account for cycles New path to a node may be cheaper than one previously found Can include additional factors in path cost: terrain, presence of enemies, etc. 13

14 Metric Planning: Wavefront Approach Based on concept of heat propagation through solids (or CG color-fill algorithms) Create regular grid From start node, propagate outward Propagation rate dependent on nature of cell If free, conductivity = infinite If occupied, conductivity = 0 Can model types of terrain, etc. by assigning low values to those least desirable Path that reaches goal first is optimal 14

15 Metric Planning: Wavefront Approach (2) 15

16 Metric Planning: Wavefront Approach (3) 16

17 Metric Planning: Wavefront Approach (4) 17

18 Metric Planning: Wavefront Approach (5) 18

19 Metric Planning: Wavefront Approach (6) Trulla developed variant that uses this approach to generate potential field like vectors in each cell Side effect is optimal path to goal from any cell in world 19

20 Metric Planning: Interleaving Planning and Execution Hybrid planners tend to generate paths in terms of waypoints, which are passed to reactive component one-by-one 2 problems can arise during reactive execution of metric path: 1. Subgoal obsession (1) Result of goal-satisfaction requirements that are too strict Observed behavior is robot making lots of fine positioning adjustments at subgoal Problem worse for non-holonomic robots Wastes time and effort Can alleviate by easing satisfaction requirements e.g., use tolerance of 1 robot diameter 2. Subgoal obsession (2) Subgoal is unreachable Since in reactive mode, robot may not recognize is stuck Remedy: Impose time limit on subgoal satisfaction 3. Lack of opportunistic improvements Recognizing when current plan should be abandoned in favor of revised plan: Abandoning current subgoal in favor of subsequent one Replanning current path to subgoal 20

21 Metric Planning: Interleaving Planning and Execution (2) Solutions to subgoal obsession and opportunistic improvement 1. D algorithm (Stentz) Uses A to compute optimal path from every node in map 2. Trulla s wavefront algorithm Generates optimal path from every node as part of basic algorithm 3. In both cases, if robot diverted from optimal path, will know optimal path from current location if it can localize itself 4. Problems: (a) May cause excessive wandering or entrapment if many obstacles 5. Solutions to problems: (a) Continuous replanning When encounter unexpected obstacle, update map and adjust paths affected (D ) Problems: i. Computationally expensive ii. Dependent on sensor quality - may result in erratic paths (b) Event-driven replanning Event triggered by sensor readings causes replanning (Trulla) Problems: i. Cannot take advantage of favorable differences 21