Path Planning & Motion Control

5.1The Three-Layer Stack

Navigation is a hierarchy because no single algorithm can be simultaneously globally optimal, dynamically feasible, and fast enough to dodge a pedestrian. So the field splits the problem by horizon: a global planner finds a route through the whole map and can afford to be slow; a local planner turns the next few metres of that route into a dynamically feasible trajectory around fresh obstacles, fast; a tracking controller turns the trajectory into wheel commands at control rate. Each layer trusts the one above for direction and the one below for execution, and every layer reads Phase 2’s state estimate.

The planner’s world model is the costmap: the Phase 3 map with obstacles inflated. Planning treats the robot as a point, so every obstacle grows by the robot’s radius (the configuration-space trick), then by a decaying safety margin — cost falls off from lethal at the obstacle through "inscribed" (guaranteed collision for the footprint) to a soft exponential buffer that makes planners prefer the middle of corridors. The ESDF of Phase 3.4 is this idea done continuously: distance is the cost, and its gradient points to safety.

Configuration space (C-space)

The space of robot configurations. Inflating obstacles by the robot radius turns "will this footprint collide" into "is this point in collision" — planning’s favorite simplification.

Kinodynamic feasibility

Whether a trajectory respects the robot’s motion limits — velocity, acceleration, turning radius. Global paths usually aren’t; local trajectories must be.

Anytime planner

Returns a valid (suboptimal) solution fast, then improves it while time remains. RRT* and weighted-A* variants are the classic examples.

Recovery behavior

What runs when planning fails: rotate in place, back up, clear the costmap. Unsexy, and the difference between a stuck robot and a support ticket.

5.2Graph Search: A*, D* Lite, JPS

On a grid costmap, planning is shortest-path search, and A* is the reference algorithm: expand nodes in order of f = (cost so far) + (heuristic cost to go), and the heuristic’s honesty is the entire contract:

f(n) = g(n) + h(n)          g: best known cost from start
                            h: estimated cost to goal

h admissible  (never overestimates)           → A* returns the optimal path
h consistent  (h(n) ≤ c(n,n') + h(n'))        → each node expanded only once
h = 0                                          → Dijkstra (blind, fat search)
f = g + w·h,  w > 1  ("weighted A*")           → faster, cost ≤ w × optimal

grids: octile heuristic for 8-connected — max+min formula of |Δx|,|Δy|

Two SOTA refinements matter on real robots. D* Lite solves the replanning problem: mid-route, a door turns out closed — a fresh A* from scratch wastes everything learned. D* Lite searches backward from the goal (so cost-to-goal values stay valid as the robot moves) and, when edge costs change, repairs only the locally inconsistent nodes — those where the one-step lookahead rhs(u) = min over successors of c(u,s’) + g(s’) disagrees with g(u). Typical repairs touch a few percent of the map. Jump Point Search attacks uniform grids’ symmetry: a thousand equal-cost staircase paths cross an open room, and plain A* dutifully expands them all. JPS proves that only jump points (where an optimal path is forced to turn, at obstacle corners) need expansion, and leaps straight lines between them — order-of-magnitude speedups, exact same optimal path, zero preprocessing; JPS+ precomputes the jump distances for another large factor.

// astar.cc — A* on a grid; w=0 Dijkstra, w=1 optimal, w>1 bounded-suboptimal.
#include <algorithm>
#include <cmath>
#include <iostream>
#include <queue>
#include <vector>

