1. Slide 1 Robot Lab: Robot Path Planning William Regli Department of Computer Science (and Departments of ECE and MEM) Drexel University

2. Slide 2 Introduction to Motion Planning Applications Overview of the Problem Basics – Planning for Point Robot –Visibility Graphs –Roadmap –Cell Decomposition –Potential Field

3. Slide 3 Goals Compute motion strategies, e.g., –Geometric paths –Time-parameterized trajectories –Sequence of sensor-based motion commands Achieve high-level goals, e.g., –Go to the door and do not collide with obstacles –Assemble/disassemble the engine –Build a map of the hallway –Find and track the target (an intruder, a missing pet, etc.)

4. Slide 4 Fundamental Question Are two given points connected by a path?

5. Slide 5 Basic Problem Problem statement: Compute a collision-free path for a rigid or articulated moving object among static obstacles. Input –Geometry of a moving object (a robot, a digital actor, or a molecule) and obstacles –How does the robot move? –Kinematics of the robot (degrees of freedom) –Initial and goal robot configurations (positions & orientations) Output Continuous sequence of collision-free robot configurations connecting the initial and goal configurations

6. Slide 6 Example: Rigid Objects

7. Slide 7 Example: Articulated Robot

8. Slide 8 Is it easy?

9. Slide 9 Hardness Results Several variants of the path planning problem have been proven to be PSPACE-hard. A complete algorithm may take exponential time. –A complete algorithm finds a path if one exists and reports no path exists otherwise. Examples –Planar linkages [Hopcroft et al., 1984] –Multiple rectangles [Hopcroft et al., 1984]

10. Slide 10 Tool: Configuration Space Difficulty –Number of degrees of freedom (dimension of configuration space) –Geometric complexity

11. Slide 11 Extensions of the Basic Problem More complex robots –Multiple robots –Movable objects –Nonholonomic & dynamic constraints –Physical models and deformable objects –Sensorless motions (exploiting task mechanics) –Uncertainty in control

12. Slide 12 Extensions of the Basic Problem More complex environments –Moving obstacles –Uncertainty in sensing More complex objectives –Optimal motion planning –Integration of planning and control –Assembly planning –Sensing the environment Model building Target finding, tracking

13. Slide 13 Practical Algorithms A complete motion planner always returns a solution when one exists and indicates that no such solution exists otherwise. Most motion planning problems are hard, meaning that complete planners take exponential time in the number of degrees of freedom, moving objects, etc.

14. Slide 14 Practical Algorithms Theoretical algorithms strive for completeness and low worst-case complexity –Difficult to implement –Not robust Heuristic algorithms strive for efficiency in commonly encountered situations. –No performance guarantee Practical algorithms with performance guarantees –Weaker forms of completeness –Simplifying assumptions on the space: “exponential time” algorithms that work in practice

15. Slide 15 Problem Formulation for Point Robot Input –Robot represented as a point in the plane –Obstacles represented as polygons –Initial and goal positions Output –A collision-free path between the initial and goal positions

16. Slide 16 Framework

17. Slide 17 Visibility Graph Method Observation: If there is a collision-free path between two points, then there is a polygonal path that bends only at the obstacles vertices. Why? –Any collision-free path can be transformed into a polygonal path that bends only at the obstacle vertices. A polygonal path is a piecewise linear curve.

18. Slide 18 Visibility Graph A visibility graphis a graph such that –Nodes: q init, q goal, or an obstacle vertex. –Edges: An edge exists between nodes u and v if the line segment between u and v is an obstacle edge or it does not intersect the obstacles.

19. Slide 19 A Simple Algorithm for Building Visibility Graphs

20. Slide 20 Computational Efficiency Simple algorithm O(n 3 ) time More efficient algorithms –Rotational sweep O(n 2 log n) time –Optimal algorithm O(n 2 ) time –Output sensitive algorithms O(n 2 ) space

21. Slide 21 Framework

22. Slide 22 Breadth-First Search

23. Slide 23 Breadth-First Search

24. Slide 24 Breadth-First Search

25. Slide 25 Breadth-First Search

26. Slide 26 Breadth-First Search

27. Slide 27 Breadth-First Search

28. Slide 28 Breadth-First Search

29. Slide 29 Breadth-First Search

30. Slide 30 Breadth-First Search

31. Slide 31 Breadth-First Search

32. Slide 32 Other Search Algorithms Depth-First Search Best-First Search, A*

33. Slide 33 Framework

34. Slide 34 Summary Discretize the space by constructing visibility graph Search the visibility graph with breadth-first search Q: How to perform the intersection test?

35. Slide 35 Summary Represent the connectivity of the configuration space in the visibility graph Running time O(n3) –Compute the visibility graph –Search the graph –An optimal O(n2) time algorithm exists. Space O(n2) Can we do better?

