本文档属于 Robotics Tutorial 项目，作者：Pengfei Guo，达妙科技。采用 CC BY 4.0 协议，转载请注明出处。

第 60 章：感知驱动的落脚规划——从盲走到看路，让机器人"长眼睛"¶

本章定位：足式/140_落脚点规划经典方法讲了 Raibert 启发式和 Capture Point 等经典落脚方法，足式/150_优化驱动落脚与接触规划讲了优化驱动的接触规划（CITO / GCS）。但这两章有一个共同的隐含假设——地面是平的，或者至少是已知的。现实世界中，四足机器人面对的是碎石、台阶、斜坡、沟渠、泥地，甚至冰面。本章系统讲解如何让机器人"看见"地形并据此选择落脚点，涵盖地形表示、Elevation Mapping、GPU 加速建图、可通行性分析、感知驱动的落脚评分、TAMOLS 地形感知优化、以及学习型落脚选择。

前置依赖：足式/70_腿足简化模型理论（LIPM / Capture Point）、足式/130_腿足状态估计（状态估计 / 里程计）、足式/140_落脚点规划经典方法（落脚经典方法）、足式/150_优化驱动落脚与接触规划（优化驱动接触规划）

关键文献：Fankhauser 2014（Elevation Mapping）、Wermelinger 2016（可通行性）、Jenelten 2022（TAMOLS, T-RO）、Miki 2022（Science Robotics）、Grandia 2023（Perceptive MPC, T-RO）、Hoeller 2024（ANYmal Parkour, Science Robotics）

前置自测¶

📋 答不出 >= 2 题 → 先回对应章节复习

[足式/70_腿足简化模型理论] LIPM 方程 $\ddot{x} = \omega^2(x - p)$ 中，$p$ 代表什么？Raibert 启发式的三项分别对应什么物理含义？
[足式/130_腿足状态估计] 状态估计输出的里程计（odometry）在 Elevation Map 更新中起什么作用？如果里程计有漂移会怎样？
[足式/140_落脚点规划经典方法] Raibert 启发式 $\boldsymbol{p}_f = \boldsymbol{p}_{\text{hip}} + \frac{T_s}{2}\dot{\boldsymbol{p}} + k(\dot{\boldsymbol{p}} - \dot{\boldsymbol{p}}_{\text{ref}})$ 在什么场景下会失效？
[足式/150_优化驱动落脚与接触规划] Contact-Implicit TO 的核心思想是什么？它与本章"感知驱动"的区别在哪？
[基础] 什么是 Kalman 滤波的预测-更新两步？写出标量情况下的更新公式。

本章目标¶

学完本章，你应能：

理解"盲走"与"感知行走"的本质差异——为什么平地控制器在不平地形上会失效，感知如何弥补
掌握五种地形表示方法的优劣——高程图、点云、体素、网格、SDF，知道何时用哪种
完整理解 Elevation Mapping 的数据结构和算法——从点云投影到 Kalman 融合到漂移补偿
理解 GPU 加速建图的工程实现——elevation_mapping_cupy 的架构、性能和部署
能计算地形特征并评估可通行性——坡度、粗糙度、台阶检测、可通行性评分
掌握感知驱动的落脚评分框架——地形代价 + 运动学可达 + 稳定裕度的多准则融合
理解 TAMOLS 的优化框架——地形感知运动优化、SDF 碰撞回避、graduated optimization
了解学习型落脚选择的最新进展——Miki 2022 的 teacher-student 框架、端到端感知控制
能进行传感器选型——深度相机 vs LiDAR 的工程权衡、标定流程

60.1 从"盲"到"看"——为什么需要感知 ⭐¶

本节解决什么问题：建立"感知驱动"的动机——不是因为"有传感器就该用"，而是因为盲走在真实世界中会系统性地失效。

动机：一只"闭着眼"的四足在碎石坡上¶

想象你把足式/140_落脚点规划经典方法的 Raibert 控制器部署到真实的 ANYmal 或 Go2 上，让它在实验室平地上跑。效果完美——trot 步态稳定，速度跟踪精准，即使你推它一把也能快速恢复。

然后你把它带到户外的碎石坡上。

场景：碎石坡（坡度 15°，表面散布 3-8cm 碎石）

盲走控制器的行为：
  Step 1: Raibert 计算落脚点 → 假设地面高度 = 0（平地假设！）
  Step 2: 摆动腿按照平地高度放下 → 实际地面高了 8cm → 提前碰撞
  Step 3: 冲击力触发接触检测 → 但关节角度不对 → 支撑力不足
  Step 4: 另一只脚的 Raibert 也算错 → 踩在碎石上 → 滑动
  Step 5: MPC 的接触力分配基于错误的法向量（以为是平地）
         → 摩擦锥约束被违反 → 脚滑 → 机器人摔倒

根因：控制器假设了一个不存在的世界（平地）

这不是个别案例。下表总结了盲走控制器在各种地形上的系统性失效模式：

地形类型	盲走失效原因	后果	感知能解决什么
斜坡	假设 $z = 0$，不知道地面倾斜	脚提前/延迟着地，冲击或悬空	预知地面高度，调整摆动轨迹
台阶	不知道前方有高差	脚撞台阶面或踩空	检测台阶边缘，选择踩上去或绕过
碎石	不知道哪里平整、哪里是尖石	踩在不稳定表面上打滑	评估表面粗糙度，选择平整区域
沟渠	不知道前方有凹陷	脚掉进沟里，关节过伸	检测凹陷，增大步幅跨过
冰/水面	不知道表面摩擦系数低	滑动导致失稳	结合 RGB 识别材质，降低速度
狭窄踏板	不知道只有某些区域可踩	落脚在悬空区域	检测可踩区域，精确落脚

如果不用感知会怎样——纯触觉反应式的局限¶

有人会问："我不用视觉，用触觉反馈行不行？脚碰到障碍就抬高，踩到软地就换脚。"

这就是所谓的**反应式（reactive）**策略。它确实有一定效果——自然界的昆虫就是这么做的。但它有三个根本局限：

局限 1：反应滞后。触觉是"碰到才知道"，视觉是"没碰就知道"。当机器人以 1 m/s 行走时，从脚接触到控制响应需要 20-50 ms（传感器延迟 + 控制器计算），这段时间机器人已经走了 2-5 cm。在台阶边缘，这 2-5 cm 就是"站稳"和"滑落"的差别。

局限 2：不可逆冲击。脚撞到台阶面产生的冲击力峰值可达体重的 3-5 倍。即使控制器在 10 ms 内响应，关节和结构已经承受了这个冲击。长期累积会导致减速器磨损和结构疲劳。

局限 3：无法全局规划。反应式策略是逐步决策——每一步只看当下，不考虑未来。在 stepping stones 场景中（足式/150_优化驱动落脚与接触规划），正确的策略可能需要"当前步故意踩近一点，为下一步留出空间"。反应式做不到这种前瞻性规划。

历史：从盲走到感知的里程碑¶

时间线：足式机器人感知的演进

1986  Raibert (MIT)         纯盲走 + 本体感知 → 平地跑跳的经典
  |
2008  Kolter & Ng (Stanford) 首次用机器学习预测地形可通行性
  |
2014  Fankhauser (ETH)      Elevation Mapping 框架 → 2.5D 地形表示的事实标准
  |
2016  Wermelinger (ETH)     手工规则可通行性分析 → 几何特征组合评分
  |
2018  Fankhauser (ETH)      Robot-Centric Elevation Mapping 论文 → 理论完善
  |
2022  Miki (ETH)            Science Robotics → 学习型感知运动控制的分水岭
  |    Jenelten (ETH)       TAMOLS → 地形感知运动优化 + SDF 碰撞回避
  |    Grandia (ETH)        Perceptive MPC → 高程图嵌入 OCS2 的代价函数
  |
2023  elevation_mapping_cupy GPU 加速建图 → 实时性突破
  |    Zhuang (UCSD)        Robot Parkour Learning → CoRL → 端到端 RL 跑酷
  |
2024  Hoeller (ETH)         ANYmal Parkour → Science Robotics → 工业级感知跑酷
  |    Jenelten (ETH)       DTC → Science Robotics → RL + MPC 混合架构
  |
2025  多模态高程图          语义 + 几何 + RGB 融合 → 超越纯几何感知

💡 概念澄清：本章的"感知"特指**地形感知**（terrain perception），即"地面长什么样、能不能踩"。这与 SLAM（03_SLAM 方向的全局定位与建图）和物体识别（机械臂抓取）是不同的感知层次。腿足机器人的感知核心是 2.5D 局部地形，不是 3D 全局地图。回顾 SLAM 的核心功能:SLAM 提供全局一致的位姿估计和环境地图,但其空间分辨率(通常 5-50cm)不足以支撑厘米级的落脚点选择。Elevation Mapping 的角色是在 SLAM 提供的局部位姿基础上,构建高分辨率(2-5cm)的近场地形。两者是上下游关系,不是替代关系。

⚠️ 常见陷阱¶

⚠️ 概念误区：认为"感知行走"一定比"盲走"好 新手想法："既然有感知，就应该永远开着" 实际上：感知引入了额外的延迟（传感器处理 10-50 ms）、噪声（深度相机在强光下失效）、和计算负担（GPU 占用）。在平坦地形上，盲走的 Raibert + MPC 组合更简单、更快、更可靠。ANYmal 的工程实践中也有"感知置信度低时切回盲走"的降级逻辑。 正确做法：感知是工具，不是信仰。在平地用盲走，在复杂地形用感知，中间态用渐变融合。

💡 概念误区：混淆"感知驱动落脚"与"SLAM" 新手想法："做感知落脚就是做 SLAM" 实际上：SLAM 关注**全局**定位与地图构建（数十到数百米），Elevation Map 关注**局部**地形（机器人周围 2-6 米）。SLAM 的输出（里程计/位姿）是 Elevation Map 的**输入**——用来对齐点云。两者是上下游关系，不是同一件事。 关系链：SLAM → 位姿估计 → 点云对齐 → Elevation Map → 可通行性 → 落脚点选择

🧠 思维陷阱：认为"传感器越多越好" 新手想法："装 4 个深度相机 + 2 个 LiDAR + 激光雷达" 实际上：每增加一个传感器，就增加了标定复杂度、数据同步难度、计算负担、功耗和重量。ANYmal C 用 1 个 Velodyne VLP-16 + 2 个 RealSense D435 就覆盖了大部分场景。关键不是传感器数量，而是**传感器配置的互补性**和**数据融合质量**。 正确思维：先确定任务需求（室内/室外、速度、地形复杂度），再选最少够用的传感器组合。

练习¶

[思考题] 列举三种你在日常生活中"盲走"（不看路）也能安全行走的场景，以及三种必须"看路"的场景。从中总结：什么因素决定了是否需要视觉感知？
[分析题] 如果一个四足机器人在 0.5 m/s 速度下行走，传感器到控制器的总延迟为 30 ms，请计算在这段延迟内机器人走了多远。如果前方有一个 3 cm 高的台阶，这个延迟是否会导致问题？给出你的分析。
[设计题] 为一个需要在仓库货架间巡检的四足机器人设计感知方案：地面类型（水泥、金属格栅）、障碍物（包裹、叉车）、光照条件（室内荧光灯、暗角）。你会选什么传感器？为什么？

60.2 地形表示方法 ⭐⭐¶

本节解决什么问题：在使用感知数据之前，需要选择一种合适的数据结构来表示地形。不同的表示方法在精度、计算量、内存和适用场景上差异巨大。

动机：为什么不能直接用原始点云¶

深度相机或 LiDAR 输出的原始数据是**点云**——数十万个 $(x, y, z)$ 点。直接用点云做落脚规划有三个问题：

查询效率低："(2.1, 0.5) 这个位置的地面高度是多少？"——需要最近邻搜索，O(log N) 对数十万点
缺乏融合：每帧点云都是独立快照，不同帧的观测没有被整合
信息冗余：大量点描述同一块平地，却没有"这块地形的坡度是多少"的摘要信息

因此，我们需要将原始点云**转换为结构化的地形表示**。下面对比五种主要方法。

五种地形表示方法对比¶

特性	点云	高程图 (2.5D)	体素 (3D)	网格 (Mesh)	SDF
维度	3D 散点	2D 栅格 + 高度	3D 栅格	2D 三角面	3D 标量场
存储	O(N) 点	O(W/r × H/r)	O((W/r)³)	O(V) 顶点+面	O((W/r)³)
查询速度	O(log N) KD-tree	O(1) 栅格索引	O(1) 栅格索引	O(log V)	O(1) 栅格索引
表达能力	任意形状	无悬垂/洞穴	任意形状	任意形状	任意形状
MPC 兼容	差（不可微）	好（插值可微）	差（离散）	中（法向量可用）	好（梯度天然可用）
更新效率	追加 O(1)	格子更新 O(1)	格子更新 O(1)	重建 O(N)	重建 O(N)
腿足适用性	低	最高	低（内存过大）	中	高（碰撞检测）
典型分辨率	—	2-5 cm	5-20 cm	可变	5-20 cm
代表框架	PCL	grid_map	OctoMap	CGAL	CHOMP/TAMOLS

高程图（Elevation Map）——腿足感知的事实标准¶

为什么高程图是最佳选择？ 原因有三：

第一，查询 O(1)。落脚规划需要频繁查询"某个 (x, y) 位置的地面高度"。高程图把连续空间离散化为 2D 栅格，每个格子存储高度值 $z(i, j)$。给定世界坐标 $(x, y)$，通过简单的整数除法就能找到对应格子：

\[i = \lfloor (x - x_{\text{origin}}) / r \rfloor, \quad j = \lfloor (y - y_{\text{origin}}) / r \rfloor\]

其中 $r$ 是分辨率（典型 0.04 m）。这是 O(1) 操作，比点云的 KD-tree 查询快两个数量级。

第二，天然支持融合。每个格子可以维护一个 Kalman 滤波器，将不同时刻、不同传感器的观测融合为一个最优估计。这在点云表示中需要额外的配准（registration）步骤。

第三，可微。通过双线性插值，离散栅格可以变成连续、可微的函数 $z(x, y)$。这使得高程图可以直接嵌入 MPC 的代价函数中（见 60.7 TAMOLS 和 Grandia 2023 Perceptive MPC）。

高程图的局限——2.5D 假设：

高程图假设每个 $(x, y)$ 位置只有一个高度值。这意味着它**无法表示悬垂结构**（overhanging，如桥底、洞穴顶部）和**多层结构**（如楼梯下方的空间）。对于大多数户外地形和室内平层，这个假设是合理的。当需要处理多层结构时，可以使用 Multi-Level Surface Map（MLS Map）或体素表示。

体素（Voxel）——3D 完整但内存昂贵¶

体素把 3D 空间离散化为立方格子，每个格子标记为"占据"或"空闲"。代表框架是 OctoMap（Hornung 2013）。

优点：能表示任意 3D 形状，包括悬垂和洞穴。 致命缺点：内存爆炸。4m × 4m × 2m 空间，分辨率 4cm，需要 $(100 \times 100 \times 50) = 50$ 万个格子。分辨率提高到 2cm 就变成 400 万格子。OctoMap 用八叉树压缩可以缓解，但查询速度也相应下降。

对腿足的适用性：低。腿足机器人关心的是**地面表面**（一个 2D 流形），不需要知道头顶 2m 处的空间是否占据。用 3D 体素表示 2D 地面是维度浪费。

SDF（Signed Distance Field）——碰撞检测利器¶

SDF 在每个 3D 格子存储"到最近表面的距离"。表面内部为负，外部为正。

核心优势：梯度天然可用。$\nabla \text{SDF}(x)$ 指向远离表面的方向，大小等于 1（在 Eikonal 方程解的意义下）。这使得 SDF 非常适合碰撞回避——把 $\text{SDF}(x) \geq d_{\text{safe}}$ 作为约束即可。

在腿足中的应用：TAMOLS（60.7）用高程图推导出地面 SDF，作为腿碰撞回避的约束。Perceptive MPC（Grandia 2023）也用类似的方式从高程图导出距离函数。

与高程图的关系：SDF 通常不是独立建图的结果，而是从高程图或体素地图**推导**出来的。对于 2.5D 地形，可以从高程图直接计算每个位置到地面的距离：$\text{SDF}(x, y, z) = z - z_{\text{map}}(x, y)$。

如何选择——决策树¶

你的机器人需要什么？
  │
  ├─ "快速查询某位置地面高度" → 高程图 (Elevation Map)
  │   └─ 分辨率选择：
  │       ├─ 精细操作（踩踏板）→ 2 cm
  │       ├─ 一般行走 → 4 cm（最常用）
  │       └─ 快速导航 → 10 cm
  │
  ├─ "检测头顶障碍/穿越洞穴" → 体素 (OctoMap) 或 MLS Map
  │
  ├─ "MPC 中的碰撞回避约束" → 从高程图推导 SDF
  │
  ├─ "高质量 3D 可视化" → 网格 (Mesh)
  │
  └─ "原始数据存档" → 点云

⚠️ 常见陷阱¶

