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

Traversability 工程¶

难度：⭐⭐~⭐⭐⭐ | 建议时间：1.5 周（20-25 小时） | 前置：Ch27（PCL）、Ch36（数据布局）、Ch38（CUDA+LibTorch）、Ch57（状态估计）、Ch60（感知驱动落脚）

一句话概要：高程图（Elevation Map）是腿足感知的 de facto 标准数据结构——把 3D 点云压缩为 2.5D 栅格，每格存储高度、方差、坡度、粗糙度、可通行性等多层信息。本章从 grid_map 库的内部数据布局讲起，深入 Kalman 融合的贝叶斯推导、光线投影一致性检查的几何推导、elevation_mapping_cupy 的 GPU 架构，以及 traversability 的数学定义和多种计算方法。

前置自测¶

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

[Ch27] PCL 的 pcl::PointCloud<pcl::PointXYZ> 和 sensor_msgs::PointCloud2 的区别是什么？为什么 ROS 使用后者作为消息格式？
[Ch36] AoS（Array of Structures）和 SoA（Structure of Arrays）的内存布局差异是什么？哪种更适合 GPU 并行处理？为什么？
[Ch57] 状态估计输出的里程计（odometry）精度如何影响高程图？如果里程计有 1% 的漂移，10 米行走后高程图会出现什么问题？
[Ch38] CuPy 的 RawKernel 和 NumPy 风格的 CuPy 操作各适合什么场景？为什么 elevation_mapping_cupy 同时使用两种？
[基础] 标量 Kalman 滤波的更新公式：给定先验 \((\hat{z}, P)\) 和观测 \((z_{\text{meas}}, R)\)，写出后验 \((\hat{z}_{\text{new}}, P_{\text{new}})\) 的表达式。Kalman 增益 \(K\) 的物理含义是什么？

本章目标¶

学完本章，你应能： 1. 理解 grid_map 库的内部数据布局——包括 circular buffer 实现滚动地图的原理 2. 从贝叶斯角度完整推导 Kalman 融合——理解为什么高程图融合本质上是贝叶斯估计 3. 推导光线投影一致性检查的几何原理——理解如何检测和清除过时的高程数据 4. 掌握 elevation_mapping_cupy 的 CUDA kernel 设计——GPU 并行高程图更新 5. 计算 traversability 并理解多种方法的数学定义——坡度、粗糙度、可踩性的精确计算 6. 理解 C++/Python 混合架构的设计取舍——为什么感知用 Python 而控制用 C++ 7. 能在仿真环境中部署并验证完整的感知管线——深度相机 -> 点云 -> 高程图 -> traversability（真机部署指引见 Ch69 综合实战）

66.1 grid_map 库架构深入 ⭐⭐¶

本节解决什么问题：grid_map 是 ANYbotics 开发的 ROS 2.5D 地图库，是腿足感知的行业标准。我们需要理解它的内部数据布局、迭代器设计和性能特性。

动机：为什么不直接用点云？¶

Ch60 讲过，腿足 MPC 需要查询"某个 XY 坐标的地面高度"——这要求 \(O(1)\) 的空间查询。原始点云是无序的，查询需要 KD-tree，复杂度 \(O(\log n)\)，且对频繁的区域查询效率低。

高程图把 3D 问题投影为 2.5D：

3D 点云                2.5D 高程图
(x, y, z) 无序         (row, col) -> z
N=100k 点              200x200 格子
查询 O(log N)          查询 O(1)
内存 ~2.4 MB           内存 ~160 KB (单层)

"2.5D" 的含义：每个 (x, y) 位置只存一个 z 值（最高点或平均值）。这意味着高程图**不能表示悬空结构**（如桥梁下方）——但对于腿足行走，地面通常是 2.5D 的，这个简化是合理的。

跨领域类比：高程图之于 3D 点云,就像灰度图之于 RGB 图——通过降维换取查询效率。灰度图把 3 通道压缩为 1 通道,丢失了颜色信息但保留了形状;高程图把 3D 空间压缩为 2D 栅格,丢失了垂直方向的多层结构但保留了地面形状。两者的共同思想是:在任务允许的信息损失范围内,选择计算最高效的表示。灰度图足以做边缘检测,高程图足以做腿足落脚规划。

如果不用 2.5D 会怎样¶

如果坚持用 3D 表示（如体素 OctoMap），内存和计算开销会急剧增加：

维度	2.5D 高程图	3D 体素（0.04m）
分辨率	0.04 m/格	0.04 m/体素
10m x 10m 区域	250x250 = 62.5K 格	250x250x50 = 3.1M 体素
单层内存	250 KB	12.5 MB
查询复杂度	\(O(1)\)	\(O(\log n)\)（八叉树）
更新复杂度	\(O(1)\) 每格	\(O(\log n)\) 每体素
MPC 集成	简单（查表）	困难（遍历）

对于腿足行走，2.5D 高程图是性能和表达能力的最佳折中。

历史：grid_map 的演变¶

grid_map 库的发展经历了三个阶段：

Fankhauser 2014：Peter Fankhauser, Michael Bloesch, Christian Gehring, Marco Hutter, Roland Siegwart 在 CLAWAR 2014 发表 "Robot-Centric Elevation Mapping with Uncertainty Estimates"——提出了机器人中心的高程图框架，首次将 Kalman 滤波引入高程图融合
Fankhauser 2016：Fankhauser & Hutter 在 Springer 出版 "A Universal Grid Map Library"——将框架开源为 grid_map 库，定义了多层栅格的标准接口
ROS2 迁移：grid_map 现已支持 ROS2 Humble / Iron / Jazzy / Rolling，是 ANYbotics 和 ETH RSL 的生产工具

grid_map 核心数据结构¶

// grid_map_core/include/grid_map_core/GridMap.hpp（简化）
class GridMap {
 public:
  // 构造：指定层名称
  GridMap(const std::vector<std::string>& layers);

  // 设置几何参数
  void setGeometry(const Length& length, double resolution,
                   const Position& position);

  // 添加/访问层（每层是 Eigen::MatrixXf）
  void add(const std::string& layer, double value = NAN);
  Matrix& operator[](const std::string& layer);
  const Matrix& operator[](const std::string& layer) const;

  // 坐标转换
  bool getIndex(const Position& position, Index& index) const;
  bool getPosition(const Index& index, Position& position) const;

  // 地图移动（circular buffer）
  void move(const Position& position);

 private:
  std::unordered_map<std::string, Matrix> data_;
  // 每层是一个 Eigen::MatrixXf, 大小 (rows, cols)

  Length length_;          // 物理尺寸 (m x m)
  Position position_;      // 中心点世界坐标 (x, y)
  double resolution_;      // 分辨率 (m/格)
  Size size_;              // 格子数 (rows, cols)
  Index startIndex_;       // circular buffer 起始索引
};

层（Layer）的概念：grid_map 的核心设计是多层栅格——每个 (row, col) 位置存储多个属性：

层名称	数据类型	含义	典型范围
`elevation`	float	地面高度 (m)	-2.0 ~ 5.0
`variance`	float	高度不确定性	0 ~ 1.0
`slope`	float	坡度 (rad)	0 ~ \(\pi/2\)
`roughness`	float	粗糙度 (m)	0 ~ 0.5
`traversability`	float	可通行性	0.0 ~ 1.0
`semantic_class`	float	语义类别 ID	0 ~ 20
`color`	float	RGB 编码	0 ~ 1

所有层共享同一个几何参数（分辨率、尺寸、位置），确保空间对齐。

Circular Buffer：滚动地图的实现¶

问题：机器人在行走，高程图需要跟着移动。如果每次移动都重新分配内存并拷贝数据，开销巨大。

解决方案：grid_map 使用 circular buffer（环形缓冲区）实现 \(O(1)\) 的地图移动。

原理：数据存储在固定大小的矩阵中，通过偏移起始索引 startIndex_ 实现逻辑上的"移动"，而不实际搬移任何数据。

普通数组移动（需要整体拷贝）：
移动前：[A B C D E F G H]  中心在 D
移动后：[_ B C D E F G X]  中心在 E，A 被丢弃，X 是新数据
        需要把 B-H 全部左移一格 -> O(N)

Circular Buffer 移动（只改起始指针）：
移动前：[A B C D E F G H]  startIndex=0, 中心在 data[3]
移动后：[X B C D E F G H]  startIndex=1, 中心在 data[4]
        A 的位置写入新数据 X -> O(1)
        不需要移动任何数据！

坐标转换中的 circular buffer 处理：

// grid_map 的坐标转换考虑 circular buffer
bool GridMap::getIndex(const Position& position, Index& index) const {
  // 1. 世界坐标 -> 未偏移的栅格索引
  Index rawIndex;
  rawIndex(0) = static_cast<int>(
      (position_(0) - position(0)) / resolution_ + size_(0) / 2);
  rawIndex(1) = static_cast<int>(
      (position_(1) - position(1)) / resolution_ + size_(1) / 2);

  // 2. 应用 circular buffer 偏移（取模）
  index(0) = (rawIndex(0) + startIndex_(0)) % size_(0);
  index(1) = (rawIndex(1) + startIndex_(1)) % size_(1);

  return isInside(position);
}

为什么用 circular buffer 而不是直接移动数据？ 因为高程图的移动频率很高——以 1 m/s 行走、0.04 m 分辨率计算，每秒需要移动 25 次。如果每次移动都拷贝 200x200 的矩阵（多层），开销不可接受。Circular buffer 把 \(O(N^2)\) 的拷贝操作变成了 \(O(1)\) 的指针偏移。

迭代器设计¶

grid_map 提供了多种迭代器，支持高效遍历：

// 全图遍历
for (grid_map::GridMapIterator it(map); !it.isPastEnd(); ++it) {
  grid_map::Position pos;
  map.getPosition(*it, pos);
  float z = map.at("elevation", *it);
  // 处理每个格子...
}

// 子图遍历（只遍历局部区域）
grid_map::Position center(1.0, 2.0);   // 世界坐标
grid_map::Length length(3.0, 3.0);     // 3m x 3m 子图
for (grid_map::SubmapIterator it(map, center, length);
     !it.isPastEnd(); ++it) {
  // 只遍历这个区域——用于局部特征计算
}

// 圆形区域遍历
for (grid_map::CircleIterator it(map, center, radius);
     !it.isPastEnd(); ++it) {
  // 遍历以 center 为圆心、radius 为半径的区域
}

