"The lidar swore the corridor was clear. The depth camera saw a glass door. I trusted the one that was wrong about glass, and now I am part of the door."
A Chastened AI Agent
Prerequisites
This section assumes the body-state sensing of Section 57.1 (wheel odometry and IMU), which supplies the moving frame that exteroceptive readings are stamped into. It draws on the range-sensing physics of Chapter 40 and the lidar geometry of Chapter 42, the fusion taxonomy of Chapter 48, and the recursive-estimation view of Chapter 9. It is a companion to, not a duplicate of, the map-building of Chapter 52: here the map is a means to the immediate end of moving safely.
The Big Picture
Proprioception tells a robot where its body is; exteroception tells it where the world is. Navigation lives entirely in the second category, and no single exteroceptive sensor is trustworthy enough to steer by alone. A 2D lidar is crisp and long-range but sees one horizontal slice and is fooled by glass and by a table it passes underneath. A depth camera sees a dense frustum but only a few meters ahead and blinks in sunlight. Sonar is cheap and catches glass but is angularly blurry. Fusion for navigation is the discipline of pouring all of these, each with its own field of view, range, and failure surface, into one shared spatial representation that a motion planner can query with a single question: is the cell in front of me free or occupied. This section builds that representation and shows why the fusion happens in a common metric grid rather than in the sensors themselves.
What navigation asks of exteroception
A mobile robot planner does not want raw point clouds; it wants a decision surface. Concretely it needs two products. The first is a local obstacle field: for every patch of ground within a few meters, is it safe to drive over. The second is a global position fix: where is the robot in a map frame that does not drift, so a goal expressed as "the loading dock" stays put as the robot moves. Exteroceptive fusion produces both, and they have different tempos. The obstacle field must refresh at sensor rate (10 to 30 Hz) because a forklift can appear in a tenth of a second. The global fix can update slowly because the world's geometry does not move. Recognizing that navigation is really two fusion problems at two timescales, not one, is the first design decision, and it explains why production stacks keep a fast local costmap separate from a slow global localizer.
Both products are fusion in the strict sense of Chapter 48: they combine sources that are individually incomplete. The lidar defines the obstacle field's outline; the depth camera fills the low overhangs the lidar's single scan-plane misses; sonar patches the glass the other two pass straight through. The point is not redundancy but coverage of each other's blind spots.
One robot, many sensors, one frame
You cannot fuse readings that live in different coordinate frames, and every exteroceptive sensor reports in its own. A lidar return is a range and bearing in the lidar's frame, bolted somewhere on the chassis; a depth pixel is a point in the camera's optical frame, tilted down at the floor. The bridge is a static transform tree: each sensor's mounting pose relative to a common robot base is measured once, and forward kinematics (from Section 57.1) plus the odometry pose place that base in the world. Composing the chain lets a depth point and a lidar return be written as coordinates of the same metric grid cell, which is the precondition for fusing them at all.
This registration inherits the calibration discipline of Chapter 2: a two-degree error in a sensor's mounting angle throws a return at five meters off by roughly seventeen centimeters, enough to hallucinate an obstacle in a clear doorway or, worse, to erase a real one. Time alignment matters just as much. A scan captured while the robot spins must be stamped with the pose at capture time, not at arrival time, or the whole scan smears; this is the synchronization problem of Chapter 3 reappearing as motion distortion.
Key Insight
Navigation fuses in the map, not in the sensor. Rather than train one network to consume lidar, depth, and sonar jointly, the classical and still-dominant approach projects every sensor independently into a shared occupancy grid and lets the grid be the fusion operator. This works because occupancy is additive in log-odds: two sensors that both see a cell as occupied reinforce each other by simple summation, and a sensor's blind spot contributes nothing rather than a confident zero. The grid is a late-fusion substrate that is robust precisely because it demands no agreement about sensor models, only a shared frame and a per-sensor hit-or-miss vote.
The occupancy grid as a fusion substrate
The workhorse representation is the probabilistic occupancy grid (Moravec and Elfes, 1985): tile the ground into cells, and hold for each cell the posterior probability that it is occupied. The elegant part is the update. Storing occupancy in log-odds turns Bayesian fusion into addition. For cell \(m\) with prior log-odds \(l_0\), each measurement \(z_t\) contributes an inverse-sensor-model term, and the running estimate is $$l_t(m) = l_{t-1}(m) + \operatorname{logit}\, p(m \mid z_t) - l_0.$$ A ray that passes through a cell and ends beyond it votes "free" (a negative term); the cell where the ray terminates votes "occupied" (a positive term); unobserved cells are untouched. Because the terms simply sum, a depth camera and a lidar contribute to the same accumulator with no special fusion logic, and a sensor that never illuminates a cell leaves its estimate at the prior instead of corrupting it. Listing 57.2.1 implements one sensor's contribution for a 2D ray so the mechanism is concrete.
import numpy as np
L_OCC, L_FREE, L_MIN, L_MAX = 0.85, -0.4, -5.0, 5.0 # log-odds votes and clamps
def integrate_ray(logodds, res, origin, angle, rng, max_rng):
"""Fuse one range return into a log-odds occupancy grid (in place).
logodds: HxW grid; res: metres/cell; origin: sensor (x,y) in metres."""
hit = rng < max_rng # a miss = no obstacle seen, only free space
end = min(rng, max_rng)
n = int(end / res)
ox, oy = origin
for k in range(n): # walk the beam, marking free space
x = int((ox + k * res * np.cos(angle)) / res)
y = int((oy + k * res * np.sin(angle)) / res)
if 0 <= y < logodds.shape[0] and 0 <= x < logodds.shape[1]:
logodds[y, x] = np.clip(logodds[y, x] + L_FREE, L_MIN, L_MAX)
if hit: # only a true return marks a cell occupied
x = int((ox + rng * np.cos(angle)) / res)
y = int((oy + rng * np.sin(angle)) / res)
if 0 <= y < logodds.shape[0] and 0 <= x < logodds.shape[1]:
logodds[y, x] = np.clip(logodds[y, x] + L_OCC, L_MIN, L_MAX)
def prob(logodds): # recover occupancy probability when queried
return 1.0 - 1.0 / (1.0 + np.exp(logodds))integrate_ray once per lidar bearing and once per depth column, into the same logodds grid, fuses the two sensors by summation. The clip bounds keep any one sensor from saturating a cell, so a later disagreeing sensor can still change the estimate; the miss-versus-hit distinction is what lets a clear beam carve out free space.As Listing 57.2.1 shows, "fusion" here is nothing more than routing several sensors' rays into one accumulator, which is why the occupancy grid has survived four decades of navigation research. The clamps matter for navigation dynamics: bounding the log-odds keeps the map adaptive, so when a parked pallet is removed the cell can be voted back to free within a few scans instead of staying occupied forever.
From occupancy to a plannable costmap
A raw occupancy grid is not yet a thing a planner can drive on, because a robot has width and a cell marked "occupied" is dangerous for a whole robot-radius around it. Production navigation wraps the fused occupancy in a layered costmap: an obstacle layer that integrates the live sensors exactly as above, an inflation layer that grows a safety cushion around obstacles by the robot's footprint, and often a static layer holding the prior map from Chapter 52. The layers compose top to bottom into a single cost field the planner queries. Keeping each sensor and each concern in its own layer is what makes the stack debuggable: when the robot freezes in an open hallway, you can switch off the sonar layer and watch the phantom obstacle vanish.
The Right Tool
Hand-rolling multi-sensor fusion, ray-casting, footprint inflation, and layer composition into a real-time costmap is on the order of 1500 to 2500 lines of C++ once you handle transform lookups, out-of-range clearing, and rolling windows. The Nav2 costmap_2d package delivers all of it as configuration:
local_costmap:
plugins: ["obstacle_layer", "inflation_layer"]
obstacle_layer:
observation_sources: laser depth # both sensors fuse into one grid
laser: {topic: /scan, data_type: LaserScan, clearing: true, marking: true}
depth: {topic: /camera/points, data_type: PointCloud2, clearing: true, marking: true}
inflation_layer: {inflation_radius: 0.55, cost_scaling_factor: 3.0}The wiring of costmap_2d into a running robot is the subject of Section 57.7; here the point is that the fusion is a solved, reusable substrate.
In the field: a warehouse AMR that kept stopping at glass loading doors
A fleet of autonomous mobile robots (AMRs) ran a single 2D safety lidar and navigated cleanly until the loading bays were fitted with glass roll-up doors. The lidar passed straight through the glass, mapped the truck bay behind it as free space, and the robots drove into the doors. The fix was pure exteroceptive fusion: they added a downward-canted depth camera and a ring of ultrasonic sensors, and configured all three as marking sources in one obstacle layer like Listing 57.2.2. Sonar reflects off glass and voted the door occupied; the depth camera caught the low pallet jacks the lidar's scan-plane sailed over; the lidar still owned the long-range corridor picture. Door collisions went to zero, and the low-obstacle near-misses that nobody had been tracking dropped sharply as a side effect. The lesson is that the fix was not a better sensor but a second and third sensor covering the first one's specific blindness, fused in the grid where their disagreements resolve by voting.
Folding in a global fix
The obstacle field keeps the robot from hitting things, but it drifts with the odometry it is stamped against, so a goal set an hour ago slowly wanders. The second fusion product corrects this: exteroceptive readings are matched against a prior map to produce a drift-free pose in the map frame. Two mechanisms dominate. Scan-matching localization (adaptive Monte Carlo localization, AMCL) aligns the live lidar scan to the static map and outputs a corrected pose; where the sky is visible, a GNSS fix from Chapter 25 supplies an absolute anchor. Either way, the global fix is fused with the fast odometry through a recursive estimator in the Kalman family of Chapter 9, so the pose is smooth between corrections and accurate across them. The deep interplay between this exteroceptive fix and the proprioceptive dead-reckoning that carries the robot between fixes is important enough to get its own treatment in Section 57.4; here it is enough that the global fix and the local costmap are separate, cooperating products.
Research Frontier
The frontier is moving beyond the flat occupancy grid toward learned, three-dimensional navigation substrates. Bird's-eye-view and 3D-occupancy networks (Chapter 43) fuse multiple cameras and lidar into a dense, semantic occupancy volume that distinguishes drivable pavement from a curb the way a binary grid never could, and neural-field maps (Chapter 51) hold a continuous, queryable geometry instead of a fixed lattice. Open problems are running these representations inside the tens-of-milliseconds latency budget of a moving robot (Section 57.6), and giving them the calibrated uncertainty (Chapter 18) that the humble log-odds grid provides almost for free.
Exercise
Simulate two range sensors observing a small 2D scene with a glass wall (invisible to sensor A, opaque to sensor B) and a low obstacle (visible to A, invisible to B). Using Listing 57.2.1, integrate both sensors' beams into one shared log-odds grid, then into two separate grids. Threshold each to occupied/free and compare against ground truth. Quantify how many cells the fused grid gets right that neither single-sensor grid does, and identify the cell type where the fused grid still fails. Then vary L_MIN and observe how quickly a removed obstacle is forgotten.
Self-Check
1. Navigation fusion produces two separate products at two tempos. Name them and explain why keeping them separate is a deliberate design choice rather than an implementation accident.
2. Why does storing occupancy in log-odds turn multi-sensor fusion into simple addition, and what does clamping the log-odds buy you when a parked obstacle later drives away?
3. A robot repeatedly stops in the middle of a clear corridor. Give two specific fusion-side causes (one registration-related, one sensor-model-related) and how the layered-costmap structure helps you isolate which one it is.
What's Next
The fused occupancy field of this section answers "is this cell free," but it treats every obstacle as an anonymous blob of geometry. A navigating robot in a warehouse full of moving people and forklifts needs more: it must know that this blob is a person walking left and that one is a pallet at rest. In Section 57.3, we add the detection and tracking layer that turns the fused sensor stream into labeled, persistent objects with velocities, the representation a planner needs to yield to a pedestrian rather than merely swerve around a wall of points.