最初让我决定复现这个方向是因为我对“二次规划”这个词特别敏感。很多做多智能体仿真的同行谈起安全控制就默认要上 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)] ]这样每个智能体都在同一圈上转但相位不同会不断产生相互接近和避碰的需求。关键安全参数如下参数符号取值说明安全距离D1.0双智能体最小中心距离扰动上界( w_m )0.1每个控制通道的最大扰动幅值CBF 衰减系数( \alpha )2.0安全约束向安全集内部收敛的速率控制周期( T_s )0.1sQP 求解频率控制限幅( u_{max} )2.0速度输入上限性能指标我用三个最小间距、跟踪误差累计值、QP 求解失败次数。最小间距用来判断安全跟踪误差累计用来判断性能求解失败次数则直接反映算法在不确定性下的可行性项。4.2 不确定性程度对 QP 可行性的影响把扰动上界从 0.05 逐步增加到 0.4观察结果会发现一个非常明显的趋势扰动上界越大CBF 约束中的 (-2w_m) 项越紧保守性越强。当 ( 2 w_m ) 已经大到接近 ( \alpha h ) 的典型值时很多本可以安全通过的场景会变成 QP 无解。我的实验里( w_m0.1 ) 时 20 秒仿真内一次求解失败都没出现( w_m0.3 ) 时开始出现零星失败主要是两个智能体距离接近临界点时最坏情况约束和跟踪目标互相冲突当 ( w_m0.5 ) 时碰撞频发因为控制器被迫大幅减速跟踪误差增大反而导致在后面追赶时与前方智能体发生队形挤压。这里给同行一个非常实用的经验鲁棒 CBF 不是把扰动上界设得越大越好。它换来的是更保守的安全边界换走的是 QP 可行域。如果你期望的扰动最大幅值是 0.4那么安全距离 D 也要随之放大。比如把 D 从 1.0 调到 1.3系统就能在 ( w_m0.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 秒采样下即使 ( D1.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 约束。如果初始时距离不足 DQP 约束本身在 t0 就不可行。这时再好的约束也无济于事。我建议仿真前增加一个距离检查如果初始状态不满足所有安全条件要么重新随机生成初始位置要么给一个较长的可达安全域过渡阶段。论文里通常只会一笔带过但在复现时这是最先卡住人的隐性条件。5. 从 TAC 复现延伸我踩过这些坑后的最终体会5.1 复现过程容易让人懵的几件事最让我意外的是所有理论推导都看起来顺理成章但真正把代码跑起来时大量的时间花在了“约束索引对齐”上。比如在集中式 QP 里所有智能体的状态拼接成一个向量约束行索引稍微错一位整个安全保证就失去意义。后来我每组装一个约束行都会同时打印出智能体编号、邻居编号和约束数值人眼核对一遍再正式进入仿真循环。另一个容易懵的地方是“鲁棒化是充分条件而非充要条件”。我们用 (-2w_m) 收紧约束得到的控制律在扰动下安全但它可能是过度保守的。如果你希望减小编程保守性就得做随机化的可行域扩大或者引入扰动估计来动态调整 (-2w_m)。我在这版复现里没做扰动估计但我把它挂在了扩展计划里。在大多数起步场景中固定上界已经足够让系统稳定而且代码结构最清晰。5.2 如果换一种不确定性处理方式如果下次再遇到类似题目我可能会尝试 tube-based MPC 的思路而不是纯 CBF 鲁棒化。Tube MPC 的基本思想是把系统分解成标称轨迹和一个在鲁棒不变集内的误差轨迹Q 在标称系统上求解同时把误差集映射成约束收紧量。这和 CBF 鲁棒化的区别在于CBF 是逐时刻约束收紧问题永远保持单步 QPTube 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 求解测量每个采样周期的平均求解耗时。如果平均耗时超过控制周期的三分之一就要考虑换求解器、减少邻居数量或者减小安全约束数量。很多安全关键系统出问题不是算法不对而是实时性没守住。把求解耗时写进实验结果里比单纯画轨迹更有说服力。
