Autonomous robots commonly aim to achieve a nominal objective while minimizing a cost. Without contingency preparation, this can leave them vulnerable to mission failure. This is formalized as a trajectory optimization problem over the nominal cost with a contingency constraint: from each checked state along the nominal plan, an admissible contingency maneuver must exist that reaches a designated recovery target within a prescribed contingency horizon. Existing methods either optimize contingency trajectories alongside the nominal plan or evaluate this constraint through computationally expensive nested sampling-based searches. Instead, we introduce SCRAMPPI, which represents this requirement as a reach-avoid problem and leverages Hamilton--Jacobi (HJ) reachability analysis to evaluate contingency feasibility. By either computing a reach-avoid value function offline or updating it only as the environment is revealed, and integrating it with model predictive path integral (MPPI) control via resampling-based rollouts, SCRAMPPI provides contingency-feasibility evaluations while still planning in 15-20 hz. Finally, we present simulated and hardware experiments demonstrating real-time nominal planning and execution of contingency maneuvers in mobile-robot navigation, alongside a five-dimensional friction-limited recovery simulation.
Figures & tables
Fig. 1 : Contingency-constrained navigation for an occlusion based retreat task. The robot plans toward its nominal goal while retaining a retreat to a recovery target occluded from an adversary. The feasible set represents states from which this target can be reached within the contingency horizon.
Fig. 2 : SCRAMPPI Architecture : (From left to right) We compute a finite-horizon reach-avoid value function from the recovery target, avoid set, and contingency dynamics. Together with the nominal goal, these are passed to our planner, which uses NMPC to seed a multimodal MPPI optimizer with resampling-based rollouts. The robot executes a validated nominal command or, upon a task-defined trigger, switches to HJ feedback to reach the recovery target within the remaining contingency horizon.
Fig. 3 : Trajectory and sample rollouts of SCRAMPPI in the Limited Sensing Environment. The heat map shows a workspace projection of the numerical reach-avoid value, while rollout checks use the full state. Green stars denote recovery targets. RBR enables the samples to both be safe and ensure coverage.
Limited Sensing Environment (100 episodes)
Metric
MPPI
C-MPPI
CMPC
SCRAMPPI
Success Rate (%) ↑
88
97
84
100
Goal Time (s) ↓
16.0
22.4
23.1
20.4
Safety Violations/Episode ↓
15.4
0.4
0.8
0.0
Ice Slip Environment (25 episodes)
Metric
MPPI
C-MPPI
CMPC
SCRAMPPI
TABLE I : Simulation results. SCRAMPPI has the highest success rate and lowest safety violations of all methods while being faster than the contingency aware baselines. For Ice Slip, we additionally report the number of successes over the number of episodes our contingency model is activated in, and demonstrate our method recovers in the case of ice slip consistently.
Fig. 4 : Representative Ice Slip Environment trial. Solid curves show nominal operation and dotted segments show execution under recovery control. The green band is the known-traction recovery area, blue patches are ice, and gray disks are obstructions. SCRAMPPI is the only method able to navigate the bend while recovering from ice patches, while all other methods either crash or are unable to pull off to the shoulder of the road after slipping.
SCRAMMPI
C-MPPI
HJ Reachability (Min/Avg/Max) (ms)
18.6 / 55.4 / 181.9
-
TopoPRM+NMPC (ms)
41.8
57.2
MPPI Sampling (ms)
2.6
107.7
Avg Total Time (ms)
50.2
164.9
TABLE II: Average Runtime. SCRAMPPI can, on average, run in 50 ms while C-MPPI takes substantially longer. Of note, we do not need to recompute our HJ value function every planning step, leading to the lower total time
Safe motion planning develops methods for robots and vehicles to reach goals while avoiding collisions and respecting dynamic and uncertainty constraints.
Hamilton-Jacobi (HJ) reachability provides formal safety guarantees for dynamical systems, but solving high-dimensional HJ partial differential equations limits its use in real-time planning. This paper presents a contingency-aware multi-goal navigation framework that integrates learning-based reachability with sampling-based planning in unknown environments. We use Fourier Neural Operator (FNO) to approximate the solution operator of the Hamilton-Jacobi-Isaacs variational inequality under varying obstacle configurations. We first provide a theoretical under-approximation guarantee on the safe backward reach-avoid set, which enables formal safety certification of the learned reachable sets. Then, we integrate the certified reachable sets with an incremental multi-goal planner, which enforces reachable-set constraints and a recovery policy that guarantees finite-time return to a safe region. Overall, we demonstrate that the proposed framework achieves asymptotically optimal navigation with provable contingency behavior, and validate its performance through real-time deployment on KUKA's youBot in Webots simulation.
Kasidit Muenprasitivej, Derya Aksaray
Department of Electrical and Computer Engineering, Northeastern University, Boston, MA, USA, 02115
Autonomous navigation requires planning to reach a goal safely and efficiently in complex and potentially dynamic environments. Graph search-based algorithms are widely adopted due to their generality and theoretical guarantees when equipped with admissible heuristics. However, the computational complexity of graph search grows rapidly with the dimensionality of the search space, often making real-time planning in dynamic environments intractable. In this paper, we combine offline Hamilton-Jacobi (HJ) reachability with online graph search to leverage the complementary strengths of both. Precomputed HJ value functions, used as informative heuristics and proactive safety constraints, amortize online computation of the graph search process. At the same time, graph search enables reachability-based reasoning to be incorporated into online planning, overcoming the long-standing challenge of HJ reachability requiring full knowledge of the environment. Extensive simulation studies and real-world experiments demonstrate that the proposed approach consistently outperforms baseline methods in terms of planning efficiency and navigation safety, in environments with and without human presence.
Hanyang Hu, Cameron Siu, Mo Chen
Simon Fraser University, Burnaby, BC V5A 1S6, Canada
Sampling-Based Model-Predictive Control (MPC) algorithms are a flexible class of controllers used for navigation on a wide range of robotic systems. Historically, such approaches have lacked hard safety guarantees, a shortcoming which we remedy in this work by computing guaranteed reachable-set overapproximations online with a fast, interval-based pipeline. We show that our method achieves similar performance to a state-of-the-art reachability-based planner without the need for the expensive pre-computation step, and can be scaled to systems that are infeasible using existing approaches. Finally, we demonstrate that our technique reduces safety violations by over 99% in a racing simulation and successfully controls a model racecar on real hardware experiments without crashes.