v2_humanoid_navigation_tutorial

Chapter 5. 人形与行人跟踪（Humanoid/Person Tracking）

开篇段落

本章是 Humanoid 室内导航中“感知”环节的核心基石，专门讨论如何稳定、准确地跟踪环境中的人和 Humanoid。一个优秀的导航系统不仅要“看到”人，更要理解他们的位置、意图和动态，从而实现安全、高效且符合社会规范的交互。这要求跟踪系统超越简单的目标检测，进入到对身份、姿态、轨迹和意图的持续性理解。我们将从现代多目标跟踪（MOT）的“检测后跟踪”范式出发，深入剖析其每个模块的算法选型与权衡。随后，我们将探讨如何利用三维姿态和步态先验来丰富跟踪状态，解决长时遮挡这一经典难题，并拥挤、反射等复杂场景设计鲁棒性策略。最终，本章将落脚于如何将一个高性能的跟踪系统无缝地耦合到导航规划模块中，为其提供动态体积约束与各向异性的社交导航代价。本章的学习目标是让读者能够设计并评估一套完整的、服务于复杂导航任务的行人跟踪算法方案。

5.1 基础：检测→ReID→多目标跟踪（MOT）数据关联

现代多目标跟踪（MOT）系统，特别是服务于实时应用的系统，大多遵循“检测后跟踪”（Tracking-by-Detection, TBD）的范式。这是一个逻辑清晰、易于调试和迭代的模块化流水线。

                                            +------------------------+
                                            |   4. 轨迹管理         |
                                            | (Track Management)     |
                                            | - Init, Confirm, Lost  |
+------------+     +-----------------+      | - State Update         |
| 图像序列   | --> | 1. 行人检测器   | --+--> +------------------------+
| (Frames)   |     | (Person Detector) |   |
+------------+     +-----------------+   |   +------------------------+
                             |           |-->|   3. 数据关联         |
                             |               | (Data Association)     |
                             |               | - Cost Matrix          |
                             v               | - Matching (Hungarian) |
                   +------------------+      +------------------------+
                   | 2. 外观特征提取  |              ^
                   | (ReID Extractor) |--------------+
                   +------------------+

5.1.1 行人检测器 (Person Detector)

任务: 在每一帧图像中以边界框（Bounding Box, bbox）的形式定位所有可见的人，并给出每个检测的置信度。这是整个跟踪系统的“眼睛”，其性能上限直接决定了跟踪的上限。
模型选型:
- 一阶段检测器 (One-stage): 以 YOLO 系列（如 YOLOv8, YOLOX）为代表，速度极快，是实时应用的首选。它们直接回归 bbox 坐标和类别概率。
- 二阶段检测器 (Two-stage): 以 Faster R-CNN 系列为代表，先生成候选区域（Region Proposals）再进行分类和回归，精度通常更高，但延迟也更大。
- 基于 Transformer 的检测器: 以 DETR 及其变体为代表，将检测视为一个集合预测问题，避免了 NMS（非极大值抑制）等后处理，在拥挤场景下表现出色。
关键考量:
- 高召回率 (High Recall): 对于导航安全至关重要。一个漏检（False Negative）可能直接导致碰撞，而一个误检（False Positive）通常可以在后续的轨迹管理中被过滤掉。因此，在部署时，宁可稍微降低置信度阈值以换取更高的召回率。
- 实时性: 整个感知-规划-控制循环需要在几毫秒内完成，检测器作为第一环，其耗时必须严格控制。
输出: 在时间 t，检测器输出一个检测集合 $D_t = {d_1, d_2, …, d_m}$，其中每个检测 $d_i = (bbox_i, conf_i)$。

5.1.2 外观特征提取 (Re-identification, ReID)

任务: 为每个检测框 bbox 提取一个紧凑且具有高度判别力的特征向量（Embedding）。这个向量是行人的“外观指纹”。
模型原理: ReID 模型通常基于深度卷积网络（如 ResNet, OSNet）或 Vision Transformer，并通过度量学习 (Metric Learning) 进行训练。其目标不是分类，而是在特征空间中拉近同一个体（Positive Pair）的距离，推远不同个体（Negative Pair）的距离。
- 常用损失函数: Triplet Loss 是最经典的损失函数之一。它处理一个三元组（Anchor, Positive, Negative），并优化使得 Anchor 与 Positive 的距离 $d(A, P)$ 加上一个间隔（margin）后，仍然小于 Anchor 与 Negative 的距离 $d(A, N)$。 $L_{triplet} = \max(d(A, P) - d(A, N) + \alpha, 0)$
输出: 对每个检测 $d_i$，计算得到一个 L2 归一化的特征向量 $f_i \in \mathbb{R}^D$（例如 D=512）。

