CS 4649/7649 Robot Intelligence: Planning...1 S. Joo ([email protected]) 9/30/2014 1 CS...
Transcript of CS 4649/7649 Robot Intelligence: Planning...1 S. Joo ([email protected]) 9/30/2014 1 CS...
![Page 1: CS 4649/7649 Robot Intelligence: Planning...1 S. Joo (sungmoon.joo@cc.gatech.edu) 9/30/2014 1 CS 4649/7649 Robot Intelligence: Planning Sungmoon Joo School of Interactive Computing](https://reader034.fdocuments.in/reader034/viewer/2022042515/5f56a8ea5e8971163b7c8884/html5/thumbnails/1.jpg)
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
![Page 2: CS 4649/7649 Robot Intelligence: Planning...1 S. Joo (sungmoon.joo@cc.gatech.edu) 9/30/2014 1 CS 4649/7649 Robot Intelligence: Planning Sungmoon Joo School of Interactive Computing](https://reader034.fdocuments.in/reader034/viewer/2022042515/5f56a8ea5e8971163b7c8884/html5/thumbnails/2.jpg)
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
![Page 3: CS 4649/7649 Robot Intelligence: Planning...1 S. Joo (sungmoon.joo@cc.gatech.edu) 9/30/2014 1 CS 4649/7649 Robot Intelligence: Planning Sungmoon Joo School of Interactive Computing](https://reader034.fdocuments.in/reader034/viewer/2022042515/5f56a8ea5e8971163b7c8884/html5/thumbnails/3.jpg)
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
![Page 4: CS 4649/7649 Robot Intelligence: Planning...1 S. Joo (sungmoon.joo@cc.gatech.edu) 9/30/2014 1 CS 4649/7649 Robot Intelligence: Planning Sungmoon Joo School of Interactive Computing](https://reader034.fdocuments.in/reader034/viewer/2022042515/5f56a8ea5e8971163b7c8884/html5/thumbnails/4.jpg)
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
![Page 5: CS 4649/7649 Robot Intelligence: Planning...1 S. Joo (sungmoon.joo@cc.gatech.edu) 9/30/2014 1 CS 4649/7649 Robot Intelligence: Planning Sungmoon Joo School of Interactive Computing](https://reader034.fdocuments.in/reader034/viewer/2022042515/5f56a8ea5e8971163b7c8884/html5/thumbnails/5.jpg)
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?
![Page 6: CS 4649/7649 Robot Intelligence: Planning...1 S. Joo (sungmoon.joo@cc.gatech.edu) 9/30/2014 1 CS 4649/7649 Robot Intelligence: Planning Sungmoon Joo School of Interactive Computing](https://reader034.fdocuments.in/reader034/viewer/2022042515/5f56a8ea5e8971163b7c8884/html5/thumbnails/6.jpg)
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
![Page 7: CS 4649/7649 Robot Intelligence: Planning...1 S. Joo (sungmoon.joo@cc.gatech.edu) 9/30/2014 1 CS 4649/7649 Robot Intelligence: Planning Sungmoon Joo School of Interactive Computing](https://reader034.fdocuments.in/reader034/viewer/2022042515/5f56a8ea5e8971163b7c8884/html5/thumbnails/7.jpg)
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
![Page 8: CS 4649/7649 Robot Intelligence: Planning...1 S. Joo (sungmoon.joo@cc.gatech.edu) 9/30/2014 1 CS 4649/7649 Robot Intelligence: Planning Sungmoon Joo School of Interactive Computing](https://reader034.fdocuments.in/reader034/viewer/2022042515/5f56a8ea5e8971163b7c8884/html5/thumbnails/8.jpg)
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 !!
![Page 9: CS 4649/7649 Robot Intelligence: Planning...1 S. Joo (sungmoon.joo@cc.gatech.edu) 9/30/2014 1 CS 4649/7649 Robot Intelligence: Planning Sungmoon Joo School of Interactive Computing](https://reader034.fdocuments.in/reader034/viewer/2022042515/5f56a8ea5e8971163b7c8884/html5/thumbnails/9.jpg)
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
![Page 10: CS 4649/7649 Robot Intelligence: Planning...1 S. Joo (sungmoon.joo@cc.gatech.edu) 9/30/2014 1 CS 4649/7649 Robot Intelligence: Planning Sungmoon Joo School of Interactive Computing](https://reader034.fdocuments.in/reader034/viewer/2022042515/5f56a8ea5e8971163b7c8884/html5/thumbnails/10.jpg)
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
![Page 11: CS 4649/7649 Robot Intelligence: Planning...1 S. Joo (sungmoon.joo@cc.gatech.edu) 9/30/2014 1 CS 4649/7649 Robot Intelligence: Planning Sungmoon Joo School of Interactive Computing](https://reader034.fdocuments.in/reader034/viewer/2022042515/5f56a8ea5e8971163b7c8884/html5/thumbnails/11.jpg)
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
![Page 12: CS 4649/7649 Robot Intelligence: Planning...1 S. Joo (sungmoon.joo@cc.gatech.edu) 9/30/2014 1 CS 4649/7649 Robot Intelligence: Planning Sungmoon Joo School of Interactive Computing](https://reader034.fdocuments.in/reader034/viewer/2022042515/5f56a8ea5e8971163b7c8884/html5/thumbnails/12.jpg)
12
9/30/2014S. Joo ([email protected]) 23
Bushfire
Bushfire Algorithm to Generate GVD
9/30/2014S. Joo ([email protected]) 24
Voronoi Diagrams: Analysis
?
![Page 13: CS 4649/7649 Robot Intelligence: Planning...1 S. Joo (sungmoon.joo@cc.gatech.edu) 9/30/2014 1 CS 4649/7649 Robot Intelligence: Planning Sungmoon Joo School of Interactive Computing](https://reader034.fdocuments.in/reader034/viewer/2022042515/5f56a8ea5e8971163b7c8884/html5/thumbnails/13.jpg)
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
![Page 14: CS 4649/7649 Robot Intelligence: Planning...1 S. Joo (sungmoon.joo@cc.gatech.edu) 9/30/2014 1 CS 4649/7649 Robot Intelligence: Planning Sungmoon Joo School of Interactive Computing](https://reader034.fdocuments.in/reader034/viewer/2022042515/5f56a8ea5e8971163b7c8884/html5/thumbnails/14.jpg)
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
![Page 15: CS 4649/7649 Robot Intelligence: Planning...1 S. Joo (sungmoon.joo@cc.gatech.edu) 9/30/2014 1 CS 4649/7649 Robot Intelligence: Planning Sungmoon Joo School of Interactive Computing](https://reader034.fdocuments.in/reader034/viewer/2022042515/5f56a8ea5e8971163b7c8884/html5/thumbnails/15.jpg)
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”
![Page 16: CS 4649/7649 Robot Intelligence: Planning...1 S. Joo (sungmoon.joo@cc.gatech.edu) 9/30/2014 1 CS 4649/7649 Robot Intelligence: Planning Sungmoon Joo School of Interactive Computing](https://reader034.fdocuments.in/reader034/viewer/2022042515/5f56a8ea5e8971163b7c8884/html5/thumbnails/16.jpg)
16
9/30/2014S. Joo ([email protected]) 31
Exact Cell Decomposition: Trapezoidal
9/30/2014S. Joo ([email protected]) 32
Exact Cell Decomposition: Trapezoidal
![Page 17: CS 4649/7649 Robot Intelligence: Planning...1 S. Joo (sungmoon.joo@cc.gatech.edu) 9/30/2014 1 CS 4649/7649 Robot Intelligence: Planning Sungmoon Joo School of Interactive Computing](https://reader034.fdocuments.in/reader034/viewer/2022042515/5f56a8ea5e8971163b7c8884/html5/thumbnails/17.jpg)
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
![Page 18: CS 4649/7649 Robot Intelligence: Planning...1 S. Joo (sungmoon.joo@cc.gatech.edu) 9/30/2014 1 CS 4649/7649 Robot Intelligence: Planning Sungmoon Joo School of Interactive Computing](https://reader034.fdocuments.in/reader034/viewer/2022042515/5f56a8ea5e8971163b7c8884/html5/thumbnails/18.jpg)
18
9/30/2014S. Joo ([email protected]) 35
Exact Cell Decomposition: Trapezoidal
9/30/2014S. Joo ([email protected]) 36
Exact Cell Decomposition: Trapezoidal
![Page 19: CS 4649/7649 Robot Intelligence: Planning...1 S. Joo (sungmoon.joo@cc.gatech.edu) 9/30/2014 1 CS 4649/7649 Robot Intelligence: Planning Sungmoon Joo School of Interactive Computing](https://reader034.fdocuments.in/reader034/viewer/2022042515/5f56a8ea5e8971163b7c8884/html5/thumbnails/19.jpg)
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)
![Page 20: CS 4649/7649 Robot Intelligence: Planning...1 S. Joo (sungmoon.joo@cc.gatech.edu) 9/30/2014 1 CS 4649/7649 Robot Intelligence: Planning Sungmoon Joo School of Interactive Computing](https://reader034.fdocuments.in/reader034/viewer/2022042515/5f56a8ea5e8971163b7c8884/html5/thumbnails/20.jpg)
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
![Page 21: CS 4649/7649 Robot Intelligence: Planning...1 S. Joo (sungmoon.joo@cc.gatech.edu) 9/30/2014 1 CS 4649/7649 Robot Intelligence: Planning Sungmoon Joo School of Interactive Computing](https://reader034.fdocuments.in/reader034/viewer/2022042515/5f56a8ea5e8971163b7c8884/html5/thumbnails/21.jpg)
21
9/30/2014S. Joo ([email protected]) 41
Better Approximate Cell Decomposition
9/30/2014S. Joo ([email protected]) 42
Better Approximate Cell Decomposition
![Page 22: CS 4649/7649 Robot Intelligence: Planning...1 S. Joo (sungmoon.joo@cc.gatech.edu) 9/30/2014 1 CS 4649/7649 Robot Intelligence: Planning Sungmoon Joo School of Interactive Computing](https://reader034.fdocuments.in/reader034/viewer/2022042515/5f56a8ea5e8971163b7c8884/html5/thumbnails/22.jpg)
22
9/30/2014S. Joo ([email protected]) 43
Remember BFS: Policy?
9/30/2014S. Joo ([email protected]) 44
Remember BFS: Policy?
![Page 23: CS 4649/7649 Robot Intelligence: Planning...1 S. Joo (sungmoon.joo@cc.gatech.edu) 9/30/2014 1 CS 4649/7649 Robot Intelligence: Planning Sungmoon Joo School of Interactive Computing](https://reader034.fdocuments.in/reader034/viewer/2022042515/5f56a8ea5e8971163b7c8884/html5/thumbnails/23.jpg)
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*)
![Page 24: CS 4649/7649 Robot Intelligence: Planning...1 S. Joo (sungmoon.joo@cc.gatech.edu) 9/30/2014 1 CS 4649/7649 Robot Intelligence: Planning Sungmoon Joo School of Interactive Computing](https://reader034.fdocuments.in/reader034/viewer/2022042515/5f56a8ea5e8971163b7c8884/html5/thumbnails/24.jpg)
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
![Page 25: CS 4649/7649 Robot Intelligence: Planning...1 S. Joo (sungmoon.joo@cc.gatech.edu) 9/30/2014 1 CS 4649/7649 Robot Intelligence: Planning Sungmoon Joo School of Interactive Computing](https://reader034.fdocuments.in/reader034/viewer/2022042515/5f56a8ea5e8971163b7c8884/html5/thumbnails/25.jpg)
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