企业官网建设流程全解析

做机器人控制或无人车规划的人，多半都遇到过这个诡异场景：仿真里MPC跑得行云流水，障碍物绕得干干净净，结果一上实车，某个工况下它还是蹭着障碍物边沿过去了。你翻log一看，预测时域内明明每个点都在安全范围里，偏偏就出了事。我以前也以为这是模型不准，后来才意识到，MPC的约束本质是“离散时刻的约束”，它保证的是采样点安全，不是连续时间安全。如果想要一套真正能把“安全”两个字写死到数学里的控制框架，就得认真看看MPC和CBF（Control Barrier Function，控制屏障函数）的结合。这篇文章我从原理讲到具体实现，一步步拆MPC-CBF，最后附一个可以直接拿去跑的仿真例程，适合有一定控制理论基础、但还没系统接触过CBF的工程师和研究生。

1. 先弄清楚：为什么MPC本身难以承诺安全

1.1 MPC的安全性是“点约束”，不是“集合约束”

模型预测控制每一拍都在求解一个有限时域开环最优控制问题。假设系统离散模型是：

x(k+1) = f(x(k), u(k))

MPC在k时刻把未来N步的状态和输入全部预测一遍，把障碍物距离、关节限位这一类约束写成对所有预测时刻的不等式：

g(x(k+i)) ≥ 0, i = 1,...,N

然后交给QP或非线性求解器，得到一串最优控制序列，只执行第一个。听起来没问题，信息量也够大。但你注意，它约束的是“预测轨迹上离散点的状态”，相邻两个采样点之间系统怎么走，MPC根本不关心。采样时间越短，这个漏洞越小，可是实车系统的计算时间摆在那，10ms到100ms都是常事。在这段时间里，系统完全可能从障碍物边缘的合法状态，穿过约束边界再回到下一个合法采样点。

这就像走夜路，每隔一米有一盏路灯，你只保证自己路过路灯时站在灯下面，两盏灯之间的黑暗路段摔不摔跤，没人管。CBF做的事情，就是把“必须站在灯下”升级为“整条路都是安全走廊”，一旦进入安全集合，就永远不离开。

1.2 求解失败与模型失配会进一步撕开安全口子

MPC的安全性还有两个软肋。第一是求解失败：遇到强非线性、非凸约束或者初始点不可行，求解器可能来不及收敛，返回一个次优解甚至空解。工程上通常的处理方式是丢弃这一拍、沿用上一拍指令，可上一拍指令在那个新时刻未必安全。第二是模型误差和外部扰动：MPC预测基于模型，如果模型偏差让预测轨迹比真实轨迹更乐观，那所有“看起来安全”的约束点都不可信。

CBF的思路恰好和MPC互补。它不追求预测多少步，而是在当前状态上直接建立一个“安全集合”，并推导出一个要满足的微分不等式，让控制输入无论怎么选，都能保证状态轨迹永远停留在集合内部。它做的是连续时间意义下的前向不变性（forward invariance），和离散采样无关。所以主流的做法，就是把MPC的性能优化能力和CBF的安全硬保证焊在一起，一个负责跑得快、跑得省，一个负责别出事。

2. 控制屏障函数的不变性条件是怎么回事

2.1 从安全集合到屏障函数

考虑控制仿射系统：

x_dot = f(x) + g(x)u

我们想定义一组“绝对不能进”或“必须待在里”的状态。多数安全约束都能表示成连续函数的子水平集或超水平集。比如防碰撞，定义：

h(x) = distance_to_obstacle - safe_margin

要求h(x) ≥ 0，这个集合C = {x | h(x) ≥ 0}就是安全集。h的0水平集是安全边界。CBF的核心逻辑很朴素：如果系统从安全集内部出发，并且在所有时刻都满足一个关于h的微分不等式，那么h永远不会降到0以下，系统就不会离开安全集。

这个微分不等式长这样：

h_dot(x,u) ≥ -α(h(x))

其中α是一个严格递增、α(0)=0的K类函数，最常用的是线性函数α(h) = γ h。

2.2 为什么这个不等式能保证安全

