Path Planning for Nonholonomic Robots

Concept of Nonholonomy

Consider a car moving on a horizontal plane. Model the car as a rectangle. Select the reference point to be the midpoint between the two rear wheels, and the reference direction to be the main axis of the rectangle pointing in the forward direction.

A car's configuration is defined by (x,y,d). So the car's configuration space has 3 dimensions. However, these parameters and their derivatives are related by the following equations:

- x' sin d + y' cos d = 0.

where x' and y' denote the derivatives of x and y w.r.t. time.

What does that mean? At any configuration (x,y,d) along a trajectory parametrized by time, the triplet (x',y',d') is the velocity of the robot. The set of all vectors (x',y',d') for all possible trajectories passing through (x,y,d) is a vector space. If the robot was not constrained by any kinematic constraints (as the one above), this vector space would exactly be the tangent space of the configuration space at (x,y,d) and would have dimension 3. Because of the above relation (which one can show is not integrable), the velocity vectors only span a subspace of the tangent space of dimension 2. It is called a nonholomic constraint.

Generalization

More generally a robot whose configuration space has dimension m is a nonholonomic robot if it is constrained by k non-integrable constraints (0<k<m) of the form :

Gi(q,q') = 0.

Examples:

Questions

  1. How can we know that a constraint is nonholonomic (i.e., non-integrable)?
  2. Do nonholonomic constraints restrict the space of reachable configurations?
  3. How can we build an efficient planner in the presence of nonholonomic constraints?

Lie Brackets

Consider two smooth vector fields X and Y over the robot's configuration space C. The Lie bracket of X and Y is the (smooth) vector field whose ith component is:

X1(dYi/dq1) - Y1(dXi/dq1) + ... + Xm(dYi/dqm) - Ym(dXi/dqm).

Let D be the space of smooth vector fields on C satisfying the k constraints Gi(q,q') = 0. The Contol Lie Algebra associated with D, CLA(D), is the smallest space of all vector fields containing D that is closed under the Lie bracker operation.

Characterization of nonholonomy:
A robot A subject to k independent constraints Gi(q,q') = 0 is holonomic if the dimension of CLA(D) is m-k; otherwise, it is nonholonomic.

Characterization of controllability

Let U be any subset of the configuration space C. A configuration q1 in C is said to be U-accessible from a configuration q0 in U, if there exists a piecewise constant q'(t) in the space of feasible velocities whose integral is a trajectory joining q0 to q1 that fully lies in U.

The robot is said to be locally controllable at q0 iff for every neighborhood U of q0, the set of configurations U-accessible from q0 is also a neighborhood of q0. The robot is locally controllable iff this is true for all q0 in C.

Local controllability implies global controllability. Indeed, every path is compact, so that every covering of the path by neighborhood contains a finite covering. Hence, one can patch sufficiently small open subsets along the path and transform it, within these subsets, into a feasible paths

q0 Characterization of controllability:
A nonholonomic robot A is symmetric if every feasible trajectory is feasible in both directions. A symmetric nonholonomic robot A is controllable if the dimension of CLA(D) is equal to that of the configuration space (m).

The degree of nonholonomy of the robot is the minimal number of Lie bracket sufficient to express any vector in CLA(D).

Major Controllability Results

A car-like robot is globally controllable.

This result still holds if the steering wheel can only achieve two distinct positions.

It does not hold if the car can only go forward, or if the steering wheel can only turn in one direction. Then the car is not symmetric.


A tractor-trailer with N bodies is controllable.


The degree of nonholonomy of a car is 1. That of a tractor-trailer with 2 bodies (the tractor and the trailer) is 2.

Path Planning Approaches for Car-Like Robot

  1. Use two linear velocities, v0 and -v0, and at least two steering angles, s1 and s2 (usually, its better to use 3 angles: -s, 0, and s).

    From the initial configuration integrate the equations of motion over a unit interval of time. This gives 4 successors (or 6, if 3 steering angles have been used). From the collision-free ones, integrate again, And so on.

    Search the constructed graph using, say, the number of backups as the cost function and the Euclidean distance to the goal as the heuristics.

    Index every configuration in a regular array of cells defined over configuration space, and accept every new configuration reached during the search only if it falls in a cell that has not been visited yet. Stop when the cell containing the goal configuration is attained.

    This algorithm is asymptotically complete and optimal. That is, if there exists a path, the planner will find one and this path will be optimal if the resolution of the indexing array is fine enough.

    Extendable to tractor-trailer, but then the indexing array is 4-dimensional.

  2. First generate a non-feasible collision-free path, by ignoring the kinematic constraints. Next transform that path into a feasible one, by decomposing the original path into segments small enough that they can be replaced by feasible paths that are collision-free. Finally, optimize the resulting path.

    Typical path substitution: Reeds and Shepp curves.

Open Questions

Complexity of path planning for a non-controllable robot, e.g., a car-like robot that can only go forward.