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

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

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

前置依赖：Ch51（LIPM / DCM / Capture Point）、Ch53（WBC）、Ch55（OCS2 架构）、Ch56（步态管理 / 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 题 → 先回前置章节复习

[Ch51] 写出 LIPM 方程 $\ddot{x} = \omega^2(x - p)$，说明 $\omega$ 的物理含义和 Go2 的典型数值。
[Ch51] 什么是 Divergent Component of Motion（DCM）？写出其定义 $\xi = x + \dot{x}/\omega$，解释为什么它叫"发散分量"。
[Ch51] Capture Point 的定义是什么？它与 DCM 的关系？
[Ch56] OCS2 的 ModeSchedule 如何告诉 MPC"哪只脚在什么时候着地"？
[Ch56] Raibert 启发式的三项分别对应什么物理含义？

本章目标¶

学完本章，你应能：

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

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

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

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

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

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

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

💡 概念澄清："落脚点规划"与"摆动轨迹生成"是两个不同的问题。落脚点回答"脚往哪里放"，摆动轨迹回答"脚怎么过去"。本章聚焦前者。Ch56.4 讲解了摆动轨迹生成——通过 Cubic Spline 或 Bezier 曲线在"当前足端位置"与"目标落脚点"之间插值出平滑路径，并用抬腿高度（swing height）参数避免地面刮蹭。本章输出的目标落脚点正是 Ch56.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（Ch56）给出的接触序列——哪只脚在什么时刻从支撑转为摆动
环境信息（可选）：高程图、可通行性图、障碍物位置

输出：

对每只即将进入摆动相的脚，给出目标落脚点 $\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)
       ↓
┌──────────────────┐
│  步态管理 (Ch56)  │ → ModeSchedule: 哪只脚何时摆动
└──────┬───────────┘
       ↓
┌──────────────────┐         ┌──────────────┐
│  落脚点规划       │ ←───── │  地形感知     │
│  (本章 Ch58)      │         │  (Ch60)       │
└──────┬───────────┘         └──────────────┘
       ↓ p_foot_target
┌──────────────────┐
│  摆动轨迹生成     │ → 脚的位置/速度/加速度轨迹
│  (Ch56.4)        │
└──────┬───────────┘
       ↓
┌──────────────────┐
│  MPC (Ch55)       │ → 最优接触力
└──────┬───────────┘
       ↓
┌──────────────────┐
│  WBC (Ch53)       │ → 关节力矩
└──────────────────┘

⚠️ 常见陷阱¶

⚠️ 编程陷阱：落脚点计算后忘记投影到地面 - 错误做法：直接用 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 攀墙）的手脚协调规划是落脚问题的高维推广

练习¶

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

58.2 Raibert 启发式 ⭐⭐¶

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

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

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

这个问题的难度在于：倒立摆是不稳定的，任何微小偏差都会指数增长（Ch51.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 模型

从 Ch51.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 的时间常数。精确分析给出最优 $k_p = \frac{1}{\omega}\tanh(\omega T_s/2) - \frac{T_s}{2}$。当 $\omega T_s \gg 1$ 时 $\tanh \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 增益）。

对 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 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 是落脚规划的"牛顿第一定律"——简单但是基础

练习¶

[推导题] 从 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$ 的百分比误差。
[仿真题] 实现一维 LIPM 仿真器（参考 Ch51 代码），施加 Raibert 控制律。画出 $k_p$ 从 $0$ 到 $0.5$ 变化时，速度误差收敛的步数。验证 $k_p = 1/\omega$ 附近是否最优。
[对比题] 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 上保证了**不会因发散模态而摔倒**，但不保证**运动学可行性**

练习¶

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

58.4 Capture Point 落脚规划 ⭐⭐⭐¶

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

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

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

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

这引出了 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$），使得：

每个 $p_i$ 在对应时刻可达：$p_i \in \mathcal{W}_{\text{kin}}(x_i)$
执行完 $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$ 的示例计算：

对每个候选状态 $(x, \dot{x})$，遍历所有可达落脚点 $p$
计算一步后的状态 $(x', \dot{x}')$
检查 $(x', \dot{x}') \in \mathcal{C}_1$
如果存在至少一个 $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 的多步前瞻

