v2_humanoid_navigation_tutorial

第二章：形式化与指标

开篇段落

本章的目标是为 Humanoid 室内导航建立一套精确、可度量的语言。在构建复杂的智能系统时，脱离了严谨的定义和量化的指标，我们的工作就会退化为基于直觉的“炼丹”。“感觉还行”和“基本能用”是无法指导算法迭代，更无法满足商业化落地对可靠性和安全性的苛刻要求的。本章中，我们将深入探讨部分可观察马尔可夫决策过程（POMDP）这一强大的理论框架，它不仅能帮助我们精确地描述导航问题中的不确定性，还能清晰地界定系统各模块的职责。随后，我们将系统性地梳理从纯几何到富含语义的各类地图表示方法，并分析其在不同场景下的利弊。最后，也是本章的实践核心，我们将定义一套从学术界到工业界广泛认可的评估指标与评测协议。这套体系将成为我们的“罗盘”，确保每一次算法优化、每一次数据迭代，都是在朝着一个可衡量、可复现、可信赖的目标前进。

2.1 状态/观测/动作空间的形式化（POMDP/决策过程）

为了从根本上理解导航的挑战，我们将其置于部分可观察马爾可夫决策过程 (Partially Observable Markov Decision Process, POMDP) 的框架下进行分析。这并非要求我们去完整求解一个POMDP（这在计算上通常是不可行的），而是借用它的语言来精确地剖析问题。POMDP是处理“带有不确定性的序贯决策”问题的标准模型，完美契合机器人导航的本质。

一个 POMDP 由一个七元组定义：$(S, A, T, R, \Omega, O, \gamma)$

状态空间 $S$ (State Space): 述了宇宙的完整且真实的状态，这是机器人希望知道但永远无法完全获知的“上帝视角”信息。一个状态 $s \in S$ 包含：
- 机器人自身状态: 不仅仅是其在全局坐标系下的六自由度位姿 $p_{robot} \in SE(3)$（即位置 $x, y, z$ 和姿态 roll, pitch, yaw），还包括其线速度、角速度，甚至其内部关节的角度和角速度。
- 环境动态状态: 场景中所有动态实体的完整状态，例如，每个人的精确位置、速度、意图，甚至视线方向。
- 环境静态状态: 所有静态物体（墙壁、家具）的精确几何形状、位置、物理材质（如摩擦系数、反光度）以及语义属性（“这是一把椅子”）。
- 关键洞察: 状态 $S$ 是一个庞大到无法想象的向量。导航算法的本质，就是用一个紧凑的内部模型去近似这个真实但不可知的状态。
动作空间 $A$ (Action Space): 机器人可以执行的令集合。动作空间的设计直接影响了规划的难度和控制的平顺度。
- 离散动作空间: move_forward(0.5m), turn_left(30deg), stop。简单、易于高层规划（如图搜索），但可能导致动作不连贯、机械感强。
- 连续动作空间: set_velocity(v_x, v_y, \omega_z)。通常用于底层控制器，能够产生平滑的轨迹，但规划空间是无限的，需要采样或优化方法。
- 技能/原语动作空间: traverse_doorway(), follow_person("Alice"), dock_at_charger()。这是更高层次的抽象动作，将一系列低层控制打包成一个有意义的技能。VLA模型尤其擅长生成这类动作序列。

**转移函数 $T(s’

s, a) = P(s_{t+1}=s’

s_t=s, a_t=a)$**: 这是世界的“物理引擎”。它描述了在状态 $s$ 执行动作 $a$ 后，世界将以何种概率转移到新状态 $s’$。这个函数捕捉了：

机器人动力学: 执行 move_forward(0.5m) 可能因为轮子滑，实际只前进了0.48m。
环境的自主动态: 即使机器人静止，场景中的人也会自己走动。
动作的副作用: 机器人开门这个动作，改变了门的状态。

奖励函数 $R(s, a)$: 定义了任务的目标和偏好，是指导机器人行为的“价值函数”。一个精心设计的奖励函数至关重要。
- 稀疏奖励: 仅在任务最终完成时给予一个大的正奖励（例如，+100），在失败时给予负奖励（碰撞 = -50）。易于定义，但学习/规划效率低。
- 塑形奖励 (Shaped Rewards): 提供更密集的引导信号。例如，每秒钟消耗 -0.1 (时间成本)，每米消耗 -0.5 (能量成本)，与目标的距离每减小1米 +1.0 (进度激励)。
- 多目标奖励: 奖励函数是多个项的加权和，例如 $R = w_1 R_{goal} + w_2 R_{time} + w_3 R_{safety}$，权重 $w_i$ 体现了任务的优先级。
观测空间 $\Omega$ (Observation Space): 机器人传感器读数的集合，是机器人感知世界的唯一窗口。一个观测 $o \in \Omega$ 是一个包含多种信息的复合体：
- 多视角 RGB 图像、深度图、红外图。
- IMU 的三轴加速度和三轴角速度读数。
- 激光雷达扫描出的 3D 点云。
- 轮式里程计或腿部运动学解算出的自身运动估计。
- 麦克风阵列捕捉到的声音信号。

