Path Planning in Expansive C-Spaces D. HsuJ.-C. LatombeR. 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. q A is 1-good q B 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 P t+1 is chosen from the lookout of the subset of points seen by p 0, p 1, …, p t.

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 q init and q goal, 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. q init q goal

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. x w(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 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 x T

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 x T

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 x T y

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 x T y

The Connection Algorithm For every x in T init and y in T goal such that dist(x,y)<L do: If x and y see each other, then connect x and y q init q goal T init T goal x y

The Connection Algorithm For every x in T init and y in T goal such that dist(x,y)<L do: If x and y see each other, then connect x and y q init q goal T init T goal x y

The Connection Algorithm For every x in T init and y in T goal such that dist(x,y)<L do: If x and y see each other, then connect x and y q init q goal T init T goal x y

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