v2_humanoid_navigation_tutorial

第九章 OCC 方案：3D 占据与隐式场重建

开篇段落

本章将深入探讨导航算法中一个至关重要的组成部分：三维环境表示 (3D Environment Representation)。传统的二维栅格地图将世界简化为一个“平面”，这对于轮式机器人或许足够，但对于需要与三维世界进行复杂交互（如屈膝躲避桌面、伸手开门、上下楼梯）的 Humanoid 机器人而言，则是完全不够的。一个精确、动态、语义丰富且对计算友好的三维世界模型，是其实现真正空间智能的基石。本章将系统性地剖析两大主流 3D 表示范式：以体素为基础的显式离散化表示（如占据栅格、TSDF/ESDF），以及由神经网络参数化的连续隐式场表示（如 NeRF、神经 SDF、3D 高斯溅射）。学习完本章，您将深刻理解各类 3D 表示的数学原理、性能权衡、适用场景，掌握从原始传感数据构建这些表示的算法路径，并了解如何将它们无缝集成到下游的规划与控制模块中。

9.1 占据网格/TSDF/ESDF/体素 + 神经隐式（SDF/NeRF/3DGS）

显式离散化表示：确定性的空间划分

这类方法的核心思想是将连续的三维空间划分为有限个、规则的单元——体素 (Voxel)，并为每个单元赋予特定属性。

占据栅格 (Occupancy Grid)：
- 原理：最基础的概率表示法。每个体素 $m_i$ 存储其被障碍物占据的概率 $P(m_i)$。为了在计算上更稳定和高效，通常不直接存储概率，而是存储其对数赔率 (log-odds)：$l(m_i) = \log\frac{P(m_i)}{1 - P(m_i)}$。
- 更新法则：当有新的传感器测量 $z_t$ 到来时，更新过程是一个简单的加法，这完美体现了贝叶斯更新的精髓： $l(m_i|z_{1:t}) = l(m_i|z_{1:t-1}) + l(m_i|z_t) - l(m_i, prior)$ 其中 $l(m_i|z_t)$ 是由当前测量 $z_t$ 提供的“证据”，$l(m_i, prior)$ 是先验（通常为0，代表未知）。这种形式避免了概率值在0或1附近更新困难的饱和问题。
- 优点：概念简单，能自然地融合来自不同传感器的不确定信息。
- 缺点：仅有“占据/非占据”信息，无法精确表达物体表面几何，导致重建结果通常是“膨胀”的、模糊的。内存消耗巨大，一个 $10m \times 10m \times 3m$ 的空间以 5cm 分辨率划分，就需要 $200 \times 200 \times 60 = 240$ 万个体素。

截断符号距离场 (Truncated Signed Distance Field, TSDF)：

原理：专为表面重建而设计。每个体素存储一个到最近物体表面的有符号距离 $d$。符号为正表示在表面外（自由空间），为负表示在表面内（被占据）。为减少无效更新噪声影响，距离值被“截断”在一个小范围 $[-\tau, +\tau]$ 内。体素还需存储一个权重 $w$，表示该距离估计的置信度。

融合伪代码：

function fuse_depth_map(TSDF_grid, depth_map, camera_pose):
  for each pixel (u, v) with depth d in depth_map:
    // 反投影到相机坐标系，再转换到世界坐标系
    point_camera = back_project(u, v, d)
    point_world = camera_pose * point_camera
    camera_origin_world = camera_pose.translation

    // 沿着视线，在截断范围内更新体素
    ray_direction = normalize(point_world - camera_origin_world)
    for t from -τ to τ step voxel_size:
      voxel_center = point_world + t * ray_direction
      voxel_idx = world_to_voxel_index(voxel_center)
              
      // 计算该体素到真实表面的SDF
      sdf_sample = norm(point_world - voxel_center) * sign(t)
              
      // 加权平均更新
      old_sdf, old_weight = TSDF_grid.get(voxel_idx)
      new_weight = old_weight + 1
      new_sdf = (old_sdf * old_weight + sdf_sample) / new_weight
      TSDF_grid.set(voxel_idx, new_sdf, new_weight)

优点：通过对大量深度观测进行加权平均，能生成非常平滑和精确的物体表面模型，是KinectFusion等经典算法的核心。
缺点：原生算法主要为静态场景设计。内存问题依然存在。

