企业官网建设流程全解析

在自动驾驶编队、仓储机器人协同、无人机集群这类多智能体场景里，“安全”和“连续控制”从来都是两难问题——你希望每个agent都完成自己的目标动作，同时又要保证它们不会互相撞上、不会冲出边界。纯靠人工给定规则处理不了动态环境，纯靠优化又容易出现险情。去年我复现了TAC上那篇关于不确定条件下安全关键多智能体系统连续控制的文章，它的核心思路是用时变控制障碍函数（CBF） + 二次规划（QP）把安全和性能目标统一成一个实时求解的连续控制信号。这篇文章从复现的角度，把里面的原理、代码结构、我踩过的坑，以及Matlab实现细节全部梳理一遍。

如果你正在做多机器人协同、自主驾驶决策或者安全控制相关研究，这篇内容应该能帮你少走不少弯路。尤其是那些从论文公式往实际代码搬时“对不上”的地方，我尽量把关键环节都展开讲透。

1. 问题拆解与整体设计思路

1.1 标题背后的核心需求

先把这个标题拆开看。所谓“安全关键型多智能体系统”，本质上就是一群有动力学约束的agent，在共享空间里同时执行任务，它们之间的交互一旦失控，轻则任务失败，重则安全问题。而“不确定性条件”才是这篇文章真正要硬刚的部分——多数控制方法在设计时假设模型精确、外界无干扰，但真实系统里状态估计有噪声、执行器有偏差、环境影响无法忽略。如果设计出来的控制器不考虑这些，安全约束会形同虚设。

“连续控制”这三个字也很关键。它不是那种离散的、规则表驱动的决策，而是每一时刻给执行层输出一个合适的控制量，频率一般在几十到几百赫兹。这里的难点在于：安全约束是非线性的，系统本身又有多个agent互相耦合，要在实时性要求下输出连续的控制信号，就需要一个既可靠又高效的计算框架。二次规划（QP）恰好同时满足了实时求解和约束表达能力，再加上CBF把安全要求转成约束不等式，这个框架就通顺了。

还有一个容易被忽视的点：系统运行过程中，安全条件本身是随时间变化的。比如两个agent之间的安全距离会随着相对速度变化而动态调整，安全边界可能是移动的。这就是“时变”CBF的切入点。

1.2 复现时的方案选型

我复现时最核心的决策是：采用CBF-CLF-QP框架。CBF处理安全约束，CLF处理任务收敛（即让系统稳定地往目标走），QP在两者之间找平衡点。这个框架最大的优势是模块化——你不需要为了加一个约束条件修改整个控制器逻辑，只需要往QP里追加一行不等式就行。

为什么不用模型预测控制（MPC）？MPC也能做约束控制，但它的计算量随预测时域、agent数量、约束个数呈指数式增长。对于多个agent需要在同一时刻完成协调控制的场景，MPC的实时性很难保证。CBF-QP是单步优化的，不展开预测时域，计算负担低很多。代价是它的“预见性”弱，只能说当前时刻保证安全，不能像MPC那样做一个长远的规划。对于系统状态变化不剧烈的场景，CBF-QP是性价比更高的选择。

Matlab作为实现平台也很合适，内置quadprog求解器，自带丰富矩阵操作接口，仿真验证的效率远高于C++和Python的配置成本。我后面的完整代码也是在Matlab环境下跑的。

复现建议：这套框架不挑系统类型，一阶积分模型、二阶积分模型、甚至非完整的轮式机器人模型都能适配，只是CBF设计的复杂度不同。

2. 理论基础与关键公式落地

2.1 控制障碍函数到底在做什么

CBF的前向不变性原理，通俗讲就是：如果系统当前在安全集合内，那么控制输入必须保证系统下一时刻不会离开这个集合。这个“安全集合”由一个连续可导函数h(x,t)的零超平面定义，要求h(x,t) ≥ 0。

对h(x,t)求导，得到：

h_dot = Lf_h(x,t) + Lg_h(x,t)·u + ∂h/∂t

