v2_humanoid_navigation_tutorial

Chapter 7: 动态障碍物检测与避障

开篇段落

本章是导航算法从静态世界走向动态现实的关键一步。前序章节我们假定了一个相对静止的环境，但真实的室内场景——无论是家庭、办公室还是商场——都充满了移动的人、物（如宠物、服务车）和其他 Humanoid。静态的 SLAM 地图在动态环境中会迅速失效，单纯依赖它的规划将导致频繁的碰撞或不必要的急停。本章的目标是深入探讨动态障碍物检测与规避的算法方案，构建一个从快速反应到深思熟虑的多层次安全系统。我们将从基础的、不依赖语义的几何方法（光流、深度变化）出发，过渡到能理解地形材质的于学习的可通行性分析。随后，我们将深入剖析经典的反应式避障算法（如速度障碍法及其变种 ORCA），并最终讨论如何通过轨迹预测，实现更符合社会规范的、有预见性的高级避让策略。学完本章，读者将能够为 Humanoid 设计一个分层的、鲁棒的动态避障系统，使其能够在复杂人流中安全、高效、合乎礼仪地穿行，真正融入人类环境。

7.1 几何避障：光流/深度/占据增量

几何避障是动态避障的基石，它构成了安全系统的第一道防线。这类方法不关心障碍物是什么（“What”），只关心“那里有个东西在动”（“Where” and “How”）。其核心思想是检测出与静态背景模型不一致的运动或占据变化，优点是速度快、不依赖昂贵的语义模型。

7.1.1 光流法 (Optical Flow)

光流描述了图像中像素亮度的时序运动模式。通过补偿机器人自身的运动（ego-motion）后，剩余的显著光流场通常对应于场景中的独立运动物体。

原理详述：

稠密光流计算：对于连续两帧图像 $I_{t-1}$ 和 $I_t$，计算一个稠密光流场 $\mathbf{F}(u, v) = (\delta u, \delta v)$，表示像素 $(u,v)$ 从 $t-1$ 到 $t$ 的运动向量。经典方法如 Lucas-Kanade 或 Horn-Schunck，现代方法则基于深度学习（如 RAFT）。
自运动补偿：获取机器人从 $t-1$ 到 $t$ 的刚体变换矩阵 $\mathbf{T}{t, t-1}$（来自 VIO/SLAM）。对于每个像素 $(u,v)$，其深度为 $d(u,v)$，可以计算出由自运动引起的预期光流 $\mathbf{F}{\text{ego}}(u, v)$。
- 令相机内参矩阵为 $\mathbf{K}$，像素点 $(u,v)$ 对应的三维点为 $\mathbf{P}_{t-1} = d(u,v) \mathbf{K}^{-1} [u, v, 1]^T$。
- 该点在 $t$ 时刻的相机坐标为 $\mathbf{P}t = \mathbf{T}{t, t-1} \mathbf{P}_{t-1}$。
- 将 $\mathbf{P}t$ 重新投影回 $t$ 时刻的图像平面得到 $(u’, v’)$，则 $\mathbf{F}{\text{ego}}(u, v) = (u’ - u, v’ - v)$。
残差分析：计算残差光流 $\mathbf{F}{\text{res}} = \mathbf{F} - \mathbf{F}{\text{ego}}$。

动态物体分割：对 $\mathbf{F}_{\text{res}}$ 的幅值 $

\mathbf{F}_{\text{res}}

$ 进行阈值化处理。然后使用形态学操作（如腐蚀、膨胀）和连通域分析，提取出显著运动的区域作为动态障碍物的掩码。

  +------------------+  Ego-Motion(T_t,t-1)  +------------------+
  |    Frame t-1     |---------------------->|     Frame t      |
  |   Depth D_t-1    |                       |                  |
  +------------------+                       +------------------+
          |                                        |
          |       +--------------------------------+
          |       |         (e.g., RAFT)
          +-------+-----> Dense Optical Flow -----> F(u,v)
          |
          v (Project 3D points using D_t-1 & T_t,t-1)
  Warped Frame -> Expected Flow F_ego(u,v)
          |
          +----------- Subtract & Threshold ---------> Dynamic Masks