我来解释一下这个不等式的几何直觉。h_dot是h沿着系统轨迹的变化率。如果边界上h=0，那么在边界处要求h_dot ≥ 0，状态要么停住、要么回头，这就是最简单的“不让出去”。但单纯的h_dot ≥ 0只在边界起作用，内部状态可以慢悠悠往边界走，等到了边界才突然紧急制动，这会造成非常激进的输入。

CBF条件聪明在：它允许h小的时候h_dot可以取负值，也就是状态可以慢慢靠近边界，但在h越接近0时，h_dot的允许下界逼近0，绝不允许以朝向边界的速度穿越。用线性α展开就是：

h_dot ≥ -γ h

如果h比较大，右边是比较负的数，h可以以一定的速度下降；一旦h变小，h下降的速率必须被限制住。h = 0.01时，h_dot必须大于等于-0.01γ，几乎等于不允许继续贴近。这个性质可以保证边界处的“接触速度”为零，从根源上避免撞击。数学证明用的是比较引理（Comparison Lemma）：把h_dot ≥ -γ h看成动态系统，真实h始终不小于参考解h_ref(t) = h(0)e^{-γt}，而参考解永远非负，所以h也永远非负。

2.3 CBF约束对输入来说是线性的

把h_dot展开：

h_dot = L_f h(x) + L_g h(x)u

其中L_f h = ∂h/∂x · f(x)，L_g h = ∂h/∂x · g(x)。对控制仿射系统来说，h_dot对u是仿射关系。于是CBF约束变成：

L_g h(x) · u ≥ -L_f h(x) - γ h(x)

这是一个关于u的线性不等式。在每一拍，状态x已知，L_f h和L_g h都是已知向量，这个约束可以直接查进一个QP优化问题里。我解MPC-CBF时，最喜欢CBF的一点就是它不增加非线性程度，代价函数随便你定，安全约束始终是线性的。

注意：这里说的线性是“对u线性”。如果系统本身非线性，f(x)和g(x)求值还是要依赖非线性模型，但CBF约束本身不引入额外的非线性优化复杂度。

3. MPC-CBF融合的三种主流做法与取舍

3.1 做法一：CBF作为硬约束直接进MPC优化

这是学术论文里最干净的形式。把MPC的约束集合里额外加上未来每一步的CBF不等式：

min ∑_{i=0}^{N-1} ( ||x(k+i)-x_ref||_Q^2 + ||u(k+i)||_R^2 )

s.t. x(k+i+1) = f(x(k+i), u(k+i)) u_min ≤ u(k+i) ≤ u_max L_f h(x(k+i)) + L_g h(x(k+i)) u(k+i) + γ h(x(k+i)) ≥ 0

预测域里每一步都要求CBF条件成立，相当于把安全走廊铺满了整个预测范围。好处是MPC可以提前看到障碍物变化趋势，CBF约束和轨迹规划协同最优；坏处是随着预测步数N增加，约束数量线性膨胀，如果模型非线性，整个问题就从QP变成了NLP，算力开销很现实。

我用这个方案做纯仿真没问题，但上真机时要格外小心求解时间。N=15以上、非线性模型、100 Hz控制频率，普通工控机经常会跑到单拍求解时间上限。除非你项目里有还不错的NLP求解器或者专门做了代码生成，否则慎重选。

3.2 做法二：CBF作为安全滤波器，后置修正MPC输出

我实际项目里更常用这一套。既然CBF约束是线性的，那完全可以让MPC先按原有逻辑求解，出一个性能最优的指令u_mpc，然后在后端接一个专门的CBF-QP：

min ||u - u_mpc||^2

s.t. L_f h(x) + L_g h(x)u + γ h(x) ≥ 0 u_min ≤ u ≤ u_max

这个QP只有输入维度那么多变量，通常2到6个，闭着眼睛都能在毫秒级解完。它做的事情是：在保证安全的前提下，找一个离MPC原始指令最近的可行指令。如果MPC给的指令本来安全，QP原样放行；如果它想违章，QP强制把它拉回安全边界内。

这种“性能规划+安全滤波”的分层架构有个被低估的好处：MPC那一层不需要管理复杂的安全约束，可以做得很快，而CBF-QP这一层是凸优化，有唯一解，鲁棒性好得多。即便MPC因为别的原因挂掉了，CBF-QP也会按“尽量贴近原始指令”的原则硬撑住安全底线。很多学术文献里称之为Safety Filter，工程上我觉得它就是最省心的安全兜底。

