CMU-Q Lecture 4: Path Planning. Teacher: Gianni A. Di Caro

Transcription

1 CMU-Q Lecture 4: Path Planning Teacher: Gianni A. Di Caro

2 APPLICATION: MOTION PLANNING Path planning: computing a continuous sequence ( a path ) of configurations (states) between an initial configuration (start) and a final configuration (goal) in a physical environment o Respecting constraints (e.g., avoiding obstacles, physical limitations in rotations and translations) o Optimizing metrics (length, energy, time, smoothness, ) start goal Trajectory planning: pp + time parameter ( velocity profile) 2

3 MOTION PLANNING EXAMPLES Kia car factory 3

4 PATH PLANNING SEARCH PROBLEM Let s consider an omnidirectional point agent Let s discretize the free world representing it into a graph Let s search for a (discrete) path in the graph Let the world be static Let the cost be the length of the path Let s forget about time and velocity Combinatorial planning We have the entire graph over the planning horizon (A* vs. Dijkstra?) Roadmap graphs The graph can be huge depending on discretization 4

5 PATH PLANNING EXAMPLES (WITH A* AND RELATIVES) Baldur s gate Mario AI Competition 5

6 CELL DECOMPOSITION The continuous world is covered by a discrete set of cells How to make the coverage? First guess: a grid Mark cells that intersect obstacles as blocked, free otherwise The motion through a cell happens through its center Each cell has n=8 neighbors Find path through centers of free cells Which are the nodes and the edges of the roadmap graph? Is cell decomposition Complete? Optimal? The way we perform decomposition matters for completeness! The shortest path through cell centers is the optimal path on the graph! 6

7 COMBINATORIAL EXPLOSION How many cells (vertices, edges) do we need? Mobile planar robot moving in an area of 100 m % We want cm-level precision: 100 & 100 % = 100 & 10 ( cells If the robot moves in the 3D space 100 & 100 ) = 1 & 10 *% cells! Articulated robots move in 3D spaces and have multiple dimensions (one for each joint) We need to find smart ways to define the grid, and, in general to trade some precision off 7

