Path Planning in Expansive C-Path Planning in Expansive C-SpacesSpaces

D. Hsu J.-C. Latombe R. Motwani

CS Dept., Stanford University, 1997

What we Want: Good Connectivity

For each connected component of the free space, there should beonly one connected component of the roadmap.

What we Want: Good Coverage

Given a pre-computed roadmap, it should be easy to connect newstart and goal configurations.

Main Result

If the C-space is expansive, then we can build a roadmapthat has both good connectivity and good coverage.

Definition - -goodness

A free configuration q is -good if it sees an -fraction of thevolume of the free space F.

F is -good if every free configuration is -good.

qA is 1-good

qB is ½-good

F is ½-good

Definition - Lookout of a Subset

The -lookout of a subset S of F is the subset of points of S that seea -fraction of the volume of F\S.

Definition - Expansiveness

A free space F is expansive if every subset S of F has a large lookout.

More formally:The free space F is (, , )-expansive if:1. F is -good2. For every subset S of F, the volume of a -lookout of S is an

-fraction of the volume of S.

Main Result

If the C-space is expansive, then we can build a roadmapthat has both good connectivity and good coverage.

Definition: Linking Sequence

Pt+1 is chosen from the lookout of the subset of points seen byp0, p1, …, pt.

Linking Sequences in Expansive Spaces

Any milestone of a roadmap is likely to have a linking sequence ofarbitrary length t, provided the roadmap is big enough.

For large t, the linking sequence of any milestone spans a large fraction of the volume of F.

Hence, the intersection of two linking sequences is likely to containa milestone of the roadmap.

Theorem 1: Roadmap Connectivity

The probability that a roadmap fails to achieve good connectivityin an expansive space decreases exponentially with the number of milestones.

Theorem 2: Roadmap Coverage

The probability that a roadmap fails to achieve good coveragein an expansive space decreases exponentially with the number ofmilestones.

In Practice…

How to build linking sequences ?

Problem: lookouts cannot be easily computed.

However, we know that lookouts occupy a large fraction of the free space.

Hence, linking sequences can be found by sampling uniformly at random, and by keeping only those points that see a large portion of the free space.

The New Planner

We grow two trees from qinit and qgoal, respectively.

New nodes are selected by sampling uniformly at random around thealready existing nodes.

We incorporate the nodes that are most likely to see a large portionof the free space.

A path is found when the two trees can be connected.

qinitqgoal

The Weight Function

We incorporate the nodes that are most likely to see a large portionof the free space.

For each node x of the tree T, w(x) is equal to the number of samplednodes of T that lie in a fixed neighborhood of x.

Selecting nodes with probability 1/w(x) ensures the tree spreads uniformly in the free space.

xw(x)=3

T

The Expansion Algorithm

Pick a node x from T with probability 1/w(x)Sample K points from a fixed neighborhood of xFor each sampled configuration y, retain y with probability 1/w(y)If:

1. y is retained2. y has no collision3. x and y see each other

Then, place an edge between x and y

x

T

The Expansion Algorithm

Pick a node x from T with probability 1/w(x)Sample K points from a fixed neighborhood of xFor each sampled configuration y, retain y with probability 1/w(y)If:

1. y is retained2. y has no collision3. x and y see each other

Then, place an edge between x and y

x

T

The Expansion Algorithm

Pick a node x from T with probability 1/w(x)Sample K points from a fixed neighborhood of xFor each sampled configuration y, retain y with probability 1/w(y)If:

1. y is retained2. y has no collision3. x and y see each other

Then, place an edge between x and y

x

Ty

The Expansion Algorithm

Pick a node x from T with probability 1/w(x)Sample K points from a fixed neighborhood of xFor each sampled configuration y, retain y with probability 1/w(y)If:

1. y is retained2. y has no collision3. x and y see each other

Then, place an edge between x and y

x

Ty

The Connection Algorithm

For every x in Tinit and y in Tgoal such that dist(x,y)<L do:If x and y see each other, then connect x and y

qinitqgoal

Tinit Tgoal

xy

The Connection Algorithm

For every x in Tinit and y in Tgoal such that dist(x,y)<L do:If x and y see each other, then connect x and y

qinitqgoal

Tinit Tgoal

xy

The Connection Algorithm

For every x in Tinit and y in Tgoal such that dist(x,y)<L do:If x and y see each other, then connect x and y

qinitqgoal

Tinit Tgoal

xy

Example

Conclusion

If the C-space is expansive, then we can efficiently build a roadmapthat has both good connectivity and good coverage.

Suggested improvements:Find a parametrization of the C-space that maximizes expansivenessApply geometric transforms that increase expansivenessDecompose the free space into expansive components