Rule-of-thumb：光流法对深度信息依赖较弱（虽然精确补偿需要深度），在 RGB 传感器上即可运行，是低成本的运动检测信号。但其致命弱点是孔径问题（无法确定平行于边缘的运动）和对光照变化的敏感性。在实践中，它通常用于触发“警觉”状态，或作为其他更鲁棒方法的补充验证信号，而不直接用于精确的避障决策。

7.1.2 深度/点云增量法

当拥有深度数据时，动态检测变得更加直接和可靠。核心思想是“找不同”：当前场景中多出来的东西就是动态障碍。

原理详述：
1. 维护静态背景模型：SLAM 系统会输出一个（半）静态的全局地图，例如点云地图 $\mathcal{P}_{\text{static}}$ 或 TSDF 体素网格。
2. 当前帧对齐：获取当前帧的点云 $\mathcal{P}{\text{current}}$，并使用最新的机器人位姿 $\mathbf{T}{\text{world}, \text{cam}}$ 将其变换到世界坐标系。
3. 差异计算：
  - 方法A (点云比较)：对于 $\mathcal{P}{\text{current}}$ 中的每个点 $p_c$，在 $\mathcal{P}{\text{static}}$ 中查找其最近邻点。如果距离大于某个阈值 $\tau_{dist}$，则 $p_c$ 被认为是动态点。此方法计算开销大。
  - 方法B (投影差异)：更高效的方法是，将静态地图 $\mathcal{P}{\text{static}}$ 根据当前位姿投影成一个预期的深度图 $D{\text{expected}}$。然后直接与当前观测到的深度图 $D_{\text{current}}$ 逐像素比较。 $ \text{IsDynamic}(u, v) = (D_{\text{expected}}(u, v) > 0) \land (D_{\text{current}}(u, v) > 0) \land (D_{\text{expected}}(u, v) - D_{\text{current}}(u, v) > \tau_{\text{depth}}) $ 这个公式表示：只有在静态地图和当前帧都观测到有效深度，并且当前深度显著小于（即更近）预期深度时，才判断为动态障碍。这能有效过滤掉传感器噪声和空洞。
Rule-of-thumb：基于深度的方法是几何避障的主力。它的鲁棒性远超光流法。关键挑战在于静态地图的质量和实时性。一个过时的静态地图会导致将新放置的家具误判为动态障碍。因此，SLAM 系统需要有能力区分长期静止物体和短期动态物体。

7.1.3 占据增量法 (Occupancy Delta)

该方法将动态检测与三维占据地图（详见 Chapter 9）紧密结合，是目前最鲁棒的几何方法之一。

原理详述：
1. 分层代价地图：维护一个局部的、高频更新的代价地图（Costmap），它由多个层叠加而成：
  - 静态层 (Static Layer)：来自长期 SLAM 地图，表示墙壁等永久障碍。
  - 障碍层 (Obstacle Layer)：由当前传感器数据实时渲染，包含所有观测到的障碍。
  - 动态层 (Dynamic Layer)：本的核心。
2. 动态层更新：
  - 将当前传感器的观测数据（如点云）光线投射（ray-casting）到三维栅格中，更新体素状态。
  - 将此局部、瞬时的占据情况与静态层进行对比。
  - 若一个体素在静态层中为“空闲”（Free），但在当前观测中为“占用”（Occupied），则将其标记为动态障碍，并添加到动态层。
3. 时间衰减：动态障碍不会永远存在。动态层中的占据值需要随时间衰减。例如，每隔 $\Delta t$，将动态层的占据概率 $p$ 更新为 $p \leftarrow p \cdot e^{-\lambda \Delta t}$。这确保了离开视野的障碍物“幽灵”会逐渐消失。
Rule-of-thumb：占据增量法天然融合了时序信息，并能处理任意形状的障碍物。它构成了现代机器人导航系统中局部规划器的基础（如 ROS Navigation Stack）。其计算开销和内存占用是主要考量，通常只在机器人周围的一个部窗口（如 10m x 10m）内高频运行。

7.2 学习型可通行性估计 (Traversability)