int main() {
  std::vector<std::string> map = {          // S start, G goal, # wall
    "..................................................",
    "..................................................",
    "......S...............#..........................",
    ".......................#.........................",
    "........................#........................",
    ".........########################.................",
    ".........#......................#.................",
    ".........#......................#.................",
    ".........#........########......#.................",
    ".........#........#......#......#.................",
    ".........#........#..G...#......#.................",
    ".........#........#......#......#.................",
    ".........#........#......#####..#.................",
    ".........#........#..............#................",
    ".........##########...............#...............",
    "..................................................",
    "..................................................",
  };
  const int H = map.size(), W = map[0].size();
  int sx, sy, gx, gy;
  for (int y = 0; y < H; ++y) for (int x = 0; x < W; ++x) {
    if (map[y][x] == 'S') { sx = x; sy = y; }
    if (map[y][x] == 'G') { gx = x; gy = y; }
  }
  const int DX[8] = {1,-1,0,0,1,1,-1,-1}, DY[8] = {0,0,1,-1,1,-1,1,-1};

  for (double w : {0.0, 1.0, 2.5}) {
    std::vector<double> g(W * H, 1e18);
    std::vector<int> par(W * H, -1);
    auto h = [&](int x, int y) {              // octile distance (admissible)
      double dx = std::abs(x - gx), dy = std::abs(y - gy);
      return std::max(dx, dy) + 0.41421356 * std::min(dx, dy);
    };
    using QE = std::pair<double, int>;        // (f, index)
    std::priority_queue<QE, std::vector<QE>, std::greater<>> open;
    g[sy * W + sx] = 0.0;
    open.push({w * h(sx, sy), sy * W + sx});
    long expanded = 0;

    while (!open.empty()) {
      auto [f, id] = open.top(); open.pop();
      const int x = id % W, y = id / W;
      if (f > g[id] + w * h(x, y) + 1e-9) continue;   // stale entry
      ++expanded;
      if (x == gx && y == gy) break;
      for (int k = 0; k < 8; ++k) {
        const int nx = x + DX[k], ny = y + DY[k];
        if (nx < 0 || ny < 0 || nx >= W || ny >= H || map[ny][nx] == '#') continue;
        const double nc = g[id] + ((k < 4) ? 1.0 : 1.41421356);
        if (nc < g[ny * W + nx]) {
          g[ny * W + nx] = nc;  par[ny * W + nx] = id;
          open.push({nc + w * h(nx, ny), ny * W + nx});
        }
      }
    }
    auto shown = map;                          // overlay the path
    for (int id = gy * W + gx; id != -1; id = par[id])
      if (shown[id / W][id % W] == '.') shown[id / W][id % W] = 'o';
    std::cout << "w = " << w << "   expanded " << expanded
              << " nodes   path cost " << g[gy * W + gx] << "\n";
    for (auto& row : shown) std::cout << "  " << row << "\n";
  }
}

5.3Sampling-Based Planning: RRT*, PRM, and the Informed Trick

Grids die of dimensionality: fine discretization in 6-DoF (an arm, a drone in SE(3)) is astronomically many cells. Sampling-based planners skip the discretization: draw random configurations, keep the collision-free ones, connect near neighbors, and let the graph emerge. PRM builds that roadmap offline for many queries (LazyPRM* defers expensive collision checks until a candidate path actually proposes an edge, validating lazily); RRT grows a tree online from the start, pulled toward unexplored space; and RRT* adds the two moves that buy asymptotic optimality — as samples n → ∞, the best path in the tree converges to the true optimum:

RRT* per sample x_new:
  1. choose parent: among tree nodes within radius r_n, wire x_new
     to the neighbor minimizing (cost-to-come + edge cost)
  2. rewire: for each neighbor, if routing THROUGH x_new is cheaper,
     swap its parent — the tree continuously repairs itself

  r_n = γ · (log n / n)^(1/d)        shrinks as the tree densifies

Informed RRT*: once any solution of cost c_best exists, the only samples
that can improve it lie in the ellipsoid  ‖x−x_start‖ + ‖x−x_goal‖ ≤ c_best
→ sample only there; the ellipse shrinks with every improvement.

5.4Local Planning: DWA and TEB

The global path ignores dynamics and fresh obstacles; the local planner owns both. DWA — the dynamic window approach — is honest brute force in velocity space: consider only the (v, ω) pairs reachable within one control cycle under acceleration limits (the "dynamic window"), forward-simulate each for a couple of seconds through the local costmap, score, pick the best, repeat at 20 Hz:

window:  V = { (v, ω) :  |v − v_now| ≤ v̇_max·Δt,  |ω − ω_now| ≤ ω̇_max·Δt }
         ∩ { admissible: can still stop before the nearest obstacle }

score:   G(v, ω) = α·heading(goal alignment) + β·clearance + γ·velocity
→ simulate every candidate, take argmax, emit one (v, ω), repeat

DWA’s weakness is myopia: it commits to one arc at a time, so a robot facing a U-shaped obstacle happily maximizes progress into the pocket, then oscillates. TEB — timed elastic band — plans a whole trajectory instead: take the global path’s next stretch as a band of poses with time intervals, and optimize band and times jointly (a sparse graph problem, solved with g2o — Phase 1’s loop yet again) under objectives for time-optimality, obstacle clearance, and kinodynamic limits; the standalone C++ library plans in several path topologies in parallel (left of the pillar vs right) and picks the best, which is exactly the decision DWA cannot represent. The price of both: tuning. Weights α, β, γ and their TEB equivalents are robot- and site-specific, and every practitioner has a scar story.

5.5Path Tracking: Pure Pursuit and Stanley

Below planning sits the controller — given the trajectory, produce commands now. Two geometric controllers own the classical tier. Pure pursuit: pick the point on the path one lookahead distance L_d ahead, and command the unique circular arc that passes through it:

