When the Policy Is Unsure, Widen the Margin: Doubt-Inflated Safety Filters for Hierarchical Imitation

Robots that learn by copying human demonstrations, an approach called imitation learning, fail in a predictable way. A small mistake carries the robot into a situation the demonstrator never showed it. There its next actions are guesses, and the errors pile up. This failure is most dangerous near obstacles, because careful demonstrators keep a wide berth and so never show how to recover from a near miss. The usual fix on the safety side is a "safety filter": a fast controller that minimally edits each commanded action so the robot stays outside an inflated zone around each obstacle. Today those zones grow with only one kind of uncertainty, the robot's uncertainty about where obstacles are. They ignore a second kind that matters just as much: the learned policy's uncertainty about what it should do, which spikes exactly when the robot has drifted off the demonstrations.

We propose to make the safety margin around each obstacle depend on both. The margin grows with perception uncertainty and also with a calibrated measure of how far the imitation policy has drifted off the data it was trained on. When the widened margins make the current subgoal unreachable, the low-level controller does not freeze. It hands control back to a slower high-level planner and passes it the widened obstacle zones to plan around. The policy's own doubt therefore does two jobs. It sets how conservative the fast safety layer is, and it decides when the slow planner is called. Near obstacles that are well covered by demonstrations the margin stays tight, so the robot does not freeze. In unfamiliar situations the margin opens up, so a policy that has lost its footing cannot drive itself into a wall.

Core Claim

Our claim concerns the trade-off between task success and collisions. Hierarchical imitation policies under distribution shift can do better on this trade-off when obstacle margins are inflated by policy doubt than when margins are fixed or inflated by perception uncertainty alone. The benefit should show up as a better trade-off frontier, not just a single better point. It should come from two sources: tight margins in-distribution, and replanning instead of freezing when the robot is off-distribution.

The reasoning is simple. A safety filter that runs every control step only edits the current action. It trusts that the policy will keep behaving sensibly over the next few steps until the filter can react again. When the policy is off-distribution, that trust is misplaced. The worst-case extra drift over the filter's reaction window grows with the size of the policy's action error. So the margin should grow by a distance proportional to that error. For obstacle $i$ at time $t$ we use the radius

$$r_i(t) = r_0 + \kappa_p \sqrt{\lambda_{\max}(\Sigma_i(t))} + q_{1-\delta}\big(\nu(s_t)\big).$$

Here $r_0$ is the physical clearance and $\Sigma_i(t)$ is the obstacle's tracking covariance, with $\lambda_{\max}$ its largest eigenvalue and $\kappa_p$ a confidence multiplier. The term $\nu(s_t)$ is the spread of actions proposed by an ensemble of low-level policies at state $s_t$. The function $q_{1-\delta}$ maps that spread to a distance. It is fitted by conformal calibration on held-out closed-loop rollouts, as the $(1-\delta)$ quantile of observed position deviation over the filter's reaction window given $\nu$. The calibration does not assume Gaussian errors. This matters because the file identifies Gaussian assumptions as a weak point of the convex safety formulations.

Relation to Prior Work

The perception-uncertainty term is borrowed directly. Jian et al. inflate obstacles in a model-predictive controller with barrier-function constraints using Kalman tracking covariances. The decentralized MPPI work shifts a sampling controller's distribution through a second-order cone program to meet a 99.9% chance constraint. UncAD turns map-boundary uncertainty into elliptical risk zones that filter candidate driving trajectories. All three inflate margins only for world uncertainty, and all three assume either a known controller or a fixed set of candidate trajectories. None treats the controller itself as a learned, possibly off-distribution object. Our added term is the step from these works to imitation learning.

The doubt signal comes from interactive imitation. ThriftyDAgger uses ensemble action variance as a novelty score and a learned Q-function as a risk score, and calls a human when either crosses a budgeted threshold. SHAIL computes a scalar safety probability for driving options and terminates an option early when it turns unsafe. Neither turns doubt into geometry. ThriftyDAgger spends doubt on a human. SHAIL's safety signal is a scalar over a fixed library of options, which its authors report produces jerky motion. We spend doubt on a continuous margin inside an automated filter, and on calling an automated high-level planner.

On the hierarchy side, our work answers two open problems the file names. HiRT decouples a slow 7B vision-language model from a fast 10 Hz policy. It runs on a fixed asynchronous schedule, and its authors point to uncertainty-driven replanning as the natural next step. SuSIE generates visual subgoals with no model of whether the low level can reach them. It occasionally places goals inside obstacles. Our handoff passes the inflated obstacle zones upstream, which gives the high level exactly that missing feasibility information.

Two theoretical results limit what we may change. The action-chunking analysis shows that predicting open-loop action sequences turns exponential error growth into linear growth for stable systems. That argues against simply executing one action at a time. We therefore keep chunked low-level policies and let the filter edit chunk actions step by step. The incremental-stability results of Tu et al. tie imitation sample complexity to closed-loop stability. A filter that edits actions changes the closed-loop system, so we measure whether filtering destabilizes tracking.

SAFE-GIL is the main competitor in spirit. It fixes the near-obstacle blind spot at data-collection time by pushing the expert toward the reachability boundary. It needs Hamilton-Jacobi reachability, which does not scale beyond low dimensions. Our method fixes the same blind spot at deployment time and needs no reachability computation.

Research Program

We will build a two-level navigation system. The high level proposes waypoints or subgoals at low frequency. In the driving and multi-robot settings it is a learned subgoal planner; in a vision variant it is a vision-language model in the HiRT style. The low level is an ensemble of five chunked behavior-cloning policies trained on the same demonstrations with different seeds and data orderings. It runs at 10–20 Hz. Between the low level and the actuators sits a control-barrier-function quadratic program. It minimally changes the first action of the current chunk so that the robot stays outside the inflated radius $r_i(t)$. It re-solves every step, so obstacle motion is handled in closed loop even though the policy itself predicts chunks.