其中Lf_h和Lg_h是李导数，分别表示系统漂移项和控制项对h变化率的贡献。CBF条件要求：

Lf_h(x,t) + Lg_h(x,t)·u + ∂h/∂t + α·h(x,t) ≥ 0

这里的α是一个正定的扩展函数K，通常简单取α·h(x,t) = γ·h(x,t)，γ > 0。它的作用是确保h在趋向边界时有一定的“安全阻尼”，如果h已经很小了，控制输入必须更保守，保证它不会穿过零面。

我复现第一版代码时没有理解这个α的物理意义，直接取了很小的γ，结果系统总是贴着边界走，一旦有扰动就直接跑出安全边界。后来调大γ之后，约束的响应明显变快，安全性也上来了。

2.2 时变CBF的引入

时变这个点，一开始我以为是作者为了数学完备性才加的，实际复现的时候才发现，它是解决多智能体编队和跟踪问题绕不开的需求。

举一个例子：两个agent做避碰，安全函数是h = ||p1 - p2||² - R²，这个形式本身是时不变的。但如果两个agent有各自的目标轨迹，或者说安全边界是一个跟随前方车队参考轨迹移动的区域，那h里面就天然带t，h(x,t) = ||p1(t) - p2(t)||² - R²(t)。对t求偏导就出来了。

在代码里实现时，需要把∂h/∂t这一项加到QP的约束向量里。很多复现版本容易漏掉这个偏导，导致约束在高动态场景下失效。

2.3 不确定性建模的安全鲁棒设计

这篇论文里不确定性建模的思路值得多说两句。它不要求你精确知道扰动的行为，只假设扰动是有界的，然后把这个界引入到CBF条件中。类似的设计思路在文献里叫安全鲁棒CBF（Safe-RCBF）或者输入到状态安全（ISSf）。

核心变化是在CBF条件右边增加一个“扰动补偿项”：

Lf_h + Lg_h·u + ∂h/∂t + α·h ≥ δ·||Lg_h·d|| + ε

这里的d是扰动信号，δ是它的上界，ε是一个安全裕度。这个式子直观理解就是：既然扰动可能会把h往危险方向推，我就在约束里预留出一截余量。

我在Matlab里实际实现的时候，是把扰动直接注入到动力学模型中：

% 系统动力学，x_dot = f(x) + g(x)*u + d function xdot = dynamics(x, u, d, params) xdot = params.f(x) + params.g(x) * u + d; end

然后在CBF的设计中直接考虑d的界，这样就实现了“扰动存在也不怕”的效果。

3. 多智能体系统建模与约束设计

3.1 agent动力学模型选择

复现时我用了二维平面内的双积分模型，即每个agent的状态为x = [px; py; vx; vy]，控制输入u = [ax; ay]。这个模型的好处是：既能体现惯性和加速度限制（对安全控制至关重要），又不至于让CBF的解析表达式变得过于复杂。

双积分模型的矩阵形式是：

% 单个agent的状态更新，A是状态矩阵，B是输入矩阵 A = [eye(2), zeros(2); zeros(2), eye(2)]; % 这里需要按状态排列改写 B = [zeros(2); eye(2)];

注意这里的排列方式会影响后续所有矩阵维度，我建议一上来就把状态排列写清楚：x(1) = px, x(2) = py, x(3) = vx, x(4) = vy。这样CBF的李导数计算才不容易乱。

3.2 多agent避碰约束设计

对两个agent i和j，定义安全函数：

h_ij(t) = ||p_i(t) - p_j(t)||² - R_ij²(t)

R_ij(t)是两个agent之间的最小允许距离，它可以是时变的——比如相对速度大的时候，要求间距更大。这个函数对系统状态求导时，注意要分别对agent i和agent j的状态求偏导再合并成完整的行向量。

实际代码里，我维护一个CBF行向量和一个约束边界向量。每个agent的解算器只需要感知邻居agent的状态，就能计算出自己的安全约束：

