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

第 58 章：落脚点规划经典方法——从 Raibert 启发式到混合整数优化

本章定位：落脚点规划（foothold planning）是腿足运动控制中最"牵一发而动全身"的决策——落在哪里决定了后续的力分配、平衡裕度、速度追踪和地形适应。本章系统讲解从最经典的 Raibert 启发式（1986）到基于 Capture Point 的动态规划、ZMP 步态规划、评分函数选点、反应式调整，直至混合整数优化的完整方法谱系。

前置依赖：足式/70_腿足简化模型理论（LIPM / DCM / Capture Point）、足式/90_WBC分层优化与TSID（WBC）、足式/110_OCS2完整栈与双线程MPC（OCS2 架构）、足式/120_步态管理与接触序列（步态管理 / ModeSchedule）

关键文献：Raibert 1986（MIT Press）、Pratt 2006（Capture Point）、Koolen 2012（N-step Capturability）、Kajita 2003（Preview Control）、Englsberger 2015（DCM 3D）、Deits & Tedrake 2014（MIQP）、Jenelten 2022（TAMOLS）

---

前置自测

📋 答不出 >= 2 题 → 先回前置章节复习

1. [足式/70_腿足简化模型理论] 写出 LIPM 方程 \(\ddot{x} = \omega^2(x - p)\)，说明 \(\omega\) 的物理含义和 Go2 的典型数值。
2. [足式/70_腿足简化模型理论] 什么是 Divergent Component of Motion（DCM）？写出其定义 \(\xi = x + \dot{x}/\omega\)，解释为什么它叫"发散分量"。
3. [足式/70_腿足简化模型理论] Capture Point 的定义是什么？它与 DCM 的关系？
4. [足式/120_步态管理与接触序列] OCS2 的 ModeSchedule 如何告诉 MPC"哪只脚在什么时候着地"？
5. [足式/120_步态管理与接触序列] Raibert 启发式的三项分别对应什么物理含义？

---

本章目标

学完本章，你应能：

1. 从 LIPM 倒立摆动力学**完整推导** Raibert 启发式的每一项，理解其物理含义和局限性
2. 理解 Capture Point / DCM 如何将落脚点问题转化为"捕获发散分量"，掌握一步 / N 步 capturability 的数学框架
3. 掌握 ZMP Preview Control 的步态规划思路及其在人形机器人上的经典应用
4. 能设计多准则落脚点评分函数（地形、可达性、稳定裕度），并理解各分量的权衡
5. 理解反应式落脚调整的时机与策略——摆动相中途修改落脚点以恢复平衡
6. 理解 MPC 落脚点规划如何把落脚位置提升为优化变量，与接触力联合、多步前瞻，以及双线性非凸的来源与两条求解路线
7. 理解混合整数优化（MIQP）如何将离散接触选择纳入数学规划框架
8. 能对比启发式、优化式和学习式三大路线的适用场景

---

知识导航

本章沿一条主线展开：先严格定义"脚往哪里放"这个问题（问题定义）→ 用最简的闭式公式解决它（Raibert）→ 用动力学把它做精确（Capture Point / DCM / ZMP）→ 给它加上现实世界的硬约束（可达性 / 地形 / 评分 / 反应式调整）→ 把它升级为联合多步优化（MPC 落脚点 / MIQP）。计算量从 \(O(1)\) 一路爬升到 \(O(2^K)\)，最优性也随之从"局部近似"走向"全局最优"——这条复杂度-最优性的权衡轴贯穿全章。下表给出各节定位（括号内为难度）。

节	主题	在主线中的位置
58.1 问题定义 ⭐	输入/输出/决策空间、方法谱系	定义：把工程直觉变成数学问题
58.2 Raibert 启发式 ⭐⭐	LIPM 对称摆动 → 前馈+反馈+航向	闭式解：\(O(1)\) 的落脚点基线
58.3 Raibert → Capture Point ⭐⭐	消除发散模态、DCM、与 Raibert 的精确联系	精确化：从直觉公式到动力学最优
58.4 Capture Point 落脚规划 ⭐⭐⭐	一步 / N 步 capturability、DCM 反时间规划	精确化：单步停车 → 多步可行性
58.5 ZMP 步态规划 ⭐⭐	Preview Control、ZMP → 落脚点	精确化：人形机器人的"日本路线"
58.6 可达性与地形约束 ⭐⭐	工作空间、坡度/粗糙度/法向量	硬约束：腿能到 + 地面能踩
58.7 落脚点评分函数 ⭐⭐	多准则加权、TAMOLS 联合优化	硬约束：可行集合中选最优
58.8 反应式落脚调整 ⭐⭐⭐	摆动相中途重规划、限速+滤波	硬约束：应对计划外扰动
58.9 MPC 落脚点规划 ⭐⭐⭐	落脚点升为优化变量、双线性非凸、多步前瞻	联合优化：连续地形上与力联合
58.10 混合整数落脚规划 ⭐⭐⭐⭐	MIQP、Big-M、IRIS 凸分解、分层求解	联合优化：离散地形上的组合选择

知识依赖关系：58.1（问题定义）是全章地基；58.2 Raibert 是其余所有方法的"牛顿第一定律"，58.3–58.4 沿"消除发散模态"把它做成精确的 Capture Point / DCM，58.5 ZMP 则是并行的另一条稳定性表达。58.6–58.8 是任何落脚方法都绕不开的工程约束三件套（可达性、地形质量、在线反应）。58.9 与 58.10 是同一条"联合优化"主线的连续两段——58.9 处理连续地形上的多步优化，当可踩区域退化为离散凸块时即升级为 58.10 的混合整数 MPC；二者都建立在 58.4 N-step capturability 的"多步前瞻"思想之上，区别只在落脚点是连续变量还是含整数变量。

推荐阅读路径：

• 速查（2 h）：58.1 问题定义 → 58.2 Raibert 完整公式 → 58.3 Capture Point 定义 → 58.11 小结表。只求知道"有哪些方法、各自解决什么"。
• 主线（精读 10–12 h）：按 58.1 → 58.2 → 58.3 → 58.4 → 58.6 → 58.7 → 58.8 顺序通读，这是四足落脚规划工程师的必经之路；双足方向额外读 58.5。
• 进阶（研究向，+6 h）：在主线基础上深入 58.9（MPC 落脚点）、58.10（MIQP），以及延伸阅读中的 Grandia 2023、GCS、学习型落脚选择等前沿。

---

58.1 落脚点规划的问题定义 ⭐

这一节解决什么问题：严格定义"落脚点规划"的输入、输出和决策空间，把模糊的工程直觉变为明确的数学问题。

动机：为什么落脚点是腿足控制最关键的决策

轮式和履带式机器人的"去哪里"由路径规划决定，执行层只需跟踪速度指令。但腿足机器人的情况完全不同——它的运动本质上是**离散接触序列**，每一步踩在哪里不仅决定了当前的平衡，还约束了后续若干步的可行性。

一个糟糕的落脚点选择会导致连锁反应：

影响维度	具体后果	举例
平衡	支撑多边形过小或 ZMP 逼近边界	四足 trot 时一只脚踩在斜坡边缘
力分配	接触法向量不利于产生期望的 GRF	踩在 45 度斜面上，法向力分量不足
可达性	落脚点超出关节极限	步幅过大，膝关节完全伸直
后续步可行性	当前步过于保守/激进，导致下一步无解	踩太远，收腿时髋关节卡死
地形安全	踩在不稳定表面	踩在碎石、裂缝或冰面上

💡 概念澄清："落脚点规划"与"摆动轨迹生成"是两个不同的问题。落脚点回答"脚往哪里放"，摆动轨迹回答"脚怎么过去"。本章聚焦前者。足式/120_步态管理与接触序列.4 讲解了摆动轨迹生成——通过 Cubic Spline 或 Bezier 曲线在"当前足端位置"与"目标落脚点"之间插值出平滑路径，并用抬腿高度（swing height）参数避免地面刮蹭。本章输出的目标落脚点正是 足式/120_步态管理与接触序列.4 摆动轨迹的终点约束。

问题的形式化定义

输入：

• 用户指令：期望基座速度 \(\boldsymbol{v}^{\text{cmd}} \in \mathbb{R}^3\)，期望航向角速度 \(\omega_z^{\text{cmd}}\)
• 当前状态：基座位姿 \((\boldsymbol{p}_{\text{base}}, \boldsymbol{R}_{\text{base}})\)，基座速度 \((\boldsymbol{v}_{\text{base}}, \boldsymbol{\omega}_{\text{base}})\)，各关节角度 \(\boldsymbol{q}_j\)
• 步态时序：由 GaitSchedule（足式/120_步态管理与接触序列）给出的接触序列——哪只脚在什么时刻从支撑转为摆动
• 环境信息（可选）：高程图、可通行性图、障碍物位置

输出：

• 对每只即将进入摆动相的脚，给出目标落脚点 \(\boldsymbol{p}_{\text{foot}}^{\text{target}} \in \mathbb{R}^3\)

决策空间：

落脚点的决策空间受以下约束限制：

\[\mathcal{F} = \{\boldsymbol{p} \in \mathbb{R}^3 \mid \boldsymbol{p} \in \mathcal{W}_{\text{kin}}(\boldsymbol{q}_{\text{base}}) \cap \mathcal{T}_{\text{safe}} \cap \mathcal{S}_{\text{stable}}\}\]

其中： - \(\mathcal{W}_{\text{kin}}\)：运动学可达工作空间（由髋关节位置和腿长决定） - \(\mathcal{T}_{\text{safe}}\)：地形安全区域（非悬空、非滑面、坡度适当） - \(\mathcal{S}_{\text{stable}}\)：稳定性约束（ZMP / 支撑多边形 / Capture Point 可达）

方法论谱系总览

落脚点规划方法可以按**计算复杂度**和**最优性**两个维度分类：

方法	计算量	最优性	地形适应	实时性	典型频率
Raibert 启发式	O(1)	局部近似	差（平地）	极好	1 kHz
Capture Point	O(1)	一步最优	中等	极好	1 kHz
ZMP Preview	O(N)	horizon 最优	中等	好	100 Hz
评分函数选点	O(K)	贪心	好	好	50-200 Hz
反应式调整	O(1)	修正量最优	好	极好	1 kHz
MIQP	O(2^K)	全局最优	极好	差	1-10 Hz

其中 \(N\) 是预览步数，\(K\) 是候选落脚点数量。

历史演进脉络

1986  Raibert         启发式："对称跑步" → 简单有效
  |
1991  Kajita           LIPM 应用于步行
  |
2003  Kajita           Preview Control → ZMP 步态规划（HRP-2）
  |
2006  Pratt            Capture Point → 把平衡转化为"捕获"
  |
2012  Koolen           N-step Capturability → 多步可达性理论
  |
2014  Deits & Tedrake  MIQP → 混合整数优化选步
  |
2015  Englsberger      3D DCM → 三维 Capture Point 扩展
  |
2018  Di Carlo         MIT Cheetah 3 → Raibert + 凸 MPC
  |
2020  Jenelten         ANYmal → 在线落脚优化 + 高程图
  |
2022  Jenelten         TAMOLS → 地形感知运动优化
  |
2024+ RL 残差策略       Raibert + RL 修正 → 混合方法

落脚点规划与其他模块的数据流

理解落脚点规划在整个控制架构中的位置非常重要：

用户指令 (v_cmd, ω_cmd)
       ↓
┌──────────────────┐
│  步态管理 (足式/120_步态管理与接触序列)  │ → ModeSchedule: 哪只脚何时摆动
└──────┬───────────┘
       ↓
┌──────────────────┐         ┌──────────────┐
│  落脚点规划       │ ←───── │  地形感知     │
│  (本章 足式/140_落脚点规划经典方法)      │         │  (足式/160_感知驱动落脚规划)       │
└──────┬───────────┘         └──────────────┘
       ↓ p_foot_target
┌──────────────────┐
│  摆动轨迹生成     │ → 脚的位置/速度/加速度轨迹
│  (足式/120_步态管理与接触序列.4)        │
└──────┬───────────┘
       ↓
┌──────────────────┐
│  MPC (足式/110_OCS2完整栈与双线程MPC)       │ → 最优接触力
└──────┬───────────┘
       ↓
┌──────────────────┐
│  WBC (足式/90_WBC分层优化与TSID)       │ → 关节力矩
└──────────────────┘

⚠️ 常见陷阱

⚠️ 编程陷阱：落脚点计算后忘记投影到地面 - 错误做法：直接用 Raibert 公式计算出的 \((x, y, z)\) 作为落脚点 - 现象：在非平坦地面上，脚悬在空中或插入地面 - 根本原因：Raibert 公式假设 \(z = 0\)（平地），实际需要将 \((x, y)\) 投影到高程图上获取正确的 \(z\) - 正确做法：p_foot.z() = heightMap.query(p_foot.x(), p_foot.y());

💡 概念误区：认为"落脚点规划只需要考虑当前一步" - 新手想法："只要这一步踩稳了就行" - 实际上：当前步的落脚点约束了下一步的可达范围。一个极端的落脚点可能让当前步安全但下一步无解。这就是 N-step capturability（58.4 节）要解决的问题 - 正确思维：落脚点规划至少要考虑"这一步踩了之后，下一步还有地方踩吗？"

🧠 思维陷阱：认为"轮式机器人不需要落脚点规划，所以这个问题是腿足独有的" - 新手想法："轮式直接走就行，没有离散决策" - 实际上：轮式机器人的路径规划中，"车轮经过的地面点"也隐含了类似约束（可通行性、摩擦力）。腿足的特殊性在于接触是**离散的**而非连续的，因此决策空间从连续曲线变为离散点集 - 延伸思考：攀爬机器人（如波士顿动力的 Atlas 攀墙）的手脚协调规划是落脚问题的高维推广

练习

1. [估算题] Go2 四足机器人的腿长约 0.25 m，髋关节到地面高度约 0.3 m。估算单腿的可达工作空间半径。如果步频为 4 Hz（trot），最大步幅是多少？
2. [思考题] 为什么双足机器人的落脚点规划比四足更难？从支撑多边形、冗余度、失败后果三个角度分析。
3. [设计题] 如果你要设计一个"落脚点规划器接口"，需要哪些输入和输出？画出接口的数据流图。

---

58.2 Raibert 启发式 ⭐⭐

这一节解决什么问题：从 LIPM 动力学完整推导出 Raibert 1986 的落脚点公式，理解每一项的物理来源，以及为什么这个看似简单的公式能在平地上如此有效。

动机：最简单的落脚点控制器

1986 年，Marc Raibert 在 MIT 构建了一系列能跑步的机器人——从单腿弹跳机器人（hopping robot）到双足再到四足机器人。他面临一个核心问题：机器人应该把脚放在哪里才能保持平衡并追踪期望速度？

这个问题的难度在于：倒立摆是不稳定的，任何微小偏差都会指数增长（足式/70_腿足简化模型理论.2 已证明 LIPM 的发散模态 \(e^{\omega t}\)）。所以落脚点不能随意选——必须恰好放在能抑制发散的位置。

Raibert 的天才之处在于：他不是从复杂的优化理论出发，而是从一个简洁的**物理直觉**出发——对称运动。这个直觉后来被证明与最优控制理论的结论一致，但它的推导过程比最优控制简单得多。

如果不用 Raibert，会怎样

最朴素的想法：把脚放在髋关节正下方。

\[\boldsymbol{p}_{\text{foot}}^{\text{naive}} = \boldsymbol{p}_{\text{hip}}\]

这在**零速静止站立**时是正确的。但一旦机器人有前进速度，问题就来了：

• 支撑相期间，基座继续向前移动
• 支撑相结束时，基座已经远离脚的正上方
• 如果下一步还是放在髋正下方，机器人会越来越倾斜
• 最终摔倒

让我们用 LIPM 做定量分析。假设 Go2 以 \(v = 1\) m/s 行走，支撑相 \(T_s = 0.3\) s，如果把脚放在髋正下方（\(p = x_0\)）：

\[x(T_s) = x_0 \cosh(\omega T_s) + \frac{v}{\omega}\sinh(\omega T_s)\]

代入 \(\omega = \sqrt{9.81/0.3} \approx 5.72\) rad/s，\(\omega T_s \approx 1.72\)：

\[\Delta x = x(T_s) - x_0 \approx x_0 (\cosh(1.72) - 1) + \frac{1.0}{5.72}\sinh(1.72) \approx 1.87 x_0 + 0.48 \text{ m}\]

CoM 在一个支撑相内偏移近 0.5 m——远超腿的可达范围。这说明"踩在髋正下方"在有速度时是完全不可行的。

核心矛盾：机器人有速度，但脚的位置没有"前瞻性"——没有考虑到支撑相期间基座还会继续移动。

Raibert 的洞察：对称摆动

Raibert 的核心洞察来自一个简单的物理图像：如果机器人在支撑相的运动是关于中点对称的，那么它的速度在支撑相结束时会和开始时相同。

        支撑相开始        支撑相中点        支撑相结束
           v →              v →              v →
           ●                 ●                ●
          /                  |                 \
         / (减速)           | (对称点)        \ (加速)
        /                    |                   \
    ───●────────────────────●────────────────────●─── 地面
    脚(落脚点)          基座正上方          脚(落脚点)

对称摆动意味着： - 支撑相前半段：基座从脚后方向前移动（倒立摆"下坡"，减速） - 支撑相后半段：基座从脚正上方继续前移（倒立摆"上坡"，加速） - 如果前后半段完全对称：加速和减速抵消，出速度 = 入速度

这就是 Raibert 1986 年论文 "Running With Symmetry" 的核心思想。Raibert 称能实现对称运动的落脚位置为 neutral point（中性点）。

完整推导

Step 1：建立 LIPM 模型

从 足式/70_腿足简化模型理论.2 的 LIPM 方程出发：

\[\ddot{x} = \omega^2(x - p)\]

其中 \(x\) 是 CoM 的水平位置，\(p\) 是支撑点（脚的位置），\(\omega = \sqrt{g/h}\)。

Step 2：写出通解

设支撑相从 \(t = 0\) 开始，持续 \(T_s\)。以支撑点 \(p\) 为参考，定义相对位移 \(\bar{x} = x - p\)。

方程变为 \(\ddot{\bar{x}} = \omega^2 \bar{x}\)，通解：

\[\bar{x}(t) = A \cosh(\omega t) + B \sinh(\omega t)\]

初始条件 \(\bar{x}(0) = x_0 - p\)，\(\dot{\bar{x}}(0) = v_0\)：

\[A = x_0 - p, \quad B = \frac{v_0}{\omega}\]

所以：

