企业官网建设流程全解析

最初让我决定复现这个方向，是因为我对“二次规划”这个词特别敏感。很多做多智能体仿真的同行，谈起安全控制就默认要上 MPC、混合整数规划，或者直接端到端强化学习，而实际上有一大类安全关键系统，只要把控制目标和约束认真写成二次规划，在采样周期内解出来，就已经能跑出非常稳定的效果。这篇来自 TAC 的方法研究，主线正是在“不确定性条件”下处理“安全关键型多智能体系统”的“连续控制”问题，并且最终落到了一套能直接在 Matlab 里复现的代码实现。我复现完整个框架后最大的感受是：真正难的不是 QP 本身，而是怎么把安全约束、扰动模型和连续控制要求捏合成一个可在每个采样周期内求解的 QP，以及踩完论文没写出来的那些坑之后，还能让系统在扰动下稳定工作。

这篇文章我会从项目定位、方法拆解、Matlab 复现路径、实验验证和参数敏感性这几个方面，把我完整跑通一遍的过程记录下来。对正在复现同行论文、或者想把安全关键多智能体控制落地仿真代码的读者，应该能省下不少弯路。

1. 为什么选它来复现：从“二次规划”这个关键字开始的项目定位

1.1 复现这类论文的真实价值

安全关键型多智能体系统，说白了就是多个移动单元同时在一个环境里执行任务，并且任何时刻都不能发生碰撞、不能越界、不能失稳。智能体数量一多，约束之间互相耦合，直接手工设计控制律会非常痛苦。而把这套约束写成数学规划，让求解器在每个控制周期内算出一个满足安全条件同时又尽量跟踪性能的控制输入，是工程上最朴素也最可靠的做法之一。

我选择复现这个方法，还有一层原因是它把“连续控制”和“二次规划”放在了一起。很多教材里连续控制是微分方程加李雅普诺夫函数，优化控制则偏向离散采样下的规划问题。TAC 这篇文章处理的其实是“连续时间系统的采样控制实现”：系统本身是连续的，控制器在采样时刻解一个 QP，然后把求出的控制输入以零阶保持的方式作用到连续系统上。复现时如果把这个采样逻辑搞错了，后面所有实验都会乱套。

1.2 标题里藏着哪些必须啃下的技术点

一个标题往往就是一篇论文的索引。我在复现前先把标题拆成了几个词，逐个确认代码里要对应什么：

• 不确定性条件：系统受到未知扰动，或者模型参数不精确。仿真里我用的是有界扰动，即每一项控制输入上叠加一个幅值受限的外力扰动。
• 安全关键型多智能体系统：多个智能体之间存在避碰约束，同时还要避开障碍物、保持在地图边界范围内。
• 连续控制：控制器输出的是一个连续变化的速度或加速度指令，而不是离散的模态切换逻辑。
• 二次规划方法：每个采样周期内求解一个目标函数为二次、约束为线性不等式的问题，最终返回控制输入。

这些词不是并列关系，而是一条完整的逻辑链：因为系统存在不确定性，所以安全约束不能只按标称模型去设计；因为系统需要连续安全控制，所以不能等避碰已经失效再做干预，而要每步都解带约束的 QP；因为多智能体规模可能增长，所以目标函数和约束要尽量保持线性和凸性，这样 QP 求解才快，实时性才有着落。

1.3 我最终划定的方法路线：CBF + QP + 鲁棒紧致

看完整套思想后，我划定的复现路线非常明确：用控制障碍函数（CBF）把安全约束表达成一阶线性不等式，再把有界扰动投影到 CBF 梯度方向上做最坏情况鲁棒化，最后把跟踪性能目标写成二次型，合并成一个标准 QP。这个方法既保留 CBF 的安全性保证，又没引入非线性规划，求解稳定性很高，特别适合复现代码。

这条路线和我过去见过的“软约束惩罚”思路有本质区别。软约束是让系统尽可能不碰撞，允许碰撞但扣分；CBF 则是把安全集设计成控制不变集，只要约束满足，系统轨迹就不会进入危险区域。虽然后者在强扰动下可能无解，但在本文的鲁棒化框架里，我们可以通过调整安全阈值和扰动上界来换取可行性。理解了这一点，后面看 Matlab 代码时就不会被一堆矩阵吓到。

