One Filter to Deploy Them All: Robust Safety for Quadrupedal Navigation in Unknown Environments

In plain words

A walking robot dog follows a planner that is good in familiar places but may not know about a new obstacle, a slippery floor, or a heavier load. This paper adds a safety layer that sits between the planner and the walking controller. The layer is only allowed to change the robot's commands when the robot is about to get into trouble, and otherwise lets the planner decide.

The layer is driven by a small neural network that was trained offline on many made-up rooms. It looks at a laser scan of the surroundings, a guess of how much the robot's motion is being pushed off course, and the robot's position and heading. It then outputs a number that says how safe the situation is and a direction in which things get safer. When the number is low, a small optimisation nudges the planner's command as little as possible to keep the robot clear of trouble.

The authors' point is that one such layer can work with several different planners and walking controllers, and does not need to be redesigned for each one. The paper reports tests in simulation and on a real quadruped. Those tests show the layer keeping the robot clear of obstacles more often than a comparison method, especially on a slippery surface.

TL;DR

• Safety filter for a quadruped: a learned Hamilton-Jacobi value function, conditioned on a LiDAR scan and an online disturbance-bound estimate, drives a least-restrictive QP filter over the nominal twist command §V §V-B.2.
• The network is trained by supervised regression on value and spatial gradient, using ground truth from a reachability solver on 1,000 random environments §V-A §V-A.2 §V-A.3.
• Failure probability is handled with conformal calibration of the network output, with a stated guarantee §V-A.4 eq. 11 eq. 12.
• Author-reported: 100-trial simulation on a hard payload and friction setting; OCR reaches 0.91 success with ABS-Agile, versus 0.80 for ABS tab. III.
• The released code departs from the paper in several places, including the threshold, the disturbance window and the disturbance formula (see Paper vs code).

Problem

Learned locomotion and planners often assume a known environment and known dynamics. A safety guarantee that depends on a fixed map or a fixed controller stops being valid when either changes §I §III. The authors want a filter that does not need the controller or the environment in advance §III.

The dynamics depend on the environment, through friction, payload and unseen obstacles. The failure set depends on the environment too §III §V. The paper treats the reduced-order model's modelling error as a bounded adversarial disturbance, which is conservative §V-A.1.

The paper assumes that the nominal policy outputs a planar twist command (forward speed and yaw rate) which a low-level controller tracks §III. It also assumes that the reduced-order model is accurate enough that the remaining error fits inside the estimated bounds §VIII.

Method

Reduced model. The robot is modelled as a 3-state Dubins car: planar position and heading. The control is a twist (v, w). The modelling error enters as an additive disturbance, bounded in position and in heading §V-A.1 eq. 10.

Training data. For each of 1,000 training environments and 100 validation environments, random circular obstacles are placed and a disturbance bound is sampled. A reachability solver computes the converged value and its gradient on a grid, over a 2 s horizon §V-A.2. Training pairs are made from robot origins outside obstacles, each with its LiDAR scan, and states sampled in the origin's frame that are not occluded §V-A.2.

Network. The value network V_ψ takes the relative state, the disturbance bound and the 100-ray LiDAR scan. It is an MLP with sine activations, chosen because they model spatial gradients well §V-A.3. The loss is a squared error on the value plus a squared error on its spatial gradient, averaged over N = 50,000 samples per batch §V-A.3.

Calibration. Let s_i be prediction errors on validation samples. Choose a confidence β and a violation rate ε. Find the largest k that satisfies a binomial tail bound. The calibration level δ is the (N − k)/N quantile of the errors. With probability at least 1 − β, the fraction of samples where the network overestimates the value by more than δ is at most ε §V-A.4 eq. 11 eq. 12. The proof is a split-conformal argument with a Beta distribution app. A eq. 17.

Disturbance estimation. At deployment, the robot compares its measured state with a disturbance-free prediction over a short window. The prediction horizon is 2 s. The estimated disturbance is the prediction error divided by that horizon §V-B.1 eq. 13. A sliding window of such estimates gives the bound. The bound keeps the middle fraction c of sorted values, and is set at the mean plus or minus b standard deviations §V-B.1 eq. 14. The paper uses c = 0.8, b = 2, and a window of 2 s §V-B.1.

