Safe Policy Optimization via Control Barrier Function-based Safety Filters

This paper proposes a policy optimization framework that jointly tunes nominal controllers and Control Barrier Function-based safety filters using trajectory-based objectives and Lyapunov-constrained gradient flow to enhance stability and eliminate undesirable behaviors while preserving safety guarantees.

The Gist

Imagine you are teaching a robot to drive a car. You have two main goals:

1. Get to the destination safely (don't hit walls or other cars).
2. Get there efficiently and smoothly (don't drive in circles or get stuck).

In the world of robotics, engineers use two different tools to handle these goals. This paper is about making those two tools work together without fighting each other.

The Two Tools: The "Navigator" and the "Guardian"

1. The Navigator (Nominal Controller): This is the robot's brain. It knows the destination and says, "Drive straight there!" It's great at getting you to the goal, but it doesn't know about obstacles. If you just let the Navigator drive, the robot might crash into a wall.
2. The Guardian (Safety Filter / CBF): This is the robot's reflex. It watches the Navigator's commands and says, "Wait! If you turn left, you'll hit that wall. Turn right instead!" It modifies the Navigator's commands just enough to keep the robot safe.

The Problem: When the Guardian Gets Too Bossy

The paper points out a funny but dangerous problem. Sometimes, the Guardian is too good at its job.

Imagine the Navigator wants to drive straight to the goal. The Guardian sees a wall and says, "No, turn right!" The Navigator tries to correct, but the Guardian says, "No, that's too close, turn left!"

• The Result: The robot gets stuck in a loop, driving in circles (a "limit cycle") or getting stuck in a corner where it thinks it's safe but can't move forward (a "deadlock").
• The Analogy: It's like a parent (the Guardian) who is so protective of a child (the robot) that they won't let the child take any step without holding their hand. Eventually, the child stops walking entirely because the parent is constantly correcting their every move. The robot is safe, but it's not moving toward the goal.

The Solution: Training the Team to Dance Together

The authors of this paper realized that you can't just pick a random Navigator and a random Guardian and hope they work well together. You have to train them as a team.

They developed a new method called Safe Policy Optimization. Here is how it works, step-by-step:

1. The "Simulator" Training

Instead of letting the robot crash in the real world, they run thousands of simulations. They let the robot try to drive from many different starting points to the goal.

• They measure how well the robot did: Did it get stuck? Did it take too long? Did it hit a wall?
• This creates a "score" for how good the current team is.

2. The "Safety Net" (The Hard Part)

Here is the tricky part. When you are training a robot, you usually try things that might fail to see if they work better. But you can't let the robot become unsafe while you are training it. If the robot learns a new trick that makes it crash, you can't use that.

The authors created a mathematical "Safety Net" (using something called Robust Safe Gradient Flow).

• The Analogy: Imagine you are teaching a gymnast new flips. You have a safety net underneath them. If they try a move and start to fall, the net catches them before they hit the ground.
• In the computer, this "net" ensures that at every single step of the training, the robot's "Navigator" remains stable. Even if the training is messy, the robot never enters a state where it becomes unstable or dangerous.

3. The Optimization

The computer tweaks the Navigator's brain and the Guardian's rules simultaneously.

• It asks: "If I make the Navigator slightly more aggressive, and the Guardian slightly more lenient, does the robot get to the goal faster without crashing?"
• It keeps making these tiny adjustments, always staying inside the "Safety Net."

What Did They Achieve?

The paper tested this on robots trying to avoid obstacles (like circles and boxes).

• Before Training: The robot would often get stuck in a corner or drive in circles because the Guardian was fighting the Navigator too hard.
• After Training: The team learned to cooperate. The Guardian still stops the robot from hitting walls, but it lets the Navigator find a smooth path around them.
  • The "deadlocks" (stuck spots) disappeared.
  • The robot stopped driving in circles.
  • The robot got to the goal much faster and smoother, while still being 100% safe.

The Big Takeaway

This paper gives us a recipe for building robots that are both safe and smart. It solves the problem where safety features accidentally make robots stupid or stuck. By training the "brain" and the "reflexes" together, while keeping a strict safety net on the training process, we can create autonomous systems that are reliable, efficient, and ready for the real world.

Technical Summary

1. Problem Statement

The paper addresses a critical limitation in modern autonomous control: the interaction between nominal stabilizing controllers and Control Barrier Function (CBF)-based safety filters.

• The Context: CBFs are widely used to enforce safety (forward invariance of a safe set) by minimally modifying a nominal controller via a quadratic program (QP).
• The Problem: While CBFs guarantee safety, they can severely degrade the stability properties of the closed-loop system. Even if the nominal controller is globally asymptotically stable, the safety-filtered system may exhibit:
  • Asymptotically stable undesired equilibria (causing the system to get stuck in unsafe or suboptimal states).
  • Limit cycles.
  • Unbounded trajectories.
• The Goal: Develop a systematic framework to optimize the nominal controller and safety filter parameters jointly. The objective is to maximize stability (e.g., eliminate stable undesired equilibria, improve convergence) while strictly maintaining safety guarantees throughout the training process.

2. Methodology

The authors propose a policy optimization framework that jointly parameterizes the nominal controller, the CBF class-$\mathcal{K}_\infty$ function, and the safety filter weighting matrix. The approach focuses on linear systems with linear nominal controllers.

A. Joint Parameterization

The optimization variables $\theta$ include:

1. Nominal Controller Gain ($K$): $u = -Kx$.
2. CBF Class-$\mathcal{K}_\infty$ Function: Parameterized as $\alpha(s) = \alpha s$ (linear scaling).
3. Safety Filter Weighting Matrix ($G$): Parameterized as $G = R^{-1}$ where $R \succ 0$.
4. Lyapunov Matrix ($P$): Implicitly handled via change of variables to ensure stability.

B. Trajectory-Based Objective

Instead of analytical stability proofs, the method uses a trajectory-based cost function computed from closed-loop rollouts:
$$L(\theta) = \mathbb{E}_{x_0} \left[ \phi(x_\theta(T)) + \lambda \int_0^T \psi(x_\theta(t)) dt \right]$$

• Terminal Cost ($\phi$): Penalizes the distance from the origin at time $T$.
• Running Cost ($\psi$): Penalizes deviation from the origin during the trajectory.
• Goal: Minimize $L(\theta)$ to encourage trajectories that converge to the desired equilibrium and avoid getting stuck in undesired states.

C. Enforcing Stability Constraints (The Core Innovation)

A major challenge is ensuring the nominal controller remains stabilizing during optimization. If the controller becomes unstable, the cost function becomes ill-defined or infinite.

1. Lyapunov Conditions: Stability is enforced via the condition $(A-BK)^\top P + P(A-BK) \prec 0$.
2. Linearization: Using the change of variables $Y = KP$ and $Q = P^{-1}$, the bilinear constraint is converted to a Linear Matrix Inequality (LMI): $AQ + QA^\top - BY - Y^\top B^\top \prec 0$.
3. Scalarization: To make the constraints amenable to gradient-based methods, the LMIs are converted into smooth scalar constraints using leading principal minors (via Lemma 1). The condition "Matrix is Positive Definite" becomes "All leading principal minors are positive."
4. Robust Safe Gradient Flow (RSGF): The authors employ RSGF to solve the constrained optimization problem.
   • Key Property: If initialized with a stabilizing controller, RSGF guarantees that all subsequent iterates remain feasible (i.e., the controller remains stabilizing at every step).
   • This prevents the training process from crashing due to instability.

D. Handling Multiple Obstacles

For environments with multiple safety constraints (multiple obstacles), solving a multi-constraint QP at every time step is computationally expensive and makes gradient computation difficult (due to implicit differentiation through the QP solution).

• Solution: The authors use a log-sum-exp relaxation to combine multiple barrier functions $h_j(x)$ into a single smooth barrier function $\tilde{h}_\beta(x)$.
• Benefit: This reduces the safety filter to a single constraint, allowing for a closed-form solution for the control input. This eliminates the need for online QP solving and simplifies gradient computation.

3. Key Contributions

1. Formulation: A trajectory-based optimization problem that jointly tunes the nominal controller and safety filter parameters to shape closed-loop dynamics.
2. Stability Enforcement: A novel method to encode Lyapunov stability conditions as smooth scalar constraints using principal minors, enabling gradient-based optimization without violating stability.
3. Robust Training Algorithm: Implementation of Robust Safe Gradient Flow (RSGF) with rollout-based gradient estimates. Theoretical guarantees (Proposition 1) ensure that if training starts with a stabilizing controller, it remains stabilizing throughout, and the algorithm converges to a KKT point.
4. Computational Efficiency: The use of log-sum-exp relaxation for multi-obstacle scenarios enables efficient gradient computation and avoids the complexity of differentiating through QP solutions.

4. Numerical Results

The framework was tested on planar obstacle avoidance problems (single integrator dynamics):

• Bounded Safe Set (Disk):
  • Initial: The safety filter created two undesired equilibria on the boundary, one of which was asymptotically stable (trapping trajectories).
  • Result: Optimization eliminated the stable undesired equilibrium. All trajectories converged to the origin.
• Single Obstacle:
  • Initial: An asymptotically stable undesired equilibrium existed on the obstacle boundary, causing some trajectories to converge to the obstacle (unsafe behavior relative to the goal).
  • Result: The stable equilibrium was eliminated; the remaining undesired equilibrium became unstable. All trajectories converged to the origin.
• Multiple Obstacles (Complex Geometry):
  • Initial: Three asymptotically stable undesired equilibria were observed near obstacles.
  • Result: Optimization converted all undesired equilibria to unstable states. Trajectories successfully avoided obstacles and converged to the origin.

5. Significance

This paper bridges the gap between safety (CBFs) and performance/stability (Policy Optimization).

• Safety-Critical Learning: It provides a rigorous method to learn high-performance controllers without sacrificing safety guarantees. Unlike standard reinforcement learning which might explore unstable regions, this method guarantees stability at every training step.
• Systematic Design: It moves beyond heuristic tuning of CBF parameters, offering a systematic way to design controllers that are not only safe but also possess desirable dynamical properties (e.g., large regions of attraction, no stable "traps").
• Scalability: The log-sum-exp approach makes the method applicable to complex environments with many constraints, addressing a major bottleneck in CBF-based control.

In summary, the paper presents a robust, mathematically grounded framework for optimizing safety-filtered controllers, ensuring that the resulting systems are both safe and dynamically stable, effectively removing the "undesired equilibria" often introduced by naive safety filtering.