% 计算当前agent与邻居j之间的距离和距离的导数 h = (px_i - px_j)^2 + (py_i - py_j)^2 - R_ij^2; Lf_h = 2*(px_i-px_j)*(-vx_j) + 2*(py_i-py_j)*(-vy_j); % 注意只含邻居速度项 Lg_h = [2*(px_i-px_j)*dt, 2*(py_i-py_j)*dt]; % 假设离散时间步长dt

在连续时间模型中Lg_h不需要乘dt，我这里只是示意了离散化的写法，实际公式推导要回到连续时间形式。

3.3 编队保持与目标跟踪约束

多智能体系统除了避碰，还要完成任务。比如某一组agent需要保持特定的几何队形，或者每个agent需要到指定目的地。这个需求可以建模成另一个CBF或CLF约束。

我复现时，为每个agent定义了一个“目标跟踪CLF”：

V_i(t) = ||p_i(t) - p_des_i(t)||² + ||v_i(t) - v_des_i(t)||²

最优控制就是让V尽快收敛到零，而CLF条件则保证每一时刻它都在收缩：

Lf_V + Lg_V·u + γ·V ≤ δ

这里δ是松弛变量，它的存在很关键——因为安全约束、控制输入极限和目标收敛之间可能不存在同时满足的解，必须允许任务目标略微让步，保证安全约束不被牺牲。δ会被QP的代价函数惩罚，但权重一般不会设得太大，否则为了追目标，安全约束会被压缩到极限。

4. QP求解器配置与Matlab代码实现

4.1 QP问题形式化

在每个控制周期内，我们需要求解下面的优化问题：

minimize (u - u_ref)'·W·(u - u_ref) + ρ·δ²

subject to CBF安全约束不等式 CLF收敛约束不等式 u_min ≤ u ≤ u_max

这里的决策变量z = [u; δ]。W是控制输入权重矩阵，ρ是松弛变量惩罚系数，u_ref是一个理想控制输入（比如参考轨迹给出的前馈控制）。

QP的标准形式是：

min 1/2·z'·H·z + f'·z s.t. A·z ≤ b

对照一下：H = diag([2W, 2ρ])，f = [-2*W·u_ref; 0]，不等式约束矩阵A则是把所有CBF和CLF的行向量叠在一起。

4.2 使用quadprog求解

Matlab自带的quadprog是求解中规模QP的良好选择，特别是对一百个左右的变量，速度完全够。我在代码里这样配置求解器：

options = optimoptions('quadprog', ... 'Algorithm', 'interior-point-convex', ... 'Display', 'off', ... 'OptimalityTolerance', 1e-6, ... 'ConstraintTolerance', 1e-6); % 组装QP矩阵 H = blkdiag(2*W, 2*rho); f = [-2*W*u_ref; 0]; A = [A_cbf; A_clf; A_control]; b = [b_cbf; b_clf; b_control]; % 注意：quadprog的输入lb和ub需要包含δ变量 lb = [u_min; 0]; ub = [u_max; 1e3]; % 求解 z = quadprog(H, f, A, b, [], [], lb, ub, [], options); u = z(1:dim_u);

这里有一个细节容易踩坑：如果你的CBF约束矩阵比较大，建议用稀疏矩阵存储A，quadprog在稀疏模式下会快很多。我在复现时因为状态不超过十个，直接用稠密矩阵问题不大，但如果你要扩展到二十个agent以上，稀疏化就不是可选项，而是必选项。

4.3 主循环与实时控制流程

整体控制器的运行逻辑可以浓缩成一个主循环，每个周期执行以下步骤：

1. 获取当前系统状态（含邻居状态）。
2. 根据当前状态和时变参数，计算h值、李导数、以及CLF相关量。
3. 组装QP矩阵。
4. 调用quadprog求解。
5. 将控制输入施加入系统。
6. 使用四阶Runge-Kutta或离散化方法更新系统状态。

我在复现时采用了0.05秒的控制周期，系统的动态响应速度足够覆盖编队和避碰任务的需求。

4.4 不确定性扰动的注入方式