8 ITERATIVE CELL DECOMPOSITION Distinguish between o Cells that are fully contained in obstacles o Cells that intersect obstacles Iterate: If no path found, subdivide the cells intersecting obstacles Any n-tree decomposition can be used (e.g., quad- / oct-trees) High resolution (# cells) only where needed, save on states 8

9 IS IT COMPLETE NOW? A path planning algorithm is resolution complete when: a. If a path exists, it finds it in finite time b. If a path does not exist, it returns in finite time Cell decomposition satisfies: 1. a but not b 2. b but not a 3. Both a and b 4. Neither a nor b 9

10 CELL SHAPES AND PATH EXECUTION Navigation inside a cell can be done in many different ways Cells can have different shapes Reduce distance distortion Small area / Large perimeter Cells centers can be replaced by edges or vertices More flexibility for local motion 10

11 CELL SHAPES AND PATH EXECUTION Meshes can be used instead of uniform cells 11

12 A* WITH TILES AND CENTERS A* is complete and optimal: it will find the shortest path, on the road map graph, moving between neighbor nodes Shortest paths through cell centers Shortest path A shortest path on the road map graph is not equivalent to a shortest path in the continuous environment! 12

13 A* WITH TILES AND CENTERS: IMPLEMENTATION Node data structure Moves and step costs g(cur) = 20, h(cur) = 48 f(cur) = 68 Current Goal Obstruction Node id (x, y) coordinates on the grid g-cost h-cost = Obstacle-free distance on the graph Cost f = g + h Parent node id Free / Obstruction node Explored / Frontier / Unseen Note: The graph is given Start 13

14 A* WITH TILES AND CENTERS: IMPLEMENTATION 14

15 A* WITH TILES AND CENTERS: IMPLEMENTATION Select node with least f 15

16 A* WITH TILES AND CENTERS: IMPLEMENTATION Select node with least f Resolve ties considering least h 16

17 A* WITH TILES AND CENTERS: IMPLEMENTATION Keep going 17

18 A* WITH TILES AND CENTERS: IMPLEMENTATION f increases along any expanded path from start to goal No node with f > f is expanded A number of nodes with f = f have been expanded 18

19 WAVE FRONT EXPANSION This is one-query: from a specific start to a destination What if, we keep the destination fixed and try to be ready to answer queries from any starting state? IF we have enough time, we can compute the distance to the goal from any starting state All collision-free 4 neighbors are given a cost of 1 Iterative wave front construction Follow the downhill path! 19

20 SOLUTION 1 TO NON OPTIMALITY: POST SMOOTHING Allow connections to further states than neighbors on the graph! Key observation: o If x *, x 7, x 8 x 9 is a valid path o And x 8 is visible from x 7 (in line-of-sight) o Then x *,, x 7, x 8,, x 9 is a valid path being contained in the free space (in the real world) o We can then iterate on this new path (i.e., previous nodes are out) o It s a post-processing of the initially obtained path 20

21 SMOOTHING WORKS! A shortest path through cell centers Smoothed shortest path Shortest path What is left are only the navigation points that go around the corners of obstacles (not in line-of-sight) 21

22 SMOOTHING DOESN T WORK! A shortest path through cell centers Shortest path Even going through a different direction doesn t help to find the (real-world) optimal path! A* can have many turns, it doesn t look realistic 22

23 SOLUTION 2: THETA* Idea: Allow parents that are non-neighbors in the graph to be used during search, looking backward. In Post Smoothing this kind of process is done on the final path, in Theta* while the path is built Standard A* o Cost-to-come: g(y) = g(x) + c(x, y) o Insert y in frontier with cost estimate, y is a successor of its parent f y = g(x) + c(x, y) + h(y) Theta* o If parent(x) is visible from y, insert y with cost estimate f(y) = g(parent(x)) + c(parent(x), y) + h(y) o parent(x) becomes the parent of y, allowing the two-step stretching to iterate, if possible parent(x) y x 23

24 THETA* TRACES goal Each vertex is labeled with its g-value and an arrow pointing to its parent vertex. h, not shown, is computed as the Euclidean distance to the goal. The hollow circle indicates the vertex being expanded 24

25 THETA* WORKS! Theta* path, likely J Shortest path 25

26 THETA* WORKS!

27 THE OPTIMAL (SHORTEST) PATH? Polygonal path: sequence of connected straight lines Inner vertex of polygonal path: vertex that is not beginning or end Theorem: Assuming polygonal obstacles, a shortest path is a polygonal path whose inner vertices are vertices of obstacles 27

28 PROOF OF THEOREM Suppose for contradiction that shortest path is not polygonal Obstacles are polygonal point p in interior of free space such that (shortest) path through p is curved p p in free space disc of free space around p Path through disc can be shortened by connecting points of entry and exit à Path it s polygonal! (also true in free space) 28

29 PROOF OF THEOREM ü Path is polygonal Vertex cannot lie in interior of free space, otherwise we can do the same trick and shorten the path (that would not then be the shortest) p Vertex cannot lie on an edge, otherwise we can do the same trick ü Inner vertices are vertices of obstacles p 29

30 A* FOR THE OPTIMAL PATH? Can the theorem help us to define a roadmap graph on which A* is optimal? Vertices and edges of A*-optimal road map graph: 1. V = regular grid, vertices are the middle points 2. V = in the middle point of the polygons 3. V = vertices of the polygons 4. No graph guarantees optimality Need to include also start and end nodes 30

31 VISIBILITY GRAPHS Sweeping, O(n 2 ) complexity 31

32 Terminology: SUMMARY o Informed Search problems o Algorithms: tree search / graph search, best-first search, uniform cost search, greedy, A*, Theta* o Admissible and consistent heuristics o A* for path planning on roadmap graphs Big ideas: o Properties of the heuristic A* optimality o Don t be too pessimistic! o Be consistent! o Automatic pruning o Monotonicity o Beyond neighborhood relations for finding shortcuts 32