WorksComputer Science
Beyond Terminal Constraints: Reliable Robot Control with Learned Safe Sets
Abstract
Ensuring safety in advanced robot control methods such as Model Predictive Control (MPC) and Reinforcement Learning remains an open challenge. A common approach relies on control-invariant sets, yet their exact computation for nonlinear systems is generally intractable, and numerical approximations compromise theoretical guarantees. In this work, we show that safety does not need to rely exclusively on control invariance. We introduce novel classes of safe sets, each broader than classical control-invariant sets, and analyze corresponding MPC schemes that guarantee recursive feasibility by constraining different nodes of the prediction horizon rather than only the terminal state. Furthermore, when the safe set is an inner approximation of a suitable control-invariant set, our methods can ensure safety through a safe-abort strategy, even in the absence of recursive feasibility. We validate our approaches in extensive simulations on a 6-joint robot manipulator, showing fewer constraint violations than state-of-the-art methods while maintaining comparable tracking performance and computational efficiency. Hardware experiments further demonstrate the practicality of our schemes for real-world robotic applications.
Introduction
Safety is a fundamental concern in robotics, playing a crucial role in enabling reliable, robust, and trustworthy operations. Safety concerns are multifaceted, including obstacle avoidance, prevention of falls in legged robots, and avoidance of joint and actuator limits, which could otherwise lead to hardware damage or system failure.
Among the many challenges in ensuring robotic safety, two areas stand out due to their importance and relevance: Human-Robot Interaction (HRI) and safe learning control. In HRI Zanchettin et al. (2016); Villani et al. (2018), robots must operate in proximity to humans, necessitating strict guarantees to avoid collisions and ensure human well-being. This requires advanced perception, planning, and control systems capable of responding rapidly to unpredictable human behavior while adhering to safety standards.
In parallel, the advent of learning-based control methods [23, 40] has revolutionized the field of robotics, enabling robots to adapt to complex and uncertain environments. However, the safety of learning-based control methods remains a critical concern, as they often lack formal guarantees and can exhibit unsafe behaviors during training or deployment. Ensuring safety of learning-based control methods is essential for their widespread adoption in real-world applications.
These challenges motivate the need for advanced control methods that can provide formal safety guarantees while enabling robots to operate in complex and dynamic environments. In the literature, there are three main tools to certify the safety of a controller: Control-Invariant Sets (CISs) [9, 20], Control Barrier Functions (CBFs) [2, 3] (); Wu et al. 2019), and predictive shielding methods based on Back-Up Policies (BUPs) [8]. These tools are typically applied to different control methods, respectively Model Predictive Control (MPC), Quadratic-Programming (QP) based control, and Reinforcement Learning (RL).
Despite the great amount of work devoted to safety in the last decade, ensuring safety for general high-dimensional nonlinear systems is still an open problem. A key challenge
Corresponding author:
Gianni Lunardi and Elias Fontanari.
Email: [email protected], [email protected] seems to be the computation of so-called CISs (a.k.a. safe sets).
Contributions
In this paper, we introduce new classes of safe sets that are broader than CISs. Indeed, as shown in Figure 1, these novel classes contain the class of CISs, and rely on the concept of multi-step safety, which allows the state to leave the set for a limited number of time steps before entering it again. These sets generalize the concept of CIS, meaning that they must satisfy weaker requirements than CISs. We hypothesize that these new sets might therefore be easier to compute and certify with numerical approximation methods. This is confirmed by empirical evaluations on the computed safe sets, even though we do not thoroughly validate this hypothesis in this paper. Instead, we show that three existing MPC formulations (Receding-Constraint MPC, Hard Terminal-Constraint MPC, and Parallel-Constraint MPC) can ensure RF or safety assuming the safe set belongs to a specific class. Unlike classic approaches that only use the safe set as a terminal constraint, these MPC schemes apply the constraint at different nodes along the prediction horizon. With respect to our previous work Lunardi et al. [30]; Fontanari et al. [18], we present the following extensions spanning theory, computational efficiency, and validation:
Figure 1. Hierarchy of safe set classes proposed in this work. The innermost set represents the well-known class of Control Invariant Sets (CISs). We introduce broader classes, namely up-to--step CISs, within--step CISs, back-and-forth--step CISs, and back-and-forth-within--step CISs.
We present four classes of safe sets that contain the class of CISs, of which only one was discussed in our previous work.
We correct a mistake in the theory of Receding-Constraint MPC Lunardi et al. [30], clarifying the property of the safe set that it needs to ensure RF.
We prove that Parallel-Constraint MPC can ensure RF under weaker conditions than Receding-Constraint MPC, defining the associated novel class of safe sets (back-and-forth-within--step CISs).
We prove that the classic Hard Terminal-Constraint MPC can ensure safety (without RF) even if the terminal constraint set is not a CIS, but it belongs to the larger class of back-and-forth--step CISs.
While in our previous work the safe-abort procedure was too slow for a real-time implementation, this limitation is mitigated through a suitable choice of the safe set and the optimization algorithm.
While our previous work had only considered joint-space constraints, we consider here more realistic scenarios featuring Cartesian obstacles and self-collision constraints. Moreover, our open-source software implementation3 integrates a URDF-based dynamics library (ADAM4), which makes our software easily adaptable to any robotic system.
We present a broad simulation-based statistical analysis comparing several MPC formulations (novel and classic) in different working conditions (initial conditions, horizon lengths, sizes of the safe set, modeling errors).
We report on the first implementation of Receding-Constraint MPC Lunardi et al. [30] on hardware, which validates the practical applicability of the approach on a full 6-DOF robot manipulator.
The rest of the paper is structured as follows. In § 2, we review prior research addressing safety challenges in robotics. § 3 introduces the nominal MPC formulation and related concepts. § 4 presents the analyzed MPC formulations together with the respective safe-set definitions and the proofs of RF. § 5 and § 6 provide details on the safe-abort procedure and the computation of the safe sets. § 7 discusses the gap between the developed theory and the practical implementation. In § 8, we validate our approaches through simulations on a robotic manipulator. Then, we show the applicability of Receding-Constraint MPC on the real robot in § 9. Finally, § 10 draws the conclusions.
Related Work
MPC and Control Invariant Sets
MPC has emerged as a promising approach to address safety challenges. Indeed, given its optimization nature, MPC enables the incorporation of safety constraints within the control framework. This allows robots to operate safely in complex environments while adapting to changing conditions and uncertainties. In practice, MPC for constrained nonlinear systems still strives to persistently satisfy constraints, a problem referred to as Recursive Feasibility (RF). Indeed, while constraint satisfaction is ensured for infinite-horizon MPC, this is in general computationally intractable Rawlings et al. [37], Chapter 2). In practice, RF is achieved with a long enough horizon Grüne and Pannek [20], Chapter 8), but in general we cannot know how long it should be, and even if we knew it, it may result in unacceptable computation time. For finite horizon MPC, RF can be guaranteed if a CIS is used to constrain the terminal state of the MPC horizon Mayne et al. [31]. Still, CISs are generally unknown for nonlinear systems, which include most robotic systems. Trivial CISs often exist (e.g., the equilibria of the system), and are known. However, these CISs are small and result in poor performance, e.g., by making the system move slowly to reach zero velocity by the end of the horizon. Different numerical techniques exist to approximate larger CISs Djeridane and Lygeros [16]; Coquelin et al. [11]; Jiang et al. [25]; Rubies-Royo and Tomlin [39]; Hsu et al. [22]; Dawson et al. [13]; Zhou et al. (2020); La Rocca et al. [27]; Leung et al. [28], and they typically rely on function approximators, such as deep Neural Networks (NNs). However, all safety guarantees are lost if the computed set is not exactly control invariant, which cannot be guaranteed in general by these numerical methods. Moreover, even an under-approximation of the viability kernel (i.e., the maximal CIS) is itself not necessarily control invariant. The most common approaches for certifying approximated CISs are based either on Lipschitz constant estimates Boffi et al. (2020) or on a learner-verifier architecture combined with optimization methods Dai et al. [12]. Both approaches, however, face significant limitations: estimating Lipschitz constants can be difficult and often provides overly conservative bounds, while also suffering from the curse of dimensionality; learner-verifier methods require solving optimization problems whose computational cost grows exponentially with the number of neurons in the NN used to approximate the CIS.
Control Barrier Functions (CBF)
CBFs have emerged as an alternative to CISs: indeed, these two frameworks are strongly connected, since the zero-sublevel set of a CBF defines a CIS. CBFs are often used in safety filters: a desired control input is minimally modified through a QP to satisfy the CBF decrease condition Wabersich et al. (2023). In Xu et al. (2015), the authors prove the robustness of CBFs in presence of model uncertainties. This type of controller has been deployed in different robotic contexts, from mobile robots to manipulators and quadrupeds Singletary et al. (2022); Jian et al. [24]; Grandia et al. [19]. Nonetheless, similarly to CISs, CBFs do not have a general constructive method for nonlinear systems: practitioners tried to synthesize CBFs using Sum-of-Squares Programming Wang et al. (2023), for which analytical certifications exist, or learning techniques Dawson et al. [14, 13]; Robey et al. [38], but still it is difficult to verify the satisfaction of Nagumo’s condition Nagumo [33] on the whole state space for high-dimensional systems.
Model Predictive Shielding (MPS)
A different approach is MPS Bastani et al. [8]; Bastani [7]; Wabersich et al. (2022), which exploits a BUP that tries to drive the system to a known CIS. Contrary to MPC, MPS can achieve good performance even if this CIS is extremely small, thanks to the use of the BUP. Before applying the control computed by the nominal policy, the safety of the next state is verified by simulating the dynamics with the BUP. If the state reaches the CIS, then this certifies the safety of the next state, and the nominal policy can be used. Otherwise, the BUP is used at that time step, and the process is repeated at the next iteration. Contrary to CISs and CBFs, BUPs do not require certification, since their ability to reach the CIS is checked at each control loop. In addition, MPS does not demand the system dynamics to be differentiable, as the BUP is known in advance and simply applied at runtime. However, MPS requires simulating the system dynamics for potentially long horizons, which can be too computationally demanding for real-time applications. Moreover, since the nominal policy is unaware of safety, this can lead to frequent use of the BUP, resulting in poor performance.
Hamilton-Jacobi Reachability (HJR)
Safety has also been tackled with HJR analysis, which is closely related to control invariance theory and CBFs. HJR Bansal et al. [5] solves the Hamilton–Jacobi–Isaacs partial differential equation to compute finite or infinite-horizon Backward Reachable Tubes (BRTs), i.e. sets of states from which system trajectories inevitably reach a target set within the given time horizon, regardless of the control input. If the target set encodes constraint violations, its complement is a CIS in the infinite-horizon setting. Typically, HJR approaches take into account disturbances acting on system dynamics: indeed, safety-critical scenarios are represented as zero-sum differential games Bansal et al. [5], where Player A is the control input that moves the system towards the target, while Player B models the disturbance that pushes the system to violate the safety constraints. HJR is based on Dynamic Programming theory, thus it suffers from the curse of dimensionality. Nonetheless, recent learning approaches have been able to mitigate this issue: in Bansal and Tomlin [6], the authors proposed DeepReach, a method that exploits sinusoidal NN to solve high-dimensional reachability problems leveraging GPUs. In So et al. (2024), the relationship between HJR and CBF has been exploited to develop a novel class of neural CBFs, which has been tested with state spaces up to 16D. Another method to compute infinite-time BRTs is the Viability-Boundary Optimal Control (VBOC) algorithm La Rocca et al. [27]: VBOC finds states directly on the border of the viability kernel, thus requires less sampling than HJR and CBF methods. Nevertheless, all these methods only obtain approximate CISs and, due to their learning-based nature, do not offer any formal guarantees. Moreover, VBOC is applicable only to fully actuated systems.
Multi-Step Control Invariance
While all the works discussed so far revolved around classic control invariance, in Zhao et al. (2024), the authors proposed a novel class of safe sets: -step CISs. These sets have the property that from every state inside them it is possible to reach the set in at most steps. In other words, -step CISs allow the state to exit the set for at most steps. Using this type of sets, they developed a novel MPC scheme that guarantees RF. Assuming an MPC formulation with an -step horizon, they solve MPC problems with different horizons (from to ) and a -step CIS as terminal constraint; then they prove that at least one of the problems is guaranteed to be feasible. However, this results in long-horizon problems that can be demanding to solve in fast control loops.
A similar approach has been proposed in Lunardi et al. [30], where an -step CIS constraint moves backward along the horizon at each MPC iteration. Moreover, the safe CIS is also used as a soft terminal constraint to ensure persistent RF. Those concepts have been pushed further in Fontanari et al. [18], where (up to) MPC problems are solved in parallel, each of them having the safe-set constraint on a different node of the horizon. Differently from Zhao et al. (2024), the last two methods are more amenable to real-time implementation as they do not require increasing the horizon length.
Safety for Robotic Manipulation
Certifying the safety of motion planning algorithms in HRI is critical. A popular approach to safe manipulation is CHOMP Zucker et al. (2013), which iteratively optimizes the trajectory starting from an initial guess by using function gradient techniques, making a trade-off between smoothness and obstacle avoidance. Safety is mostly guaranteed through costs and distance fields rather than control invariance or reachability analysis. In Holmes et al. [21], a set of parameterized reachable trajectories is precomputed offline for each manipulator joint and then combined online to form conservative reachable sets in the workspace, which serve as collision-avoidance constraints within a receding-horizon optimizer. Michaux et al. [32] extend this idea from kinematic planning to uncertain manipulator dynamics by combining polynomial-zonotope reachable sets with a robust passivity-based controller. In collaborative robotics, safety has also been studied through speed and separation monitoring Scalera et al. (2024), where the supervisory controller guides the robot along a path-consistent stop trajectory when collision danger between human and robot is detected. Also, the size of the robot safety is adapted based on the stopping time of the manipulator.
Overall, these works show that manipulator safety is often handled by exploiting platform-specific structure, such as serial kinematics, swept-volume approximations, simple geometric primitives, and braking maneuvers, rather than applying generic safe-control tools directly.
Background
This section introduces the notation used in this paper, the problem of safety in MPC, together with the concepts of Control-Invariant Set (CIS) and Recursive Feasibility (RF).
Notation
Throughout this work, the following notation will be used:
stand for the sets of natural and real numbers, respectively;
represents the identity matrix (its dimension can be inferred by the context);
represents the system state, while denotes the control input;
indicates the first elements of the vector , while denotes all elements of starting from the -th one up to the last one;
designates a discrete-time trajectory, namely the sequence , where the single subscript indexes the internal time step within the sequence;
indicates the state predicted at step , obtained when solving the MPC optimization problem at time ; when the conditioning index is omitted, the remaining subscript refers to the time step ;
and are respectively the feasible state and control sets, where the control set is in general state dependent;
is the safe set used by the MPC formulation to ensure safety;
the symbol is used for the optimal values;
the symbol is used for the solver guess (i.e., previous MPC solution).
Problem Statement
Let us examine a discrete-time dynamical system subject to state constraints and (state-dependent) control constraints:
The set represents the set of admissible states. In particular, it includes both the intrinsic state bounds of the system and state constraints arising from the environment, such as collision-avoidance constraints from static obstacles. The objective is to design a control strategy that guarantees safety (i.e., constraint satisfaction), while at the same time retaining performance (i.e., minimizing the cost) as much as possible. We denote by the set of the equilibrium states of the system, namely:
To accomplish our goal, we make use of the concepts of -step Backward-Reachable Set and Control Invariant Set Blanchini [9].
Definition 1. Given a set , the -step Backward Reachable Set (BRS) of is defined as the subset of starting from which it is possible to reach in time steps without leaving :
Definition 2. Given a set , we say that is a Control Invariant Set if and only if:
As all BRSs of equilibrium states, the set is a CIS Blanchini [9]. Namely, starting inside , it is possible to remain inside indefinitely because we can reach and then remain in forever. If we knew (or any other CIS) we could use it to construct a safe controller. Now we discuss how the knowledge of a CIS can be exploited in an MPC formulation to achieve safety.
MPC and Recursive Feasibility (RF)
Let us consider the following MPC problem:
where is the running/terminal cost, is the current state, and is the terminal set Mayne et al. [31].
A standard approach to enforce RF in MPC consists in introducing a terminal set that constrains the final state to lie within a CIS, for instance . Unfortunately, we cannot assume to know in general, since CISs are notably difficult (and often impossible) to compute for nonlinear systems. As recalled in § 2.1, numerical techniques are available to approximate , yet an approximate set is generally not control invariant. Therefore, adopting as terminal set in the MPC formulation does not guarantee RF. A widely used strategy to handle the potential loss of feasibility is to soften the terminal constraint by introducing a slack variable, which is heavily penalized in the cost function Kerrigan and Maciejowski [26]; Zeilinger et al. (2014). In this way, even when the terminal constraint cannot be met exactly, the optimization still provides a solution that enables control of the system, with the expectation that feasibility of the terminal constraint will be eventually recovered. Nonetheless, this approach does not guarantee safety or RF, since the softening allows the state to exit the set , which eventually may cause constraint violations.
Safe Model Predictive Control
This section introduces the analyzed MPC schemes, namely the Receding-Constraint MPC, the Hard Terminal-Constraint MPC, and the Parallel-Constraint MPC. These controllers are presented together with the novel safe sets that are necessary to guarantee RF or safety.
Receding-Constraint MPC (RC-MPC)
RC-MPC Lunardi et al. [30] is based on the concept of receding constraint: instead of relying exclusively on the final state to ensure safety, RC-MPC exploits the fact that, as long as at least one state (with ), we know that is safe because from we can reach . RC-MPC dynamically adjusts online the time step at which the state is required to lie in . For example, if during iteration of the MPC scheme the condition was satisfied, then at iteration we can infer (under the assumption of no disturbances or modeling errors) that is feasible, and thus we can impose it as a hard constraint. This mechanism ensures safety for consecutive iterations, with the receding constraint shifting backward along the prediction horizon. Once the constraint has receded all the way to step 0, however, we can no longer rely on it to ensure safety. For this reason, RC-MPC also incorporates a soft constraint on the terminal state, encouraging it to remain inside . A simple example highlighting the working principle of RC-MPC is depicted in Figure 2, that presents a corrected version of a similar scheme in Lunardi et al. [30].
Figure 2. Example of RC-MPC with and a 3-step CIS. At MPC iteration 3, the condition implies, from (7), that the state can return to in 3 steps. The soft terminal constraint can be and can be satisfied. Consequently, the receding constraint shifts forward to time step 2.
This MPC scheme is summarized in Alg. 1 and it relies on the following OCP:
where the L1 penalty, with weight , is applied to the slack variable , denotes the Minkowski sum, and is the unit ball. After completing the MPC optimization at iteration , we verify whether . If this condition holds, then at cycle the receding constraint can be shifted to , which guarantees safety for additional iterations. Even if not theoretically necessary, to improve the empirical performance we actually check whether for any (Line 6 in Alg. 1), and if that is the case we set . The last optimal solution is shifted by one time step and used as initial guess for the next MPC iteration (Line 9).
| Algorithm 1 | Receding-Constraint MPC |
| Require: | Number of time steps , Initial state , Initial guess , up-to--step CIS, OCP (6) |
| 1: | — Receding constraint index |
| 2: | for do |
| 3: | |
| 4: | for do — Search last state in |
| 5: | if then |
| 6: | |
| 7: | — Recede constraint |
| 8: | — Simulate dynamics |
| 9: |
Table 1.
To clarify the theoretical properties of RC-MPC, we introduce two novel classes of multi-step CISs.
Definition 3. Given a natural number , a set is -step control invariant3 if, starting from any state in , it is possible to reach in exactly time steps:
This notion generalizes the classical concept of control invariance, where the special case of 1-step control invariance coincides with the standard definition. We now introduce two classes of safe sets.
Definition 4. A set is up-to--step control invariant if there exists a value such that is -step control invariant. This condition can be expressed as:
Notice that the class of up-to--step CIS contains all -step CIS for all going from 1 to .
Definition 5. A set is within--step control invariant5 if, starting from any state in , it is possible either to remain in , or to leave and come back to it within time steps:
Using the notation introduced above for BRS, this condition can also be expressed as: .
This definition is an extension of the previous one; indeed, any up-to--step CIS is also a within--step CIS. The difference between these set classes is that for up-to--step CISs the number of steps to return to the set is the same for all states, while for within--step CISs this number can be different for different states.
Now we can state the conditions for the recursive feasibility of RC-MPC (6).
Theorem 1. Consider the RC-MPC formulation (6). Assuming that the set is up-to--step control invariant, and the penalty on the soft terminal constraint is larger than the associated optimal Lagrange multiplier, then the RC-MPC described in Alg. 1 is recursively feasible.
Proof. Suppose that the RC-MPC is feasible at the initial iteration , which means that . In this case, RF is ensured for at least subsequent iterations, during which the receding constraint can shift backwards along the horizon. Throughout these iterations, two possible scenarios can occur. (i) While receding the condition (Line 5 of Alg. 1) is satisfied for a certain . Then the receding constraint moves forward to time step . This scenario is possible, however it is not guaranteed to occur. (ii) Since the set is up-to--step control invariant by assumption, we know it exists such that is -step control invariant. Suppose the receding constraint reaches node , since is -step control invariant, the soft terminal constraint is feasible because condition (7) ensures it exists a state trajectory starting from reaching in time steps. Assuming that the weight is sufficiently large Nocedal and Wright [35], the adopted L1 penalty function becomes exact. As a consequence, we can infer that the soft terminal constraint will be exactly satisfied. Once this occurs, the hard receding constraint can be moved to step , which guarantees RF for additional iterations. By repeating the same reasoning, RF can then be maintained indefinitely. □
This theorem highlights how RC-MPC guarantees RF even if the set is not control invariant. We rely indeed on a weaker condition, which is up-to--step control invariance.
| Algorithm 2 Hard Terminal-Constraint MPC |
| Require: Number of time steps , Initial state , Initial guess , , back-and-forth--step CIS, OCP (9) |
| 1: for do |
| 2: |
| 3: if False then |
| 4: Copy last feasible solution |
| 5: |
| 6: Simulate system |
| 7: |
Table 2.
Our condition is weaker because any 1-step CIS is also up-to--step control invariant, therefore our approach guarantees RF for a larger class of sets, which contains the class of CISs.
Comparison with Lunardi2024
In previous work Lunardi et al. [30], within--step CIS (Def. (5)) was incorrectly used for proving the RF of RC-MPC. The error stems from the implicit assumption that the state satisfying the receding constraint remains the same throughout the MPC iterations. If this were the case, the proof in Lunardi et al. [30] would be correct. However, since this is not the case in general, it can happen that changes throughout the MPC iterations in such a way that always prevents reaching from in time steps.
Hard Terminal-Constraint MPC (HTC-MPC)
Let us consider a classic Hard Terminal-Constraint MPC (HTC-MPC) formulation with terminal constraint set :
As discussed in § 3, we cannot guarantee RF of problem (9) if is not a CIS. However, when problem (9) is unfeasible, we can use the last computed solution while we keep trying to solve the problem at each MPC iteration. This idea is detailed in Alg. 2, and in the following we prove that it leads to safety under a weaker condition than control invariance.
Definition 6. A set is back-and-forth--step control invariant if it satisfies the following condition:
The reason for the name back-and-forth is that this condition requires that from every state it must be possible to move backward in time of time steps (inside ) and then forward in time of time steps to reach .
Theorem 2. Assuming is back-and-forth--step control invariant, the HTC-MPC described in Alg. 2 is safe.
Proof. Assume the HTC-MPC is feasible at the first MPC iteration, which implies that . Without loss of generality, suppose that at the MPC iteration 1 the OCP gets unfeasible, thus the terminal constraint cannot be satisfied. In this scenario, we use the last solution to control the system and we keep trying to solve the OCP (9). After steps of unfeasibility, we will have , since . This implies that . Since is a back-and-forth--step CIS by assumption, we know that in one of those cycles we must have: . This implies that , and therefore the terminal constraint must be feasible. When this happens, the solution ensures safety for another iterations. At this point the same reasoning can be applied to ensure safety indefinitely.
This theorem proves the safety of HTC-MPC with a back-and-forth--Step CIS, when used as described in Alg. 2. However, this safety comes at the cost of potentially controlling the system in open-loop for up to time steps. This could lead to poor performance in practice, and is therefore a drawback with respect to the other two MPC formulations discussed in this work, which can instead ensure RF.
Parallel-Constraint MPC (PC-MPC)
Starting from the idea of RC-MPC (§ 4.1), instead of using a single state along the horizon to ensure safety, Parallel-Constraint MPC (PC-MPC) Fontanari et al. [18] tries to reach the safe set at different time steps exploiting parallel computation. As long as at least one state (with ), we can ensure constraint satisfaction because . This is because from we can reach in steps. If parallel computation is available, we can solve problems in parallel, each constraining the state inside at a different time step :
Then, we can use the solution of one of the problems that have been solved. Since our main concern is safety, we decide to use the solution satisfying the safe set constraint at the furthest time. A pseudo-code is available in Alg. 3. Now we focus on the theoretical properties of PC-MPC, starting with a novel sufficient condition on the safe set to ensure RF.
Algorithm 3 Parallel-Constraint MPC
Require: Number of time steps T, Initial state x_0, Initial guess {x_i}_0^N, {u_i}_0^{N-1}, S back-and-forth-within-N-step CIS, OCP (11)
1: N_safe ← N
2: for t = 0 → T − 1 do
3: for p = 1 → N do
4: {x_i^p}_0^N, {u_i^p}_0^{N−1} ← OCP(p, x_t, {x_i^p}_0^N, {u_i^p}_0^{N−1})
5: j ← N − 1
6: for j = N_safe → N do
7: if x_j^p ∈ S then
8: τ_p ← j
9: p ← ArgMax({τ_p}_{p=1}^N)
10: N_safe ← p − 1
11: x_{t+1} ← f(x_t,u_0^p)
12: {x_i}_0^N, {u_i}_0^{N−1} ← WarmStart({x_i^p}_1^N,{u_i^p}_1^{N−1})
Theorem 3. Assuming is back-and-forth-within--step control invariant, the PC-MPC formulation described in Alg. 3 with OCP (11) is recursively feasible.
Proof. The safe horizon length is initialized to and it decreases by at most 1 per control loop. To prove this theorem, we show that every time takes the value , it is increased to a value . This proves recursive feasibility because as long as the MPC algorithm retains feasibility.
Having implies that from the current state we can reach the safe set in time steps, i.e., . By assumption (12) we know that . Putting it all together we can infer that . This means that from it is possible to reach in time steps, for some value . Given that, we know that the OCP (11) must be feasible using as initial state for , implying that will be increased (at least) to in line 10 of Alg. 3.
The reason for the weaker requirements of PC-MPC w.r.t. RC-MPC and HTC-MPC is hidden in their working principles. Indeed, RC-MPC features two safety constraints in its formulation (a hard receding constraint and a soft terminal constraint). Both of them must be satisfied to slide the receding constraint forward and ensure RF. Therefore, the system must be able to reach at the terminal node starting from at the receding-constraint node. HTC-MPC is somewhat more similar to PC-MPC because they both work with a single safety constraint per OCP. However, while HTC-MPC only tries to satisfy it at the terminal node, PC-MPC tries it on all nodes of the horizon, which reasonably leads it to weaker requirements.
Summary of safe sets and MPC formulations
We have presented five types of multi-step CISs, which generalize the concept of CIS, as depicted in Figure 1. To summarize, we report here the different safe-set conditions. Classic CISs must satisfy the following condition:
-step CISs must satisfy:
Up-to--step CISs must satisfy:
Within--step CISs must satisfy:
Back-and-forth--step CISs must satisfy:
Finally, back-and-forth-within--step CISs must satisfy:
These sets are related to BRS, and Appendix § 11 discusses their relationship. Figure 1 shows the relationships between these set classes, highlighting that most of them can be seen as relaxations of the previous ones. We have shown how three MPC formulations from the literature, namely RC-MPC, HTC-MPC, and PC-MPC, can guarantee RF or safety using these novel multi-step CISs.
Unfortunately, the exact computation of any of these multi-step CISs remains as challenging as determining a standard CIS. In practice, however, numerical procedures for approximating CISs are more likely to produce a set that belongs to one of the newly introduced classes, rather than a CIS. Therefore, as empirically suggested by our results, these approaches tend to be safe with higher probability compared to a conventional MPC scheme, even though, in practice, the assumptions underlying Theorems 1, 2 and 3 cannot be guaranteed.
Safe Task Abortion
Since we do not have any theoretical guarantee on the set , in practice we cannot ensure RF of any of the MPC schemes discussed above. Thus, we rely on the safe-abort strategy proposed in Lunardi et al. [30] to safely stop the robot in case of unfeasibility. To ensure safety we rely on the following three assumptions.
Assumption 1. The safe set is a conservative approximation of the set :
Note that this assumption does not require that satisfy any type of multi-step control invariance.
Assumption 2. To use HT-MPC and RC-MPC we assume the availability of two computational units, denoted as unit A and unit B. To use PC-MPC we assume the availability of computational units.
In this paper, a computational unit denotes an independent execution resource, such as a processor core or computational thread, capable of running one control-related computation in parallel with the others.
Assumption 3. We can solve the following OCP for any , in time steps (with ):
OCP (19) can be employed to compute a feasible trajectory that steers the system from to an equilibrium state. Since only constraint satisfaction is relevant for the abort procedure, we use 1 as a dummy cost function. In the following we discuss how the safe-abort procedure is implemented with the three MPC schemes discussed in § 4.
Safe Abort with HTC-MPC
Let us consider the HTC-MPC formulation given by problem (9), and assume that at iteration the MPC problem becomes unfeasible. In this scenario, the task can be safely aborted by following these steps:
unit A employs the MPC solution computed at iteration while attempting to solve the MPC problem (9) for the current state of the robot;
simultaneously, unit B solves OCP (19), taking as initial state;
if the system reaches (because the MPC problem resulted to be unfeasible for consecutive iterations), the solution of OCP (19) is followed to safely bring the system to an equilibrium state.
This strategy allows us to reach a safe equilibrium state, from which we can try to solve again the MPC. We have noticed that sometimes this approach leads to completing the task after the abort procedure, increasing the success rate. This strategy ensures safety, as formalized in the following Lemma.
Lemma 1. Under Assumptions 1 to 3, the HTC-MPC (outlined in Alg. 2 with safe task abortion (described above) guarantees that constraints are never violated.
Proof. This proof follows directly from the stated assumptions. OCP (19) is always feasible because, by Assumption 1, any state within can reach an equilibrium in at most time steps. Assumption 3 (with ) ensures that dedicating a computational unit to solving OCP (19) provides a solution before reaching the terminal state of the last feasible MPC problem. Once is reached, following the solution of OCP (19) safely drives the system to an equilibrium state, where it can remain indefinitely without violating any constraints.
The most critical assumption is probably Assumption 3, which requires the MPC horizon to be sufficiently long, and that is not excessively large, to provide adequate time to solve OCP (19). However, contrary to Lunardi et al. [30], we suggest to choose so that OCP (19) is not too computationally demanding. Of course this implies a trade-off because choosing a small value of would result in a small safe set , leading in turn to poor performance, so we must choose as large as possible, while satisfying the computation time constraint.
Algorithm 4 Receding-Constraint MPC with Safe Abort
Require: Number of time steps , Initial state , Initial guess , , OCP (6), Safe-abort OCP (19), Safe-abort trigger time
▷ Receding constraint index
for do
if then ▷ Abort task
FOLLOWSAFEABORTTRAJECTORY()
▷ Reset
if and then
for do ▷ Search last state in
if then
▷ Move receding constraint forward
▷ Recede constraint
if then ▷ Trigger Safe Abort
SOLVESAFEABORTOCP
▷ Simulate dynamics
WARMSTART
Safe Abort with RC-MPC and PC-MPC
Let us assume that we are using the RC-MPC formulation, and that the receding constraint on node has reached time step . At this point we would need to trigger the task-abortion strategy because we have only time steps to solve OCP (19). However, for ensuring RF of RC-MPC under the assumptions of Theorem 1, we need to allow to reach node 0. This would imply solving the safe abort OCP in zero time, which is impossible. Therefore, to ensure consistency with the safe abort procedure, while maintaining RF, we propose an extended version of RC-MPC. This new version (summarized in Alg. 4) does not alter the computed trajectory after has reached node , unless can be moved forward.
Suppose that at time step the receding constraint has reached . At this point we must use Unit B to start solving the safe-abort OCP (19) using the predicted state as initial state. In parallel, unit A can be used to keep trying to solve the RC-MPC formulation from the current state. If the solution of RC-MPC satisfies the terminal constraint, we can resume normal operation, setting . Otherwise, we use the solution computed at time step to control the system, until we reach , and then we follow the solution of the safe-abort OCP to reach an equilibrium state.
The same strategy outlined above for RC-MPC can also be used for PC-MPC. When the receding constraint node has reached the value we start the safe-abort computation. At the same time we stop using the new trajectories computed by PC-MPC, unless they satisfy the safety constraint on a node after . We use instead the last solution computed when .
Solving the Safe Abort OCP
Clearly the safe abort OCP should be solved as efficiently as possible to guarantee good performance of the controller. One possible strategy is to pre-compute a warm-start for OCP (19), before reaches 0. Although the exact state of the system at that moment is unknown, the trajectory predicted by the MPC can be used as initial guess. If this warm-start proves to be insufficient, RC-MPC can be modified to ensure that the pre-computed safe-abort trajectory begins precisely at the state where the task abortion is triggered. To accomplish this, the receding constraint can be adjusted from to the more conservative , effectively forcing the predicted state within to remain unchanged across the MPC cycles. However, this strategy is expected to reduce overall performance: indeed, we have tested this solution in simulation and found that it performs poorly. The adopted solution in our work was to rely on the Real-Time Iteration scheme Diehl et al. [15] for solving OCP (19) (further details are provided in § 7.5). In this case, Assumption 2 is no longer required: when the safe abort procedure is triggered, unit A swaps the MPC problem with OCP (19) and solves it in one time step, without any involvement of unit B.
Safe Set Computation
This section provides some details on how the safe set used in the MPC formulations is computed. Among the different methods mentioned in § 2.1 for approximating CIs, we chose VBOC La Rocca et al. [27]. Indeed, VBOC is an efficient algorithm for fully-actuated systems (like robot manipulators) since it can sample states directly on the border of the safe set. In the original work, VBOC is used to approximate the viability kernel, which is assumed to be equivalent to the -step BRS of the equilibria . We note that the two notions are not identical in full generality: imposes asymptotic convergence to , whereas viability only requires indefinite constraint satisfaction. VBOC finds states exactly on the set’s boundary through the following OCP:
where the quantities , , and are defined as:
The main insight of OCP (20) is to maximize the velocity norm of the initial state by fixing the initial configuration to and the direction of the initial velocity in joint space to , chosen as uniformly random values. Even though the shape of is not known a priori, uniform sampling of and allows a uniform coverage of the input space of our set representation, i.e., the space of joint angles and joint velocity directions. The constraint ensures (all the proofs can be found in La Rocca et al. [27]).
Under the assumption adopted in the original VBOC formulation that the relevant viability boundary coincides with the boundary of , the horizon must be sufficiently long: starting from a reasonable initial guess, problem (20) is repeated with an increasing , until the optimal cost converges.
Algorithm 5 Modified version of the VBOC algorithm
Require: Number of trajectories $K$, Time horizon $M$, Lower and upper bounds of the joint positions $q_{\min}$, $q_{\max}$, OCP {xref}`(21)`
1: $\mathcal{D} \leftarrow [\ ]$
2: $k \leftarrow 0$
3: repeat
4: $q \leftarrow \operatorname{RANDOMUNIFORM}(q_{\min},q_{\max})$
5: if $\operatorname{ISCOLLISIONFREE}(q)$ then
6: $d \leftarrow \operatorname{RANDOMNORMAL}(0,1)$
7: $d \leftarrow d/\lVert d\rVert$
8: $a,H,h \leftarrow$ {xref}`(22)`
9: $\{x_i\}_{0}^{M+1},\{u_i\}_{0}^{M} \leftarrow \operatorname{OCP}(a,H,h)$
10: insert $x_0^*$ in $\mathcal{D}$
11: $k \leftarrow k+1$
12: until $k=K$
13: $\mathcal{D} \leftarrow \operatorname{REBALANCEVELOCITYNORM}(\mathcal{D})$
14: return $\mathcal{D}$
Software Implementation
This section discusses the software implementation of the MPC methods. In particular, we focus on how to integrate the nonlinear dynamics of the robot manipulator and the approximated CIS inside a unique MPC formulation for Alg. 4 and Alg. 2, 3 with safe task abortion. We have used CasADi Andersson et al. [4] for the symbolic computation of the dynamics, costs, and constraints, and Acados Verscheuren et al. (2019) to solve the OCPs and integrate the dynamics. The whole framework has been developed using Python.
The first state obtained by (20) is always on , and in the paper the authors have proposed an algorithm to check if the successive states are also on .
We have slightly modified the algorithm proposed in La Rocca et al. [27] to better perform in our setting. Indeed, the original implementation did not consider the presence of Cartesian obstacles in the environment. In addition, in our work we are interested in -step BRS , which are subsets of the -step BRS , therefore the iterative procedure to increase the horizon is no longer needed for our purpose.
The modified version of the algorithm is shown in Alg. 5. Given the presence of Cartesian obstacles inside the environment, after the sampling of a joint configuration we must check if it is in collision (Line 5). In case the configuration is feasible, the VBOC problem can be solved (Line 10). The process is repeated until we find states on , collected in the dataset .
Algorithm 5 needs more samples than the original formulation, since it only uses the first state computed by the OCP, even though other states from that trajectory could be on . However, this comes with the great benefit of having uniformly distributed data, which would not be the case using the original algorithm. Moreover, the computational cost of our approach is rather close to the one of the previous implementation: indeed, each OCP takes less time to be solved, since the horizon is fixed and smaller than the one used in La Rocca et al. [27].
As in the original work, the dataset is used to train a NN , which takes as input the joint configuration and the velocity direction, and outputs the maximum velocity norm. is used to express the approximated safe set , exploiting it in an MPC formulation to achieve safety. We have noticed that the NN tends to exhibit larger prediction errors for values of close to zero. This behavior is attributed to the non-uniform distribution of in the dataset , which is heavily biased toward higher velocity values. To address this issue, we rebalance the dataset (Line 13 in Alg. 5) by copying data points so that the data frequency is approximately uniform across the spectrum of .
Robot dynamics
To integrate the dynamics of robotic manipulators using CasADi symbols, we have used the Adam library, which implements a set of algorithms for calculating rigid-body dynamics for robotic systems starting from Universal Robotic Description Format (URDF) files. In our implementation, the joint accelerations are considered as control input , thus the system dynamics is linear (i.e., it is a double integrator). Then, we have used the Recursive Newton-Euler Algorithm Featherstone [17] to compute the joint torques and impose their bounds as nonlinear constraints in (5d):
where is the mass matrix, contains the Coriolis and centrifugal effects, is the gravitational term, while are the joint torque bounds. This is an important extension with respect to Lunardi et al. [30]; Fontanari et al. [18], where only a simple dynamics (triple pendulum system) with hand-written equations of motion has been considered.
Collision modeling
Unlike in Lunardi et al. [30]; Fontanari et al. [18], where only joint constraints were considered, this work accounts for Cartesian obstacles and self-collision avoidance of the robotic arm. We modeled the robot links and the obstacles inside the environment using simple geometric shapes: spheres, capsules, and planes. This allows us to check collisions with a low computational effort, while ensuring the differentiability of the collision constraints, which is required by the MPC solver. Actually, distance between capsules is not differentiable everywhere because its computation contains up to 6 evaluations of the min and max functions. However, Acados Verscheuren et al. (2019) can deal with this kind of functions that are differentiable almost everywhere.
Mismatch between MPC state and NN input
Set membership to is verified with the following constraint Lunardi et al. [30]; Fontanari et al. [18]:
where is a NN computing an upper bound on the joint velocity norm as described in § 6, and is a safety margin that we introduced to satisfy Assumption 1. We have used L4CASADI Salzmann et al. [41] for integrating the PyTorch Paszke et al. [36] neural model inside ACADOS.
The VBOC algorithm La Rocca et al. [27], used for the computation of the safe set, suffers from the well-known curse of dimensionality. Indeed, the complexity of the algorithm scales exponentially with the dimension of the state, which limits the practical applicability of VBOC for high-dimensional systems such as full robotic manipulators, whose state dimension is typically around 12.
Given as the number of joints of the manipulator, a possible solution to mitigate this issue is to derive the safe set considering only a subset of joints, with . The remaining joints are considered static in the configuration and neglected by the VBOC algorithm. Still, the space occupied by all the links of the robot in the environment, even the fixed ones, is taken into account. Then, assuming without loss of generality that the considered joints are the first ones, we can check set membership to with the following constraints:
This is an additional contribution with respect to Lunardi et al. [30]; Fontanari et al. [18], that allows us to validate our approach on a full 6-DOF robot manipulator (details are provided in § 9).
Real-Time Iteration scheme
MPC requires to solve optimization problems inside fast control loops, which is hard for high-dimensional systems. Indeed, the converging solution of OCP (5) cannot be found online at each sampling time. Our MPC implementation relies on the Real-Time Iteration (RTI) scheme Diehl et al. [15], which boils down to performing a single iteration of Newton’s method, without line search. Therefore, after each solver iteration we need to check if constraints are satisfied. In particular, also the torque limits can be violated, that in our case is expressed as a nonlinear constraint. For this reason, for every problem, we perform a check on the state bounds for all the running nodes. Also, we verify the satisfaction of the obstacle avoidance constraints. Then, the MPC step is accepted if the previous conditions are satisfied, otherwise the previous solution is used. In case the joint torques do not satisfy the bounds (22), is saturated and the corresponding acceleration is computed through the forward dynamics, followed by an update of the state. The NN constraint (23) instead is not checked after each OCP solution: empirically we have noticed that demanding the exact satisfaction of (23) results in a lot of unnecessary safe aborts. Notice that the feasibility checks and fallback mechanisms described above are applied only to the three proposed methods, namely HTC-MPC, RC-MPC, and PC-MPC.
Safe-Abort Computation
The safe abort strategy described in § 5 requires the solution of OCP (19). The best scenario for RC-MPC and PC-MPC is that the safe abort OCP can be solved in one time step. For this reason, we have applied the RTI scheme also for this case, thus . Indeed, we noticed through simulations that solving OCP (19) using the RTI scheme leads to the same number of safely aborted tasks as using Sequential Quadratic Programming until convergence, while having a computation time lower than one time step. This is a contribution with respect to Lunardi et al. [30]; Fontanari et al. [18], where no solutions were explored to solve the OCP (19) in one time step.
Simulation Results
Simulation Setup
To evaluate the effectiveness of the analyzed safe MPC formulations, we simulated them together with three classic MPC formulations of increasing complexity:
Naive: it has no terminal constraint, i.e., problem (5) with . This is the baseline for all the experiments.
Zerovel: it enforces an equilibrium terminal state (), which is the most conservative approach to get RF.
STC-MPC: it introduces a soft terminal constraint set with a penalty weight of , as an attempt towards RF (§ 3.3).
For the simulations, we have considered the Z1 joint manipulator from Unitree®. The horizon for VBOC (20) and safe abort (19) has been fixed to . Note that the baseline methods do not incorporate the safe-abort mechanism: Naive and Zerovel do not have access to the safe set, and although STC-MPC enforces feasibility of the solution, it does not provide any safety guarantees.
Setpoint regulation
The first task is a setpoint regulation problem (depicted in Figure 3), where the End-Effector (EE) of the robot has to reach a target position (in green) close to a ball (cyan) and ground (blue) obstacles, avoiding collisions. The EE bounding volume is defined by a sphere obstacle (faded yellow). Given as the current and reference EE positions, we have used a least-squares function as running cost, which penalizes deviations from and control efforts:

Figure 3. Simulation setup for the setpoint regulation task. The robot starts from a random initial configuration (left) and must reach a target with the EE (right), avoiding collisions with the ball and the ground.
Notice that we deliberately refrain from constraining the end-effector velocity, because the goal of this paper is to assess whether our proposed MPC schemes can enforce safety on a given controller, even when that controller alone might result in unsafe behavior. We have run 400 simulations for each MPC formulation, starting from the same 400 random joint positions with . First, we check the configuration to being not in collision with obstacles; then, we try to compute an OCP solution to warm-start the subsequent MPC controller. Since at beginning the optimal safety condition for each safe-set based controller is the same, i.e. satisfaction of the safe set constraint at the end of the control horizon, the only controllers that we used to generate the initial conditions and the initial warm-start trajectories are Naive, Zerovel, and HTC. The MPC time step is ms, while the total simulation lasts time steps. The robot is commanded on the first 4 joints, while the remaining two are fixed, thus , . The NN has 2 hidden layers with 256 neurons and GELU activation functions to guarantee differentiability of (23). The training was performed on a dataset of 600k points, generated using the VBOC algorithm on a 32-cores machine, requiring approximately 10 hours.
We know from Rawlings et al. [37] that the horizon length has a strong influence on RF and performance of MPC, thus we decided to analyze how the results change with different values of the horizon. Figure 4a shows how the number of failed tasks changes with respect to the horizon: a task is considered as failed if the robot hits the joint bounds or the EE collides with any obstacle. Naive is the least safe method, with almost 70% of the tests failed with . Indeed, we know that RF is practically impossible for short horizons without using an appropriate terminal constraint. Excluding Zerovel, which never fails, the other formulations are less sensitive to the horizon. In addition, for short horizons (), HTC-MPC, RC-MPC, and PC-MPC fail less than the STC-MPC approach.
Figure 4. Simulation results of the different MPC controllers for the setpoint regulation task, considering different values of horizons and NN safety margins.
To evaluate the performance of the MPC formulations, we focus our attention on the cost function (5a): the lower the value of , the better the tracking performances. Given as the total cost of a simulation, thus:
we define the cost-surplus as:
where the subscript mpc indicates the formulation used, and is the number of tasks in which no controller failed (if any controller fails a task, that task is excluded from the computation of the cost-surplus). The optimal cost is computed by solving an OCP on the full horizon, thus . This OCP results in a high number of optimization variables, thus a good warm-start and a robust solver are necessary: for this purpose, we have used the trajectories obtained during the MPC simulations, taking the ones performing the best in terms of cost, among the different formulations for each initial condition; for the solver, we chose IPOPT Wächter and Biegler (2006). The higher , the worse the tracking performance.
Figure 4b compares the cost-surplus, among the tests, of each formulation against the horizon. For Zerovel, the cost-surplus rapidly increases for smaller horizons, since the predicted velocity must reach zero in a smaller time. Instead, for the other formulations the cost-surplus increases slowly as expected, due to less conservative terminal/receding constraints. Surprisingly, the cost-surplus of the novel formulations are slightly lower than Naive: empirically, we noticed that the EE with the Naive controller often overshoots the target, while the other formulations do so less often since they include a safety constraint on the terminal and/or running node.
The choice of the safety margin is another critical parameter for Alg. 4 and Alg. 2, Alg. 3 with safe task abortion. Indeed, Assumption 1 may be not valid if is too small. The larger the safety margin, the less the false positives of the NN. We evaluated the variations on the number of failed tasks and the cost-surplus by choosing different values of . The MPC horizon has been fixed to . Notice that Naive is excluded from this analysis because it is independent of , while Zerovel is maintained for comparison. In Figure 4c, the number of failed tasks decreases with higher values of for the methods with safe abort procedure. In particular, RC-MPC and PC-MPC are the safest approaches, with RC-MPC failing less than 2% of the times for .
Larger implies a smaller safe set, thus the cost-surplus increases as a drawback. This behavior can be noticed in Figure 4d: for the constraint formulations, the cost-surplus inflates only by a small quantity, from to . These values are much lower with respect to Zerovel, that has a cost-surplus always larger than 30%. The reason for this increase is that with higher the safe MPC approaches activate more frequently the safe abort procedure, thus the system avoids failures but the robot stops typically at a large distance from the target before restarting the task, resulting in larger tracking cost.
The variable in RC-MPC plays a crucial role in guaranteeing RF under the assumption that the set is up-to--step control invariant. Figure 5 illustrates the output of the NN, defined by (23), evaluated at the predicted states and . Throughout the simulation, remains nearly constant at 44 (that is ) except for a brief drop at the beginning of the test. A closer inspection at the interval between and reveals a reduction in when the NN output at becomes negative. As outlined in Algorithm 4, this triggers the receding mechanism, where decreased until a state is found (Lines 9-11). At , the NN output for the terminal state becomes positive again, and returns to 44. Theoretically, the NN output evaluated at the receding state should remain strictly positive. However, a careful examination of the middle plot shows that it becomes slightly negative at certain points. As discussed in § 7.4, this occurs because the RTI optimization scheme does not guarantee constraint satisfaction and we decided not to check the satisfaction of constraint (23) at every MPC iteration because this would lead to premature safe aborts.
Figure 5. Values of and NN outputs (left-hand side of 24) evaluated at and , in one particular simulation, with horizon and safety margin .
| NAIVE | ZEROVEL | STC | HTC | RC | |
| RTI [ms] | 4.68 | 4.18 | 7.76 | 4.15 | 4.78 |
Table 1 (PDF p. 13). 99-percentile computation times for the MPC using the RTI scheme.
| Mean [ms] | 1.71 |
| STD [ms] | 0.48 |
| 99-quantile [ms] | 2.59 |
| Times < 5 ms [%] | 99.76 |
Table 2 (PDF p. 13). RTI safe abort time statistics, for simulation with horizon , on 423 samples.
Computation Time
Table Table 1 reports the 99th percentile computation times of the MPC iterations for each method. These results refer to the 400 simulations of the setpoint regulation task presented in § 8.2. This metric provides a conservative estimate that includes many of the worst-case (i.e., slowest) iterations. For all the formulations except STC-MPC, the 99th percentile remains below the duration of the time step, allowing the MPC to be recomputed at each time step in real-time. In the current implementation, PC-MPC solves subproblems (11) sequentially rather than in parallel. Thus, the computation times are not reported, but we expect them to be comparable to those of HTC-MPC, since each PC-MPC subproblem involves only a single constraint (23) on a single node.
For what concerns the time required for the safe abort control computation, the times are almost always under the time step duration (5 ms) of the MPC in the simulation campaign of § 8.2, as can be seen in Table 2, allowing an efficient computation of the controls to drive the system to a safe configuration.
Robustness evaluation
Using the same setpoint regulation task described in § 8.2, we evaluate the robustness of the safe MPC methods to modeling errors. The setup for these simulations is shown in Figure 6. In this scenario, the EE must reach the green target avoiding collisions with the plane (blue) and a window-shaped obstacle (cyan), modeled by three capsules. Unlike the previous setup, collisions are checked over the full

Figure 6. Simulation setup to evaluate disturbance rejection capability in the setpoint task. The robot starts from a fixed initial configuration (left) and must reach the EE’s target (right), avoiding collisions with the ground and the window-shaped obstacle. The bounding volume of the robot, represented using capsules (faded yellow). Here, five joints of the robot are controlled (, ), while the wrist joint is fixed. The MPC time step is ms, while the total simulation lasts time steps as before. A new NN, using the same architecture as before, is trained on a dataset of 1.2 million points, generated in approximately 72 hours.
To simulate modeling errors, we randomly perturb the inertial parameters of the robot links by a maximum factor of , using uniform sampling. Since the control methods discussed in this paper do not directly account for modeling errors, we need to introduce some hand-tuned safety margins in the state constraints (i.e., joint limits and obstacle dimensions). Typically, such margins should be introduced in the controller constraints. However, this would imply re-training the NN used in the constraint (23) for every margin.
To avoid this computational overhead, we decided to keep the controller constraints unaltered, while changing instead the joint limits and obstacle dimensions in the simulation environment. Given a user-defined margin and , the enlarged bounds are:
Similarly, we shrink the obstacles in the simulator by adding a positive margin to the distance computation.
Figure 7a reports the number of failed tasks as a function of the noise level , using joint and obstacle margins of and m. For each value of , we perform 100 simulations, sampling each time different inertial parameters, and using the same initial robot configuration shown in Figure 6. As expected, the number of failures increases with higher modeling errors. Naive is the worst controller, while the safe MPC formulations, particularly RC-MPC and PC-MPC, demonstrate significantly higher robustness, excluding the over-conservative Zerovel controller. Figure 7b shows the cost-surplus across different values of modeling errors. Here, from equation (27) is computed as the minimum cost among all simulations and controllers for each value of . The high safety of Zerovel comes at a price of a high cost-surplus, reflecting poor tracking performance. In contrast, the safe controllers maintain values of comparable to Naive.
Figure 7. Simulation results with modeling errors, considering different values of parameter noise and joint/collision margins. The horizon and NN safety margin for these tests are respectively and .
A similar analysis is presented in Figure 7c and 7d, where we evaluate the effect of different joint and obstacle margins and , while keeping the modeling errors fixed at . The larger the margins, the lower the number of failed tasks. With the exception of Zerovel, the safe MPC formulations consistently result in fewer failures compared to Naive. In particular, for small safety margins, RC-MPC and PC-MPC exhibit fewer failures than Naive and the terminal constraint controllers. The cost-surplus in Figure 7d follows the same trend as in Figure 7b, highlighting the trade-off between safety and performance. Note that the modeling error margins used in this experiment are not meant to provide an exhaustive characterization of the uncertainty of the model. Rather, they were manually selected as representative mismatch levels to illustrate the qualitative evolution of failures and cost surplus for the considered MPC schemes, while keeping the figure readable.
Set statistics
To empirically support our hypothesis that it is easier to find sets with weaker membership conditions compared to CIS, and that the use of an approximate CIS is in practice more effective with the novel controllers not requiring CISs, we performed a statistical analysis on the sets found for the two setups of § 8.2, 8.4 and 9.
We have tested different invariance conditions (listed in Figure 1) on 100k states, sampled close to the set boundaries, as they are more likely to violate them. These tests boil down to solving one or multiple OCPs considering the conditions summarized in § 4.4. The percentage of states for which the solver fails to find a solution for a given condition is a measure of how far the set is to belong to the associated set class.
The results of this analysis are reported in Table 3 and Table 4. The horizon used to test classic Control Invariance was 45 timesteps, for Up-to-step Control Invariance (14) we tested all the horizons in and we reported the results for the one () that caused fewer solver failures, for Within-N-step Control Invariance (15) we reported the total number of state for which none of the horizons in can satisfy the set condition. For back-and-forth-N-step Control Invariance (16) we always used as forward horizon , reporting the results for the value of in satisfying the set condition for more states. Finally, for back-and-forth-within-N-step Control Invariance (17), we reported the number of states for which a combination and that satisfy the set condition does not exist.
| Set | Failures (%) |
| CIS | 0.91 |
| Up-to-N-step CIS, [] | 0.69 |
| Within-N-step CIS | 0.62 |
| Back-and-forth-N-step CIS, [] | 0.031 |
| Back-and-forth-within-N-step CIS | 0.02 |
Table 3. Statistical test results for the 4-DOF set used in Section 8.2.
| Set | Failures (%) |
| CIS | 0.72 |
| Up-to-N-step CIS, [] | 0.56 |
| Within-N-step CIS | 0.52 |
| Back-and-forth-N-step CIS, [] | 0.079 |
| Back-and-forth-within-N-step CIS | 0.071 |
Table 4. Statistical test results for the 5-DOF set used in Section 8.4.
Unsurprisingly, the two sets we tested are not exactly control invariant, and the invariance condition is violated in almost 1% of the sampled states. However, as we move towards less strict multi-step invariance definitions, the number of sampled states violating the invariance condition decreases by a factor of for the first set and for the second one. This explains why the newly presented controllers perform better in practice, even though the used safe set is computed as the approximation of a classic CIS. Moreover, these results also support the claim that numerical procedures are more likely to produce a set that belongs to one of the presented classes.
Experiments
We validate one of the analyzed MPC approaches, namely the RC-MPC, on real hardware experiments, which are shown in the companion video. We perform the MPC on the full 6-DOF Z1 manipulator. We were able to use a time step ms consistent with the simulations of § 8.4, despite the increased state dimension. The setup and some frames of robot motion are reported in Figure 8. The robot’s EE must track the black box in the hand of the human, keeping a safety distance cm along the -axis. The task has been defined with the following cost function:

Figure 8. Setup and example of robot motion with the RC-MPC controller (videos can be found in the supplementary material). The manipulator tracks the moving target (the cube with markers in the human’s hand) without colliding with the environment and without violating the joint limits. The frames, ordered in time from left to right, have been taken from a single experiment.
with:
The position of the black box is provided by a motion capture system and it is updated with a frequency of 10 Hz. While tracking the target, the robot’s EE must also maintain a reference orientation , which is aligned with the global reference frame on the robot’s base. The environment is the same as that shown in Figure 6; therefore, we reused the NN trained on the 5-DOF system, adapting the safety constraint to the full DOF manipulator using (24) with . Due to the high joint friction of the Z1, we conducted the experiments using an open-loop MPC approach. In this setup, the MPC re-computes the state and control trajectories at each time step based solely on its previous prediction, rather than incorporating real-time state feedback, following an approach similar to that proposed in Neunert et al. [34]. Specifically, we avoided closed-loop MPC execution, as the significant friction would interfere with accurate feedback control. Instead, at each time step, we sent the MPC state to the hardware using the Unitree® Z1 Python API. The robot then tracked this reference using its internal Proportional Derivative controller.
Computation time
Figure 9 shows the computation time recorded during a single RC-MPC experiment. The blue curve refers to the time required by ACADOS for solving a single MPC iteration using the RTI scheme. The green curve represents the total time taken by the entire Python cycle, which includes the MPC computation, the reference target position update, and the verification checks on the next predicted states,
Figure 9. Cumulative percentage of the computation times of the RC-MPC during a real experiment. These plots show the likelihood of the computation time being less than a certain threshold.
such as bounds satisfaction and obstacle avoidance. A horizontal dashed line at the 0.99 cumulative probability level marks the 99th percentile. The intersections of this line with the two curves define the upper bounds of the time intervals within which 99% of the solver and total cycle durations fall, respectively of 8.29 ms and 11.2 ms. The 99th percentile of the solver time is lower than the time step , thus the MPC can be run at 100 Hz. Unfortunately, the requirement is not satisfied if we consider the total time required by the Python cycle, with 92% of the iterations having a computation time below 10 ms. Still, we believe that an optimized implementation (e.g., using compiled programming languages such as C++) will largely decrease the gap between solver and total cycle durations.
Constraint satisfaction
As discussed in § 7.4, constraint satisfaction must be verified after each MPC iteration. Figure 10 illustrates the evolution of constraint violations related to state bounds, obstacle avoidance, and safe set membership according to the NN output. While the first two quantities depend on the current state at each sampling instant, the NN output is evaluated at the receding node . Given the lower and upper state bounds and , the bounds violation is defined as:
Figure 10. Constraint violations during the real experiment with RC-MPC.
Obstacle violation is quantified by computing the minimum distance between the robot and surrounding obstacles at each instant.
During the experiments, the robot manipulator is intentionally guided towards the obstacles through a controlled movement of the black box target, as shown in Figure 8, to evaluate the robustness of the RC-MPC. All violation measures remain positive, indicating that the constraints are always respected.
Conclusions
In this paper, we tackled the challenge of ensuring safety in control strategies for robotics systems. We extended classical theory of control invariance by introducing novel classes of safe sets that generalize the traditional notion of CIS. These definitions allow for the design of more flexible MPC schemes, especially when dealing with approximate CIS in nonlinear systems.
Using some classes of safe sets, we have shown that some recently developed MPC schemes can guarantee RF. By moving beyond the classical terminal constraint paradigm and moving the safe set constraint across the prediction horizon, our schemes offer an adaptable framework for incorporating safety without highly diminishing the controller’s performance. To handle the inherent approximation of the safe sets, we introduced a safe abort procedure that is triggered when a potential violation of constraints is recognized.
We validated the effectiveness of our methods through extensive simulations on robotic manipulators and demonstrated real-world applicability with hardware experiments on a 6-DOF robot arm. The results show that our approach reduces constraint violations compared to state-of-the-art methods, while maintaining acceptable tracking performance and computational efficiency.
The assumptions on the safe set belonging to one of the multi-step classes are still hard to verify in practice, as shown by the constraint violations in our tests. Inspired by recent work [38, 1], we want to investigate the use of robust optimization or probabilistic techniques to certify those sets. Another promising future work is the integration of our safety framework as a filter for data-driven policies, such as those trained with Reinforcement Learning. Additionally, applying our safe MPC schemes to higher-dimensional and underactuated systems, such as legged robots, poses a relevant challenge. Using hardware accelerators for the computation of safe sets could help scaling to higher dimensionality. However, the challenges introduced by the non-smooth contact dynamics seem harder to overcome. Investigating the integration of our novel idea of multi-step safe sets with the framework of Model Predictive Shielding could be a possible solution, as it does not require differentiable dynamics. Future work will investigate more flexible multi-step CIS representations, either by augmenting the system state with obstacle states, despite the resulting increase in computational cost, or by leveraging learning-based HJR methods that incorporate lidar measurements directly into the safe-set approximation Lin et al. [29].
Acknowledgements
Funded by the European Union. Views and opinions expressed are however those of the author(s) only and do not necessarily reflect those of the European Union or the European Health and Digital Executive Agency (HADEA). Neither the European Union nor HADEA can be held responsible for them. EU - HE Inverse - Grant Agreement 101136067.
Relationship between Multi-Step Control Invariance and Backward Reachability
We clarify here the relation between the multi-step control invariant sets introduced in this paper and classical backward reachable sets/tubes. Although these notions are related, they are not equivalent.
Given a target set , an -step backward reachable tube (BRT) characterizes the set of states from which the system can be driven to in at most steps. Similarly, an -step backward reachable set (BRS) characterizes the set of states from which the system can be driven to in exactly steps. These constructions therefore require the prior specification of an external target set .
By contrast, the multi-step control invariant sets considered in this paper are self-referential. In particular, a set is within--step control invariant if, from every state in , there exists an admissible control sequence that drives the system back to within at most steps. Thus, the set to be reached is the same set whose invariance property is being assessed. Equivalently, using the notation of this paper, is within--step control invariant if
where denotes the -step backward reachable set of .
This expression highlights the connection with reachability: backward reachable sets can be used as a tool to verify whether a given set satisfies the within--step control invariance property. However, the property itself is not simply an -step BRT or BRS of a prescribed target set. In a standard BRT/BRS computation, the target set is fixed a priori and the reachable set is computed relative to that target. In the present setting, the candidate set must instead be contained in its own finite-step backward reachable tube. This self-consistency requirement is what defines the multi-step control invariance property.
Therefore, while reachability operators naturally appear in the characterization of the proposed set classes, the proposed multi-step control invariant sets are distinct from classical backward reachable sets and tubes. Classical BRT/BRS constructions may support the verification of the proposed properties for a given set, but they do not by themselves define or compute the self-referential multi-step control invariant sets introduced in this work.
References
- [1]Akella P, Ubellacker W and Ames AD (2023) Probabilistic guarantees for nonlinear safety-critical optimal control. In: IEEE/RSJ International Conference on Intelligent Robots and Systems. pp. 8120–8126.arxiv.org/abs/2303.06258
- [2]Ames AD, Grizzle JW and Tabuada P (2014) Control barrier function based quadratic programs with application to adaptive cruise control. In: IEEE Conference on Decision and Control. pp. 6271–6278.DOI
- [3]Ames AD, Xu X, Grizzle JW and Tabuada P (2017) Control barrier function based quadratic programs for safety critical systems. IEEE Transactions on Automatic Control 62(8): 3861–3876.DOI
- [4]Andersson JAE, Gillis J, Horn G, Rawlings JB and Diehl M (2019) CasADi – A software framework for nonlinear optimization and optimal control. Mathematical Programming Computation 11(1): 1–36.DOI
- [5]Bansal S, Chen M, Herbert S and Tomlin CJ (2017) Hamilton-Jacobi reachability: A brief overview and recent advances. ISBN 9781509028733. DOI:10.1109/CDC.2017.8263977.DOI
- [6]Bansal S and Tomlin CJ (2021) Deepreach: A deep learning approach to high-dimensional reachability. In: IEEE International Conference on Robotics and Automation. pp. 1817–1824.arxiv.org/abs/2011.02082
- [7]Bastani O (2021) Safe reinforcement learning with nonlinear dynamics via model predictive shielding. In: American Control Conference. pp. 3488–3494.arxiv.org/abs/1905.10691
- [8]Bastani O, Li S and Xu A (2021) Safe Reinforcement Learning via Statistical Model Predictive Shielding. In: Robotics Science and Systems.DOI
- [9]Blanchini F (1999) Set invariance in control. Automatica 35: 1747–1767.DOI
- [11]Coquelin PA, Martin S and Munos R (2007) A dynamic programming approach to viability problems. In: IEEE International Symposium on Approximate Dynamic Programming and Reinforcement Learning. pp. 178–184.DOI
- [12]Dai H, Landry B, Pavone M and Tedrake R (2020) Counter-example guided synthesis of neural network lyapunov functions for piecewise linear systems. In: IEEE Conference on Decision and Control. pp. 1274–1281.DOI
- [13]Dawson C, Gao S and Fan C (2023) Safe Control With Learned Certificates : A Survey of Neural Lyapunov , Barrier , and Contraction Methods for Robotics and Control. IEEE Transactions on Robotics 39(3): 1749–1767.DOI
- [14]Dawson C, Qin Z, Gao S and Fan C (2022) Safe nonlinear control using robust neural lyapunov-barrier functions. In: Conference on Robot Learning, volume 164. pp. 1724–1735.arxiv.org/abs/2109.06697
- [15]Diehl M, Findeisen R, Allgöwer F, Bock HG and Schlöder JP (2005) Nominal stability of real-time iteration scheme for nonlinear model predictive control. IEE Proceedings - Control Theory and Applications 152(3): 296–308.DOI
- [16]Djeridane B and Lygeros J (2006) Neural approximation of pde solutions: An application to reachability computations. In: IEEE Conference on Decision and Control. pp. 3034–3039.DOI
- [17]Featherstone R (2014) Rigid body dynamics algorithms. Springer.
- [18]Fontanari E, Lunardi G, Saveriano M and Del Prete A (2025) Parallel-constrained model predictive control: Exploiting parallel computation for improving safety. In: IEEE International Conference on Robotics and Automation. pp. 14881–14887.arxiv.org/abs/2509.03261
- [19]Grandia R, Taylor AJ, Ames AD and Hutter M (2021) Multi-layered safety for legged robots via control barrier functions and model predictive control. In: IEEE International Conference on Robotics and Automation. pp. 8352–8358.arxiv.org/abs/2011.00032
- [20]Grüne L and Pannek J (2017) Nonlinear model predictive control. Springer.
- [21]Holmes P, Kousik S, Zhang B, Raz D, Barbalata C, Johnson-Roberson M and Vasudevan R (2020) Reachable sets for safe, real-time manipulator trajectory design. arXiv preprint arXiv:2002.01591.arxiv.org/abs/2002.01591
- [22]Hsu KC, Rubies-Royo V, Tomlin CJ and Fisac JF (2021) Safety and Liveness Guarantees through Reach-Avoid Reinforcement Learning. Robotics: Science and Systems.DOI
- [23]Hwangbo J, Lee J, Dosovitskiy A, Bellicoso D, Tsounis V, Koltun V and Hutter M (2019) Learning agile and dynamic motor skills for legged robots. Science Robotics 4(26): eaau5872.
- [24]Jian Z, Yan Z, Lei X, Lu Z, Lan B, Wang X and Liang B (2023) Dynamic control barrier function-based model predictive control to safety-critical obstacle-avoidance of mobile robot. In: IEEE International Conference on Robotics and Automation. pp. 3679–3685.arxiv.org/abs/2209.08539
- [25]Jiang F, Chou G, Chen M and Tomlin CJ (2017) Using neural networks to compute approximate and guaranteed feasible hamilton-jacobi-bellman pde solutions. ArXiv:1611.03158.arxiv.org/abs/1611.03158
- [26]Kerrigan EC and Maciejowski JM (2000) Soft constraints and exact penalty functions in model predictive control. In: UKACC International Conference on Control.
- [27]La Rocca A, Saveriano M and Del Prete A (2023) Vboc: Learning the viability boundary of a robot manipulator using optimal control. IEEE Robotics and Automation Letters 8(11): 6971–6978.arxiv.org/abs/2305.07535
- [28]Leung K, Schmerling E, Zhang M, Chen M, Talbot J, Gerdes JC and Pavone M (2020) On infusing reachability-based safety assurance within planning frameworks for human–robot vehicle interactions. The International Journal of Robotics Research 39(10-11): 1326–1345. DOI:10.1177/0278364920950795. URL https://doi.org/10.1177/0278364920950795.
- [29]Lin A, Peng S and Bansal S (2026) One Filter to Deploy Them All: Robust Safety for Quadrupedal Navigation in Unknown Environments. IEEE Transactions on Robotics 42: 545–560. DOI:10.1109/TRO.2025.3644957. URL https://ieeexplore.ieee.org/document/11301616/.
- [30]Lunardi G, La Rocca A, Saveriano M and Del Prete A (2024) Receding-constraint model predictive control using a learned approximate control-invariant set. In: IEEE International Conference on Robotics and Automation. pp. 11626–11632.DOI
- [31]Mayne DQ, Rawlings JB, Rao CV and Scokaert PO (2000) Constrained model predictive control: Stability and optimality. Automatica 36(6): 789–814.DOI
- [32]Michaux J, Holmes P, Zhang B, Chen C, Wang B, Sahgal S, Zhang T, Dey S, Kousik S and Vasudevan R (2025) Can not touch this: Real-time, safe motion planning and control for manipulators under uncertainty. IEEE Transactions on Robotics 41: 4719–4740. DOI:10.1109/TRO.2025.3584557.DOI
- [33]Nagumo M (1942) Über die lage der integralkurven gewöhnlicher differentialgleichungen. Proceedings of the Physico-Mathematical Society of Japan. 3rd Series 24: 551–559.DOI
- [34]Neunert M, Farshidian F, Winkler AW and Buchli J (2017) Trajectory optimization through contacts and automatic gait discovery for quadrupeds. IEEE Robotics and Automation Letters 2(3): 1502–1509.arxiv.org/abs/1607.04537
- [35]Nocedal J and Wright SJ (2006) Numerical Optimization. 2e edition. Springer.
- [36]Paszke A, Gross S, Massa F, Lerer A, Bradbury J, Chanan G, Killeen T, Lin Z, Gimelshein N, Antiga L, Desmaison A, Kopf A, Yang E, DeVito Z, Raison M, Tejani A, Chilamkurthy S, Steiner B, Fang L, Bai J and Chintala S (2019) Pytorch: An imperative style, high-performance deep learning library. In: Advances in Neural Information Processing Systems, volume 32.arxiv.org/abs/1912.01703
- [37]Rawlings JB, Mayne DQ, Diehl M et al. (2017) Model predictive control: theory, computation, and design, volume 2. Nob Hill Publishing Madison, WI.
- [38]Robey A, Hu H, Lindemann L, Zhang H, Dimarogonas DV, Tu S and Matni N (2020) Learning control barrier functions from expert demonstrations. In: IEEE Conference on Decision and Control. pp. 3717–3724.arxiv.org/abs/2004.03315
- [39]Rubies-Royo V and Tomlin C (2017) Recursive Regression with Neural Networks: Approximating the HJI PDE Solution. In: International Conference on Learning Representations.
- [40]Rudin N, Hoeller D, Reist P and Hutter M (2022) Learning to walk in minutes using massively parallel deep reinforcement learning. In: Conference on Robot Learning, volume 164. pp. 91–100.arxiv.org/abs/2109.11978
- [41]Salzmann T, Arrizabalaga J, Andersson J, Pavone M and Ryll M (2024) Learning for casadi: Data-driven models in numerical optimization. In: Annual Learning for Dynamics & Control Conference. pp. 541–553.arxiv.org/abs/2312.05873