5.1.3 数据关联 (Data Association)

这是 MOT 的核心，负责将当前帧的检测 $D_t$ 与已存活的轨迹集合 $\mathcal{T}_{t-1}$ 进行匹配。

运动模型与状态预测:
- 每个轨迹 $T_i \in \mathcal{T}_{t-1}$ 内部维护一个状态估计器，最常用的是卡尔曼滤波器 (Kalman Filter)。
- 状态向量 $\mathbf{x}$: 通常包含位置、尺寸和它们的一阶导数（速度），例如 $\mathbf{x} = [c_x, c_y, a, h, \dot{c}_x, \dot{c}_y, \dot{a}, \dot{h}]^T$，其中 $(c_x, c_y)$ 是中心点，a 是宽高比，h 是高度。
- 预测: 在处理第 t 帧之前，卡尔曼滤波器首先为每个轨迹预测其在当前帧的状态 $\hat{\mathbf{x}}_t$ 和协方差矩阵 $\mathbf{P}_t$。
代价矩阵构建:
- 构建一个 $N \times M$ 的代价矩阵 $C$，其中 $N$ 是轨迹数，$M$ 是检测数。$C_{ij}$ 表示将第 $j$ 个检测 $d_j$ 关联到第 $i$ 个轨迹 $T_i$ 的代价。
- 运动代价 ($C_{motion}$): 使用马氏距离 (Mahalanobis Distance) 计算预测的 bbox 与实际检测的 bbox 之间的差异。它优于欧氏距离，因为它考虑了预测的不确定性（协方差 $\mathbf{P}_t$）。 $d_M(T_i, d_j) = \sqrt{(\mathbf{y}_j - H\hat{\mathbf{x}}_i)^T S_i^{-1} (\mathbf{y}_j - H\hat{\mathbf{x}}_i)}$ 其中 $\mathbf{y}_j$ 是检测 $d_j$ 的测量值， $H$ 是观测矩阵，$S_i$ 是创协方差。
- 外观代价 ($C_{appearance}$): 计算新检测的 ReID 特征 $f_j$ 与轨迹 $T_i$ 外观特征库（gallery）的余弦距离。轨迹的 gallery 通常会保存最近几帧的特征，以增加鲁棒性。 $d_A(T_i, d_j) = 1 - \max_{f_k \in \text{gallery}(T_i)} (\cos(f_j, f_k))$
- 组合代价: $C_{ij} = \lambda d_M(T_i, d_j) + (1-\lambda) d_A(T_i, d_j)$
  
  Rule-of-Thumb: $\lambda$ 的选择是场景相关的。对于低速、遮挡少的场景，外观信息更可靠，$\lambda$ 可取 0.2-0.4。对于高速、拥挤、姿态变化大的场景，运动信息更稳定，$\lambda$ 可取 0.7-0.9。
匹配门控 (Gating):
- 在计算昂贵的外观代价之前，先用廉价的运动代价进行一次筛选。只对马氏距离小于某个阈值 $\chi^2$（根据卡方分布设定，如 95% 置信区间）的 (轨迹, 检测) 对计算完整代价。这能极大地减少计算量并排除不合理的匹配。
分配问题求解:
- 有了代价矩阵后，这就是一个分配问题。匈牙利算法 (Hungarian Algorithm) 或 Jonker-Volgenant 算法可用于找到最优匹配（总代价最小）。对于实时性要求更高的场景，简单的贪心匹配也是一种可行的次优策略。

5.1.4 轨迹管理 (Track Management)

负责轨迹生命周期维护。

状态机: 每条轨迹都有一个状态：Tentative (暂定), Confirmed (确认), Lost (丢失)。
- Tentative: 一个未匹配的检测会初始化一条暂定轨迹。如果它在接下来的 N_init 帧（e.g., 3 帧）内被连续匹配，则升级为 Confirmed。否则，删除。这有效过滤了孤立的假正检测。
- Confirmed: 成功匹配的轨迹，其卡尔曼滤波器状态会被观测值更新。
- Lost: Confirmed 轨迹若在某一帧未被匹配，状态变为 Lost。如果连续 max_age 帧（e.g., 30-60 帧）都处于 Lost 状态，则该轨迹被彻底删除。

5.2 三维姿态/骨架估计与步态先验