几何方法无法理解语义。一个矮台阶在几何上是障碍，但 Humanoid 可以跨越；一滩水在几何上是平地，但应该避开。学习型可通行性估计通过数据驱动的方式，让机器人学会“哪里能走，哪里不好走，哪里绝对不能走”。

算法框架详述：
- 输入：多模态传感器数据是关键。
  - RGB 图像：提供纹理、颜色信息（识别水渍、地毯）。
  - 深度图：提供几何信息（识别高度变化、坡度）。
  - 表面法线图：由深度图计算得到，对坡度变化非常敏感。
  - IMU 重力向量：提供绝对的“上”方向，帮助模型正确理解坡道和楼梯，消除姿态倾斜带来的歧义。
- 输出：一个像素级的、多类别的可通行性地图。类别定义应服务于规划器：
  - Class 0: 高信通行区 (High-Confidence Traversable) - 成本 0。如硬质平地。
  - Class 1: 低成本通行区 (Low-Cost Traversable) - 成本 10。如短毛地毯。
  - Class 2: 中成本通行区 (Medium-Cost Traversable) - 成本 50。如长毛地毯、草地，可能影响稳定性。
  - Class 3: 高成本/特殊动作区 (High-Cost / Special Action) - 成本 200。如楼梯、可跨越的门槛。规划器应避免，或触发特殊动作原语。
  - Class 4: 不可通行区 (Non-Traversable) - 成本 255 (无穷大)。如墙壁、桌腿、深坑、水渍。
- 模型架构：通常采用轻量级的 Encoder-Decoder 分割网络（如 MobileNetV3-UNet），以在端侧实现实时推理。输入的多模态数据在早期或中期进行融合。
数据闭环：
1. 冷启动：使用仿真器（如 Isaac Sim, Habitat）生成大量带完美标签的合成数据。
2. 公开数据集：利用 RELLIS-3D 等户外越野数据集，迁移学习其对地形的理解能力。
3. 在线学习与人工标注：部署后，记录所有导致机器人打滑、颠簸、触发急停或被人工干预的场景。将这些“困难样本”回传，由标注团队进行精细标注，用于模型的持续迭代。这是提升模型鲁棒性的唯一途径。
Rule-of-thumb：可通行性估计是连接感知和规划的桥梁，极大提升了导航系统的泛化能力。务必将其作为一个独立的代价地图层，而不是与几何障碍层混淆。这样可以灵活调整不同成本的权重。始终保持一个由纯几何方法构成的“安全底线”层，防止学习模型在未见过的场景下做出灾难性的错误判断。

7.3 速度障碍/避碰优化 (VO/ORCA/Model Predictive Checks)

检测到障碍物后，机器人需要立即做出反应。这类算法在“速度空间”或短时“轨迹空间”中进行规划，为机器人选择一个既能朝向目标前进，又能避免在不久的将来发生碰撞的动作。

7.3.1 速度障碍法 (Velocity Obstacles, VO)

核心思想：对于机器人 A 和障碍物 B，VO 方法在 A 的速度空间中标出一个“禁止区域”。任何落在该区域内的速度向量，都会在未来导致 A 和 B 发生碰撞。
数学定义：令机器人 A 和障碍物 B 为半径分别为 $r_A, r_B$ 的圆盘，其位置为 $\mathbf{p}_A, \mathbf{p}_B$，速度为 $\mathbf{v}_A, \mathbf{v}_B$。
1. 碰撞区域 (Collision Cone, CC)：将 B 的半径扩大为 $R = r_A + r_B$（闵可夫斯基和）。从 $\mathbf{p}_A$ 到这个扩大圆盘的两条切线所形成的锥形区域，即为相对位置上的碰撞区域。
2. 速度障碍 (Velocity Obstacle, VO)：所有会导致未来某时刻相对位置 $\mathbf{p}{BA}(t) = (\mathbf{p}_B + \mathbf{v}_B t) - (\mathbf{p}_A + \mathbf{v}_A t)$ 进入碰撞区域的相对速度 $\mathbf{v}{AB} = \mathbf{v}A - \mathbf{v}_B$ 集合，构成了一个以原点为顶点，与 CC 平行的锥形。 $ \text{VO}{A|B} = { \mathbf{v} | \exists t>0, (\mathbf{p}_A + \mathbf{v} t) \in \text{Circle}(\mathbf{p}_B + \mathbf{v}_B t, R) } $
这个集合可以简化为： $ \text{VO}{A|B} = \text{CC}(\mathbf{p}{BA}) \oplus \mathbf{v}_B $ 其中 $\oplus$ 是闵可夫斯基和，即把碰撞锥的原点平移到 $\mathbf{v}_B$。

