PaperlayerView exact revision on arXiv

Source-linked AI summary

Reachability-Guided Sequential Quadratic Programming-Guarded Model Predictive Path Integral for Safe Nonlinear Predictive Control

Alexandre Didier, Jason J. Choi, Namhoon Cho, Claire J. Tomlin, Melanie N. Zeilinger

arXiv:2610.04406v1cs.ROcs.SYeess.SY

TL;DR

Safe nonlinear robot MPC must reconcile long-horizon performance with hard constraints, despite limitations in sampling, online optimization, and reachability scaling. ReSQ-MPPI guides MPPI sampling with reduced-order reachability and refines candidates using full-order constrained SQP. Simulations report improved safety and performance across cluttered navigation and racing.

Abstract

from arXiv · show

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.

I. INTRODUCTION

The introduction frames safe nonlinear MPC as a balance among hard constraints, long-horizon performance, and real-time optimization. ReSQ-MPPI combines reachability-guided sampling with SQP refinement to address limitations of optimization-based, sampling-based, and certificate-based approaches.

I. INTRODUCTION

  • Nonlinear MPC can incorporate safety constraints but depends on online optimization and good initialization, while finite-sample MPC may miss feasible trajectories or aggregate them unsafely.When SQP fails to find a feasible solution within real-time iteration limits, the resulting input may violate safety constraints.Source: §I · p. 1
  • ReSQ-MPPI uses reduced-order HJR to guide MPPI candidate generation, then applies a few SQP iterations to satisfy full-order system constraints.The overview also depicts MPPI supplying a reference and warm start, with the SQP trajectory fed back as MPPI’s nominal input.Source: §I · p. 1, Figure 1 · p. 2
  • Constrained SQP refinement replaces MPPI’s unconstrained weighted least-squares aggregation, recovering its update when feasible and modifying it to satisfy constraints otherwise.The aggregation can be unsafe even when every sampled trajectory satisfies safety constraints.Source: §I · p. 2
  • Bidirectional MPPI–SQP coupling lowers the likelihood of infeasible sampling or SQP failing to converge to a feasible solution within limited iterations.The SQP-refined sequence becomes the nominal input for MPPI at the next sampling instant.Source: §I · p. 2
  • Conventional numerical HJR scales poorly to higher-dimensional systems, whereas MPPI and MPC can impose hard safety constraints using full-order dynamics.HJR and related safety certificates are described as most suitable for reduced-order models.Source: §I · pp. 1–2

A. Related Works

  • Prior safety approaches include MPC and predictive safety filters, certificate-guided sampling, and learned certificates, but the cited methods retain feasibility, scalability, or verification challenges.The related-work discussion notes that finite-sample methods lack general recursive-feasibility guarantees and learned certificates remain difficult to verify formally.Source: A. Related Works · p. 2

B. Notation

The notation describes discrete-time constrained dynamics and the sequence variables used in prediction. The problem formulation seeks an approximate solution that satisfies system constraints while minimizing a performance objective.

B. Notation

  • The notation uses x_i|k for an optimization variable at time k predicting i steps ahead, and x_k for the sequence of optimization variables.N denotes the sequence length.Source: B. Notation · p. 3

A. Problem Formulation

  • The problem considers discrete-time dynamics with state and input constraints, where state limits can encode obstacles or velocity limits and input limits capture actuator restrictions.The state constraint set is described using a signed distance function.Source: A. Problem Formulation · p. 3
  • The objective is to keep constraints satisfied during operation while minimizing a performance cost, using an approximate solution because the optimal control problem is generally intractable.The cost includes a stage cost incurred at every time step.Source: A. Problem Formulation · p. 3

B. Hamilton-Jacobi Reachability

HJR characterizes a maximal control-invariant safe set and its safe controller through a value function. MPPI approximates optimal control by sampling trajectories and taking a cost-weighted update, which is an unconstrained weighted least-squares solution.

B. Hamilton-Jacobi Reachability

  • The zero-sublevel set of the HJR value function is the maximal control-invariant safe set, and the value function induces a safe controller.The controller can keep the system safe when V(x) ≤ 0.Source: B. Hamilton-Jacobi Reachability · p. 3

C. Model Predictive Path Integral Control

  • MPPI approximates finite-horizon optimal control by rolling out random input disturbances, evaluating trajectory costs, and taking a weighted-average update.The terminal cost approximates the infinite-horizon cost-to-go; temperature λ tunes the effective number of trajectories contributing to the average.Source: C. Model Predictive Path Integral… · p. 3
  • The MPPI update is the solution to an unconstrained weighted least-squares problem, so its resulting closed-loop control is agnostic to system constraints.The supplied passages note that this update therefore lacks safety guarantees.Source: C. Model Predictive Path Integral… · p. 3