对于与人交互的 Humanoid 来说，2D bbox 提供的几何和意图信息远远不够。3D 姿态是解锁精细化社交导航的关键。

3D 姿态估计 (3D Human Pose Estimation):
- 方法:
  1. 两阶段法 (2D-to-3D Lifting): 先使用成熟的 2D 关键检测器（如 HRNet, OpenPose）在图像上定位 2D 关节，然后用一个轻量级网络将这些 2D 坐标“提升”到 3D 空间。这种方法模块化好，且能利用大规模 2D 标注数据。
  2. 端到端法: 直接从图像回归 3D 关节坐标。这类方法（如 MobiFormer）更简洁，但通常需要 3D 标注数据，获取成本高。
- 单目深度模糊性: 从单张 RGB 图像估计 3D 姿态存在固有的尺度和深度模糊性。在机器人上，这个问题可以通过融合深度传感器（ToF, Stereo）的数据，或将 3D 人体模型与 SLAM 构建的 3D 地图进行几何约束来缓解。
用途:
1. 精确体积占用与预测:
  - 不再使用简单的圆柱体或 bbox 投影，而是根据 3D 骨架构建一个更贴合人形的胶囊体模型（Capsule Model）或高斯混合模型 (GMM)。这在通过狭窄空间（如门、走廊）时至关重要。
  - 通过手臂和腿的展，可以更准确地预测未来 0.5-1 秒内人可能占据的空间范围。
2. 朝向与意图判断:
  - 头部/躯干朝向: 直接提供了人的注意力方向。机器人应尽量避免打断别人的视线或从其正前方近距离穿过。
  - 意图预测: 结合历史轨迹和当前姿态，可以训练一个简单的意图预测模型（如 LSTM, Transformer），预测行人是准备直行、转弯还是停止。例如，一个朝向门口且抬起一只脚的人，极有可能要进入该门。
3. 步态分析 (Gait Analysis):
  - 通过分析下肢关节点（如脚踝、膝盖、髋部）在垂直方向上的周期性运动，可以精确估计步频和步幅。
  - 改善运动模型: 步态信息可以用来动态调整卡尔曼滤波器中的过程噪声协方差矩阵 $Q$。站立时 $Q$ 应该很小，行走时适中，奔跑时则应显著增大，以反映更大的运动不确定性。

5.3 时遮挡与再识别

当一个人被柱子、人群或墙角长时间遮挡（超过 max_age）后再次出现，简单的 MOT 系统会错误地分配一个新 ID（ID Switch）。这对于需要长期交互的任务是致命的。

解决方案：两阶段匹配与轨迹拼接
1. 第一阶段：短时关联 (Short-term Association):
  - 即 5.1 节描述的基于卡尔曼滤波和 ReID 的逐帧匹配。这处理了大部分短时遮挡和流畅运动。
2. 第二阶段：再识别关联 (Re-identification Association):
  - 机制: 当一条 Confirmed 轨迹因为连续 max_age 帧未匹配而被删除时，我们不立即丢弃其所有信息。而是将其最终状态（位置、速度）、外观特征 gallery 和时间戳存入一个“失活轨迹缓冲区”（Inactive Track Buffer），并设定一个更长的存活时间（如 10-20 秒）。
  - 触发: 当一条新的 Tentative 轨迹被创建并即将升级为 Confirmed 时，触发第二阶段匹配。
  - 匹配逻辑: 将这条新轨迹的初始 ReID 特征与“失活轨迹缓冲区”中的所有轨迹进行一次查询。这个查询过程需要满足两个条件：
    - 外观相似性: 余弦距离必须低于一个严格的阈值。
    - 时空可行性 (Spatio-temporal Feasibility): 检查从失活位置到新出现位置的隐含速度是否合理。 $\frac{|\text{pos}{\text{new}} - \text{pos}{\text{lost}}|2}{t{\text{new}} - t_{\text{lost}}} < V_{\text{max_human}}$ 其中 $V_{\text{max_human}}$ 是一个预设的人类最大可能速度（例如 4 m/s）。
  - 轨迹拼接: 如果找到一个满足条件的失活轨迹，则将新轨迹与这条旧轨迹合并，恢复其原始 ID，并用新旧数据之间的插值来填补遮挡期间的轨迹空白。

5.4 人群集/镜面/弱光场景的鲁棒化

导航算法必须能在各种非理想的“长尾”场景下保持稳定。