\[x(t) - p = (x_0 - p) \cosh(\omega t) + \frac{v_0}{\omega} \sinh(\omega t)\]

\[\dot{x}(t) = (x_0 - p) \omega \sinh(\omega t) + v_0 \cosh(\omega t)\]

Step 3：对称条件——在支撑相中点 CoM 位于脚正上方

对称摆动要求 CoM 在支撑相中点 \(t = T_s/2\) 时刻正好在脚正上方，即：

\[x(T_s/2) = p \quad \Rightarrow \quad \bar{x}(T_s/2) = 0\]

代入通解：

\[0 = (x_0 - p) \cosh(\omega T_s/2) + \frac{v_0}{\omega} \sinh(\omega T_s/2)\]

解出 neutral point：

\[\boxed{p^{\text{neutral}} = x_0 + \frac{v_0}{\omega} \tanh(\omega T_s/2)}\]

Step 4：验证对称性保速度

在对称条件下，计算支撑相结束时的速度：

\[\dot{x}(T_s) = (x_0 - p) \omega \sinh(\omega T_s) + v_0 \cosh(\omega T_s)\]

利用 \(x_0 - p = -\frac{v_0}{\omega}\tanh(\omega T_s/2)\) 和双曲函数恒等式：

\[\sinh(\omega T_s) = 2\sinh(\omega T_s/2)\cosh(\omega T_s/2)\]

\[\cosh(\omega T_s) = 2\cosh^2(\omega T_s/2) - 1\]

代入化简：

\[\dot{x}(T_s) = -\frac{v_0}{\omega} \cdot \frac{\sinh(\omega T_s/2)}{\cosh(\omega T_s/2)} \cdot \omega \cdot 2\sinh(\omega T_s/2)\cosh(\omega T_s/2) + v_0[2\cosh^2(\omega T_s/2) - 1]\]

\[= -2v_0\sinh^2(\omega T_s/2) + 2v_0\cosh^2(\omega T_s/2) - v_0\]

\[= 2v_0[\cosh^2(\omega T_s/2) - \sinh^2(\omega T_s/2)] - v_0 = 2v_0 - v_0 = v_0\]

出速度 = 入速度！对称性确实保证了速度不变。

Step 5：近似简化

当 \(\omega T_s/2\) 不太大时（典型值约 0.5-1.0），\(\tanh(\omega T_s/2) \approx \omega T_s/2\)（一阶 Taylor 展开）：

\[p^{\text{neutral}} \approx x_0 + \frac{v_0}{\omega} \cdot \frac{\omega T_s}{2} = x_0 + \frac{T_s}{2} v_0\]

由于 \(x_0 \approx p_{\text{hip}}\)（CoM 水平位置近似等于髋关节位置），我们得到**Raibert 前馈项**：

\[\boldsymbol{p}_{\text{foot}}^{\text{ff}} = \boldsymbol{p}_{\text{hip}} + \frac{T_s}{2} \boldsymbol{v}_{\text{base}}\]

💡 物理直觉：\(\frac{T_s}{2} \boldsymbol{v}_{\text{base}}\) 的含义是"如果基座以当前速度匀速运动半个支撑相，它会到达哪里"。把脚放在这个位置，可以保证支撑相中点时基座恰好在脚正上方——实现对称摆动。

Step 6：添加速度反馈项

前馈项只能维持**当前速度**不变。如果我们希望机器人加速或减速到某个目标速度 \(v_{\text{ref}}\)，需要**故意打破对称**：

• 要加速（\(v_{\text{ref}} > v_{\text{base}}\)）：脚放得比中性点**更后面**（少迈一点），让倒立摆在支撑相后半段获得更多加速
• 要减速（\(v_{\text{ref}} < v_{\text{base}}\)）：脚放得比中性点**更前面**（多迈一点），让倒立摆在前半段减速更多

反馈项的形式：

\[\boldsymbol{p}_{\text{foot}}^{\text{fb}} = k_p (\boldsymbol{v}_{\text{base}} - \boldsymbol{v}_{\text{ref}})\]

其中 \(k_p > 0\) 是反馈增益。当 \(v_{\text{base}} > v_{\text{ref}}\)（机器人太快），反馈项为正 → 脚往前多放 → 倒立摆多"上坡" → 减速。符号自洽。

\(k_p\) 的物理意义和选择：

\(k_p\) 的自然选择来自 LIPM 的时间常数。下面通过 Poincare 映射稳定性分析给出完整推导。

Poincare 映射稳定性推导

定义每个支撑相开始时的速度误差 \(e_k = \dot{x}_k - v_{\text{ref}}\)。目标:选择 \(k_p\) 使得 \(|e_{k+1}| < |e_k|\)(误差收敛)。

LIPM 的支撑相解(足式/70_腿足简化模型理论 已推导)为:

\[x(t) = x_0 \cosh(\omega t) + \frac{\dot{x}_0}{\omega}\sinh(\omega t) + p(1 - \cosh(\omega t))$$ $$\dot{x}(t) = x_0 \omega\sinh(\omega t) + \dot{x}_0 \cosh(\omega t) - p\omega\sinh(\omega t)\]

在支撑相结束时 \(t = T_s\):

\[\dot{x}_{k+1} = (x_k - p_k)\omega\sinh(\omega T_s) + \dot{x}_k\cosh(\omega T_s)\]

将 Raibert 控制律 \(p_k = x_k + \frac{T_s}{2}\dot{x}_k + k_p(\dot{x}_k - v_{\text{ref}})\) 代入:

\[\dot{x}_{k+1} = -\left(\frac{T_s}{2}\dot{x}_k + k_p(\dot{x}_k - v_{\text{ref}})\right)\omega\sinh(\omega T_s) + \dot{x}_k\cosh(\omega T_s)\]

在不动点 \(\dot{x}^* = v_{\text{ref}}\) 处线性化,令 \(e_k = \dot{x}_k - v_{\text{ref}}\):

\[e_{k+1} = \left[\cosh(\omega T_s) - \left(\frac{T_s}{2} + k_p\right)\omega\sinh(\omega T_s)\right] e_k\]

定义**Poincare 映射特征值**(也叫 Floquet 乘子):

\[\lambda = \cosh(\omega T_s) - \left(\frac{T_s}{2} + k_p\right)\omega\sinh(\omega T_s)\]

系统稳定的充要条件是 \(|\lambda| < 1\)。

最优 \(k_p\) 的推导:令 \(\lambda = 0\)(最快收敛——dead-beat control):

\[\cosh(\omega T_s) = \left(\frac{T_s}{2} + k_p^*\right)\omega\sinh(\omega T_s)\]

\[k_p^* = \frac{\cosh(\omega T_s)}{\omega\sinh(\omega T_s)} - \frac{T_s}{2} = \frac{1}{\omega\tanh(\omega T_s)} - \frac{T_s}{2}\]

利用恒等式 \(\frac{1}{\tanh(x)} = \coth(x)\),等价地写为:

\[\boxed{k_p^* = \frac{1}{\omega}\coth(\omega T_s) - \frac{T_s}{2}}\]

注意:另一种常见形式 \(k_p = \frac{1}{\omega}\tanh(\omega T_s/2) - \frac{T_s}{2}\) 来自**半支撑相对称分析**(假设中点对称),两者在 \(\omega T_s \ll 1\) 时一致,但在大步幅时有差异。工程中通常用 \(\tanh\) 形式。

渐近分析:

• 当 \(\omega T_s \gg 1\) 时, \(\tanh(\omega T_s/2) \to 1\),得 \(k_p \approx \frac{1}{\omega} - \frac{T_s}{2}\)
• 若进一步 \(T_s \ll 1/\omega\),则 \(k_p \approx \frac{1}{\omega}\)
• 这恰好等于 Capture Point 增益!即:Raibert 反馈项在极限情况下退化为 Capture Point 控制律——这不是巧合,而是因为两者都在抑制 LIPM 的发散模态 \(e^{\omega t}\),只是分析框架不同(Poincare 映射 vs Capture Point 稳定性)。

本质洞察: Raibert 启发式的核心数学结构是一个**离散时间 dead-beat 控制器**——通过选择 \(k_p\) 使 Poincare 映射的特征值为零,实现一步速度误差收敛。这也解释了为什么 Raibert 公式在实践中如此鲁棒:dead-beat 控制器对参数误差的容忍度远高于一般的稳定控制器。即使 \(k_p\) 偏离最优值 50%,只要 \(|\lambda| < 1\),系统仍然稳定——只是收敛变慢。

对 Go2（\(h \approx 0.3\) m）：

\[k_p \approx \frac{1}{\omega} = \sqrt{\frac{h}{g}} = \sqrt{\frac{0.3}{9.81}} \approx 0.175 \text{ s}\]

实际工程中，\(k_p\) 通常在 \(0.03\)-\(0.3\) 之间调节，是最重要的调参旋钮之一。

Step 7：完整 Raibert 公式

\[\boxed{\boldsymbol{p}_{\text{foot}}^{\text{target}} = \boldsymbol{p}_{\text{hip}} + \frac{T_s}{2} \boldsymbol{v}_{\text{base}} + k_p (\boldsymbol{v}_{\text{base}} - \boldsymbol{v}_{\text{ref}})}\]

项	名称	物理含义	量级（Go2 trot, v=1 m/s）
\(\boldsymbol{p}_{\text{hip}}\)	髋投影	零速时的静平衡位置	基准位置
\(\frac{T_s}{2} \boldsymbol{v}\)	前馈项	维持当前速度的中性点	~0.15 m
\(k_p \Delta\boldsymbol{v}\)	反馈项	速度误差修正	~0.01-0.05 m

航向修正项

如果用户发出转弯指令 \(\omega_z^{\text{cmd}}\)，Raibert 公式还需要添加航向修正。在三维中，转弯造成的髋关节偏移为：

\[\boldsymbol{p}_{\text{foot}}^{\text{yaw}} = \frac{T_s}{2} \boldsymbol{\omega}_z \times (\boldsymbol{p}_{\text{hip}} - \boldsymbol{p}_{\text{base}})\]

这一项的物理含义是：如果机器人要转弯，每条腿的落脚点应该沿着转弯方向偏移。对左前腿，\(\boldsymbol{p}_{\text{hip}} - \boldsymbol{p}_{\text{base}}\) 指向左前方，叉乘旋转角速度后得到一个向前（或向后）的修正量。

完整公式带航向修正：

\[\boldsymbol{p}_{\text{foot}} = \boldsymbol{p}_{\text{hip}} + \frac{T_s}{2}\boldsymbol{v} + k_p(\boldsymbol{v} - \boldsymbol{v}_{\text{ref}}) + \frac{T_s}{2}(\boldsymbol{\omega}_z \times \boldsymbol{r}_{\text{hip}})\]

其中 \(\boldsymbol{r}_{\text{hip}} = \boldsymbol{p}_{\text{hip}} - \boldsymbol{p}_{\text{base}}\) 是髋关节相对基座的偏移向量。

⚠️ 工程陷阱：Raibert 公式在纯转向时的落足点发散

当机器人执行原地转向（线速度≈0，角速度较大）时，Raibert 公式中的角速度补偿项仅提供切向方向的偏移。这会导致落足点相对于髋关节的距离不断增大，可能超出足端的运动学可达域。

表现：机器人在原地旋转时，脚越放越远，最终超出关节限位或运动学极限。

工程处理：需要额外添加可达域裁剪（将落足点投影回可达区域内），或使用基于优化的落足点规划替代 Raibert 启发式。

Raibert 为什么有效——稳定性分析

Raibert 的有效性可以用 Poincare 映射严格证明。定义每个支撑相开始时的状态 \((x_k, \dot{x}_k)\) 为 Poincare 截面上的点。一步的动力学为：

\[\dot{x}_{k+1} = (x_k - p_k)\omega\sinh(\omega T_s) + \dot{x}_k \cosh(\omega T_s)\]

将 Raibert 控制律 \(p_k = x_k + \frac{T_s}{2}\dot{x}_k + k_p(\dot{x}_k - v_{\text{ref}})\) 代入，在 \(v_{\text{ref}}\) 附近线性化，可以得到误差 \(e_k = \dot{x}_k - v_{\text{ref}}\) 的映射：

\[e_{k+1} = \lambda \cdot e_k\]

其中 \(\lambda\) 是特征值。通过详细计算可以证明：当 \(k_p\) 在某个范围内时，\(|\lambda| < 1\)，系统渐近稳定。

直觉上：\(k_p\) 太小 → 反馈不够 → 速度误差消除慢 → \(|\lambda|\) 接近 1；\(k_p\) 太大 → 过度修正 → 振荡甚至发散 → \(|\lambda| > 1\)。

Raibert 的局限性

局限	原因	后果	解决方案
不考虑地形	公式假设平地 \(z = 0\)	斜坡/阶梯上失效	58.6-58.7 评分函数
不考虑角动量	LIPM 忽略转动	躯干大幅倾斜时不准	58.3-58.4 DCM 扩展
只看一步	没有多步前瞻	走到死胡同	58.4 N-step / 58.9 MPC 落脚 / 58.10 MIQP
没有运动学检查	可能超出关节极限	脚伸不到	58.6 可达性约束
没有力约束	不检查摩擦锥	踩滑面会滑倒	58.7 评分函数

C++ 实现

#include <Eigen/Dense>
#include <cmath>

struct RaibertParams {
    double stance_time;     // 支撑相时长 [s]，典型 0.2-0.4
    double kp_feedback;     // 速度反馈增益 [s]，典型 0.03-0.3
};

/// @brief Raibert 启发式落脚点计算
/// @param hip_pos_world    髋关节世界坐标（地面投影）
/// @param base_vel_world   基座线速度（世界系）
/// @param base_vel_ref     参考速度（世界系）
/// @param base_pos_world   基座位置（世界系）
/// @param yaw_rate_cmd     航向角速度指令 [rad/s]
/// @param params           Raibert 参数
/// @return 目标落脚点 (x, y, z=0)
Eigen::Vector3d computeRaibertFoothold(
    const Eigen::Vector3d& hip_pos_world,
    const Eigen::Vector3d& base_vel_world,
    const Eigen::Vector3d& base_vel_ref,
    const Eigen::Vector3d& base_pos_world,
    double yaw_rate_cmd,
    const RaibertParams& params) {

    Eigen::Vector3d p_foot = hip_pos_world;

    // 项1: 前馈——半步距离
    p_foot += 0.5 * params.stance_time * base_vel_world;

    // 项2: 速度反馈
    p_foot += params.kp_feedback * (base_vel_world - base_vel_ref);

    // 项3: 航向修正
    Eigen::Vector3d hip_offset = hip_pos_world - base_pos_world;
    Eigen::Vector3d yaw_correction;
    yaw_correction << -yaw_rate_cmd * hip_offset.y(),
                       yaw_rate_cmd * hip_offset.x(),
                       0.0;
    p_foot += 0.5 * params.stance_time * yaw_correction;

    // 投影到地面（平地假设）
    p_foot.z() = 0.0;

    return p_foot;
}

// ❌ 错误写法 1：忘记航向修正项
Eigen::Vector3d raibertWrong1(
    const Eigen::Vector3d& hip, const Eigen::Vector3d& v,
    const Eigen::Vector3d& v_ref, double Ts, double kp) {
    return hip + 0.5 * Ts * v + kp * (v - v_ref);
    // 问题：转弯时左右脚落在同一侧 → 交叉步 → 摔倒
}

// ❌ 错误写法 2：前馈项用参考速度
Eigen::Vector3d raibertWrong2(
    const Eigen::Vector3d& hip, const Eigen::Vector3d& v,
    const Eigen::Vector3d& v_ref, double Ts, double kp) {
    return hip + 0.5 * Ts * v_ref + kp * (v - v_ref);
    // 问题：前馈项补偿的是"期望运动"而非"实际运动"
    // 当 v ≠ v_ref 时，对称点计算错误
}

// ❌ 错误写法 3：z 分量未清零
Eigen::Vector3d raibertWrong3(
    const Eigen::Vector3d& hip, const Eigen::Vector3d& v,
    const Eigen::Vector3d& v_ref, double Ts, double kp) {
    return hip + 0.5 * Ts * v + kp * (v - v_ref);
    // 问题：hip 的 z 分量不为零（髋关节在地面以上）
    // 落脚点的 z 值错误 → 摆动轨迹指向空中
}

⚠️ 常见陷阱

⚠️ 编程陷阱：\(k_p\) 设得太大 - 错误做法：kp_feedback = 1.0（远大于 \(1/\omega \approx 0.175\)） - 现象：机器人在追踪速度指令时剧烈振荡，左右摇摆，最终摔倒 - 根本原因：\(k_p\) 过大时 Poincare 映射的特征值 \(|\lambda| > 1\)——过度修正引发正反馈振荡 - 正确做法：从 \(k_p = 0.05\) 开始，逐步增大到 \(0.1\)-\(0.2\)，用仿真验证稳定性 - 自检方法：画出每步的实际速度与参考速度的误差曲线，应该指数收敛而非振荡

💡 概念误区：认为"前馈项用当前速度还是参考速度无所谓" - 新手想法："\(\frac{T_s}{2} \boldsymbol{v}_{\text{base}}\) 和 \(\frac{T_s}{2} \boldsymbol{v}_{\text{ref}}\) 差不多" - 实际上：前馈项**必须用当前实际速度**，因为它要补偿的是"实际的基座运动"，而不是"期望的运动"。如果用参考速度，当实际速度偏离参考时，对称点计算就错了——推导中 \(v_0\) 是实际初速度，不是参考速度 - 正确理解：前馈保证"按当前速度走是稳定的"，反馈保证"逐步收敛到目标速度"

🧠 思维陷阱：认为"Raibert 太简单不值得深入研究" - 新手想法："这只是一个线性公式，没什么深度" - 实际上：Raibert 公式的每一项都有严格的动力学推导基础（倒立摆对称性）。更重要的是，即使是 2024-2026 年最先进的四足控制器（如 MIT Cheetah、ANYmal、Unitree Go2），底层的落脚点规划**仍然以 Raibert 为基础**，加上各种修正项。理解 Raibert 是理解所有后续方法的前提 - 正确思维：Raibert 是落脚规划的"牛顿第一定律"——简单但是基础

练习