D. HJR-guided MPPI (Dualguard-MPPI)

Dualguard-MPPI uses HJR safety filtering during trajectory sampling, replacing an unsafe sampled input with the safe optimal control. Its HJR value function is limited by poor scalability, so reduced-order computation can compromise full-order safety.

D. HJR-guided MPPI (Dualguard-MPPI)

  • When a sampled next state falls outside the maximal invariant set, Dualguard-MPPI replaces that input with the safe optimal control to keep the trajectory within the set.The method applies this filtering at each sampled input step.Source: D. HJR-guided MPPI (Dualguard-MPPI) · p. 4
  • Existing safety-certificate approaches either add barrier or value-function penalties to trajectory costs or reject unsafe samples, but neither guarantees closed-loop constraint satisfaction.Dualguard-MPPI is identified as an HJR-based safety-filtering approach for generating safe trajectory samples.Source: D. HJR-guided MPPI (Dualguard-MPPI) · p. 4
  • Because HJR computation scales poorly, its value function is computed on a reduced-order model, introducing errors in the safe set and controller that compromise full-order safety.The passage attributes the scalability limit to the curse of dimensionality.Source: D. HJR-guided MPPI (Dualguard-MPPI) · p. 4

E. Model Predictive Control

MPC solves constrained optimization problems and uses an invariant terminal set to ensure recursive feasibility. SQP offers an online iterative approach, but its solution quality depends on initialization, motivating the use of MPPI exploration to address local optima.

E. Model Predictive Control

  • MPC formulates the control problem as constrained optimization and uses an invariant terminal set to ensure recursive feasibility.Its terminal cost approximates the cost-to-go.Source: E. Model Predictive Control · p. 4
  • SQP solves MPC iteratively by approximating the problem as a quadratic program at each iterate and updating until convergence to a local KKT point.The method is considered for online real-time solution of the MPC problem.Source: E. Model Predictive Control · p. 4
  • SQP solution quality depends heavily on its warm start, which can leave nonconvex problems in local optima or infeasible; the paper uses MPPI exploration to address this.MPC initial iterates are typically time-shifted solutions from the previous time step.Source: E. Model Predictive Control · p. 4

III. METHOD

ReSQ-MPPI combines HJR-guided MPPI sampling with SQP refinement in a constrained MPC formulation. The MPC formulation preserves the MPPI solution when feasible, modifies it as little as possible otherwise, and can also track its state trajectory.

III. METHOD

  • The method links MPPI and SQP through reference-tracking MPC, MPPI warm starts for SQP, and feedback of the converged SQP sequence as MPPI's next nominal input.The architecture uses MPPI sampling to escape local optima, then refines its potentially unsafe solution with SQP.Source: §III · p. 4

A. MPPI Solution via MPC

  • The constrained MPC formulation tracks the MPPI weighted-average input as closely as possible while satisfying state and input constraints, leaving it unchanged when already feasible.It therefore acts as a constrained version of MPPI's weighted least-squares update.Source: A. MPPI Solution via MPC · p. 4
  • When the dynamics are bijective with respect to input and the MPPI input and state sequences are feasible, state-trajectory tracking recovers the MPPI input solution.This allows recovery through either input matching or state trajectory tracking.Source: A. MPPI Solution via MPC · p. 5
  • A reduced-order HJR-guided MPPI planner can provide a reference for full-order MPC tracking in a hierarchical architecture.The paper states that this structure can provide competitive performance in an autonomous racing example.Source: A. MPPI Solution via MPC · p. 5

B. MPPI as Warmstart for SQP

The section motivates using an MPPI solution to initialize SQP refinement, addressing sampling uncertainty and local optima. It also identifies convergence analysis of this combined approach as future work.

B. MPPI as Warmstart for SQP

  • MPPI solution suboptimality grows second-order with input sampling uncertainty.The authors propose using that solution as the initial SQP iterate, minimizing the same objective to guide it toward the closest local KKT point.Source: B. MPPI as Warmstart for SQP · p. 5
  • A tracking cost can regularize local optimization in environments with multiple local optima, such as cluttered environments.The section describes this cost as an additional regularizer to the SQP objective.Source: B. MPPI as Warmstart for SQP · p. 5
  • The authors identify large input-sampling noise and sparse samples in high-dimensional state spaces as conditions that can reduce MPPI solution accuracy.The proposed SQP refinement is intended to remedy these effects.Source: B. MPPI as Warmstart for SQP · p. 5
  • The MPPI update is a preconditioned gradient step, followed by SQP updates corresponding to Gauss-Newton steps under the unconstrained Hessian approximation.The convergence properties of this approach remain a future research direction.Source: B. MPPI as Warmstart for SQP · p. 5

C. SQP Solution as MPPI Nominal Sequence