36. Slide 36 Classic Path Planning Approaches Roadmap – Represent the connectivity of the free space by a network of 1-D curves Cell decomposition – Decompose the free space into simple cells and represent the connectivity of the free space by the adjacency graph of these cells Potential field – Define a potential function over the free space that has a global minimum at the goal and follow the steepest descent of the potential function

37. Slide 37 Classic Path Planning Approaches Roadmap – Represent the connectivity of the free space by a network of 1-D curves Cell decomposition – Decompose the free space into simple cells and represent the connectivity of the free space by the adjacency graph of these cells Potential field – Define a potential function over the free space that has a global minimum at the goal and follow the steepest descent of the potential function

38. Slide 38 Roadmap Visibility graph Shakey Project, SRI [Nilsson, 1969] Voronoi Diagram Introduced by computational geometry researchers. Generate paths that maximizes clearance. Applicable mostly to 2-D configuration spaces.

39. Slide 39 Voronoi Diagram Space O(n) Run time O(n log n)

40. Slide 40 Other Roadmap Methods Silhouette First complete general method that applies to spaces of any dimensions and is singly exponential in the number of dimensions [Canny 1987] Probabilistic roadmaps

41. Slide 41 Classic Path Planning Approaches Roadmap – Represent the connectivity of the free space by a network of 1-D curves Cell decomposition – Decompose the free space into simple cells and represent the connectivity of the free space by the adjacency graph of these cells Potential field – Define a potential function over the free space that has a global minimum at the goal and follow the steepest descent of the potential function

42. Slide 42 Cell-decomposition Methods Exact cell decomposition The free space F is represented by a collection of non-overlapping simple cells whose union is exactly F Examples of cells: trapezoids, triangles

43. Slide 43 Trapezoidal Decomposition

44. Slide 44 Computational Efficiency Running time O(n log n) by planar sweep Space O(n) Mostly for 2-D configuration spaces

45. Slide 45 Adjacency Graph Nodes: cells Edges: There is an edge between every pair of nodes whose corresponding cells are adjacent.

46. Slide 46 Summary Discretize the space by constructing an adjacency graph of the cells Search the adjacency graph

47. Slide 47 Cell-decomposition Methods Exact cell decomposition Approximate cell decomposition –F is represented by a collection of non- overlapping cells whose union is contained in F. –Cells usually have simple, regular shapes, e.g., rectangles, squares. –Facilitate hierarchical space decomposition

48. Slide 48 Quadtree Decomposition

49. Slide 49 Octree Decomposition

50. Slide 50 Algorithm Outline

51. Slide 51 Classic Path Planning Approaches Roadmap – Represent the connectivity of the free space by a network of 1-D curves Cell decomposition – Decompose the free space into simple cells and represent the connectivity of the free space by the adjacency graph of these cells Potential field – Define a potential function over the free space that has a global minimum at the goal and follow the steepest descent of the potential function

52. Slide 52 Potential Fields Initially proposed for real-time collision avoidance [Khatib 1986]. Hundreds of papers published. A potential field is a scalar function over the free space. To navigate, the robot applies a force proportional to the negated gradient of the potential field. A navigation function is an ideal potential field that –has global minimum at the goal –has no local minima –grows to infinity near obstacles –is smooth

53. Slide 53 Attractive & Repulsive Fields

54. Slide 54 How Does It Work?

55. Slide 55 Algorithm Outline Place a regular grid G over the configuration space Compute the potential field over G Search G using a best-first algorithm with potential field as the heuristic function

56. Slide 56 Local Minima What can we do? –Escape from local minima by taking random walks –Build an ideal potential field – navigation function – that does not have local minima

57. Slide 57 Question Can such an ideal potential field be constructed efficiently in general?

58. Slide 58 Completeness A complete motion planner always returns a solution when one exists and indicates that no such solution exists otherwise. –Is the visibility graph algorithm complete? Yes. –How about the exact cell decomposition algorithm and the potential field algorithm?

59. Slide 59 Why Complete Motion Planning? Probabilistic roadmap motion planning –Efficient –Work for complex problems with many DOF –Difficult for narrow passages –May not terminate when no path exists Complete motion planning –Always terminate –Not efficient –Not robust even for low DOF

60. Slide 60 Path Non-existence Problem Obstacle Goal Initial Robot

61. Slide 61 Main Challenge Obstacle Goal Initial Robot Exponential complexity: n DOF –Degree of freedom: DOF –Geometric complexity: n More difficult than finding a path –To check all possible paths

62. Slide 62 Approaches Exact Motion Planning –Based on exact representation of free space Approximation Cell Decomposition (ACD) A Hybrid planner

63. Slide 63 Configuration Space: 2D Translation Workspace Configuration Space x y Robot Start Goal Free Obstacle C- obstacle

