v2_humanoid_navigation_tutorial

第 4 章 — 传感输入与多摄算法（不谈硬件）

开篇段落

本章是连接物理世界与算法模型的桥梁，其质量直接决定了整个导航系统的天花板。我们将纯粹从算法视角，深入探讨如何处理与融合来自多个传感器的原始数据流，尤其是复杂的、非理想的多摄像头系统。学习本章后，您将不仅理解理论模型，更能掌握在实践中如何将分散、带有噪声、时序偏差和几何形变的原始传感信号，转化为下游模块（如 SLAM、OCC、VLA）所需的，经过时空对齐且几何关系明确的、干净且带有不确定性量化的观测数据。我们将不讨论任何硬件选型或驱动实现，而专注于解决传感数据在法层面的核心挑战：精密的观测建模、亚毫秒级的时空同步、鲁棒的在线内外参标定以及一致性的多视角信息融合。

4.1 单/双/多摄像头观测模型与基线误差

一切视觉算法始于对成像过程的精确数学建模。这不仅是几何投影，还包括了光学系统带来的非线性畸变。

单目相机（Monocular Camera）的完整模型

针孔相机模型是理想化的第一步。一个在世界坐标系下的三维点 $P_w = [X_w, Y_w, Z_w]^T$ 到像素 $p = [u, v]^T$ 的投影，是一个多步过程：

  1. 世界坐标系 → 相机坐标系：通过外参 $T_{c \leftarrow w} = [R | t]$ 变换，得到点在相机坐标系下的坐标 $P_c = [X_c, Y_c, Z_c]^T$。 \(\begin{bmatrix} X_c \\ Y_c \\ Z_c \end{bmatrix} = R \begin{bmatrix} X_w \\ Y_w \\ Z_w \end{bmatrix} + t\)

  2. 相机坐标系 → 归一化平面：将点投影到焦距为1的虚拟平面上，得到归一化坐标 $p’ = [x’, y’]^T$ \(x' = X_c / Z_c, \quad y' = Y_c / Z_c\)

  3. 畸变模型：真实镜头存在畸变，必须对归一化坐标进行校正。最常用的是 Brown-Conrady 模型，包含径向畸变和切向畸变。 令 $r^2 = x’^2 + y’^2$，畸变后的坐标 $p’’ = [x’’, y’’]^T$ 为： \(x'' = x'(1 + k_1 r^2 + k_2 r^4 + k_3 r^6) + 2p_1 x'y' + p_2(r^2 + 2x'^2) y'' = y'(1 + k_1 r^2 + k_2 r^4 + k_3 r^6) + p_1(r^2 + 2y'^2) + 2p_2 x'y'\) 其中 $k_1, k_2, k_3$ 是径向畸变系数，$p_1, p_2$ 是切向畸变系数。

  4. 归一化平面 → 像素平面：最后，通过内参矩阵 $K$ 变换到最终的像素坐标 $(u, v)$。 \(K = \begin{bmatrix} f_x & s & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix}\) \(u = f_x x'' + s y'' + c_x v = f_y y'' + c_y\) $s$ 是坐标轴倾斜因子，现代相机中通常为 0。

核心困境：从最终的 $(u, v)$ 反推 $P_w$ 时，由于 $Z_c$ 在步骤2中被除掉，导致尺度信息丢失。这是单目视觉的“尺度模糊性”根源。

双目相机（Stereo Camera）的深度误差精析

双目系统通过三角化来恢复深度。假设左右相机已经过立体校正（共面且光轴平行），则视差 $d = u_l - u_r$ 与深度的关系为 $Z = (f_x \cdot b) / d$。

深度误差的来源与量化： 深度的不确定性 $\sigma_Z$ 主要来源于视差测量的误差 $\sigma_d$。$\sigma_d$ 通常与像素量化和匹配算法的亚像素精度有关，可近似为一个常数（如 0.2 像素）。通过误差传播定律，我们得到：

\[\sigma_Z \approx \left| \frac{\partial Z}{\partial d} \right| \sigma_d = \left| -\frac{f_x \cdot b}{d^2} \right| \sigma_d = \frac{Z^2}{f_x \cdot b} \sigma_d\]

Rule-of-thumb:

多摄像头系统的拓扑构型

人形机器人的多摄系统通常是混合构型，服务于不同目的：

4.2 时间同步与漂移建模（算法层）

精确的时间戳是多传感器融合的“通用语言”。任何时间上的不确定性都会转化为空间上的不确定性，尤其是在机器人高速运动时。

\[ascii 真实事件: ----------------|----------------> Time (Robot at Pos X) 相机流: -----[Frame @ t1]-------------> (Processing) IMU流: -[IMU @ t_a]--[IMU @ t_b]-----> (Processing) 问题: t1, t_a, t_b 均带有各自的延迟和漂移. 我们需要精确知道 Frame 曝光中点的真实时刻， 并找到该时刻对应的 IMU 状态。\]

时钟漂移的物理模型： 传感器的时钟由石英晶体振荡器驱动。其频率会受温度、电压和老化的影响，导致与主时钟（如 CPU 的 $CLOCK_MONOTONIC$）之间存在一个微小但持续变化的频率差异。一阶线性模型 $t_{master} = \alpha_i \cdot t_{sensor_i} + \beta_i$ 是对这种行为的良好近似：