决策：机器人 A 选择一个新的速度 $\mathbf{v}A’$，使其满足 $\mathbf{v}_A’ \notin \bigcup_i \text{VO}{A

B_i}$，并且最接近预设的目标速度 $\mathbf{v}_{\text{pref}}$。

缺陷：当两个 VO 控制的机器人相遇时，它们可能会同时选择对称的避让策略，导致“迎面抖动”或僵持。

7.3.2 最优相互避碰算法 (ORCA)

ORCA 是对 VO 的重大改进，它通过引入“共同责任”原则，解决了振荡问题。

核心思想：A 和 B 都对避免碰撞负有责任。ORCA 不会完全禁止整个 VO 锥，而是计算出一个允许速度的“半平面”。A 只需要确保其新速度在自己负责的半平面内即可。

计算导致碰撞的最小速度修正向量 $\mathbf{u}$，它是将当前相对速度 $\mathbf{v}_{AB}$ 投影到 VO 锥边界上所需的最小扰动。
A 承担一半的责任，即需要至少做出 $\frac{1}{2}\mathbf{u}$ 的修正。

这定义了一个半平面：$\text{ORCA}_{A

B} = { \mathbf{v}

(\mathbf{v} - (\mathbf{v}_A + \frac{1}{2}\mathbf{u})) \cdot \mathbf{n} \ge 0 }$，其中 $\mathbf{n}$ 是 $\mathbf{u}$ 的法线方向。

机器人的任务是在所有障碍物定义的半平面的交集（这是一个凸多边形）内，寻找最接近 $\mathbf{v}_{\text{pref}}$ 的速度。这是一个二次规划问题，可以高效求解。

Rule-of-thumb：ORCA 是当前多智能体反应式避障的黄金标准。它计算极快（毫秒级），无振荡，效果稳定。在任何需要与人群交互的场景中，ORCA 或变种都应作为局部规划器的核心反应层。

7.3.3 模型预测检查 (如 DWA, Trajectory Rollout)

这类方法将避障问题转化为一个短时域的轨迹优化问题，更适合考虑机器人的非完整约束和动力学特性。

原理 (以 DWA 为例)：
1. 速度采样：在当前可达的速度空间（一个由最大速度和加速度限制的窗口）内，离散地采样多组控制指令 (v, ω)。
2. 轨迹前向模拟 (Rollout)：对每一个采样的 (v, ω)，使用机器人的运动学模型向前推演一小段时间（如 1-3 秒），生成一条候选轨迹。
3. 轨迹评估：设计一个多目标代价函数 $G(v, \omega)$ 来评价每条轨迹的好坏： $ G(v, \omega) = \alpha \cdot \text{GoalHeading}(traj) + \beta \cdot \text{Clearance}(traj) + \gamma \cdot \text{Velocity}(traj) $
  - GoalHeading: 轨迹末端点与全局路径或目标的对齐程度。
  - Clearance: 轨迹与最近障碍物的最小距离。
  - Velocity: 轨迹的平均速度，鼓励机器人前进。
4. 选择与执行：选择代价值最低的、且全程无碰撞的轨迹，并执行其对应的 (v, ω) 指令一小步，然后重复整个过程。
Rule-of-thumb：DWA/Trajectory Rollout 对于差速轮或全向轮机器人非常有效，但对于腿式机器人（Humanoid），运动模型更为复杂。其思想可以借鉴，但“Rollout”需要替换为更真实的步态规划器。它比 ORCA 更能发挥机器人的运动性能，但计算开销更大，且容易陷入局部最优（例如，在U形障碍物中）。实践中，常将 ORCA 作为 DWA 轨迹评估中的一个强制约束或高权重成本项。