3.3 做法三：软约束形式的CBF项

CBF硬约束有个头疼的情况：状态一旦因为模型失配、大扰动或者传感器噪声落到安全集外（h < 0），硬约束会强迫系统“几乎不可能回归安全集”。这背后的数学原因是，不等式右侧的-γh在h为负数时变成正值，对系统提出了非常苛刻的要求，常常导致QP无解。

工程上我惯用的处理是给CBF约束加一个松弛变量δ：

L_f h + L_g h u + γ h ≥ -δ， δ ≥ 0

并且把δ以较高的权重写进代价函数。这样在正常工况下δ被压到0附近，CBF约束是硬的；一旦异常状态发生，δ允许短暂滋长给系统一个“重新回到安全集”的通道。这个技巧在论文里的名字叫relaxed CBF。代价是安全保证从“绝对保证”变成“尽量保证”，但收益是可解性大幅提升，不至于在关键时刻求解器直接罢工。

我给这三种做法排个序，方便选型：

4. 从零写一个可跑的MPC-CBF仿真：公式与代码

4.1 仿真系统与CBF设计

我拿一个平面双积分器小机器人来做完整示例。状态x = [px, py, vx, vy]，输入u = [ax, ay]是加速度。这是一个最简单的包含位置和速度的模型，足够说明CBF是怎么工作的，而不会让公式淹没在动力学细节里。

离散动力学直接精确离散化：

A = [[1,0,dt,0], [0,1,0,dt], [0,0,1,0], [0,0,0,1]]

B = [[0,0], [0,0], [dt,0], [0,dt]]

障碍物放在原点，半径r_obs = 0.5，机器人半径r_rob = 0.2。安全函数：

h(x) = ||p - p_obs|| - (r_obs + r_rob)

要求h(x) ≥ 0，也就是机器人边缘和障碍物边缘之间的间隙不能是负的。梯度为：

∂h/∂p = (p - p_obs) / ||p - p_obs||

对速度的梯度为0。于是：

L_f h = [∂h/∂p, 0] · [v; 0] = (p-p_obs)^T v / ||p-p_obs||

L_g h = [∂h/∂p, 0] · [I_2×2; I_2×2] = (p-p_obs)^T / ||p-p_obs||

CBF约束写成：

(p-p_obs)^T ax / ||p-p_obs|| + (p-p_obs)^T v / ||p-p_obs|| + γ(||p-p_obs|| - 0.7) ≥ 0

4.2 搭建MPC-CBF的QP

我直接基于Python + OSQP写。OSQP是一个开源的凸QP求解器，数组接口比其它求解器更直接，非常适合快速搭原型。核心代码分三块：预测模型、约束组装、滚动循环。

import numpy as np from osqp import OSQP import scipy.sparse as sp dt = 0.1 A = np.array([[1,0,dt,0], [0,1,0,dt], [0,0,1,0], [0,0,0,1]]) B = np.array([[0,0], [0,0], [dt,0], [0,dt]]) nx, nu, N = 4, 2, 20 p_obs = np.array([0.0, 0.0]) r_obs, r_rob = 0.5, 0.2 gamma = 1.5 def h_fun(x): return np.linalg.norm(x[:2] - p_obs) - (r_obs + r_rob) def dhdq_fun(x): dp = x[:2] - p_obs n = np.linalg.norm(dp) if n < 1e-6: return np.zeros(4) dg = np.zeros(4) dg[:2] = dp / n return dg

预测状态和输入按“先全部状态、再全部输入”的顺序排列成一个优化向量z。每个时刻的状态可以用A、B递推展开成初始状态x_current的线性函数，这是线性MPC的标准技巧。代价函数用对角权重矩阵，Q对应状态的权重，R对应输入的权重。

关键的CBF约束矩阵组装如下：

