Sampling and Searching Methods for Practical Motion Planning Algorithms Anna Yershova PhD Preliminary Examination Dept. of Computer Science University of Illinois 29 August 2007
Jan 03, 2016
Sampling and Searching
Methods for Practical Motion
Planning Algorithms
Anna Yershova
PhD Preliminary Examination
Dept. of Computer Science
University of Illinois
29 August 2007
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
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
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
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
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
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
State of Progress
100% Efficient Nearest Neighbor Searching
85% Uniform Deterministic Sampling Methods
75% Guided Sampling for Efficient Exploration
20% Motion Primitives Generation
MPNN: Nearest Neighbor Library For Motion Planning
Publications: Improving Motion Planning Algorithms by Efficient Nearest Neighbor
Searching Anna Yershova and Steven M. LaValleIEEE Transactions on Robotics 23(1):151-157, February 2007
Efficient Nearest Neighbor Searching for Motion PlanningAnna Yershova and Steven M. LaValleIn Proc. IEEE International Conference on Robotics and Automation (ICRA 2002)
Software: http://msl.cs.uiuc.edu/~yershova/mpnn/mpnn.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
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
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: 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
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
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 RRTsL. 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 DomainA. Yershova, L. Jaillet, T. Simeon, and S. M. LaValle, In Proc. IEEE International Conference on Robotics and Automation (ICRA 2005)
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
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
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!
Appendix
Kd-trees. Construction
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
Kd-trees. Query
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
q
Algorithm Presentation
l1
l8
1
l2l3
l4 l5 l7 l6
l9l10
3
2 5 4 11
9 10
8
6 7
q
47
6
5
1
3
2
9
8
10
11
l5
l1 l9
l6
l3
l10 l7
l4
l8
l2
1
3
l4
l8
l2
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.
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
Connecting Sample Quality to Problem Difficulty
Problem Quality Measure
Difficulty Measure
Theoretical Bound
integration discrepancy bounded Hardy-Krause variation
Koksma-Hlawka inequality
optimization dispersion modulus of continuity
[N92]
motion planning dispersion corridor thickness
our analysis
Decidability of Configuration Spaces
x
Undecidability Results
Comparing to Random Sequences
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)
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
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
The RRT Construction Algorithm
GENERATE_RRT(xinit, K, t)
1. T.init(xinit);
2. For k = 1 to K do
3. xrand RANDOM_STATE();
4. xnear NEAREST_NEIGHBOR(xrand, T);
5. if CONNECT(T, xrand, xnear, xnew);
6. T.add_vertex(xnew);
7. T.add_edge(xnear, xnew, u);
8. Return T;
xnear
xinit
xnew
The result is a tree rooted at xinit
A Rapidly-exploring Random Tree (RRT)
Voronoi Biased Exploration
Is this always a good idea?
Voronoi Diagram in R 2
Voronoi Diagram in R 2
Voronoi Diagram in R 2
Refinement vs. Expansion
refinement expansion
Where will the random sample fall? How to control the behavior of RRT?
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
Determining the Boundary
Expansion dominates Balanced refinement and expansion
The tradeoff depends on the size of the bounding box
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?
Voronoi Bias for the Original RRT
Visibility-Based Clipping of the Voronoi Regions
Nice idea, but how can this be done in practice?Even better: Voronoi diagram for obstacle-based metric
(a) Regular RRT, unbounded Voronoi region
(b) Visibility region
(c) Dynamic domain
A Boundary Node
A Non-Boundary Node
(a) Regular RRT, unbounded Voronoi region
(b) Visibility region
(c) Dynamic domain
Dynamic-Domain RRT Bias
Dynamic-Domain RRT Construction
Dynamic-Domain RRT Bias
Tradeoff between nearest neighbor calls and collision detection calls
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
Adaptive Tuning of Parameter
Adaptive Tuning of Parameter
Motion Primitives Generation Motivating example 2:
Real-Time Motion Planning For Agile Autonomous Vehicles (2000)
Emilio Frazzoli, Munther A. Dahleh, Eric Feron