GAZEBO SIMULATION · SAFE MOTION CONTROL · 2026

ROS 2 NMPC-CBF Controller for Moving-Obstacle Avoidance

基于 CBF-NMPC 的底盘安全运动控制系统

A ROS 2 and Gazebo local-control simulation for a differential-drive robot. The controller jointly optimizes trajectory tracking and softened safety constraints over a receding horizon, then applies the first optimal command online.

Gazebo simulation of a differential-drive robot in an obstacle corridor

GAZEBO 11 / ROS 2 HUMBLE

Independent project: Algorithm design · C++ implementation · Systems integration · Simulation

TECH STACK: ROS 2 Humble · C++ · CasADi · IPOPT · Gazebo 11 · RViz2 · Python

Download clean sourceRepository link pending

Objectives & constraints

Track a reference path in simulated narrow spaces with oncoming and crossing obstacles.
Explicitly respect nonholonomic dynamics and linear/angular velocity limits.
Complete state synchronization, prediction, optimization, and command publication within a 20 Hz loop.
Fail safely by publishing a zero-velocity command whenever the solver does not return a valid solution.

Validation results

These figures are reported in the project presentation. The supplied source does not include reproducible full-system benchmark logs; video and rosbag evidence will be added next.

20 Hz

Configured control-loop rate

< 25 ms

Per-solve figure reported in slides

Test collisions reported in slides

100%

Solve success reported in slides

The controller consumes odometry, a reference path, and obstacle states on a nominal 20 Hz loop. It publishes velocity commands together with the predicted path, CBF values, and obstacle forecasts for RViz2 diagnostics.

Python · 1 Hz

Reference Publisher

The demo uses a straight path; utilities also generate circle and slalom points

Python · 20 Hz

Obstacle Mover

Synchronizes obstacle odometry and broadcasts velocity states

C++ · 20 Hz

NMPC-CBF Controller

Builds the receding-horizon problem and outputs v / ω

Simulation

Gazebo + RViz2 Simulation

Executes the simulation and visualizes predicted trajectories

Actual ROS 2 node and topic graph — ROS 2 node and topic graph captured at runtime

Closed-loop workflow

Synchronize

Read robot pose, reference path, and obstacle states

Predict

Propagate obstacle positions across the N-step horizon

Optimize

Formulate with CasADi and solve the NLP using IPOPT

Execute

Apply the first control, publish diagnostics, and repeat

A differential-drive unicycle model describes planar motion. Continuous-time dynamics are discretized with RK4 to balance prediction accuracy and numerical stability.

x = [p_x,\, p_y,\, \theta],\quad u = [v,\, \omega]

NMPC objective

J = J_x + J_u + J_{\Delta u} + J_N + S\varepsilon^{2}

The objective combines state tracking, control effort, control-rate smoothing, terminal error, and CBF slack. Heading error uses 1 − cos(θ − θ_ref) to avoid discontinuities at the angle wraparound.

Discrete control barrier function

h_i(x_{k+1}) + \varepsilon_k \ge (1 - \gamma)\, h_i(x_k)

h_i(x_k) = \lVert p_k - p_{\mathrm{obs},i,k}\rVert^{2} - (r_{\mathrm{robot}} + r_{\mathrm{obs},i})^{2}

For each obstacle the safety function above is enforced with a discrete forward-invariance constraint. A heavily weighted nonnegative slack and the tolerance h_k ≥ −0.01 soften instantaneous infeasibility, so this is a softened discrete safety constraint rather than an absolute safety proof.

Implementation details

Warm start & rebuild boundary

The previous solution initializes IPOPT while obstacle constraints remain unchanged. The current implementation rebuilds the problem and clears this initial guess when obstacle positions change—a target for further real-time optimization.

Dynamic obstacle prediction

Live position and velocity are propagated with a constant-velocity model, and CBF constraints are applied throughout the horizon.

Reference extraction

N+1 points are selected from the nearest path index, with lateral error blended out progressively. Reference heading is currently fixed to +X, so circle and slalom tracking still require controller work.

Safe fallback

Solver exceptions immediately produce zero linear and angular velocity. Diagnostics retain solve time, status, and CBF values.

Tests cover tiered oncoming obstacles, a narrow corridor, and dense multi-obstacle conflicts. Each scenario below includes a Gazebo demo video of the closed-loop avoidance run.

TEST 01
Tiered oncoming obstacles
Dynamic obstacles move head-on in formation, testing prediction-aware avoidance.

TEST 02
Narrow corridor
Tests moving-obstacle avoidance with limited clearance; corridor walls exist in Gazebo but are not yet represented as CBF constraints.

TEST 03
Dense multi-obstacle scene
Obstacles of different radii create simultaneous conflicts and stress continuous solver stability.

NMPC-CBF tracking error, control inputs, and solve-time log — An early NMPC prototype tuning plot included with the source. It does not benchmark the integrated CBF/Gazebo system and is not evidence for the figures above.

Obstacle intent

Prediction currently assumes constant velocity and observed state. The next step is short-horizon probabilistic prediction using an EKF or learned model.

Local-planner boundary

A fully blocked corridor can make the local NMPC infeasible or trap it in a local minimum. Integration with PRM or Hybrid A* is planned.

Perception & physical validation

Obstacle states currently come from Gazebo ground truth rather than a perception pipeline. Hardware tests must add detection/tracking and calibrate latency, wheel slip, and state-estimation error.

ROS 2 Humble · C++ · CasADi · IPOPT · Gazebo 11 · RViz2 · Python

Download clean source