def build_qp(x_current, x_ref, u_ref): n_vars = (nx + nu) * N # 状态展平索引: x_{k+i} 在 z[(nx+nu)*i : (nx+nu)*i+nx] # 输入展平索引: u_{k+i} 在 z[(nx+nu)*i+nx : (nx+nu)*i+nx+nu] P = np.zeros((n_vars, n_vars)) q = np.zeros(n_vars) Q_diag = np.array([10.0, 10.0, 1.0, 1.0]) R_diag = np.array([0.1, 0.1]) for i in range(N): idx_x = (nx + nu) * i idx_u = idx_x + nx P[idx_x:idx_x+nx, idx_x:idx_x+nx] += Q_diag P[idx_u:idx_u+nu, idx_u:idx_u+nu] += R_diag q[idx_x:idx_x+nx] += -2.0 * Q_diag * x_ref q[idx_u:idx_u+nu] += -2.0 * R_diag * u_ref # 动力学等式约束构建 # Aeq z = beq，形式是 x_{k+i+1} - A x_{k+i} - B u_{k+i} = 0 Aeq_rows, Aeq_cols, Aeq_vals = [], [], [] beq = np.zeros(nx * N) # CBF不等式约束 Aineq_rows, Aineq_cols, Aineq_vals = [], [], [] bineq = [] for i in range(N): # 预测到第i步的名义状态（线性递推展开） # 这里为了展示直接用一个名义参考轨迹代替，完整实现需要递归计算 pass return None

上面代码我只搭了骨架。完整实现里需要用递推公式把x_{k+i}表示为x_current和前面所有输入的线性组合，然后逐行生成Aeq和Aineq。为了让文章不膨胀成一份代码仓库，我把完整代码的要点写清楚：

• 定义扩展状态向量z = [x_{k+1}, u_k, x_{k+2}, u_{k+1}, ..., x_{k+N}, u_{k+N-1}]。
• 等式约束每行对应一条状态递推关系。
• CBF不等式每步i对应一条：dhdq(x_{k+i})·(A x_{k+i} + B u_{k+i}) + gamma·h(x_{k+i}) ≥ 0。
• 求解后用第一段输入u_k作用于真实系统，然后推进一个dt，重新初始化QP。

4.3 实测出来的行为与参数影响

我跑固定轨迹跟踪实验，目标点放在障碍物后方。没有CBF约束时，MPC的轨迹会穿过障碍物，因为它每个离散点都避开了，但相邻离散点间的连线可能切入障碍圆。加上CBF约束后，轨迹会提前开始绕行，并且在贴近障碍物时速度明显降低，间隙保持在安全margin以上。

参数增益γ对行为的影响非常直观：

• γ取0.1时，CBF约束很“软”，系统慢悠悠地靠近障碍物，而且会贴着安全边界走，像是司机很自信地贴线过弯。
• γ取1.5时，系统明显更早、更猛地把h往高处抬，走出来的轨迹离障碍物更远，但是控制量会出现小幅抖振。
• γ取10以上时，控制量高频抖动非常明显，实际物理执行器根本吃不消，这也是我在下一节要重点说的坑。

调参顺序我建议固定Q、R，先把γ从0.2开始以2倍步长往上扫；每跑完一次仿真记录h的最小值和控制量变化率，选出h最小值不太接近0、控制变化率又可接受的最小γ。贪心地拉高γ不会让“安全余量”线性变大，只会让系统变得神经质。

5. 实测中最容易翻车的地方与调整经验

5.1 增益γ：调大了抖，调小了pass

我见过很多刚上手CBF的人第一反应都是把γ调大，觉得“安全边界越硬越好”。真实物理系统不是那么回事。CBF约束本质上是把状态往安全集内部“推”的速度和强度相关联，γ太大等同于在边界附近施加一个增益极高的反馈项，体现在控制输入上就是剧烈变化。

以我调无人车避障的经验，γ的上限并不取决于仿真，而取决于执行器的响应带宽和执行器延迟。你仿真里用0.1ms步长没问题，但真实电机的电流环、位置环都有延迟，控制量的高频分量追不上，反而会激励机械共振。建议用一阶惯性环节模拟执行器延迟，先把γ扫一轮，再上真机。

5.2 离散化误差：连续时间公式不能直接照搬

CBF条件是连续时间版本h_dot ≥ -γh，但MPC在数字芯片上离散运行，每一拍之间控制量是零阶保持的。如果直接拿上一拍的梯度信息和当前状态去算L_f h、L_g h，然后强行插入离散QP，这两个之间的不匹配会造成轻微的约束违规。

