CS 4649/7649 Robot Intelligence: Planning Roadmap Approaches Sungmoon Joo School of Interactive Computing College of Computing Georgia Institute of Technology S. Joo (sungmoon.joo@cc.gatech.edu) 1 *Slides based in part on Dr. Mike Stilman s lecture slides Course Info. HW#1 due Oct 6th - Wiki - Add your group info. - Need a repo.? - Late policy No late HWs S. Joo (sungmoon.joo@cc.gatech.edu) 2 1
Navigation Planning Assuming full knowledge, how does the robot plan its path from S to G? S G S. Joo (sungmoon.joo@cc.gatech.edu) 3 Visibility Graph Assuming polygonal obstacles, it looks like the shortest path is a sequence of straight lines joining the vertices of the obstacles S. Joo (sungmoon.joo@cc.gatech.edu) 4 2
Visibility Graph 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 (v i,v j ) disjoint from obstacle interiors} S. Joo (sungmoon.joo@cc.gatech.edu) 5 Visibility Graphs* - Point robot - Polygonal obstacles - Build Visibility Graph - Reduce VG - Search * 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 S. Joo (sungmoon.joo@cc.gatech.edu) 6 3
Path Planning for Robots with Geometric Shapes Figure from Dr. Seth Teller s lecture slide S. Joo (sungmoon.joo@cc.gatech.edu) 7 Visibility Graph Analysis S. Joo (sungmoon.joo@cc.gatech.edu) 8 4
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 q init and q goal on the roadmap S. Joo (sungmoon.joo@cc.gatech.edu) 9 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? S. Joo (sungmoon.joo@cc.gatech.edu) 10 5
Voronoi Diagrams Another type of roadmap Voronoi diagram is a way of dividing space into a number of regions S. Joo (sungmoon.joo@cc.gatech.edu) 11 Voronoi Diagrams Voronoi Diagram = The set of line segments separating the regions corresponding to different colors S. Joo (sungmoon.joo@cc.gatech.edu) 12 6
Voronoi Diagrams Line segment: Points on the edge are equidistant from two data points Vertices = Equidistant points from >2 data points Voronoi Diagram = The set of line segments separating the regions corresponding to different colors S. Joo (sungmoon.joo@cc.gatech.edu) 13 Voronoi Diagrams Complexity (in 2D plane) O(n log n) in time, O(n) in space Voronoi Diagram = The set of line segments separating the regions corresponding to different colors S. Joo (sungmoon.joo@cc.gatech.edu) 14 7
Generalized Voronoi Diagram (GVD) S. Joo (sungmoon.joo@cc.gatech.edu) 15 Generalized Voronoi Diagram (GVD) The points on the edges are the furthest from the obstacles & workspace boundary!! S. Joo (sungmoon.joo@cc.gatech.edu) 16 8
Voronoi Diagram Navigation Planning Idea: Construct a path between q init and q goal by following edges on the Voronoi diagram Voronoi diagram = Roadmap q* init q init q* goal q goal Step1. Find the point q* init of the Voronoi diagram closest to q init Step2. Find the point q* goal of the Voronoi diagram closest to q goal Step3. Compute shortest path from q* init to q* goal on the Voronoi diagram S. Joo (sungmoon.joo@cc.gatech.edu) 17 Practical Bug Alternative using GVD S. Joo (sungmoon.joo@cc.gatech.edu) 18 9
Practical Bug Alternative using GVD S. Joo (sungmoon.joo@cc.gatech.edu) 19 Bushfire Algorithm to Generate GVD Bushfire S. Joo (sungmoon.joo@cc.gatech.edu) 20 10
Bushfire Algorithm to Generate GVD Bushfire S. Joo (sungmoon.joo@cc.gatech.edu) 21 Bushfire Algorithm to Generate GVD Bushfire S. Joo (sungmoon.joo@cc.gatech.edu) 22 11
Bushfire Algorithm to Generate GVD Bushfire S. Joo (sungmoon.joo@cc.gatech.edu) 23 Voronoi Diagrams: Analysis? S. Joo (sungmoon.joo@cc.gatech.edu) 24 12
Voronoi Diagrams: Summary S. Joo (sungmoon.joo@cc.gatech.edu) 25 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 C obstacle Large changes in GVD S. Joo (sungmoon.joo@cc.gatech.edu) 26 13
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 S. Joo (sungmoon.joo@cc.gatech.edu) 27 Other Options S. Joo (sungmoon.joo@cc.gatech.edu) 28 14
Exact Cell Decomposition: Convex Polygons S. Joo (sungmoon.joo@cc.gatech.edu) 29 Exact Cell Decomposition: Convex Polygons The graph of midpoints of edges between adjacent cells defines a roadmap S. Joo (sungmoon.joo@cc.gatech.edu) 30 15
Exact Cell Decomposition: Trapezoidal S. Joo (sungmoon.joo@cc.gatech.edu) 31 Exact Cell Decomposition: Trapezoidal S. Joo (sungmoon.joo@cc.gatech.edu) 32 16
Exact Cell Decomposition: Trapezoidal Critical events: Create new cell, Split cell, Merge cells S. Joo (sungmoon.joo@cc.gatech.edu) 33 Exact Cell Decomposition: Trapezoidal S. Joo (sungmoon.joo@cc.gatech.edu) 34 17
Exact Cell Decomposition: Trapezoidal S. Joo (sungmoon.joo@cc.gatech.edu) 35 Exact Cell Decomposition: Trapezoidal S. Joo (sungmoon.joo@cc.gatech.edu) 36 18
Exact Cell Decomposition: Trapezoidal S. Joo (sungmoon.joo@cc.gatech.edu) 37 Exact Cell Decomposition: Trapezoidal Complexity (in 2D plane) - Time: O(n log n) - Space: O(n) S. Joo (sungmoon.joo@cc.gatech.edu) 38 19
Exact Cell Decomposition: Analysis? * *Expensive and difficult to implement in higher dimensions S. Joo (sungmoon.joo@cc.gatech.edu) 39 Approximate Cell Decomposition Use grid S. Joo (sungmoon.joo@cc.gatech.edu) 40 20
Better Approximate Cell Decomposition S. Joo (sungmoon.joo@cc.gatech.edu) 41 Better Approximate Cell Decomposition S. Joo (sungmoon.joo@cc.gatech.edu) 42 21
Remember BFS: Policy? S. Joo (sungmoon.joo@cc.gatech.edu) 43 Remember BFS: Policy? S. Joo (sungmoon.joo@cc.gatech.edu) 44 22
Dijkstra & A* S. Joo (sungmoon.joo@cc.gatech.edu) 45 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*) S. Joo (sungmoon.joo@cc.gatech.edu) 46 23
Policy State Action We know what to do at each cell Policy! S. Joo (sungmoon.joo@cc.gatech.edu) 47 Why is this useful - Replanning S. Joo (sungmoon.joo@cc.gatech.edu) 48 24
D* / D* Lite S. Joo (sungmoon.joo@cc.gatech.edu) 49 D* / D* Lite Concept - Maintains cost-to-go values - Propagate backward from the goal Backward Dijkstra - Useful for navigation in an unknown environment S. Joo (sungmoon.joo@cc.gatech.edu) 50 25