⚡ electrical engineering

Cooptimizing Safety and Performance Using Safety Value-Constrained Model Predictive Control

This paper proposes a Model Predictive Control (MPC) framework that ensures long-term safety for autonomous systems by incorporating a reachability-based safety value function as a terminal constraint, providing a less conservative and more robust alternative to traditional methods.

Original authors: Hao Wang, Nam Nguyen, Armand Jordana, Ludovic Righetti, Somil Bansal

Published 2026-04-28

📖 4 min read☕ Coffee break read

CC BY 4.0

Original authors: Hao Wang, Nam Nguyen, Armand Jordana, Ludovic Righetti, Somil Bansal

Original paper licensed under CC BY 4.0 (http://creativecommons.org/licenses/by/4.0/). ✨ This is an AI-generated explanation of the paper below. It is not written or endorsed by the authors. For technical accuracy, refer to the original paper. Read full disclaimer

Imagine you are teaching a high-speed delivery drone how to navigate a busy city. You want it to be two things: super fast (performance) and never crash (safety).

The problem is that in robotics, these two goals usually fight each other. If you focus only on speed, the drone might take a corner so fast that it realizes too late it’s heading for a building. If you focus only on safety, the drone becomes a "nervous wreck"—it moves so cautiously and stays so far away from everything that it never actually gets anything done.

This paper introduces a way to make the drone both a "pro athlete" and a "safety expert" at the same time. Here is how they do it, broken down into simple ideas.

1. The Problem: The "Short-Sighted Driver"

Most robots use something called Model Predictive Control (MPC). Think of MPC like a driver who only looks 50 feet ahead through a foggy windshield. The driver plans a perfect path for those 50 feet, drives a bit, and then looks ahead again.

The danger? The driver might see a clear road for the next 50 feet, but they don't realize that at 51 feet, there is a brick wall. By the time they see the wall, they are going too fast to stop. They are "short-sighted."

2. The Solution: The "Safety Map" (The Value Function)

The researchers added a "Safety Map" to the driver’s brain. Instead of just looking at the next 50 feet, the driver now has a mental map that tells them: "If you end up at this specific spot at the end of your 50-foot look-ahead, you will be in a 'Safe Zone' from which you can always recover, no matter what happens next."

They use a mathematical tool called HJ Reachability to create this map.

The Analogy: Imagine you are playing a game of soccer. You don't just look at where the ball is right now; you subconsciously know which parts of the field are "danger zones" (where you'll get trapped) and which are "safe zones" (where you have room to move). This paper gives the robot that same "spatial intuition."

3. The "Co-Optimization": The Balanced Athlete

In many robots, safety is treated like a "nanny." The robot plans a fast path, and then a separate safety system steps in at the last second to yank the steering wheel away. This makes the robot jerky and inefficient.

This paper does something different: Co-optimization. It puts the "Speed Goal" and the "Safety Map" in the same math equation.

The Analogy: It’s like training a gymnast. You don't teach them to do a flip, and then have a coach grab them mid-air to stop them from hitting the floor. Instead, you teach them to perform the flip specifically so that they land in a stable position. The speed and the safety are part of the same single movement.

4. How they proved it worked

They tested this on a complex robotic arm (a manipulator) that has many "joints," making it very difficult to calculate.

In Simulation: The robot had to move a heavy weight around an obstacle. The "standard" robots either crashed or got too close to the obstacle. The researchers' robot stayed safe and moved smoothly.
In the Real World: They put the software on a real, heavy-duty industrial robot arm. Even when the robot was carrying more weight than it was trained for, it successfully navigated around obstacles without crashing.

Summary in a Nutshell

Instead of a robot that is either fast and reckless or safe and slow, this paper creates a robot that is fast and smart. It uses a "safety map" to ensure that every move it makes—no matter how fast—always ends in a position where it is guaranteed to stay safe.

Technical Summary: Cooptimizing Safety and Performance Using Safety Value-Constrained MPC

1. Problem Statement

A fundamental challenge in autonomous robotics is the tension between task performance (e.g., speed, accuracy) and rigorous safety (e.g., obstacle avoidance, actuator limits). Current approaches often treat these as disconnected:

Safety Filtering: Acts as a reactive, corrective layer applied after a nominal controller is designed. This often leads to "myopic" or overly conservative behavior because the primary controller is unaware of the safety constraints during its optimization phase.
Standard Model Predictive Control (MPC): While MPC can handle constraints, its planning horizon is finite due to real-time computational limits. A finite horizon cannot guarantee that a trajectory, while safe for the next few seconds, won't lead the system into an "unrecoverable" state (a state from which safety violation is inevitable) once the horizon ends.

2. Methodology

The authors propose a framework that co-optimizes safety and performance by integrating a reachability-based terminal constraint directly into the MPC formulation.

Safety Value Function via HJ Reachability: The core of the method is the use of Hamilton-Jacobi (HJ) Reachability analysis. This identifies the maximal control-invariant safe set ( $S$ )—the largest possible set of states from which a control policy exists to keep the system safe indefinitely.
Terminal Constraint Integration: Instead of relying on local linearizations or conservative approximations, the authors augment the MPC cost function with a terminal constraint: $V_s(x_j(h)) \geq 0$ . This forces the MPC to ensure that at the end of its finite planning horizon, the system is guaranteed to be within the certified safe set.
Learning-Based Approximation: To overcome the "curse of dimensionality" (as HJ reachability is traditionally intractable for high-dimensional systems), the authors use DeepReach, a learning-based method that approximates the safety value function using neural networks.
Optimization: The resulting non-convex optimization problem is solved online using Sequential Convex Programming (SCP), allowing for real-time execution on high-dimensional systems.

3. Key Contributions

Theoretical Framework: A novel MPC formulation that uses a safety value function as a terminal constraint, providing a mathematical bridge between finite-horizon planning and infinite-horizon safety.
Recursive Feasibility Proof: The authors prove that if the system starts in a safe state, the proposed MPC scheme is recursively feasible, meaning it will always be able to find a safe control input at the next time step.
Scalability to High Dimensions: By combining HJ reachability with neural network approximations, the method is applied to a 14D manipulator (7 joint positions, 7 velocities), a significant jump from traditional reachability methods.
Co-optimization: Unlike reactive filters, this method allows the optimizer to "see" the safety boundary ahead of time, allowing for smoother, more performant trajectories that respect constraints.

4. Results

The framework was validated through extensive simulations and hardware experiments on a Flexiv Rizon 10s manipulator (carrying a 6.8kg payload).

Simulation Results:
- Safety Rate: The proposed method significantly outperformed standard MPC and matched/exceeded the safety of reactive "Smooth Blending" filters.
- Safety Margin: The method maintained a larger average distance to obstacles compared to baselines, providing a more robust buffer.
- Efficiency: It required fewer solver iterations than standard MPC, though it had a slightly higher computational cost due to the neural network passes.
Hardware Results:
- In real-world trials with a heavy payload and obstacle avoidance tasks, the proposed method achieved an 80% safety rate, whereas the standard MPC and safety filters achieved only 30% and 40%, respectively.

5. Significance

This work represents a significant step toward provably safe autonomous manipulation. By moving safety from a "reactive correction" to a "proactive constraint," the authors demonstrate that robots can operate near the limits of their capabilities (high performance) without sacrificing the rigorous safety guarantees required for real-world deployment in human-centric environments. While the reliance on neural networks means formal guarantees are currently limited by approximation errors, the framework provides a scalable path for controlling complex, high-dimensional robotic systems safely.