Path Planning in Expansive C-Spaces

1
Path Planning in Expansive C-Spaces
D. Hsu J.-C. Latombe R. Motwani CS Dept.,
Stanford University, 1997
2
What we Want Good Connectivity
For each connected component of the free space,
there should be only one connected component of
the roadmap.
3
What we Want Good Coverage
Given a pre-computed roadmap, it should be easy
to connect new start and goal configurations.
4
Main Result
If the C-space is expansive, then we can build a
roadmap that has both good connectivity and good
coverage.
5
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
6
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.
7
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.
8
Main Result
If the C-space is expansive, then we can build a
roadmap that has both good connectivity and good
coverage.
9
Definition Linking Sequence
Pt1 is chosen from the lookout of the subset of
points seen by p0, p1, , pt.
10
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.
11
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.
12
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.
13
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.
14
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.
qgoal
qinit
15
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
w(x)3
x
16
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
17
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
18
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
19
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
20
The Connection Algorithm
For every x in Tinit and y in Tgoal such that
dist(x,y)ltL do If x and y see each other, then
connect x and y
qgoal
qinit
y
x
Tgoal
Tinit
21
The Connection Algorithm
For every x in Tinit and y in Tgoal such that
dist(x,y)ltL do If x and y see each other, then
connect x and y
qgoal
qinit
y
x
Tgoal
Tinit
22
The Connection Algorithm
For every x in Tinit and y in Tgoal such that
dist(x,y)ltL do If x and y see each other, then
connect x and y
qgoal
qinit
y
x
Tgoal
Tinit
23
Example
24
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