PX4 + ROS 2: CBF Safety Filter

Control Barrier Function (CBF) safety filter demo for UAV obstacle avoidance using PX4 SITL, ROS 2 Humble, and gz-sim Harmonic.

No CBF (Collision) CBF Only (Zigzag) CBF + Sliding (Smooth)
No CBF CBF Only CBF + Sliding
Crashes at obstacle 1 Safe but slow (87s) Safe and fast (64s)

Problem

Autonomous drones often follow a nominal controller (e.g., trajectory tracking), but that controller may violate safety constraints such as minimum distance to obstacles.
The goal is to enforce safety without redesigning the full controller.

System Architecture

Planner / Nominal Controller → CBF Safety Filter (QP)
(ROS2 node) → PX4 SITL / Vehicle
State feedback (pose/velocity) is used to evaluate barrier constraints and solve the QP in real time.

Features

CBF Mathematical Formulation

Single-integrator dynamics (position-controlled UAV):

$$ \dot{x} = u \quad \text{where } x = [p_x, p_y],\; u = [v_x, v_y] $$

Barrier function for a circular obstacle:

$$ h(x) = \|x - x_{\text{obs}}\|^2 - d_{\text{safe}}^2 $$

CBF constraint (forward invariance of safe set):

$$ \dot{h} + \alpha h \ge 0 \qquad\Longleftrightarrow\qquad \nabla h(x)^\top u \ge -\alpha h(x) $$

QP-CBF formulation:

$$ \begin{aligned} \min_{u,\delta}\quad & \|u-u_{\text{nom}}\|^2 + \lambda\delta^2 \\ \text{s.t.}\quad & \nabla h_i(x)^\top u \ge -\alpha h_i(x) - \delta,\;\;\forall i \\ & \|u\| \le v_{\max} \\ & \delta \ge 0 \end{aligned} $$

Where $\delta$ is a relaxation variable to ensure feasibility. The QP solver (OSQP via cvxpy) finds the closest safe velocity to the nominal command.

Results

We evaluate the safety filter by monitoring the barrier function (h(x)) during simulation.

Barrier function evolution under different controllers. Safety is guaranteed when \(h(x) \ge 0\).

The figure above shows the evolution of the barrier function (h(x)) over time for three cases:

The dashed line represents the safety boundary (h(x)=0).
Maintaining (h(x) \ge 0) guarantees forward invariance of the safe set.

These results demonstrate that the safety filter successfully enforces safety constraints while minimally modifying the nominal control input.

Comparison of trajectory for different scenario.

Comparison Summary

Metric CBF OFF CBF Only CBF + Sliding
Min barrier h(x)-0.9750.0360.491
Min distance (m)1.5051.8101.932
Safety violatedYESNONO
CollisionYESNONO
Reached targetNO (crash)YESYES
Flight time (s)7.587.464.2

Key observations:

Prerequisites

How to Run

Terminal 1: Agent

./tools/start_agent.sh

Terminal 2: PX4 + gz-sim with obstacles

./tools/spawn_drone.sh

Terminal 3: Controller

source /opt/ros/humble/setup.bash source install/setup.bash

WITH CBF (safe):

ros2 launch cbf_demo_bringup cbf_demo.launch.py use_cbf:=true

WITHOUT CBF (collision):

ros2 launch cbf_demo_bringup cbf_demo.launch.py use_cbf:=false

Terminal 4: RViz2

rviz2 -d $(ros2 pkg prefix cbf_demo_bringup)/share/cbf_demo_bringup/config/cbf_viz.rviz

Tech Stack

References