CS 4649/7649 Robot Intelligence: Planning

Transcription

1 CS 4649/7649 Robot Intelligence: Planning Roadmap Approaches Sungmoon Joo School of Interactive Computing College of Computing Georgia Institute of Technology S. Joo 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

2 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

3 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

4 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

5 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

6 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

7 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

8 Generalized Voronoi Diagram (GVD) S. Joo 15 Generalized Voronoi Diagram (GVD) The points on the edges are the furthest from the obstacles & workspace boundary!! S. Joo 16 8

9 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

10 Practical Bug Alternative using GVD S. Joo 19 Bushfire Algorithm to Generate GVD Bushfire S. Joo 20 10

11 Bushfire Algorithm to Generate GVD Bushfire S. Joo 21 Bushfire Algorithm to Generate GVD Bushfire S. Joo 22 11

12 Bushfire Algorithm to Generate GVD Bushfire S. Joo 23 Voronoi Diagrams: Analysis? S. Joo 24 12

13 Voronoi Diagrams: Summary S. Joo 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

14 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

15 Exact Cell Decomposition: Convex Polygons S. Joo 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

16 Exact Cell Decomposition: Trapezoidal S. Joo 31 Exact Cell Decomposition: Trapezoidal S. Joo 32 16

17 Exact Cell Decomposition: Trapezoidal Critical events: Create new cell, Split cell, Merge cells S. Joo 33 Exact Cell Decomposition: Trapezoidal S. Joo 34 17

18 Exact Cell Decomposition: Trapezoidal S. Joo 35 Exact Cell Decomposition: Trapezoidal S. Joo 36 18

19 Exact Cell Decomposition: Trapezoidal S. Joo 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

20 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

21 Better Approximate Cell Decomposition S. Joo 41 Better Approximate Cell Decomposition S. Joo 42 21

22 Remember BFS: Policy? S. Joo 43 Remember BFS: Policy? S. Joo 44 22

23 Dijkstra & A* S. Joo 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

24 Policy State Action We know what to do at each cell Policy! S. Joo 47 Why is this useful - Replanning S. Joo (sungmoon.joo@cc.gatech.edu) 48 24

25 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