64. Slide 64 Configuration Space Computation [Varadhan et al, ICRA 2006] 2 Translation + 1 Rotation 215 seconds Obstacle Robot x  y

65. Slide 65 Exact Motion Planning Approaches –Exact cell decomposition [Schwartz et al. 83] –Roadmap [Canny 88] –Criticality based method [Latombe 99] –Voronoi Diagram –Star-shaped roadmap [Varadhan et al. 06] Not practical –Due to free space computation –Limit for special and simple objects Ladders, sphere, convex shapes 3DOF

66. Slide 66 Approaches Exact Motion Planning –Based on exact representation of free space Approximation Cell Decomposition (ACD) A Hybrid Planner Combing ACD and PRM

67. Slide 67 Approximation Cell Decomposition (ACD) Not compute the free space exactly at once But compute it incrementally Relatively easy to implement –[Lozano-Pérez 83] –[Zhu et al. 91] –[Latombe 91] –[Zhang et al. 06]

68. Slide 68 fullmixed empty Approximation Cell Decomposition Full cell Empty cell Mixed cell –Mixed –Uncertain Configuration Space

69. Slide 69 Connectivity Graph G f : Free Connectivity GraphG: Connectivity Graph G f is a subgraph of G

70. Slide 70 Finding a Path by ACD Goal Initial G f : Free Connectivity Graph

71. Slide 71 Finding a Path by ACD L: Guiding Path First Graph Cut Algorithm –Guiding path in connectivity graph G –Only subdivide along this path –Update the graphs G and G f Described in Latombe’s book

72. Slide 72 First Graph Cut Algorithm Only subdivide along L L

73. Slide 73 Finding a Path by ACD

74. Slide 74 ACD for Path Non-existence C-space Goal Initial

75. Slide 75 Connectivity Graph Guiding Path ACD for Path Non-existence

76. Slide 76 ACD for Path Non-existence Connectivity graph is not connected No path! Sufficient condition for deciding path non-existence

77. Slide 77 Two-gear Example no path! Cells in C-obstacle Initial Goal Roadmap in F Vide o 3.356s

78. Slide 78 Cell Labeling Free Cell Query –Whether a cell completely lies in free space? C-obstacle Cell Query –Whether a cell completely lies in C-obstacle? fullmixed empty

79. Slide 79 Free Cell Query A Collision Detection Problem Does the cell lie inside free space? Do robot and obstacle separate at all configurations? Obstacle Workspace Configuration space ? Robot

80. Slide 80 Clearance Separation distance –A well studied geometric problem Determine a volume in C-space which are completely free d

81. Slide 81 C-obstacle Query Another Collision Detection Problem Does the cell lie inside C-obstacle? Do robot and obstacle intersect at all configurations? Obstacle Workspace Configuration space ? Robot

82. Slide 82 ‘Forbiddance’ ‘Forbiddance’: dual to clearance Penetration Depth –A geometric computation problem less investigated [Zhang et al. ACM SPM 2006] PD

83. Slide 83 Limitation of ACD Combinatorial complexity of cell decomposition Limited for low DOF problem –3-DOF robots

84. Slide 84 Approaches Exact Motion Planning –Based on exact representation of free space Approximation Cell Decomposition (ACD) A Hybrid Planner Combing ACD and PRM

85. Slide 85 Hybrid Planning Probabilistic roadmap motion planning + Efficient + Many DOFs -Narrow passages -Path non-existence Complete Motion Planning + Complete -Not efficient Can we combine them together?

86. Slide 86 Hybrid Approach for Complete Motion Planning Use Probabilistic Roadmap (PRM): –Capture the connectivity for mixed cells –Avoid substantial subdivision Use Approximation Cell Decomposition (ACD) –Completeness –Improve the sampling on narrow passages

87. Slide 87 Connectivity Graph G f : Free Connectivity GraphG: Connectivity Graph G f is a subgraph of G

88. Slide 88 Pseudo-free edges Pseudo free edge for two adjacent cells Goal Initial

89. Slide 89 Pseudo-free Connectivity Graph: G sf Goal Initial G sf = G f + Pseudo-edges

90. Slide 90 Algorithm G f G sf G

91. Slide 91 Results of Hybrid Planning

92. Slide 92 Results of Hybrid Planning

93. Slide 93 Results of Hybrid Planning 2.5 - 10 times speedup 3 DOF4 DOF timingcells #timingcells #timingcells # Hybrid34s50K16s48K102s164K ACD85s168K???? Speedup2.53.3≥10? ?

94. Slide 94 Summary Difficult for Exact Motion Planning –Due to the difficulty of free space configuration computation ACD is more practical –Explore the free space incrementally Hybrid Planning –Combine the completeness of ACD and efficiency of PRM