2. 方法核心拆解：不确定性建模与 QP 控制器设计

2.1 系统模型与不确定性来源

复现时我把每个智能体建模成最简单的单积分器模型：

[ \dot{p}_i = u_i + d_i ]

其中 ( p_i \in \mathbb{R}^2 ) 是位置坐标，( u_i \in \mathbb{R}^2 ) 是控制输入，( d_i ) 是扰动。很多全向移动机器人的速度层、无人机水平位置层的控制，都可以近似成这个形式。选单积分器不是为了偷懒，而是为了让 CBF 约束可以只用位置及其一阶导数完成，这样 QP 的表达非常干净，复现重点全部落在“不确定性处理”和“QP 组装”上。

不确定性来源我分了三个层次：

• 外部扰动：仿真中用幅值有界的随机信号或正弦信号模拟风、摩擦、测量噪声。
• 参数扰动：智能体的执行器增益偏离标称值，导致实际实际输入为 ((1+\delta)u_i)。
• 邻居状态误差：分布式控制里，每个智能体获得的邻居位置和速度可能来自旧的通信包，存在信息延迟。

其中外部扰动是导致安全约束破坏的主要因素。对扰动 ( d_i )，我假设它属于一个已知的有界集合 ( |d_i| \le w_m )。这个假设在工程上是常见且合理的，尤其是对低速地面机器人或室内无人机，扰动能量可以被估算出来。

2.2 安全约束与控制障碍函数

安全关键系统的核心是“约束不能违反”。我定义智能体 (i) 与智能体 (k) 之间的安全距离为 ( D )，那么安全集为：

[ h_{ik}(p) = |p_i - p_k|_2 - D \ge 0 ]

这里用了欧几里得距离，不是距离的平方。这样 h 对位置的梯度是单位向量，几何意义非常直观，鲁棒化时得到的扰动投影也有明确的“最坏情况方向”。

为了保证 ( h_{ik}(t) \ge 0 ) 对任意时刻成立，CBF 方法要求：

[ \dot{h}{ik} + \alpha h{ik} \ge 0 ]

其中 ( \alpha > 0 ) 是衰减系数。这个不等式本质上是一个关于控制输入的线性约束。对上述单积分器模型展开：

[ \dot{h}_{ik} = \frac{(p_i-p_k)^T}{|p_i-p_k|} (u_i - u_k + d_i - d_k) ]

可以看到，如果不考虑扰动，那么只有 ( u_i ) 是决策变量，( u_k ) 可以是上一采样时刻的已知量，整个不等式是线性的，放进 QP 非常自然。

2.3 鲁棒化处理：从 CBF 到线性不等式的推导

真正的麻烦在扰动项 ( d_i - d_k )。它会影响约束的满足。我采用的鲁棒化思路是“最坏情况投影”：不管扰动 ( d_i, d_k ) 取什么值，只要约束在扰动最不利时也成立，那么在任意实际扰动下也成立。

由于 ( h_{ik} ) 的梯度是单位向量，扰动差 ( d_i - d_k ) 在这个梯度方向上的投影最大就是 ( |d_i - d_k| \le 2 w_m )。因此安全约束在所有可能扰动下成立的充分条件是：

[ \frac{(p_i-p_k)^T}{|p_i-p_k|}(u_i - u_k) - 2 w_m \ge -\alpha h_{ik} ]

整理后：

[ -\frac{(p_i-p_k)^T}{|p_i-p_k|} u_i \le -\frac{(p_i-p_k)^T}{|p_i-p_k|} u_k - 2 w_m + \alpha h_{ik} ]

这个形式完美匹配 QP 的线性不等式约束 ( A_{ineq} u_i \le b_{ineq} )。其中 ( u_k ) 取邻居在上一控制周期实际发出的控制输入。如果系统通信延迟更大，也可以在取邻居状态时把扰动上界相应放大。

