v2_humanoid_navigation_tutorial

第八章 — 传统导航方案（SLAM + 栅格/图 + 规划）

开篇段落

本章将深入探讨人形机器人在室内导航中最为经典和成熟的“三段式”算法方案：SLAM（同步定位与建图）、地图表示（代价地图）与路径规划。这套体系结构清晰、模块化、可解释性强，是工业界和学术界久经考验的导航框架。即便在端到端学习方案兴起的今天，理解这一经典范式仍至关重要，因为它不仅是现代复杂导航系统的基石，其诸多组件（如代价地图、运动规划器）也常作为 VLA 等新架构的“小脑”或安全冗余。我们将从 VIO/SLAM 如何构建环境的几何与语义认知开始，过渡到何用多层代价地图精细地表达动态、静态和语义约束，最后详细拆解从 A* 搜索到模型预测控制的各类规划算法。学完本章，你将能为人形机器人设计一个鲁棒、可解释且高效的传统导航算法栈，并深刻理解其设计哲学与工程权衡。

8.1 VIO/SLAM：特征/直/语义增强的定位与建图

传统导航的第一步是回答两个基本问题：“我在哪？”（定位）和“周围环境长什么样？”（建图）。SLAM (Simultaneous Localization and Mapping) 正是为此而生。对于具备多传感器（尤其是 IMU）且运动自由度高的人形机器人，视觉惯性里程计 (VIO) 或视觉惯性 SLAM 是主流选择，因其能提供鲁棒的 6-DoF 位姿估计。

• VIO vs. SLAM：
  • VIO (Visual-Inertial Odometry) 通常指一个紧耦合的系统，融合视觉和 IMU 数据以提供高频（>100Hz）、局部一致的位姿增量估计（里程计）。它专注于短期内的平滑运动跟踪但会随时间累积漂移。
  • SLAM (Simultaneous Localization and Mapping) 则是一个更完整的系统，它不仅包含 VIO 的前端（Front-end），还拥有一个优化后端（Back-end）和闭环检测 (Loop Closure) 模块。其目标是构建全局一致的地图，并通过识别曾到过的场景来消除长期累积的漂移。
  • 在实践中，两者协同工作：VIO 前端提供实时、平滑的位姿，SLAM 后端在后台异步进行图优化和闭环修正，并将修正后的结果反馈给前端。
• 核心方法论：滤波 vs. 优化
  • 滤波法 (Filtering-based)：以扩展卡尔曼滤波 (EKF) 为代表，将机器人状态（位姿、速度、偏置等）和地图点的坐标都放入一个巨大的状态向量，通过观测不断更新状态的均值和协方差。计算效率高，但由于线性化误差和状态维度过大，精度和一致性不如优化法。
  • 优化法 (Optimization-based)：以捆集调整 (Bundle Adjustment, BA) 为核心，将 SLAM 问题建模为一个非线性最小二乘问题。它通过构建一个因子图 (Factor Graph) 来表达所有约束关系（如 IMU 预积分约束、视觉重投影约束、闭环约束），然后联合优化所有机器人位姿和地图点，以最小化全局误差。这是当前 SOTA (State-of-the-Art) SLAM 系统的标准做法。
• 视觉前端流派：
  • 特征点法 (Feature-based)：如 ORB-SLAM3，通过提取和匹配图像中的稀疏特征点（ORB, SIFT 等）来构建几何约束。优点是对光照变化和快速运动相对鲁棒，计算高效，是工业界最成熟的方案。
  • 直接法 (Direct)：如 DSO，直接利用像素块的光度不变性假设，最小化不同视角下相同场景点的光度误差。它能利用图像的全部信息（包括弱纹理区域），理论上能达到更高精度，但对相机内参、曝光时间等参数的标定精度要求极高，且对光照变化敏感。
  • 混合法：结合两者优势，例如用特征点进行长期数据关联和闭环，用直接法进行稠密建图和运动估计。