欧几里得符号距离场 (Euclidean Signed Distance Field, ESDF)：
- 原理：与TSDF不同，ESDF计算的是体素中心到最近的已确定障碍物的欧几里得距离，并且通常不截断或截断范围很大。它的主要目的不是重建，而是服务于规划。
- 优点：为基于优化的规划器（如MPPI, CHOMP）提供了场景的梯度信息。查询任意点的碰撞代价及其梯度都接近 $O(1)$，极大提了规划效率。
- 生成：通常由已经建好的占据栅格或TSDF（通过提取零水平面为障碍物）批量转换而来。高效的增量式生成算法（如Voxblox）可以在线维护ESDF。

核心挑战与解决方案：稀疏化

显式表示的共同致命弱点是内存。对于空旷的室内空间，大部分体素都是“自由”的，存储它们是巨大的浪费。

八叉树 (OctoMap)：递归地将空间划分为8个子块，如果一个大块内的所有体素状态相同（全空闲或全占据），则不再细分。这极大地压缩了空旷或被完全占据的区域。

体素哈希 (Voxel Hashing)：将一个无限大的虚拟体素网格，通过哈希函数映射到一个固定大小的物理哈希表上。只有当传感器观测到某个区域时，才为其分配实际的内存块。这是目前最高效的稀疏化方案之一，被广泛用于大规模实时SLAM系统。

ASCII 图示：Voxel Hashing
 World Space (Infinite)          Hash Function             Hash Table (Finite Memory)
+-------------------------+      h(vx,vy,vz)       +---------------------------------+
| ...                     |         |              | Bucket 0: -> VoxelBlock Data_A  |
|   (vx,vy,vz)            | --------+              | Bucket 1: -> VoxelBlock Data_B  |
|      * Voxel            |                        | ...                             |
|                         |                        | Bucket k: -> VoxelBlock Data_C  |
+-------------------------+                        +---------------------------------+
                                                          (Collision handling)

连续神经隐式表示：用神经网络学习空间

这类方法不再将空间离散化，而是用一个紧凑的神经网络 $f_\theta$ 来参数化整个连续的三维场景函数。

神经符号距离函数 (Neural SDF)：

原理：训练一个MLP网络 $f_\theta: \mathbb{R}^3 \rightarrow \mathbb{R}$，使其能够对任意输入的三维坐标 $\mathbf{x}=(x, y, z)$，输出该点的SDF值。物体表面被隐式地定义为零水平集 ${\mathbf{x}

f_\theta(\mathbf{x})=0}$。

优点：内存占用极低（仅模型权重），与分辨率无关，可以查询任意精度的表面点和法线。
缺点：查询代价高（每次查询都是一次网络前向传播），训练过程缓慢，难以进行实时的、增量式的场景更新。

神经辐射场 (Neural Radiance Fields, NeRF)：
- 原理：用一个MLP $f_\theta: (\mathbf{x}, \mathbf{d}) \rightarrow (\sigma, \mathbf{c})$ 来学习一个场景。输入是3D坐标 $\mathbf{x}$ 和相机视角方向 $\mathbf{d}$，输出是该点的体密度 (volume density) $\sigma$ (衡量光线在这里被阻挡的概率) 和颜色 (color) $\mathbf{c}$。通过沿一条光线进行积分的体积渲染 (volume rendering) 来合成新视角的图像。
- 公式（体积染）：一条光线 $\mathbf{r}(t) = \mathbf{o} + t\mathbf{d}$ 的颜色 $C(\mathbf{r})$ 由以下积分得到： $C(\mathbf{r}) = \int_{t_n}^{t_f} T(t) \sigma(\mathbf{r}(t)) \mathbf{c}(\mathbf{r}(t), \mathbf{d}) dt$, 其中 $T(t) = \exp\left(-\int_{t_n}^{t} \sigma(\mathbf{r}(s)) ds\right)$ 是光线从近端 $t_n$ 传播到 $t$ 时的累积透射率。
- 几何提取：场景的几何形状隐含在体密度 $\sigma$ 场中。高 $\sigma$ 值的区域对应物体表面。
- 优点：能生成无与伦比的照片级真实感的新视角图像。
- 缺点：训练和渲染都极其缓慢。虽然Instant-NGP等技术通过引入显式结构（如哈希网格）大幅提速，但在线建图和处理动态场景仍是前沿研究课题。
三维高斯溅射 (3D Gaussian Splatting, 3DGS)：
- 原理：一种混合表示。它不再使用MLP，而是用数百万个显式的三维高斯分布来表示场景。每个高斯由以下参数定义：位（均值 $\mu$）、形状（协方差矩阵 $\Sigma$，通常分解为缩放和旋转）、颜色（球谐函数系数）和不透明度 $\alpha$。
- 渲染：通过一个高效的、可微分的光栅化器，将这些三维高斯“溅射”(splat)到二维图像平面上，并按深度排序进行alpha混合，从而实时合成图像。
- 优点：兼具显式表示的速度和隐式表示的质量。训练速度比NeRF快1-2个数量级，渲染速度达到实时。其显式特性使其更易于编辑、控制和与物理引擎交互。
- 导航应用：可以直接将高斯中心点云用于碰撞检测，或通过渲染深度图来驱动传统的TSDF/ESDF建图流程。对于动态场景，可以直接操纵或为特定高斯集合赋予运动模型。

