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

TL;DR

This paper introduces a safety value-constrained MPC that guarantees safety and high performance in autonomous systems by using a reachability-based safety value function for terminal constraints.

Contribution

It proposes a novel MPC formulation with safety value function-based terminal constraints that ensure recursive feasibility and less conservative safety guarantees.

Findings

Enhanced safety and constraint satisfaction in simulations and hardware tests.

Improved robustness over standard MPC and reactive safety filtering.

Maintained competitive task performance with safety guarantees.

Abstract

Autonomous systems are increasingly deployed in real-world environments, where they must achieve high performance while maintaining safety under state and input constraints. Although Model Predictive Control (MPC) provides a principled framework for constrained optimal control, guaranteeing safety beyond its finite planning horizon remains a fundamental challenge. In this work, we augment MPC with a safety value function-based terminal constraint that enforces membership in a control-invariant safe set at the end of each planning horizon. This formulation enables real-time synthesis of trajectories that are both high-performing and provably safe. We show that, under an exact safety value function and a feasible initialization, the proposed MPC scheme is recursively feasible, thereby ensuring persistent safety. In contrast to traditional terminal set constructions that rely on local linearizations or conservative approximations, our approach incorporates a reachability-based safety value function for terminal constraints, yielding less conservative and more expressive safety guarantees. We validate the proposed framework through simulation and hardware experiments on a Flexiv Rizon 10s manipulator. Results demonstrate improved constraint satisfaction and robustness compared to standard state-constrained MPC and reactive safety filtering, while maintaining competitive task performance. The full implementation and experiments are available on the project website.