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

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

3. Goals Compute motion strategies, e.g., Achieve high-level goals, 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.)
Slide 3

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

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
Slide 5

6. Example: Rigid Objects
Slide 6

7. Example: Articulated Robot
Slide 7

8. Is it easy?
Slide 8

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]
Slide 9

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

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
Slide 11

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
Slide 12

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.
Slide 13

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
Slide 14

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
Slide 15

16. Framework
Slide 16

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.
Slide 17

18. Visibility Graph A visibility graphis a graph such that
Nodes: qinit, qgoal, 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.
Slide 18

19. A Simple Algorithm for Building Visibility Graphs
Slide 19

20. Computational Efficiency
Simple algorithm O(n3) time
More efficient algorithms
Rotational sweep O(n2log n) time
Optimal algorithm O(n2) time
Output sensitive algorithms
O(n2) space
Slide 20

21. Framework
Slide 21

22. Breadth-First Search
Slide 22

23. Breadth-First Search
Slide 23

24. Breadth-First Search
Slide 24

25. Breadth-First Search
Slide 25

26. Breadth-First Search
Slide 26

27. Breadth-First Search
Slide 27

28. Breadth-First Search
Slide 28

29. Breadth-First Search
Slide 29

30. Breadth-First Search
Slide 30

31. Breadth-First Search
Slide 31

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

33. Framework
Slide 33

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

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?
Slide 35

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
Slide 36

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
Slide 37

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.
Slide 38

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

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
Slide 40

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
Slide 41

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
Slide 42

43. Trapezoidal Decomposition
Slide 43

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

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

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

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
Slide 47

48. Quadtree Decomposition
Slide 48

49. Octree Decomposition
Slide 49

50. Algorithm Outline
Slide 50

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
Slide 51

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
Slide 52

53. Attractive & Repulsive Fields
Slide 53

54. How Does It Work?
Slide 54

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
Slide 55

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
Slide 56

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

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?
Slide 58

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
When the input problem does not have a path, no matter how many samples the planner make, it can never end.
Slide 59

60. Path Non-existence Problem
Initial
Robot
Obstacle
Obstacle
Goal
Here is a slightly more complex example. We have 2 gear shaped obstacles in 2D plane. We need to move the gear-shaped robot from left to right through these two obstacles. The figure shows that the robot can be placed between these two obstacles. But can you quickly identify path non-existence for this problem?
% But it’s not intuitive to tell whether there is a collision-free path or no path exists for this example.
% In some sense, to claim the path non-existence is more difficulty than to find a collision free path.
% And demands more computations
Slide 60

61. Main Challenge Exponential complexity: nDOF
Degree of freedom: DOF
Geometric complexity: n
More difficult than finding a path
To check all possible paths
Initial
Robot
Obstacle
Here is a slightly more complex example. We have 2 gear shaped obstacles in 2D plane. We need to move the gear-shaped robot from left to right through these two obstacles. The figure shows that the robot can be placed between these two obstacles. But can you quickly identify path non-existence for this problem?
% But it’s not intuitive to tell whether there is a collision-free path or no path exists for this example.
% In some sense, to claim the path non-existence is more difficulty than to find a collision free path.
% And demands more computations
Goal
Slide 61

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

63. Configuration Space: 2D Translation
Workspace
Configuration Space
Goal
Obstacle
C-obstacle
Free
Robot
The figure shows a triangle shaped robot translating in a 2D environment.
Each position of the robot maps to a point in the configuration space.
If we add an obstacle, it maps to a region in the configuration space.
This region is called the configuration space obstacle or C-obstacle. y x
Start
Slide 63

64. Configuration Space Computation
[Varadhan et al, ICRA 2006]
2 Translation + 1 Rotation
215 seconds
Obstacle  y x
The configuration space is three dimensional. I have shown the contact surfaces. These are close to 4000 surfaces.
And these surfaces are non-linear and can have a maximum degree of 4.
Robot
Slide 64

65. Exact Motion Planning Approaches Not practical
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
Slide 65

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

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]
Slide 67

68. Approximation Cell Decomposition
Configuration Space
full
mixed
Full cell
Empty cell
Mixed cell
Mixed
Uncertain
Now let us look at how C-obstacle query can be used for cell decomposition for path non-existence. The cell decomposition will subdivide the configuration space into cells. If a cell lies entirely in C-obstacle, the cell is labelled as full cell. If … Otherwise, …
Here the full cells can be identified using C-obstacle query.
empty
Slide 68

69. Gf : Free Connectivity Graph
G: Connectivity Graph
Interpration
Gf is a subgraph of G
Slide 69

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

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

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

73. Finding a Path by ACD
Here is the summary of
Slide 73

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

75. ACD for Path Non-existence
Guiding Path
Connectivity Graph
The cell decomposition will build a connectivity graph for the complement of identified C-obstacle region.
% for the complement of empty and mixed cells. The node in the graph is to represent each empty or mixed % cell. Two nodes are connected, if their corresponding cells are adjacent.
Then it searches a guiding path connecting the cell containing the initial configuration, and the cell containing the goal configuration. The cell decomposition is only applied for the mixed on this path.
% If all cells along this path are empty cells, a collision free path has been found. Otherwise, we have to
% perform further subdivision. In order to avoid the substantial subdivision, a smarter way is to subdivide
% the mixed cells along this path. That is the reason, this path as a guiding path.
Slide 75

