CBF与CCG：应对未知动态障碍物的机器人概率安全导航

1. 项目概述当机器人面对“看不见”的威胁在机器人导航领域让机器人在已知的、结构化的环境中安全移动已经是一个相对成熟的问题。无论是工厂里的AGV小车还是家里的扫地机器人它们大多依赖预设的地图或清晰的边界线。然而一旦把机器人放到一个充满“未知动态障碍物”的环境中比如一个行人穿梭的商场、一个可能有其他车辆突然出现的非结构化道路问题就变得异常棘手。这里的“未知”和“动态”是两个核心挑战障碍物不在先验地图中且它们的位置和速度都在实时变化。传统的导航方法比如基于动态窗口法DWA或模型预测控制MPC的局部规划器通常需要精确的障碍物状态位置、速度、形状作为输入。当障碍物未知时这些方法要么失效要么需要依赖一个完美的、能实时识别并追踪所有障碍物的感知系统——这在复杂、动态的现实世界中几乎不可能实现。感知总有延迟、有误差、有盲区。一个突然从拐角跑出来的小孩一个被风吹动的购物车都可能成为导航系统的“黑天鹅事件”。这正是“基于CBF与CCG的未知动态障碍物概率安全导航方法”所要解决的核心痛点。它不再强求“看见并精确预测”每一个障碍物而是换了一种思路承认感知的不完美性将这种不确定性量化为概率并设计一套控制律确保即使在最坏的情况下比如障碍物突然出现在最近的可能位置机器人也能以极高的概率保持安全。这就像一位经验丰富的司机在雾天行车他看不清远处的具体路况但他会根据能见度和车速在心中划出一个“安全边界”确保即使有意外出现也有足够的反应空间和距离来避免事故。这个方法的名字听起来很学术但拆解开来就清晰了CBF控制屏障函数这是实现安全性的“数学保险丝”。你可以把它想象成一个围绕机器人的、无形的“安全气泡”。CBF的数学定义保证了机器人的状态位置、速度一旦进入这个气泡控制算法就会产生一个“力”强行把机器人推离危险区域确保这个气泡永不破裂。它提供的是硬性的安全保证。CCG凸-凹博弈这是处理“未知”和“最坏情况”的“策略大脑”。因为障碍物未知我们只能假设它可能出现在传感器范围内的任何地方。CCG将导航问题建模为一个双人博弈机器人控制方试图找到一条安全且通往目标的路径而一个虚拟的“对手”干扰方则试图在感知不确定性范围内为障碍物选择一个最危险的位置和运动来“攻击”机器人。通过求解这个博弈我们得到的就是针对最坏情况的安全控制策略。概率安全这是结合现实的“柔性约束”。纯粹的CBFCCG可能会因为过于保守总是假设最坏情况而导致机器人“寸步难行”。概率安全引入了一个可接受的风险阈值例如99.9%的安全概率。它允许在极小的、可控的风险下进行更高效的导航在安全性和效率之间取得平衡。简单来说这套方法的核心思想是面对未知动态障碍物我不去猜你到底在哪、要怎么动我只假设你可能会出现在我最难受的地方并以数学上可证明的方式确保即使你真的出现在那里我也能安全避开。接下来我将深入拆解这套方法的每一个技术环节分享在实际仿真和实验中的具体操作、参数调校心得以及那些容易踩坑的细节。2. 核心原理拆解CBF、CCG与概率安全如何协同工作要理解这套方法我们需要把CBF、CCG和概率安全这三个核心部件像搭积木一样组装起来看它们是如何一环扣一环最终形成一个鲁棒的导航系统的。2.1 控制屏障函数安全的“底线守护者”CBF的核心是为系统的安全集定义一个“屏障”。假设机器人的状态是x包含位置、速度等我们希望它始终停留在一个安全集合S {x: h(x) 0}里。这里的h(x)就是屏障函数。例如h(x)可以定义为机器人与最近障碍物之间的距离减去一个安全阈值。CBF理论的关键在于它不仅仅要求h(x)在当前时刻大于0还要求h(x)沿着系统动力学的时间导数在未来也能保持非负。这通过一个扩展的类K函数来实现约束。最终这导出一个关于控制输入u的仿射约束条件L_f h(x) L_g h(x) * u α(h(x)) 0其中L_f h和L_g h是李导数α是一个扩展的类K函数。这个不等式意味着在任何时候我们选择的控制指令u都必须满足这个条件从而保证h(x)不会衰减到0以下即安全“气泡”不会被戳破。注意α函数的选择至关重要。它通常取为α(h) γ * h其中γ 0。这个γ参数可以理解为安全性的“刚度”或“反应速度”。γ越大系统对安全边界的逼近就越敏感会更快、更强烈地“推开”机器人但也可能导致控制指令抖动剧烈。在实际调参中这是一个需要仔细权衡的 knob。2.2 凸-凹博弈与“最坏情况”对弈当障碍物状态位置p_obs速度v_obs未知但属于一个已知的有界集合D时例如由传感器最大量程和最大检测误差定义的集合CBF约束中的h(x)就变成了一个与p_obs、v_obs相关的函数h(x, p_obs, v_obs)。如果我们盲目地要求对所有可能的(p_obs, v_obs) ∈ D都满足CBF约束那这个约束会变得极其保守甚至无解机器人可能被“吓”得不敢动。CCG提供了一个聪明的思路我们不要求对所有情况安全而是针对一个由“对手”选择的最坏情况来保证安全。我们将问题建模为一个零和博弈控制方机器人选择控制输入u最小化目标函数如与目标点的距离同时满足针对“对手”所选障碍物状态的安全约束。干扰方对手在集合D内选择障碍物状态d (p_obs, v_obs)最大化控制方的“不安全度”通常是让CBF约束最难满足。这个博弈的数学形式通常是一个 min-max 优化问题。神奇的是对于一类常见的动力学模型如微分驱动、全向轮和二次型/线性目标函数这个 min-max 问题可以转化为一个等价的单层凸优化问题通常是二次规划QP。这是因为最大化操作max over d可以解析地处理掉最终得到的约束是关于u的凸约束。这就是“凸-凹”的由来——目标函数对u是凸的对d是凹的在特定条件下可化简。实操心得这个转化是算法能实时运行的关键。在实现时我们需要仔细推导出针对特定机器人模型和障碍物不确定性集合D的最终QP形式。集合D的形状选择很重要圆形或椭球不确定性区域通常能导出更简洁的约束。如果D太复杂如多边形可能无法得到简洁的凸约束需要采用更保守的近似或不同的求解器。2.3 概率安全在保守与冒险间走钢丝纯粹的CCG-CBF方案是“绝对安全”导向的它针对的是不确定性集合D内的所有可能情况包括那些概率极低的极端情况。这可能导致机器人行为过于保守比如在一条宽阔的走廊里因为远处一个模糊传感器噪声可能被解释为障碍物机器人就紧贴墙壁蠕动。概率安全放松了这一要求。我们不再要求100%安全而是要求安全概率不低于某个预设水平1 - δ例如δ 0.001即99.9%安全。实现方式通常有两种收缩不确定性集合我们不再使用完整的感知不确定性集合D而是使用一个收缩后的、更小的集合D_δ使得障碍物真实状态落在D_δ内的概率至少为1 - δ。然后将CCG-CBF中的D替换为D_δ。这样博弈的“对手”只能在更小的范围内制造麻烦从而给控制方机器人更多行动自由。机会约束直接在优化问题中以概率形式表述约束P( h(x, p_obs, v_obs) 0 ) 1 - δ。这类约束通常更难处理可能需要通过场景法或分布ally鲁棒优化来近似求解。在工程实践中第一种方法收缩集合更为常见和实用。它需要我们知道感知噪声的概率分布通常是高斯分布。例如如果位置估计误差服从零均值高斯分布N(0, Σ)那么一个99.9%的概率椭球就可以作为收缩后的集合D_δ。椭球的大小由卡方分布的分位数决定。踩坑记录概率阈值δ的选择是艺术也是科学。δ太小如1e-6集合收缩不明显行为依然保守δ太大如0.1虽然机器人更“大胆”但风险显著增加。我的经验是从一个中等保守值开始如δ0.01在仿真中观察机器人在大量随机障碍物场景下的碰撞统计逐步调整。同时必须结合具体任务运送咖啡的机器人和执行救援任务的机器人其可接受的风险阈值截然不同。3. 系统实现与实操步骤理论需要落地。下面我将以一个典型的差分驱动机器人为例详细阐述如何从零搭建这套导航系统包括感知处理、控制律推导、优化求解和仿真验证。3.1 机器人动力学与安全集合定义假设我们的机器人是经典的差分驱动模型状态为x [px, py, θ, v, ω]^T平面位置x,y航向角线速度角速度控制输入为u [a, α]^T线加速度角加速度。其动力学方程为px_dot v * cos(θ) py_dot v * sin(θ) θ_dot ω v_dot a ω_dot α为了简化后续的CBF约束为控制输入u的仿射形式我们通常将速度也视为状态而加速度作为输入。安全集合的定义这是整个方法的基础。对于每个检测到的或可能存在的障碍物i我们定义一个屏障函数h_i(x, p_obs_i)。最常见的选择是欧几里得距离减去安全半径h_i(x, p_obs_i) ||[px, py]^T - p_obs_i||^2 - (R_robot R_obs R_safe)^2其中R_robot和R_obs是机器人和障碍物的物理半径R_safe是额外添加的安全裕度。h_i 0意味着两者中心距离大于合并半径是安全的。3.2 感知不确定性建模与集合构造这是处理“未知”和“动态”的关键输入。假设我们的感知模块如激光雷达目标检测提供了一个障碍物的估计位置p_obs_est和估计速度v_obs_est同时给出了这些估计的不确定性协方差矩阵Σ_p和Σ_v可以从感知算法或传感器标定数据中得到。那么在任意时刻t我们认为障碍物的真实状态(p_obs, v_obs)落在一个以估计值为中心、以不确定性为形状的集合内。一个常用且数学上方便处理的模型是椭球不确定集D { (p, v) : [p - p_est; v - v_est]^T * P^{-1} * [p - p_est; v - v_est] 1 }其中P是一个块对角矩阵由Σ_p和Σ_v缩放得到。这个椭球包含了在给定置信水平下如95%障碍物可能处于的所有状态。对于概率安全我们需要收缩这个集合。如果假设噪声是高斯分布一个(1-δ)概率水平的集合是D_δ { (p, v) : [p - p_est; v - v_est]^T * P^{-1} * [p - p_est; v - v_est] χ^2_{n, 1-δ} }其中n是状态维度这里是4x,y位置和x,y速度χ^2_{n, 1-δ}是卡方分布的分位数。δ0.001时这个值大约在15左右对于n4这意味着D_δ是原始椭球D的sqrt(15) ≈ 3.9倍轴长收缩等等这里容易混淆。实际上不等式右边变大了从1变成15这意味着集合D_δ变大了因为它允许误差的范数更大。这不对。这里是一个关键纠正在概率安全语境下我们通常定义的是置信区域。如果P对应于95%的置信椭球即不等式右边1对应95%那么要得到99.9%的置信椭球我们需要扩大这个椭球即不等式右边应该用一个更大的数c_δ。而“收缩集合”法指的是我们用一个更小的、但概率水平仍为1-δ的集合来设计控制器这个更小的集合是通过调整P矩阵例如放大协方差或直接用一个缩放因子来实现的以确保在这个小集合内保证安全就能推导出全局的高概率安全。具体实现依赖于严格的理论推导如基于矩或分布假设的稳健优化在工程上一种简化做法是我们仍然使用D例如95%集合作为博弈集合但将安全裕度R_safe根据δ和不确定性分布进行加大以此来隐含地提供概率保证。这是实践中一个更易实现的技巧。3.3 CCG-CBF-QP控制器的构建与求解有了动力学、安全函数和不确定性集合我们可以构建每时每刻需要求解的优化问题了。核心步骤是将CCG思想应用于每个障碍物的CBF约束。对于单个障碍物其最坏情况下的CBF约束要求是对于所有(p_obs, v_obs) ∈ D都有L_f h L_g h * u γh 0。这是一个鲁棒约束。通过CCG转化具体推导涉及对偶理论或S-procedure这个无穷多的约束可以转化为一个关于u的二阶锥约束或一个更简单的线性约束如果D是范数有界集且h对障碍物状态是线性的。最终我们的控制器在每个控制周期例如100Hz需要求解如下形式的二次规划问题目标函数min_u (u - u_nom)^T Q (u - u_nom)其中u_nom是一个名义控制律产生的输入比如一个简单的PD控制器指向目标点。最小化与名义输入的偏差是为了在保证安全的前提下尽可能执行原定任务。约束条件控制输入限幅u_min u u_max加速度/减速度物理限制。CBF安全约束针对每个障碍物iA_i(u) * u b_i(u)。注意这里的A_i和b_i可能依赖于当前状态x和不确定性集合D_i的参数在QP中它们是已知常数矩阵/向量。经过CCG处理每个障碍物对应一个线性约束。求解器选择由于问题是一个标准的QP我们可以使用高效的求解库如OSQP、qpOASES或CVXOPT。对于嵌入式平台qpOASES因其热启动功能两次求解间问题结构变化小解相近特别适合实时应用。在ROS中通常将求解器封装成一个节点订阅机器人状态、目标点和障碍物感知信息发布解算出的控制指令Twist消息。实操步骤总结感知数据订阅从激光雷达、视觉等传感器节点订阅障碍物的检测框或点云簇并利用滤波器如卡尔曼滤波估计其位置和速度同时估算或设定其不确定性协方差。不确定性集合构造为每个活跃障碍物构建其当前时刻的椭球不确定集D_i。构建QP参数根据机器人当前状态x、目标点、名义控制律u_nom以及每个障碍物的D_i计算目标函数中的Q矩阵和所有CBF约束对应的A_i,b_i。求解QP调用QP求解器得到当前最优且安全的控制输入u*。指令发布与执行将u*线加速度和角加速度积分或直接转换为机器人底层驱动能理解的线速度和角速度指令Twist发布到对应的控制话题。循环以固定频率50-100Hz重复步骤1-5。4. 参数调校与性能优化实战算法框架搭建好后真正的挑战在于调参。参数直接决定了机器人在“安全”、“平滑”、“高效”之间的行为平衡。以下是我在多次仿真和实物测试中积累的核心参数调校经验。4.1 关键参数及其影响分析参数物理意义影响调校建议与初始值安全半径R_safe在物理半径外增加的缓冲距离。增大机器人更早、更远地避开障碍物行为更保守可能导致在狭窄通道卡死。减小机器人更“贴身”通过效率高但容错空间小对模型和控制的精度要求高。从0.3m开始。对于差分驱动考虑机器人旋转时的包络可以适当加大。在动态环境中建议R_safe 最大速度 * 控制周期 定位误差。CBF类K函数系数γ控制安全约束的“刚度”或收敛速率。增大系统对安全边界的侵犯更敏感恢复力更强但可能导致控制量剧烈抖动特别是γ 5时。减小系统反应“温和”但可能无法在紧急情况下快速恢复安全。典型范围[0.5, 3.0]。从1.0开始。可以将其设计为与距离相关的函数离障碍物越近γ越大以提供非线性增益。名义控制器权重QQP目标函数中偏离名义控制u_nom的惩罚权重。对角线元素值大机器人严格跟踪名义控制器如直奔目标可能牺牲安全性如果QP无解会返回失败。值小机器人可以自由偏离名义路径以满足安全约束行为更灵活但可能偏离目标方向。Q diag([w_a, w_α])。w_a线加速度权重和w_α角加速度权重需要根据机器人动力学调整。例如对于转向灵活的机器人可以给w_α较小值允许其灵活转向避障。初始可设为diag([1.0, 0.5])。感知不确定性缩放因子κ用于缩放感知协方差矩阵以构造概率安全集合D_δ。增大κ认为障碍物不确定性更大博弈集合D更大控制器更保守。减小κ更信任感知控制器更激进。如果直接使用95%置信椭球 (κ1)。要实现99.9%概率安全可能需要κ2~3通过卡方分位数计算。这是一个关键的安全-性能权衡旋钮。控制频率fQP求解和指令发布的频率。频率高如100Hz控制更及时能处理更高速的动态障碍物对CBF离散化误差更小但计算负荷大。频率低可能因响应延迟导致安全约束被违反。强烈建议 50Hz。CBF理论基于连续时间离散化执行会引入误差频率越高误差越小。确保QP求解能在控制周期内完成。4.2 平滑性与抖动处理CBF-QP控制器的一个常见问题是输出指令抖动尤其是在多个约束激活或接近安全边界时。这会导致机器人运动不平滑加速电机磨损。解决方法滤波对QP输出的控制指令u*进行低通滤波。但需谨慎滤波会引入相位滞后可能破坏CBF保证的安全性。一种折中是对名义控制指令u_nom进行滤波而QP的硬约束能保证即使参考输入平滑安全依然成立。软化约束在QP中可以将硬约束A_i u b_i改为A_i u b_i s_i并惩罚松弛变量s_i。这样当约束难以满足时允许轻微违反付出代价而不是让求解器在可行域边缘剧烈震荡。这能显著平滑控制信号。预测与前瞻使用更复杂的名义控制器如MPC它能生成一小段未来时间窗内平滑的参考轨迹。然后CBF-QP作为“安全滤波器”作用于MPC的输出上。这样既有了MPC的平滑性和前瞻性又有了CBF的强安全保证。4.3 实时性保障与计算优化实时求解QP是工程落地的关键。优化策略包括障碍物筛选不是所有感知到的障碍物都需要纳入QP。可以基于距离只考虑h(x)小于某个阈值的障碍物和相对速度只考虑可能发生碰撞的障碍物进行筛选。通常维护一个“活跃障碍物列表”数量控制在5-10个以内。热启动使用qpOASES这类支持热启动的求解器。由于相邻控制周期的问题结构相似最优解也相近用上一周期的解作为本次求解的初始猜测能极大加速收敛。简化模型对于非常高速的动态障碍物有时可以将其简化为一个以当前位置为中心、以最大速度膨胀的静态障碍区域速度障碍物VO思想然后用一个静态CBF约束来处理这比包含速度不确定性的动态CCG约束计算量小得多。代码优化构建QP系数矩阵A_i,b_i的代码往往是瓶颈。使用Eigen等线性代数库并尽可能预计算不变量避免在循环中进行重复的矩阵运算和内存分配。5. 仿真测试与典型问题排查在将算法部署到实物机器人之前充分的仿真测试是必不可少的。我推荐使用ROS Gazebo或Pybullet进行测试。下面分享一套测试流程和常见问题。5.1 仿真环境搭建与测试场景设计机器人模型在Gazebo中导入或创建差分驱动机器人URDF模型并配置好激光雷达或深度相机插件、差分驱动控制器插件。感知仿真Gazebo的传感器插件会发布真实的点云或激光扫描数据。你需要编写一个模拟的感知节点这个节点订阅传感器数据并加入符合你预设模型的噪声高斯噪声和延迟例如50ms然后发布带协方差的障碍物消息列表。这比使用完美的Ground Truth数据更贴近现实。控制器节点实现上述CCG-CBF-QP算法作为一个ROS节点。测试场景静态未知障碍物在地图中放置一些机器人先验地图中没有的障碍物。匀速动态障碍物让一个障碍物以恒定速度穿过机器人的路径。交互动态障碍物设计一个“恶意”的障碍物其运动策略就是试图撞向机器人用于测试CCG应对最坏情况的能力。狭窄通道测试在保守参数下机器人是否会被“困住”。传感器抖动与丢帧在感知节点中模拟传感器偶尔的较大噪声或短暂的数据丢失。5.2 常见问题、现象与排查技巧以下表格整理了开发调试中常见的问题问题现象可能原因排查步骤与解决方案机器人原地振荡或画圈QP问题可行域在某个局部变得非常小或成为一点机器人在几个可行解之间来回跳变。1. 检查R_safe是否过大导致在狭窄空间无解。2. 检查名义控制器u_nom是否过于激进总是把机器人推向导致QP无解的方向。可以尝试降低名义控制器的增益。3.引入软化约束这是解决振荡最有效的方法之一。机器人过于保守在开阔地停滞不前不确定性集合D设置过大或概率安全参数κ过大导致CBF约束过于严格。1. 检查感知噪声协方差Σ的设定值是否合理。可能传感器标定数据过于悲观。2. 调小概率安全缩放因子κ或在开阔区域动态调整κ环境感知模块提供场景开阔度信息。3. 检查是否把远距离的、无关的障碍物也纳入了约束增加距离筛选阈值。在高速动态障碍物前反应迟钝险些碰撞控制频率过低或CBF约束中的γ参数太小系统“反应慢”。1.首要检查控制频率。确保QP求解和循环能在预期周期内完成用ros::Rate和ros::WallTime测量循环实际时间。2. 适当增大γ值但注意可能引发抖动。3. 考虑在CBF约束中显式包含障碍物速度项如果估计可靠这能提供更好的前瞻性。QP求解失败返回无解在某个时刻所有安全约束和控制输入约束共同构成的可行域为空集。1. 启用求解器的不可行性检测并输出不可行的约束编号定位是哪个障碍物导致了冲突。2. 检查障碍物数据是否有异常值如位置坐标突然跳变。3.必须实现应急策略当QP无解时不能简单发送零速度。可以回退到一个纯刹车指令最大减速度或者忽略最不紧急的障碍物约束直到问题重新可行。运动轨迹不平滑有尖角虽然控制指令经过滤波但路径层面仍不光滑。1. 检查名义控制器u_nom是否本身就不平滑如基于栅格地图的路径规划器输出折线。可以对其输出的路径进行样条插值平滑。2. 在QP的目标函数中除了惩罚u与u_nom的偏差还可以加入对控制量变化率Δu的惩罚即min u^T Q u Δu^T R Δu这能直接产生更平滑的控制序列。Gazebo中机器人模型穿透障碍物虽然算法输出安全但仿真中发生碰撞。1.区分控制安全与物理碰撞CBF保证的是质点模型的安全。机器人有体积Gazebo的碰撞检测是基于形状的。确保R_robot包含了机器人的碰撞半径。2. 检查Gazebo中机器人的惯性参数和控制器增益是否合理不合理的增益可能导致底层电机跟踪不上上层指令产生实际轨迹偏离。3. 在CBF的安全距离中增加一个与相对速度成正比的项即R_safe R0 β * v_rel其中v_rel是相对速度大小β是反应时间这能提供速度自适应的安全裕度。5.3 性能评估指标在仿真中需要定量评估算法性能安全性碰撞次数/总运行时间。在随机生成的数百个动态场景中统计。效率从起点到终点的路径长度和总时间。与不考虑安全的最短路径时间对比计算时间代价。平滑度控制输入u的均方根变化率或者机器人轨迹的曲率变化。计算耗时每个控制周期内构建QP和求解QP的最大时间与平均时间必须远小于控制周期。成功率在设定的时间限制内到达目标点的任务比例。这套方法最让我欣赏的一点是它提供了一种可证明的安全框架。你不再仅仅依靠“测试没撞”来说服自己系统安全而是可以通过调整概率参数δ从数学上界定系统所能达到的安全等级。当然这建立在你的不确定性模型D能准确反映真实感知误差的假设上。因此对感知模块进行严格的标定和误差建模是整套系统在实际中可靠工作的基石。没有准确的误差模型再精巧的控制算法也是空中楼阁。