cs 4649/7649 robot intelligence: planning...1 s. joo ([email protected]) 9/30/2014 1 cs...
TRANSCRIPT
1
9/30/2014S. Joo ([email protected]) 1
CS 4649/7649Robot Intelligence: Planning
Sungmoon Joo
School of Interactive Computing
College of Computing
Georgia Institute of Technology
Roadmap Approaches
*Slides based in part on Dr. Mike Stilman’s lecture slides
9/30/2014S. Joo ([email protected]) 2
Course Info.
• HW#1 due Oct 6th
- Wiki - Add your group info.
- Need a repo.?
- Late policy – No late HWs
2
9/30/2014S. Joo ([email protected]) 3
Navigation Planning
S
G
Assuming full knowledge, how does the robot ‘plan’ its path from S to G?
9/30/2014S. Joo ([email protected]) 4
Visibility Graph
• Assuming polygonal obstacles, it looks like the shortest path is a
sequence of straight lines joining the vertices of the obstacles
3
• Assuming polygonal obstacles, it looks like the shortest path is a
sequence of straight lines joining the vertices of the obstacles
• Visibility graph G = set of unblocked lines between vertices of the
obstacles + initial & goal configuration
= (V, E), V ={obstacle vertices} U {start, goal},
E = {edges (vi,vj) disjoint from obstacle interiors}
9/30/2014S. Joo ([email protected]) 5
Visibility Graph
9/30/2014S. Joo ([email protected]) 6
Visibility Graphs*
* “An algorithm for planning collision-free paths among polyhedral obstacles” 1979 T. Lozano-Perez & M. A. Wesley* “A mobile automaton: An application of artificial intelligence techniques” 1969 N.J Nilson
- Point robot
- Polygonal obstacles
- Build Visibility Graph
- Reduce VG
- Search
4
9/30/2014S. Joo ([email protected]) 7
Path Planning for Robots with Geometric Shapes
Figure from Dr. Seth Teller’s lecture slide
9/30/2014S. Joo ([email protected]) 8
Visibility Graph Analysis
5
9/30/2014S. Joo ([email protected]) 9
Roadmap Approach to Navigation Planning
• Assumption: Static environment
• General Idea:
- Avoid searching the entire space
- Pre-compute a (hopefully small) graph (i.e.
the roadmap) s.t. staying on the roads is
guaranteed to avoid the ‘obstacles’ (& to
take us to the goal)
- Search a path between qinit and qgoal on
the roadmap
9/30/2014S. Joo ([email protected]) 10
Roadmap Approach to Navigation Planning
• Visibility Graph is an example of the roadmap approach
• Assumptions/Limitations? Polygonal objects, Zero distance to obstacles
Do we really care about strict optimality with distance metric?
What about safety?
6
9/30/2014S. Joo ([email protected]) 11
Voronoi Diagrams
• Another type of ‘roadmap’
Voronoi diagram is a way of dividing space into a number of regions
9/30/2014S. Joo ([email protected]) 12
Voronoi Diagrams
• Voronoi Diagram = The set of line segments separating the regions
corresponding to different colors
7
9/30/2014S. Joo ([email protected]) 13
Voronoi Diagrams
• Voronoi Diagram = The set of line segments separating the regions
corresponding to different colors
Line segment: Points on the edge are equidistant from two data points
Vertices = Equidistant points from >2 data points
9/30/2014S. Joo ([email protected]) 14
Voronoi Diagrams
• Voronoi Diagram = The set of line segments separating the regions
corresponding to different colors
Complexity (in 2D plane) – O(n log n) in time, O(n) in space
8
9/30/2014S. Joo ([email protected]) 15
Generalized Voronoi Diagram (GVD)
9/30/2014S. Joo ([email protected]) 16
Generalized Voronoi Diagram (GVD)
The points on the edges are the furthest from the obstacles & workspace boundary !!
9
9/30/2014S. Joo ([email protected]) 17
Voronoi Diagram Navigation Planning
qinit
qgoal
q*init
q*goal
• Idea: Construct a path between qinit and qgoal by following edges on the Voronoi
diagramVoronoi diagram = Roadmap
Step1. Find the point q*init of the Voronoi diagram closest to qinit
Step2. Find the point q*goal of the Voronoi diagram closest to qgoal
Step3. Compute shortest path from q*init to q*goal on the Voronoi diagram
9/30/2014S. Joo ([email protected]) 18
Practical Bug Alternative using GVD
10
9/30/2014S. Joo ([email protected]) 19
Practical Bug Alternative using GVD
9/30/2014S. Joo ([email protected]) 20
Bushfire Algorithm to Generate GVD
Bushfire
11
9/30/2014S. Joo ([email protected]) 21
Bushfire
Bushfire Algorithm to Generate GVD
9/30/2014S. Joo ([email protected]) 22
Bushfire
Bushfire Algorithm to Generate GVD
12
9/30/2014S. Joo ([email protected]) 23
Bushfire
Bushfire Algorithm to Generate GVD
9/30/2014S. Joo ([email protected]) 24
Voronoi Diagrams: Analysis
?
13
9/30/2014S. Joo ([email protected]) 25
Voronoi Diagrams: Summary
9/30/2014S. Joo ([email protected]) 26
Voronoi Diagrams: Summary
• Difficult in higher dimensions or nonpolygonal worlds
• Use of Voronoi is not necessarily the best heuristic
(“stay away from obstacles”)
- Can be much too conservative
- Can be unstable:
Small changes in Cobstacle Large changes in GVD
14
9/30/2014S. Joo ([email protected]) 27
Summary
Roadmap Approach
• Static environment
• Avoid searching the entire space
- Pre-compute a graph (i.e. roadmap) Search space reduction
Roadmap approach for Navigation Planning
- Visibility Graph Short path
- Voronoi Diagram Safe/Conservative path
9/30/2014S. Joo ([email protected]) 28
Other Options
15
9/30/2014S. Joo ([email protected]) 29
Exact Cell Decomposition: Convex Polygons
9/30/2014S. Joo ([email protected]) 30
Exact Cell Decomposition: Convex Polygons
“The graph of midpoints of edges between adjacent cells defines a roadmap”
16
9/30/2014S. Joo ([email protected]) 31
Exact Cell Decomposition: Trapezoidal
9/30/2014S. Joo ([email protected]) 32
Exact Cell Decomposition: Trapezoidal
17
9/30/2014S. Joo ([email protected]) 33
Exact Cell Decomposition: Trapezoidal
Critical events: Create new cell, Split cell, Merge cells
9/30/2014S. Joo ([email protected]) 34
Exact Cell Decomposition: Trapezoidal
18
9/30/2014S. Joo ([email protected]) 35
Exact Cell Decomposition: Trapezoidal
9/30/2014S. Joo ([email protected]) 36
Exact Cell Decomposition: Trapezoidal
19
9/30/2014S. Joo ([email protected]) 37
Exact Cell Decomposition: Trapezoidal
9/30/2014S. Joo ([email protected]) 38
Exact Cell Decomposition: Trapezoidal
Complexity (in 2D plane)- Time: O(n log n)- Space: O(n)
20
9/30/2014S. Joo ([email protected]) 39
Exact Cell Decomposition: Analysis
*Expensive and difficult to implement in higher dimensions
*
?
9/30/2014S. Joo ([email protected]) 40
Approximate Cell Decomposition
Use grid
21
9/30/2014S. Joo ([email protected]) 41
Better Approximate Cell Decomposition
9/30/2014S. Joo ([email protected]) 42
Better Approximate Cell Decomposition
22
9/30/2014S. Joo ([email protected]) 43
Remember BFS: Policy?
9/30/2014S. Joo ([email protected]) 44
Remember BFS: Policy?
23
9/30/2014S. Joo ([email protected]) 45
Dijkstra & A*
9/30/2014S. Joo ([email protected]) 46
Dijkstra & A*
A* - Cost = Optimal Cost-to-Come + Heuristic of Cost-to-Go
Dijkstra - Cost = Optimal Cost-to-Come
• If a heuristic of ‘Cost-to-Go’ becomes closer to the true optimal cost-to-go, fewer vertices tend to be explored in comparison with Dijkstra’s algorithm
• Dijkstra = A* with Zero-Cost-to-Go (degenerate case of A*)
24
9/30/2014S. Joo ([email protected]) 47
Policy
State Action
We know what to do at each cell Policy!
9/30/2014S. Joo ([email protected]) 48
Why is this useful - Replanning
25
9/30/2014S. Joo ([email protected]) 49
D* / D* Lite
9/30/2014S. Joo ([email protected]) 50
D* / D* Lite Concept
- Maintains cost-to-go values
- Propagate backward from the goal Backward Dijkstra
- Useful for navigation in an unknown environment