Reachability-Guided Sequential Quadratic Programming-Guarded Model Predictive Path Integral for Safe Nonlinear Predictive Control
Authors: Alexandre Didier, Jason J. Choi, Namhoon Cho, Claire J. Tomlin, Melanie N. Zeilinger
Organizations: Institute for Dynamic Systems and Control, ETH Zurich, 8092 Zurich, Switzerland. · Electrical and Computer Engineering Department, University of California, Los Angeles, CA 90095, USA. · Department of Aerospace Engineering, Seoul National University, Seoul 08826, Republic of Korea. · Department of Electrical Engineering and Computer Sciences, University of California, Berkeley, CA 94704, USA.
Safe robot control often requires combining long-horizon performance optimization with hard state and input constraints, but existing approaches tend to address this tradeoff partially. Sampling-based model predictive control (MPC) methods such as model predictive path integral (MPPI) are effective in handling nonlinear and nonconvex environments, yet their finite-sample rollouts and unconstrained weighted-average update can return an unsafe control. Deterministic nonlinear MPC can explicitly incorporate constraints, but its real-time safety and performance depends strongly on warm starts and local convergence. Hamilton--Jacobi reachability (HJR) provides rigorous safety certificates, but offline value-function computation remains practical only for reduced-order models. We propose ReSQ-MPPI, a reachability-informed generation--refinement architecture that combines these complementary strengths. An HJR value function computed offline for a reduced-order model guides online MPPI sampling toward safe, promising trajectory candidates. The MPPI solution is then refined by a small number of sequential quadratic programming (SQP) iterations in a full-order MPC problem. A key observation is that the standard MPPI inference step is an unconstrained weighted least-squares problem; ReSQ-MPPI replaces it with a constrained MPC refinement that recovers the MPPI update when it is feasible and minimally modifies it otherwise. Simulations in cluttered navigation and autonomous racing environments demonstrate that ReSQ-MPPI improves safety and performance over standalone MPPI, MPC, and reachability-filtered sampling-based control baselines.
Figures & tables
Fig. 1: Overview of ReSQ-MPPI. An offline-computed HJ reachability value function and safety filter are used in the MPPI trajectory sampling. The three proposed interactions of MPPI with sequential quadratic programming (SQP) are depicted with green arrows: 1) using the MPPI solution as a reference in the MPC optimization problem, 2) as a warmstart for the SQP solver, and 3) using the SQP optimal trajectory as a nominal input in MPPI.
Fig. 2: Two examples of simulated episodes. MPC suffers from local optima, resulting in collisions with the obstacles, while the HJR-based safety filter can lead to performance agnostic actions (left). The proposed SQP can improve performance compared of obtained MPPI solutions.
Avg. Cost
Success [%]
Collisions [%]
MPC (SQP)
1.039 ± 0.089
26
71
Dualguard MPPI [ 7 ]
1.554 ± 0.124
91
0
ReSQ-MPPI (ref. + ws. + nom.)
1.0 ± 0.102
96
0
TABLE I: Cluttered environment comparison
Fig. 3: Closed-loop trajectories for the dynamic bicycle model with Pacejka tire forces on the considered track for four random obstacle configuration. The HJR value function level set computed for the kinematic model capturing the track boundaries and the obstacles is depicted ( black ).
Avg. Lap Time [s]
Success [%]
Collision [%]
MPC (SQP)
4.274 ± 0.020
32
68
Dualguard [ 7 ]
5.831 ± 0.210
96
4
ReSQ-MPPI (ws. + nom.)
3.884 ± 0.012
100
0
TABLE II: Kinematic Model Racing with Obstacles Comparison
Pred. Hor. [s]
Avg. Lap Time [s]
Success [%]
Collision [%]
1
3.867 ± 0.010
100
0
0.5
3.851 ± 0.010
98
2
0.4
3.870 ± 0.009
100
0
0.3
4.126 ± 0.012
100
0
0.2
7.144 ± 0.028
100
0
0.1
14.996 ± 0.106
62
38
TABLE III: Kinematic Model ReSQ-MPPI Prediction Horizon Study
Avg. Lap Time [s]
Succ. [%]
Coll. [%]
MPC (SQP)
7.629 ± 0.124
42
58
MPPI
-
0
38
ReSQ-MPPI (ref. + ws. + nom.)
7.546 ± 0.198
98
0
TABLE IV: Dynamic Pacejka Model Racing with Obstacles Comparison
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.
Model Predictive Path Integral (MPPI) control provides a sampling-based framework for optimal control, while Control Barrier Functions (CBFs) provide a principled means of enforcing safety constraints. We introduce BR-MPPI, which integrates CBF-like conditions into MPPI's control sampling procedure. CBFs impose inequality constraints that bound the rate of change of barrier functions using a class-K function of the barrier value. We instead impose the CBF condition as an equality constraint using a parametric linear class-K function and augment the system state with its parameter. The parameter's time derivative serves as an additional control input optimized by MPPI. We further design a cost function that promotes parameter values consistent with Nagumo's condition at the safe-set boundary, thereby encouraging safety. The resulting multiple state- and control-dependent equality constraints pose a challenge for random control sampling. We address this through state transformations and control projections inspired by manifold path planning that map sampled controls onto the constraint manifold. We also incorporate learned signed distance fields to represent robot geometry and reduce computation time. Simulations demonstrate improved sample efficiency over vanilla MPPI and higher navigation success rates across five robot models compared with safety-oriented MPPI variants. Hardware experiments on a quadrotor further demonstrate the method's ability to navigate constrained environments near safe-set boundaries.
Hardik Parwana, Taekyung Kim, Kehan Long +4
Robotics Department, University of Michigan, Ann Arbor, MI 48109, USA · Contextual Robotics Institute, University of California San Diego, La Jolla, CA 92093, USA. · Toyota Motor North America, Research & Development, Ann Arbor, MI 48105, USA.
We present GPU-SLS, a GPU-parallelized framework for safe, robust nonlinear model predictive control (MPC) that scales to high-dimensional uncertain robotic systems and long planning horizons. Our method jointly optimizes an inequality-constrained, dynamically-feasible nominal trajectory, a tracking controller, and a closed-loop reachable set under disturbance, all in real-time. To efficiently compute nominal trajectories, we develop a sequential quadratic programming procedure with a novel GPU-accelerated quadratic program (QP) solver that uses parallel associative scans and adaptive caching within an alternating direction method of multipliers (ADMM) framework. The same GPU QP backend is used to optimize robust tracking controllers and closed-loop reachable sets via system level synthesis (SLS), enabling reachability-constrained control in both fixed- and receding-horizon settings. We achieve substantial performance gains, reducing nominal trajectory solve times by 97.7% relative to state-of-the-art CPU solvers and 71.8% compared to GPU solvers, while accelerating SLS-based control and reachability by 237x. Despite large problem scales, our method achieves 100% empirical safety, unlike high-dimensional learning-based reachability baselines. We validate our approach on complex nonlinear systems, including whole-body quadrupeds (61D) and humanoids (75D), synthesizing robust control policies online on the GPU in 20 milliseconds on average and scaling to problems with 2 x 10^5 decision variables and 8 x 10^4 constraints. The implementation of our method is available at https://github.com/Jeff300fang/gpu_sls.
Jeffrey Fang, Glen Chou
Georgia Institute of Technology, Atlanta, GA 30308