机器人不会撞墙了！CBF安全控制从数学到C++实现
导读：机器人怎么保证不撞到障碍物？本文从控制障碍函数（CBF）的原理出发，用C++实现一个自包含的碰撞避免控制器，核心代码不到200行，29个单元测试全部通过，0内存泄漏。配套代码仓库在 D:\code\test\cbf_qp_controller。
一、原理简析
控制障碍函数（Control Barrier Function, CBF） 是Ames等人在2019年提出的安全控制框架，已在Cassie双足机器人、自动驾驶Apollo等系统中落地。核心逻辑：定义一个安全集，然后保证机器人永远不离开。
对于障碍物，CBF定义如下：
h(x) = 距离障碍物 - 安全半径
当 h(x) >= 0 时，机器人处于安全区域。CBF对控制输入施加约束：
∇h(x)^T · u + α · h(x) >= 0
这个不等式保证：只要初始状态安全，系统就永远安全。然后把这个约束丢进一个二次规划（QP） 求解器：
minimize ||u - u_desired||^2
subject to ∇h_i(x)^T · u + α_i · h_i(x) >= 0 （所有障碍物约束）
u_min <= u <= u_max （控制限幅）
QP求解器在"尽量接近期望速度"和"绝对不撞"之间找到最优解。
二、实操步骤
2.1 项目结构
项目自包含，无外部依赖，仅需C++17编译器：
cbf_qp_controller/
├── src/
│ ├── common/Vec2.h # 2D向量库
│ ├── cbf/ControlBarrierFunction.h # CBF基类
│ ├── qp/QPSolver.cpp # 主动集QP求解器
│ ├── controller/CBFController.cpp # CBF+QP控制器
│ └── simulation/Simulator.cpp # 2D仿真器
├── tests/unit/ # 29个单元测试
└── CMakeLists.txt
2.2 障碍物CBF实现
class ObstacleCBF : public ControlBarrierFunction {
public:
double evaluate(const Vec2& position) const override {
double dist = position.distance_to(center_);
return dist - (radius_ + safety_margin_);
}
Vec2 gradient(const Vec2& position) const override {
double dist = position.distance_to(center_);
if (dist < EPS) return {EPS, 0};
return (position - center_) / dist;
}
};
2.3 QP求解器核心
实现了一个主动集法QP求解器，专为2D控制输入优化。流程：
- 先求无约束最优解（直接使用期望速度）
- 如果满足所有约束，直接返回
- 否则，尝试激活1个或2个约束，用KKT条件求解
// KKT system for single constraint
// λ = (a^T · P^{-1} · q + b) / (a^T · P^{-1} · a)
// u = -P^{-1} · q + λ · P^{-1} · a
2.4 控制器集成
Vec2 CBFController::compute_safe_control(
const Vec2& position, const Vec2& desired_velocity) {
// P = I, q = -u_desired
double P[4] = {1, 0, 0, 1};
double q[2] = {-desired_velocity.x, -desired_velocity.y};
// Build CBF constraints: ∇h_i * u + α_i * h_i >= 0
for (int i = 0; i < cbfs_.size(); ++i) {
double h_i = cbfs_[i]->evaluate(position);
Vec2 grad = cbfs_[i]->gradient(position);
A[2*i] = grad.x; A[2*i+1] = grad.y;
b[i] = -cbfs_[i]->alpha() * h_i;
}
// Solve QP
QPResult result = qp_solver_.solve(P, q, A, b, total_constraints);
return {result.x[0], result.x[1]};
}
2.5 编译与运行
mkdir build && cd build
cmake -DCMAKE_BUILD_TYPE=Release ..
cmake --build . -j4
# 运行测试
./cbf_test
# 29/29 passed
# 运行演示
./cbf_demo
# SUCCESS: Robot reached goal safely!
三、对比表格
| 维度 | 人工势场法 | MPC模型预测控制 | CBF+QP（本文方案） |
|---|---|---|---|
| 安全性保证 | 无严格保证 | 有（约束满足） | 有（Lyapunov证明） |
| 计算复杂度 | O(n) | O(n³) | O(n) |
| 实时性 | 极快 | 中等 | 极快（<1ms） |
| 局部极小值 | 常见 | 可以避免 | 可以避免（需全局规划） |
| 实现难度 | 简单 | 复杂 | 中等 |
| 适用场景 | 简单避障 | 复杂轨迹优化 | 安全关键系统 |
四、核心流程图
五、常见问题解决
Q：CBF+QP会让机器人"卡住"不动吗？
会。CBF是反应式安全过滤器，不是路径规划器。如果期望速度一直指向障碍物，QP输出零速度，机器人就停在CBF边界上。解决方案：配合全局规划器（如A*）或提供绕行路径点（waypoint），本项目已内置waypoint支持。
Q：QP求解器会无解吗？
多个约束互相冲突时可能无解。本项目在无解时返回零速度作为安全回退。实际部署中，可以通过松弛变量（slack variable）保证QP始终有解。
Q：能用于真实机器人吗？
本项目是2D仿真验证。真实部署需要：真实障碍物检测（激光雷达/深度相机）、机器人动力学模型、实时操作系统（如ROS2）。Michigan大学已用CLF-CBF-QP在Cassie双足机器人上实现实机避障。论文链接：arxiv 2301.01906。
六、总结
从零实现了一个CBF+QP碰撞避免控制器，三点：
- CBF原理：用数学不等式定义安全区域，Lyapunov证明安全不变性
- QP求解：主动集法，在"追踪期望"和"保持安全"之间找最优平衡
- 代码验证：29个测试全部通过，0内存泄漏，纯C++17自包含
CBF配合QP是机器人安全控制领域数学上可证明安全的方案，工程实现只需要一个QP求解器。完整项目代码在 D:\code\test\cbf_qp_controller。
下期：把CBF+QP方案扩展到ROS2环境，Gazebo仿真中实现真实避障。
参考来源
- 李宽宽《轮式人形机器人全身运动控制系统研究》，江南大学，2025
- Ames, A. D. et al. "Control Barrier Functions: Theory and Applications." ECC 2019
- Liu, J. et al. "Realtime Safety Control for Bipedal Robots via CLF-CBF Constraints." 2023