1. 29 August 2007
Sampling and Searching Methods for Practical Motion Planning Algorithms
Anna Yershova
PhD Preliminary Examination
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
Thesis Statement
Technical Approach
Efficient Nearest Neighbor Searching
Uniform Deterministic Sampling Methods
Guided Sampling for Efficient Exploration
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. 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

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

11. Thesis 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

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

13. State of Progress 100% Efficient Nearest Neighbor Searching
85% Uniform Deterministic Sampling Methods
75% Guided Sampling for Efficient Exploration
20% Motion Primitives Generation

14. MPNN: Nearest Neighbor Library For Motion Planning
Publications:
Improving Motion Planning Algorithms by Efficient Nearest Neighbor Searching Anna Yershova and Steven M. LaValle IEEE Transactions on Robotics 23(1): , February 2007
Efficient Nearest Neighbor Searching for Motion Planning Anna Yershova and Steven M. LaValle In Proc. IEEE International Conference on Robotics and Automation (ICRA 2002)
Software:

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

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

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

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

19. Library For Generating Deterministic Sequences Of Samples Over SO(3)
Publications:
Deterministic sampling methods for spheres and SO(3) Anna Yershova and Steven M. LaValle, 2004 IEEE International Conference on Robotics and Automation (ICRA 2004)
Incremental Grid Sampling Strategies in Robotics Stephen R. Lindemann, Anna Yershova, and Steven M. LaValle, Sixth International Workshop on the Algorithmic Foundations of Robotics (WAFR 2004)
Software:

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

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

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

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

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

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

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

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

28. Dynamic-Domain RRTs Publications:
Planning for closed chains without inverse kinematics Anna Yershova and Steven M. LaValle, To be submitted to ICRA 2008
Adaptive Tuning of the Sampling Domain for Dynamic-Domain RRTs L. Jaillet, A. Yershova, S. M. LaValle and T. Simeon, In Proc. IEEE International Conference on Intelligent Robots and Systems (IROS 2005)
Dynamic-Domain RRTs: Efficient Exploration by Controlling the Sampling Domain A. Yershova, L. Jaillet, T. Simeon, and S. M. LaValle, In Proc. IEEE International Conference on Robotics and Automation (ICRA 2005)

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

30. Voronoi Bias for the Original RRT

31. KD-Tree Bias for the RRT

32. KD-Tree Bias for the RRT

33. KD-Tree Bias for the RRT

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

35. Motion Primitives Generation
Reachability graph

36. Dubin’s Car Reachability Graph

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

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

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

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

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

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

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

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

45. Thank you!

46. Appendix

47. Kd-trees. Construction4 7 6 5 1 3 2 9 8 10 11 l1 l9 l1 l5 l6 l2 l3 l2 l3
l10 l8 l7 l4 l5 l7 l6 l4 l8 2 5 4 11
l10 8 l9 1 3 9 10 6 7

48. Kd-trees. Query l1 l2 l3 l4 l5 l7 l6 l8 2 5 4 11 l10 8 l9 1 3 9 10 6 7

49. Algorithm Presentation4 7 6 5 1 3 2 9 8 10 11 l5 l1 l9 l6 l3
l10 l7 l4 l8 l2 1 3 l4 l8 l2 l1 l8 1 l2 l3 l4 l5 l7 l6 l9
l10 3 2 5 4 11 9 10 8 6 7 q

50. Analysis of the Algorithm
Proposition 1. The algorithm correctly returns the nearest neighbor.
Proof idea: The points of kd-tree not visited by an algorithm will always be further from the query point then some point already visited.
Proposition 2. For n points in dimension d, the construction time is O(dn lgn), the space is O(dn), and the query time is logarithmic in n, but exponential in d.
Proof idea: This follows directly from the well-known complexity of the basic kd-tree.

51. A Spectrum of Planners
Grid-Based Roadmaps (grids, Sukharev grids) []
optimal dispersion; poor discrepancy; explicit neighborhood structure
Lattice-Based Roadmaps (lattices, extensible lattices)
optimal dispersion; near-optimal discrepancy; explicit neighborhood structure
Low-Discrepancy/Low-Dispersion (Quasi-Random) Roadmaps (Halton sequence, Hammersley point set)
optimal dispersion and discrepancy; irregular neighborhood structure
Probabilistic (Pseudo-Random) Roadmaps
non-optimal dispersion and discrepancy; irregular neighborhood structure
Literature: 1916 Weyl; 1930 van der Corput; 1951 Metropolis; 1959 Korobov; 1960 Halton, Hammersley; 1967 Sobol'; 1971 Sukharev; 1982 Faure; 1987 Niederreiter; 1992 Niederreiter; 1998 Niederreiter, Xing; 1998 Owen, Matousek;2000 Wang, Hickernell