方案对比与选型

特性	稀疏TSDF/ESDF (Voxel Hashing)	神经SDF (DeepSDF)	NeRF (Instant-NGP)	3D 高斯溅射 (3DGS)
原理	离散体素，哈希表存储	MLP拟合SDF函数	MLP拟合辐射场	显式3D高斯集合
内存占用	中（取决于场景复杂度和分辨率）	极低 (网络权重)	低 (哈希网格+小MLP)	高 (数百万高斯参数)
查询几何速度	极快 (哈希查找)	慢 (网络前传)	慢 (多次网络前传+积分)	快 (但需专门数据结构)
渲染质量	无 (仅几何)	中 (可提取Mesh)	极高 (照片级)	极高 (接近NeRF，但实时)
在线更新能力	强 (增量融合)	弱 (需要重训练)	中 (部分方法支持)	强 (可实时增删/移动高斯)
1B 档方案	首选 (成熟、高效、可靠)	不适用 (离线)	辅助 (离线数据生成)	探索中 (作为轻量级表示)
10B 档方案	基础模块 (小脑)	不适用 (离线)	VLA视觉输入/离线数据	核心表示 (大脑的”世界模型”)

Rule-of-Thumb:

1B 档方案 (资源受限): 核心采用基于体素哈希的 TSDF/ESDF 系统。它在性能、精度和工业成熟度之间取得了最佳平衡。这是机器人的“小脑”和反射系统赖以生存的几何基础。

10B 档方案 (云端/高性能计算): 采用混合架构。底层依然运行高效的 ESDF 用于快速避障和路径优化（小脑）。上层则由 3DGS 构建一个丰富、逼真的世界模型，作为 VLA “大脑”进行场景理解、长期规划和人机交互的视觉基础。NeRF/神经SDF 主要用于离线高精度场景扫描和大规模合成数据生成。

9.2 单目/多目/事件相机到 3D 占据的算法路径

将传感器数据转换为上述3D表示，需要一条完整的处理流水线。

数据流总览 (ASCII):

+-----------------+   +---------------------+   +-----------------------+   +-------------------+
|  Sensor Stream  |-->|  Preprocessing      |-->|  Depth & Ego-Motion   |-->|  3D Fusion Engine |--> Occupancy Map
| (RGB, IMU, etc) |   | (Sync, Rectify, ..) |   |  (SLAM/VIO/MVS)       |   |  (TSDF/3DGS/etc)  |
+-----------------+   +---------------------+   +-----------------------+   +-------------------+

单目相机 (Monocular) + IMU:
- 核心挑战: 单目视觉固有的尺度模糊性 (Scale Ambiguity)。
- 路径:
  1. 视频帧 + IMU数据 输入 视觉-惯性里程计 (VIO) 系统，输出尺度漂移的相机位姿和稀疏地图点。
  2. 视频帧 输入 单目深度估计模型 (如 ZoeDepth)，生成度量不准的深度图。
  3. 尺度恢复与对齐: 结合 VIO 的运动估计和深度图的几何结构，或利用场景先验（如假设地面为平面、Humanoid自身高度已知）来联合优化，确定真实物理尺度。
  4. 校正后的位姿 + 尺度准确的深度图 输入 TSDF 融合引擎进行建图。
多目相机 (Stereo/Multi-camera):
- 核心优势: 利用已知的相机基线 (baseline) 进行三角测量，直接获得具有真实尺度的深度。
- 路径:
  1. 同步的多视角图像 进行立体矫正 (Stereo Rectification)。
  2. 通过立体匹配 (Stereo Matching) 算法 (如 SGBM, 或基于学习的 RAFT-Stereo) 计算稠密视差图。
  3. 视差图 转换为 深度图 ($depth = baseline \times focal_length / disparity$)。
  4. 深度图 + 由 VIO/SLAM 提供的位姿 输入 TSDF 融合引擎。这是目前最稳定和工业化的方案。
