Section 30.7: Field Navigation Under Degraded Sensing

"A plan is only smart if the wheels, floor, people, and clock all agree to it."

A Local Planner With Commitment Issues

Problem First

Picture the same robot driving the same corridor twice: the first pass returns fifty thousand clean lidar points and a confident pose, the second pass (now thick with kicked-up dust) returns ten thousand points of mostly noise and a pose estimate quietly drifting toward a wall. Field navigation is the discipline of surviving the second pass, and it starts when the assumptions break. Dust, darkness, wheel slip, GPS denial, glass, crowds, and map aging make the planner reason about confidence and fallback behavior. A lidar that returns clean geometry indoors may yield 80% noise returns in a dusty mine corridor, cutting usable points from 50,000 to under 10,000 per scan; this is called the sensing floor problem, and every field-deployable planner must define its response before the robot leaves the lab.

The sensing floor matters because point count governs everything downstream. Once point count drops below a geometry-dependent threshold, localization confidence, costmap density, and obstacle detection all degrade non-linearly. On a physical platform this typically produces not just reduced accuracy but abrupt failure. Iterative Closest Point (ICP) scan matching diverges when feature overlap falls below roughly 30% (a widely reported empirical threshold, as of 2024, though the exact knee shifts with scene geometry). In physical terms, asking ICP to align two scans with less than 30% overlap is like assembling a jigsaw puzzle when 7 of every 10 pieces belong to a different box. The pose estimate then jumps. That jump can trigger incorrect replanning and steer the robot into the very obstacle it could no longer see.

The floor is determined by treating the sensor model statistically. Each return is classified as a valid surface reflection or a noise return based on intensity, return time, and neighbor density. The planner counts valid returns per scan, computes a rolling percentile over a short window, and compares it against a per-environment calibration baseline. When the ratio drops below the calibrated threshold, the confidence signal fires and the degraded-mode policy takes over.

From Detecting the Floor to Acting on It

Defining that response is not a coding detail tacked on at the end. You have to write it into the planner's objective from the start. The planning problem itself must therefore carry the confidence signal as a first-class constraint. The field stack must define degraded-mode policies before deployment. It should know when to slow down, switch sensors, request teleoperation, hold position, or retreat. Continuing without a confidence bound is not autonomy; it is unmanaged risk. A robot that navigates flawlessly in a clean lab but has no fallback for a dusty corridor is not a field system: it is a demonstration waiting for better weather.

Formal Model

Most navigation methods can be read as constrained search or optimization:

$$ u_t\in\mathcal U_{\mathrm{safe}}(b_t),\quad b_t=p(x_t,m,\text{hazards}\mid z_{\le t},u_{<t}) $$

Here \(u_t\) is the control action chosen at time \(t\), \(b_t\) is the belief state (a probability distribution over robot pose \(x_t\), map \(m\), and hazards, given all sensor observations \(z_{\le t}\) and past actions \(u_{<t}\)), and \(\mathcal U_{\mathrm{safe}}(b_t)\) is the set of actions judged safe under that belief. The cost term names what the robot wants. The constraints name what reality permits: collision clearance, velocity and acceleration limits, curvature bounds, kinodynamic feasibility (motion limited by both the robot's geometry and its velocity/acceleration bounds), perception confidence, and safety monitors.

Figure 30.7.2 traces this loop end to end: the monitors on the left feed a confidence gate, a crossed bound routes the robot into a fallback policy rather than nominal driving, and that fallback writes a replay log that the monitors read on the next pass.

Worked Diagnostic

The loop in Figure 30.7.2 is abstract until you see the fallback-selection step decide a real case, so the next fragment collapses that whole diagram down to its single most consequential branch: turning monitored signals into a chosen action. Code Fragment 1 isolates the planning idea in a tiny runnable example. The goal is not to replace Nav2 or OMPL; the goal is to make the invariant visible before the full stack absorbs it.

# Select a degraded-mode response from confidence signals.
# The policy chooses the least risky action before full failure.
signals = {"localization": 0.42, "costmap_age_s": 3.8, "clearance_m": 0.31}
if signals["localization"] < 0.5:
    action = "hold_and_relocalize"
elif signals["costmap_age_s"] > 2.0:
    action = "slow_and_refresh_map"
elif signals["clearance_m"] < 0.35:
    action = "stop_and_replan"
else:
    action = "continue"
print(action)

Expected output interpretation. The selected action names which safety bound fired first: localization confidence crossed threshold before map age or clearance did. In a field log this ordering is what matters, because the useful operator question is not "why did it stop" but "which monitored assumption failed first."

Tool Workflow

Keep the small implementation as a regression test. Use the maintained stack for maps, costmaps, behavior trees, controllers, plugins, simulation replay, and deployment telemetry.