1. [推导题] 从 LIPM 通解出发，不用小角度近似，推导精确的 Raibert 前馈项 \(\frac{v_0}{\omega} \tanh(\omega T_s/2)\)。对 Go2（\(h = 0.3\) m，\(T_s = 0.3\) s），计算精确值和近似值 \(\frac{T_s}{2} v_0\) 的百分比误差。
2. [仿真题] 实现一维 LIPM 仿真器（参考 足式/70_腿足简化模型理论 代码），施加 Raibert 控制律。画出 \(k_p\) 从 \(0\) 到 \(0.5\) 变化时，速度误差收敛的步数。验证 \(k_p = 1/\omega\) 附近是否最优。
3. [对比题] OCS2 的 SwingTrajectoryPlanner.cpp 中，找到 Raibert 公式的实现位置。与本节推导对比，指出 OCS2 实现了哪些额外的修正项（提示：查看 height map query 和 clamp 操作）。

---

58.3 从 Raibert 到 Capture Point ⭐⭐

这一节解决什么问题：解释 Raibert 在哪些情况下失效，以及 Capture Point / DCM 如何系统性地修补这些缺陷。这是从"工程启发式"到"基于动力学的最优规划"的关键桥梁。

动机：Raibert 失效的三个场景

Raibert 公式在平坦地面、稳态行走时表现出色。但以下场景会让它失效：

场景 1：大扰动推动

一个人从侧面推了机器人一把。此时实际速度 \(\boldsymbol{v}_{\text{base}}\) 突然变大，Raibert 的反馈项 \(k_p \Delta\boldsymbol{v}\) 会让落脚点大幅偏移。但 \(k_p\) 是一个**固定常数**，它不知道"多大的偏移足以捕获这个扰动"。

定量分析：假设 Go2 被推后速度从 0 增加到 2 m/s。Raibert（\(k_p = 0.175\)）给出的额外步幅为 \(0.175 \times 2 = 0.35\) m。但要消除发散模态，精确值为 \(2/\omega = 0.35\) m——在这个例子中 Raibert 恰好给出正确答案！但这只在 \(k_p = 1/\omega\) 时成立。如果 \(k_p\) 被手动调小（如 0.1），步幅只有 0.2 m，不足以捕获扰动。

场景 2：斜坡行走

在斜坡上，重力在水平方向有分量，LIPM 的动力学变成：

\[\ddot{x} = \omega^2(x - p) + g \sin\theta\]

\(g \sin\theta\) 项是一个持续的加速度偏移。Raibert 的前馈项没有考虑这个，导致机器人在下坡时越走越快（加速度累积），上坡时越走越慢直到停下。

场景 3：需要紧急停止

机器人高速运动时突然需要停下（\(v_{\text{ref}} = 0\)）。Raibert 的反馈项给出 \(k_p v\)，但是否足以在一步内停下？如果 \(k_p v > r_{\max}\)（超出腿的可达范围），就需要**多走几步才能停下来**。Raibert 不告诉你"需要几步"。

关键洞察：从速度反馈到"捕获"思维

Raibert 的思维方式是"调节速度"——通过落脚点让速度收敛到目标。但还有一种更本质的思维方式：让机器人能停下来。

问：如果你**现在就想让机器人停住**（速度归零），脚应该放在哪里？

这个位置就叫 Capture Point（捕获点），由 Jerry Pratt 在 2006 年的 Humanoids 会议论文中提出。Pratt 当时在 IHMC（佛罗里达人类和机器认知研究所）研究人形机器人的推恢复问题。

从 LIPM 推导 Capture Point

Step 1：我们要找一个落脚点 \(p^*\)，使得机器人从当前状态 \((x_0, \dot{x}_0)\) 出发，在无穷远时间后速度趋于零：

\[\lim_{t \to \infty} \dot{x}(t) = 0\]

Step 2：回顾 LIPM 的通解（以 \(p^*\) 为支撑点）：

\[x(t) - p^* = C_1 e^{\omega t} + C_2 e^{-\omega t}\]

\[\dot{x}(t) = C_1 \omega e^{\omega t} - C_2 \omega e^{-\omega t}\]

速度在 \(t \to \infty\) 趋于零，要求发散模态系数 \(C_1 = 0\)（否则 \(C_1 \omega e^{\omega t} \to \infty\)）。

Step 3：由初始条件确定 \(C_1\) 和 \(C_2\)。

\[C_1 + C_2 = x_0 - p^*\]

\[C_1 \omega - C_2 \omega = \dot{x}_0\]

解这个线性系统：

\[C_1 = \frac{1}{2}\left[(x_0 - p^*) + \frac{\dot{x}_0}{\omega}\right]\]

\[C_2 = \frac{1}{2}\left[(x_0 - p^*) - \frac{\dot{x}_0}{\omega}\right]\]

令 \(C_1 = 0\)：

\[x_0 - p^* + \frac{\dot{x}_0}{\omega} = 0\]

\[\boxed{p^* = x_0 + \frac{\dot{x}_0}{\omega} \equiv \xi}\]

这就是 Capture Point 的定义：\(\xi = x + \dot{x}/\omega\)。

Step 4：物理解释

Capture Point \(\xi\) 的含义是：如果把脚放在 \(\xi\) 处，倒立摆的发散模态被精确消除，系统只剩收敛模态 \(e^{-\omega t}\)，CoM 会渐近地收敛到脚的正上方并最终停下来。

脚的位置 vs Capture Point	发散模态系数 \(C_1\)	运动趋势
\(p = \xi\)（精确在 CP 上）	\(C_1 = 0\)	渐近停下
\(p < \xi\)（CP 前方）	\(C_1 > 0\)	继续加速 → 摔向前
\(p > \xi\)（CP 后方）	\(C_1 < 0\)	先倒退再加速 → 可能倒摔

Capture Point 与 Raibert 的精确联系

将 Capture Point 公式与 Raibert 公式对比：

\[\text{Capture Point}: \quad p^* = x + \frac{\dot{x}}{\omega}\]

\[\text{Raibert}: \quad p = p_{\text{hip}} + \frac{T_s}{2} v + k_p(v - v_{\text{ref}})\]

当 \(v_{\text{ref}} = 0\)（想要停下来）且 \(p_{\text{hip}} \approx x\)（CoM 近似在髋正上方）时：

\[\text{Raibert} = x + \left(\frac{T_s}{2} + k_p\right) v\]

Capture Point 要求总系数为 \(1/\omega\)。所以：

\[\frac{T_s}{2} + k_p = \frac{1}{\omega} \quad \Rightarrow \quad k_p = \frac{1}{\omega} - \frac{T_s}{2}\]

结论：当 \(k_p = 1/\omega - T_s/2\) 且 \(v_{\text{ref}} = 0\) 时，Raibert 退化为精确的 Capture Point 控制。

这揭示了 Raibert 启发式的深层含义：它是 Capture Point 控制的一个特例，加上了参考速度追踪的额外项。Capture Point 是从"消除发散模态"出发推导的精确公式，Raibert 是基于对称性直觉的近似公式。

DCM——Divergent Component of Motion

日本的 Takenaka（2009）和德国 DLR 的 Englsberger 等人（2013-2015）独立发现了 Capture Point 的另一个面目——DCM（发散运动分量）。

定义：DCM \(\boldsymbol{\xi}\) 是 CoM 状态的线性组合：

\[\boldsymbol{\xi} = \boldsymbol{x} + \frac{1}{\omega} \dot{\boldsymbol{x}}\]

推导 DCM 动力学（从 LIPM 出发）：

\[\dot{\boldsymbol{\xi}} = \dot{\boldsymbol{x}} + \frac{1}{\omega}\ddot{\boldsymbol{x}} = \dot{\boldsymbol{x}} + \frac{1}{\omega}\omega^2(\boldsymbol{x} - \boldsymbol{p}) = \dot{\boldsymbol{x}} + \omega(\boldsymbol{x} - \boldsymbol{p})\]

\[= \omega\boldsymbol{x} + \dot{\boldsymbol{x}} - \omega\boldsymbol{p} = \omega\left(\boldsymbol{x} + \frac{\dot{\boldsymbol{x}}}{\omega}\right) - \omega\boldsymbol{p}\]

\[\boxed{\dot{\boldsymbol{\xi}} = \omega(\boldsymbol{\xi} - \boldsymbol{p})}\]

这是一个**一阶线性 ODE**！它比原始的 LIPM（二阶 ODE）简单得多。

关键性质： - DCM 的动力学是**一阶不稳定**的（特征值 \(+\omega > 0\)） - 如果 \(\boldsymbol{\xi} \neq \boldsymbol{p}\)，DCM 会指数发散——这正是"divergent"的含义 - 要让 DCM 稳定，必须让支撑点 \(\boldsymbol{p}\) 追踪 DCM

DCM 控制律：

\[\boldsymbol{p} = \boldsymbol{\xi} + \frac{k_{\xi}}{\omega} (\boldsymbol{\xi} - \boldsymbol{\xi}_{\text{ref}})\]

其中 \(k_{\xi} > 0\) 是控制增益。代入 DCM 动力学 \(\dot{\boldsymbol{\xi}} = \omega(\boldsymbol{\xi} - \boldsymbol{p})\) 可得误差方程 \(\dot{\boldsymbol{e}}_\xi = -k_\xi \boldsymbol{e}_\xi\)（\(k_\xi > 0\) 时收敛）。

Pratt vs Englsberger：历史与术语辨析

维度	Pratt 2006 (Capture Point)	Englsberger 2013-2015 (DCM)
定义域	地面平面 (2D)	三维空间 (3D)
物理解释	"踩在这里能停下来"	"LIPM 状态的发散分量"
数学形式	在 LIPM 模型下与 DCM 等价	可推广到非平面/非 LIPM 模型
规划方式	单步 Capture Point 目标	DCM 参考轨迹 + 反时间积分
典型应用	推恢复（push recovery）	连续行走轨迹生成
学术传统	美国（IHMC / MIT）	德国/日本（DLR / AIST）

两个术语在 LIPM 模型下**数学完全等价**。本教材中根据上下文交替使用：讨论"能否停下来"时用 Capture Point，讨论"轨迹规划"时用 DCM。

⚠️ 常见陷阱

💡 概念误区：把 Capture Point 和 CoP/ZMP 混淆 - 新手想法："Capture Point 就是 ZMP 的另一种说法" - 实际上：ZMP/CoP 是**地面反力的等效作用点**（力的概念），Capture Point 是**为了停下来脚应该放的位置**（规划的概念）。ZMP 是一个瞬时量，由当前的力决定；CP 是一个规划目标，由当前的状态决定 - 数学区分：ZMP 由力决定 \(p_{\text{ZMP}} = x - \ddot{x}h/g\)，CP 由状态决定 \(\xi = x + \dot{x}/\omega\)。在稳态匀速行走中，ZMP 在脚的位置，CP 在 ZMP 前方 \(\dot{x}/\omega\) 处

🧠 思维陷阱：认为"只要踩在 Capture Point 上就永远安全" - 新手想法："每一步都踩在 CP 上，机器人就不会摔" - 实际上：踩在 CP 上只能**在无穷远时间后停下来**——这需要无穷长的支撑相。如果摆动相时间有限（实际必然如此），且下一步的 CP 超出可达范围，仍然会摔。这就是 N-step capturability 要解决的问题（58.4 节） - 更精确的说法：踩在 CP 上保证了**不会因发散模态而摔倒**，但不保证**运动学可行性**

练习

1. [推导题] 从 DCM 定义 \(\xi = x + \dot{x}/\omega\) 出发，对两边求导，结合 LIPM 方程 \(\ddot{x} = \omega^2(x - p)\)，推导出 DCM 动力学 \(\dot{\xi} = \omega(\xi - p)\)。写出每一步用了什么数学工具。
2. [计算题] Go2 以 \(v = 1\) m/s 行走，\(h = 0.3\) m。计算 Capture Point 相对于 CoM 的偏移量 \(\xi - x = v/\omega\)。如果被推了一把使速度变为 2 m/s，CP 偏移变为多少？与 Go2 腿长（0.25 m）相比如何？
3. [思考题] 为什么 Capture Point 控制对双足比对四足更重要？（提示：四足有多只支撑脚带来的冗余性。即使一步的 CP 未被精确捕获，其他脚仍提供支撑。双足在单脚支撑相只有一个支撑点，必须精确。）

---

58.4 Capture Point 落脚规划 ⭐⭐⭐

这一节解决什么问题：将 Capture Point 从"紧急停车"扩展为系统性的落脚规划框架——一步 capturability、N 步 capturability、以及基于 DCM 的连续行走规划。

动机：从"能否停下来"到"走多少步能停下来"

58.3 节推导了 Capture Point——踩在这个点上，机器人最终能停下来。但实际中有两个问题：

1. Capture Point 可能**不可达**（超出腿的工作空间）——比如机器人以 3 m/s 跑步时，CP 偏移 \(v/\omega = 3/5.72 = 0.52\) m，远超 Go2 腿长 0.25 m
2. 即使可达，也不意味着**必须在这一步停下来**——也许走两步、三步后停下来更自然、更节能

这引出了 Koolen、Pratt 等人在 2012 年发表的 N-step capturability 理论（IJRR, Part 1 & 2）。

一步 Capturability

定义：一个状态 \((x, \dot{x})\) 是**一步可捕获的**（1-step capturable），当且仅当存在一个可达的落脚点 \(p \in \mathcal{W}_{\text{kin}}\)，使得从 \((x, \dot{x})\) 出发、以 \(p\) 为支撑点的 LIPM 动力学最终收敛（即发散模态系数为零）。

从 58.3 节的分析，这个条件等价于：

\[\xi = x + \frac{\dot{x}}{\omega} \in \mathcal{W}_{\text{kin}}\]

即 Capture Point 在运动学可达范围内。

一步 Capture Region \(\mathcal{C}_1\)：所有一步可捕获状态的集合。在相平面 \((x, \dot{x})\) 上：

\[\mathcal{C}_1 = \{(x, \dot{x}) \mid x + \frac{\dot{x}}{\omega} \in [p_{\text{hip}} - r_{\max}, \, p_{\text{hip}} + r_{\max}]\}\]

这是一条**带状区域**——斜率为 \(-\omega\) 的两条平行线之间的区域。速度越大，CP 越远，越容易超出可达范围。

N 步 Capturability

定义（Koolen 2012）：一个状态 \((x, \dot{x})\) 是 N-step capturable，当且仅当存在一个不超过 N 步的落脚序列 \(\{p_1, p_2, \ldots, p_k\}\)（\(k \leq N\)），使得：

1. 每个 \(p_i\) 在对应时刻可达：\(p_i \in \mathcal{W}_{\text{kin}}(x_i)\)
2. 执行完 \(k\) 步后，状态变为 0-step capturable（即可以原地站稳）或 1-step capturable

N-step Capture Region \(\mathcal{C}_N\) 有嵌套性质：

\[\mathcal{C}_0 \subseteq \mathcal{C}_1 \subseteq \mathcal{C}_2 \subseteq \mathcal{C}_3 \subseteq \cdots \subseteq \mathcal{C}_{\infty}\]

直觉：允许走的步数越多，能恢复的扰动越大。\(\mathcal{C}_{\infty}\) 对应**可行性核**（viability kernel）——能永远走下去而不摔的所有状态。

N-step Capture Region 的递推计算

Koolen 给出了用 LIPM 解析解进行递推的方法。

递推关系：\((x, \dot{x}) \in \mathcal{C}_{N+1}\) 当且仅当存在可达落脚点 \(p \in \mathcal{W}_{\text{kin}}\)，使得经过一个支撑相（时长 \(T_s\)）后的状态 \((x', \dot{x}')\) 属于 \(\mathcal{C}_N\)。

利用 LIPM 的解析前向映射：

