Sampling and searching methods for practical motion planning algorithms
Download
1 / 79

Sampling and Searching Methods for Practical Motion Planning Algorithms - PowerPoint PPT Presentation


  • 128 Views
  • Uploaded on

29 August 2007. Sampling and Searching Methods for Practical Motion Planning Algorithms. Anna Yershova PhD Preliminary Examination Dept. of Computer Science University of Illinois. Presentation Overview. Motion Planning Problem Basic Motion Planning Problem

loader
I am the owner, or an agent authorized to act on behalf of the owner, of the copyrighted work described.
capcha
Download Presentation

PowerPoint Slideshow about ' Sampling and Searching Methods for Practical Motion Planning Algorithms' - nero-henson


An Image/Link below is provided (as is) to download presentation

Download Policy: Content on the Website is provided to you AS IS for your information and personal use and may not be sold / licensed / shared on other websites without getting consent from its author.While downloading, if for some reason you are not able to download a presentation, the publisher may have deleted the file from their server.


- - - - - - - - - - - - - - - - - - - - - - - - - - E N D - - - - - - - - - - - - - - - - - - - - - - - - - -
Presentation Transcript
Sampling and searching methods for practical motion planning algorithms

29 August 2007

Sampling and Searching Methods for Practical Motion Planning Algorithms

Anna Yershova

PhD Preliminary Examination

Dept. of Computer Science

University of Illinois


Presentation overview
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


Basic Motion Planning Problem”Moving Pianos”

Given:

  • (geometric model of a robot)

  • (space of configurations, q, thatare applicable to )

  • (the set of collision freeconfigurations)

  • Initial and goal configurations

    Task:

  • Compute a collision free path that connects initial and goal configurations


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


Motion Planning Problemunder 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


Presentation overview1
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 overview2
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

xnew

xinit


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 overview3
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
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
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 formulation
Problem Formulation

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


Example a torus

4

4

4

4

4

4

4

4

6

6

6

6

6

6

6

6

7

7

7

7

7

7

7

7

8

8

8

8

8

8

8

8

5

5

5

5

5

5

5

5

9

9

9

9

9

9

9

9

10

10

10

10

10

10

10

10

3

3

3

3

3

3

3

3

2

2

2

2

2

2

2

2

1

1

1

1

1

1

1

1

11

11

11

11

11

11

11

11

Example: a torus

4

6

7

q

8

5

9

10

3

2

1

11


Kd trees

4

6

l1

l9

7

l5

l6

8

l3

l2

5

9

10

3

l10

l8

l7

2

5

4

11

8

2

1

l4

11

1

3

9

10

6

7

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(dnlgn) precomputation time, O(dn) space and answers queries in time logarithmic in n, but exponential in d.

l1

l3

l2

l4

l5

l7

l6

l8

l10

l9


Presentation overview4
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
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 Over

A Spectrum of Roadmaps

Hammersley Points Lattice Grid


Questions

What uniformity criteria are best suited for Motion Planning Over

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

Questions


Measuring the (Lack of) Quality Over

  • 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 Over

  • Let  denote metric on a sphere

  • Dispersion: “radius of the largest empty ball”


The Goal for Motion Planning Over

  • 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 Sequence Over in [0, 1]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 Over

  • 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 overview5
Presentation Overview Over

  • 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
Dynamic-Domain RRTs Over

Publications:

  • Planning for closed chains without inverse kinematicsAnna 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
Bug Trap Over

Which one will perform better?

Small Bounding Box Large Bounding Box






Presentation overview6
Presentation Overview Over

  • 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
Motion Primitives Generation Over

Reachability graph



Motion primitives generation1
Motion Primitives Generation Over

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 generation2
Motion Primitives Generation Over

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 generation3
Motion Primitives Generation Over

Criteria:

  • Hand-picked “pleasing to the eye” trajectories

  • Efficient performance of the online planner


Motion primitives generation4
Motion Primitives Generation Over

Motivating example 2:

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

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


Motion primitives generation5
Motion Primitives Generation Over

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

Criteria:

  • Well separated trajectories

  • Efficiency in performance


Motivational literature
Motivational Literature Over

  • 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
Proposed problem Over

  • 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
Things to investigate: Over

  • 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?



Appendix
Appendix Over


Kd trees construction

4 Over

6

l1

7

8

5

9

10

3

2

1

11

2

5

4

11

8

1

3

9

10

6

7

Kd-trees. Construction

l9

l1

l5

l6

l3

l2

l3

l2

l10

l4

l5

l7

l6

l8

l7

l4

l8

l10

l9


Kd trees query

l Over 1

q

2

5

4

11

8

1

3

9

10

6

7

Kd-trees. Query

4

6

l9

l1

7

l5

l6

l3

8

l2

l3

l2

5

9

10

3

l10

l4

l5

l7

l6

l8

l7

2

1

l4

11

l8

l10

l9


Algorithm presentation

l Over 1

l3

l2

l4

l5

l7

l6

l8

l10

l9

l1

l2

4

6

l9

7

l5

3

l6

l8

8

l3

l2

5

1

l4

9

10

3

l10

l8

l7

2

1

l4

11

q

2

5

4

11

8

1

3

9

10

6

7

Algorithm Presentation


Analysis of the algorithm
Analysis of the Algorithm Over

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(dnlgn), 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 Over (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

A Spectrum of Planners






Sequences for so 3
Sequences for Over 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 Over Sd

  • 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 Over

  • 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
The RRT Construction Algorithm Over

GENERATE_RRT(xinit, K, t)

  • T.init(xinit);

  • Fork = 1 toKdo

  • 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



Voronoi biased exploration
Voronoi Biased Exploration Over

Is this always a good idea?





Refinement vs expansion
Refinement vs. Expansion Over

refinement

expansion

Where will the random sample fall? How to control the behavior of RRT?


Limit case pure expansion
Limit Case: Pure Expansion Over

  • 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
Determining the Boundary Over

Expansion dominates

Balanced refinement and expansion

The tradeoff depends on the size of the bounding box


Controlling the voronoi bias
Controlling the Voronoi Bias Over

  • 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?



Visibility based clipping of the voronoi regions
Visibility Over -BasedClippingoftheVoronoiRegions

Nice idea, but how can this be done in practice?

Even better: Voronoi diagram for obstacle-based metric


A boundary node
A Boundary Node Over

(a) Regular RRT, unbounded Voronoi region

(b) Visibility region

(c) Dynamic domain


A non boundary node
A Non-Boundary Node Over

(a) Regular RRT, unbounded Voronoi region

(b) Visibility region

(c) Dynamic domain




Dynamic domain rrt bias1
Dynamic-Domain RRT Bias Over

Tradeoff between nearest neighbor calls and collision detection calls


Recent efforts
Recent Efforts Over

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(logn) 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




Motion primitives generation6
Motion Primitives Generation Over

Motivating example 2:

Real-Time Motion Planning For Agile Autonomous Vehicles (2000)

Emilio Frazzoli, Munther A. Dahleh, Eric Feron


ad