CMU-Q 15-381 Lecture 4: Path Planning Teacher: Gianni A. Di Caro - - PowerPoint PPT Presentation

CMU-Q 15-381

Lecture 4: Path Planning

Teacher: Gianni A. Di Caro

APPLICATION: MOTION PLANNING

2

§ 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

• Respecting constraints (e.g., avoiding obstacles, physical

limitations in rotations and translations)

• Optimizing metrics (length, energy, time, smoothness, …)

§ Trajectory planning: pp + time parameter (→ velocity profile) goal start

MOTION PLANNING EXAMPLES

3

Kia car factory

PATH PLANNING → SEARCH PROBLEM

4

§ 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

Roadmap graphs We have the entire graph

• ver the planning horizon

(A* vs. Dijkstra ?) The graph can be huge depending on discretization

PATH PLANNING EXAMPLES (WITH A* AND RELATIVES)

5

Baldur’s gate Mario AI Competition

https://www.youtube.com/watch?v=DlkMs4ZHHr8

CELL DECOMPOSITION

6

§ 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 shortest path through cell centers is the optimal path on the graph! The way we perform decomposition matters for completeness!

COMBINATORIAL EXPLOSION

7

§ 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

ITERATIVE CELL DECOMPOSITION

8

§ Distinguish between

• Cells that are fully contained in obstacles
• 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

IS IT COMPLETE NOW?

9

§ 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

CELL SHAPES AND PATH EXECUTION

10

Reduce distance distortion Small area / Large perimeter

§ Cells can have different shapes

§ Cells’ centers can be replaced by edges or vertices

More flexibility for local motion

§ Navigation inside a cell can be done in many different ways

CELL SHAPES AND PATH EXECUTION

11

§ Meshes can be used instead of uniform cells

A* WITH TILES AND CENTERS

12

Shortest paths through cell centers Shortest path

A* is complete and optimal: it will find the shortest path, on the road map graph, moving between neighbor nodes

A shortest path on the road map graph is not equivalent to a shortest path in the continuous environment!

A* WITH TILES AND CENTERS: IMPLEMENTATION

13

Moves and step costs § Node id § (𝑦, 𝑧) coordinates on the grid § 𝑕-cost § ℎ-cost = Obstacle-free distance

• n the graph

§ Cost 𝑔 = 𝑕 + ℎ § Parent node id § Free / Obstruction node § Explored / Frontier / Unseen Node data structure Start Goal Current Obstruction 𝑕(cur) = 20, ℎ(cur) = 48 𝑔(cur) = 68 Note: The graph is given

A* WITH TILES AND CENTERS: IMPLEMENTATION

14

A* WITH TILES AND CENTERS: IMPLEMENTATION

15

Select node with least 𝑔

A* WITH TILES AND CENTERS: IMPLEMENTATION

16

Select node with least 𝑔 Resolve ties considering least ℎ

A* WITH TILES AND CENTERS: IMPLEMENTATION

17

Keep going …

A* WITH TILES AND CENTERS: IMPLEMENTATION

18

𝑔 increases along any expanded path from start to goal No node with 𝑔 > 𝑔 ∗ is expanded A number of nodes with 𝑔 = 𝑔 ∗ have been expanded

WAVE FRONT EXPANSION

19

§ 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!

SOLUTION 1 TO NON OPTIMALITY: POST SMOOTHING

20

Key observation:

• If 𝑦*,… 𝑦7, … 𝑦8 … 𝑦9 is a valid path
• And 𝑦8 is visible from 𝑦7 (in line-of-sight)
• Then 𝑦*,… ,𝑦7, 𝑦8, … , 𝑦9 is a valid path being contained in the free

space (in the real world)

• We can then iterate on this new path (i.e., previous nodes are out)
• It’s a post-processing of the initially obtained path

Allow connections to further states than neighbors on the graph!

SMOOTHING WORKS!

21

A shortest path through cell centers Shortest path

What is left are only the navigation points that go around the corners of obstacles (not in line-of-sight)

Smoothed shortest path

SMOOTHING DOESN’T WORK!

22

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

SOLUTION 2: THETA*

23

§ 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*

• Cost-to-come: 𝑕(𝑧) = 𝑕(𝑦) + 𝑑(𝑦,𝑧)
• Insert 𝑧 in frontier with cost estimate, 𝑧 is a successor of its parent

𝑔 𝑧 = 𝑕(𝑦) + 𝑑(𝑦, 𝑧) + ℎ(𝑧) § Theta*

• If parent(𝑦) is visible from 𝑧, insert 𝑧 with cost estimate

𝑔(𝑧) = 𝑕(parent(𝑦)) + 𝑑(parent(𝑦),𝑧) + ℎ(𝑧)

• parent(𝑦) becomes the parent of 𝑧, allowing the two-step stretching

to iterate, if possible

parent(𝑦) 𝑦 𝑧

THETA* TRACES

24

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 goal

THETA* WORKS!

25

Theta* path, likely J Shortest path

THETA* WORKS!

26

http://www.jair.org/media/2994/live-2994-5259-jair.pdf

http://aigamedev.com/open/tutorials/theta-star-any-angle-paths/

THE OPTIMAL (SHORTEST) PATH?

27

§ Polygonal path: sequence of connected straight lines § Inner vertex of polygonal path: vertex that is not beginning or end § Theorem: Assuming polygonal

• bstacles, a shortest path is a

polygonal path whose inner vertices are vertices of obstacles

PROOF OF THEOREM

28

§ 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)

𝑞

PROOF OF THEOREM

29

ü 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 ∎

𝑞 𝑞

A* FOR THE OPTIMAL PATH?

30

Can the theorem help us to define a roadmap graph

• n which A* is optimal?

Need to include also start and end nodes 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

VISIBILITY GRAPHS

31

Sweeping, O(n2) complexity

SUMMARY

32

§ Terminology:

• Informed Search problems
• Algorithms: tree search / graph search, best-first search,

uniform cost search, greedy, A*, Theta*

• Admissible and consistent heuristics
• A* for path planning on roadmap graphs

§ Big ideas:

• Properties of the heuristic ⇒ A* optimality
• Don’t be too pessimistic!
• Be consistent!
• Automatic pruning
• Monotonicity
• Beyond neighborhood relations for finding shortcuts