Path Planning in High-Dimensional Configuration Space

Configuration Space

The number m of dimensions of the configuration space of a robot is equal to the number of degrees of freedom of the robot, e.g., the number of articulated joints if the robot is an arm with revolute and prismatic joints.

If m > 3, computing C-obstacles can be very difficult. When m > 6, it is practically impossible.

Two complete general path planning algorithms have been proposed:

An exact cell decomposition method, based on the Collins decomposition, by Schwartz and Sharir (1983), but it takes twice exponential time in m.
A roadmap method, proposed by Canny (1987), based on elimination theory, which takes exponential time in m.

None of these two techniques has been effectively implemented.

High-Dimensional Configuration Spaces ...

... start with dimension 5 or 6.

Examples:

Rigid object that translates and rotates in 3-D workspace (e.g., planning for maintainance tasks).
Coordination of mutiple robots (e.g., spot welding workcells in the automotive industry often require coordinating 6 robots having 6 dof each).
Animation of digital actors.
Prediction of molecular motions.

Randomized Path Planning

General ideas:

No explicit computation/representation of the C-obstacles. Instead, C-obstacles are represented implicitly by a function that computes the distance between the robot at an input configuration q and the obstacles in the workspace (a 2- or 3-D space).
A network of simple paths (usually, straight line segments in configuration space) is generated by picking free configurations at random. This network is called a probabilistic roadmap.

Two configurations that can be connected by a simple path in the free space are said to see each other.

Preprocessing:
Construction of a probabilistic roadmap represented as a graph R = (M,L):

Pick s configurations uniformly at random in the free space. Call them milestones and let M be the set of milestones.
Construct the graph R = (M,L) in which L consists of every pair of milestones that see each other.

Query Processing:
Connection of any two input free configurations qinit and qgoal to a probabilistic roadmap R = (M,L):

For i = {init,goal} do:
1. if there exists a milestone m in M that sees qi then set mi to m.
2. else
  1. repeat t times:
    Pick a free configuration q at random in a neighborhood of qi, until q seesqi both and a milestone m.
  2. if all t trials failed then return FAILURE, else set mi to m.
if minit and mgoal are in the same component of R, then return a path connecting them, else return NO-PATH.

Desirable Properties of a Probabilistic Roadmap

Coverage: Collectively the milestones in M should see a large portion of the free space to guarantee that each query configuration can be connected to one of them efficiently with high probability.
Connectivity: There should be a one-to-one correspondance between the components of R and those of the free space.

With high probability, the above algorithm (preprocessing) generates a roadmap that satisfies these two properties in expansive spaces. (See Capturing the Connectivity of High-Dimensional Geometric Spaces by Parallelizable Random Sampling Techniques.)

An expansive free space is one which contains no ``narrow passages".

Sampling Strategies for PRM planners

Uniform sampling.
Re-sampling in ``difficult" areas.
Wave propagation from initial and goal configurations.
Incremental "learning".
Denser sampling near the free space boundary, e.g.:

N. Amato and Y. Wu. A Randomized Roadmap Method for Path and Manipulation Planning. Proc. IEEE Int. Conf. on Robotics and Automation, Minneapolis, MN, 1996, pp. 113-120.

M. Overmars and P. Svestka. A Probabilistic Learning Approach to Motion Planning. In Algorithmic Foundations of Robotics, K. Goldberg et al (eds.), A.K. Peters, Wellesley, MA, 1995, pp.19-37.
Dilatation of the free space (to find narrow passages). D. Hsu, L.E. Kavraki, J.C. Latombe, R. Motwani, and S. Sorkin. On Finding Narrow Passages with Probabilistic Roadmap Planners. Proc. of the 1998 Workshop on Algorithmic Foundations of Robotics, Houston, TX, March 1998.
Sampling biased by potential field.

Collision Checking in 2-D Workspace

Case of a multi-link robot:

For every link, precompute a C-obstacle bitmap representing the obstacle map into the 3-D configuration space of the link (ignoring the other link).
For every pair of links, precompute a C-obstacle bitmap representing the relative configurations at which the two links collide/do not collide.
Each time one needs to check that a configuration is free, compute the configuration of each link and teh relative configuration of every pair of links, and read the corresponding cell in each bitmap.

Collision Checking in 3-D Workspace

Many existing techniques. Most represent objects by hierarchy of objects of simple shapes (e.g., spheres, parallelepipeds) and eventually reduce collision checking/distance computation between two objects of basic shape (e.g., two convex polyhedra).

S. Quinlan. Efficient Distance Computation Between Non-Convex Objects. Proc. IEEE Int. Conf. on Robotics and Automation, San Diego, CA, 1994, pp. 117-123.

Principle:

Triangulate the boundary of each object.
Represent each object by a binary tree of spheres, such that:
- The sphere at each node N contains the sphere at each of the two children of N.
- The tree is approximately balanced.
To check for collision or compute the distance between two objects, traverse their sphere trees concurrently. When leaf nodes are reached check collision or compute distance between two triangles.