事件相机 (Event Camera):
- 核心优势: 极高的时间分辨率 (微秒级)，高动态范围 (HDR)，低功耗。非常适合处理高速运动和光照剧变。
- 路径:
  1. 异步事件流 被累积成一种中间表示，如事件体素网格 (Event Voxel Grid)。
  2. 事件表示 输入基于事件的 VIO 算法，估计相机运动。
  3. 通过运动补偿后的事件流，可以重建图像梯度或光流，进而估计稀疏或半稠密的深度。
  4. 位姿 + 深度 输入融合引擎。该领域算法仍在快速发展中，但潜力巨大。

9.3 动态更新：时序占据、流场、记忆压缩与蒸馏到轻图

一个静态地图很快就会过时。Humanoid 必须能感知和适应环境中的动态变化。

动态物体分割与移除:
- 方法: 结合语义分割网络（标记出人、椅子等可移动类别）和运动分析（如光流、多帧几何不一致性检测）。
- 实现: TSDF 融合前，将识别出的动态物体区域在深度图上标记为无效(mask out)，避免它们被“烧入”静态地图。这些动态物体应由一个独立的多目标跟踪模块来处理（见第五章）。
场景流 (Scene Flow):
- 原理: 估计 3D 空间中每个点的运动向量 (motion vector)。这是一个比 2D 光流更强大的概念，它直接描述了三维世界的运动场。
- 应用: 可以预测动态障碍物的短期轨迹，为规划器提供更丰富的避障信息，实现更平滑、更具预见性的避让行为。这是一个典型的 10B 级模型应用场景。
多层地图表示 (Multi-layered Map):
- 为了平衡稳定性和响应速度，可以将地图分解为多个层级：
  - 静态层 (Static Layer): 缓慢更新，包含墙壁、大型家具等长期不变的结构。可靠性高。
  - 动态层 (Dynamic Layer): 快速更新，存储被跟踪的动态障碍物的位置、速和预测轨迹。
  - 临时层 (Ephemeral Layer): 短期记忆，存储不确定或临时的障碍物（如一个刚关上的门），会随时间快速衰减。
记忆压缩与蒸馏:
- 稠密的体素地图对于长期、大范围的记忆是不可持续的。机器人需要将感知细节抽象和压缩成更高层次的知识。
- 流程: 稠密 TSDF/3DGS → 表面网格提取 (Marching Cubes) → 网格分割 (基于平面、曲率、颜色) → 物体识别与关系推断 → 构建语义场景图 (Semantic Scene Graph)。
- 场景图: 一个由节点（物体、房间、地标）和边（空间关系如“在…上”，功能关系如“可以打开”）组成的图结构。这才是 VLA 大脑进行长期任务规划、语言推理和记忆检索的核心数据结构。

9.4 由占据直达可通行性与风险场

有了 3D 地图，还需将其翻译成机器人“能懂”的导航指令。

可通行性分析 (Traversability Analysis):
- 对于 Humanoid，可通行性是复杂的。需要从局部几何中实时计算多个指标：
  - 平坦度: 局部邻域内点的法向量分布是否集中且指向上方。
  - 坡度: 拟合的局部平面的倾斜角。
  - 粗糙度: 局部邻域内点的高度标准差。
  - 台阶高度: 检测局部区域是否存在陡峭的高度不连续性。
- 综合评分: $S_{traversable} = w_1 f_{flat} + w_2 f_{slope} + w_3 f_{rough} + w_4 f_{step}$。这个分数可以被投影到一个 2.5D 的代价地图上，供全局规划器使用。
风险场 (Risk Field):
- 比二元的代价地图更精细，它为空间中的每一点都赋予一个连续的风险值。
- 构建: 风险通常是多个因素的组合： $Cost(\mathbf{x}) = w_{obs} C_{obstacle}(\mathbf{x}) + w_{unc} C_{uncertainty}(\mathbf{x}) + w_{dyn} C_{dynamic}(\mathbf{x})$
  - $C_{obstacle}$: 与最近障碍物的距离有关，通常是 ESDF 的函数，如 $e^{-\alpha \cdot ESDF(\mathbf{x})}$。
  - $C_{uncertainty}$: 与该区域的观测置信度有关。未被充分观测的区域风险更高。
  - $C_{dynamic}$: 与动态物体的预测轨迹有关，靠近预测路径的区域风险更高。
- 这个风险场是现代轨迹优化规划器的“燃料”。

9.5 占据驱动的规划：体素 A* 与体素级优化

