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

第 67 章：Perceptive MPC——让 MPC "长眼睛"的地形感知控制¶

难度：⭐⭐⭐ | 预计学时：25-30 小时 | 前置：足式/110_OCS2完整栈与双线程MPC（OCS2 完整栈）、足式/160_感知驱动落脚规划（感知驱动落脚规划）

一句话概要：Perceptive MPC 把高程图、SDF、地形法向量等感知信息嵌入 OCS2 的代价函数和约束中，使 NMPC 能在线生成避障、适应地形的全自由度运动——这是 ETH RSL 在 ANYmal 上实现鲁棒崎岖地形行走的核心技术，也是经典 MPC 与感知融合的标杆工作（Grandia et al. T-RO 2023）。

前置自测¶

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

[足式/110_OCS2完整栈与双线程MPC] OCS2 的 SQP 求解器如何处理不等式约束？Real-Time Iteration（RTI）只做 1 次 SQP 迭代的前提是什么？
[足式/110_OCS2完整栈与双线程MPC] OCS2 的双线程架构中，MPC 线程和 MRT 线程如何通过 Triple Buffer 通信？为什么这样设计？
[足式/160_感知驱动落脚规划] 高程图（Elevation Map）的分辨率、更新频率和查询方式各是什么？为什么高程图适合嵌入 MPC？
[足式/160_感知驱动落脚规划] SDF（Signed Distance Field）的正负值分别代表什么？为什么 SDF 的梯度天然可用于优化？
[足式/40_CppAD与代码生成] CppAD 的 AD<double> 和 CppADCodeGen 各解决什么问题？为什么 MPC 代价函数需要自动微分？

本章目标¶

学完本章，你应能：

理解盲 MPC 在崎岖地形上的系统性失效模式，以及 Perceptive MPC 如何通过感知信息弥补
掌握 Grandia 2023 T-RO 的完整管线：传感器 --> 高程图 --> 可踩性分类 --> 平面分割 --> SDF --> MPC 约束/代价 --> WBC
能从数学上推导 SDF 碰撞约束如何经一阶线性化变成 MPC 可处理的不等式约束，以及梯度如何计算
理解地形感知代价函数的设计：躯体高度跟踪、躯体避障、摆动腿地形回避
能读懂 OCS2 ocs2_perceptive 模块的真实接口和本章教学抽象之间的对应关系，并知道如何添加自定义感知约束
能进行 RL vs MPC 感知运动方案的系统性对比（Miki 2022 vs Grandia 2023）
掌握部署实践中的延迟预算、降级策略、计算资源分配

论文-代码映射表（总览）¶

本章是**论文解读型**教学章，核心论文是 Grandia et al. 2023 (T-RO, arXiv:2208.08373)，对照代码是 leggedrobotics/ocs2 的 ocs2_perceptive 模块及 ANYmal perceptive 示例。下表把论文的方法论各部分映射到代码模块，并标注**清晰度级别**（SPECIFIED = 论文明确且与代码一致；PARTIAL = 论文提及但细节须从代码补全；UNSPECIFIED = 论文未提及但代码存在；CONFLICT = 论文与代码/本章简化叙述不一致）。所有 PARTIAL/UNSPECIFIED/CONFLICT 项在正文对应小节展开，并汇总于章末"歧义审计汇总表"。

论文部分	核心内容	对应代码	清晰度	本章小节
§III 系统综述	感知-规划-控制三层管线	ANYmal perceptive 示例（多包）	SPECIFIED	67.2
§IV-A 可踩性分类	坡度/粗糙度/台阶阈值	`convex_plane_decomposition`（独立仓库）	PARTIAL（阈值从配置提取）	67.3
§IV-B 平面分割	连通域 + 凸分解	`convex_plane_decomposition`	PARTIAL	67.3
§IV-C SDF 预计算	从分割平面构建 3D SDF	`SegmentedPlanesSignedDistanceField`、`ComputeDistanceTransform.h`	PARTIAL	67.4, 67.7
§V-A 落脚可行性约束	凸不等式（半空间）序列	OCP 约束装配（示例 `*Interface.cpp`）	SPECIFIED	67.3
§V-B 碰撞回避约束	末端-地形距离约束	`EndEffectorDistanceConstraint(CppAd).h/.cpp`	SPECIFIED	67.4, 67.7
§V-C 地形代价	躯体高度跟踪等	代价装配（示例代码）	PARTIAL（权重从 `.info` 提取）	67.5
§V 地图查询可微化	双线性插值的值与梯度	`interpolation/BilinearInterpolation.h`	SPECIFIED	67.4, 67.7
§VI 求解器	SQP + HPIPM，RTI	`ocs2_sqp`、`ocs2_qp_solver`（见 110 章）	SPECIFIED	67.7, 67.9
§VII 实验	gaps/slopes/stepping stones	无对应开源复现脚本	UNSPECIFIED	67.8 数据
全章"2D SDF"叙述	本章降维简化	接口实为 3D（`vector3_t`）	CONFLICT	67.4, 67.7

使用方法：读到任一小节遇到"论文-代码差异"或"论文没告诉你的"标注时，可回到本表定位该项的清晰度级别与代码出处。convex_plane_decomposition 与高程图、grid_map_sdf 等是 ANYmal perceptive 示例引入的**外部工程包**，不全在 ocs2_perceptive 目录内——这点本身就是初次读 OCS2 perceptive 容易踩的坑（详见 67.7）。

67.1 从盲 MPC 到感知 MPC ⭐¶

本节解决什么问题：建立"为什么需要把感知嵌入 MPC"的动机——不是因为技术上能做，而是因为盲 MPC 在真实崎岖地形上会系统性地失效。

动机：盲 MPC 在崎岖地形上的三重失效¶

足式/110_OCS2完整栈与双线程MPC 讲了 OCS2 的 NMPC 框架如何在**平地**上为四足机器人生成流畅的全身运动。在实验室环境下，这套控制器表现优异——MPC 以 100 Hz 求解，WBC 以 400 Hz 执行，ANYmal 可以稳健地 trot、pace、甚至跑步。

但当 ANYmal 走出实验室，面对碎石坡、台阶、沟渠时，盲 MPC 会遭遇三重系统性失效：

盲 MPC 的三重失效

失效 1：落脚点不可行
  MPC 规划的落脚点基于平地假设 → 实际落在悬崖边/尖石上/沟渠中
  后果：接触力分配失败 → 摩擦锥约束违反 → 滑动/摔倒

失效 2：摆动腿碰撞地形
  MPC 的摆动轨迹按照"抬高 5cm → 前移 → 放下"生成
  不知道路径中间有石头或台阶 → 摆动腿正面撞击障碍
  后果：关节电机过载、减速器冲击、结构损坏

失效 3：躯体高度不匹配
  MPC 维持恒定的躯体高度（如 0.48m）
  上坡时实际地面升高 → 腿几乎伸直，失去调节余量
  下坡时地面降低 → 腿过度弯曲，膝关节力矩过大
  后果：运动学奇异 → MPC 不可行 → 紧急停机

这三重失效不是偶发的——在典型的 >10 度坡度或 >5cm 高差条件下容易出现。这意味着**没有感知的 MPC 只能在平地上工作**，这极大限制了腿足机器人的应用范围。

如果不用感知 MPC 会怎样——反应式补偿的局限¶

一个自然的想法是：不改 MPC，在下游用反应式（reactive）策略补偿。比如：

脚碰到障碍就抬高（力/力矩反馈）
着地冲击过大就缩回（阻抗调节）
倾斜超过阈值就减速（安全监控）

足式/160_感知驱动落脚规划已经分析过这类反应式策略的三个根本局限——反应滞后、不可逆冲击、无法全局规划。从 MPC 的视角来看,还有两个额外的问题：

问题 1：MPC 的预测视野被浪费了。MPC 的核心价值在于它能**预测未来**——典型的预测视野是 1-2 秒，对应 2-4 步。但如果 MPC 不知道未来的地形，这个预测视野里的"地形模型"都是平地假设，等于在预测一个**不存在的未来**。反应式补偿只能在"已经发生"之后起作用，完全浪费了 MPC 的前瞻能力。

问题 2：MPC 和反应式层会互相冲突。MPC 生成的轨迹假设平地，反应式层修改了这条轨迹。下一个 MPC 周期看到的状态偏离了自己的预测——MPC 会试图"纠正"回来，而反应式层又会再次修改。两者形成振荡，降低整体性能。

方案	前瞻能力	一致性	复杂地形能力	计算复杂度
盲 MPC + 反应式补偿	无（反应式是事后）	差（MPC 与反应层冲突）	有限（仅处理小扰动）	低
感知 MPC	有（预测视野内看到真实地形）	好（地形信息统一在 MPC 内）	强（直接优化避障轨迹）	中-高

历史：感知嵌入 MPC 的演进¶

感知 + MPC 融合的里程碑

2014  Fankhauser (ETH)       Elevation Mapping 框架
      → 为后续的感知嵌入提供了标准地形表示
  |
2016  Mastalli et al.        Terrain-aware locomotion planning
      → 离线优化中考虑地形，但不是实时 MPC
  |
2018  Di Carlo (MIT)         Convex MPC for quadrupeds
      → 高效的盲 MPC，成为后续感知扩展的基线
  |
2019  Grandia et al.         Frequency-Aware MPC (RA-L)
      → OCS2 框架成熟，为感知嵌入做好了架构准备
  |
2022  Jenelten (ETH)         TAMOLS (T-RO)
      → 地形感知运动优化，SDF 碰撞回避，但不是在线 MPC
  |
2022  Miki (ETH)             Learning perceptive locomotion (Science Robotics)
      → RL 方案：teacher-student 框架，端到端学习感知运动
  |
2023  Grandia (ETH)          Perceptive Locomotion via NMPC (T-RO)  ← 本章核心
      → 完整的感知-NMPC 管线，100 Hz 实时，ANYmal 实机验证
  |
2025  Corbères et al.        Whole-Body Perceptive MPC + Optimal Region Selection (IEEE Access)
      → 扩展到全身 MPC，结合最优区域选择
  |
2025  Jacquet et al.         Neural NMPC with SDF encoding (IJRR)
      → 用神经网络编码 SDF，实现无地图碰撞避免

Grandia 2023 的历史地位在于：它是较早系统性展示**实机 100 Hz 感知 NMPC**的代表性工作之一。之前的工作要么偏离线优化（TAMOLS），要么是 RL 方案（Miki 2022），要么没有把完整感知管线嵌入在线 NMPC。Grandia 2023 把高程图、SDF、平面分割全部实时嵌入 OCS2，在 ANYmal 上验证了台阶攀爬、间隙跨越、stepping stones 等任务。

本质洞察：Perceptive MPC 的核心挑战**不是感知本身,也不是 MPC 本身,而是两者之间的"接口翻译"**——高程图是离散栅格（例如 4cm 分辨率、4m x 4m 局部地图约 100x100 个格子），MPC 的 SQP 求解器要求连续可微或至少分片可微的代价和约束。Grandia 2023 的真正贡献是提出了一套系统的方法把离散地形数据转化为 SQP 可处理的局部模型——包括双线性插值实现地形高度查询、SDF 编码实现碰撞约束线性化、凸区域分解实现落脚约束。没有这套"翻译层",感知和 MPC 就像说不同语言的两个人——各自能力再强也无法协作。

"Perceptive" 到底意味着什么¶

"Perceptive MPC"不是简单地"给 MPC 加个摄像头"。它意味着**感知信息成为 MPC 最优控制问题的一等公民**——地形数据直接影响代价函数和约束，而不是在 MPC 外部做预处理。

盲 MPC 的 OCP（最优控制问题）：
  min  J = Σ l(x, u)           ← 代价只依赖状态和控制
  s.t. ẋ = f(x, u)            ← 动力学
       h(x, u) >= 0            ← 摩擦锥、关节限位等

感知 MPC 的 OCP：
  min  J = Σ l(x, u, M)        ← 代价还依赖地形 M
  s.t. ẋ = f(x, u)            ← 动力学（不变）
       h(x, u) >= 0            ← 物理约束（不变）
       g(x, M) >= 0            ← 地形约束（新增！）
            ↑
            地形约束：
            - 落脚点必须在可踩区域
            - 摆动腿必须避开地形障碍
            - 躯体高度必须适应地面

这里 $M$ 代表地形信息（高程图、SDF、平面参数等）。关键挑战是：$M$ 是**离散栅格数据**，而 MPC 的 SQP 求解器需要**连续可微**的代价和约束。Grandia 2023 的核心贡献就是解决这个"离散感知 vs 连续优化"的桥接问题。

跨领域类比：感知 MPC 中地形数据 $M$ 嵌入优化问题的方式,类似于计算机图形学中纹理映射(Texture Mapping)嵌入渲染管线的方式。在图形学中,纹理是一张离散的像素图,但 GPU 的光栅化管线需要在任意连续坐标处查询颜色——解决方案是双线性插值。感知 MPC 面临完全相同的问题:高程图是离散栅格,SQP 需要在任意连续足端坐标处查询地面高度——解决方案同样是双线性插值。不同之处在于,图形学只需要插值的值,而 MPC 还需要插值的梯度(用于 SQP 的 Jacobian)——这要求插值方案本身是可微的。

跨章桥接（回顾足式/110_OCS2完整栈与双线程MPC）：上面写的 OCP $\min \sum l(x,u)$ s.t. $\dot{x}=f(x,u),\ h(x,u)\geq 0$ 正是 110 章 OCS2 求解的标准连续时间最优控制问题——状态 $x$ 含躯体位姿/速度与关节状态，输入 $u$ 含接触力/关节指令，$f$ 是 Centroidal 或全身动力学，$h$ 是摩擦锥与关节限位。110 章用 SQP 把它线性化、用 HPIPM 求解，RTI 模式每周期只做 1 次迭代。本章**不重写这套求解机制**，只在已有 OCP 上新增 $M$ 依赖的项 $l(x,u,M)$ 和 $g(x,M)\geq 0$。换句话说：感知 MPC = 110 章的 OCS2 求解器 + 本章的地形项，求解器一行代码都不用改——这正是"地形感知是增量"（67.5）在 OCP 层面的体现。

⚠️ 常见陷阱¶

⚠️ 概念误区：认为"感知 MPC"只是在 MPC 代价里加一个地形罚项 新手想法：在代价函数里加 $w \cdot (z_{\text{foot}} - z_{\text{terrain}})^2$ 就是 Perceptive MPC 了。 实际上：如果只加代价项而不加约束，MPC 的 SQP 可能找到一条"代价低但物理不可行"的轨迹——比如穿过地形的轨迹。感知 MPC 需要同时处理**代价**（引导优选方向）和**约束**（强制可行性）。Grandia 2023 的关键创新之一，是把落脚区域等局部几何约束转化为线性/凸不等式；而 SDF 避障约束整体仍可能非凸，需要依赖 SQP 的局部线性化和 warm start。

💡 概念误区：认为感知 MPC 需要精确的 3D 地图 新手想法："要做感知 MPC，先要把 SLAM 做到厘米级精度" 实际上：Grandia 2023 使用的是以机器人为中心的**局部高程图**（4cm 分辨率，4m x 4m 范围），不是全局 SLAM 地图。局部高程图只需要里程计（odometry）来对齐点云，对全局精度要求不高。MPC 的预测视野只有 1-2 秒（约 1-2m 移动），在这个范围内局部高程图的精度完全够用。 正确理解：感知 MPC 需要的是"局部准确"，不是"全局准确"。

🧠 思维陷阱：认为"有了感知 MPC 就不需要反应式层了" 新手想法："MPC 已经看到地形了，不需要下游补偿" 实际上：感知永远不完美——遮挡、传感器噪声、动态障碍（如其他机器人、人、移动物体）都可能导致 MPC 的地形模型与现实不符。ANYmal 的部署中始终保留了一个**反应式安全层**：当接触力异常、关节力矩过大时，触发紧急保护。感知 MPC 是"主力"，反应式层是"安全网"。

练习¶

[分析题] 假设一个四足机器人以 0.5 m/s 的速度行走，MPC 预测视野为 1.5 秒。计算 MPC 预测范围内的最大移动距离。如果前方 0.6m 处有一个 10cm 高的台阶，盲 MPC 和感知 MPC 分别会怎么处理？画出两种情况下的摆动腿轨迹对比。
[思考题] 为什么 Grandia 2023 选择在 MPC 层而不是在 WBC 层嵌入感知信息？如果在 WBC 层处理地形约束，会有什么问题？（提示：考虑 WBC 的优化视野和 MPC 的预测视野的差异。）
[设计题] 如果你要为一个在仓库环境中工作的四足机器人设计感知 MPC 系统，仓库地面有货架、叉车通道（金属格栅）、偶尔散落的包裹。列出你需要在 MPC 的 OCP 中添加的代价项和约束项，并说明每一项的物理含义。

67.2 感知 MPC 系统架构 ⭐⭐¶

本节解决什么问题：从全局视角理解 Grandia 2023 的完整管线——从传感器原始数据到 MPC 求解，中间经过了哪些步骤，每步的频率和延迟是多少。

动机：为什么需要理解全管线¶

很多论文只关注"MPC 算法本身"，对感知前端一笔带过。但在实际部署中，管线的每一个环节都可能成为瓶颈。理解全管线意味着： - 知道系统的端到端延迟预算 - 知道哪些计算可以并行、哪些必须串行 - 知道哪个环节失效时如何降级

全管线架构图¶

Grandia 2023 的管线可以分为三个层次、六个模块：

┌─────────────────────────────────────────────────────────────────┐
│                    感知层（Perception, ~20 Hz）                   │
│                                                                   │
│  ┌──────────┐    ┌──────────────┐    ┌─────────────────────┐     │
│  │ LiDAR /  │───>│ Elevation    │───>│ (A) Steppability    │     │
│  │ 深度相机  │    │ Mapping      │    │ Classification      │     │
│  │ 点云      │    │ (grid_map)   │    │ (可踩性分类)         │     │
│  └──────────┘    └──────┬───────┘    └─────────┬───────────┘     │
│                         │                       │                  │
│                         │               ┌───────▼───────────┐     │
│                         │               │ (A) Plane         │     │
│                         │               │ Segmentation      │     │
│                         │               │ (平面分割)         │     │
│                         │               └───────┬───────────┘     │
│                         │                       │                  │
│                   ┌─────▼───────────────────────▼───────────┐     │
│                   │ (B) SDF Precomputation                  │     │
│                   │ + Torso Reference Height                │     │
│                   │ (SDF 预计算 + 躯体参考高度)               │     │
│                   └─────────────────┬───────────────────────┘     │
│                                     │                              │
├─────────────────────────────────────▼──────────────────────────────┤
│                    规划层（Planning, 100 Hz MPC）                   │
│                                                                     │
│  ┌──────────────────────────────────────────────────────────┐      │
│  │ (C) Nonlinear MPC (OCS2 SQP + HPIPM)                    │      │
│  │                                                           │      │
│  │  代价函数:                                                │      │
│  │    - 躯体高度跟踪（terrain-adaptive）                      │      │
│  │    - 速度跟踪、姿态跟踪                                    │      │
│  │                                                           │      │
│  │  约束:                                                    │      │
│  │    - 落脚点约束（凸不等式，来自平面分割）                    │      │
│  │    - 摆动腿 SDF 碰撞约束                                  │      │
│  │    - 摩擦锥、关节限位（物理约束）                           │      │
│  │                                                           │      │
│  │  输出: 最优状态/控制轨迹 x*(t), u*(t)                     │      │
│  └──────────────────────────┬───────────────────────────────┘      │
│                              │                                      │
├──────────────────────────────▼──────────────────────────────────────┤
│                    执行层（Execution, 400 Hz）                       │
│                                                                     │
│  ┌─────────────────┐    ┌───────────────┐    ┌────────────────┐    │
│  │ 状态估计         │    │ 全身力矩控制   │    │ 反应式安全层    │    │
│  │ (IMU + 关节)     │    │ (WBC)         │    │ (力/力矩保护)  │    │
│  └─────────────────┘    └───────────────┘    └────────────────┘    │
│                                                                     │
└─────────────────────────────────────────────────────────────────────┘

