CMU-Q 15-381 Lecture 4: Path Planning Teacher: Gianni A. Di Caro
A PPLICATION : M OTION P LANNING § 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
M OTION P LANNING E XAMPLES Kia car factory 3
P ATH P LANNING → S EARCH P ROBLEM § 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 ?) The graph can be huge depending on discretization Roadmap graphs 4
P ATH P LANNING E XAMPLES ( WITH A* AND RELATIVES ) Baldur’s gate Mario AI Competition https://www.youtube.com/watch?v=DlkMs4ZHHr8 5
C ELL D ECOMPOSITION § 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
C OMBINATORIAL EXPLOSION § How many cells (vertices, edges) do we need? § Mobile planar robot moving in an area of 100 𝑛 % § 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
I TERATIVE C ELL D ECOMPOSITION § 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
I S 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
C ELL SHAPES AND P ATH EXECUTION § Navigation inside a cell can be done in many different ways § Cells can have different shapes Small area / Large perimeter Reduce distance distortion § Cells’ centers can be replaced by edges or vertices More flexibility for local motion 10
C ELL SHAPES AND P ATH EXECUTION § Meshes can be used instead of uniform cells 11
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
A* WITH TILES AND CENTERS : I MPLEMENTATION Node data structure § Node id § (𝑦, 𝑧) coordinates on the grid § -cost § ℎ -cost = Obstacle-free distance Moves and on the graph step costs § Cost 𝑔 = + ℎ § Parent node id § Free / Obstruction node Goal § Explored / Frontier / Unseen Current (cur) = 20, ℎ (cur) = 48 Obstruction 𝑔 (cur) = 68 Note: The graph is given Start 13
A* WITH TILES AND CENTERS : I MPLEMENTATION 14
A* WITH TILES AND CENTERS : I MPLEMENTATION Select node with least 𝑔 15
A* WITH TILES AND CENTERS : I MPLEMENTATION Select node with least 𝑔 Resolve ties considering least ℎ 16
A* WITH TILES AND CENTERS : I MPLEMENTATION Keep going … 17
A* WITH TILES AND CENTERS : I MPLEMENTATION 𝑔 increases along any expanded path from start to goal No node with 𝑔 > 𝑔 ∗ is expanded A number of nodes with 𝑔 = 𝑔 ∗ have been expanded 18
W AVE 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
S OLUTION 1 TO NON OPTIMALITY : P OST SMOOTHING Allow connections to further states than neighbors on the graph! Key observation: o If 𝑦 * ,… 𝑦 7 , … 𝑦 8 … 𝑦 9 is a valid path o And 𝑦 8 is visible from 𝑦 7 (in line-of-sight) o Then 𝑦 * ,… ,𝑦 7 , 𝑦 8 , … , 𝑦 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
S MOOTHING 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
S MOOTHING 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
S OLUTION 2: T HETA * § 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: (𝑧) = (𝑦) + 𝑑(𝑦,𝑧) o Insert 𝑧 in frontier with cost estimate, 𝑧 is a successor of its parent 𝑔 𝑧 = (𝑦) + 𝑑(𝑦, 𝑧) + ℎ(𝑧) § Theta* o If parent (𝑦) is visible from 𝑧 , insert 𝑧 with cost estimate 𝑔(𝑧) = ( parent (𝑦)) + 𝑑( parent (𝑦),𝑧) + ℎ(𝑧) o parent( 𝑦 ) becomes the parent of 𝑧 , allowing the two-step stretching to iterate, if possible parent (𝑦) 𝑦 𝑧 23
T HETA * 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
T HETA * WORKS ! Theta* path, likely J Shortest path 25
T HETA * WORKS ! http://aigamedev.com/open/tutorials/theta-star-any-angle-paths/ http://www.jair.org/media/2994/live-2994-5259-jair.pdf 26
T HE 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
P ROOF OF THEOREM § Suppose for contradiction that shortest path is not polygonal § Obstacles are polygonal ⇒ ∃ point 𝑞 in interior of 𝑞 free space such that “(shortest) path through 𝑞 is curved” § 𝑞 in free space ⇒ ∃ disc of free space around 𝑞 § Path through disc can be shortened by connecting points of entry and exit § à Path it’s polygonal! (also true in free space) 28
P ROOF 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) § Vertex cannot lie on an edge, otherwise we can do the same trick ü Inner vertices are vertices of obstacles ∎ 𝑞 29
A* FOR THE O PTIMAL P ATH ? 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
V ISIBILITY G RAPHS Sweeping, O(n 2 ) complexity 31
S UMMARY § Terminology: 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
Recommend
More recommend
