Planning for car-like and nonholonomic robots

A differential-drive robot can turn on the spot, so a path through free space in (x, y) is enough: the robot can stop, rotate to face the next segment and drive on. A car cannot. It can only move along its heading, and it can only change heading while moving, with a curvature limited by its steering. Forklifts, tugs pulling carts, tractors, ships and fixed-wing aircraft share the same kind of restriction. These nonholonomic constraints change planning fundamentally: a collision-free geometric path may be impossible to follow, and getting into a parking space may need a maneuver that goes backward and forward several times. This lesson explains what nonholonomic constraints are and why such robots can still reach every pose, introduces the Dubins and Reeds-Shepp curves that give shortest paths for car-like robots without obstacles, shows how these curves are used as steering functions inside the planners of lessons 5 and 7, and touches on trailers and differential flatness.

Nonholonomic constraints

For a robot moving in the plane with pose \( (x, y, \theta) \), the condition that it cannot slide sideways is

This constrains velocities, not positions, and it cannot be integrated into a relation among x, y and θ alone, which is what makes it nonholonomic. The robot therefore has three configuration variables but only two independent ways to move: forward along its heading and turning. Remarkably, it can still reach any pose, because combining the two motions generates the missing sideways one: drive forward while turning left, back up while turning right, and repeat, and the car shuffles sideways, as in parallel parking. In control theory this is the Lie bracket of the two motion directions, and Chow's theorem guarantees that when the brackets span all directions the system is controllable. For planning, the consequence is that every pose is reachable, but the path to a nearby pose may be long and complicated.

Dubins curves

In 1957 Lester Dubins proved that for a vehicle that can only move forward at constant speed with curvature bounded by \( 1/R \), the shortest path between two poses in the absence of obstacles is always made of at most three segments, each either a circular arc of radius R turning left (L) or right (R), or a straight segment (S). Only six combinations can be optimal: LSL, RSR, LSR, RSL, RLR and LRL. To find the shortest Dubins path, compute the length of all six (some may not exist for a given pair of poses) and take the smallest. Each takes a few lines of trigonometry, so the whole computation runs in microseconds. Dubins paths are the right model for robots that cannot reverse, such as fixed-wing aircraft at constant altitude or boats, and they are the steering function in many kinodynamic sampling planners.

import math
def mod2pi(a): return a % (2 * math.pi)
def dubins_lengths(x0, y0, t0, x1, y1, t1, R):
    """Lengths of the six Dubins words using the standard normalized formulas; None if infeasible."""
    dx, dy = x1 - x0, y1 - y0
    d = math.hypot(dx, dy) / R; th = math.atan2(dy, dx)
    a, b = mod2pi(t0 - th), mod2pi(t1 - th)
    sa, ca, sb, cb, cab = math.sin(a), math.cos(a), math.sin(b), math.cos(b), math.cos(a - b)
    out = {}
    p2 = 2 + d * d - 2 * cab + 2 * d * (sa - sb)                     # LSL
    out["LSL"] = None if p2 < 0 else (mod2pi(math.atan2(cb - ca, d + sa - sb) - a) + math.sqrt(p2) + mod2pi(b - math.atan2(cb - ca, d + sa - sb)))
    p2 = 2 + d * d - 2 * cab + 2 * d * (sb - sa)                     # RSR
    out["RSR"] = None if p2 < 0 else (mod2pi(a - math.atan2(ca - cb, d - sa + sb)) + math.sqrt(p2) + mod2pi(math.atan2(ca - cb, d - sa + sb) - b))
    p2 = -2 + d * d + 2 * cab + 2 * d * (sa + sb)                    # LSR
    if p2 < 0: out["LSR"] = None
    else:
        p = math.sqrt(p2); tmp = math.atan2(-ca - cb, d + sa + sb) - math.atan2(-2, p)
        out["LSR"] = mod2pi(tmp - a) + p + mod2pi(tmp - b)
    p2 = d * d - 2 + 2 * cab - 2 * d * (sa + sb)                     # RSL
    if p2 < 0: out["RSL"] = None
    else:
        p = math.sqrt(p2); tmp = math.atan2(ca + cb, d - sa - sb) - math.atan2(2, p)
        out["RSL"] = mod2pi(a - tmp) + p + mod2pi(b - tmp)
    t = (6 - d * d + 2 * cab + 2 * d * (sa - sb)) / 8                # RLR
    if abs(t) > 1: out["RLR"] = None
    else:
        p = mod2pi(2 * math.pi - math.acos(t)); tt = mod2pi(a - math.atan2(ca - cb, d - sa + sb) + p / 2)
        out["RLR"] = tt + p + mod2pi(a - b - tt + p)
    t = (6 - d * d + 2 * cab + 2 * d * (sb - sa)) / 8                # LRL
    if abs(t) > 1: out["LRL"] = None
    else:
        p = mod2pi(2 * math.pi - math.acos(t)); tt = mod2pi(-a + math.atan2(-ca + cb, d + sa - sb) + p / 2)
        out["LRL"] = tt + p + mod2pi(mod2pi(b) - a - tt + p)
    return {w: (None if v is None else v * R) for w, v in out.items()}