频率与延迟预算¶

管线的各个模块运行在不同的频率上，这是理解系统行为的关键：

模块	频率	延迟	运行硬件	瓶颈
点云获取	20-30 Hz	5-10 ms	传感器	传感器帧率
Elevation Mapping	20 Hz	10-20 ms	CPU / GPU	点云投影+Kalman
可踩性分类	20 Hz	2-5 ms	CPU	特征计算
平面分割	20 Hz	5-10 ms	CPU	连通域标记
SDF 预计算	20 Hz	3-8 ms	CPU	距离变换
NMPC 求解	100 Hz	5-10 ms	CPU (专用核)	SQP+HPIPM
WBC 执行	400 Hz	<1 ms	CPU	QP 求解
状态估计	400 Hz	<1 ms	CPU	Kalman 更新

端到端延迟：从传感器看到障碍到 MPC 输出避障轨迹，总延迟约为 30-60 ms。在 0.5 m/s 行走速度下，这对应 1.5-3 cm 的移动距离——对于 4cm 分辨率的高程图来说可以接受。

延迟链的分解推演：上面这个"30-60ms"不是拍脑袋的，把它沿管线拆开能看清每一段的来源，也能看清"为什么这个延迟可容忍"：

障碍进入视野
  │  ① 传感器曝光+传输      5-10 ms   (帧率上限决定)
  ▼
点云到达
  │  ② Elevation Mapping    10-20 ms  (点云投影+Kalman, 最重一段)
  ▼
高程图更新
  │  ③ 分类+分割+SDF        10-25 ms  (三个子模块串行)
  ▼
约束/SDF 就绪
  │  ④ 等待下一个 MPC 周期  0-10 ms   (相位等待, 100Hz 即 0-10ms)
  ▼
MPC 触发
  │  ⑤ SQP+HPIPM 求解       5-10 ms
  ▼
轨迹输出 → WBC
合计 ≈ 30-75 ms (典型 40-50 ms)

本质洞察：这条链里有一段最容易被忽略却很关键——④ 相位等待。感知是 20 Hz、MPC 是 100 Hz，但感知更新和 MPC 触发**不同步**，所以新地图就绪后，最坏要等近一个 MPC 周期（10ms）才被消费。这解释了"为什么单纯把感知提到 100 Hz 也救不了延迟"（67.2 后面的陷阱）：瓶颈在②③的绝对耗时，不在触发频率。而整条链可容忍的根本原因是**地形是准静态的**——在 50ms 内地形几乎不变，机器人只移动了 2.5cm（半个格子），MPC 用"50ms 前的地形 + 实时的本体状态"做预测，误差被高程图分辨率（4cm）吸收掉了。这正是 67.2 开头"地形时间尺度远慢于运动时间尺度"那句话的定量兑现。

管线的三个子管线 (A)(B)(C) 详解¶

Grandia 2023 把感知-规划管线明确分为三个子管线：

(A) 可踩性分类与平面分割（Steppability Classification + Plane Segmentation）： - 输入：高程图的每个格子 - 处理：计算坡度、粗糙度、步高等特征 --> 二值分类（可踩/不可踩）--> 连通域标记 --> 对每个连通域拟合平面 - 输出：每个可踩区域的平面参数（法向量 $\boldsymbol{n}$、平面上一点 $\boldsymbol{p}_0$）

(B) SDF 预计算与躯体参考（SDF Precomputation + Torso Reference）： - 输入：可踩性分类结果 + 高程图 - 处理：对不可踩区域加垂直边距（2cm） --> 膨胀一个格子 --> 计算 2D 距离变换得到 SDF --> 从可踩区域高度计算躯体参考高度 - 输出：2D SDF 场（每个格子到最近不可踩区域的距离）+ 躯体参考高度 $z_{\text{ref}}(x, y)$

(C) MPC 集成（Integration into OCP）： - 输入：(A) 的平面参数、(B) 的 SDF 和参考高度、当前状态估计、用户指令 - 处理：构建 OCP --> SQP+HPIPM 求解 - 输出：全自由度最优轨迹

为什么 (A)(B) 在 20 Hz 而 (C) 在 100 Hz？ 因为地形变化的时间尺度远慢于机器人运动的时间尺度。高程图在 20 Hz 更新已经足够捕捉地形变化（机器人在两帧之间只移动了 2.5cm @0.5m/s）。而 MPC 需要 100 Hz 来跟踪快速变化的动力学状态。(A)(B) 的结果被缓存，(C) 在每个 MPC 周期直接查询缓存。

数据流的线程模型¶

在 OCS2 的双线程架构（足式/110_OCS2完整栈与双线程MPC）基础上，Perceptive MPC 增加了一个感知线程：

线程模型（3 线程 + Triple Buffer）

线程 1: 感知线程 (Perception Thread, 20 Hz)
  - 接收点云
  - 更新 Elevation Map
  - 运行 (A)(B) 子管线
  - 把结果写入共享内存（double buffer）

线程 2: MPC 线程 (MPC Thread, 100 Hz)
  - 读取感知线程的最新结果（无锁读）
  - 构建 OCP（代价+约束引用感知数据）
  - SQP 求解
  - 把策略写入 Triple Buffer

线程 3: MRT 线程 (MRT Thread, 400 Hz)
  - 从 Triple Buffer 读取最新策略
  - 线性插值得到当前时刻的参考
  - WBC 执行

关键设计：
  - 感知线程和 MPC 线程之间用 double buffer：
    感知写 buffer A，MPC 读 buffer B → 交换
  - MPC 线程和 MRT 线程之间用 triple buffer（足式/110_OCS2完整栈与双线程MPC 已详述）
  - 三个线程完全无锁，无互斥量等待

⚠️ 常见陷阱¶

⚠️ 编程陷阱：感知线程和 MPC 线程竞争同一块内存 错误做法：MPC 线程直接引用 Elevation Map 对象，感知线程同时在更新它。现象：MPC 在一次 SQP 迭代中间，SDF 数据被感知线程更新了 --> 前后不一致 --> 梯度跳变 --> SQP 不收敛。 根本原因：感知更新和 MPC 查询不是原子操作。在一次 MPC 求解的 5-10ms 内，感知线程可能完成一次更新。 正确做法：用 double buffer——感知线程写入"后台 buffer"，写完后原子交换指针。MPC 线程始终读"前台 buffer"，保证一次 SQP 内数据不变。

💡 概念误区：认为感知频率越高越好 新手想法："为什么不让感知也跑到 100 Hz？" 实际上：感知管线（Elevation Mapping + 分类 + 分割 + SDF）单次计算耗时 20-40 ms，即使用 GPU 也需要 5-10 ms。更重要的是，20 Hz 的感知更新对 MPC 已经够用——MPC 的预测视野是 1-2 秒，而高程图在 50ms 内变化微乎其微。提高感知频率只会浪费算力而不提升性能。

🧠 思维陷阱：认为管线是严格串行的 新手想法："必须等感知更新完，MPC 才能开始求解" 实际上：三个线程是**完全并行**的。MPC 不等感知——如果感知还没更新，MPC 用上一帧的感知数据。这意味着 MPC 看到的地形可能比实际延迟了 50ms，但在通常的行走速度下这完全可接受。只有在极高速运动（>1.5 m/s）时，感知延迟才会成为问题。

练习¶

[计算题] 假设 ANYmal 以 0.8 m/s 行走，感知管线延迟 40 ms，MPC 求解延迟 8 ms。计算从传感器看到障碍到 MPC 输出避障轨迹的总延迟，以及这段时间内机器人走了多远。如果高程图分辨率是 4 cm，这个延迟对应多少个格子的移动？
[设计题] 为什么 Grandia 2023 把可踩性分类和 SDF 预计算放在感知线程（20 Hz）而不是 MPC 线程（100 Hz）？如果放在 MPC 线程会怎样？（提示：计算每个 MPC 周期的时间预算。）
[编程题] 用 C++ 实现一个简化的 double buffer 类：一个写者线程、一个读者线程、无锁交换。测试在写者以 20 Hz、读者以 100 Hz 运行时，读者是否永远读到一致的数据。

67.3 地形约束构建 ⭐⭐¶

本节解决什么问题：从高程图原始栅格数据出发，一步步构建 MPC 能用的地形约束——可踩性分类 --> 平面分割 --> 凸不等式约束。

动机：为什么不能直接把高程图丢给 MPC¶

高程图是一个 $N \times M$ 的浮点矩阵，每个格子存一个高度值。MPC 的 SQP 求解器需要的是**连续可微的约束函数** $g(\boldsymbol{x}) \geq 0$。直接把离散栅格数据放入优化器有三个问题：

不可微：栅格边界是阶跃变化，梯度不存在。SQP 需要约束的雅可比，阶跃函数的雅可比要么是零要么是无穷大，都无法用于求解。
维度爆炸：一个 100x100 的高程图有 10000 个格子。如果对每个格子都加一个约束，OCP 的约束数量从几十个暴增到上万个——HPIPM 无法实时求解。
非凸：真实地形的可踩区域是不规则形状，"脚必须落在可踩区域内"是一个非凸约束。MPC 的 SQP 求解器依赖局部凸近似，非凸约束会导致收敛困难。

Grandia 2023 的解决方案：把全局非凸问题转化为局部凸问题——对每只脚，找到它当前最可能落脚的平面，用该平面的参数构造少量凸不等式约束。

步骤 1：可踩性分类（Steppability Classification）¶

目标：对高程图的每个格子，判断它是否适合落脚。

判断标准：一个格子被标记为"可踩"需要同时满足三个条件：

\[\text{steppable}(i,j) = \begin{cases} 1 & \text{if } s(i,j) < s_{\max} \text{ AND } r(i,j) < r_{\max} \text{ AND } \Delta h(i,j) < h_{\max} \\ 0 & \text{otherwise} \end{cases}\]

其中： - $s(i,j)$：局部坡度（slope），由格子邻域的高度梯度计算 - $r(i,j)$：局部粗糙度（roughness），由格子邻域的高度方差计算 - $\Delta h(i,j)$：台阶高度（step height），格子与邻域最大高差

典型阈值（ANYmal 配置）：

参数	符号	典型值	物理含义
最大坡度	$s_{\max}$	30-35 度	超过此坡度摩擦锥无法保证
最大粗糙度	$r_{\max}$	0.05 m	表面不平度超过 5cm 不稳定
最大台阶高度	$h_{\max}$	0.15 m	超过 15cm 关节运动学受限

坡度的计算：对每个格子 $(i,j)$，用 Sobel 算子或中心差分计算高度梯度：

\[\frac{\partial z}{\partial x} \approx \frac{z(i+1,j) - z(i-1,j)}{2r}, \quad \frac{\partial z}{\partial y} \approx \frac{z(i,j+1) - z(i,j-1)}{2r}\]

其中 $r$ 是栅格分辨率。坡度为：

\[s(i,j) = \arctan\left(\sqrt{\left(\frac{\partial z}{\partial x}\right)^2 + \left(\frac{\partial z}{\partial y}\right)^2}\right)\]

粗糙度的计算：取 $(i,j)$ 周围 $k \times k$ 窗口，拟合一个平面 $z_{\text{plane}}$，粗糙度定义为残差的标准差：

\[r(i,j) = \sqrt{\frac{1}{k^2} \sum_{(i',j') \in \text{window}} (z(i',j') - z_{\text{plane}}(i',j'))^2}\]

步骤 2：平面分割（Plane Segmentation）¶

目标：把连续的可踩区域分割成若干平面，每个平面用法向量和偏移描述。

这一步为什么重要？因为 MPC 的落脚约束需要知道"脚要落在哪个平面上"——平面的参数（法向量、高度）直接决定了约束的数学形式。

算法流程：

平面分割算法

Step 1: 连通域标记（Connected Component Labeling）
  输入: 二值可踩性图（0/1）
  方法: 4-连通扫描
  输出: 每个连通区域一个 label

Step 2: 对每个连通域拟合平面
  输入: 某连通域内所有格子的 3D 坐标 {(x_i, y_i, z_i)}
  方法: 计算协方差矩阵 Sigma = (1/N) Sigma (p_i - p_bar)(p_i - p_bar)^T
         最小特征值对应的特征向量 = 平面法向量 n
  检查: 最小特征值 lambda_min < 阈值 → 接受为平面
         lambda_min 过大 → 表面太不平，拒绝

Step 3: 精化分类
  被拒绝的区域标记为"不可踩"
  接受的平面: 记录 (n, p_bar, 边界多边形)

平面参数：每个被接受的平面由以下参数描述：

\[\text{Plane}_k = (\boldsymbol{n}_k, d_k, \mathcal{B}_k)\]

其中 $\boldsymbol{n}_k$ 是单位法向量，$d_k = \boldsymbol{n}_k \cdot \bar{\boldsymbol{p}}_k$ 是平面偏移，$\mathcal{B}_k$ 是平面的边界（用于后续的 SDF 计算）。

平面方程为：

\[\boldsymbol{n}_k \cdot \boldsymbol{p} = d_k\]

任何在这个平面上的落脚点 $\boldsymbol{p}_{\text{foot}}$ 都应满足此方程（在法向方向上）。

步骤 2.5：凸内接多边形分解（论文-代码补全）¶

📄 论文没告诉你的（来源：论文 §IV-B + 外部包 convex_plane_decomposition）：本章到这里只讲了"把可踩区域拟合成平面"。但仅有平面方程 $\boldsymbol{n}_k\cdot\boldsymbol{p}=d_k$ 只约束了**法向高度**，没约束"脚是否落在这块平面的边界内"。真实的 Grandia 2023 多了一步——对每个分割出的平面区域，提取一个**凸内接多边形**（convex inscribed polygon）。这一步不在 ocs2_perceptive，而在独立的 convex_plane_decomposition 包里。

为什么需要凸分解：平面分割得到的可踩区域边界通常是**非凸、不规则**的多边形（比如 L 形平台、带缺口的台阶）。直接用这种边界做约束，会引入非凸约束，破坏 SQP 的实时性（67.3 开头的"非凸"问题就在这里复现）。解决办法是在不规则区域内部**内接一个最大凸多边形**：

不规则可踩区域            内接凸多边形（约束用）
┌──────┐                ┌──────┐
│██████│___              │░░░░░░│
│████████ │   ──凸分解──> │░░░░░░│   ← 只保留这块凸区域做落脚约束
│██████│‾‾‾              │░░░░░░│
└──────┘                └──────┘
(L形, 非凸)              (矩形, 凸, 略小但安全)

凸多边形如何变成约束：一个凸多边形可以表示为若干半空间的交集。设凸多边形有 $m$ 条边，每条边给出一个 2D 半空间约束 $\boldsymbol{a}_j^\top (x_{\text{foot}}, y_{\text{foot}}) \leq b_j$，则"脚落在凸多边形内"等价于：

\[\boldsymbol{a}_j^\top \begin{bmatrix} x_{\text{foot}} \\ y_{\text{foot}} \end{bmatrix} \leq b_j, \quad j = 1, \dots, m\]

这就是 WebSearch 核实的论文摘要里那句"a sequence of convex inequality constraints is extracted as local approximations of foothold feasibility"的真实含义——落脚可行性 = 法向高度等式（步骤3）+ 切向的凸多边形半空间约束序列（本步骤）。每条边一个线性不等式，全部凸，HPIPM 高效处理。

本质洞察：凸内接多边形是"用可行性换凸性"的经典手法——内接凸多边形比原区域小，牺牲了一点可落脚面积（边角踩不到了），但换来了约束的凸性和求解器的实时性。这与 67.3 开头"把全局非凸问题转化为局部凸问题"的主线完全一致，只是这次发生在**切向（水平面内），而半空间高度约束发生在**法向。两者正交互补，共同把"脚落在这块不规则平台上"翻译成 MPC 能实时处理的凸约束集。

步骤 3：落脚点约束的凸不等式形式¶

核心问题：如何把"脚必须落在某个可踩平面上"变成 MPC 能处理的约束？

Grandia 2023 的方法：为每只脚指定一个目标平面，然后用该平面构造半空间约束（half-space constraint）。

对于摆动腿 $i$ 在接触时刻 $t_c$ 的落脚约束：

\[\boldsymbol{n}_k^T \boldsymbol{p}_{\text{foot},i}(t_c) = d_k\]

这个等式约束表示"脚必须落在平面上"。但等式约束在有噪声的情况下太严格——实际中用一个**松弛的不等式约束**：

\[|\boldsymbol{n}_k^T \boldsymbol{p}_{\text{foot},i}(t_c) - d_k| \leq \epsilon_z\]

其中 $\epsilon_z$ 是允许的法向偏差（典型 1-2 cm）。

平面选择：每个 MPC 周期，根据 Raibert 启发式预测的落脚位置，查询该位置所在的平面。如果预测位置在不可踩区域，则选择最近的可踩平面。这个选择在感知线程以 20 Hz 更新，MPC 线程以 100 Hz 使用（不会每帧都切换平面）。

为什么这是凸的？ 半空间约束 $\boldsymbol{n}^T \boldsymbol{p} \leq d + \epsilon$ 和 $\boldsymbol{n}^T \boldsymbol{p} \geq d - \epsilon$ 都是线性不等式，天然凸。这使得 HPIPM 可以高效处理——每个落脚约束只增加 2 个线性不等式，计算开销极小。

与 SDF 约束的配合：落脚点约束确保脚**最终**落在可踩平面上，SDF 约束确保脚在**摆动过程中**不碰撞地形。两者配合，覆盖了落脚的全过程。

对比：直接插值 vs 平面分割¶

方面	直接插值高程图	平面分割+半空间约束
约束光滑性	依赖插值质量，边界可能不光滑	线性约束，完美光滑
约束数量	每个查询点 1 个	每只脚 2 个
凸性	不保证	天然凸
法向量信息	需要额外计算	平面分割自带
鲁棒性	对高程图噪声敏感	平面拟合天然滤噪
局限	无	只能描述平面区域

⚠️ 常见陷阱¶

