在自动驾驶编队、仓储机器人协同、无人机集群这类多智能体场景里“安全”和“连续控制”从来都是两难问题——你希望每个agent都完成自己的目标动作同时又要保证它们不会互相撞上、不会冲出边界。纯靠人工给定规则处理不了动态环境纯靠优化又容易出现险情。去年我复现了TAC上那篇关于不确定条件下安全关键多智能体系统连续控制的文章它的核心思路是用时变控制障碍函数CBF 二次规划QP把安全和性能目标统一成一个实时求解的连续控制信号。这篇文章从复现的角度把里面的原理、代码结构、我踩过的坑以及Matlab实现细节全部梳理一遍。如果你正在做多机器人协同、自主驾驶决策或者安全控制相关研究这篇内容应该能帮你少走不少弯路。尤其是那些从论文公式往实际代码搬时“对不上”的地方我尽量把关键环节都展开讲透。1. 问题拆解与整体设计思路1.1 标题背后的核心需求先把这个标题拆开看。所谓“安全关键型多智能体系统”本质上就是一群有动力学约束的agent在共享空间里同时执行任务它们之间的交互一旦失控轻则任务失败重则安全问题。而“不确定性条件”才是这篇文章真正要硬刚的部分——多数控制方法在设计时假设模型精确、外界无干扰但真实系统里状态估计有噪声、执行器有偏差、环境影响无法忽略。如果设计出来的控制器不考虑这些安全约束会形同虚设。“连续控制”这三个字也很关键。它不是那种离散的、规则表驱动的决策而是每一时刻给执行层输出一个合适的控制量频率一般在几十到几百赫兹。这里的难点在于安全约束是非线性的系统本身又有多个agent互相耦合要在实时性要求下输出连续的控制信号就需要一个既可靠又高效的计算框架。二次规划QP恰好同时满足了实时求解和约束表达能力再加上CBF把安全要求转成约束不等式这个框架就通顺了。还有一个容易被忽视的点系统运行过程中安全条件本身是随时间变化的。比如两个agent之间的安全距离会随着相对速度变化而动态调整安全边界可能是移动的。这就是“时变”CBF的切入点。1.2 复现时的方案选型我复现时最核心的决策是采用CBF-CLF-QP框架。CBF处理安全约束CLF处理任务收敛即让系统稳定地往目标走QP在两者之间找平衡点。这个框架最大的优势是模块化——你不需要为了加一个约束条件修改整个控制器逻辑只需要往QP里追加一行不等式就行。为什么不用模型预测控制MPCMPC也能做约束控制但它的计算量随预测时域、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里面就天然带th(x,t) ||p1(t) - p2(t)||² - R²(t)。对t求偏导就出来了。在代码里实现时需要把∂h/∂t这一项加到QP的约束向量里。很多复现版本容易漏掉这个偏导导致约束在高动态场景下失效。2.3 不确定性建模的安全鲁棒设计这篇论文里不确定性建模的思路值得多说两句。它不要求你精确知道扰动的行为只假设扰动是有界的然后把这个界引入到CBF条件中。类似的设计思路在文献里叫安全鲁棒CBFSafe-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约束矩阵比较大建议用稀疏矩阵存储Aquadprog在稀疏模式下会快很多。我在复现时因为状态不超过十个直接用稠密矩阵问题不大但如果你要扩展到二十个agent以上稀疏化就不是可选项而是必选项。4.3 主循环与实时控制流程整体控制器的运行逻辑可以浓缩成一个主循环每个周期执行以下步骤获取当前系统状态含邻居状态。根据当前状态和时变参数计算h值、李导数、以及CLF相关量。组装QP矩阵。调用quadprog求解。将控制输入施加入系统。使用四阶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 核心参数初期取值与调整方向以下是我复现时使用的一组能够仿真实跑的初始参数参数初始值作用调试方向控制周期dt0.05s控制频率调小会增加计算负担但更平滑CBF增益γ5.0安全边界响应速度调大增强安全响应过大会导致控制器过于保守CLF增益1.0任务收敛速度调大加快收敛但要小心与安全约束冲突权重矩阵W10·eye(2)控制平滑度调大让控制变化平缓但牺牲跟踪精度松弛惩罚ρ500任务优先级调太大容易让安全让步调太小任务可能不收敛目标参考控制u_ref0前馈信号有参考轨迹时能大幅提高跟踪质量这些参数不是独立的。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 高频报错盘点问题常见原因解决方案quadprog返回不可行CBF/CLF约束互相矛盾引入松弛变量降低CLF增益控制输入抖动剧烈α项太小或W权重过小增加α、调大W必要时在控制输入上做一阶低通滤波agent间距小于安全值忽略了∂h/∂t或扰动补偿项检查时变CBF偏导计算确认扰动项已加入仿真速度极慢稠密矩阵参与quadprog改用稀疏矩阵并设置MaxIterations等选项优化系统不收敛CLF约束权重不足或δ惩罚过大提高CLF相关权重降低ρ值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约束关系上的时间远多于调求解器的时间。把安全集合的形式、时变参数的表达式、扰动界的估计这三件事想清楚了整个框架自然就能转起来。如果你也在复现这篇文章或者类似的方法希望这份笔记能让你少折腾几天。