混乱环境下的移动机器人连续安全控制，最近两三年在业内被反复提起。我做这个方向也有一段时间了，从最开始的势场法、动态窗口法，一路折腾到基于控制屏障函数（CBF）的二次规划控制器，最大的体感变化是：以前是“想办法躲开障碍物”，现在是“让安全成为系统的一个硬约束”。标题里这个“连续”二字，很多文章一笔带过，但真正落到Matlab仿真里，你会发现连续性问题比安全约束本身更磨人——控制输入跳变、QP可行性丢失、障碍物在检测边缘反复横跳，这些才是工程实践里真正劝退人的地方。这篇内容我会从问题定义、CBF/CLF数学底座、CBF-QP控制器设计与连续性处理、Matlab代码实现细节、乱场景仿真设计和实测避坑几个层面展开，适合刚接触安全控制、打算用Matlab跑通原理样机的同学参考。
1. 混乱环境下的安全控制难在哪：三个经典失败场景
1.1 动态障碍突然切入：反应型规划器的“来不及”
“混乱环境”这个概念，我理解下来至少包含三层意思：一是障碍物数量多，二是障碍物运动不可预测，三是机器人自身状态估计和控制存在误差。很多人一开始会用传统路径规划或者反应型避障算法去试，比如人工势场法、动态窗口法DWA。这类方法在障碍物静止、数量少的时候效果还行，但一旦出现“动态障碍从侧向突然切入”的情况，立刻暴露出问题。
我做过一个典型仿真：机器人沿着直线往目标点走，一个速度为1.2m/s的障碍物从侧面横向穿过机器人路径。人工势场法的处理方式是“感知到障碍就生成一个排斥力”，排斥力方向永远指向障碍物当前所在方向的远处。听起来合理，但问题在于感知是有延迟的——只有当障碍物进入传感器范围，排斥力才开始起作用。等排斥力足够大的时候，障碍物已经离机器人很近了，这时候机器人要么急转弯，要么急刹车。急转弯的后果是路径弧度过大，转向过程里如果正好又有第二个障碍物进入范围，机器人会在两个排斥力之间被“夹住”，出现反复震荡、不知所措的状态。急刹车的后果更实际：轮式机器人速度从1.2m/s突然降到0，电机电流瞬间拉满，控制输入严重饱和，真机上这大概率会打滑甚至碰撞。
这个场景反映的本质问题是：安全控制不能只是“事后反应”，必须带有前瞻性。前瞻性不仅仅指“提前预测障碍物轨迹”，还包括“在控制律层面就把安全作为约束参与计算”，而不是先规划一条路径再回过头来检查它是否安全。后者是两层逻辑，规划层认为安全、跟踪层却因为动态误差破坏安全，这两层之间的缝隙，就是事故多发地。
1.2 安全距离累积误差：阈值式避障的抖振根源
第二个常见失败模式，是“阈值式避障”导致的系统抖振。所谓阈值式避障，就是规定一个安全距离，比如机器人距障碍物小于0.5m就触发避障机动，大于0.6m就解除避障，回到目标跟踪。听起来人畜无害，但你把这件事在连续时间系统里跑一下就会发现：避障动作的开启和退出，本质上是两个控制模式之间的硬切换。硬切换发生时，控制输入几乎必然有一个阶跃跳变。
我实测过一组数据：阈值避障模式下，机器人每秒钟经历2～3次模式切换，切换瞬间转向角速度从1rad/s跳到2.5rad/s再跳回来，控制输入的变化率是CBF-QP连续控制器的4倍以上。频繁硬切换带来的第一个直接后果就是抖振，第二个后果是系统模型不确定性被放大。比如机器人模型里有未建摩擦力项、有电机响应延迟，这些不确定性在平滑控制下可以被控制器自然消化，但在阶跃式切换下会被反复激励，最后变成一条有锯齿的、贴边走的轨迹，看着非常狰狞。
所以真正意义上的“连续安全控制”，核心诉求之一就是控制输入的连续性。它不是说切换完全不行，而是要求切换有过渡、有缓冲、有约束层面的平滑设计，让安全机制从“触发式的事件”变成“始终存在的约束”。
1.3 多目标冲突：目标吸引与安全约束的拉锯
第三个场景更隐蔽，也更常见：机器人接近目标点时，目标点周围有几个动态障碍物在来回游走。从目标跟踪的角度看，机器人应该继续往前走，尽快到达目标点；从安全角度看，机器人应该减速、绕行，甚至原地等待空窗期。两个控制目标直接冲突，控制器的输出会产生“拉锯效应”：一会儿想让机器人冲进去，一会儿又想让机器人退回来，位置轨迹像一只犹豫不决的猫。
我以前处理这种冲突的方法是手动调权重：目标跟踪的权重大一点，就把安全跳过；安全权重大一点，机器人就停在目标点外慢慢等。但惩罚系数这种“软约束”的问题在于，你还没法保证它在所有状态组合下都可靠。只有把安全性提升为硬约束——一个在任何情况下都不允许违反的不等式条件——同时把目标跟踪作为软目标放进优化函数，才算是从机制上解决了多目标冲突。这正是CBF（Control Barrier Function）和CLF（Control Lyapunov Function）分工协作的经典思路，后面章节详细展开。
2. 把“安全”写成数学约束：CBF与CLF的核心工具链
2.1 机器人和障碍物的动力学模型怎么写
在做控制设计之前，先得有模型。为了把问题聚焦在安全控制本身，我采用的是简化的双积分模型，没有加非完整约束、没有加执行器饱和之外的其他约束，演示逻辑清晰，也方便后面换模型。机器人状态写为 ( x = [p_x, p_y, v_x, v_y]^T )，其中位置向量 ( p = [p_x, p_y]^T )，速度向量 ( v = [v_x, v_y]^T )，控制输入 ( u = [u_x, u_y]^T ) 是加速度。系统可以写成：
[ \dot{p} = v, \quad \dot{v} = u ]
更紧凑的形式是仿射非线性系统：( \dot{x} = f(x) + g(x)u )，其中 ( f(x) ) 描述了自然漂移，对双积分模型就是速度推进，( g(x) ) 把控制输入映射到速度变化率，对双积分模型是常数矩阵 ( [0; I] )。
为什么用双积分模型而不是更复杂的运动学模型？因为CBF方法的核心推导依赖相对简单的高阶导数关系。如果你用差速轮模型，状态里有头朝向角θ，控制量是线速度和角速度，那安全距离的求导会多出好几层，写出来的CBF约束包含非线性项更多，调试难度大一个量级。先用双积分模型把核心逻辑跑通，再迁移到差速轮模型，是效率最高的路径。
障碍物方面，我假设每个动态障碍物可被测量或估计出位置 ( p_{obs,i} ) 和速度 ( v_{obs,i} )，短期内用恒定速度模型外推。感知噪声和预测误差后面再谈，先把理想情况走通。
2.2 从安全集到控制屏障函数的不等式
安全控制第一步，是把“安全”写成一个明确的集合。对单个障碍物，定义机器人位置与障碍物位置的距离 ( d_i = |p - p_{obs,i}| )，定义安全距离为 ( d_{safe} )。安全集合可以定义为一个超水平集：
[ h_i(x) = d_i^2 - d_{safe}^2 \geq 0 ]
注意这里我用了距离平方而不是距离本身。直接取 ( d = |p - p_{obs,i}| ) 在 ( d=0 ) 处不可导，求导会出问题；用平方形式不仅处处光滑，而且求导后会和速度项形成线性关系，后续QP约束写起来非常漂亮。实际上这也是业内实现CBF的通行细节。
为什么 ( h_i(x) \geq 0 ) 就是安全约束？因为CBF定义的本质是要求这个系统在集合边界上“往内推”。更严格地说，设 ( h_i(x) ) 是集合 ( S ) 的边界函数，我们希望：只要初始状态在集合内，系统未来的状态就一直留在集合内；即使状态靠近边界，也不允许跨界。这就是不变集的概念。
对 ( h_i(x) \geq 0 ) 求导，得到：
[ \dot{h}_i(x) = L_f h_i(x) + L_g h_i(x) u ]
其中 ( L_f h_i ) 是沿标称动力学的李导数，( L_g h_i ) 是控制方向的李导数。为了保证状态不离开集合，一种经典做法是要求沿轨线的衰减速率有界，即存在 ( \alpha > 0 ) 使得：
[ L_f h_i(x) + L_g h_i(x) u + \alpha h_i(x) \geq 0 ]
这个不等式的实际含义很直白：当机器人离障碍物很远时，( h_i(x) ) 很大，不等式很好满足，CBF基本不干预；当机器人靠近边界时，( h_i(x) \to 0 )，不等式就强制控制输入往“推离障碍物”的方向调整。参数 ( \alpha ) 决定了“边界吸引力”的强度——它越大，系统越激进地远离障碍物边界，但代价是控制输入变化越大。
2.3 CLF负责“尽快到达”，CBF负责“绝不越界”
安全性有了数学表达式，接下来是目标跟踪。目标跟踪的任务是让机器人状态收敛到目标点 ( p_{goal} )、速度收敛到0。我定义李雅普诺夫候选函数：
[ V(x) = (p - p_{goal})^T Q_p (p - p_{goal}) + v^T Q_v v ]
其中 ( Q_p )、( Q_v ) 是正定权重矩阵。为了让状态收敛，要求 ( \dot{V}(x) \leq -\gamma V(x) )（γ>0），即李雅普诺夫函数指数衰减，展开后得到一个关于控制输入 ( u ) 的线性不等式：
[ L_f V(x) + L_g V(x) u + \gamma V(x) \leq 0 ]
问题来了：这个“目标跟踪”不等式和前面那个“安全”不等式，在物理上经常不可同时满足。当机器人前方有障碍物时，要加速接近目标点（CLF的要求）和要减速远离障碍物（CBF的要求）是矛盾的。这时怎么取舍？
CLF和CBF的关系，我习惯用一句话总结：CLF是“想跑多快跑多快”，CBF是“但绝不能越雷池一步”。为了解决双方冲突，标准做法是在CLF不等式里引入一个松弛变量 ( \delta )，允许CLF条件被轻微放松——也就是允许机器人暂时偏离最优跟踪，而CBF条件保持严格。这个设计是有深刻工程逻辑的：安全是硬约束，glà质量可以牺牲一点；如果允许CBF也被松弛，那安全就不再是绝对保证，整个方法就失去了意义。
3. 融合安全与稳定：CBF-QP控制器设计与连续性问题
3.1 标称控制律与QP问题结构
有了上面两个不等式，控制器的设计就变成了每控制周期求解一个约束优化问题。设标称控制律（用户期望的控制输入，比如PD跟踪控制器的输出）为 ( u_{des} = K_p (p_{goal} - p) + K_d (0 - v) )。我们希望最终作用于机器人的控制输入 ( u ) 尽量接近标称控制律，同时满足CBF硬约束和带松弛的CLF约束。
典型的CBF-QP问题写成：
[ \min_{u, \delta} ; |u - u_{des}|^2 + \rho \delta^2 ]
约束条件包括：
[ L_f h_i(x) + L_g h_i(x) u + \alpha h_i(x) \geq 0, \quad i = 1,\dots,N ]
[ L_f V(x) + L_g V(x) u + \gamma V(x) \leq \delta ]
[ u_{min} \leq u \leq u_{max}, \quad \delta \geq 0 ]
第一组约束是安全约束，每个障碍物对应一条不等式；第二组是带松弛的稳定性约束；第三组是执行器饱和约束和松弛变量非负约束。这个QP规模很小，变量维度一般是“控制输入维度+1”，约束数量是“障碍物数+2”，即便有10个动态障碍物，也就十几条约束，Matlab的quadprog毫秒级求解毫无压力。
为什么目标是让 ( u ) 尽量接近 ( u_{des} ) 而不是直接最小化能量？因为实际系统通常已经有一个可以工作的跟踪控制器，我们希望安全机制是一个“监督层”，在正常情况下不干预、不改变系统的原有性能，只在快要越界时把输入掰回安全方向。这种结构的好处是可以模块化复用——今天换一个底层的路径跟踪算法，明天换一个编队控制律，CBF-QP层不需要大改。
3.2 为什么QP解会出现输入跳变
连续安全控制的“连续”主要体现在控制输入的时间连续性上。而我实测中发现，CBF-QP控制器天然有跳变的隐患，根源是约束的激活/失活切换（active set切换）。
举一个具体的例子：一个障碍物在机器人传感器检测半径 ( R_{det} = 3m ) 的边缘徘徊。当障碍物距离刚小于检测半径时，它对应的CBF约束被“激活”，被加入QP问题；当它距离刚大于检测半径时，约束被移除。问题是，障碍物可能在边缘来回穿越，于是约束不断被添加和移除，控制解来回跳变。
另一个更隐蔽的跳变来源，是多个CBF约束之间的交互。比如两个障碍物同时从左右两侧靠近，左侧约束要求机器人向右偏，右侧约束要求机器人向左偏，两个约束同时活跃时可行域可能突然变窄，解qp会在约束边界上从一组active set跳到另一组active set。约束集合的拓扑结构变化，会导致QP最优解作为优化变量的非线性函数发生突变。严格数学上，即便约束本身是线性的，active set切换处的解也可能不连续，除非满足某些约束品性（比如LICQ条件）才能保证局部连续性。
3.3 缓解连续性的三个工程手段
既然跳变的根源是约束激活切换和active set变化，那么工程上缓解连续性的思路也就清晰了，我在Matlab里实际测试过下面三种手段。
手段一：障碍物检测迟滞。给检测半径加迟滞区间——障碍物进入检测半径 ( R_{in} = 3.0m ) 时加入约束，但只有离开检测半径 ( R_{out} = 2.7m ) 时才移除约束。这样约束不会在边缘快速反复切换。这个办法实现简单，效果非常明显，我在随机噪声场景里测试，输入跳变次数下降了70%。它的本质是给约束的进入/退出加一个“毛刺滤波器”，在工程上是合法且符合物理直觉的——真实传感器的检测也不会是一个尖锐的阈值。
手段二：QP解的一阶低通后处理。把求解得到的控制输入 ( u_{qp} ) 通过一个一阶低通滤波器再发给系统模型：( u_{cmd, k} = \beta u_{qp, k} + (1-\beta) u_{cmd, k-1} )。这个办法能直接保证控制指令的连续性，但要注意，低通滤波会引入相位延迟，而安全约束是对当前状态的即时要求，把滤波后的 ( u_{cmd} ) 送给系统时，实际应用的控制输入可能已经偏离QP算出的安全输入，极端情况下会破环CBF的不变集性质。我的处理方式是滤波系数只在CBF约束裕量较大时放开，裕量接近0时直接使用QP原始输出，把低通滤波的优先级降级。这个策略在Matlab里用一段条件判断就能实现，实测效果很稳。
手段三：在QP目标里加入防抖正则项。把目标函数扩展为 ( |u - u_{des}|^2 + \rho \delta^2 + \lambda |u - u_{prev}|^2 )，其中 ( u_{prev} ) 是上一时刻的控制输入。这样控制量不能离上一个时刻太远，输入变化率被直接限制。( \lambda ) 是一个权衡参数，调大了输入平滑但响应变慢，调小了跳变又回来。我一般从 ( \lambda = 0.1 ) 起步，结合低通滤波一起用。需要说明的是，这个办法并没有在理论上保证解一定连续，只是把不连续解的代价变得很高，让QP倾向于选择连续的解。但从工程实测来看，已经足以把最大输入跳变量降低90%左右。
4. Matlab实现要点：主循环、QP求解与可视化调试
4.1 仿真主循环与状态更新
整个Matlab实现其实不复杂，主循环是标准的离散时间控制流程。固定时间步长 ( dt = 0.05s )，仿真总时长视场景而定，一般是10～30秒。控制循环每一步做三件事：第一步，读取当前状态和障碍物状态，计算CBF/CLF相关函数值和李导数；第二步，组装QP问题，调用quadprog求最优控制输入；第三步，把控制输入代入系统模型，用欧拉法或ode45推进状态，同时记录所有轨迹数据。
状态更新的代码骨架是这样的：
for k = 1:K x = x_history(:, end); obs = update_obstacles(k * dt); % 更新障碍物位置/速度 u_qp = solve_cbf_qp(x, obs, params); % 求解QP u_cmd = apply_smoothing(u_qp, u_prev, params); u_prev = u_cmd; % 欧拉积分推进 x_next(1:2) = x(1:2) + x(3:4) * dt; x_next(3:4) = x(3:4) + u_cmd * dt; x_history(:, end+1) = x_next; end这里的代码只是演示结构，真正的坑在细节函数里。比如CBF约束参数的组装，很多第一次写的人会直接在循环里用符号工具箱求导，速度极慢，10秒仿真能跑出一分钟；正确做法是手工推导 ( L_f h ) 和 ( L_g h ) 的闭式表达式，因为它们对双积分模型就是简单的向量运算。
4.2 quadprog的调用细节与正定性处理
quadprog的标准调用格式是：
options = optimoptions('quadprog', 'Display', 'off', 'Algorithm', 'interior-point-convex'); [u_opt, fval, exitflag] = quadprog(H, f, Aineq, bineq, [], [], lb, ub, [], options);H和f需要根据目标函数展开。目标函数 ( |u - u_{des}|^2 + \rho \delta^2 + \lambda |u - u_{prev}|^2 ) 展开后，决策变量 ( z = [u_x; u_y; \delta] )，H矩阵是：
H11 = (1 + lambda) * eye(2); H12 = zeros(2, 1); H22 = rho; H = blkdiag(H11, H22);H必须正定，quadprog的interior-point-convex算法要求目标函数是凸的。这里有一个小坑：如果松弛变量惩罚系数rho取的太小，比如0.01，H矩阵可能数值上接近半正定，quadprog会报错或者解出很大的delta，稳定性约束被过度放松。我的建议是rho不要低于0.5，CLF的松弛应当只是一个“临时安全阀”，而不是一个“廉价的后门”。
Aineq矩阵的组装则是把CBF约束的系数行、CLF约束的系数行、控制饱和约束统一拼起来。每个CBF约束对应一行，系数是 ( [L_g h_i(1), L_g h_i(2), 0] )，不等式右侧是 ( -L_f h_i - \alpha h_i )。CLF约束的系数是 ( [L_g V(1), L_g V(2), -1] )，因为delta前的系数是-1，形式是 ( L_f V + L_g V u \leq \delta )，移项后变成 ( L_f V + L_g V u - \delta \leq 0 )。控制饱和约束直接写 lb、ub 即可，四轮独立的加速度上下限。
4.3 可视化调试：把“约束裕量”画出来
我个人最推荐的调试工具不是看轨迹图，而是画“CBF裕量曲线”。所谓CBF裕量就是 ( h_i(x) ) 的值，以及它离边界还有多远。如果一条裕量曲线总是在接近0的地方徘徊，说明这个障碍物对应的约束一直在临界工作，机器人一直在极限边缘走，非常危险；如果裕量曲线全程远大于0，说明这个约束白设了，可以扩大检测半径或者减小alpha，让安全机制更早介入。
我在调试一个动态场景时，曾经只看轨迹图觉得路径绕得挺顺，但把裕量曲线画出来才发现，机器人与某个障碍物的距离始终在0.32m和0.5m之间振荡，安全边界是0.3m，几乎是贴着墙走。根因是alpha取值过小，CBF约束不够“用力”，系统每次都是等距离逼近边界才被勉强拉回。把alpha从0.5调到2.0后，裕量曲线稳定在0.5m以上，轨迹也更平滑了。这个经验说明：不看约束裕量，只看最终轨迹，很多潜在危险是看不见的。
5. 从静态障碍到混乱场景：仿真设计与指标验证
5.1 三档场景设置
仿真不能一上来就上“极端混乱环境”，那样出了问题你根本不知道是哪个环节导致的。我把测试场景分成三档，层层递进。
第一档，静态密集障碍。在机器人路径上随机放置10～15个静态圆形障碍物，障碍物半径0.3m，安全距离0.5m，随机生成的障碍物需保证起点到目标存在理论可行路径。这一档用来验证CBF-QP在多约束共存时的可行性——因为所有CBF约束都是同时激活的，如果发生矛盾，QP会不可行，这是最基础的压力测试。
第二档，低速动态障碍。在静态障碍基础上加入3～5个速度为0.3～0.6m/s的线性运动障碍物，运动方向固定，路线穿越机器人路径区域。这一档验证动态避障能力，以及CLF松弛变量在目标跟踪和安全约束冲突时的行为。
第三档，混乱动态环境。障碍物数量增加到8～10个，速度提升到0.7～1.5m/s，运动方向随机变化，部分障碍物还加上正弦扰动轨迹。另外，其中一个障碍物的运动路径是“从侧面高速切入”的，专门复现开头说的那种经典危险场景。这一档用来验证连续性的三个缓解手段是否真的有效。
5.2 指标体系：安全性、连续性、到达性
只靠眼睛看轨迹图评价控制器是不靠谱的，我建议量化指标至少覆盖三个方面。
安全性指标：全程最小距离 ( d_{min} = \min_{t,i} |p(t) - p_{obs,i}(t)| )，要求大于 ( d_{safe} )；同时统计CBF裕量曲线低于某个阈值的时长占比，占比越高说明系统越经常处于临界状态。连续性指标：计算相邻两步控制输入的差分的最大值和均方根值 ( rms(|u_k - u_{k-1}|) )，这是衡量“连续安全控制”这个核心诉求的最直接证据。到达性指标：记录到达目标点的时间、终点的距离误差，以及在无特殊困难时平均速度是否合理。三个指标放在一起看，才能判断一个控制器是“真安全”还是“运气好没撞上”。
5.3 一组典型结果与失败模式的复盘
我在第三档混乱环境里跑过一组代表性实验，结果是：使用了迟滞+防抖正则+低通滤波之后，最大输入跳变量从2.1 m/s²降到0.32 m/s²，全程最小距离0.42m，略小于0.5m的理论安全距离但没有发生碰撞。为什么实际最小距离会比理论安全距离小？因为我模型里用了欧拉积分离散化，理论约束是在连续时间成立，离散化带来的积分误差让实际轨迹比QP预测轨迹晚了半个步长，距离被吃掉了一点。
这个失败模式的教训是：仿真步长不能拍脑袋选。我最初用dt=0.1s，最小距离只有0.34m，改成dt=0.05s后提升到0.42m，再改成0.02s收益就不明显了。对CBF这类依赖正则性的控制器，离散误差是安全性的隐形敌人，仿真时要么步长取足够小，要么在设计安全距离时预留离散化余量——比如把理论安全距离0.5m再加5cm作为CBF约束里的 ( d_{safe} )。
另一组失败模式也有代表性：障碍物速度超过1.8m/s时，恒定速度预测模型失效，障碍物在机器人CBF预测的“安全方向”和实际运动方向之间产生了严重偏差，机器人好几次差点撞上。这说明CBF方法在模型预测误差面前并不神奇，它只能保证“基于模型预测的轨迹”是安全的。要应对更剧烈的运动不确定性，得引入鲁棒CBF或者tube MPC的概念，这也是我后续准备扩展的方向。
6. 实操中踩过的坑与后续可以扩展的方向
6.1 四个容易翻车的细节
把CBF-QP在Matlab里完整跑通之后，我发现真正决定这套方法能不能落地的，其实是一些看起来很不起眼的细节。随便拆四个容易翻车的点说说。
第一个坑：CBF约束右侧的符号。这是新手最容易犯的错误。CBF不等式是 ( L_f h + L_g h \cdot u + \alpha h \geq 0 )，整理后 ( -L_g h \cdot u \leq L_f h + \alpha h )。很多人组装Aineq时会把符号写反，结果就是控制器在拼命把机器人往障碍物里推。我在代码审查里见过不止一次这种错误，症状非常迷惑——因为它在简单场景下不报错，只是偶尔会让机器人“莫名其妙”地靠近障碍物。第一个建议：写好QP后，先跑一个机器人正对静态障碍物、让标称控制律把它推向障碍物的测试，如果CBF正确生效，机器人应该在安全距离外停住或者绕开。
第二个坑：障碍物检测边缘的约束抖动。这个前面已经讲了很多，但值得再强调一遍：靠近检测半径或安全边界处，障碍物的“出现/消失”和障碍物预测值的微小变化，都会让CBF约束的不等式右侧发生跳变，进而造成QP解跳变。迟滞处理几乎是必须的，而且在代码里要把“加入约束”和“移除约束”做成有状态的事件，而不是简单的阈值判断，否则迟滞逻辑很容易在循环里写错。
第三个坑：松弛变量的上界。很多人只记得CLF约束要带松弛变量，但忘了给delta设上界。delta理论上可以无限大，如果优化器发现“彻底放弃一切稳定性目标”能让目标函数降低，它就会把delta推到很大的值，CLF约束名存实亡，机器人停在原地不动。我给delta加上界 ( \delta \leq \delta_{max} )，比如0.5，这样松弛只允许有限度地牺牲跟踪性能。
第四个坑：数值量纲不一致。CBF约束里 ( h_i(x) ) 的单位是m²，( L_f h_i ) 的量纲是m²/s，( L_g h_i ) 的量纲是m²/(s·m/s²)，不同约束项之间数值大小差几个数量级很常见。如果不做归一化或者量纲匹配，alpha取同一个值在不同障碍物上的实际效果完全不同。我的做法是对每个障碍物单独设置alpha，或者用障碍物速度动态调整alpha：障碍物速度越快，alpha调大，让安全反应更激进。
6.2 理论扩展思路
如果想把这套东西从原理验证推向真正可用的水平，有两条线值得深挖。
一条线是鲁棒CBF。把感知误差、预测误差建模为有界扰动，把CBF不等式改为在所有可能扰动下都成立的鲁棒版本。代价是可行域变小，控制更保守，但安全保证从“名义系统”升级为“实际系统”。在Matlab里实现鲁棒CBF并不算复杂，主要是把模型中的不确定项用一个凸包或者区间来描述，然后把约束变换成相应的最坏情况约束。
另一条线是backup CBF或可变安全策略。当多个CBF约束的可行域因为目标跟踪需求和保护需求冲突而变成空集时，QP会直接无解，这在极端的混乱环境里面是会真实发生的。一个比较实用的工程策略是：预计算一组“backup控制律”（比如紧急刹车、倒退到开阔区），如果原始QP不可行，系统自动切换到backup控制律保证安全，之后再重新规划。把“绝对安全”和“可用性不足”之间的矛盾，用分层或者说多级保护的方式去调和，比试图让单一QP在所有状态都可行要现实得多。
6.3 给后续仿真的小建议
如果你的目标是快速复现CBF-QP在混乱环境下的效果，我建议你从三档场景的中间档开始，代码结构按照“系统模型模块、CBF/CLF计算模块、QP组装模块、可视化模块”来划分，而不是把所有逻辑堆在一个脚本里。Debug的顺序也很重要：第一步，只保留一个静态障碍物，验证CBF约束是否正确，机器人是否会绕过障碍物；第二步，加入目标跟踪和松弛变量，验证CLF松弛行为；第三步，加入第二个障碍物，验证多约束可行域变化；第四步，逐步增加动态障碍物和连续性问题处理。每加一个特性就重新跑一遍全部指标，这样出问题的时候，你能准确锁定是哪个环节引入了异常。
另外，如果你打算把这段代码作为课程设计或者论文的一部分，建议把仿真指标和可视化图表的生成脚本做成单独模块，并且把每一档场景的参数保存成独立的配置文件。回头整理实验报告的时候，改参数只需改配置文件，不用动主循环代码，能省下大量重复调试的时间。
最后再说一个我自己的体会：连续安全控制这顶帽子，听起来像是理论家才会碰的东西，但真正把CBF-QP跑起来之后就会发现，它更像是一个工程系统工程——数学推导占三分，剩下七分都在处理连续性的细节、离散化的误差、约束激活的抖动这些“脏活”。把这套东西完整做一遍，你对安全控制和最优化求解的感觉，会比看十篇论文都实在。