⚠️ 编程陷阱：平面法向量方向不一致 错误做法：对不同的可踩区域，法向量可能朝上也可能朝下——取决于特征值分解的符号约定。现象：某些平面的落脚约束"反了"——MPC 把脚推向地下而不是地面上。 根本原因：特征值分解返回的特征向量只确定了方向，不确定朝向（sign ambiguity）。 正确做法：每次计算法向量后，强制 $n_z > 0$（法向量朝上）。如果 $n_z < 0$，取反 $\boldsymbol{n} \leftarrow -\boldsymbol{n}$。

💡 概念误区：认为所有可踩区域都应该分割为一个平面 新手想法："弧形坡面也是一个'平面'" 实际上：弧形坡面应该被分割为多个小平面。Grandia 2023 的连通域分割和协方差检查正是为了处理这种情况——如果一个连通域的协方差矩阵最小特征值太大（表面弯曲），该区域会被进一步细分或拒绝。 正确理解：平面分割的"平面"是局部近似。在 4cm 分辨率下，大多数自然地形局部都可以近似为平面。

练习¶

[编程题] 给定一个 50x50 的高程图矩阵（用 Python/NumPy 随机生成，包含一个台阶和一个斜坡），实现可踩性分类：计算每个格子的坡度和粗糙度，用阈值二值化，可视化结果。
[推导题] 证明：对于一个水平平面（法向量 $\boldsymbol{n} = [0, 0, 1]^T$），半空间约束 $|\boldsymbol{n}^T \boldsymbol{p}_{\text{foot}} - d| \leq \epsilon_z$ 退化为 $|z_{\text{foot}} - d| \leq \epsilon_z$，即脚的高度必须在平面高度附近。这验证了在平地情况下，Perceptive MPC 的约束等价于盲 MPC 的固定高度约束。
[分析题] 如果高程图分辨率从 4cm 改为 2cm，平面分割的计算量会怎么变化？连通域标记和协方差计算的复杂度分别是多少？

67.4 SDF 碰撞避障约束 ⭐⭐⭐¶

本节解决什么问题：摆动腿在移动过程中如何避开地形障碍——从 SDF 的计算到 MPC 的不等式约束公式化，包括梯度计算和光滑近似。

动机：为什么摆动腿需要专门的避障约束¶

67.3 节解决了"脚落在哪里"的问题。但落脚点是摆动相的**终点**——脚从当前位置到落脚点的**路径**同样需要安全。

摆动腿碰撞的典型场景

场景 1: 台阶上方通过
  摆动轨迹: A ──────────── B （低矮弧线）
  台阶:          ████████
  问题: 轨迹穿过台阶实体 → 腿撞台阶

场景 2: 沟渠上方跨越
  摆动轨迹: A ──────────── B
  沟渠:    ██        ██
  问题: 如果抬得不够高，脚尖刮到沟渠边缘

场景 3: 碎石区域
  摆动轨迹: A ── ── ── ── B
  碎石:        ▲  ▲  ▲
  问题: 尖锐碎石可能刮伤腿的侧面

盲 MPC 的摆动轨迹是预设的抛物线（抬高-前移-放下），高度固定（如 5cm）。它不知道路径上有什么障碍。感知 MPC 需要一种方法来表达"摆动腿与地形之间的距离不能太近"——这就是 SDF 约束的用途。

SDF 的数学定义与计算¶

定义：给定地形表面 $\mathcal{S}$，SDF 在任意点 $\boldsymbol{p}$ 的值是该点到最近表面点的有符号距离：

\[\phi(\boldsymbol{p}) = \begin{cases} +\min_{\boldsymbol{q} \in \mathcal{S}} \|\boldsymbol{p} - \boldsymbol{q}\| & \text{if } \boldsymbol{p} \text{ 在自由空间} \\ -\min_{\boldsymbol{q} \in \mathcal{S}} \|\boldsymbol{p} - \boldsymbol{q}\| & \text{if } \boldsymbol{p} \text{ 在障碍内部} \end{cases}\]

$\phi > 0$：点在自由空间，距离最近障碍表面 $\phi$ 远
$\phi = 0$：点在障碍表面上
$\phi < 0$：点在障碍内部（已经穿透）

SDF 的核心性质——梯度指向远离障碍的方向：

\[\nabla \phi(\boldsymbol{p}) = \frac{\boldsymbol{p} - \boldsymbol{q}^*}{\|\boldsymbol{p} - \boldsymbol{q}^*\|}\]

其中 $\boldsymbol{q}^*$ 是表面上离 $\boldsymbol{p}$ 最近的点。梯度的物理含义是"离开障碍最快的方向"——这正是优化器需要的信息。当 MPC 的 SQP 计算约束的雅可比时，SDF 的梯度直接提供了"如何修改轨迹来增大与障碍的距离"的方向。

这为什么如此关键？ 如果用 occupancy grid（二值占据栅格），只知道"某个格子被占据"，没有距离信息也没有梯度。优化器不知道该往哪个方向移动轨迹。SDF 提供了距离和方向，让 SQP 能做有意义的梯度步。

Grandia 2023 的 2D SDF 计算¶

在 Grandia 2023 中，SDF 不是直接在 3D 空间计算的——这太昂贵了。取而代之的是**2D SDF**：在高程图的 $(x, y)$ 平面上计算每个格子到最近不可踩区域的水平距离。

计算步骤：

2D SDF 计算流程

Step 1: 准备二值图
  从可踩性分类得到: steppable(i,j) in {0, 1}
  对所有 steppable = 0 的格子:
    - 加垂直边距: z(i,j) += 2cm（向上抬高不可踩区域）
    - 膨胀一格: 将不可踩标记扩展到相邻格子
  目的: 保守估计，增大安全裕度

Step 2: 距离变换（Distance Transform）
  输入: 二值图（0 = 不可踩/障碍, 1 = 可踩/自由）
  方法: Euclidean Distance Transform (EDT)
  输出: 每个格子到最近 0-格子的 L2 距离
  复杂度: O(N) 使用 Felzenszwalb & Huttenlocher 2012 算法

Step 3: 添加符号
  仅计算到障碍的 EDT 得到的是 unsigned distance；
  若需要真正 signed distance，需要再计算“到自由格”的距离:
    outside = distance_to_obstacle
    inside  = distance_to_free
    sdf(i,j) = outside(i,j) - inside(i,j)
  因此可踩区为正，障碍内部为负，边界附近约为 0。

**Euclidean Distance Transform（EDT）**是这里的关键算法。它能在 $O(N)$ 时间内（$N$ 是格子总数）计算所有格子到最近零格子的精确欧氏距离。原理基于"扫描+抛物线包络"：

\[\text{EDT}[i] = \min_j \left( (i - j)^2 + f[j] \right)\]

其中 $f[j]$ 是输入（前一维度的结果）。这个 min 操作的图像是一族抛物线的下包络——可以用线性时间的算法（类似凸包）求解。Felzenszwalb & Huttenlocher 2012 的算法先在 x 方向扫描，再在 y 方向扫描，两次 1D 变换组合得到 2D 结果。

为什么用 2D 而不是 3D SDF？ 三个原因：

计算量：3D SDF 需要 $O(N^3)$ 空间和计算，2D 只需 $O(N^2)$。在 4cm 分辨率的 4m x 4m 地图上，2D 是 100x100=10000 格子，3D 是 100x100x50=500000 格子。
更新频率：2D EDT 在 CPU 上可以亚毫秒完成，3D 需要 GPU。
足够用：对于四足机器人的摆动腿碰撞回避，水平方向的距离信息比垂直方向更重要——腿主要在水平面上移动，垂直方向用高程图的高度信息就够了。

跨领域类比：SDF 在感知 MPC 中的角色，类似于导航系统中的"距离场地图"——飞行员不需要记住每座山的形状，只需要在每个位置知道"距最近障碍物多远"和"哪个方向远离障碍物"。SDF 把复杂的地形几何压缩为两个信息：距离值（标量）和梯度方向（向量）。这恰好是 MPC 优化器需要的——距离值用于判断约束是否满足，梯度方向用于计算如何修正轨迹以远离障碍物。正如飞行员只需要"terrain proximity warning + escape direction"就能安全飞行，MPC 也只需要 SDF 的值和梯度就能避开障碍。

SDF 约束的数学公式化¶

目标：在 MPC 的摆动相轨迹上，约束摆动腿末端与地形的距离大于安全裕度 $d_{\min}$。

对于摆动腿 $i$ 在时刻 $t$（摆动相内），约束为：

\[\phi(\boldsymbol{p}_{\text{foot},i}(t)) \geq d_{\min}\]

其中 $\boldsymbol{p}_{\text{foot},i}(t)$ 是脚的位置（通过正运动学从状态 $\boldsymbol{x}$ 计算），$\phi$ 是 SDF 值。

教学模型中的常见分解：如果把腿部避障近似为 2D SDF + 高程图高度约束，约束可分解为水平和垂直两个分量：

水平约束（使用 2D SDF）：

\[\phi_{2D}(x_{\text{foot}}, y_{\text{foot}}) \geq d_{\min,h}\]

这确保脚在水平方向上远离不可踩区域的边界。

垂直约束（使用高程图插值）：

\[z_{\text{foot}} - z_{\text{terrain}}(x_{\text{foot}}, y_{\text{foot}}) \geq d_{\min,v}\]

这确保脚在垂直方向上高于地形表面。$z_{\text{terrain}}$ 通过双线性插值从高程图获得。

SDF 查询的可微化¶

SDF 是离散栅格数据。MPC 的 SQP 需要约束对状态的雅可比 $\partial \phi / \partial \boldsymbol{x}$。这需要两步：

第一步：SDF 的连续化——双线性插值

给定连续坐标 $(x, y)$，在 SDF 栅格上做双线性插值：

\[\hat{\phi}(x, y) = (1-\alpha)(1-\beta)\phi_{00} + \alpha(1-\beta)\phi_{10} + (1-\alpha)\beta\phi_{01} + \alpha\beta\phi_{11}\]

其中 $\alpha = (x - x_0) / r$，$\beta = (y - y_0) / r$，$\phi_{ij}$ 是四个邻接格子的 SDF 值，$r$ 是栅格分辨率。

这是连续、分片可微的。在单个栅格单元内部，对 $x$ 的偏导数：

\[\frac{\partial \hat{\phi}}{\partial x} = \frac{1}{r}\left[ (1-\beta)(\phi_{10} - \phi_{00}) + \beta(\phi_{11} - \phi_{01}) \right]\]

对 $y$ 的偏导数：

\[\frac{\partial \hat{\phi}}{\partial y} = \frac{1}{r}\left[ (1-\alpha)(\phi_{01} - \phi_{00}) + \alpha(\phi_{11} - \phi_{10}) \right]\]

第二步：链式法则——从 SDF 梯度到状态雅可比

SQP 需要的是 $\partial \phi / \partial \boldsymbol{x}$（约束对 OCP 状态的雅可比）。通过链式法则：

\[\frac{\partial \phi}{\partial \boldsymbol{x}} = \frac{\partial \hat{\phi}}{\partial \boldsymbol{p}_{\text{foot}}} \cdot \frac{\partial \boldsymbol{p}_{\text{foot}}}{\partial \boldsymbol{x}}\]

其中 $\partial \boldsymbol{p}_{\text{foot}} / \partial \boldsymbol{x}$ 是正运动学的雅可比（Pinocchio 提供），$\partial \hat{\phi} / \partial \boldsymbol{p}_{\text{foot}}$ 来自上面的双线性插值梯度。

CppAD 与距离场接口的角色：教学上可以把“正运动学 + 插值”整体写成 AD<double> 表达式来理解链式法则。但当前 OCS2 EndEffectorDistanceConstraintCppAd 的真实实现更克制：CppADCodeGen 主要用于生成末端执行器正运动学及其雅可比；运行时距离场通过 DistanceTransformInterface::getValue() 和 getLinearApproximation() 提供 SDF 值与空间梯度，最后显式相乘得到 $$ \frac{\partial \phi}{\partial x} = \nabla_p \phi(p)^\top \frac{\partial p}{\partial x}. $$ 这样地图和 clearance 可以在运行时更新，而不需要每次地图变化都重新生成 .so。

// 更接近当前 OCS2 EndEffectorDistanceConstraintCppAd 的结构
auto ee_positions = kinematicsModelPtr_->getFunctionValue(state);
auto ee_jacobians = kinematicsModelPtr_->getJacobian(state);

for (size_t i = 0; i < num_ee; ++i) {
  vector3_t p = ee_positions.segment<3>(3 * i);
  auto [distance, grad_p] = distanceTransform.getLinearApproximation(p);

  g(i) = weight * (distance - clearances(i));
  dfdx.row(i) = weight * grad_p.transpose()
              * ee_jacobians.middleRows<3>(3 * i);
}

三级近似：SQP 到底向距离约束要什么¶

读 OCS2 源码会发现 EndEffectorDistanceConstraintCppAd 暴露了**三个**层级递增的方法，而不只是上面用到的 getLinearApproximation。这对应 SQP 在不同求解模式下对约束的不同需求：

方法（源码核实）	返回	SQP 用途	计算成本
`getValue(t,x,u,·)`	约束值向量 $g$	检查约束违反、线搜索	最低（仅正运动学 + 查表）
`getLinearApproximation(...)`	$g$ 与 $\partial g/\partial(x,u)$	一阶 SQP（RTI 默认）	中（加末端雅可比）
`getQuadraticApproximation(...)`	再加二阶项	完整二阶 SQP / 罚函数 Hessian	高（加二阶导）

本质洞察：很多人以为"MPC 求解器只要约束的雅可比就够了"，这只在**一阶 SQP（RTI）下成立。当 OCS2 用 Augmented Lagrangian 把距离约束转成罚项并入代价时，求解器需要罚项的 **Hessian 才能构造正定的 QP 子问题——这就要到 getQuadraticApproximation。理解"约束有三个近似层级、求解模式决定调用哪个"，才算真正读懂了 OCS2 约束接口的设计：它不是为某一种求解器写死的，而是把"提供多少阶信息"的选择权交给上层。

距离约束 Hessian 的结构：把约束写成 $g_i(x) = \phi(p_i(x)) - c_i$，对状态求二阶导，用链式法则得到：

\[\frac{\partial^2 g_i}{\partial x^2} = \underbrace{J_{p}^\top \, \nabla_p^2\phi \, J_{p}}_{\text{距离场曲率项}} + \underbrace{(\nabla_p\phi)^\top \frac{\partial^2 p_i}{\partial x^2}}_{\text{运动学曲率项}}\]

其中 $J_p = \partial p_i/\partial x$ 是末端运动学雅可比。工程上几乎总是用 Gauss-Newton 近似——丢掉第二项（运动学二阶导，张量、昂贵），并把 $\nabla_p^2\phi$ 近似为零或小正定项（因为 SDF 在远离障碍处近似线性，曲率小）。

💡 论文没告诉你的（数值 trick，来源：SQP 通用实践 + OCS2 罚函数实现）：为什么敢丢掉 $\nabla_p^2\phi$？因为理想 SDF 满足 $\|\nabla\phi\|=1$（单位梯度，eikonal 性质），其等值面曲率在平直障碍附近接近零；只有在凹角/狭缝处曲率才显著。Gauss-Newton 丢掉这一项的代价，是在这些高曲率区域收敛略慢——但换来了**保证正定**的 QP 子问题（$J_p^\top J_p \succeq 0$ 天然半正定），避免了 SQP 因不定 Hessian 而失败。这是"用收敛速度换数值稳定"的经典工程取舍，论文不会讨论，但决定了实机上 MPC 跑不跑得稳。

⚠️ 符号约定陷阱：clearance 的正负与约束方向¶

源码里 g(i) = weight * (distance - clearances(i))，OCS2 约定 StateInputConstraint 表示 $g \geq 0$ 的不等式。代入得约束 $\phi(p_i) \geq c_i$——即"末端到障碍的有符号距离不小于 clearance"。这个方向看似显然，但有两个隐藏陷阱：

支撑腿与摆动腿用不同 clearance：摆动腿要 $c_i>0$（悬空避障），支撑腿却要允许 $\phi \approx 0$ 甚至接触（脚本就该踩在地面上）。若给所有腿同一个正 clearance，支撑腿会被"推离地面"，MPC 直接不可行。真实代码用逐末端 clearances_ 向量正是为此（67.7 的 set(vector_t clearances,...) 重载）。
clearance 是运行时量，不是编译期常量：它通过 set() 注入，可随步态相位在线切换。这与"SDF 进 AD tape"是两回事——改 clearance 不需要重生成 .so（67.7 陷阱）。

光滑近似：处理 SDF 梯度不连续点¶

SDF 在某些点的梯度不存在——具体来说，在"等距面"（equidistant loci）上，即到两个最近障碍表面距离相等的点。这些点的 SDF 值连续，但梯度有跳变。

虽然双线性插值能把栅格值变成连续函数，但它只是分片可微，跨格子边界时梯度仍可能跳变。Grandia 2023 还使用了额外的平滑和松弛技巧来降低 SQP 对这些跳变的敏感性：

技巧 1：高斯模糊 SDF。在计算 EDT 后，对 SDF 栅格应用一个小核的高斯滤波：

\[\tilde{\phi}(i,j) = \sum_{(i',j') \in \mathcal{N}} w(i',j') \cdot \phi(i',j')\]

这会缓和等距面和栅格边界附近的梯度跳变，让 SQP 看到的局部模型更平滑；代价是 SDF 值不再是严格的几何距离，安全裕度需要留出这部分滤波误差。

技巧 2：松弛约束的光滑惩罚。不是用硬约束 $\phi \geq d_{\min}$，而是用光滑的惩罚函数：

\[c(\phi) = \begin{cases} 0 & \text{if } \phi \geq d_{\min} \\ \frac{1}{2\mu}(d_{\min} - \phi)^2 & \text{if } \phi < d_{\min} \end{cases}\]

这是一个二次惩罚，在 $\phi = d_{\min}$ 处连续且一阶可微（值和梯度都为零）。$\mu$ 是松弛参数。OCS2 通过 Augmented Lagrangian 方法（足式/110_OCS2完整栈与双线程MPC 已详述）来处理这类松弛约束。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：双线性插值在栅格边界的越界访问 错误做法：查询坐标 $(x, y)$ 刚好在栅格最后一列时，$i+1$ 越界。现象：段错误（segfault）或读到垃圾值 --> SDF 约束的梯度异常 --> SQP 发散。 正确做法：调用插值函数之前，由距离场或地图接口把查询坐标裁剪到有效范围，或者在栅格外围加一圈 padding（填充最大安全距离）。当前 OCS2 的 BilinearInterpolation.h 只负责给定 referenceCorner 和四个角点值后的插值与梯度计算，边界选择应由调用方完成。

💡 概念误区：认为 SDF 约束总是凸的 新手想法："SDF 本身是连续的，约束 $\phi \geq d$ 应该是凸的" 实际上：SDF 约束一般**不是凸的**。考虑两个障碍之间的狭窄通道：SDF 在通道中心是局部最大值，两侧递减。约束 $\phi \geq d$ 定义的可行域可能是非凸的（两块分离的区域）。这就是为什么 Grandia 2023 用 SQP（局部优化器）而不是全局优化器——SQP 只保证收敛到局部最优，但实时性好。 正确理解：单个障碍外侧的局部线性化常常能给出有效的远离方向，但不要把它理解成全局凸约束。MPC 的初始猜测（warm-start）足够好时，SQP 通常在局部可行区域内工作；初始猜测穿过狭窄通道或障碍边界时，非凸性仍会导致局部最优或收敛失败。

