Path Planning. Ioannis Rekleitis

Transcription

1 Path Planning Ioannis Rekleitis

2 Outline Path Planning Visibility Graph Potential Fields Bug Algorithms Skeletons/Voronoi Graphs C-Space CSCE-574 Robotics 2

3 Mo+on Planning The ability to go from A to B Known map Off-line planning Unknown Environment Online planning Static/Dynamic Environment q init q goal q goal q goal q init q init CSCE-574 Robotics 3

4 Path Planning World Robot Map CSCE-574 Robotics 4

5 Path Planning Robot World Indoor/Outdoor 2D/2.5D/3D Static/Dynamic Known/Unknown Abstract (web) Map CSCE-574 Robotics 5

6 Path Planning World Robot CSCE-574 Robotics Mobile Ø Indoor/Outdoor Ø Walking/Flying/Swimming Manipulator Humanoid Abstract Map 6

7 Path Planning World Robot Map Topological Metric Feature Based 1D,2D,2.5D,3D CSCE-574 Robotics 7

8 Path Planning World Robot CSCE-574 Robotics Indoor/Outdoor 2D/2.5D/3D Static/Dynamic Known/Unknown Abstract (web) Mobile Ø Indoor/Outdoor Ø Walking/Flying/Swimming Manipulator Humanoid Abstract Map Topological Metric Feature Based 1D,2D,2.5D,3D 8

9 Path Planning: Assump+ons Known Map Roadmaps (Graph representations) Polygonal Representation q init q goal CSCE-574 Robotics 9

10 Visibility Graph Connect Initial and goal locations with all the visible vertices q init q goal CSCE-574 Robotics 10

11 Visibility Graph Connect initial and goal locations with all the visible vertices Connect each obstacle vertex to every visible obstacle vertex q init q goal CSCE-574 Robotics 11

12 Visibility Graph Connect initial and goal locations with all the visible vertices Connect each obstacle vertex to every visible obstacle vertex Remove edges that intersect the interior of an obstacle q init q goal CSCE-574 Robotics 12

13 Visibility Graph Connect initial and goal locations with all the visible vertices Connect each obstacle vertex to every visible obstacle vertex Remove edges that intersect the interior of an obstacle Plan on the resulting graph q init q goal CSCE-574 Robotics 13

14 An alternative path Visibility Graph Alternative name: Rubber band algorithm q init q goal CSCE-574 Robotics 14

15 Major Fault Point robot Path planning like that guarantees to hit the obstacles CSCE-574 Robotics 15

16 Path Planning Potential Field methods compute a repulsive force away from obstacles CSCE-574 Robotics 16

17 Local techniques Potential Field methods compute a repulsive force away from obstacles compute an attractive force toward the goal CSCE-574 Robotics 17

18 Local techniques Potential Field methods compute a repulsive force away from obstacles compute an attractive force toward the goal let the sum of the forces control the robot CSCE-574 Robotics 18 key advantages?

19 Local techniques Potential Field methods compute a repulsive force away from obstacles compute an attractive force toward the goal let the sum of the forces control the robot CSCE-574 Robotics To a large extent, this is 19 computable from sensor readings

20 Sensor Based Calculations CSCE-574 Robotics 20

21 Major Problem? CSCE-574 Robotics 21

22 Local Minima! F goal F obst CSCE-574 Robotics 22

23 Simulated Annealing Every so often add some random force CSCE-574 Robotics 23

24 Limited-knowledge path planning Path planning with limited knowledge Insect-inspired bug algorithms Goal known direction to goal otherwise local sensing walls/obstacles encoders reasonable world 1. finitely many obstacles in any finite disc 2. a line will intersect an obstacle finitely many times Start CSCE-574 Robotics 24

25 Not truly modeling bugs... Insects do use several cues for navigation: visual landmarks polarized light chemical sensing neither are the current bugsized robots they re not ears... Other animals use information from magnetic fields electric currents temperature CSCE-574 Robotics bacteria 25 migrating bobolink

26 Bug Strategy Insect-inspired bug algorithms known direction to goal otherwise only local sensing walls/obstacles encoders Bug 0 algorithm 1) head toward goal 2) follow obstacles until you can head toward the goal again 3) continue assume a leftturn robot CSCE-574 Robotics 26

27 Does It Work? CSCE-574 Robotics 27