对障碍物约束也做同样的处理。圆形障碍物 ( o )，安全距离为 ( R )，则 ( h_{io}(p) = |p_i - p_o| - R )，梯度方向是智能体指向障碍中心的反方向，鲁棒化处理后同样得到线性不等式。

2.4 单智能体 QP 的完整列式

每个采样周期，智能体 ( i ) 需要求解如下 QP：

[ \min_{u_i} \quad |u_i - u_{ref,i}|^2 ]

[ \text{s.t.} \quad A_{ik} u_i \le b_{ik},\quad \forall k \in \mathcal{N}_i ]

[ \quad A_{io} u_i \le b_{io},\quad \forall o \in \mathcal{O} ]

[ \quad u_{min} \le u_i \le u_{max} ]

这里 ( u_{ref,i} ) 来自上层路径跟踪控制器，比如跟踪一个参考轨迹时，( u_{ref,i} ) 就是参考速度。目标函数取跟踪误差平方，展开后写成 QP 标准形式：

[ \frac{1}{2} u_i^T H u_i + f^T u_i ]

其中 ( H = 2 I_2 )，( f = -2 u_{ref,i} )。为什么 H 这么简单？因为我们当前只考虑纯速度控制。如果后续要扩展到加速度控制，只需要把决策变量换成加速度，并在模型中叠加速度状态，H 就会变成包含质量和阻尼的矩阵。

这个 QP 的约束数量取决于邻居数量和障碍物数量。在典型仿真里，一个智能体的邻居不超过 5 个，障碍物不超过 3 个，所以决策变量只有 2 维，约束不到 10 行，quadprog 求解非常快。

3. Matlab 复现的具体路径：从仿真环境到 QP 求解器对接

3.1 代码目录与模块设计

复现代码我建议按照下面结构组织，思路清晰，也方便后续改造：

project_root/ simulate.m % 主仿真脚本 dynamics.m % 系统动力学与扰动生成 controller.m % 核心QP控制器 plot_results.m % 画轨迹和距离曲线 config.m % 参数配置文件

simulate.m里跑时间循环，每个采样周期调用controller.m求解 QP，然后把控制输出传给dynamics.m做积分。这样把问题拆开以后，调试某一层就不会牵动整条链路。

主循环里最容易被忽略的是“零阶保持”的语义。系统是连续的，控制器是离散采样，所以每个采样周期求出的 ( u_i ) 要一直保持到下一个采样时刻，而不是每个仿真步长重新解一次。如果你的 ODE 积分器内部步长是 0.001 秒，而控制器采样周期是 0.1 秒，那么控制输入需要在 0.1 秒区间内恒定不变。

3.2 核心函数：控制矩阵 H 和线性项 f 怎么写

先用 Matlab 实现最基础的 QP 目标函数。假设决策变量是二维速度控制量，目标是对参考速度的跟踪误差最小化：

function H = buildHessian() % 目标函数为 ||u - u_ref||^2 % 展开: u^T u - 2 u_ref^T u + const % 对应 quadprog: 0.5*u'*H*u + f'*u H = 2 * eye(2); end function f = buildLinearTerm(u_ref) f = -2 * u_ref; end

如果你希望同时抑制过大控制量，可以改成 ( |u - u_{ref}|^2 + \lambda |u|^2 )，对应的 H 和 f 会自动变化：

[ H = 2(1+\lambda) I_2, \quad f = -2 u_{ref} ]

实际调试时会发现，( \lambda ) 取太大，控制器会变得“懒”，安全约束会频繁激活；取太小，跟踪速度很快，但控制量可能超限。后面第五节会讲参数敏感性。

3.3 鲁棒 CBF 约束矩阵的组装

这是整个复现里最容易写错的一步。以单个邻居 ( k ) 为例，根据推导出的线性不等式：

function [A_row, b_val] = robustCBFConstraint(p_i, p_k, u_k_prev, D, wm, alpha) dp = p_i - p_k; dist = norm(dp); if dist < 1e-6 dist = 1e-6; end grad = dp / dist; % 从 k 指向 i 的单位方向 h = dist - D; % grad'*(u_i - u_k_prev) - 2*wm >= -alpha*h % 转化为 -grad'*u_i <= -grad'*u_k_prev - 2*wm + alpha*h A_row = -grad'; b_val = -grad' * u_k_prev - 2 * wm + alpha * h; end