\[x'(T_s) = (x - p)\cosh(\omega T_s) + \frac{\dot{x}}{\omega}\sinh(\omega T_s) + p\]

\[\dot{x}'(T_s) = (x - p)\omega\sinh(\omega T_s) + \dot{x}\cosh(\omega T_s)\]

从 \(\mathcal{C}_1\) 到 \(\mathcal{C}_2\) 的示例计算：

1. 对每个候选状态 \((x, \dot{x})\)，遍历所有可达落脚点 \(p\)
2. 计算一步后的状态 \((x', \dot{x}')\)
3. 检查 \((x', \dot{x}') \in \mathcal{C}_1\)
4. 如果存在至少一个 \(p\) 使得条件成立，则 \((x, \dot{x}) \in \mathcal{C}_2\)

实际观察：\(\mathcal{C}_N\) 随 \(N\) 增大而增大，但增量递减：

N	Capture Region 增长	最大可恢复速度（示例）
1	基准	\(\omega r_{\max} \approx 1.1\) m/s
2	显著增大	~1.8 m/s
3	中等增大	~2.2 m/s
5	微小增大	~2.5 m/s
\(\infty\)	收敛	~2.6 m/s

与 Viability Theory 的联系

Koolen 的 N-step capturability 与控制论中的 viability theory（Aubin 1991）有深刻联系：

• Viable state：存在一条从该状态出发的轨迹，永远不离开约束集
• Viability kernel：所有 viable state 的集合
• \(\mathcal{C}_\infty \approx\) viability kernel（在离散步态模型下）

这意味着 N-step capturability 不仅仅是一个工程概念，而是有坚实的数学理论支撑。

基于 DCM 的连续行走规划

Englsberger 等人将 DCM 理论发展为一套完整的行走规划框架。核心思路是**反时间规划**（backward planning）。

Step 1：给定一系列落脚点 \(\{p_1, p_2, \ldots, p_N\}\) 和每步的支撑时间 \(\{T_1, T_2, \ldots, T_N\}\)。

Step 2：设定终态约束。最后一步结束时，DCM 必须收敛到最终支撑点：

\[\boldsymbol{\xi}_{\text{eos},N} = \boldsymbol{p}_N \quad \text{（end of step N）}\]

Step 3：DCM 动力学 \(\dot{\xi} = \omega(\xi - p)\) 在每个支撑相内有解析解（\(p\) 恒定）：

\[\boldsymbol{\xi}(t) = \boldsymbol{p} + e^{\omega t}(\boldsymbol{\xi}(0) - \boldsymbol{p})\]

因此从一步结束的 DCM 可以**反推**该步开始的 DCM：

\[\boldsymbol{\xi}_{\text{ini},i} = \boldsymbol{p}_i + e^{-\omega T_i}(\boldsymbol{\xi}_{\text{eos},i} - \boldsymbol{p}_i)\]

其中步间连续性 \(\boldsymbol{\xi}_{\text{eos},i} = \boldsymbol{\xi}_{\text{ini},i+1}\)。

Step 4：从 \(N\) 向 1 递推，得到每个时刻的 DCM 参考轨迹，然后通过 DCM 控制律确定实时的支撑点位置。

反时间递推过程:

    步 N (最后):  ξ_eos,N = p_N  (终态约束)
         ↑
    步 N-1:       ξ_ini,N = p_{N-1} + e^{-ωT_{N-1}}(ξ_eos,N-1 - p_{N-1})
         ↑                  其中 ξ_eos,N-1 = ξ_ini,N
    步 N-2:       同理递推
         ↑
    ...
         ↑
    步 1 (当前):   ξ_ini,1 ← 这就是当前应该追踪的 DCM 目标

Capture Point 混合落脚公式

在实际工程中，可以将 Capture Point 融入 Raibert 框架：

\[\boldsymbol{p}_{\text{foot}} = \alpha \boldsymbol{p}_{\text{hip}} + (1-\alpha) \boldsymbol{\xi} + k_p (\boldsymbol{v} - \boldsymbol{v}_{\text{ref}})\]

其中 \(\alpha \in [0, 1]\) 是混合系数： - \(\alpha = 1\)：纯 Raibert（只看髋位置 + 速度前馈） - \(\alpha = 0\)：纯 Capture Point（只看 DCM 位置） - \(\alpha \in (0, 1)\)：混合——实际工程中通常取 \(\alpha = 0.3\)-\(0.7\)

为什么要混合而不直接用 CP？纯 Capture Point 在高速行走时会给出过大的步幅（因为 \(\xi\) 远离髋关节），可能超出运动学可达范围。混合可以在动态稳定性和运动学可行性之间权衡。

⚠️ 常见陷阱

⚠️ 编程陷阱：DCM 反时间递推中 \(e^{\omega T}\) 的数值爆炸 - 错误做法：当 \(\omega T\) 较大（如 \(\omega = 5.7\)，\(T = 1.0\) s）时，\(e^{\omega T} = e^{5.7} \approx 298\) - 现象：DCM 参考轨迹数值极大，控制器发散 - 根本原因：长支撑相 + 高自然频率导致指数项爆炸。物理上意味着"如果走一大步，初始 DCM 必须非常精确才能在步末收敛到目标" - 正确做法：限制单步时间 \(T_i\)，使 \(\omega T_i < 2\)-\(3\)（即 \(T_i < 0.35\)-\(0.52\) s for Go2）；或者增加步数减小每步时间

💡 概念误区：认为 "N-step capturability 中 N 越大越好" - 新手想法："看得越远越安全，所以 N 应该尽可能大" - 实际上：\(\mathcal{C}_N\) 增大的速度递减，\(N > 5\) 后几乎不再增大。而且大 N 意味着更长的规划 horizon、更多的不确定性（未来地形、状态估计误差累积）。实际中 N=2-3 步就已经覆盖了绝大多数场景 - 正确做法：只有在特殊地形（如 stepping stones）上才需要大 N 的多步前瞻

练习

1. [计算题] Go2 的腿长约 0.25 m，可达工作空间半径约 0.2 m。计算 1-step Capture Region 允许的最大速度（即 \(\dot{x}_{\max}\) 使得 \(\xi = x + \dot{x}/\omega\) 仍在可达范围内）。答案应约为 \(\omega \cdot r_{\max}\)。
2. [推导题] 对两步行走序列（步 1 和步 2），给定 \(p_1, p_2, T_1, T_2\) 和终态 \(\xi_{\text{eos},2} = p_2\)。用反时间递推写出 \(\xi_{\text{ini},1}\) 的表达式（展开到具体的 \(p_1, p_2, T_1, T_2\) 组合）。
3. [思考题] IHMC 的 Atlas 人形机器人使用 Capture Point 进行推恢复。为什么推恢复场景特别适合 Capture Point 方法？（提示：推恢复的目标就是"尽快停下来"，正好对应 CP 的定义。）

---

58.5 ZMP 步态规划 ⭐⭐

这一节解决什么问题：讲解人形机器人传统的 ZMP 步态规划方法——通过 Preview Control 从期望 CoM 轨迹反推 ZMP 参考，以及如何从 ZMP 参考推导落脚点序列。

动机：人形机器人的"日本路线"

1990-2010 年代，日本的人形机器人研究（Honda ASIMO、AIST HRP 系列）走了一条不同于 Raibert 的技术路线。Raibert 研究的是弹跳式动态运动，而日本研究者追求的是**准静态步行**——让 ZMP 始终严格在支撑多边形内，确保机器人在任何瞬间都不会翻倒。

这个路线的核心问题是：给定期望的步行速度和步态时序，如何生成一条 CoM 轨迹，使得 ZMP 始终满足约束？

Kajita Shuuji 等人在 2003 年 ICRA 发表的 Preview Control 论文 "Biped walking pattern generation by using preview control of zero-moment point" 是这条路线的里程碑，在 HRP-2 人形机器人上实现了稳定行走。

如果不控制 ZMP 会怎样

考虑一个简单的 CoM 轨迹：匀加速从静止到 0.5 m/s。

ZMP 公式 \(p_{\text{ZMP}} = x - \frac{h}{g}\ddot{x}\)。如果 \(\ddot{x} = 1\) m/s\(^2\)（匀加速），\(h/g \approx 0.082\)：

\[p_{\text{ZMP}} = x - 0.082 \text{ m}\]

ZMP 比 CoM 落后 8 cm。这看起来不大，但如果加速度更大（如快速启动），ZMP 可能移出支撑多边形——机器人翻倒。

Preview Control 的作用就是：提前"看到"需要加速，从 CoM 轨迹中**预先调整**，使 ZMP 始终在安全范围内。

ZMP 的数学定义回顾

零力矩点（Zero Moment Point）是 Vukobratovic 于 1972 年提出的。对 LIPM：

\[p_{\text{ZMP}} = x - \frac{h}{g} \ddot{x}\]

推导过程（从力矩平衡出发）：

在支撑点 \(p\) 处，对 CoM 质点做力矩分析。重力 \(mg\) 在 \(p\) 处的力矩为 \(mg(x - p)\)（向前翻倒方向）。地面反力的法向分量 \(N = mg\)（竖直平衡），不产生水平力矩。地面反力的水平分量提供加速度：\(F_x = m\ddot{x}\)。

ZMP 定义为"净力矩为零的点"：

\[mg(x - p_{\text{ZMP}}) - m\ddot{x} \cdot h = 0\]

\[\Rightarrow p_{\text{ZMP}} = x - \frac{h}{g}\ddot{x}\]

ZMP 稳定性判据：如果 \(p_{\text{ZMP}}\) 在支撑多边形内，则地面能够提供足够的反力矩来阻止翻倒。ZMP 移出支撑多边形意味着地面反力矩不够——机器人开始绕支撑边缘翻转。

Preview Control 方法

核心思想：将 LIPM 方程改写为以"CoM 加加速度"（jerk，\(\dddot{x}\)）为输入的三阶系统，然后用最优预览控制（optimal preview control）理论设计一个跟踪 ZMP 参考的控制器。

Step 1：系统建模

LIPM 方程给出 \(p_{\text{ZMP}} = x - \frac{h}{g}\ddot{x}\)。将 CoM 的 jerk \(u = \dddot{x}\) 作为控制输入，定义状态 \(\mathbf{s} = (x, \dot{x}, \ddot{x})^T\)：

\[\dot{\mathbf{s}} = \underbrace{\begin{pmatrix} 0 & 1 & 0 \\ 0 & 0 & 1 \\ 0 & 0 & 0 \end{pmatrix}}_{A_c} \mathbf{s} + \underbrace{\begin{pmatrix} 0 \\ 0 \\ 1 \end{pmatrix}}_{B_c} u\]

\[p_{\text{ZMP}} = \underbrace{\begin{pmatrix} 1 & 0 & -h/g \end{pmatrix}}_{C_c} \mathbf{s}\]

Step 2：增广系统（误差积分器）

为了实现零稳态误差跟踪，引入 ZMP 跟踪误差的积分 \(e = p_{\text{ZMP}} - p_{\text{ZMP}}^{\text{ref}}\)。增广状态 \(\tilde{\mathbf{s}} = (e, x, \dot{x}, \ddot{x})^T\)。

Step 3：Preview Control 代价函数

\[J = \sum_{k=0}^{\infty} \left[ Q_e \cdot e_k^2 + R \cdot u_k^2 \right]\]

其中 \(Q_e\) 权衡 ZMP 跟踪精度，\(R\) 权衡 jerk 的平滑性。

Step 4：最优解的形式

通过离散时间 LQR 理论 + 预览项，最优控制律为：

\[u_k = -K_I \sum_{j=0}^{k} e_j - K_x \mathbf{s}_k + \sum_{j=1}^{N_p} K_{\text{pv}}(j) \cdot p_{\text{ZMP},k+j}^{\text{ref}}\]

项	名称	作用
\(K_I \sum e\)	积分项	消除 ZMP 稳态误差
\(K_x \mathbf{s}\)	状态反馈	稳定化
\(\sum K_{\text{pv}} p^{\text{ref}}\)	预览项	"看"未来 ZMP 参考，提前调整

直觉理解：控制器"看"未来 \(N_p\) 步的 ZMP 参考，提前调整 CoM 加速度，使得 CoM 轨迹平滑地跟踪 ZMP 参考。这就像开车时"看远处弯道提前打方向盘"。

ZMP 参考如何确定落脚点

Preview Control 的输入是 ZMP 参考轨迹 \(p_{\text{ZMP}}^{\text{ref}}(t)\)。这个参考怎么来？

在**预定义步态**中，ZMP 参考直接由落脚点决定：

• 单脚支撑相（单足站立）：\(p_{\text{ZMP}}^{\text{ref}} =\) 当前支撑脚的位置
• 双脚支撑相（双足同时着地的过渡期）：\(p_{\text{ZMP}}^{\text{ref}}\) 从后脚线性过渡到前脚

因此，**落脚点序列 → ZMP 参考序列 → Preview Control → CoM 轨迹**构成了完整的规划管线。

Kajita 2003 的实验参数

参数	值
机器人	HRP-2 人形（26 DOF）
控制周期	5 ms（200 Hz）
预览窗口	1.6 s（320 个控制步）
CoM 高度	0.8 m
步行速度	0.25 m/s
步长	0.3 m
步态周期	1.2 s
ZMP 参考更新	每 40 ms 重新规划

ZMP 路线 vs Capture Point 路线的比较

维度	ZMP Preview Control	Capture Point / DCM
哲学	"ZMP 永远在支撑多边形内"	"能停下来就安全"
稳定性定义	ZMP 约束满足	发散模态被消除
计算方式	LQR + 预览 + 矩阵运算	代数公式（闭合解）
适用步态	准静态步行	动态步行/跑步
地形适应	平地为主	可扩展到斜坡
抗扰动	中等（依赖预览窗口长度）	好（直接关联状态）
代表平台	HRP-2, ASIMO, NAO	Atlas, Valkyrie, Digit
时代主流	2000-2015	2006-至今

⚠️ 常见陷阱

💡 概念误区：认为"ZMP 方法过时了，不值得学" - 新手想法："Capture Point 和 RL 才是主流，ZMP 是上一代的东西" - 实际上：ZMP 概念**至今仍在使用**——即使是最新的 MPC 控制器（如 Di Carlo 2018 的凸 MPC），也将"ZMP 在支撑多边形内"作为约束条件之一。理解 ZMP 和 Preview Control 的数学对理解现代 MPC 的约束设计非常有帮助 - 正确思维：ZMP 不是"过时的替代品"，而是"稳定性约束的一种表达方式"

⚠️ 编程陷阱：Preview Control 的预览窗口太短 - 错误做法：N_preview = 10（只看 50 ms 前方） - 现象：CoM 轨迹出现大幅振荡，ZMP 超出支撑多边形 - 根本原因：Preview Control 需要"看到"至少一两步之后的 ZMP 参考。预览窗口短于步行周期时，控制器没有足够的时间预先调整 CoM - 正确做法：预览窗口至少覆盖 2-3 个步态周期（对 \(T_{\text{gait}} = 1.2\) s，至少 \(N_p = 2.4\) s / 5 ms = 480 步）

练习

1. [推导题] 从 LIPM 方程和力矩平衡出发，推导 ZMP 公式 \(p_{\text{ZMP}} = x - \frac{h}{g}\ddot{x}\)。验证：当 CoM 匀速运动（\(\ddot{x} = 0\)）时，ZMP 在 CoM 正下方。
2. [对比题] 对同样的"从静止加速到 1 m/s"任务，定性分析 Raibert、Capture Point、ZMP Preview Control 三种方法各自如何确定落脚点。哪种方法的 CoM 轨迹最平滑？
3. [思考题] ZMP Preview Control 需要预知未来的 ZMP 参考（即未来的落脚点）。在不确定地形上，这个"预知"如何实现？（提示：分层架构——高层规划给出粗略的步态计划，低层用 Preview Control 执行；地形变化时更新高层计划。）

---

58.6 运动学可达性与地形约束 ⭐⭐

这一节解决什么问题：无论用哪种落脚规划方法，最终的落脚点必须满足两个硬约束——(1) 腿能够到（运动学可达性），(2) 地面能踩（地形安全性）。本节分析这两个约束的数学形式和工程实现。

动机：Raibert/CP 算出来的点不一定能踩

考虑以下场景：

1. 机器人被推了一把，CP 公式给出落脚点距离髋关节 0.4 m——但 Go2 腿只有 0.25 m
2. Raibert 算出的落脚点恰好在一个深坑上方
3. 落脚点在 45 度斜坡上，接触法向量几乎水平，摩擦力不足以产生所需的 GRF

所有这些情况都说明：落脚点规划不能只考虑动力学最优，还必须检查运动学和地形约束。

运动学可达工作空间

单腿可达工作空间 \(\mathcal{W}_{\text{kin}}\) 是足端相对于髋关节能到达的所有位置的集合。

对于典型的三自由度四足腿（髋 abduction \(q_1\)、髋 flexion \(q_2\)、膝 flexion \(q_3\)），足端位置通过正运动学给出：

\[\boldsymbol{p}_{\text{foot}} = \boldsymbol{p}_{\text{hip}} + \boldsymbol{f}_{\text{FK}}(q_1, q_2, q_3)\]

\(\mathcal{W}_{\text{kin}}\) 的精确形状是一个被关节极限裁剪的**球壳子集**：

\[\mathcal{W}_{\text{kin}} \approx \{(x, y, z) \mid r_{\min} \leq \|\boldsymbol{r}\| \leq r_{\max}, \quad \boldsymbol{r} \in \mathcal{J}\text{-feasible}\}\]

其中： - \(r_{\min} = |l_1 - l_2|\)（大腿和小腿完全折叠时的最短距离） - \(r_{\max} = l_1 + l_2\)（完全伸直时的最长距离——但实际不可达，因为膝关节有角度限制） - \(\mathcal{J}\text{-feasible}\) 由关节角度范围约束

Go2 的典型参数：

参数	值
大腿长 \(l_1\)	0.213 m
小腿长 \(l_2\)	0.213 m
膝关节范围	\([0.92, 2.70]\) rad
髋 flexion 范围	\([-1.05, 2.10]\) rad
髋 abduction 范围	\([-0.86, 0.86]\) rad
实际可达半径	~0.12-0.38 m

可达性裕度（reachability margin）：

\[\delta_{\text{kin}} = \min_i \left( \min(q_i - q_{\min,i}, \quad q_{\max,i} - q_i) \right)\]

好的落脚点应该让 \(\delta_{\text{kin}}\) 尽可能大——即关节远离极限位置，保留余量应对扰动。

地形约束

假设已有高程图 \(h_{\text{map}}(x, y)\)（由深度相机或 LiDAR 获取，足式/160_感知驱动落脚规划 详述），可以分析以下地形特征：

特征	计算方式	安全要求
坡度	\(\theta = \arctan(\|\nabla h_{\text{map}}\|)\)	\(\theta < \theta_{\max}\)（通常 25-35 度）
粗糙度	局部高度方差 \(\sigma^2\)	\(\sigma^2 < \sigma_{\max}^2\)（通常 0.005-0.02 m\(^2\)）
法向量	\(\hat{n} = (-\partial h/\partial x, -\partial h/\partial y, 1)^T / \|\cdot\|\)	\(\hat{n} \cdot \hat{z} > \cos(\theta_{\max})\)
间隙检测	到最近无效区域的距离 \(d_{\text{edge}}\)	\(d_{\text{edge}} > r_{\text{foot}}\)
高度跳变	相邻格子高度差 $	\Delta h

法向量约束与摩擦锥：

如果地面法向量过于倾斜，即使脚能到达该点，地面反力也不足以支撑机器人。摩擦锥约束要求：

\[\frac{f_t}{f_n} \leq \mu\]

其中 \(f_t\) 是切向力，\(f_n\) 是法向力，\(\mu\) 是摩擦系数。在斜面上，法向量倾斜使得 \(f_n\) 减小，同时重力的切向分量增大——双重不利。

联合约束的实现

struct FootholdFeasibility {
    bool is_reachable;       // 运动学可达
    bool is_terrain_safe;    // 地形安全
    double kin_margin;       // 关节裕度 (0-1)
    double terrain_score;    // 地形质量 (0-1)
};

FootholdFeasibility checkFeasibility(
    const Eigen::Vector3d& foothold,
    const Eigen::Vector3d& hip_pos,
    const HeightMap& terrain,
    const KinematicModel& kin_model) {

    FootholdFeasibility result;

    // === 运动学可达性 ===
    Eigen::Vector3d leg_vec = foothold - hip_pos;
    double leg_length = leg_vec.norm();

    // 粗检查：腿长范围
    if (leg_length < kin_model.min_reach ||
        leg_length > kin_model.max_reach) {
        result.is_reachable = false;
        result.kin_margin = 0.0;
        return result;
    }

    // 精检查：逆运动学求解
    Eigen::Vector3d q_solution;
    bool ik_success = kin_model.solveIK(hip_pos, foothold,
                                         q_solution);
    result.is_reachable = ik_success;
    if (ik_success) {
        result.kin_margin = kin_model.jointMargin(q_solution);
    }

    // === 地形安全性 ===
    double h = terrain.query(foothold.x(), foothold.y());
    if (std::isnan(h)) {
        result.is_terrain_safe = false;
        return result;
    }

    Eigen::Vector3d normal = terrain.getNormal(
        foothold.x(), foothold.y());
    double slope_cos = normal.z();  // cos(slope_angle)

    double roughness = terrain.getLocalVariance(
        foothold.x(), foothold.y(), /*radius=*/0.05);

    double edge_dist = terrain.distToInvalidRegion(
        foothold.x(), foothold.y());

    result.is_terrain_safe =
        (slope_cos > std::cos(0.52)) &&    // < 30 度
        (roughness < 0.01) &&               // 低粗糙度
        (edge_dist > 0.03);                 // 离边缘 3 cm

    result.terrain_score =
        0.4 * slope_cos +
        0.3 * std::max(0.0, 1.0 - roughness / 0.01) +
        0.3 * std::min(1.0, edge_dist / 0.1);

    return result;
}

⚠️ 常见陷阱

⚠️ 编程陷阱：可达性检查只用腿长，忽略关节极限和方向 - 错误做法：if ((foothold - hip).norm() < max_leg_length) return true; - 现象：落脚点在球壳内但在关节不可达的方向上（如髋关节正后方），导致逆运动学无解 - 根本原因：三自由度腿的工作空间不是完整球壳，而是被关节极限裁剪后的子集 - 正确做法：调用完整的逆运动学求解器验证：bool ok = IK::solve(hip, foothold, q_out);

💡 概念误区：认为"只要地面平就安全" - 新手想法："坡度为零的区域一定是好的落脚点" - 实际上：平坦的冰面、积水区域、松软土壤同样危险。地形安全不仅是几何问题，还包括摩擦系数和承载能力。几何分析是第一层过滤器，之后还需要物理属性分析 - 延伸：ANYmal 等先进平台使用力矩估计来检测脚是否在滑动，作为后验地形安全判断

练习

1. [计算题] Go2 的大腿长 \(l_1 = 0.213\) m，小腿长 \(l_2 = 0.213\) m，膝关节角度范围 \([0.92, 2.70]\) rad。用余弦定理计算腿的最短和最长工作长度。
2. [编程题] 写一个函数，输入高程图（2D 数组）和候选落脚点列表，输出每个点的可行性评分（0-1），综合考虑坡度、粗糙度和距离边界的裕度。
3. [思考题] 为什么四足机器人的落脚可达性约束通常比人形宽松？从腿的自由度、可达范围与步幅需求的比值、支撑冗余度三个角度分析。

---

58.7 落脚点评分函数 ⭐⭐

这一节解决什么问题：当有多个可行的落脚点时，如何选最好的？本节设计一个多准则评分函数，综合地形质量、运动学裕度、稳定性裕度等因素。

动机：可行不等于最优

58.6 节的可达性和地形检查给出了"哪些点可以踩"。但在可行集合中，不同的点质量差异很大：

• 一个在斜坡边缘的点虽然可行，但稍有偏差就会踩空
• 一个让膝关节接近极限的点虽然可达，但关节力矩很大
• 一个离 Raibert 名义点太远的点虽然安全，但需要额外的摆动相能量

评分函数设计

定义总评分为各分项的加权和：

\[S(\boldsymbol{p}) = w_1 S_{\text{terrain}}(\boldsymbol{p}) + w_2 S_{\text{kin}}(\boldsymbol{p}) + w_3 S_{\text{stability}}(\boldsymbol{p}) + w_4 S_{\text{nominal}}(\boldsymbol{p})\]

所有分项都归一化到 \([0, 1]\)，然后用权重 \(w_i\)（\(\sum w_i = 1\)）调节相对重要性。

分项 1：地形质量 \(S_{\text{terrain}} \in [0, 1]\)

\[S_{\text{terrain}} = \alpha_1 \underbrace{\left(1 - \frac{\theta}{\theta_{\max}}\right)}_{\text{坡度项}} + \alpha_2 \underbrace{\left(1 - \frac{\sigma^2}{\sigma_{\max}^2}\right)}_{\text{粗糙度项}} + \alpha_3 \underbrace{\min\left(\frac{d_{\text{edge}}}{d_{\text{safe}}}, 1\right)}_{\text{边缘距离项}}\]

\(\alpha_1 + \alpha_2 + \alpha_3 = 1\)（子权重），各项含义： - 坡度项：地面越平越好 - 粗糙度项：局部高度变化越小越好 - 边缘距离项：离悬崖、台阶边缘越远越好

分项 2：运动学裕度 \(S_{\text{kin}} \in [0, 1]\)

\[S_{\text{kin}} = \min_i \frac{\min(q_i - q_{\min,i}, \; q_{\max,i} - q_i)}{(q_{\max,i} - q_{\min,i})/2}\]

归一化后的关节角度裕度。落脚点使关节越靠近工作空间中心越好。

分项 3：稳定性裕度 \(S_{\text{stability}} \in [0, 1]\)

\[S_{\text{stability}} = \frac{d(\text{ZMP}, \partial\text{SP})}{d_{\max}(\text{ZMP}, \partial\text{SP})}\]

其中 \(d(\text{ZMP}, \partial\text{SP})\) 是 ZMP 到支撑多边形（Support Polygon）边界的最短距离，\(d_{\max}\) 是可能的最大距离（通常取支撑多边形内切圆半径）。

分项 4：名义偏移 \(S_{\text{nominal}} \in [0, 1]\)

\[S_{\text{nominal}} = \exp\left(-\frac{\|\boldsymbol{p} - \boldsymbol{p}_{\text{Raibert}}\|^2}{2\sigma_{\text{nom}}^2}\right)\]

离 Raibert 名义点越近越好（Gaussian 衰减）。这一项使得在没有地形压力时，落脚点自然回归到 Raibert 解——不做多余的偏移。

权重选择指南

场景	\(w_{\text{terrain}}\)	\(w_{\text{kin}}\)	\(w_{\text{stability}}\)	\(w_{\text{nominal}}\)
平地快速行走	0.1	0.2	0.3	0.4
崎岖地形慢速	0.4	0.2	0.3	0.1
推恢复	0.1	0.1	0.6	0.2
阶梯攀爬	0.3	0.3	0.3	0.1

候选点生成与评估

评分函数需要候选点来评估。两种主要策略：

策略 1：网格采样——以 Raibert 名义点为中心，在圆盘内均匀采样。

策略 2：学习生成——用 CNN 从局部高程图直接预测最优落脚点。代表工作：Magana 等人（2019, Sensors）用卷积神经网络学习 ANYmal 的落脚点质量评分函数，输入 \(0.6 \times 0.6\) m 的局部高程图 patch，输出该区域的可踩性得分。

TAMOLS 的联合优化

ETH 的 TAMOLS（Jenelten 2022, IEEE T-RO）将落脚点选择与运动优化统一为一个优化问题：

• 决策变量：基座姿态轨迹 + 落脚点位置
• 代价：运动平滑性 + 地形适配度
• 约束：运动学可达 + 动力学可行 + 地形法向量约束
• 求解：graduated optimization（从粗到细的多分辨率优化），避免因地形复杂导致的局部最优

TAMOLS 的核心创新是**联合优化**——不是"先选点再规划运动"的两阶段方法，而是同时优化落脚位置和身体运动，因此能找到更好的全局方案。

⚠️ 常见陷阱

⚠️ 编程陷阱：评分函数各分项的量级不统一 - 错误做法：\(S_{\text{terrain}} \in [0, 1]\) 但 \(S_{\text{kin}}\) 直接用关节角度弧度值 \(\in [0, 3.14]\) - 现象：运动学分项完全主导了总评分，地形项被忽略 - 根本原因：未归一化。不同量纲的量直接相加毫无意义 - 正确做法：每个分项先归一化到 \([0, 1]\)，然后用权重调节

💡 概念误区：认为"评分函数的权重可以一劳永逸地固定" - 新手想法："调好权重就不用再动了" - 实际上：不同地形、不同速度、不同任务需要不同的权重。平地上 \(w_{\text{nominal}}\) 应该大（保持名义步态），崎岖地形上 \(w_{\text{terrain}}\) 应该大（优先地形安全） - 工程实践：ANYmal 的控制器根据估计的地形类型（平地/粗糙/阶梯）自动切换权重

练习

1. [设计题] 为四足机器人在 stepping stones 场景（离散可踩区域）设计评分函数。除了本节的四个分项，还需要哪些额外分项？（提示：可踩区域大小、到区域中心的距离、该区域是否已被其他脚占用）
2. [编程题] 实现一个简单的落脚点选择器：输入 Raibert 名义点和 2D 高程图，输出最优落脚点。用 20x20 网格采样，评估每个候选点的地形质量和名义偏移。
3. [分析题] TAMOLS 的"联合优化"相比"先选点再规划"有什么优势？什么情况下两者结果一致？（提示：当 Raibert 名义点恰好是地形最优时，两者等价。）

---

58.8 反应式落脚调整 ⭐⭐⭐

这一节解决什么问题：摆动相已经开始了，但在执行过程中发生了意外（被推、地形误判、步态延迟）——如何在摆动中途修改落脚点以恢复平衡？

动机：规划不可能完美

无论预先的落脚规划有多精确，实际执行中总会遇到计划外的情况：

意外类型	发生时机	后果
外部推力	摆动相中	基座速度突变，原规划落脚点不再最优
地形误判	脚接近地面时	原落脚点实际上是坑或滑面
步态延迟	着陆延迟	实际着陆时刻与规划不符
状态估计漂移	持续累积	髋关节位置估计有偏差

如果坚持使用摆动相开始时计算的落脚点，就像开车时只在路口看一次地图就闭眼开到下一个路口——显然不安全。

如果不做反应式调整会怎样

考虑一个四足 trot 中的场景：

1. 摆动相开始时，Raibert 算出落脚点 \(p_0 = (0.15, 0, 0)\)（前方 15 cm）
2. 摆动相进行到一半（~0.1 s），机器人被推了一下，速度增加了 0.5 m/s
3. 如果不调整，脚仍然落在 \(p_0\)，但此时 CP 已经移动到 \((0.24, 0, 0)\)
4. 落脚点距离 CP 约 0.09 m——发散模态被激发，\(C_1 = 0.09/2 = 0.045\) m
5. 发散模态以 \(e^{\omega t}\)（时间常数 ~0.175 s）增长
6. 如果下一步来不及补偿 → 可能摔倒

反应式调整：在被推的瞬间，重新计算 Raibert/CP，把落脚点从 \(p_0\) 更新为 \(p_1 = (0.23, 0, 0)\)，然后修改摆动轨迹使脚转向新目标。

调整窗口与时序

摆动相时间线（典型 T_swing = 0.2-0.3 s）:
0 ──────── 0.4*T ──────── 0.7*T ──────── T
│           │               │              │
│  上升段    │   顶点/平飞    │   下降段     │
│           │               │              │
│ 调整空间大 │  调整空间中    │  调整空间小  │
│ → 效果好  │  → 效果一般   │  → 可能来不及│

调整能力受限于： - 脚的当前速度和加速度限制：中途改变方向需要额外的关节力矩 - 剩余摆动时间：时间越少，能改变的距离越小 - 轨迹平滑性要求：突然改变轨迹会导致高频振动

反应式调整的实现方法

方法 1：持续重规划（Continuous Replanning）

每个控制周期都用最新状态重新运行 Raibert/CP 公式，更新落脚目标：

void updateFootholdDuringSwing(
    SwingTrajectory& traj,
    const RobotState& state,
    double t_current,
    double t_swing_end) {

    // 1. 用最新状态重算 Raibert 落脚点
    Eigen::Vector3d p_new = computeRaibertFoothold(
        state.hip_pos, state.base_vel,
        state.base_vel_ref, state.base_pos,
        state.yaw_rate_cmd, params_);

    // 2. 检查新落脚点的可行性
    if (!isFootholdFeasible(p_new, state.hip_pos, terrain_)) {
        p_new = findNearestFeasible(p_new, terrain_);
    }

    // 3. 限制调整速率（每 ms 最多移动 0.5 mm）
    Eigen::Vector3d p_old = traj.getTarget();
    double max_rate = 0.0005;  // m per ms
    Eigen::Vector3d delta = p_new - p_old;
    if (delta.norm() > max_rate) {
        p_new = p_old + max_rate * delta.normalized();
    }

    // 4. 更新摆动轨迹（保持当前位置/速度连续）
    traj.updateTarget(p_new, t_current, t_swing_end);
}

方法 2：DCM 误差驱动的增量修正

不完全替换落脚点，而是在原落脚点基础上加修正量：

\[\boldsymbol{p}_{\text{adjusted}} = \boldsymbol{p}_{\text{original}} + k_{\text{react}} \cdot (\boldsymbol{\xi}_{\text{current}} - \boldsymbol{\xi}_{\text{planned}})\]

其中 \(k_{\text{react}} \in [0, 1]\) 是反应增益，\(\boldsymbol{\xi}_{\text{current}}\) 是当前实际 DCM，\(\boldsymbol{\xi}_{\text{planned}}\) 是规划时预期的 DCM。

优点：修正量平滑（与 DCM 误差成正比），不会因噪声导致落脚点跳动。

摆动轨迹的在线修改

落脚点变了，摆动轨迹也要跟着变。关键是保证**位置和速度连续性**——脚不能瞬间跳到新位置。

保持连续性的方法：以当前脚的位置和速度为新的"初始条件"，以更新后的落脚点为新的"终点"，重新生成从当前时刻到着陆时刻的轨迹（Cubic Spline 或 5 次多项式）。

⚠️ 常见陷阱

⚠️ 编程陷阱：反应式调整没有限制调整速率 - 错误做法：每个控制周期都完全替换落脚点为最新的 Raibert 计算结果 - 现象：在有噪声的状态估计下，落脚点每毫秒跳动 1-5 mm，摆动脚高频"抖动" - 根本原因：状态估计的噪声（特别是速度估计）通过 Raibert 公式直接映射为落脚点噪声 - 正确做法：(1) 对基座速度估计做低通滤波（截止频率 ~20 Hz）再输入 Raibert；(2) 限制落脚点的最大调整速率（如 0.5 mm/ms） - OCS2 做法：SwingTrajectoryPlanner 中有 clamp 操作限制单次调整幅度

💡 概念误区：认为"反应式调整越频繁越好" - 新手想法："每 1 ms 都重算最新最优落脚点，这样最安全" - 实际上：过于频繁的调整会导致摆动轨迹的高频振荡，增加关节力矩需求（\(\tau \propto \ddddot{q}\)），甚至引发伺服电机的振荡。合理的重算频率是 50-200 Hz（每 5-20 ms 更新一次），远低于控制器的 1 kHz - 工程智慧：MIT Cheetah 的 FootSwingTrajectory 也有类似的限幅和平滑处理

练习

1. [设计题] 设计一个"反应式调整触发器"：定义何时需要调整（基于 DCM 误差阈值）、调整幅度（增益）、以及何时"放弃调整"（摆动相末期调不动了就不调）。
2. [仿真题] 在 Raibert + LIPM 仿真器中，在摆动相中途施加一个 0.5 m/s 的脉冲扰动。对比"不调整"和"反应式调整"两种策略下的速度恢复曲线和摔倒率。
3. [思考题] 反应式落脚调整和 MPC 的关系是什么？MPC 每次求解都在"调整"——它和本节的反应式调整有什么本质区别？（提示：MPC 调整的是**接触力**，反应式调整的是**落脚位置**。两者在不同层面工作，可以协同。）

---

58.9 MPC 落脚点规划：把落脚位置变成优化变量 ⭐⭐⭐

这一节解决什么问题：前面 58.2-58.8 节的方法（Raibert / Capture Point / 评分函数 / 反应式调整）有一个共同的隐含假设——落脚点是先算出来、再喂给下游 MPC 的一个外部输入。本节回答一个更深的问题：能不能把"脚踩哪里"直接当作 MPC 的优化变量，让落脚位置和接触力在同一个优化里被联合求解？这正是 2018 年以来工业级四足控制器（MIT Cheetah 3、ANYmal）的主流做法。

动机：把落脚点"外包"给 Raibert 的代价

回顾一下到目前为止的数据流（58.1 节的架构图）：步态管理给出接触序列 → Raibert/CP 算出落脚点 → 摆动轨迹生成 → MPC 算接触力 → WBC 算力矩。在这条流水线里，落脚点规划和接触力规划是**两个独立的阶段**，中间用一个 \(\boldsymbol{p}_{\text{foot}}^{\text{target}}\) 单向连接。

这种"先选点、再算力"的两阶段解耦有一个根本的盲点：落脚点和接触力其实是强耦合的。同一个期望加速度，既可以靠"把脚踩远一点"实现（改变力臂），也可以靠"加大接触力"实现（改变力的大小）。Raibert 公式在第一阶段就把落脚点钉死了，第二阶段的 MPC 只能在给定落脚点的前提下分配力——它没有机会说"如果脚再往前 3 cm，我用更小的力就能达到同样的减速"。

💡 概念澄清：这里要区分两个"MPC"。接触力 MPC（足式/110_OCS2完整栈与双线程MPC 的主体，Di Carlo 2018 的凸 MPC）以接触力为决策变量，落脚点是它的输入参数；落脚点 MPC（本节主题）把落脚位置也提升为决策变量。在 OCS2 的完整非线性 MPC 里，两者其实是**同一个优化问题的不同变量块**——这正是本节要讲透的统一视角。

如果继续用两阶段解耦会怎样

设想 Go2 以 \(1.5\) m/s 高速 trot，操作员突然要求急停（\(v_{\text{ref}}=0\)）。

• 两阶段做法：Raibert/CP 在摆动相开始时算出一个落脚点 \(\boldsymbol{p}_0\)，这个点只用到了**当前一步**的信息。它不知道下一步、下下步的接触力能不能跟上。如果 \(\boldsymbol{p}_0\) 让当前步勉强稳住，但导致下一步的支撑相力分配超出摩擦锥，机器人会在第二步打滑。两阶段架构里，第一阶段的落脚点规划器**根本看不到**第二阶段才会暴露的摩擦锥约束。
• 联合做法：把未来 \(N\) 步的落脚点 \(\{\boldsymbol{p}_1,\dots,\boldsymbol{p}_N\}\) 和每一步的接触力 \(\{\boldsymbol{f}_1,\dots,\boldsymbol{f}_N\}\) 一起放进 MPC，约束里同时写上"每步摩擦锥"和"运动学可达"。优化器会自动协调：第一步踩在哪、第二步踩在哪，才能让整条轨迹的力都落在摩擦锥内。

这就是从"贪心一步"到"前瞻 N 步"的本质跃迁。我们在 58.4 节的 N-step capturability 里已经见过这个思想的**可行性分析**版本（能不能 N 步停下来）；本节是它的**最优控制**版本（N 步内怎么走最优）。

本质洞察：两阶段解耦不是因为它更优，而是因为它更**快**——Raibert 是 \(O(1)\) 闭式公式，能跑 1 kHz；联合优化要解一个 QP/NLP，只能跑 50-500 Hz。工业界长期用两阶段，本质是"实时性"对"最优性"的妥协。MPC 落脚点规划的全部技术努力，都是为了把这个妥协的代价压到最低——既要联合优化的最优性，又要尽量逼近实时性。

把落脚点写进 MPC：决策变量与约束

我们以一个**简化的凸 MPC**（基于 LIPM / SRBM，足式/110_OCS2完整栈与双线程MPC 已建模）为载体来展示落脚点如何进入优化。

Step 1：决策变量的扩充

标准接触力 MPC 的决策变量是状态轨迹 \(\{\boldsymbol{x}_k\}\) 和接触力 \(\{\boldsymbol{f}_k\}\)。落脚点 MPC 额外引入摆动脚的落地位置 \(\{\boldsymbol{p}_k^{\text{foot}}\}\)：

\[\boldsymbol{z} = \big[\,\underbrace{\boldsymbol{x}_0,\dots,\boldsymbol{x}_N}_{\text{状态}}\,,\ \underbrace{\boldsymbol{f}_0,\dots,\boldsymbol{f}_{N-1}}_{\text{接触力}}\,,\ \underbrace{\boldsymbol{p}_1^{\text{foot}},\dots,\boldsymbol{p}_M^{\text{foot}}}_{\text{落脚点}}\,\big]\]

其中 \(M\) 是 horizon 内发生的落地事件数（由 ModeSchedule 决定，足式/120_步态管理与接触序列）。

Step 2：落脚点如何耦合进动力学

在单刚体模型（SRBM）里，接触力对基座的力矩取决于"接触点相对质心的位置向量"\(\boldsymbol{r}_k = \boldsymbol{p}_k^{\text{foot}} - \boldsymbol{p}_k^{\text{com}}\)：

\[\dot{\boldsymbol{L}} = \sum_{i \in \text{contact}} \boldsymbol{r}_k^{(i)} \times \boldsymbol{f}_k^{(i)}\]

这里 \(\boldsymbol{L}\) 是关于质心的角动量。关键：\(\boldsymbol{r}_k\) 里含有落脚点 \(\boldsymbol{p}_k^{\text{foot}}\)，而它又和 \(\boldsymbol{f}_k\) 相乘——这是一个**双线性项**（bilinear term）。这就是落脚点联合优化的核心困难：力矩 = 力臂 × 力，两个都是决策变量时，问题变成**非凸**。

💡 这看起来和接触力 MPC 很像，但关键区别在于：在标准凸 MPC 里，落脚点 \(\boldsymbol{p}_k^{\text{foot}}\) 是**已知常数**（Raibert 提前给的），所以 \(\boldsymbol{r}_k \times \boldsymbol{f}_k\) 是"常数 × 变量"=线性，问题是凸 QP。一旦把落脚点也变成变量，就出现"变量 × 变量"，凸性被破坏。这一个区别，决定了整个求解策略的不同。

Step 3：处理双线性的两条路线

路线	做法	代表工作	代价
凸化 / 解耦	固定一个 horizon 内的落脚点，在外层用启发式（Raibert）更新，内层保持凸 QP	Di Carlo 2018（MIT Cheetah 3）	损失联合最优性，但保住 1 kHz 凸 QP
完整非线性	直接解非凸 NLP（SQP/DDP），落脚点和力同时优化	Grandia 2023（ANYmal）	最优性好，但单次求解 ms 级

Di Carlo 2018 的工程智慧是：承认双线性的非凸，然后绕过它——落脚点仍由 Raibert 在 MPC 之外提供，MPC 只优化接触力（凸 QP）。这保证了 MPC 是凸的、可在毫秒内求解。落脚点的"次优"由高频反应式调整（58.8 节）和 Raibert 的鲁棒性来弥补。

Grandia 2023（ANYmal 的 perceptive NMPC，IEEE T-RO）则正面迎战非凸：把落脚点、接触力、基座轨迹全部塞进一个非线性 MPC，用 SQP（Sequential Quadratic Programming）在线求解，并把高程图地形约束写成可微的不等式约束。它能让 ANYmal 在没有预设落脚点的情况下完成动态攀爬——这是两阶段方法做不到的。

Step 4：地形约束如何进入 MPC

落脚点 MPC 相比 Raibert 的最大优势，是能把**地形约束直接写进优化**，而不是事后裁剪。对每个落脚点 \(\boldsymbol{p}_k^{\text{foot}}\)：

\[\underbrace{\boldsymbol{p}_k^{\text{foot}} \in \mathcal{W}_{\text{kin}}(\boldsymbol{x}_k)}_{\text{运动学可达(58.6)}},\qquad \underbrace{p_{k,z}^{\text{foot}} = h_{\text{map}}(p_{k,x}^{\text{foot}}, p_{k,y}^{\text{foot}})}_{\text{贴地约束}},\qquad \underbrace{\hat{\boldsymbol{n}}_k \cdot \hat{\boldsymbol{z}} \geq \cos\theta_{\max}}_{\text{坡度约束}}\]

其中贴地约束 \(p_z = h_{\text{map}}(p_x, p_y)\) 是非线性的（高程图是任意曲面）。Grandia 2023 的处理是把高程图局部线性化为"可踩平面"，使约束在每次 SQP 迭代内是线性的——这又是一个"非凸问题、局部凸化求解"的典型范式。

Footstep over terrain：多步规划的地形语义

当地形从"平地"变成"崎岖/离散"时，落脚点 MPC 还要回答一个 Raibert 完全无法处理的问题：这一步踩的地方，会不会让下一步无路可走？

考虑一段带缝隙的地形。Raibert 只看当前速度算出一个名义点；如果那个点恰好落在缝隙里，58.6 节的做法是"裁剪到最近可行点"。但裁剪是**局部**的——它不知道裁剪后的落脚点会不会导致下一步的名义点又落进另一道缝。多步 MPC 把未来 \(N\) 步的落脚点一起优化，地形约束 \(\boldsymbol{p}_k^{\text{foot}} \in \mathcal{T}_{\text{safe}}\) 对每一步都成立，于是优化器会自动找出"这一步稍微保守、换取下一步有好落点"的全局协调解。

单步裁剪 (Raibert + 58.6):           多步 MPC (本节):
  步1: 名义点落缝里 → 裁剪到边缘        步1,2,3 一起优化
  步2: 基于步1边缘点重算 → 又落缝里      约束: 每步都 ∈ T_safe
  步3: ...局部挣扎,可能无解              结果: 步1主动后撤,让步2,3落在石块中心
       ↑ 贪心,看不到下游                      ↑ 前瞻,全局协调

本质洞察：Footstep over terrain 的难点不在"单步选点"（评分函数 58.7 就能做），而在"多步之间的地形耦合"——当前步的落点改变了下一步髋关节的位置，从而改变了下一步的可达地形区域。这种耦合是**链式**的，只有把多步放进同一个优化才能正确处理。这也解释了为什么 stepping stones、楼梯、parkour 这类地形是 MPC/MIQP 方法的主战场，而平地上 Raibert 就够了。

当"可踩区域"进一步退化为**离散的凸块**（stepping stones、楼梯踏面），落脚点到哪个块的分配就变成**整数变量**——多步 MPC 随之升级为混合整数 MPC。这正是下一节（58.10）MIQP 的切入点。所以本节和下一节是连续的：MPC 落脚点规划处理连续地形上的多步优化，MIQP 处理离散地形上的组合选择。

一个具体例子：上楼梯。设每级踏面深 0.25 m、高 0.15 m，Go2 名义步幅约 0.16 m。考虑机器人正面上一段楼梯：

                         ┌──────  踏面3 (z=0.45)
                  ┌──────┤
           ┌──────┤ 踏面2 (z=0.30)
   ────────┤ 踏面1 (z=0.15)
   地面(z=0)

• 单步 Raibert + 裁剪（58.6）的困境：Raibert 按当前速度给出名义点（水平方向约 0.16 m），但它**不知道踏面边缘在哪**。若名义点恰好落在踏面 1 与踏面 2 的台阶边缘附近，58.6 的裁剪只会把它推到"最近的可踩格子"，可能把脚顶在台阶竖直面上或踩在踏面极边缘（\(d_{\text{edge}}\) 极小，58.6 的边缘约束濒临违反）。更糟的是，单步规划看不到：这一步踩在踏面 1 靠前位置后，下一步髋关节随之前移，使下一步名义点正好落进踏面 2 的边缘——误差沿楼梯逐级累积。
• 多步 MPC 的处理：把未来 3 步落脚点 \(\{\boldsymbol{p}_1,\boldsymbol{p}_2,\boldsymbol{p}_3\}\) 一起优化，对每步施加"落在某踏面中心带内"的地形约束。优化器会自动调节**步幅相位**——即让步幅与踏面深度同步（每步恰好跨一级或两级踏面中心），而不是机械地维持 0.16 m 名义步幅。这种"步幅与地形周期对齐"的解，单步方法永远找不到，因为它没有未来踏面的信息。

💡 类比（标注边界）：多步落脚点 MPC 在楼梯上做的事，像**驾驶时根据前方一连串红绿灯的相位调整车速（绿波带）——不是盯着眼前这一个灯，而是让速度与灯的节奏同步。**相似点：都在用"未来一串离散事件的时空分布"反推当前的连续控制量（车速 / 步幅）。不同点：绿波带的灯相位是外界给定且不受车影响的，而楼梯落脚里"下一步的可达踏面"会被"当前步踩在哪"改变（髋关节随之移动）——存在车流没有的**状态反馈耦合**。所以不要把这个类比延伸到"楼梯落脚也可以离线一次性规划好"——它必须在线滚动重规划。

🧠 思维陷阱：认为"楼梯/离散地形必须用 MIQP，连续 MPC 处理不了" - 新手想法："踏面是离散的，所以一定是整数变量问题，连续 MPC 无能为力" - 实际上：只要每步**预先指定**踩哪一级踏面（由步态/高层给出接触序列），落脚点在该踏面内的位置仍是连续优化，连续 MPC 完全够用。只有当"踩哪一级"本身也要优化器决定时，才升级为 MIQP（58.10） - 正确思维：离散地形 ≠ 必须整数变量；**先固定离散分配、再连续优化**是工程中最常用的降复杂度手段（也正是 58.10 分层架构的思想）

简化实现：单步落脚点 MPC（一维 LIPM 演示）

为了让"落脚点作为优化变量"这件事可触摸，下面给一个最小的一维 LIPM 落脚点 MPC 框架。它优化未来 \(N\) 步的落脚点，使速度跟踪参考且落脚点平滑。

#include <Eigen/Dense>
#include <vector>

// 一维 LIPM 多步落脚点 MPC（演示用，凸 QP）
// 决策变量: 未来 N 步的落脚点 p_1...p_N
// 状态: 每步开始时的 (x, xdot)，通过 LIPM 解析映射递推
struct LipmFootstepMpc {
    double omega;       // sqrt(g/h)
    double Ts;          // 单步支撑时长 [s]
    int    N;           // horizon 步数
    double w_track;     // 速度跟踪权重
    double w_smooth;    // 落脚点平滑权重
    double r_max;       // 可达半径（约束）

    // LIPM 一步前向映射: 给定步初 (x, v) 和落脚点 p，返回步末 (x', v')
    // 复用 58.2 / 58.4 的解析解，omega 已知
    void stepForward(double x, double v, double p,
                     double& x_next, double& v_next) const {
        const double ch = std::cosh(omega * Ts);
        const double sh = std::sinh(omega * Ts);
        x_next = (x - p) * ch + (v / omega) * sh + p;
        v_next = (x - p) * omega * sh + v * ch;
    }

    // 求解：返回未来 N 步落脚点。这里用最简单的投影梯度演示思路，
    // 工程中应调用 OSQP/qpOASES 等 QP 求解器。
    std::vector<double> solve(double x0, double v0, double v_ref,
                              const std::vector<double>& p_hip) const {
        std::vector<double> p(N, x0);          // 初值
        for (int iter = 0; iter < 50; ++iter) {
            double x = x0, v = v0;
            for (int k = 0; k < N; ++k) {
                double xn, vn;
                stepForward(x, v, p[k], xn, vn);
                // 速度跟踪误差驱动落脚点（dead-beat 风格更新，见 58.2 推导）
                double e = vn - v_ref;
                p[k] += w_track * e / (omega * std::sinh(omega * Ts));
                // 平滑项：向相邻落脚点靠拢
                if (k > 0)   p[k] -= w_smooth * (p[k] - p[k - 1]);
                // 可达性投影（58.6 的硬约束）
                p[k] = std::clamp(p[k], p_hip[k] - r_max, p_hip[k] + r_max);
                x = xn; v = vn;
            }
        }
        return p;
    }
};

// ❌ 错误写法 1：把落脚点当常数，只优化力，却声称"联合优化"
//    —— 这其实是 Di Carlo 2018 的两阶段，不是落脚点 MPC
std::vector<double> p_fixed = raibert_all_steps();   // 落脚点先定死
auto f = solveConvexMpc(p_fixed);                    // 只解力
// 问题：这是合法的工程方案，但不要误称它"联合优化了落脚点"

// ❌ 错误写法 2：在凸 QP 里直接写 r × f 双线性项
// cost += (p_foot - p_com).cross(f);   // r 和 f 都是变量 → 非凸！
// 问题：QP 求解器要么报 "not convex"，要么给出错误解。
//      必须凸化（固定其一）或换非线性求解器（SQP/DDP）

// ❌ 错误写法 3：地形贴地约束写成等式硬约束塞进凸 QP
// constraint: p_z == heightMap(p_x, p_y);   // 高程图非线性 → 破坏凸性
// 正确：在每次 SQP 迭代内把高程图局部线性化为平面约束（Grandia 2023 做法）

一个可手算的对比：单步 Raibert vs 两步 MPC 的急停

为了让"多步前瞻"的收益从抽象变具体，我们用 Go2 的 LIPM 参数手算一个急停场景，并暴露单步方法的盲点。

设定：\(h = 0.3\) m，\(\omega = \sqrt{g/h} = \sqrt{9.81/0.3} \approx 5.72\) rad/s，单步 \(T_s = 0.25\) s（则 \(\omega T_s \approx 1.43\)，\(\cosh(\omega T_s) \approx 2.21\)，\(\sinh(\omega T_s) \approx 1.97\)）。可达半径 \(r_{\max} = 0.20\) m。初始：质心在 \(x_0 = 0\)，速度 \(\dot{x}_0 = 1.5\) m/s，目标 \(v_{\text{ref}} = 0\)（急停）。

单步方案（Capture Point / Raibert 极限）：一步停下要求踩在 Capture Point 上：

\[p^* = x_0 + \frac{\dot{x}_0}{\omega} = 0 + \frac{1.5}{5.72} \approx 0.262 \text{ m}\]

但 \(0.262 > r_{\max} = 0.20\)——Capture Point 不可达！单步方法此时只能把落脚点裁剪到 \(p = 0.20\) m（可达边界）。裁剪后发散模态系数不为零：

\[C_1 = \tfrac{1}{2}\big[(x_0 - p) + \tfrac{\dot{x}_0}{\omega}\big] = \tfrac{1}{2}\big[(0 - 0.20) + 0.262\big] \approx 0.031 \text{ m}\]

\(C_1 \neq 0\) 意味着**残余发散**——这一步停不下来，速度还会增长。用前向映射算步末速度：

\[\dot{x}_1 = (x_0 - p)\,\omega\sinh(\omega T_s) + \dot{x}_0\cosh(\omega T_s) = (-0.20)(5.72)(1.97) + (1.5)(2.21) \approx -2.25 + 3.32 = 1.07 \text{ m/s}\]

单步裁剪后，速度从 1.5 只降到 1.07 m/s，还在快速前进，且质心已前移——下一步的可达窗口随之平移，单步贪心看不到这一点。

两步 MPC 方案：把两步落脚点 \(\{p_1, p_2\}\) 一起优化，约束 \(|p_k - x_{k-1}| \le r_{\max}\)，终态约束 \(\dot{x}_2 = 0\)（两步停下）。两步的发散模态总消除条件给出一组可行解，例如 \(p_1 = 0.20\) m（第一步用满可达，最大化减速），第二步在新质心 \(x_1\) 附近落在其 Capture Point 上：

\[x_1 = (x_0 - p_1)\cosh(\omega T_s) + \tfrac{\dot{x}_0}{\omega}\sinh(\omega T_s) + p_1 \approx (-0.20)(2.21) + (0.262)(1.97) + 0.20 \approx 0.275 \text{ m}$$ $$p_2 = x_1 + \frac{\dot{x}_1}{\omega} = 0.275 + \frac{1.07}{5.72} \approx 0.462 \text{ m},\quad p_2 - x_1 \approx 0.187 < r_{\max}\ \checkmark\]

第二步的 Capture Point 偏移仅 0.187 m，可达！于是两步内 \(\dot{x}_2 \to 0\)，机器人停住。

本质洞察：单步方法在 \(t=0\) 就被 \(r_{\max}\) 卡死、停不下来；而两步 MPC 看到"第一步先尽力减速、把速度降到 1.07 m/s，第二步的 Capture Point 自然落回可达范围"。多步前瞻的价值不是在每一步都更优，而是把一个单步不可行的问题，拆成两步都可行的子问题——这正是 N-step capturability（58.4）从"可行性分析"到"最优规划"的落地。注意第二步 Capture Point 0.187 m 之所以可达，靠的是第一步主动减速带来的速度下降；贪心方法无法发现这种跨步的因果链。

MPC 落脚点规划 vs 经典方法的定位

维度	Raibert/CP（58.2-58.4）	评分函数（58.7）	MPC 落脚点（本节）	MIQP（58.10）
落脚点是	闭式公式输出	候选集打分选最优	优化变量	整数+连续变量
前瞻	1 步	1 步	N 步	N 步
地形	事后裁剪	显式评分	约束进优化	离散区域约束
与力耦合	解耦	解耦	可联合	通常解耦
频率	1 kHz	50-200 Hz	50-500 Hz	1-10 Hz
凸性	不涉及	不涉及	非凸(双线性)→局部凸化	凸松弛+分支

💡 概念误区：认为"MPC 落脚点规划一定比 Raibert 好，应该全面替代" - 新手想法："既然能联合优化，为什么还用 Raibert？" - 实际上：在平地稳态行走时，Raibert 给出的落脚点已经几乎最优（58.3 节证明了它在 \(k_p=1/\omega\) 时退化为 Capture Point），此时联合优化的收益微乎其微，却要付出 50-500 Hz vs 1 kHz 的实时性代价。MPC 落脚点规划的价值只在**地形复杂 + 大扰动 + 需要多步协调**时才显现 - 正确思维：MPC 落脚点规划不是 Raibert 的替代品，而是它的"重型补充"——平地用 Raibert，难地形切到 MPC

⚠️ 常见陷阱

⚠️ 编程陷阱：在凸 MPC 里同时把落脚点和接触力当变量，期望它仍是凸的 - 错误做法：在 OSQP/qpOASES 的代价或约束里写 \((\boldsymbol{p}_{\text{foot}}-\boldsymbol{p}_{\text{com}}) \times \boldsymbol{f}\)，两者都是决策变量 - 现象：求解器报 "problem is not convex / not PSD"，或返回明显违反物理的解（力臂为负、力穿透地面） - 根本原因：力矩 = 力臂 × 力，当力臂（含落脚点）和力都是变量时，乘积是双线性项，Hessian 不再半正定，QP 凸性被破坏 - 正确做法：(1) 凸化——固定落脚点（由 Raibert 提供），MPC 只优化力，保持凸 QP（Di Carlo 2018）；或 (2) 换非线性求解器——用 SQP/DDP 解非凸 NLP，落脚点和力同时优化（Grandia 2023） - 自检方法：求解前检查代价 Hessian 是否半正定（Eigen::SelfAdjointEigenSolver 看最小特征值是否 \(\geq 0\)）

💡 概念误区：把"落脚点 MPC"和"接触力 MPC"当成同一个东西 - 新手想法："MPC 不就是算力的吗？落脚点 MPC 有什么不一样？" - 实际上：接触力 MPC（足式/110_OCS2完整栈与双线程MPC 主体）的决策变量是力，落脚点是外部输入；落脚点 MPC 把落脚位置也提升为决策变量。两者的本质区别是**问题的凸性**——前者是凸 QP，后者因双线性项而非凸 - 正确理解：在 OCS2 的完整非线性 MPC 里，状态、力、落脚点是同一个 NLP 的不同变量块；在 Di Carlo 的凸 MPC 里，它们被刻意拆成两个阶段以保住凸性

🧠 思维陷阱：认为"多步前瞻总是越多步越好" - 新手想法："MPC horizon 越长，规划越优，应该尽量长" - 实际上：和 58.4 节 N-step capturability 的结论一致——前瞻收益随步数递减，而代价（求解时间、未来地形/状态估计的不确定性）随步数线性甚至超线性增长。地形信息在远处本身就不可靠（高程图边缘噪声大），规划那么远是"基于错误信息的精确优化" - 正确思维：horizon 长度应匹配**地形信息的可靠半径**和**实时性预算**，工程中四足 MPC 落脚通常 horizon 覆盖 1-3 个步态周期（约 0.5-1.5 s）

练习

1. [推导题] 写出单刚体模型（SRBM）中接触力对质心角动量的贡献 \(\dot{\boldsymbol{L}} = \sum_i (\boldsymbol{p}_i^{\text{foot}} - \boldsymbol{p}^{\text{com}}) \times \boldsymbol{f}_i\)。指出当 \(\boldsymbol{p}_i^{\text{foot}}\) 和 \(\boldsymbol{f}_i\) 都是优化变量时，哪一项导致非凸，并说明 Di Carlo 2018 如何通过"固定落脚点"恢复凸性。
2. [编程题] 扩展本节的 LipmFootstepMpc：把它接到一个开源 QP 求解器（OSQP 的 C++ 接口或 Python 的 osqp），把速度跟踪和落脚点平滑写成标准 QP 的 \(\frac{1}{2}\boldsymbol{z}^\top P \boldsymbol{z} + q^\top \boldsymbol{z}\) 形式，把可达性写成线性不等式约束。对比它和单步 Raibert 在"急停"任务上的速度恢复曲线。
3. [分析题] Grandia 2023 用 SQP 在线解非凸落脚点 NLP，能在 ANYmal 上实时运行（约 100 Hz）。但 58.9 节也说 NLP 单次求解是 ms 级。请分析：是什么工程手段让 ms 级的单次求解能撑起 100 Hz 的控制频率？（提示：warm-start、实时迭代 RTI scheme、单次 SQP 迭代而非求解到收敛、双线程架构见 足式/110_OCS2完整栈与双线程MPC。）

---

58.10 混合整数落脚规划 ⭐⭐⭐⭐

这一节解决什么问题：当地形复杂（stepping stones、缝隙、障碍物）时，落脚点的选择不是连续优化问题，而是**组合优化**——哪些区域可以踩是离散决策。混合整数规划（MIP）是处理这类问题的数学框架。

动机：连续优化无法处理的场景

考虑一个 stepping stones 场景：

        ┌──┐     ┌──┐     ┌──┐
        │S1│     │S2│     │S3│
        └──┘     └──┘     └──┘
   ───────────────────────────── 水面/裂缝

机器人必须在 S1、S2、S3 上选择落脚点。这个问题有两层决策：

1. 离散决策：每只脚踩在哪个 stone 上？（组合选择）
2. 连续决策：在选定的 stone 上，脚的精确位置在哪？

Raibert、CP、评分函数等方法只能处理连续决策。离散决策需要**组合搜索**，这正是混合整数规划的领域。

如果用贪心方法会怎样

贪心方法：每只脚独立选择最近的可行 stone。

问题： - 两只脚可能选同一个 stone → 物理碰撞 - 当前脚选了"最好"的 stone，但导致下一只脚没有好的 stone 可选 → 短视决策 - 无法处理需要"先踩远再踩近"的路径（如绕过大缝隙）

这说明离散落脚选择是一个**全局协调**问题，需要同时考虑所有脚的分配。整个过程好比下棋——你不能只看当前这一步棋的好坏，必须预判后续几步的布局；贪心地吃眼前的子，往往导致后面全盘被动。

MIQP 建模（Deits & Tedrake 2014）

Deits 和 Tedrake 在 2014 年 Humanoids 会议上提出用 MIQP（Mixed-Integer Quadratic Program）进行落脚规划。论文标题："Footstep Planning on Uneven Terrain with Mixed-Integer Convex Optimization"。

Step 1：环境表示

将安全地形分解为 \(M\) 个**凸区域**（convex regions）\(\{R_1, R_2, \ldots, R_M\}\)。每个区域用线性不等式描述：

\[R_j = \{\boldsymbol{p} \in \mathbb{R}^3 \mid A_j \boldsymbol{p} \leq b_j\}\]

凸分解可以用 IRIS（Iterative Regional Inflation by Semidefinite programming）算法自动生成。

Step 2：决策变量

• 连续变量：每步的落脚点 \(\boldsymbol{p}_i \in \mathbb{R}^3\)，\(i = 1, \ldots, N_{\text{steps}}\)
• 整数变量：分配矩阵 \(z_{ij} \in \{0, 1\}\)，表示第 \(i\) 步是否分配到区域 \(R_j\)

Step 3：约束

1. 每步只能分配到一个区域： $\(\sum_{j=1}^{M} z_{ij} = 1, \quad \forall i\)$

2. 落脚点在分配区域内（Big-M 方法）： $\(A_j \boldsymbol{p}_i \leq b_j + (1 - z_{ij}) \cdot \boldsymbol{M}_j, \quad \forall i, j\)$ （当 \(z_{ij} = 0\) 时，Big-M 值 \(\boldsymbol{M}_j\) 使约束失效；当 \(z_{ij} = 1\) 时，约束生效）

3. 运动学可达： $\(\|\boldsymbol{p}_i - \boldsymbol{p}_{\text{hip},i}\|_2 \leq r_{\max}\)$

4. 步幅约束（相邻步之间）： $\(\|\boldsymbol{p}_{i+1} - \boldsymbol{p}_i\| \leq s_{\max}\)$

5. 左右脚不交叉： $\((\boldsymbol{p}_i^{\text{left}} - \boldsymbol{p}_i^{\text{right}}) \cdot \hat{y}_{\text{body}} \geq d_{\min}\)$

Step 4：代价函数

\[\min_{\boldsymbol{p}, \boldsymbol{z}} \sum_i \|\boldsymbol{p}_i - \boldsymbol{p}_i^{\text{nominal}}\|^2 + w_{\text{smooth}} \sum_i \|\boldsymbol{p}_{i+1} - 2\boldsymbol{p}_i + \boldsymbol{p}_{i-1}\|^2\]

第一项：落脚点尽量接近名义位置（如 Raibert 给出的）。 第二项：步幅变化尽量平滑（二阶差分惩罚）。

组合复杂度分析

MIQP 的整数变量总数为 \(N_{\text{steps}} \times M\)，最坏情况需要搜索 \(M^{N_{\text{steps}}}\) 种组合。

步数 \(N\)	区域数 \(M\)	组合数	典型求解时间
4	5	625	< 1 s
8	10	\(10^8\)	10-100 s
16	20	\(\sim 10^{20}\)	不可行

**实际求解时间**远好于最坏情况，因为 Branch-and-Bound 算法的剪枝策略可以大幅减少搜索空间。但对长 horizon 仍然不适合实时。

分层求解策略

在实际系统中，MIQP 不会单独使用，而是与实时方法**分层组合**：

高层 MIQP (0.5-5 Hz):   "左前脚踩 S2，右前脚踩 S3，..."
       ↓ 区域分配 + 粗略落脚点
中层评分函数 (50 Hz):    "在 S2 区域内最优位置是 (0.23, 0.11, 0.05)"
       ↓ 精确落脚点
低层 Raibert (1 kHz):    反应式微调 + 摆动轨迹生成

Big-M 值的选择

Big-M 方法的数值稳定性取决于 M 值的选择：

M 值选择	后果
太大（如 \(10^6\)）	约束矩阵条件数极大，数值精度下降，求解时间暴增
太小（如 0.1 m）	约束可能对有效分配也生效，导致无解
合适（如 2-5 m）	覆盖物理可达范围，数值稳定

选择原则：\(M\) 应该略大于落脚点的物理活动范围（通常 \(\leq 2\) m）。

替代方法

GCS（Graph of Convex Sets）：Marcucci 等人（2023, Science Robotics）提出的方法。将凸区域视为图的节点，区域间的可达关系视为边。落脚规划变为图上的最短路径问题，可以用高效的图搜索算法（如 A*）求解，避免了 MIQP 的指数复杂度。

⚠️ 常见陷阱

⚠️ 编程陷阱：Big-M 值设得太大导致数值问题 - 错误做法：double big_M = 1e6; - 现象：求解器报告"numerical difficulties"或给出明显错误的解 - 根本原因：Big-M 过大导致约束矩阵中同时存在 \(O(1)\) 和 \(O(10^6)\) 量级的元素，浮点运算精度不足 - 正确做法：根据物理范围设置 \(M = 2\)-\(5\) m

🧠 思维陷阱：认为"有了 MIQP 就不需要 Raibert 了" - 新手想法："MIQP 是全局最优的，Raibert 是启发式的，应该用 MIQP 替代 Raibert" - 实际上：MIQP 求解需要 0.1-100 秒，完全不能实时。在平地上用 MIQP 是杀鸡用牛刀。正确的架构是分层：MIQP 提供粗略的区域分配，Raibert 在区域内做实时微调 - 工程格言："能用 O(1) 解决的问题，不要用 O(2^N)"

练习

1. [建模题] 一个双足机器人在 3 个 stepping stones 上走 4 步。写出完整的 MIQP 建模：列出所有决策变量、约束和代价函数。计算整数变量和连续变量的总数。
2. [编程题] 用 Python + 开源 MIP 求解器（如 PuLP + HiGHS，或 Google OR-Tools）求解上述 MIQP。可视化最优落脚序列。
3. [思考题] Contact-Implicit Trajectory Optimization（足式/150_优化驱动落脚与接触规划 将详述）和 MIQP 有什么区别？提示：MIQP 显式枚举接触区域，Contact-Implicit TO 用互补约束隐式处理。各自的优缺点是什么？

---

🔧 故障排查手册

落脚点规划是连接步态管理与轨迹优化的枢纽，调试时需要同时观察上下游数据流。以下是工程实践中最常见的故障场景。

症状	可能原因	排查步骤	相关章节
Raibert 落脚点随速度指令振荡发散	\(k_p\) 反馈增益过大，Poincare 映射特征值 $	\lambda	>1$
DCM 参考轨迹数值爆炸（值达 \(10^3\) 以上）	反时间递推中 \(e^{\omega T}\) 指数项过大，通常是单步支撑时间 \(T_s\) 设置过长	1. 检查 \(\omega T_s\) 是否超过 3（Go2 对应 \(T_s > 0.52\) s） 2. 缩短单步时间或增加步数 3. 打印每步 \(\xi_{\text{ini}}\) 值确认递推过程	足式/140_落脚点规划经典方法.4
落脚点系统性偏向一侧	航向修正项的叉乘方向错误或基座偏航估计有偏	1. 零速站立时打印左右脚落脚点，检查是否对称 2. 单独关闭航向修正项观察是否恢复 3. 检查 IMU 偏航角是否有静态偏差	足式/140_落脚点规划经典方法.2, 足式/130_腿足状态估计
步幅过大超出运动学限制（脚伸不到）	速度指令突变导致前馈项 \(\frac{T_s}{2}v\) 过大，或 Capture Point 偏移超出腿长	1. 在落脚点计算后加 clamp：p_foot = clamp(p_foot, p_hip - r_max, p_hip + r_max) 2. 对速度指令做低通滤波平滑突变 3. 检查 \(v/\omega\) 是否超出可达半径	足式/140_落脚点规划经典方法.3-58.4
MIQP 求解超时或返回 infeasible	候选接触区域定义不合理、Big-M 值过大导致松弛间隙、或整数变量过多	1. 减少候选区域数量（只保留可达范围内的区域） 2. 检查 Big-M 值是否紧致 3. 给 MIQP 设置时间上限并使用 warm-start	足式/140_落脚点规划经典方法.10
高速时 Capture Point 给出的步幅明显失真	LIPM 近似失效（质心高度变化大、角动量不可忽略）	1. 检查质心高度 \(h\) 假设与实际是否偏差大 2. 切换到 SRBM/预览控制等更高保真模型 3. 用 DCM 的飞轮扩展（含角动量）替代纯点质量 LIPM	足式/140_落脚点规划经典方法.3, 足式/70_腿足简化模型理论
ZMP 计算值高频抖动	力/力矩传感器噪声、或质心加速度估计噪声大	1. 对 ZMP 信号做低通滤波（截止 ~20 Hz） 2. 检查 F/T 传感器采样率与滤波器截止频率是否匹配 3. 用模型预测的加速度替代差分估计	足式/130_腿足状态估计
落脚点投影到地面后高度错误（脚悬空/插地）	高程图估计延迟、或 TF 时间戳不同步	1. 融合 IMU 高度与 elevation map 2. 检查高程图与机器人位姿的 TF 时间戳是否对齐 3. 对查询点附近做局部平面拟合而非取单格高度	足式/160_感知驱动落脚规划
MPC 落脚点优化报"非凸/Hessian 非半正定"	在凸 QP 中同时把落脚点和接触力当变量，出现力臂×力的双线性项	1. 检查代价 Hessian 最小特征值是否 \(<0\) 2. 凸化——固定落脚点只优化力（Di Carlo 2018） 3. 或换 SQP/DDP 非线性求解器（Grandia 2023）	足式/140_落脚点规划经典方法.9, 足式/110_OCS2完整栈与双线程MPC

---

58.11 本章小结

知识点总表

知识点	难度	核心思想	关键公式/方法	代表工作
问题定义	⭐	落脚规划的输入/输出/约束	\(\mathcal{F} = \mathcal{W}_{\text{kin}} \cap \mathcal{T}_{\text{safe}} \cap \mathcal{S}_{\text{stable}}\)	—
Raibert 启发式	⭐⭐	LIPM 对称摆动 → 前馈+反馈	\(p = p_{\text{hip}} + \frac{T_s}{2}v + k_p\Delta v\)	Raibert 1986
Capture Point	⭐⭐	消除发散模态 → 一步停下	\(\xi = x + \dot{x}/\omega\)	Pratt 2006
DCM 规划	⭐⭐⭐	反时间递推 DCM 参考轨迹	\(\dot{\xi} = \omega(\xi - p)\)	Englsberger 2015
N-step Capturability	⭐⭐⭐	多步可达性嵌套	\(\mathcal{C}_1 \subseteq \mathcal{C}_2 \subseteq \cdots\)	Koolen 2012
ZMP Preview Control	⭐⭐	预览未来 ZMP 参考生成 CoM	LQR + 预览项	Kajita 2003
运动学可达性	⭐⭐	关节极限 → 工作空间约束	IK 可行性检查	—
评分函数	⭐⭐	多准则加权选优	\(S = \sum w_i S_i\)	Magana 2019
反应式调整	⭐⭐⭐	摆动相中途修改落脚点	DCM 误差驱动 + 速率限制	—
MPC 落脚点规划	⭐⭐⭐	落脚位置升为优化变量，多步前瞻 + 与力联合	\(\dot{\boldsymbol{L}}=\sum_i\boldsymbol{r}_i\times\boldsymbol{f}_i\)（双线性→局部凸化）	Di Carlo 2018 / Grandia 2023
MIQP	⭐⭐⭐⭐	离散区域分配 + 连续位置优化	Branch-and-Bound	Deits 2014

方法选择决策树

落脚点规划问题
├── 地形是否平坦？
│   ├── 是 → Raibert 启发式（O(1)，1 kHz）
│   │       └── 是否需要推恢复？→ 加 Capture Point 混合项
│   └── 否 → 地形是否连续（斜坡/粗糙）？
│       ├── 是 → 评分函数选点（O(K)，100 Hz）
│       │       ├── 需要实时调整？→ 加反应式修正（58.8）
│       │       └── 需要多步协调 / 与力联合？→ MPC 落脚（58.9，50-500 Hz）
│       └── 否（离散可踩区域）→ MIQP 分层（O(2^K)，1 Hz）
│                                  └── + Raibert 实时微调（1 kHz）
├── 是否双足人形？
│   └── 是 → ZMP Preview Control + DCM 轨迹规划
└── 是否需要全局最优规划？
    └── 是 → MIQP / GCS + 分层架构

---

本章常见误解汇总

下表汇总本章各节反复出现的认知误区，供快速回顾自检。

误解	正确理解	出处
Raibert 公式是某种最优控制的结果	Raibert 源于"对称摆动"的物理直觉，其反馈项在 \(k_p \to 1/\omega\) 极限下才退化为 Capture Point；它是工程折中而非最优解	58.2-58.3
Raibert 前馈项用当前速度还是参考速度无所谓	前馈项**必须用当前实际速度** \(\boldsymbol{v}_{\text{base}}\)，因为它补偿的是实际基座运动；用参考速度会在 \(\boldsymbol{v}\neq\boldsymbol{v}_{\text{ref}}\) 时算错对称点	58.2
Capture Point 就是 ZMP 的另一种说法	ZMP 是地面反力的等效作用点（由力决定 \(p_{\text{ZMP}}=x-\ddot{x}h/g\)）；Capture Point 是为停下来脚应放的位置（由状态决定 \(\xi=x+\dot{x}/\omega\)）	58.3-58.5
踩在 Capture Point 上就永远安全	踩在 CP 上只保证不因发散模态摔倒，且需无穷长支撑相；不保证运动学可行，也不保证下一步可达——这是 N-step capturability 要解决的	58.3-58.4
N-step / MPC horizon 越长越好	前瞻收益随步数递减，而求解时间与未来信息不确定性递增；horizon 应匹配地形信息可靠半径与实时性预算	58.4, 58.9
ZMP 方法已过时，不值得学	"ZMP 在支撑多边形内"至今是现代 MPC 的核心约束之一；ZMP 是一种稳定性约束的表达，而非过时替代品	58.5
只要地面平坦就一定是好落脚点	平坦的冰面/积水/松软土同样危险；地形安全还包括摩擦系数与承载能力，几何只是第一层过滤	58.6
评分函数权重可一劳永逸固定	不同地形/速度/任务需要不同权重；先进平台按估计地形类型自动切换权重	58.7
反应式调整越频繁越好	过频调整引发摆动轨迹高频振荡、增大关节力矩；合理重算频率 50-200 Hz，远低于 1 kHz 控制频率	58.8
落脚点 MPC 与接触力 MPC 是同一个东西	接触力 MPC 以力为变量、落脚点是输入（凸 QP）；落脚点 MPC 把落脚位置也提升为变量，因双线性项而非凸	58.9
有了 MPC/MIQP 就不需要 Raibert 了	MPC/MIQP 求解慢（ms 到秒级），平地上用是杀鸡用牛刀；正确架构是分层——MPC/MIQP 给粗规划，Raibert 做 1 kHz 实时微调	58.9-58.10

---

API 速查表

本章涉及的核心数据结构与函数签名汇总（基于正文 C++ 示例，便于实现累积项目时查阅）。

名称	签名 / 字段	作用	出处
RaibertParams	{ double stance_time; double kp_feedback; }	Raibert 落脚点的两个核心参数：支撑时长 \(T_s\)、速度反馈增益 \(k_p\)	58.2
computeRaibertFoothold	Vector3d(hip, base_vel, base_vel_ref, base_pos, yaw_rate, params)	计算完整 Raibert 落脚点（前馈+反馈+航向修正），返回 \((x,y,z{=}0)\)	58.2
FootholdFeasibility	{ bool is_reachable; bool is_terrain_safe; double kin_margin; double terrain_score; }	单个候选落脚点的可行性体检结果	58.6
checkFeasibility	FootholdFeasibility(foothold, hip_pos, terrain, kin_model)	联合检查运动学可达性 + 地形安全性，含粗检查(腿长)与精检查(IK)	58.6
HeightMap::query	double query(double x, double y)	查询高程图在 \((x,y)\) 处的高度；无效区返回 NaN	58.6
HeightMap::getNormal	Vector3d getNormal(double x, double y)	查询地面法向量，用于坡度/摩擦锥判断	58.6
KinematicModel::solveIK	bool solveIK(hip_pos, foothold, q_out)	逆运动学求解；返回是否可达，可达时输出关节角	58.6
评分函数 \(S(\boldsymbol{p})\)	\(\sum_i w_i S_i(\boldsymbol{p})\)，各分项 \(\in[0,1]\)	多准则落脚点打分（地形/可达/稳定/名义），权重 \(\sum w_i=1\)	58.7
updateFootholdDuringSwing	void(traj, state, t_current, t_swing_end)	摆动相中途重算落脚点 + 限速 + 更新摆动轨迹	58.8
LipmFootstepMpc	{ omega, Ts, N, w_track, w_smooth, r_max; } + solve(x0,v0,v_ref,p_hip)	一维 LIPM 多步落脚点 MPC 框架（演示落脚点作为优化变量）	58.9
LipmFootstepMpc::stepForward	void(x, v, p, x_next, v_next)	LIPM 单步解析前向映射，复用 58.2/58.4 闭式解	58.9

---

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

项目：从零构建四足站立/行走控制器

本章新增**落脚点规划模块**：

累积进度:
  足式/30_Pinocchio深度精读 加载 URDF + 正运动学
  足式/50_空间向量与浮动基座动力学 逆动力学 + 质心计算
  足式/70_腿足简化模型理论 LIPM 仿真器 + DCM 计算
  足式/90_WBC分层优化与TSID WBC 力分配
  足式/110_OCS2完整栈与双线程MPC OCS2 MPC 集成
  足式/120_步态管理与接触序列 步态管理 + ModeSchedule
→ 足式/140_落脚点规划经典方法 落脚点规划（本章新增）
    ├── RaibertFootholdPlanner：完整 Raibert 公式（前馈+反馈+航向）
    ├── CapturePointPlanner：融合 DCM 的混合落脚规划
    ├── FootholdScorer：多准则评分函数（地形+可达+稳定+名义）
    ├── ReactiveAdjuster：摆动相中途调整（速率限制+低通滤波）
    └── LipmFootstepMpc：多步落脚点 MPC（落脚位置作为优化变量）

本章项目任务：

1. Task A：实现 RaibertFootholdPlanner 类，包含完整的前馈+反馈+航向修正。在 Go2 仿真中用 RViz 可视化落脚点（球标记）
2. Task B：在 RaibertFootholdPlanner 中添加 Capture Point 混合模式（\(\alpha\) 参数），对比纯 Raibert 和混合 CP 在推恢复中的表现
3. Task C：实现 FootholdScorer，在有高程图的地形上选择最优落脚点。可视化评分热图
4. Task D：实现 ReactiveAdjuster，在摆动相中途施加扰动后自动调整落脚点，对比有/无调整的恢复效果
5. Task E：实现 LipmFootstepMpc（58.9），把速度跟踪与落脚点平滑写成标准 QP 接入 OSQP，在"急停"任务上对比单步 Raibert 与多步 MPC 的速度恢复曲线与最大步幅

---

58.13 延伸阅读

必读经典

文献	难度	核心贡献
Raibert M., Legged Robots That Balance, MIT Press, 1986	⭐⭐	落脚启发式的原始出处，单腿/双足/四足弹跳实验
Kajita S. et al., "Biped walking pattern generation by using preview control of ZMP", ICRA 2003	⭐⭐	ZMP Preview Control，HRP-2 实验验证
Pratt J. et al., "Capture Point: A step toward humanoid push recovery", Humanoids 2006	⭐⭐	Capture Point 定义与推恢复应用
Koolen T. et al., "Capturability-based analysis and control of legged locomotion, Part 1 & 2", IJRR 2012	⭐⭐⭐	N-step capturability 理论框架与 M2V2 实验

进阶文献

文献	难度	核心贡献
Englsberger J. et al., "Three-dimensional bipedal walking control based on DCM", IEEE T-RO 2015	⭐⭐⭐	3D DCM 轨迹规划与反时间递推
Deits R. & Tedrake R., "Footstep planning on uneven terrain with MIQP", Humanoids 2014	⭐⭐⭐⭐	混合整数落脚规划 + IRIS 凸分解
Jenelten F. et al., "Perceptive locomotion in rough terrain: online foothold optimization", RA-L 2020	⭐⭐⭐	ANYmal 在线落脚优化 + 高程图
Jenelten F. et al., "TAMOLS: Terrain-Aware Motion Optimization for Legged Systems", IEEE T-RO 2022	⭐⭐⭐⭐	地形感知的联合运动-落脚优化
Grandia R., Jenelten F., Yang S., Farshidian F., Hutter M., "Perceptive Locomotion Through Nonlinear Model-Predictive Control", IEEE T-RO 2023	⭐⭐⭐⭐	落脚点/接触力/基座轨迹联合的非线性 MPC，ANYmal 动态攀爬，落脚点作为优化变量（58.9 节核心参考）

前沿方向

文献	难度	核心贡献
Magana O. A. V. et al., "Learning the cost function for foothold selection", Sensors 2019	⭐⭐⭐	CNN 学习落脚点评分函数
Marcucci T. et al., "Motion planning around obstacles with convex optimization", Science Robotics 2023	⭐⭐⭐⭐	GCS 图搜索替代 MIQP
Di Carlo J. et al., "Dynamic locomotion in MIT Cheetah 3 through convex MPC", IROS 2018	⭐⭐	Raibert + 凸 MPC 的工业级集成
Sim-to-Real foothold RL residuals (multiple groups, 2023-2026)	⭐⭐⭐	Raibert + RL 残差策略

把落脚点写进优化器——从多步 MPC 到 MIQP（呼应 58.9–58.10） ⭐⭐⭐

58.9 节已系统展开"把落脚位置提升为优化变量"的多步 MPC 框架（决策变量、双线性非凸、地形约束、单步 vs 两步急停的手算对比），58.10 节进一步处理离散地形上的整数分配（MIQP）。这里从延伸阅读的视角补一句贯穿性的本质差异，作为 58.9–58.10 的收束：

单步方法 vs 多步方法的本质差异：Raibert/CP 是**贪心策略**——每步只做局部最优决策；多步 MPC/MIQP 是**前瞻策略**——牺牲当前步的局部最优性，换取整段轨迹的全局协调。这与 58.4 节 N-step capturability 从"可行性分析"到"最优规划"的升级一脉相承。一个便于记忆的类比：Raibert 看一步，Capture Point 看两步，多步 MPC/MIQP 看 N 步——看得越远，越能避开"当前步安全、下一步无解"的死胡同，代价是求解从 \(O(1)\) 闭式公式升级为在线 QP/MIQP。这条主线的代表工作（Di Carlo 2018、Grandia 2023、Deits & Tedrake 2014）见下方"进阶文献"。

学习型落脚选择——CNN/Transformer 地形评估 ⭐⭐⭐

2019 年以来,基于深度学习的落脚点选择逐步从学术原型走向工程应用。核心思路是用神经网络替代手工设计的评分函数(58.7 节),直接从地形数据学习"哪里适合踩"。

CNN 方法 (Magana et al. 2019, Sensors):将高程图裁剪为以候选落脚点为中心的局部窗口(如 20x20 cm),输入 CNN,输出该点的"可踩性"评分。训练数据来自仿真中的成功/失败落脚经验。

Transformer 方法 (2024-2025 前沿):Transformer 的自注意力机制天然适合处理"多个候选点之间的相对关系"。将 \(K\) 个候选落脚点编码为 token,用自注意力建模候选点之间的运动学耦合(如:左前脚踩得远,右前脚就不能也踩得远),输出每个候选点的评分和最优选择。

方法	输入	输出	推理时间	优势	局限
手工评分函数 (58.7)	地形特征 + 几何参数	加权评分	< 0.1 ms	可解释、可调试	泛化差,需人工设计
CNN 评分	局部高程图 patch	可踩性概率	1-5 ms (GPU)	自动学习地形特征	需大量标注数据
Transformer 评分	候选点集 + 全局上下文	最优选择	5-20 ms (GPU)	建模候选点间关系	计算量大,部署难
端到端 RL (足式/190_腿足RL训练栈)	本体感知 + 高程图	关节角度	< 1 ms	无需显式落脚规划	黑盒,难调试

本质洞察: 落脚点规划方法的演进本质上是**"领域知识"向"数据驱动"的渐进迁移**。Raibert 公式是 100% 领域知识(手推公式);评分函数是 80% 领域知识 + 20% 数据(特征手工设计,权重可调);CNN 是 30% 领域知识 + 70% 数据(网络结构有感受野先验,但特征自动学习);端到端 RL 是接近 0% 显式领域知识 + 100% 数据(但奖励函数中仍然编码了大量隐式知识)。目前最成功的方法不在两个极端,而在中间——用领域知识定义问题结构(如 Raibert + RL 残差),用数据学习难以手工建模的部分(如地形适应)。这种"混合范式"在足式控制的各个子问题上都是最佳实践。

开放研究问题

1. 学习 Raibert 增益：不同地形/速度/步态下的最优 \(k_p\) 是否可以用强化学习自动调节？初步工作表明 RL 残差策略可以在 sim-to-real 中提升崎岖地形的成功率
2. 落脚点-力联合优化：当前方法先选落脚点再做力分配（足式/90_WBC分层优化与TSID WBC），是否可以联合优化？TAMOLS 做了初步尝试但受限于求解速度
3. 语义落脚点：利用视觉语义信息（"这是泥地"、"这是冰面"、"这是铁板"）辅助落脚决策，超越纯几何的高程图分析
4. 多机器人协调落脚：两个四足机器人协作搬运物体时，如何协调各自的步态和落脚点以保持共同稳定性？
5. Model Predictive Footstep Planning 的实时化：Deits 2014 的 MIQP 在人形上需要数秒求解。能否用学习型 warm-start 或 GCS 凸松弛将求解时间降到百毫秒级?

---

研究实践建议

针对不同阶段的学习者，给出分层的落脚点规划实践路线：

入门（先把平地走稳）

1. 从一维 LIPM 仿真器（足式/70_腿足简化模型理论）起步，实现纯 Raibert 前馈项，观察机器人能否维持匀速——这是理解"对称摆动"的最快路径。
2. 加上速度反馈项，用 Poincare 映射（58.2）离线扫描 \(k_p\)，画出速度误差随步数的收敛曲线，亲眼看到 \(k_p\) 过大时 \(|\lambda|>1\) 的振荡发散。
3. 扩到三维 + 航向修正，在 Go2 仿真里用 RViz 把落脚点画成球标记，零速站立时检查左右脚是否对称（这是抓航向项符号错误的最有效手段）。

进阶（应对扰动与地形）

1. 实现 Capture Point 混合落脚（58.4 的 \(\alpha\) 混合公式），施加侧向推力做推恢复，对比纯 Raibert 与混合 CP 的恢复成功率。
2. 接入高程图，实现评分函数（58.7）+ 可达性/地形约束（58.6），在斜坡与粗糙地形上选点；务必先验证"裁剪到最近可行点"再上评分函数，便于隔离 bug。
3. 实现反应式调整（58.8），重点练习**限速 + 低通滤波**——不加这两者，状态估计噪声会让摆动脚高频抖动，这是最常见的"仿真好、上车抖"故障。

研究级（联合优化与离散地形）

1. 把单步 Raibert 升级为多步落脚点 MPC（58.9 的 LipmFootstepMpc），接 OSQP，在急停任务上复现本章的"单步不可行、两步可行"对比。
2. 在 stepping stones / 楼梯上实现 MIQP（58.10），用开源 MIP 求解器（HiGHS / OR-Tools），重点体会 Big-M 取值对数值稳定性的影响。
3. 阅读 Grandia 2023（ANYmal perceptive NMPC）源码思路，理解 SQP + RTI 如何让 ms 级单次求解撑起 ~100 Hz 控制——这是从"能解"到"实时解"的关键工程跨越。

💡 贯穿性建议：无论哪个阶段，都把"落脚点"和"下游接触力/力矩"一起看。一个落脚点好不好，最终要看它让 MPC（足式/110_OCS2完整栈与双线程MPC）和 WBC（足式/90_WBC分层优化与TSID）的力分配是否落在摩擦锥内、关节力矩是否超限。脱离下游孤立地评价落脚点规划器，是初学者最容易陷入的视角误区。

---

与其他章节衔接

向前承接： - 足式/70_腿足简化模型理论（LIPM / DCM / Capture Point）→ 本章的数学基础（LIPM 通解、DCM 动力学） - 足式/90_WBC分层优化与TSID（WBC）→ 落脚点确定后的力分配和关节力矩计算 - 足式/110_OCS2完整栈与双线程MPC（OCS2 架构）→ MPC 中的 SwingTrajectoryPlanner 模块 - 足式/120_步态管理与接触序列（步态管理 / Raibert 简介）→ 本章对 Raibert 的完整展开和推广

向后指向： - 足式/140_落脚点规划经典方法 Raibert / MIQP → 足式/150_优化驱动落脚与接触规划（Contact-Implicit TO：从启发式到优化驱动的接触选择） - 足式/140_落脚点规划经典方法 评分函数 / 地形约束 → 足式/160_感知驱动落脚规划（感知驱动落脚规划：高程图 + 可通行性 + stepping stones + ANYmal parkour） - 足式/140_落脚点规划经典方法 反应式调整 → 足式/190_腿足RL训练栈（RL 学习落脚策略：从规则到策略网络的演进）

---

[跨章综合题] 练习 58.X：从 LIPM 到实时落脚——Raibert + MPC + WBC 全链路

本题需要综合 足式/70_腿足简化模型理论(LIPM/DCM)、足式/110_OCS2完整栈与双线程MPC(OCS2 MPC)和本章(落脚规划)的知识。

场景: Go2 以 0.8 m/s 向前 trot 行走,操作员突然发出 -0.3 m/s 减速命令(从 0.8 降到 0.5 m/s)。

1. (足式/70_腿足简化模型理论) 用 LIPM 参数(\(h = 0.3\) m, \(\omega = \sqrt{g/h} \approx 5.72\) rad/s)计算当前的 Capture Point \(\xi = x + \dot{x}/\omega\)。减速命令后,目标 Capture Point 如何变化?
2. (本章 58.2) 用 Raibert 公式计算减速前和减速后的目标落脚点。计算落脚点的位移量 \(\Delta p_{\text{foot}}\)。验证:减速时脚应该往前多迈(增大步幅)——与直觉一致吗?
3. (足式/110_OCS2完整栈与双线程MPC) Raibert 计算的落脚点通过 SwingTrajectoryPlanner 传入 OCS2 MPC 的 reference 中。MPC 的 horizon 内,接触力分配会如何变化以实现减速?定性描述支撑腿的力方向变化(是否从"向后推"变为"向前刹车"?)。
4. (综合) 如果 \(k_p\) 设置得太小(比最优值低 50%),Poincare 映射特征值 \(|\lambda|\) 会如何变化?对应的实际表现是什么(一步减到目标速度 vs 多步振荡减速)?

---

掌握了经典落脚点方法之后，足式/150_优化驱动落脚与接触规划 将带你进入优化驱动的接触规划——让接触时刻和位置都成为优化变量（Contact-Implicit TO、MIQP 的推广、Graph-of-Convex-Sets），足式/160_感知驱动落脚规划 则接入感知系统（高程图、可通行性分析、ANYmal parkour），将本章的数学框架与真实地形连接。

---