v2_humanoid_navigation_tutorial

chapter20.md — 进阶场景库与策略

20.0 开篇段落

前面的章节为我们构建了一个能在理想化、结构化的环境中完成基本导航的算法框架。然而，要使 Humanoid 从实验室的受控环境迈向混乱、动态且充满意外的真实世界，就必须攻克一系列“进阶场景”。这些场景是检验导航系统鲁棒性、智能性和社会适应性的试金石，也是区分“技术演示”与“可靠产品”的核心所在。本章将深入剖超控动态交互设施（电梯、闸机）、破解感知陷阱（玻璃、镜面）、适应极端环境变化（光照骤变）、处理复杂社会交互（狭路会车），以及实现任务级的失败与恢复。本章的目标不仅是识别这些长尾问题，更是为它们提供一套层次分明、原理清晰、且能在不同算力预算（1B/10B）下权衡的算法策略。

20.1 升降梯/旋转门/闸机：状态观测与交互协议

这类场景的本质挑战在于，机器人必须与一个具有内部逻辑和隐藏状态的外部“智能体”进行精确协同。这超越了简单的几何避障，需要一个能够理解并执行特定“社会协议”的决策系统。我们将此问题域称为状态化交互导航 (Stateful Interactive Navigation)。

核心算法：分层有限状态机 (Hierarchical Finite State Machine, HFSM) 与感知锚点

我们将交互过程建模为一个分层的 FSM。顶层 FSM 管理宏观流程（如“使用电梯”），而其内部状态可以自身就是一个更小的 FSM（如“按按钮”这个动作包含“定位按钮”、“伸出机械臂”、“确认按压”等子状态）。

以乘坐电梯为例，一个详尽的 FSM 流程：

+-----------------------------+     User Command: "Go to Floor 5"     +-----------------------------+
|           IDLE              |------------------------------------->|     NAV_TO_ELEVATOR_LOBBY   |
| (Awaiting Task)             |<-------------------------------------| (Path to call panel area)   |
+-----------------------------+    Task Fail/Cancel                  +-----------------------------+
                                                                                  |
                                                                                  | Goal Reached
                                                                                  v
+-----------------------------+ <----------------------------------- +-----------------------------+
|    MONITOR_ELEVATOR_ARRIVAL |   Press Fail (Timeout/Occluded)    |      EXECUTE_CALL_SEQUENCE  |
| (Listen for 'ding', watch door)|----------------------------------->| (Detect button, align, press) |
+-----------------------------+   Press Success                    +-----------------------------+
        |
        | Door_State == OPEN and Path_Clear == TRUE
        v
+-----------------------------+ <----------------------------------- +-----------------------------+
|        ENTER_ELEVATOR       |      Entry Path Blocked            |       VALIDATE_ENTRY_PATH   |
| (Move inside carefully)     |------------------------------------| (Check for people, obstacles) |
+-----------------------------+      Path is Clear                 +-----------------------------+
        |
        | Entry Complete
        v
+-----------------------------+ <----------------------------------- +-----------------------------+
|      WAIT_FOR_DOOR_CLOSE    |      Panel Not Found               |   NAV_TO_INTERNAL_PANEL     |
| (Wait for door to shut)     |------------------------------------| (Locate floor buttons)      |
+-----------------------------+      Panel Found                   +-----------------------------+
        |                                                                         |
        | Door_State == CLOSED                                                    | Press '5' button
        v                                                                         v
+-----------------------------+                                     +-----------------------------+
|      MONITOR_FLOOR_CHANGE   |                                     |    EXECUTE_PRESS_SEQUENCE   |
| (OCR screen, listen to voice) |                                     | (Press target floor button) |
+-----------------------------+                                     +-----------------------------+
        ^
        | Target_Floor_Reached == TRUE
        v
+-----------------------------+
|         EXIT_ELEVATOR       |
| (Wait for door, then exit)  |
+-----------------------------+

感知锚点 (Perceptual Anchors): FSM 的状态转换完全由一系列专用的感知模块触发，我们称之为“感知锚 ઉ 点”。

1B vs. 10B 方案对比:

20.2 玻璃门/镜面/透明物体：占据不确定性与光学陷阱

这类材质对主流的几何传感器（LiDAR, ToF, Structured Light）构成了根本性挑战，因为它们违反了“表面反射”的基本假设。处理不当会导致机器人直接撞墙，或在 SLAM 地图中构建出不存在的“幻影空间”。

核心算法：多模态线索冲突检测与不确定性建模