76. ACD for Path Non-existence
Connectivity graph is not connected
No path!
After one level subdivision, we update the connectivity graph, and perform the graph search again. At this time, we find the graph is disjoint with respect to the initial and the goal configurations. Therefore, we can conclude no-path exists for this problem.
From the above analysis, we can tell the C-obstacle query plays an important role to decide the path non-existence.??? However, most of previous work of C-obstacle query are either hard to implement or overly conservative.
Sufficient condition for deciding path non-existence
Slide 76

77. Two-gear Example Video no path! 3.356s Initial Cells in C-obstacle
Roadmap in F
The first example is two-gear, which we have seen before. We need to move the red gear from left corner to right corner.
Now I will show a video for the running process of our algorithm.
the configuration space,
initial and goal configurations.
The pink volume denotes the cells in C-obstacle identified by our C-obstacle query. A guiding search is searched. And the mixed cell is iterated.
In the right figure, after a few iterations of cell decomposition, we could find no path exists since initial and goal configurations are separated by the C-obstacle region.
Goal
Slide 77

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

79. Free Cell Query A Collision Detection Problem
Does the cell lie inside free space?
Do robot and obstacle separate at all configurations?
Robot
Obstacle
In order to design an efficient … let us analyze the problem of C-obstacle query. The C-obstacle query actually can be regarded as a class of collision detection problem. Suppose the query primitive is a cell. The C-obstacle query is to check whether the cell lies completely inside the c-obstacle.
We know if the robot’s configuration is in C-obstacle, the robot is intersecting with the obstacle. To ensure a cell lies complete inside C-obstacle, we had to make sure at every configuration in the cell, the robot intersects with the obstacle.
In another way, this means when the robot’s configuration varies within the cell, the robot can never escape from the obstacle. In fact, the later intuitive interpretation is very helpful to understand our C-obstacle query algorithm, which will be discussed in the next slide. ?
Configuration space
Workspace
Slide 79

80. Clearance d Separation distance
A well studied geometric problem
Determine a volume in C-space which are completely free d
Suppose two objects are disjoint, we can use the separation distance between them. This distance describes the clearance of the robot, here the robot. If the motion of the robot is less than the clearance, the robot can not intersect with the obstacle. The dual concept of clearance here is called forbiddance.
When two objects intersects, we can measure the penetration depth between them. This depth describes the forbiddance, which means …
Slide 80

81. C-obstacle Query Another Collision Detection Problem
Does the cell lie inside C-obstacle?
Do robot and obstacle intersect at all configurations?
Robot ?
Obstacle
In order to design an efficient … let us analyze the problem of C-obstacle query. The C-obstacle query actually can be regarded as a class of collision detection problem. Suppose the query primitive is a cell. The C-obstacle query is to check whether the cell lies completely inside the c-obstacle.
We know if the robot’s configuration is in C-obstacle, the robot is intersecting with the obstacle. To ensure a cell lies complete inside C-obstacle, we had to make sure at every configuration in the cell, the robot intersects with the obstacle.
In another way, this means when the robot’s configuration varies within the cell, the robot can never escape from the obstacle. In fact, the later intuitive interpretation is very helpful to understand our C-obstacle query algorithm, which will be discussed in the next slide.
Configuration space
Workspace
Slide 81

82. ‘Forbiddance’ ‘Forbiddance’: dual to clearance Penetration Depth
A geometric computation problem less investigated
[Zhang et al. ACM SPM 2006]
Suppose two objects are disjoint, we can use the separation distance between them. This distance describes the clearance of the robot, here the robot. If the motion of the robot is less than the clearance, the robot can not intersect with the obstacle. The dual concept of clearance here is called forbiddance.
When two objects intersects, we can measure the penetration depth between them. This depth describes the forbiddance, which means … PD
Slide 82

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

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

85. Can we combine them together?
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?
Slide 85

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
Slide 86

87. Gf : Free Connectivity Graph
G: Connectivity Graph
Interpration
Gf is a subgraph of G
Slide 87

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

89. Pseudo-free Connectivity Graph: Gsf
Gsf = Gf + Pseudo-edges
Initial
Goal
Slide 89

90. Algorithm Gf
Gsf G
Slide 90

91. Results of Hybrid Planning
Slide 91

92. Results of Hybrid Planning
Slide 92

93. Results of Hybrid Planning
times speedup
3 DOF
4 DOF
timing
cells #
Hybrid
34s
50K
16s
48K
102s
164K
ACD
85s
168K ?
Speedup
2.5
3.3
≥10
For 4DOF, the ACD method could not terminate within 10 times of the timing for the hybrid planners.
Slide 93

94. Summary Difficult for Exact Motion Planning ACD is more practical
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
Slide 94