Transcript Our goals - Stanford University

Path Planning in Expansive C-Spaces
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 be
only one connected component of the roadmap.
What we Want: Good Coverage
Given a pre-computed roadmap, it should be easy to connect new
start and goal configurations.
Main Result
If the C-space is expansive, then we can build a roadmap
that has both good connectivity and good coverage.
Definition - -goodness
A free configuration q is -good if it sees an -fraction of the
volume 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 see
a -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 -good
2.
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 roadmap
that has both good connectivity and good coverage.
Definition: Linking Sequence
Pt+1 is chosen from the lookout of the subset of points seen by
p0, p1, …, pt.
Linking Sequences in Expansive Spaces
Any milestone of a roadmap is likely to have a linking sequence of
arbitrary 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 contain
a milestone of the roadmap.
Theorem 1: Roadmap Connectivity
The probability that a roadmap fails to achieve good connectivity
in an expansive space decreases exponentially with the number of
milestones.
Theorem 2: Roadmap Coverage
The probability that a roadmap fails to achieve good coverage
in an expansive space decreases exponentially with the number of
milestones.
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 the
already existing nodes.
We incorporate the nodes that are most likely to see a large portion
of the free space.
A path is found when the two trees can be connected.
qinit
qgoal
The Weight Function
We incorporate the nodes that are most likely to see a large portion
of the free space.
For each node x of the tree T, w(x) is equal to the number of sampled
nodes 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.
T
x
w(x)=3
The Expansion Algorithm
Pick a node x from T with probability 1/w(x)
Sample K points from a fixed neighborhood of x
For each sampled configuration y, retain y with probability 1/w(y)
If:
1.
y is retained
2.
y has no collision
3.
x and y see each other
Then, place an edge between x and y
T
x
The Expansion Algorithm
Pick a node x from T with probability 1/w(x)
Sample K points from a fixed neighborhood of x
For each sampled configuration y, retain y with probability 1/w(y)
If:
1.
y is retained
2.
y has no collision
3.
x and y see each other
Then, place an edge between x and y
T
x
The Expansion Algorithm
Pick a node x from T with probability 1/w(x)
Sample K points from a fixed neighborhood of x
For each sampled configuration y, retain y with probability 1/w(y)
If:
1.
y is retained
2.
y has no collision
3.
x and y see each other
Then, place an edge between x and y
y
T
x
The Expansion Algorithm
Pick a node x from T with probability 1/w(x)
Sample K points from a fixed neighborhood of x
For each sampled configuration y, retain y with probability 1/w(y)
If:
1.
y is retained
2.
y has no collision
3.
x and y see each other
Then, place an edge between x and y
y
T
x
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
qgoal
qinit
Tinit
x
y
Tgoal
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
qgoal
qinit
Tinit
x
y
Tgoal
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
qgoal
qinit
Tinit
x
y
Tgoal
Example
Conclusion
If the C-space is expansive, then we can efficiently build a roadmap
that has both good connectivity and good coverage.
Suggested improvements:
Find a parametrization of the C-space that maximizes expansiveness
Apply geometric transforms that increase expansiveness
Decompose the free space into expansive components