28 Bug 1 Insect-inspired bug algorithms known direction to goal otherwise only local sensing walls/obstacles encoders Bug 1 algorithm 1) head toward goal CSCE-574 Robotics 28

29 Bug 1 Insect-inspired bug algorithms known direction to goal otherwise only local sensing walls/obstacles encoders Bug 1 algorithm 1) head toward goal 2) if an obstacle is encountered, circumnavigate it and remember how close you get to the goal CSCE-574 Robotics 29

30 Bug 1 Insect-inspired bug algorithms known direction to goal otherwise only local sensing walls/obstacles encoders Bug 1 algorithm 1) head toward goal 2) if an obstacle is encountered, circumnavigate it and remember how close you get to the goal 3) return to that closest point (by wall-following) and continue Vladimir Lumelsky & Alexander Stepanov Algorithmica 1987 CSCE-574 Robotics 30

31 Bug 1 analysis Distance Traveled What are bounds on the path length that the robot takes? Available Information: D = straight-line distance from start to goal P i = perimeter of the i th obstacle D P 2 Lower and upper bounds? Lower bound: Upper bound: P 1 CSCE-574 Robotics 31

32 Bug 1 analysis Distance Traveled What are bounds on the path length that the robot takes? Available Information: D = straight-line distance from start to goal P i = perimeter of the i th obstacle Lower and upper bounds? D P 2 Lower bound: Upper bound: D P 1 CSCE-574 Robotics 32

33 Bug 1 analysis Distance Traveled What are bounds on the path length that the robot takes? Available Information: D = straight-line distance from start to goal P i = perimeter of the i th obstacle Lower and upper bounds? Lower bound: D D P 2 Upper bound: D Σ P i i How good a bound? P 1 How good an algorithm? CSCE-574 Robotics 33

34 Bug Mapping CSCE-574 Robotics 34

35 A better bug? Call the line from the starting point to the goal the s-line Bug 2 algorithm CSCE-574 Robotics 35

36 A better bug? Call the line from the starting point to the goal the s-line Bug 2 algorithm 1) head toward goal on the s-line CSCE-574 Robotics 36

37 A better bug? Call the line from the starting point to the goal the s-line Bug 2 algorithm 1) head toward goal on the s-line 2) if an obstacle is in the way, follow it until encountering the s- line again. CSCE-574 Robotics 37

38 A better bug? s-line Bug 2 algorithm 1) head toward goal on the s-line 2) if an obstacle is in the way, follow it until encountering the s- line again. 3) Leave the obstacle and continue toward the goal CSCE-574 Robotics 38 OK?

39 A better bug? Bug 2 algorithm Start 1) head toward goal on the s-line 2) if an obstacle is in the way, follow it until encountering the s- line again closer to the goal. 3) Leave the obstacle and continue toward the goal Goal CSCE-574 Robotics 39 OK?

40 Bug 2 analysis Distance Traveled What are bounds on the path length that the robot takes? Available Information: Start D = straight-line distance from start to goal P i = perimeter of the i th obstacle Lower and upper bounds? Lower bound: Upper bound: Goal CSCE-574 Robotics 40

41 Bug 2 analysis Distance Traveled What are bounds on the path length that the robot takes? Available Information: Start D = straight-line distance from start to goal P i = perimeter of the i th obstacle N i = number of s-line intersections with the i th obstacle Lower and upper bounds? Lower bound: Upper bound: Goal CSCE-574 Robotics 41

42 Bug 2 analysis Distance Traveled What are bounds on the path length that the robot takes? Available Information: Start D = straight-line distance from start to goal P i = perimeter of the i th obstacle N i = number of s-line intersections with the i th obstacle Lower and upper bounds? Lower bound: D Upper bound: Goal CSCE-574 Robotics 42

43 Bug 2 analysis Distance Traveled Start What are bounds on the path length that the robot takes? Available Information: D = straight-line distance from start to goal P i = perimeter of the i th obstacle N i = number of s-line intersections with the i th obstacle Lower and upper bounds? Lower bound: D Upper bound: D Σ N i P i i Goal CSCE-574 Robotics 43

44 head-to-head comparison or thorax-to-thorax, perhaps What are worlds in which Bug 2 does better than Bug 1 (and vice versa)? Bug 2 beats Bug 1 Bug 1 beats Bug 2 CSCE-574 Robotics 44

45 head-to-head comparison or thorax-to-thorax, perhaps What are worlds in which Bug 2 does better than Bug 1 (and vice versa)? Bug 2 beats Bug 1 Bug 1 beats Bug 2 zipper world CSCE-574 Robotics 45