为了验证控制器的鲁棒性，我给每个agent加入了随时间变化的有界扰动：

% 有界扰动生成函数 d = @(t, i) 0.2 * [sin(2*t + i); cos(1.5*t + 2*i)]; % 有界且agent间不同相

扰动界取0.2，在CBF约束中设置相应的安全裕度。我做了三组对照实验：无扰动、有扰动但无补偿、有扰动且补偿，结果差异非常明显。无补偿版本在扰动较强时，会出现某两个agent距离低于安全阈值的瞬时事件，而补偿版始终把约束维持在安全区间内。

5. 实际过程中的参数调节与代码调试

5.1 核心参数初期取值与调整方向

以下是我复现时使用的一组能够仿真实跑的初始参数：

这些参数不是独立的。CBF的γ和CLF的γ之间相互作用很大，我尝试过CBF γ = 8、CLF γ = 1的组合，效果最好，既能保守应对突发距离变化，又能保持目标跟踪的快速性。而如果反过来设CBF γ = 1、CLF γ = 8，系统就会被任务欲望带着跑，安全边界像橡皮筋一样被拉扯。

5.2 遇到不可行解的处理方式

QP的CBF约束如果在极端状态下互相矛盾（比如两个agent已经极近且速度相对很大），会出现无解。为了防止程序崩溃，我在每个CBF约束中都加了松弛变量，并且在QP里显式加入“软约束”概念：

% CBF约束变软：左边加松弛项 A_cbf = [-Lg_h, 1]; % 最后一列对应松弛变量 b_cbf = Lf_h + alpha*h;

这样一来，即使安全条件无法严格满足，也能得到最小违约的控制输入。实际运行中，只要参数不是特别离谱，短时间内极少触发松弛项，但它给了整个系统一个“紧急逃生通道”。

5.3 数值稳定性与求解精度

Matlab的quadprog在这种小规模问题上数值稳定性很好，但我碰到一个比较隐蔽的问题：当h值非常小（接近0）且 α·h 项的量级在10⁻⁶以下时，约束右侧几乎为零，求解器给出的控制输入会出现抖动。解决方案是给h做一个小的偏移修正，在CBF中增加一个安全垫：

h_safe = h + safety_margin; % 取0.05的安全垫

这样类似于“不等到撞上才刹车，而是提前50厘米开始减速”，规避了零边界附近的数值振荡。

6. 常见问题与排查思路

6.1 高频报错盘点

6.2 排查方法

我自己调试时最有效的套路是先单agent、后多agent。先用一个agent跑通CBF-CLF-QP的完整逻辑，验证目标到达和边界约束，再扩展成两个agent做避碰，最后才上全体agent的编队协同。每个阶段保留一个独立的测试脚本，功能回归时很方便定位是哪个环节出了偏差。

有一个小技巧：把CBF的h值实时打印出来。不仅看它是否保持非负，还要看它的变化率是不是平稳。如果h值频繁地剧烈碰撞0边界，说明α或安全裕度不足，需要调节参数而不是继续调求解器。

6.3 扩展经验

这个框架后续扩展的空间其实很大。我自己的下一步想法是接入更复杂的系统动力学，比如轮式机器人非完整约束、无人机的六自由度模型。CBF的设计会复杂很多，需要精确处理非完整约束，但QP框架本身不用动。

如果对实时性要求进一步提升，可以考虑用OSQP或ECOS替换quadprog，它们在嵌入式环境中的性能更优。Matlab做验证没问题，但放到实际机器人上还是需要一个更轻量的求解器。

最后再分享一个小技巧：我复现过程中最大的收获，是意识到CBF-QP本质上不是一个“控制器设计问题”，而是一个“约束设计问题”。你花在定义h函数、建立多agent约束关系上的时间，远多于调求解器的时间。把安全集合的形式、时变参数的表达式、扰动界的估计这三件事想清楚了，整个框架自然就能转起来。如果你也在复现这篇文章或者类似的方法，希望这份笔记能让你少折腾几天。