🧠 思维陷阱：认为 2D SDF 足以处理所有碰撞情况 新手想法："水平距离够了，不需要 3D" 反例：机器人的小腿从一个高台上方摆过——水平距离很远（2D SDF 显示安全），但垂直方向上小腿可能刮到台面。这种情况需要**3D SDF** 或者对小腿的多个检查点做高程图查询。Grandia 2023 为了实时性选择了 2D SDF + 高程图垂直检查的组合，但这在极端地形（如高窄障碍）上可能不足。 最新进展：Jacquet et al. 2025（IJRR）用神经网络编码 3D SDF，在 NMPC 中实现了真正的 3D 碰撞避免。

练习¶

[编程题] 用 Python 实现 Felzenszwalb & Huttenlocher 的 2D EDT 算法：输入一个二值栅格（0/1），输出每个格子到最近 0-格子的欧氏距离。对一个包含若干矩形障碍的 100x100 栅格测试，用 matplotlib 可视化 SDF 热力图。
[推导题] 证明双线性插值函数 $\hat{\phi}(x,y)$ 的梯度在栅格内部是连续的，但在栅格边界上（$\alpha = 0$ 或 $\beta = 0$）梯度**可能不连续**。这对 MPC 的 SQP 收敛有什么影响？为什么 Grandia 2023 使用高斯模糊来缓解？
[分析题] 假设 SDF 的分辨率是 4cm，安全裕度 $d_{\min} = 5$ cm。对于一个宽度为 8cm 的缝隙（两侧都是不可踩区域），SDF 在缝隙中心的值是多少？MPC 能否在这个缝隙中规划出满足约束的轨迹？这说明了 SDF 约束的什么局限性？

67.5 地形感知代价函数 ⭐⭐¶

本节解决什么问题：约束保证了可行性（"不撞"、"不踩空"），但 MPC 还需要代价函数来引导**优选方向**——在所有可行轨迹中，哪条最好？

动机：约束不够，还需要代价¶

考虑一个斜坡场景：所有满足落脚约束和 SDF 约束的轨迹都是"可行的"。但有些轨迹让机器人的重心过高（不稳定），有些让膝关节过度弯曲（力矩大），有些让躯体倾斜过大（不舒适）。代价函数的作用是在可行域内选择"最好的"轨迹。

Grandia 2023 在盲 MPC 的代价函数基础上增加了三个地形感知代价项。

代价项 1：躯体高度跟踪（Body Height Tracking）¶

问题：盲 MPC 维持固定的躯体高度（如 0.48m）。在斜坡上，这导致上坡时腿伸直、下坡时腿过弯。

解决：让躯体高度跟踪**地形自适应的参考高度**：

\[J_{\text{height}} = w_h \left( z_{\text{body}} - z_{\text{ref}}(x_{\text{body}}, y_{\text{body}}) \right)^2\]

其中 $z_{\text{ref}}(x, y)$ 是地形参考高度——从高程图计算的、机器人躯体应该处于的高度。

参考高度的计算：取躯体投影下方四个脚的落脚平面高度的加权平均，再加上标称站立高度：

\[z_{\text{ref}}(x, y) = \bar{z}_{\text{terrain}}(x, y) + h_{\text{nominal}}\]

其中 $\bar{z}_{\text{terrain}}$ 是四脚落脚平面高度的均值，$h_{\text{nominal}} = 0.48\text{m}$（ANYmal 的标称站立高度）。

为什么用平面高度而不是直接查高程图？ 因为高程图在碎石区域有很大的局部波动——如果直接用高程图的高度，参考高度会随碎石一起"抖动"，导致躯体上下振荡。用平面拟合后的高度天然滤掉了局部噪声。

代价项 2：躯体避障（Body Clearance）¶

问题：在台阶或大石头旁边行走时，即使脚没有碰到障碍，**躯体**可能撞到。特别是 ANYmal 的躯体宽约 0.5m，在狭窄通道或大石头旁边时需要注意。

解决：用 SDF 对躯体位置也加一个代价：

\[J_{\text{clearance}} = w_c \cdot \max(0, d_{\text{body}} - \phi_{2D}(x_{\text{body}}, y_{\text{body}}))^2\]

其中 $d_{\text{body}}$ 是躯体的最小安全距离（考虑躯体宽度），$\phi_{2D}$ 是 2D SDF。当 SDF 值小于安全距离时，代价增大，推动 MPC 让躯体远离障碍。

注意：这是一个**代价项**而非约束——允许躯体偶尔靠近障碍（代价增大但不禁止），因为在狭窄环境中完全避免可能导致 MPC 不可行。

代价项 3：摆动腿地形回避（Swing Terrain Avoidance）¶

问题：SDF 约束保证了脚不穿透地形，但约束是"硬边界"——MPC 可能让脚刚好擦过地形表面，没有余量。

解决：在 SDF 约束之外，额外加一个**软代价**，鼓励摆动腿远离地形表面：

\[J_{\text{swing}} = w_s \sum_{t \in \text{swing}} \max(0, d_{\text{swing}} - \phi(\boldsymbol{p}_{\text{foot}}(t)))^2\]

这是一个"安全裕度代价"——即使满足了 SDF 约束（$\phi \geq d_{\min}$），如果 $\phi < d_{\text{swing}}$（$d_{\text{swing}} > d_{\min}$），也会有额外代价，鼓励 MPC 保持更大裕度。

三个代价项的权重调节¶

代价项	权重符号	典型值范围	过大后果	过小后果
躯体高度跟踪	$w_h$	50-200	躯体僵硬跟踪地形，忽略其他目标	躯体高度不适应地形，腿过伸/过弯
躯体避障	$w_c$	10-50	机器人不敢靠近任何障碍	躯体可能碰撞大障碍
摆动腿回避	$w_s$	20-100	摆动高度过大，浪费能量	脚擦过地形表面，缺乏安全裕度

调参原则：在不同地形上需要不同的权重。Grandia 2023 使用了一组**固定权重**在所有测试地形上都表现良好，但最新的工作（如 DTC, Jenelten 2024）开始用 RL 学习权重。

代价函数的完整组合¶

把地形感知代价加到盲 MPC 的代价上：

\[J = \underbrace{J_{\text{vel}} + J_{\text{ori}} + J_{\text{input}}}_{\text{盲 MPC 基础代价}} + \underbrace{J_{\text{height}} + J_{\text{clearance}} + J_{\text{swing}}}_{\text{地形感知代价}}\]

其中基础代价包括： - $J_{\text{vel}}$：速度跟踪（跟踪用户指令的线速度和角速度） - $J_{\text{ori}}$：姿态跟踪（保持躯体水平或跟踪参考姿态） - $J_{\text{input}}$：控制输入正则化（惩罚过大的关节力矩）

地形感知代价只是"增量"——它不改变盲 MPC 的基础结构，只是在上面加了三项。这使得系统可以优雅降级：如果感知失效，把地形感知代价的权重设为零，就回到了盲 MPC。

同一地形需求：硬约束、软约束、还是代价？¶

读到这里你应该有个疑问：地形信息一会儿做成"约束"（67.4 的 SDF $\phi\geq d_{\min}$），一会儿做成"代价"（67.5 的三个 $J$），它们到底怎么选？这是 Perceptive MPC 工程实现中最高频的决策，OCS2 提供了三个层级的工具，对应三种"违反的代价"：

实现层级	数学形式	违反的后果	适用场景	OCS2 工具
硬约束	$g(x)\geq 0$ 强制满足	不可行则 MPC 失败	物理铁律（摩擦锥、关节限位）	`StateInputConstraint`
软约束（罚函数）	违反量进代价，权重大	强烈不鼓励但允许	安全关键但偶尔需妥协（SDF 避障）	`SoftConstraintPenalty` + Augmented Lagrangian
纯代价	直接加权进 $J$	只是"不够好"	偏好引导（躯体高度、摆动裕度）	加进 `StateInputCost`

本质洞察：硬→软→代价是一条"强制力递减、可行性递增"的光谱。把所有地形需求都做成硬约束，理论最安全，但实际会让 MPC 频繁不可行（狭窄环境里硬约束互相矛盾）；全做成代价，MPC 永远有解，但可能输出穿透地形的"低代价不可行解"。Grandia 2023 的工程智慧在于**按需求的物理刚性分配层级**：摩擦锥是硬约束（违反=物理上不可能），SDF 避障是软约束（违反=危险但有时是唯一出路，靠 Augmented Lagrangian 用逐渐增大的罚权逼近可行），躯体高度是纯代价（违反=不舒服但安全）。理解这条光谱，比记住"SDF 是约束、高度是代价"这种结论重要得多——它让你面对一个新地形需求时，能自己判断该放在光谱的哪一档。

💡 论文没告诉你的（来源：OCS2 SoftConstraintPenalty 实现）：软约束不是"把约束乘个大权重塞进代价"那么简单。OCS2 的 Augmented Lagrangian 实现会**在迭代中动态调整罚权和对偶变量**——初期罚权小（允许探索），随迭代增大（逼近可行）。这避免了固定大权重导致的两个老问题：权重太小约束形同虚设，权重太大 QP 病态（Hessian 条件数爆炸、SQP 数值失败）。这正是 67.4 末尾"光滑惩罚 + Augmented Lagrangian"那句话的工程内核。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：代价函数中 max(0, x) 的不可微点 错误做法：直接在代价中用 std::max(0.0, x)。现象：当 $x$ 从负变正时，CppAD 计算的梯度在 $x=0$ 处不连续 --> SQP 的二次子问题不正定 --> 求解失败。 正确做法：用光滑近似，如 softplus：$\text{softplus}(x) = \frac{1}{\beta}\log(1 + e^{\beta x})$，其中 $\beta$ 控制光滑程度。或者用 Huber-like 函数在零点附近做二次过渡。OCS2 的 SoftConstraintPenalty 类提供了这种光滑近似。

💡 概念误区：认为代价和约束可以互相替代 新手想法："既然有 SDF 约束了，为什么还要 SDF 代价？" 实际上：约束定义"可行边界"（绝对不能越过），代价定义"偏好方向"（越远越好）。只有约束没有代价时，MPC 可能让脚刚好擦过地形表面——技术上可行，但缺乏安全裕度，任何扰动都可能导致碰撞。代价在约束之外提供了"软性引导"。类比：约束是"悬崖边的护栏"，代价是"请远离护栏的警告标志"。

练习¶

[计算题] 假设 ANYmal 在 15 度斜坡上行走，标称站立高度 0.48m。计算地形自适应参考高度 $z_{\text{ref}}$ 在斜坡上某点的值（已知该点地面高度 0.3m）。与盲 MPC 的固定参考高度 0.48m 相比，差了多少？
[设计题] 如果你要为一个双足人形机器人设计地形感知代价函数（与四足不同，双足只有两个支撑点），你会怎么修改三个代价项？特别是躯体高度跟踪——双足的参考高度应该怎么计算？
[分析题] 解释为什么在平地上，三个地形感知代价项的值都趋近于零，感知 MPC 的行为退化为盲 MPC。这是通过什么数学机制保证的？

67.6 Swing 轨迹地形适应 ⭐⭐¶

本节解决什么问题：MPC 输出的摆动腿轨迹需要适应地形——在台阶上抬高、在沟渠上跨越、在碎石区域绕行。这里的"适应"不仅是约束层面的（不碰撞），更是轨迹形状层面的（摆动高度、速度、形状应根据地形调整）。

动机：固定摆动轨迹的局限¶

盲 MPC 中的摆动轨迹通常是预设的样条曲线——从起点到终点的抛物线，最大高度固定（如 5cm 或 8cm）。这在平地上足够了，但在崎岖地形上有严重问题：

固定摆动轨迹的失效场景

场景 1: 上台阶（10cm 高）
  固定抬高 5cm → 抬高不够 → 脚趾撞台阶面
  需要: 抬高至少 15cm（台阶高度 + 安全裕度）

场景 2: 跨沟渠（8cm 宽，5cm 深）
  固定抬高 5cm → 刚好与沟渠边缘齐平
  任何下垂都会让脚掉进沟渠
  需要: 增大步幅 + 增大抬高

场景 3: 下台阶（10cm 高）
  固定放下高度 → 脚在空中悬停太久 → 冲击力大
  需要: 提前开始下降，减小着地冲击

地形感知摆动轨迹生成¶

Grandia 2023 不是预设固定的摆动轨迹，而是让 MPC 优化器自己决定摆动轨迹——通过地形约束和代价，SQP 会自动找到避开地形的最优摆动路径。

但 MPC 需要一个好的**初始猜测**（warm-start）。如果初始猜测的摆动轨迹穿过地形，SQP 可能无法收敛（约束违反太大）。因此，感知管线还要提供一个**地形感知的摆动轨迹初始猜测**。

初始猜测的生成算法：

地形感知摆动轨迹初始猜测

输入: 起点 p_start (当前脚位置)
      终点 p_end (目标落脚点，来自落脚规划)
      地形高程图 M
      SDF 场 phi

Step 1: 计算路径上的最大地形高度
  沿 p_start → p_end 的直线，采样 N 个点
  对每个采样点查询高程图: z_terrain(x_i, y_i)
  z_max = max(z_terrain(x_i, y_i)) for all i

Step 2: 计算安全抬高高度
  h_swing = max(h_min, z_max - min(z_start, z_end) + d_clearance)
  其中:
    h_min = 3cm (最小抬高，平地也需要)
    d_clearance = 5cm (安全裕度)

Step 3: 生成参数化摆动曲线
  使用三段样条:
    段 1 (抬起): p_start → p_mid_up (垂直抬起)
    段 2 (前移): p_mid_up → p_mid_down (水平+向下)
    段 3 (放下): p_mid_down → p_end (垂直放下)

  p_mid_up = p_start + [0, 0, h_swing]
  p_mid_down = p_end + [0, 0, h_swing * 0.3]

输出: 摆动轨迹的初始猜测 p_swing(t), t in [t_liftoff, t_touchdown]

📄 论文没告诉你的（UNSPECIFIED，来源：本教学重构）：上面算法里的几个魔数——h_min=3cm、d_clearance=5cm、落点侧抬高系数 0.3——都是**本章为讲清三段样条结构而设的教学值，不是 Grandia 2023 的源码原文**。论文把"摆动轨迹由 MPC 优化器决定"作为卖点，并未公布初始猜测生成器的具体参数（这类 warm-start 启发式通常藏在示例代码里，随机器人尺寸而变）。这里的 0.3 表示"落脚点上方的中间控制点比抬起侧低一些"，让轨迹呈现"快抬、缓落"的不对称形状以减小着地冲击——系数本身可调，读者复现时应以本机示例代码为准，把它当作可调超参而非定值。

MPC 在此基础上进一步优化：初始猜测提供了一条"大致安全"的轨迹，MPC 的 SQP 会在此基础上微调，考虑动力学约束（关节速度限制、力矩限制）和代价函数（最小化能量、保持平衡）。最终的轨迹通常比初始猜测更平滑、更高效。

特殊地形的摆动策略¶

地形	关键调整	物理原因
上台阶	大幅增大抬高，先垂直抬起再水平移	避免脚趾碰撞台阶面
下台阶	提前下降，减小着地速度	减小着地冲击力
跨沟渠	增大步幅 + 增大抬高	确保脚完全越过沟渠
碎石区域	中等抬高，快速放下	减少空中时间，快速建立支撑
斜坡	抬高略增，倾斜放下方向	适应斜坡法向量
stepping stones	精确瞄准，最小水平偏移	落脚面积有限，偏差不可接受

摆动时间分配¶

地形还影响**摆动相的时间分配**。在复杂地形上，MPC 可能需要：

减慢摆动速度：给足够时间绕开障碍
增加触地时的减速段：减小着地冲击
缩短空中时间：在不稳定地形上，三脚支撑比两脚支撑更稳定，所以应尽快恢复四脚支撑

Grandia 2023 通过 MPC 的代价函数间接控制摆动时间——控制输入正则化惩罚过大的关节速度，使得在需要大幅度移动时自动减慢。但步态时序（liftoff/touchdown 时刻）仍然是预设的（足式/120_步态管理与接触序列的步态调度器决定），不由 MPC 优化。

本质洞察：感知 MPC 的核心工程难题**不是算法本身,而是"离散感知"与"连续优化"之间的阻抗匹配**。高程图是离散的（4cm 栅格）、异步更新的（20 Hz），而 MPC 是连续的（连续状态/输入空间）、同步执行的（100 Hz）。Grandia 2023 的所有工程创新——双线性插值、SDF 预计算、凸区域分解、双线程架构——都是在解决同一个根本问题：如何让离散的感知数据无缝地流入连续的优化管线,既不引入非光滑性（破坏 SQP 收敛），也不引入过大延迟（破坏实时性）。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：摆动轨迹初始猜测穿过地形 错误做法：初始猜测简单地从起点到终点做线性插值，不考虑中间的地形。现象：SQP 第一次迭代时约束违反量极大 --> 线搜索步长几乎为零 --> SQP 不收敛 --> MPC 超时 --> 使用上一帧的旧策略 --> 控制器反应迟钝。 正确做法：初始猜测必须"基本安全"——至少不穿过地形。上面的算法通过查询路径上的最大地形高度来保证这一点。

💡 概念误区：认为 MPC 直接优化摆动轨迹的每个点 新手想法："MPC 在每个时间步都独立优化脚的位置" 实际上：MPC 优化的是**状态轨迹**（包括关节角度和速度）。脚的位置是状态通过正运动学的函数。所以 MPC 是间接优化摆动轨迹——通过约束关节角度来约束脚的运动。这意味着摆动轨迹自动满足运动学约束（关节限位、速度限制），不需要额外检查。

练习¶

[编程题] 给定一个包含 10cm 台阶的高程图，实现摆动轨迹初始猜测算法：从台阶底部到台阶顶部生成一条地形感知的摆动曲线。用 matplotlib 3D 可视化轨迹和地形。
[分析题] 在 stepping stones 场景中（离散踏板，间距 30cm，踏板面积 10cm x 10cm），摆动轨迹的水平精度要求是多少？如果 MPC 的控制频率是 100 Hz，每个控制周期的水平位置变化是多少？这说明了 MPC 频率对落脚精度的什么关系？
[思考题] Grandia 2023 的步态时序是预设的。如果把步态时序也作为 MPC 的优化变量（自由步态时序），Perceptive MPC 会更好还是更差？（提示：考虑优化变量增加对 SQP 收敛速度的影响，以及自由时序对 stepping stones 场景的潜在优势。）

67.7 OCS2 Perceptive 实现对照 ⭐⭐⭐¶

本节解决什么问题：把理论落实到代码——对照 OCS2 ocs2_perceptive 的真实接口和本章的教学简化模块，理解从距离场到 MPC 约束的代码路径。下面凡是标为“教学简化”的文件名都不是承诺真实仓库中存在同名文件。

动机：为什么要读源码¶