然后把所有邻居、障碍物的约束行堆叠进去：

function u = controller(p_i, p_neighbors, u_neighbors_prev, p_obs, u_ref_i, params) H = buildHessian(); f = buildLinearTerm(u_ref_i); A = []; b = []; % 邻居避碰 for k = 1:size(p_neighbors, 1) [A_row, b_val] = robustCBFConstraint(p_i, p_neighbors(k,:), ... u_neighbors_prev(k,:)', params.D, params.wm, params.alpha); A = [A; A_row]; b = [b; b_val]; end % 障碍物避碰 for o = 1:size(p_obs, 1) [A_row, b_val] = robustCBFConstraint(p_i, p_obs(o,:), ... [0 0]', params.R, params.wm, params.alpha); A = [A; A_row]; b = [b; b_val]; end % 控制量限幅 lb = -params.u_max * ones(2,1); ub = params.u_max * ones(2,1); % 求解QP options = optimoptions('quadprog', ... 'Display', 'none', ... 'Algorithm', 'interior-point-convex'); u = quadprog(H, f, A, b, [], [], lb, ub, [], options); % 如果QP不可行，回退到上一时刻控制量 if ~isempty(u) u_sol = u; else warning('QP infeasible at time step, using previous input'); u_sol = params.prev_u; end params.prev_u = u_sol; end

这里有个细节很多人会踩坑：障碍物处理时我把障碍物看作静止智能体，它的“控制输入”为零。这样可以直接复用同一个 CBF 约束函数。如果障碍物是移动的，就还得在约束里加上障碍物速度投影，鲁棒界也要相应修改。

3.4 quadprog 与其他求解器的切换

quadprog是 Matlab 自带的 QP 求解器，复现时最省事。但如果你要做大规模多智能体仿真，比如 50、100 个智能体，集中式 QP 的变量和约束数量会迅速膨胀，interior-point-convex算法可能显得慢。我建议切换到 OSQP 这类基于 ADMM 的嵌入式求解器。

在 Matlab 里接 OSQP 的方式有两种：一是直接编译官方提供的 MEX 接口，二是通过OSQP的 Matlab 类接口。切换时只需要把控制器里的求解调用替换成：

prob = osqp; prob.setup(H_sparse, f, A_sparse, b_minus_inf, b, zeros(n,1), ub); result = prob.solve();

注意 OSQP 需要稀疏矩阵格式。很多第一次接的人会在这里报内存错误，因为 Matlab 稀疏矩阵默认是列压缩，而 quadprog 接受稠密小矩阵也能工作。为了后续扩展，我建议从一开始就把 H 和 A 写成sparse形式。

比较两个求解器会发现，在维数低于 10 的本地 QP 上两者差距不大；但到集中式、约束上百条时，OSQP 往往更稳。论文复现阶段用 quadprog 完全够，因为重点在验证算法逻辑。

3.5 仿真主循环与数据记录

主循环我建议这样设计：先配置参数，再用小步长 ODE 积分器推进系统，在每个控制器采样时刻收集状态并调用控制器。