7.4 台阶/坡道/玻璃/反光/地毯的特殊处理

现实世界充满了传感器的“陷阱”和需要特殊对待的几何结构。一个鲁棒的系统必须有专门的模块来处理这些“corner cases”。

台阶/坡道：
- 检测：在局部点云中，使用 RANSAC 或霍夫变换进行平面分割。当检测到多个法线方向显著不同（例如，一个垂直、一个水平）且空间上相邻的平面时，即可识别为台阶。坡道则表现为法线方向一致但与重力方向有夹角的单个大平面。
- 策略：这些结构不应被视为普通障碍物。检测到后，应立即抑制默认的避障行为，并向任务规划器发送事件，触发一个专门的动作原语（Action Primitive），如 ExecuteStairClimbing(params)。
玻璃/镜面/透明物体：
- 检测线索：这是一个多模态融合问题。
  1. 视觉-深度不一致：RGB 图像显示可以“看穿”某个区域，但深度图（尤其 ToF）在该区域显示为一堵墙或返回无效值。
  2. 点云伪影：镜面反射会产生一个“虚拟”的、对称的房间点云。检测这种对称性是识别镜面的强信号。
  3. Specular Highlights高光反射的特征在图像中非常明显。
  4. 学习模型：训练一个专门的分割模型，用于识别玻璃、镜子等透明或反光表面。
- 策略：一旦检测到，应在代价地图中将该区域标记为高成本区域，引导机器人绕行。在无法绕行时，应减速慢行，并依赖其他传感器（如接触式）进行最终确认。
地毯/线缆：
- 挑战：这些低矮障碍物可能被深度传感器忽略，但对 Humanoid 的步态稳定性构成威胁。
- 策略：
  1. 高分辨率视觉：使用高分辨率摄像头，结合纹理分析和边缘检测算法，识别地毯边缘和地面上的线缆。
  2. 可通行性模型：一个好的可通行性模型应该能学会将这些区域标记为“中等成本”，提醒规划器谨慎通过。
  3. 控制器适配：通知底层的步态控制器，前方将进入不同材质的地面，以便其提前调整足部抬高和落足策略，避免绊倒。

最高级的避障是“预判”，即预测其他智能体的意图，并做出符合社会规范的、可预测的让行。这使得机器人从一个“笨拙的闯入者”变为一个“有礼貌的参与者”。

轨迹预测：
- 经典模型：
  - 匀速模型 (CV)：假设行人在短时间内（<1s）保持当前速度，简单有效，是很好的基线。
  - 社会力模型 (Social Force Model)：将行人建模为受多种“力”驱动的粒子：一个将他拉向目标的力，以及来自其他行人和障碍物的排斥力。能模拟一些基本的人群现象。
- 学习型模型：
  - RNN/LSTM based：如 Social-LSTM，使用循环神经网络对行人的历史轨迹进行编码，并解码出未来轨迹。
  - GAN/CVAE based：如 Social-GAN，能够预测多模态的未来轨迹（一个人可能左转也能右转），并给出概率分布，更符合现实。
  - Transformer based：如 AgentFormer，利用注意力机制更好地捕捉长时依赖和复杂的时空交互。
让行策略集成：
1. 时变代价地图 (Time-Varying Costmap)：将预测出的行人未来轨迹（及其概率分布）“绘制”到代价地图上。具体做法是，在未来时间点 $t$ 的预测位置 $(x_t, y_t)$ 处，放置一个高斯核（Gaussian blob）作为成本。这个成本随时间演化，形成一个动态的“斥力场”。规划器（如 DWA/MPC）在评估轨迹时，会自然地避开这些时空区域。
2. 显式社交规则：将社会规范编码为代价函数的一部分。 * 个人空间 (Personal Space)：在每个行人周围定义一个半径较大的“社交区”（低成本）和一个半径较小的“私密区”（高成本），惩罚侵入行为。 * 靠右通行 (Keep-Right)：当检测到通道或走廊时，偏离右侧的路径施加额外成本。 * 视线交互 (Gaze Direction)：如果能估计行人的头部姿态，机器人应尽量避免从其正前方穿过，以免打断其注意力。
Rule-of-thumb：从简单的匀速模型和固定大小的社交区开始，这已经能解决 80% 的问题。只有在需要处理非常密集和复杂的双向人流时，才引入计算昂贵的学习型预测模型。可预测性比最优性更重要：机器人的行为应该简单、明确，让周围的人能轻松预判它的意图。一个突然以最优路径横穿人群的机器人，远不如一个稍微减速、靠边等待的机器人受欢迎。

