做了很多年编队控制和安全关键控制，我拿到这个标题时第一反应是：又是一篇CBF（控制障碍函数）相关的内容。但细看“TAC——具有全状态和输入约束的安全一致性跟踪”这个表述，其实指向一个非常具体的工程场景：一群移动机器人在跟踪虚拟领航者的过程中，既要保证队形一致、跟踪误差有界，又不能让任何一个状态量越过物理极限，更不能让执行器饱和破坏安全性。这一类问题在无人机集群、仓储机器人调度、无人车编队里太常见了，常见到很多人第一版代码都是用饱和函数硬截断，结果跑出来安全性和稳定性双双出问题。
CBF方法的价值在于，它把安全要求形式化成一组约束不等式，再通过在线求解二次规划（QP），对名义控制量做“最小修正”。这样既保留了原控制器的跟踪性能，又能在数学上保证系统状态始终停留在安全集合里。这篇内容我不会停在公式层，而是把TAC问题拆开，讲清楚CBF为什么能干这活，CBF条件怎么从“状态约束”变成“输入不等式”，再给出一份可以直接改参数跑起来的Matlab仿真框架。适合正在做多智能体编队、无人机集群或移动机器人避障的同学参考，尤其是那些已经能跑通一致性协议、但一加约束就发散的人。
1. 先拆解TAC问题：一致性跟踪中的“全状态+输入约束”到底难在哪
1.1 从一致性跟踪到安全一致性跟踪
一致性跟踪问题，简单说就是一群跟随者通过局部通信，在保证编队结构的前提下，收敛到领航者的轨迹上。经典的名义控制器长得很漂亮：每个智能体i根据自己和邻居的相对误差、自己和领航者的误差计算控制量，所有误差归零，任务完成。图论里那些拉普拉斯矩阵、代数连通度的分析，在无约束理想状态下确实好用。
但麻烦在于，这个名义控制器假设执行器可以输出任意大的力，假设机器人可以在任意速度下继续加速，这在物理世界根本站不住脚。一旦机器人接近速度上限，或者执行器饱和，名义控制器给出的指令无法被系统真实执行，跟踪误差可能不降反升，甚至引发碰撞。
TAC问题的核心就在这里。我理解TAC这个缩写，最自然的展开就是“Tracking with All-state and input Constraints”，也就是“全状态和输入约束下的跟踪控制”。我们需要的不再只是一个能保证平均收敛的协议，而是一个在所有状态受限、输入受限前提下仍然安全、仍然能完成跟踪任务的控制器。
1.2 全状态约束和输入约束的典型场景
全状态约束和输入约束在工程里到处都是，列一个表看得更清楚。
| 约束类别 | 典型形式 | 工程来源 |
|---|---|---|
| 位置约束 | 工作空间边界、禁飞区边界 | 场地边缘、障碍物、跑道边界 |
| 速度约束 | 最大线速度、最大角速度 | 电机转速限制、载荷要求、乘客舒适度 |
| 其他状态约束 | 俯仰角限制、电流限制 | 车辆侧翻风险、执行机构行程 |
| 输入约束 | 驱动力/力矩上限 | 执行器饱和、能量管理 |
这些约束不是孤立的。输入约束会通过动力学耦合到状态约束里：你踩油门太大，速度就会冲过上限；你转向太猛，位置就可能冲进障碍物。所以简单的饱和函数处理，本质上是在“事后截断”，而不是“事前预防”。
在TAC问题里，状态约束和输入约束还要同时满足。这比单一约束难得多，因为多个约束可能互相冲突：为了不撞到邻居，你希望机器人减速转向，但执行器已经在饱和上限，转向力矩不够，安全约束就无法满足。这类问题用传统方法处理起来非常别扭。
1.3 为什么传统方法解决不好
很多人第一反应是上模型预测控制（MPC）。MPC确实能同时处理状态约束和输入约束，预测时域一拉，滚动优化一跑，效果也不错。但MPC有三个痛点：一是需要预测模型，模型稍有偏差，约束保证就变得脆弱；二是在线优化计算量大，低速移动机器人或许扛得住，高速无人机就很难；三是代价函数和约束的权重调整比较敏感，工程上梳理起来费时间。
饱和函数是最粗暴的办法，直接把超出执行器能力的控制量截断。但“截断”之后的真实控制量已经不是控制器设计时的那个量了，闭环性能不可控。积分饱和、极限环振荡、安全性被破坏，都是常见的连锁反应。
人工势场法也常被用来避障，它把障碍物映射成虚拟排斥力，叠加到名义控制量上。这个方法实现快，但有两个老问题：容易陷入局部极小点，以及没有形式化的约束保证。“看起来避开了”和“数学上保证避开”是两回事。
CBF方法之所以在近几年的安全关键控制里火起来，是因为它把安全集合设计成“前向不变集”，通过一个障碍函数不等式把安全性转成对输入的不等式约束，然后在线求解一个最小修正问题。计算量比MPC小得多，安全性又有严格保证。下面详细讲原理。
2. 控制障碍函数方法的核心原理与工程解读
2.1 从控制李雅普诺夫函数到控制障碍函数
控制领域有个老朋友叫控制李雅普诺夫函数（CLF），它关心的问题是“系统能不能收敛到目标点”，通过构造一个V(x)，让V的导数小于等于0来保证吸引性。CBF和CLF是互补关系：CLF负责“别飘”，CBF负责“别出事”。
直观理解，可以把CLF当成油门，CBF当成护栏。油门让你往目标跑，护栏阻止你冲出安全边界。CBF的思想是：定义安全集合S = {x | h(x) ≥ 0}，比如h(x)可以是机器人与障碍物的距离减去一个安全阈值。如果系统从某个安全状态出发，能保证h(x)一直不小于0，那么系统就永远留在安全集合里。
这个“保证”不是靠祈祷，而是要求h的下降趋势受到限制。在系统ẋ = f(x) + g(x)u作用下，h的导数可以写成L_f h(x) + L_g h(x)u。CBF条件要求存在一个扩展类K函数α(·)，使得：
L_f h(x) + L_g h(x)u + α(h(x)) ≥ 0
如果这个条件满足，边界就越推越远，系统就不可能穿过h = 0的“墙”。α通常取γh的形式，γ是一个正增益，控制着“排斥力度”。
2.2 全状态约束如何写成CBF不等式
把抽象的CBF定义落到TAC问题上，就需要把每个状态约束都变成一个合适的h函数。
先说速度约束。对每个智能体i，取h_{v,i} = v_max² - ‖v_i‖²，这个函数在速度低于上限时为正。求导后，由于速度的导数就是控制输入u_i，h的导数里会直接出现u_i，这种约束叫相对阶为1，标准CBF可以直接处理。
真正容易翻车的是位置约束。比如避碰约束h_{c,ij} = ‖p_i - p_j‖² - d_min²，这个h的表达式里只有位置p，而位置是通过速度、再通过输入间接控制的。求一次导，表达式里只有速度，没有输入；求二次导，输入才出现。这种相对阶为2的约束，标准CBF条件不适用，需要用到指数控制障碍函数（Exponential CBF）或者高阶CBF。
ECBF的处理方式很简单：把二阶约束条件写成
L_f²h + L_g L_f h u + α₁ h + α₂ L_f h ≥ 0
只要α₁、α₂选得合适，h和L_f h组成的误差系统稳定，位置约束就能被保证。这一点是TAC实现里的关键，后面代码部分我会专门演示。
输入约束本身不需要写成CBF，它本来就是关于u的直接不等式：u_min ≤ u ≤ u_max，可以直接作为QP的硬约束加进去。可以认为：CBF把状态约束转化成了关于输入的不等式，输入约束本身又是关于输入的不等式，两类约束最后在同一个QP里合并求解。
2.3 CBF-QP：把安全性变成一个有约束最优化问题
现在有了名义控制器u_nom，有了CBF不等式，有了输入约束，CBF-QP要做的事情就是——
找一个u，让u尽可能接近u_nom，同时满足所有CBF不等式和输入约束。数学上就是：
min (u - u_nom)ᵀ W (u - u_nom) s.t. L_f h(x) + L_g h(x)u + αh(x) ≥ 0 u_min ≤ u ≤ u_max
W是权重矩阵，通常取单位阵。这就是标准的凸二次规划，Matlab里一个quadprog就解了。
工程上需要特别注意的是多种CBF约束并存时的可行域。状态约束、避碰约束、输入约束可能互相矛盾，导致QP在某个时刻无解。这种情况下要么引入松弛变量放宽部分CBF约束，要么调整增益，要么从控制协议层面重新设计。后面“踩坑”部分我会展开说。
3. Matlab代码实现全过程（可直接复现的仿真框架）
3.1 数学模型与仿真场景
为了让代码不脱离实际，我这里选一个双积分器模型作为被控对象：
ṗ_i = v_i v̇_i = u_i
其中p_i是位置，v_i是速度，u_i是加速度控制输入。这个模型虽然简单，但具备二阶系统的本质特征，位置约束的相对阶为2，处理方式可以直接迁移到无人机、机械臂这类系统上。
仿真场景这样设置：5个跟随者，1个虚拟领航者。领航者沿一条圆形轨迹运动，跟随者初始位置随机散布在圆心附近，初始速度为零。约束包括：
- 系统状态p和v的维度：二维平面，每个智能体状态维度为4（位置x、y和速度vx、vy）。
- 速度上限：1.5 m/s。
- 加速度（输入）界限：±3 m/s²。
- 智能体之间的最小避碰距离：0.6 m。
- 所有智能体都限制在一个矩形工作空间内。
跟踪目标：跟随者最终形成圆形编队，并跟踪虚拟领航者轨迹。
3.2 代码架构与主循环
建议的代码结构分四个函数：
nominal_control.m：计算名义一致性跟踪控制律。cbf_constraints.m：根据当前状态计算所有CBF约束和输入约束，输出A_ineq、b_ineq。solve_cbf_qp.m：调用quadprog求解最小修正控制量。dynamics_update.m：用欧拉法或四阶龙格库塔更新系统状态。
主循环示意如下：
dt = 0.01; T = 20; t = 0:dt:T; n = 5; % 状态向量 x: [px_i; py_i; vx_i; vy_i] 按智能体拼接 % 初始状态初始化 x_state = init_state(n); u_all_log = zeros(length(t), 2*n); h_log = zeros(length(t), n*(n-1)/2 + n); for k = 1:length(t) % 1. 计算名义控制量 u_nom = nominal_control(x_state, leader_pos, leader_vel, ...); % 2. 构造CBF约束和输入约束 [A_ineq, b_ineq] = cbf_constraints(x_state, params); % 3. 求解CBF-QP u_safe = solve_cbf_qp(u_nom, A_ineq, b_ineq, params); % 4. 更新状态（欧拉法） x_state = dynamics_update(x_state, u_safe, dt); % 记录数据供后处理 end这个结构的扩展性很好，后面想加入障碍物、加入更多智能体，只要改cbf_constraints函数就行。
3.3 核心函数实现细节
名义控制器可以设计成带速度阻尼的一致性跟踪协议。每个智能体i的名义加速度：
u_nom_i = -k_p * (p_i - p_leader) - k_v * (v_i - v_leader) - k_c * Σ (p_i - p_j)
其中后一项是邻居间的编队耦合项。为了突出CBF的作用，名义控制器可以先不考虑任何约束，正常设计即可。代码大致如下：
function u_nom = nominal_control(x, pL, vL, adj, kp, kv, kc) n = size(x,1)/2; % 智能体数量 u_nom = zeros(2*n,1); for i = 1:n pi = x(2*i-1:2*i); vi = x(2*n+2*i-1:2*n+2*i); % 跟踪领航者 u_nom(2*i-1:2*i) = -kp*(pi - pL) - kv*(vi - vL); % 编队耦合（简化：所有跟随者与领航者保持期望距离） for j = 1:n if adj(i,j) ~= 0 pj = x(2*j-1:2*j); u_nom(2*i-1:2*i) = u_nom(2*i-1:2*i) - kc*(pi - pj); end end end end接下来是CBF约束计算。这一步最容易出错的是相对阶处理。对速度约束，定义
h_v = v_max² - v_i'*v_i
那么L_f h_v = 0，L_g h_v = -2*v_i'，CBF不等式为：
-2*v_i'u_i + gamma_vh_v ≥ 0
等价于 -2*v_i'u_i ≥ -gamma_vh_v。写成A_ineq * u ≤ b_ineq的形式就是：
2*v_i'u_i ≤ gamma_vh_v
注意符号不要反了。这一条是CBF约束里最常出错的地方。
对避碰约束，用ECBF处理。定义
h_ij = (p_i - p_j)'*(p_i - p_j) - d_min²
则前面推导过：
L_f h_ij = 2*(p_i - p_j)'(v_i - v_j) L_f² h_ij = 2(v_i - v_j)'(v_i - v_j) L_g L_f h_ij = [2(p_i - p_j)', -2*(p_i - p_j)']
ECBF条件为：
L_f² h_ij + [2*(p_i - p_j)', -2*(p_i - p_j)'][u_i; u_j] + alpha1h_ij + alpha2*L_f h_ij ≥ 0
同样要转成A_ineq * u ≤ b_ineq形式。因为不等式里有u_i和u_j，而u_j是邻居的控制量，在集中式仿真中所有u一起求解，所以可以直接放入同一个QP里算。
cbf_constraints函数里，把所有智能体的控制量拼成一个大的决策向量u_all = [u_1; u_2; ...; u_n]，然后逐条添加约束行。
function [A_ineq, b_ineq] = cbf_constraints(x, params) n = params.n; dim = 2*n; % 决策变量维度 A_ineq = []; b_ineq = []; % 速度约束（CBF） for i = 1:n vi = x(2*n+2*i-1 : 2*n+2*i); h_v = params.v_max^2 - vi'*vi; A_i = zeros(1, dim); A_i(2*i-1:2*i) = 2*vi'; % 因为转成 <= 形式 b_i = params.gamma_v * h_v; A_ineq = [A_ineq; A_i]; b_ineq = [b_ineq; b_i]; end % 避碰约束（ECBF） for i = 1:n for j = i+1:n pi = x(2*i-1:2*i); pj = x(2*j-1:2*j); vi = x(2*n+2*i-1:2*n+2*i); vj = x(2*n+2*j-1:2*n+2*j); dp = pi - pj; dv = vi - vj; h = dp'*dp - params.d_min^2; Lfh = 2 * dp' * dv; Lf2h = 2 * dv' * dv; A_ij = zeros(1, dim); A_ij(2*i-1:2*i) = 2*dp'; A_ij(2*j-1:2*j) = -2*dp'; b_ij = -(Lf2h + params.alpha1*h + params.alpha2*Lfh); A_ineq = [A_ineq; A_ij]; b_ineq = [b_ineq; b_ij]; end end % 输入约束 A_ineq = [A_ineq; eye(dim); -eye(dim)]; b_ineq = [b_ineq; params.u_max*ones(dim,1); params.u_min*ones(dim,1)]; end然后solve_cbf_qp用quadprog求解。这里要注意，quadprog的标准型是求解最小化0.5*u'Hu + f'*u，所以目标函数中需要把名义控制量加进去：
function u_safe = solve_cbf_qp(u_nom, A_ineq, b_ineq, params) dim = length(u_nom); H = 2 * eye(dim); f = -2 * u_nom; opts = optimoptions('quadprog', 'Display', 'off', 'Algorithm', 'active-set'); [u_safe, ~, exitflag] = quadprog(H, f, A_ineq, b_ineq, [], [], [], [], [], opts); if exitflag < 0 % 说明QP无解，可以考虑加松弛变量 % 这里为了演示，退回名义控制量，但实际情况必须处理 u_safe = u_nom; end end更新状态用欧拉法就够了，dt取0.01秒，双积分器模型非常稳：
function x_new = dynamics_update(x, u, dt) n = length(u)/2; x_new = x; for i = 1:n % 位置更新 x_new(2*i-1:2*i) = x(2*i-1:2*i) + dt * x(2*n+2*i-1:2*n+2*i); % 速度更新 x_new(2*n+2*i-1:2*n+2*i) = x(2*n+2*i-1:2*n+2*i) + dt * u(2*i-1:2*i); end end整套代码跑完以后，建议画三张图：一是所有智能体的轨迹和领航者轨迹；二是控制量随时间变化曲线，观察是否触及输入边界；三是所有CBF值随时间变化曲线，确认始终大于等于0。
3.4 参数怎么选
仿真能不能跑起来，一大半靠参数。我根据自己的测试经验给一组能正常工作的初值：
- dt = 0.01 s
- 速度上限 v_max = 1.5 m/s
- 输入上限 u_max = 3 m/s²
- 避碰距离 d_min = 0.6 m
- 名义控制增益 k_p = 1.5，k_v = 2.0，k_c = 0.8
- CBF速度约束增益 gamma_v = 5
- ECBF增益 alpha1 = 4，alpha2 = 6
这里有个经验公式可以参考：对于二阶ECBF，alpha2至少要比sqrt(alpha1)大一些，否则约束边界会出现欠阻尼振荡。一般我先固定alpha1，然后从1倍sqrt(alpha1)到3倍sqrt(alpha1)之间扫，看h的曲线有没有明显穿越零值。
还有一个关键经验是采样周期和CBF增益的匹配。连续时间下的CBF条件放到离散仿真里会有半步误差，增益越大越明显。我一般会要求gamma_v * dt ≤ 0.1，alpha1 * dt² ≤ 0.01左右，这样能有效避免离散化造成的约束违反。
4. 实操过程中踩过的坑与调参技巧实录
这部分是拿代码换来的经验，希望能帮你少走点弯路。
4.1 quadprog报警“约束不一致”怎么办
第一次跑的时候大概率会遇到QP无解。最容易触发的原因是初始状态本身已经不安全：某个智能体的速度大于v_max，或者两个智能体之间的距离小于d_min。此时无论怎么求解，CBF不等式都无法满足，因为h已经是负数了。
处理办法分两步。第一步是在仿真开始前检查初始状态是否满足所有约束，不满足就重新初始化。第二步是为CBF约束引入松弛变量。具体做法是在决策变量里加一个非负的delta，CBF不等式全部变成A_cbf * u ≤ b_cbf + delta，然后在目标函数里加一个很大的惩罚项rho * delta²。这样即使某个约束暂时无法满足，QP也有可行解，系统在下一时刻会尽量把约束“拉”回安全区域。
松弛变量的惩罚系数rho我一般取100以上。太小的话，系统会习惯性走“软约束”路线，安全性没有保证；太大的话，数值上可能出现病态。我的做法是先100起步，观察delta是否长时间不为零，如果长时间不为零，说明CBF参数或者路径规划本身有问题。
4.2 CBF值在边界附近抖振
跑通以后第二个常见问题是，h的曲线贴着0附近来回抖动，视觉上“看似安全”但总觉得不踏实。这个现象多半是CBF增益过大导致的，增益越大，控制量在边界处的修正越“猛”，一旦采样步长跟不上，就容易越过边界再拉回来，形成抖振。
我的排查顺序是：先输出u_safe，看控制量是否在边界附近来回切换；再调小gamma_v和alpha1，观察h曲线是否平滑。如果还是抖，可以在CBF不等式里加一个安全裕量，把h替换成h - h_margin，例如h_margin = 0.02。这相当于人为把约束边界往里缩了一点，牺牲一点安全空间换取工程稳定性。
还有一种做法是给最终控制量加低通滤波，但这里必须谨慎。滤波会破坏CBF条件中的“当前输入直接决定导数值”的逻辑，严格意义上安全性会受影响。我的建议是优先调增益，而不是依赖滤波。
4.3 位置约束总在极端工况下失效
如果你用的是双积分器模型，而位置约束又被当成标准CBF处理，那必然失效。位置约束的相对阶是2，必须用ECBF或者高阶CBF。我在很多代码里看到过这个问题，症状是：常规情况下约束都满足，一旦智能体激烈变速或追踪机动目标，位置就突然越界，看起来像是“数值问题”，实际上是对系统相对阶理解不到位。
用ECBF时还有一个很容易出错的细节：L_g L_f h那一项，对双积分器模型来说，智能体i的输入u_i前面的系数是2*(p_i-p_j)'，智能体j的输入u_j前面的系数是-2*(p_i-p_j)'。很多人只写了u_i那部分，忽略了邻居控制量，结果集中式仿真时还好，分布式实现时安全性能明显下降。在集中式仿真的代码里，所有u一起求解，这个项必须完整写进去。
4.4 代码跑得慢怎么办
如果智能体数量比较多，quadprog在循环里反复调用会拖慢仿真速度。我的经验是几个方面优化：
第一，循环外预先分配矩阵。名义控制量、CBF矩阵、quadprog选项在循环开始前都创建好，循环内只更新与状态相关的数值内容。Matlab里动态拼接矩阵[ ]是性能杀手，一定要预分配。
第二，quadprog算法选active-set而不是默认的内点法。对规模不大的QP，active-set迭代更快，而且对热启动友好。如果你用Matlab 2023b之后的版本，还可以试试quadprog的'Algorithm','interior-point-convex'但搭配稀疏矩阵，效果也不错。
第三，如果你不需要严格在实数域仿真，也可以把dt放大到0.02甚至0.05，前提是CBF增益相应调小。工程上移动机器人控制周期一般20到100Hz，dt=0.01对应100Hz，已经是比较充裕的采样率。
4.5 CBF约束和输入约束天生冲突
第三种无解情况是CBF约束本身和输入饱和冲突。比如系统要以最大速度冲向障碍物，想停下来需要很大的反向加速度，但输入上限不够，CBF条件就永远无法满足。这不是代码问题，是物理问题。
处理方式有几个思路。第一个是提前规划路径，避免系统进入“刹车距离不足”的状态，这正是很多团队把CBF和路径规划结合起来的原因。第二个是降低运行速度上限，相当于给CBF留出更充足的执行能力裕量。第三是在某些非紧急情况下放宽部分CBF软约束，但核心的安全约束不能放松。
我还试过一个比较取巧的办法：给每个智能体设定不同的输入界限，领航者前方的智能体分配更大的加速度余量，后方的智能体分配稍小的权重。这样能缓解多约束同时冲突的情况，但需要根据任务背景具体设计，不能盲目套用。
5. 回到工程：这套方法能怎么扩展
双积分器模型下的TAC仿真只是一个起点。实际系统里无人机、机械臂、无人车的动力学模型各不相同，但CBF-QP的框架几乎是可以直接迁移的。你需要重新定义的只有f(x)、g(x)和对应的L_f、L_g导数项。只要相对阶搞清楚了，CBF的约束形式并不会因为系统变复杂而改变。
这套方法还可以和CLF结合，变成CLF-CBF-QP。具体做法是在QP目标函数里加一个关于CLF的时间导数项，要求系统在安全的同时保持收敛速度。不过CLF约束和CBF约束也可能冲突，需要引入优先级或者权重调节，这一点是另一个大话题。
从我的实际体会来说，CBF方法最大的价值不在数学证明本身，而在于给控制工程师提供了一种“把安全约束放进优化问题”的清晰范式。测试时你会发现，名义控制器可以随便设计，只要不违反物理极限，CBF-QP都能在后台帮你兜住底线。但这种兜底不是万能的，它需要一个可行的QP、合理的增益、足够的状态信息。先把双积分器模型上的TAC问题跑透，再去碰更复杂的动力学，是性价比最高的路径。