解决该问题的核心思想是：任何单一传感器都不可信，必须建立一个交叉验证框架，主动寻找不模态信息之间的矛盾，并将这种矛盾量化为导航风险。

  1. 几何-语义冲突 (Geometry-Semantic Conflict):
    • 数据流:
      1. 几何路径: 深度传感器数据 → 3D 点云 → 地面分割 → 可通行区域（Traversable Space）判定。结果：“前方 5 米内为平坦自由空间”。
      2. 语义路径: RGB 图像 → 语义分割/开集检测模型 → 像素级标签。结果：“前方区域被标记为 glass_door, window, mirror”。
    • 冲突决策逻辑: 
      # 伪代码
for region in image:
  is_traversable = geometry_module.check(region) # -> True
  semantic_label = semantic_module.predict(region) # -> "glass_door"
  if is_traversable and semantic_label in GHOST_OBJECTS:
    occupancy_map.set_cost(region, costs.HIGH_UNCERTAINTY)
    • GHOST_OBJECTS 是一个预定义的、包含“玻璃”、“镜子”、“高透明亚克力板”等汇的集合。
  2. 运动-外观一致性检验 (Motion-Appearance Consistency Check):
    • 应用场景: 专门用于检测镜面。
    • 原理: 机器人自身运动（Ego-motion）与其在反射面中“镜像”的运动之间存在严格的数学关系。真实世界中的物体运动与机器人自身运动通常是解耦的。
    • 算法:
      1. 通过 VIO 或轮式里程计获取机器人自身的运动 T_ego = (R, t)。
      2. 在 RGB 图像上计算稠密光流场 F。
      3. 对图像中的候选平面区域（可通过 RANSAC 等方法拟合），验证该区域内的光流是否可以用 T_ego 和平面参数 (n, d) 来解释。如果光流场与机器人自身运动引起的反射光流高度相关，则该平面有极高概率是镜面。
      4. Score_mirror = correlation(F_region, project(T_ego, plane))
    • 结果: 将高 Score_mirror 的区域在 SLAM 和规划的代价图中标记为不可信区域，应被排除在地图构建之外。
  3. 主动探测与物理试探 (Active Probing & Physical Interaction):
    • 场景: 当以上方法均无法给出确定性结论，且任务优先级极高时。
    • 策略:
      1. 低速接近: 机器人以极低速度（如 0.05 m/s）向不确定区域移动。
      2. 接触感知: 利用末端执行器（如果配备）或机身上的碰撞传感器/力矩传感器进行轻微接触。
      3. 决策: 一旦检测到物理接触，立即停止并更新地图，将该不确定区域标记为 occupied。这是一个高风险的最后手段，需要严格的安全限制。

1B vs. 10B 方案对比:

20.3 夜间/突发照明变化：鲁棒感知与紧急降级

视觉算法，特别是依赖特征点匹配的 V-SLAM 和 VIO，对光照条件极为敏感。光照的剧烈变化（如开关灯、进出隧道）或极端条件（夜间、过曝）会导致特征点大量丢失，从而引发定位失败和感知系统崩溃。

核心算法：感知质量自评估与分级降级策略 (Perception Quality Self-Assessment & Tiered Degradation Strategy)

系统必须具备内省能力，即实时、量化地评估其核心感知模块的健康状况，并据此自动切换到预设的、不同等级的运行模式。

  1. 感知质量量化指标 (Quality Metrics):
    • VIO/SLAM 健康度:
      • Num_Tracked_Features: 持续跟踪的特征点数量。低于某个阈值（如 < 50）则为危险信号。
      • Feature_Distribution_Entropy: 特征点在图像上的分布均匀度。如果特征点聚集在一小块区域，说明场景纹理单一，定位不可靠。
      • Reprojection_Error_Variance: 重投影误差的方差。方差突然增大意味着匹配质量下降或出现外点。
    • 深度学习模型健康度:
      • Confidence_Score_Histogram: 模型输出置信度的均值和方差。均值持续偏低表明模型对当前输入不确定。
      • Uncertainty_Estimation: 对于支持不确定性估计的模型（如使用 Monte Carlo Dropout），可以直接量化模型的不确定性。
    • 图像质量:
      • Histogram_Entropy: 图像直方图的熵，可以衡量图像的对比度。
      • Exposure_Level: 图像的平均亮度，判断是否过曝或暗。
  2. 分级降级状态机 (Tiered Degradation FSM):
    • L1 - NOMINAL (正常模式): 所有质量指标均在健康范围内。全功能运行，速度、规划策略正常。
    • L2 - CAUTIOUS (谨慎模式): 部分指标开始恶化（如特征点数下降 20%）。
      • 行动: 最大速度/加速度限制降低 50%。碰撞安全距离加倍。
      • 融合策略: 动态增加视觉测量在多传感器融合（如 EKF）中的噪声协方差 R_vision，从而让系统更相信 IMU 和轮式里程计的推算。
      • W_vision ∝ 1 / QualityScore_vision
    • L3 - SAFE_STOP (安全停止模式): 关键指标跌破生存阈值（如 VIO 连续 1s 无法收敛）。
      • 行动: 立即以平滑的减速度曲线停止运动。
      • 状态: 挂起所有上层导航任务，进入“重定位”子程序。
      • 重定位: 执行预设的恢复动作，如原地 360° 慢速旋转，以期在某个视找到足够的特征重新定位。
    • L4 - MANUAL_RECOVERY (人工干预): 重定位尝试多次失败。
      • 行动: 放弃自主恢复，通过蜂鸣、灯光和网络通知向操作员请求远程接管或现场援助。