**观测函数 $O(o

s’, a) = P(o_{t+1}=o

s_{t+1}=s’, a_t=a)$**: 这是“传感器模型”，描述了传感器读数 $o$ 与真实世界状态 $s’$ 之间的概率关系。它刻画了传感器的不完美性：

噪声: 图像的噪点，IMU 的零偏漂移。
歧义: 从单个2D图像中无法确定物体的精确距离。
漏检: 在强光下，摄像头可能无法检测到远处的物体。

折扣因子 $\gamma \in [0, 1)$: 体现了对未来的“耐心程度”。$\gamma$ 接近1表示机器人更有远见，会为长远的巨大利益而牺牲眼前的小利。$\gamma$ 接近0则表示机器人更“短视”。

在POMDP框架下，机器人通过维护一个置信度分布 (Belief State) $b(s)$ 来应对状态的不确定性。$b(s)$ 是一个在所有可能的状态 $s$ 上的概率分布。每当执行一个动作 $a_t$ 并获得一个观测 $o_{t+1}$，机器人就通过贝叶斯滤波来更新其置信度： $b_{t+1}(s') = \eta \cdot O(o_{t+1} | s', a_t) \int_{s \in S} T(s' | s, a_t) b_t(s) ds$ 其中 $\eta$ 是归一化常数。导航策略 $\pi$ 的目标就是找到一个从置信度 $b$ 到动作 $a$ 的映射 $\pi(b) \rightarrow a$，以最大化期望的累积折扣奖励。

Rule-of-Thumb: 永远不要试图在工程中实现一个完整的POMDP求解器。它的价值在于提供了一个思考问题的通用语言。在实践中，我们将问题分解：

SLAM/定位模块 的职责就是近似地计算和更新置信度 $b(s)$ 中关于机器人位姿的部分。

感模块 负责从观测 $o$ 中提取关于环境中其他物体状态的摘要信息。

规划模块 则基于当前置信度（通常是其峰值，即最可能的状态）来选择最优动作，近似求解策略 $\pi$。这个框架帮助我们清晰地划分模块，并管理每个环节的不确定性。

2.2 地图表示：拓扑/度量/语义/占据/混合图

地图是机器人对环境状态 $S$ 的一个长期、稳定的内部表示。选择合适的地图表示是导航系统设计的基石。

地图类型	核心思想	典型实现	优点	缺点	主要用途
度量地图	按几何坐标精确描述空间	2D/3D栅格图, 点云图, Costmap2D	几何精度高，利于精细避障	存储/计算开销大，对环境变化敏感，缺乏高层语义	局部路径规划、避障
拓扑地图	将空间抽象为节点和连通关系图	G=(V,E)，V=房间/门口，E=连通性	内存占用小规划速度快，对几何扰动不敏感	丢失几何细节，无法直接用于避障	全局路径规划、长时程任务规划
语义地图	在地图上标注对象的类别和属性	带标签的点云/体素，图数据库中的节点属性	让机器人“理解”环境，支持语言交互	依赖感知的准确性，标注成本高	语义导航（“去冰箱”），人机交互
占据地图	用概率描述空间单元被占用的信度	Occupancy Grid, OctoMap, TSDF/ESDF	优雅地融合多源/不确定信息，处理动态环境	高分辨率下内存开销巨大，无法表达物体身份	探索、建图、动态避障

在现代系统中，单一的地图表示已无法满足需求，混合地图 (Hybrid Map) 成为事实标准。它通常采用分层结构：

      +-------------------------------------------------+
      |  (高层)  任务规划 / VLA 大脑                     |
      |  使用: 拓扑-语义图 (Topological-Semantic Graph)   |
      |  示例: ("厨房")--[门口]-->("客厅")--[沙发附近]--> ... |
      +-----------------------|-------------------------+
                              | (全局路径)
      +-----------------------|-------------------------+
      |  (中层)  全局路径规划 (A*, D*)                    |
      |  使用: 粗粒度代价地图 / 简化的拓扑图            |
      +-----------------------|-------------------------+
                              | (局部目标点)
      +-----------------------|-------------------------+
      |  (底层)  局部规划与避障 (DWA, MPC)               |
      |  使用: 高分辨率、多层代价地图 (Costmap)         |
      |        - 静态障碍物层 (来自 SLAM 地图)          |
      |        - 动态障碍物层 (来自实时传感器)          |
      |        - 语义禁区层 (来自语义地图)              |
      |        - 社交距离膨胀层                         |
      +-------------------------------------------------+

Rule-of-Thumb: 设计导航系统时，应从混合地图的架构出发。高层负责“去哪里”（战略），中层负责“怎么走”（战役），底层负责“别撞上”（战术）。各层地图的数据流和更新频率需要精心设计。

2.3 任务模板

任务模板是将用户的模糊意图转化为机器可执行的、结构化的目标表示。一个好的任务模板系统应具备表达力、组合性和可扩展性。

点到点导航 (Point-to-Point): GOTO(target: PoseSE2/PoseSE3, tolerance: float)
- 这是最基础的原子任务，所有复杂导航的基础。
语义目标导航 (Semantic Goal): GOTO(target: SemanticLabel, constraint: str = None)
- target 可以是对象实例 ("张三的杯子")，对象类别 ("任意椅子")，或区域 ("厨房水槽附近")。
- constraint 可以是 "最近的一个" 或 "视野内可见的那个"。
社交约束导航 (Socially-Constrained): NAVIGATE(goal, social_rules: List[Rule])
- FOLLOW(person_id: "Alice", distance_range: [1.5m, 2.0m])
- AVOID_PROXIMITY(person_id: "Bob", radius: 2.5m)
- ESCORT(person_id: "VIP_Guest", to: "exit", formation: "lead_by_1m")
程序化/多步任务 (Procedural/Multi-step): EXECUTE_PLAN(steps: List[Task])
- 一个典型的送物任务可以被分解为：
  1. GOTO("办公室")
  2. SEARCH_OBJECT("文件A")
  3. CONFIRM_WITH_USER("是这个吗？")
  4. PICKUP("文件A")
  5. GOTO("会议室B")
  6. HANDOVER_TO_PERSON("李四")

Rule-of-Thumb: 将任务模板设计成一种领域特定语言 (DSL)。这使得非算法工程师（如产品经理）也能通过组合这些模板来定义复杂的机器人行为，同时也为VLA模型提供了一个清晰、结构化的输出目标。

2.4 指标与基准 (Metrics & Benchmarks)

没有测量，就没有优化。以下是评估导航算法性能的行业标准标。

指标	缩写	定义	为何重要
成功率	SR	任务完成次数 / 总任务次数。成功通常定义为与目标的距离小于某个阈值 $d_{succ}$。	最顶层的有效性指标，衡量“能不能做到”。
路径加权成功率	SPL	$\frac{1}{N} \sum_i S_i \frac{L_i^{\text{opt}}}{ \max(L_i^{\text{actual}}, L_i^{\text{opt}}) }$	衡量“效率”的黄金指标。惩罚了绕路和失败，是综合评价导航质量的最佳单一度量。
碰撞率	CR	发生碰撞的任务次数 / 总任务次数。	安全性的生命线。在任何场景下，CR都应被严格控制在极低的水平。
导航误差	NE	任务成功时，终点位置与目标位置的欧氏距离。	衡量“精度”。对于需要精细对接的任务（如充电）至关重要。
路径长度	PL	机器人实际行走的路径总长度。	衡量“经济性”。直接关系到任务耗时和能耗。
路径平顺度	Jerk	机器人轨迹的加速度变化率（或更高阶导数）的积分。	衡量“舒适性”和“稳定性”。高Jerk意味着频繁的加减速和转向，对人形机器人稳定性是巨大考验，也让旁观者感到不安。
推理延迟	Latency	从传感器输入到控制指令输出的端到端时间。	决定了机器人的反应速度。在动态避障场景中，低延迟是避免碰撞的前提。
Token预算	Tokens	（仅VLA）单次决策消耗的prompt token和generation token总数。	VLA模型的“计算成本”和“能耗”代理指标。

Rule-of-Thumb: 建立一个指标仪表盘。将 SPL 作为核心优化目标 (Primary Metric)，将 CR, Latency, Jerk 作为守护指标 (Guardrail Metrics)。任何对 SPL 的提升如果导致守护指标的显著恶化，都应被视为无效或危险的改动。

2.5 离线与在线评测协议

算法的迭代需要一个闭环、安全的评测流程

离线评测 (Offline Evaluation / Replay)
- 流程: 使用机器人采集大量包含各种场景的传感器日志（如rosbags）。在服务器上，将这些日志数据“喂”给新的导航算法，记录其决策序列，并计算上述离线指标（SR, SPL等）。
- 优点: 绝对安全、成本极低、可100%复现、可进行大规模并行测试，是算法快速迭代的基石。
- 陷阱 (开环问题): 离线评测是“开环”的。算法做出的决策无法影响后续的传感器输入。例如，算法决定向左避障，但日志中的下一帧图像仍然是机器人撞上去的视角。这会导致对算法的纠错能力和长期行为的评估出现偏差。
仿真评测 (Simulation Evaluation)
- 流程: 在高保真仿真环境（如NVIDIA Isaac Sim, Gibson）中运行导航算法。
- 优点: 闭环测试，可以评估算法的长期影响。可以程序化地生成海量、多样化甚至危险的测场景。
- 陷阱: 现实-仿真差距 (Sim-to-Real Gap)。仿真器无法完美模拟真实世界的光照、物理材质和传感器噪声，在仿真中表现优异的算法在现实中可能失效。
在线评测 (Online Evaluation)
- 影子模式 (Shadow Mode): 线上机器人由稳定可靠的旧算法A驱动。新算法B在后台并行运行，接收与A完全相同的输入，并记录下它“本应”做出的决策。通过对比A和B的决策日志，可以在不影响线上服务的前提下，安全地评估B的表现。
- A/B 测试: 将一小部分机器人流量（例如，5%的测试机器人）切换到新算法B，而其余机器人继续使用算法A。在真实环境中运行一段时间后，收集并对比两组的各项指标，以统计上显著的方式判断B是否优于A。这是算法上线前的最终“大考”。
- 灾难回退机制: 为在线评测配备强大的监控告警系统。一旦新算法组的守护指标（如碰撞率、系统崩溃率）突破预设的安全阈值，立即自动将该组的算法版本回滚至稳定的旧版A，并向工程师发出警报。

Rule-of-Thumb: 遵循“离线回放 -> 仿真评测 -> 影子模式 -> 小流量A/B -> 全量部署”的渐进式评测流程。任何跳过中间环节的行为都是对安全和稳定的不负责任。

本章小结

本章为整个课程铺设了理论和度量的基石。我们借助POMDP框架，获得了剖析复杂导航问题的“手术刀”，能够精确定义状态、动作、观测以及它们之间的不确定关系。我们系统地学习了从几何到语义的多种地图表示方法，并明确了混合分层地图是现代导航系统的必然选择。最重要的是，我们建立了一套包含有效性（SR, SPL）、安全性（CR）、效率（PL, Latency）和舒适性（Jerk）的完整指标体系，以及一套从离线到在线的、层层递进的安全评测协议。有了这些工具，我们接下来的算法探索将不再是盲人摸象，而是有图可循、有尺可量的科学工程。

常见陷阱与错误 (Gotchas)

将POMDP框架与具体算法混为一谈: 新手常犯的错误是试图去“实现”一个POMDP。请牢记，POMDP是用于分析问题的数学模型，而不是一个可以直接编码的算法。实际系统是用SLAM、感知、规划等工程模块来近似POMDP的解。
SPL至上主义: 过分追求单一的SPL指标，可能会训练出“艺高人胆大”的机器人，它会以极小的间隙贴着障碍物和人行走，以获取最短路径。这在现实中是极其危险且令人反感的。必须用碰撞率、社交距离保持率等安全指标作为硬性约束。
对离线评测结果的盲目自信: 离线评测的高分是上线的必要非充分条件。由于开环问题，离线评测无法检验算法在真实物理互动中的鲁棒性。一个在离线日志中表现完美的算法，可能在现实中为一个微小的未建模效应（如地面反光）而彻底崩溃。
基准（Benchmark）选择不当: 用于计算SPL的“最优路径” $L_{opt}$ 是如何定义的？如果用简单的A*在栅格图上的路径作为最优解，那么一个能走出更平滑、更短曲线的先进算法的SPL反而可能会受影响。最优路径的定义本身就应是一个经过深思熟虑的、尽可能接近现实物理极限的基准。
忽略“无解”场景的评估: 评测数据集里充满了从A点到B点的“可解”任务，但现实中机器人常会收到无法完成的指令（例如，“去一个不存在的房间”）。算法在这种情况下应该如何表现？是原地报告失败，还是进行探索？对这些“失败模式”的优雅处理能力，也应纳入评测范围，例如通过“任务拒绝率”或“探索成功率”等指标来衡量。