体素 A* (Voxel A*):
- 原理: 将 A* 搜索算法从 2D 栅格扩展到 3D 体素空间。
- 挑战: 搜索空间维度诅咒。一个 $100 \times 100 \times 20$ 的体素空间比一个 $100 \times 100$ 的栅格空间大 20 倍。
- 优化:
  - 多分辨率搜索: 先在粗糙分辨率的体素图上找到一条路径，再在精细分辨率下对该路径周围进行优化。
  - 启发函数设计: 简单的欧氏距离可能不够好。可以使用预计算的 2D A* 路径长度作为更优的启发。
基于优化的规划 (Optimization-based Planning):
- 原理: 将轨迹参数化为一系列路标点或多项式，然后定义一个包含多个目标的代价函数进行优化。 $J_{traj} = \alpha J_{smooth} + \beta J_{clearance} + \gamma J_{dynamics}$
  - $J_{smooth}$: 轨迹的平滑度（如加速度、jerk的积分）。
  - $J_{clearance}$: 碰撞代价。通过在轨迹采样点上查询ESDF或风险场的值并求和得到。这是连接感知与规划的核心桥梁。
  - $J_{dynamics}$: 轨迹是否符合机器人的动力学约束。
- 算法: MPPI, CHOMP, TrajOpt 等。它们通过迭代优化，”推”动轨迹离开高代价区域，最终收敛到一条局部最优的安全平滑路径。

本章小结

从 2D 到 3D 是 Humanoid 导航的必然。本章探讨了实现这一跨越的两大核心技术路线：显式离散表示和连续隐式表示。
显式表示（稀疏 TSDF/ESDF）是当前1B 档方案的基石，它成熟、稳健且计算高效，为机器人的底层避障提供了坚实的几何基础。
连续隐式表示，特别是 3DGS，因其卓越的渲染质量、实时更新能力和显式结构，正成为10B 档方案中承载“世界模型”的理想选择，是连接 VLA 大脑与物理世界的桥梁。
从传感器到 3D 地图的构建是一条包含 VIO/SLAM、深度估计和数据融合的完整流水线，其中多目立体视觉是当前最可靠的深度获取方式。
一个有用的 3D 地图必须是动态的、分层的、并能被抽象为更高层的语义知识（场景图）。
原始的 3D 几何必须被转换为可通行性分数和风险场，才能有效地指导体素搜索或轨迹优化等高级规划算法。

常见陷阱与错误 (Gotchas)

时间戳对齐不精确 (Timestamp Misalignment): 在融合多传感器数据（如图像和 IMU）时，哪怕是几毫秒的误差，在速运动时也会导致位姿估计错误，进而造成地图重影和模糊。必须使用硬件同步或精确的软件时间戳同步协议。调试技巧：可视化 VIO 输出的轨迹，检查高速旋转时是否有明显的“抖动”或“漂移”。
传感器外参标定错误 (Incorrect Extrinsics): 机器人本体坐标系、多摄像头之间、相机与 IMU 之间的相对位姿关系（外参）若不准，TSDF 融合时会导致物体表面分层、变厚。调试技巧：将来自不同传感器的点云投影到同一世界坐标系下，观察静态物体的边缘是否对齐。
动态物体污染静态地图: 未能有效过滤动态物体，会导致 TSDF 中出现“鬼影”。调试技巧：实现一个“持久性”过滤器，一个体素必须在多个不同时间戳下被一致地观测为“占据”，才能被固化到静态层。可视化 TSDF 中每个体素的更新权重，权重低但被占据的区域很可能是动态物体。
式场的过拟合与“幻觉”: NeRF 或神经 SDF 可能会在观测稀疏的区域产生看似合理但完全错误的几何结构（例如，脑补出门后的走廊）。调试技巧：绝不完全信任隐式场。始终保留一份稀疏的原始点云作为交叉验证。在规划时，对从隐式场中提取的几何赋予一个基于观测密度的不确定性。
天真地使用稠密体素: 在项目初期直接使用 numpy.zeros((W, H, D)) 作为地图，是导致性能灾难的常见原因。调试技巧：从第一天起就基于成熟的稀疏体素库（如 Open3D 的 Voxel Hashing, gtsam, OctoMap）进行开发，否则后期重构成本极高。
ESDF 更新延迟: 在线从 Occupancy Grid 生成 ESDF 是有计算开销的。如果更新不及时，机器人可能会根据过时的 ESDF 进行规划，导致碰撞。调试技巧：监控 ESDF 更新的频率和延迟。在规划时，只使用与当前时间戳足够接近的 ESDF 地图。对于高速运动的机器人，增量式 ESDF 算法（如 Voxblox）是必需的。