46 Other bug-like algorithms The Pledge maze-solving algorithm 1. Go to a wall 2. Keep the wall on your right 3. Continue until out of the maze CSCE-574 Robotics 46

47 Other bug-like algorithms The Pledge maze-solving algorithm 1) Go to a wall 2) Keep the wall on your right 3) Continue until out of the maze int a[1817];main(z,p,q,r){for(p=80;q+p-80;p=2*a[p]) for(z=9;z--;)q=3&(r=time(0)+r*57)/7,q=q?q-1?q-2?1- p%79?-1:0:p%79-77?1:0:p<1659?79:0:p>158?-79:0,q?! a[p+q*2]?a[p+=a[p+=q]=q]=q:0:0;for(;q ;)printf(q%79?"%c":"%c\n"," #"[!a[q-1]]);} IOCCC random maze generator ############################################################################### # # ### # # # # # # # # # # # # # # # # ####### ##### ##### # ####### # # ### ### # ##### ##### # ### ##### # ### # # # # # # # # # # # # # # # # # # # # # # ##### # # # # # # ##### ##### # ### ### # # # ### ### ### ##### # ### ##### # # # # # # # # # # # # # # # # # # # ### ##### ### ##### ##### ### # ### # # # # ### ##### ### ##### # ##### ### # # # # # # # # # # # # # # # # # # # # # # ######### ##### # # # ########### ### ########### ### ##### ##### # # # ### # # # # # # # # # # # # # # # # # # # # # # # # ### ### ##### ####### ### # # ### # # # # ##### # # ##### # # # ### # # ##### # # # # # # # # # # # ### # # # # # # # # # ### # ### # ##### ### # ##### # # # ##### ##### ### ### ####### # ##### # ### # # # # # # # # # # # # # # # # # # # # # # # # # # # ########### ### ### ### ####### # # ### # ####### ### ##### ### ### ##### # # # # # # # # # # # # # # # # # # # ### ##### ##### # ####### # ### ####### ### # ### ####### ### ##### ### # # # # # # # # # # # # # # # # # # # # # # # # # ### ### ### # # # # ### # # # ### # ####### # # ####### # # # # # # ### # # # # # # # # # # # # # # # # # # # # # # # # ### ##### ####### # ##### # ##### ### ### # # # ### ##### ### # ### # # # # # # # # ### # # # # # # # # # # # # # ############################################################################### discretized RRT CSCE-574 Robotics 47 mazes of unusual origin

48 Limited Range Sensor Tangent Bug Tangent Bug relies on finding endpoints of finite, continues segments of the obstacles CSCE-574 Robotics 48

49 Tangent Bug CSCE-574 Robotics 49

50 Contact Sensor Tangent Bug 1. Robot moves toward goal until it hits obstacle 1 at H1 2. Pretend there is an infinitely small sensor range and the direction which minimizes the heuristic is to the right 3. Keep following obstacle until robot can go toward obstacle again 4. Same situation with second obstacle 5. At third obstacle, the robot turned left until it could not increase heuristic 6. D_followed is distance between M3 and goal, d_reach is distance between robot and goal because sensing distance is zero CSCE-574 Robotics 50

51 Limited Sensor Range Tangent-Bug CSCE-574 Robotics 51

52 Infinite Sensor Range Tangent Bug CSCE-574 Robotics 52

53 Known Map Brushfire Transform CSCE-574 Robotics 53

54 The Wavefront Planner: Setup CSCE-574 Robotics 54

55 The Wavefront in Action (Part 1) Starting with the goal, set all adjacent cells with 0 to the current cell Point Connectivity or 8-Point Connectivity? Your Choice. We ll use 8-Point Connectivity in our example CSCE-574 Robotics 55

56 The Wavefront in Action (Part 2) Now repeat with the modified cells This will be repeated until no 0 s are adjacent to cells with values >= 2 0 s will only remain when regions are unreachable CSCE-574 Robotics 56

57 The Wavefront in Action (Part 3) Repeat CSCE-574 Robotics 57

58 The Wavefront in Action (Part 3) Repeat CSCE-574 Robotics 58

59 The Wavefront in Action (Part 3) Until Done 0 s would only remain in the unreachable areas CSCE-574 Robotics 59

