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

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

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

前置依赖：Ch51（LIPM / Capture Point）、Ch57（状态估计 / 里程计）、Ch58（落脚经典方法）、Ch59（优化驱动接触规划）

关键文献：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 题 → 先回对应章节复习

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

本章目标¶

学完本章，你应能：

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

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

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

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

想象你把 Ch58 的 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 场景中（Ch59），正确的策略可能需要"当前步故意踩近一点，为下一步留出空间"。反应式做不到这种前瞻性规划。

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

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

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（Ch27-28 的全局定位与建图）和物体识别（机械臂抓取）是不同的感知层次。腿足机器人的感知核心是 2.5D 局部地形，不是 3D 全局地图。

⚠️ 常见陷阱¶

⚠️ 概念误区：认为"感知行走"一定比"盲走"好 新手想法："既然有感知，就应该永远开着" 实际上：感知引入了额外的延迟（传感器处理 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)  │    │ 投影         │    │ (每格独立)   │
└─────────────┘    └──────────────┘    └──────────────┘    └─────────────┘
                          ↑                                       │
                   ┌──────────────┐                        ┌──────────────┐
                   │ 状态估计      │                        │ 漂移补偿     │
                   │ (Ch57 输出)  │                        │ + 光线清理   │
                   └──────────────┘                        └──────────────┘
                                                                  │
                                                           ┌──────────────┐
                                                           │ 输出：        │
                                                           │ 高程图 + 方差 │
                                                           └──────────────┘

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

来龙去脉：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}$：基座在世界系中的位姿（从 Ch57 状态估计获得）

为什么这一步是关键：如果 $\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 感知驱动的落脚点评分 ⭐⭐¶

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

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

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

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

反面推演：

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

只看动力学的落脚规划（Ch58）：
  "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 裕度（Ch51 / Ch58）：

\[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 场景中（Ch59），落脚点必须在指定的石头上。如何修改评分框架来处理这种硬约束？提示：可以将石头外的区域设置 $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 + 贪心到联合优化¶

进化路径：

Ch58 Raibert启发式  →  不考虑地形
       ↓
Ch60.6 贪心评分     →  考虑地形，但单腿单步
       ↓
TAMOLS 联合优化     →  考虑地形 + 多腿耦合 + 多步前瞻
       ↓
Perceptive MPC      →  地形嵌入 OCS2 的 NMPC（Ch67 精读）

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}}$：任务代价（速度跟踪、姿态保持），与 Ch55 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（Ch67）
落脚选择	网格搜索	优化	优化
基座-落脚耦合	无	有	有
多步前瞻	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 设计需要深刻理解传统控制的目标（速度跟踪、能耗、平滑性——这些概念来自 Ch55-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（前方精细感知）

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

外参 $\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, Ch57
可通行性评分全部接近零，规划器找不到可行落脚点	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 本章小结¶

知识点总结¶

节号	主题	核心要点	难度
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、外参标定、时间同步	⭐⭐

方法论谱系全景¶

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

本章首尾呼应¶

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

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

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

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

章节	新增模块	功能
Ch47-49	运动学	URDF 加载、正/逆运动学
Ch50-51	简化模型	LIPM、Capture Point
Ch55-56	MPC + 步态	OCS2 轨迹优化、步态管理器
Ch57	状态估计	EKF 融合 IMU + 关节
Ch58	落脚规划	Raibert 启发式
Ch60（本章）	感知建图	Elevation Map + 可通行性 + 落脚评分

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

集成方式：PerceptiveFootholdSelector 的输出（落脚点位置）替换 Ch58 的 Raibert 输出，输入 Ch55 的 OCS2 MPC 作为参考落脚点。

延伸阅读¶

必读（⭐⭐）¶

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，Ch67 精读对象。
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（⭐⭐⭐⭐）

与其他章节衔接¶

向前承接： - Ch51 LIPM + Capture Point → 60.6 稳定裕度评分的基础 - Ch57 状态估计 → 60.3 Elevation Mapping 依赖里程计 - Ch58 Raibert 启发式 → 60.6 作为候选集合的中心点 - Ch59 CITO / GCS → 60.7 TAMOLS 的优化框架思想

向后指向： - 60.3-60.4 Elevation Mapping → Ch66 深入精读 grid_map 和 elevation_mapping_cupy 的代码 - 60.7 TAMOLS → Ch67 Perceptive MPC 精读（Grandia 2023 在 OCS2 中的实现） - 60.8 学习型 → Ch65 RL + MPC 混合架构（DTC 精读） - 60.9 传感器 → Ch62 实时系统中的传感器驱动与数据流

地形类型	盲走失效原因	后果	感知能解决什么
斜坡	假设 \(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}}\)	间隙	脚陷入缝隙

第 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——全面对比¶

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