Dynamic-Domain RRTs

Anna Yershova, Steven M. LaValle

03/08/2006

Basic Motion Planning Problem

Given: 2D or 3D world Geometric models of obstacles Geometric models Configuration space Initial and goal configurations

Task: Compute a collision free path that connects initial and goal

configurations

Rapidly-exploring Random Trees (RRTs)

Introduced by LaValle and Kuffner, ICRA 1999.

Applied, adapted, and extended in many works: Frazzoli, Dahleh, Feron, 2000; Toussaint, Basar, Bullo, 2000; Vallejo, Jones, Amato, 2000; Strady, Laumond, 2000; Mayeux, Simeon, 2000; Karatas, Bullo, 2001; Li, Chang, 2001; Kuffner, Nishiwaki, Kagami, Inaba, Inoue, 2000, 2001; Williams, Kim, Hofbaur, How, Kennell, Loy, Ragno, Stedl, Walcott, 2001; Carpin, Pagello, 2002; Branicky, Curtiss, 2002; Cortes, Simeon, 2004; Urmson, Simmons, 2003; Yamane, Kuffner, Hodgins, 2004; Strandberg, 2004; ...

Also, applications to biology, computational geography, verification, virtual prototyping, architecture, solar sailing, computer graphics, ...

The RRT Construction Algorithm

GENERATE_RRT(xinit, K, t)

1. T.init(xinit);

2. For k = 1 to K do

3. xrand RANDOM_STATE();

4. xnear NEAREST_NEIGHBOR(xrand, T);

5. if CONNECT(T, xrand, xnear, xnew);

6. T.add_vertex(xnew);

7. T.add_edge(xnear, xnew, u);

8. Return T;

xnear

xinit

xnew

The result is a tree rooted at xinit

A Rapidly-exploring Random Tree (RRT)

Voronoi Biased Exploration

Is this always a good idea?

Voronoi Diagram in R 2

Voronoi Diagram in R 2

Voronoi Diagram in R 2

Refinement vs. Expansion

refinement expansion

Where will the random sample fall? How to control the behavior of RRT?

Limit Case: Pure Expansion

Let X be an n-dimensonal ball,

in which r is very large.

The RRT will explore n 1 opposite directions.

The principle directions are vertices of a regular n 1-simplex

Determining the Boundary

Expansion dominates Balanced refinement and expansion

The tradeoff depends on the size of the bounding box

Controlling the Voronoi Bias

Refinement is good when multiresolution search is needed

Expansion is good when the tree can grow and not blocked by obstacles

Main motivation:

Voronoi bias does not take into account obstacles

How to incorporate the obstacles into Voronoi bias?

Bug Trap

Which one will perform better?

Small Bounding Box Large Bounding Box

Voronoi Bias for the Original RRT

Visibility-Based Clipping of the Voronoi Regions

Nice idea, but how can this be done in practice?Even better: Voronoi diagram for obstacle-based metric

(a) Regular RRT, unbounded Voronoi region(b) Visibility region(c) Dynamic domain

A Boundary Node

A Non-Boundary Node

(a) Regular RRT, unbounded Voronoi region(b) Visibility region(c) Dynamic domain

Dynamic-Domain RRT Bias

Dynamic-Domain RRT Construction

Dynamic-Domain RRT Bias

Tradeoff between nearest neighbor calls and collision detection calls

Recent Efforts

Adaptive tuning of the radius: the radius is not fixed but is increased with every extension

success and is decreased with every failure

Nearest neighbor calls: kd-tree based implementation O(log n) instead of naïve O(n) query time

Uniform sampling from dynamic domain: Rejection-based method is not efficient for high dimensions Uniform distribution should be generated directly

Adaptive Tuning of Parameter

Adaptive Tuning of Parameter

Nearest Neighbor Calls & Uniform Sampling

Efficient implementation using kd-trees

O(log n) query time instead of naïve O(n) query time

The kd-tree is a powerful data structure that is based on recursively subdividing a set of points with alternating axis-aligned hyperplanes.

The classical kd-tree uses O(dn lgn) precomputation time, O(dn) space and answers queries in time logarithmic in n, but exponential in d.

47

6

5

1

3

2

9

8

10

11

l5

l1 l9

l6

l3

l10 l7

l4

l8

l2

l1

l8

1

l2l3

l4 l5 l7 l6

l9l10

3

2 5 4 11

9 10

8

6 7

KD-trees

Kd-trees. Construction

47

6

5

1

3

2

9

8

10

11

l5

l1 l9

l6

l3

l10 l7

l4

l8

l2

l1

l8

1

l2l3

l4 l5 l7 l6

l9l10

3

2 5 4 11

9 10

8

6 7

Kd-trees. Query

47

6

5

1

3

2

9

8

10

11

l5

l1 l9

l6

l3

l10 l7

l4

l8

l2

l1

l8

1

l2l3

l4 l5 l7 l6

l9l10

3

2 5 4 11

9 10

8

6 7

q

Shrinking Bug Trap

Large Medium Small

The smaller the bug trap, the better the improvement

Shrinking Bug Trap

Wiper Motor (courtesy of KINEO)

6 dof problem CD calls are

expensive

Molecule

68 dof problem was solved in 2 minutes 330 dof in 1 hour 6 dof in 1 min. 30 times improvement comparing to RRT CD calls are expensive

Labyrinth

3 dof problem CD calls are not

expensive

3D grid

6 dof problem CD calls are not expensive

Spiral

6 dof problem CD calls are not expensive

Conclusions

Controlling Voronoi bias is important in RRTs. Provides dramatic performance improvements on some

problems. Does not incur much penalty for unsuitable problems.

Work in Progress:

Application to planning under differential constraints.

Application to planning for closed chains.