The section closes the MPPI–SQP loop by feeding the time-shifted SQP solution back as MPPI’s nominal sequence. It also describes the simulation models, offline HJR computation, and comparison methods and metrics.

C. SQP Solution as MPPI Nominal Sequence

  • The time-shifted SQP optimum becomes MPPI’s nominal input sequence at the next time step, closing the loop between the methods.The nominal sequence biases MPPI’s random-sample exploration and can substantially affect closed-loop performance.Source: C. SQP Solution as MPPI Nominal Sequence · p. 5

IV. SIMULATIONS

  • The simulations evaluate a vehicle navigating clutter and racing with random track obstacles.The kinematic bicycle model serves navigation and the reduced-order racing model; the dynamic bicycle model includes Pacejka tire forces and captures tire slip.Source: §IV · p. 5, A. System Models & Offline Computation · p. 5

A. System Models & Offline Computation

  • HJR value functions are computed for each environment instance in approximately 20 minutes on an NVIDIA GeForce RTX 5090 GPU.The value functions are computed for cluttered navigation and the race track.Source: A. System Models & Offline Computation · p. 5

B. Baselines & Metrics

  • The baselines are plain MPPI, HJR-guided Dualguard-MPPI, and SQP-based MPC; collision and success rates are defined per episode.The full-order eight-dimensional HJR value function is computationally prohibitive, and Dualguard-MPPI is omitted from the racing results because its reduced-order implementation performed poorly under full-order simulation.Source: B. Baselines & Metrics · pp. 5–6
  • Fig. 2 contrasts MPC collisions from local optima, performance-agnostic HJR-filter actions, and performance improvement from SQP refinement of MPPI solutions.The caption describes two simulated episodes.Source: Figure 2 · p. 6

C. Experiment 1: Navigation in Cluttered Environment

Across cluttered navigation and obstacle racing, ReSQ-MPPI combines MPPI sampling with SQP refinement and HJR guidance. The reported results include improved navigation outcomes, safe racing, and similar performance with shorter prediction horizons.

C. Experiment 1: Navigation in Cluttered Environment

  • The navigation study uses 100 randomly generated start-goal pairs, while racing uses 50 random obstacle configurations and a contouring-control formulation.The racing objective is to achieve low lap time while avoiding collisions.Source: C. Experiment 1: Navigation in… · p. 6, D. Experiment 2: Racing with obstacles · p. 6
  • In cluttered navigation, ReSQ-MPPI achieves the highest success rate, no obstacle collisions, and the lowest cost among the compared approaches.The authors attribute the outcome to MPPI warmstarting MPC and the additional optimization step; MPC has many collisions, while Dualguard-MPPI remains safe but has lower success and substantially higher cost.Source: C. Experiment 1: Navigation in… · p. 6

D. Experiment 2: Racing with obstacles

  • For kinematic racing, MPPI struggles to sample the bang-bang torque needed for fast laps, while MPPI warmstarting and SQP nominal feedback yield fast laps without collisions.The comparison reports lap times and obstacle collisions for the kinematic model.Source: D. Experiment 2: Racing with obstacles · p. 6, Table II · p. 7
  • HJR guidance permits significantly shorter MPPI and SQP prediction horizons while preserving similar performance.The horizon study is reported for the kinematic racing model.Source: D. Experiment 2: Racing with obstacles · p. 7, Table III · p. 7
  • ReSQ-MPPI achieves the fastest lap time and remains safe across dynamic Pacejka racing episodes, with one episode failing due to simulation timeout.MPPI collides or times out in all episodes, while MPC has fast lap times but incurs many collisions.Source: D. Experiment 2: Racing with obstacles · p. 7

E. Real-time implementation

In a real-time Chronos miniature race-car simulation, ReSQ-MPPI completes ten consecutive laps without collision. The paper also reports its computation time and concludes with simulation evidence across navigation and racing tasks.

E. Real-time implementation

  • ReSQ-MPPI averages 20.37 ms per step, with a 28.33 ms maximum, and completes ten consecutive laps at an average 5.9 s per lap without collision.The simulation runs at 40 Hz with 40 samples, a 40-step prediction horizon, and three SQP iterations; MPPI and MPC both collide.Source: E. Real-time implementation · p. 7

V. CONCLUSIONS

  • The paper reports improved safety and performance over nonlinear MPC and MPPI in navigation and racing simulations, with additional validation in a real-time vehicle simulation.The conclusion describes the framework as addressing local-optimality issues in nonlinear MPC and improving MPPI safety and performance.Source: §V · p. 7

APPENDIX

The appendix reports ablations of MPPI and MPC interconnections across cluttered-environment, kinematic-model, and dynamic-model simulations.

APPENDIX

Loading 2610.04406v1…