理解算法和理解实现是两回事。论文告诉你"用插值/线性化让距离场可用于 SQP"，但代码告诉你： - 边界条件怎么处理？ - 距离场的值和梯度由谁提供？ - CppADCodeGen 生成的是整条 SDF 查询链，还是只生成末端运动学？ - clearance、地图和机器人模型变化分别会不会触发重新生成？

真实 `ocs2_perceptive` 核心结构¶

ocs2_perceptive/
├── include/ocs2_perceptive/
│   ├── interpolation/
│   │   ├── BilinearInterpolation.h        ← 核心：SDF/高程图的可微查询
│   │   └── TrilinearInterpolation.h       ← 3D 栅格插值
│   ├── distance_transform/
│   │   ├── ComputeDistanceTransform.h     ← EDT 计算模板
│   │   └── DistanceTransformInterface.h   ← 距离场值/梯度接口
│   ├── end_effector/
│   │   ├── EndEffectorDistanceConstraint.h           ← 基础约束类
│   │   └── EndEffectorDistanceConstraintCppAd.h      ← CppAD 可微版本
├── src/
│   └── end_effector/
│       ├── EndEffectorDistanceConstraint.cpp
│       └── EndEffectorDistanceConstraintCppAd.cpp
├── test/
│   └── interpolation/
│       ├── testBilinearInterpolation.cpp
│       └── testTrilinearInterpolation.cpp
└── CMakeLists.txt

版本提示：上述结构对应当前 OCS2 main 分支的核心文件。分支和发行版可能变化，源码阅读时以本机 checkout 为准。ANYmal perceptive 示例还会引入 segmented_planes_terrain_model、grid_map_sdf 等工程包，它们不全在 ocs2_perceptive 目录内。

真实模块 1：`BilinearInterpolation.h`¶

这个模块实现双线性插值的值和一阶近似，用来解释 OCS2 感知约束中“地图查询 + 梯度”的核心思想。当前 OCS2 main 分支中的接口并不接收整张栅格图，也不在函数内部查找整数索引；它只接收当前单元的参考角点 referenceCorner、四个角点值 cornerValues 和查询点 position：

namespace ocs2::bilinear_interpolation {

template <typename Scalar>
Scalar getValue(
    Scalar resolution,
    const Eigen::Matrix<Scalar, 2, 1>& referenceCorner,
    const std::array<Scalar, 4>& cornerValues,
    const Eigen::Matrix<Scalar, 2, 1>& position);

template <typename Scalar>
std::pair<Scalar, Eigen::Matrix<Scalar, 2, 1>> getLinearApproximation(
    Scalar resolution,
    const Eigen::Matrix<Scalar, 2, 1>& referenceCorner,
    const std::array<Scalar, 4>& cornerValues,
    const Eigen::Matrix<Scalar, 2, 1>& position);

}  // namespace ocs2::bilinear_interpolation

把“选哪四个格子”和“在这四个格子内部插值”拆开，是这个接口最重要的设计。距离场实现先在普通运行时代码中决定查询点落在哪个栅格单元，完成边界裁剪并取出四个角点值；随后 getLinearApproximation() 只计算当前单元内部的连续值和二维梯度：

// 运行时距离场接口内部的典型调用形态，省略边界裁剪和格子索引细节。
Eigen::Vector2d referenceCorner = gridCornerWorldPosition(i, j);
std::array<double, 4> cornerValues = {
    sdf(i, j), sdf(i + 1, j), sdf(i, j + 1), sdf(i + 1, j + 1)
};
Eigen::Vector2d position(x, y);

auto [distance, grad_xy] =
    ocs2::bilinear_interpolation::getLinearApproximation(
        resolution, referenceCorner, cornerValues, position);

关键设计：整数索引与连续插值分离

整数索引（选择哪四个格子）不是查询坐标的连续函数，它是阶跃函数。真实实现先在普通 double 空间确定参考角点和四个角点值，再调用 bilinear_interpolation::getValue() 或 getLinearApproximation() 计算值与梯度。这样做的含义是：在当前格子内部线性化，跨格子时由下一次查询重新选择角点。

不要误解为“CppAD 可以对任意地图索引自动求导”。若在录制函数里从 AD 变量强行取出普通数值，索引会变成 tape 的参数/常量，运行时不能自动跳到新的格子。OCS2 的距离约束通过 DistanceTransformInterface::getLinearApproximation() 在运行时提供局部值和梯度，避开了把整张地图录进 AD tape 的问题。

这个设计有一个微妙后果：当查询点移动越过格子边界时，梯度可能跳变（因为参与插值的四个角点变了）。工程上依赖三件事来降低影响：较细的地图分辨率、良好的 warm start，以及对 SDF/惩罚函数的适度平滑。不能把它理解成全局光滑函数。

真实接口 2.5：`DistanceTransformInterface` 与 `ComputeDistanceTransform`（源码核实）¶

在写 TeachingSignedDistanceTransform 教学模块之前，必须先把真实接口讲清楚——否则会形成一个根深蒂固的误解："OCS2 的距离场是 2D 的"。核对 main 分支源码后，真实的抽象接口签名是：

// 来源：ocs2_perceptive/.../distance_transform/DistanceTransformInterface.h（源码原文）
class DistanceTransformInterface {
 public:
  using vector3_t = Eigen::Matrix<scalar_t, 3, 1>;

  // 给定 3D 点 p，返回有符号距离值
  virtual scalar_t getValue(const vector3_t& p) const = 0;

  // 给定 3D 点 p，返回其在零等值面（障碍表面）上的投影点
  virtual vector3_t getProjectedPoint(const vector3_t& p) const = 0;

  // 给定 3D 点 p，返回 {距离值, 距离对 p 的 3D 梯度}
  virtual std::pair<scalar_t, vector3_t> getLinearApproximation(const vector3_t& p) const = 0;
};

📄 论文-代码差异（CONFLICT，源码核实）：本章 67.4 出于教学简化，把 SDF 讲成"2D EDT + 高程图垂直检查"。这在**直觉层面**没错（水平避障是主要矛盾），但在**接口层面**与真实代码不一致——DistanceTransformInterface 的三个方法全部接收 vector3_t，返回 3D 梯度。Grandia 2023 的 ANYmal 示例（SegmentedPlanesSignedDistanceField）从分割平面构建的是**3D 体素 SDF**，沿高度方向也有距离信息。约束侧的 EndEffectorDistanceConstraintCppAd 拿到的 grad 是完整的 3D 向量，与末端运动学雅可比 middleRows<3> 相乘。判断：本章前文的 2D 叙述是**降维教学近似**，适合先建立直觉；但读者读源码时会看到 3D 接口，二者必须能对上。正确理解是：距离场的数学定义和 OCS2 接口都是 3D 的；2D EDT 只是某些轻量实现（或本教学简化）选择的具体计算方式，不是接口契约。

底层的距离变换由 ComputeDistanceTransform.h 提供，它不是一个类，而是一对模板函数——关键设计是**用 lambda 解耦"数据怎么存"**：

// 来源：ocs2_perceptive/.../distance_transform/ComputeDistanceTransform.h（源码原文，简化注释）
// GetValFunc: Scalar(size_t index)        —— 取第 index 个采样点的当前值
// SetValFunc: void(size_t index, Scalar)  —— 把结果写回第 index 个采样点
template <typename GetValFunc, typename SetValFunc, typename Scalar = float>
void computeDistanceTransform(size_t numSamples, GetValFunc&& getValue, SetValFunc&& setValue,
                              size_t start, size_t end,
                              std::vector<size_t>& vBuffer, std::vector<Scalar>& zBuffer);

💡 论文没告诉你的（工程 trick，来源：ComputeDistanceTransform.h）：这个模板函数实现的就是 67.4 讲的 Felzenszwalb & Huttenlocher 一维抛物线下包络算法（vBuffer 存抛物线的分界横标、zBuffer 存交点）。它故意不接收任何具体的栅格/图像类型，而是用 getValue/setValue 两个 lambda 抽象"第 $i$ 个采样点怎么读写"。这样同一份 1D 变换代码既能在 $x$ 方向扫，也能在 $y$、$z$ 方向扫（多维 EDT = 沿各维顺序做 1D EDT），还能适配 grid_map、稠密 Eigen::Matrix 或自定义体素容器——这是论文完全不会提、但工程上极其关键的可复用性设计。

教学简化模块 2：`TeachingSignedDistanceTransform`¶

这个教学模块解释从二值可踩性图到 SDF 的计算链路。当前 OCS2 main 分支没有 TeachingSignedDistanceTransform 这个文件，也没有把下面这些步骤封装成同名类；真实代码中请查看 ComputeDistanceTransform.h、DistanceTransformInterface.h，以及 perceptive ANYmal 示例中的 SegmentedPlanesSignedDistanceField / grid_map_sdf 相关链路。下面代码是概念伪代码（按 2D 简化叙述），用来表达数据流，不是可直接编译的源码；真实链路按上面的 3D 接口工作。

// 概念伪代码：说明 SDF 计算流程，不对应 OCS2 中的真实类名。
class TeachingSignedDistanceTransform {
public:
  void compute(const grid_map::GridMap& elevation_map) {
    // 1. 从高程图计算可踩性（坡度+粗糙度+台阶检查）
    Eigen::MatrixXi steppability = classifySteppability(elevation_map);

    // 2. 膨胀不可踩区域（安全裕度）
    dilateObstacles(steppability, dilation_radius_);  // 典型 1-2 个格子

    // 3. 添加垂直边距
    addVerticalMargin(elevation_map, steppability, vertical_margin_);  // 典型 2cm

    // 4. 分别计算到障碍和到自由区的 Euclidean Distance Transform
    Eigen::MatrixXf distance_to_obstacle =
        euclideanDistanceTransform(/*zero set = non-steppable cells*/);
    Eigen::MatrixXf distance_to_free =
        euclideanDistanceTransform(/*zero set = steppable cells*/);

    // 5. 添加符号
    for (int i = 0; i < rows_; ++i)
      for (int j = 0; j < cols_; ++j)
        sdf_(i, j) = distance_to_obstacle(i, j) - distance_to_free(i, j);

    // 6. 可选：高斯模糊
    if (smooth_sigma_ > 0)
      gaussianBlur(sdf_, smooth_sigma_);
  }

  // 查询接口（MPC 线程调用）：真实实现还要做边界裁剪、
  // 角点选择、双线性插值和梯度返回。
  std::pair<float, Eigen::Vector2f> queryLinearApproximation(float x, float y) const {
    return lookupCurrentCellAndInterpolate(sdf_, x, y, resolution_, origin_x_, origin_y_);
  }

private:
  Eigen::MatrixXf sdf_;
  float resolution_, origin_x_, origin_y_;
  float dilation_radius_, vertical_margin_, smooth_sigma_;
};

真实接口 3：`EndEffectorDistanceConstraint(CppAd)`¶

EndEffectorDistanceConstraint.h 和 EndEffectorDistanceConstraintCppAd.h 把距离场查询包装成 OCS2 约束接口。读源码时第一个要注意的差异是**两者的基类不同**——这直接决定了约束依赖哪些 OCP 变量：

类	基类	约束依赖	雅可比来源
`EndEffectorDistanceConstraint`	`StateConstraint`	仅状态 $x$	`EndEffectorKinematics::getPositionLinearApproximation` 普通运动学线性化
`EndEffectorDistanceConstraintCppAd`	`StateInputConstraint`	状态 $x$ 与输入 $u$	`CppAdInterface` 生成的末端位置雅可比

📄 论文-代码差异（源码核实）：很多二手资料笼统地说"OCS2 用 CppAD 对距离约束求导"。核对 main 分支源码后更准确的说法是：距离场本身不进 AD tape。CppAd 版本只用 CppAdInterface 对"末端正运动学 $p(x)$"求导，距离值 $\phi$ 和空间梯度 $\nabla_p\phi$ 由 DistanceTransformInterface 在运行时显式提供，最后两者相乘。非 CppAd 版本连 AD 都不用，直接拿运动学的解析线性化。两个版本共享同一个 set() 注入接口。

真实的 set() 有多个重载，覆盖"不带 clearance""统一 clearance""逐末端 clearance"三种用法（EndEffectorDistanceConstraintCppAd 没有第一个重载，因为它要求显式给出 clearance）：

// 来源：ocs2_perceptive/.../EndEffectorDistanceConstraint.h（源码原文）
void set(const DistanceTransformInterface& distanceTransform);                       // clearance 全 0
void set(scalar_t clearance, const DistanceTransformInterface& distanceTransform);   // 统一 clearance
void set(const scalar_array_t& clearances, const DistanceTransformInterface&);       // 逐末端 clearance

下面是对齐 main 分支结构的 CppAd 版本骨架。相比早期教学稿，这里补上了真实存在的 Config（权重 + 是否生成模型 + 是否打印日志）和 getQuadraticApproximation：

// 教学简化（结构对齐 main 分支，省略构造细节）
class EndEffectorDistanceConstraintCppAd final : public ocs2::StateInputConstraint {
 public:
  struct Config {                       // 源码原文：默认 weight=1, generateModel=true, verbose=true
    scalar_t weight;
    bool generateModel;
    bool verbose;
  };

  void set(vector_t clearances, const DistanceTransformInterface& distanceTransform) {
    clearances_ = std::move(clearances);
    distanceTransformPtr_ = &distanceTransform;
  }

  VectorFunctionLinearApproximation getLinearApproximation(
      scalar_t t, const vector_t& state, const vector_t& input,
      const PreComputation& preComp) const override {
    // 末端位置与其对 (x) 的雅可比：CppAD 生成的部分仅限这一步
    auto eePositions = kinematicsModelPtr_->getFunctionValue(state);
    auto eeJacobians = kinematicsModelPtr_->getJacobian(state);

    const size_t numEEs = clearances_.size();
    VectorFunctionLinearApproximation approx =
        VectorFunctionLinearApproximation::Zero(numEEs, stateDim_, inputDim_);
    for (size_t i = 0; i < numEEs; ++i) {
      // 距离值 + 3D 空间梯度：运行时由距离场提供，不在 AD tape 内
      auto [distance, grad] =
          distanceTransformPtr_->getLinearApproximation(
              eePositions.segment<3>(3 * i));
      approx.f(i) = config_.weight * (distance - clearances_(i));
      approx.dfdx.row(i) = config_.weight * grad.transpose()
                          * eeJacobians.middleRows<3>(3 * i);   // 链式法则的显式相乘
    }
    return approx;
  }

 private:
  Config config_;
  std::unique_ptr<CppAdInterface> kinematicsModelPtr_;
  const DistanceTransformInterface* distanceTransformPtr_ = nullptr;
  vector_t clearances_;
};

配置文件解读¶

OCS2 perceptive 的配置通过 .info 文件管理（与足式/110_OCS2完整栈与双线程MPC 中的 task.info 类似）：

; perceptive_task.info 关键配置项

[perception]
elevation_map_topic = "/elevation_mapping/elevation_map"
map_resolution = 0.04          ; 4cm 分辨率
map_size_x = 4.0               ; 4m x 4m 范围
map_size_y = 4.0

[steppability]
max_slope = 0.6                ; ~34 度 (tan 值)
max_roughness = 0.05           ; 5cm
max_step_height = 0.15         ; 15cm

[sdf]
dilation_radius = 1            ; 膨胀 1 个格子
vertical_margin = 0.02         ; 2cm 垂直边距
smooth_sigma = 0.5             ; 高斯模糊标准差（格子数）
min_distance = 0.05            ; 5cm 最小安全距离

[cost_weights]
body_height_tracking = 100.0
body_clearance = 30.0
swing_terrain_avoidance = 50.0

[solver]
mpc_frequency = 100            ; Hz
sqp_iterations = 1             ; RTI: 只做 1 次 SQP 迭代
hpipm_mode = "SPEED"           ; HPIPM 求解模式

配置项注解（HPIPM 模式）：hpipm_mode 有 SPEED_ABS、SPEED、BALANCE、ROBUST 几档，本质是内点法**精度/迭代次数与速度**的权衡。SPEED 用较松的收敛容差换最少迭代，适合 100 Hz 实时 MPC（每周期只有 5-10ms）；ROBUST 收敛更稳但更慢，适合离线或对数值稳定要求极高的场景。加入 SDF 约束后 QP 子问题变大，若出现求解不稳，可先尝试 BALANCE 再决定是否降频——这比盲目增大 sqp_iterations 更对症。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：分清运行时参数和 AD 代码生成内容 错误理解：修改了 SDF clearance / min_distance 后，必须删除 CppADCodeGen 的 .so 文件。 实际情况：在 OCS2 的 EndEffectorDistanceConstraint(CppAd) 中，clearance 和距离场通过 set(clearance, distanceTransform) 在运行时传入；修改这类配置不需要重新生成运动学 .so。 什么时候才需要重生成：改变 AD 表达式结构、状态维度、末端数量、机器人模型或被录进 tape 的常量时，旧的生成库才会失效。修改地图、clearance、权重等运行时数据，应优先检查配置是否被节点重新加载，而不是盲目删除代码生成缓存。

💡 概念误区：认为 ocs2_perceptive 是独立模块，可以单独使用 新手想法："我只编译 ocs2_perceptive 就够了" 实际上：ocs2_perceptive 依赖 ocs2_core（基础类型）、ocs2_oc（最优控制接口）、ocs2_pinocchio（运动学）、ocs2_robotic_tools（工具函数）等多个模块。它不是独立可用的——必须在 OCS2 的完整编译环境中使用。推荐用 catkin 或 colcon 编译整个 OCS2 workspace。

练习¶

[源码阅读题] 克隆 OCS2 仓库（https://github.com/leggedrobotics/ocs2），找到 ocs2_perceptive/include/ocs2_perceptive/interpolation/BilinearInterpolation.h。阅读完整实现，回答：(a) getValue() 与 getLinearApproximation() 的输入分别是什么？(b) 为什么它不接收整张栅格图？(c) 边界裁剪和四个角点选择应该由哪一层代码负责？
[源码阅读题] 阅读 EndEffectorDistanceConstraintCppAd.h 的完整实现。画出从 MPC 调用 getLinearApproximation() 到最终得到雅可比矩阵的完整调用链（包括 CppADCodeGen 的 .so 加载和调用）。
[实操题] 修改 OCS2 perceptive 的配置文件，把 min_distance 从 5cm 改为 10cm。观察 MPC 在仿真中的行为变化——摆动腿是否抬得更高了？有没有出现 SQP 不收敛的情况？

🔬 研究视角：Grandia 2023 的贡献结构分析 ⭐⭐⭐¶

本节解决什么问题：前七节讲清了 Grandia 2023"做了什么、怎么做的"。本节从**研究方法论**的角度退一步问：这篇论文为什么够发 T-RO？它的贡献是如何分层的？读完它，你应该学会的不只是"感知 MPC 怎么写"，而是"如何评估一篇系统类机器人论文的贡献结构"。这是论文解读教学区别于普通技术讲解的核心价值。

维度一：问题定义贡献——它开创了新问题吗？¶

Grandia 2023 没有**开创"感知运动"这个问题——Fankhauser (2014) 的高程图、Mastalli (2016) 的地形感知规划、Jenelten (2022) 的 TAMOLS 都在它之前。它的问题定义贡献更精确地说是**问题的重新框定（reframing）：