练习¶

[计算题] Go2 的腿长约 0.25 m，可达工作空间半径约 0.2 m。计算 1-step Capture Region 允许的最大速度（即 $\dot{x}_{\max}$ 使得 $\xi = x + \dot{x}/\omega$ 仍在可达范围内）。答案应约为 $\omega \cdot r_{\max}$。
[推导题] 对两步行走序列（步 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$ 组合）。
[思考题] 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 步）

练习¶

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

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

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

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

考虑以下场景：

机器人被推了一把，CP 公式给出落脚点距离髋关节 0.4 m——但 Go2 腿只有 0.25 m
Raibert 算出的落脚点恰好在一个深坑上方
落脚点在 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 获取，Ch60 详述），可以分析以下地形特征：

特征	计算方式	安全要求
坡度	$\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 等先进平台使用力矩估计来检测脚是否在滑动，作为后验地形安全判断

练习¶

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

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 的控制器根据估计的地形类型（平地/粗糙/阶梯）自动切换权重

练习¶

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

58.8 反应式落脚调整 ⭐⭐⭐¶

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

动机：规划不可能完美¶

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

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

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

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

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

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

反应式调整：在被推的瞬间，重新计算 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 也有类似的限幅和平滑处理

练习¶

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

58.9 混合整数落脚规划 ⭐⭐⭐⭐¶

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

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

考虑一个 stepping stones 场景：

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

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

离散决策：每只脚踩在哪个 stone 上？（组合选择）
连续决策：在选定的 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：约束

每步只能分配到一个区域： $$\sum_{j=1}^{M} z_{ij} = 1, \quad \forall i$$
落脚点在分配区域内（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$ 时，约束生效）
运动学可达： $$\|\boldsymbol{p}_i - \boldsymbol{p}_{\text{hip},i}\|_2 \leq r_{\max}$$
步幅约束（相邻步之间）： $$\|\boldsymbol{p}_{i+1} - \boldsymbol{p}_i\| \leq s_{\max}$$
左右脚不交叉： $$(\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)"

练习¶

[建模题] 一个双足机器人在 3 个 stepping stones 上走 4 步。写出完整的 MIQP 建模：列出所有决策变量、约束和代价函数。计算整数变量和连续变量的总数。
[编程题] 用 Python + 开源 MIP 求解器（如 PuLP + HiGHS，或 Google OR-Tools）求解上述 MIQP。可视化最优落脚序列。
[思考题] Contact-Implicit Trajectory Optimization（Ch59 将详述）和 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}}$ 值确认递推过程	Ch58.4
落脚点系统性偏向一侧	航向修正项的叉乘方向错误或基座偏航估计有偏	1. 零速站立时打印左右脚落脚点，检查是否对称 2. 单独关闭航向修正项观察是否恢复 3. 检查 IMU 偏航角是否有静态偏差	Ch58.2, Ch57
步幅过大超出运动学限制（脚伸不到）	速度指令突变导致前馈项 $\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$ 是否超出可达半径	Ch58.3-58.4
MIQP 求解超时或返回 infeasible	候选接触区域定义不合理、Big-M 值过大导致松弛间隙、或整数变量过多	1. 减少候选区域数量（只保留可达范围内的区域） 2. 检查 Big-M 值是否紧致 3. 给 MIQP 设置时间上限并使用 warm-start	Ch58.9

58.10 本章小结¶

知识点总表¶

知识点	难度	核心思想	关键公式/方法	代表工作
问题定义	⭐	落脚规划的输入/输出/约束	$\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 误差驱动 + 速率限制	—
MIQP	⭐⭐⭐⭐	离散区域分配 + 连续位置优化	Branch-and-Bound	Deits 2014

方法选择决策树¶

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

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

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

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

累积进度:
  Ch47 加载 URDF + 正运动学
  Ch49 逆动力学 + 质心计算
  Ch51 LIPM 仿真器 + DCM 计算
  Ch53 WBC 力分配
  Ch55 OCS2 MPC 集成
  Ch56 步态管理 + ModeSchedule
