Road Map Methods. Including material from Howie Choset / G.D. Hager S. Leonard

Transcription

1 Road Map Methods Including material from Howie Choset

2 The Basic Idea Capture the connectivity of Q free by a graph or network of paths , Howie Choset, with significant copying from who loosely based his notes on notes by Nancy Amato

3 RoadMap Definition A roadmap, RM, is a set of trajectories (i.e. f ( t, q A, q B ) ) such that for all q start Q free and q goal Q free can be connected by a path: The three ingredients of a roadmap Accessibility: There is a path from q start Q free to some q RM Departability: There is a path from some q RM to q goal Q free Connectivity: there exists a path in RM between q and q , Howie Choset, with significant copying from who loosely based his notes on notes by Nancy Amato

4 1. Build the roadmap RoadMap Path Planning a) nodes are points in QQ free or its boundary b) two nodes are connected by an edge if there is a free path between them (i.e. f ( t, q A, q B )) 2. Connect q start and q goal points to the road map at point q and q, respectively 3. Find a path on the roadmap between q and q. The result is a path in Q free from start to goal , Howie Choset, with significant copying from who loosely based his notes on notes by Nancy Amato

5 Overview Deterministic methods Some need to represent Q free, and some don t. are complete are complexity-limited to simple (e.g. low-dimensional) problems example: Canny s Silhouette method (5.5) applies to general problems is singly exponential in dimension of the problem , Howie Choset, with significant copying from who loosely based his notes on notes by Nancy Amato

6 Visibility Graph methods Defined for polygonal obstacles Nodes correspond to vertices of obstacles Nodes are connected if they are already connected by an edge on an obstacle the line segment joining them is in free space Not only is there a path on this roadmap, but it is the shortest path If we include the start and goal nodes, they are automatically connected Algorithms for constructing them can be efficient O(n 3 ) brute force , Howie Choset, with significant copying from who loosely based his notes on notes by Nancy Amato

7 Visibility Graph in Action 1. Draw lines of sight from the start and goal to all visible vertices and corners of the world. goal start , Howie Choset, with significant copying from who loosely based his notes on notes by Nancy Amato

8 Visibility Graph in Action 2. Second, draw lines of sight from every vertex of every obstacle like before. Remember lines along edges are also lines of sight. goal start , Howie Choset, with significant copying from who loosely based his notes on notes by Nancy Amato

9 Visibility Graph in Action goal start , Howie Choset, with significant copying from who loosely based his notes on notes by Nancy Amato

10 The Visibility Graph in Action (Part 4) goal start , Howie Choset, with significant copying from who loosely based his notes on notes by Nancy Amato

11 Visibility Graph in Action Repeat until you re done. goal start , Howie Choset, with significant copying from who loosely based his notes on notes by Nancy Amato

12 Visibility Graphs Find a path in the graph. Bread first, depth first, Dijkstra s, etc. goal start , Howie Choset, with significant copying from who loosely based his notes on notes by Nancy Amato

13 The Sweepline Algorithm Analysis: For a vertex, n log n to create initial list, log n for each α i Overall: n log (n) (or n 2 log (n) for all n vertices

14 Algorithm: Initially: 1. calculate the angle α i of segment vvvv ii and sort vertices by this creating list εε 2. create a list S of edges that intersect the horizontal from v sorted by intersection distance For each α i if vv ii is visible to v then add vvvv ii to graph if vv ii is the beginning of an edge E, insert EE in S if vv ii is the end of and edge E, remove E from S , Howie Choset, with significant copying from who loosely based his notes on notes by Nancy Amato

15 Reduced Visibility Graphs The current graph as too many lines lines to concave vertices lines that head into the object A reduced visibility graph consists of Edges that are separating Edges that are supporting (i.e. do not head into the object at either endpoint) , Howie Choset, with significant copying from who loosely based his notes on notes by Nancy Amato

16 Robotic Motion Planning: Cell Decompositions (with some discussion on coverage)

17 Types of Decompositions Trapezoidal Decomposition Morse Cell Decomposition Boustrophedon decomposition Morse decomposition definition Sensor-based coverage Examples of Morse decomposition Voronoi Decomposition Visibility-based Decomposition

18 Adjacency Graph c 14 Node correspond to a cell Edge connects nodes of adjacent cells Two cells are adjacent if they share a common boundary c 1 c 4 c 2 c 3 c 6 c 5 c 8 c 7 c 10 c 11 c 13 c 9 c 12 c 15 c 7 c 14 c 4 c 5 c 15 c 2 c 8 c 11 c 1 c 10 c 3 c 6 c 9 c 12 c 13

19 Trapezoidal Decomposition Path Planning Path Planning in three steps: A trapezoid is convex. Any two points on its boundary can be connected by a straight line Planner determines cells that contain the start and goal Planner searches for a path within adjacency graph Find the exact path in the configuration space. Start Goal Thanks to H. Choset, N. Amato

20 Trapezoidal Decomposition Thanks to H. Choset, N. Amato

21 Trapezoidal Decomposition c 1 Thanks to H. Choset, N. Amato

22 Trapezoidal Decomposition c 2 c 1 Thanks to H. Choset, N. Amato

23 Trapezoidal Decomposition c 4 c 5 c 8 c 2 c 1 c 3 c 6 c 9 Thanks to H. Choset, N. Amato

24 Trapezoidal Decomposition Thanks to H. Choset, N. Amato

25 Trapezoidal Decomposition The robot crosses the boundary between adjacent cells at the midpoints Thanks to H. Choset, N. Amato

26 Implementation Input is vertices and edges Sort n vertices O(n log(n)) by x coordinates Determine vertical extensions For each vertex, intersect vertical line with each edge O(n) time Total O(n 2 ) time

27 Sweep Line Approach Sweep a line through the space stopping at vertices v i Maintain a list L of the current edges the slice intersects Determining the intersection of slice with L requires O(n) time but with an efficient data structure like a balanced tree, perhaps O(log n) Determine between which two edges the vertex v lies (e LOWER and e UPPER ) Maintaining L takes O(n log n) log n for insertions, n for vertices For each vertex v, let e lower and e upper be the two edges that contain v. Delete e upper and e lower Insert e upper and e lower (,e LOWER, e lower, e upper, e UPPER, )(,e LOWER, e UPPER, ) Delete e upper, insert e lower Insert e upper, delete e lower (,e LOWER, e lower, e UPPER, )(,e LOWER, e upper, e UPPER, (,e LOWER, e UPPER, ) (,e LOWER, e lower, e upper, e UPPER, )(,e LOWER, e upper, e UPPER, )(,e LOWER, e lower, e UPPER,

28 Thanks to H. Choset, N. Amato

29 Thanks to H. Choset, N. Amato

30 Thanks to H. Choset, N. Amato

31 Coverage Planner determines an exhaustive walk through the adjacency graph Planner computes explicit robot motions within each cell Problems 1. Polygonal representation 2. Quantization 3. Position uncertainty 4. Full information

32 Complete Coverage