把"感知运动"从"离线/准静态地形优化问题"重新框定为"在线、全自由度、实时 NMPC 问题"。这个 reframing 不是换个说法，而是抬高了难度门槛——它要求在 100 Hz 的预算内，同时处理离散感知数据、非凸碰撞约束、全身动力学。

本质洞察：系统类论文的"问题定义贡献"往往不是发明新问题，而是**把一个已知问题推到一个新的、更难的工作点**（operating point）。判断这类贡献的价值，要看这个新工作点是否"卡在真实部署的关键路径上"。Grandia 2023 选的工作点——实时 + 全自由度 + 真机——恰恰是工业落地必须跨过、而此前没人系统跨过的那道坎。这就是它的问题定义价值。

维度二：框架贡献——它给出了完整端到端方案吗？¶

这是 Grandia 2023 最强的一维。它的框架贡献可以拆成三个"翻译层"（这也是本章 67.3-67.5 的主线）：

翻译层	把什么翻译成什么	关键技术	没有它会怎样（反事实）
几何翻译	离散栅格 -> 凸落脚约束	平面分割 + 半空间不等式	落脚约束非凸，HPIPM 实时性崩溃
距离翻译	障碍占据 -> 可微距离场	SDF + 双线性插值	优化器没有"远离障碍的方向"，无法做梯度步
时间翻译	20 Hz 异步感知 -> 100 Hz 同步优化	三线程 + double/triple buffer	数据竞争导致 SQP 内梯度跳变、不收敛

本质洞察：框架贡献的"完整性"不在于模块多，而在于**模块之间的接口是否闭合**——上一层的输出恰好是下一层能直接消费的输入，中间没有需要人工干预的缝隙。Grandia 2023 的三个翻译层首尾相接：感知前端吐出凸约束 + SDF + 参考高度，MPC 直接装进 OCP。这种"接口闭合"才是它能在真机上 24/7 跑起来的根本原因，也是区分"demo 级"和"系统级"工作的分水岭。

维度三：实验方法论贡献——消融是否系统？¶

Grandia 2023 的实验在 gaps（间隙）、slopes（斜坡）、stepping stones（踏石）三类地形上做了仿真 + ANYmal 真机验证（来源：论文 §VII，WebSearch 核实摘要）。从方法论看，它的实验设计有两个值得学习的点：

任务难度梯度：三类地形对应三种不同的失效模式（间隙=落脚不可行、斜坡=高度不匹配、踏石=落脚精度），系统性地覆盖了 67.1 提出的"盲 MPC 三重失效"。这不是随便选的地形，而是**针对动机逐项验证**。
真机闭环：很多优化类论文止步于仿真。Grandia 2023 在 ANYmal 上跑通，证明了延迟预算（67.9）和降级策略在真实噪声下成立。

📄 论文没告诉你的（实验复现，来源：未提及/无开源脚本）：论文给出了实验结论，但**没有公开端到端复现脚本**（映射表中标 UNSPECIFIED）。这意味着想精确复现 Figure/Table 的数字，需要自己拼装 ocs2_perceptive + convex_plane_decomposition + elevation_mapping + ANYmal 模型，并自行调参。这是系统类论文的普遍现象——算法开源 $\neq$ 实验可一键复现。评估这类论文时，要区分"方法可信"（核心模块开源、接口清晰）和"数字可复现"（需要完整环境与调参），两者是不同的可信度等级。

局限：从 Grandia 2023 通向下一篇的动机¶

任何论文的局限都是下一篇的起点。Grandia 2023 有三个明确局限，恰好驱动了本章后续两节（67.8 的 RL 对比、67.10 的前沿）的内容：

依赖确定性感知：MPC 把高程图当确定性输入，没有概率框架。感知噪声直接传播为轨迹误差（67.9 的 sim-to-real 调参就是在补偿这一点）。-> 驱动 Miki 2022 的 RL 方案，用 domain randomization 学会对噪声鲁棒（67.8）。
手工设计的代价/约束：三个地形代价的权重靠人工调（67.5）。固定权重在所有测试地形上"够用"，但不是每种地形的最优。-> 驱动 DTC (Jenelten 2024) 等"用 RL 学习 MPC 参数/参考"的混合范式（67.8、67.10）。
几何感知，非语义感知：可踩性只看几何（坡度/粗糙度/台阶），区分不了"看起来平但很滑的冰面"或"草丛 vs 实地"。-> 驱动 **NaVILA (2025) 等把感知从几何扩展到语义**的方向（67.10）。

这三个局限不是缺陷，而是**把 Grandia 2023 精确定位在技术树上的坐标**：它是"经典 MPC + 几何感知 + 确定性"这一支的集大成者；它的每个边界，都是另一条技术路线的入口。下一节就从它最直接的"对手兼互补者"——Miki 2022 的 RL 方案——开始。

67.8 RL vs MPC 感知运动对比 ⭐⭐⭐¶

本节解决什么问题：系统对比两种感知运动方案——Miki 2022 的 RL 方案（Science Robotics）和 Grandia 2023 的 MPC 方案（T-RO）——帮助理解各自的优势、局限和适用场景。

动机：为什么需要对比¶

2022-2023 年，ETH 同一个实验室（RSL）同时产出了两篇标杆论文，分别代表感知运动控制的两条技术路线。它们的目标相同（让四足机器人在崎岖地形上稳健行走），但方法论截然不同。理解这两条路线的差异和互补性，对于选择自己的研究方向至关重要。

Miki 2022：RL 方案概述¶

论文：Miki T., Lee J., Hwangbo J., et al. (2022) "Learning robust perceptive locomotion for quadrupedal robots in the wild" -- Science Robotics, Vol. 7, eabk2822.

核心思想：用强化学习在仿真中训练一个**端到端**的感知运动策略，直接从本体感知（关节角度、IMU）和外界感知（高程图扫描）映射到关节目标位置。

架构：

Teacher-Student 训练框架

训练阶段（仿真环境，Isaac Gym）：
  Teacher 策略:
    输入: 特权信息（精确地形、精确状态、接触力）
    输出: 关节目标位置
    训练: PPO，奖励 = 速度跟踪 + 稳定性 + 能耗

  Student 策略:
    输入: 可观测信息（关节角/速度、IMU、高程图扫描）
    输出: 关节目标位置
    训练: 行为克隆 Teacher + PPO 微调

  关键模块——注意力编码器（Attention-based Encoder）:
    输入: 本体感知 + 外界感知（高程图扫描点）
    输出: 潜在表示 z（用于 Student 策略）
    作用: 学习融合不同模态的感知，自动处理遮挡/噪声

部署阶段（真实 ANYmal）：
  只部署 Student 策略
  输入: 真实传感器数据 → 推理 → 关节 PD 目标
  频率: 100 Hz（神经网络推理）

Grandia 2023：MPC 方案概述¶

（前面几节已详述，这里做简要总结以便对比。）

核心思想：把地形信息显式嵌入 NMPC 的代价函数和约束中，用 SQP+HPIPM 实时求解全自由度最优轨迹。

架构：感知管线（20 Hz）--> NMPC（100 Hz）--> WBC（400 Hz）

全面对比¶

维度	Miki 2022 (RL)	Grandia 2023 (MPC)
方法论	端到端学习	基于模型的优化
感知处理	隐式（编码器自动学习）	显式（SDF、平面分割）
动力学模型	不需要（仿真中隐式学习）	需要（Centroidal/全身模型）
训练/调参	训练 24-48h（GPU 集群），奖励函数设计	无训练，代价/约束权重调参
部署推理	神经网络前向传播（<1ms）	SQP 求解（5-10ms）
可解释性	低（黑盒策略）	高（每个约束/代价都有物理含义）
安全保证	无理论保证（依赖训练覆盖）	约束满足有理论保证（SQP 收敛时）
泛化能力	强（训练中见过多样地形）	弱（依赖精确的感知前端）
极端地形	更好（见过极端训练样本）	受限（SQP 可能不收敛）
新机器人适配	需要重新训练（sim-to-real gap）	更换 URDF + 配置即可
实时修改	困难（需要重新训练）	容易（修改配置文件）
感知失效处理	隐式降级（编码器学会忽略坏数据）	显式降级（切换到盲 MPC）

更深层的差异分析¶

差异 1：如何处理感知不确定性

MPC 方案把感知数据当作"确定性输入"——高程图说高度是 0.3m，MPC 就假设是 0.3m。如果高程图有 3cm 噪声，MPC 的轨迹就有 3cm 误差。Grandia 2023 通过高斯模糊和安全裕度来部分补偿，但没有**概率框架**。

RL 方案在训练时就见过各种噪声水平的感知数据（domain randomization），编码器自动学会了在不确定时"保守行动"。这种鲁棒性不是设计出来的，而是训练出来的。

差异 2：如何利用预测视野

MPC 的核心优势是**多步预测**——在 1-2 秒的视野内优化未来轨迹。这在 stepping stones 等需要前瞻规划的场景中至关重要。

RL 策略通常是**反应式**的——只看当前观测，输出当前动作。虽然 LSTM/Transformer 可以隐式编码短期历史，但没有显式的多步规划能力。

差异 3：如何应对新场景

当机器人遇到训练/设计时没有考虑的新场景（如泥地、冰面、动态障碍），两种方案的表现不同：

MPC：如果感知前端能提供正确的地形信息，MPC 自动适应（因为优化是在线的）。但如果感知失效（如泥地没有被正确标记为不可踩），MPC 会生成不安全的轨迹。
RL：如果新场景与训练分布"足够接近"，策略可以泛化。但如果完全 out-of-distribution，策略的行为不可预测——可能突然摔倒，而且没有可解释的原因。

混合方案：DTC 和未来方向¶

鉴于 RL 和 MPC 各有优势，近年的趋势是**混合**：

DTC（Deep Tracking Control, Jenelten 2024, Science Robotics, arXiv:2309.15462）：TO/MPC 生成参考轨迹，RL 策略负责跟踪。注意方向——是优化器（基于模型、planning 准确、可泛化）输出运动参考，RL（离线学习、对模型失配鲁棒）把这条参考在真实动力学下执行下去。论文原话是"prior knowledge of motion can be rolled out from MPC"，RL 策略学的是"如何稳健地跟踪 MPC 给的参考"。

⚠️ 常见记反的点（事实核实）：很多人把 DTC 说成"RL 出参考、MPC 跟踪"——方向反了。DTC 的设计动机恰恰是：MPC 擅长**规划**（用模型前瞻、给出最优且满足约束的参考），但在真实机器人上因模型失配而**执行**不稳；RL 擅长**执行**（从数据中学会对抗失配），但缺乏前瞻规划。所以让各自做擅长的：MPC 规划参考，RL 跟踪执行。这正好把 Grandia 2023 的局限（确定性感知、执行端对噪声敏感，见 67.7 后 🔬 研究视角）补上了——参考仍由可审计的 MPC 给出，鲁棒性由 RL 兜住。

分工	谁来做	为什么
感知 + 规划参考	TO/MPC（保留约束满足）	模型前瞻准确、可泛化、可审计
真实执行跟踪	RL 策略	离线学习对模型失配/噪声鲁棒

RL-augmented MPC（2023-2025 多项工作）：用 RL 学习 MPC 的代价函数权重或终端代价，使 MPC 能适应更多场景。

混合方案的演进

Level 1: 独立
  RL 和 MPC 各做各的，选一个部署

Level 2: 串联（DTC）
  RL → 参考轨迹 → MPC → 控制

Level 3: 融合（RL-augmented MPC）
  RL 学习 MPC 的代价/约束参数 → MPC 优化

Level 4: 统一（未来方向）
  可微 MPC 作为 RL 的一个层 → 端到端训练
  (Amos & Kolter 2017 "OptNet", Agrawal 2019 "differentiable MPC")

⚠️ 常见陷阱¶

🧠 思维陷阱：认为"RL 比 MPC 好"或"MPC 比 RL 好" 新手想法："看了 ANYmal Parkour (RL) 的视频，MPC 完全不行啊" 实际上：RL 在极限性能上确实超越 MPC（跳跃、翻越、跑酷）。但在工业部署中，MPC 的优势是**可解释性**和**可调性**——当机器人在客户现场出问题时，工程师可以看 MPC 的约束违反记录找到原因，但 RL 策略只能"重新训练一个版本试试"。 正确思维：选择取决于应用场景。工业巡检（安全优先）--> MPC；极端地形探索（性能优先）--> RL；复杂但需可靠（两者兼顾）--> 混合。

💡 概念误区：认为 Miki 2022 不需要任何模型 新手想法："RL 是 model-free 的，完全不需要动力学模型" 实际上：Miki 2022 的训练需要一个**精确的仿真环境**（Isaac Gym + ANYmal URDF + 地形生成器）。仿真本身就是一个模型——而且对 sim-to-real 的成功至关重要。如果仿真模型不准确（如接触模型错误），训练出来的策略无法迁移到真实机器人。 正确理解：RL 不是 "model-free"——它只是不在**策略执行时**显式使用模型，但在**训练时**隐式依赖仿真模型。

练习¶

[对比分析题] 假设你需要为一个在建筑工地巡检的四足机器人选择控制方案。工地有临时堆放的建材（动态障碍）、斜坡、脚手架（狭窄通道）、积水。你会选 RL、MPC 还是混合方案？给出理由，列出各方案的风险。
[设计题] 设计一个 "Level 3" 的 RL-augmented MPC 系统：RL 学习 MPC 的三个地形感知代价权重（$w_h, w_c, w_s$）。RL 的观测空间、动作空间、奖励函数分别应该是什么？
[论文阅读题] 阅读 Miki et al. 2022 的 Section III (Method)。回答：(a) Teacher 策略的特权信息包括哪些？(b) 注意力编码器为什么比 MLP 更适合融合多模态感知？(c) Domain randomization 随机化了哪些参数？

67.9 部署实践 ⭐⭐¶

本节解决什么问题：从实验室到真实世界的部署，需要解决延迟预算、计算资源分配、降级策略等工程问题。

动机：算法正确不等于部署成功¶

一个在仿真中完美工作的 Perceptive MPC 系统，部署到真实机器人上可能面对： - 传感器延迟比仿真中大 3 倍 - CPU 负载导致 MPC 频率降到 60 Hz - 阳光直射导致深度相机失效 - 机器人振动导致点云模糊

延迟预算¶

部署的首要任务是建立**延迟预算表**（latency budget）——每个模块允许多少毫秒：

模块	预算	实测（ANYmal D）	余量	超预算后果
点云获取	10 ms	5-8 ms	充足	地图更新延迟
Elevation Mapping	15 ms	10-15 ms	紧张	地图过期
可踩性+分割+SDF	10 ms	5-8 ms	充足	MPC 用旧数据
MPC 求解	10 ms	5-10 ms	紧张	控制延迟，WBC 用旧轨迹
WBC 求解	2.5 ms	1-2 ms	充足	关节指令延迟
通信+其他	2.5 ms	1-2 ms	充足	--
总计	50 ms	30-45 ms	--	--

关键瓶颈：MPC 求解是最紧张的环节。RTI（Real-Time Iteration）只做 1 次 SQP 迭代，把求解时间控制在 10ms 以内。但加入感知约束后，HPIPM 的 QP 子问题变大（多了 SDF 约束），求解时间可能增加 2-3ms。

计算资源分配¶

ANYmal D 搭载 Intel i7 NUC（4 核 8 线程）+ NVIDIA Jetson Xavier NX（GPU）。资源分配如下：

CPU 核心绑定（isolcpus 策略）

Core 0-1: 系统 + ROS 通信
Core 2:   感知线程（Elevation Mapping, 分类, SDF）
Core 3:   MPC 线程（SQP + HPIPM）
Core 4:   MRT 线程（WBC + 状态估计）
Core 5-7: 空闲 / 用户程序

GPU (Xavier NX):
  elevation_mapping_cupy（GPU 加速高程图更新）
  深度图像处理

关键: MPC 线程绑定到专用核心，设置 SCHED_FIFO 实时调度
     优先级: MRT > MPC > 感知 > 其他

降级模式（Degraded Mode）¶

感知系统不可能永远正常工作。部署必须有降级策略：

故障	检测方式	降级行为	恢复条件
深度相机失效	无点云 >0.5s	冻结高程图 + 减速	点云恢复
高程图过期	时间戳检查 >1s	切换到盲 MPC	高程图更新
SDF 计算超时	watchdog	用上一帧 SDF	SDF 更新
MPC 不收敛	SQP 残差过大	用上一帧轨迹 + 减速	SQP 收敛
所有感知失效	多重检测	完全盲 MPC + 减速至停	人工干预

降级不是失败——是设计。ANYmal 的工程实践中，约 5-10% 的运行时间处于某种降级模式。重要的是降级是**平滑的**（不是突然切换），并且能**自动恢复**。

地图更新频率与 MPC 频率的协调¶

一个微妙的问题：高程图以 20 Hz 更新，MPC 以 100 Hz 运行。MPC 的 5 次求解共享同一张高程图。如果机器人在这 50ms 内移动了 2.5cm（@0.5m/s），MPC 在第 5 次求解时看到的地形已经"偏移"了半个格子。

处理方式：MPC 在查询高程图时，用当前的**状态估计**做坐标变换——把查询点从当前机器人坐标系变换到高程图的坐标系。状态估计以 400 Hz 更新，所以坐标变换是准确的。

MPC 查询高程图的坐标变换

每次 MPC 查询 SDF(x_foot, y_foot):
  1. x_foot, y_foot 是在 MPC 模型坐标系中的位置
  2. 用最新的状态估计 T_world_base 变换到世界坐标系
  3. 用高程图的 T_world_map 变换到高程图坐标系
  4. 在高程图坐标系中做双线性插值

注意: 如果 SLAM 发生回环修正, T_world_map 可能突然跳变
     → 用指数平滑过渡, 避免 MPC 看到的地形突然"跳"

⚠️ 常见陷阱¶

⚠️ 编程陷阱：MPC 线程没有设置实时优先级 错误做法：用默认的 SCHED_OTHER 调度策略运行 MPC 线程。现象：当系统负载高时（如 RViz 渲染、rosbag 录制），MPC 线程被抢占，求解时间从 8ms 突然跳到 30ms --> MPC 频率降到 33 Hz --> 控制器响应迟钝，机器人走路不稳。 正确做法：用 pthread_setschedparam 设置 SCHED_FIFO + 高优先级，或者用 chrt -f 90 ./mpc_node 启动。同时用 isolcpus 把 MPC 绑定到专用 CPU 核心。

🧠 思维陷阱：认为仿真中调好的参数可以直接用于实机 新手想法："仿真中 $w_h = 100$ 效果完美，实机也用这个" 实际上：仿真的传感器模型（完美点云、零延迟）与实际传感器（噪声、遮挡、延迟）差异显著。通常需要在实机上重新调参——增大安全裕度、降低代价权重（避免过激反应）、增加地图平滑（降噪）。典型的 sim-to-real 调参工作量是仿真调参的 2-3 倍。

SDF 梯度计算的实现细节 ⭐⭐⭐¶

SDF 碰撞约束是 Perceptive MPC 的核心。SDF 值通过双线性插值从离散栅格获得：