// 线段遍历（用于光线投影一致性检查）
grid_map::Position start(0, 0), end(5, 3);
for (grid_map::LineIterator it(map, start, end);
     !it.isPastEnd(); ++it) {
  // 遍历从 start 到 end 的线段经过的所有格子
  // 内部使用 Bresenham 算法
}

ROS2 支持现状¶

截至 2026 年，grid_map 已支持多个 ROS2 发行版：

发行版	状态	Ubuntu 版本
Humble	稳定支持	22.04
Iron	支持	22.04
Jazzy	支持（2024.05 发布）	24.04
Rolling	持续维护	最新

所有子包（core, ros, filters, pcl, cv, sdf, visualization 等）均已迁移到 ROS2。

性能考虑¶

参数	典型值	影响
分辨率	0.04 m	越小越精确，但格子数 \(\propto 1/r^2\)
尺寸	8m x 8m	覆盖范围 vs 内存
格子数	200x200 = 40K	单层 160 KB，8 层 1.28 MB
层数	4-8	每层 4 bytes/格
总内存	~1-2 MB	完全在 L2 缓存内

缓存友好性分析：200x200 的 float 矩阵占 160 KB，典型 CPU 的 L2 缓存为 256 KB-1 MB。单层遍历几乎全在 L2 缓存命中——这是 grid_map 高性能的关键。但 8 层总计 1.28 MB，可能超出 L2 而落入 L3。

grid_map 分辨率与覆盖范围的工程权衡：

分辨率和覆盖范围的选择取决于机器人的速度和感知需求。以下是不同场景的推荐配置：

场景	分辨率	覆盖范围	格子数	内存 (5层)	更新频率
室内低速 (0.3 m/s)	0.02 m	4m x 4m	200x200	800 KB	30 Hz
室外中速 (0.8 m/s)	0.04 m	8m x 8m	200x200	800 KB	20 Hz
室外高速 (1.5 m/s)	0.06 m	12m x 12m	200x200	800 KB	15 Hz
极端地形 (0.3 m/s)	0.02 m	6m x 6m	300x300	1.8 MB	20 Hz

格子数保持 200x200 左右是一个工程经验值——更大的格子数对 CPU 版本来说遍历开销增加，对 GPU 版本来说 kernel launch 开销占比增大。如果需要更大覆盖范围，优先增大分辨率（降低精度）而不是增加格子数。

跨领域类比：grid_map 的 circular buffer 滚动机制，就像老式打字机的滚筒（platen）——纸张（数据）是固定的，滚筒旋转（start index 移动）让打字头（查询）始终对准当前工作区域。新数据写入时不需要搬移整张纸（不需要 memcpy 整个矩阵），只需要转动滚筒让旧区域转到前面被新数据覆盖。这个 \(O(1)\) 的地图移动操作是 grid_map 能在 1 kHz 控制循环中使用的关键——如果每次机器人移动都要搬移整个 200x200 矩阵（\(O(n^2)\)），高程图更新会成为系统瓶颈。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：忽略 circular buffer 的索引偏移 错误做法：直接用 map["elevation"](row, col) 访问底层 Eigen 矩阵，不经过 getPosition / getIndex 转换现象：地图移动后，相同的 (row, col) 对应不同的世界坐标——数据全乱了根本原因：circular buffer 的 startIndex_ 改变了 (row, col) 到世界坐标的映射正确做法：始终通过 map.at("layer", index) 或迭代器访问，不直接操作底层矩阵

⚠️ 概念误区：认为高程图能表示所有地形 新手想法："高程图存了每个点的高度，足够了" 实际上：2.5D 高程图**不能表示悬空结构**（桥梁下方、多层楼梯的上下层）。每个 (x,y) 只存一个 z 值——如果同一 (x,y) 有两个不同高度的表面（如桥面和桥下地面），高程图只能存其中一个。替代方案：对于复杂 3D 结构，使用 2.5D+ 高程图（多层高程）或体素地图（OctoMap）。

⚠️ 思维陷阱：认为分辨率越高越好 新手想法："0.01m 分辨率比 0.04m 好 4 倍" 实际上：分辨率从 0.04m 提高到 0.01m，格子数增加 16 倍（\(1/r^2\)），内存和计算也增加 16 倍。而深度相机在 3m 距离的测量噪声约 +-1cm——0.01m 的分辨率已经接近噪声极限，再高也不会更准确。正确做法：分辨率应匹配传感器精度和任务需求。平地 trot 用 0.05-0.1m，parkour 用 0.02-0.03m。

练习¶

[代码实践] 创建一个 grid_map，尺寸 8m x 8m，分辨率 0.04m。添加 elevation 层并填充 \(z = 0.3 \sin(2\pi x / 4) \cos(2\pi y / 4)\) 的正弦波地形。发布到 RViz 可视化。
[性能分析] 测量 grid_map 在不同分辨率（0.02m, 0.04m, 0.08m）下的全图遍历时间。验证：时间是否与格子数成正比？
[深入理解] circular buffer 的 move() 操作需要清除新暴露区域的旧数据（设为 NaN）。分析：如果机器人以 2m/s 移动，每秒需要清除多少格子？这个开销相对于全图更新占多大比例？

过渡：理解了 grid_map 的数据结构后，下一步是了解它的模块化算子——grid_map 提供了一整套滤波器链，用于坡度计算、插值、阈值化等常见操作。

66.2 grid_map 的主要算子与滤波器链 ⭐⭐¶

本节解决什么问题：grid_map 提供了一套模块化的算子和滤波器链（filter chain），用于点云投影、数学变换、坡度计算等常见操作。理解这些工具能避免重复造轮子。

从点云到高程图¶

#include <grid_map_pcl/GridMapPclLoader.hpp>

// 方法 1：使用 GridMapPclLoader（最简单，适合离线处理）
grid_map::GridMapPclLoader loader;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = ...;
loader.setInputCloud(cloud);
loader.preProcessInputCloud();
loader.initializeGridMapGeometryFromInputCloud();
loader.addLayerFromInputCloud("elevation");
grid_map::GridMap map = loader.getGridMap();

// 方法 2：手动投影（更灵活，适合在线实时更新）
void projectPointCloud(
    const pcl::PointCloud<pcl::PointXYZ>& cloud,
    grid_map::GridMap& map) {
  for (const auto& pt : cloud.points) {
    grid_map::Position pos(pt.x, pt.y);
    grid_map::Index idx;
    if (!map.getIndex(pos, idx)) continue;  // 超出地图范围

    float& z = map.at("elevation", idx);
    if (std::isnan(z) || pt.z > z) {
      z = pt.z;  // 取最高点（简单策略）
    }
  }
}

为什么两种方法？ GridMapPclLoader 适合离线处理（一次性从点云构建地图）。手动投影适合在线更新（每帧点云增量融合）。后者是 elevation_mapping 的核心操作。

滤波器链¶

grid_map 提供了类似 ROS filter_chain 的滤波器框架——通过 YAML 配置文件定义一系列操作，无需写 C++ 代码：

# grid_map_filter_chain.yaml
grid_map_filters:
  - name: buffer_normalizer
    type: gridMapFilters/BufferNormalizerFilter

  - name: slope_filter
    type: gridMapFilters/SlidingWindowMathExpressionFilter
    params:
      input_layer: elevation
      output_layer: slope
      expression: "(maxOfFinites(elevation) - minOfFinites(elevation))"
      compute_window_size: 3

  - name: roughness_filter
    type: gridMapFilters/MathExpressionFilter
    params:
      output_layer: roughness
      expression: "sqrt(variance)"

  - name: traversability_threshold
    type: gridMapFilters/ThresholdFilter
    params:
      layer: slope
      upper_threshold: 0.5  # 约 28 度
      set_to: 0.0
      layer_suffix: _binary

在 C++ 中使用滤波器链：

#include <grid_map_ros/grid_map_ros.hpp>
#include <filters/filter_chain.hpp>

// 初始化
filters::FilterChain<grid_map::GridMap> filter_chain("grid_map::GridMap");
filter_chain.configure("grid_map_filters", nh);

// 每帧应用
grid_map::GridMap output;
filter_chain.update(input_map, output);

常用滤波器¶

滤波器	功能	典型用途
`MathExpressionFilter`	对层做数学表达式计算	`"abs(elevation - 0.5)"`
`ThresholdFilter`	二值化/截断	坡度阈值化
`SlidingWindowMathExpressionFilter`	滑窗计算	局部坡度、粗糙度
`MinInRadiusFilter`	半径内最小值	侵蚀操作
`MedianFillFilter`	中值填充缺失值	处理稀疏区域
`InpaintFilter`	插值填充	平滑缺失区域

转为 ROS 消息¶

// GridMap -> ROS2 消息
grid_map_msgs::msg::GridMap ros_msg;
grid_map::GridMapRosConverter::toMessage(map, ros_msg);
publisher_->publish(ros_msg);

// GridMap -> OccupancyGrid（用于导航栈）
nav_msgs::msg::OccupancyGrid occ;
grid_map::GridMapRosConverter::toOccupancyGrid(
    map, "traversability", 0.0, 1.0, occ);

// GridMap -> PointCloud2（用于 RViz 3D 可视化）
sensor_msgs::msg::PointCloud2 cloud;
grid_map::GridMapRosConverter::toPointCloud(
    map, "elevation", cloud);

⚠️ 常见陷阱¶

⚠️ 编程陷阱：滤波器链的顺序依赖 错误做法：把计算 roughness 的滤波器放在计算 slope 之前，但 roughness 依赖 slope 层现象：roughness 层全是 NaN 根本原因：滤波器链按 YAML 中的顺序执行，后面的滤波器可以使用前面创建的层正确做法：确保滤波器的依赖关系反映在 YAML 的顺序中——先计算被依赖的层

⚠️ 概念误区：认为 NaN 是错误 新手想法："高程图里有 NaN，是不是代码出 bug 了？" 实际上：NaN 是 grid_map 表示"未观测"的标准方式。很多格子（尤其是远处或遮挡区域）确实没有观测数据，NaN 是正确的语义。代码中必须处理 NaN——用 std::isnan() 检查，而不是假设所有格子都有值。

练习¶

[代码实践] 编写一个 YAML 滤波器链，依次计算：slope -> roughness -> traversability（线性组合）。在 RViz 中切换显示不同层，比较结果。
[设计分析] grid_map 的 MathExpressionFilter 使用字符串表达式（如 "abs(elevation - 0.5)"）。分析这种设计的优缺点——与直接写 C++ 代码相比，灵活性、性能和调试便利性如何？

过渡：grid_map 提供了数据结构和基本算子，但最核心的问题还没解决——如何把带噪声的多帧点云融合成一张精确的高程图？这是一个经典的贝叶斯估计问题。