L = dubins_lengths(0, 0, math.radians(90), 10, 6, math.radians(90), 2.0)
for w, v in L.items():
    print(w, "infeasible" if v is None else "%.2f m" % v)
best = min((v, w) for w, v in L.items() if v is not None)
print("shortest word: %s, %.2f m (straight-line distance %.2f m)" % (best[1], best[0], math.hypot(10, 6)))

LSL 24.23 m
RSR 24.23 m
LSR 36.23 m
RSL 12.59 m
RLR infeasible
LRL infeasible
shortest word: RSL, 12.59 m (straight-line distance 11.66 m)

Reeds-Shepp curves

Cars can reverse. In 1990 Reeds and Shepp extended Dubins' result to a vehicle that may drive forward or backward with bounded curvature. The shortest path then has at most five segments, with cusps where the direction changes, and belongs to one of 46 families (48 in the original count, two of which are redundant). Reeds-Shepp paths are never longer than Dubins paths between the same poses, and they are typically much shorter for nearby poses that differ mainly in position sideways or heading, where reversing helps, as in parallel parking. They are the obstacle-free distance used as a heuristic in Hybrid A* (lesson 5) and as the analytic expansion that completes its search, and they are available in OMPL as a state space for sampling planners. One subtlety: Reeds-Shepp paths minimize length, not the number of direction changes, so practical planners add a penalty for each cusp, since stopping and reversing take time and are less comfortable.

Using steering functions in planners

Dubins and Reeds-Shepp curves ignore obstacles, but they are exact steering functions: given two poses, they return the shortest feasible connection. Planners use them in three ways. As heuristics, their lengths are admissible lower bounds on the true cost, so they guide A* on lattices and in Hybrid A*. As local planners, they replace straight lines in PRM and RRT: to connect two samples, compute the curve between them and check it for collisions; OMPL's DubinsStateSpace and ReedsSheppStateSpace do this, letting RRT* and PRM* plan for car-like robots directly. And as final connections, they finish a search exactly at the goal pose, which grid search alone cannot do. For robots with more complex dynamics, such as a car at speed where tire forces matter, these geometric curves are replaced by trajectory optimization or motion primitives computed from the full model.

Trailers, curvature continuity and flatness

Real vehicles complicate the picture. Dubins and Reeds-Shepp paths switch instantly between straight lines and full-lock turns, which requires the steering wheel to jump; real steering changes at a finite rate, so planners for road vehicles use continuous-curvature paths built from clothoids (curves whose curvature changes linearly with length), which are slightly longer but drivable at speed. A tractor pulling one or more trailers adds a heading variable per trailer and a hitch-angle constraint, and reversing a trailer is unstable without feedback, as any driver knows. Lattice planners with primitives computed for the full articulated model are the usual answer, as in automated port and airport tugs. Finally, some nonholonomic systems are differentially flat: there exist flat outputs (for a car, the rear-axle position) such that the whole state and input can be computed from the output trajectory and its derivatives. For such systems, planning reduces to finding a smooth enough curve for the flat outputs, often with polynomials, and the steering and speed follow algebraically. Quadrotors are also flat, with position and yaw as outputs, which is why polynomial trajectory planning is standard for drones.