本章小结

本章构建了一个完整的、从反应到预测的动态避障算法栈。它是一个逻辑上清晰、实现上可解耦的多层次系统：

安全底线 (Safety Layer)：由几何方法（深度/占据增量）构成，确保在任何情况下都不会发生物理碰撞。这是最优先最快速的反应层。
环境理解 (Understanding Layer)：通过学习型可通行性分析，赋予机器人理解地面语义的能力，使其能做出超越纯几何的智能决策，如跨越门槛、避开水渍。
反应规划 (Reactive Planning Layer)：利用 ORCA 或 MPC/DWA 等算法，在考虑机器人动力学和多重约束的情况下，实时生成无碰撞的局部轨迹。
社会交互 (Social Interaction Layer)：通过轨迹预测和社交规范建模，实现主动、礼让、可预测的避让行为，提升机器人的社会接受度。
异常处理 (Exception Handling)：针对玻璃、镜面、台阶等特殊场景，设计专门的检测与应对策略，填补通用算法的盲点，确保系统的鲁棒性。

这些层次协同工作，共同确保 Humanoid 在动态、非结构化的真实世界中的安全、效率与合规性。

常见陷阱与错误 (Gotchas)

检测闪烁与决策抖动 (Detection Flickering & Decision Oscillations)：原始的几何检测结果可能是断续的（前一帧检测到，后一帧丢失），导致机器人行为犹豫不决，反复横跳。
- 解决方案：为每个检测到的动态障碍物实例化一个跟踪器（如基于卡尔曼滤波或更简单的IOU跟踪），为其维护一个包含位置、速度、置信度的状态。只有当一个障碍物被连续多帧稳定跟踪后，才对其采取强烈的避让动作。
“冻结机器人”问题 (The Freezing Robot Problem)：在拥挤环境中，避障算法可能发现所有方向都存在风险，导致机器人速度降为零并卡住。
- 解决方案：引入动态参数调整。例如，当机器人长时间速度过低时，可以适当缩小安全边界（inflation radius），或暂时降低对社交规范的权重。同时，规划器应有一个“脱困”模式，允许它执行一些微小的、能打破僵局的“扰动”动作。
高延迟的位姿估计 (High-Latency Pose Estimation)：自运动补偿是所有动态检测的前提。如果 VIO/SLAM 系统的位姿输出有显著延迟，那么补偿就是错误的，会导致静态物体被误识别为动态，或者动态物体被忽略。
- 解决方案：必须对整个数据流进行端到端延迟分析。使用硬件时间戳进行同步，并通过预测模型（如匀速模型）对传入的位姿进行“前向外推”，以补偿计算和传输延迟。
过度依赖预测 (Over-reliance on Prediction)：人类行为充满不确定性，任何预测模型都有其失效边界。如果系统完全信赖预测结果，当行人突然改变主意时，可能会导致危险。
- 解决方案：采用置信度感知的规划。预测模型应输出不确定性（如轨迹的协方差）。高不确定性的预测应在代价地图中产生更弥散、权重更低的影响。同时，必须有一个快速的、不依赖预测的纯几何反应层（如于当前位置的 ORCA）作为最后的安全保障。
忽略机器人自身体积和形状 (Ignoring the Robot’s Body Model)：将机器人视为一个点或圆盘进行规划，在狭窄空间或转身时是危险的。Humanoid 的形状复杂，其手臂或背包可能在转身时扫到旁边的物体。
- 解决方案：在代价地图中，使用机器人精确的三维足迹（3D footprint）进行碰撞检测，而不仅仅是一个二维圆。这个足迹应根据机器人的当前姿态（如手臂位置）动态更新。这确保了规划出的轨迹对整个身体都是安全的。