人群集 (Crowds):
- 挑战: 严重遮挡导致检测框不完整、ReID 特征被背景或其他人的部分污染。
- 策略:
  1. 代价权重动态调整: 如前所述，增大运动模型的权重 $\lambda$。
  2. 部分感知 ReID (Partial ReID): 使用能处理遮挡的 ReID 模型，例如基于注意力机制或将人体分为多个部分（头、上身、下身）分别提取特征，匹配时只使用可见部分的特征。
  3. 群组跟踪: 在极度拥挤的情况下，先将无法分割的人群作为一个整体“群组”进行跟踪，当人群散开时，再将群组分裂为个体轨迹。
镜面/玻璃反射 (Reflections):
- 挑战: 检测器会在反射表面上产生高度逼真的“幽灵”行人。
- 策略:
  1. 多视角几何一致性: 如果 Humanoid 配备多个（如双目或环视）摄像头，一个真实的 3D 点在不同视角下的投影必须足对极几何约束。而反射产生的虚假目标通常无法满足这一约束。
  2. 地图先验约束: 将检测到的行人的 3D 位置（通过深度图或三角化得到）投影到预先构建的 3D 地图（如 Occupancy Grid 或 Mesh）中。如果一个行人的 3D 位置落在了墙体内部、空中，或与场景几何严重冲突，那么它有极大概率是反射。
    function is_reflection(detection_3d_pos, map_3d): if map_3d.is_inside_solid_object(detection_3d_pos): return True if not map_3d.is_on_floor_surface(detection_3d_pos, tolerance=0.3m): return True return False
弱光 (Low-light):
- 挑战: 图像信噪比低，导致检测置信度下降、特征模糊。
- 策略:
  1. 图像增强预处理: 在送入检测器前，使用一个轻量级的低光照图像增强模型（如 Zero-DCE）进行预处理。
  2. 放宽检测门槛，收紧轨迹确认: 适当降低检测器的置信度阈值以提高召回率，但同时增加轨迹确认所需的帧数 N_init（例如从 3 帧增加到 5 帧），以确保只有持续出现的物体才被确认为有效轨迹。

5.5 与导航的耦合

行人跟踪的最终价值体现在它如何赋能下游的规划与控制模块。

动态体积约束 (Dynamic Volumetric Constraints):
- 跟踪系统输出的每个行人（ID, 位置, 速度, 3D 姿态）被转化为一个时变的动态障碍物。
- 其在代价地图上的表示不应只是一个点，而是一个预测性的体积场。这个场覆盖了该行人在未来 $\Delta T$ (e.g., 2-3s) 时间内可能占据的空间。
- 体积的构建: 基于 3D 骨架构建的胶囊体，并根据其线速度和角速度进行前向积分。
- 不确定性建模: 预测的体积应随时间膨胀，以反映预测不确定性的增加。例如，在 $t+\tau$ 时刻，障碍物的半径可以是在当前半径基础上增加一个与 $\tau$ 和速度相关的项 $k \cdot v \cdot \tau$。
- 代价地图层: 规划器（如 Hybrid A*, DWA）使用的代价地图中会有一个专门的“动态障碍物层”，这些预测体积内的代价值被设为极高（LETHAL_OBSTACLE），强制路径规划器绕开。
各向异性社交导航代价 (Anisotropic Social Cost):
- 为了实现符合人类社会规范的导航，机器人不仅要避免碰撞，还要保持礼貌和可预测性。这通过在代价地图中叠加一个“社交场”来实现。
- 这个场是各向异性 (Anisotropic) 的，即成本在不同方向上是不同的，主要由人的朝向决定。
- 数学建模: 可以用高斯混合模型来优雅地表示这个场。围绕每个人的成本可以建模为多个高斯函数的和： $Cost(\mathbf{p}) = w_{personal} \cdot \mathcal{N}(\mathbf{p} | \mu_{person}, \Sigma_{personal}) + w_{front} \cdot \mathcal{N}(\mathbf{p} | \mu_{front}, \Sigma_{front})$
  - 个人空间 (Personal Space): 一个以人为中心 $\mu_{person}$ 的圆形高斯分布（$\Sigma_{personal}$ 为对角阵），半径约 0.5-1 米。
  - 前方交互空间 (Frontal Interaction Space): 一个位于人前方 $\mu_{front}$ 的、被拉长的椭圆形高斯分布。其协方差矩阵 $\Sigma_{front}$ 的主轴方向由人的头部/躯干朝向决定。这个区域的权重 $w_{front}$ 最高，因为正面穿越最不礼貌。

      <-- Person's Heading (from 3D pose)

                 (High Cost Ellipse)
          /////////////////////////////
         //                         //
        //       (Frontal Zone)      //
       //                           //
      +-----------------------------+
     |   (Medium Cost Circle)      |
     |                             |
     |       ^ (Person)            |  <-- Social cost field as sum of Gaussians
     |      (Personal Zone)        |
     |                             |
      +-----------------------------+

