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.
Figures & tables
Fig. 1: Preview of finite-time guaranteed contingency planning using Fourier Neural Operator–based Hamilton–Jacobi reachability. The robot state lies within a safe under-approximation (blue) of the true reachable set (green). The final trajectory (red, solid) is obtained by following a gradient-based policy toward the safe set (light green) while avoiding obstacles (grey), which may be unknown (red, dashed) and detected by onboard sensors (cyan). (Rightmost) Realization of the contingency control policy on KUKA’s youBot.
Fig. 2: Overview of the planning framework in Alg. 1 with K=1 goal (red star) and initially unknown obstacles (red, dashed). The ε -sublevel set of the FNO-predicted reachable set (dark blue) is guaranteed to under-approximate the true reachable set (green). The FNO model is trained over a local domain Dloc (blue) and integrated into planning within the global environment Ω (grey). Prior to planning, the heading-agnostic reachable sets (cyan) must sufficiently overlap to contain a closed Euclidean ball (yellow), ensuring feasibility of the nominal planner with step length δ across adjacent sets. The final trajectory toward the goal is shown in red.
Training Epochs
ε ( L∞ )
ηinclude(RAϕε/RAϕ)
ε0 ( W1,2 )
meas(Dviolate) over meas(X)
MSE [×10−4]
1500
0.483
1.0 / 0.9927
0.118
2.465 %
9.62
2000
0.455
1.0 / 0.9928
0.114
2.281 %
8.60
2500
0.442
1.0 / 0.9937
0.110
2.138 %
7.84
3000
0.431
1.0 / 0.9935
0.107
2.011 %
7.06
TABLE I: FNO Training and Approximation Metrics
Scenario
Success
Avg. Treach
Avg. V(x,0) (failure)
Avg. Dist. to L (failure)
1 Obs
100%
4.564
—
—
3 Obs
98.00%
4.600
0.054
1.544
5 Obs
96.80%
4.769
0.111
1.582
7 Obs
95.60%
4.707
0.019
2.046
TABLE II: Contingency Policy Case Studies (500 runs each)
Fig. 3: Comparison of two cases in the multi-goal planning and routing framework: (a) an environment with known obstacles (grey) and no reachable-set constraint for contingency planning; (b) an environment in which all obstacles are a priori unknown (red, dashed) and later detected (grey), with a guaranteed finite-time reachability strategy to a safe set (green, dashed) enforced at every state along the trajectory. The robot starts at the red star, and trajectory colors denotes the sequence of the goals visited.
Unknown Map
Priori-Known Map
RRT X Constraint
Dist.
Time
Dist.
Time
p∈Ωfeasible
105.614
89.494 s
105.428
64.741 s
p∈Ω
103.879
47.604 s
102.568
42.067 s
TABLE III: Multi-Goals Planning Case Studies (10 runs each)
Fig. 4: Contingency-aware multi-goal planning on KUKA’s youBot simulation. On the plot, the robot initializes at the red star, and the trajectory color encodes the active goal index. While navigating toward the green star, an adversarial event is triggered and activates the contingency mechanism, prompting the robot to execute a certified fallback policy (black, squared) toward the safe set (green, dashed). Upon reaching the safe set, the robot resumes the mission and visits the remaining goals in a locally optimal sequences.
Safe motion planning develops methods for robots and vehicles to reach goals while avoiding collisions and respecting dynamic and uncertainty constraints.