Safe Large-Scale Robust Nonlinear MPC in Milliseconds via Reachability-Constrained System Level Synthesis on the GPU

1. Problem Statement

The paper addresses the critical challenge of safe, real-time control for high-dimensional robotic systems (e.g., humanoids, quadrupeds) operating under uncertainty.

• The Core Conflict: Traditional Nonlinear Model Predictive Control (NMPC) is effective for trajectory planning but struggles with robustness (handling disturbances) and scalability.
  • Scalability: Standard NMPC solvers become computationally intractable for systems with $>60$ states and long planning horizons.
  • Robustness: Ensuring safety under disturbances usually requires reachability analysis. Traditional methods (Hamilton-Jacobi reachability) scale exponentially with state dimension, while data-driven methods often lack formal safety guarantees.
  • Existing Gaps: Existing GPU-accelerated MPC methods often ignore inequality constraints or fail to integrate robust reachability analysis, making them unsafe for uncertain environments.

The goal is to develop a framework that simultaneously optimizes a nominal trajectory, a robust tracking controller, and a closed-loop reachable set in real-time (milliseconds) for high-dimensional nonlinear systems.

2. Methodology: GPU-SLS

The authors propose GPU-SLS, a framework that unifies NMPC and System Level Synthesis (SLS) using GPU parallelism. The method operates within a Sequential Quadratic Programming (SQP) loop for the nonlinear problem and an Alternating Direction Method of Multipliers (ADMM) loop for the quadratic subproblems.

Key Technical Components:

1. Joint Optimization via SLS:

   • Instead of solving for a trajectory and a controller separately, the method uses SLS to parameterize the closed-loop response to disturbances.
   • It optimizes a nominal trajectory $(X, U)$ and a disturbance-feedback controller $(\Phi)$ simultaneously.
   • Constraints are tightened based on the computed forward reachable sets (the set of states the system can reach under worst-case disturbances), ensuring robust constraint satisfaction.

2. GPU-Accelerated ADMM Solver:

   • The core computational bottleneck in NMPC is solving the constrained Linear Time-Varying Quadratic Program (LTV-QP) at each SQP iteration.
   • The authors reformulate the QP using ADMM, splitting the problem into an unconstrained QP (primal update) and a projection step (dual update).
   • Parallel Associative Scans (PAS): Instead of sequential Riccati recursions (which are $O(N)$), the method uses reverse parallel associative scans to solve the primal update. This reduces the time complexity from linear to logarithmic with respect to the horizon length $N$.
   • Factorization Caching: To further accelerate ADMM, the solver caches matrix factorizations when the penalty parameter $\rho$ is fixed, avoiding redundant computations across iterations.

3. Complexity Reduction:

   • Prior State-of-the-Art: $O(N(n_x^3 + n_u^3))$ (Sequential, scales linearly with horizon).
   • GPU-SLS: $O(\log N \log^2 n_x + \log^2 n_u)$.
   • This logarithmic scaling allows the solver to handle horizons of $N=3000$ and decision variables up to $2 \times 10^5$ in real-time.

4. Real-Time Iteration (RTI):

   • To meet strict timing constraints (e.g., 50 Hz control loops), the framework employs an RTI scheme. It performs only one SQP iteration per control step, relying on the rapid re-linearization of subsequent steps to correct errors.

3. Key Contributions

• GPU-Parallel ADMM QP Solver: A novel solver for inequality-constrained LTV-QPs that leverages parallel associative scans and caching, achieving logarithmic depth scaling.
• Unified Robust NMPC Framework: A method that embeds the LQR solver into an SQP loop, enabling nonlinear trajectory optimization with robust constraint tightening derived from SLS.
• Scalable Reachability Analysis: A parallelized SLS approach that computes closed-loop reachable sets and robust controllers in milliseconds, replacing slow, sequential reachability methods.
• Hardware Validation: Successful deployment on high-dimensional hardware systems (61D quadruped, 75D humanoid) with formal safety guarantees.

4. Results and Performance

The paper validates the approach through extensive simulations and hardware experiments:

• Computational Speed:
  • Nominal Trajectory: Reduced solve times by 97.7% compared to state-of-the-art CPU solvers (OSQP, HPIPM) and 71.8% compared to existing GPU solvers.
  • SLS/Reachability: Achieved a 237× speedup over the previous SLS state-of-the-art (FastSLS) for long horizons.
  • Scalability: Solved problems with $2 \times 10^5$ decision variables and $8 \times 10^4$ constraints in <20 ms on average.
• Safety and Robustness:
  • 100% Empirical Safety: In hardware and simulation tests involving adversarial and random disturbances, the method achieved zero collisions.
  • Comparison: Outperformed data-driven baselines (e.g., DeepReach), which failed to certify safety in ~20% of rollouts due to inaccurate offline approximations.
  • Legged Robots: Successfully controlled a Unitree Go2 quadruped (61 states) and a Unitree H1 humanoid (75 states) navigating obstacle fields at 50 Hz.
• Hardware Experiments:
  • The quadruped successfully navigated obstacle fields in real-time (50 Hz) with an average solve time of 16ms, maintaining safety margins defined by the computed reachable tubes.

5. Significance

This work represents a significant breakthrough in robotic control theory and implementation:

1. Bridging the Gap: It successfully bridges the gap between theoretical robustness (formal reachability guarantees) and practical real-time performance on high-dimensional systems.
2. Paradigm Shift: It demonstrates that GPU parallelism can fundamentally change the complexity class of optimal control problems, moving from linear scaling to logarithmic scaling, making previously intractable problems solvable.
3. Safety-Critical Applications: By providing formal safety guarantees without the computational cost of traditional reachability analysis, this method enables the deployment of autonomous robots in dynamic, uncertain, and cluttered environments (e.g., search and rescue, autonomous driving, complex manipulation).
4. Open Source: The implementation is released publicly, fostering further research in scalable, safe robotics.

In summary, GPU-SLS provides a scalable, mathematically rigorous, and computationally efficient solution for safe control of complex robots, enabling them to operate robustly in the real world at speeds previously unattainable.