SCRAMPPI: Efficient Contingency Planning for Mobile Robot Navigation via Hamilton-Jacobi Reachability

Abstract

Autonomous robots commonly aim to complete a nominal behavior while minimizing a cost; this leaves them vulnerable to failure or unplanned scenarios, where a backup or contingency plan to a safe set is needed to avoid a total mission failure. This is formalized as a trajectory optimization problem over the nominal cost with a safety constraint: from any point along the nominal plan, a feasible trajectory to a designated safe set must exist. Previous methods either relax this hard constraint, or use an expensive sampling-based strategy to optimize for this constraint. Instead, we formalize this requirement as a reach-avoid problem and leverage Hamilton-Jacobi (HJ) reachability analysis to certify contingency feasibility. By computing the value function of our safe-set's backward reachable set online as the environment is revealed and integrating it with a sampling based planner (MPPI) via resampling based rollouts, we guarantee satisfaction of the hard constraint while greatly increasing sampling efficiency. Finally, we present simulated and hardware experiments demonstrating our algorithm generating nominal and contingency plans in real time on a mobile robot in an adversarial evasion task.

Figures (5)

• Figure 2: To ensure safe planning, we require the existence of backup plans. For example, in hide-and-seek, the robot must always be able to reach a safe (occluded) region within a fixed time. We enforce this by constraining the robot to remain within the finite-time reachable set of safe regions (i.e., outside the unsafe set).
• Figure 3: SCRAMPPI Architecture: (From left-to-right) We compute a finite-time reach-avoid value function from the safe set, occupancy map, and robot dynamics. These, along with the goal state, are passed to our planner, which uses MPC to seed an MPPI trajectory optimizer with rollout-based resampling. The robot executes the optimized trajectory during nominal operation, but switches to value-function control during contingencies to optimally drive the system to a safe zone.
• Figure 4: SCRAMPPI
• Figure 5: Simulation result of SCRAMPPI in a randomly generated environment. The background heatmap shows the reach-avoid value function $V(\mathbf{x})$ minimum over $\theta$, with the zero level set (dashed red) delineating the contingency-feasible region ($V \leq 0$, blue) from infeasible states ($V > 0$, red). Light blue traces show multimodal, safe MPPI rollout samples.
• Figure 6: Snapshot from hardware deployment comparing MPPI and SCRAMPPI in an adversarial evasion scenario. (Left) MPPI selects the shortest path to the goal without maintaining backup feasibility. (Right) SCRAMPPI selects the longer path that preserves contingency feasibility to occluded safe sets