Single Robot Motion Planning II

1
Single Robot Motion Planning - II

• Liang-Jun Zhang
• COMP790-058
• Sep 24, 2008

2
Review C-space
Workspace
Configuration Space
Goal
C-obstacle
Obstacle
Free
Robot
y
x
Initial
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
Approximate Cell Decomposition
Configuration Space

• Full cell
• Empty cell
• Mixed cell
• Mixed
• Uncertain
• Cell labelling algorithms
• Zhang et al 06

full
mixed
7
Finding a Path by ACD
Initial
Goal
8
Connectivity Graph
Gf Free Connectivity Graph
Gf is a subgraph of G
9
Finding a Path by ACD
Initial
Goal
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
L Guiding Path
Only subdivide the cells along L
new Gf
12
Finding a Path by ACD
Gf

• A channel
• Can be used for path smoothing.

13
ACD for Path Non-existence
Initial
Goal
C-space
14
ACD for Path Non-existence
Connectivity Graph
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
Initial
Goal
roadmap in free space
Video
18
Two-gear Example
Video
no path!
3.356s
Initial
Cells in C-obstacle
Roadmap in F
Goal
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
Kavraki, Svetska, Latombe,Overmars, 95
24
Basic PRM Aalgorithm
Input geometry of the moving object
obstacles Output roadmap G (V, E) 1 V ? ?
and E ? ?. 2 repeat 3 q ? a configuration
sampled uniformly at random from C. 4 if
CLEAR(q)then 5 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)then 8 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
  checkingHierarchical 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
  checkingHierarchical 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
qinit
Alpha puzzle
qgoal
Configuration Space
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
  answermotion-planning queries

33
Sampling Strategies
34
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
Workspace-guided strategies

• Identify narrow passages in the workspace and map
  them into the configuration space

37
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

F
O
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 completenessIf 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