Complete Motion Planning
Liang-Jun Zhang
Robotics, Comp790-072
Oct 26, 2006
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Motion Planning
To find a path
72 DOFno path
To report
Courtesy of P. Isto and M. Saha, 2006
Goal
Goal
Robot
Robot
Initial
Initial
Obstacle
3
Obstacle
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Why Complete Motion Planning?
Probabilistic roadmap
motion planning
4
Complete motion
planning
Efficient
Work for complex problems
with many DOF
Always terminate
Difficult for narrow passages
May not terminate when no
path exists
Not efficient
Not robust even for
low DOF
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Path Non-existence Problem
Robot
Initial
5
Obstacle
Obstacle
Goal
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Main Challenge
Exponential complexity: nDOF
Degree of freedom: DOF
Geometric complexity: n
More difficult than finding a path
To check all possible paths
Robot
Initial
6
Obstacle
Goal
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Approaches
Exact Motion Planning
Based on exact representation of free space
Approximation Cell Decomposition
(ACD)
A Hybrid planner
7
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Configuration Space: 2D Translation
Workspace
Configuration Space
Goal
Obstacle
C-obstacle
Free
Robot
Start
8
y
x
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Configuration Space Computation
[Varadhan et al, ICRA 2006]
2 Translation + 1 Rotation
215 seconds
Obstacle

y
x
Robot
9
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
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
10
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Approaches
Exact Motion Planning
Based on exact representation of free space
Approximation Cell Decomposition
(ACD)
A Hybrid Planner Combing ACD and
PRM
11
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
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]
12
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Approximation Cell Decomposition
Full cell
Configuration Space
full
mixed
Empty cell
Mixed cell
Mixed
Uncertain
empty
13
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Connectivity Graph
Gf : Free Connectivity Graph
G: Connectivity Graph
Gf is a subgraph of G
14
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Finding a Path by ACD
Gf : Free Connectivity Graph
Initial
Goal
15
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Finding a Path by ACD
First Graph Cut
Algorithm
L: Guiding Path
Guiding path in connectivity
graph G
Only subdivide along this path
Update the graphs G and Gf
Described in Latombe’s book
16
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
First Graph Cut Algorithm
L
Only subdivide along L
17
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Finding a Path by ACD
18
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
ACD for Path Non-existence
Initial
Goal
C-space
19
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
ACD for Path Non-existence
Connectivity Graph
20
Guiding Path
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
ACD for Path Non-existence
Connectivity graph
is not connected
No path!
Sufficient condition for deciding path non-existence
21
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Two-gear Example
Vide
o
no path!
Initial
3.356s
Cells
in C-obstacle
Roadmap
in F
Goal
22
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Cell Labeling
full
Free Cell Query
mixed
Whether a cell
completely lies in
free space?
C-obstacle Cell
Query
Whether a cell
completely lies in
C-obstacle?
23
empty
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Free Cell Query
A Collision Detection Problem
•Does the cell lie inside
free space?
• Do robot and obstacle
separate at all configurations?
Robot
Obstacle
?
Configuration space
24
Workspace
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Clearance
Separation distance
A well studied geometric problem
d
Determine a volume in C-space
which are completely free
25
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
C-obstacle Query
Another Collision Detection Problem
•Does the cell lie inside
C-obstacle?
• Do robot and obstacle
intersect at all configurations?
Robot
?
Obstacle
Configuration space
26
Workspace
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
‘Forbiddance’
‘Forbiddance’: dual to clearance
Penetration Depth
A geometric computation problem less
investigated
PD
[Zhang et al. ACM SPM 2006]
27
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Limitation of ACD
Combinatorial complexity of cell
decomposition
Limited for low DOF problem
3-DOF robots
28
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Approaches
Exact Motion Planning
Based on exact representation of free space
Approximation Cell Decomposition
(ACD)
A Hybrid Planner Combing ACD and
PRM
29
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Hybrid Planning
Probabilistic roadmap
motion planning
Complete Motion
Planning
+ Efficient
+ Many DOFs
+ Complete
- Narrow passages
- Path non-existence
- Not efficient
Can we combine them together?
30
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
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
31
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Connectivity Graph
Gf : Free Connectivity Graph
G: Connectivity Graph
Gf is a subgraph of G
32
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Pseudo-free edges
Initial
Goal
Pseudo free edge for
two adjacent cells
33
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Pseudo-free Connectivity Graph:
Gsf
Gsf = Gf + Pseudo-edges
Initial
34
Goal
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Algorithm
Gf
Gsf
G
35
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Results of Hybrid Planning
36
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Results of Hybrid Planning
37
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Results of Hybrid Planning
2.5 - 10 times speedup
3 DOF
4 DOF
4 DOF
timing
cells #
timing
cells #
timing
cells #
Hybrid
34s
50K
16s
48K
102s
164K
ACD
85s
168K
?
?
?
?
Speedup
2.5
3.3
≥10
?
≥10
?
38
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
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
39
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Future Work
Complete motion planning for
6DOF rigid robots
More accurate PDg computation
Efficient C-Obstacle representation and
computation
Extend for articulated robots
40
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Reference: Exact Motion Planning
• J. Canny. The Complexity of Robot Motion Planning.
ACM Doctoral Dissertation Award. MIT Press, 1988.
• F. Avnaim and J.-D. Boissonnat. Practical exact
motion planning of a class of robots with three
degrees of freedom. In Proc. of Canadian Conference
on Computational Geometry, page 19, 1989.
• J. T. Schwartz and M. Sharir. On the piano movers
probelem ii, general techniques for computing
topological properties of real algebraic manifolds.
Advances of Applied Maths, 4:298–351, 1983.
Gokul Varadhan, Dinesh Manocha, Star-shaped
Roadmaps - A Deterministic Sampling Approach for
Complete Motion Planning, Robotics: Science and
Systems 2006
41
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL
Reference: Approximation Cell
Decomposition
•
T. Lozano-P´erez and M. Wesley. An algorithm for planning
collisionfree paths among polyhedral obstacles. Comm.
ACM, 22(10):560–570, 1979.
•
R. A. Brooks and T. Lozano-P´erez. A subdivision algorithm
in configuration space for findpath with rotation. IEEE
Trans. Syst, SMC-15:224–233, 1985.
•
D. Zhu and J. Latombe. Constraint reformulation in a
hierarchical path planner. Proceedings of International
Conference on Robotics and Automation, pages 1918–
1923, 1990.
L. Zhang, Y. Kim, and D. Manocha. A simple path nonexistence algorithm using c-obstacle query. In Proc. of
WAFR, 2006.
L. Zhang, Y.J. Kim, and D. Manocha, A Hybrid Approach for
Complete Motion Planning, UNC-CS Tech Report 06-022
42
The UNIVERSITY of NORTH CAROLINA at CHAPEL HILL