Safety filter. When the value is above δ, the nominal command passes through unchanged §V-B.2 eq. 15. Otherwise a QP picks the command closest to the nominal one, subject to a worst-case margin from the disturbance bound, plus a slack variable with weight λ = 10^3 §V-B.2 eq. 16. The constraint uses the gradient of V_ψ, and the margin is taken over all disturbances in the bound §V-B.2. The filter is the smooth least-restrictive filter from the reachability literature §IV-B eq. 8 eq. 9.

Hierarchical use. The filter needs a high-level twist, so it works with any planner that outputs twists and a low-level tracker. For an end-to-end policy, the paper runs the policy unchanged while V_ψ > 0, and falls back to its own planner and tracker otherwise (Remark in §V-B.2).

Simulation nominal controllers. The paper combines three planners (predictive sampling, a naive goal-seeker, and a human teleoperator) with three low-level policies (Walk-These-Ways, Unitree's MPC, and the ABS recovery policy) §VI-A. Predictive sampling plans with a sampled MPC over 4 s, and cost is distance to goal plus a collision penalty §VI-A.1.

# Training data for one environment  [§V-A.2]
# Input:  obstacles (1 to 10 circles), disturbance bounds
# Output: training samples (state, value, gradient, LiDAR scan)

def make_samples(obstacles, disturbance_bounds):
    # Ground-truth value on a 100 x 100 x 60 grid, 2 s horizon  [§V-A.2]
    value_grid = solve_reachability(obstacles, disturbance_bounds)
    samples = []
    for origin in sample_free_origins(obstacles, count=10):
        # LiDAR: 100 rays over [-pi, pi], clipped to [0.2, 10] m  [§V-A.2]
        scan = render_lidar(obstacles, origin)
        for state in sample_unoccluded_states(obstacles, origin, count=500):
            # State is expressed in the frame of the new origin  [§V-A.2]
            value, gradient = value_grid.query(state)
            samples.append((state, disturbance_bounds, scan, value, gradient))
    return samples

# Value network loss  [§V-A.3]
# Input:  batch of samples with predicted values and gradients
# Output: scalar loss

def loss(pred_value, pred_grad, value, grad):
    # Squared error on the value  [§V-A.3]
    value_term = (pred_value - value) ** 2
    # Squared error on the spatial gradient  [§V-A.3]
    grad_term = sum((pred_grad - grad) ** 2)
    # Averaged over the batch of N samples  [§V-A.3]
    return mean(value_term + grad_term)

# Calibration of the value network  [§V-A.4] [eq. 11] [eq. 12]
# Input:  beta, epsilon, N validation errors s_i = V_pred - V_true
# Output: calibration level delta (m)

def calibration_level(errors, beta, epsilon):
    # Largest k with binomial tail at most beta  [eq. 11]
    k = largest_k(errors_count=len(errors), epsilon=epsilon, beta=beta)
    # (N - k)/N quantile of the errors  [§V-A.4]
    delta = quantile(errors, (len(errors) - k) / len(errors))
    return delta

# Online disturbance bound  [§V-B.1] [eq. 13] [eq. 14]
# Input:  last window of states, controls and time steps
# Output: bound on the disturbance, per state dimension

def estimate_bounds(states, controls, dts, horizon, window, c, b):
    # Disturbance = (measured - predicted) / horizon  [eq. 13]
    disturbances = per_step_disturbances(states, controls, dts, horizon)
    # Keep the middle fraction c of the sorted window  [§V-B.1]
    middle = central_fraction(disturbances, c)
    # Bound = |mean| +/- b standard deviations  [eq. 14]
    # The released code drops the absolute value, see Paper vs code
    center = abs(mean(middle))
    return center - b * std(middle), center + b * std(middle)

# Adaptive safety filter  [§V-B.2] [eq. 15] [eq. 16]
# Input:  robot state, LiDAR scan, disturbance bounds, nominal twist
# Output: filtered twist (v, w)

def safety_filter(state, scan, bounds, nominal_twist, delta):
    # Calibrated value and its gradient at the current state  [§V-A.4]
    value, grad = value_network_with_gradient(state, scan, bounds)
    # Outside the boundary the nominal command passes through  [eq. 15]
    if value > delta:
        return nominal_twist
    # QP: closest twist to nominal, with worst-case margin  [eq. 16]
    # Slack s >= 0 keeps the QP feasible, weighted by lambda = 10^3  [§V-B.2]
    return solve_qp(nominal_twist, grad, bounds, state, slack_weight=1e3)

Hyperparameters

Paper values are listed first. Values found only in the released code are marked as code-only and are not claims of the paper. The paper does not say which hardware or software versions were used beyond the GPU and the Go1 platform §V-A.2 §VII.

Results

Author-reported. Simulation: Unitree Go1 in Isaac Sim, 100 trials per row, hard payload and friction setting §VI-D app. B. Hardware: Unitree Go1 with an RPLiDAR A2, 10 trials per row §VII tab. IV. Baselines: ABS, which is an end-to-end safety framework, and ablations without disturbance estimation (OCR \ DE) or calibration (OCR \ C) §VI-B.

Simulation, hard setting, 100 trials tab. III

The main text notes that the OCR framework is compared with ABS on ABS-Agile and that the ABS framework is designed for that policy §VI-E.1. The table is the hard setting from the appendix app. B.

Hardware, 10 trials per row tab. IV

Simulation, all difficulty levels, 100 trials each tab. VI

Hardware, all conditions, 10 trials tab. VII

Simulation environment parameters tab. V

Calibration levels tab. II (N = 10^7, beta = 10^-12)

Other reported figures. Safety rates across settings for 1,000 trials are plotted fig. 5. Disturbance estimates in a payload and friction case, and a sudden friction change, are plotted fig. 6. Trajectories on two hand-designed cases and the dead-end case are shown in fig. 7. Hardware slippery-floor runs are shown in fig. 8. Real-world demonstrations are in fig. 1, with videos on the project page (linked in the paper).

Paper vs code

Repository: albertklin/observation-conditioned-reachability, MIT licence, linked from the project page and owned by the first author's GitHub account LICENSE. Commit 92d14861fcb0bf9ca663738e93a7c156afa62b0a. The LICENSE copyright line names all three authors. The repository contains the training, data, calibration, filter and hardware code.

• Network depth. The paper uses 4 hidden layers of 512 §V-A.3. The code builds an MLP with hidden size 512 and num_hidden_layers=3 models.py. The weight-layer count depends on how FCBlock in modules.py counts layers, which was not checked.
• Loss scale. The paper's loss sums squared norms over the gradient components and averages over samples §V-A.3. The code uses nn.MSELoss on value and gradient, which averages over elements train_value_network.py train_value_network.py. This rescales the gradient term relative to the paper's by the number of gradient components.
• Filter threshold. The paper switches to the QP when the uncalibrated network output is at most δ = 0.49 m §V-B.2 eq. 15. The code adds a calibration offset of -0.5 and switches when the calibrated value is below 0.35 safety_filter.py safety_filter.py filter.py. In raw network units that is about 0.85, not 0.49.
• Disturbance window. The paper uses a 2 s window §V-B.1. The config sets window_horizon to 10 s hardware.yaml.
• Integrator. The paper says the disturbance-free rollout uses Euler's method §V-B.1. The code uses runge_kutta_step for the rollout disturbance_estimator.py disturbance_estimator.py.
• Bound formula. The paper's bound is |mean| plus or minus b times the standard deviation, applied to a window of disturbance norms for position §V-B.1 eq. 14. The code uses mean minus or plus b times the standard deviation, per dimension, with no absolute value disturbance_estimator.py. The hardware filter then takes the element-wise maximum of absolute bounds, takes the norm of the position part, and clips to 1.0 m/s and 2.0 rad/s filter.py.
• Short history. When the history is shorter than the prediction horizon, the code uses half the cap as the disturbance bound filter.py. The paper does not say what happens in that case.
• Calibration grid. The paper tabulates ε = 10^-3 with δ = 0.99 m tab. II. The code's calibration grid covers only 0.01 to 0.1 calibrate_value_network.py. The code computes the bound from the order statistics of a Beta distribution, which is a rearrangement of the paper's binomial form app. A.
• Noise in the planner. The paper's predictive sampler uses σ = 0.5 §VI-A.1. The code uses (max - min) / 4, so 0.5 for speed and 1.0 for yaw rate run_sims.py.
• Cluttered-scene adjustments. The paper says that for cluttered hardware scenes, it discards LiDAR beyond a front cone of π/2 rad and uses the uncalibrated output §VII. These are not separate flags in the code I read. The hardware LiDAR interface has its own focus settings lidar.py that were not compared with the paper.
• Fallback. If the QP fails, the code uses a bang-bang fallback on the sign of the gradient safety_filter.py. The paper does not describe a fallback.
• Frame. The code evaluates the network in the frame of the LiDAR capture, rotating the current relative state into that frame and the gradient back to world frame safety_filter.py. The paper says states are expressed in the frame of the new origin §V-A.2 but does not give the transform for deployment.
• Simulation payload and friction. The sampling in generate_sims.py matches the paper's hard ranges, with the excluded middle bands generate_sims.py.
• Predictive sampling. Horizon, step, samples and collision cost match run_sims.py hardware.yaml navigation_task.py. The sampler uses a Gaussian around the previous best sequence, as the paper says predictive_sampler.py.
• Naive planner. I did not find the naive goal-seeking planner in the files I searched (scripts/run_sims.py, scripts/generate_sims.py, utils/online_environment.py). Results for it cannot be checked against code.
• Baselines. The ABS framework and the Walk-These-Ways and Unitree MPC controllers are external §VI-A and are not in this repository, apart from libraries/walk-these-ways which is vendored.

Pitfalls

• Threshold mismatch. The paper's equations switch at δ = 0.49 m on the raw output. The released hardware config switches at about 0.85 in raw units. An implementation that follows one source will behave differently from one that follows the other. Pick one and say so in your implementation.
• Disturbance window. The paper says 2 s, the config says 10 s. A 10 s window reacts slowly to a change in friction, and the paper says the delay is a failure mode §VIII.
• Conservatism. The authors say that the worst-case disturbance model makes the filter conservative in crowded spaces, and that it can stall in clutter §VIII §VII.
• Guarantee scope. The calibration gives a probabilistic bound on the validation distribution. The authors say it does not carry over to the real world under distribution shift §VIII.
• Quadratic program. The QP has a slack variable with a large weight. If the solver fails, the code uses a fallback that is not in the paper safety_filter.py.
• Numerically sensitive estimation. The disturbance estimate is the state error divided by the 2 s horizon §V-B.1 eq. 13. Any error in the measured state, such as a time-stamp or a state-estimate jump, passes straight into the bound. The paper names latency and state-estimation issues as a source of large estimated disturbances §VIII.
• Input scaling. The paper does not state input normalisation. The code's means and standard deviations are the only values in the repository train_value_network.py. Use them, or state your own.
• Gradient supervision. The QP uses the network's spatial gradient, so the gradient is part of the deployed model, not only a training target §V-B.2 §V-A.3.
• Figure numbers. The paper has no PDF in this workspace; figure numbers were taken from the LaTeX source order, which is how LaTeX numbers floats. Check against the compiled PDF before relying on them.
• Hardware timing. The hardware config uses a 0.05 s robot period and a 0.2 s planner step hardware.yaml hardware.yaml. The estimator's window is set in seconds, so it depends on the control period. Check both when porting to another platform.

Lineage

• Builds on: Hamilton-Jacobi reachability and its robust optimal control formulation §IV-A; the smooth least-restrictive filter from the reachability literature §IV-B; sinusoidal networks for gradient fitting, cited as DeepReach §V-A.3; conformal prediction, cited as split conformal calibration §V-A.4 app. A.
• Compared against: ABS, an end-to-end safety framework with a recovery policy, used as the main baseline §VI-B tab. III tab. IV; ablations without disturbance estimation or calibration §VI-B.
• Low-level policies used: Walk-These-Ways §VI-A.2; Unitree's MPC-based ROS package §VI-A.2; ABS recovery policy §VI-A.2.
• Superseded by: not known from content already in this wiki.