Motion Planning, Part IV Graph Search Part II. Howie Choset

Transcription

1 Motion Planning, Part IV Graph Search Part II Howie Choset

2 Map-Based Approaches: Properties of a roadmap: Roadmap Theory Accessibility: there exists a collision-free path from the start to the road map Departability: there exists a collision-free path from the roadmap to the goal. Connectivity: there exists a collision-free path from the start to the goal (on the roadmap). a roadmap exists a path exists Examples of Roadmaps Generalized Voronoi Graph (GVG) Visibility Graph

3 The Visibility Graph in Action (Part 1) First, draw lines of sight from the start and goal to all visible vertices and corners of the world. start goal

4 The Visibility Graph in Action (Part 2) Second, draw lines of sight from every vertex of every obstacle like before. Remember lines along edges are also lines of sight. start goal

5 The Visibility Graph in Action (Part 3) Second, draw lines of sight from every vertex of every obstacle like before. Remember lines along edges are also lines of sight. start goal

6 The Visibility Graph in Action (Part 4) Second, draw lines of sight from every vertex of every obstacle like before. Remember lines along edges are also lines of sight. start goal

7 The Visibility Graph (Done) Repeat until you re done. goal start

8 Graph Search Howie Choset

9 Informed Search: A* Notation n node/state c(n 1,n 2 ) the length of an edge connecting between n 1 and n 2 b(n 1 ) = n 2 backpointer of a node n 1 to a node n 2. 9

10 Informed Search: A* Evaluation function, f(n) = g(n) + h(n) Operating cost function, g(n) Actual operating cost having been already traversed Heuristic function, h(n) Information used to find the promising node to traverse Admissible never overestimate the actual path cost Cost on a grid 10

11 Example (1/5) 0 Start 1 1 D A C B E F J L 3 K Legend h(x) c(x) G H I 2 Priority = g(x) + h(x) Note: g(x) = sum of all previous arc costs, c(x), from start to x GOAL Example: c(h) = 2 11

12 Example (2/5) 0 Start 1 1 D A C B E F J G H I GOAL L 3 K 2 2 First expand the start node B(3) If goal not found, A(4) expand the first node C(4) in the priority queue (in this case, B) H(3) Insert the newly expanded A(4) nodes into the priority que C(4) and continue until the goal I(5) found, or the priority queu G(7) empty (in which case no p Note: for exists) each expanded node, you also need a pointer to its respective parent. For example, nodes A, B and C point to Start 12

13 Example (3/5) 0 Start 1 1 D A C B E F J G H I L 3 K 2 B(3) A(4) C(4) H(3) A(4) C(4) I(5) G(7) No expansion E(3) GOAL(5) C(4) D(5) I(5) F(7) G(7) We ve found a path to the goal: Start => A => E => Goal (from the pointers) GOAL Are we done? 13

14 0 Start 1 1 D A C L B E F J K G H I GOAL B(3) A(4) C(4) Example (4/5) H(3) A(4) C(4) I(5) G(7) No expansion E(3) C(4) D(5) I(5) F(7) G(7) GOAL(5) There might be a shorter path, but assuming non-negative arc costs, nodes with a lower prio than the goal cannot yield a better path. In this example, nodes with a priority greater th equal to 5 can be pruned. Why don t we expand nodes with an equivalen (why not expand nodes D and I?) 14

15 1 1 D A C B E F J Start 1 If the priority queue still wasn t empty, we would continue expanding while throwing away nodes with priority lower than 4. (remember, lower numbers = higher priority) 2 G H I GOAL L 2 3 K 2 B(3) A(4) C(4) Example (5/5) H(3) A(4) C(4) I(5) G(7) No expansion E(3) C(4) D(5) I(5) F(7) G(7) GOAL(5) We can continue to throw away nodes with priority levels lower than the lowest goal found. As we can see from this example, there was a shorter path through node K. To find the path, simply follow the back pointers. Therefore the path would be: Start => C => K => Goal K(4) L(5) J(5) GOAL(4) 15

16 Monotonic never overestimates the cost of getting from a node to its neighbor. for all paths x,y where y is a successor of x, i.e., h(x) g(y) - g(x) h(y) h(a) = 3 g(a) = 1 h(e) = 1 g(e) = 2 h(a) 3 g(e) - g(a) h(e)

17 Non-opportunistic 1. Put S on priority Q and expand it 2. Expand A because its priority value is 7 3. The goal is reached with priority value 8 4. This is less than B s priority value which is 13 17

18 Roadmap: GVG A GVG is formed by paths equidistant from the two closest objects Remember spokes, start and goal This generates a very safe roadmap which avoids obstacles as much as possible

19 Distance to Obstacle(s) D( q) min d ( q) i

20 Two-Equidistant Two-equidistant surface S ij { x Qfree : d ( x) d ( x) i j 0} QOi QOj