\[d(x, y) = (1-s)(1-t) \cdot d_{00} + s(1-t) \cdot d_{10} + (1-s)t \cdot d_{01} + st \cdot d_{11}\]

其中 $s = (x - x_0) / \Delta x$，$t = (y - y_0) / \Delta y$。对 $x$ 和 $y$ 的梯度为：

\[\frac{\partial d}{\partial x} = \frac{1}{\Delta x}\left[(1-t)(d_{10} - d_{00}) + t(d_{11} - d_{01})\right]\]

\[\frac{\partial d}{\partial y} = \frac{1}{\Delta y}\left[(1-s)(d_{01} - d_{00}) + s(d_{11} - d_{10})\right]\]

这些梯度先在距离场接口中作为 $\nabla_p d$ 给出，再与末端运动学雅可比相乘传播到 MPC 的决策变量。OCS2 的 BilinearInterpolation 提供插值值和局部梯度，EndEffectorDistanceConstraint(CppAd) 负责把距离场梯度与末端位置雅可比组合起来。

部署延迟优化技巧 ⭐⭐¶

优化手段	延迟节省	实现难度	说明
SDF 预计算（离线 EDT）	~5 ms/次	低	每次高程图更新后离线计算 SDF，MPC 只查表
SDF 分辨率降低（0.04m → 0.08m）	~2 ms	低	牺牲精度换速度，对大障碍物够用
感知线程与 MPC 线程异步	~3-5 ms	中	MPC 不等待最新地图，用上一帧地图
约束稀疏化（只检查摆动腿）	~1-2 ms	低	支撑腿已在地面，无需碰撞检查
初始猜测热启动	~2-3 ms	中	用上一次 MPC 的解平移作为初始猜测

练习¶

[计算题] 一个四足机器人搭载 Intel i7-1260P（4 性能核 + 8 能效核）和 NVIDIA Jetson Orin NX。设计 CPU 核心分配方案，并计算每个线程的最大允许延迟（假设 MPC 目标频率 100 Hz、WBC 目标频率 500 Hz）。
[设计题] 为 Perceptive MPC 系统设计一个完整的降级状态机（state machine）：定义所有降级状态、状态间的转移条件、每个状态下的行为。用 UML 状态图表示。
[分析题] 如果 SLAM 系统在 MPC 运行期间触发了回环修正，机器人的全局位姿跳变了 5cm。这对 Perceptive MPC 有什么影响？高程图的坐标系也跳变了 5cm 吗？MPC 应该怎么处理？

67.10 前沿：可微仿真与端到端感知运动 ⭐⭐⭐⭐¶

本节解决什么问题：Grandia 2023 的 Perceptive MPC 是"感知数据 -> 手工设计的约束/代价 -> SQP 求解"的经典管线。近年来，两条新路径正在挑战这一范式：(1) 可微仿真使地形交互可以端到端反向传播；(2) 端到端学习直接从感知到动作，跳过显式 MPC。

可微仿真在 Perceptive MPC 中的应用（DiffTaichi / Warp / MJX）¶

核心思想：传统 MPC 把地形视为"外部查表数据"——SQP 每步查询高程图得到约束值和梯度。可微仿真（differentiable simulation）提供了另一条路：直接把地形交互（接触、摩擦、碰撞）放进可微的物理引擎中，让梯度从仿真结果一路反传到控制输入。

代表性可微仿真框架：

框架	开发者	后端	地形支持	适用场景
DiffTaichi (Hu et al., ICLR 2020)	MIT CSAIL	Taichi (CPU/GPU)	高程图 heightfield	研究原型、可微物理推导
NVIDIA Warp	NVIDIA	CUDA	SDF + mesh	工业级可微仿真、大规模并行
MuJoCo MJX (Freeman et al., 2021)	Google DeepMind	JAX (XLA)	高程图 hfield	大规模 RL 训练、可微 MPC
Drake + JAX	MIT Robot Locomotion Group	JAX	接触几何	接触隐式优化

可微地形交互的数学本质：

在传统 MPC 中，地形约束 $g(\mathbf{x}, M) \geq 0$ 的梯度 $\partial g / \partial \mathbf{x}$ 通过查表+有限差分或解析公式获得。在可微仿真中，接触力 $\lambda$ 本身是优化问题的解：

\[\lambda^* = \arg\min_{\lambda \geq 0} \frac{1}{2} \lambda^T A \lambda + \lambda^T b\]

通过对这个内层 QP 应用隐函数定理（Implicit Function Theorem），可以得到 $\partial \lambda^* / \partial \mathbf{q}$——即接触力相对于关节状态的精确梯度。这使得 MPC 可以"感知"到地形摩擦、弹性等物理属性的变化，而不仅仅是几何形状。

Warp 在 Perceptive MPC 中的应用前景：

NVIDIA Warp 提供了 CUDA 加速的可微 SDF 查询，可以直接替代 OCS2 中手工实现的 BilinearInterpolation + EndEffectorDistanceConstraint：

# Warp 的可微 SDF 查询（概念示意）
import warp as wp

@wp.kernel
def sdf_constraint(
    foot_positions: wp.array(dtype=wp.vec3),
    sdf_volume: wp.Volume,
    distances: wp.array(dtype=float),
    gradients: wp.array(dtype=wp.vec3)):

    tid = wp.tid()
    p = foot_positions[tid]

    # 可微 SDF 查询——自动获得值和梯度
    d = wp.volume_sample_f(sdf_volume, p, wp.Volume.LINEAR)
    distances[tid] = d

    # 梯度通过 Warp 的自动微分自动计算
    gradients[tid] = wp.volume_sample_grad_f(sdf_volume, p, wp.Volume.LINEAR)

端到端感知运动（End-to-End Perceptive Locomotion，2024-2026 进展）¶

核心问题：Grandia 2023 的管线有 6 个模块（高程图 -> 分类 -> 分割 -> SDF -> MPC -> WBC），每个模块的误差会累积传播。能否用端到端学习直接从原始感知到关节动作，跳过中间的手工管线？

2024-2026 标志性进展：

工作	年份	方法	关键创新	与 Perceptive MPC 的关系
ANYmal Parkour (Hoeller et al.)	2024, Science Robotics	纯 RL + 深度图	在极端地形上超越 MPC	证明端到端 RL 在感知运动上的上限
Extreme Parkour (Cheng et al.)	2024, ICRA	RL + 深度图 + 特权学习	跑酷级别的敏捷运动	不需要高程图/SDF 等中间表示
DTC (Jenelten et al.)	2024, Science Robotics	RL + MPC 混合	MPC 生成参考 + RL 跟踪补偿	保留 MPC 的约束满足能力
High-speed discrete terrain (Jenelten et al.)	2025, Science Robotics	RL + 离散落脚	复杂离散地形上的高速控制导航	把 DTC 路线推向高速 + 踏石极限
Attention map encoding (Lee et al.)	2025, Science Robotics	RL + 注意力地图编码	泛化的腿足运动地图表征	给"感知如何编码进策略"提供新表征
NaVILA	2025, RSS	VLA + 运动策略	语言指令驱动的感知导航运动	把感知从几何扩展到语义

本质洞察：把上表按年份纵看，2023→2025 有一条清晰的演进主轴——感知运动的"竞争前沿"正从"能不能过崎岖地形"转向"能多快、多泛化、多语义地过"。Grandia 2023 解决了"能不能"（约束满足保证通过性）；DTC 2024 和 Jenelten 2025 把战线推向"多快"（高速 + 离散踏石）；Lee 2025 的注意力地图编码攻"多泛化"（一个表征适配多地形）；NaVILA 攻"多语义"（听懂"走到那张桌子旁"）。Perceptive MPC 在这条主轴上的位置没有被淘汰，而是**沉淀为"安全基座"**——越往高速/语义走，越需要一个可证明约束满足的底层来兜底，这正是 DTC 把 MPC 留作参考生成器的根本原因。

端到端 vs 模块化管线的深层对比：

维度	Perceptive MPC（模块化）	端到端 RL
可解释性	每个模块的输出可检查（高程图、SDF、约束值）	黑盒——策略失败时难以定位原因
安全保证	MPC 的约束满足提供形式化安全保证	无形式化保证——只有统计安全性
泛化能力	受限于手工设计的特征（坡度、粗糙度等）	可以学到人类未想到的特征
极限性能	受限于 SQP 的实时性和模型精度	在训练分布内可达到更高的敏捷性
部署可靠性	管线中任何模块故障可检测和降级	一旦策略失效，通常是灾难性的

本质洞察：Perceptive MPC 和端到端 RL 的分歧**不是**"哪个更好"的问题，而是"安全保证 vs 极限性能"的根本权衡。对于工业部署（矿井巡检、管道检修），Perceptive MPC 的可审计性和降级能力更重要——你需要向客户解释"为什么机器人做了这个决策"。对于研究探索（parkour、极端地形），端到端 RL 能达到更高的性能上限。2024-2026 的趋势是**融合两者**——用 MPC 提供安全框架和约束满足，用 RL 在 MPC 的约束空间内学习最优行为（DTC 模式），同时用可微仿真打通两者之间的梯度通道。

⚠️ 常见陷阱¶

🧠 思维陷阱：认为"可微仿真可以让 MPC 自动学会处理地形" 新手想法："有了可微仿真，MPC 的代价函数权重和约束参数可以自动学出来，不需要手动调了" 实际上：可微仿真的梯度在接触不连续点（如脚着地/离地瞬间）处是不准确的——接触是一个互补问题，其解在接触/非接触边界上不可微。Randomized smoothing 和 relaxation 等技术可以缓解这个问题，但完全消除还需要更多研究。 正确理解：可微仿真的梯度在接触**持续**阶段是准确的，在接触**切换**瞬间需要特殊处理。

练习¶

[文献调研] 阅读 DTC（Jenelten et al., 2024, Science Robotics）的方法部分，画出 MPC 参考生成 + RL 跟踪策略的完整数据流图。与 Grandia 2023 的纯 MPC 管线对比：哪些模块被 RL 替代了？哪些被保留了？
[跨章综合题] 综合本章的 Perceptive MPC 管线（67.2-67.9）和足式/210_RL与MPC混合范式的 RL+MPC 混合范式，设计一个"DTC-style Perceptive Controller"：MPC 利用高程图和 SDF 生成安全参考轨迹，RL 策略从深度图直接学习残差补偿。定义 MPC 和 RL 各自的输入/输出接口、运行频率和责任边界。讨论当感知失效时，这个混合架构如何降级。

🔧 故障排查手册¶

症状	可能原因	排查步骤	相关小节
MPC 输出 NaN 或求解失败（SQP 不收敛）	SDF 查询返回异常值（超出地图范围返回 0 而非安全默认值），或地形约束与物理约束矛盾导致不可行	1. 打印每次 SQP 迭代的 cost 和 constraint violation 2. 检查 SDF 边界处理逻辑 3. 临时关闭地形约束确认是否为感知数据导致 4. 检查初始猜测是否在可行域内	67.4, 67.5
摆动腿仍然碰撞地形——虽然有 SDF 约束	SDF 分辨率过粗导致小障碍物被"平滑掉"，或 SDF 膨胀半径小于机器人脚的实际尺寸	1. 在 RViz 中叠加 SDF 等高线和实际地形 2. 检查膨胀半径是否 >= 脚掌半径 + 安全裕度 3. 减小 SDF 分辨率（0.04m -> 0.02m）看碰撞是否消失	67.4
感知线程和 MPC 线程之间数据不一致——机器人在平地上也出现异常避障动作	double buffer 交换时序错误，MPC 读到了只写了一半的感知数据	1. 在 double buffer 的读写两端加时间戳日志 2. 确认交换操作是原子的（`std::atomic<int>` 指针交换） 3. 在 MPC 侧检查 SDF 场的数据完整性（无全零或全 NaN 行）	67.2
躯体高度跟踪在上坡时过度补偿——机器人过度下蹲导致膝关节力矩过大	地形参考高度 $z_{\text{ref}}(x,y)$ 的平滑窗口过小，地形噪声直接传递到高度参考	1. 可视化 $z_{\text{ref}}$ 层的时间序列 2. 增大高度参考的空间平滑窗口（从 3x3 到 7x7） 3. 降低高度跟踪代价权重 $w_h$	67.5
部署后整体延迟超出预算（端到端 > 100ms）——机器人反应迟钝	感知管线某个模块耗时异常（常见：点云预处理未降采样、SDF 计算未用 EDT 而用暴力搜索）	1. 对每个模块单独计时（用 `std::chrono` 或 `ros2 topic delay`） 2. 找到最慢的模块并优化 3. 参考 67.2 频率预算表检查各模块是否达标	67.2, 67.9

67.11 本章小结¶

知识点总结¶

知识点	核心内容	难度	关联章节
盲 MPC 的失效模式	落脚不可行、摆动碰撞、高度不匹配	⭐	足式/110_OCS2完整栈与双线程MPC
感知 MPC 系统架构	三层六模块管线，频率/延迟预算	⭐⭐	足式/110_OCS2完整栈与双线程MPC, 足式/160_感知驱动落脚规划
可踩性分类	坡度+粗糙度+台阶检测，阈值判断	⭐⭐	足式/160_感知驱动落脚规划
平面分割	连通域标记+协方差拟合，凸不等式约束	⭐⭐	--
2D SDF 计算	EDT 距离变换，膨胀+边距+高斯模糊	⭐⭐	足式/160_感知驱动落脚规划
SDF 碰撞约束	距离场局部梯度 + 末端运动学雅可比	⭐⭐⭐	足式/40_CppAD与代码生成
地形感知代价	高度跟踪、躯体避障、摆动回避	⭐⭐	--
Swing 地形适应	查询路径最大高度，调整抬高	⭐⭐	足式/140_落脚点规划经典方法
OCS2 perceptive 源码	Bilinear/TrilinearInterpolation, DistanceTransformInterface, EndEffectorDistanceConstraint	⭐⭐⭐	足式/110_OCS2完整栈与双线程MPC
RL vs MPC 对比	Miki 2022 vs Grandia 2023，混合方案	⭐⭐⭐	足式/210_RL与MPC混合范式
部署实践	延迟预算、核心绑定、降级策略	⭐⭐	--

本章在课程中的位置¶

向前承接:
  足式/110_OCS2完整栈与双线程MPC (OCS2 完整栈) → 本章在 OCS2 上增加感知模块
  足式/160_感知驱动落脚规划 (感知落脚规划) → 本章把感知从落脚扩展到全 MPC

向后指向:
  足式/240_legged_control精读 (legged_control 精读) → legged_control 中的感知集成
  足式/260_研究方向与博士导引 (前沿方向) → 感知-MPC-RL 的融合研究
  复合/30_多模态MPC (mobile_manipulator) → 移动机械臂的感知 MPC

设计空间全景分析：感知运动方案如何选型¶

前面各节分散讲了 Grandia 2023（MPC）、Miki 2022（RL）、DTC（混合）、Parkour（纯 RL）等方案。本节做**文档级**的系统综合——不是简单罗列优缺点，而是定义统一的对比维度，并给出一张决策流程图，帮助你在面对真实选型时做出有理有据的判断。

五维设计空间¶

把感知运动控制方案放进五个正交维度比较：

维度	Grandia 2023 (Perceptive MPC)	Miki 2022 (RL)	DTC (Jenelten 2024, 混合)	Parkour (Hoeller 2024, 纯 RL)
感知表示	显式：高程图 + 分割平面 + 3D SDF	隐式：高程图扫描点 -> 注意力编码	显式（MPC 侧）+ 隐式（RL 侧）	隐式：深度图 -> CNN
决策机制	在线优化（SQP/HPIPM）	前向推理（NN）	MPC 出参考 + RL 跟踪	前向推理（NN）
安全保证	强（约束满足，SQP 收敛时）	弱（统计安全）	中（MPC 约束 + RL 残差）	弱（统计安全）
极限敏捷	受限（实时 + 模型精度）	高	中高	最高（跳跃/攀爬/跑酷）
工程可调性	高（改 `.info` 即生效）	低（须重训）	中（MPC 可调，RL 须重训）	低（须重训）

本质洞察：这五个维度不是独立的，而是被一条主轴串起来——"把多少决策权交给离线学习 vs 在线优化"。从 Grandia（几乎全在线优化）到 Parkour（几乎全离线学习），是一条连续光谱。安全保证、可调性沿光谱单调递减，极限敏捷单调递增。理解了这条主轴，就不会把这些方法看成"四个孤立选项"，而是"同一权衡的四个采样点"——你的应用需求决定了应该落在光谱的哪个位置。

决策流程图¶

感知运动方案选型决策树

开始：你的首要约束是什么？
│
├── 安全/可审计性优先（工业巡检、矿井、管道、有人环境）
│   │
│   ├── 地形以结构化为主（台阶/斜坡/平台）？
│   │   └── 是 → Perceptive MPC（Grandia 2023）
│   │            理由：约束满足可审计，失败可定位，改配置即调
│   │
│   └── 需要一定敏捷但仍要安全网？
│       └── 混合（DTC 模式）：MPC 提供安全参考 + RL 补偿
│                理由：保留约束满足，同时获得 RL 的鲁棒性
│
├── 极限性能优先（跑酷、极端地形探索、科研 demo）
│   └── 纯 RL（Parkour / Extreme Parkour）
│            理由：性能上限最高，可学到人类设计不出的策略
│            代价：黑盒、失败灾难性、须大量训练
│
├── 算力极度受限（低成本小型机器人，无专用核）
│   └── 纯 RL（NN 推理 <1ms） 优于 MPC（SQP 5-10ms）
│            理由：MPC 的实时 QP 在弱算力上跑不到 100 Hz
│
└── 频繁更换机器人/任务，无 GPU 训练资源
    └── Perceptive MPC
             理由：换 URDF + 配置即可，无 sim-to-real gap，无需重训

使用方法：这张决策树的每个分支都对应正文某一节的论证（安全 vs 性能见 67.8；算力预算见 67.9；可调性见 67.7 配置）。它不是"标准答案"，而是把分散的权衡浓缩成一条可操作的判断路径——真实项目里往往多个约束并存，此时沿树走到第一个"硬约束"节点决策即可。

范式总结与研究启发¶

这一节是**跨论文、跨方向**的全局视角，区别于 67.7 后 🔬 研究视角对单篇 Grandia 2023 的分析。目标是帮你从"理解方法"上升到"理解范式"。

本方向的核心范式¶

读完 Grandia 2023、Miki 2022、TAMOLS、DTC、Parkour 这批工作，可以提炼出一个统一视角：

本质洞察：感知运动控制这一整个方向，本质上都在解决同一个问题——如何把"高维、离散、含噪、异步"的感知信号，转化为"满足物理约束、实时可执行"的全身运动。所有方法的分歧，仅在于这个"转化"在哪里发生、由谁完成： - Perceptive MPC：转化发生在**显式优化**里，由人设计的约束/代价完成（白盒）； - RL：转化发生在**神经网络权重**里，由数据和奖励塑造（黑盒）； - 混合（DTC）：转化被**拆成两段**——感知理解交给 RL，物理约束交给 MPC。

一旦看清"转化在哪发生"是唯一的根本分歧，就能预测任何新方法的位置：它要么移动转化的发生地，要么改变转化的分工方式。这正是评估"下一篇感知运动论文"的统一标尺。