dt_ode = 0.001; dt_ctrl = 0.1; t_final = 20; t = 0; p = ini_positions; u_prev = zeros(2, N); p_history = zeros(N, 2, round(t_final/dt_ode) + 1); step = 1; while t < t_final % 每个控制周期求解QP if abs(t - round(t/dt_ctrl)*dt_ctrl) < dt_ode/2 for i = 1:N u(:,i) = controller(p(i,:)', neighbors, u_prev, obstacles, ref_speed, params); end u_prev = u; end % ODE积分：零阶保持控制量 for i = 1:N d = generateDisturbance(t, wm); p_dot = u(:,i) + d; p(i,:) = p(i,:) + p_dot * dt_ode; end p_history(:,:,step) = p; step = step + 1; t = t + dt_ode; end

我建议把仿真用的 ODE 步长设置成 1ms，控制周期设置成 100ms。这样既能看清连续系统在零阶保持控制下的动态，又不至于把 QP 求解频率提到过高。高控制频率下如果 CBF 的 ( \alpha ) 调得太大，系统会像控制工程里常见的“抖动”一样，每个周期都在安全边界上来回震荡，后面实验部分会专门讲。

4. 实验验证与参数敏感性：那些论文不会写明的坑

4.1 仿真场景设定与性能指标

我的标准实验场景是 5 个智能体在二维平面内执行追踪任务，地图范围 ( [-5,5] \times [-5,5] )，两个固定圆形障碍物。初始位置随机但不互相重叠，参考轨迹是圆周运动：

[ p_{ref,i}(t) = p_{center,i} + r_i [\cos(0.1 t + \phi_i), \sin(0.1 t + \phi_i)] ]

这样每个智能体都在同一圈上转但相位不同，会不断产生相互接近和避碰的需求。关键安全参数如下：

性能指标我用三个：最小间距、跟踪误差累计值、QP 求解失败次数。最小间距用来判断安全；跟踪误差累计用来判断性能；求解失败次数则直接反映算法在不确定性下的可行性项。

4.2 不确定性程度对 QP 可行性的影响

把扰动上界从 0.05 逐步增加到 0.4，观察结果会发现一个非常明显的趋势：扰动上界越大，CBF 约束中的 (-2w_m) 项越紧，保守性越强。当 ( 2 w_m ) 已经大到接近 ( \alpha h ) 的典型值时，很多本可以安全通过的场景会变成 QP 无解。

我的实验里，( w_m=0.1 ) 时 20 秒仿真内一次求解失败都没出现；( w_m=0.3 ) 时开始出现零星失败，主要是两个智能体距离接近临界点时，最坏情况约束和跟踪目标互相冲突；当 ( w_m=0.5 ) 时，碰撞频发，因为控制器被迫大幅减速，跟踪误差增大，反而导致在后面追赶时与前方智能体发生队形挤压。

这里给同行一个非常实用的经验：鲁棒 CBF 不是把扰动上界设得越大越好。它换来的是更保守的安全边界，换走的是 QP 可行域。如果你期望的扰动最大幅值是 0.4，那么安全距离 D 也要随之放大。比如把 D 从 1.0 调到 1.3，系统就能在 ( w_m=0.4 ) 下保持几乎零失败。这背后的逻辑是：鲁棒化没有免费午餐，安全指标越紧，可用控制权限越小，系统越可能不满足动态可行性。

4.3 调参经验：权重、安全阈值与采样周期

参数调优过程中我总结出一个相对可靠的调参顺序：先固定 D 和 ( \alpha )，调整 QP 目标函数里的跟踪权重；再固定权重，调整安全距离；最后再来调采样周期。千万不要一上来就调 ( \alpha )，因为 ( \alpha ) 和采样周期的耦合最复杂。

( \alpha ) 的物理意义是安全边界收敛回安全集内部的速率。( \alpha ) 太大，约束会非常激进，要求系统立刻改变方向远离危险区，但离散采样下很难精确满足，容易导致振荡；( \alpha ) 太小，系统可能长时间停留在距离安全边界很近的地方，一旦扰动连续往危险方向推，控制器反应不够快，最终还是碰撞。我的经验值是 ( \alpha ) 与采样周期的乘积 ( \alpha T_s ) 控制在 0.2 到 0.5 之间。( T_s = 0.1 ) 时，( \alpha ) 取 2 到 5 附近比较合理。

采样周期对系统性能影响也很大。我在一组测试中把 ( T_s ) 从 0.05 改到 0.5，结果 0.5 秒采样下即使 ( D=1.0 )，系统也频频出现最小间距掉到 0.7 左右的危险情况。原因很简单：约束只在采样时刻被验证，而连续系统的轨迹在采样间隔内自由演化，零阶保持的延迟会破坏 CBF 不变性。这也解释了为什么很多 CBF 论文都强调采样定理时间间隔要足够小。

4.4 典型失败案例与处理方案

第一个失败案例是 QP 无解后我采用“保持上一时刻控制量”的后果。当两个智能体距离很近、参考速度又指向彼此时，QP 无解，保持上一时刻可能仍然指向对方，导致距离继续缩小，下一周期约束系数更差，陷入恶性循环。我后来把 fallback 改成“只解决安全 CBF 约束的最小范数安全控制”，具体做法是额外构建一个只包含安全约束的 QP，目标函数换成 ( |u_i|^2 )，然后取最优解。这样即使跟踪性能被牺牲，至少能保证约束可满足。

第二个失败案例是邻居控制输入的通信噪声。我用上一时刻的 ( u_k ) 作为约束里的已知量，但如果通信包丢失，应该把丢失时的扰动上界放大。我在代码里维护了一个通信超时计数器，如果超过 0.5 秒没收到邻居信息，就把对方的 ( u_k ) 当作零，同时把鲁棒界从 ( 2w_m ) 提升到 ( 2 w_m + \epsilon )。这个处理让系统在突发丢包下依然能基本保持安全。

第三个失败案例是初始状态不满足 CBF 约束。如果初始时距离不足 D，QP 约束本身在 t=0 就不可行。这时再好的约束也无济于事。我建议仿真前增加一个距离检查，如果初始状态不满足所有安全条件，要么重新随机生成初始位置，要么给一个较长的可达安全域过渡阶段。论文里通常只会一笔带过，但在复现时这是最先卡住人的隐性条件。

5. 从 TAC 复现延伸：我踩过这些坑后的最终体会

5.1 复现过程容易让人懵的几件事

最让我意外的是，所有理论推导都看起来顺理成章，但真正把代码跑起来时，大量的时间花在了“约束索引对齐”上。比如在集中式 QP 里，所有智能体的状态拼接成一个向量，约束行索引稍微错一位，整个安全保证就失去意义。后来我每组装一个约束行，都会同时打印出智能体编号、邻居编号和约束数值，人眼核对一遍再正式进入仿真循环。

另一个容易懵的地方是“鲁棒化是充分条件而非充要条件”。我们用 (-2w_m) 收紧约束，得到的控制律在扰动下安全，但它可能是过度保守的。如果你希望减小编程保守性，就得做随机化的可行域扩大，或者引入扰动估计来动态调整 (-2w_m)。我在这版复现里没做扰动估计，但我把它挂在了扩展计划里。在大多数起步场景中，固定上界已经足够让系统稳定，而且代码结构最清晰。

5.2 如果换一种不确定性处理方式

如果下次再遇到类似题目，我可能会尝试 tube-based MPC 的思路，而不是纯 CBF 鲁棒化。Tube MPC 的基本思想是把系统分解成标称轨迹和一个在鲁棒不变集内的误差轨迹，Q 在标称系统上求解，同时把误差集映射成约束收紧量。这和 CBF 鲁棒化的区别在于：CBF 是逐时刻约束收紧，问题永远保持单步 QP；Tube MPC 则要考虑一个预测时域，通常需要滚动优化。前者更轻快，后者有更强的预测能力，尤其在强扰动下避障表现可能更好。

但 TAC 这篇文章选择 QP 而不是 MPC，一定是有道理的。对于连续控制的高速场景，求解时延是一个硬约束，MPC 即使做凸优化，时域带来的变量数量也比单步 QP 要大不少。复现时把两种思路都放在心里，遇到具体问题才能知道用哪个。

5.3 后续可扩展的方向

这套代码扩展性非常好。你只需要改dynamics.m和controller.m，就能把系统扩展到加速度控制，把单积分器换成双积分器，把 2D 变成 3D。CBF 的高阶形式对应的高阶 QP 也可以通过增设虚拟输入来实现。我在复现后顺手扩展了一版双积分器模型，决策变量从 2 维变成 4 维，H 矩阵从 ( 2I_2 ) 变成对角块矩阵，其余框架完全没动。

最后再分享一个小技巧，数控制器的求解时间。在 Matlab 里用tic/toc包裹 QP 求解，测量每个采样周期的平均求解耗时。如果平均耗时超过控制周期的三分之一，就要考虑换求解器、减少邻居数量或者减小安全约束数量。很多安全关键系统出问题，不是算法不对，而是实时性没守住。把求解耗时写进实验结果里，比单纯画轨迹更有说服力。