66.3 Elevation Mapping 的 Kalman 融合 ⭐⭐⭐¶

本节解决什么问题：每帧传感器（LiDAR / 深度相机）给出一批带噪声的点云观测。简单覆盖会导致高频抖动，不合并又无法积累多帧信息。Kalman 滤波提供了最优融合方案。

动机：为什么不能直接覆盖？¶

场景：Go2 的深度相机以 30 Hz 输出点云。每帧约 100K 点，投影到高程图后，每格平均 2-3 个观测。

如果直接用最新帧覆盖：

帧 t=0: 某格观测 z = 0.51m（噪声偏高）
帧 t=1: 同格观测 z = 0.48m（噪声偏低）
帧 t=2: 同格观测 z = 0.50m（接近真值）
帧 t=3: 同格没有观测（被遮挡）
帧 t=4: 同格观测 z = 0.53m（噪声偏高）

直接覆盖：高程在 0.48-0.53 之间跳动
  -> 下游 MPC 收到的地形忽高忽低，产生不必要的关节振动

如果取所有帧的简单平均：

平均：(0.51 + 0.48 + 0.50 + 0.53) / 4 = 0.505m
问题：没有考虑各帧观测的噪声大小不同
      （远距离观测噪声大，近距离噪声小）

Kalman 滤波：最优加权融合，自动权衡"信不信新观测"——这正是 Ch57 中状态估计的核心思想，现在应用到每个高程图格子。

跨领域类比：高程图的 Kalman 融合就像维基百科的编辑机制——每个格子相当于一个词条，每帧传感器数据相当于一次编辑。直接覆盖就像无审核的 wiki（最新编辑直接生效，质量不稳定）；简单平均就像投票制（所有编辑等权，不区分专家和新手）；Kalman 融合则像带信誉权重的审核系统——新编辑（观测）的采纳程度取决于它的可信度（\(R\) 小的近距离观测权重更高）和现有内容的置信度（\(P\) 小的多次融合结果更难被推翻）。方差 \(P\) 随着融合次数单调递减，就像词条被越多可靠来源确认后越难被单次不可靠编辑推翻。

从贝叶斯角度推导 Kalman 融合¶

问题设定：

真实高度：\(z\)（未知常数，假设地面是静态的）
当前估计（先验）：\(p(z) = \mathcal{N}(\hat{z}, P)\)
新观测：\(z_{\text{meas}} = z + v\)，\(v \sim \mathcal{N}(0, R)\)
即 \(p(z_{\text{meas}} | z) = \mathcal{N}(z, R)\)

Step 1：贝叶斯公式

\[p(z | z_{\text{meas}}) \propto p(z_{\text{meas}} | z) \cdot p(z)\]

为什么用贝叶斯？ 因为我们要把先验知识（之前的估计 \(\hat{z}\)）和新证据（观测 \(z_{\text{meas}}\)）最优地结合。贝叶斯公式正是做这件事的数学工具。

Step 2：高斯乘法

两个高斯的乘积仍然是高斯。展开指数部分：

\[p(z | z_{\text{meas}}) \propto \exp\left(-\frac{(z_{\text{meas}} - z)^2}{2R}\right) \cdot \exp\left(-\frac{(z - \hat{z})^2}{2P}\right)\]

\[= \exp\left(-\frac{1}{2}\left[\frac{(z_{\text{meas}} - z)^2}{R} + \frac{(z - \hat{z})^2}{P}\right]\right)\]

Step 3：配方（对 \(z\)）

展开指数中的二次型：

\[\frac{(z_{\text{meas}} - z)^2}{R} + \frac{(z - \hat{z})^2}{P}\]

\[= \frac{z^2 - 2z \cdot z_{\text{meas}} + z_{\text{meas}}^2}{R} + \frac{z^2 - 2z \cdot \hat{z} + \hat{z}^2}{P}\]

\[= \left(\frac{1}{R} + \frac{1}{P}\right) z^2 - 2\left(\frac{z_{\text{meas}}}{R} + \frac{\hat{z}}{P}\right) z + \text{const}\]

配方得到后验分布 \(p(z | z_{\text{meas}}) = \mathcal{N}(\hat{z}_{\text{new}}, P_{\text{new}})\)，其中：

\[P_{\text{new}}^{-1} = P^{-1} + R^{-1} \quad \Rightarrow \quad P_{\text{new}} = \frac{PR}{P + R}\]

\[\hat{z}_{\text{new}} = P_{\text{new}} \left(\frac{z_{\text{meas}}}{R} + \frac{\hat{z}}{P}\right) = \frac{P \cdot z_{\text{meas}} + R \cdot \hat{z}}{P + R}\]

Step 4：Kalman 增益形式

定义 Kalman 增益 \(K = \frac{P}{P + R}\)，则上述公式简化为：

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

\[P_{\text{new}} = (1 - K) P\]

物理含义：

\(K \to 1\)（当 \(R \to 0\)，观测非常精确）：\(\hat{z}_{\text{new}} \approx z_{\text{meas}}\)，完全相信观测
\(K \to 0\)（当 \(R \to \infty\)，观测非常嘈杂）：\(\hat{z}_{\text{new}} \approx \hat{z}\)，完全不信观测
\(K = 0.5\)（当 \(P = R\)）：先验和观测等权平均

重要性质：\(P_{\text{new}} = (1-K)P < P\)——每次融合后方差严格减小（假设 \(K > 0\)）。这意味着**估计越来越确定**，符合直觉——越多观测越精确。

本质洞察：高程图的 Kalman 融合和 Ch57 中机器人状态估计的 Kalman 滤波在数学上是**完全相同的操作**,只是应用对象不同——一个估计的是"机器人在哪",另一个估计的是"地面多高"。两者的本质都是贝叶斯推断:用先验(上一次的估计)和似然(新观测)的乘积得到后验(更新后的估计)。理解了这一点,你就掌握了一个通用工具——任何"从带噪声的重复观测中估计固定量"的问题,都可以用同样的框架。

观测噪声模型¶

观测噪声 \(R\) 不是常数——它取决于**传感器到观测点的距离**和**入射角**：

\[R = R_0 + k_d \cdot d^2 + k_\alpha / \cos^2(\alpha)\]

参数	含义	典型值
\(R_0\)	基础噪声方差	\(0.001\) m\(^2\)
\(k_d\)	距离系数	\(0.01\) m\(^{-2}\)
\(d\)	传感器到点的距离	1-5 m
\(k_\alpha\)	入射角系数	\(0.01\)
\(\alpha\)	光线与表面法线的夹角	0-80 度

直觉：距离越远，深度相机的测量噪声越大（\(R \propto d^2\)）。光线越斜（与表面几乎平行），测量越不可靠（\(R \propto 1/\cos^2\alpha\)）。

完整融合代码¶

void fusePointCloud(const PointCloud& cloud,
                    const Eigen::Isometry3d& T_world_sensor,
                    GridMap& map) {
  Eigen::Vector3d sensor_pos = T_world_sensor.translation();

  for (const auto& pt_sensor : cloud) {
    // 变换到世界系
    Eigen::Vector3d pt_world = T_world_sensor *
        Eigen::Vector3d(pt_sensor.x, pt_sensor.y, pt_sensor.z);

    // 投影到高程图
    grid_map::Position pos2d(pt_world.x(), pt_world.y());
    grid_map::Index idx;
    if (!map.getIndex(pos2d, idx)) continue;

    float z_meas = static_cast<float>(pt_world.z());

    // 计算观测噪声
    double d = (pt_world - sensor_pos).norm();
    double R = R0_ + k_d_ * d * d;

    // 读取先验
    float z_hat = map.at("elevation", idx);
    float P = map.at("variance", idx);

    if (std::isnan(z_hat)) {
      // 第一次观测：直接初始化
      map.at("elevation", idx) = z_meas;
      map.at("variance", idx) = static_cast<float>(R);
    } else {
      // Kalman 更新
      float K = P / (P + static_cast<float>(R));
      map.at("elevation", idx) = z_hat + K * (z_meas - z_hat);
      map.at("variance", idx) = (1.0f - K) * P;
    }
  }
}

时间衰减：处理久未观测的格子¶

如果机器人走过某个区域后再也没回来，那个区域的高程估计应该越来越不可信——因为地面可能已经变了。

时间衰减：每个控制周期，对所有格子的方差加一个小量：

\[P_{t+1} = P_t + Q_{\text{process}} \cdot \Delta t\]

\(Q_{\text{process}}\) 是过程噪声，表示"地面每秒可能变化多少"。对于静态环境，\(Q_{\text{process}}\) 很小（\(10^{-6}\) m\(^2\)/s）。对于动态环境（如有人走动），\(Q_{\text{process}}\) 应更大。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：Kalman 更新后方差变为负数 错误做法：浮点精度问题导致 \(P_{\text{new}} = (1-K) P < 0\) 现象：某些格子的方差变成极小的负数，导致后续更新 K > 1，继而发散根本原因：当 \(K\) 非常接近 1 时，\((1-K)\) 可能因浮点误差变成负数正确做法：使用 Joseph 形式更新 \(P_{\text{new}} = (1-K) P (1-K) + K R K\)，或简单地 clamp：P_new = std::max(P_new, 1e-8f)

⚠️ 概念误区：认为 Kalman 融合总是改善估计 新手想法："每帧融合都让估计更准" 实际上：如果观测噪声模型 \(R\) 设得太小（过于相信观测），一个异常值会把整个估计拉偏。如果 \(R\) 设得太大（过于不信观测），融合收敛太慢。\(R\) 的标定至关重要。验证方法：在已知平面上运行 10 秒，检查方差是否单调下降到合理值。

⚠️ 思维陷阱：忽略里程计漂移对高程图的影响 新手想法："高程图只关心 z 值，xy 漂移无所谓" 实际上：点云投影到高程图需要精确的 \((x, y)\) 坐标。如果里程计在 \(x\) 方向漂移 5cm，同一个物体在不同帧中会被投影到不同的格子——导致高程图出现"重影"或"模糊"。这正是 Ch57 中状态估计精度至关重要的原因。

练习¶

