1 single robot motion planning - ii liang-jun zhang comp790-058 sep 24, 2008
Post on 19-Dec-2015
215 views
TRANSCRIPT
![Page 1: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/1.jpg)
1
Single Robot Motion Planning - II
Liang-Jun ZhangCOMP790-058
Sep 24, 2008
![Page 2: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/2.jpg)
2
Review: C-space
Workspace Configuration Space
xyRobot
Initial
Goal
Free
Obstacle C-obstacle
A 2D Translating Robot
![Page 3: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/3.jpg)
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
![Page 4: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/4.jpg)
4
Outline
• Approximate cell decomposition
• Sampling-based motion planning
![Page 5: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/5.jpg)
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
![Page 6: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/6.jpg)
6
full mixed
empty
Approximate Cell Decomposition
• Full cell
• Empty cell
• Mixed cell– Mixed– Uncertain
• Cell labelling algorithms– [Zhang et al 06]
Configuration Space
![Page 7: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/7.jpg)
7
Finding a Path by ACD
Goal
Initial
![Page 8: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/8.jpg)
8
Connectivity GraphGf : Free Connectivity Graph G: Connectivity Graph
Gf is a subgraph of G
![Page 9: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/9.jpg)
9
Finding a Path by ACD
Goal
Initial Gf : Free Connectivity Graph
![Page 10: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/10.jpg)
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
![Page 11: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/11.jpg)
11
First Graph Cut Algorithm
Only subdivide the cells along L
L : Guiding Path
new Gf
![Page 12: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/12.jpg)
12
Finding a Path by ACDGf
• A channel
• Can be used for path smoothing.
![Page 13: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/13.jpg)
13
ACD for Path Non-existence
C-space
Goal
Initial
![Page 14: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/14.jpg)
14
Connectivity Graph Guiding Path
ACD for Path Non-existence
![Page 15: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/15.jpg)
15
ACD for Path Non-existence
Connectivity graph is not connected
No path!
A sufficient condition for deciding path non-existence
![Page 16: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/16.jpg)
16
• Live Demo– Gear-2DOF– Gear-3DOF
![Page 17: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/17.jpg)
17
Five-gear Example
Video
Initial
Goal
roadmap in free space
Total timing 85s
# of total cells 168K
![Page 18: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/18.jpg)
18
Two-gear Example
no path!
Cells in C-obstacle
Initial
Goal
Roadmap in F
Video 3.356s
![Page 19: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/19.jpg)
19
Motion Planning Framework
• Continuous representation
• Discretization
• Graph search
![Page 20: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/20.jpg)
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
![Page 21: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/21.jpg)
21
Outline
• Approximate cell decomposition
• Sampling-based motion planning
![Page 22: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/22.jpg)
22
Motivation
• Geometric complexity• Space dimensionality
![Page 23: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/23.jpg)
23
Probabilistic Roadmap (PRM)
free space
qqinitinit
qqgoalgoal
milestone
[Kavraki, Svetska, Latombe,Overmars, 95]
local path
![Page 24: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/24.jpg)
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.
![Page 25: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/25.jpg)
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?
![Page 26: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/26.jpg)
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
![Page 27: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/27.jpg)
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)
![Page 28: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/28.jpg)
28
Why does it work? Intuition• A small number of milestones almost
“cover” the entire free space.
![Page 29: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/29.jpg)
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)
![Page 30: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/30.jpg)
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
![Page 31: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/31.jpg)
31
Difficulty• Many small connected components
![Page 32: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/32.jpg)
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
![Page 33: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/33.jpg)
33
Sampling Strategies
![Page 34: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/34.jpg)
34
small visibility sets
good visibility
poor visibility
Poor Visibility in Narrow Passages
• Non-uniform sampling strategies
![Page 35: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/35.jpg)
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
![Page 36: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/36.jpg)
36
Identify narrow passages in the workspace and map them into the configuration space
Workspace-guided strategies
![Page 37: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/37.jpg)
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
![Page 38: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/38.jpg)
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.
![Page 39: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/39.jpg)
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
![Page 40: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/40.jpg)
40
Kinodynamic Planning
![Page 41: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/41.jpg)
41
RRT for Kinodynamic Systems
• Rapidly-exploring Random Tree
• Randomly select a control input
![Page 42: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/42.jpg)
42
More Examples
• Car pulling trailers (complicated kinematics -- no dynamics)
![Page 43: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/43.jpg)
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
![Page 44: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/44.jpg)
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
![Page 45: 1 Single Robot Motion Planning - II Liang-Jun Zhang COMP790-058 Sep 24, 2008](https://reader035.vdocuments.mx/reader035/viewer/2022062714/56649d405503460f94a19d39/html5/thumbnails/45.jpg)
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