1. Anna Yershova Dept. of Computer Science University of Illinois
Sampling and Searching Methods for Practical Motion Planning Algorithms
Anna Yershova
Dept. of Computer Science
University of Illinois

2. 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

3. Basic Motion Planning Problem ”Moving Pianos”
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

4. Extensions of Basic Motion Planning Problem
Given:
, ,
(kinematic closure constraints)
Initial and goal configurations
Task:
Compute a collision free path that connects initial and goal configurations

5. Motion Planning Problem under Differential Constraints
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

6. 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

7. History of Motion Planning
Grid Sampling, AI Search (beginning of time-1977)
Experimental mobile robotics, etc.
Problem Formalization ( )
PSPACE-hardness (Reif, 1979)
Configuration space (Lozano-Perez, 1981)
Combinatorial Solutions ( )
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)

8. Applications of Motion Planning
Manipulation Planning
Computational Chemistry and Biology
Medical applications
Computer Graphics (motions for digital actors)
Autonomous vehicles and spacecrafts

9. 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
xnew
xinit

10. 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

11. Nearest Neighbor Searching for Motion Planning
Software:

12. Problem Formulation The manifolds of interest:
Given 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

13. Example: a torus 4 7 6 5 1 3 2 9 8 10 11 4 6 7 q 8 5 9 10 3 1 2 11

14. 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. l1 4 7 6 5 1 3 2 9 8 10 11 l5 l1 l9 l6 l3
l10 l7 l4 l8 l2 l2 l3 l4 l5 l7 l6 l8 2 5 4 11
l10 8 l9 1 3 9 10 6 7

15. Dynamic-Domain RRTs

16. Small Bounding Box Large Bounding Box
Bug Trap
In order to investigate this we consider a toy example. A 3-d bug trap is a device for catching bugs. If the food (something sweet) is put inside the trap, the bug can get easily inside, but can never leave the trap. Already by constraining the boundary would improve the performance, since…. However, let’s consider Voronoi regions more carefully.
Small Bounding Box Large Bounding Box
Which one will perform better?

17. Voronoi Bias for the Original RRT

18. KD-Tree Bias for the RRT

19. KD-Tree Bias for the RRT

20. KD-Tree Bias for the RRT

21. Library For Generating Deterministic Sequences Of Samples Over SO(3)
Software:

22. A Spectrum of Roadmaps Random Samples Halton sequence
Hammersley Points Lattice Grid

23. Questions What uniformity criteria are best suited for Motion Planning
Which of the roadmaps alone the spectrum is best suited for Motion Planning?

24. 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”

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

26. 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

27. Layered Sukharev Grid Sequence in [0, 1]d
Places Sukharev grids one resolution at a time
Achieves low dispersion at each resolution
Achieves low discrepancy
Has explicit neighborhood structure
[Lindemann, LaValle 2003]

28. 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

29. Motion Primitives Generation
Reachability graph

30. Dubin’s Car Reachability Graph

31. 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

32. 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.

33. Motion Primitives Generation
Criteria:
Hand-picked “pleasing to the eye” trajectories
Efficient performance of the online planner

34. Motion Primitives Generation
Motivating example 2:
Optimal, Smooth, Nonholonomic Mobile Robot Motion Planning in State Lattices
M. Pivtoraiko, R.A. Knepper, and A. Kelly

35. Motion Primitives Generation
The controls are chosen to reach the points on the state lattice
Criteria:
Well separated trajectories
Efficiency in performance

36. 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]

37. 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

38. 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?

39. Thank you!