21 More Rigorous Definition Going through obstacles QO i QOk QO d ( x) d ( x) d ( x) j SS ij k i j Two-equidistant face F ij { x SS : d ( x) d ( x) d ( x), h i, j} ij i j h

22 GVD General Voronoi Diagram n 1 n i 1 j i 1 F ij

23 What about concave obstacles? vs

24 What about concave obstacles? d j d i vs d j d i

25 What about concave obstacles? d j d i vs d j d i d j d i d j d i

26 Two-Equidistant Two-equidistant surface S ij { x Qfree : d ( x) d ( x) 0} Two-equidistant surjective surface SS ij i { x S : d ( x) d ( x)} ij i j j d j Two-equidistant Face F ij { x SS : d ( x) d ( x), h i} ij i h C i d i GVD n 1 n F ij i 1 j i 1 S ij C j

27 Voronoi Diagram: Metrics

28 Note the curved edges Voronoi Diagram (L2)

29 Note the lack of curved edges Voronoi Diagram (L1)

30 Exact Cell vs. Approximate Cell Cell: simple region

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

32 Set Notation

33 Examples

34 Definition

35 Cell Decompositions: Trapezoidal Decomposition A way to divide the world into smaller regions Assume a polygonal world

36 Cell Decompositions: Trapezoidal Decomposition Simply draw a vertical line from each vertex until you hit an obstacle. This reduces the world to a union of trapezoid-shaped cells

37 Applications: Coverage By reducing the world to cells, we ve essentially abstracted the world to a graph.

38 Find a path By reducing the world to cells, we ve essentially abstracted the world to a graph.

39 Find a path With an adjacency graph, a path from start to goal can be found by simple traversal start goal

40 Find a path With an adjacency graph, a path from start to goal can be found by simple traversal start goal

41 Find a path With an adjacency graph, a path from start to goal can be found by simple traversal start goal

42 Find a path With an adjacency graph, a path from start to goal can be found by simple traversal start goal

43 Find a path With an adjacency graph, a path from start to goal can be found by simple traversal start goal

44 Find a path With an adjacency graph, a path from start to goal can be found by simple traversal start goal

45 Find a path With an adjacency graph, a path from start to goal can be found by simple traversal start goal

46 Find a path With an adjacency graph, a path from start to goal can be found by simple traversal start goal

47 Find a path With an adjacency graph, a path from start to goal can be found by simple traversal start goal

48 Find a path With an adjacency graph, a path from start to goal can be found by simple traversal start goal

49 Find a path With an adjacency graph, a path from start to goal can be found by simple traversal start goal

50 Connect Midpoints of Traps

51 Applications: Coverage First, a distinction between sensor and detector must be made Sensor: Senses obstacles Detector: What actually does the coverage We ll be observing the simple case of having an omniscient sensor and having the detector s footprint equal to the robot s footprint

52 Howie Choset Robotics Institute (snake robots)

53 Cell Decompositions: Trapezoidal Decomposition How is this useful? Well, trapezoids can easily be covered with simple back-and-forth sweeping motions. If we cover all the trapezoids, we can effectively cover the entire reachable world.

54 Applications: Coverage Simply visit all the nodes, performing a sweeping motion in each, and you re done.

55 Cell Decomp. in Terms of Critical Points h m Slice is a level set Slice function: h(x,y)= x, slice={(x,y) h(x,y)= } At a critical point x of h M, h( x) m( x) where M = {x m(x)=0}

56 1-connected h(x,y) = a1

57 2-connected h(x,y) = a2

58 h(x,y) = a3 1-connected

59 h(x,y) = a4 2-connected

60 a1 a2 a3 a4 Connectivity of the slice in the free space changes at the critical points (Morse theory)

61 Each cell can be covered by back and forth motions

62 Cell-Decomposition Approach Define Decomposition Completeness Sensor-based Construction Define Other Decompositions Other patterns Extended detector

63 Provably completeness = guaranteed completeness

64 Simultaneous Coverage* and Localization *mapping too 25m x 30m Successful Experiment: Stopped because of robot battery limitations

65 Operational Hierarchy Path Planning Position Planning Local Planning Where to cover and map How to collaboratively move and maintain low position error (w/o GPS) Obstacle detection/avoidance Terrain features to inform Path Planner Calibrate robots initial location and go

66 Probabilistic Coverage

67 Process Variables Uniformity Waste Positioning Cycle-time Time-to-completion Programming time Surface Deposition

68 Conclusion: Complete Overview The Basics Motion Planning Statement The World and Robot Configuration Space Metrics Path Planning Algorithms Start-Goal Methods Lumelsky Bug Algorithms Potential Charge Functions The Wavefront Planner Map-Based Approaches Generalized Voronoi Graphs Visibility Graphs Cellular Decompositions => Coverage Done with Motion Planning!