The handoff rule works as follows. If the quadratic program becomes infeasible, or progress toward the subgoal stalls for a fixed window, the low level discards its remaining chunk. It sends the high level the current inflated obstacle set, and the high level replans. We hold the replanning rate under a budget that matches HiRT's average high-level cadence, so gains cannot come from simply calling the slow model more often.

The analysis has three parts. First, we test whether ensemble spread actually predicts closed-loop deviation near obstacles. We measure rank correlation and conformal coverage of $q_{1-\delta}$ on held-out perturbed rollouts. Second, we compare the full margin, the perception-only margin, and fixed margins as complete success-versus-collision frontiers, traced by sweeping $\delta$ or the fixed radius. Third, we measure jerk and tracking error to check that filtering does not undo the stabilizing effect of chunking.

Baselines

The naive reference point is plain chunked behavior cloning with no filter. In the ground-navigation setting of SAFE-GIL, vanilla behavior cloning from 8 demonstrations reports roughly 80% failure, and on Crazyflie hardware it reports 20–80% collisions (SAFE-GIL). Our reproduction should land near those numbers before any method is credited.

The strongest existing method on our core question is SAFE-GIL, which reports about 10% failure with 8 demonstrations in ground navigation and 0% hardware collisions. We will run it in the same planar ground-navigation environment, where its reachability computation is tractable. It gets the same 8-demonstration budget, the same policy architecture, and the same number of tuning trials for its disturbance magnitude as we get for $\delta$. Beating it means matching its roughly 10% failure rate without any reachability computation. Beating it also means keeping that rate as we add state dimensions (obstacle velocities, robot heading rate) where SAFE-GIL cannot be run. We will also run SAFE-GIL data together with our filter, since the two are complementary. An honest outcome may be that the combination wins.

On the hierarchical driving side, the reference is SHAIL on the Interaction roundabout (DR_USA_Roundabout_FT). It reports 77.7% success in-distribution and 64.4% out-of-distribution (SHAIL). We will use the same split and the same simulator. Beating it means higher out-of-distribution success with lower acceleration Jensen-Shannon divergence, the jerk measure SHAIL itself reports as its weakness.

For multi-agent crowds we use the circle and random scenarios of the decentralized MPPI paper. That method reports 100% success with zero collisions, versus up to 15% collisions for deterministic MPPI-ORCA. Because it is a planner with known dynamics rather than a learned policy, we treat it as an upper reference and not something a learned policy must beat. The question is how much of that gap the doubt term closes for an imitation policy.

Three ablations isolate the key ingredient. The first is a fixed-radius filter swept across radii. The second is a perception-covariance-only filter in the style of Jian et al. and UncAD. The third uses doubt only to trigger replanning, with no margin inflation, which is a ThriftyDAgger-style gate with the high level replacing the human. All variants share the ensemble, the demonstrations, and the replanning budget.

Why not just…?

Why not just use a fixed safety radius and tune it? A sufficiently large fixed radius is safe, but Liu et al. document the resulting freezing in dense settings, and a small one gives up safety precisely off-distribution. The fixed-radius sweep produces a full frontier. Our claim fails if any point on it matches our frontier, so this alternative is tested directly rather than argued away.

Why not just inflate margins by perception uncertainty, as UncAD and Jian et al. do? In our ground-navigation and roundabout settings, obstacle positions are often well estimated while the policy has drifted. Perception-only margins stay tight in exactly the cases where the policy is guessing. If the perception-only ablation matches the full method, then policy doubt adds nothing beyond what the world model already captures, and we would report that.

Why not just fix the data, with SAFE-GIL or exploratory noise injection? This is the most serious threat. In low dimensions SAFE-GIL may simply win, and the chunking analysis shows passive noise injection can match DAgger. Our answer is scaling. We increase state dimension until reachability becomes infeasible and check whether isotropic noise alone still leaves near-obstacle failures. We also report the combination with SAFE-GIL data, since deployment-time margins and training-time recovery data are not exclusive.

Why not just replan the high level more often, or execute one action at a time? Faster fixed replanning costs latency. HiRT reports that a monolithic vision-language policy at about 4 Hz reached 48% success, versus 75% for the decoupled policy at about 10 Hz, on a moving-object task (HiRT). The chunking theory warns that dropping chunks reintroduces compounding error. We equalize the replanning budget and include a chunk-length-one variant, so any gain is shown not to come from extra high-level calls or shorter open-loop execution.

Decisive Checks

The idea stands or falls on one measurement made first. On held-out perturbed rollouts near obstacles, does ensemble spread $\nu$ predict realized deviation over the filter's reaction window? We require that the conformal map $q_{1-\delta}$ achieve near-nominal coverage. If rank correlation is weak, the doubt term is noise that only adds conservatism, and the project should stop or switch to a different doubt signal, such as ThriftyDAgger's learned risk Q-function.

Given a usable signal, the method succeeds under three conditions. Its success-versus-collision frontier must dominate both the fixed-radius and perception-only frontiers on ground navigation and the roundabout. It must reach SAFE-GIL's roughly 10% failure level without reachability, and stay there as dimension grows. Its out-of-distribution roundabout success must exceed SHAIL's reported 64.4% at lower jerk.

A final check concerns the handoff. We will count how often filter infeasibility leads to a successful replan rather than a freeze. This tells us whether passing inflated zones upstream actually fixes SuSIE-style infeasible subgoals, or merely moves the freezing to the high level.