通过在总代价中加入这个社交成本，路径规划器会自然地生成更平滑、更符合社会直觉的绕行轨迹，例如从人身后绕过，而不是在最后一刻从侧面惊险地擦过。

本章小结

本章系统性地拆解了服务于 Humanoid 导航的行人跟踪算法方案，强调了从基础到高级、从感知到应用的完整链路。

核心框架: 坚实的“检测后跟踪”范式是基础，其四大模块——检测、ReID、数据关联、轨迹管理——各司其职且相互依赖。
关键算法: 数据关联是算法核心，通过卡尔曼滤波器进行运动预测，并结合ReID外观特征，构建鲁棒的代价矩阵，最后通过门控和匈牙利匹配求解。
导航为本: 跟踪的价值在于为导航提供丰富信息。引入3D 姿态来精细化体积建模和意图理解，利用步态先验优化动模型。
鲁棒性设计: 通过两阶段匹配和轨迹拼接克服长时遮挡；利用多视角几何和地图先验对抗镜面反射等疑难场景。
规划接口: 最终输出被抽象为规划模块可直接使用的两种关键信息：用于安全避障的动态预测体积，以及用于礼貌交互的各向异性社交代价场。

常见陷阱与错误 (Gotchas)

ID 切换 (ID Switching):
- 现象: 两个外观相似的人擦肩而过时，他们的轨迹 ID 发生交换。
- 原因: 在近距离交叉点，运动模型的预测位置高度重叠，如果此时他们的 ReID 特征也因姿态、光照相似而难以区分，匹配算法就可能出错。
- 调试技巧: 可视化匹配过程中的代价矩阵。检查导致错误匹配的单元格 $C_{ij}$ 的运动代价和外观代价的构成。如果外观代价是主因，考虑增强 ReID 模型或在短时交叉时临时提高运动代价的权 $\lambda$。
轨迹碎片化 (Track Fragmentation):
- 现象: 同一个人在场景中连续移动，却被分配了多个不连续的轨迹 ID。
- 原因: 1) 检测器不稳定，导致频繁漏检。2) max_age 参数设置过小，轨迹因几次短暂遮挡就被错误地删除。3) ReID 特征在姿态或光照剧烈变化时漂移，导致无法与旧轨迹关联。
- 调试技巧: 检查被终止轨迹的最后几帧和新轨迹的最初几帧，分析是检测失败还是 ReID 失败。适当增大 max_age，并检查 5.3 节的轨迹拼接逻辑是否被正确触发。
坐标系与时间戳混乱:
- 现象: 跟踪到的 3D 行人在世界坐标系中抖动、漂移，或与机器人自身的运动不一致。
- 原因: 传感器数据（图像、IMU）的时间戳未精确同步；相机内参、外参（相机到机器人基座的变换）不准；机器人自身的定位（SLAM/VIO）有漂移。
- 调试技巧: 立严格的时间同步机制（如 PTP）。使用 TF (Transform) 库管理所有坐标系变换。在静态场景下，验证一个静止的人其在 map 坐标系下的位置是否稳定。可视化所有坐标系，检查时间戳和变换关系。
过度谨慎或鲁莽的社交行为:
- 现象: 机器人要么在人流中畏缩不前（过度谨慎），要么以不礼貌的方式穿行（鲁莽）。
- 原因: 社交代价场参数标定不当。过大或过高的代价导致机器人“社交恐惧”，而过小或形状不合理的代价则无法起到引导作用。
- 调试技巧: 将代价地图可视化，观察社交场的形状、范围和强度是否符合预期。在仿真环境中对不同的人群密度和场景布局进行测试，系统性地调整高斯模型的权重、均值和协方差参数，找到一个在安全性和通行效率之间的良好平衡。
忽略视野边界效应 (Field-of-View Edge Effects):
- 现象: 一个人走出视野后马上又走回来，系统却分配了一个新 ID。
- 原因: max_age 机制没有考虑物体是否在视野边界。一个在图像中心的物体丢失，很可能是被遮挡；而在边界丢失，则极有可能是走出了视野。
- 调试技巧: 在轨迹管理中，当一个轨迹的最后观测位置靠近图像边界时，可以给它一个更长的“失活”缓冲时间，或者在再识别阶段优先匹配那些从视野外围重新进入的轨迹。