Sampling and Searching

Methods for Practical Motion

Planning Algorithms

Anna Yershova

Dept. of Computer Science

University of Illinois

Presentation Overview Motion Planning Problem

Basic Motion Planning Problem Extensions of Basic Motion Planning Motion Planning under Differential Constraints

State of the Art

Research Statement

Technical Approach Efficient Nearest Neighbor Searching Guided Sampling for Efficient Exploration Uniform Deterministic Sampling Methods Motion Primitives Generation

Conclusions and Discussion

Given: (geometric model of a robot) (space of configurations, q, that

are applicable to ) (the set of collision free

configurations) Initial and goal configurations

Task: Compute a collision free path that connects initial and

goal configurations

Basic Motion Planning Problem ”Moving Pianos”

Given:

, , (kinematic closure

constraints) Initial and goal configurations

Task: Compute a collision free path that connects initial and

goal configurations

Extensions of Basic Motion Planning Problem

Given: , , State space X Input space U state transition

equation Initial and goal states

Task: Compute a collision free path that connects initial and

goal states

Motion Planning Problemunder Differential Constraints

Presentation Overview Motion Planning Problem

Basic Motion Planning Problem Extensions of Basic Motion Planning Motion Planning under Differential Constraints

State of the Art

Thesis Statement

Technical Approach Efficient Nearest Neighbor Searching Uniform Deterministic Sampling Methods Guided Sampling for Efficient Exploration Motion Primitives Generation

Conclusions and Discussion

History of Motion Planning Grid Sampling, AI Search (beginning of time-1977)

Experimental mobile robotics, etc.

Problem Formalization (1977-1983) PSPACE-hardness (Reif, 1979) Configuration space (Lozano-Perez, 1981)

Combinatorial Solutions (1983-1988) Cylindrical algebraic decomposition (Schwartz, Sharir, 1983) Stratifications, roadmap (Canny, 1987)

Sampling-based Planning (1988-present) Randomized potential fields (Barraquand, Latombe, 1989) Ariadne's clew algorithm (Ahuactzin, Mazer, 1992) Probabilistic Roadmaps (PRMs) (Kavraki, Svestka, Latombe, Overmars,

1994) Rapidly-exploring Random Trees (RRTs) (LaValle, Kuffner, 1998)

Applications of Motion Planning

Manipulation Planning

Computational Chemistryand Biology

Medical applications

Computer Graphics(motions for digital actors)

Autonomous vehicles and spacecrafts

Sampling and Searching Framework

Build a graph over the state (configuration) space that connects initial state to the goal:

INITIALIZATION

SELECTION METHOD

LOCAL PLANNING METHOD

INSERT AN EDGE IN THE GRAPH

CHECK FOR SOLUTION

RETURN TO STEP 2

xbest

xinit

xnew

Research Statement

The performance of motion planning algorithms can be significantly improved by careful consideration of sampling issues.

ADDRESSED ISSUES:

STEP 2: nearest neighbor computation

STEP 2: uniform sampling over configuration space

STEPS 2,3: guided sampling for exploration

STEP 3: motion primitives generation

Nearest Neighbor Searching for Motion Planning

Software: http://msl.cs.uiuc.edu/~yershova/sampling/sampling.tar.gz

Problem FormulationGiven a d-dimensional manifold, T, and a set of data points in T.

Preprocess these points so that, for any query point q T, the nearest data point to q can be found quickly.

The manifolds of interest: Euclidean one-space, represented by (0,1) R . Circle, represented by [0,1], in which 0 1 by identification. P3, represented by S3 with antipodal points identified.

Examples of topological spaces:

cylinder torus projective plane

Example: a torus

47

6

5

1

3

2

9

8

10

11

q

47

6

5

1

3

2

9

8

10

11

47

6

5

1

3

2

9

8

10

11

47

6

5

1

3

2

9

8

10

11

47

6

5

1

3

2

9

8

10

11

47

6

5

1

3

2

9

8

10

11

47

6

5

1

3

2

9

8

10

11

47

6

5

1

3

2

9

8

10

11

47

6

5

1

3

2

9

8

10

11

Kd-trees

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

Dynamic-Domain RRTs

Bug Trap

Which one will perform better?

Small Bounding Box Large Bounding Box

Voronoi Bias for the Original RRT

KD-Tree Bias for the RRT

KD-Tree Bias for the RRT

KD-Tree Bias for the RRT

Library For Generating Deterministic

Sequences Of Samples Over SO(3)

Software: http://msl.cs.uiuc.edu/~yershova/sampling/sampling.tar.gz

A Spectrum of Roadmaps Random Samples Halton sequence

Hammersley Points Lattice Grid

Questions

What uniformity criteria are best suited for Motion Planning

Which of the roadmaps alone the spectrum is best suited for Motion Planning?

Measuring the (Lack of) Quality Let R (range space) denote a collection of subsets of a

sphere Discrepancy: “maximum volume estimation error over

all boxes”

Measuring the (Lack of) Quality Let denote metric on a sphere Dispersion: “radius of the largest empty ball”

The Goal for Motion Planning

We want to develop sampling schemes with the following properties:

uniform (low dispersion or discrepancy) lattice structure incremental quality (it should be a sequence) on the configuration spaces with different topologies

Layered Sukharev Grid Sequencein d

Places Sukharev grids one resolution at a time

Achieves low dispersion at each resolution

Achieves low discrepancy

Has explicit neighborhoodstructure

[Lindemann, LaValle 2003]

Layered Sukharev Grid Sequence for Spheres

Take a Layered Sukharev Grid sequence inside each face Define the ordering on faces Combine these two into a sequence on the sphere

Ordering on faces +Ordering inside faces

Motion Primitives Generation

Reachability graph

Dubin’s Car Reachability Graph

Motion Primitives Generation Numerical integration can be costly for complex control

models.

In several works it has been demonstrated that the performance of motion planning algorithms can be improved by orders of magnitude by having good motion primitives

Motion Primitives Generation Motivating example 1:

Autonomous Behaviors for Interactive Vehicle Animations

Jared Go, Thuc D. Vu, James J. Kuffner

Generated spacecraft trajectories in a field of moving asteroid obstacles.

Motion Primitives Generation

Criteria: Hand-picked “pleasing to the eye” trajectories Efficient performance of the online planner

Motion Primitives Generation Motivating example 2:

Optimal, Smooth, Nonholonomic Mobile Robot Motion Planning in State Lattices

M. Pivtoraiko, R.A. Knepper, and A. Kelly

Motion Primitives Generation

The controls are chosen to reach the points on the state lattice

Criteria: Well separated

trajectories Efficiency in

performance

Motivational Literature

Robotics literature:

[Kehoe, Watkins, Lind 2006] [Anderson, Srinivasa 2006] [Pivtoraiko, Knepper, Kelly 2006] [Green, Kelly 2006] [Go, Vu, Kuffner 2004] [Frazzoli, Dahleh, Feron 2001]

Motion Capture literature

[Laumond, Hicheur, Berthoz 2005] [Gleicher]

Proposed problem

Formulate the criteria of “goodness” for motion primitives in the context of Motion Planning

Automatically generate the motion primitives

Propose Efficient Motion Planning algorithms using the motion primitives

Things to investigate:

Dispersion, discrepancy in state space? In trajectory space? Robustness with respect to the obstacles? Complexity of the set of trajectories? Is it extendable to second order systems?

Thank you!