• 语义增强 (Semantic SLAM)： 纯几何 SLAM 视世界为点、线、面的集合，缺乏高级理解。语义 SLAM 将物体检测、实例分割等语义先验融入建图过程，构建一个既包含几何又包含语义的综合地图。
  • 实现路径：将 3D 物体（如椅子、桌子、冰箱）视为高质量、长期稳定的“语义地标”。在因子图中，除了视觉重投影误差，还加入“物体观测”因子。例如，一个被识别为“冰箱”的物体立方体，比一堆无差别的特征点，能提供更强的长期数据关联约束。
  • 核心优势：
    1. 动态场景鲁棒性：通过语义分割，可以预先剔除属于动态物体（如人）的特征点，只依赖静态背景和语义地标进行定位，极大提升了在人多场景下的稳定性。
    2. 数据关联强化：即使视角、光照发生剧变，一个被识别为“打印机”的物体，其语义类别提供了强大的先验，使得机器人更容易在很久之后重新认出它，从而实现更鲁棒的闭环检测。
    3. 构建人机交互地图：输出的地图直接包含 (物体类别, 实例ID, 位姿, 尺寸) 等信息，为后续的语言导航（“去冰箱拿瓶水”）提供了直接接口。

Rule-of-Thumb (经验法则)

• 对于人形机器人，由于其运动可能包含快速旋转和颠簸，紧耦合的、基于优化的 VIO/SLAM 是标准配置。IMU 不仅提供尺度信息，还能在视觉特征丢失或模糊时提供短时运动预测，是系统鲁棒性的关键。
• 在 1B 模型预算下，语义增强可通过轻量级 2D 检测器（如 YOLOv8）结果投影到 3D 实现松耦合。在 10B 预算下，可以采用一个统一的视觉基础模型，同步输出深度、语义分割、特征点，实现更紧密的“语义-几何”联合优化。

8.2 栅格地图与代价地图：动态层 + 衰减 + 通道分层

SLAM 构建了环境的静态“底图”，但导航需要一个能实时反映动态障碍物、机器人约束和任务偏好的数据结构。代价地图 (Costmap) 正是为此而生，它是一种二维或三维的栅格地图，每个栅格的值 C(x, y) 代表机器人通过该点的“成本”或“风险”。一个典型的代价地图是多个图层 (Layers) 叠加的结果。

      +-------------------------------------------------+
      | Semantic Layer (e.g., forbidden zones, speed   |  (来自语义地图和任务指令)
      |                  profiles)                      |
      +-------------------------------------------------+
                             | (叠加)
      +-------------------------------------------------+
      |      Inflation Layer (Safety Buffer)            |  (基于机器人尺寸的膨胀)
      +-------------------------------------------------+
                             | (叠加)
      +-------------------------------------------------+
      |      Obstacle Layer (Dynamic Obstacles)         |  (来自实时传感器, e.g., Depth, Sonar)
      +-------------------------------------------------+
                             | (叠加)
      +-------------------------------------------------+
      |       Static Layer (Base Map)                   |  (来自 SLAM 的全局地图)
      +-------------------------------------------------+
                                ||
      +-------------------------------------------------+
      |              Final Composite Costmap            |  (输入给路径规划器)
      +-------------------------------------------------+

• 静态层 (Static Layer)：由 SLAM 生成的全局地图（通常是 2D 占据栅格图）初始化。栅格值通常为 FREE (0), OCCUPIED (100), UNKNOWN (-1)。
• 障碍物层 (Obstacle Layer)：融合实时传感器数据。具体流程为：获深度图像 -> 根据相机位姿将其投影为 3D 点云 -> 滤除地面点和过高点 -> 将剩余点云投影到 2D 栅格地图上，标记为 OCCUPIED。同时，通过光线追踪 (Ray Tracing) 将传感器原点到障碍物点之间的栅格标记为 FREE。

• 膨胀层 (Inflation Layer)：为确保安全，在障碍物周围创建一个成本递减的缓冲区。成本 C 与离最近致命障碍物（OCCUPIED）的距离 d 相关。一个常见的模型是： $C(d) = \begin{cases} C_{lethal} & \text{if } d = 0 \ C_{inscribed} & \text{if } 0 < d \le r_{inscribed} \ C_{max} \cdot e^{-k \cdot (d - r_{inscribed})} & \text{if } d > r_{inscribed} \end{cases}$ 其中，$r_{inscribed}$ 是机器人内切圆半径，$C_{lethal}$ 是致命成本（规划器无法穿越），$C_{inscribed}$ 是内切成本（机器人本体会接触），$k$ 是衰减系数。

• 动态更新与衰减 (Dynamic Updates with Decay)：为了处理移动的障碍物（如行人），障碍层引入时间衰减机制。被观测到的障碍物占据栅格后，若后续帧未再观测到它，其成本不会立即清除，而是随时间 t 指数衰落。这既能处理短暂遮挡，又能避免“幽灵”障碍物（已离开的物体残留痕迹）永久阻塞路径。