⚠️ 编程陷阱：高程图分辨率设太小导致内存爆炸 错误做法：把分辨率设为 1 cm，地图大小 10m × 10m → $(1000 \times 1000) = 100$ 万格子 × 每格多层数据（高度、方差、坡度、粗糙度等 6 个 float）→ 24 MB 数据，每帧更新计算量巨大现象：建图频率从 50 Hz 掉到 5 Hz，MPC 拿不到及时的地形数据 正确做法：根据任务选分辨率。4 cm 是绝大多数场景的最佳平衡点。如果需要更精细，缩小地图范围而不是提高全图分辨率

💡 概念误区：认为 2.5D 是"精度不够的 3D" 新手想法："2.5D 不如 3D 精确，应该尽量用 3D" 实际上：2.5D 不是 3D 的简化版，而是针对地面这一**特定几何结构**的最优表示。地面在绝大多数情况下就是一个 2D 流形（每个 xy 只有一个 z）。用 2.5D 表示 2D 流形是**精确匹配**，不是近似。用 3D 表示反而引入了大量无意义的空格子。类比：用灰度图（2D）表示灰度图像是精确的，用体素（3D）表示灰度图像是浪费。

🧠 思维陷阱：忽略高程图的"时效性"问题 新手想法："建好高程图就可以一直用" 实际上：机器人是移动的，世界也在变化（其他机器人、人、门）。高程图需要持续更新，旧的观测需要被淘汰。grid_map 用以机器人为中心的滑动窗口处理这个问题——机器人移动时，地图跟着平移，离开视野的区域被清除。 正确做法：理解高程图是"局部、实时、短记忆"的地形快照，不是全局持久地图。全局地图由 SLAM 负责。

练习¶

[计算题] 一个 4m × 4m 的高程图，分辨率 4 cm，每个格子存储 4 个 float（高度、方差、坡度、可通行性）。计算总内存占用（字节）。如果同样的区域用体素表示（高度范围 2m，分辨率 4cm），计算体素地图的内存占用。两者的比值是多少？
[设计题] 你的四足机器人需要穿越一个地下停车场（有低矮的天花板管道、斜坡、减速带）。高程图能满足需求吗？如果不能，你会用什么替代方案？画出你的数据表示方案。
[思考题] 为什么 SLAM 后端（如 iSAM2）用的是 3D 点云 / 因子图，而落脚规划用的是 2.5D 高程图？从"问题需求"的角度分析这两种选择的合理性。

60.3 Elevation Mapping 详解 ⭐⭐¶

本节解决什么问题：深入讲解 Fankhauser（2014, 2018）提出的 Elevation Mapping 框架——从传感器原始数据到结构化高程图的完整管线。

动机：从点云到"能用的地图"需要几步¶

假设你已经有了深度相机或 LiDAR 的点云输出，以及状态估计给的里程计。你想得到一张高程图用于落脚规划。这中间需要什么步骤？

传感器原始数据处理管线：

┌─────────────┐    ┌──────────────┐    ┌──────────────┐    ┌─────────────┐
│ 深度相机/     │───→│ 坐标系变换    │───→│ 高程图格子    │───→│ Kalman 融合  │
│ LiDAR 点云   │    │ (TF lookup)  │    │ 投影         │    │ (每格独立)   │
└─────────────┘    └──────────────┘    └──────────────┘    └─────────────┘
                          ↑                                       │
                   ┌──────────────┐                        ┌──────────────┐
                   │ 状态估计      │                        │ 漂移补偿     │
                   │ (足式/130_腿足状态估计 输出)  │                        │ + 光线清理   │
                   └──────────────┘                        └──────────────┘
                                                                  │
                                                           ┌──────────────┐
                                                           │ 输出：        │
                                                           │ 高程图 + 方差 │
                                                           └──────────────┘

每一步都有工程细节和数学基础。下面逐一展开。

来龙去脉：Fankhauser 的贡献¶

Peter Fankhauser 在 ETH Zurich 的博士工作（2014-2018）建立了 Elevation Mapping 的标准框架。他面对的问题是：ANYmal 机器人需要在不平地形上行走，但当时没有适合腿足机器人的地形感知方案。

为什么已有方案不够用？

已有方案	问题
OctoMap（2013）	3D 体素，内存和计算过重，查询不是 O(1)
Costmap（ROS Navigation）	为轮式设计，只有 2D 占据，没有高度信息
TSDF（KinectFusion）	为 3D 重建设计，不是以机器人为中心，不适合移动机器人

Fankhauser 的核心贡献是：以机器人为中心的 2.5D 栅格地图，每格用 Kalman 滤波融合多帧观测，并处理传感器噪声和里程计漂移。

Step 1：坐标系变换——从传感器系到世界系¶

每个传感器点 $\boldsymbol{p}_s = (x_s, y_s, z_s)$ 需要变换到世界坐标系（或者更准确地说，以机器人为中心的局部坐标系）：

\[\boldsymbol{p}_w = \boldsymbol{T}_{ws} \cdot \boldsymbol{p}_s = \boldsymbol{T}_{wb} \cdot \boldsymbol{T}_{bs} \cdot \boldsymbol{p}_s\]

其中： - $\boldsymbol{T}_{bs}$：传感器相对于基座的固定变换（从 URDF 或标定获得） - $\boldsymbol{T}_{wb}$：基座在世界系中的位姿（从足式/130_腿足状态估计状态估计获得）

为什么这一步是关键：如果 $\boldsymbol{T}_{wb}$ 有误差（里程计漂移），所有点云都会被投影到错误的位置，高程图就会出现"鬼影"——同一个障碍物在地图上出现两次。这就是**漂移补偿**问题的根源。

Step 2：栅格投影——从 3D 点到 2D 格子¶

将世界坐标系下的点 $\boldsymbol{p}_w = (x_w, y_w, z_w)$ 投影到高程图的格子 $(i, j)$：

\[i = \lfloor (x_w - x_{\text{center}}) / r + N/2 \rfloor, \quad j = \lfloor (y_w - y_{\text{center}}) / r + N/2 \rfloor\]

其中 $(x_{\text{center}}, y_{\text{center}})$ 是地图中心（通常是机器人当前位置），$r$ 是分辨率，$N$ 是格子数。

一个格子可能收到多个点。例如，分辨率 4 cm 的格子面积为 $16 \text{ cm}^2$，在 Velodyne VLP-16（单帧约 30000 点、4m 范围内约 5000 点）的情况下，每个格子平均收到 $5000 / (100 \times 100) = 0.5$ 个点。分辨率更高或传感器更密时，一个格子会收到更多点。

同一格子多点的处理：取最高点（保守，防止低点是噪声）或取均值（平滑）。Fankhauser 的方案是用 Kalman 滤波逐点融合。

Step 3：Kalman 融合——每个格子的最优估计¶

每个格子独立维护一个 1D Kalman 滤波器，状态是高度 $z$：

预测步（当没有新观测时，状态不变，但方差增大以反映不确定性增长）：

\[\hat{z}^- = \hat{z}, \quad P^- = P + Q\]

其中 $Q$ 是过程噪声，反映"地面可能发生变化"的不确定性（通常很小，$Q \approx 10^{-4}$ m$^2$/s 级别）。

更新步（收到新的高度观测 $z_{\text{meas}}$ 时）：

\[K = \frac{P^-}{P^- + R}\]

\[\hat{z} = \hat{z}^- + K(z_{\text{meas}} - \hat{z}^-)\]

\[P = (1 - K) P^-\]

其中 $R$ 是观测噪声方差，取决于传感器类型和距离：

传感器	典型 R（近距 1m）	典型 R（远距 4m）
RealSense D435	$(2 \text{ mm})^2$	$(20 \text{ mm})^2$
Velodyne VLP-16	$(3 \text{ mm})^2$	$(5 \text{ mm})^2$
Ouster OS1-64	$(2 \text{ mm})^2$	$(4 \text{ mm})^2$

为什么用 Kalman 而不是简单平均？

简单平均 $\hat{z} = \frac{1}{n}\sum z_i$ 对所有观测赋予相同权重。但实际中，远距离的观测比近距离更不准确（$R$ 更大），旧的观测可能已经过时。Kalman 滤波通过**加权融合**自动处理这两个问题：

噪声大的观测（$R$ 大）→ Kalman 增益 $K$ 小 → 新观测的影响小
当前估计已经很精确（$P^-$ 小）→ $K$ 小 → 不容易被新观测拉偏

物理直觉：Kalman 增益 $K$ 是"信不信新观测"的权衡系数。$K \to 1$ 意味着"完全相信新观测"（当前估计不确定性极大），$K \to 0$ 意味着"完全不信新观测"（当前估计已经很准了）。

Step 4：漂移补偿——处理里程计误差¶

里程计漂移是 Elevation Mapping 最棘手的工程问题。漂移导致同一个物理表面在不同时刻被投影到不同的格子上，产生"鬼影"或"模糊边缘"。

Fankhauser 的解决方案：将里程计的不确定性传播到高程图的每个格子中。

具体做法是：里程计提供位姿 $\boldsymbol{T}_{wb}$ 及其协方差 $\boldsymbol{\Sigma}_{wb}$。当位姿协方差增大（漂移累积），高程图中所有格子的高度方差 $P$ 也相应增大：

\[P_{\text{new}} = P + J_z \boldsymbol{\Sigma}_{wb} J_z^T\]

其中 $J_z$ 是从位姿不确定性到高度不确定性的雅可比。这样，当里程计漂移严重时，高程图的方差会自动增大，后续的 Kalman 更新会更"信任"新的观测——本质上是让地图"遗忘"旧的、可能已经偏了的数据。

Step 5：光线投影清理——去除伪观测¶

当机器人移动到新位置，传感器射线穿过之前标记为"占据"的格子时，如果该射线上现在没有障碍物返回，说明之前的"占据"信息是**错误的**（可能是动态物体已经移走，或者是噪声造成的伪观测）。

光线清理算法： 1. 对每条传感器射线（从传感器到反射点），用 Bresenham 直线算法遍历沿途的所有格子 2. 如果某格子标记为"已占据"（有高度值），但当前射线穿过了它的上方（射线 z > 格子 z），则清除该格子的高度信息 3. 这等价于说："我刚才看过那里，那里是空的"

计算代价：光线清理是 O(N × L) 操作，其中 N 是点数，L 是每条射线穿过的格子数。对于 30000 点、平均穿过 50 格子的情况，这是 150 万次格子操作——是建图管线中最耗时的步骤，也是 GPU 加速的主要动机之一。

grid_map 库：数据结构与 API¶

Fankhauser 开发的 grid_map 是 Elevation Mapping 的开源基础设施，目前由 ANYbotics 维护（当前维护者 Maximilian Wulf 和 Magnus Gartner），支持 ROS2 Humble / Iron / Jazzy / Rolling。

核心数据结构：

// grid_map core: multi-layer 2D grid backed by Eigen::MatrixXf
#include <grid_map_core/GridMap.hpp>

grid_map::GridMap map({"elevation", "variance", "traversability"});
map.setFrameId("odom");
map.setGeometry(grid_map::Length(4.0, 4.0),   // 4m x 4m
                0.04,                           // resolution 4cm
                grid_map::Position(0.0, 0.0));  // center position

// Write height value
grid_map::Index index;
map.getIndex(grid_map::Position(1.2, 0.5), index);
map.at("elevation", index) = 0.15;  // 15cm height
map.at("variance", index) = 0.001;  // 1mm^2 variance

// Read height value — O(1) query
float height = map.atPosition("elevation", grid_map::Position(1.2, 0.5));

关键设计决策——环形缓冲区（Circular Buffer）：

当机器人移动时，地图需要跟着平移。朴素做法是复制整个矩阵并偏移——O(N^2) 操作。grid_map 使用**2D 环形缓冲区**：物理存储不变，只修改起始索引。这使得地图平移变成 O(1) 操作。

环形缓冲区示意（1D 简化）：

物理存储: [c, d, e, f, g, a, b]
           ↑ 起始索引 = 5
逻辑顺序: [a, b, c, d, e, f, g]

机器人向右移动一格后：
物理存储: [c, d, e, f, g, a, h]  ← 'h' 覆盖了 'a'（最左边出视野）
           ↑ 起始索引 = 6
逻辑顺序: [b, c, d, e, f, g, h]

操作：修改 startIndex，写入新格子，无需复制矩阵

迭代器（Iterators）——高效遍历：

grid_map 提供 8 种迭代器，用于遍历特定形状的区域：

迭代器	用途	落脚规划中的应用
`GridMapIterator`	遍历所有格子	全图可通行性计算
`CircleIterator`	遍历圆形区域	以候选落脚点为中心评估局部地形
`SubmapIterator`	遍历矩形子图	截取脚下区域做精细分析
`PolygonIterator`	遍历多边形区域	评估足底接触面积内的地形
`LineIterator`	遍历线段上的格子	检测脚从起点到终点路径上的障碍
`EllipseIterator`	遍历椭圆区域	评估椭圆形足底
`SpiralIterator`	从中心向外螺旋遍历	搜索最近的可踩点

⚠️ 常见陷阱¶

⚠️ 编程陷阱：忘记检查格子是否有效（is_valid） 错误做法：直接读取 map.at("elevation", index) 而不检查该格子是否有数据现象：返回 NaN 或初始化时的垃圾值，后续计算全部出错。坡度计算变成 NaN，可通行性变成 NaN，MPC 代价变成 NaN → MPC 发散 根本原因：高程图初始化时并非所有格子都有传感器覆盖。机器人背后、远处、被遮挡的区域没有数据 正确做法：先检查 map.isValid(index, "elevation")，对无数据区域使用默认值或标记为不可通行

💡 概念误区：认为 Kalman 滤波需要精确的 R 值 新手想法："我不知道传感器噪声到底多大，Kalman 滤波还能用吗？" 实际上：Kalman 滤波对 R 的精确值并不敏感。R 偏大一倍，只是融合速度慢一点（需要更多帧才收敛）；R 偏小一倍，融合速度快但对噪声更敏感。在实际工程中，R 的数量级对就行（1mm$^2$ vs 10mm$^2$ 的差别远比 1mm$^2$ vs 1m$^2$ 重要）。 正确做法：用传感器手册的噪声参数作为初值，然后在实际环境中微调。关注的指标是"高程图在静态场景中是否收敛到稳定值"。

🧠 思维陷阱：忽略光线清理的计算代价 新手想法："光线清理就是一个简单的遍历，应该很快" 实际上：光线清理是 O(点数 × 射线长度) 操作。Velodyne VLP-16 每帧 30000 点，每条射线穿过约 50 个格子，共 150 万次格子操作。在 CPU 上，这个操作在 4cm 分辨率下需要 5-15 ms——这已经是 50 Hz 更新时间的 25-75%。这就是为什么 elevation_mapping_cupy 把光线清理放到 GPU 上。 正确做法：如果用 CPU 版 grid_map，可以降低光线清理频率（如每 5 帧一次），或只对关键区域做清理。

练习¶

[推导题] 写出标量 Kalman 滤波的完整推导：从贝叶斯推断 $P(z | z_1, z_2, \ldots, z_n)$ 出发，假设高斯分布，推导出递推公式。验证当所有观测噪声相同（$R_i = R$）时，$n$ 次更新后的方差是否等于 $R/n$。
[计算题] 一个格子初始方差 $P_0 = 1.0$ m$^2$（完全不确定），传感器噪声 $R = 0.001$ m$^2$（即 $\sigma = 3.16$ cm）。计算前 5 次 Kalman 更新后的方差 $P_1, P_2, \ldots, P_5$。需要多少次更新，方差才能降到 $R$ 的两倍以下？
[编程题] 使用 grid_map 库，编写一个 ROS2 节点：(1) 创建一个 4m x 4m、分辨率 4cm 的高程图；(2) 订阅 PointCloud2 话题；(3) 将点云投影到高程图中，对每个格子做简单的均值更新（先不用 Kalman）；(4) 发布 grid_map 消息在 RViz 中可视化。

60.4 GPU 加速地形建图 ⭐⭐⭐¶

本节解决什么问题：CPU 版 Elevation Mapping 在高分辨率或高帧率下力不从心。本节讲解 GPU 加速的 elevation_mapping_cupy，理解为什么 GPU 适合这个问题，以及实时建图的工程实现。

动机：CPU 瓶颈在哪¶

让我们量化 CPU 版 Elevation Mapping 的计算瓶颈：

操作	计算量	CPU 单核耗时	GPU 耗时
坐标变换	30000 点 × 矩阵乘法	0.5 ms	0.02 ms
栅格投影	30000 点 × 索引计算	0.3 ms	0.01 ms
Kalman 更新	10000 格 × 标量 KF	1.0 ms	0.05 ms
光线清理	30000 × 50 格	5-15 ms	0.3 ms
特征提取	10000 格 × 邻域计算	3-8 ms	0.2 ms
总计		10-25 ms	< 1 ms

光线清理和特征提取这两个步骤吃掉了 CPU 80%以上的时间。它们的共同特点是：每个格子/点的计算完全独立。这正是 GPU 大规模并行计算的理想场景。

如果不用 GPU 会怎样¶

不用 GPU 意味着建图频率受限于 CPU 性能。在 4cm 分辨率、4m x 4m 地图、Velodyne VLP-16 输入的典型配置下：