[数学推导] 证明：经过 \(n\) 次独立观测（噪声均为 \(R\)）后，Kalman 融合的方差为 \(P_n = \frac{PR}{nP + R}\)（其中 \(P\) 是初始方差）。当 \(n \to \infty\) 时，\(P_n \to 0\)——估计越来越确定。
[代码实践] 实现上述 fusePointCloud 函数。用仿真深度相机对准一个平面（真实高度 0.5m），观察 10 秒内各格子的 \(\hat{z}\) 和 \(P\) 如何收敛。绘制某格子的 \(\hat{z}(t)\) 和 \(P(t)\) 曲线。
[分析题] 如果传感器标定有 2 度的倾斜误差（深度相机没有完全水平安装），对 3m 距离的点会产生多大的 z 方向偏差？这个偏差能被 Kalman 融合消除吗？

过渡：Kalman 融合解决了"如何把新数据融入旧估计"的问题。但还有一个问题没解决——如果地面形状变了（桌子被搬走、门打开了），旧数据怎么清除？光线投影一致性检查解决这个问题。

66.4 光线投影一致性检查 ⭐⭐⭐¶

本节解决什么问题：高程图中的旧数据可能已经过时——物体被移走了，但高程图还记着它的高度。光线投影一致性利用"传感器看到了一个点，意味着光线路径上没有障碍"这个几何约束来检测和清除过时数据。

动机：消失的障碍物¶

场景：机器人前方 2m 处有一个箱子（高 30cm）。高程图正确记录了这个箱子。然后有人把箱子搬走了。如果没有一致性检查，高程图会**永远记着**这个不存在的箱子——MPC 会绕道走，或者计算出不必要的落脚约束。

几何原理¶

当传感器观测到一个点 \(p\)（位于地面上），光线从传感器位置 \(o\) 到 \(p\) 的路径上**不应该有任何障碍**。如果高程图在光线路径上的某个格子 \(g\) 的高度 \(\hat{z}_g\) 高于光线在该位置的高度 \(z_{\text{ray}}(g)\)，说明 \(g\) 的数据是过时的——因为如果 \(g\) 真的那么高，传感器不可能"穿过"它看到 \(p\)。

传感器 o ─────── 光线 ─────── 观测点 p
          |                      |
          |   格子 g              |
          |   z_hat_g = 0.3m     |
          |                      |
          |   光线在 g 的高度     |
          |   z_ray(g) = 0.1m    |
          |                      |
          |   z_hat_g > z_ray(g) |
          |   -> 数据过时！       |

数学推导¶

光线参数化：

\[\mathbf{r}(t) = \mathbf{o} + t(\mathbf{p} - \mathbf{o}), \quad t \in [0, 1]\]

在参数 \(t\) 处，光线的高度（z 分量）为：

\[z_{\text{ray}}(t) = o_z + t(p_z - o_z)\]

对于光线经过的格子 \(g\)，设 \(g\) 的 \((x, y)\) 坐标对应参数 \(t_g\)：

\[t_g = \frac{\|g_{xy} - o_{xy}\|}{\|p_{xy} - o_{xy}\|}\]

则光线在 \(g\) 处的高度为：

\[z_{\text{ray}}(g) = o_z + t_g \cdot (p_z - o_z)\]

一致性检查规则：

\[\text{if } \hat{z}_g > z_{\text{ray}}(g) + \delta \text{ then mark } g \text{ as outdated}\]

其中 \(\delta\) 是容忍阈值（考虑噪声），通常取 2-3 倍的高程标准差。

实现¶

void rayCastConsistencyCheck(
    const Eigen::Vector3d& sensor_pos,
    const pcl::PointCloud<pcl::PointXYZ>& cloud_world,
    GridMap& map, float delta, float var_increase) {

  for (const auto& pt : cloud_world) {
    grid_map::Position start(sensor_pos.x(), sensor_pos.y());
    grid_map::Position end(pt.x, pt.y);

    for (grid_map::LineIterator it(map, start, end);
         !it.isPastEnd(); ++it) {
      grid_map::Position grid_pos;
      map.getPosition(*it, grid_pos);

      // 计算光线在此格子的高度
      double dx = grid_pos.x() - sensor_pos.x();
      double dy = grid_pos.y() - sensor_pos.y();
      double dist = std::sqrt(dx*dx + dy*dy);
      double total_dist = std::sqrt(
          std::pow(pt.x - sensor_pos.x(), 2) +
          std::pow(pt.y - sensor_pos.y(), 2));
      if (total_dist < 1e-6) continue;
      double t = dist / total_dist;
      double z_ray = sensor_pos.z() + t * (pt.z - sensor_pos.z());

      // 一致性检查
      float z_hat = map.at("elevation", *it);
      if (!std::isnan(z_hat) && z_hat > z_ray + delta) {
        map.at("variance", *it) += var_increase;
        if (map.at("variance", *it) > 1.0f) {
          map.at("elevation", *it) = NAN;
          map.at("variance", *it) = NAN;
        }
      }
    }
  }
}

性能考量¶

光线投影一致性是**计算密集型**操作——每个观测点需要遍历一条光线上的所有格子。如果有 100K 个点，每条光线经过 50 个格子，则总计 5M 次格子访问。

实现方式	时间/帧	适用场景
CPU 逐点遍历	~50 ms	不可行（30 Hz）
CPU 降采样（1/10 点）	~5 ms	可行，但精度降低
GPU 并行（每点一个 thread）	~2 ms	推荐

⚠️ 常见陷阱¶

⚠️ 编程陷阱：光线起点使用机器人基座而非传感器位置 错误做法：sensor_pos = robot_base_position（基座在地面附近）现象：光线几乎水平，一致性检查误判大量格子根本原因：传感器（深度相机/LiDAR）的位置与机器人基座不同，通常高出 30-50cm 正确做法：使用传感器的精确外参 \(T_{\text{world\_sensor}}\) 计算光线起点

⚠️ 概念误区：认为光线一致性总是正确的 新手想法："如果光线穿过了某个格子，那个格子一定是过时的" 实际上：(1) 里程计漂移导致光线方向不准；(2) 半透明物体（如栅栏）允许光线穿过但仍是障碍；(3) 动态物体导致错误判断。因此不应立即删除被穿过的格子，而是**逐步增加方差**——多次被穿过才重置。

练习¶