60 The Wavefront in Action To find the shortest path, according to your metric, simply always move toward a cell with a lower number The numbers generated by the Wavefront planner are roughly proportional to their distance from the goal CSCE-574 Robotics 60 Two possible shortest paths shown

61 An alternative roadmap CSCE-574 Robotics 61

62 Voronoi diagrams These line segments make up the Voronoi diagram for the four points shown here. Solves the Post Office Problem CSCE-574 Robotics 62

63 Voronoi diagrams These line segments make up the Voronoi diagram for the four points shown here. Solves the Post Office Problem or, perhaps, more important problems... CSCE-574 Robotics 63

64 Voronoi diagrams true Voronoi diagram (isolates a set of points) generalized Voronoi diagram What is it? CSCE-574 Robotics 64

65 Voronoi diagrams C free Let B = the boundary of C free. Let q be a point in C free. ( ) q B CSCE-574 Robotics 65

66 Voronoi diagrams C free Let B = the boundary of C free. Let q be a point in C free. q B Define clearance(q) = min { q - p }, for all p B CSCE-574 Robotics 66

67 Voronoi diagrams C free Let B = the boundary of C free. Let q be a point in C free. q B Define clearance(q) = min { q - p }, for all p B Define near(q) = { p B such that q - p = clearance(q) } CSCE-574 Robotics 67

68 Voronoi diagrams Evaluation + maximizes distance from obstacles + reduces to graph search C free + can be used in higher-dimensions - nonoptimal - real diagrams tend to be noisy Let B = the boundary of C free. q Let q be a point in C free. B Define clearance(q) = min { q - p }, for all p B Define near(q) = { p B such that q - p = clearance(q) } q is in the Voronoi diagram of C free if near(q) > 1 CSCE-574 Robotics number of set elements 68

69 Generalized Voronoi Graph (GVG) CSCE-574 Free Space Robotics 69

70 Generalized Voronoi Graph (GVG) CSCE-574 Robotics Free Space with Topological Map (GVG) 70

71 Generalized Voronoi Graph (GVG) Access GVG Free CSCE-574 Space Robotics with Topological Map (GVG) 71

72 Generalized Voronoi Graph (GVG) Access GVG Follow Edge Free Space with Topological Map (GVG) CSCE-574 Robotics 72

73 Generalized Voronoi Graph (GVG) Access GVG Follow Edge Home to the MeetPoint Free Space with Topological Map (GVG) CSCE-574 Robotics 73

74 Generalized Voronoi Graph (GVG) Access GVG Follow Edge Home to the MeetPoint Select Edge Free Space with Topological Map (GVG) CSCE-574 Robotics 74

75 GVG construction using sonar Nomadic Scout Sonar (GVG navigation) Camera with omni-directional mirror (feature detection) Onboard 1.2 GHz processor CSCE-574 Robotics 75

76 GVG construction using sonar CSCE-574 Robotics 76

77 GVG construction using sonar CSCE-574 Robotics 77

78 Slammer in Action CSCE-574 Robotics 78

79 Removing Edges CSCE-574 Robotics 79

80 Meetpoint Detection 3σ uncertainty ellipse of explored meetpoints Meetpoint degree (branching factor) Distances to local obstacles Relative angle bearings Edge signature Edge length Edge Curvature Vertex signal CSCE-574 Robotics 80

81 Ear-based Exploration CSCE-574 Robotics 81

82 Uncertainty Reduction Before Loop-closure After Loop-closure CSCE-574 Robotics 82

83 Simulation Code available online at CSCE-574 Robotics 83

84 Real Environment CSCE-574 Robotics 84

85 Voronoi applications A retraction of a 3d object == medial surface what? Skeletonizations resulting from constant-speed curve evolution in 2d, it s called a medial axis CSCE-574 Robotics 85

86 skeleton shape curve evolution where wavefronts collide centers of maximal disks again reduces a 2d (or higher) problem to a question about graphs... CSCE-574 Robotics 86

87 skeleton shape curve evolution where wavefronts collide centers of maximal disks again reduces a 2d (or higher) problem to a question about graphs... CSCE-574 Robotics 87 graph matching

88 Problems The skeleton is sensitive to small changes in the object s boundary. - graph isomorphism (and lots CSCE-574 of other Robotics graph questions) : NP-complete 88

89 Roadmap problems If an obstacle decides to roll away... (or wasn t there to begin with) CSCE-574 Robotics 89 recomputing in less than O(N 2 ) time?