CPU 版 grid_map：20-50 Hz 更新频率（取决于是否做光线清理和特征提取）
MPC 需求：10-50 Hz 的地形更新就够用（MPC 本身运行在 20-100 Hz）

看起来 CPU 够用？问题在于：

多传感器融合：ANYmal 同时有 LiDAR + 深度相机，数据量翻倍
更高分辨率：2cm 分辨率意味着 4 倍的格子数
语义层：加上 RGB 语义分割后，每帧还要跑 CNN → 数据量再翻倍
CPU 已被占用：MPC、WBC、状态估计都在抢 CPU → 留给建图的余量很少

GPU 不只是"快"，而是**释放 CPU 给控制算法**。

elevation_mapping_cupy 架构¶

ETH 的 elevation_mapping_cupy 是目前工业部署中最常用的 GPU 加速 Elevation Mapping 框架。2023 年之后的版本支持 ROS2 Jazzy，GitHub 上超过 800 stars。

架构概览：

┌────────────────────────────────────────────────────────┐
│                    ROS2 C++ Wrapper                     │
│  ── 订阅 PointCloud2, Image, TF ──                     │
│  ── 发布 GridMap, Visualization ──                      │
└───────────────┬────────────────────────────────────────┘
                │ (NumPy 数组传递)
┌───────────────▼────────────────────────────────────────┐
│              Python + CuPy 核心                         │
│                                                         │
│  ┌──────────┐  ┌──────────┐  ┌──────────────────────┐  │
│  │ 点云投影  │→│ KF 融合   │→│ 光线清理 (GPU kernel) │  │
│  │(GPU)     │  │(GPU)     │  │                       │  │
│  └──────────┘  └──────────┘  └──────────────────────┘  │
│                                                         │
│  ┌──────────────────────┐  ┌────────────────────────┐  │
│  │ 特征提取 (GPU kernel) │→│ 可通行性 (CNN / 规则)   │  │
│  │ slope, roughness     │  │                        │  │
│  └──────────────────────┘  └────────────────────────┘  │
│                                                         │
│  ┌──────────────────────────────────────────────────┐  │
│  │ 语义融合 (可选, GPU CNN inference)                │  │
│  └──────────────────────────────────────────────────┘  │
└────────────────────────────────────────────────────────┘

为什么用 CuPy 而不是 CUDA C++？

方案	开发效率	运行效率	可维护性
CUDA C++	低（手写 kernel）	最高	低（需要 CUDA 专家）
CuPy	高（NumPy 语法）	高（90%+ CUDA 性能）	高（Python 生态）
PyTorch	高	中（overhead 大）	高

CuPy 的核心优势：NumPy 兼容的 API + 自动 CUDA 编译。写 cupy.sum(array, axis=0) 就自动生成 CUDA kernel 执行 GPU 并行求和，不需要手写 __global__ 函数。

核心 GPU 操作示例：

import cupy as cp
import numpy as np

