JD
返回项目列表

GAZEBO SIMULATION · SAFE MOTION CONTROL · 2026

基于 ROS 2 的差速驱动 NMPC-CBF 控制器

A CBF-NMPC-Based Safe Motion Control System for Mobile Robots

一套基于 ROS 2 与 Gazebo 的差速底盘局部控制仿真系统。针对动态障碍物避让问题，控制器在滚动时域内联合优化轨迹跟踪与软化安全约束，并将首个最优控制量在线下发给仿真底盘。

Gazebo 中差速机器人在障碍物走廊中的仿真场景
GAZEBO 11 / ROS 2 HUMBLE
独立项目
算法设计 · C++ 实现 · 系统集成 · 仿真验证
TECH STACK
ROS 2 Humble · C++ · CasADi · IPOPT · Gazebo 11 · RViz2 · Python
下载精简源码代码仓库链接待补充

目标与约束

  • 在动态障碍物迎面切入和窄通道仿真场景中保持在线轨迹跟踪。
  • 显式满足差速底盘非完整约束与速度、角速度边界。
  • 在 20 Hz 闭环周期内完成状态同步、障碍物预测、在线求解与控制下发。
  • 求解失败时触发零速度指令，保证系统具备明确的安全降级行为。

验证结果

以下指标来自项目演示材料；随附源码未包含可复现的全系统性能日志，后续将随测试视频与 rosbag 补充验证。

20 Hz

配置的控制循环频率

< 25 ms

演示材料报告的单次求解

0

演示材料报告的测试碰撞

100%

演示材料报告的解算成功率

控制器订阅里程计、参考路径和障碍物状态，按 20 Hz 标称周期构造预测问题。求解结果驱动仿真底盘，并发布预测轨迹、CBF 值和障碍物预测，供 RViz2 监控。

Python · 1 Hz

参考路径发布器

演示使用直线路径；工具同时支持圆形与 Slalom 点列

Python · 20 Hz

障碍物仿真器

同步动态障碍物里程计并广播速度状态

C++ · 20 Hz

NMPC-CBF 控制器

构建滚动时域优化并输出 v / ω

Simulation

Gazebo + RViz2 仿真环境

执行动力学仿真与预测轨迹可视化

项目实际 ROS 2 节点与话题通信图
实际运行时 ROS 2 节点与话题图

闭环工作流

01

状态同步

读取机器人位姿、参考路径与障碍物状态

02

时域预测

以恒速模型预测障碍物在 N 步内的位置

03

在线优化

由 CasADi 构造问题并使用 IPOPT 求解

04

滚动执行

下发首个控制量，发布诊断并进入下一周期

采用差速 Unicycle 模型描述平面运动。连续时间动力学经 RK4 离散化，以兼顾预测精度与数值稳定性。

x=[px, py, θ],u=[v, ω]x = [p_x,\, p_y,\, \theta],\quad u = [v,\, \omega]

NMPC 目标函数

J=Jx+Ju+JΔu+JN+Sε2J = J_x + J_u + J_{\Delta u} + J_N + S\varepsilon^{2}

优化项由状态跟踪、控制输入、控制增量平滑、终端状态和 CBF 松弛变量组成。航向误差采用 1 − cos(θ − θ_ref) 形式，避免角度周期带来的不连续。

离散控制屏障函数

hi(xk+1)+εk≥(1−γ) hi(xk)h_i(x_{k+1}) + \varepsilon_k \ge (1 - \gamma)\, h_i(x_k)
hi(xk)=∥pk−pobs,i,k∥2−(rrobot+robs,i)2h_i(x_k) = \lVert p_k - p_{\mathrm{obs},i,k}\rVert^{2} - (r_{\mathrm{robot}} + r_{\mathrm{obs},i})^{2}

对每个障碍物定义上述安全函数，并施加离散前向不变约束。高权重非负松弛变量与 h_k ≥ −0.01 容差用于缓解瞬时不可行，因此这是软化的离散安全约束，而非绝对安全证明。

关键实现

01

Warm Start 与重建边界

障碍物约束不变时保留上一周期解作为 IPOPT 初值；当前实现会在障碍物位置变化时重建问题并清空初值，这是后续实时性优化重点。

02

动态障碍物预测

根据实时位置和速度执行恒速外推，并在整个预测时域内逐步施加 CBF 约束。

03

参考路径截取

从最近路径点截取 N+1 个参考点并渐进消除横向偏差。当前控制节点将参考航向固定为 +X，圆形与 Slalom 跟踪仍需完善。

04

安全降级

求解异常时立即发布零线速度和零角速度；诊断消息持续记录求解耗时、成功状态和 CBF 值。

测试覆盖多障碍物迎面拦截、窄通道卡口和复杂多障碍物场景。下方每组场景附有 Gazebo 测试视频，可直接播放查看闭环避障过程。

TEST 01

多障碍物迎面拦截

动态障碍物以梯队形式迎面运动，验证预测约束与在线绕行能力。

TEST 02

窄通道卡口

在有限横向空间中测试动态障碍物避让；通道墙体用于 Gazebo 碰撞环境，尚未作为 CBF 约束输入。

TEST 03

复杂多障碍物

不同半径障碍物形成高密度冲突，测试连续求解稳定性。

NMPC-CBF 轨迹误差、控制输入和求解耗时记录
随源码附带的早期 NMPC 原型调参图，不包含完整 CBF/Gazebo 集成，因此不作为上方性能指标的验证证据。

障碍物意图建模

当前采用恒速、已知状态预测。下一步将结合 EKF 或学习模型，输出短时多轨迹概率预测。

局部规划边界

当通道完全阻塞时，局部 NMPC 可能进入局部极小或不可行状态。计划接入 PRM / Hybrid A* 全局规划器。

感知与物理平台验证

当前障碍物状态来自 Gazebo 真值而非传感器感知。后续需接入检测与跟踪，并在真实底盘上标定延迟、轮胎侧滑和状态估计误差。

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

下载精简源码