→ Ch58 落脚点规划（本章新增）
    ├── RaibertFootholdPlanner：完整 Raibert 公式（前馈+反馈+航向）
    ├── CapturePointPlanner：融合 DCM 的混合落脚规划
    ├── FootholdScorer：多准则评分函数（地形+可达+稳定+名义）
    └── ReactiveAdjuster：摆动相中途调整（速率限制+低通滤波）

本章项目任务：

Task A：实现 RaibertFootholdPlanner 类，包含完整的前馈+反馈+航向修正。在 Go2 仿真中用 RViz 可视化落脚点（球标记）
Task B：在 RaibertFootholdPlanner 中添加 Capture Point 混合模式（$\alpha$ 参数），对比纯 Raibert 和混合 CP 在推恢复中的表现
Task C：实现 FootholdScorer，在有高程图的地形上选择最优落脚点。可视化评分热图
Task D：实现 ReactiveAdjuster，在摆动相中途施加扰动后自动调整落脚点，对比有/无调整的恢复效果

58.12 延伸阅读¶

必读经典¶

文献	难度	核心贡献
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	⭐⭐⭐⭐	地形感知的联合运动-落脚优化

前沿方向¶

文献	难度	核心贡献
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 残差策略

开放研究问题¶

学习 Raibert 增益：不同地形/速度/步态下的最优 $k_p$ 是否可以用强化学习自动调节？初步工作表明 RL 残差策略可以在 sim-to-real 中提升崎岖地形的成功率
落脚点-力联合优化：当前方法先选落脚点再做力分配（Ch53 WBC），是否可以联合优化？TAMOLS 做了初步尝试但受限于求解速度
语义落脚点：利用视觉语义信息（"这是泥地"、"这是冰面"、"这是铁板"）辅助落脚决策，超越纯几何的高程图分析
多机器人协调落脚：两个四足机器人协作搬运物体时，如何协调各自的步态和落脚点以保持共同稳定性？

与其他章节衔接¶

向前承接： - Ch51（LIPM / DCM / Capture Point）→ 本章的数学基础（LIPM 通解、DCM 动力学） - Ch53（WBC）→ 落脚点确定后的力分配和关节力矩计算 - Ch55（OCS2 架构）→ MPC 中的 SwingTrajectoryPlanner 模块 - Ch56（步态管理 / Raibert 简介）→ 本章对 Raibert 的完整展开和推广

向后指向： - Ch58 Raibert / MIQP → Ch59（Contact-Implicit TO：从启发式到优化驱动的接触选择） - Ch58 评分函数 / 地形约束 → Ch60（感知驱动落脚规划：高程图 + 可通行性 + stepping stones + ANYmal parkour） - Ch58 反应式调整 → Ch63（RL 学习落脚策略：从规则到策略网络的演进）

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

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

项	名称	物理含义	量级（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

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

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

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

参数	值
大腿长 \(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

特征	计算方式	安全要求
坡度	\(\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

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

症状	可能原因	排查步骤	相关章节
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}}\) 值确认递推过程	Ch58.4
落脚点系统性偏向一侧	航向修正项的叉乘方向错误或基座偏航估计有偏	1. 零速站立时打印左右脚落脚点，检查是否对称 2. 单独关闭航向修正项观察是否恢复 3. 检查 IMU 偏航角是否有静态偏差	Ch58.2, Ch57
步幅过大超出运动学限制（脚伸不到）	速度指令突变导致前馈项 \(\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\) 是否超出可达半径	Ch58.3-58.4
MIQP 求解超时或返回 infeasible	候选接触区域定义不合理、Big-M 值过大导致松弛间隙、或整数变量过多	1. 减少候选区域数量（只保留可达范围内的区域） 2. 检查 Big-M 值是否紧致 3. 给 MIQP 设置时间上限并使用 warm-start	Ch58.9

知识点	难度	核心思想	关键公式/方法	代表工作
问题定义	⭐	落脚规划的输入/输出/约束	\(\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 误差驱动 + 速率限制	—
MIQP	⭐⭐⭐⭐	离散区域分配 + 连续位置优化	Branch-and-Bound	Deits 2014