52. Connecting Sample Quality to Problem Difficulty
Quality Measure
Difficulty Measure
Theoretical Bound
integration
discrepancy
bounded Hardy-Krause variation
Koksma-Hlawka inequality
optimization
dispersion
modulus of continuity
[N92]
motion planning
corridor thickness
our analysis

53. Decidability of Configuration Spacesx

54. Undecidability Results

55. Comparing to Random Sequences

56. Sequences for SO(3) Important points:
Uniformity depends on the parameterization.
Haar measure defines the volumes of the sets in the space, so that they are invariant up to a rotation
The parameterization of SO(3) with quaternions respects the unique (up to scalar multiple) Haar measure for SO(3)
Quaternions can be viewed as all the points lying on S 3 with the antipodal points identified
Notions of dispersion and discrepancy can be extended to the surface of the sphere
Close relationship between sampling on spheres and SO(3)

57. Sukharev Grid on S d Take a cube in Rd+1
Place Sukharev grid on each face
Project the faces of the cube outwards to form spherical tiling
Place a Sukharev grid on each spherical face

58. Conclusions
Random sampling in the PRMs seems to offer no advantages over the deterministic sequences
Deterministic sequences can offer advantages in terms of dispersion, discrepancy and neighborhood structure for motion planning

59. The RRT Construction Algorithm
GENERATE_RRT(xinit, K, t)
T.init(xinit);
For k = 1 to K do
xrand  RANDOM_STATE();
xnear  NEAREST_NEIGHBOR(xrand, T);
if CONNECT(T, xrand, xnear, xnew);
T.add_vertex(xnew);
T.add_edge(xnear, xnew, u);
Return T;
xnear
xnew
xinit
The result is a tree rooted at xinit

60. A Rapidly-exploring Random Tree (RRT)
Here is an example of the RRT growing in the square environment with no obstacles.

61. Voronoi Biased Exploration
In this work we investigate the Voronoi bias which controls the growth of the tree
At different stages consider the Voronoi regions of the nodes in the tree. The tree grows in the directions of the largest Voronoi regions.
Is this always a good idea?

62. Voronoi Diagram in R 2
More carefully, consider a simple Voronoi diagram of some tree. We differentiate between the two types of Voronoi regions.

63. Voronoi Diagram in R 2

64. Voronoi Diagram in R 2

65. Refinement vs. Expansion
If the random sample falls in …… These two modes of behavior are controlled by Voronoi bias only. We want to learn to control it.
refinement
expansion
Where will the random sample fall? How to control the behavior of RRT?

66. Limit Case: Pure Expansion
Let X be an n-dimensonal ball,
in which r is very large.
The RRT will explore n + 1 opposite directions.
The principle directions are vertices of a regular (n + 1)-simplex

67. Determining the Boundary
in general, changing the boundary of the domain controls the two modes in some way
Expansion dominates
Balanced refinement and expansion
The tradeoff depends on the size of the bounding box

68. Controlling the Voronoi Bias
Refinement is good when multiresolution search is needed
Expansion is good when the tree can grow and not blocked by obstacles
Main motivation:
Voronoi bias does not take into account obstacles
How to incorporate the obstacles into Voronoi bias?

69. Voronoi Bias for the Original RRT

70. Visibility-Based Clipping of the Voronoi Regions
If this would be for free – the tree would grow in the same manner as the rrt but the expensive collision checks would be saved. The idea is to use the restricted Voronoi regions but which is cheaper to compute.
Nice idea, but how can this be done in practice?
Even better: Voronoi diagram for obstacle-based metric

71. A Boundary Node (a) Regular RRT, unbounded Voronoi region
(b) Visibility region
(c) Dynamic domain

72. A Non-Boundary Node (a) Regular RRT, unbounded Voronoi region
(b) Visibility region
(c) Dynamic domain

73. Dynamic-Domain RRT Bias

74. Dynamic-Domain RRT Construction

75. Dynamic-Domain RRT Bias
Tradeoff between nearest neighbor calls and collision detection calls

76. Recent Efforts Adaptive tuning of the radius:
the radius is not fixed but is increased with every extension success and is decreased with every failure
Nearest neighbor calls:
kd-tree based implementation
O(log n) instead of naïve O(n) query time
Uniform sampling from dynamic domain:
Rejection-based method is not efficient for high dimensions
Uniform distribution should be generated directly

77. Adaptive Tuning of Parameter

78. Adaptive Tuning of Parameter

79. Motion Primitives Generation
Motivating example 2:
Real-Time Motion Planning For Agile Autonomous Vehicles (2000)  
Emilio Frazzoli, Munther A. Dahleh, Eric Feron