Planning, Navigation & Localization

9.1The Frame Contract

If you take one thing from this chapter, take this section. It is the single most senior thing you can say in a navigation interview, and it takes ninety seconds to say well.

map ──(localization / SLAM loop closure)──► odom ──(wheel/VIO odometry)──► base_link

Chapter 0 introduced these frames; now the payoff. Two transforms, two personalities:

• odom → base_link is published by odometry. It is continuous, smooth, and high-rate, and it drifts. It never jumps. Not once, not ever.
• map → odom is published by the localizer or the SLAM system. It is the correction: the difference between where odometry thinks you are and where you actually are. It jumps whenever a loop closure or a relocalization fires, and that is fine, because nothing latency-critical reads it directly.

Why split them at all? Because two consumers want contradictory things. The local controller, the code deciding wheel velocities right now, needs a pose that is continuous: a feedback loop that sees its input teleport 30 cm will command a violent correction for motion that never happened. The global planner needs a pose that is correct, drift included, or its paths start and end in the wrong places. You cannot have both properties in one transform, so the convention gives you two and composes them. The controller lives in odom. The planner lives in map. When loop closure snaps the map, map→odom absorbs the entire jump, the global path replans, and the wheels never feel a thing.

9.2Localization on Yesterday's Map

SLAM built the map. In deployment you usually freeze it and just localize: cheaper, more stable, repeatable, and auditable. The reference recipe is Monte Carlo Localization (MCL), so we go deep on it, then survey the alternatives. (You will also meet it under the name of its best-known implementation, AMCL; the parameter vocabulary below comes from there and has equivalents in every clone.)

Monte Carlo Localization, the Particle Filter You Actually Ship

MCL is the particle filter from Chapter 1 applied to one specific problem: estimate (x, y, θ) against a known 2D occupancy grid. A few thousand pose hypotheses (particles) live in the map. Each cycle: predict every particle forward using odometry plus sampled noise, weight each by how well the live laser scan agrees with the map as seen from that particle, resample so heavy particles multiply and light ones die. The "adaptive" part is KLD sampling: the particle count itself shrinks when the cloud is tight and confident, and grows when it is spread out and unsure, so you pay compute only for uncertainty.

An MCL implementation has dozens of parameters and about ten that matter. Learn these and you can tune any of them (Setup B builds the whole filter from scratch so none of them stay abstract):

Global Relocalization and the Kidnapped Robot

Power the robot on anywhere, or carry it while it's off (the classic "kidnapped robot" test). Triggering global relocalization (every MCL implementation exposes the switch) scatters particles uniformly over free space and lets a few seconds of driving kill the wrong ones. It works when the environment is distinctive and fails in symmetric ones: two identical corridors produce a genuinely multimodal belief, two blobs of probability, and only driving past something asymmetric resolves it. Particle filters can represent two blobs; a Kalman filter, being a single Gaussian, cannot. That is the whole reason Monte-Carlo methods own global localization.

Kidnapped robot problem

The robot is moved without being told; the localizer must notice its belief is wrong and recover globally, not just track.

Particle deprivation

Resampling killed all particles near the true pose. Nobody is left to be right; the filter is confidently wrong.

KLD sampling

Adapting the particle count so the sample-based belief stays within a KL-divergence bound of the true posterior. The A in AMCL.

Likelihood field

A precomputed distance-to-nearest-obstacle grid; scoring a beam becomes one lookup instead of a raycast.

The Alternatives, and How to Pick

The picking rule is short: match the localizer to the map you actually have and the sensor you actually trust. If you built a 2D grid and carry a planar lidar, run MCL and spend your week elsewhere. Reach for scan-to-map ICP when you need centimeter repeatability, and for visual relocalization when geometry is ambiguous but appearance is not.

9.3From Map to Plannable Structure

Planners do not read occupancy grids; they read costmaps. A costmap is built by stacking layers, each painting cost into a shared grid:

• Static layer: the SLAM map from disk. Walls that existed yesterday.
• Obstacle layer: live sensor marking and clearing via raycast. Things that exist right now.
• Voxel / spatio-temporal layer: 3D-aware versions. A voxel layer keeps a column of voxels per cell and projects down; the spatio-temporal variant (Macenski's STVL design) replaces raycast-clearing with time-based decay, which scales to 3D lidars and depth cameras without eating a CPU core.
• Inflation layer: paints a decaying cost skirt around every lethal cell, so "legal but uncomfortable" exists as a concept the planner can trade off.

The inflation math is worth knowing exactly, because it explains the most mis-set parameter in robot navigation. A cell containing an obstacle costs 254 (lethal). Any cell closer than the robot's inscribed radius costs 253: the center being there guarantees a collision, no orientation can save you. Beyond that, cost decays exponentially:

cost(d) = 252 · exp( −cost_scaling_factor · (d − inscribed_radius) )   for d > inscribed_radius,
          until d > inflation_radius, where cost = 0

Why is inflation_radius the most mis-set parameter in navigation? Because people set it to "about the robot radius" and stop, which produces a costmap that is lethal-or-free with almost no gradient in between. Every planner and every controller critic in the sections below reads this cost surface. With no skirt, A* happily plans one cell away from walls, MPPI's cost critic has nothing to push against until it is already too late, and your robot hugs walls with total confidence. The standard guidance is the opposite of the folk habit: inflate generously (well past the robot radius) with a gentle cost_scaling_factor, so cost decays smoothly across most of the free space and everything downstream gets a usable gradient.

One more structural fact: there are two costmaps. The global costmap covers the whole map in the map frame and feeds the planner. The local costmap is a small rolling window (a few meters square) centered on the robot, usually in the odom frame, updated fast, and feeds the controller. This is the frame contract again, in costmap form: the global one is allowed to be big, slow, and jump-corrected; the local one must be small, fast, and smooth.

For 3D platforms the plannable structure changes representation: project OctoMap down to 2.5D, or build a TSDF→ESDF distance field (Voxblox, nvblox, FIESTA, all from Chapter 3) so every point in space knows its distance and direction to the nearest obstacle.

9.4Global Planners

Dijkstra expands cells from the start, cheapest total cost first, and is complete and optimal on the grid. A* adds a heuristic, an optimistic guess of remaining cost (straight-line distance, usually), and expands best-guess-first instead. Same guarantees if the heuristic never overestimates, far fewer cells touched. That is the entire difference; everything else in this section is about what "the grid" fails to capture.

The lineage, oldest first:

• The wavefront planner: the legacy default of every stack, a Dijkstra (optionally A*) flood over the costmap with gradient descent to extract the path. Reliable, fast, and it treats your robot as a holonomic point.
• Cost-aware A*: a modern A* over the same grid that trades path length against accumulated cell cost, with smoothing built in. The drop-in upgrade for differential-drive robots that can turn in place, and what Setup A implements.
• Hybrid-A*: search in (x, y, θ) where every expansion is a motion primitive, a short Dubins or Reeds-Shepp arc the vehicle can physically drive. Periodically it attempts an analytic expansion: shoot one exact Dubins/RS curve straight at the goal, and if it is collision-free, finish early. This is the direct descendant of the DARPA Urban Challenge planner and the answer for anything car-like.
• State Lattice: like Hybrid-A* but the primitives come from a precomputed lattice generated for your kinematics, including omni and legged-ish motion models, instead of hardcoded car curves.

The phrase to internalize: grid-feasible is not drivable. A holonomic grid planner will cheerfully produce a path with a 90° corner in a hallway for a robot with a 1.2 m turning radius. Every cell of that path is free space; no continuous motion of that vehicle can follow it. Kinematically constrained platforms need the constraint inside the search, not smoothed on afterward, which is exactly what Hybrid-A* and State Lattice do.

Off the grid entirely: sampling-based planners (RRT, RRT*, PRM, in OMPL and MoveIt) grow a random tree through free space instead of discretizing it. They are the answer for high-dimensional problems; a 7-joint arm lives in 7-D, where grids explode exponentially. RRT* is asymptotically optimal, meaning the path keeps improving as samples accumulate.

9.5Local Planners and Controllers

The global path is a suggestion. The controller turns it into velocity commands at 20 Hz while dodging whatever the map didn't know about. Four names cover the field:

• DWA, the Dynamic Window Approach (Fox, Burgard, Thrun, 1997): sample reachable (v, ω) pairs from the current velocity's "dynamic window", forward-simulate each for a second or two, score the arcs with critics (progress, clearance, path distance), pick the winner. Later implementations made the critics pluggable. Legacy, but the vocabulary (rollout, critic, scoring) survives in everything since.
• TEB, Timed Elastic Band (Rösmann et al.): treat the local trajectory as an elastic band of poses with timestamps and optimize it as a sparse graph under kinodynamic constraints, obstacles pushing, the path pulling, time shrinking. True trajectory optimization, excellent for car-like footprints, and famously fiddly: it optimizes within one homotopy class and can oscillate when two ways around an obstacle score similarly.
• MPPI, Model Predictive Path Integral (Williams et al., ICRA 2017), the modern default for wheeled robots: every cycle, sample a batch of control sequences (thousands of rollouts, batch_size × time_steps), simulate each through the motion model, score each rollout with a sum of critics (cost functions: obstacle cost, path alignment, path following, goal, prefer-forward, constraint limits), then blend the rollouts into one command with softmax weighting controlled by temperature. Low temperature acts like argmax over rollouts, sharp but jittery; higher temperature averages more broadly and smooths. No gradients required, so critics can be anything, and the sampling is embarrassingly parallel.
• Regulated Pure Pursuit (Macenski et al., after Coulter's 1992 pure pursuit): chase a lookahead point on the path, with speed regulated down by curvature and obstacle proximity. It does not avoid obstacles; it tracks a path exactly. That exactness is why it wins at docking, row-following, and anywhere the path is the specification.

The senior framing: DWA and TEB are optimizers over a small structured space (arcs, one band); MPPI is a sampler over an unstructured one. Sampling costs compute and buys robustness to non-convex, fast-changing cost surfaces, which is precisely what a cafe full of chair legs and ankles is.

9.6Behavior Trees Run the Show

Above the planners sits orchestration, and the industry pattern is a behavior tree, typically built on BehaviorTree.CPP, a plain C++ library with no framework attached. A production navigation tree reads roughly as: replan the global path once a second, follow it continuously, and if either fails, walk a recovery sequence. The standard recoveries: clear costmaps (forget possibly-stale obstacles), spin (rotate in place to refresh perception), wait (the world may unblock itself; people move), back up (get out of a lethal pocket). Each recovery is followed by a retry of the thing that failed.

Why did BTs replace the state machines that ran the previous generation of navigation stacks? Because recovery logic is combinatorial. A finite-state machine encoding "if planning fails twice, clear the global costmap, but if following fails, first wait then spin, unless we already spun" turns into a transition-table hairball nobody can review. A BT expresses the same policy as a readable tree of fallbacks and sequences, each node reusable, the whole thing swappable per robot via one XML file, no recompilation. You can hand a fleet operator a different recovery policy as a config change.

Two extensions you should be able to sketch in an interview: waypoint following (a loop node wrapping navigate-to-pose, with a task hook at each stop: take a photo, wait for confirmation, charge) and custom recoveries (write a new BT action node in C++, insert it into the XML; a common one is "call for help", which pings a human and blocks until teleop releases it).

import heapq
import numpy as np
import matplotlib.pyplot as plt
from PIL import Image
from scipy.ndimage import distance_transform_edt

RES, R_INS, R_INFL, K = 0.05, 0.22, 0.55, 3.0   # m/cell, radii, decay

occ = np.array(Image.open('map.pgm').convert('L'))  R_INS) & (d = 253: continue          # lethal or inscribed
            ng = g[cur] + step + w_cost * cost[nxt] / 252
            if ng < g.get(nxt, np.inf):
                g[nxt], came[nxt] = ng, cur
                heapq.heappush(pq, (ng + h(nxt), nxt))

path = astar(cost, (40, 35), (410, 520))
plt.imshow(cost, cmap='magma'); ys, xs = zip(*path)
plt.plot(xs, ys, lw=2, color='cyan'); plt.savefig('plan.png', dpi=150)

9.7Safety Is a Separate Layer

The planner is not the safety system. Say that sentence early in any interview that touches deployment. The navigation stack is where you put intelligence; safety goes in layers below it, each simpler and more auditable than the one above:

• A collision monitor: a thin process between the controller and the base that watches raw sensor data against configured polygons (a stop zone, a slowdown zone, an approach zone that scales speed by time-to-collision) and clamps or zeroes the velocity command regardless of what the controller wanted. Software, but simple software, with no map, no planner, no belief.
• Certified lidar safety fields: safety-rated scanners (SICK, Hokuyo UAM) evaluate configurable protective fields inside the sensor and open a hardware output when violated. No software stack in the loop. This is what a safety audit actually accepts.
• The e-stop chain: a hardware circuit (safety relays, STO inputs on motor drivers) that removes torque. It must function with every computer on the robot crashed. If your e-stop is a software message, you do not have an e-stop.

These layers exist because they answer at different timescales, which is the shape of the standard interview question: a person steps in front of the robot. The safety field reacts in milliseconds, independent of all software above it. The controller reacts within a control cycle, tens of milliseconds, steering rollouts away from the new cost. The costmap and global replan react in hundreds of milliseconds if the route must change entirely. Safety is the layer you certify; the other two are the layers you tune.

9.8SLAM and Navigation, Coupled Honestly

The clean story is "SLAM makes the map, navigation uses it". Real deployments are messier, and the mess is where senior candidates separate from junior ones:

1. Mapping while navigating. People walking by get baked into the map as phantom walls. Keep the static layer (the SLAM map) separate from a live obstacle layer with time-based decay, and gate any map updates on repeated, confident observation.
2. Loop closure mid-plan. Handled, if you honored §8.1. The map jumps, map→odom absorbs it, the global path replans, the controller never flinches. Violate the contract (plan or control against map-frame poses directly) and the same event produces a robot that lunges sideways. §8.10 has the war story.
3. Frozen maps vs lifelong mapping. A frozen map is stable, testable, certifiable, and wrong within a month in any warehouse where pallets move. Lifelong mapping (continued pose-graph sessions from Chapter 3, RTAB-Map's incremental memory from Chapter 5) keeps the map current at the cost of a moving baseline: yesterday's regression test ran on a map that no longer exists. Most products freeze the map and schedule re-mapping, because auditability beats freshness until the environment churns fast enough to force the issue.
4. Multi-session. Merging Tuesday's map with Wednesday's requires anchoring both to a shared frame: relocalize in the old map first, then extend it. RTAB-Map does this natively; 2D pose-graph mappers do it via serialized graphs. The failure smell is a map file that grows without bound while quality degrades, because sessions were appended without loop closures binding them.
5. No loop closures available. Short drone sortie, GPS-denied sprint, disposable inspection run: do not pretend. Plan entirely in the odom frame and accept bounded drift. Common, correct, and worth saying out loud because it shows you know the map frame is optional.
6. Where goals come from. Trace it end to end on a product: operator clicks a map tile in the fleet UI → goal pose in the map frame → navigate-to-pose request → behavior tree → planner → controller → velocity command → base driver. Owning that trace, with the frames named at each hop, is a complete interview answer by itself.
7. Learning-based navigation, one sentence of currency: navigation foundation models (ViNT, NoMaD) and RL local planners are promising, and production in 2026 is still a classical stack with learned perception feeding it (Chapter 7's trade-offs apply unchanged).

9.9Drones and 3D in One Honest Section

Same skeleton, different flesh. A quadrotor still has localization, a world representation, a global plan, a local plan, and a safety layer; every box just changes contents. The costmap becomes an ESDF (Voxblox, nvblox, FIESTA), because a drone needs true 3D clearance and, more usefully, the gradient of clearance, which pushes trajectories away from obstacles during optimization.

The planning trick that makes aerial trajectories tractable is differential flatness: quadrotor dynamics are such that position and yaw trajectories, with their derivatives, determine the full state and inputs, so you can plan a smooth curve in space and recover attitude and thrust afterward. The classic realization is minimum-snap polynomial trajectories (Mellinger & Kumar, ICRA 2011): piecewise polynomials through waypoints, minimizing the fourth derivative of position, because snap maps to control effort. In clutter, the modern local planner is EGO-Planner, which cleverly avoids building the full ESDF and optimizes B-spline trajectories against only the obstacles that actually conflict, with Fast-Planner as the ESDF-based alternative lineage.

When asked to compare stacks, resist the urge to describe two unrelated systems. Walk the same five boxes twice: VIO instead of wheel odometry, ESDF instead of costmap, kinodynamic polynomial search instead of Hybrid-A*, geometric tracking controller instead of MPPI, and a certified height/geofence layer instead of lidar safety fields. Interviewers reward the parallel structure because it is how people who have built both actually think.

9.10Tuning War Stories

Five failure signatures, each with the cause and the knob. These are the ones that eat afternoons.

rng = np.random.default_rng(3)
free = np.argwhere(~occ)
N_BEAMS, Z_HIT, SIGMA = 8, 0.8, 0.25          # the z_hit / sigma_hit knobs

def raycast(pose, n=N_BEAMS, max_r=6.0):      # simulated lidar endpoints
    x, y, th = pose
    angs = th + np.linspace(0, 2*np.pi, n, endpoint=False)
    pts = []
    for a in angs:
        for r in np.arange(0.1, max_r, RES):
            px, py = x + r*np.cos(a), y + r*np.sin(a)
            i, j = int(py/RES), int(px/RES)
            if not (0 <= i < occ.shape[0] and 0 <= j < occ.shape[1]) or occ[i, j]:
                break
        pts.append((px, py))
    return np.array(pts)

def weight(particle, scan_pts_body):           # likelihood-field model
    x, y, th = particle
    c, s_ = np.cos(th), np.sin(th)
    w = 1.0
    for bx, by in scan_pts_body:               # beam endpoints into map frame
        mx, my = x + c*bx - s_*by, y + s_*bx + c*by
        i, j = int(my/RES), int(mx/RES)
        if 0 <= i < d.shape[0] and 0 <= j < d.shape[1]:
            w *= Z_HIT * np.exp(-d[i, j]**2 / (2*SIGMA**2)) + (1-Z_HIT)*0.1
        else:
            w *= 0.05
    return w

# init: N particles scattered over free space (global localization)
N = 2000
P = np.column_stack([free[rng.integers(0, len(free), N)][:, ::-1] * RES,
                     rng.uniform(-np.pi, np.pi, N)])

truth = np.array([2.0, 2.0, 0.0])
for step in range(120):
    u = np.array([0.06, 0.0, 0.03])                       # forward + slight turn
    truth += [u[0]*np.cos(truth[2]), u[0]*np.sin(truth[2]), u[2]]
    delta = raycast(truth) - truth[:2]                    # endpoints, map-aligned
    ct, st = np.cos(-truth[2]), np.sin(-truth[2])         # rotate into body frame
    scan = np.column_stack([ct*delta[:,0] - st*delta[:,1],
                            st*delta[:,0] + ct*delta[:,1]])
    P += np.column_stack([u[0]*np.cos(P[:,2]), u[0]*np.sin(P[:,2]),
                          np.full(N, u[2])])              # predict
    P += rng.normal(0, [0.01, 0.01, 0.008], P.shape)      # motion noise (alphas!)
    W = np.array([weight(p, scan) for p in P]); W /= W.sum()
    n_eff = 1.0 / np.sum(W**2)
    if n_eff < N/2:                                       # adaptive resampling
        P = P[rng.choice(N, N, p=W)]
    est = np.average(P, axis=0, weights=W)
    print(f'{step:3d}  spread {P[:,:2].std():.2f} m   err {np.hypot(*(est[:2]-truth[:2])):.2f} m')

9.11Reading List

9.12Interview Questions

· End of the Book: You Now Own the Skeleton ·

Go back through the interview questions in every chapter and answer them out loud, cold. When you can do that, the word in the title is yours.