Randomized Approaches to MotionPlanning Problem

1
Randomized Approaches to Motion-Planning Problem
Hennadiy Leontyev The University of North
Carolina at Chapel Hill
2
Talk Outline

• Brief review of path-planning.
• Rapidly-growing Random Trees.
• Randomized Roadmaps.
• Ariadnes Clew algorithm.
• Conclusion.

3
Configuration Space Model
Real World

• 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.

Start
Finish
Configuration Space is a bizarre 3D manifold
R2xS!
?
4
Aspects of Path-Planning

• Dynamic or static?
• Single-query or multiple-query?
• How fast should it be?
• Randomized or complete?

5
Talk Outline

• Brief review of path-planning.
• Rapidly-growing Random Trees 1.
• Randomized Roadmaps 2.
• Ariadnes Clew algorithm 3.
• Conclusion.

6
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.
• The procedure
• NEW-CONFIG is a local movement planning function.

BUILD-RRT(qinit) T.init(qinit) for i1 to
K do qnextRandomConfig()
EXTEND(T,qnext) return T.
EXTEND(T,q) qnearNEAREST-NEIGHBOR(T,q)
If NEW-CONFIG(q,qnear,qnew) Then
T.addVertex(qnew) T.addEdge(qnear,qnew)
If qnewq Then return Reached
Else return Advanced
7
RRT-Basic (in action)
qinit
An obstacle
EXTEND advances to the distance at most e
8
RRT-Connect (the algorithm)
CONNECT(T,q) repeat SEXTEND(T,q)
until S!Advanced

• Extends RRT-basic.
• Uses two trees Ti and Tg to find the path.
• Aggressive tree expansion.

RRT-CONNECT(qinit,qgoal) Ti.init(qinit)
Tg.init(qgoal) for j1 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
9
RRT-Connect (in action)
Tg
qinit
qgoal
Ti
10
RRT-Connect (performance)
Two search trees of RRT-Connect are growing
towards each other
In RRT-Basic search is biased by large Voronoi
regions. The tree covers unexplored space rapidly.
11
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.

12
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.

13
Talk Outline

• Brief review of path-planning.
• Rapidly-growing Random Trees 1.
• Randomized Roadmaps 2.
• Ariadnes Clew algorithm 3.
• Conclusion.

14
Randomized Roadmaps (the algorithm)

• Planning is also performed in C-space.
• Sample points are generated on obstacle surfaces
  dCfree (collision detection algorithms are
  heavily used) .
• These sample points are then connected to a
  roadmap.

Ssi-obstacles in real-world Ssi-obstacles
in C-space Vi candidate nodes V - final set of
nodes
For each object si in S do
ViGenerateRandomPoints(si) VVVi For
each p in V do For each sj in S do
If sj.contains(p) Then VV-p BUILD-ROADMA
P(V)
15
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
16
Randomized Roadmaps (improvements)

• How do we select a center?
• How do we deal with thin objects?
• What happens if the object has folds?

17
Randomized Roadmaps (selecting object center)
Random origin is bad the distance between
sample points is uneven.
Bad news Needs more time for preprocessing and
fast algorithms.
18
Randomized Roadmaps (dealing with thin objects)
For thin objects no selection of origin will
give good distribution.
Sample points
Bad news Requires knowledge of object topology
and more preprocessing time.
Object boundaries
Origin
19
Randomized Roadmaps (dealing with foldings)
If we know that the object folds this does
not help to avoid complex computations.
o
20
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
21
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.

Boundary points
Collision-free route
qgoal goal
22
Randomized Roadmaps (performance)

• 100 MHz MIPS R4600.
• 6-DOF articulated robot with fixed base.

E1 five objects
E2-5 two objects with a narrow passage
Time to connect configurations is near 1/100 of a
second.
23
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.

24
Talk Outline

• Short definitions of path-planning.
• Rapidly-growing Random Trees 1.
• Randomized Roadmaps 2.
• Ariadnes Clew algorithm 3.
• Conclusion.

25
Ariadnes Clew (the algorithm)
q0

• Search is performed in trajectory space
  (a1,r1,a2,r2,al,rl) which leads to ql this
  path has kl steps in it, where k is the number
  of DOF.
• 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.
• EXPLORE attempts to approximate free space by a
  set of landmarks to which the path of l steps is
  known.

a1
r1
a2
r2
r3
a3
al
q0
Initial configuration
ql
ql
Final configuration
ai
Rotation angle
ri
Distance in the world
26
Ariadnes Clew (search)
Attempts to find a path of l steps length that
minimizes distance minRl d(ql,qgoal) to the goal.
q0
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
27
Ariadnes 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.
• Approximates free space by a set of landmarks
• --Knows nothing about the goal

28
Ariadnes Clew (in action)
ALGORITHM-ARIADNE(q0,qgoal,r) i1 ?iq0
Li?ieinfinity while(egtr) / Run
local planner SEARCH / if( minl
d(qgoal,ql)0) Then return Path
else / Place new landmark with
EXPLORE/ ii1
?iqsupl d(Li-1,ql) LiLi-1 ?i
ed(Li-1, ?i) LLi return NO
PATH
Landmark (given by EXPLORE)
Config found by SEARCH
Goal configuration
Initial config
29
Ariadnes 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.

30

• Generate a random movement for Robot B
• Send the position of B and desired position of A
  to the Meganode.
• Get planned path for A from the Meganode and
  execute it.
• 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.
31
Ariadnes 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.

32
Conclusion
33
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
  113120, April 1996.
• 3 Jean Manuel Ahuactzin, Emmanuel Mazer and
  Pierre Bessiere The ariadnes clew algorithm.,
  Journal of Artificial Intelligence Research, 9,
  1998.
• Steven LaValle. Planning Algorithms., Cambridge
  University Press, 2006.