• 通道分层与多目标成本 (Channel Stacking & Multi-objective Cost)： 为融合更丰富的导航约束，代价地图可扩展为多通道，最终成本是各通道的加权和： $C_{total}(x, y) = \max_{i} (w_i \cdot C_i(x, y))$ 或 $\sum_{i} w_i \cdot C_i(x, y)$
  • C_geometry: 基础的几何障碍成本。
  • C_social: 社交成本。基于人形跟踪（章节 5），在检测到的人周围创建一个随其移动的、非对称的成本区域（前方空间成本更高），以实现主动、礼貌的避让。
  • C_semantic: 语义成本。例如，“禁止区”（地毯、玻璃幕墙）成本设为无穷大，“偏好区”（走廊右侧、人行道）成本设一个负值（奖励）。
  • C_uncertainty: 定位不确定性成本。将 SLAM 输出的位姿协方差投影到地图上，定位不确定性越高的区域，通行成本也越高，引导机器人走向定位更稳定的区域。
  • 3D 代价表示：对于需要考虑头部、手臂碰撞的人形机器人，2D 代价地图不足。此时需要升级到 3D 占据地图（Voxel Grid）或更高效的 ESDF (Euclidean Signed Distance Field)。ESDF 每个体素存储到最近障碍物的（有向）欧氏距离，可以极快地查询任意点的碰撞风险，是 3D 轨迹优化的理想选择。

Rule-of-Thumb (经验法则)

• 代价地图分辨率是关键权衡：5cm 是室内场景的良好起点。太高（如1cm）会带来巨大计算和内存开销；太低（如10cm）则无法通过狭窄空间。
• 多层代价地图的更新频率需要分层：静态层几乎不变，障碍物层应与传感器帧率同步（10-30Hz），膨胀层则在障碍物层更新立即计算。

8.3 路径规划：A/D/Hybrid A/RRT/轨迹优化

有了代价地图，路径规划的目标就是在其中找到一条从起点到终点的总成本最低的、且符合机器人运动学约束的路径。

• 全局规划 (Global Planning)：在较大范围的地图上规划出一条宏观路径。
  • Dijkstra/A: 基于图搜索的经典算法。A 通过引入一个启发函数 $h(n)$（通常是到目标的欧氏距离）来引导搜索，效率远高于 Dijkstra。其代价函数为 $f(n) = g(n) + h(n)$，其中 $g(n)$ 是从起点到节点 n 的已知最小代价。A* 保证在 $h(n)$ 是“可接受的”（admissible，即不高于实际代价）时找到最优路径。
  • D* Lite: 增量式搜索算法，专为动态环境设计。它从目标点向起点反向搜索，当代价地图更新时，它只需更新受影响区域的路径，而非全局重算，效率极高。
  • Hybrid A*: 针对具有非完整约束（如汽车、人形机人不能瞬时横移）的系统。它在 (x, y, θ) 状态空间中搜索，使用一系列离散的运动原语（如前进、左转、右转）或连续的曲线（如 Reeds-Shepp/Dubins 曲线）来扩展节点，确保生成的路径在运动学上是可行的。
  • RRT* (Rapidly-exploring Random Tree Star): 基于采样的规划算法。它通过在状态空间中随机采样点，并逐步生长一棵连接到这些点的树来探索可行路径。它特别适用于高维（如带手臂的机器人）或有复杂约束的空间。RRT* 在找到首个可行解后，会继续优化树的连接，使其概率性地收敛到最优解。
• 局部规划 (Local Planning) / 轨迹优化 (Trajectory Optimization)： 全局路径通常只是一个粗糙的路径点序列。局部规划器在一个滚动的局部窗口内，根据实时传感器信息生成平滑、安全且可执行的控制指令（速度、角速度）。
  • DWA (Dynamic Window Approach): 在机器人的 (v, ω) 速度空间中进行前向模拟。首先，它根据机器人的动力学约束（最大/最小速度、加速度）确定一个可行的速度采样窗口。然后，对窗口内的多组 (v, ω)，向前模拟一小段时间（如1-2秒）得到候选轨迹。最后，通过一个多目标优化函数对这些轨迹评分并选择最优者。目标函数通常包含： $G(v, \omega) = \sigma(\alpha \cdot \text{heading}(v, \omega) + \beta \cdot \text{dist}(v, \omega) + \gamma \cdot \text{vel}(v, \omega))$ 其中 heading 衡量轨迹末端与全局路径的对齐程度，dist 是轨迹与最近障碍物的距离，vel 是速度大小。
  • TEB (Timed Elastic Band): 将全局路径的一部分视为一条可变形的“橡皮筋”(elastic band)，其由一系列带时间间隔的机器人位姿点构成。TEB 将局部规划问题建模为一个多目标优化问题，通过优化这些位姿点的位置，使得整条轨迹在满足运动学、动力学约束的同时，最小化路径长度、执行时间，并最大化与障碍物的距离。
  • MPPI (Model Predictive Path Integral Control): 一种先进的采样式模型预测控制方法。它通过 GPU 并行采样成千上万条带噪声的控制序列，并根据每条序列产生的轨迹成本（考虑碰撞、目标、平滑度等）进行加权平均，得到最终的“最优”控制指令。MPPI 能处理复杂的非线性系统模型和任意形式的成本函数，表现出极强的动态避障和轨迹生成能力。