pure pursuit:  α = bearing of lookahead point in the robot frame
               κ = 2·sin(α) / L_d          curvature of the joining arc
               ω = v·κ                      (L_d often scheduled: L_d = k·v)

Stanley:       δ = θ_e + arctan( k·e_fa / (k_s + v) )
               heading error + cross-track correction at the front axle

Pure pursuit is beloved because it is ten lines, has one knob, and degrades gracefully. Stanley (the controller that won the DARPA Grand Challenge aboard the eponymous VW) instead regulates the cross-track error at the front axle directly, plus heading alignment — it converges to the path exactly rather than orbiting it, at the cost of more sensitivity to noise at speed. Beyond both sits full MPC tracking, which is the next section wearing its controller hat.

5.6MPC and MPPI: Optimization at Control Rate

Model predictive control collapses the planner/controller boundary: at every tick, solve a short optimal-control problem — minimize tracking error plus effort over the next N steps, subject to the robot’s actual dynamics and constraints — apply the first control, shift the horizon, repeat:

MPC, each tick:   min_{u₀..u_{N−1}}  Σₖ ℓ(xₖ, uₖ) + ℓ_f(x_N)
                  s.t.  xₖ₊₁ = f(xₖ, uₖ),   uₖ ∈ U,   xₖ ∈ X_free

nonlinear f → NMPC: transcribe (multiple shooting) and solve the NLP with
CasADi (+IPOPT) or ACADO's code-generated fast solvers — kHz-rate on ARM.

MPPI (sampling instead of gradients):
  draw K control sequences  uₖ + εₖ,   rollout each on the GPU, cost Sₖ
  wₖ = exp(−(Sₖ − min S)/λ)             softmin weighting
  u  ← Σ wₖ(uₖ + εₖ) / Σ wₖ             the noise-weighted average, repeat

The two branches split on differentiability. NMPC (via CasADi or ACADO) wants smooth dynamics and costs, and repays them with tight constraint handling and mature solvers. MPPI — model predictive path integral control — asks nothing of the cost function: sample thousands of trajectories, weight by exponentiated cost, average. Collision indicators, discrete gates, learned costs — all legal, because nothing is differentiated; a modern GPU rolls out 10,000 samples of a 2 s horizon fast enough for 30–50 Hz control, which is why GPU-MPPI has become the SOTA local planner for aggressive ground robots and the default advanced controller in modern navigation stacks. Its failure mode is its honesty: with too few samples or too-cold λ it averages between modes ("half left, half right of the pillar") — the same multimodality TEB solves with parallel topologies.

// pursuit.cc — pure pursuit on a 4×4 m square path, unicycle model.
#include <cmath>
#include <iostream>
#include <vector>

struct P { double x, y; };

int main() {
  std::vector<P> path;                       // dense square, 5 cm spacing
  auto edge = [&](P a, P b) {
    const int n = 80;
    for (int i = 0; i < n; ++i)
      path.push_back({a.x + (b.x - a.x) * i / n, a.y + (b.y - a.y) * i / n});
  };
  edge({0,0},{4,0}); edge({4,0},{4,4}); edge({4,4},{0,4}); edge({0,4},{0,0});

  for (double Ld : {0.15, 0.6, 1.8}) {
    double x = 0.2, y = -0.3, th = 0.3;      // start off-path, misaligned
    const double v = 0.5, dt = 0.02;
    double sum_e = 0.0, max_e = 0.0;  int steps = 0;

    for (double t = 0.0; t < 40.0; t += dt, ++steps) {
      size_t near = 0;  double dbest = 1e18;  // nearest path sample
      for (size_t i = 0; i < path.size(); ++i) {
        const double d = std::hypot(path[i].x - x, path[i].y - y);
        if (d < dbest) { dbest = d; near = i; }
      }
      sum_e += dbest;  max_e = std::max(max_e, dbest);

      size_t tgt = near;                      // walk forward to lookahead
      while (std::hypot(path[tgt].x - x, path[tgt].y - y) < Ld)
        tgt = (tgt + 1) % path.size();

      const double ax = path[tgt].x - x, ay = path[tgt].y - y;
      const double alpha = std::atan2(ay, ax) - th;      // bearing in body
      const double kappa = 2.0 * std::sin(alpha) / Ld;   // the whole law
      const double w = std::max(-2.5, std::min(2.5, v * kappa));

      th += w * dt;                           // unicycle integration
      x  += v * std::cos(th) * dt;
      y  += v * std::sin(th) * dt;
    }
    std::cout << "Ld = " << Ld << " m   mean cross-track "
              << sum_e / steps << " m   max " << max_e << " m\n";
  }
}

5.7Choosing the Stack, and How It Fails

5.8Reading List

5.9Interview Questions