解决方式有两种。第一种是尽量缩短MPC控制周期，让离散点之间的间隔小到CBF连续条件近似成立，这个方法最省事，但对计算平台要求高。第二种是用离散时间CBF条件：

h(x_{k+1}) - h(x_k) ≥ -γ_d h(x_k)

其中γ_d ∈ (0,1]。这个条件直接基于一步预测的状态差来构造，天然适应零阶保持。代价是约束里会出现h(f(x_k,u_k))对u的非线性，线性系统还好，非线性系统就麻烦了。我自己在工程上用的是折中：用连续CBF条件构造QP，但是把h的安全余量从0改成一个正的margin。本来要求h ≥ 0，现在要求h ≥ 0.15。这个margin把连续和离散之间的差值以及模型误差全部吞掉，简单粗暴，但极其有效。

5.3 状态一旦进入非安全集：如何自救

这个坑几乎一定会踩到。传感器跳变、障碍物检测短暂丢失、模型误差稍大，h就可能出现负值。此时如果你还坚持原始CBF硬约束，QP可能直接无解，安全兜底系统反而在关键时刻掉链子。

我的自救预案分三档：

第一档：把CBF约束改成带松弛变量的软约束，让δ承担短暂违规，代价函数里把δ权重设成比正常控制量权重高一两个数量级的数值。这时系统的行为是：如果还来得及避免碰撞，就避免；如果实在避免不了，也保证把状态拉回安全集的路径是优化可行的。

第二档：在滤波后端再加一个备份安全滤波器。MPC求解失败或超时时，直接用上一时刻的最优控制指令或默认刹车指令，不追求性能，只保证系统按照动力学减速并避开已检测障碍物。

第三档：对h的输入信号做中值滤波或者卡尔曼滤波。CBF对状态质量很敏感，h的毛刺会直接变成控制量尖峰。这个不改变算法，但对实机稳定性提升巨大。

5.4 别把CBF当成势场法用

我在复现项目时踩过一个认知上的坑：以为CBF就是更平滑的人工势场法（APF）。不是的。势场法把避障描述成虚拟排斥力，加在系统输入上，力的大小通常和距离成反比。它本质上是个连续的软惩罚，对安全和稳定没有任何保证，而且容易陷入局部极小。CBF则是通过约束把“不安全控制输入”从允许集合中直接剔除，它不会引入额外的力，它只负责划出红线。CBF的优点是安全有证明，缺点是对那些远离障碍物、但想朝障碍物方向运动的轨迹，它约束的是“逼近速度”，而不是“绝对位置”，所以有时你会看到系统贴着障碍物平行飞过，看起来像是“不怕死”，其实从不变性角度看，它是安全的。

这个区别经常被新手忽略。如果你想追求工程师直觉上那种“和障碍物保持足够远”的效果，就得把安全函数里h的排布设计好，比如直接用距离减去一个较大的期望间距，而不是物理碰撞距离。

5.5 关于模型不确定性的最后提醒

CBF的数学保证是以精确模型为前提的。模型有偏差时，不变性不成立。这和MPC面临的问题是共通的，只不过CBF把风险暴露得更直白：你明明计算出的L_f h满足不等式，实际系统动力学和模型不一致，真实h照样可能掉下去。所以稍微负责任的工程落地，都会给CBF不等式里额外留出一个与模型不确定界成比例的鲁棒项。学术文献里有基于扰动的鲁棒CBF、基于学习的CBF修正等分支，但工程上先走一步：把摩擦、延迟、传感器噪声的实测上界折算成h_dot的扰动界，加到一个更严的约束里。这个处理不需要改框架，只改约束的常数项。

说回到我自己几次项目的体会。MPC-CBF这套东西，理论上非常漂亮，但真正落地时的性格更多取决于你如何处理离散化、异常状态、模型误差这些“脏活”。“安全是靠证明的”这是CBF最吸引我的地方，但在验证完所有数学条件后，留好margin、备好后手，才是把论文搬进实物的关键。如果你准备在下一个项目里引入这套控制框架，我建议先在小状态维度、小预测时域上把CBF滤波器跑通，再逐步放开MPC的复杂度。实践顺序上，纯MPC先跑通、CBF安全滤波器后置接入、最后再考虑把CBF做成预测域内的硬约束，这条路走下来，你会比直接上手完整NLP方案的人稳得多。