第一次把 MPC 和 CBF 写进同一个优化问题是我做四轮差速机器人避障的时候。MPC 我熟滚动优化、约束处理、预测时域那一套闭着眼能给你写好 OSQP 接口CBF 那套安全不变性理论论文也读了不少。可等真把它们合在一起跑第一版代码直接在仿真里疯狂报 infeasible要么是安全约束太紧导致压根没有可行解要么是好不容易解出来实际轨迹却贴着障碍物边界抖得跟筛子一样。之后我反复调参数、换求解器、改约束形式才慢慢摸清楚这套组合拳的门道。如果你平时搞移动机器人、无人车、无人机或者哪怕只是做舵机、BLDC 电机控制只要系统里存在“既要跑得稳又不能越界”的需求MPC-CBF 这套思想就非常值得花时间弄明白。它解决的问题很具体MPC 擅长在满足约束的前提下找最优控制CBF 擅长用数学上可证明的方式保证某个安全集合永远不被突破。两者结合本质上是把“性能优化”和“安全保证”两件事塞进同一个优化框架里让系统既聪明又守规矩。这篇文章我就把原理、公式推导、工程实现和踩坑记录一起讲清楚适合有一定控制基础、但还不熟悉安全关键控制设计的工程师。1. 为什么要把 MPC 和 CBF 放在一起1.1 先捋清楚 MPC 的强项和软肋模型预测控制的核心思想说穿了就一句话在每一个控制周期用当前状态作为起点根据系统模型往前预测一小段时间在这段时间里寻找一组最优控制序列使得某个目标函数最小同时满足状态和控制约束最后只取第一个控制量作用于系统下一拍再重新来。这个框架最大的优势是“看得远”。传统 PID 只能根据当前误差做反应而 MPC 能提前看到未来几步的状态趋势因此它对大惯性、强耦合、有硬约束的系统特别有效。我见过很多工程实践比如无人机的轨迹跟踪、机械臂的力位混合控制、四足机器人步态控制核心决策层都是 MPC 在扛。但 MPC 也有一个天然软肋它所有约束都是“优化问题里的约束”优化器只能在数学上找一个可行解但它并不保证这个解在物理世界扰动下还可行。比如你设置了一个位置边界约束MPC 在标称模型下算出的轨迹不越界可一旦模型失配、摩擦变化、外部扰动进来实际轨迹可能轻轻松松就冲出去了。而且MPC 对约束的处理是“软性”的——理论上硬约束不可违背但实际中因为采样时间、模型误差、求解精度边界很容易在两次采样之间被穿越。1.2 CBF 到底在保护什么控制障碍函数是一套以集合不变性为核心的安全理论。它做的事非常纯粹设计一个标量函数 h(x)让安全区域表示为 h(x) ≥ 0然后构造一个约束条件迫使系统状态在边界上时“不会往外走”而是保持在安全集合内部。打个比方CBF 就像是给系统画了一个无形的“气泡”。气泡内部是安全区边界是危险线。CBF 的作用不是告诉系统怎么走最优路线而是每当系统试图穿出气泡时它会在控制量上叠加一个“反向推力”把系统拉回来。这个“反向推力”在数学上是一个对控制输入的线性不等式约束因此可以直接嵌入到任何优化问题里。这也是 CBF 能跟 MPC 无缝结合的最重要原因。它不像人工势场法那样只是给控制输入加个虚拟力也不像规划层那样先算安全轨迹再跟踪而是直接在控制层约束输入从机理上保证状态不离开安全区域。1.3 两者的分工与三种结合姿势MPC 和 CBF 的结合我见过三种常见姿势。第一种CBF 作为安全滤波层。任意给定一个标称控制器可能是 PID、纯跟踪、或者已经写好的 MPC它的输出先不直接给执行器而是经过一个 CBF-QP 优化器。这个 QP 的目标是让实际控制量尽可能接近标称控制量同时必须满足 CBF 安全约束。这样做的好处是改动极小只需要在原有控制链路上插一个安全校验环节。缺点是安全滤波层本身不关心长期性能它只处理当前这一拍是否安全。第二种CBF 约束直接嵌入 MPC 优化问题。也就是说在 MPC 原有的动力学约束、控制约束之外再额外加上 CBF 不等式约束。这样一个控制器同时承担性能优化和安全保证信息是共享的MPC 知道安全边界在哪在规划轨迹时就会提前避开。这也是现在学术界和工业界主流采用的方式。第三种MPC 算完轨迹之后再用一个独立的 CBF-QP 做校验和修正。本质上跟第一种类似只是标称控制器换成了 MPC。这种方案实现复杂度低适合改造已有系统。我个人的建议是新系统直接采用第二种因为它能让“安全”真正参与优化决策而不是事后补救。后面讲的原理和代码也以第二种为主。2. 核心数学基础安全集合、前向不变性与 CBF2.1 从“安全集合”说起要理解 CBF先得理解前向不变性。假设系统状态方程为[ \dot{x} f(x) g(x)u ]这是一个典型的仿射非线性系统f(x) 是漂移项g(x) 是控制输入矩阵。对于移动机器人x 可以是位置、速度对于倒立摆x 可以是角度、角速度。我们定义安全集合[ C { x \in \mathbb{R}^n \mid h(x) \ge 0 } ]如果系统从集合 C 内部的某个状态出发经过任意时间之后状态仍然一直停留在 C 内部那么这个集合就叫前向不变集。前向不变性就是我们想要的安全性质只要初始状态是安全的系统就不会在运行过程中走出安全区域。“不走出安全区域”说起来容易做起来难。关键在于如何把“状态不离开 C”翻译成对控制输入 u 的约束。CBF 就是干这个翻译工作的。2.2 CBF 是怎么保证安全的对安全函数 h(x) 求时间导数[ \dot{h}(x,u) \frac{\partial h}{\partial x}(f(x) g(x)u) L_f h(x) L_g h(x)u ]其中 (L_f h(x) \frac{\partial h}{\partial x}f(x))(L_g h(x) \frac{\partial h}{\partial x}g(x))分别是 h 沿 f 和 g 方向的李导数。你可以简单理解成h 的变化率由系统自身动态和控制输入共同决定。如果我们要求[ L_f h(x) L_g h(x)u \alpha(h(x)) \ge 0 ]其中 (\alpha(\cdot)) 是一个扩展类 (\mathcal{K}) 函数最简单的形式是 (\alpha(s) \gamma s)(\gamma 0)。那这个不等式就构成了一个 CBF 约束。为什么这个不等式能保证安全直观上来看当系统趋于 h(x)0 的边界时如果 h(x) 很小那么 (\alpha(h(x))) 也很小不等式主要靠控制量 u 来保证 (\dot{h} \ge 0)。这迫使系统在边界处要么停下要么往回走而不是继续往外冲。严格来说这个条件可以保证集合 C 是前向不变的这是 CBF 理论的核心结论。更妙的是这个约束对 u 是线性的。不管 h(x) 本身多非线性只要 h 的表达式清楚那么 (\frac{\partial h}{\partial x}g(x)) 就是已知的系数矩阵CBF 不等式就是一个关于 u 的线性不等式。这意味着它可以被高效嵌入到 QP、非线性 MPC 等各种优化框架中。2.3 相对阶问题与高阶 CBF这里必须提醒一个常见坑CBF 能用得好前提是 h 的导数里必须显式含有控制量 u。数学上叫“相对阶为 1”。举个例子。假设你要约束一个机器人的位置不越过某个边界取 h(x) x_boundary - x。系统是双积分模型 (\ddot{x}u)那么 (\dot{h} -\dot{x})它只含速度不含 u。再求一次导数(\ddot{h} -u)这时候 u 才出现。也就是说h 对系统的相对阶是 2不是 1。这时候普通 CBF 约束没法直接构造因为 (\dot{h} \gamma h \ge 0) 这个式子里根本没有 u没法直接转成控制约束。解决办法是用高阶 CBFHOCBF。基本思路是构造一个新的组合函数[ h_2(x) \dot{h}(x) \alpha_1 h(x) ]然后让 h_2 满足普通 CBF 条件。这样一阶一阶递推最终得到一个含 u 的安全约束。虽然表达式变复杂了但在实现时只需要提前算好几阶导数q系成固定的代码就行。我在做移动机器人避障时就吃过这个亏。障碍物距离函数 (h |p - p_{obs}|^2 - d_{safe}^2) 对单积分模型是相对阶 1没问题但对双积分模型位置的速度不直接受 u 控制必须用高阶 CBF。如果不管这些直接套公式仿真里你会发现系统的确有概率穿越障碍物边界因为约束根本管不住加速度。2.4 离散化与采样系统注意事项工程上控制都是离散的采样周期 (\Delta t)MPC 也是多步离散预测。所以 CBF 约束也要离散化。最常用的一阶近似是把连续条件[ \dot{h} \gamma h \ge 0 ]改写成[ \frac{h(x_{k1}) - h(x_k)}{\Delta t} \gamma h(x_k) \ge 0 ]变形之后就是[ h(x_{k1}) \ge (1 - \gamma \Delta t) h(x_k) ]这个形式在 MPC 里用起来非常方便。它说的是下一时刻的“安全裕度”不能比当前时刻少太多。如果当前已经离边界很近了也就是 h(x_k) 趋近于 0那么下一时刻 h(x_{k1}) 必须也趋近于 0 或更大安全裕度不能继续下降。这里注意(\gamma \Delta t) 不能太大否则约束相当于直接要求 h_{k1} ≥ 负数起不到限制作用也不能太小否则安全响应迟钝。一般取 (\gamma) 在 (1/\Delta t) 到 (2/\Delta t) 之间具体要看系统响应速度。这是一个经验范围我后面还会细说。3. 把 CBF 写进 MPC优化问题的设计与调参3.1 标准 MPC-CBF 优化问题长什么样假设我们面对的是线性离散系统[ x_{k1} A x_k B u_k ]MPC 的目标函数通常是预测误差和控制能量的加权和。在不考虑安全约束时优化问题长这样[ \min_{u_0, \dots, u_{N-1}} \sum_{k0}^{N-1} \left( |x_k - x_{ref,k}|_Q^2 |u_k|R^2 \right) |x_N - x{ref,N}|_P^2 ]约束包括[ x_{k1} A x_k B u_k,\quad k0,\dots,N-1 ][ u_k \in \mathcal{U},\quad x_k \in \mathcal{X} ]加上 CBF 约束之后还需要在每一步额外加一条[ h(x_{k1}) \ge (1 - \gamma \Delta t) h(x_k) ]如果 h 是状态的线性函数比如 (h(x) a^T x b)那么这条约束可以展开成[ a^T x_{k1} b \ge (1 - \gamma \Delta t)(a^T x_k b) ]再把系统方程代入 (x_{k1} Ax_k Bu_k)最终变成关于 (u_k) 和 (x_k) 的线性不等式。整个优化问题仍然是一个 QP可以用通用 QP 求解器高效求解。如果 h 是非线性的比如避障场景里的距离平方函数直接写进来会让优化问题变成非线性规划。工程上常用的近似手段是把 h 在当前预测轨迹点附近线性化或者用上一轮 MPC 解出的预测状态作为参考点做线性化。这样每个控制周期都得到一个近似线性的 CBF 约束精度足够计算量也可控。3.2 预测时域、权重和 CBF 衰减系数怎么取调参是 MPC-CBF 最枯燥也最关键的环节。我总结了几条经验。第一预测时域 N 和采样周期 \Delta t 必须匹配实际动态。N 太小MPC 看得不够远性能接近贪婪控制N 太大计算延迟上升而且后面几步的预测值受模型误差影响很大意义不大。对移动机器人\Delta t0.05sN20 到 40 比较常见对电机电流环这种快速系统\Delta t0.001sN5 到 10 可能就够了。第二Q 和 R 的比值决定系统“激进”还是“平稳”。Q 大系统会拼命追随参考轨迹控制量变化大R 大系统会克制控制幅度响应更迟缓。别一上来就对标论文里的参数最好从单位化开始先把状态变量归一化到量纲统一的范围再设置 Q 为单位阵R 为 0.01 倍单位阵然后根据波形调整。第三CBF 衰减系数 γ 这个参数是最容易翻车的。γ 过小比如小于 0.5/\Delta t安全约束太软状态会缓慢“渗”出安全区域γ 过大比如大于 5/\Delta t安全约束太硬稍微靠近边界就要求控制量剧烈反向很容易让优化问题无解或者在边界处振荡。我的经验是先从 (1/\Delta t) 开始观察边界附近的控制量变化如果太抖就调低如果穿边就调高。不要试图一步到位。3.3 求解器选型与代码骨架带线性 CBF 约束的 MPC 本质是 QP可选求解器很多比如 OSQP、qpOASES、PIQP、PROXQP。我自己用得最多的是 OSQP因为它处理不等式约束很稳而且支持 warm start适合连续在线求解。下面给一个极简 Python 风格的伪代码骨架展示 MPC-CBF 每个周期要做的事def solve_mpc_cbf(x0, x_ref, obstacle_pos): # 构建QP矩阵 # 决策变量: z [u_0, x_1, u_1, x_2, ..., u_{N-1}, x_N] # 这里为了简洁只画核心约束 # 1. 动力学约束: x_{k1} A x_k B u_k # 2. 控制约束: u_min u_k u_max # 3. CBF约束: h(x_{k1}) (1 - gamma*dt)*h(x_k) # 对线性h: a*x_{k1} b coef * (a*x_k b) P, q, A, l, u build_qp(x0, x_ref, obstacle_pos) result osqp.solve(PP, qq, AA, ll, uu, warm_startTrue) return result.x[0] # 取第一个控制量别小看这段伪代码它把 MPC-CBF 的核心抽象出来了动力学约束保证不违背物理规律控制约束保证执行器不超限CBF 约束保证安全边界不被穿越。至于目标函数只是在这三层约束围出来的可行空间里挑一个性能最优的解。3.4 求解实时性怎么优化MPC-CBF 在线求解最大的压力在于每个采样周期都要解一个优化问题而且预测时域 N 一旦拉长决策变量数量线性增加。我实测过一个 10 状态系统N30QP 变量接近 600 个OSQP 单次求解大概在 2 到 5 毫秒在 50ms 周期里完全够用但如果系统状态更多或者用的是非线性求解器延迟就可能成为瓶颈。优化实时性有几个土办法。第一热启动。上一周期的解作为本周期的初始值OSQP 这种主动集或 ADMM 类求解器对热启动非常友好能省掉一半以上的迭代。第二缩短预测时域但增加终端约束。比如把 N 从 30 降到 15同时在最后一步加上 CBF 或终端代价保证性能损失不大。第三CBF 约束只在第一步严格施加后续步骤放宽或者干脆只施加到前几步。这一步对实时性提升明显因为安全最紧要的是当前这一步控制不能破边界后面的预测值本来就是不确定的没必要在所有步都锁死。当然这种方法牺牲了一部分理论上的前向不变性工程上需要留更大的安全裕度来弥补。4. 三个实操案例从简到难4.1 一维质点避边界先从最简单的一维单积分器说起。系统模型[ \dot{x} u ]安全要求是 x 不能超过 x_max定义 h(x) x_max - x。这个 h 相对阶为 1所以普通 CBF 直接可用。连续 CBF 条件[ \dot{h} \gamma h -u \gamma (x_{max} - x) \ge 0 ]离散化后[ h(x_{k1}) \ge (1 - \gamma\Delta t) h(x_k) ]代入 (x_{k1} x_k u_k\Delta t)得到[ u_k \le \frac{\gamma(x_{max} - x_k)}{1-\gamma\Delta t} ]也就是说越靠近边界允许的最大速度越小。这个约束放进 MPC 里系统就会自动在接近边界前减速。我用这个案例测试过 γ 的作用。γ 取 1/Δt 时系统在边界处刚好停住不抖γ 取 5/Δt 时系统在边界附近反复加/减速控制量剧烈跳动虽然没穿边界但执行器听了会很难受。这说明 CBF 参数和整个控制回路的时间常数必须匹配不是越大越好。4.2 倒立摆角度安全约束倒立摆是控制理论里的经典对象。假设状态是摆角 θ 和角速度 ω目标是跟踪期望角度同时希望摆角不要超过某个极限比如 ±30°。系统模型[ \dot{\theta} \omega ][ \dot{\omega} \frac{g}{l}\sin\theta \frac{1}{ml^2}u ]如果直接定义 h θ_max^2 - θ^2那么 (\dot{h} -2θω)这里没有 u相对阶是 2。这时候需要用高阶 CBF。一种做法是定义两个障碍函数层[ h_1 \theta_{max}^2 - \theta^2 ][ h_2 \dot{h}1 \gamma_1 h_1 -2\theta\omega \gamma_1(\theta{max}^2 - \theta^2) ]然后要求[ \dot{h}_2 \gamma_2 h_2 \ge 0 ]展开之后就会得到关于 u 的约束。看起来繁琐但代码写起来其实就是不停求导代入。这个案例非常适合用来理解 HOCBF 的构造逻辑每一层 CBF 条件都是在“限制上一层函数的变化速度”。4.3 移动机器人避障移动机器人避障是我实际工程里做的最多的场景。假设机器人是四轮差速或履带式状态取位置 p 和速度 v控制输入是加速度 a。安全函数取[ h(p) |p - p_{obs}|^2 - d_{safe}^2 ]这个 h 对位置来说有直观的几何意义机器人到障碍物中心的距离平方减去安全半径的平方。大于 0 代表安全。由于系统是双积分器h 相对阶为 2实际工程中我通常有两种处理路线。路线一用高阶 CBF 严格推导把 h、h 的一阶导、h 的二阶导都算出来形成一个关于加速度 a 的线性约束。这样理论严谨但公式展开较长而且需要对障碍物速度或者位置噪声做鲁棒化处理。路线二把障碍物距离函数的约束“降阶”处理。比如我定义一个辅助安全函数[ h_v(p, v) h(p) c \cdot \dot{h}(p, v) ]把位置安全转换成位置和速度的组合安全。这样理论上还是需要在线计算但实现起来更直观。从工程结果来看路线一在仿真中表现更可靠特别是障碍物多的场景不容易出现“虽然位置安全但速度仍朝障碍物冲”的问题。实际落地时我会额外在 (d_{safe}) 上留 10% 到 20% 的裕量因为传感器噪声、定位延迟都会让真实距离小于估算距离。4.4 延伸舵机和 BLDC 电机控制中如何借用 MPC-CBF 思想聊到这儿也许有做电机控制的朋友会觉得这跟自己关系不大。其实不然。舵机控制原理和 BLDC 电机控制原理本质上也是一套带约束的控制问题。舵机通常有位置环、速度环、电流环三层。位置环负责转角和限位你肯定不希望舵机转到机械限位处还硬顶。这就可以借鉴 CBF 的思路把限位位置定义成安全边界 h(x) ≥ 0让控制器提前减速而不是靠机械限位硬扛。MPC 在这里并不需要太长的预测时域三五个采样周期的预测就足够让系统在限位前稳稳停住。BLDC 电机控制更是如此。BLDC 控制原理里最经典的反向电动势过零点检测原理本质上是利用电机三相绕组的反电动势过零点来确定换相时刻。这个过程本身是开关逻辑但在换相瞬间电流可能出现尖峰如果超过 MOSFET 或绕组的耐受值就可能烧驱动。此时可以在电流环上叠加一个 CBF 形式的电流安全约束把电流的平方限制作为 h(x)一旦电流接近上限安全约束强制降低 PWM 占空比或调整换相时序防止过流。这个思路比单纯靠硬件过流保护更主动因为你不需要等电流真的超限而是在趋势上提前拦住它。从更广泛的电机控制原理来看任何电机控制系统本质上都在做三件事换相逻辑、速度/位置闭环、电流/力矩保护。MPC-CBF 不能替代换相逻辑但它能很好地承担“安全保护”这一层让控制算法更大胆地去追求性能而不必时刻担心过冲。5. 训练与调试中的常见坑5.1 求解失败与 infeasibleMPC-CBF 最常遇到的问题就是求解器直接告诉你 infeasible优化问题无解。我第一次跑仿真时遇到这个报错整个人是懵的。后来逐步排查发现大多数情况是下面几种原因一是 CBF 约束和其他约束冲突。比如控制量上限不够大无法同时满足“追踪参考轨迹”和“不越安全边界”的要求。解决办法是给非关键约束加松弛变量让优化问题总是有解然后把安全约束作为硬约束其他约束作为软约束。二是预测时域内 CBF 约束跨步太多。第一二步满足后面几步因为模型预测误差或参考轨迹变化导致后面几步找不到可行控制序列。解决办法是 CBF 约束只在前两步施加或者只在第一步施加后几步用较小的安全裕度。三是初始状态已经在安全边界附近(\gamma) 又设得太大导致第一步就要求一个不可达的高反向加速度。解决办法是检测到 infeasible 时切换到纯 CBF-QP 安全滤波模式放弃性能目标只求当前控制量满足安全约束。5.2 边界附近抖动和 chatteringCBF 之所以会导致抖动是因为安全约束被激活时控制量会突然反转从“追轨迹”瞬间变成“推回去”。这个过程如果增益太大就会像继电器控制一样出现高频切换。我从实践里总结出三种缓解手段。第一种在目标函数中增加对控制量变化率的惩罚也就是限制 (\Delta u u_k - u_{k-1})。这样控制量即使要反转也是逐渐反转而不是跳变。第二种改变安全函数的形状。把线性的 h 换成带平滑过渡的版本让安全约束从“一碰就弹”变成“靠近才开始软推”。本质上是在约束里加入了凸近似的缓冲带。第三种最直接也最有效加大安全裕量。别让 CBF 约束在边界上精确生效而是在边界内缩一段距离。虽然这样会牺牲一点可用空间但换来了实控稳定性在工程上非常划算。5.3 模型失配导致安全失效CBF 的数学保证建立在系统模型准确的前提上。如果实际系统跟模型偏差很大即使 CBF 约束完全满足真实状态也已经穿出安全边界了。应对模型失配我常用的手段是给 h 加一个保守偏置项。比如避障距离是 d实际距离估计误差是 ±δ那么安全函数写成 h d^2 - (d_safe δ)^2。这个 δ 要根据实验标定别拍脑袋定。更理论化的方案是鲁棒 CBFRCBF。它会同时考虑扰动范围内所有可能的系统状态把 CBF 约束中的导数项替换成最坏情况下的值。代价是约束更紧控制量消耗更大。对于安全攸关的系统这个代价是值得的。5.4 计算延迟和执行时序MPC-CBF 的优化求解需要时间而求解期间系统状态一直在变。如果你的代码是“采样 → 求解 → 输出”那么执行输出时状态已经比求解时晚了一拍。处理这个问题有两个思路。思路一延迟补偿。在控制器里维护系统的名义模型求解时用“当前状态外推一个采样周期后的预测状态”作为 MPC 的起点这样执行时正好弥补了求解延迟。思路二把求解时间压进采样周期的一半以内。比如采样周期 50ms求解控制在 15ms 内完成剩下的 35ms 让执行器有充足的反应时间并在状态反馈时使用 FPGA 或实时操作系统打时间戳避免时间对齐错误。我实际推荐两个都做先把求解时间压下来再做一拍状态外推。双管齐下基本能消除延迟对安全性的影响。5.5 快速排查速查表为了你以后排查问题方便我把常见现象、可能原因和解决办法整理成一张表。遇到问题先按表查一遍能省下大量时间。常见现象可能原因解决办法优化器报 infeasibleCBF 约束与其他约束冲突安全约束保留为硬约束其他约束加松弛变量边界附近控制量高频抖动γ 过大或控制量变化率未惩罚降低 γ增加 Δu 惩罚增大安全裕量实际状态仍穿越安全边界模型失配加大 h 的保守项或使用鲁棒 CBF控制量饱和导致性能差安全裕量留得过大调小裕量或改用高阶 CBF 精确建模相同参数仿真正常实机失败传感器噪声、定位延迟增加滤波对状态做时间戳对齐和延迟补偿这张表里的每一条我都在实际项目中踩过。特别是最后一条仿真和实机表现不一致基本都可以归因到“模型输入和真实状态不一致”这件事上。控制算法再漂亮也抵不过一个脏数据。最后再分享一个我个人的经验MPC-CBF 这套东西理论的完美和工程的实用之间永远存在一道距离。你不可能靠一个论文公式包打天下安全边界的设定、γ 的选取、约束的松弛都是在试验台上一点点磨出来的。刚开始做的时候别追求数学上的严格前向不变性先保证工程上不穿边界、不抖动、不出安全事故等系统稳定了再慢慢收紧边界。把 CBF 当作一个“能协商的安全守卫”而不是一个“绝对拦死的墙”这套方法才能真正在项目里落地。