Forward Invariance in Trajectory Spaces for Safety-critical Control

TL;DR

The paper tackles safety-critical control for robotic systems by addressing the reactive limitations of traditional Control Barrier Functions (CBFs) and the computational challenges of nonlinear MPC (NMPC). It introduces Forward Invariance in Trajectory Spaces (FITS), which lifts receding-horizon planning into a trajectory-space dynamical system where the current planned trajectory $\mathcal{T}_x^I$ evolves under a virtual input $\bm{v}$ that governs the rate of change of the input trajectory $\mathcal{T}_u^I$. A quadratic program (QP) in trajectory space enforces forward invariance of safety and actuation-constraint sets while optionally minimizing a performance objective $J(\bm{s})$, enabling proactive safety with a planning horizon. Experiments on a planar quadrotor geofencing task and navigation in cluttered environments show that FITS strictly enforces safety like CBFs, matches NMPC in performance, and substantially reduces computation time, demonstrating practical applicability for safety-critical robotics.

Abstract

Useful robot control algorithms should not only achieve performance objectives but also adhere to hard safety constraints. Control Barrier Functions (CBFs) have been developed to provably ensure system safety through forward invariance. However, they often unnecessarily sacrifice performance for safety since they are purely reactive. Receding horizon control (RHC), on the other hand, consider planned trajectories to account for the future evolution of a system. This work provides a new perspective on safety-critical control by introducing Forward Invariance in Trajectory Spaces (FITS). We lift the problem of safe RHC into the trajectory space and describe the evolution of planned trajectories as a controlled dynamical system. Safety constraints defined over states can be converted into sets in the trajectory space which we render forward invariant via a CBF framework. We derive an efficient quadratic program (QP) to synthesize trajectories that provably satisfy safety constraints. Our experiments support that FITS improves the adherence to safety specifications without sacrificing performance over alternative CBF and NMPC methods.

Figures (4)

• Figure 1: Illustration of trajectory spaces for a quadrotor where $\mathcal{T}_x$ represents the current planned trajectory in state space, $\mathcal{T}_u$ is the current control input trajectory of length $T$ and $\bm{x}_0(t)$ is the current state of the quadrotor. The white set in trajectory space indicates the set of all safe planned trajectories and $\mathcal{T}_{\textrm{col}}$ is an exemplary unsafe trajectory.
• Figure 2: Notation used for time variables and the trajectory space dynamics for a one-dimensional state space example. The surface color changes along the system time $t$. We denote by $x_{\tau_p}(t_p)$ the state at system time $t_p$ and trajectory time $\tau_p$. The time derivative of the state trajectory at time $t_k$ within the planning interval, i.e., $\dot{\mathcal{T}}_x^I(t_k)$, is shown as a vector field (shaded in red) on the corresponding state trajectory $\mathcal{T}_x^I(t_k)$. The executed trajectory $\{x_0(t) \mid \forall t \geq 0 \}$ is shown in blue.
• Figure 3: A two-dimensional quadrotor tracking task. The dotted black circle depicts the circular trajectory that we want to track while the red rectangle shows the constrained area that we want to remain in.
• Figure 4: A two-dimensional navigation task. Obstacles are shown by red circles while the goal position is indicated by the green star.

Theorems & Definitions (9)

• Definition 1
• Definition 2
• Theorem 1
• Lemma 1
• Remark 1
• Theorem 2
• proof
• Theorem 3
• proof