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

Configuration Space is a bizarre 3D manifold R2xS!

?

Finish

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.

• The procedure NEW-CONFIG is a local movement planning function.

BUILD-RRT(qinit) T.init(qinit) for i=1 to K do qnext=RandomConfig() EXTEND(T,qnext) return T.

EXTEND(T,q) qnear=NEAREST-NEIGHBOR(T,q) If NEW-CONFIG(q,qnear,qnew) Then T.addVertex(qnew) T.addEdge(qnear,qnew) If qnew=q Then return Reached Else return Advanced

RRT-Basic (in action)

qinit

An obstacle

qnear q

qnew

Configurations in the tree

Goal configuration

e

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)

qinit

qnew

qgoal

qrand

Ti

Configurations in the tree Ti

Goal configuration

Initial configuration

Configurations in the tree Tg

Tg

RRT-Connect (performance)

In RRT-Basic search is biased by large Voronoi regions. The treecovers unexplored space rapidly.

Two search trees of RRT-Connectare 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-worldS’={si’}-obstacles in C-spaceVi – candidate nodesV - 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-pBUILD-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.

Candidate points

Object boundaries

o

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)

od2

d1

Random origin is bad:the distance between sample points is uneven.

Center point

Object boundaries

Using the center of masses as an origin within the object

o

d2

d1

Bad news: Needs more time for preprocessing and fast algorithms.

Randomized Roadmaps (dealing with “thin” objects)

For “thin” objects no selection oforigin will give good distribution.

od2

d1

Sample points

Object boundaries

Origin

Bad news: Requires knowledge ofobject topology and more preprocessing time.

od2

d1

Auxiliary points

Use auxiliary points to cover spaces.

Randomized Roadmaps (dealing with foldings)

If we know that the object “folds” this does nothelp to avoid complex computations.

Sample points

Object boundaries

Origin

o

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.Boundary points

Collision-free route

q0 – start configuration

qgoal – goal

Randomized Roadmaps (performance)

• 100 MHz MIPS R4600.

• 6-DOF articulated robot with fixed base.

E1 – five objects E2-5 – two objectswith a narrow passage

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.

• 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

a2

a3

al

r1

r2

r3

ql

q0

q0 Initial configuration

ql Final configurationai Rotation angle

ri Distance in the world

Ariadne’s Clew (search)

qj

qgoal

q0

minl d(ql,qgoal)

Goal configuration

Min distance configuration

Non-mins

+Tries to find a configuration with minimal distance to the goal.-- Could be trapped in local minima-- Length of trajectories may be too small

Attempts to find a path of l steps length that minimizes distance minR

l d(ql,qgoal) to the goal.

qk

qj

ql

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.

+ Approximates free space by a set of landmarks

--Knows nothing about the goal

maxl d(ql,L)

New landmark

Previously set landmarks L

ql

Ariadne’s Clew (in action)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

Landmark (given by EXPLORE)

Config found by SEARCH

Initial config

Goal configuration

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.