class ElevationMapGPU:
    def __init__(self, size=4.0, resolution=0.04):
        self.n = int(size / resolution)  # 100 for 4m/0.04m
        self.resolution = resolution
        # GPU arrays — all data lives on GPU
        self.height = cp.full((self.n, self.n), cp.nan, dtype=cp.float32)
        self.variance = cp.full((self.n, self.n), 1.0, dtype=cp.float32)
        self.valid = cp.zeros((self.n, self.n), dtype=cp.bool_)

    def update(self, points_gpu, R_sensor):
        """
        Kalman update for each cell with new observations.
        points_gpu: (N, 3) CuPy array of (x, y, z) in map frame
        R_sensor: scalar observation noise variance
        """
        # Step 1: Compute grid indices (all on GPU, parallel over N points)
        ix = ((points_gpu[:, 0] / self.resolution) + self.n // 2).astype(cp.int32)
        iy = ((points_gpu[:, 1] / self.resolution) + self.n // 2).astype(cp.int32)

        # Step 2: Filter out-of-bounds points
        mask = (ix >= 0) & (ix < self.n) & (iy >= 0) & (iy < self.n)
        ix, iy, z_meas = ix[mask], iy[mask], points_gpu[mask, 2]

        # Step 3: Kalman update (per-cell, using scatter operations)
        P = self.variance[ix, iy]
        K = P / (P + R_sensor)                    # Kalman gain
        innovation = z_meas - self.height[ix, iy]

        # Handle first observation (height is NaN)
        first_obs = cp.isnan(self.height[ix, iy])
        self.height[ix, iy] = cp.where(
            first_obs, z_meas,
            self.height[ix, iy] + K * innovation
        )
        self.variance[ix, iy] = cp.where(
            first_obs, R_sensor,
            (1.0 - K) * P
        )
        self.valid[ix, iy] = True

注意：上述代码是简化的教学版本。真实的 elevation_mapping_cupy 还处理了同一格子多点冲突（用 atomicMin/atomicAdd）、环形缓冲区平移、多传感器时间同步等工程细节。

性能对比¶

在 NVIDIA Jetson Orin（ANYmal 的边缘计算平台）上的实测数据：

配置	CPU (grid_map)	GPU (cupy)	加速比
4m x 4m, 4cm, VLP-16	20 ms	1.5 ms	13x
4m x 4m, 2cm, VLP-16	80 ms	4 ms	20x
6m x 6m, 4cm, VLP-16 + D435	40 ms	2.5 ms	16x
4m x 4m, 4cm + 语义 CNN	N/A (太慢)	8 ms	—

⚠️ 常见陷阱¶

⚠️ 编程陷阱：CuPy 数组与 NumPy 数组混用 错误做法：result = cupy_array + numpy_array 现象：CuPy 会隐式地把 NumPy 数组上传到 GPU，或者把 CuPy 数组下载到 CPU。每次传输约 0.1-1 ms，在循环中累积后严重影响性能。 根本原因：CPU-GPU 之间的数据传输带宽有限（PCIe 约 12 GB/s），且有固定延迟开销（约 10 us），比 GPU 计算本身还慢 正确做法：所有数据在同一设备上处理。用 cp.asarray() 一次性上传，全部计算完后再用 .get() 下载结果

💡 概念误区：认为"GPU 建图就不需要 CPU 了" 新手想法："所有计算都放 GPU 上" 实际上：ROS 消息的序列化/反序列化、TF 查询、话题订阅/发布都在 CPU 上。GPU 只负责计算密集型部分（投影、融合、特征提取）。C++ ROS wrapper 负责 CPU 侧的 IO，Python CuPy 核心负责 GPU 计算。这种 CPU-GPU 分工是工程上的最佳实践。

🧠 思维陷阱：认为"更贵的 GPU 就能解决所有问题" 新手想法："买一块 RTX 4090 就够了" 实际上：机器人上装不了桌面 GPU（功耗 450W、体积大、需要外接电源）。实际部署用的是边缘 GPU（NVIDIA Jetson Orin，功耗 15-60W、GPU 算力是桌面的 1/5-1/3）。在资源受限平台上，算法优化比硬件升级更重要。 正确思维：先用性能分析工具（CuPy profiler）找到瓶颈，再针对性优化。通常 90% 的时间花在 10% 的代码上。

练习¶

[计算题] 一个 6m x 6m、分辨率 2cm 的高程图有多少格子？如果每个格子 6 个 float32 值（高度、方差、坡度 x、坡度 y、粗糙度、可通行性），总共需要多少 GPU 显存？Jetson Orin 有 32 GB 共享内存，这个地图占用多少比例？
[分析题] 为什么光线清理在 GPU 上加速比最高（50x）？从算法特征（数据并行性、内存访问模式、计算密集度）三个角度分析。
[设计题] 如果你的机器人只有一个 Jetson Nano（4 GB 显存、128 CUDA cores），elevation_mapping_cupy 能否运行？如果不能，你会如何修改（降低分辨率？缩小地图？减少层数？）来适配？给出你的配置方案和预期性能。

60.5 地形特征提取与可通行性分析 ⭐⭐¶

本节解决什么问题：高程图只告诉你"地面高度是多少"，但落脚规划需要知道"这里能不能踩"。本节讲解如何从高程图提取坡度、粗糙度、台阶等特征，并综合评估可通行性。

动机：高度不等于可踩¶

考虑两个高程图格子，高度都是 0.1 m：

格子 A：位于一个平缓斜坡的中间，周围 8 个格子高度分别从 0.08 到 0.12 平滑过渡。可以踩。
格子 B：位于一块尖石头的顶部，周围 8 个格子高度从 -0.05 到 0.12 剧烈变化。不能踩。

两者高度相同，但可通行性完全不同。区别在于**局部地形特征**——坡度、粗糙度、曲率。

反面推演：如果只用高度做落脚规划¶

场景：机器人前方有一条 10cm 深的浅沟

只看高度的落脚规划：
  "沟底高度 = -0.1m，沟边高度 = 0.0m"
  → 沟底高度较低但合理
  → 落脚点选在沟底 → 脚陷入 → 卡住

看坡度+粗糙度的规划：
  "沟边坡度 = 45 度（太陡），沟底粗糙度 = 高（碎石）"
  → 沟边和沟底都标记为不可通行
  → 落脚点选在沟两侧的平地 → 跨过 → 安全

四大地形特征¶

特征 1：坡度（Slope）¶

坡度衡量地形的倾斜程度。在高程图上，坡度等于高度的空间梯度：

\[\nabla z = \left(\frac{\partial z}{\partial x}, \frac{\partial z}{\partial y}\right)\]

离散化计算使用 Sobel 算子（3x3 卷积核）：

\[\frac{\partial z}{\partial x} \approx \frac{1}{8r}\begin{bmatrix} -1 & 0 & 1 \\ -2 & 0 & 2 \\ -1 & 0 & 1 \end{bmatrix} * z, \quad \frac{\partial z}{\partial y} \approx \frac{1}{8r}\begin{bmatrix} -1 & -2 & -1 \\ 0 & 0 & 0 \\ 1 & 2 & 1 \end{bmatrix} * z\]

其中 $r$ 是格子分辨率，$*$ 是卷积运算。坡度角：

\[\theta_{\text{slope}} = \arctan\left(\sqrt{\left(\frac{\partial z}{\partial x}\right)^2 + \left(\frac{\partial z}{\partial y}\right)^2}\right)\]

为什么用 Sobel 而不是简单差分？ 简单差分 $(z_{i+1,j} - z_{i-1,j}) / 2r$ 只用了两个点，对噪声非常敏感。Sobel 用 6 个邻居点的加权平均，等效于先平滑再求导，抗噪声能力强得多。

坡度阈值：

坡度	四足可通行性	说明
0-15 度	完全可通行	绝大多数表面
15-30 度	可通行但需减速	摩擦锥约束开始紧张
30-45 度	勉强可通行	需要特殊步态和力控策略
> 45 度	不可通行	超出大多数四足的摩擦锥

特征 2：粗糙度（Roughness）¶

粗糙度衡量地形表面的不规则程度。定义为局部高度值与拟合平面之间的残差标准差：

\[\sigma_{\text{rough}} = \sqrt{\frac{1}{|\mathcal{N}|}\sum_{(i,j) \in \mathcal{N}} (z_{i,j} - \hat{z}_{i,j})^2}\]

其中 $\mathcal{N}$ 是 $(i,j)$ 的邻域（如 5x5 格子），$\hat{z}_{i,j}$ 是对该邻域拟合的平面在 $(i,j)$ 处的高度。

简化计算：如果不拟合平面，可以用局部标准差近似：

\[\sigma_{\text{rough}} \approx \text{std}(\{z_{i,j}\}_{(i,j) \in \mathcal{N}})\]

粗糙度阈值：

粗糙度	含义	四足适用性
< 1 cm	光滑表面	理想
1-3 cm	轻微不平	良好
3-5 cm	碎石/粗糙	谨慎（可能滑脚）
> 5 cm	极度不平	不可通行

特征 3：台阶高度（Step Height）¶

台阶检测是落脚规划特别关注的特征——台阶边缘既是机会（踩上去）也是风险（踩空）。

\[h_{\text{step}} = \max_{(i,j) \in \mathcal{N}} z_{i,j} - \min_{(i,j) \in \mathcal{N}} z_{i,j}\]

用形态学操作实现：

import cupy as cp
from cupyx.scipy.ndimage import maximum_filter, minimum_filter

def compute_step_height(elevation, kernel_size=5):
    """Compute local step height using morphological operations."""
    z_max = maximum_filter(elevation, size=kernel_size)
    z_min = minimum_filter(elevation, size=kernel_size)
    return z_max - z_min

台阶高度阈值（以 Go2 为例，腿长约 20cm）：

台阶高度	含义	策略
< 3 cm	可忽略	正常行走
3-10 cm	小台阶	调整摆动高度
10-20 cm	大台阶	需要特殊步态（爬台阶）
> 20 cm	障碍	绕行

特征 4：间隙检测（Gap Detection）¶

间隙（gap）是地形中没有支撑面的区域——沟渠、裂缝、高程图中的未观测区域。

检测方法：找到连续的无效格子（is_valid == false）区域。如果无效区域宽度超过足底宽度，则标记为"不可跨越间隙"。

可通行性评分（Traversability Score）¶

可通行性地图好比**登山者心中的安全路线图**——登山者站在山脚仰望时，会在脑海中将眼前的岩壁划分为"稳固可踩"、"勉强能抓"和"绝对不碰"三类区域，然后据此规划攀登路线。可通行性评分做的正是同样的事：将每个地形格子标记为从 0（绝对不可踩）到 1（完全安全）的评分，为后续的落脚点搜索提供一张"安全性热力图"。

将四大特征综合为一个 $T \in [0, 1]$ 的评分：

Wermelinger 2016 的方法（手工规则）：

\[T = f_{\text{slope}}(\theta) \cdot f_{\text{rough}}(\sigma) \cdot f_{\text{step}}(h) \cdot f_{\text{gap}}(g)\]

每个分量用 sigmoid 函数归一化：

\[f_{\text{slope}}(\theta) = \frac{1}{1 + e^{k_s(\theta - \theta_{\text{max}})}}\]

其中 $\theta_{\text{max}}$ 是最大可通行坡度（如 30 度），$k_s$ 控制过渡的锐度。

各分量的物理意义：

分量	衡量什么	如果不考虑会怎样
$f_{\text{slope}}$	倾斜程度	踩在陡坡上滑倒
$f_{\text{rough}}$	表面不规则性	踩在碎石上摇晃
$f_{\text{step}}$	局部高差	踩在台阶边缘踩空
$f_{\text{gap}}$	间隙	脚陷入缝隙

学习型可通行性（Miki 2022 / Chavez-Garcia 2018）：

用 CNN 直接从高程图 patch（如 16x16 格子）预测可通行性评分，不需要手工设计特征和阈值。优势在于可以学到复杂的非线性特征组合（如"15 度坡度 + 低粗糙度 → 可通行"vs"15 度坡度 + 高粗糙度 → 不可通行"），手工规则用乘法做不到这种条件组合。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：Sobel 算子在边界格子上产生伪坡度 错误做法：对整个高程图做 Sobel 卷积，包括边界和无效格子现象：有效区域与无效区域（NaN）的边界处，Sobel 输出异常大的梯度值 → 误判为"悬崖边缘" → 可通行性归零 → 规划器找不到可行路径 根本原因：Sobel 卷积核覆盖了 NaN 值的格子，任何包含 NaN 的运算结果都是 NaN 或垃圾值 正确做法：(1) 先用膨胀（dilation）操作标记边界无效区域的邻居为"待定"；(2) Sobel 只在所有 3x3 邻居都有效的格子上计算；(3) 边界格子赋予默认坡度（如 0 或 "unknown"）

💡 概念误区：认为可通行性评分越高越该踩 新手想法："T = 0.95 的格子比 T = 0.90 的好" 实际上：可通行性评分是"能不能踩"的度量，不是"应不应该踩"的度量。落脚点的最终选择还要考虑运动学可达性（这个点脚够不够得着？）和动态稳定性（踩这里后重心会不会超出支撑多边形？）。一个 T = 0.95 但运动学不可达的点，不如一个 T = 0.85 但运动学最优的点。 正确理解：可通行性是落脚评分的一个分量，不是全部。完整评分见 60.6 节。

🧠 思维陷阱：特征阈值不会随机器人类型变化 新手想法："30 度坡度阈值是固定的" 实际上：坡度阈值取决于机器人的腿长、足底面积、摩擦系数和力控能力。ANYmal（大型四足、足底有橡胶垫）可以走 35 度坡，Go2（小型四足、足底光滑）在 25 度就开始打滑。Spot 的橡胶足可以走更陡的坡。 正确做法：阈值参数化，不硬编码。为每种机器人标定一组阈值。

练习¶

[计算题] 给定以下 5x5 高程图 patch（单位 cm），计算中心格子 (3,3) 的 Sobel 坡度（x 和 y 方向）。分辨率 r = 4 cm。
```
10  10  10  10  10
10  12  14  16  18
10  14  18  22  26
10  16  22  28  34
10  18  26  34  42
```
[编程题] 实现一个 Python 函数，输入高程图（NumPy 2D 数组），输出可通行性图。要求：(1) 计算坡度（Sobel）；(2) 计算粗糙度（5x5 邻域标准差）；(3) 用 sigmoid 归一化各分量；(4) 乘积得到可通行性。用 matplotlib 可视化结果。
[设计题] 你的四足机器人需要在建筑工地上行走（有砂砾、钢筋、木板、水坑）。设计一个可通行性评估方案：(1) 需要哪些地形特征？(2) 是否需要 RGB 语义信息（识别水坑）？(3) 手工规则还是学习型？给出你的方案和理由。

60.6 感知驱动的落脚点评分 ⭐⭐¶

本节解决什么问题：有了可通行性地图，如何将它与运动学可达性、动态稳定性结合，形成完整的落脚点评分框架。

动机：可通行性只是必要条件，不是充分条件¶

足式/140_落脚点规划经典方法的 Raibert 启发式给出了一个"动力学最优"的落脚点——它平衡了速度跟踪和稳定性，但完全不考虑地形。60.5 的可通行性分析给出了"地形安全"的度量，但完全不考虑动力学。

真正的落脚点选择需要**两者的交集**。

反面推演：

只看地形的落脚规划：
  "最可通行的格子在 (2.5, 0.3)"
  → 但 Raibert 的理想落脚点在 (2.0, 0.1)
  → (2.5, 0.3) 离髋关节太远 → 膝关节完全伸直 → 力矩不够
  → 机器人腿撑不住 → 摔倒

只看动力学的落脚规划（足式/140_落脚点规划经典方法）：
  "Raibert 最优点在 (2.0, 0.1)"
  → 但 (2.0, 0.1) 是一个凹坑 → T = 0.1（不可通行）
  → 脚陷进去 → 接触力方向不对 → 摔倒

综合评分：
  在 Raibert 最优点附近搜索，找到 (2.1, 0.15)
  → T = 0.85（安全）
  → 运动学余量 70%（够得着）
  → 稳定裕度 85%（ZMP 在支撑多边形内）
  → 最优！

多准则落脚评分框架¶

落脚点评分函数 $J(\boldsymbol{p})$ 由三个分量加权组成：

\[J(\boldsymbol{p}) = w_T \cdot T(\boldsymbol{p}) + w_K \cdot K(\boldsymbol{p}) + w_S \cdot S(\boldsymbol{p})\]

其中： - $T(\boldsymbol{p})$：地形可通行性评分（60.5 节），$T \in [0, 1]$ - $K(\boldsymbol{p})$：运动学可达性评分，$K \in [0, 1]$ - $S(\boldsymbol{p})$：稳定裕度评分，$S \in [0, 1]$ - $w_T, w_K, w_S$：权重，典型值 $w_T = 0.4, w_K = 0.3, w_S = 0.3$

落脚点选择就是在候选集合中最大化评分：

\[\boldsymbol{p}^* = \arg\max_{\boldsymbol{p} \in \mathcal{C}} J(\boldsymbol{p})\]

分量 1：地形可通行性 $T(\boldsymbol{p})$¶

直接查询可通行性地图（60.5 节的输出）。如果需要亚格子精度，使用双线性插值。

分量 2：运动学可达性 $K(\boldsymbol{p})$¶

衡量候选落脚点是否在腿的工作空间内，以及有多少运动学余量。

定义：设髋关节在世界系中的位置为 $\boldsymbol{p}_{\text{hip}}$，腿长为 $l_{\text{leg}}$（大腿 + 小腿），则：

\[d = \|\boldsymbol{p} - \boldsymbol{p}_{\text{hip}}\|\]

\[K(\boldsymbol{p}) = \begin{cases} 0 & d > l_{\max} \text{ or } d < l_{\min} \\ 1 - \left(\frac{d - d_{\text{nom}}}{d_{\text{range}}}\right)^2 & l_{\min} \leq d \leq l_{\max} \end{cases}\]

其中 $d_{\text{nom}}$ 是名义腿长（站立时髋到脚的距离），$d_{\text{range}} = l_{\max} - d_{\text{nom}}$。这是一个以名义位置为中心的二次衰减函数——离名义位置越远，可达性评分越低。

物理含义：$K = 1$ 表示"脚在最舒服的位置"（关节在中间角度，力矩余量最大），$K = 0$ 表示"够不着"或"太近了"（关节在极限角度）。

分量 3：稳定裕度 $S(\boldsymbol{p})$¶

衡量选择这个落脚点后，机器人的动态稳定性。

简化方法——支撑多边形裕度：计算新落脚点加入后的支撑多边形，评估 ZMP 到多边形边界的距离：

\[S(\boldsymbol{p}) = \frac{d_{\text{ZMP-to-edge}}}{\max(d_{\text{ZMP-to-edge}})}\]

更精确的方法——Capture Point 裕度（足式/70_腿足简化模型理论 / 足式/140_落脚点规划经典方法）：

\[S(\boldsymbol{p}) = 1 - \frac{\|\boldsymbol{\xi} - \boldsymbol{p}\|}{\|\boldsymbol{\xi} - \boldsymbol{p}_{\text{hip}}\|}\]

其中 $\boldsymbol{\xi}$ 是 DCM / Capture Point。落脚点越接近 Capture Point，稳定裕度越高。

候选集合的生成¶

候选落脚点不是在全图搜索（太慢），而是在 Raibert 启发式的预测点附近生成：

候选集合生成策略：

Raibert 预测点 p_raibert
    ↓
以 p_raibert 为中心，生成 N x N 网格（如 11x11，间距 2cm）
    ↓
对每个候选点查询 T, K, S
    ↓
选择 J(p) 最大的点作为最终落脚点

候选集合大小：$11 \times 11 = 121$ 个点。每个点的评分计算是 O(1)（查表 + 简单算术），总计 121 次 O(1) 操作，耗时 < 0.1 ms。

代码示例¶

struct FootholdCandidate {
    Eigen::Vector3d position;
    double terrain_score;      // T
    double kinematic_score;    // K
    double stability_score;    // S
    double total_score;        // J = w_T*T + w_K*K + w_S*S
};

Eigen::Vector3d selectBestFoothold(
    const Eigen::Vector3d& raibert_prediction,
    const Eigen::Vector3d& hip_position,
    const Eigen::Vector3d& capture_point,
    const ElevationMap& terrain_map,
    double search_radius = 0.1,       // 10cm search radius around Raibert
    double search_resolution = 0.02,  // 2cm grid
    double w_T = 0.4, double w_K = 0.3, double w_S = 0.3)
{
    FootholdCandidate best;
    best.total_score = -1.0;

    int n = static_cast<int>(search_radius / search_resolution);
    for (int di = -n; di <= n; ++di) {
        for (int dj = -n; dj <= n; ++dj) {
            Eigen::Vector3d candidate = raibert_prediction;
            candidate.x() += di * search_resolution;
            candidate.y() += dj * search_resolution;

            // Query terrain height at candidate position
            if (!terrain_map.isInside(candidate.head<2>())) continue;
            candidate.z() = terrain_map.atPosition("elevation",
                                                    candidate.head<2>());

            // T: terrain traversability
            double T = terrain_map.atPosition("traversability",
                                               candidate.head<2>());
            if (T < 0.3) continue;  // Hard threshold: skip bad terrain

            // K: kinematic reachability
            double dist = (candidate - hip_position).norm();
            double d_nom = 0.25;    // nominal leg length for Go2
            double d_range = 0.08;
            double K = std::max(0.0,
                1.0 - std::pow((dist - d_nom) / d_range, 2));

            // S: stability margin (simplified Capture Point distance)
            double cp_dist = (capture_point - candidate).head<2>().norm();
            double cp_range = (capture_point - hip_position).head<2>().norm();
            double S = std::max(0.0,
                1.0 - cp_dist / std::max(cp_range, 0.01));

            double J = w_T * T + w_K * K + w_S * S;
            if (J > best.total_score) {
                best = {candidate, T, K, S, J};
            }
        }
    }
    return best.position;
}

⚠️ 常见陷阱¶

⚠️ 编程陷阱：搜索半径过大导致选择远离 Raibert 点的落脚位 错误做法：设置 search_radius = 0.5（50cm）现象：找到了一个可通行性很高的点，但离 Raibert 预测点 40cm 远 → 步幅突变 → MPC 跟踪失败 → 摇晃或摔倒 根本原因：Raibert 启发式基于 LIPM 动力学的稳定性分析，大幅偏离意味着违反动力学约束 正确做法：搜索半径不超过步幅的 30%（Go2 步幅约 20cm → 搜索半径 <= 6cm），或在评分中加入"离 Raibert 点距离"的惩罚项

💡 概念误区：认为权重 $w_T, w_K, w_S$ 是固定的 新手想法："一组权重 everywhere 通用" 实际上：在平坦地形上，地形评分到处都是 1.0，运动学和稳定性权重更重要；在复杂地形上，地形评分才是关键区分因素。自适应权重策略：$w_T \propto (1 - \bar{T})$，地形越复杂，地形权重越大。

练习¶

[计算题] 给定 Raibert 预测点 $(0.20, 0.05, 0.0)$，髋关节位置 $(0.0, 0.1, 0.35)$，Capture Point $(0.22, 0.04)$，名义腿长 $d_{\text{nom}} = 0.25$ m，范围 $d_{\text{range}} = 0.08$ m。计算 Raibert 点的 $K$ 和 $S$ 值。
[设计题] 在 stepping stones 场景中（足式/150_优化驱动落脚与接触规划），落脚点必须在指定的石头上。如何修改评分框架来处理这种硬约束？提示：可以将石头外的区域设置 $T = 0$。
[思考题] 权重 $w_T = 0.4, w_K = 0.3, w_S = 0.3$ 意味着什么？如果把 $w_K$ 设为 0 会怎样？如果把 $w_T$ 设为 1.0, 其他为 0 呢？讨论不同权重配置的行为差异。

60.7 TAMOLS：地形感知运动优化 ⭐⭐⭐¶

本节解决什么问题：60.6 的评分框架是一个**贪心搜索**——对每条腿独立评分，没有考虑四条腿之间的耦合和多步前瞻。TAMOLS 把地形感知嵌入到**轨迹优化**中，同时优化基座轨迹和所有落脚点。

动机：贪心评分的局限¶

60.6 的评分方法有三个根本局限：

单腿独立：对每条腿分别评分，不考虑四条腿落脚点的几何关系（如支撑多边形的形状）
单步贪心：只看当前步最优，不考虑下一步。"当前步踩在最高评分点"可能导致"下一步无处可踩"
基座-落脚耦合：基座的位姿和速度决定了落脚点的可达范围，反过来落脚点又约束了基座的运动。贪心评分忽略了这种双向耦合

TAMOLS（Terrain-Aware Motion Optimization for Legged Systems）正是为解决这些局限而设计的。

历史：从 MPC + 贪心到联合优化¶

进化路径：

足式/140_落脚点规划经典方法 Raibert启发式  →  不考虑地形
       ↓
足式/160_感知驱动落脚规划.6 贪心评分     →  考虑地形，但单腿单步
       ↓
TAMOLS 联合优化     →  考虑地形 + 多腿耦合 + 多步前瞻
       ↓
Perceptive MPC      →  地形嵌入 OCS2 的 NMPC（足式/230_Perceptive_MPC 精读）

TAMOLS 的核心思想¶

Jenelten 等人（2022, T-RO）提出了一个优雅的框架：在轨迹优化的代价函数中加入地形信息，让优化器自己发现最优的基座轨迹和落脚点组合。

这篇论文的作者来自 ETH Zurich，包括 Fabian Jenelten、Ruben Grandia、Farbod Farshidian 和 Marco Hutter。论文于 2022 年 6 月首次在 arXiv 发布，随后发表在 IEEE Transactions on Robotics（T-RO）。

数学框架：

\[\min_{\boldsymbol{x}, \boldsymbol{p}_f} \quad J_{\text{task}}(\boldsymbol{x}) + J_{\text{terrain}}(\boldsymbol{p}_f) + J_{\text{collision}}(\boldsymbol{x})\]

\[\text{s.t.} \quad \boldsymbol{x}_{k+1} = f(\boldsymbol{x}_k, \boldsymbol{u}_k) \quad \text{(dynamics)}\]

\[\boldsymbol{p}_f^{(l)} \in \mathcal{W}_{\text{kin}}^{(l)}(\boldsymbol{x}) \quad \text{(kinematic reachability)}\]

\[p_{f,z}^{(l)} = z_{\text{map}}(p_{f,x}^{(l)},\; p_{f,y}^{(l)}) \quad \text{(foot on terrain)}\]

其中： - $\boldsymbol{x}$ 是基座状态轨迹（位置、姿态、速度） - $\boldsymbol{p}_f = \{\boldsymbol{p}_f^{(1)}, \ldots, \boldsymbol{p}_f^{(L)}\}$ 是所有脚的落脚点 - $J_{\text{task}}$：任务代价（速度跟踪、姿态保持），与足式/110_OCS2完整栈与双线程MPC OCS2 的代价相同 - $J_{\text{terrain}}$：地形代价，偏好高可通行性区域 - $J_{\text{collision}}$：碰撞回避代价，防止腿部碰到地形障碍

地形代价 $J_{\text{terrain}}$ 的设计：

\[J_{\text{terrain}} = \sum_{l=1}^{L} \left[ -\alpha_T \cdot T(\boldsymbol{p}_f^{(l)}) + \alpha_{\text{soft}} \cdot \text{softness}(\boldsymbol{p}_f^{(l)}) \right]\]

负号表示**最大化可通行性**（因为优化器做最小化）。$\text{softness}$ 衡量地面的柔软程度——在松软地面上落脚会下陷，影响后续运动。

碰撞回避——SDF 的妙用¶

TAMOLS 最巧妙的部分之一是用 SDF 做腿部碰撞回避。传统方法需要检测腿部几何体与地形的碰撞——计算量大且不可微。TAMOLS 的做法：

从高程图计算地面 SDF：$\text{SDF}(x, y, z) = z - z_{\text{map}}(x, y)$
在腿部采样若干检查点（如膝关节、胫骨中点）
对每个检查点，要求 $\text{SDF}(\boldsymbol{p}_{\text{check}}) \geq d_{\text{safe}}$
用 log-barrier 或 relaxed inequality 把约束变成代价

碰撞回避示意：

              机器人基座
              /        \
             /          \
   大腿──[膝关节检查点]──大腿
            |            |
   小腿──[胫骨检查点]──小腿
            |            |
          [脚]          [脚]
            ↓            ↓
   ~~~~~~~~地形~~~~~~~~   ← SDF = 0 的等值面
   ///////障碍///////

如果[膝关节检查点]处 SDF < d_safe → 加惩罚 → 优化器调整基座高度或落脚位置

为什么 SDF 是理想选择？

方案	可微性	计算量	精度
几何碰撞检测	不可微	O(三角面数)	精确
体素碰撞	不可微	O(检查点数)	离散
SDF	可微	O(检查点数)	连续

SDF 天然可微意味着优化器可以用梯度方法高效求解。

Graduated Optimization——避免局部最优¶

地形代价是高度非凸的——高程图有很多局部起伏。如果直接优化，优化器容易陷入局部最优（比如落脚在一个小平台上，周围还有更好的大平台但被"山谷"隔开）。

TAMOLS 使用 **graduated optimization（渐进优化）**策略：

先模糊后清晰：开始时用一个平滑（大核高斯模糊）版本的高程图，代价函数近似凸；然后逐步用越来越清晰的高程图，让优化结果逐步精化
物理类比：类似于模拟退火——先"高温"看到全局景观，再"降温"收敛到精确位置

Graduated Optimization 示意：

第 1 轮（模糊 sigma=20cm）：地形看起来像一个大斜面
  → 优化器找到大致方向

第 2 轮（模糊 sigma=10cm）：地形看起来有一些大的起伏
  → 优化器在大致方向上精化

第 3 轮（模糊 sigma=4cm）：地形细节出现
  → 优化器在精确位置上收敛

第 4 轮（原始分辨率）：最终结果
  → 落脚点在最优位置

实时性能¶

TAMOLS 使用直接配点法（Direct Collocation）做轨迹优化，每次求解在 < 10 ms 内完成（在台式机 CPU 上）。这意味着可以在 MPC 的外层循环中以 100 Hz 运行——足以应对动态变化的地形。

关键实现细节： - 转录方法：直接配点（Hermite-Simpson） - 求解器：IPOPT（Interior Point） - 梯度：解析梯度 + CppAD 自动微分 - 高程图查询：双线性插值 + 缓存

TAMOLS vs 贪心评分 vs Perceptive MPC¶

特性	贪心评分（60.6）	TAMOLS	Perceptive MPC（足式/230_Perceptive_MPC）
落脚选择	网格搜索	优化	优化
基座-落脚耦合	无	有	有
多步前瞻	1 步	多步	多步
碰撞回避	无	SDF	SDF
求解时间	< 0.1 ms	< 10 ms	10-50 ms
适用场景	轻度不平	中度不平	复杂地形
开源	自行实现	部分	OCS2

⚠️ 常见陷阱¶

⚠️ 编程陷阱：高程图插值在地图边界产生不连续梯度 错误做法：用最近邻插值代替双线性插值现象：优化器收到阶梯状的代价函数 → 梯度不连续 → IPOPT 步长振荡 → 不收敛或收敛到次优解 根本原因：IPOPT 等 NLP 求解器假设代价函数至少 C1 连续（一阶连续可微）。最近邻插值产生的代价函数是 C0（连续但不可微） 正确做法：双线性插值保证 C0 连续且几乎处处可微（除了格子边界）。如果需要更高光滑度，用三次样条插值（C2）

💡 概念误区：认为 TAMOLS 替代了 MPC 新手想法："有了 TAMOLS 就不需要 OCS2 MPC 了" 实际上：TAMOLS 是在 MPC 的**代价函数中加入地形信息**，它本身就是一种增强的 MPC。更准确地说，TAMOLS 优化的是**离线/低频**的落脚点和基座参考轨迹，而 OCS2 MPC 负责**在线/高频**的轨迹跟踪和力控。两者是层级关系：TAMOLS 给 MPC 提供更好的参考，MPC 负责实时跟踪。

练习¶

[推导题] 从高程图 $z_{\text{map}}(x, y)$ 推导 2.5D SDF：$\text{SDF}(x, y, z) = z - z_{\text{map}}(x, y)$。验证 (1) 地面上 SDF = 0；(2) 地面上方 SDF > 0；(3) 地面下方 SDF < 0。计算 $\nabla \text{SDF}$ 并验证其是否满足 Eikonal 方程 $\|\nabla \text{SDF}\| = 1$（提示：一般不严格满足，为什么？）。
[分析题] Graduated optimization 为什么能避免局部最优？用一个 1D 的例子说明：$f(x) = \sin(10x) + 0.1x^2$ 的局部最优很多，但高斯模糊后 $f_\sigma(x) = (f * g_\sigma)(x)$ 的局部最优减少。画出 $\sigma = 0.1, 0.5, 2.0$ 时的函数图形。
[思考题] TAMOLS 的求解时间 < 10 ms，Perceptive MPC 的求解时间 10-50 ms。性能差异来自哪里？（提示：比较决策变量的数量、约束的复杂度、优化的 horizon 长度。）

60.8 学习型落脚选择 ⭐⭐⭐¶

本节解决什么问题：手工设计的评分函数和基于模型的优化在复杂、未知地形上会力不从心。本节讲解用强化学习（RL）和深度学习直接从感知数据到落脚/运动决策的端到端方法。

动机：为什么需要学习¶

手工方法（60.5-60.7）需要人类设计特征、阈值、代价函数。这在**已知地形类型**上工作良好（碎石、台阶、斜坡这些都有明确的几何定义），但在**未知或复杂地形**上会遇到天花板：

形状奇异的地形：树根、倒下的树干、不规则建筑废墟——无法用简单的坡度/粗糙度描述
材质差异：冰面和石面的几何特征相似，但可通行性完全不同——纯几何方法区分不了
动态交互：松软泥地会因为踩踏而变形——静态高程图无法预测这种动态效果
高度敏捷动作：跑酷（parkour）——跳跃、攀爬、翻越——无法用简单的评分函数覆盖

反面推演：如果只用手工方法做跑酷¶

场景：ANYmal 需要跳上 30cm 高的平台

手工方法的困境：
  1. 可通行性地图说"平台顶面 T = 1.0，侧面 T = 0.0"
  2. Raibert 启发式不知道如何"跳" → 只会走
  3. TAMOLS 优化器可以找到"腿踩在平台顶部"的解 → 但不知道要先蹲下蓄力
  4. 整个动作序列（蹲 → 蓄力 → 跳 → 着陆 → 稳定）需要非周期步态 → 手工设计不了

学习方法的解法：
  1. 在仿真中随机生成各种高度的平台
  2. RL agent 通过 reward shaping 学会跳跃
  3. Teacher 在训练时知道完美地形 → 学会最优策略
  4. Student 只用传感器数据 → 模仿 teacher → 在真机部署

Miki 2022（Science Robotics）——分水岭论文¶

Takahiro Miki 等人（2022）发表在 Science Robotics 上的论文 "Learning robust perceptive locomotion for quadrupedal robots in the wild" 是学习型感知运动控制的里程碑工作。

核心架构：

┌───────────────────────────────────────────────┐
│                 Student Policy                 │
│                                                │
│  ┌──────────┐   ┌──────────────────────────┐  │
│  │本体感知   │   │ 地形感知                  │  │
│  │(proprio)  │   │ (extero)                 │  │
│  │           │   │                          │  │
│  │·关节角度  │   │·高程图采样点 (52 个)      │  │
│  │·关节速度  │   │·沿脚前方扇形分布          │  │
│  │·IMU      │   │·从 Elevation Map 查询     │  │
│  │·接触状态  │   │                          │  │
│  └────┬─────┘   └────────┬─────────────────┘  │
│       │                  │                     │
│       └──────┬───────────┘                     │
│              ↓                                 │
│     ┌────────────────┐                         │
│     │  Belief Encoder │ ← 关键创新              │
│     │  (GRU/LSTM)     │                        │
│     │  估计隐藏状态    │                        │
│     └────────┬───────┘                         │
│              ↓                                 │
│     ┌────────────────┐                         │
│     │  MLP Policy     │                        │
│     │  输出：关节位置  │                        │
│     └────────────────┘                         │
└───────────────────────────────────────────────┘

关键创新——Belief Encoder：

传感器数据（尤其是来自深度相机的高程采样）是**有噪声、部分遮挡、有延迟**的。直接把原始传感器数据输入策略网络，策略会学到"不信任传感器"——因为噪声太大时，信任传感器反而不如忽略它。

Miki 的解决方案是加入一个 Belief Encoder——一个循环神经网络（GRU），用历史观测来推断"我相信地形长什么样"。这类似于一个**学习型 Kalman 滤波器**：

当传感器可靠时（近距离、良好光照），Belief Encoder 主要依赖当前观测
当传感器不可靠时（远距离、遮挡、强光），Belief Encoder 依赖历史记忆和本体感知

Teacher-Student 训练框架：

阶段	输入	环境	目的
Teacher 训练	完美地形 + 完美状态（"作弊"）	Isaac Gym 仿真	学习最优策略（无传感器限制）
Student 蒸馏	有噪声的传感器 + 本体感知	Isaac Gym + 噪声注入	从 Teacher 模仿，学习在真实传感器下工作
真机部署	真实传感器	真实世界	Student 直接部署

为什么需要 Teacher-Student？ 直接用真实传感器训练（end-to-end RL）太难了——传感器噪声使得 reward 信号非常嘈杂，RL 收敛极慢。Teacher 在"完美感知"下先学到最优行为，Student 再学习"如何用不完美的感知做到同样的事"。

实战验证：

这个控制器在 DARPA Subterranean Challenge 中作为 ANYmal 的默认运动控制器，驱动四台 ANYmal 在地下环境中自主探索超过 1700 米**无一摔倒**。地形包括碎石、泥地、管道、台阶、斜坡。团队 CERBERUS 赢得了决赛冠军。

Hoeller 2024（ANYmal Parkour）——端到端跑酷¶

如果说 Miki 2022 是"在复杂地形上可靠行走"，Hoeller 2024（Science Robotics）则是"在极端地形上做高难度动作"。

ANYmal Parkour 的突破： - 跳上 40cm 高的平台 - 跳过 60cm 宽的间隙 - 在狭窄木板上平衡行走 - 连续跑酷动作链（跳+爬+平衡） - 真机持续运行 30 分钟不重启

架构差异（vs Miki 2022）：

特性	Miki 2022	Hoeller 2024
目标	鲁棒行走	敏捷跑酷
步态	周期性 trot	非周期性（跳、爬）
感知	52 个高程采样点	局部高程图 patch
训练	单阶段课程	多阶段课程（从简到难）
真机性能	稳定但保守	激进且快速

Gangapurwala 等人——CNN 落脚评估¶

另一条研究路线（Gangapurwala 2019, ICRA）不做端到端控制，而是**用 CNN 评估落脚质量**，然后将评估结果传递给传统的 MPC/WBC 控制器。

方法： 1. 截取候选落脚点周围的高程图 patch（如 16x16 格子） 2. 用 CNN 预测"如果脚放在这里，接下来 0.5 秒的稳定性如何" 3. 用预测分数替代手工评分函数（60.6 节）

优势：保留了 MPC/WBC 的物理约束保证，只用学习替代最难的"地形评估"环节。劣势：需要大量标注数据（每个落脚点的"实际稳定性"需要在仿真中测试）。

三种方法的对比¶

维度	手工评分 (60.6)	TAMOLS (60.7)	学习型 (60.8)
设计难度	低	中	高（需要 RL 专家）
泛化能力	低	中	高
可解释性	高	高	低（黑箱）
安全保证	有（物理约束）	有（优化约束）	弱（靠训练分布）
极端地形	差	中等	强
计算需求	低	中（IPOPT）	高（GPU 推理）
典型代表	Wermelinger 2016	Jenelten 2022	Miki 2022, Hoeller 2024

⚠️ 常见陷阱¶

⚠️ 编程陷阱：RL 策略在仿真和真机之间的 sim-to-real gap 错误做法：在 Isaac Gym 中用完美的传感器训练策略，直接部署到真机现象：真机上策略行为混乱——转圈、抖动、摔倒 根本原因：仿真中的传感器噪声模型与真实传感器差异巨大。深度相机在近距离有失效区域（< 0.3m），在强光下有过曝区域，在透明物体上返回错误深度——这些在仿真中通常被忽略 正确做法：(1) Domain randomization：在训练时随机化传感器噪声、延迟、遮挡；(2) Teacher-Student 蒸馏；(3) Belief Encoder 处理传感器不确定性

💡 概念误区：认为"端到端 RL 不需要理解传统方法" 新手想法："既然 RL 可以端到端学，我不需要学 Raibert / MPC / WBC" 实际上：(1) RL 的 reward 设计需要深刻理解传统控制的目标（速度跟踪、能耗、平滑性——这些概念来自足式/110_OCS2完整栈与双线程MPC-58）；(2) 当 RL 策略失败时，需要用传统方法诊断原因；(3) 工业部署中，RL + MPC 混合方案（如 DTC）比纯 RL 更常见。传统方法是基础，RL 是上层建筑。

🧠 思维陷阱：认为"学习型方法一定比优化方法好" 新手想法："论文里 RL 效果最好，所以应该用 RL" 实际上：RL 在**极端地形**上表现最好，但在**常规地形**上通常不如 TAMOLS + MPC 稳定。RL 策略的行为在训练分布外不可预测，可能在看似简单的场景中突然失效。工程实践中的趋势是**分层混合**：高层用 RL 做决策（步态选择、大致方向），低层用 MPC/WBC 做执行（力控、轨迹跟踪）。 DTC（Jenelten 2024, Science Robotics） 就是这种混合方案的代表：RL 输出参考关节轨迹 → MPC 跟踪轨迹 → WBC 输出扭矩。

练习¶

[分析题] Teacher-Student 框架中，Teacher 的"作弊信息"（完美地形、完美状态）具体包括哪些？列举 5 个 Teacher 知道但 Student 不知道的信息。解释为什么直接训练 Student（不用 Teacher）更难。
[设计题] 你想用 RL 训练一个四足机器人爬台阶。设计 reward function：(1) 正向奖励哪些行为？(2) 惩罚哪些行为？(3) 如何设计课程学习（从什么开始，逐步增加什么难度）？
[思考题] Miki 2022 的 Belief Encoder 和 Kalman 滤波有什么相似和不同？从"预测-更新"、"不确定性处理"、"模型假设"三个角度比较。

60.9 传感器选型与标定 ⭐⭐¶

本节解决什么问题：前面讲了算法，这里讲硬件——用什么传感器看地形、如何标定传感器、如何融合多传感器数据。

动机：算法再好，传感器选错也白搭¶

同样的 Elevation Mapping 算法，配上不同的传感器，效果天差地别：

传感器配置	室内效果	室外效果	问题
只用深度相机	优秀	差	强光下深度失效
只用 LiDAR	良好	优秀	室内近距离盲区
深度相机 + LiDAR	最佳	最佳	标定复杂，数据同步难

深度相机 vs LiDAR——全面对比¶

维度	深度相机 (Intel RealSense D435)	旋转 LiDAR (Velodyne VLP-16)	固态 LiDAR (Livox Mid-360)
工作原理	红外结构光/双目	905nm 激光旋转扫描	905nm 花瓣扫描
测距范围	0.3-10 m	1-100 m	0.1-40 m
精度（1m）	约 2 mm	约 3 mm	约 2 mm
精度（4m）	约 20 mm	约 5 mm	约 3 mm
分辨率	848x480 深度像素	16 线 x 1800 点/圈	等效 200 线
帧率	30-90 fps	10-20 Hz	10-20 Hz
视场角	87 x 58 度	360 x 30 度	360 x 59 度
环境鲁棒性	差（强光、透明、反光失效）	好	好
重量	72 g	830 g	265 g
功耗	< 2 W	约 8 W	约 9 W
价格	约 $300	约 $4000	约 $1200
RGB 同步	内置 RGB 相机	无	无
近距离	0.3m 盲区	1m 盲区	0.1m（优势）

选型决策指南：

你的机器人主要在哪里工作？
  │
  ├─ 室内 → 深度相机为主 + 可选小型 LiDAR
  │   └─ 原因：室内距离近（< 5m），深度相机分辨率高；
  │          LiDAR 在近距离有盲区
  │
  ├─ 室外 → LiDAR 为主 + 可选深度相机
  │   └─ 原因：室外光照变化大，深度相机在强光下失效；
  │          LiDAR 不受光照影响
  │
  └─ 混合 → LiDAR + 深度相机（推荐）
      └─ ANYmal 方案：1x Velodyne VLP-16（全局建图）
                     + 2x RealSense D435（前方精细感知）

传感器安装位置的工程考量¶

传感器的安装位置对高程图质量有决定性影响。以下是工程实践中的关键设计参数：

设计参数	影响	Go2 推荐值	ANYmal 推荐值
前视相机俯仰角	决定前方可视距离 vs 近场覆盖	前倾 20-25 度	前倾 15-20 度
安装高度	高 → 视野广但精度低；低 → 精度高但盲区大	头顶 ~30 cm	头顶 ~50 cm
LiDAR 安装位置	中心 vs 前方 → 影响遮挡模式	背部中心	背部中心
下视传感器类型	深度相机 vs 1D 激光	1D TOF（轻量）	RealSense D435i

安装位置优化的本质：传感器的视场（Field of View）和机器人的工作空间（腿的可达范围）之间需要最大化重叠。落脚规划只关心腿能够到的区域——在这个区域内必须有高质量的高程数据。

如果传感器安装位置不对会怎样？一个常见的失败模式：前视相机安装得太水平（俯仰 5 度），导致前方 3-5 米的地面可以看到但脚下 0.5 米看不到。MPC 可以规划 5 步之后的路径，但当前步的落脚点却没有高程数据——这比完全没有感知还危险，因为系统"以为"自己有感知。

本质洞察：传感器安装位置的选择本质上是"看远"和"看近"之间的权衡。看远有利于前瞻规划（提前发现障碍），看近有利于当前步精确落脚。理想情况下需要两个传感器分别覆盖远近——这正是 ANYmal 使用 LiDAR（看远）+ 深度相机（看近）组合的原因。对于预算有限的小型四足（如 Go2），一个前倾 20 度的深度相机是性价比最高的选择——牺牲了远视距但保证了关键的 0.5-2 米近场覆盖。

外参标定——传感器到基座的变换¶

外参 $\boldsymbol{T}_{bs}$（base → sensor）的精度直接影响高程图质量。1 度的角度误差在 4m 距离上产生 7cm 的高度误差——超过了 Go2 的足底宽度。

标定方法：

方法 1：CAD 直接读取（粗略，精度约 5 mm / 1 度）：从机器人的 URDF/CAD 模型直接读取传感器安装位置和角度。

方法 2：靶标标定（精确，精度约 1 mm / 0.1 度）： 1. 在环境中放置已知位置的标定靶标（棋盘格、ArUco marker） 2. 用传感器和外部参考系统（全站仪、动捕系统）同时观测靶标 3. 通过最小化重投影误差 $\min_{\boldsymbol{T}_{bs}} \sum \|\boldsymbol{p}_{\text{ref}} - \boldsymbol{T}_{ws} \boldsymbol{p}_s\|^2$ 求解外参

方法 3：手眼标定（hand-eye calibration）（适用于没有外部参考的场景）：利用机器人自身的运动，通过 $AX = XB$ 方程求解。

时间同步——被忽视的关键问题¶

不同传感器的时间戳不同步会导致高程图"拖影"。机器人以 1 m/s 移动，10 ms 的时间差导致 1 cm 的位置偏移——在 4 cm 分辨率的高程图上占 1/4 格子。

同步方案：

方案	精度	复杂度
ROS 时间戳（`header.stamp`）	约 10 ms	低
PTP/NTP 硬件同步	约 1 ms	中
触发线同步	约 0.1 ms	高

对于四足行走（速度 < 1.5 m/s），ROS 时间戳的约 10 ms 精度通常足够。对于高速跑动（> 2 m/s），建议使用 PTP 同步。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：深度相机在机器人行走时产生运动模糊 错误做法：在深度相机曝光时间内机器人大幅运动现象：深度图出现大面积无效区域或错误深度值 根本原因：结构光深度相机需要投射红外图案并匹配，运动导致图案位移 → 匹配失败。曝光时间 33 ms 内，1 m/s 运动产生 33 mm 位移 正确做法：(1) 降低曝光时间（提高增益作为补偿，牺牲信噪比）；(2) 在摆动相开始时（机身最稳定）触发深度采集；(3) 使用 LiDAR 作为主传感器（不受运动影响）

💡 概念误区：认为标定做一次就永久有效 新手想法："出厂标定好了就不用管了" 实际上：振动、温度变化、碰撞都会导致传感器安装位置微小偏移。腿足机器人的振动尤其强烈（每步着地冲击 3-5 g），长期累积可能导致螺丝松动、支架变形。 正确做法：定期校验标定质量（如让机器人观测一个已知平面，检查高程图是否平整）。如果误差超过阈值，重新标定。

练习¶

[选型题] 为以下三种场景选择传感器配置，给出理由：(a) 仓库巡检机器人（室内、平整水泥地、有货架遮挡）；(b) 野外搜救机器人（户外、碎石斜坡、夜间作业）；(c) 实验室研究平台（室内、已知环境、需要最高精度）。
[计算题] 传感器外参有 0.5 度的俯仰角误差。计算在 2m 和 4m 距离处，这个误差造成的高度偏差分别是多少 mm。如果高程图分辨率是 4cm，这个偏差会导致什么问题？
[工程题] 设计一个简单的标定质量检测程序：让机器人站在已知的平面上（如地板），采集高程图，计算所有有效格子高度的标准差。如果标准差超过某个阈值，报警提示需要重新标定。这个阈值应该设为多少？

🔧 故障排查手册¶

感知建图和感知驱动落脚涉及传感器、数据融合、特征计算、控制接口等多个环节，任何一环出错都会导致机器人踩错或摔倒。以下是工程部署中最常见的故障场景。

症状	可能原因	排查步骤	相关章节
高程图出现"鬼影"（同一障碍物重复出现）	里程计漂移导致不同帧点云投影到错误位置	1. 在 RViz 中叠加点云与高程图检查对齐 2. 打印里程计协方差是否持续增大 3. 确认回环修正后地图是否自愈	60.3, 足式/130_腿足状态估计
可通行性评分全部接近零，规划器找不到可行落脚点	Sobel 坡度计算在有效区域与 NaN 边界处产生虚假大梯度	1. 可视化 `is_valid` 层检查有效覆盖率 2. 确保 Sobel 只在所有 3x3 邻居都有效的格子上计算 3. 对无效格子赋默认坡度	60.5
深度相机在强光或全黑环境下高程图大面积缺失	结构光/主动立体相机在强日光或无纹理表面失效	1. 检查点云数量是否骤降 2. 加入 LiDAR 补充 3. 实现"置信度低时切回盲走"的降级逻辑	60.9
GPU 建图延迟突然增大（从 2ms 涨到 20ms+）	CuPy 与 NumPy 数组混用导致隐式 CPU-GPU 数据传输	1. 用 CuPy profiler 检查隐式转换 2. 确保所有计算在同一设备上 3. 检查 GPU 显存是否触发 swap	60.4
落脚点选在可通行但运动学不可达的位置	评分只考虑地形可通行性，未检查关节极限或腿长约束	1. 在评分函数中加入运动学可达性分量 2. 对候选点做快速 IK 可行性检查 3. 用可达工作空间掩码预过滤候选集	60.6

60.10 前沿:Foundation Model 与 Neural SDF 在地形感知中的应用 ⭐⭐⭐⭐¶

2024-2025 年,两项技术趋势正在重塑腿足地形感知的面貌:大规模视觉-语言基础模型(Foundation Model / VLM)和神经隐式地形表示(Neural SDF)。

Foundation Model for Terrain Perception (VLM + Elevation Map)¶

动机: 传统的 Elevation Mapping(60.3)和可通行性分析(60.5)只使用**几何信息**——地面的高度、坡度、粗糙度。但同一个几何形状在不同材质上的可通行性完全不同:一块坡度 20 度的石板可以安全行走,但同样坡度的冰面就会滑倒;一块粗糙度很高的草地可能安全,但粗糙度同样高的碎玻璃则不行。

VLM 的角色:视觉-语言模型(如 GPT-4V, Gemini, LLaVA)可以从 RGB 图像中提取**语义信息**——"这是草地"、"这是水坑"、"这是铁板"、"这是冰面"。将这些语义标签与 Elevation Map 的几何特征结合,可以构建**语义增强的可通行性图**:

\[T_{\text{semantic}}(i, j) = T_{\text{geom}}(i, j) \times f_{\text{material}}(\text{label}(i, j))\]

其中 $f_{\text{material}}$ 是材质到摩擦系数/承载力/可踩性的映射函数。

当前进展与局限: - Google DeepMind RT-2 (2023) 展示了 VLM 在机械臂操作中的语义推理能力,但尚未直接用于足式落脚 - Concept2Robot (2024) 尝试用自然语言描述地形("避开水坑,走石头路"),VLM 解析为可通行性约束 - 局限: VLM 推理速度慢(100-500 ms/帧),无法在 1kHz 控制循环中使用。工程上通常在 1-5 Hz 的"感知层"运行 VLM,生成语义标签后缓存在 Elevation Map 中,供高频控制层查询

架构设计:

RGB 图像 (30 Hz)
    ↓
VLM 推理 (1-5 Hz, GPU)
    ↓
语义标签图 ("grass", "ice", "gravel", ...)
    ↓
┌──────────────────────────────────────┐
│ Elevation Map (grid_map)             │
│  layer: "elevation"  ← 几何 (60.3)  │
│  layer: "slope"      ← 几何 (60.5)  │
│  layer: "roughness"  ← 几何 (60.5)  │
│  layer: "material"   ← 语义 (VLM)   │ ← 新增
│  layer: "friction"   ← 语义 → 物理  │ ← 新增
│  layer: "traversability" ← 融合     │
└──────────────────────────────────────┘
    ↓
落脚点评分 (60.6) / TAMOLS (60.7) / MPC

Neural SDF 用于地形表示¶

动机: 传统高程图(60.2)是 2.5D 栅格——内存占用与分辨率的平方成正比,且无法表示悬垂结构。Neural SDF(Neural Signed Distance Field)用一个小型神经网络 $f_\theta: \mathbb{R}^3 \to \mathbb{R}$ 隐式表示地形,输入 3D 坐标,输出到最近表面的有符号距离。

Neural SDF 的优势:

特性	高程图 (2.5D)	Neural SDF
内存	$O(W^2/r^2)$,与分辨率平方成正比	$O(
分辨率	固定(2-5 cm)	连续(理论无限分辨率)
悬垂结构	不支持	支持
梯度(法向量)	有限差分近似	解析梯度 $\nabla f_\theta$
MPC 兼容性	双线性插值可微	天然可微(神经网络输出)
更新效率	O(1) 格子更新	需重训练或微调
查询速度	O(1) 栅格索引	~0.1 ms(GPU 推理)

在 TAMOLS/MPC 中的应用: TAMOLS(60.7)使用 SDF 做碰撞回避。如果用 Neural SDF 替代离散 SDF,可以获得更精确的梯度(用于 NLP 求解),且内存占用不随地形复杂度增加而增大。Grandia 2023 Perceptive MPC 中的高程查询也可以用 Neural SDF 替代双线性插值,获得更平滑的代价函数梯度。

当前局限:Neural SDF 的训练/微调需要时间(约 0.1-1 秒),无法像 Elevation Map 那样逐帧增量更新。研究前沿正在探索**在线微调**(用少量新观测快速更新已训练的 SDF)和**条件 SDF**(用输入特征条件化 SDF,实现快速自适应)。

本质洞察: 地形感知技术的演进反映了机器人学中一个更深层的趋势:从离散、显式、有限的世界表示走向连续、隐式、无限的世界表示。高程图(2003)是"把世界装进格子里",Neural SDF(2024+)是"用函数逼近世界"。前者简单高效但有分辨率和维度限制,后者灵活强大但有计算和训练成本。这和 SLAM 从体素(OctoMap)到神经隐式(NeRF/Gaussian Splatting)的演进完全平行。在腿足领域,高程图在未来 3-5 年仍将是工程主流(因为其 O(1) 查询和增量更新的不可替代优势),但 Neural SDF 将在需要高精度碰撞检测和可微优化的场景中逐步渗透。

本质洞察: 感知驱动落脚规划的本质矛盾是**信息丰富度与决策延迟的权衡**。传感器越多、处理越复杂,获得的地形信息越丰富,但从传感器输入到控制输出的端到端延迟也越大。在 1 m/s 行走速度下,50 ms 延迟意味着 5 cm 的"盲区"——这恰好是一个台阶边缘的宽度。所有感知行走系统的工程核心都是在这个矛盾中找到最优平衡点:用尽可能少的计算获取"刚好够用"的地形信息。Miki 2022 的 Teacher-Student 框架之所以成功,正是因为它把大量的"感知计算"转移到了离线训练阶段(Teacher),而在线推理(Student)只需要轻量级的本体感知编码——从根本上打破了信息丰富度与决策延迟的权衡。

🔧 故障排查手册¶

症状	可能原因	排查步骤	相关章节
高程图鬼影	运动模糊/里程计漂移	融合IMU补偿/增大Kalman过程噪声；检查里程计精度	足式/130
深度相机盲区	近距离无数据(>0.3m最小距离)	融合足端触觉/切换几何先验；考虑多传感器互补	足式/180
SDF计算延迟	GPU内存不足/分辨率过高	降分辨率/用octree加速；profiling GPU占用率	足式/160 §60.7
地形分类错误	训练集与实际地形分布不匹配	在线微调/增加domain randomization；收集目标场景数据	足式/160 §60.8
elevation map与body frame不同步	TF延迟	用时间戳插值/预测补偿；检查TF树的发布频率	足式/160 §60.3

60.11 本章小结¶

知识点总结¶

节号	主题	核心要点	难度
60.1	盲走 vs 感知	盲走在不平地形系统性失效；感知弥补地形信息缺失	⭐
60.2	地形表示方法	五种表示对比；高程图是腿足最优选择（O(1)查询、可微、可融合）	⭐⭐
60.3	Elevation Mapping	Fankhauser 框架：点云投影 → Kalman 融合 → 漂移补偿 → 光线清理	⭐⭐
60.4	GPU 加速建图	elevation_mapping_cupy：CuPy 实现、10-20x 加速、释放 CPU	⭐⭐⭐
60.5	地形特征与可通行性	坡度/粗糙度/台阶/间隙 → 可通行性评分 T in [0,1]	⭐⭐
60.6	落脚点评分	多准则框架：地形T + 运动学K + 稳定性S 加权融合	⭐⭐
60.7	TAMOLS	地形感知轨迹优化、SDF 碰撞回避、graduated optimization、< 10ms	⭐⭐⭐
60.8	学习型落脚	Miki 2022 Belief Encoder、Teacher-Student、ANYmal Parkour	⭐⭐⭐
60.9	传感器选型	深度相机 vs LiDAR、外参标定、时间同步	⭐⭐
60.10	Foundation Model + Neural SDF	VLM 语义增强可通行性、Neural SDF 隐式地形表示	⭐⭐⭐⭐

方法论谱系全景¶

                        感知驱动落脚规划方法谱系
                              │
            ┌─────────────────┼──────────────────┐
            │                 │                  │
      手工规则方法         优化方法           学习方法
            │                 │                  │
    ┌───────┤          ┌──────┤           ┌──────┤
    │       │          │      │           │      │
 几何特征  评分函数   TAMOLS  Perceptive   CNN   端到端
 阈值判断  多准则      (NLP)   MPC       评分    RL
 (60.5)   融合(60.6)  (60.7) (足式/230_Perceptive_MPC)    (Gang.) (60.8)
    │       │          │      │           │      │
    简单     ← 复杂度/泛化能力增大 →       │  Miki/Hoeller
    快速     ← 计算量增大 →              │      │
    脆弱     ← 鲁棒性增大 →             │      │
    可解释   ← 可解释性降低 →           │      │

本章首尾呼应¶

回到 60.1 开头的问题：一只"闭着眼"的四足在碎石坡上会摔倒。现在我们有了完整的解决方案：

传感器采集（60.9）：深度相机 + LiDAR → 原始点云
地形建图（60.3-60.4）：Elevation Mapping + GPU 加速 → 实时高程图
特征分析（60.5）：坡度/粗糙度/台阶/间隙 → 可通行性地图
落脚选择（60.6-60.8）：评分/优化/学习 → 最优落脚点
执行控制（足式/110_OCS2完整栈与双线程MPC-58）：MPC + WBC → 关节扭矩 → 机器人安全行走

累积项目：本章新增模块¶

项目：从零构建四足感知运动控制器

章节	新增模块	功能
足式/30_Pinocchio深度精读-49	运动学	URDF 加载、正/逆运动学
足式/60_QP_NLP建模-51	简化模型	LIPM、Capture Point
足式/110_OCS2完整栈与双线程MPC-56	MPC + 步态	OCS2 轨迹优化、步态管理器
足式/130_腿足状态估计	状态估计	EKF 融合 IMU + 关节
足式/140_落脚点规划经典方法	落脚规划	Raibert 启发式
足式/160_感知驱动落脚规划（本章）	感知建图	Elevation Map + 可通行性 + 落脚评分

本章新增代码模块： 1. ElevationMapNode：订阅点云和 TF，维护高程图，发布 grid_map 消息 2. TraversabilityAnalyzer：计算坡度、粗糙度、可通行性评分 3. PerceptiveFootholdSelector：在 Raibert 预测点附近搜索最优落脚点，替代原来的盲走落脚选择

集成方式：PerceptiveFootholdSelector 的输出（落脚点位置）替换足式/140_落脚点规划经典方法的 Raibert 输出，输入足式/110_OCS2完整栈与双线程MPC 的 OCS2 MPC 作为参考落脚点。

[跨章综合题] 练习 60.X：感知 + 状态估计 + 落脚规划的闭环¶

本题需要综合足式/130_腿足状态估计(状态估计)、足式/140_落脚点规划经典方法(落脚规划)和本章(感知)的知识。

场景: ANYmal 四足机器人在一个废弃工厂中巡检。地面混合了平整水泥地、金属格栅、积水区和小碎石堆。

(本章 60.3/60.4) 设计 Elevation Mapping 的配置方案。选择传感器(深度相机 vs LiDAR),解释选型理由。设置地图分辨率、大小、融合参数。如果金属格栅的 LiDAR 反射率极低(点云稀疏),如何处理?
(足式/130_腿足状态估计) Elevation Map 依赖里程计来对齐不同帧的点云。如果机器人在积水区域脚底打滑,状态估计的位置会如何漂移?这个漂移对 Elevation Map 有什么影响?(提示:回顾 60.3 中的"鬼影"问题)
(本章 60.5/60.6) 设计可通行性评分函数。积水区域在几何特征上(坡度、粗糙度)可能与正常平地无异——纯几何方法能检测出积水吗?如果引入 VLM 语义(60.10 节),如何将"积水"标签转化为可通行性的惩罚?
(足式/140_落脚点规划经典方法) 在可通行性图基础上,Raibert 公式计算的落脚点如果落在不可通行区域(积水/碎石),评分函数选点(58.7 节)的候选集如何生成?搜索半径过大会导致什么问题(运动学不可达)?过小会导致什么问题(所有候选都不安全)?

延伸阅读¶

必读（⭐⭐）¶

Fankhauser P., Bloesch M., Hutter M. (2018) "Probabilistic Terrain Mapping for Mobile Robots with Uncertain Localization" — IEEE RA-L. Elevation Mapping 的理论完善版，详细推导了漂移补偿的数学框架。
Miki T., Lee J., Hwangbo J., Wellhausen L., Koltun V., Hutter M. (2022) "Learning robust perceptive locomotion for quadrupedal robots in the wild" — Science Robotics. 学习型感知运动控制的分水岭论文，在多种户外非结构化地形上完成了实机部署验证。

核心论文（⭐⭐⭐）¶

Jenelten F., Grandia R., Farshidian F., Hutter M. (2022) "TAMOLS: Terrain-Aware Motion Optimization for Legged Systems" — T-RO. 地形感知运动优化框架，graduated optimization + SDF 碰撞。
Grandia R., Jenelten F., Yang S., Farshidian F., Hutter M. (2023) "Perceptive locomotion through nonlinear model predictive control" — T-RO. 高程图嵌入 OCS2 NMPC，足式/230_Perceptive_MPC 精读对象。
Hoeller D., Rudin N., Sako D., Hutter M. (2024) "ANYmal parkour: Learning agile navigation for quadrupedal robots" — Science Robotics. 端到端 RL 跑酷的工业级实现。
Jenelten F., He J., Farshidian F., Hutter M. (2024) "DTC: Deep Tracking Control" — Science Robotics. RL + MPC 混合架构的代表。

进阶阅读（⭐⭐⭐⭐）¶

Fankhauser P., Bloesch M., Gehring C., Hoepflinger M., Hutter M. (2014) "Robot-centric elevation mapping with uncertainty estimates" — CLAWAR. grid_map 的起源论文。
Wermelinger M., Fankhauser P., Diethelm R., Krusi P., Siegwart R., Hutter M. (2016) "Navigation planning for legged robots in challenging terrain" — IROS. 手工可通行性分析的经典。
Gangapurwala S., Geisert M., Orsolino R., Fallon M., Havoutis I. (2022) "RLOC: Terrain-Aware Legged Locomotion Using Reinforcement Learning and Optimal Control" — T-RO. RL + OC 混合方案。
Zhuang Z., Fu Z., Wang J. et al. (2023) "Robot Parkour Learning" — CoRL. 端到端跑酷的 RL 训练框架。

开源项目¶

grid_map — github.com/ANYbotics/grid_map — ROS2 grid map 库，支持 Humble / Iron / Jazzy / Rolling（⭐⭐）
elevation_mapping_cupy — github.com/leggedrobotics/elevation_mapping_cupy — GPU 加速 Elevation Mapping，ROS2 Jazzy 支持，800+ stars（⭐⭐⭐）
rsl_rl — github.com/leggedrobotics/rsl_rl — ETH 的 RL 训练框架，用于 Miki 2022 和 Hoeller 2024（⭐⭐⭐⭐）

本章常见误解汇总¶

误解	正确理解
"感知只是加个相机"	感知驱动落脚涉及传感器选型、数据融合、特征计算、评分框架、优化或学习的完整管线
"Elevation Map 就够了，不需要 traversability"	MPC 不直接消费原始高程值——需要把几何信息转化为"能不能走"的语义判断
"RL 端到端比传统方法好"	在常规地形上传统方法更稳定可靠，RL 的优势在极端地形和未知场景
"传感器越多越好"	多传感器增加标定复杂度和数据同步难度；关键是互补而非冗余
"可通行性评分越高越该踩"	可通行性只是必要条件，落脚还需考虑运动学可达性和动态稳定性
"标定做一次就永久有效"	振动和碰撞会导致外参漂移，需定期校验
"TAMOLS 替代了 MPC"	TAMOLS 是增强的 MPC，为标准 MPC 提供更好的参考
"延迟不重要"	在台阶边缘，40 ms 的延迟可能导致 15 cm 的高度查询误差

符号表¶

符号	含义	首次出现
$T(\boldsymbol{p})$	地形可通行性评分 $\in [0,1]$	60.5
$K(\boldsymbol{p})$	运动学可达性评分 $\in [0,1]$	60.6
$S(\boldsymbol{p})$	稳定裕度评分 $\in [0,1]$	60.6
$J(\boldsymbol{p})$	落脚点综合评分	60.6
$\boldsymbol{p}_f$	最终选定的落脚点位置	60.6
$\boldsymbol{p}_{\text{hip}}$	髋关节在世界系中的位置	60.6
$\boldsymbol{\xi}$	DCM / Capture Point 位置	60.6
$\theta_{\max}$	最大可通行坡度	60.5
$Tr(c_T, \pi)$	RL 定义的可通行性（策略相关）	60.5
$\Delta t_{\text{delay}}$	端到端感知延迟	60.11
$\boldsymbol{T}_{bs}$	传感器到基座的外参变换	60.9

定理与公式速查表¶

公式	一句话说明	对应节
$T = f_{\text{slope}} \cdot f_{\text{rough}} \cdot f_{\text{step}} \cdot f_{\text{gap}}$	乘性可通行性评分	60.5
$J(\boldsymbol{p}) = w_T T + w_K K + w_S S$	多准则落脚评分	60.6
$\boldsymbol{p}^* = \arg\max J(\boldsymbol{p})$	最优落脚点选择	60.6
$\boldsymbol{p}_{\text{query}} = \boldsymbol{p} + \boldsymbol{v} \cdot \Delta t$	延迟补偿前向预测	60.11
$K = P/(P+R)$	Kalman 增益（高程图融合）	60.3

预计阅读时间¶

阅读方式	时间	适合谁
精读（含练习和论文阅读）	25-30 小时	需要深入理解感知-规划闭环的研究者
速读（跳过 TAMOLS 推导和 RL 细节）	10-12 小时	工程师了解感知落脚的整体框架
速查（只看对比表和公式速查）	1-2 小时	遇到落脚相关问题时回来查

版本信息速查¶

工具/库	版本	状态	说明
grid_map	ROS2 Humble/Iron/Jazzy	活跃	高程图数据结构标准库
elevation_mapping_cupy	最新	活跃	GPU 加速高程图
IsaacLab	3.0 (Newton)	活跃	RL 训练平台
rsl_rl	ETH RSL	活跃	RL 训练库
OCS2	v1.0	维护模式	MPC 框架，TAMOLS 基础
IPOPT	3.14+	稳定	NLP 求解器

研究实践建议¶

给新手的建议¶

先掌握盲走控制器：在学习感知落脚之前，确保你已经理解了足式/140_落脚点规划经典方法的 Raibert 启发式和足式/110_OCS2完整栈与双线程MPC 的 MPC 控制。感知落脚是在这些基础上的增量改进
在仿真中对比开/关感知：最好的学习方式是在 Gazebo 中分别跑"盲走 Raibert"和"感知驱动落脚"，直观感受差异
从简单地形开始：先在单个台阶上验证感知落脚，再逐步增加地形复杂度

给有经验者的建议¶

关注 PUMA 和注意力地图编码等最新方法：它们代表了从"显式建图+评分"到"隐式端到端"的范式转变
延迟补偿是工程关键：学术论文很少讨论延迟（因为仿真没有延迟），但真机上延迟是落脚质量的第一瓶颈
混合方法是工程最优解：结合传统评分（简单地形）和 RL（复杂地形）的自适应切换，比任一方法单独使用都更鲁棒
多传感器状态估计与感知的耦合：最新的多 IMU 融合研究（Multi-IMU Sensor Fusion for Legged Robots, ArXiv 2507.11447, 2025）和 UWB-IMU-Leg 融合方法（2025）表明，状态估计的精度直接影响高程图的质量。如果你的里程计漂移大，先改善状态估计比优化感知算法更有效
DogLegs 系统：DogLegs（ArXiv 2503.04580, 2025）通过在腿部安装额外的 IMU，利用腿部运动学约束来计算身体 IMU 和腿部 IMU 之间的相对位置约束，从而减少误差漂移。这种方法在不同地形上表现出比传统腿部里程计更好的状态估计精度

如果跳过本章会怎样¶

你的 MPC 控制器只能在平地上工作——因为它假设地面高度为 0，遇到台阶、斜坡就会失效
你无法理解足式/230_Perceptive_MPC——Perceptive MPC 的地形约束是本章的直接应用
你对 Miki 2022 和 Hoeller 2024 等里程碑论文只能"看热闹"——不理解感知驱动落脚的框架，就无法理解这些论文的贡献和局限
你在真机上部署感知时会踩所有已知的坑——传感器选型、延迟补偿、标定等工程知识只有在本章中系统讲解

与其他章节衔接¶

向前承接： - 足式/70_腿足简化模型理论 LIPM + Capture Point → 60.6 稳定裕度评分的基础 - 足式/130_腿足状态估计状态估计 → 60.3 Elevation Mapping 依赖里程计 - 足式/140_落脚点规划经典方法 Raibert 启发式 → 60.6 作为候选集合的中心点 - 足式/150_优化驱动落脚与接触规划 CITO / GCS → 60.7 TAMOLS 的优化框架思想

向后指向： - 60.3-60.4 Elevation Mapping → 足式/220_腿足感知数据结构深入精读 grid_map 和 elevation_mapping_cupy 的代码 - 60.7 TAMOLS → 足式/230_Perceptive_MPC Perceptive MPC 精读（Grandia 2023 在 OCS2 中的实现） - 60.8 学习型 → 足式/210_RL与MPC混合范式 RL + MPC 混合架构（DTC 精读） - 60.9 传感器 → 足式/180_腿足硬件栈实时系统中的传感器驱动与数据流 - 60.11 延迟分析 → 足式/170_实时CPP工程实时工程的延迟约束 - 60.12 计算预算 → 足式/220_腿足感知数据结构 GPU 高程图加速的工程动机

知识导航回顾¶

回到本章开头提出的核心问题：一只"闭着眼"的四足在碎石坡上为什么会摔倒？现在我们有了系统性的答案：

问题分解:
  1. 盲走为什么失败？ → 60.1（平地假设被违反）
  2. 地形如何表示？   → 60.2（五种表示方法对比）
  3. 高程图怎么建？   → 60.3-60.4（Kalman 融合 + GPU 加速）
  4. 地形能不能走？   → 60.5（traversability 评分）
  5. 脚该踩哪里？     → 60.6-60.8（评分/优化/学习三种方法）
  6. 用什么看？       → 60.9（传感器选型与标定）
  7. 看了之后怎么用？ → 60.10（VLM + Neural SDF 前沿）
  8. 延迟怎么处理？   → 60.11（补偿策略）
  9. 算得过来吗？     → 60.12（计算预算分配）

每个子问题对应本章的一个节，形成了从"为什么需要感知"到"如何在实时约束下部署感知"的完整知识链。这条链上的每个环节都不可省略——省略任何一个，系统都可能在对应的场景下失效。

这也是为什么感知驱动落脚是腿足控制中最**综合**的主题之一——它同时涉及信号处理（传感器）、概率推理（Kalman 融合）、几何计算（坡度/粗糙度）、优化（TAMOLS）、学习（RL/CNN）和实时工程（延迟补偿）。掌握了这个主题，你就具备了将理论、算法和工程三者融合的能力。

本章与前沿研究的连接¶

本章的内容与以下 2024-2025 年最新研究方向直接相关：

研究方向	与本章的关系	代表工作
端到端感知运动控制	60.8 的自然延伸——取消显式高程图，直接从像素到动作	Hoeller 2024, Robot Parkour Learning
语义地形理解	60.10 的 VLM 增强——理解"什么是水""什么是草"	NaVILA (RSS 2025), ConceptGraphs
注意力地图编码	60.8 的改进——用注意力机制自适应编码地图	Attention-Based Map Encoding (2025)
统一落脚先验	60.6 的端到端替代——直接从感知预测极坐标落脚先验	PUMA (2025)
多传感器状态估计	60.3 的基础改进——更准确的里程计意味着更准确的高程图	Multi-IMU Fusion (2025), DogLegs
Contact-Implicit MPC	60.7 TAMOLS 的进化——不预定义接触模式	Le Cleac'h 2024, Aligator
RL-Augmented MPC	60.7-60.8 的统一——RL 提供步态指令给 MPC	RL-Augmented MPC (2025)
全向碰撞回避	60.5 的扩展——360 度感知 + 动态障碍物回避	Omni-Perception (2025)
地形感知 Gap 穿越	60.5 间隙检测的深入——用高程图指导间隙穿越	MARG (2025)

这些方向都是博士研究的优秀切入点。如果你对感知驱动运动控制感兴趣，建议从复现 Miki 2022 开始，然后选择上述一个方向深入。

复现 Miki 2022 的实用路线图：

第 1 周：在 IsaacLab 中搭建四足训练环境，用 rsl_rl 训练一个基础 trot 策略（不带感知）
第 2 周：添加地形 curriculum——随机地形生成 + 难度自适应（用 Miki 的可通行性定义控制难度）
第 3 周：添加高程图观测——在训练环境中采样 52 个高程点作为 exteroceptive 观测
第 4 周：实现 Teacher-Student 蒸馏——Teacher 用完美高程图，Student 用带噪声的采样
第 5-6 周：在仿真中验证 Student 策略在多种地形上的表现，对比纯本体感知策略

整个复现大约需要 6 周。复现过程中你会深刻理解 curriculum learning、Teacher-Student 框架和 Belief Encoder 的工程细节——这些理解是纯读论文无法获得的。

复现中最可能遇到的问题：

阶段	常见问题	解决方案
第 1 周	IsaacLab 安装失败	使用官方 Docker 镜像
第 2 周	地形 curriculum 不工作——策略总在简单地形	检查可通行性阈值是否设置正确
第 3 周	高程图观测全是零	确认 terrain mesh 的高度信息正确传入环境
第 4 周	Student 性能远差于 Teacher	增大 Student 训练步数；检查噪声注入是否过大
第 5-6 周	在特定地形上持续失败	分析失败地形的 traversability 分布，调整 curriculum

60.11 感知延迟对落脚安全的定量分析 ⭐⭐⭐¶

本节解决什么问题：感知管线从传感器采集到高程图可用有固有延迟。这个延迟如何量化影响落脚点的安全性？如何在规划中补偿？

动机：感知不是瞬时的¶

前面的章节把感知管线当作"输入高程图、输出落脚点"的黑盒。但在真机上，从深度相机采集一帧图像到高程图更新完成，有一条完整的延迟链：

环节	典型延迟	可变性
传感器采集 + 曝光	10-30 ms	与光照相关
USB/以太网传输	5-10 ms	稳定
点云转换（ROS2 callback）	2-5 ms	与点云大小相关
TF 查询 + 坐标变换	1-3 ms	稳定
Kalman 融合（GPU）	2-5 ms	与地图大小相关
Traversability 计算	1-3 ms	稳定
MPC 读取高程图	1-2 ms	稳定
端到端总延迟	22-58 ms

以 1 m/s 的行走速度计算，40 ms 的延迟意味着 MPC 看到的高程图比机器人当前实际位置**滞后 4 cm**。对于 4 cm 分辨率的高程图，这意味着落脚查询可能查到错误的格子。

延迟补偿策略¶

策略 1：前向预测

用当前速度和角速度预测延迟结束时机器人的位置，查询预测位置处的高程：

\[\boldsymbol{p}_{\text{query}} = \boldsymbol{p}_{\text{current}} + \boldsymbol{v} \cdot \Delta t_{\text{delay}}\]

\[\theta_{\text{query}} = \theta_{\text{current}} + \omega \cdot \Delta t_{\text{delay}}\]

这是最简单的补偿方法，但假设了匀速运动。当机器人加速或减速时，预测不准。

策略 2：时间戳对齐

不预测未来，而是**回溯过去**——高程图的每个更新都带有时间戳，MPC 查询时用里程计将当前位置回溯到高程图更新的时间点，在该历史位置上查询。

这种方法更精确（因为里程计的短时间内通常是准确的），但要求保存里程计的历史记录。

策略 3：自适应延迟估计

不假设固定延迟，而是在运行时动态估计端到端延迟——通过在已知位置（如平地）上比较"高程图显示的值"和"状态估计器报告的值"，计算实际延迟。

\[\Delta t_{\text{est}} = \arg\min_{\Delta t} \|z_{\text{map}}(\boldsymbol{p}(t - \Delta t)) - z_{\text{estimator}}(t)\|^2\]

这是最精确的方法，但计算复杂度较高且需要已知地面作为参考。

延迟对不同地形的影响¶

延迟的影响与地形的空间梯度成正比——平地上 4 cm 的查询偏移无关紧要（高度不变），但在台阶边缘 4 cm 可能是"踩在台阶上"和"踩在台阶下"的区别：

地形类型	空间梯度	4 cm 偏移的高度误差	安全影响
平地	0 m/m	0 cm	无
缓坡（10 度）	0.18 m/m	0.7 cm	低
陡坡（30 度）	0.58 m/m	2.3 cm	中
台阶边缘	约 5 m/m	20 cm（跨越整个台阶）	极高

反事实推演：如果不做延迟补偿，在台阶边缘会发生什么？MPC 查询的是 4 cm 前的高程——在台阶边缘，这 4 cm 是台阶面和台阶顶的分界。MPC 以为脚会踩在台阶顶（高 15 cm），实际踩在台阶面（高 0 cm）→ 15 cm 的高度误差 → 摆动腿提前着地 → 冲击力传入关节 → 步态紊乱。或者相反，MPC 以为脚踩在台阶面，实际踩在台阶顶 → 脚悬空 15 cm → 失去支撑 → 跌倒。

PUMA 框架：感知统一落脚先验（2025 前沿）¶

PUMA（Perception-driven Unified foothold prior for Mobility Augmented quadruped parkour）是 2025 年的最新工作（ArXiv 2601.15995），提出了一种端到端学习框架，将视觉感知和落脚先验统一到单阶段训练中。

核心思想：传统方法分两步——先建高程图再选落脚点。PUMA 直接从地形特征估计以自我为中心的极坐标落脚先验（相对距离和航向角），跳过显式的地形重建。

特性	传统管线	PUMA
地形表示	显式高程图	隐式特征
落脚选择	评分/优化	端到端预测
训练阶段	多阶段（建图→评分→策略）	单阶段
实时性	受限于建图延迟	直接推理

与本章内容的关系：PUMA 代表了一种"跳过中间表示"的趋势——不再显式构建高程图和 traversability，而是直接从感知到动作。这与 60.8 节的端到端 RL 方法（Miki 2022、Hoeller 2024）在精神上一致，但 PUMA 更关注落脚先验的结构化表示而非纯黑箱。

注意力机制驱动的地图编码（2025 前沿）¶

传统方法将高程图作为固定大小的网格输入策略网络。最新研究（如 Attention-Based Map Encoding, ArXiv 2506.09588, 2025）提出用注意力机制自适应地编码地图信息：

核心思想：地图中不同区域对当前决策的重要性不同——前方 1 步之内的地形比 3 米外的地形更重要。注意力机制让策略网络自动"关注"最相关的地图区域
优势：(1) 自适应分辨率——重要区域获得更多注意力，等效于更高分辨率的感知；(2) 对地图大小的鲁棒性——注意力机制可以处理不同大小的地图输入
与 60.8 Belief Encoder 的区别：Belief Encoder 用 RNN 处理时间序列（历史观测），注意力机制处理空间结构（地图的不同区域）。两者互补——可以同时使用

⚠️ 常见陷阱¶

⚠️ 编程陷阱：延迟补偿时用 MPC 预测速度而非当前速度 错误做法：用 MPC 输出的参考速度做前向预测现象：当机器人被推（外部扰动）时，参考速度和实际速度不同，延迟补偿方向错误 根本原因：MPC 的参考速度是"期望"速度，不是"实际"速度。延迟补偿应用状态估计器输出的实际速度 正确做法：从状态估计器读取实际速度 $\boldsymbol{v}_{\text{est}}$，用于前向预测

💡 概念误区：认为降低感知频率可以增加感知质量 新手想法："30 Hz 的高程图太快了，不如降到 10 Hz 让每帧质量更好" 实际上：降低频率**增大**了延迟（从 33 ms 变成 100 ms），在 1 m/s 行走速度下查询偏移从 3.3 cm 增大到 10 cm。降低频率确实可以让每帧的计算更充分（比如做更多 Kalman 融合迭代），但延迟增大带来的安全风险通常更严重 正确权衡：保持高频率（>= 20 Hz），通过 GPU 加速来提升每帧质量，而不是牺牲频率

🧠 思维陷阱：认为所有场景都需要最高精度的感知 新手想法："必须达到 1 cm 精度的高程图才能做感知落脚" 实际上：不同场景对精度的要求差异巨大。平地行走 5 cm 精度足够，台阶攀爬需要 1-2 cm，跑酷可能需要亚厘米级。应该根据**当前任务的安全裕度**动态调整精度要求——Miki 2022 的 Belief Encoder 正是在做这件事：在传感器不精确时依赖本体感知，在传感器精确时依赖高程图

练习¶

[定量计算] 假设端到端感知延迟为 50 ms，机器人行走速度 0.8 m/s，高程图分辨率 4 cm。计算：(a) 落脚查询的空间偏移；(b) 在 20 度斜坡上，这个偏移导致的高度误差；(c) 如果台阶高度 10 cm、宽度 3 cm，在台阶边缘的最大落脚点位误差是多少？
[系统设计] 设计一个端到端延迟最小化的感知管线。列出每个环节的延迟瓶颈和对应的优化措施。目标：将端到端延迟从 50 ms 降到 20 ms 以内。
[文献调研] 阅读 PUMA（ArXiv 2601.15995）的核心方法部分，回答：(a) 极坐标落脚先验是如何定义的？(b) 与传统高程图+评分函数方法相比，PUMA 在计算延迟上有多大优势？(c) PUMA 牺牲了什么（提示：可解释性和安全保证）？

60.12 实时落脚规划的计算预算分析 ⭐⭐¶

本节解决什么问题：感知驱动的落脚规划涉及多个计算模块（建图、特征计算、评分、搜索），总计算时间必须在一个步态周期内完成。如何分配计算预算？

动机：时间是最稀缺的资源¶

四足机器人以 trot 步态行走时，步态周期约 0.3-0.5 秒。落脚规划必须在**摆动相开始前**完成——因为摆动轨迹的终点就是落脚点。如果规划延迟超过摆动相持续时间，就来不及修正摆动轨迹，机器人只能用上一步的落脚计划。

步态	步态周期	摆动相持续时间	可用规划时间
Walk	0.6 s	0.3 s	0.2-0.25 s
Trot	0.4 s	0.2 s	0.15-0.18 s
Bound	0.25 s	0.12 s	0.08-0.10 s

计算预算分配表¶

以 trot 步态（可用 150 ms）为例，各模块的典型计算时间：

模块	CPU 时间	GPU 时间	预算占比
点云预处理（降采样+滤波）	5-10 ms	1-2 ms	5-7%
KF 融合 + 光线一致性	50-80 ms	3-5 ms	3-5% (GPU)
Traversability 计算	10-15 ms	1-2 ms	1-2% (GPU)
候选集生成	< 0.1 ms	-	< 0.1%
多准则评分（121 个候选）	< 0.5 ms	-	< 0.5%
TAMOLS 优化（若使用）	5-10 ms	-	5-7%
总计（GPU 管线）	-	~10 ms	~7%

关键洞察：GPU 管线把感知计算从"吃掉大部分预算"变成了"只占预算的一小部分"，为 MPC 和 TAMOLS 优化留出了充裕的计算时间。这就是为什么 elevation_mapping_cupy 的 GPU 加速不仅是"性能优化"，更是**使感知驱动控制成为可能**的关键使能技术。

异步管线设计¶

为了最大化计算效率，感知和控制应在不同线程中异步运行：

感知线程（20-30 Hz, 非实时优先级）        控制线程（1 kHz, RT 优先级）
    │                                        │
    │  点云采集                                │  读取最新高程图（无锁）
    │  KF 融合                                │  MPC 求解（50-100 Hz 子循环）
    │  Traversability                          │  WBC 求解
    │  写入共享高程图（double buffer）           │  关节扭矩输出
    │                                        │
    ▼ 重复                                    ▼ 重复

double buffer（双缓冲）是关键的无锁通信机制：感知线程写入"后台缓冲区"，写完后用原子指针交换使其成为"前台缓冲区"。控制线程始终读取"前台缓冲区"。这保证了控制线程永远不会读到半写入的数据，且不需要锁。

跨领域类比：感知-控制的双缓冲机制与 GPU 渲染中的前后缓冲交换（swap chain）完全类似。GPU 在"后缓冲区"渲染下一帧，渲染完成后与"前缓冲区"交换，显示器始终读取"前缓冲区"。两者解决的核心问题相同：生产者和消费者以不同频率运行，如何保证消费者始终获取完整且一致的数据。

计算预算的动态调整¶

在不同地形上，计算预算的分配应该不同：

地形复杂度	建图精度需求	TAMOLS 需求	预算策略
平地	低	不需要	降低感知频率（10 Hz），节省 GPU 给 RL
缓坡	中	可选	标准配置（20 Hz 感知）
台阶/碎石	高	需要	最大感知频率（30 Hz），TAMOLS < 10 ms
跑酷	极高	不需要（RL 替代）	GPU 全部给 RL 推理

这种动态调整需要一个**任务管理器**来监控地形复杂度并切换计算模式——本质上是一个基于 traversability 方差的简单状态机。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：在控制线程中调用 grid_map 的查询函数 错误做法：在 1 kHz 的 update() 中直接调用 map.at("elevation", position) 现象：如果感知线程正在写入高程图，控制线程的读取可能读到不一致的数据（部分新、部分旧） 根本原因：grid_map 不是线程安全的——读写同时发生时行为未定义 正确做法：使用 double buffer 或 realtime_tools::RealtimeBuffer 封装高程图的读写

💡 概念误区：认为感知管线必须和控制同频 新手想法："MPC 100 Hz，高程图也得 100 Hz 更新" 实际上：地形不会以 100 Hz 变化。20 Hz 的高程图更新对于 1 m/s 的行走速度已经足够（每 5 cm 更新一次，小于高程图分辨率 4 cm）。感知管线以 20-30 Hz 运行，MPC 以 100 Hz 运行但查询同一份高程图——这正是 Perceptive MPC（足式/230_Perceptive_MPC）的异步架构

练习¶

[计算题] 机器人以 trot 步态（周期 0.4s，摆动相 0.2s）在碎石坡上行走。感知管线端到端延迟 40 ms。计算：(a) 摆动相开始后，感知管线最多有几次更新机会？(b) 如果第一次更新在 40 ms 后才到达，摆动轨迹规划器使用的是哪个时刻的高程图？(c) 这个延迟在什么行走速度下开始影响安全性？
[架构设计] 画出 Go2 的完整感知-控制异步架构图，包含：传感器驱动线程、感知处理线程、MPC 线程、WBC 线程。标注每个线程的频率、优先级和数据通信方式（共享内存/ROS topic/double buffer）。

🔧 故障排查手册（补充）¶

症状	可能原因	排查步骤	相关小节
落脚点在台阶边缘反复切换	高程图在台阶边缘处有 1-2 格子的不确定带，评分函数在这些格子上的值振荡	1. 检查台阶边缘的 traversability 值分布 2. 在评分函数中加入时间一致性约束（连续两步的落脚变化不超过 5cm） 3. 增大 traversability 的 sigmoid 锐度 $k_s$	60.5, 60.6
感知驱动落脚比盲走更差——平地上出现不必要的步态扰动	高程图噪声导致平坦地面的 traversability < 1.0，评分函数偏移了 Raibert 最优点	1. 在已知平面上检查 traversability 是否接近 1.0 2. 增大 $\sigma_{\max}$ 和 $\theta_{\max}$ 的容忍度 3. 当 traversability 方差足够小且值接近 1 时自动切回盲走模式	60.5, 60.11
TAMOLS 在大坡度地形不收敛	SDF 碰撞约束导致可行域过小，IPOPT 找不到满足所有约束的解	1. 检查 SDF 安全裕度是否设置过大 2. 放松 SDF 约束（从硬约束改为大权重惩罚项） 3. 用 graduated optimization 从大平滑开始	60.7
端到端 RL 策略在未见过的地形上失效	训练分布不覆盖测试地形（如训练只有碎石，测试遇到台阶）	1. 检查训练地形的覆盖范围 2. 增加 domain randomization 的地形种类 3. 加入 curriculum learning 逐步引入新地形	60.8
感知延迟导致落脚点位偏移	端到端延迟超过 40 ms，在 1 m/s 速度下偏移超过 1 个格子	1. 测量端到端延迟（传感器时间戳 vs MPC 读取时间） 2. 实现前向预测补偿 3. 降低行走速度	60.11

地形类型	盲走失效原因	后果	感知能解决什么
斜坡	假设 \(z = 0\)，不知道地面倾斜	脚提前/延迟着地，冲击或悬空	预知地面高度，调整摆动轨迹
台阶	不知道前方有高差	脚撞台阶面或踩空	检测台阶边缘，选择踩上去或绕过
碎石	不知道哪里平整、哪里是尖石	踩在不稳定表面上打滑	评估表面粗糙度，选择平整区域
沟渠	不知道前方有凹陷	脚掉进沟里，关节过伸	检测凹陷，增大步幅跨过
冰/水面	不知道表面摩擦系数低	滑动导致失稳	结合 RGB 识别材质，降低速度
狭窄踏板	不知道只有某些区域可踩	落脚在悬空区域	检测可踩区域，精确落脚

传感器	典型 R（近距 1m）	典型 R（远距 4m）
RealSense D435	\((2 \text{ mm})^2\)	\((20 \text{ mm})^2\)
Velodyne VLP-16	\((3 \text{ mm})^2\)	\((5 \text{ mm})^2\)
Ouster OS1-64	\((2 \text{ mm})^2\)	\((4 \text{ mm})^2\)

分量	衡量什么	如果不考虑会怎样
\(f_{\text{slope}}\)	倾斜程度	踩在陡坡上滑倒
\(f_{\text{rough}}\)	表面不规则性	踩在碎石上摇晃
\(f_{\text{step}}\)	局部高差	踩在台阶边缘踩空
\(f_{\text{gap}}\)	间隙	脚陷入缝隙

特性	高程图 (2.5D)	Neural SDF
内存	\(O(W^2/r^2)\),与分辨率平方成正比	$O(
分辨率	固定(2-5 cm)	连续(理论无限分辨率)
悬垂结构	不支持	支持
梯度(法向量)	有限差分近似	解析梯度 \(\nabla f_\theta\)
MPC 兼容性	双线性插值可微	天然可微(神经网络输出)
更新效率	O(1) 格子更新	需重训练或微调
查询速度	O(1) 栅格索引	~0.1 ms(GPU 推理)

符号	含义	首次出现
\(T(\boldsymbol{p})\)	地形可通行性评分 \(\in [0,1]\)	60.5
\(K(\boldsymbol{p})\)	运动学可达性评分 \(\in [0,1]\)	60.6
\(S(\boldsymbol{p})\)	稳定裕度评分 \(\in [0,1]\)	60.6
\(J(\boldsymbol{p})\)	落脚点综合评分	60.6
\(\boldsymbol{p}_f\)	最终选定的落脚点位置	60.6
\(\boldsymbol{p}_{\text{hip}}\)	髋关节在世界系中的位置	60.6
\(\boldsymbol{\xi}\)	DCM / Capture Point 位置	60.6
\(\theta_{\max}\)	最大可通行坡度	60.5
\(Tr(c_T, \pi)\)	RL 定义的可通行性（策略相关）	60.5
\(\Delta t_{\text{delay}}\)	端到端感知延迟	60.11
\(\boldsymbol{T}_{bs}\)	传感器到基座的外参变换	60.9

公式	一句话说明	对应节
\(T = f_{\text{slope}} \cdot f_{\text{rough}} \cdot f_{\text{step}} \cdot f_{\text{gap}}\)	乘性可通行性评分	60.5
\(J(\boldsymbol{p}) = w_T T + w_K K + w_S S\)	多准则落脚评分	60.6
\(\boldsymbol{p}^* = \arg\max J(\boldsymbol{p})\)	最优落脚点选择	60.6
\(\boldsymbol{p}_{\text{query}} = \boldsymbol{p} + \boldsymbol{v} \cdot \Delta t\)	延迟补偿前向预测	60.11
\(K = P/(P+R)\)	Kalman 增益（高程图融合）	60.3

第 60 章：感知驱动的落脚规划——从盲走到看路，让机器人"长眼睛"¶

前置自测¶

本章目标¶

60.1 从"盲"到"看"——为什么需要感知 ⭐¶

动机：一只"闭着眼"的四足在碎石坡上¶

如果不用感知会怎样——纯触觉反应式的局限¶

历史：从盲走到感知的里程碑¶

⚠️ 常见陷阱¶

练习¶

60.2 地形表示方法 ⭐⭐¶

动机：为什么不能直接用原始点云¶

五种地形表示方法对比¶

高程图（Elevation Map）——腿足感知的事实标准¶

体素（Voxel）——3D 完整但内存昂贵¶

SDF（Signed Distance Field）——碰撞检测利器¶

如何选择——决策树¶

⚠️ 常见陷阱¶

练习¶

60.3 Elevation Mapping 详解 ⭐⭐¶

动机：从点云到"能用的地图"需要几步¶

来龙去脉：Fankhauser 的贡献¶

Step 1：坐标系变换——从传感器系到世界系¶

Step 2：栅格投影——从 3D 点到 2D 格子¶

Step 3：Kalman 融合——每个格子的最优估计¶

Step 4：漂移补偿——处理里程计误差¶

Step 5：光线投影清理——去除伪观测¶

grid_map 库：数据结构与 API¶

⚠️ 常见陷阱¶

练习¶

60.4 GPU 加速地形建图 ⭐⭐⭐¶

动机：CPU 瓶颈在哪¶

如果不用 GPU 会怎样¶

elevation_mapping_cupy 架构¶

性能对比¶

⚠️ 常见陷阱¶

练习¶

60.5 地形特征提取与可通行性分析 ⭐⭐¶

动机：高度不等于可踩¶

反面推演：如果只用高度做落脚规划¶

四大地形特征¶

特征 1：坡度（Slope）¶

特征 2：粗糙度（Roughness）¶

特征 3：台阶高度（Step Height）¶

特征 4：间隙检测（Gap Detection）¶

可通行性评分（Traversability Score）¶

⚠️ 常见陷阱¶

练习¶

60.6 感知驱动的落脚点评分 ⭐⭐¶

动机：可通行性只是必要条件，不是充分条件¶

多准则落脚评分框架¶

分量 1：地形可通行性 \(T(\boldsymbol{p})\)¶

分量 2：运动学可达性 \(K(\boldsymbol{p})\)¶

分量 3：稳定裕度 \(S(\boldsymbol{p})\)¶

候选集合的生成¶

代码示例¶

⚠️ 常见陷阱¶

练习¶

60.7 TAMOLS：地形感知运动优化 ⭐⭐⭐¶

动机：贪心评分的局限¶

历史：从 MPC + 贪心到联合优化¶

TAMOLS 的核心思想¶

碰撞回避——SDF 的妙用¶

Graduated Optimization——避免局部最优¶

实时性能¶

TAMOLS vs 贪心评分 vs Perceptive MPC¶

⚠️ 常见陷阱¶

练习¶

60.8 学习型落脚选择 ⭐⭐⭐¶

动机：为什么需要学习¶

反面推演：如果只用手工方法做跑酷¶

Miki 2022（Science Robotics）——分水岭论文¶

Hoeller 2024（ANYmal Parkour）——端到端跑酷¶

Gangapurwala 等人——CNN 落脚评估¶

三种方法的对比¶

⚠️ 常见陷阱¶

练习¶

60.9 传感器选型与标定 ⭐⭐¶

动机：算法再好，传感器选错也白搭¶

深度相机 vs LiDAR——全面对比¶

传感器安装位置的工程考量¶