跨方向迁移¶

本章的"离散感知 -> 连续优化"翻译范式，可以迁移到机器人学的其他方向：

迁移目标	为什么可行	预期挑战
移动机械臂避障 MPC（复合/30_多模态MPC）	障碍 SDF + 末端距离约束的数学结构完全一致，只是把"脚"换成"夹爪/连杆"	机械臂自碰撞约束更多，SDF 需覆盖整个臂体而非几个末端点
自动驾驶的占据栅格 MPC	占据栅格 -> ESDF -> 距离约束，与高程图 -> SDF 同构	动态障碍预测（行人/车辆）引入时变 SDF，比静态地形难
无人机穿越 gap 的 NMPC	3D SDF + 可微插值正是无人机走廊飞行的核心，本章 67.4 的梯度推导可直接复用	3D SDF 计算量比 2.5D 高程图大，须 GPU 或稀疏表示
灵巧手抓取的接触隐式优化	接触切换的不可微性（67.10）与抓取中的接触模式切换同构	接触点数量远多于四足，组合复杂度爆炸

组合创新头脑风暴¶

把本章范式与其他方向的技术组合，列出潜在创新方向（捕捉想法，不深入可行性分析）：

组合方案	预期效果	可行性	最大风险
Neural SDF（160 章）替换双线性插值 SDF	更平滑的梯度，内存不随地形复杂度增长	中（Jacquet 2025 已验证雏形）	NN 推理延迟可能挤占 MPC 时间预算
可微 MPC（67.10）+ RL 端到端学代价权重	让 67.5 的三个权重自适应地形，告别人工调参	中低（接触不可微仍是拦路虎）	接触切换处梯度偏差导致训练不稳定
语义分割（VLA/NaVILA）增强可踩性分类	区分冰面/草丛/实地，弥补几何感知盲区	中（语义实时性是瓶颈）	语义误判比几何误判更难检测和降级
概率 SDF（含不确定性）+ chance-constrained MPC	把感知噪声显式建模，约束变为机会约束	低（chance constraint 实时求解难）	实时性与保守性难两全

定位：以上是教学文档的**附加产出**，旨在记录想法种子。若某条值得深入，可启动科研实现工作流系统评估。它们也呼应了 67.10 的前沿讨论——前沿不是终点，而是这些组合方向的当前进度条。

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

足式累积项目：从零构建四足感知控制器

本章新增模块：Perceptive MPC 管线

累积项目进度:
  足式/110_OCS2完整栈与双线程MPC: OCS2 基础 MPC (盲)
  足式/120_步态管理与接触序列: 步态管理器
  足式/130_腿足状态估计: 状态估计
  足式/140_落脚点规划经典方法: 落脚规划 (Raibert)
  足式/160_感知驱动落脚规划: 高程图 + 可踩性分析
  ────────────────────────────────
  足式/230_Perceptive_MPC: [新增] Perceptive MPC 管线
    - 距离场计算/接口模块 (ComputeDistanceTransform + DistanceTransformInterface)
    - SDF 碰撞约束 (EndEffectorDistanceConstraint)
    - 地形感知代价 (高度跟踪 + 躯体避障)
    - 摆动轨迹地形适应 (初始猜测生成器)
    - 降级逻辑 (感知失效 → 盲 MPC)
  ────────────────────────────────
  足式/240_legged_control精读: [下一章] legged_control 整合

本章实操目标：在 OCS2 + Gazebo 仿真中，部署 Perceptive MPC，让四足机器人走过一段包含 5cm 台阶和 15 度斜坡的地形。对比"开感知"和"关感知"的通过率。

复现指南¶

论文解读教学的硬性要求之一是给出可操作的复现路径。如映射表所述，Grandia 2023 没有官方一键复现脚本，但其核心模块（ocs2_perceptive、convex_plane_decomposition、elevation_mapping）均开源。下面给出一条从环境到运行的最小路径，以及最容易踩的复现坑。

环境配置¶

⚠️ 前提：以下命令基于 Ubuntu 20.04 + ROS Noetic（OCS2 的 perceptive 示例对 ROS1/catkin 支持最成熟）。ROS2 移植仍在演进，新分支以本机 checkout 为准。

# 1. 系统依赖（OCS2 核心依赖，与 110 章一致）
sudo apt install ros-noetic-pybind11-catkin libglpk-dev
sudo apt install ros-noetic-grid-map ros-noetic-elevation-mapping

# 2. 建立 catkin 工作区
mkdir -p ~/perceptive_ws/src && cd ~/perceptive_ws/src

# 3. 拉取 OCS2（含 ocs2_perceptive）
git clone https://github.com/leggedrobotics/ocs2.git

# 4. 拉取外部感知前端（平面分割，独立于 ocs2_perceptive）
git clone https://github.com/leggedrobotics/elevation_mapping_cupy.git
git clone https://github.com/leggedrobotics/convex_plane_decomposition_ros.git

# 5. 依赖项（OCS2 文档列出的第三方）
#    Pinocchio、HPIPM、blasfeo、raisim（可选仿真器）等
#    建议严格按 OCS2 官方 installation 文档逐项装，版本敏感

编译命令¶

cd ~/perceptive_ws
# 先编译 OCS2 核心 + perceptive（注意 Release 模式，Debug 下 SQP 慢 10 倍以上）
catkin build ocs2_perceptive -DCMAKE_BUILD_TYPE=Release
# 再编译 ANYmal perceptive 示例（包名以本机 checkout 为准）
catkin build ocs2_legged_robot_ros -DCMAKE_BUILD_TYPE=Release
source devel/setup.bash

运行与预期结果¶

# 启动 perceptive MPC 示例（具体 launch 名以仓库为准）
roslaunch ocs2_legged_robot_ros legged_robot_sqp.launch
# 在 RViz 中：发布速度指令，观察机器人通过台阶/斜坡
# 预期：开感知时摆动腿自动抬高越过台阶；关感知（盲 MPC）时脚趾撞台阶

复现目标	预期现象	对应论文结论
平地行走	三个地形代价趋近 0，行为退化为盲 MPC	67.5 平地等价性
通过台阶	摆动腿抬高 > 台阶高 + 裕度	§VII 台阶攀爬
通过斜坡	躯体高度跟随地形升降	§V-C 高度跟踪
关感知对照	通过率显著下降	67.1 三重失效

常见复现问题¶

问题	原因	解决
SQP 求解慢（>50ms）	Debug 模式编译	改 `-DCMAKE_BUILD_TYPE=Release`
找不到分割平面	`convex_plane_decomposition` 未编译或话题未连	确认平面分割节点在跑，topic remap 正确
高程图为空	elevation_mapping 没收到点云/里程计	检查 TF 树与点云话题，确认 odometry 对齐
首次启动卡顿数十秒	CppADCodeGen 正在生成 `.so`	正常，仅首次；勿误删生成缓存（见 67.7 陷阱）
改了 `.info` 不生效	节点缓存了旧配置	重启节点，而非删 `.so`（见 67.7 陷阱）

💡 论文没告诉你的（复现陷阱，来源：实际复现经验 + OCS2 文档）：复现 Grandia 2023 的最大障碍**不是 ocs2_perceptive 本身，而是把分散在多个仓库的感知前端（elevation_mapping + convex_plane_decomposition）正确接上 OCS2**。论文 §IV 把这条链路讲得很顺，但代码里它们是**三个独立维护、版本各异的包**，topic/TF/坐标系约定的对接才是真正耗时的地方。这正是 67.7"ocs2_perceptive 不是独立模块"那条陷阱在复现层面的体现。

歧义审计汇总表¶

下表汇总本章在解读 Grandia 2023 + OCS2 源码过程中识别的所有 PARTIAL/UNSPECIFIED/CONFLICT 项。SPECIFIED 项不在此列（它们论文明确且与代码一致，正文正常讲解）。每一项都在正文对应小节有展开分析。

项目	级别	论文说法	代码/真实情况	判断与处理	小节
可踩性阈值（坡度/粗糙度/台阶）	PARTIAL	给出判据，未给全部具体阈值	从 `.info`/配置提取（如 max_slope≈0.6、max_step≈0.15）	以配置为准，本章给典型值	67.3
地形代价权重 $w_h,w_c,w_s$	PARTIAL	"固定权重，表现良好"	具体数值在 `.info`（如 100/30/50）	数值须从配置读取	67.5
SDF 维度	CONFLICT	本章为教学简化为"2D EDT + 垂直检查"	`DistanceTransformInterface` 接收 `vector3_t`，ANYmal 示例用 3D 体素 SDF	2D 是降维教学近似；接口与数学定义均为 3D	67.4, 67.7
距离约束是否进 AD tape	CONFLICT	笼统称"用 CppAD 求导距离约束"	仅末端正运动学进 tape；距离值/梯度运行时由距离场显式提供	以源码为准，本章已澄清	67.4, 67.7
`EndEffectorDistanceConstraint` 基类	PARTIAL	论文不涉及实现基类	非 CppAd 版继承 `StateConstraint`，CppAd 版继承 `StateInputConstraint`	从源码补全，区别重要	67.7
高斯模糊 SDF	PARTIAL	提及平滑处理	平滑核大小须从配置/实现确认	本章标为缓解梯度跳变的 trick	67.4
摆动轨迹初始猜测算法	UNSPECIFIED	论文未详细给出生成算法	本章给教学版三段样条；真实实现细节须查示例代码	标为教学重构，非源码原文	67.6
实验数字的端到端复现	UNSPECIFIED	给结论，无开源复现脚本	需自行拼装多仓库 + 调参	区分"方法可信"与"数字可复现"	67.8 🔬
双线性插值角点顺序	SPECIFIED（列此供对照）	论文不涉及	`(0,0),(1,0),(0,1),(1,1)`，与本章一致	已核实一致	67.7

审计方法说明：本表的 CONFLICT 项中，两处"2D vs 3D"和"AD tape 边界"是本轮源码核对（OCS2 main 分支）相对常见二手叙述最值得修正的点。它们不是论文错误，而是"教学简化/二手转述"与"真实接口契约"之间的落差——论文解读教学的独特价值，恰恰在于把这种落差显式标注出来，让读者读源码时不会困惑。

延伸阅读¶

必读¶

文献	类型	难度	核心贡献
Grandia R., Jenelten F., Yang S., Farshidian F., Hutter M. (2023) "Perceptive Locomotion Through Nonlinear Model-Predictive Control" -- T-RO, Vol. 39, pp. 3402-3421	论文	⭐⭐⭐	本章核心：完整的感知 NMPC 管线
Miki T., Lee J., Hwangbo J., et al. (2022) "Learning robust perceptive locomotion for quadrupedal robots in the wild" -- Science Robotics, Vol. 7, eabk2822	论文	⭐⭐⭐	RL 感知运动的标杆
OCS2 官方文档: https://leggedrobotics.github.io/ocs2/	文档	⭐⭐	OCS2 框架使用指南

进阶¶

文献	类型	难度	核心贡献
Jenelten F., He J., Farshidian F., Hutter M. (2024) "DTC: Deep Tracking Control" -- Science Robotics	论文	⭐⭐⭐	RL+MPC 混合架构
Hoeller D., et al. (2024) "ANYmal parkour" -- Science Robotics	论文	⭐⭐⭐	纯 RL 极限感知运动
Corbères A., et al. (2025) "Perceptive Locomotion through Whole-Body MPC and Optimal Region Selection" -- IEEE Access	论文	⭐⭐⭐	全身 MPC + 最优区域选择
Jenelten F., et al. (2022) "TAMOLS: Terrain-Aware Motion Optimization for Legged Systems" -- T-RO	论文	⭐⭐⭐	地形感知运动优化，SDF 碰撞回避

前沿¶

文献	类型	难度	核心贡献
Jacquet M., Harms M., Alexis K. (2025) "Neural NMPC through signed distance field encoding for collision avoidance" -- IJRR	论文	⭐⭐⭐⭐	神经网络 SDF 编码 + NMPC
Jenelten F., et al. (2025) "High-speed control and navigation for quadrupedal robots on complex and discrete terrain" -- Science Robotics	论文	⭐⭐⭐	DTC 路线推向高速 + 离散踏石
Lee J., et al. (2025) "Attention-based map encoding for learning generalized legged locomotion" -- Science Robotics	论文	⭐⭐⭐	注意力地图编码，泛化运动表征
Agarwal A., et al. (2023) "Legged Locomotion in Challenging Terrains using Egocentric Vision" -- CoRL	论文	⭐⭐⭐	第一人称视觉驱动运动
Yang R., et al. (2023) "Neural volumetric memory for visual locomotion control" -- CVPR	论文	⭐⭐⭐⭐	神经体积记忆
Felzenszwalb P., Huttenlocher D. (2012) "Distance Transforms of Sampled Functions" -- Theory of Computing, Vol. 8, pp. 415-428	论文	⭐⭐	EDT 算法的理论基础

参数	符号	典型值	物理含义
最大坡度	\(s_{\max}\)	30-35 度	超过此坡度摩擦锥无法保证
最大粗糙度	\(r_{\max}\)	0.05 m	表面不平度超过 5cm 不稳定
最大台阶高度	\(h_{\max}\)	0.15 m	超过 15cm 关节运动学受限

方法（源码核实）	返回	SQP 用途	计算成本
`getValue(t,x,u,·)`	约束值向量 \(g\)	检查约束违反、线搜索	最低（仅正运动学 + 查表）
`getLinearApproximation(...)`	\(g\) 与 \(\partial g/\partial(x,u)\)	一阶 SQP（RTI 默认）	中（加末端雅可比）
`getQuadraticApproximation(...)`	再加二阶项	完整二阶 SQP / 罚函数 Hessian	高（加二阶导）

代价项	权重符号	典型值范围	过大后果	过小后果
躯体高度跟踪	\(w_h\)	50-200	躯体僵硬跟踪地形，忽略其他目标	躯体高度不适应地形，腿过伸/过弯
躯体避障	\(w_c\)	10-50	机器人不敢靠近任何障碍	躯体可能碰撞大障碍
摆动腿回避	\(w_s\)	20-100	摆动高度过大，浪费能量	脚擦过地形表面，缺乏安全裕度

实现层级	数学形式	违反的后果	适用场景	OCS2 工具
硬约束	\(g(x)\geq 0\) 强制满足	不可行则 MPC 失败	物理铁律（摩擦锥、关节限位）	`StateInputConstraint`
软约束（罚函数）	违反量进代价，权重大	强烈不鼓励但允许	安全关键但偶尔需妥协（SDF 避障）	`SoftConstraintPenalty` + Augmented Lagrangian
纯代价	直接加权进 \(J\)	只是"不够好"	偏好引导（躯体高度、摆动裕度）	加进 `StateInputCost`

类	基类	约束依赖	雅可比来源
`EndEffectorDistanceConstraint`	`StateConstraint`	仅状态 \(x\)	`EndEffectorKinematics::getPositionLinearApproximation` 普通运动学线性化
`EndEffectorDistanceConstraintCppAd`	`StateInputConstraint`	状态 \(x\) 与输入 \(u\)	`CppAdInterface` 生成的末端位置雅可比

第 67 章：Perceptive MPC——让 MPC "长眼睛"的地形感知控制¶

前置自测¶

本章目标¶

论文-代码映射表（总览）¶

67.1 从盲 MPC 到感知 MPC ⭐¶

动机：盲 MPC 在崎岖地形上的三重失效¶

如果不用感知 MPC 会怎样——反应式补偿的局限¶

历史：感知嵌入 MPC 的演进¶

"Perceptive" 到底意味着什么¶

⚠️ 常见陷阱¶

练习¶

67.2 感知 MPC 系统架构 ⭐⭐¶

动机：为什么需要理解全管线¶

全管线架构图¶

频率与延迟预算¶

管线的三个子管线 (A)(B)(C) 详解¶

数据流的线程模型¶

⚠️ 常见陷阱¶

练习¶

67.3 地形约束构建 ⭐⭐¶

动机：为什么不能直接把高程图丢给 MPC¶

步骤 1：可踩性分类（Steppability Classification）¶

步骤 2：平面分割（Plane Segmentation）¶

步骤 2.5：凸内接多边形分解（论文-代码补全）¶

步骤 3：落脚点约束的凸不等式形式¶

对比：直接插值 vs 平面分割¶

⚠️ 常见陷阱¶

练习¶

67.4 SDF 碰撞避障约束 ⭐⭐⭐¶

动机：为什么摆动腿需要专门的避障约束¶

SDF 的数学定义与计算¶

Grandia 2023 的 2D SDF 计算¶

SDF 约束的数学公式化¶

SDF 查询的可微化¶

三级近似：SQP 到底向距离约束要什么¶

⚠️ 符号约定陷阱：clearance 的正负与约束方向¶

光滑近似：处理 SDF 梯度不连续点¶

⚠️ 常见陷阱¶

练习¶

67.5 地形感知代价函数 ⭐⭐¶

动机：约束不够，还需要代价¶

代价项 1：躯体高度跟踪（Body Height Tracking）¶

代价项 2：躯体避障（Body Clearance）¶

代价项 3：摆动腿地形回避（Swing Terrain Avoidance）¶

三个代价项的权重调节¶

代价函数的完整组合¶

同一地形需求：硬约束、软约束、还是代价？¶

⚠️ 常见陷阱¶

练习¶

67.6 Swing 轨迹地形适应 ⭐⭐¶

动机：固定摆动轨迹的局限¶

地形感知摆动轨迹生成¶

特殊地形的摆动策略¶

摆动时间分配¶

⚠️ 常见陷阱¶

练习¶

67.7 OCS2 Perceptive 实现对照 ⭐⭐⭐¶

动机：为什么要读源码¶

真实 ocs2_perceptive 核心结构¶

真实模块 1：BilinearInterpolation.h¶

真实接口 2.5：DistanceTransformInterface 与 ComputeDistanceTransform（源码核实）¶

教学简化模块 2：TeachingSignedDistanceTransform¶

真实接口 3：EndEffectorDistanceConstraint(CppAd)¶

配置文件解读¶

⚠️ 常见陷阱¶

练习¶

🔬 研究视角：Grandia 2023 的贡献结构分析 ⭐⭐⭐¶

维度一：问题定义贡献——它开创了新问题吗？¶

维度二：框架贡献——它给出了完整端到端方案吗？¶

维度三：实验方法论贡献——消融是否系统？¶

局限：从 Grandia 2023 通向下一篇的动机¶

67.8 RL vs MPC 感知运动对比 ⭐⭐⭐¶

动机：为什么需要对比¶

Miki 2022：RL 方案概述¶

Grandia 2023：MPC 方案概述¶

全面对比¶

更深层的差异分析¶

混合方案：DTC 和未来方向¶

⚠️ 常见陷阱¶

练习¶

真实 `ocs2_perceptive` 核心结构¶

真实模块 1：`BilinearInterpolation.h`¶

真实接口 2.5：`DistanceTransformInterface` 与 `ComputeDistanceTransform`（源码核实）¶

教学简化模块 2：`TeachingSignedDistanceTransform`¶

真实接口 3：`EndEffectorDistanceConstraint(CppAd)`¶