Randomized Approaches to
Motion-Planning Problem
Hennadiy Leontyev
The University of North Carolina at Chapel Hill
Talk Outline
•
•
•
•
•
Brief review of path-planning.
Rapidly-growing Random Trees.
Randomized Roadmaps.
Ariadne’s Clew algorithm.
Conclusion.
Configuration Space Model
• Path planning is usually
performed in
configuration space C.
• Path is a subset of
Cfree.
• Explicit representation
of free space Cfree is
not usually available.
• We can test whether
certain configuration q
is in Cfree.
Real World
Start
Finish
Configuration Space is
a bizarre 3D manifold R2xS!
?
Aspects of Path-Planning
•
•
•
•
Dynamic or static?
Single-query or multiple-query?
How fast should it be?
Randomized or complete?
Talk Outline
•
•
•
•
•
Brief review of path-planning.
Rapidly-growing Random Trees [1].
Randomized Roadmaps [2].
Ariadne’s Clew algorithm [3].
Conclusion.
RRT-Basic (the algorithm)
•
BUILD-RRT constructs a tree of
sample points in Cfree, which is
rooted at initial configuration.
•
The procedure RandomConfig
returns a random point from Cfree
•
The procedure EXDEND spans the
tree as much as possible to given
direction.
EXTEND(T,q)
qnear=NEAREST-NEIGHBOR(T,q)
The procedure
If NEW-CONFIG(q,qnear,qnew) Then
NEW-CONFIG is a local movement
T.addVertex(qnew)
planning function.
T.addEdge(qnear,qnew)
If qnew=q Then return Reached
Else return Advanced
•
BUILD-RRT(qinit)
T.init(qinit)
for i=1 to K do
qnext=RandomConfig()
EXTEND(T,qnext)
return T.
RRT-Basic (in action)
e
qnear
q
qnew
qinit
Configurations in the tree
Goal configuration
An obstacle
EXTEND advances to the distance at most e
RRT-Connect (the algorithm)
• Extends RRT-basic.
• Uses two trees Ti
and Tg to find the
path.
• Aggressive tree
expansion.
CONNECT(T,q)
repeat
S=EXTEND(T,q)
until S!=Advanced
RRT-CONNECT(qinit,qgoal)
Ti.init(qinit); Tg.init(qgoal)
for j:=1 to K do
qrand :=RANDOM-CONFIG()
If EXTEND(Ti, qrand)!=Trapped Then
If CONNECT(Tg,qnew)=Reached Then
return PATH(Ti,Tg)
SWAP(Ti,Tg)
return Failure
RRT-Connect (in action)
Tg
qrand
qnew
qinit
qgoal
Ti
Goal configuration
Configurations in the tree Tg
Configurations in the tree Ti
Initial configuration
RRT-Connect (performance)
In RRT-Basic search is biased
by large Voronoi regions. The tree
covers unexplored space rapidly.
Two search trees of RRT-Connect
are growing towards each other
RRT-Connect (performance)
• 270 MHz Silicon O2
workstation.
• ~1 sec. to find a solution
for examples on the
right.
• Grand Piano moving
problem (4500
triangles) was solved in
12 sec.
• Virtual chess player arm
modelled as 7-DOF
kinematic chain and
8000 triangles required
2 sec to move a figure.
RRT-Connect (conclusion)
• Does not require parameter tuning.
• Preprocessing is not required.
• Simple and consistent behavior is
observed.
• The method is well-suited for
incremental distance computation and
nearest-neighbor algorithms.
Talk Outline
•
•
•
•
•
Brief review of path-planning.
Rapidly-growing Random Trees [1].
Randomized Roadmaps [2].
Ariadne’s Clew algorithm [3].
Conclusion.
Randomized Roadmaps
(the algorithm)
• Planning is also
performed in C-space.
• Sample points are
generated on obstacle
surfaces δCfree
(collision detection
algorithms are heavily
used) .
• These sample points are
then connected to a
roadmap.
S={si}-obstacles in real-world
S’={si’}-obstacles in C-space
Vi – candidate nodes
V - final set of nodes
For each object si’ in S’ do
Vi:=GenerateRandomPoints(si’)
V:=V+Vi
For each p in V do
For each sj’ in S’ do
If sj’.contains(p) Then
V:=V-p
BUILD-ROADMAP(V)
Randomized Roadmaps (sampling)
• Find an origin point o
inside the object.
• Generate m uniformly
distributed directions.
• Use binary search to
identify boundaries.
+ Rather simple
implementation.
- Not good for “thin”
objects.
o
Candidate points
Object boundaries
Randomized Roadmaps (improvements)
• How do we select a center?
• How do we deal with “thin” objects?
• What happens if the object has folds?
Randomized Roadmaps
(selecting object center)
Random origin is bad:
the distance between
sample points is uneven.
Using the center of masses
as an origin within the object
d1
d1
o
o
d2
d2
Center point
Object boundaries
Bad news: Needs more time for
preprocessing and fast algorithms.
Randomized Roadmaps
(dealing with “thin” objects)
For “thin” objects no selection of
origin will give good distribution.
Use auxiliary points to cover spaces.
d1
d2
o
d1
d2
o
Auxiliary points
Sample points
Object boundaries
Origin
Bad news: Requires knowledge of
object topology and more
preprocessing time.
Randomized Roadmaps
(dealing with foldings)
If we know that the object “folds” this does not
help to avoid complex computations.
o
Sample points
Object boundaries
Origin
Randomized Roadmaps
(building the roadmap)
• Connect samples from
different objects using
a local planner.
– How accurate the map
should be?
– What resources do we
have to build the map?
– This affects the number
of connections (usually a
tradeoff).
• Identify connected
components.
• Build “corridors” that
determine collision-free
paths in C-space
Boundary points
Collision-free route
Randomized Roadmaps (planning)
• Connect start and
finish nodes with a
roadmap by simple
or random planner
• Identify the path
between roadmap
points
• Build the full path.
q0 – start configuration
Boundary points
qgoal – goal
Collision-free route
Randomized Roadmaps (performance)
E1 – five objects
E2-5 – two objects
with a narrow passage
• 100 MHz MIPS R4600.
• 6-DOF articulated
robot with fixed base.
Time to connect configurations is near 1/100 of a second.
Randomized Roadmaps (conclusion)
• Multiple planning queries could be served
quickly.
• The preprocessing could be parallelized.
• The complexity of the map depends on the
number of obstacles (may be huge for
cluttered environments).
• Significant amount of preprocessing is
required.
• Taking good sample points is a non-trivial task.
Talk Outline
•
•
•
•
•
Short definitions of path-planning.
Rapidly-growing Random Trees [1].
Randomized Roadmaps [2].
Ariadne’s Clew algorithm [3].
Conclusion.
Ariadne’s Clew (the algorithm)
•
•
Search is performed in trajectory
space (a1,r1,a2,r2,…al,rl) which leads
to ql – this path has k*l steps in it,
where k is the number of DOF.
r1
Exists a distance function d(ql,qgoal) .
•
The combination of two routines:
SEARCH and EXPLORE
•
SEARCH attempts to find a
configuration that minimizes the
distance to the goal.
•
q0
EXPLORE attempts to approximate
free space by a set of landmarks to
which the path of l steps is known.
a1
r2 a2
r3
a3
al
q0 Initial configuration
ql
ai
Final configuration
Rotation angle
ri
Distance in the world
ql
Ariadne’s Clew (search)
Attempts to find a path of l steps length
that minimizes distance minRl d(ql,qgoal) to the goal.
q0
qk
qj qj
ql
qgoal
Goal configuration
Non-mins
Min distance configuration
minl d(ql,qgoal)
+Tries to find a configuration with minimal distance to the goal.
-- Could be trapped in local minima
-- Length of trajectories may be too small
Ariadne’s Clew (explore)
• Path from initial position to
any landmark is known.
• Landmarks are spread
uniformly in C.
• The procedure finds a
configuration which
maximizes the distance and
path no longer than l.
maxl d(ql,L)
ql
+ Approximates free space by a
set of landmarks
--Knows nothing about the goal
New landmark
Previously set landmarks L
Ariadne’s Clew (in action)
Landmark (given by EXPLORE)
Config found by SEARCH
Goal configuration
Initial config
ALGORITHM-ARIADNE(q0,qgoal,r)
i:=1; λi=q0
Li={λi};e=infinity
while(e>r)
/* Run local planner SEARCH */
if( minl d(qgoal,ql)=0) Then
return Path
else
/* Place new landmark with
EXPLORE*/
i:=i+1
λi:=q:supl d(Li-1,ql)
Li:=Li-1+ {λi}
e=d(Li-1, λi)
L=Li
return NO PATH
Ariadne’s Clew (performance)
• Two 6-DOF arms are used. One arm is a robot,
the other performs random movements.
• Meganode of 128 T800 (25MHz) transputers is
used to make planning.
• Parallel genetic algorithm implementation.
– Trajectories are Manhattan paths of order 2. R12 for
SEARCH and (i,R12) for EXPLORE.
– Each DOF is discretized with 9 bits.
– Max number of landmarks 256.
– All possible data is encoded with 119 bit strings.
1. Generate a random movement
for Robot B
2. Send the position of B and
desired position of A to the
Meganode.
3. Get planned path for A from
the Meganode and execute it.
4. Wait for a random time and
stop A, goto 1.
The mean time to compute a single path for the new environment is
1.5 sec.
Ariadne’s Clew (conclusion)
• SEARCH and EXPLORE could work in parallel
over the same set of landmarks.
• Thus the performance of the algorithm is
improved greatly by using parallel search and
collision-detection.
• Constraints might not be only obstacles but
rather arbitrary constraints on paths.
• Experiments showed that the algorithm
performs well in realistic dynamic
environments.
Conclusion
RRT
Randomized
Roadmaps
Ariadne’s Clew
Number of
queries
Single
Multiple
Single
Preprocessing
No
Yes
No
Is dynamic
Barely
No
Yes
Implementation
complexity
Low
High
Medium
Parallelizable
No
Yes
Yes
References
• [1] Kuffner J.J. Jr. and LaValle S.M. “RRT-connect: An efficient
approach to single-query path planning.” In Proceedings of the ’00.
IEEE International Conference on Robotics and Automation, vol. 2
pages 995-1001, April 2000.
• [2] Amato N.M and Wu Y. “A randomized roadmap method for path
and manipulation planning.”, In Proceedings of the 1996
International Conference on Robotics and Automation, vol. 1, pages
113—120, April 1996.
• [3] Jean Manuel Ahuactzin, Emmanuel Mazer and Pierre Bessiere
“The ariadne’s clew algorithm.”, Journal of Artificial Intelligence
Research, 9, 1998.
• Steven LaValle. “Planning Algorithms.”, Cambridge University Press,
2006.