8.4 全局-局部层级与重规划策略

将全局和局部规划器组合成一个分层系统，是实现鲁棒导航的标准架构。

• 分层架构与数据流：
  1. 全局规划器 (Global Planner)：运行频率较低（如 1 Hz），基于（大部分）静态的全局代价地图，规划出一条从当前位置到最终目标的宏观路径。
  2. 局部规划器 (Local Planner)：运行频率较高（如 20-50 Hz），它只关注机器周围一小块（例如 5x5 米）实时更新的局部代价地图。它以全局路径为引导（而非严格遵守），生成下一时刻的具体控制指令，并负责处理突发的动态障碍物。
  3. 恢复行为 (Recovery Behaviors)：当局部规划器持续失败（找不到有效控制指令）时触发的一系列预设动作，如原地旋转以清理传感器盲区、后退一小步以脱离卡死状态等。若恢复行为也失败，则会请求全局重规划。
• 高效重规划策略 (Replanning Strategies)： 导航过程中的环境是动态变化的，因此重规划是常态。
  • 触发条件：
    1. 路径阻塞：局部代价地图显示当前的全局路径片段已被新障碍物完全阻断。
    2. 偏离航线：机器人实际位置与全局路径的横向误差超过预设阈值。
    3. 周期性更新：即使路径仍然有效，也定期（如每 5-10 秒）进行一次全局重规划，以确保路径能反映地图的最新变化，避免陷入“次优”路径。
  • 执行策略：触发后，全局规划器会基于最新的代价地图信息重新计算路径。使用 D* Lite 这类增量式算法可以显著降低重规划的计算成本，实现近乎实时的响应。

8.5 与语义的轻耦合：地标锚点与禁止区

传统导航方案虽以几何为核心，但通过代价地图这一灵活接口，可以非常高效地与语义信息进行“轻耦合”。

• 语义代价层 (Semantic Cost Layer)：这是一个特殊的代价地图图层，其成本来源不是几何传感器，而是语义地图。
  • 禁止区 (Forbidden Zones)：通过物体识别（如识别到“玻璃门”）或人工标注，在代价地图上动态绘制成本为 LETHAL_OBSTACLE 的区域，规划器会自然绕开。
  • 偏好区与速度区 (Preferred/Speed Zones)：在走廊中，可以为右侧通道设置一个较低的成本，引导机器人“靠右行驶”。在识别为“开阔区域”的地方，可以降低成本以允许更高的速度；在“人群密集区”则提高成本，迫使局部规划器选择更慢、更保守的速度。
  • 表面依赖成本 (Surface-Dependent Cost)：通过材质识别，可以为不同地面赋予不同成本。例如，人形机器人可能在光滑瓷砖上行走稳定，但在长绒地毯上容易失衡，因此地毯区域的成本应显著提高。
• 拓扑-度量混合导航 (Topological-Metric Hybrid Navigation)： 对于大型复杂环境（如商场、医院），纯粹在巨大的度量地图（栅格图）上规划效率低下且易出错。混合导航结合了两种地图的优点：
  1. 拓扑地图 (Topological Map)：将环境抽象为由关键节点（如房间、路口、电梯口）和边（连接这些节点的走廊）组成的图。
  2. 规划过程：当收到“去 305 办公室”的指令时，首先在拓扑图上进行高层规划，得到一个节点序列（例如大厅 -> A区走廊 -> 电梯口 -> 三楼走廊 -> 305门口）。然后，度量规划器（如 Hybrid A*）只负责执行两个相邻拓扑节点之间的短距离几何路径规划。 这种分层方法极大简化了全局规划问题，使其能扩展到任意大的环境中，并提高了任务的鲁棒性。

本章小结

本章系统地介绍了传统机器人导航的“三位一体”方案，这是一个经过充分验证、高度模块化的强大框架。