Rule-of-Thumb: 系统的安全性不取决于它在最佳状态下能跑多快，而在于它在最差状态下能否安全地停下来。降级策略是主动安全的核心，必须在设计阶段就作为一等公民来考虑。

20.4 狭窄通道会车与社交导航

在拥挤空间中与人或其他机器人交互，纯粹的几何避障是远远不够的。这会导致机器人行为生硬、不自然（如在人面前突然急停），甚至引发死锁（如在门口与人对峙）。成功的社交导航要求机器人能够理解并遵守隐含的社会规范。

核心算法：预测性规划与社会规范代价函数 (Predictive Planning with Social Norm Cost Functions)

其核心是将不可见的社会规则，通过数学建，转化为规划器可以理解和优化的“代价”。

  1. 行人意图与轨迹预测:
    • 短期预测 (0-3s): 使用基于学习的模型（如 Social-LSTM, AgentFormer）预测每个行人的未来轨迹分布，而不仅仅是一条单一路径。这些模型会考虑人与人之间、人与环境之间的交互。
    • 长期意图预测: 预测行人的最终目的地（如门口、座位）。这可以通过分析场景的语义（如功能区域）和行人的姿态、朝向来实现。
  2. 社会代价函数 C_social: 规划器的总代价函数 Cost_total = w_path * C_path + w_obs * C_obs + w_social * C_social。其中 C_social 由多个部分组成：
    • 个人空间 (Personal Space): C_personal(p) = Σ_i A_i * exp(-d(p, person_i)^2 / (2σ_i^2)) 这是一个以每个行人 i 为中心的高斯惩罚场。关键在于 σ_i 不是常数，而是各向异性的：
      • σ_front > σ_side > σ_back，即人们对前方的侵犯更敏感。
      • σ 会根据行人的速度 v 动态调整，速度越快，前方的安全距离要求越大。
    • 群体代价 (Group Cost): 检测到成组行走的人群时，机器人应避免从中穿过，而是选择绕行。这可以通过在人群的凸包（convex hull）内设置一个额外的代价项来实现。
    • 礼让与路权 (Yielding & Right-of-Way):
      • 场景: 在狭窄通道入口，机器人 R 与行人 H 即将相遇。
      • 决策逻辑: 计算双方到达冲突点 P_c 的预计时间 T_R 和 T_H。 
        IF |T_R - T_H| < threshold_time: # 可能冲突
  IF distance(H, P_c) < distance(R, P_c): # 对方更近
    robot_action = YIELD # 机器人让行
  ELSE:
    robot_action = PROCEED_CAUTIOUSLY # 机器人先行
      • 这种基于规则的决策可以作为 VLA 模型的备选或验证层。

1B vs. 10B 方案对比:

20.5 任务级容错：中断、恢复、优先级与多任务调度

导航只是实现更复杂任务（如送货、引导）的一个子环节。任务执行链条可能因各种外部因素而中断（目标点无法到达、收到新指令等）。系统必须具备健壮的任务管理能力，以优雅地处理这些中断。

核心算法：行为树 (Behavior Trees, BT) 与抢占式任务调度器

相比于容易产生“状态爆炸”问题的 FSM，行为树为复杂、分层的任务逻辑提供了更模块化、可组合和响应式的解决方案。

为何选择行为树 (BT) 而非 FSM?

一个更复杂的“取物”任务 BT 示例:

-> Sequence (Fetch Coffee Task)
|
+--> ? Selector (Get to Kitchen)
|    |
|    +--> -> Sequence (Primary Route)
|    |    |
|    |    +--> PathToKitchenExists?
|    |    +--> NavigateTo(Kitchen)
|    |
|    +--> -> Sequence (Recovery: Ask for Help)
|         |
|         +--> Announce("The path to the kitchen is blocked.")
|         +--> WaitForHumanHelp(timeout=60s)
|
+--> ? Selector (Operate Coffee Machine)
|    |
|    +--> -> Sequence (Main Operation)
|    |    |
|    |    +--> Localize(CoffeeMachine)
|    |    +--> PressButton("Brew")
|    |
|    +--> FailSafe("Failed to operate coffee machine.")
|
+--> NavigateTo(UserLocation)

抢占式调度 (Preemptive Scheduling):

  1. 任务优先级: 每个任务（如 FetchCoffee, EmergencyStop）都被赋予一个先级。
  2. 中断处理: 当一个更高优先级的任务（如用户发出的 EmergencyStop 指令）到达时：
    • 当前正在执行的低优先级任务（FetchCoffee 的 BT）被挂起 (Suspend)。
    • 其完整的执行上下文（当前活跃的节点路径、所有内部变量）被序列化保存。
    • 高优先级任务的 BT 被加载并开始执行。
  3. 恢复与中止 (Resume & Abort):
    • 当高优先级任务完成后，调度器可以根据策略决定如何处理被挂起的任务：
      • 恢复 (Resume): 从保存的上下文中恢复，继续执行。
      • 中止 (Abort): 彻底丢弃该任务及其上下文。
      • 重启 (Restart): 从头重新开始执行该任务的 BT。

本章小结

常见陷阱与错误 (Gotchas)

  1. FSM 交互死锁 (Interaction Deadlock): 在双向闸机或狭窄门口，两个都采用“礼让”策略的机器人可能陷入“您先请”—“不，您先请”的无限循环。解决方案: 引入非对称的决策规则。例如，机器人 ID 较小的一方拥有优先通行权，或在等待超时后引入一个随机的退让或前进决策，以打破对称性僵局。
  2. SLAM 地图被镜面永久污染: 如果在建图阶段未能有效识别镜面，SLAM 系统会在地图中创建一个与真实世界对称的“镜像世界”。这个错误一旦被固化到全局地图中，就极难清除。解决方案: 在建图流程中集成在线镜面检测。一旦检测到镜面，应立即将该区域的特征点标记为“不可靠”，并阻止它们参与后续的姿态优化和地图构建。可以考虑使用主动式检测，如投射特定模式的结构光，观察其在镜面中的反射是否符合预期。
  3. 安全降级模式震荡 (Mode Thrashing): 如果进入和退出“谨慎模式”的感知质量阈值设为同一个值，当质量在该值附近轻微波动时，系统会频繁地在“正常”和“谨慎”模式间切换，导致机器人行为不稳定（反复加减速）。解决方案: 采用滞后比较 (Hysteresis)。设置两个阈值：一个较低的进入阈值 T_enter 和一个较高的退出阈值 T_exit。只有当质量低于 T_enter 时才进入谨慎模式，而必须当质量恢复到 T_exit 以上时才能返回正常模式。
  4. 任务中止后的资源泄露 (Resource Leak on Abort): 一个被中止的任务可能在系统中留下了“幽灵”状态，例如，在全局代价地图上添加了一个临时的禁行区但未被清除。解决方案: 实施严格的生命周期管理，采用类似 RAII (Resource Acquisition Is Initialization) 的设计模式。每个任务对象在创建时获取资源（如修改代价地图的句柄），并在其析构函数中（无论任务是正常完成还是被中止）确保所有资源被正确释放
  5. 对 VLA 的盲目信任 (Blind Trust in VLA): 完全依赖一个庞大的端到端模型进行决策，可能会因模型的“幻觉”或在分布外（OOD）场景中的失效而导致灾难性后果。解决方案: 建立一个安全验证层 (Safety Verification Layer) 或 行为护栏 (Behavioral Guardrail)。VLA 的输出（如一个高层计划或一个具体的动作）在执行前，必须经过一个由简单、可靠、易于验证的规则和几何检查组成的模块进行审查。例如，VLA 提议“穿过这扇看似开着的门”，安全层必须用最新的深度数据再次确认路径确实通畅。VLA 是大脑，但必须有一个基于物理定律的“脊髓反射”来兜底。