[几何推导] 推导：如果传感器高度 \(h = 0.6\) m，观测点距传感器水平距离 \(d = 3\) m，地面高度 0，光线在距传感器水平距离 \(d' = 1.5\) m 处的高度是多少？
[性能优化] 提出两种优化方案并分析其精度影响：(a) 只对点云的 1/10 做光线检查；(b) 只检查距传感器 0.5m 以外的格子（近处不检查）。

过渡：CPU 上的 Kalman 融合和光线一致性检查对于 30 Hz 的感知管线来说太慢了。elevation_mapping_cupy 通过 GPU 并行化解决了这个性能瓶颈。

66.5 elevation_mapping_cupy 的 GPU 架构 ⭐⭐⭐¶

本节解决什么问题：把高程图的所有核心操作搬到 GPU 上，实现 50+ Hz 的实时更新。

动机：CPU 的性能瓶颈¶

操作	CPU 时间	GPU 时间	加速比
点云投影 (100K 点)	~15 ms	~0.5 ms	30x
Kalman 融合	~5 ms	~0.2 ms	25x
光线一致性	~50 ms	~2 ms	25x
Traversability 计算	~10 ms	~0.5 ms	20x
总计	~80 ms (12 Hz)	~3.2 ms (300+ Hz)	25x

elevation_mapping_cupy 由 Miki T., Wellhausen L., Grandia R., Jenelten F., Homberger T., Hutter M. 开发，发表在 IROS 2022 "Elevation Mapping for Locomotion and Navigation using GPU"。项目在 GitHub（leggedrobotics/elevation_mapping_cupy）上积极维护。

架构¶

ROS2 节点（C++）
│ 订阅 /points + /odom
▼
点云预处理（C++, PCL）
│ 变换到世界系（TF2）
│ 距离滤波、体素降采样
▼
Python 主算法（CuPy + CUDA）
│ - 点云投影到 GPU tensor
│ - KF 融合（CUDA kernel）
│ - 光线一致性（CUDA kernel）
│ - 缺失值插值（CuPy NumPy-style）
│ - Traversability 计算（CuPy NumPy-style）
▼
多层结果（CuPy array on GPU）
│ 拷贝到 CPU（~0.5 ms）
▼
grid_map 消息发布（C++, ROS2）

CUDA Kernel 设计模式¶

elevation_mapping_cupy 使用 CuPy 的 RawKernel 编写 CUDA kernels：

# 点云投影 + Kalman 融合的 CUDA kernel（简化示意）
add_points_kernel = cp.RawKernel(r'''
extern "C" __global__
void add_points_with_kf(
    const float* points,     // [N, 3] 点云 (x, y, z)
    const float* sensor_pos, // [3] 传感器位置
    float* elevation,        // [H, W] 高程层
    float* variance,         // [H, W] 方差层
    int N, int H, int W,
    float resolution, float cx, float cy,
    float R0, float k_d)
{
    int idx = blockIdx.x * blockDim.x + threadIdx.x;
    if (idx >= N) return;

    float x = points[idx * 3 + 0];
    float y = points[idx * 3 + 1];
    float z = points[idx * 3 + 2];

    // 世界坐标 -> 栅格索引
    int r = (int)((cx - x) / resolution + H / 2);
    int c = (int)((cy - y) / resolution + W / 2);
    if (r < 0 || r >= H || c < 0 || c >= W) return;

    // 计算观测噪声
    float dx = x - sensor_pos[0];
    float dy = y - sensor_pos[1];
    float dz = z - sensor_pos[2];
    float d = sqrtf(dx*dx + dy*dy + dz*dz);
    float R = R0 + k_d * d * d;

    int cell = r * W + c;
    // 注意：真实实现使用原子操作处理并发写入
    float z_hat = elevation[cell];
    float P = variance[cell];

    if (isnan(z_hat)) {
        atomicExch(&elevation[cell], z);
        atomicExch(&variance[cell], R);
    } else {
        float K = P / (P + R);
        float z_new = z_hat + K * (z - z_hat);
        float P_new = (1.0f - K) * P;
        atomicExch(&elevation[cell], z_new);
        atomicExch(&variance[cell], P_new);
    }
}
''', 'add_points_with_kf')

为什么用 CuPy RawKernel 而不是纯 CuPy NumPy 风格？ CuPy 的 NumPy 风格操作（如 cp.where, cp.mean）对于逐元素操作够用，但点云投影涉及**散布写入**（scatter write——多个点可能写入同一格子），需要原子操作，这只能用 RawKernel 实现。

并发写入的竞争条件¶

问题：多个 CUDA thread 可能同时更新同一个格子（多个点投影到同一位置）。

方案	优点	缺点	适用
原子操作	简单	Kalman 多步不原子	简单覆盖
Bin sorting	精确	排序有开销	Kalman 融合
只取最新值	最快	丢失融合好处	实时性极端要求

实际做法：elevation_mapping_cupy 使用 bin sorting 的变体——先按格子索引对点排序（GPU radix sort），再对每个格子串行融合（每个格子分配一个 thread）。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：GPU 内存不够导致 CuPy 崩溃 错误做法：在 Jetson Orin（8GB 共享内存）上同时运行 elevation_mapping_cupy + RL 推理 + 其他 GPU 任务现象：CuPy 分配内存失败，Python 进程崩溃根本原因：Jetson 的 GPU 和 CPU 共享内存，8GB 中 GPU 可用约 4-5GB 正确做法：监控 GPU 内存使用（nvidia-smi），为 elevation_mapping 预留固定内存池

⚠️ 概念误区：认为 GPU 版本总是比 CPU 快 新手想法："GPU 加速嘛，一定快" 实际上：对于小地图（50x50 格子），GPU 的内核启动开销（kernel launch overhead, ~10 \(\mu\)s）和 CPU-GPU 数据传输开销可能超过计算本身的节省。GPU 加速在大地图（200x200+）和大点云（50K+ 点）时才有显著优势。判断标准：如果 CPU 版本已经能在 10 ms 内完成，不需要 GPU。

⚠️ 编程陷阱：CuPy 和 PyTorch 的 GPU 内存冲突 错误做法：在同一 Python 进程中同时使用 CuPy（elevation mapping）和 PyTorch（RL 推理），不设 CUDA stream 现象：偶发的 GPU 计算错误或性能下降根本原因：CuPy 和 PyTorch 默认使用不同的 CUDA 内存管理器，可能竞争 GPU 资源正确做法：使用 CUDA stream 隔离两者的计算，或分配到不同 GPU（如果有多个）

练习¶

[代码部署] 安装 elevation_mapping_cupy（需要 CuPy + CUDA）。在 Gazebo 仿真中让机器人走路，观察高程图在 GPU 上的实时更新。记录 GPU 利用率和帧率。
[性能对比] 对比 CPU 版的 grid_map + Kalman 融合和 GPU 版的 elevation_mapping_cupy 在 100x100、200x200、400x400 三种尺寸下的更新频率。
[源码阅读] 阅读 elevation_mapping_cupy/script/elevation_mapping.py：(a) 点云如何从 CPU 传到 GPU？(b) Kalman 更新在哪个 kernel 中？(c) 光线一致性检查是否并行化？

到这里,我们已经掌握了高程图的全部核心算法——从 Kalman 融合到光线一致性再到 GPU 并行化。但还有一个架构层面的问题没有回答:为什么 elevation_mapping_cupy 选择用 Python+CuPy 实现,而不是像 legged_control 和 OCS2 那样用纯 C++？这个选择背后的工程取舍,对于你未来设计自己的感知-控制系统至关重要。

elevation_mapping_cupy GPU 架构的深层设计考量 ⭐⭐⭐¶

内存管理策略：elevation_mapping_cupy 在初始化时一次性分配所有 GPU 内存，避免运行时动态分配：

# 初始化时预分配所有 GPU 数组
class ElevationMapping:
    def __init__(self, resolution=0.04, width=200, height=200, n_layers=5):
        # 预分配 5 层高程图（elevation, variance, slope, roughness, traversability）
        self.layers = cp.zeros((n_layers, height, width), dtype=cp.float32)

        # 预分配点云接收缓冲区（最大 100K 点）
        self.point_buffer = cp.zeros((100000, 3), dtype=cp.float32)

        # 预分配中间计算缓冲区
        self.sort_indices = cp.zeros(100000, dtype=cp.int32)
        self.cell_counts = cp.zeros(height * width, dtype=cp.int32)

        # 总 GPU 内存占用：
        # 5层 * 200*200 * 4B = 800 KB
        # 点云 + 缓冲：~2.4 MB
        # 总计：~3.2 MB（对 Jetson 完全可接受）

多流（Multi-Stream）并行：elevation_mapping_cupy 使用多个 CUDA stream 将不同层的计算并行化：

# 多 stream 并行计算示意
stream_kf    = cp.cuda.Stream()   # Kalman 融合
stream_trav  = cp.cuda.Stream()   # Traversability 计算
stream_ray   = cp.cuda.Stream()   # 光线一致性

# Kalman 融合完成后，坡度计算和光线一致性可以并行
with stream_kf:
    self.kalman_update(points)       # ~0.2 ms

stream_kf.synchronize()              # 等 KF 完成

# 以下两个操作并行执行
with stream_trav:
    self.compute_traversability()    # ~0.5 ms（与 ray check 并行）

with stream_ray:
    self.ray_consistency_check()     # ~2 ms（最耗时，但与 trav 并行）

# 总时间 = 0.2 + max(0.5, 2.0) = 2.2 ms（而非串行的 2.7 ms）

grid_map 与 ROS2 集成的完整代码模板：

以下代码展示如何在 ROS2 节点中使用 grid_map 库创建、填充和发布高程图消息：

// ROS2 节点：从点云构建高程图并发布
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <grid_map_ros/GridMapRosConverter.hpp>
#include <grid_map_core/GridMap.hpp>

class ElevationMapNode : public rclcpp::Node {
 public:
  ElevationMapNode() : Node("elevation_map_node") {
    // 初始化 grid_map
    map_ = grid_map::GridMap({"elevation", "variance", "traversability"});
    map_.setGeometry(grid_map::Length(8.0, 8.0),  // 8m x 8m
                     0.04,                         // 4cm 分辨率
                     grid_map::Position(0.0, 0.0));
    map_.setFrameId("odom");

    // 所有层初始化为 NaN（表示"尚未观测"）
    map_["elevation"].setConstant(NAN);
    map_["variance"].setConstant(NAN);

    // 订阅点云
    pc_sub_ = create_subscription<sensor_msgs::msg::PointCloud2>(
        "/depth_camera/points", 10,
        std::bind(&ElevationMapNode::pointCloudCallback, this,
                  std::placeholders::_1));

    // 发布高程图
    map_pub_ = create_publisher<grid_map_msgs::msg::GridMap>(
        "/elevation_map", 1);

    // 定时发布（10 Hz）
    timer_ = create_wall_timer(
        std::chrono::milliseconds(100),
        [this]() {
          auto msg = grid_map::GridMapRosConverter::toMessage(map_);
          map_pub_->publish(std::move(msg));
        });
  }

 private:
  void pointCloudCallback(
      const sensor_msgs::msg::PointCloud2::SharedPtr msg) {
    // 点云 -> 世界坐标系变换 -> Kalman 融合
    // （此处省略变换和融合逻辑，详见 66.3 节）
  }

  grid_map::GridMap map_;
  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr pc_sub_;
  rclcpp::Publisher<grid_map_msgs::msg::GridMap>::SharedPtr map_pub_;
  rclcpp::TimerBase::SharedPtr timer_;
};

点云预处理 Pipeline：在点云投影到高程图之前，需要经过一系列预处理步骤。以下是工程中常用的完整管线：

原始点云（深度相机/LiDAR）
│
├─ 1. 距离滤波：剔除 d < 0.3m 和 d > 5.0m 的点
│     （近处有深度相机盲区，远处噪声大）
│
├─ 2. 体素降采样：0.02-0.05m 分辨率
│     （100K 点 → 10K-20K 点，减少计算量）
│
├─ 3. 统计离群值滤波：k=20 邻居，2σ 剔除
│     （去除飞点——多径反射或传感器噪声导致的孤立点）
│
├─ 4. 地面以上高度滤波：z > z_base - 1.0m
│     （去除地面以下的误检点，如透射反射）
│
├─ 5. 坐标系变换：sensor_frame → world_frame (TF2)
│     （使用状态估计器（Ch57）输出的里程计）
│
└─ 6. 投影到高程图 + Kalman 融合

每一步的耗时（CPU，100K 原始点）：

步骤	PCL 函数	耗时	输出点数
距离滤波	`PassThrough`	~1 ms	~80K
体素降采样	`VoxelGrid`	~3 ms	~15K
离群值滤波	`StatisticalOutlierRemoval`	~5 ms	~14K
高度滤波	`PassThrough`	~0.5 ms	~13K
坐标变换	`transformPointCloud`	~1 ms	~13K
总计		~10.5 ms

在 GPU 版本（elevation_mapping_cupy）中，步骤 1-4 仍在 CPU 上用 PCL 完成（因为 PCL 没有 GPU 版本），步骤 5-6 在 GPU 上完成。CPU 预处理成为瓶颈——如果需要进一步加速，可以用 CUDA 重写距离滤波和体素降采样（Open3D 的 GPU 后端可用于此目的）。

反事实推理：如果不做点云预处理直接投影会怎样？100K 个原始点中包含大量飞点（深度相机的多径反射）和地面以下的误检点。这些点会在高程图上产生"鬼影"——不存在的障碍物凭空出现，导致 MPC 做出不必要的避障动作，甚至导致机器人在平坦地面上也无法正常行走。预处理的 10 ms 开销换来的是高程图数据质量的根本保证。

66.6 C++ / Python 混合架构的设计取舍 ⭐⭐¶

本节解决什么问题：为什么 elevation_mapping_cupy 用 Python 而 legged_control 和 OCS2 用纯 C++？什么时候该用 Python，什么时候该用 C++？

不同模块的实时性需求¶

模块	频率	延迟容忍	推荐语言	原因
WBC	1 kHz	< 0.5 ms	C++	GIL 不可接受
MPC	50-100 Hz	< 10 ms	C++	需要确定性延迟
RL 推理	50-200 Hz	< 2 ms	C++ (LibTorch)	需要确定性延迟
高程图	10-50 Hz	< 20 ms	Python (CuPy)	开发速度优先
训练	离线	无限制	Python (PyTorch)	生态系统优势

Python 用于感知的理由与代价¶

理由： 1. 开发速度：CuPy 的 NumPy-like 接口比 CUDA C++ 开发快 5-10 倍 2. 算法迭代：感知是活跃研究领域，新方法频出，Python 更容易跟进 3. 性能够用：10-50 Hz 的高程图更新，Python GIL 开销不是瓶颈——真正的计算在 GPU 上 4. 深度学习集成：学习型 traversability（CNN 预测可通行性）用 PyTorch 最自然

代价： 1. 部署复杂度：需要在机器人上安装 Python + CuPy + PyTorch（约 2-3 GB 存储） 2. 调试困难：C++ 节点与 Python 节点交互出错时，跨语言栈追踪难以阅读 3. 内存开销：Python 进程本身约 100-500 MB 4. GIL 限制：纯 Python 的多线程受 GIL 限制

通信方式对比¶

方式	延迟	零拷贝	跨进程	复杂度
ROS topic	~0.5-1 ms	否（序列化）	是	低
pybind11	~0.01 ms	是	否（进程内）	中
Shared Memory (DDS)	~0.1 ms	是	是	高
CUDA IPC	~0.05 ms	是（GPU）	是	高

⚠️ 常见陷阱¶

⚠️ 思维陷阱：认为应该全部用 C++ 以追求性能 新手想法："既然 C++ 更快，为什么不把所有东西都用 C++ 写？" 实际上：开发时间是最稀缺的资源。用 C++ 写一个 GPU 高程图模块可能需要 2-3 个月，用 Python+CuPy 只需要 2-3 周。如果性能已经够用（30 Hz），多花 2 个月换取 300 Hz 没有意义。决策原则：先用 Python 原型验证，确认是性能瓶颈后再用 C++ 重写。

⚠️ 编程陷阱：Python 和 C++ 节点的时间同步 错误做法：C++ 订阅点云后通过 ROS topic 发给 Python 处理，但没有保留原始时间戳现象：高程图更新时使用的里程计是"现在"的，但点云是"50ms 前"的——时间不匹配导致投影偏移正确做法：Python 节点在接收点云时，用点云消息头中的 header.stamp 通过 TF2 的 lookupTransform 查询对应时刻的里程计

练习¶

[设计题] 如果要把 elevation_mapping_cupy 从 Python 重写为纯 C++（CUDA），需要替换哪些 CuPy 功能？估计工程量（人月）。
[分析题] 对于 Jetson Orin NX（8 核 ARM，GPU 1024 CUDA 核，8GB 共享内存），设计 C++/Python 混合架构的内存预算分配方案——WBC、MPC、RL 推理、elevation_mapping 各分多少？
[跨章综合] 综合 Ch38（CUDA+LibTorch）和本章，设计一个方案使 elevation_mapping_cupy 和 RL 推理（Ch64 LibTorch）在同一 GPU 上共存而不冲突。需要考虑：CUDA stream 隔离、内存预算分配、错误恢复策略。

工程实践：完整的 ROS2 感知管线部署清单 ⭐⭐¶

部署一个完整的感知管线涉及多个 ROS2 节点的协调。以下清单覆盖了从硬件驱动到 MPC 消费的全栈：

ROS2 Humble/Iron 完整感知管线

节点 1: 深度相机驱动 (realsense2_camera_node)
  发布: /camera/depth/color/points (PointCloud2, 30 Hz)
  参数: depth_module.profile=640x480x30

节点 2: 点云预处理 (pcl_preprocess_node) [C++]
  订阅: /camera/depth/color/points
  发布: /filtered_points (PointCloud2, 30 Hz)
  处理: 距离滤波 + 体素降采样 + 离群值去除

节点 3: 高程图 (elevation_mapping_node) [Python/CuPy]
  订阅: /filtered_points, /odom
  发布: /elevation_map (grid_map_msgs/GridMap, 20 Hz)
  处理: KF 融合 + 光线一致性 + traversability

节点 4: MPC 消费端 (perceptive_mpc_node) [C++]
  订阅: /elevation_map, /joint_states, /imu
  发布: /mpc_command (自定义消息, 50 Hz)
  处理: 高程图查询 + SDF 计算 + NMPC 求解

QoS 配置要点：

Topic	Reliability	Durability	History	原因
点云	BEST_EFFORT	VOLATILE	KEEP_LAST(1)	丢帧可接受，不要积压
高程图	RELIABLE	VOLATILE	KEEP_LAST(1)	MPC 必须收到最新地图
里程计	BEST_EFFORT	VOLATILE	KEEP_LAST(1)	高频，丢帧可接受

上面几节解决了"如何高效构建高程图"的问题——从 CPU 上的 grid_map 到 GPU 上的 elevation_mapping_cupy,从 Kalman 融合到光线一致性检查。但高程图本身只是中间产品——它存储的是原始几何信息(每个格子的高度),而下游的 MPC 和落脚规划需要的是"这里能不能走"的语义判断。下一节正是从几何到语义的桥梁:traversability 计算。

66.7 Traversability 的多层次计算 ⭐⭐¶

本节解决什么问题：高程图存储了地面的几何信息，但 MPC 和落脚规划需要的是"这里能不能走"的语义信息。Traversability 把几何特征转化为 0-1 的可通行性评分。

动机：MPC 不直接用高程图¶

Ch67（Perceptive MPC）会讲如何把感知信息嵌入 MPC。MPC 需要的不是原始高程值，而是"这里能不能安全踩""这个方向能不能走"的判断。Traversability 是从几何到语义的桥梁。

Traversability 的数学定义¶

💡 地形可通行性的 RL 定义（Miki et al., 2022）

在 RL 训练中,可通行性 (Traversability) \(Tr \in [0, 1]\) 有一个基于策略本身能力的数学定义,不同于下面基于几何特征的工程定义。Miki et al. (2022) 首先定义了单步标签函数 \(v\):

\[v(s_t, a_t, s_{t+1}) = \begin{cases} 1 & \text{如果 } v_{pr}(s_{t+1}) > 0.2 \\ 0 & \text{如果 } v_{pr}(s_{t+1}) < 0.2 \lor \text{终止} \end{cases}\]

其中 \(v_{pr}(s_{t+1})\) 是时间步 \(t+1\) 时机器人基座速度与指令方向的内积。阈值 \(0.2\) m/s 约为机器人最大速度的三分之一。然后可通行性定义为策略 \(\pi\) 在给定地形配置 \(c_T\) 下的期望通过率:

\[Tr(c_T, \pi) = \mathbb{E}_{\xi \sim \pi} \{v(s_t, a_t, s_{t+1} \mid c_T)\} \in [0, 1]\]

直觉上,可通行性就是当前策略在该地形上能否沿指令方向持续产生有效前进的概率。地形生成方法的目标是寻找具有中等可通行性 (\(Tr \in [0.5, 0.9]\)) 的地形——"既不太容易也不太困难"。地形"期望度(desirability)"定义为:

\[Td(c_T, \pi) := \Pr(Tr(c_T, \pi) \in [0.5, 0.9]) = \mathbb{E}_{\xi \sim \pi} \{\mathbb{I}(Tr(c_T, \pi) \in [0.5, 0.9])\}\]

在训练中,这个定义用于 curriculum learning——控制地形采样的难度分布,通过粒子滤波维持一条"刚好能学"的难度前沿,使策略从简单地形逐步过渡到复杂地形。与下面基于几何特征的 traversability 计算不同,这个定义是**策略相关的**——同一地形对不同策略的可通行性不同,这使得 curriculum 能自适应地跟随策略能力的增长。

给定高程图中一个格子 \((r, c)\)，其 traversability \(\tau(r,c) \in [0, 1]\) 定义为多个地形特征的综合评分。

特征 1：坡度（Slope）

坡度衡量局部地面的倾斜程度。用中心差分近似梯度：

\[\frac{\partial z}{\partial x} \approx \frac{z(r+1, c) - z(r-1, c)}{2 \Delta x}\]

\[\frac{\partial z}{\partial y} \approx \frac{z(r, c+1) - z(r, c-1)}{2 \Delta y}\]

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

// 正确：使用 atan 得到弧度角度
float dz_dx = (elev[r+1][c] - elev[r-1][c]) / (2 * res);
float dz_dy = (elev[r][c+1] - elev[r][c-1]) / (2 * res);
float slope = std::atan(std::sqrt(dz_dx*dz_dx + dz_dy*dz_dy));

// 错误：只用 sqrt 得到梯度大小，没有 atan
// float slope = std::sqrt(dz_dx*dz_dx + dz_dy*dz_dy);
// 这不是角度！是梯度的模，单位是 m/m，不是 rad
// 当梯度 = 1 时，实际坡度是 45 度，但这个值只是 1.0

特征 2：粗糙度（Roughness）

粗糙度衡量局部地面的不平整程度。在一个 \(w \times w\) 窗口内计算高度的标准差：

\[\text{roughness}(r, c) = \sqrt{\frac{1}{|\mathcal{W}|} \sum_{(i,j) \in \mathcal{W}} (z(i,j) - \bar{z})^2}\]

其中 \(\bar{z}\) 是窗口内的平均高度，\(\mathcal{W}\) 是以 \((r,c)\) 为中心的 \(w \times w\) 窗口。

// 计算 5x5 窗口内的粗糙度
constexpr int w = 5;
float sum = 0, sum_sq = 0;
int count = 0;
for (int di = -w/2; di <= w/2; ++di) {
  for (int dj = -w/2; dj <= w/2; ++dj) {
    int ri = r + di, ci = c + dj;
    if (ri < 0 || ri >= H || ci < 0 || ci >= W) continue;
    float z = elev[ri][ci];
    if (!std::isnan(z)) {
      sum += z;
      sum_sq += z * z;
      count++;
    }
  }
}
if (count < 3) return NAN;  // 观测太少
float mean = sum / count;
float roughness = std::sqrt(sum_sq / count - mean * mean);

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

台阶高度衡量相邻格子之间的最大高度差——四足机器人的腿能跨越的台阶有限（通常 5-15 cm）：

\[\text{step}(r, c) = \max_{(i,j) \in \mathcal{N}_8} |z(r,c) - z(i,j)|\]

其中 \(\mathcal{N}_8\) 是 \((r,c)\) 的 8 邻域。

综合 Traversability¶

将多个特征组合为综合评分。两种常见方法：

方法 A：乘性组合（保守，推荐）

\[\tau(r,c) = \underbrace{\max(0, 1 - \text{slope}/\theta_{\max})}_{\text{坡度因子}} \cdot \underbrace{\max(0, 1 - \text{roughness}/\sigma_{\max})}_{\text{粗糙度因子}} \cdot \underbrace{\mathbb{1}[\text{step} < h_{\max}]}_{\text{台阶因子}}\]

方法 B：加权线性组合（宽松）

\[\tau(r,c) = w_s (1 - \text{slope}/\theta_{\max}) + w_r (1 - \text{roughness}/\sigma_{\max}) + w_h \mathbb{1}[\text{step} < h_{\max}]\]

方法 A vs 方法 B：乘性组合是保守的——任何一个因子为 0 则总评分为 0（"一票否决"）。线性组合更宽松——坡度大但粗糙度小时仍可能通过。工程实践中通常用**乘性组合**，因为安全性更重要。

本质洞察：Traversability 的计算本质上是将**连续的物理世界**离散化为**机器人能力的二值判断**——"能不能过"。所有特征（坡度、粗糙度、台阶高度）都是在回答同一个问题：当前地形的物理约束是否在机器人的摩擦锥、关节力矩和运动学范围之内。这意味着 traversability 的阈值参数（\(\theta_{\max}\)、\(\sigma_{\max}\)、\(h_{\max}\)）不是任意设定的，而是由机器人的物理能力决定的——换一个机器人，所有阈值都要重新标定。这也解释了为什么学习型方法（如自监督 traversability）更有前途：它可以从机器人自身的行走经验中自动学到能力边界，而不依赖人工设定的物理阈值。

跨领域类比：乘性 vs 加性组合的选择,与机器学习中 precision vs recall 的权衡类似。乘性组合追求高 precision(宁可错过可通行区域,也不要误判不可通行区域为可通行),因为在安全关键场景中 false positive(把危险区域判为安全)的代价远大于 false negative(把安全区域判为危险)。

如果不用乘性组合会怎样?假设某区域坡度 40 度(接近 Go2 极限)但粗糙度为 0(光滑表面)。加权线性组合可能给出 \(\tau = 0.6 \times 0.0 + 0.4 \times 1.0 = 0.4\)——看起来"勉强可通行"。但实际上 40 度的光滑坡面恰恰是最危险的——机器人会直接滑下去。乘性组合给出 \(\tau = 0.0 \times 1.0 = 0.0\),正确地拒绝了这个区域。

不同机器人的典型 Traversability 阈值：

参数	Go2（小型四足）	ANYmal C（中型）	Atlas/人形	含义
\(\theta_{\max}\) (最大坡度)	35 度	45 度	25 度	超过此坡度不可通行
\(\sigma_{\max}\) (最大粗糙度)	0.06 m	0.08 m	0.04 m	超过此粗糙度不可通行
\(h_{\max}\) (最大台阶高度)	0.10 m	0.15 m	0.20 m	超过此高度不可跨越
窗口大小 \(w\)	5 (0.20m)	7 (0.28m)	5 (0.20m)	取决于足端尺寸

这些阈值必须通过真机实验标定——在多种地形上让机器人行走，记录成功/失败的地形特征，用分类器找到最优阈值边界。仅靠理论推算（如"摩擦锥角度 = arctan(mu)"）是不够的，因为真实行走涉及动态平衡、足端打滑、柔性关节等理论模型未覆盖的因素。

Traversability 的时间一致性问题：高程图是随时间积累的，但机器人自身在移动，导致高程图中不同区域的"新鲜度"不同——近处是最新观测，远处可能是数秒前的旧数据。如果环境是动态的（如有行人、移动障碍物），旧数据区域的 traversability 可能已过时。解决方案包括：

时间衰减：给 traversability 值乘以时间衰减因子 \(\tau \cdot \exp(-\alpha \Delta t)\)，长时间未更新的区域自动变为"未知"
置信度层：在 grid_map 中添加一层"观测新鲜度"，MPC 只信任新鲜度高于阈值的格子
保守策略：对超过 2 秒未更新的区域，直接标记为不可通行

参数	含义	Go2 典型值	ANYmal 典型值
\(\theta_{\max}\)	最大可通行坡度	30 度	35 度
\(\sigma_{\max}\)	最大可通行粗糙度	0.05 m	0.08 m
\(h_{\max}\)	最大可通行台阶	0.08 m	0.15 m

学习驱动的 Traversability¶

除了手工特征计算，还可以用机器学习从感知数据中预测 traversability：

Wellhausen L., Dosovitskiy A., Ranftl R., Walas K., Cadena C., Hutter M. (RA-L 2019, Vol. 4(2), pp. 1509-1516) "Where Should I Walk? Predicting Terrain Properties from Images via Self-Supervised Learning"——用 CNN 从 RGB 图像预测地面的材质属性（摩擦系数、可通行性），训练标签来自机器人实际行走时的力传感器数据（自监督学习）。

优势：可以利用视觉语义信息（"这是草地""这是冰面"）——几何特征无法区分视觉上不同但高度相同的表面（冰面和水泥地都是平坦光滑的，但摩擦系数差异巨大）。

局限：需要大量训练数据，且不同机器人/地形的训练数据不可复用。

Traversability 在下游的用途¶

下游模块	使用方式	参考章节
落脚规划	在 \(\tau >\) 阈值的区域选择落脚点	Ch60
Perceptive MPC	\(\tau\) 作为 MPC 代价函数的权重	Ch67
全局路径规划	\(\tau\) 作为 costmap 的代价	导航栈
行为选择	低 \(\tau\) 区域触发慢速/谨慎步态	行为状态机

⚠️ 常见陷阱¶

⚠️ 编程陷阱：计算坡度时忽略 NaN 邻居 错误做法：dz_dx = (elev[r+1][c] - elev[r-1][c]) / (2*res) 不检查 NaN 现象：一个 NaN 邻居导致整个坡度计算变成 NaN，进而 traversability 也变成 NaN 根本原因：高程图中有未观测的格子（NaN）正确做法：检查所有参与计算的格子是否为 NaN，如果是则用单侧差分或跳过该格子

⚠️ 概念误区：认为 traversability 是二值的（能走/不能走） 新手想法："traversability > 0.5 就能走，否则不能走" 实际上：traversability 是连续值，应该作为**代价**而非**约束**传给 MPC。0.7 的区域可以走但代价高（走慢一点），0.3 的区域应该避免但紧急时可以走。二值化丢失了渐变信息。

⚠️ 思维陷阱：只依赖几何特征做 traversability 新手想法："有了坡度和粗糙度就够了" 实际上：冰面和水泥地的几何特征完全相同，但摩擦系数差异巨大。视觉/语义信息对 traversability 至关重要——这是学习型方法（Wellhausen 2019）的核心优势。

练习¶

[代码实践] 在 grid_map 上实现 slope、roughness、step_height 三层的计算，并用乘性组合生成 traversability 层。在 RViz 中用颜色映射显示（绿色=可通行，红色=不可通行）。
[参数分析] 改变 \(\theta_{\max}\) 从 20 度到 45 度，观察 traversability 地图的变化。Go2（腿短，约 20cm）和 ANYmal（腿长，约 40cm）的 \(\theta_{\max}\) 应该如何不同？
[对比题] 乘性组合和加权线性组合在以下地形上哪个更合适：(a) 大坡度但光滑的水泥坡道；(b) 小坡度但极粗糙的碎石地。

Traversability 为下游模块提供了"能不能走"的判断,但到目前为止我们假设的都是固定分辨率、零延迟的理想高程图。真实部署中还有两个绕不开的工程问题:第一,固定分辨率在远处浪费算力、在近处不够精确;第二,从相机拍照到高程图更新完成有 25-50 ms 的感知延迟,高速运动时这个延迟会导致地形信息过时。下一节正是解决这两个问题的进阶话题。

66.8 多分辨率高程图与感知延迟补偿 ⭐⭐⭐¶

本节解决什么问题：固定分辨率的高程图面临"近处不够精确、远处计算浪费"的矛盾。感知延迟在高速运动时会导致地形信息过时。

多分辨率高程图¶

问题：0.04m 分辨率覆盖 8m x 8m = 200x200 格子。如果要覆盖 20m（用于长距离规划），格子数增加到 500x500 = 250K——计算量大增。

思路：机器人附近需要高分辨率（精确落脚），远处只需要低分辨率（路径规划）。

┌─────────────────────────────┐
│                             │
│     低分辨率 (0.2m)          │
│     远距离规划                │
│   ┌─────────────────────┐   │
│   │                     │   │
│   │  中分辨率 (0.08m)    │   │
│   │  路径规划             │   │
│   │ ┌─────────────────┐ │   │
│   │ │                 │ │   │
│   │ │ 高分辨率 (0.04m) │ │   │
│   │ │ 落脚规划         │ │   │
│   │ │    [robot]      │ │   │
│   │ └─────────────────┘ │   │
│   └─────────────────────┘   │
└─────────────────────────────┘

层级	分辨率	范围	格子数	用途
近场	0.04 m	4m x 4m	100x100	落脚规划
中场	0.08 m	8m x 8m	100x100	路径规划
远场	0.20 m	20m x 20m	100x100	全局规划

总格子数 30K——与单分辨率 200x200 = 40K 相当，但覆盖范围从 8m 扩大到 20m。

感知延迟补偿¶

问题：从深度相机拍照到高程图更新完成，通常有 25-50 ms 的延迟：

环节	延迟
相机曝光 + 读出	~10 ms
USB/以太网传输	~5 ms
点云预处理	~5 ms
GPU 融合 + traversability	~3 ms
ROS 消息发布	~2 ms
总计	~25-30 ms

影响分析：

场景	速度	30ms 延迟位移	影响
慢速 trot	0.5 m/s	1.5 cm	可忽略
正常行走	1.0 m/s	3 cm	接近分辨率
快速跑	2.0 m/s	6 cm	不可忽略
Parkour 跳跃	3.0 m/s	9 cm	严重偏移

补偿方案：

\[\text{compensated\_position} = \text{map\_position} + v_{\text{robot}} \cdot \Delta t_{\text{delay}}\]

在查询高程图时，用机器人的当前速度和已知的延迟量，预测"当感知数据被获取时，机器人在哪"，然后用那个位置做查询。

感知与状态估计的耦合¶

高程图依赖精确的机器人位姿——点云投影需要知道传感器在世界系中的位置和朝向。这个位姿来自 Ch57 的状态估计器。

数据流：

LiDAR / 深度相机
    |
    v
Ch57 InEKF / SLAM -> 位姿 T_world_base
    |                    |
    |                    v
    |              Ch66 Elevation Mapping
    |               | 用 T_world_base + T_base_sensor 投影点云
    |               v
    |              高程图 + traversability
    |               |
    |               v
    |              Ch67 Perceptive MPC（消费方）

关键依赖：如果状态估计漂移，高程图会出现不一致——同一物体在不同帧中被投影到不同位置，导致"重影"效果。Ch57 的高精度状态估计是高程图质量的前提。

⚠️ 常见陷阱¶

⚠️ 概念误区：认为感知延迟不重要 新手想法："30 ms 很短，不会有什么影响" 实际上：30 ms 对于 1 kHz 的 WBC 是 30 个控制周期。在快速运动时，过时的地形信息可能导致落脚偏差 3-9 cm——超过一个高程图格子的大小。经验法则：如果速度(m/s) x 延迟(s) > 分辨率 x 0.5，就需要延迟补偿。

⚠️ 思维陷阱：认为多分辨率地图更复杂所以不值得 实际上：多分辨率地图的实现复杂度适中（三个独立 grid_map + 距离判断），但覆盖范围提升 2.5 倍而计算量不增加。对于需要同时做落脚规划和路径规划的系统，多分辨率几乎是必须的。

练习¶

[设计题] 设计一个三级多分辨率高程图系统。定义每级的分辨率、范围、更新频率。分析总内存开销和总计算量。
[数学分析] 推导感知延迟补偿的残余误差：如果速度估计有 5% 的误差，延迟估计有 5 ms 的误差，在 2 m/s 行走时，补偿后的残余位移误差是多少？
[实验设计] 设计实验验证感知延迟对控制质量的影响：人为地在高程图查询中加入 0, 20, 50, 100 ms 的延迟，记录机器人走过台阶时的跟踪误差和摔倒率。

常见故障与排查¶

症状	可能原因	排查步骤	相关小节
高程图更新延迟明显（帧率 < 10 Hz），RViz 中地图"卡顿"	CPU 版 Kalman 融合 + 光线一致性耗时过长，或点云未降采样导致处理量过大	1. 用 `ros2 topic hz /elevation_map` 确认发布频率 2. 对点云做体素降采样（0.02-0.05m）3. 考虑切换到 GPU 版 elevation_mapping_cupy	66.3, 66.5
点云配准失败——高程图出现"重影"或"模糊"（同一物体在两个位置各有一份）	里程计漂移导致不同帧的点云投影到不同栅格位置，或 TF 时间戳不同步	1. 检查里程计精度（在已知平面上测量漂移量）2. 确认点云消息的 `header.stamp` 与 TF 查询时间一致 3. 降低地图范围减少漂移影响	66.3, 66.8
地形分类误判——平坦地面被标记为不可通行，或危险区域被标记为可通行	traversability 参数（\(\theta_{\max}\), \(\sigma_{\max}\), \(h_{\max}\)）未针对目标机器人标定，或高程图噪声导致坡度计算异常	1. 在已知平面上检查 slope 层是否接近 0 2. 调整 traversability 参数匹配机器人能力 3. 增大 Kalman 融合时间让方差收敛后再计算 traversability	66.7
GPU 内存溢出——elevation_mapping_cupy 的 Python 进程崩溃	Jetson 共享内存不足，同时运行 RL 推理 + elevation mapping + 其他 GPU 任务	1. 用 `nvidia-smi` 或 `tegrastats` 监控 GPU 内存 2. 减小高程图分辨率或范围 3. 为 CuPy 设置固定内存池 `cp.cuda.set_allocator(cp.cuda.MemoryPool().malloc)`	66.5
circular buffer 移动后地图数据错乱——RViz 中地图"闪烁"或出现不连续	直接访问底层 Eigen 矩阵而未经过 `getIndex()` 坐标转换，导致 startIndex 偏移未生效	1. 排查代码中是否有 `map["layer"](row, col)` 的直接访问 2. 全部改为 `map.at("layer", index)` 3. 在地图移动前后打印 `startIndex_` 确认偏移正确	66.1

本章小结¶

知识点	核心内容	难度	关键公式/概念
grid_map 架构	多层栅格 + circular buffer	⭐⭐	`startIndex_` 偏移
滤波器链	YAML 配置的模块化算子	⭐⭐	`FilterChain::update()`
Kalman 融合	贝叶斯最优加权	⭐⭐⭐	\(K = P/(P+R)\)
光线一致性	检测过时数据	⭐⭐⭐	\(\hat{z}_g > z_{\text{ray}}(g)\)
GPU 高程图	CuPy RawKernel 并行化	⭐⭐⭐	原子操作 + bin sort
C++/Python 混合	性能 vs 开发速度	⭐⭐	控制 C++ / 感知 Python
Traversability	坡度 + 粗糙度 + 台阶	⭐⭐	乘性组合 \(\tau = \prod f_i\)
多分辨率	近场精细 + 远场粗略	⭐⭐⭐	三级地图
延迟补偿	速度 x 延迟预测	⭐⭐⭐	\(\Delta p = v \cdot \Delta t\)

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

累积项目进度:
  Ch55: OCS2 基础 MPC（盲）
  Ch56: 步态管理器
  Ch57: 状态估计
  Ch58: 落脚规划（Raibert）
  Ch60: 高程图 + 可踩性分析（概念层）
  Ch65: Residual RL 模块
  ────────────────────────────────
  Ch66: [新增] 感知数据结构模块
    - grid_map 初始化（8m x 8m, 0.04m 分辨率）
    - 5 层：elevation, variance, slope, roughness, traversability
    - Kalman 融合（订阅深度相机点云）
    - 光线一致性检查
    - Traversability 计算（乘性组合）
    - ROS2 发布到 /elevation_map topic
  ────────────────────────────────
  Ch67: [下一章] Perceptive MPC（消费高程图）

本章实操目标：在 Gazebo 仿真中部署完整感知管线——深度相机 -> 点云 -> Kalman 融合 -> 高程图 -> traversability -> RViz 可视化。对比开/关 Kalman 融合、开/关光线一致性的效果差异。

延伸阅读¶

必读经典¶

文献	类型	难度	核心贡献
Fankhauser P., Bloesch M., Gehring C., Hutter M., Siegwart R. (2014) "Robot-Centric Elevation Mapping with Uncertainty Estimates" -- CLAWAR	论文	⭐⭐	高程图 + Kalman 融合的奠基工作
Fankhauser P., Hutter M. (2016) "A Universal Grid Map Library" -- Springer	书章	⭐⭐	grid_map 库的设计文档

进阶¶

文献	类型	难度	核心贡献
Wellhausen L., Dosovitskiy A., Ranftl R., Walas K., Cadena C., Hutter M. (2019) "Where Should I Walk?" -- RA-L, Vol. 4(2), pp. 1509-1516	论文	⭐⭐⭐	学习型 traversability
Miki T., Wellhausen L., Grandia R., Jenelten F., Homberger T., Hutter M. (2022) "Elevation Mapping for Locomotion and Navigation using GPU" -- IROS	论文	⭐⭐⭐	elevation_mapping_cupy
Jenelten F., Grandia R., Farshidian F., Hutter M. (2022) "TAMOLS: Terrain-Aware Motion Optimization for Legged Systems" -- T-RO, Vol. 38, pp. 3395-3413	论文	⭐⭐⭐	MPC + traversability 完整集成

前沿¶

文献	类型	难度	核心贡献
Grandia R., et al. (2023) "Perceptive Locomotion Through Nonlinear MPC" -- T-RO	论文	⭐⭐⭐	感知嵌入 MPC 管线
grid_map GitHub (github.com/ANYbotics/grid_map)	代码	⭐⭐	官方仓库，含 ROS2 分支
elevation_mapping_cupy GitHub (github.com/leggedrobotics/elevation_mapping_cupy)	代码	⭐⭐⭐	GPU 高程图官方仓库

预计学习时间¶

1.5 周（20-25 小时）：

内容	时间	输出
66.1 grid_map 架构 + API	4-5 小时	能创建和操作 grid_map
66.2 滤波器链	2-3 小时	能配置 YAML 滤波器
66.3 Kalman 融合推导+实现	4-5 小时	能实现点云到高程图的 KF 融合
66.4 光线一致性	2-3 小时	理解几何原理和实现
66.5 elevation_mapping_cupy	3-4 小时	能在仿真中部署 GPU 高程图
66.6 C++/Python 混合	1-2 小时	理解架构取舍
66.7 Traversability 计算	2-3 小时	能计算三种地形特征
66.8 多分辨率 + 延迟 + 练习	2-3 小时	理解进阶话题

第 66 章 腿足感知数据结构——Grid Map / Elevation Mapping / Traversability 工程¶

前置自测¶

本章目标¶

66.1 grid_map 库架构深入 ⭐⭐¶

动机：为什么不直接用点云？¶

如果不用 2.5D 会怎样¶

历史：grid_map 的演变¶

grid_map 核心数据结构¶

Circular Buffer：滚动地图的实现¶

迭代器设计¶

ROS2 支持现状¶

性能考虑¶

⚠️ 常见陷阱¶

练习¶

66.2 grid_map 的主要算子与滤波器链 ⭐⭐¶

从点云到高程图¶

滤波器链¶

常用滤波器¶

转为 ROS 消息¶

⚠️ 常见陷阱¶

练习¶

66.3 Elevation Mapping 的 Kalman 融合 ⭐⭐⭐¶

动机：为什么不能直接覆盖？¶

从贝叶斯角度推导 Kalman 融合¶

观测噪声模型¶

完整融合代码¶

时间衰减：处理久未观测的格子¶

⚠️ 常见陷阱¶

练习¶

66.4 光线投影一致性检查 ⭐⭐⭐¶

动机：消失的障碍物¶

几何原理¶

数学推导¶

实现¶

性能考量¶

⚠️ 常见陷阱¶

练习¶

66.5 elevation_mapping_cupy 的 GPU 架构 ⭐⭐⭐¶

动机：CPU 的性能瓶颈¶

架构¶

CUDA Kernel 设计模式¶

并发写入的竞争条件¶

⚠️ 常见陷阱¶

练习¶

elevation_mapping_cupy GPU 架构的深层设计考量 ⭐⭐⭐¶

66.6 C++ / Python 混合架构的设计取舍 ⭐⭐¶

不同模块的实时性需求¶

Python 用于感知的理由与代价¶

通信方式对比¶

⚠️ 常见陷阱¶

练习¶

工程实践：完整的 ROS2 感知管线部署清单 ⭐⭐¶

66.7 Traversability 的多层次计算 ⭐⭐¶

动机：MPC 不直接用高程图¶

Traversability 的数学定义¶

综合 Traversability¶

学习驱动的 Traversability¶

Traversability 在下游的用途¶

⚠️ 常见陷阱¶

练习¶

66.8 多分辨率高程图与感知延迟补偿 ⭐⭐⭐¶

多分辨率高程图¶

感知延迟补偿¶

感知与状态估计的耦合¶

⚠️ 常见陷阱¶

练习¶

常见故障与排查¶

本章小结¶

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

延伸阅读¶

必读经典¶

进阶¶

前沿¶

预计学习时间¶

第 66 章腿足感知数据结构——Grid Map / Elevation Mapping / Traversability 工程¶