When sampling-based planning is too slow, mathematics delivers exact answers. Controllability theory, Hamilton-Jacobi-Bellman, Dubins paths, and Lie brackets — the tools for steering any system precisely.
You're trying to park a car. Not a video-game car that can slide sideways — a real car. You need to reach a specific spot at a specific angle. Your car's dynamics are simple: forward speed and steering angle. But your driveway is narrow. Can you always get there, no matter how the spot is oriented?
This is the central question of system theory: given a dynamical system and a set of inputs, can you steer from any initial state to any goal state? The answer profoundly shapes your planning strategy. If the answer is no for some pair of states, no planner — however clever — will ever find a path between them.
For the car, the answer turns out to be yes — but only with multi-point turns. That "yes, but..." is what Chapter 15 is about. We'll learn how to prove controllability analytically, derive exact optimal paths for specific vehicles, and understand the algebraic structure that makes certain systems steerable at all.
The simulation below illustrates why this matters. A Dubins car cannot move sideways. Sampling-based methods (left) find a jagged, inefficient path. The analytical Dubins formula (right) finds the provably shortest path in microseconds — two circular arcs joined by a straight line.
Left: a random tree finds a valid path. Right: the Dubins formula finds the optimal path instantly. Same start/goal, very different quality.
You want to know, before you write a single line of planning code, whether your system is theoretically steerable to any goal. This is controllability: the property that for any two states x0 and xf in the state space, there exists some finite-time input trajectory u(t) that drives the system from x0 to xf.
For linear time-invariant (LTI) systems — ẋ = Ax + Bu — there is a beautifully clean test. No simulation, no search, just a matrix rank check. The controllability matrix is:
where n is the dimension of the state space. The system is controllable if and only if this matrix has rank n — meaning its columns span all of ℜn. Every state direction is reachable.
Consider a mass sliding on a frictionless surface. State: x = [position, velocity]T. Input: u = force/mass (acceleration). The equations: ẋ1 = x2, ẋ2 = u. In matrix form:
Build the controllability matrix: C = [B, AB]. We get:
A force input can independently set any position and velocity. Makes physical sense — with enough time and thrust, you can reach any point at any speed.
Now suppose two masses are coupled by a spring, but you can only push the first mass directly. If the matrices work out to rank 1 < 2 = n, the system is uncontrollable. You can push the first mass but the second's trajectory is locked — you cannot independently set both states. This happens in systems with decoupled modes: no matter how hard you try, energy never flows from one mode to the other.
Enter the dimensions of A and B, and watch the controllability matrix being built column by column. The system is controllable iff rank = n.
Chapter 10 gave you value iteration for discrete MDPs. At each step, you look at all actions and pick the one with the best cost-to-go. Now consider the same idea, but time flows continuously and the system obeys a differential equation ẋ = f(x, u). What does value iteration look like?
Define the optimal cost-to-go V*(x) as the total cost accumulated from state x if you act optimally all the way to the goal. For continuous time with a running cost l(x, u) and terminal cost lF(x), this is:
The Hamilton-Jacobi-Bellman (HJB) equation is the differential equation that V* must satisfy. It's the continuous-time analogue of the Bellman equation:
with boundary condition V*(x, T) = lF(x). This is a partial differential equation (PDE) in the state-time space. Solving it gives you V* everywhere — and the optimal policy falls out: u*(x) = argminu[l(x,u) + (∇V*)T f(x,u)].
∂V*/∂t
How cost-to-go changes with time remaining. For a finite-horizon problem, this is nonzero. For infinite-horizon discounted problems, it's replaced by γV*.
(∇V*)Tf(x,u)
The dot product of the cost gradient with the dynamics. Positive means moving toward high-cost states. Optimal control drives this negative as fast as possible.
The HJB equation is generally intractable for nonlinear systems — it's a PDE over the full state space, which suffers exponential blowup in dimension (the "curse of dimensionality"). But there are two cases where solutions exist cleanly:
| System type | HJB simplifies to | Solution method |
|---|---|---|
| Linear dynamics, quadratic cost (LQR) | Riccati equation (ODE in time) | Matrix algebra, exact |
| 1D or 2D nonlinear systems | PDE in 1-2 variables | Numerical grid methods |
| Specific vehicle models | Geometric conditions (Pontryagin) | Dubins / Reeds-Shepp (Ch 3-5) |
For a 1D mass (state = [position, velocity]), the HJB can be solved on a grid. Each cell shows V*(x): the minimum time to reach the origin. Brighter = farther from goal (higher cost-to-go). Drag to see the optimal trajectory from any start state.
A car cannot drive sideways. It has a minimum turning radius — you can't pirouette in place. These two constraints together make the shortest-path problem genuinely nontrivial. The answer isn't a straight line. It's something more elegant.
The Dubins car is the simplest model capturing this: a particle moving at fixed unit speed in the plane, with bounded steering. Its state is (x, y, θ) — position and heading. Its kinematics are:
Here u is the steering curvature (u = +1 is a left circle of unit radius, u = −1 is a right circle, u = 0 is straight). The constraint |u| ≤ 1 enforces the minimum turning radius of 1.
To minimize total path length (time, since speed = 1), we apply Pontryagin's Minimum Principle — the continuous-time generalization of the optimality conditions that HJB satisfies. This is not a full derivation (it would take a chapter), but the punchline is crucial:
This is an extraordinary result. The space of continuous control trajectories is infinite-dimensional, yet the optimal solution always lies in a set of just 6 combinatorial types, each defined by 3 parameters (the arc lengths). You reduce an infinite optimization to solving 6 geometric problems.
| Type | Structure | When it wins |
|---|---|---|
| LSL | Left arc → Straight → Left arc | Start/goal roughly aligned, both turning left |
| RSR | Right arc → Straight → Right arc | Start/goal roughly aligned, both turning right |
| LSR | Left arc → Straight → Right arc | Configurations facing roughly opposite directions |
| RSL | Right arc → Straight → Left arc | Configurations facing roughly opposite directions |
| LRL | Left → Right → Left (three arcs) | Short distance, tight turns needed |
| RLR | Right → Left → Right (three arcs) | Short distance, tight turns needed |
Here is the Dubins algorithm running live. Set a start pose (position + heading) and a goal pose, and the planner computes all 6 candidate paths, evaluates their lengths, and draws the shortest one. Every arc and straight segment is geometrically exact.
Notice what happens when you place start and goal very close together (less than ~4 turning radii apart): the path type often flips from CSC to CCC. The three-arc solution becomes shorter because a long straight line "overshoots" and requires even larger arcs on both ends.
Also notice: increasing the turning radius R (decreasing maneuverability) makes the paths longer. A large tanker ship with a 500m turning radius needs enormous space to reorient — the Dubins formula quantifies exactly how much.
The Dubins car only goes forward. Real cars can also reverse. This seemingly small addition changes the problem dramatically — with reverse, you can execute a K-point turn and reach configurations that would require enormous Dubins detours. Reeds and Shepp (1990) solved this extended problem.
The Reeds-Shepp car has the same kinematics as the Dubins car, but u ∈ {−1, 0, +1} for both steering curvature and for speed direction (forward or backward). We write paths with a sign: C means arc, S means straight, and a bar over a symbol indicates reverse motion. So L̄SR means: reverse left arc, forward straight, forward right arc.
Consider a parallel parking scenario: you need to reach a spot directly to your right, facing the same direction as a parked row. Forward-only (Dubins): you must drive a wide arc all the way around the block. With reverse: you pull forward slightly, cut right, reverse into the spot — a three-move maneuver taking a tiny fraction of the distance.
For the same start/goal, how much does reverse help? The bar on the left is the Dubins path length; the bar on the right is the Reeds-Shepp path length. Drag the goal heading to find configurations where reverse matters most.
Every time you switch between forward and reverse, you must stop — a cusp in the path. Cusps are kinematically necessary (speed must pass through zero) but can be costly for dynamic systems where stopping and restarting requires extra energy. The Reeds-Shepp optimizer doesn't penalize cusps — it only minimizes path length. In practice, you may need to add a cusp penalty to get dynamically feasible paths.
| Property | Dubins car | Reeds-Shepp car |
|---|---|---|
| Reverse motion | No | Yes |
| Optimal path families | 6 | 48 (9 canonical) |
| Cusps in optimal path | None | 0 to 2 |
| Path always ≤ Dubins path | — | Yes (reverse can only help) |
| Applies to | UAVs, ships, slow cars | Cars, forklifts, parallel parking |
So far we've worked with specific vehicles (Dubins, Reeds-Shepp). But what about a general nonholonomic system? Is there an algebraic tool that tells you whether you can reach any state from any other — without computing optimal paths explicitly?
Yes. The tool is the Lie bracket, and it's one of the most powerful ideas in geometric control theory.
Consider a system with two vector fields f and g (corresponding to two different control modes). The Lie bracket [f, g] is:
This is a new vector field. Its physical meaning: if you apply f for ε time, then g for ε time, then −f for ε time, then −g for ε time, you end up displaced from your start by approximately ε2[f,g]. In other words, the Lie bracket reveals new directions of motion that emerge from combining two control inputs, even if neither input alone can go there.
For a system ẋ = g1(x)u1 + g2(x)u2 + … with control vector fields gi, define the Lie algebra generated by {gi}: this is the set of all vector fields you can form by taking iterated Lie brackets of the gi.
Chow's theorem: If the Lie algebra generated by {gi} has dimension n (spans the full tangent space) at every point in a connected state space, then the system is small-time locally controllable — you can reach any state from any other using arbitrarily small motions.
The unicycle has state (x, y, θ) and controls: g1 = (cosθ, sinθ, 0) (move forward), g2 = (0, 0, 1) (rotate). We need [g1, g2]:
That's a vector pointing perpendicular to the unicycle's heading — the lateral direction! Together, g1, g2, [g1,g2] span all of ℜ3. By Chow's theorem, the unicycle is controllable. It can reach any (x, y, θ) from any other, using forward/backward and rotation — no sideways motion ever required directly.
Chow's theorem proves you can steer the system to any state. But it doesn't tell you how. Steering methods are algorithms that, given a start state x0 and a goal state xf, compute an explicit control trajectory u(t) that drives the system from x0 to xf.
For planning algorithms like RRT (Chapter 5), having a good steering method is crucial — it's called by thousands of times per second to extend the tree. Exact, fast steering methods make the difference between a planner that works and one that doesn't.
The classic nonholonomic steering problem is the car with trailer (or the simpler unicycle). A beautiful closed-form steering method due to Murray and Sastry (1993) uses sinusoidal inputs at specific frequencies.
For the unicycle model (ẋ=u1cosθ, ẏ=u1sinθ, θ̇=u2), to move from (0,0,0) to (xf, yf, θf), apply inputs:
where the amplitudes ai and phases φi are chosen to satisfy the three endpoint conditions. This sinusoidal approach works because the Campbell-Baker-Hausdorff formula links sinusoidal inputs to Lie bracket motion — the oscillations "generate" the bracket direction.
The car starts facing right and needs to end up displaced laterally by Δy. No steering-only motion can do this — but sinusoidal inputs generate the Lie bracket direction. Press Animate to watch the oscillating path.
Sinusoidal steering for the unicycle is exact — it hits the target precisely. For more complex systems (cars with trailers, systems needing higher-order brackets), closed-form exact steering methods may not exist. In practice, planners use:
| Method | Exactness | Speed | Generality |
|---|---|---|---|
| Sinusoidal (Murray-Sastry) | Exact | Very fast | Nilpotent systems only |
| Dubins/Reeds-Shepp | Exact + Optimal | Very fast | Specific car models |
| Flat system inversion | Exact | Fast | Differentially flat systems |
| Iterative (numerical) | Approximate | Slower | General |
We've built a powerful theoretical toolkit: controllability tests, HJB equations, exact Dubins/Reeds-Shepp paths, Lie bracket analysis, and sinusoidal steering. How do these connect to real robotic systems being deployed today?
Fixed-wing aircraft (UAVs, fighter jets) cannot hover — they must maintain minimum airspeed and have a minimum banking radius. This is exactly the Dubins car in 2D (extended to 3D as "Dubins airplane"). Commercial autopilot systems use Dubins path computation for waypoint following: given GPS waypoints, compute the optimal turn sequence. The computation takes microseconds on embedded hardware — fast enough for 100Hz control loops.
Every autonomous parking system (Tesla's "Smart Summon," Waymo's valet mode) uses Reeds-Shepp paths or generalizations. The key engineering detail: the planner generates an RS path as an initial guess, then uses nonlinear trajectory optimization to satisfy additional constraints (obstacle avoidance, dynamic feasibility, passenger comfort). The RS path provides a warm start that dramatically reduces optimizer iterations.
The Linear Quadratic Regulator (LQR) is the most deployed control law in aerospace. It solves the HJB equation exactly for linear dynamics ẋ = Ax + Bu with quadratic cost ∫(xTQx + uTRu)dt. The solution is u* = −Kx where K = R−1BTP and P is found by solving the Riccati equation:
This is a matrix equation — solvable in milliseconds. The resulting controller is optimal, linear, and stabilizing. It's used in Falcon 9 landing, NASA's ISS attitude control, and virtually every precision industrial robot.
Some systems have a special property called differential flatness: there exist "flat outputs" y(x) such that all states and inputs can be expressed as functions of y and its derivatives. The unicycle and the car-with-trailer are flat. Flat systems are steerable by simply planning smooth curves in the flat output space — no inverse kinematics, no differential equations to solve. This is why quadrotor trajectory planning reduces to planning smooth polynomial curves in (x, y, z, yaw) space.
For a 1D double integrator, adjust Q (state cost) and R (control cost) to see how the optimal LQR policy changes. High Q → keep position near zero. High R → use gentle controls. The colored lines show V*(x) for the current Q/R ratio.
Chapter 15 closes the loop on planning under differential constraints. Let's situate what we've learned within the larger landscape and look at what comes next.
| Topic from Ch 15 | Modern descendant | Where you see it |
|---|---|---|
| HJB / LQR | iLQR, DDP, MPC | Legged robots (ANYmal), quadrotors (Crazyflie) |
| Dubins paths | Fixed-wing autopilot | ArduPlane, PX4, commercial UAVs |
| Reeds-Shepp | Parking planners | Tesla FSD, Waymo, Apollo |
| Differential flatness | Polynomial trajectory | Quadrotor racing (Falanga 2018) |
| Lie brackets / Chow | Geometric mechanics | Snake robots, underactuated manipulation |
Chapter 15 gives you analytical tools — they work beautifully when the system model is exact and the state space is low-dimensional. Modern robotics increasingly deals with high-dimensional systems (humanoids, dexterous hands) and model uncertainty. The frontier methods combine what we've learned here with learning:
From this book:
From microLearning:
"The optimal control problem is well-posed if and only if the system is controllable. Otherwise, there exist goals you simply cannot reach — and no algorithm will find them."
— paraphrasing LaValle, Planning Algorithms, Ch. 15