View
213
Download
0
Tags:
Embed Size (px)
Citation preview
1
Single Robot Motion Planning - II
Liang-Jun ZhangCOMP790-058
Sep 24, 2008
2
Review: C-space
Workspace Configuration Space
xyRobot
Initial
Goal
Free
Obstacle C-obstacle
A 2D Translating Robot
3
Review: Computing C-obstacle
• Difficult due to geometric and space complexity• Practical solutions are only available for
– Translating rigid robots: Minkowski sum– Robots with no more than 3 DOFs
4
Outline
• Approximate cell decomposition
• Sampling-based motion planning
5
Approximate 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]
Octree decomposition
6
full mixed
empty
Approximate Cell Decomposition
• Full cell
• Empty cell
• Mixed cell– Mixed– Uncertain
• Cell labelling algorithms– [Zhang et al 06]
Configuration Space
7
Finding a Path by ACD
Goal
Initial
8
Connectivity GraphGf : Free Connectivity Graph G: Connectivity Graph
Gf is a subgraph of G
9
Finding a Path by ACD
Goal
Initial Gf : Free Connectivity Graph
10
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 Gf
11
First Graph Cut Algorithm
Only subdivide the cells along L
L : Guiding Path
new Gf
12
Finding a Path by ACDGf
• A channel
• Can be used for path smoothing.
13
ACD for Path Non-existence
C-space
Goal
Initial
14
Connectivity Graph Guiding Path
ACD for Path Non-existence
15
ACD for Path Non-existence
Connectivity graph is not connected
No path!
A sufficient condition for deciding path non-existence
16
• Live Demo– Gear-2DOF– Gear-3DOF
17
Five-gear Example
Video
Initial
Goal
roadmap in free space
Total timing 85s
# of total cells 168K
18
Two-gear Example
no path!
Cells in C-obstacle
Initial
Goal
Roadmap in F
Video 3.356s
19
Motion Planning Framework
• Continuous representation
• Discretization
• Graph search
20
Summary: Approximate Cell Decomposition
• Simple and easy to implement
• Efficient and practical for low DOF robots – Inefficient for 5 or more DOFs robot
• Resolution-complete– Find a path if there is one– Otherwise, report path non-existence– Up to some resolution of the cell
21
Outline
• Approximate cell decomposition
• Sampling-based motion planning
22
Motivation
• Geometric complexity• Space dimensionality
23
Probabilistic Roadmap (PRM)
free space
qqinitinit
qqgoalgoal
milestone
[Kavraki, Svetska, Latombe,Overmars, 95]
local path
24
Basic PRM AalgorithmInput: geometry of the moving object & obstaclesOutput: roadmap G = (V, E)
1: V and E .
2: repeat3: q a configuration sampled uniformly at random from C.4: if CLEAR(q)then5: Add q to V.6: Nq a set of nodes in V that are close to q.
6: for each q’ Nq, in order of increasing d(q,q’)7: if LINK(q’,q)then8: Add an edge between q and q’ to E.
25
Two Geometric Primitives in C-space
• CLEAR(q)Is configuration q collision free or not?
• LINK(q, q’)
Is the straight-line path between q and q’ collision-free?
26
Query Processing
• Connect qinit and qgoal to the roadmap
• Start at qinit and qgoal, perform a random walk, and try to connect with one of the milestones nearby
• Try multiple times
27
Two Tenets of PRM Planning Checking sampled configurations and
connections between samples for collision can be done efficiently. Hierarchical collision checking[Hierarchical collision checking methods were developed independently from PRM, roughly at the same time]
A relatively small number of milestones and local paths are sufficient to capture the connectivity of the free space. Exponential convergence in expansive
free space (probabilistic completeness)
28
Why does it work? Intuition• A small number of milestones almost
“cover” the entire free space.
29
Two Tenets of PRM Planning Checking sampled configurations and
connections between samples for collision can be done efficiently. Hierarchical collision checking[Hierarchical collision checking methods were developed independently from PRM, roughly at the same time]
A relatively small number of milestones and local paths are sufficient to capture the connectivity of the free space. Exponential convergence in expansive
free space (probabilistic completeness)
30
Narrow Passage Problem
• Narrow passages are difficult to be sampled due to their small volumes in C-space
Narrow passage
Alpha puzzle
qinitqgoal
2F
3F
Configuration Space
1F
31
Difficulty• Many small connected components
32
Strategies to Improve PRM
• Where to sample new milestones?– Sampling strategy
• Which milestones to connect?– Connection strategy
• Goal: – Minimize roadmap size to correctly answer
motion-planning queries
33
Sampling Strategies
34
small visibility sets
good visibility
poor visibility
Poor Visibility in Narrow Passages
• Non-uniform sampling strategies
35
But how to identify poor visibility regions?
• What is the source of information?– Robot and environment geometry
• How to exploit it?– Workspace-guided strategies– Dilation-based strategies– Filtering strategies– Adaptive strategies
36
Identify narrow passages in the workspace and map them into the configuration space
Workspace-guided strategies
37
F
O
Dilation-based strategies • During roadmap construction, allow milestones
with small penetration• Dilate the free space
– [Hsu et al. 98, Saha et al. 05, Cheng et al. 06, Zhang et al. 07]
A milestone with small penetration
38
Error
• If a path is returned, the answer is always correct.
• If no path is found, the answer may or may not be correct. We hope it is correct with high probability.
39
Weaker Completeness
Complete planner Too slow Heuristic planner Too unreliable
Probabilistic completeness:If a solution path exists, then the probability that the planner will find one is a fast growing function that goes to 1 as the running time increases
40
Kinodynamic Planning
41
RRT for Kinodynamic Systems
• Rapidly-exploring Random Tree
• Randomly select a control input
42
More Examples
• Car pulling trailers (complicated kinematics -- no dynamics)
43
Summary: Sampling-based Motion Planning
+ Efficient in practice + Work for robots with many DOF (high-
dimensional configuration spaces)+ Has been applied for various motion planning
problems (non-holonomic, kinodynamic planning etc.)
- Narrow passages problems (one of the hot areas)
- May not terminate when no path exists
44
Summary
• Configuration space
• Visibility graph
• Approximate cell decomposition – Decompose the free space into simple cells and represent the
connectivity of the free space by the adjacency graph of these cells
• Sampling-based approach– High-dimensional Configuration Spaces– Capture the connectivity of the free space by sampling
45
References
• Books– J.C. Latombe. Robot Motion Planning, 1991.– S.M. LaValle, Planning Algorithms, 2006
• Free book: http://msl.cs.uiuc.edu/planning/
– H. Choset et al. Principles of Robot Motion: Theory, Algorithms, and Implementations, 2005
• Conferences– ICRA: IEEE International Conference on Robotics and
Automation– IROS: IEEE/RSJ International Conference on Intelligent RObots
and Systems– WAFR: Workshop on the Algorithmic Foundations of Robotics