• 定位与建图 (SLAM/VIO)：是所有导航任务的坐标基础，为机器人提供位姿和环境地图。基于优化的 VIO/SLAM 结合语义增强，能够构建精确、鲁棒且信息丰富的世界模型。
• 代价地图 (Costmap)：是融合几何、动态、语义和不确定性信息的枢纽。其分层、多通道的设计哲学，使其成为连接感知与规划的灵活桥梁，即使在现代 VLA 架构中也依然有其用武之地。
• 分层规划 (Hierarchical Planning)：通“全局路径引导 + 局部实时避障 + 恢复行为”的架构，实现了规划效率、反应速度与鲁棒性的精妙平衡。从 A* 到 MPPI，丰富的算法库为不同场景和机器人平台提供了多样化的选择。
• 语义轻耦合：通过语义代价层和拓扑-度量混合导航，传统方案能够有效利用现代 AI 的感知成果，完成更复杂、更人性化的导航任务，展示了其强大的扩展性和生命力。

常见陷阱与错误 (Gotchas)

1. SLAM 尺度漂移与地图跳变：
   • 陷阱：仅使用单目 VIO 会导致尺度不确定性，尤其在长廊等场景。更严重的是，当 SLAM 执行一次大的闭环修正时，地图坐标系和机器人的估计位姿会发生突变（“跳变”），这会瞬间撕裂当前的全局路径，导致导航系统崩溃。
   • 调试技巧：引入立体视觉、深度相机或腿部里程计来提供绝对尺度。关键：SLAM 模块在执行闭环跳变后，必须通过一个明确的事件通知导航栈。导航栈收到通知后，应立即清除所有旧的路径和局部代价地图，并触发一次全新的全局规划。
2. 代价地图参数调优地狱：
   • 陷阱：代价地图的参数（分辨率、膨胀半径、衰减时间、各层权重等）相互影响，构成了一个复杂的优化问题。不当的参数会导致机器人行为怪异，例如过于胆小（离墙太远）、过于鲁莽（贴障太近）或在狭窄空间“振荡”。
   • 调试技巧：建立一套可复现的仿真评估场景（包括开阔地、狭窄通道、动态障碍物等）。使用版本控制系统（如 Git）严格管理参数文件。调参时，遵循“从粗到细”的原则：先确定核心几何参数（分辨率、机器人尺寸），再调整规划器行为参数（权重、速度限制），最后优化恢复行为。
3. 坐标系混淆 (TF Hell)：
   • 陷阱：在一个完整的导航系统中，至少存在 map（世界固定坐标系）、odom（里程计漂移坐标系）、base_link（机器人本体坐标系）和多个传感器坐标系。这些坐标系之间的转换（TF）关系一旦出错，是导航系统中最常见且最隐蔽的 bug。
   • 调试技巧：始终使用可视化工具（如 RViz）将所有 TF 树、地图、传感器数据和规划路径同时显示。编写自动化脚本来检查 TF 树的连通性和更新频率。rosrun tf tf_echo map base_link 是检查 map 到 base_link 最终变换的利器。
4. 全局与局部规划器“目标冲突”：
   • 陷阱：全局路径可能穿过一个局部规划器认为无法通过（但理论上能过）的狭窄区域。局部规划器会不断尝试，但由于其目标函数的设计（例如，避障权重过高），它永远无法生成一条“勇敢”穿过的轨迹，导致机器人卡在入口处。
   • 调试技巧：确保全局规划器在规划时使用的机器人“足迹”(footprint) 模型至少与局部规划器一样大，甚至更保守一些。引入“路径可行性检查”模块，对全局路径的每个点，都在一个局部窗口内评估其可通行性。对于局部规划器，考虑设计动态调整参数的策略，例如，在接近全局路径上的狭窄通道点时，临时降低避障权重。
5. 计算性能瓶颈：
   • 陷阱：高分辨率的（尤其是3D）代价地图、高频率的传感器数据处理、以及基于采样的规划器（如 MPPI）都对计算资源要求很高。CPU/GPU 负载过高会导致系统延迟增加，使得机器人的反应“慢半拍”，在动态场景中极易发生碰撞。
   • 调试技巧：持续进行系统性能剖析（profiling）。使用多分辨率地图（在远离机器人的区域使用低分辨率）。对点云数据进行体素下采样（Voxel Grid Downsampling）。优化代码，例如使用 GPU 加速代价地图更新和轨迹采样。根据系统负载动态调整规划频率或局部窗口大小。