Contingency Constrained Planning with MPPI within MPPI

TL;DR

This work tackles guaranteeable contingency planning for autonomous systems by embedding a contingency planner inside a nominal MPPI framework. It introduces Nested-MPPI to enforce a reachability constraint to safe regions within a horizon $T_c$ from every nominal state, and augments this with a frontend pipeline (Topol-PRM and NMPC) to seed the sampling with high-quality candidates. The approach is validated through simulations and hardware experiments, showing that Contingency-MPPI produces feasible contingency trajectories at all times and operates in real time, while standard MPPI may violate safety constraints. The method significantly improves safety and robustness in dynamic, uncertain environments and paves the way for real-time deployment in unknown surroundings.

Abstract

For safety, autonomous systems must be able to consider sudden changes and enact contingency plans appropriately. State-of-the-art methods currently find trajectories that balance between nominal and contingency behavior, or plan for a singular contingency plan; however, this does not guarantee that the resulting plan is safe for all time. To address this research gap, this paper presents Contingency-MPPI, a data-driven optimization-based strategy that embeds contingency planning inside a nominal planner. By learning to approximate the optimal contingency-constrained control sequence with adaptive importance sampling, the proposed method's sampling efficiency is further improved with initializations from a lightweight path planner and trajectory optimizer. Finally, we present simulated and hardware experiments demonstrating our algorithm generating nominal and contingency plans in real time on a mobile robot.

Figures (8)

• Figure 1: At each step along the nominal plan, a contingency plan must exist to reach a safe state within a time horizon.
• Figure 2: Planning Pipeline. Our Contingency-MPPI first runs (1) TopoRPM to find multiple paths through the environment (\ref{['sec:topoprm']}), (2) NMPC to find control sequences for each path (\ref{['sec:nmpc']}), and (3) Nested-MPPI that utilizes these control sequences as modes (\ref{['reachability_des']}) to find a trajectory for the vehicle to track.
• Figure 3: Nested-MPPI computes reachability cost by sampling contingency trajectories (dashed lines) along nominal trajectory rollouts (solid lines). Nominal trajectories 0, 1, and 4 collided with an obstacle or did not find a valid contingency from every state, and thus have $+\infty$ cost.
• Figure 4: Nested-MPPI
• Figure 5: $\text{FindContingencyPlan}$