在线估计算法：

  1. 滑动窗口最小二乘 (Sliding-window Least Squares): 在一个几秒到几十秒的窗口内，缓存到达的时间戳对 $(t_{sensor_i, k}, t_{master, k})$。当窗口填满或更新时，重新对窗口内的数据点进行线性回归，得到最新的 $(\alpha_i, \beta_i)$。这是一种简单有效的方法。
  2. 卡尔曼滤波器 (Kalman Filter): 可以将 $\alpha_i$ 和 $\beta_i$ 建模为状态变量。状态转移模型可以假设它们是随机游走的。每次收到新的时间戳对，就作为一次“测量”来更新状态估计。这种方法能更平滑地跟踪时变漂移。

致命影响与调试：

4.3 外参/内参标定的算法问题

标定是从像素世界通往三维世界的唯一通行证。错误的标定参数是“万恶之源”，会导致后续所有算法从基础上就出现偏差。

内参标定 (Intrinsic Calibration) 的细节

除了张正友方法，更现代的 SLAM 系统（如 ORB-SLAM3）支持在线或自标定。但离线标定仍然是初始化阶段最可靠的方式。

外参标定 (Extrinsic Calibration) 的黄金标准

  1. 相机-相机外参:
    • 基于标定物: 这是最精确的方法。对于有重叠视场的相机，可以通过联合优化（Bundle Adjustment）来同时精化所有相机的内外参、外参以及标定物的 3D 结构。工具如 Kalibr 在学术界和工业界被广泛使用。
    • 基于运动 (Motion-based): 对于无重叠视场的相机，让机器人执行一段含丰富旋转和平移的轨迹。每个相机独立运行 VIO/SLAM 得到各自的轨迹 $T_{cam1,k}$ 和 $T_{cam2,k}$。外参 $T_{cam1 \to cam2}$ 的求解，就变成了对齐这两条轨迹的问题。
  2. 手眼标定 (Hand-Eye Calibration)：$AX=XB$
    • 问题拆解:
      • $A_k = T_{body,k-1}^{-1} T_{body,k}$: 机器人基座在两次运动之间的相对变换，由里程计或运动学模型提供。
      • $B_k = T_{cam,k-1}^{-1} T_{cam,k}$: 相机在两次运动之间的相对变换，由视觉里程计或对外部静态目标的观测提供。
      • $X = T_{body \to cam}$: 待求的相机到机器人基座的变换。
    • 求解条件: 为了让 $AX=XB$ 有唯一解，运动 $A_k$ 必须包含至少两个不同旋转轴的旋转。纯平移运动无法约束旋转部分。
    • 鲁棒性: 实际应用中，$A$ 和 $B$ 都带有噪声。因此不能只用两次运动求解，而是采集数十次运动数据，构成一个超定方程组，通过线性优化求解 $X$，同时最小化所有运动的残差。

Rule-of-thumb:

4.4 多摄时空融合

融合的目标是 $1+1>2$，通过整合多个有噪声、有遮挡的局部视图，生成一个更完整、更精确、更鲁棒的全局感知。

1. 特征级融合 (Tightly-coupled)

2. 体素级融合 (Intermediate Fusion) 这是构建环境三维表示（如 OCC）的主流方法。

3. 决策级融合 (Loosely-coupled)

异常视角剔除的实践:

4.5 IMU/里程计/ToF/深度的算法级融合接口

为异构传感器设计干净的算法接口，是构建可维护、可扩展导航系统的关键。

本章小结

本章从算法工程师的视角，系统性地梳理了处理多传感器输入的完整流程和核心挑战。

常见陷阱与错误 (Gotchas)

  1. 坐标系混淆 (The Root of All Evil): 这个问题无论怎么强调都不过分。
    • 调试技巧: 使用可视化工具（如 RViz, Foxglove）并显式地画出每一个坐标系。当机器人移动时，观察 $base_link$, $camera_link$, $camera_optical_frame$, $odom$, $map$ 等坐标系的相对运动是否符合预期。编写单元测试，对一个已知点在不同坐标系间反复变换，看其是否能回到原点。
  2. 时间戳的“最后一英里”问题: 即使用了 NTP 或 PTP，从数据包在网卡被盖上时间戳，到进入应用层被算法消费，中间仍有 OS 调度、缓冲区等延迟。对于硬实时要求，应考虑 RTOS 或 DPDK 等技术，但这已超出本章范围。算法层面，要意识到应用层时间戳总有抖动。
  3. 标定的“完美主义”陷阱: 过度拟合标定数据。例如，使用一个非常高阶的多项式去拟合畸变，可能会在标定图像上得到极低的投影误差，但在真实场景中泛化能力很差。应使用交叉验证来选择合适的畸变模型。
  4. 忽略传感器的物理限制:
    • 卷帘快门 (Rolling Shutter): 大多数消费级相机的快门是逐行曝光的。在快速旋转时，这会导致图像扭曲（“果冻效应”）。VIO 算法必须对此进行建模和补偿，否则会引入系统误差。
    • IMU 的温漂: IMU 的偏置对温度极其敏感。在机器人刚开机和运行一小时后，偏置可能有巨大差异。在线估计偏置是所有现代 VIO 系统的标配。
    • 深度相机的材质依赖: ToF 和结构光对黑色吸光材质、透明材质和高反光材质基本无效。算法必须能处理深度图中的空洞和野点，而不是盲目相信其输出。
  5. 盲目相信默认参数: 无论是标定工具箱里的优化参数，还是 VIO 库里的噪声参数，都必须根据你的具体传感器和场景进行调整。例如，一个在室内低速移动机器人和一个在室外高速运动的机器人，其 IMU 噪声和运动模型参数应有显著不同。