第92章：经典人形全身控制——LIPM/DCM + TSID + 动量 WBC¶

元信息	值
难度	⭐⭐⭐⭐（双足平衡数学 + 全身 QP + 动量控制）
预计时间	2 周（45-55 小时）
核心平台	PAL Talos、Unitree G1、Unitree H1、Atlas 类平台
主线	LIPM/ZMP/DCM → CMM/角动量 → TSID/HQP → torque-level 行走

本章定位 本章面向已经学过腿足简化模型、WBC、复合机器人动力学的读者。写作重点不是罗列论文名，而是把数学假设、控制接口和真实系统故障连成一条可推理的链。读完后应能解释每个控制量从哪里来、为什么这样定义、用错以后会怎样。

92.0 前置自测¶

#	问题	前置知识	合格答案关键词
1	LIPM 的两个核心假设是什么？	足式/70	恒定 CoM 高度、忽略角动量变化
2	ZMP 与 CoP 的区别是什么？	足式/70	ZMP 是动力学等效点，CoP 是压力分布中心
3	WBC 为什么常以加速度和接触力为变量？	足式/90	力矩可由逆动力学反推
4	CMM 的定义是什么？	复合/20	$h_G=A_G(q)v$
5	HQP 相比加权 QP 的核心优势是什么？	足式/90	严格任务优先级

本章目标¶

补齐双足特有的平衡数学：ZMP、CoP、DCM、Capture Point、VRP。
完整推导 LIPM、ZMP 角动量修正和 DCM 一阶发散动力学。
理解质心动量矩阵如何把 30+ DoF 全身速度映射到 6D 动量。
掌握 TSID / HQP 中接触约束、动力学约束、CoM 任务和角动量任务的组装方式。
能设计 Talos 或 G1 的简化双足行走 demo，并能定位常见失稳原因。
能区分人形动量 WBC（IHMC/TALOS 风格）与四足 WBC（MIT Cheetah WBIC 风格）的设计取舍。
能用 N 步可捕获性（N-step capturability）判断扰动是否可恢复，以及需要几步。

本章知识导航¶

本章要回答一个核心问题：在只有一两个窄脚掌支撑的条件下，如何让 30+ 自由度的人形既站得稳又走得动？ 这个问题的答案不是单一算法，而是一条从"简化模型"到"全身求解"的控制链。下面这张全景地图给出本章所有知识点的依赖关系，建议在阅读过程中反复回看，确认当前内容挂在知识树的哪个位置。

                    人形全身控制的两层结构
   ┌──────────────────────────────────────────────────────┐
   │  规划层（低维、可解析、关注平衡）                       │
   │                                                        │
   │   支撑多边形(§1) ──> LIPM(§2) ──> ZMP/CoP/CMP(§3)       │
   │        │               │              │                │
   │        │               ▼              ▼                │
   │        │         Preview/MPC(§4)   DCM/VRP/             │
   │        │               │           Capture Point(§5)   │
   │        │               │              │                │
   │        └───────────────┴──> 落脚规划(§6) <─ N步可捕获   │
   │                                  │                      │
   └──────────────────────────────────┼──────────────────────┘
                                       │ CoM/ZMP/VRP/脚印参考
                                       ▼
   ┌──────────────────────────────────────────────────────┐
   │  全身层（高维、数值优化、关注力矩可行）                  │
   │                                                        │
   │   CMM 质心动量矩阵(§7) ──> 动量任务                     │
   │        │                      │                        │
   │        ▼                      ▼                        │
   │   TSID / HQP 全身逆动力学(§8) ──> 关节力矩 τ            │
   │        │                                               │
   │        ├──> IHMC Atlas 动量优先架构(§9)                 │
   │        ├──> WBIC：四足/人形的对照方案(§9.5)             │
   │        ├──> 多接触 / 楼梯 / 斜面扩展(§11)               │
   │        └──> TALOS 力矩级 WBC(§12)                       │
   │                                                        │
   │   历史与工程教训：DRC 2013-2015(§10)                   │
   │   完整数据流与教学 demo(§13)                            │
   └──────────────────────────────────────────────────────┘

这条链有一个贯穿全章的主线索：信息从高维压缩到低维做规划，再从低维展开回高维做执行。 §2 的 LIPM 把 30+ 维压成 2 维，§7 的 CMM 又把全身速度映射回 6 维动量，§8 的 TSID 最终把动量和任务展开成每个关节的力矩。理解这个"压缩—展开"的往返，就抓住了经典人形 WBC 的骨架。

知识簇	包含小节	解决的问题	难度
双足平衡数学	§1-§3	窄支撑下"合力作用点"为什么比"合力大小"更敏感	⭐⭐-⭐⭐⭐
平衡规划方法	§4-§6	已知未来脚印，质心现在该怎么动；扰动来了要不要迈步	⭐⭐⭐-⭐⭐⭐⭐
全身动量与求解	§7-§9.5	把平衡参考翻译成 30+ 个关节的协同力矩	⭐⭐⭐⭐
工程化与扩展	§10-§13	真实系统怎么调、怎么排障、怎么上楼梯	⭐⭐⭐-⭐⭐⭐⭐

前置知识桥接¶

本章站在三块前置知识之上。下面用 2-3 行重新激活每一块的核心要点，让你不必翻回去也能跟上当前推导：

回顾足式/70（腿足简化模型）：那里我们建立了"用一个倒立摆近似整机平衡"的思想，并引入了 SRBD（单刚体动力学）。本章把这条思路推到双足的极端情形——支撑区域窄到只剩一个脚掌，于是 SRBD 不够用，必须补上 ZMP/DCM 这套"窄支撑专用语言"。
回顾足式/90（WBC 与 HQP）：那里我们学过 WBC 把运动分解成多个任务，用 QP 求满足物理约束的关节加速度和接触力，并见过加权 QP 与分层 QP（HQP）的区别。本章直接复用这个 QP 框架（§8），但把任务栈换成人形特有的"CoM + 角动量 + 摆脚 + 姿态"，并补上四足里几乎用不到的角动量任务。
回顾复合/20（复合机器人动力学）：那里我们推导了带浮动基座、接触力和末端外力的统一动力学方程 $M\dot v+h=S^T\tau+J_c^T\lambda$，以及质心动量矩阵 CMM 的定义 $h_G=A_G(q)v$。本章把这两个结果当作直接工具：动力学方程是 §8 TSID 的硬约束，CMM 是 §7 动量任务的核心。

本质洞察 本章不是在前置章节之外"新发明"了什么，而是把已有的三块拼图（简化模型、WBC 框架、复合动力学）在"窄支撑双足"这个最苛刻的场景下重新组合。真正新增的只有两样东西：双足平衡专用的几何语言（ZMP/DCM/Capture Point）和把角动量纳入控制的动量任务。其余全是旧工具的再利用。

如果跳过本章会怎样¶

如果你直接从四足 WBC 跳到人形项目，最可能踩两个坑：

场景一（控制器照搬）：把四足的 MPC+WBC 工程直接套到人形，单支撑时机器人"力矩跟踪很完美却绕脚边翻倒"。原因是四足 trot 仍有对角两点构成支撑线段，而人形单支撑只有一个约 24cm x 12cm 的脚掌——你缺了 ZMP 约束和 DCM 反馈，QP 再准也救不回来。
场景二（奖励设计抓瞎）：做人形 RL 时想在奖励里加"平衡项"，却不知道该惩罚什么。读完本章你会知道：ZMP 裕度、DCM 误差、角动量峰值都是现成的、有物理含义的奖励信号——这正是第93章 RL 奖励设计大量借鉴本章概念的原因。

预计阅读时间¶

模式	范围	时间	适合人群
精读	全章 + 手推 §2/§5/§7 + 跑通 demo	45-55 小时（约 2 周）	要做人形控制项目的工程师
速读	§1-§9 正文，跳过部分推导细节和 §10-§12	8-12 小时	已有四足 WBC 基础、快速补双足
速查	公式速查 + API 速查 + 故障排查手册	1-2 小时	调试时定位具体问题

92.1 从四足到双足：支撑多边形为什么改变控制问题 ⭐⭐¶

动机：不是少两条腿那么简单¶

回顾复合/20：四足加机械臂的统一动力学已经包含浮动基座、接触力和末端外力。在那里，四个足端给了控制器大量接触冗余，接触力可以在多个点之间重新分配。人形在单支撑时只有一个脚掌，双支撑时也只有两个有限脚面。这让"合力作用线是否落在脚掌内"成为和摩擦锥同等重要的问题。 ZMP、DCM 和 Capture Point 正是在这种窄支撑条件下发展出来的语言。

形态	典型接触	支撑区域	主要风险	控制指标
四足站立	四点接触	大凸包	打滑或力矩饱和	CoM 投影、接触力
四足小跑	两点接触	接近线段	躯干角速度过大	SRBD 动量、落脚点
双足双支撑	双脚面接触	两脚外包络	ZMP 边界不足	ZMP、DCM
双足单支撑	单脚面接触	一个脚掌	绕脚边翻倒	ZMP、Capture Point
人形操作	脚与手混合接触	随任务变化	外力改变动量	CMM、接触 wrench

支撑多边形的线性约束形式¶

设接触点水平投影为 $p_i=(x_i,y_i)$。支撑多边形是这些点的凸包： $$ \mathcal P=\operatorname{conv}{p_1,\ldots,p_m} ={\sum_i\alpha_i p_i\mid\alpha_i\ge0,\sum_i\alpha_i=1}. $$

在 QP 中通常使用半空间形式 $A_p p\le b_p$。于是 ZMP 可行性就变成 $A_pp_{zmp}\le b_p$，这是 ZMP-MPC 可以实时求解的关键。

关键洞察 四足控制常从接触力分配开始，人形控制常从 ZMP/DCM 开始。原因不是历史偶然，而是支撑区域大小和接触冗余决定了哪些变量最敏感。当支撑区域很窄时，合力作用点比合力大小更早触发失稳。

人形控制栈的时间尺度¶

层	典型频率	输入	输出	失效表现
落脚规划	1-20 Hz	速度命令、地形、DCM	脚印和步态相位	下一步不可达
LIPM/DCM 规划	50-200 Hz	当前 CoM、脚印	CoM/ZMP/VRP 参考	DCM 发散
TSID/WBC	500-1000 Hz	全身状态和参考	力矩或目标加速度	脚滑、QP 无解
驱动伺服	1-40 kHz	力矩或位置目标	电流与电机响应	延迟、振荡、过流

陷阱警告 ⚠️ 初学者常把四足的 MPC+WBC 方案直接搬到人形，忽略了支撑区域从四点凸包缩小到一个脚掌带来的根本性变化。四足在 trot 步态中仍有对角两点接触构成一条支撑线段，而人形单支撑只有一个 20cm x 10cm 的脚掌。这意味着人形必须额外引入 ZMP 约束和 DCM 反馈，否则即使力矩跟踪完美，机器人仍会绕脚边翻倒。

练习 92.1.A：画出 Unitree G1 在单支撑相位下的支撑多边形。假设脚掌为 24cm x 12cm 的矩形，写出半空间形式 $A_p p \le b_p$ 的具体数值。

练习 92.1.B：如果人形同时用左脚和右手接触墙面，支撑多边形还是凸包吗？讨论多接触场景下 ZMP 概念的局限性。

92.2 LIPM 与 cart-table 模型完整推导 ⭐⭐⭐¶

为什么要先降维¶

完整人形动力学包含 30+ 个关节、浮动基座、脚底 wrench、关节限位和接触切换。如果把这些变量全部放进高频 MPC，计算预算会立刻被耗尽。 LIPM 的思想是先保留平衡最敏感的质心水平运动，把关节细节交给 WBC。这是一种有意识的信息压缩，不是对真实机器人的否认。

这个思想在控制领域有深厚的传统。类比信号处理中的低通滤波：LIPM 相当于只保留质心运动的"低频分量"（平移加速度），而把关节层面的"高频分量"（力矩分配、关节限位）留给下游的 WBC。如果不做这种分频处理，MPC 层的计算预算会被高维非线性动力学耗尽，无法在线求解。

LIPM 最早由 Kajita 等人在 2001 年正式提出（Kajita et al., "The 3D Linear Inverted Pendulum Mode: A simple modeling for a biped walking pattern generation", IROS 2001）。在那之前，人形行走规划主要依赖预先设计的关节轨迹，缺乏对质心动力学的系统分析。

假设	数学表达	简化效果	代价
恒定质心高度	$z=z_c,\ddot z=0$	竖直力固定为 $mg$	不能描述下蹲与跳跃
点质量	质量集中于 CoM	关节细节消失	无法直接求力矩
角动量变化小	$\dot k_G\approx0$	ZMP 与 CoM 加速度直接相关	摆臂和扭腰被忽略
平面足底	$p_{zmp}\in\mathcal P$	稳定性变成几何约束	忽略足底柔性

从牛顿方程到 LIPM¶

设质心 $r_G=[x,y,z_c]^T$，地面合力 $f=[f_x,f_y,f_z]^T$，重力为 $[0,0,-mg]^T$。 $$ m\ddot r_G=f+[0,0,-mg]^T. $$

恒定高度意味着 $\ddot z=0$，因此竖直方向有 $f_z=mg$。水平方向有 $f_x=m\ddot x$，$f_y=m\ddot y$。

取地面 ZMP 点 $p=[p_x,p_y,0]^T$，忽略质心角动量变化时，绕质心的水平力矩为零。为什么力矩为零？因为 ZMP 的定义就是使水平力矩消失的等效作用点。如果地面反力的等效作用点不在 ZMP 处，就会产生绕水平轴的净力矩，导致机器人绕脚边旋转。

\[ \begin{aligned} (p-r_G)\times f &\approx0\quad\text{的水平分量成立},\\ z_c m\ddot x-(x-p_x)mg&=0,\\ \ddot x&=\frac{g}{z_c}(x-p_x). \end{aligned} \]

推导细节：展开叉积 $(p-r_G)\times f$ 的 y 分量： $$ (p_z-z_c)f_x - (p_x-x)f_z = -z_c \cdot m\ddot x - (p_x-x)\cdot mg = 0 $$ 移项得 $m\ddot x \cdot z_c = (x-p_x)\cdot mg$，即 $\ddot x = \frac{g}{z_c}(x-p_x)$。

同理得到 y 方向方程。令 $\omega_0=\sqrt{g/z_c}$： $$ \ddot x=\omega_0^2(x-p_x),\qquad \ddot y=\omega_0^2(y-p_y). $$

本质洞察 LIPM 不是一个"近似的物理模型"，而是一个"精确描述特定假设下质心运动的数学工具"。它的价值不在于精度，而在于把双足平衡问题从 30+ 维关节空间压缩到 2 维质心平面。这种压缩让我们可以用线性系统理论分析稳定性、设计反馈控制、甚至写出解析解。

为什么它天然不稳定¶

固定 $p_x=p$，令 $\tilde x=x-p$。 $$ \ddot{\tilde x}=\omega_0^2\tilde x. $$

特征方程 $\lambda^2-\omega_0^2=0$ 给出两个特征值 $+\omega_0$ 和 $-\omega_0$。 $$ \tilde x(t)=c_1e^{\omega_0t}+c_2e^{-\omega_0t}. $$

正指数项就是倒立摆的发散模态。控制器必须通过移动 ZMP 或迈步来抑制这个模态。

如果不抑制会怎样？ 取 $z_c=0.8$ m，则 $\omega_0 \approx 3.5$ rad/s，时间常数 $1/\omega_0 \approx 0.29$ s。这意味着初始 1 cm 的质心偏移，在 0.29 秒后就会增长到 2.7 cm，0.58 秒后增长到 7.4 cm。对于脚掌宽度只有 10 cm 的人形来说，不到 1 秒就会失去平衡。这就是为什么人形控制器必须在 50-200 Hz 频率下持续调整 ZMP。

解析离散化¶

\[ s=[x,\dot x]^T,\quad \dot s=\begin{bmatrix}0&1\\\omega_0^2&0\end{bmatrix}s+\begin{bmatrix}0\\-\omega_0^2\end{bmatrix}p. \]

由于系统矩阵是常数，可以精确计算矩阵指数：

\[ A_d=e^{AT}=\begin{bmatrix}\cosh(\omega_0T)&\sinh(\omega_0T)/\omega_0\\\omega_0\sinh(\omega_0T)&\cosh(\omega_0T)\end{bmatrix}, B_d=\begin{bmatrix}1-\cosh(\omega_0T)\\-\omega_0\sinh(\omega_0T)\end{bmatrix}. \]

为什么用 cosh/sinh 而不是 cos/sin？ 因为系统特征值是实数 $\pm\omega_0$（不稳定），不是虚数 $\pm j\omega_0$（振荡）。倒立摆的自由响应是指数增长/衰减，不是正弦振荡。这是理解 LIPM 不稳定性的关键直觉。

import numpy as np


def lipm_step(state, zmp, z_com, dt):
    """一维 LIPM 解析离散步进。state = [x, dx]。"""
    omega = np.sqrt(9.81 / z_com)
    c = np.cosh(omega * dt)
    s = np.sinh(omega * dt)
    # 中文注释：利用矩阵指数的解析形式，避免数值积分误差。
    A = np.array([[c, s / omega],
                  [omega * s, c]])
    B = np.array([1.0 - c, -omega * s])
    return A @ state + B * zmp


# 中文注释：ZMP 固定时，初始速度会沿发散模态增长。
state = np.array([0.0, 0.15])
for _ in range(200):
    state = lipm_step(state, zmp=0.0, z_com=0.8, dt=0.01)
print(state)

陷阱警告 ⚠️ 使用 Euler 积分代替解析离散化会引入数值阻尼或数值激励，在长 horizon 的 MPC 中累积误差可达 10% 以上。LIPM 的一个巨大优势正是它有解析解，应当充分利用而不是退化到数值方法。

练习 92.2.A：手推 LIPM 的解析离散化。从状态空间 $\dot s = As + Bu$ 出发，利用 $A$ 的特征分解 $A = P\Lambda P^{-1}$，证明 $e^{AT}$ 恰好给出上述 cosh/sinh 形式。

练习 92.2.B：编写代码比较解析离散化和四阶 Runge-Kutta 在 $T=0.01$ s 和 $T=0.05$ s 下的差异。在 100 步后，两者的状态偏差有多大？

92.3 ZMP、CoP、CMP 与角动量修正 ⭐⭐⭐¶

ZMP 不是一个玄学稳定点¶

ZMP 是地面反力等效作用点，使该点处的水平力矩为零。脚底只能通过非负压力分布产生反力。因此 ZMP 落在脚掌内时，存在一组足底压力可以实现该合力。 ZMP 落在脚掌外时，脚底需要"拉地面"才能实现等效力矩，这在普通接触中不可能。

历史背景：ZMP 概念由 Vukobratovic 和 Juricic 在 1969 年提出（"Contribution to the synthesis of biped gait", IEEE Trans. Bio-Medical Engineering）。在此之前，双足行走的稳定性分析主要依赖直觉和简单的静态平衡条件。ZMP 准则的提出使得动态行走的稳定性分析有了可操作的数学判据，随后被 Honda、AIST 等机构的人形机器人团队广泛采用。

含角动量的 ZMP 公式¶

LIPM 的 $\dot k_G\approx0$ 是强假设。人形摆臂、扭腰、上体快速转动时，角动量变化率必须进入 ZMP。

完整的 ZMP 公式从质心角动量定理出发。绕质心的角动量变化率等于外力矩： $$ \dot{\mathbf{k}}_G = \sum_i (\mathbf{p}_i - \mathbf{r}_G) \times \mathbf{f}_i $$

对于地面单接触情况，外力只有地面反力和重力。将地面反力等效作用于 ZMP 点 $p_{zmp}$： $$ \dot{\mathbf{k}}G = (\mathbf{p}_G) $$} - \mathbf{r}_G) \times (m\mathbf{g} + m\ddot{\mathbf{r}

展开 x 分量和 y 分量并求解 $p_x$、$p_y$：

\[ \begin{aligned} p_x&=x-\frac{z\ddot x-\dot k_y/m}{g+\ddot z} =x-\frac{z\ddot x}{g+\ddot z}+\frac{\dot k_y}{m(g+\ddot z)},\\ p_y&=y-\frac{z\ddot y+\dot k_x/m}{g+\ddot z} =y-\frac{z\ddot y}{g+\ddot z}-\frac{\dot k_x}{m(g+\ddot z)}. \end{aligned} \]

当 $\ddot z=0$ 且 $\dot k_G=0$ 时，退化为 $p_x=x-z_c\ddot x/g$。这说明角动量不是附属概念，而是 LIPM 主动忽略掉的一项。

角动量项的物理含义：$\dot k_y/(mg)$ 的量纲是长度（米）。当人形快速向前摆臂时，$\dot k_y > 0$（绕 y 轴的角动量增加），ZMP 会向前移动。这意味着摆臂可以"虚拟地"扩大 ZMP 的可用范围。这正是人类在窄梁上走路时会摆臂的原因——摆臂提供了额外的角动量调节能力，等效于增大了虚拟支撑区域。

概念	定义	是否依赖角动量	典型用途
CoP	足底压力中心	由压力分布决定	足底传感与接触诊断
ZMP	水平力矩为零的动力学点	完整公式中包含 $\dot k_G$	步态规划与稳定约束
CMP	允许角动量时的等效压力点	显式体现角动量	臀策略、强扰动恢复
VRP	DCM 控制中的三维虚拟点	通过高度和压力点间接相关	3D DCM 规划

CoP 与 ZMP 的细微区别：在平地上且忽略足底摩擦力矩时，CoP 和 ZMP 是一致的。但在倾斜地面或存在足底扭矩时，两者可能不同。CoP 是一个纯粹的力学测量量（由力/力矩传感器读出），而 ZMP 是一个动力学概念（由质心运动方程定义）。调试时应分别记录两者，它们的差异可以帮助诊断接触模型是否准确。

CMP：当角动量被允许时 ZMP 该如何理解¶

上面的对比表里出现了 CMP（Centroidal Moment Pivot，质心力矩枢轴点），但它值得单独讲清楚——因为它精确刻画了"角动量到底把平衡点移到了哪里"，并直接对应人形两种最基本的恢复策略。

定义动机。 ZMP 被定义为"地面反力使水平力矩为零的点"，它**必须**落在脚掌内（否则物理不可实现）。但当人形主动用角动量（摆臂、扭腰）时，绕质心产生了非零力矩 $\dot k_G\ne0$。CMP 就是回答这个问题：把这部分角动量效应"折算"进来后，地面反力的等效作用线落在地面的哪一点？形式上 CMP 是这样定义的——它是地面反力作用线与地面的交点，与 ZMP 的关系为：

\[ \mathbf r_{CMP}=\mathbf r_{ZMP}-\frac{1}{f_z}\begin{bmatrix}\tau_{G,y}\\-\tau_{G,x}\end{bmatrix}, \]

其中 $\tau_G=\dot k_G$ 是绕质心的力矩（角动量变化率）。当 $\dot k_G=0$（不动用角动量）时，CMP 与 ZMP 重合。当人形摆臂产生 $\dot k_G\ne0$ 时，CMP 偏离 ZMP——而**CMP 可以跑到脚掌外，ZMP 不能**。

两种恢复策略的几何区分。 这正是 CMP 的教学价值所在：

策略	几何特征	物理动作	何时用
踝策略（ankle strategy）	CMP = ZMP，都在脚内	只靠踝关节力矩调 ZMP	小扰动
髋/手臂策略（hip strategy）	CMP 跑到脚外，ZMP 仍在脚内	摆臂/扭腰产生 $\dot k_G$	中大扰动
迈步（stepping）	连 CMP 策略也不够	改变支撑（§92.6）	大扰动

本质洞察 ZMP、CMP、Capture Point 三个点回答的是三个不同问题：ZMP 问"地面反力等效作用在哪、它必须在脚内"；CMP 问"算上角动量后等效作用线落在哪、它可以在脚外"；Capture Point 问"我该往哪踩才能停下"。三者的偏离量直接告诉你机器人正在用哪种策略：CMP 与 ZMP 重合 = 纯踝策略；CMP 离开 ZMP = 正在动用角动量；Capture Point 出脚 = 必须迈步。把这三个点同时画在一张俯视图上，就是一个完整的平衡状态仪表盘。

陷阱警告 ⚠️（概念误区）不要因为"CMP 可以在脚外"就以为它不受约束。CMP 在脚外是有代价的——它对应着角动量在持续累积（$\dot k_G\ne0$），而角动量不能无限增大（躯干转角、手臂行程有限）。§92.5 飞轮模型已说明：用角动量买来的是时间，不是无限的平衡能力。CMP 长时间在脚外，意味着角动量很快会饱和，之后仍必须迈步。

陷阱警告 ⚠️ ZMP 在支撑多边形内是必要条件，不是万能稳定证明。机器人仍可能因为摩擦不足、力矩饱和、状态估计错误或落脚不可达而摔倒。调试时应同时记录 ZMP 裕度、摩擦锥裕度、关节力矩裕度和 DCM 误差。

陷阱警告 ⚠️ 把 ZMP 和 CoP 混为一谈是常见的概念错误。在仿真中两者通常一致（因为接触模型精确），但在实物上足底传感器测量的是 CoP，而控制器计算的是 ZMP。两者的差异可以用来检测接触建模误差。

练习 92.3.A：推导含 $\ddot z \neq 0$ 的完整 ZMP 公式。讨论当机器人下蹲时（$\ddot z < 0$），ZMP 公式分母变小对稳定裕度的影响。

练习 92.3.B：假设人形以 $\dot k_y = 5$ Nm 的速率摆臂（质量 $m=40$ kg），计算 ZMP 偏移量。这个偏移量相对于 12 cm 宽的脚掌有多大？

92.4 Kajita Preview Control 与 ZMP-MPC ⭐⭐⭐¶

为什么需要预见未来¶

双足行走时，未来支撑脚的位置通常已经由脚印序列给出。如果 CoM 等到脚落地才移动，ZMP 会在相位切换时突然贴边。预见控制的思想是：已知未来 ZMP 参考，就让 CoM 提前准备。

这个思想类似于人类行走：当你知道前方有台阶时，你会提前调整重心，而不是到了台阶边缘才反应。Kajita 在 2003 年的论文（"Biped walking pattern generation by using preview control of zero-moment point", ICRA 2003）将这种直觉形式化为线性最优控制问题。

Cart-table 三阶状态¶

Kajita 的关键创新是把 LIPM 从二阶系统扩展为三阶系统，控制量从 ZMP 变为 CoM jerk（加速度的导数）。为什么选择 jerk 而不是直接控制 ZMP？因为 ZMP 是 CoM 加速度的函数，直接控制 ZMP 意味着控制加速度，而加速度不连续会导致力矩跳变。用 jerk 作为控制量，可以保证加速度连续，从而保证力矩平滑。

\[ \begin{aligned} x_k&=[c_k,\dot c_k,\ddot c_k]^T,\qquad u_k=\dddot c_k,\\ x_{k+1}&=Ax_k+Bu_k,\\ p_k&=Cx_k,\qquad C=[1,0,-z_c/g]. \end{aligned} \]

这里 $C$ 的形式直接来自 LIPM 方程 $p=x-z_c\ddot x/g$。

预见控制律的结构¶

\[ u_k=-G_iE_k-G_xx_k-\sum_{j=1}^{N_L}G_p(j)r_{k+j}. \]

$G_i$ 处理累计 ZMP 误差（积分项），$G_x$ 反馈当前 CoM 状态（比例项），$G_p$ 使用未来 ZMP 参考（预见项）。

从教学角度看，预见项就是"未来脚掌在哪里，质心现在就往哪里准备"的线性最优版本。

$G_p(j)$ 的权重随 $j$ 增大而衰减，这符合直觉：近期的脚步位置对当前质心运动影响更大，远期的影响被指数衰减削弱。

Preview 控制增益的物理解释¶

$G_p(j)$ 可以通过离散 Riccati 方程求解。为了理解它的物理含义，考虑一个简化的推导：

Preview 控制的目标函数为： $$ J = \sum_{k=0}^{\infty} \left[ Q_e (p_k - r_k)^2 + R u_k^2 \right] $$

其中 $Q_e$ 是 ZMP 跟踪权重，$R$ 是 jerk 正则化权重。$Q_e/R$ 的比值决定了控制器的"积极程度"——比值越大，控制器越积极地跟踪 ZMP 参考（代价是更大的 jerk），比值越小，控制器越保守（CoM 运动更平滑但 ZMP 跟踪更差）。

典型参数：Kajita 原始论文使用 $Q_e = 1$, $R = 10^{-6}$，预见步数 $N_L = 80$（对应 0.8 秒）。这些参数在 AIST HRP 系列机器人上验证过。对于不同大小的机器人，$N_L$ 应覆盖至少一个完整步态周期（约 0.6-1.0 秒），$Q_e/R$ 需要根据脚掌大小调整——脚掌小的机器人需要更积极的 ZMP 跟踪。

从 Preview 到 MPC¶

Preview 控制的局限性在于它是无约束的线性反馈。当 ZMP 需要严格限制在脚掌内时，需要显式约束优化——这就是 ZMP-MPC。

方法	控制量	约束	优点	局限
Preview LQR	CoM jerk	隐式跟踪 ZMP 参考	解析快、实现简洁	不方便显式处理多边形边界
ZMP-MPC	CoM jerk 或 ZMP	$A_pp\le b_p$	抗扰更强、可限幅	每周期求 QP
DCM 规划	ZMP、VRP、脚印	可加入步长约束	解释性强、响应快	仍依赖 LIPM 假设

把 Preview 的无约束 LQR 写成显式约束优化，就得到 ZMP-MPC 的标准形式。沿用 §92.4 的 cart-table 三阶状态 $x_k=[c_k,\dot c_k,\ddot c_k]^T$、控制量 $u_k=\dddot c_k$、输出 $p_k=Cx_k$，在长度为 $N$ 的预测窗口上求解：

\[ \begin{aligned} \min_{u_{0:N-1}}\quad & \sum_{k=1}^{N}Q_e(p_k-r_k)^2+\sum_{k=0}^{N-1}R\,u_k^2\\ \text{s.t.}\quad & x_{k+1}=Ax_k+Bu_k,\quad p_k=Cx_k,\\ & p_{min,k}\le p_k\le p_{max,k}. \end{aligned} \]

它和 Preview 控制只差一处，却带来本质区别：那条不等式约束 $p_{min,k}\le p_k\le p_{max,k}$。 Preview 控制把 ZMP 跟踪写进代价函数（软），ZMP 可以越过脚掌边界只是"被罚"；ZMP-MPC 把脚掌边界写成硬约束，优化器**绝不会**给出越界的 ZMP 参考。代价是从一个解析的线性反馈变成每个控制周期都要解一次 QP。

对比性思维：Preview 与 ZMP-MPC 不是"谁更好"，而是"软约束 vs 硬约束"的取舍。平地常规行走，ZMP 本就离边界远，Preview 的软处理足够且更快；一旦贴近边界（窄脚掌、强扰动、受限落脚区），软约束会偷偷越界导致不可实现的参考，此时必须上 ZMP-MPC 的硬约束。这与 §92.8 "加权 QP vs HQP"的软硬之争是同构的——只要边界会被触碰，软约束就不够。

如果不使用预见会怎样？ 一个纯反馈的 ZMP 控制器只能在 ZMP 偏离参考后做出反应。由于 LIPM 是不稳定的，纯反馈控制的响应总是"追着跑"。预见控制通过利用未来脚步信息，把控制从被动反馈变为主动规划，显著改善了相位切换时刻的 ZMP 裕度。实验表明，使用 $N_L=80$（对应 0.8 秒）的预见步数，ZMP 裕度可以从纯反馈的 1-2 cm 提升到 3-5 cm。

import numpy as np


def zmp_output_from_cart_table(x, z_com):
    """Cart-table 输出：x = [位置, 速度, 加速度]。"""
    # 中文注释：ZMP 由质心位置和加速度的线性组合给出。
    return x[0] - z_com / 9.81 * x[2]


def integrate_jerk_state(x, jerk, dt):
    """中文注释：用 jerk 积分更新 CoM 三阶状态。"""
    A = np.array([[1.0, dt, 0.5 * dt * dt],
                  [0.0, 1.0, dt],
                  [0.0, 0.0, 1.0]])
    B = np.array([dt**3 / 6.0, 0.5 * dt * dt, dt])
    return A @ x + B * jerk

练习 92.4.A：实现一个简化的 Preview 控制器。给定左右脚交替的 ZMP 参考轨迹（方波），使用 $N_L=50$ 步预见，绘制 CoM 轨迹和 ZMP 跟踪误差。

练习 92.4.B：在上述 Preview 控制器中加入 ZMP 约束 $|p - p_{ref}| \le 5$ cm，将问题改写为 QP。比较有约束和无约束情况下的 ZMP 裕度。

92.5 DCM、Capture Point 与 VRP ⭐⭐⭐⭐¶

隔离不稳定模态¶

回顾 92.2：LIPM 是一个二阶不稳定系统，特征值为 $\pm\omega_0$。直接控制 CoM 位置需要同时处理稳定模态和不稳定模态。DCM 的核心思想是用坐标变换把两个模态分离，让控制器只需要关注不稳定的那个。

定义 DCM（Divergent Component of Motion，运动的发散分量）： $$ \xi=x+\dot x/\omega_0. $$

这个定义不是凭空产生的。考虑 LIPM 的通解 $x(t) = c_1 e^{\omega_0 t} + c_2 e^{-\omega_0 t}$。代入 DCM 定义： $$ \xi = c_1 e^{\omega_0 t} + c_2 e^{-\omega_0 t} + \frac{c_1 \omega_0 e^{\omega_0 t} - c_2 \omega_0 e^{-\omega_0 t}}{\omega_0} = 2c_1 e^{\omega_0 t}. $$

DCM 恰好只包含发散模态 $c_1 e^{\omega_0 t}$，衰减模态被完美消除了。

对它求导并代入 LIPM： $$ \dot\xi=\dot x+\ddot x/\omega_0 =\dot x+\omega_0(x-p) =\omega_0(\xi-p). $$

同时有： $$ \dot x=-\omega_0(x-\xi). $$

于是二阶 LIPM 被拆成发散的 DCM 和收敛的 CoM 跟随。

本质洞察 DCM 变换的本质是线性系统的模态分解。它不是一种近似或简化，而是精确的坐标变换。它的价值在于把控制问题从"同时稳定两个模态"变成"只需要稳定发散模态"——因为一旦 DCM 被稳定，CoM 会自动收敛到 DCM。这种"分而治之"的思想在控制理论中非常普遍（类比：分离定理把状态估计和状态反馈解耦）。

Capture Point¶

若立即把脚放在 $p=\xi$，则 $\dot\xi=0$。 CoM 会指数收敛到这个点，因此 $\xi$ 就是 Capture Point。

物理直觉：想象你站在冰面上被推了一下。你的 CoM 开始移动，如果不迈步就会摔倒。Capture Point 就是你必须迈到的那个位置——迈到那里后，你可以停下来而不需要继续迈步。

扰动越大，CoM 速度越大，Capture Point 离当前脚越远。这给"是否必须迈步"一个直接判据：如果 Capture Point 在脚掌内，不需要迈步（原地调整 ZMP 即可）；如果 Capture Point 在脚掌外，必须迈步才能恢复平衡。

Capture Point 的来历：Capture Point 这个术语最早由 Pratt、Carff、Drakunov、Goswami 在 2006 年的论文（"Capture Point: A Step toward Humanoid Push Recovery", Humanoids 2006）提出。他们在 LIPM 上加了一个飞轮体（flywheel）来显式建模角动量，并给出了"机器人该往哪一步踩才能完全停下"的精确解。注意术语谱系：Capture Point 与 DCM 描述的是同一个量（$\xi=x+\dot x/\omega_0$），只是 Pratt 一系从"push recovery 该踩哪"的角度命名为 Capture Point，Englsberger 一系（2011, "Three-dimensional bipedal walking control using divergent component of motion", IROS 2011）从"分离发散模态"的角度命名为 DCM 并推广到三维与连续行走。本章按场景区分用词：讲落脚判据时叫 Capture Point，讲连续轨迹规划时叫 DCM。

N 步可捕获性：不止"要不要迈步"，还有"几步能停"¶

§92.5 到这里给的判据是二元的：Capture Point 在脚掌内就不迈步，在外就迈步。但真实问题更细——一脚迈不回来，两脚行不行？ 这正是 N 步可捕获性（N-step capturability）要回答的。这套框架由 Koolen、de Boer、Rebula、Goswami、Pratt 在 2012 年的两篇姊妹论文中系统建立（"Capturability-based analysis and control of legged locomotion, Part 1/Part 2", IJRR 2012），Part 1 给理论与三个简化步态模型，Part 2 在 M2V2 下肢人形上验证——实机能从单腿站立时高达 21 N·s 的前向/侧向推扰中靠迈步恢复。

定义：一个步态状态是 N 步可捕获的（N-step capturable），如果存在一个不超过 N 步的落脚序列，能让机器人最终完全停下（CoM 速度归零且 Capture Point 落在某支撑脚内）。于是：

可捕获级别	含义	对应控制动作
0 步可捕获	不迈步、只调 ZMP 就能停	Capture Point 已在当前脚掌内
1 步可捕获	再迈一步就能停	解析 DCM 落脚重规划（§92.6）一步到位
N 步可捕获	需要连续 N 步逐步耗散动量	多步落脚序列，每步把 Capture Point 拉近
不可捕获	任何步数都停不下来	只能软着陆/摔倒保护

为什么是"区域"而不是"点"。 对纯 LIPM（点质量、无角动量、瞬时迈步），0 步可捕获区域恰好是脚掌本身，1 步可捕获区域是"脚掌按 $e^{\omega_0 T}$ 因子外扩并叠加最大步长"得到的更大区域。每多允许一步，可捕获区域就向外扩张一圈。Koolen 等人的关键贡献是证明：在简化模型下这些区域有**闭式或可快速计算**的边界，因此可以在线判断"当前扰动属于第几级"。

加飞轮的意义。 Pratt 2006 的飞轮体和 §92.3 的角动量修正是同一回事的两种用途：飞轮（躯干转动、摆臂）能在不迈步的前提下临时把等效 Capture Point 拉回脚内，等价于把 0 步可捕获区域**暂时撑大**。这就是人在窄梁上被推时会猛地转动手臂的物理原因——用角动量买时间，避免被迫迈步。

\[ \xi_{eq}=\xi-\frac{\Delta k_G}{m\,\omega_0\,z_c}\quad(\text{飞轮提供的角动量}\Delta k_G\text{把等效捕获点拉近}). \]

本质洞察 可捕获性把"平衡"从一个布尔判断升级为一个**分级的、可计算的余量**。它的工程价值在于：控制器不必等到"已经要摔了"才迈步，而是可以持续监测"我现在是几步可捕获"，在余量从 1 步退化到 2 步时就提前预警或调整步态。这与 ZMP 裕度、DCM 误差一样，是又一个可以塞进监控面板、也可以塞进 RL 奖励的物理量。

陷阱警告 ⚠️（概念误区） N 步可捕获性的精确区域是在**简化模型**（LIPM 或 LIPM+飞轮）下推导的。真实机器人有摆腿质量、有限摆动时间、力矩饱和和双支撑过渡，实际可捕获区域比理论值小。把理论 1 步可捕获区域直接当作实机的迈步边界，会高估恢复能力，在边界附近反而摔倒。工程上应留 20-30% 的安全收缩。

练习 92.5.C：在纯 LIPM（$z_c=0.8$ m，最大步长 $L_{max}=0.5$ m）下，推导 1 步可捕获区域沿前进方向的边界。给定 $T_{step}=0.4$ s，问 Capture Point 在距当前脚多远以内时仍是 1 步可捕获？超出后至少需要几步？

3D DCM 与 VRP¶

将 DCM 推广到三维： $$ \boldsymbol\xi=\mathbf{x}+\dot{\mathbf{x}}/\omega, \dot{\boldsymbol\xi}=\omega(\boldsymbol\xi-\mathbf r_{VRP}). $$

VRP（Virtual Repellent Point）可以看作压力点向上平移后的虚拟排斥点： $$ \mathbf{r}{VRP} = \mathbf{r} $$} - \frac{\ddot{\mathbf{r}}_{CoM} + \mathbf{g}}{\omega^2

它把三维高度控制和水平 DCM 控制放进同一个公式。VRP 的引入解决了变高度行走（如爬楼梯）中 DCM 规划的统一性问题。

DCM 反馈¶

\[ p=p^d+(1+k_\xi/\omega_0)(\xi-\xi^d), \dot e_\xi=-k_\xi e_\xi. \]

数学上只要 $k_\xi>0$ 就能收敛。工程上还要把 $p$ 投影回脚掌，否则会得到不可实现的 ZMP。

增益选择：$k_\xi$ 过大会导致 ZMP 频繁贴边（控制太激进），$k_\xi$ 过小会导致 DCM 响应慢（扰动恢复不及时）。实践中 $k_\xi \in [1.0, 3.0] \cdot \omega_0$ 是常见范围。一个经验法则是 $k_\xi \approx 2\omega_0$，这在 Capture Point 可达和 ZMP 裕度之间取得了较好的折中。

import numpy as np


def dcm_feedback(x, dx, xi_ref, zmp_ref, z_com, gain):
    """中文注释：根据 DCM 误差计算期望 ZMP。"""
    omega = np.sqrt(9.81 / z_com)
    xi = x + dx / omega
    # 中文注释：反馈律把 DCM 误差线性映射到 ZMP 调整量。
    zmp = zmp_ref + (1.0 + gain / omega) * (xi - xi_ref)
    return xi, zmp


def project_to_foot(zmp, foot_center, half_size):
    """中文注释：把一维 ZMP 限制在脚掌范围内。"""
    return float(np.clip(zmp, foot_center - half_size, foot_center + half_size))

陷阱警告 ⚠️ DCM 公式中的 $\omega_0 = \sqrt{g/z_c}$ 依赖于质心高度 $z_c$。如果规划使用的 $z_c$ 和实际质心高度不一致（例如机器人在行走中有垂直振荡），DCM 误差会系统性偏移。调试时应检查实际 CoM 高度与规划高度的差异。

练习 92.5.A：给定初始状态 $x=0$, $\dot x = 0.3$ m/s, $z_c=0.8$ m，计算 Capture Point 位置。如果脚掌中心在 $x=0$，半宽为 6 cm，判断是否需要迈步。

练习 92.5.B：实现 DCM 反馈控制器。在支撑脚不变的情况下，输入一个 20 N 的水平推力扰动，绘制 DCM、CoM 和 ZMP 的时间演化。比较 $k_\xi = \omega_0$ 和 $k_\xi = 3\omega_0$ 的恢复性能。

92.6 双足落脚规划：离散脚印与连续平衡的接口 ⭐⭐⭐¶

落脚点是控制输入¶

LIPM/DCM 控制的是当前支撑内的 ZMP 或 VRP。当 DCM 已经跑出当前脚掌可控范围，真正有效的控制输入就是下一步脚印。这就是双足落脚规划需要作为独立层存在的原因。

落脚规划在控制层级中位于 DCM 控制之上、任务规划之下。它的核心输出是下一步的脚印位置和时序，这些信息决定了 DCM 参考轨迹的边界条件。

方式	决策变量	优点	适用场景
解析 DCM 重规划	下一步脚印	极快、解释清楚	平地推扰恢复
混合整数规划	区域选择 + 脚印	能避障和选落脚区域	复杂地形
运动学-动力学规划	脚印、CoM、全身关键帧	表达能力强	离线或低频
学习式接触规划	策略输出接触目标	可吸收数据经验	复杂交互任务

可达性约束¶

约束	形式	意义
步长	$\\|x_{next}-x_{stance}\\|\le L_{max}$	髋关节可达
步宽	$W_{min}\le \\|y_{next}-y_{stance}\\|\le W_{max}$	避免交叉腿
转角	$\\|\psi_{next}-\psi_{stance}\\|\le\psi_{max}$	足底朝向连续
地形	$A_{terrain}p\le b_{terrain}$	避开不可落脚区域
时间	$T_{swing}\ge T_{min}$	摆腿轨迹可执行

步宽下界的重要性：$W_{min}$ 防止双脚交叉，这不仅是运动学约束，更是碰撞安全约束。人形的腿部在摆动过程中如果过度内收，会发生膝盖碰撞。典型的 $W_{min}$ 为 10-15 cm。

落脚与 WBC 的接口¶

落脚规划输出的不是关节角，而是摆动脚末端轨迹和 touchdown 时刻。 WBC 负责在不破坏支撑脚和 CoM 任务的前提下，让摆动脚跟踪该轨迹。如果落脚点超出可达范围，WBC 会表现为摆脚误差大、髋膝关节速度高或 QP 无解。

落脚冲击的处理：touchdown 瞬间的速度不连续会产生冲击力，可能导致机器人弹起或滑动。工程上通常使用五次多项式生成摆动脚轨迹，保证 touchdown 时足端速度和加速度都接近零。更激进的方案是使用接触力渐入（force ramp），在 touchdown 前 30-50 ms 逐步建立接触力。

解析 DCM 落脚重规划¶

当 DCM 误差超出当前脚掌可修正范围时，最快的响应是计算下一步脚印使 DCM 在摆动相结束时归零。

设当前 DCM 为 $\xi_{now}$，期望在 $T_{step}$ 后 DCM 收敛到新脚印位置 $p_{next}$： $$ \xi_{end} = e^{\omega_0 T_{step}} (\xi_{now} - p_{cur}) + p_{cur} $$

要求 $\xi_{end} = p_{next}$，解出： $$ p_{next} = p_{cur} + e^{\omega_0 T_{step}} (\xi_{now} - p_{cur}) $$

这个解析公式可以在微秒级时间内计算，因此可以在每个控制周期都重新规划。它的物理含义是：如果当前 DCM 偏离支撑脚越远，下一步需要迈得越远。指数因子 $e^{\omega_0 T_{step}}$ 放大了偏差——等的时间越长，偏差被放大得越多。

典型数值：$\omega_0 \approx 3.5$ rad/s，$T_{step} = 0.4$ s，则 $e^{\omega_0 T_{step}} \approx 4.1$。这意味着当前 1 cm 的 DCM 偏差在 0.4 秒后需要 4.1 cm 的脚印调整来补偿。

陷阱警告 ⚠️ 解析落脚公式假设了恒定的 $\omega_0$（恒定质心高度）和瞬时切换（忽略双支撑过渡）。在实际双支撑过渡期间，支撑力在两脚之间逐步转移，$\omega_0$ 的等效值会变化。如果直接用单支撑的 $\omega_0$ 计算落脚点，会有 2-5 cm 的系统误差。高精度应用需要在双支撑期间使用修正的 DCM 公式。

双支撑过渡：被瞬时切换假设掩盖的难点¶

上面的解析公式把脚印切换当成瞬时的——支撑脚从左脚"啪"地变成右脚。但真实人形双脚都在地上有一段双支撑相（double support, DS），典型占步态周期的 20-30%。这一段不是可有可无的过渡，它恰恰是 §92.13 故障表"双支撑过渡时力矩跳变"的发生地。

问题出在哪。 单支撑相里 ZMP 只能在当前支撑脚的脚掌内；双支撑相里支撑多边形是**两脚的外包络**，ZMP 可以在两脚之间。如果规划在相位切换瞬间让 ZMP 参考从左脚中心直接跳到右脚中心，那么由 LIPM 关系 $\ddot x=\omega_0^2(x-p_{zmp})$，CoM 加速度会瞬间阶跃——而加速度阶跃意味着接触力阶跃，意味着关节力矩跳变。实机上表现为过渡时刻"咯噔"一下，重则激起结构振动。

ZMP 参考的平滑转移。 解法是让 ZMP 参考在双支撑相内**连续地**从前支撑脚移到后支撑脚，而不是阶跃。常用一段插值（线性或更平滑的多项式）： $$ p_{zmp}^{ref}(t)=(1-s(t))\,p_{foot}^{prev}+s(t)\,p_{foot}^{next},\qquad s:0\to1 \text{在 DS 相内}, $$ 其中 $s(t)$ 是从 0 平滑增到 1 的过渡函数。$s$ 用三次/五次多项式能进一步保证 ZMP 速度（乃至加速度）连续，从而力矩连续。

接触力的渐入渐出。 与 ZMP 转移配套，两脚的法向力也要平滑交接——前支撑脚的力 $f_n^{prev}$ 从满载渐降到零（force ramp-out），后支撑脚 $f_n^{next}$ 从零渐升到满载（ramp-in），二者之和始终约等于 $mg$。WBC 层通过给两个接触任务设时变权重或时变力参考实现这一点。这正是 §92.6"落脚冲击的处理"提到的 force ramp 在双支撑相的体现。

理论-工程桥接 为什么单支撑的解析 DCM 公式在双支撑期会有 2-5 cm 误差？因为双支撑期间"等效支撑点"在两脚之间移动，等效 $\omega_0$ 和单支撑不同。工程上有两种务实处理：(1) 把双支撑期当作 ZMP 在两脚间移动的已知参考，用分段 DCM 反向积分（§92.11 的变参数思路）算 DCM 参考；(2) 简化系统直接缩短 DS 相、用单支撑公式近似——代价是上面那 2-5 cm 误差，对慢速行走可接受。

本质洞察 双支撑相的存在，本质上是为了给"接触力交接"和"ZMP 转移"留出时间。它越长，过渡越平滑、越鲁棒，但行走越慢、越不像人；它越短，越接近瞬时切换的理想假设，但对力矩平滑和状态估计的要求越高。 DS 相时长是一个直接体现"鲁棒性 vs 动态性"权衡的旋钮——这与本章反复出现的工程折中（圆锥 vs 金字塔、HQP 层数、增益高低）是同一类决策。

陷阱警告 ⚠️（概念误区）不要把双支撑相理解成"两脚都是硬支撑、随便分配力"。双支撑期间两脚接触力**不是**可以任意分配的——它受各自摩擦锥、CoP 边界约束，且总和必须实现质心动力学。把它当作无约束力分配，会在过渡期算出某只脚法向力为负（拉地面）的非物理解。

练习 92.6.A：给定当前支撑脚位置和 Capture Point，计算满足可达性约束的下一步脚印。讨论当 Capture Point 超出最大步长时的应对策略。

练习 92.6.B：实现解析 DCM 落脚重规划。输入一个突发的 30 N 推力，比较有无落脚重规划时的 DCM 误差和 ZMP 裕度。

92.7 质心动量矩阵 CMM 与 Pinocchio ccrba ⭐⭐⭐⭐¶

从点质量回到全身¶

LIPM 忽略了全身角动量，但人形的双臂和躯干正是通过角动量参与平衡。 CMM 用一个矩阵把全身速度映射到质心 6D 动量： $$ h_G=\begin{bmatrix}l_G\k_G\end{bmatrix}=A_G(q)v. $$

这里 $l_G \in \mathbb{R}^3$ 是线动量，$k_G \in \mathbb{R}^3$ 是角动量，$v \in \mathbb{R}^{n_v}$ 是广义速度。

为什么需要矩阵形式？ 直接从每个连杆的速度计算动量再求和是可以的，但这样做的复杂度是 $O(n^2)$。CMM 把这个计算组织为一个矩阵乘法 $h_G = A_G v$，利用 Pinocchio 的递推算法可以在 $O(n)$ 时间内完成。更重要的是，矩阵形式使得动量任务可以直接插入 QP。

逐连杆推导¶

每个连杆 $i$ 的空间速度为 $V_i = J_i(q)v$，其 6D 动量为 $h_i = I_i V_i$（$I_i$ 是空间惯性矩阵）。将每个连杆的动量变换到质心坐标系 $G$：

\[ V_i=J_i(q)v, h_i=I_iV_i, {}^Gh_i={}^GX_i^*I_iJ_i(q)v. \]

其中 ${}^GX_i^*$ 是从连杆 $i$ 到质心 $G$ 的力变换矩阵。对所有连杆求和： $$ A_G(q)=\sum_i{}^GX_i^*I_iJ_i(q). $$

这就是 Pinocchio ccrba 背后的核心等式。

CMM 的性质： - $A_G$ 是 $6 \times n_v$ 矩阵，其中前 3 行对应线动量，后 3 行对应角动量。 - 线动量部分满足 $l_G = m \dot{r}_{CoM}$，因此 CMM 的前 3 行与 CoM Jacobian 直接相关。 - CMM 的秩最多为 6。对于人形（$n_v \approx 30+$），CMM 是极度欠定的，这意味着存在大量不影响质心动量的关节运动——这正是零空间运动（如在不改变 CoM 动量的前提下调整手臂姿态）。

角动量任务推导¶

动量任务是 Momentum-Based WBC 的核心。从 CMM 定义出发： $$ h_G = A_G(q) v $$

对时间求导： $$ \dot h_G = A_G(q) \dot v + \dot A_G(q, v) v $$

其中 $\dot A_G$ 是 CMM 关于构型的时间变化率，可以由 Pinocchio 的 dccrba 函数计算。

重排为任务加速度形式： $$ A_G \dot v = \dot h_G^{ref} - \dot A_G v $$

如果只调角动量，就取后三行： $$ A_G^{ang} \dot v = \dot k_G^{ref} - \dot A_G^{ang} v $$

角动量参考的选择： - 零角动量（$\dot k_G^{ref} = -K_k k_G$）：让角动量收敛到零，适合站立和缓步行走。 - 恒定角动量（$\dot k_G^{ref} = 0$）：保持当前角动量不变，适合惯性运动阶段。 - 前馈角动量（$\dot k_G^{ref} = \dot k_G^{plan}$）：跟踪规划给出的角动量曲线，适合需要主动利用角动量的高动态动作。

如果把角动量任务放在高优先级，WBC 会主动选择躯干、臂和腿的协同加速度来满足动量目标。

角动量任务的完整推导¶

从 CMM 的时间导数出发，推导角动量任务在 QP 中的具体形式：

Step 1：动量导数展开

\[ \dot h_G = \frac{d}{dt}(A_G v) = A_G \dot v + \dot A_G v \]

Step 2：分离线/角分量

\[ \begin{bmatrix} \dot l_G \\ \dot k_G \end{bmatrix} = \begin{bmatrix} A_G^{lin} \\ A_G^{ang} \end{bmatrix} \dot v + \begin{bmatrix} \dot A_G^{lin} \\ \dot A_G^{ang} \end{bmatrix} v \]

Step 3：构造角动量 PD 任务

设角动量参考为零（最常见情况）： $$ \dot k_G^{ref} = -K_k k_G = -K_k A_G^{ang} v $$

代入得任务方程： $$ A_G^{ang} \dot v = -K_k A_G^{ang} v - \dot A_G^{ang} v $$

整理为 QP 任务标准形式 $J_{task} \dot v = b_{task}$： $$ J_{task} = A_G^{ang}, \quad b_{task} = -(K_k + \dot A_G^{ang} / A_G^{ang}) \cdot v $$

Step 4：Pinocchio 实现

Pinocchio 的 dccrba 函数计算 $\dot A_G$。结合 ccrba 的输出 $A_G$，可以直接组装上述任务。

角动量增益 $K_k$ 的选择： - $K_k$ 过大：角动量被快速抑制，但会导致躯干和手臂出现高频振荡（因为需要大的关节加速度来快速改变角动量）。 - $K_k$ 过小：角动量变化缓慢，扰动后需要更长时间恢复，DCM 可能在恢复期间发散。 - 经验范围：$K_k \in [5, 20]$，取决于机器人的惯性特性和执行器能力。

反事实推理 如果不加角动量任务会怎样？在平地缓慢行走中，角动量变化很小，不加角动量任务几乎没有影响。但当机器人受到 20 N 水平推力时，角动量任务的有无会产生显著差异： - 有角动量任务：躯干快速恢复竖直，手臂做出补偿动作吸收角动量 - 无角动量任务：角动量在躯干中累积，机器人持续倾斜直到 ZMP 出界这就是为什么 IHMC 在 DRC 中把动量目标放在最高优先级——它是抗扰恢复的关键。

import pinocchio as pin
import numpy as np


def print_centroidal_map(urdf_path):
    """中文注释：打印人形质心动量矩阵的尺寸与秩。"""
    model = pin.buildModelFromUrdf(urdf_path, pin.JointModelFreeFlyer())
    data = model.createData()
    q = pin.neutral(model)
    v = np.zeros(model.nv)
    # 中文注释：ccrba 计算质心动量矩阵 A_G，结果存在 data.Ag 中。
    pin.ccrba(model, data, q, v)
    Ag = data.Ag.copy()
    print('nq =', model.nq, 'nv =', model.nv)
    print('Ag shape =', Ag.shape)
    print('rank =', np.linalg.matrix_rank(Ag))
    # 中文注释：角动量块的范数反映了关节运动对角动量的影响大小。
    print('angular block norm =', np.linalg.norm(Ag[3:, :]))
    return Ag

陷阱警告 ⚠️ Pinocchio 的 ccrba 返回的 CMM 是在质心坐标系中表达的。如果你在世界坐标系中定义角动量参考，需要先变换到质心坐标系。两个坐标系的平移部分不影响角动量（角动量与参考点无关，因为它已经是关于质心计算的），但旋转部分需要一致。

练习 92.7.A：用 G1 或 Talos 的 URDF 加载模型，打印 $A_G$ 的形状和秩。改变手臂构型（手臂上举 vs 下垂），观察角动量块的列向量如何变化。

练习 92.7.B：计算 CMM 的零空间基 $N_A = I - A_G^+ A_G$。在零空间中选择一个方向，施加关节速度增量，验证质心动量不变。

92.8 TSID / HQP：全身逆动力学的 QP 形态 ⭐⭐⭐⭐¶

决策变量¶

回顾足式/90：WBC 的核心是把全身运动分解为多个任务，用 QP 求解满足物理约束的最优关节加速度和接触力。

完整变量可以写成 $x=[\dot v,\lambda,\tau]$。 TSID 常用 AccForce 形式，只求 $x=[\dot v,\lambda]$，再反推关节力矩： $$ \tau=S(M\dot v+h-J_c^T\lambda). $$

为什么可以消去 $\tau$？ 因为动力学方程 $M\dot v + h = S^T\tau + J_c^T\lambda$ 中，$\tau$ 只出现在驱动关节对应的行。一旦知道了 $\dot v$ 和 $\lambda$，$\tau$ 可以唯一确定。消去 $\tau$ 把决策变量从 $n_v + 3n_c + n_a$ 减少到 $n_v + 3n_c$，QP 规模更小，求解更快。

硬约束¶

\[ S_f(M\dot v+h-J_c^T\lambda)=0, J_c\dot v+\dot J_cv=0. \]

第一条是浮动基不可驱动约束：$S_f$ 选出前 6 行（浮动基座），这些行没有驱动力矩，因此动力学方程的左边必须等于接触力提供的右边。

第二条是支撑脚不滑约束：接触点的加速度为零。注意 $\dot J_c v$ 项不能遗漏——它是速度依赖的偏置加速度，物理上对应着科氏和离心效应对接触点的影响。

接触力的单边约束：摩擦锥与 CoP¶

前面的等式约束保证了"脚不滑、浮动基自洽"，但还漏了一类至关重要的**不等式约束**——接触力本身的物理可行性。地面只能推、不能拉；切向力不能超过摩擦极限；脚掌只能在自己的边界内提供压力中心。这三条合起来，就是 §92.8/§92.9/§92.11 里反复出现却一直没展开的 $C_{fric}\lambda\le0$。

为什么需要它。 如果不约束接触力，QP 完全可能给出一组"数学上满足动力学、物理上不可能"的力：法向力为负（脚在拉地面）、切向力远超摩擦能提供的（脚会滑）、或等效压力中心跑到脚掌外（脚会翻边）。这些解在仿真里如果接触模型是软的会表现为穿透或弹跳，在实机上则直接是打滑和翻倒。

法向单边性。 设接触法向为 $\hat n$，则法向力非负： $$ f_n=\boldsymbol\lambda\cdot\hat n\ge0. $$ 这是最基本的一条——脚不能粘在地上。

库仑摩擦锥与其线性化。 切向力的合力不能超过 $\mu f_n$： $$ \sqrt{f_{t1}^2+f_{t2}^2}\le\mu f_n. $$ 这是一个二阶锥约束。直接放进 QP 需要 SOCP 求解器，比 QP 慢。工程上常把圆锥**线性化为多面锥（pyramid）**——用 $K$ 条棱（典型 4 条）的内接金字塔近似： $$ |f_{t1}|\le\frac{\mu}{\sqrt2}f_n,\qquad |f_{t2}|\le\frac{\mu}{\sqrt2}f_n. $$ 这样摩擦约束变成纯线性不等式，可以塞进标准 QP。代价是内接金字塔比真实圆锥小（保守），对角方向的可用摩擦被低估约 30%。

对比：圆锥 vs 金字塔 用真实二阶锥（精确，需 SOCP，慢）还是线性化金字塔（保守，纯 QP，快）？这正是 R6B 式的工程权衡——多数实时 WBC 选金字塔换速度，对摩擦极敏感的动作（如冰面、急转）才上 SOCP。金字塔的棱数 $K$ 越多越接近圆锥，但每条棱都是一行约束，$K$ 过大拖慢 QP。

CoP 在脚掌内（6D 接触的额外约束）。 点接触只有 3 维力，但脚掌是**面接触**，能额外提供绕水平轴的力矩——代价是等效压力中心（CoP）必须落在脚掌矩形内。对 6D 接触 wrench $[\boldsymbol\lambda;\boldsymbol\tau_c]$，CoP 约束等价于： $$ -\frac{l_x}{2}\le-\frac{\tau_{c,y}}{f_n}\le\frac{l_x}{2},\qquad -\frac{l_y}{2}\le\frac{\tau_{c,x}}{f_n}\le\frac{l_y}{2}, $$ 其中 $l_x,l_y$ 是脚掌长宽。这其实就是 §92.3 ZMP 落在脚掌内的约束，只不过现在写在单只脚的接触 wrench 上——单脚 CoP 约束是 ZMP 约束在 WBC 层的等价物。TSID 的 Contact6d 内部正是把法向单边、摩擦金字塔和 CoP 边界打包成一组线性不等式。

本质洞察 等式约束（不滑、浮动基）决定"解空间是哪个仿射子空间"，不等式约束（摩擦锥、CoP、力矩限幅）决定"这个子空间里哪一块是物理可行的"。 QP 无解的绝大多数情况，不是等式约束矛盾，而是**任务要求把接触力推出了这个可行块**——比如 CoM 任务要的横向加速度超过了摩擦能提供的。所以排查 QP 无解时，先打印各接触力是否贴着摩擦锥/CoP 边界，往往比改任务权重更快定位。

陷阱警告 ⚠️（概念误区）把"接触不滑等式约束 $J_c\dot v+\dot J_cv=0$"当成"脚一定不会滑"是错的。等式约束只是**假设**脚不滑（接触点加速度为零），真正保证不滑的是摩擦锥不等式。如果摩擦锥约束缺失或 $\mu$ 设得过大，QP 会算出一个"假设不滑、实际超出摩擦"的力，实机上脚照样滑——这正是 §92.13 故障表"支撑脚滑动"的另一类根因（除了漏掉 $\dot J_c v$）。

任务加速度¶

每个任务定义一个参考加速度： $$ J_y\dot v+\dot J_yv=\ddot y^d+K_d(\dot y^d-\dot y)+K_p(y^d-y). $$

右边是 PD 反馈加前馈的参考加速度。$K_p$ 和 $K_d$ 的选择决定了任务跟踪的带宽。临界阻尼条件 $K_d = 2\sqrt{K_p}$ 给出无振荡的最快收敛。

增益选择的工程考虑：

对于 CoM 任务，典型增益为 $K_p^{com} = 50$，$K_d^{com} = 14$（$2\sqrt{50} \approx 14.1$）。对应的闭环带宽约为 $\sqrt{K_p}/(2\pi) \approx 1.1$ Hz。这意味着 CoM 可以在约 0.5 秒内跟踪到新的参考位置。

对于摆动脚任务，增益通常更高：$K_p^{swing} = 200$，$K_d^{swing} = 28$。因为摆动脚需要在约 0.3 秒的摆动相内完成轨迹跟踪，带宽需要更高。

对于关节姿态任务（最低优先级），$K_p^{posture} = 10$，$K_d^{posture} = 6$。低增益保证它不会干扰高优先级任务。

如果增益设错了会怎样？ - $K_p$ 过大：加速度需求过大，可能与力矩限幅冲突，导致 QP 无解或力矩饱和。 - $K_p$ 过小：任务跟踪太慢，CoM 在步态切换时无法及时到位，ZMP 裕度减小。 - $K_d$ 过小（欠阻尼）：任务响应出现超调和振荡，特别是在接触切换时可能激励振动。 - $K_d$ 过大（过阻尼）：任务响应缓慢，等效于降低了 $K_p$ 的效果。

任务	Jacobian	参考	优先级
支撑脚	$J_c$	零加速度	硬约束
CoM	$J_{com}$	LIPM/DCM 参考	高
角动量	$A_G^{ang}$	$\dot k_G^d$	高
摆动脚	$J_{swing}$	落脚轨迹	中高
双臂末端	$J_{ee}$	操作位姿	中
关节姿态	$I$	默认姿态	低

加权 QP 与 HQP¶

加权 QP 把所有软任务放入一个目标函数： $$ \min_{\dot v, \lambda} \sum_i w_i |J_i \dot v + \dot J_i v - \ddot y_i^d|^2 $$

HQP 按优先级逐层求解，并固定上层最优残差。人形中支撑脚、浮动基动力学和 CoM 不能被手臂任务污染，因此 HQP 的解释性更强。

HQP 为什么能严格保护优先级：零空间投影¶

前置自测第 5 题问的就是这个：HQP 相比加权 QP 的核心优势是"严格任务优先级"。这里把"严格"二字的数学含义讲透——它靠的是**零空间投影（null-space projection）**。

先看无约束的等式任务情形（不等式约束的处理思路相同，但需要把活跃集逐层冻结，机制更复杂）。第一优先级任务要解 $J_1\dot v=b_1$。一般它欠定（任务维度小于 $n_v$），最小范数解为： $$ \dot v_1=J_1^{+}b_1, $$ 其中 $J_1^{+}$ 是伪逆。它的零空间投影矩阵是： $$ N_1=I-J_1^{+}J_1. $$ $N_1$ 的作用是**过滤器**：任何形如 $N_1 z$ 的关节加速度，乘上 $J_1$ 都为零（因为 $J_1N_1=J_1-J_1J_1^{+}J_1=0$），即不会改变第一任务的完成情况。

第二优先级任务只能在这个零空间里活动。把 $\dot v=\dot v_1+N_1 z_2$ 代入 $J_2\dot v=b_2$： $$ J_2(\dot v_1+N_1 z_2)=b_2 \Rightarrow (J_2N_1)z_2=b_2-J_2\dot v_1. $$ 解出 $z_2=(J_2N_1)^{+}(b_2-J_2\dot v_1)$，于是： $$ \dot v_2=\dot v_1+N_1(J_2N_1)^{+}(b_2-J_2\dot v_1). $$ 继续往下，第 $k$ 层在前 $k-1$ 层的联合零空间 $N_{1:k-1}$ 里求解。这就是"字典序最小化（lexicographic minimization）"的递推实现：先把高优先级做到最优，再在不动它一分一毫的前提下尽量做低优先级。

本质洞察 加权 QP 和 HQP 的差别，本质是"软约束"与"硬投影"的差别。加权 QP 把所有任务的误差加权求和——高权重只是让某项误差"更贵"，但只要把它换成另一项更便宜的改善能让总和下降，优化器就会牺牲它。这就是优先级泄漏的来源。 HQP 把高优先级任务的最优值**当作不可谈判的约束**冻结进下一层。低优先级任务再重要，也只能在不改变上层结果的零空间里腾挪。这是"严格"二字的全部含义。

为什么用动力学一致伪逆。 在浮动基系统里，直接用普通伪逆 $J^{+}$ 做投影会引入一个隐蔽问题：投影后的关节加速度可能要求浮动基座产生它根本无法主动产生的力（浮动基座那 6 行没有驱动）。正确做法是用**惯性加权伪逆** $J^{+}_M=M^{-1}J^T(JM^{-1}J^T)^{-1}$ 替代 $J^{+}$，对应的零空间投影 $N=I-J^{+}_MJ$ 才是动力学一致的——它保证零空间运动不需要浮动基出力。WBIC（§92.9.5）第一步的 $\bar J_{i|pre}$ 正是这种动力学一致伪逆。

陷阱警告 ⚠️（编程陷阱）实现零空间投影时直接对 $J_k N_{1:k-1}$ 调用 numpy.linalg.pinv 看似可行，但当某层任务和上层冲突（被零空间挤到几乎不可行）时，$J_kN_{1:k-1}$ 会奇异，朴素伪逆给出爆炸性的大加速度。工程实现必须用**阻尼伪逆**（damped least squares）$J^T(JJ^T+\lambda^2 I)^{-1}$，$\lambda$ 是小阻尼系数（典型 $10^{-3}$）。这是 §92.13 故障表里"QP 偶发无解 / 关节速度突然飙高"的常见根因之一。

加权 QP 的隐患：假设 CoM 任务权重 $w_{com}=100$，手臂任务权重 $w_{arm}=1$。看起来 CoM 优先级远高于手臂。但如果手臂任务的 Jacobian 很大（手臂离质心远），加权残差可能仍然很大，导致优化器牺牲少量 CoM 精度来大幅改善手臂跟踪。在极端情况下（如手臂抓着重物），这种"优先级泄漏"可以导致 ZMP 出界。HQP 通过字典序最小化彻底消除了这个问题。

陷阱警告 ⚠️ 把双臂末端跟踪权重设得和 CoM 一样高，会让控制器用平衡裕度换末端精度。仿真中表现为手很准、身体越来越倾斜，最后单支撑时 ZMP 出界。正确做法是先让支撑与 CoM 稳，再逐步提高操作任务权重。

陷阱警告 ⚠️ HQP 的层数不宜过多。每增加一层，QP 需要额外求解一次（固定上层最优值后再优化下层）。对于 4-5 层的 HQP，总求解时间可能是单层加权 QP 的 3-4 倍。实践中 2-3 层通常就足够了：第一层放硬约束，第二层放平衡任务（CoM + 角动量），第三层放其余所有软任务。

import numpy as np


def torque_from_acc_force(M, h, Jc, dv, lam, S):
    """中文注释：AccForce 解算后反推关节力矩。"""
    return S @ (M @ dv + h - Jc.T @ lam)


def contact_acc_residual(Jc, dv, dJv):
    """中文注释：支撑脚不滑残差，调试时应接近零。"""
    return Jc @ dv + dJv

练习 92.8.A：手推一个 2D 简化版 TSID。假设一个 3 关节平面机器人，浮动基座 + 2 个驱动关节 + 1 个接触点。写出动力学约束、接触约束和 CoM 任务的 QP 矩阵形式。

练习 92.8.B：在加权 QP 中设 $w_{com}=100$, $w_{arm}=1$，构造一个手臂任务使 CoM 任务精度下降超过 10%。这说明了什么关于加权 QP 的局限性？

92.9 IHMC Atlas Momentum-Based WBC 架构 ⭐⭐⭐⭐¶

为什么 Atlas 是人形 WBC 的标杆¶

IHMC（Institute for Human and Machine Cognition）团队在 DARPA Robotics Challenge（DRC）中使用 Atlas 机器人取得了显著成绩。他们在 2016 年发表的论文（Koolen, Bertrand et al., "Design of a Momentum-Based Control Framework and Application to the Humanoid Robot Atlas", IJHR 2016）定义了 Momentum-Based WBC 的标准架构。

这篇论文的核心贡献不是发明了新的数学工具，而是把 CMM、QP 和动力学约束组织成一个可工程化的框架，并在 Atlas 这样的真实大型人形机器人上验证了其有效性。

架构总览¶

IHMC 的框架将控制分为三个模块：

模块	功能	输入	输出
行为层	设置任务栈	任务命令（行走/抓取/爬梯）	运动任务 + 接触信息 + 动量目标
QP 核心	求解最优关节加速度和接触力	运动任务 + 约束	$\dot v^$, $\lambda^$
RNEA 反推	从加速度和接触力计算关节力矩	$\dot v^$, $\lambda^$, $q$, $v$	$\tau$

核心设计选择：IHMC 把动量变化率（而不是 CoM 位置）作为最高优先级的优化目标。这是因为： 1. 动量直接关联到接触力通过牛顿-欧拉方程：$\dot h_G = \sum_i f_i^{ext}$。控制动量等价于控制接触力的合效果。 2. 角动量在双足平衡中至关重要。CoM 位置任务只能控制线动量，而角动量需要单独的任务。 3. 动量目标的 QP 尺寸很小（只有 6 维），而 CoM 位置任务加角动量任务需要分别定义。

QP 的具体形式¶

IHMC 的 QP 核心求解以下问题：

\[ \min_{\dot v, \rho} \|\dot h_G^{ref} - A_G \dot v - \dot A_G v\|^2_{W_h} + \sum_i w_i \|J_i \dot v + \dot J_i v - \ddot y_i^d\|^2 + w_\rho \|\rho\|^2 \]

约束条件： $$ \begin{aligned} & S_f(M\dot v + h - J_c^T \lambda) = 0 & \text{（浮动基动力学）} \ & J_c \dot v = -\dot J_c v & \text{（接触不滑）} \ & C_{fric} \lambda \le 0 & \text{（摩擦锥）} \ & \tau_{min} \le S(M\dot v + h - J_c^T\lambda) \le \tau_{max} & \text{（力矩限幅）} \end{aligned} $$

其中 $\rho$ 是接触力的正则化变量，$C_{fric}$ 是线性化摩擦锥约束矩阵。

IHMC 的关键创新——小 QP：通过利用质心动量的性质，IHMC 把 QP 的规模从 $O(n_v + 3n_c)$ 缩小到 $O(6 + n_{motion})$（其中 $n_{motion}$ 是运动任务的总维度）。具体做法是先用动力学方程消去 $\dot v$ 中与动量无关的分量，只保留影响动量的部分。这使得 QP 求解时间从典型的 1-2 ms 降低到 0.2-0.5 ms，为其他计算留出了更多预算。

Atlas 在 DRC 中的实际表现¶

DRC Trials（2013年12月）到 DRC Finals（2015年6月）期间，Atlas 的 WBC 经历了显著演进：

版本	时间	特点	成绩
DRC Trials	2013.12	基础 QP + 逆运动学	20/32 分
中间版本	2014	引入动量目标、改进接触切换	内部测试改善
DRC Finals	2015.06	完整 Momentum WBC + 改进落脚	第 2 名

本质洞察 IHMC 的架构证明了一个重要观点：对于人形全身控制，动量是比位置更基本的控制量。控制 CoM 位置是控制动量的结果（$l_G = m\dot r_{CoM}$），但直接控制动量可以同时处理线动量和角动量。这就像在飞行器控制中，直接控制力矩比控制姿态角更基本——姿态角是力矩积分的结果。

陷阱警告 ⚠️ 直接复现 IHMC 论文时，容易忽略他们使用的 RNEA 反推步骤。QP 求解后得到的是 $\dot v^*$ 和 $\lambda^*$，还需要一步递推牛顿-欧拉算法才能得到关节力矩。如果跳过这一步直接用 $\tau = S(M\dot v^* + h - J_c^T\lambda^*)$，结果在数值上是等价的，但 RNEA 的计算效率更高（$O(n)$ vs $O(n^2)$）。

练习 92.9.A：阅读 Koolen et al. 2016 论文的 Section III（QP formulation），画出 QP 的变量、约束和目标函数的矩阵结构图。与 92.8 节的 TSID 公式做逐项对比。

练习 92.9.B（跨章综合题）：结合足式/90 的 WBC 框架和本节的 IHMC 动量框架，讨论以下问题：四足 WBC（MIT Cheetah 风格）和人形 Momentum WBC（IHMC Atlas 风格）在 QP 目标函数上的核心区别是什么？为什么四足通常不需要角动量任务，而人形必须要？

92.9.5 WBIC：把 MPC 和全身控制接起来的另一条路线 ⭐⭐⭐⭐¶

动机：TSID 没有回答的一个问题¶

回顾足式/90：四足 MIT Cheetah 把控制拆成两层——低频 MPC 用单刚体模型（SRBD）规划接触力和质心轨迹，高频 WBC 把这些参考翻译成关节力矩。§92.8 的 TSID 解决了"WBC 怎么求力矩"，但它默认上层给的参考是**自洽且可行**的：CoM 参考、接触力参考、摆脚参考彼此不冲突，QP 总能找到满足所有约束的解。

真实系统里这个前提常常不成立。MPC 用的是简化模型，它算出的接触力 $f^{MPC}$ 是"在单刚体假设下最优"的，但全身动力学未必能精确实现这组力——关节力矩有限、接触点真实位置和模型有偏差、摆脚任务可能和质心任务抢自由度。如果 TSID 把 MPC 的接触力当作**硬约束**强行实现，QP 很容易无解；如果当作**软任务**用权重平衡，又回到了 §92.8 讲过的"优先级泄漏"问题。

WBIC（Whole-Body Impulse Control，全身脉冲控制）就是为弥合这个裂缝提出的。它由 Donghyun Kim、Jared Di Carlo、Patrick Wensing 等人在 2019 年的论文（"Highly Dynamic Quadruped Locomotion via Whole-Body Impulse Control and Model Predictive Control", arXiv:1909.06586）中提出，并在 MIT Cheetah 3 和 Mini-Cheetah 上验证——后者用它跑出了 3.7 m/s 的无系绳奔跑速度，是当时最快的四足之一。

本质洞察 WBIC 和 TSID 的根本区别，不在于求解器，而在于**对待 MPC 接触力的态度**。 TSID 把任务跟踪放在第一位，接触力是被动反推的结果（$\tau=S(M\dot v+h-J_c^T\lambda)$ 里 $\lambda$ 由 QP 决定）。 WBIC 反过来：把"忠实复现 MPC 算出的接触力剖面"放在第一位，让浮动基座的运动**容许偏离**指令轨迹——因为 MPC 已经在更长的预测窗口里考虑了平衡，强行让全身层再纠正一遍反而会打架。

反面：如果硬实现 MPC 的力会怎样¶

设想最朴素的接法：把 MPC 输出的接触力 $f^{MPC}$ 直接当作期望 $\lambda^d$，要求 WBC 精确实现。这会引出两类失败：

力矩越界即无解：MPC 在 SRBD 下算出的力，落到真实的高维动力学上，可能要求某个关节出 1.5 倍额定力矩。硬约束 QP 直接判定不可行，控制周期丢失，机器人在这一拍没有力矩输出——在 500 Hz 高动态步态里这足以致命。
过约束抖动：即使勉强可行，把 6 维浮动基座的运动和 3$N_c$ 维接触力同时锁死，留给摆脚和姿态任务的自由度几乎为零。结果是身体被"焊死"在 MPC 轨迹上，地面稍有起伏，接触力剧烈跳变。

WBIC 的解法是引入**松弛变量（relaxation variable）**：允许浮动基座的实际加速度偏离指令一点点，把"硬实现"软化为"尽量实现且物理可行"。

理论：WBIC 的两步结构¶

WBIC 把一拍控制拆成"先投影、后微调"两步，这和纯 QP 的 TSID 是不同的计算结构：

第一步：分层运动学（投影式 IK 加速度）。 按优先级（接触 > 浮动基姿态 > 足端 > …）逐层求期望关节加速度。每加入一个低优先级任务，都把它投影到所有高优先级任务的零空间里，保证不破坏上层。这一步给出一个**运动学一致**的期望加速度 $\ddot q^{cmd}$：

\[ \ddot q_i^{cmd}=\ddot q_{i-1}^{cmd}+\bar J_{i|pre}\left(\ddot x_i^{d}-\dot J_i v-J_i\ddot q_{i-1}^{cmd}\right), \]

其中 $\bar J_{i|pre}$ 是任务 $i$ 在前序任务零空间内的动力学一致伪逆。这一步只管"运动学上各任务怎么协调"，不碰接触力。

第二步：带松弛的浮动基逆动力学 QP。 在第一步给出的 $\ddot q^{cmd}$ 附近做一个**小规模** QP，决策变量是浮动基加速度的松弛量 $\delta_{fb}\in\mathbb R^6$ 和接触力 $f_r$：

\[ \begin{aligned} \min_{\delta_{fb},\,f_r}\quad & \|\delta_{fb}\|_{Q_1}^2+\|f_r-f^{MPC}\|_{Q_2}^2\\ \text{s.t.}\quad & S_f\big(M\dot v+h-J_c^Tf_r\big)=0,\\ & \dot v=\ddot q^{cmd}+\begin{bmatrix}\delta_{fb}\\0\end{bmatrix},\\ & C_{fric}f_r\le0. \end{aligned} \]

目标函数第一项压低浮动基偏离（尽量贴近第一步的运动学解），第二项让接触力贴近 MPC 参考。约束只有浮动基 6 行动力学和摩擦锥——这正是 WBIC 的"小 QP"：变量只有 $6+3N_c$ 维，没有把全部关节加速度塞进优化。求出 $f_r$ 和 $\delta_{fb}$ 后，关节力矩照例由 RNEA 反推。

维度	TSID（§92.8）	WBIC
第一性目标	任务加速度跟踪	复现 MPC 接触力剖面
浮动基座	当作普通任务（软/硬可选）	允许松弛偏离指令
计算结构	单个（或分层）大 QP	分层 IK + 小 QP 两步
QP 变量	$[\dot v,\lambda]$，$n_v+3N_c$ 维	$[\delta_{fb},f_r]$，$6+3N_c$ 维
与 MPC 关系	MPC 给轨迹参考	MPC 给接触力参考
典型平台	TALOS、人形 TSID 栈	MIT Cheetah 3 / Mini-Cheetah

桥接：WBIC 用到人形上要补什么¶

WBIC 诞生于四足，搬到人形时有一处必须补强——角动量。四足质量分布相对紧凑、躯干转动惯量大、单步冲击小，把躯干姿态任务做好通常就够了；人形上体高、手臂长，扰动下角动量会在躯干快速累积（§92.7 的反事实推理已说明）。因此人形版 WBIC 会在第一步的分层运动学里追加一个角动量任务，或在第二步 QP 的目标里加 $\|A_G^{ang}\dot v+\dot A_G^{ang}v-\dot k_G^d\|^2$ 项。这也正是练习 92.9.B 的答案核心：四足 WBIC 可以不显式管角动量，人形不行。

反事实推理 如果用 WBIC 控人形却不加角动量项会怎样？平地慢走几乎看不出问题——和不加角动量任务的 TSID 一样。但一旦受到侧向推力，浮动基座的松弛会优先"让身体顺着动"以贴近 MPC 力，角动量无人约束便在躯干堆积，机器人持续侧倾直到 ZMP 出界。这说明 WBIC 的"松弛浮动基"哲学在四足是优点（吸收高动态冲击），在人形上必须配角动量任务才安全。

陷阱警告 ⚠️（思维陷阱）把 WBIC 理解成"另一种 QP 求解器"是常见误区。WBIC 不是求解器的替换，而是**控制器结构**的替换：它把"运动学协调"和"力分配"拆成先后两步，第二步的 QP 只负责很小的一块。如果你只是把 TSID 的求解器换成另一个 QP 库，却不改这个两步结构，得到的还是 TSID，不是 WBIC。

陷阱警告 ⚠️（概念误区） WBIC 名字里的 "Impulse"（脉冲）容易让人以为它在控制冲量/碰撞。其实这里的"脉冲控制"是指它面向**接触力剖面**做控制（接触力对时间积分即冲量），强调的是力的忠实复现，而非碰撞瞬间的冲击处理。落脚冲击的处理仍然靠摆脚轨迹平滑和力渐入（§92.6）。

练习 92.9.5.A：对照 §92.8 的 TSID QP 和本节的 WBIC 两步结构，列一张表说明：同样一组任务（接触+CoM+摆脚+姿态），两者各自在哪一步、用什么变量满足它。指出 WBIC 把哪些任务移出了大 QP。

练习 92.9.5.B（设计扩展题）：MIT Cheetah 用 MPC@30Hz + WBIC@500Hz。假设你要把它改造成人形：(1) MPC 的简化模型该从 SRBD 换成什么？(2) WBIC 第一步要新增哪个任务？(3) 为什么人形的 MPC 频率通常还要更低？从计算量和模型复杂度两方面分析。

92.10 DRC 历史回顾：2013-2015 竞赛的 WBC 演进 ⭐⭐⭐¶

DRC 为什么改变了人形控制¶

DARPA Robotics Challenge 是人形机器人领域的"登月时刻"。从 2012 年启动到 2015 年决赛，这场竞赛逼迫全球顶级团队在真实人形机器人上实现可靠的全身控制。

竞赛任务：机器人需要完成 8 个任务——驾驶汽车、穿越废墟、打开阀门、锯切墙壁、清理碎片、翻越障碍、爬楼梯、穿过门。每个任务都需要全身协调：腿部维持平衡，手臂执行操作，躯干调整重心。这些任务不是实验室里的简单站立或行走，而是需要多接触、变姿态、大力矩的复杂运动。

参赛方案的演进¶

团队	机器人	控制方案	DRC 结果	WBC 特点
IHMC	Atlas	Momentum QP + RNEA	Finals 第 2 名	小 QP、动量优先
MIT	Atlas	QP-based WBC + MPC	Finals 第 6 名	全维 QP
KAIST	DRC-HUBO	变形态（轮式+双足）	Finals 冠军	轮式减少平衡难度
CMU/Tartan	CHIMP	轮式四足	Trials 第 3 名	避免双足行走
Feng & Atkeson	Atlas	双层优化	学术贡献	高层轨迹 + 低层 QP

Feng & Atkeson (2015) 的双层优化**是学术上最有影响力的 DRC WBC 工作之一。他们的框架将控制分为： 1. **高层轨迹优化：推理 CoM 和摆动脚的轨迹 2. 低层 QP 控制：用浮动基全身逆动力学跟踪这些轨迹

这个框架后来被广泛引用，并成为现代人形 WBC 的标准模板。

从 DRC 中学到的工程教训¶

DRC 的一个残酷现实是：几乎所有团队的机器人都在某个时刻摔倒了。摔倒的原因统计如下：

摔倒原因	频率	根因分析
通信延迟导致控制中断	最高	DRC Finals 规定 1 秒通信中断
接触状态估计错误	高	真实接触面不平整
步态规划与 WBC 不协调	中	落脚点超出可达范围
力矩饱和	中	液压/电动执行器极限
软件 bug	低但致命	坐标系错误、时间戳不对齐

关键教训：DRC 证明了人形 WBC 的数学框架在 2015 年已经相当成熟，但系统集成和鲁棒性是真正的瓶颈。一个数学上完美的 QP 控制器，如果接触检测晚了 50 ms 或力矩限幅设错了 10%，就会导致机器人摔倒。这就是为什么本章反复强调故障排查和调试技巧——它们在工程中的重要性不亚于数学推导。

DRC-HUBO 的变形态策略¶

值得特别提及的是 KAIST 的 DRC-HUBO——DRC Finals 的冠军。DRC-HUBO 采用了一个出人意料的策略：在需要行走时使用双足步态，在需要精细操作或通过复杂地形时变形为轮式移动。

这个设计选择背后的工程智慧是：双足行走在 DRC 的通信中断条件下（1 秒无信号）风险极高，一旦摔倒就可能导致整个任务失败。轮式移动虽然不够灵活，但稳定性远高于双足行走。KAIST 团队选择"在最需要可靠性的环节最大化可靠性"，而不是追求技术上的优雅。

关键教训 DRC 的冠军不是 WBC 做得最好的团队，而是系统集成做得最好的团队。这提醒我们：在真实应用中，控制算法的先进性往往不如系统可靠性重要。一个能可靠完成任务的简单方案，胜过一个偶尔会失败的复杂方案。

练习 92.10.A：观看 DRC Finals 的视频（YouTube 可搜索 "DRC Finals 2015"），选择一个机器人摔倒的片段，分析可能的摔倒原因。从 ZMP、DCM、力矩饱和和接触检测四个维度进行讨论。

练习 92.10.B（跨章综合题）：对比 DRC 中的经典 WBC 方案和第93章的 RL 方案。如果 DRC 在 2025 年重新举办，RL 策略能否替代 WBC？分析 RL 方案在通信中断、未知地形和操作任务中的优劣。

92.11 多接触运动：楼梯与不平地形的 WBC 扩展 ⭐⭐⭐⭐¶

超越平面行走¶

前面的内容都假设了平地行走，但真实人形需要爬楼梯、跨越障碍、在倾斜面上行走。这些场景带来三个新问题：

接触面不再水平：ZMP 约束需要推广到倾斜接触面。
接触点不在同一高度：双支撑时两脚的 ZMP 约束不能简单合并。
多接触：当手也参与接触时，支撑多边形的概念需要推广。

倾斜接触面的 ZMP¶

在倾斜面上，接触法向量不再是竖直方向。设接触面法向量为 $\hat n$，则 ZMP 约束变为： $$ (p_{zmp} - r_G) \times f \cdot \hat n_{perp} = 0 $$

其中 $\hat n_{perp}$ 是接触面内的方向。这使得 ZMP 公式中的 $g$ 需要替换为重力在接触面法向的分量 $g \cos\alpha$（$\alpha$ 是坡角）。

数值影响：当坡角 $\alpha = 15°$ 时，$g_{eff} = g\cos(15°) \approx 0.97g$，影响约 3%。当 $\alpha = 30°$ 时，$g_{eff} \approx 0.87g$，LIPM 的自然频率 $\omega_0$ 降低约 7%。这意味着 DCM 反馈增益需要根据地面倾斜角度调整，否则会出现过度或不足的 ZMP 修正。

多接触场景¶

当手接触墙面或扶手时，支撑不再局限于双脚。多接触场景下的平衡条件变为： $$ \sum_i J_{c_i}^T \lambda_i = M\dot v + h - S^T\tau $$

每个接触点都有自己的摩擦锥约束。QP 的接触力维度增加，但数学框架不变。

多接触的 ZMP 推广：Caron 等人（2015, "Stability of surface contacts for humanoid robots"）提出了 CoM 可行性（Centroidal Wrench Cone）概念，将 ZMP 约束推广到任意接触配置。核心思想是：所有接触力的合力和合力矩必须能够在一个可行锥（由所有接触点的摩擦锥决定）内。当这个可行锥退化为一个平面时，就退化为经典的 ZMP 约束。

Centroidal Wrench Cone 的数学形式：

设有 $N_c$ 个接触点，每个接触点 $i$ 有摩擦锥约束 $C_i \lambda_i \le 0$。所有接触力对质心产生的合力和合力矩（6D wrench）为： $$ w_G = \sum_{i=1}^{N_c} \begin{bmatrix} I \ [p_i - r_G]_\times \end{bmatrix} \lambda_i $$

合力 wrench 必须满足牛顿-欧拉方程：$w_G = \dot h_G - w_{grav}$。所有可行的 $w_G$ 构成一个锥——即 Centroidal Wrench Cone (CWC)。质心位置可行的条件是 $\dot h_G - w_{grav}$ 落在 CWC 内部。

与 ZMP 的关系：在双脚平地接触的特殊情况下，CWC 在水平力矩方向上的可行范围恰好对应 ZMP 必须在支撑多边形内的条件。CWC 是 ZMP 约束的严格推广。

本质洞察 ZMP 约束只在"所有接触面在同一水平面上"时才有简洁形式。一旦接触面倾斜或在不同高度（如爬楼梯、扶墙），ZMP 概念就不再适用。 CWC 是更通用的稳定性判据，但计算成本更高——需要对多接触点的摩擦锥做 Minkowski 求和。工程上的折中是：平地行走仍用 ZMP（计算快），多接触场景切换到 CWC（计算准确）。

楼梯行走的 WBC 调整¶

楼梯行走需要在 WBC 中做以下调整：

调整项	平地设置	楼梯设置	原因
CoM 高度	恒定	逐步变化	质心需要随台阶上升
DCM 参数 $\omega_0$	固定	随高度更新	高度变化改变自然频率
摆脚轨迹	平面弧线	三维抬脚弧线	需要跨过台阶边缘
落脚高度	零	台阶高度	落脚点在不同高度
力矩预算	标准	增大	上楼需要更大的支撑力

陷阱警告 ⚠️ 楼梯行走中最常见的失败不是平衡控制本身，而是脚趾碰到台阶边缘。这不是 WBC 的问题，而是摆脚轨迹规划的问题。轨迹必须保证摆脚在台阶边缘上方有足够的间隙（通常 3-5 cm）。如果轨迹生成和 WBC 使用不同的台阶模型，碰撞几乎不可避免。

变高度 DCM 的数值实现¶

当质心高度变化时（如爬楼梯），$\omega_0$ 不再是常数。DCM 方程变为： $$ \dot\xi(t) = \omega(t)(\xi(t) - p(t)) $$

由于 $\omega(t)$ 是时变的，上述方程没有简单的解析解（不再是指数函数）。数值求解方法如下：

向后积分法：给定终端时刻 $T$ 的 DCM 参考 $\xi(T) = \xi_f$，从终端向前反向积分： $$ \xi(t - \Delta t) = \xi(t) + \Delta t \cdot \omega(t)(p(t) - \xi(t)) $$

注意这里的符号——反向积分时稳定方向和正向相反。

import numpy as np


def dcm_backward_integration(xi_final, zmp_ref, z_com_ref, dt, N):
    """中文注释：变高度 DCM 的反向积分。从终端条件向前推 DCM 参考轨迹。"""
    xi = np.zeros(N)
    xi[-1] = xi_final
    for k in range(N - 2, -1, -1):
        omega_k = np.sqrt(9.81 / z_com_ref[k])
        # 中文注释：反向 Euler 积分。注意符号：反向稳定 = 正向不稳定。
        xi[k] = xi[k + 1] + dt * omega_k * (zmp_ref[k] - xi[k + 1])
    return xi

为什么要反向积分？ DCM 方程正向是不稳定的（$\omega_0 > 0$），数值正向积分会放大误差。但反向积分是稳定的——误差会被衰减而非放大。这是利用了线性系统稳定性在时间反演下的对称性。

陷阱警告 ⚠️ 变高度 DCM 积分时，$z_{com}$ 的变化不能太快。如果在一个积分步内 $z_{com}$ 变化超过 5 cm（例如台阶高度），单步 Euler 积分的误差可能不可接受。应使用更小的步长或更高阶的积分方法（如 RK4）。

练习 92.11.A：推导 15 度倾斜面上的 LIPM 方程。与水平面方程做对比，指出哪些项需要修改。

练习 92.11.B：设计一个三接触（双脚 + 一只手扶墙）的 QP 约束。列出所有接触点的摩擦锥约束，并讨论这种配置相比纯双足站立的稳定性提升。

练习 92.11.C：实现变高度 DCM 反向积分。给定一段楼梯的 CoM 高度变化曲线（每级 15 cm，共 5 级），计算 DCM 参考轨迹。与恒定高度 DCM 做对比。

92.12 PAL Robotics TALOS：欧洲人形 WBC 方案 ⭐⭐⭐¶

TALOS 平台概览¶

TALOS 是 PAL Robotics 开发的 1.75 m、100 kg 全电动人形机器人，拥有 32 个关节和 6 自由度浮动基座（共 38 DoF）。它是欧洲最重要的人形研究平台之一，被 LAAS-CNRS（图卢兹）、Inria 和多个欧洲实验室用于 WBC 研究。

TALOS 的关键硬件特征是**全关节力矩传感**。每个关节都有扭矩传感器反馈，这使得力矩级 WBC 可以在闭环中验证扭矩跟踪精度。相比之下，许多其他平台（如 Unitree G1/H1）只提供电流反馈，力矩估计需要通过电机模型间接获得。

WBC 方案对比¶

Romualdi 等人（2022, "Benchmarking Whole-Body Controllers on the TALOS Humanoid Robot", Frontiers in Robotics and AI）在 TALOS 上系统对比了三种 WBC 方案：

方案	控制级别	QP 变量	优点	局限
TSID-Acc	加速度级	$[\dot v, \lambda]$	任务定义直观	力矩精度依赖逆动力学模型
TSID-Torque	力矩级	$[\tau, \lambda]$	直接控制力矩	QP 维度更大
位置级 WBC	关节角级	$[\Delta q]$	实现简单	不满足动力学约束

关键发现：在平地行走和爬楼梯测试中，TSID-Acc 和 TSID-Torque 的跟踪精度相近，但 TSID-Torque 在力矩受限场景下表现更好（因为力矩约束可以直接施加）。位置级 WBC 在简单任务上可行，但在动态行走和爬楼梯中明显不如力矩级方案。

与 IHMC Atlas 方案的对比¶

维度	IHMC Atlas	LAAS TALOS
执行器	液压	电动（SEA）
控制频率	333 Hz	1 kHz
QP 优先级	动量优先	可配置加权/HQP
力矩反馈	通过液压压力估计	直接力矩传感器
典型任务	粗糙地形行走、操作	平地/楼梯行走、多接触
开源程度	部分（IHMC Open Robotics）	较高（TSID 库、Pinocchio）

关键洞察 TALOS 和 Atlas 代表了人形 WBC 的两条路线：Atlas 使用液压驱动追求高动态性能，TALOS 使用电动驱动追求力矩控制精度。两者的 WBC 数学框架是一致的（都是 QP + 动力学约束），但硬件差异导致了不同的工程权衡。液压系统响应快但力矩精度差，电动系统力矩精度高但功率密度低。这说明 WBC 的数学理论是平台无关的，但参数调整和故障诊断是平台特定的。

TSID 库的架构与使用¶

TSID（Task-Space Inverse Dynamics）是 LAAS-CNRS 开源的 WBC 库，基于 Pinocchio 构建。它的架构直接对应本章的数学框架：

TSID 类	数学对应	作用
`InverseDynamicsFormulationAccForce`	AccForce QP	组装 QP
`TaskComEquality`	CoM 任务 $J_{com} \dot v = \ddot r_{com}^d$	CoM 跟踪
`TaskAMEquality`	角动量任务 $A_G^{ang} \dot v = \dot k_G^d$	角动量控制
`TaskSE3Equality`	末端任务 $J_{ee} \dot v = \ddot p_{ee}^d$	摆脚/手臂
`TaskJointPosture`	姿态任务 $I \dot v = K_p(q^d - q)$	默认关节角
`ContactPoint` / `Contact6d`	接触约束	支撑脚不滑
`ConstraintBound`	力矩/速度限幅	物理约束
`SolverHQP`	HQP 求解器	层级最优化

# TSID 使用示例骨架（伪代码风格，展示 API 结构）
import tsid
import pinocchio as pin

# 中文注释：创建模型和 QP 公式。
robot = tsid.RobotWrapper(urdf_path, pin.JointModelFreeFlyer())
formulation = tsid.InverseDynamicsFormulationAccForce("tsid", robot)

# 中文注释：添加接触约束（支撑脚）。
contact_left = tsid.Contact6d("left_foot", robot, "l_sole", ...)
formulation.addRigidContact(contact_left)

# 中文注释：添加 CoM 任务（高优先级）。
com_task = tsid.TaskComEquality("com", robot)
com_task.setKp(50.0 * np.ones(3))
com_task.setKd(2.0 * np.sqrt(50.0) * np.ones(3))
formulation.addMotionTask(com_task, weight=1.0, level=0)

# 中文注释：添加角动量任务。
am_task = tsid.TaskAMEquality("angular_momentum", robot)
am_task.setKp(10.0 * np.ones(3))
formulation.addMotionTask(am_task, weight=0.1, level=0)

# 中文注释：添加关节姿态任务（低优先级）。
posture_task = tsid.TaskJointPosture("posture", robot)
formulation.addMotionTask(posture_task, weight=0.01, level=1)

# 中文注释：求解。
solver = tsid.SolverHQuadProg("solver")
sol = solver.solve(formulation.computeProblemData(t, q, v))
tau = formulation.getActuatorForces(sol)

陷阱警告 ⚠️ TSID 的 level 参数在使用 SolverHQuadProg（HQP 求解器）时才有效。如果使用普通 QP 求解器（如 SolverHQP），所有 level 会被忽略，退化为加权 QP。这是一个常见的混淆源——设置了 level 但实际没有分层效果。

练习 92.12.A：查阅 TSID Python 库（github.com/stack-of-tasks/tsid）的 README。列出它支持的任务类型和约束类型。与本章 92.8 节的 QP 形式做对应。

练习 92.12.B：使用 TSID 和一个人形 URDF，实现一个站立平衡控制器。包含接触约束、CoM 任务和关节姿态任务。施加 10 N 侧向扰动，记录 CoM 偏移和恢复时间。

92.13 Momentum-Based WBC 的完整数据流与教学实现 ⭐⭐⭐¶

典型数据流¶

模块	输入	输出	对应知识
脚印规划	目标速度、地形、DCM	脚印与相位	92.6
ZMP/DCM 规划	脚印与当前状态	CoM、ZMP、VRP	92.2-92.5
动量任务	CMM、参考动量	$\dot h_G^d$	92.7
TSID/HQP	全身状态、任务栈	加速度、接触力、力矩	92.8-92.9
安全层	姿态、力矩、接触裕度	降级或停机	常见故障与排查

教学实现的合理简化¶

完整系统	教学系统	简化理由
6D 足底 wrench	脚掌四点接触近似	便于理解 CoP 约束
复杂混合整数脚印	平地 DCM 解析脚印	先掌握稳定主线
全状态估计	仿真真值 + 简单滤波	先验证控制链
完整角动量任务	角动量正则或后三行任务	逐步加入全身耦合

TSID + Pinocchio 双足 demo 路线¶

一个最小教学闭环包含七步。

第一步，生成左右脚交替支撑的相位表。第二步，用 DCM 生成 CoM 与 ZMP 参考。第三步，为摆动脚生成五次多项式轨迹。第四步，用 Pinocchio 计算 $M,h,J,\dot Jv,A_G$。第五步，组装支撑脚、CoM、摆脚、躯干和关节姿态任务。第六步，求解 QP 并反推力矩。第七步，在仿真中记录 ZMP、DCM、力矩和 QP 时间。

伪代码¶

class HumanoidWbcLoop:
    def __init__(self, model, planner, solver):
        self.model = model
        self.data = model.createData()
        self.planner = planner
        self.solver = solver

    def step(self, t, q, v):
        # 中文注释：查询当前相位和参考轨迹。
        ref = self.planner.reference_at(t)

        # 中文注释：更新运动学、动力学和质心动量缓存。
        pin.forwardKinematics(self.model, self.data, q, v)
        pin.computeJointJacobians(self.model, self.data, q)
        pin.centerOfMass(self.model, self.data, q, v)
        pin.ccrba(self.model, self.data, q, v)

        # 中文注释：根据支撑脚切换接触任务。
        self.solver.set_support_contacts(ref.support_feet)
        self.solver.set_com_reference(ref.com)
        self.solver.set_swing_foot_reference(ref.swing_foot)
        self.solver.set_torso_reference(ref.torso)

        # 中文注释：如果启用角动量任务，设置动量参考。
        if hasattr(ref, 'angular_momentum_ref'):
            self.solver.set_momentum_reference(ref.angular_momentum_ref)

        sol = self.solver.solve(q, v)
        return sol.tau

验收指标¶

指标	定义	建议阈值
ZMP 裕度	到脚掌边界最小距离	> 1 cm
DCM RMS 误差	$\\|\xi-\xi^d\\|$	< 3 cm
摆脚落点误差	touchdown 位姿误差	< 2 cm
QP 时间	99% 求解时间	< 3 ms
力矩饱和比例	超过限幅周期比例	< 1%
角动量峰值	$\\|k_G\\|$ 最大值	< 5 Nm·s

验收指标的诊断解读¶

每个指标的异常值对应不同的问题链：

ZMP 裕度 < 1 cm： - 直接原因：CoM 参考偏离了脚掌中心 - 上游检查：DCM 反馈增益是否过小？步宽是否太窄？ - 下游影响：裕度不足时，轻微扰动就会导致 ZMP 出界，机器人绕脚边翻倒 - 修复顺序：先检查脚印规划 → 再检查 DCM 增益 → 最后调 CoM 权重

DCM RMS > 3 cm： - 直接原因：DCM 反馈没有有效抑制发散模态 - 上游检查：$\omega_0$ 计算使用的 $z_c$ 是否与实际 CoM 高度一致？ - 下游影响：DCM 误差累积后 Capture Point 超出可达范围，必须紧急迈步 - 修复顺序：先校准 $z_c$ → 增大 DCM 增益 → 最后考虑变高度 DCM

QP 时间 > 3 ms： - 直接原因：QP 规模太大或初始猜测太差 - 上游检查：是否在每个周期都重新构造矩阵（应该复用热启动）？ - 下游影响：超时会导致控制周期丢失，机器人在一个 ms 内没有力矩更新 - 修复顺序：启用热启动 → 预分配矩阵 → 减少 QP 变量（消去 $\tau$）

力矩饱和 > 1%： - 直接原因：某个任务的加速度需求超出了执行器能力 - 上游检查：是否有任务增益 $K_p$ 设得过大？摆脚轨迹是否过于激进？ - 下游影响：饱和期间 QP 的解不再最优，任务跟踪精度下降 - 修复顺序：降低高增益任务的 $K_p$ → 放缓摆脚轨迹 → 最后加入力矩约束

练习 92.13.A：在教学 demo 中故意引入一个故障（如把 $z_c$ 设错 20%），观察哪个验收指标最先报警。讨论这种关联对调试的启示。

92.13.5 被忽略的上游：浮动基状态估计与接触检测 ⭐⭐⭐¶

动机：所有公式都假设"已知 q, v"¶

回看本章每一个公式——LIPM 要当前 CoM 和速度，DCM 要 $x,\dot x$，CMM 要 $(q,v)$，TSID 要全身状态和接触集。它们无一例外地假设这些量**已经准确已知**。仿真里这是真值，触手可得；但真实人形身上没有任何传感器直接测量"浮动基座在世界系里的位姿和速度"，也没有传感器直接告诉你"这只脚现在到底接没接触地面"。这两件事都得**估计**出来。本章前面所有故障表里反复出现的"CoM 估计有偏差""接触检测延迟""状态估计错误"，根子都在这一层。把它放在最后讲，是因为只有理解了下游怎么用这些量，才能体会上游估计为什么如此关键。

本质洞察 仿真到实机的最大鸿沟，往往不在控制算法，而在这一层不可见的状态估计。一个在仿真里完美的 WBC，换到实机上第一个崩的通常不是 QP，而是它吃进去的 $q,v$ 和接触集本身就是错的——"垃圾进，垃圾出"。这也是 §92.10 DRC 教训"系统集成比数学更难"的微观体现。

浮动基状态估计：为什么需要融合¶

人形的关节编码器能精确给出**关节角** $q_{joint}$，但浮动基座那 6 个自由度（躯干在世界系的位置和姿态）没有直接传感器。可用的线索有三类，各有短板：

传感器	提供什么	短板
IMU（陀螺+加速度计）	躯干角速度、加速度	积分得到的位置/速度漂移；重力与运动加速度耦合
关节编码器 + 运动学	支撑脚相对躯干的位姿	仅在"知道哪只脚不动"时才能反推躯干
外部定位（光学/激光）	直接的世界系位姿	实验室外往往没有；有延迟

核心思想是**运动学-惯性融合**：把"支撑脚踩在地上不动"当作一个零速度约束，用它修正 IMU 的漂移。具体地，若已知左脚此刻是支撑脚且位置固定，则由前向运动学可反推躯干位姿；IMU 给出高频但漂移的预测，运动学约束给出低频但无漂移的修正，二者用卡尔曼滤波（典型 EKF）融合。这正是足式机器人通用的"接触辅助的浮动基估计"（contact-aided floating-base estimation），人形与四足共用同一套思路。

\[ \underbrace{\hat x_{k}^{-}=f(\hat x_{k-1},u_{IMU})}_{\text{IMU 预测，高频漂移}}\quad\xrightarrow{\text{支撑脚零速度修正}}\quad\underbrace{\hat x_{k}^{+}}_{\text{无漂移估计}}. \]

理论-工程桥接 正因为状态估计依赖"支撑脚不动"这个假设，接触检测错误会直接毒化浮动基估计：如果实际已经离地、估计器还以为它是支撑脚，零速度修正就把一个错误约束喂给了滤波器，躯干位姿瞬间跳变，下游 DCM、CMM、TSID 全部跟着错。这就是接触检测和状态估计必须一起调的原因——它们在估计器里是耦合的。

接触检测：三种判据与各自的陷阱¶

"这只脚接触了吗"看似简单，实则是高频出错点。常见判据：

判据	原理	陷阱
足底力阈值	法向力 $f_n>f_{thresh}$ 即判接触	阈值太低误触发、太高漏检；落地瞬间力有冲击尖峰
相位时钟	按步态相位表硬切接触	地形不平时实际落地早于/晚于时钟，导致"踩空"或"硬怼"
运动学高度	脚低于地面估计高度即接触	依赖地面高度已知；斜面/台阶易错

实践中常融合多个判据并加**迟滞（hysteresis）**：进入接触用高阈值、退出用低阈值，避免在阈值附近抖动产生接触状态的高频翻转——而每一次翻转都会让 QP 的接触约束结构突变，正是 §92.13 "QP 求解突然变慢"的诱因。

反事实推理 如果只用相位时钟、完全不看力反馈会怎样？平地理想步态下没问题。但只要地面比预期低 1 cm，脚在时钟判定"已支撑"时其实还悬空——WBC 以为有接触力可用，往一个不存在的支撑上分配力，机器人瞬间失去支撑而下坠；反之地面高 1 cm，脚提前撞地，时钟还在"摆动相"，WBC 不收摆脚反而继续往下压，产生硬冲击。这就是纯时钟方案在真实地形上几乎必然失败、必须引入力/接触反馈的原因。

陷阱警告 ⚠️（思维陷阱）把状态估计当成"控制之外的事"丢给别人，是人形项目最常见的认知陷阱。控制工程师常默认 $q,v$ 和接触集是干净的输入，把全部精力花在调 QP 权重上。但实战中相当比例的"控制问题"其实是估计问题——CoM 慢慢倾倒可能是编码器零位错（§92.13 故障表第一行），ZMP 莫名偏一侧可能是 IMU 安装角没标定。调试铁律：先确认输入对，再怀疑算法。

练习 92.13.5.A：在教学 demo（仿真）里给浮动基状态人为注入一个小漂移（如躯干位置每秒漂 1 cm），观察 ZMP 裕度、DCM 误差中哪个先报警。这模拟了实机状态估计漂移的后果。

练习 92.13.5.B（调试挑战）：给定一段"机器人在平地慢走却周期性轻微下坠"的现象描述。从接触检测（阈值/时钟/迟滞）和状态估计（IMU/运动学融合）两个方向，列出至少 4 个可能根因和对应的排查动作。

常见故障与排查¶

现象	可能原因	排查步骤	修复方向
ZMP 长时间贴边	CoM 参考过激或步宽太窄	绘制 ZMP 到脚掌边界距离	降低速度命令或增大步宽
DCM 误差发散	$\omega$ 使用的 CoM 高度不对	对比规划高度与实时 CoM 高度	更新高度或改用变高度模型
支撑脚滑动	缺少 $\dot Jv$ 或摩擦设置错误	打印 $J_c\dot v+\dot J_cv$	补齐漂移补偿并检查摩擦系数
QP 偶发无解	任务冲突或力矩限幅过紧	保存无解时任务残差	降低软任务权重，检查脚印可达性
角动量振荡	角动量增益过高	绘制 $k_G$ 与躯干角速度	降低增益或调整任务层级
落脚冲击大	touchdown 速度不连续	检查落脚前 50 ms 足端速度	使用五次多项式和接触力渐入
手臂动作导致倾倒	末端任务压过平衡任务	关闭手臂任务做对比	降低末端优先级并加入动量补偿
上楼梯脚趾碰台阶	摆脚轨迹间隙不足	可视化摆脚轨迹与台阶边缘	增大抬脚高度或调整台阶模型

练习汇总¶

编号	题目	要求
92.1	LIPM 手推与数值验证	从力矩平衡推导 LIPM，并验证发散模态。
92.2	ZMP 角动量修正	构造不同 $\dot k_y$ 下的 ZMP 偏移曲线。
92.3	DCM 一步捕获	输入 $x,\dot x$ 判断是否需要迈步。
92.4	CMM 秩分析	用 G1 或 Talos URDF 打印 $A_G$ 的秩。
92.5	TSID 任务阅读	画出 CoM 任务进入 HQP 的矩阵流程。
92.6	IHMC 控制栈复盘	把 IHMC 的动量 QP 与本章公式做逐项对应。
92.7	多接触 QP 扩展	设计双脚+单手接触的 QP 约束矩阵。
92.8	综合项目	搭建脚印 + DCM + TSID 的简化迈步 demo。

92.1 LIPM 手推与数值验证¶

目标：从力矩平衡推导 LIPM，并验证发散模态。
步骤 1：写出质心牛顿方程和绕 ZMP 的力矩平衡。
步骤 2：推导解析离散化矩阵，并用代码验证。
步骤 3：画出不同初始速度下 CoM 轨迹的发散行为。
交付物：推导、图表、代码片段和 300 字以上分析。

92.2 ZMP 角动量修正¶

目标：构造不同 $\dot k_y$ 下的 ZMP 偏移曲线。
步骤 1：实现含角动量的 ZMP 公式。
步骤 2：取 $\dot k_y \in [-10, 10]$ Nm，绘制 ZMP 偏移量。
步骤 3：讨论角动量对 ZMP 裕度的定量影响。
交付物：推导、图表、代码片段和 300 字以上分析。

92.3 DCM 一步捕获¶

目标：输入 $x,\dot x$ 判断是否需要迈步。
步骤 1：计算 Capture Point $\xi = x + \dot x / \omega_0$。
步骤 2：判断 $\xi$ 是否在脚掌内。
步骤 3：绘制 $(x, \dot x)$ 平面上的"无需迈步"区域。
交付物：推导、图表、代码片段和 300 字以上分析。

92.4 CMM 秩分析¶

目标：用 G1 或 Talos URDF 打印 $A_G$ 的秩。
步骤 1：加载 URDF 并计算 CMM。
步骤 2：改变手臂构型，观察秩和零空间变化。
步骤 3：验证零空间内的关节运动不改变质心动量。
交付物：代码、数据和 300 字以上分析。

92.5 TSID 任务阅读¶

目标：画出 CoM 任务进入 HQP 的矩阵流程。
步骤 1：列出 QP 的决策变量、硬约束和软任务。
步骤 2：画出矩阵的块结构图。
步骤 3：标注每个块的物理含义和维度。
交付物：矩阵图和 300 字以上分析。

92.6 IHMC 控制栈复盘¶

目标：把 IHMC 的动量 QP 与本章公式做逐项对应。
步骤 1：列出 IHMC 论文中的 QP 变量、约束和目标。
步骤 2：与 92.8 节的 TSID 形式做一一映射。
步骤 3：分析 IHMC 的"小 QP"技巧如何减少变量数。
交付物：对比表和 300 字以上分析。

92.7 多接触 QP 扩展¶

目标：设计双脚+单手接触的 QP 约束矩阵。
步骤 1：列出三个接触点的 Jacobian 和摩擦锥。
步骤 2：写出完整的 QP 约束矩阵。
步骤 3：讨论增加手部接触对 QP 维度和求解时间的影响。
交付物：矩阵推导和 300 字以上分析。

92.8 综合项目¶

目标：搭建脚印 + DCM + TSID 的简化迈步 demo。
步骤 1：实现左右脚交替支撑的相位发生器。
步骤 2：实现 DCM 规划和反馈控制。
步骤 3：在仿真中连接 TSID，记录 ZMP、DCM、力矩和 QP 时间。
交付物：可运行代码和验收指标报告。

公式速查¶

编号	公式	含义	使用位置
E1	$\ddot x=\omega_0^2(x-p)$	LIPM	CoM/ZMP 规划
E2	$p_x=x-z\ddot x/(g+\ddot z)+\dot k_y/[m(g+\ddot z)]$	含角动量 ZMP	摆臂与强扰动
E3	$\xi=x+\dot x/\omega_0$	DCM / Capture Point	落脚判断
E4	$\dot\xi=\omega_0(\xi-p)$	DCM 动力学	反馈控制
E5	$h_G=A_G(q)v$	CMM 定义	动量任务
E6	$\dot h_G=A_G\dot v+\dot A_Gv$	动量加速度形式	WBC
E7	$M\dot v+h=S^T\tau+J_c^T\lambda$	全身动力学	QP 硬约束
E8	$J_c\dot v+\dot J_cv=0$	接触不滑	支撑脚
E9	$\tau=S(M\dot v+h-J_c^T\lambda)$	反推力矩	TSID 输出
E10	$p_{next}=p_{cur}+e^{\omega_0 T_{step}}(\xi_{now}-p_{cur})$	解析 DCM 落脚	一步可捕获
E11	$N_k=I-J_k^{+}J_k$，下层在 $N_{1:k-1}$ 内求解	HQP 零空间投影	严格优先级
E12	$J^{+}_M=M^{-1}J^T(JM^{-1}J^T)^{-1}$	动力学一致伪逆	浮动基投影

API 速查表¶

搭建本章 demo 主要用 Pinocchio（动力学后端）和 TSID（WBC 组装）。下表列出核心 API 的签名与用途，调试时可直接对照。函数名以 Pinocchio 3.x / TSID 当前主线为准；旧版本 API 见 §延伸阅读中的兼容性提示。

API	签名（简化）	作用	对应公式/小节
`pin.buildModelFromUrdf`	`(path, JointModelFreeFlyer()) -> Model`	以浮动基座加载 URDF	§92.7
`pin.crba`	`(model, data, q) -> M`	组装质量矩阵 $M$（关节空间惯性）	E7
`pin.nonLinearEffects`	`(model, data, q, v) -> h`	科氏+离心+重力项 $h$	E7
`pin.computeJointJacobians`	`(model, data, q)`	一次性算全部关节 Jacobian	任务 $J_y$
`pin.getFrameJacobian`	`(model, data, frame_id, ref) -> J`	取某 frame（脚/手）的 6D Jacobian	$J_c,J_{swing}$
`pin.centerOfMass`	`(model, data, q, v)`	计算 CoM 位置与速度	§92.2
`pin.jacobianCenterOfMass`	`(model, data, q) -> J_com`	CoM Jacobian（线动量任务）	$J_{com}$
`pin.ccrba`	`(model, data, q, v) -> Ag`	质心动量矩阵 $A_G$，存 `data.Ag`	E5
`pin.dccrba`	`(model, data, q, v) -> dAg`	CMM 时间导数 $\dot A_G$	E6
`pin.rnea`	`(model, data, q, v, dv) -> tau`	逆动力学（$O(n)$ 反推力矩）	E9
`tsid.InverseDynamicsFormulationAccForce`	`(name, robot) -> formulation`	组装 AccForce QP	§92.8
`tsid.TaskComEquality`	`setKp/setKd; addMotionTask(t,w,level)`	CoM 跟踪任务	$J_{com}\dot v=\ddot r^d$
`tsid.TaskAMEquality`	`setKp; addMotionTask(...)`	角动量任务	$A_G^{ang}\dot v=\dot k_G^d$
`tsid.TaskSE3Equality`	`setReference(SE3); addMotionTask(...)`	摆脚/手臂 6D 位姿任务	$J_{ee}\dot v=\ddot p^d$
`tsid.TaskJointPosture`	`setKp; addMotionTask(...)`	默认关节角姿态任务（最低优先级）	$I\dot v=K_p(q^d-q)$
`tsid.Contact6d`	`addRigidContact(c)`	脚掌 6D 接触约束（含摩擦锥+CoP 边界）	E8、§92.11
`tsid.SolverHQuadProg`	`solve(formulation.computeProblemData(t,q,v))`	HQP 求解器（`level` 参数仅此生效）	§92.8 HQP
`formulation.getActuatorForces`	`(sol) -> tau`	从 QP 解取出关节力矩	E9

陷阱警告 ⚠️（编程陷阱） pin.ccrba 和 pin.dccrba 必须在同一拍、用同一组 (q,v) 调用，并且要先调过 forwardKinematics/computeJointJacobians，否则 data.Ag 与 data.dAg 来自不同构型，组装出的动量任务 $A_G^{ang}\dot v=\dot k_G^d-\dot A_G^{ang}v$ 在数值上不自洽，表现为角动量任务"莫名其妙地跟不上"。

综合项目：G1/Talos 经典 WBC 小系统¶

阶段 1：生成左右脚交替支撑相位，包含单支撑和双支撑过渡。
阶段 2：用 DCM 生成 CoM 与 ZMP 参考，并记录每步 Capture Point。
阶段 3：用五次多项式生成摆动脚轨迹，确保落脚速度接近零。
阶段 4：用 Pinocchio 计算全身动力学量和 CMM。
阶段 5：组装 AccForce QP，包含接触、CoM、摆脚、躯干、角动量和关节姿态。
阶段 6：加入 20 N 水平扰动，比较有无角动量任务的恢复效果。
阶段 7：输出 ZMP 裕度、DCM RMS、QP 时间、力矩峰值和落脚误差。

在累积项目中的位置¶

本章 demo 不是孤立的一次性练习，而是贯穿运动控制方向的"人形控制累积项目"的一个模块。它向前承接、向后铺垫的关系如下，建议把代码组织成可复用的库而非一次性脚本：

来源章节	本章复用的模块	在本章 demo 中的角色
复合/20	浮动基动力学 $M,h,J_c$ 与 CMM 计算	demo 阶段 4 的 Pinocchio 调用直接复用
足式/90	WBC/QP 任务栈与 HQP 求解器封装	demo 阶段 5 的 QP 组装在其上扩展角动量任务
足式/70	倒立摆/SRBD 简化模型	demo 阶段 2 的 DCM 规划是其双足特化

去向章节	本章产出的可复用模块	后续如何使用
第93章（人形 RL）	ZMP 裕度 / DCM 误差 / 角动量峰值的计算函数	直接作为 RL 奖励项或安全监控器
第93章（人形 RL）	本章 demo 的 WBC 作为底层安全层	上层 RL 出高维决策，WBC 兜底保证 ZMP 可行
多接触/操作章节	Contact6d 接触约束与 CWC 判据（§92.11）	扩展到手脚混合接触

本质洞察 累积项目的设计意图，是让你在学完整条运动控制线后，手里留下的不是十几个互不相干的脚本，而是一套**可组合的控制积木**：动力学计算（复合/20）、QP 框架（足式/90）、双足平衡规划（本章）、奖励/监控指标（本章）。第93章的混合架构 demo 会把这些积木重新拼装——这正是"理解了经典 WBC 才能理解 RL 奖励含义"在工程层面的具体体现。

🔧 故障排查手册¶

以下故障场景来自实际人形 WBC 开发中的高频问题：

症状	可能原因	排查步骤	相关章节
站立时缓慢倾倒	CoM 估计有偏差	1.打印 CoM 位置 2.与 Pinocchio 计算值对比 3.检查关节编码器零位	92.2, 92.7
行走时 ZMP 持续贴右边	左右腿力矩不对称或 CoM 偏移	1.打印左右腿力矩 2.检查 URDF 质量分布 3.加 CoM 偏移补偿	92.3, 92.4
QP 求解突然变慢（从 0.5ms 到 5ms）	接触切换导致 QP 结构变化	1.记录 QP 维度变化 2.检查接触检测延迟 3.预分配最大维度矩阵	92.8
角动量任务导致手臂振荡	$K_k$ 过大或采样率不足	1.降低 $K_k$ 到 5 2.检查 $\dot A_G$ 的数值精度 3.加低通滤波	92.7, 92.9
双支撑过渡时力矩跳变	接触力在两脚之间瞬时切换	1.画接触力时间序列 2.加入力渐入/渐出 3.使用更长的双支撑时间	92.6, 92.13

调试原则：人形 WBC 问题的排查应遵循"从高层到低层"的顺序——先检查参考轨迹是否合理（规划层），再检查 QP 是否有解（WBC 层），最后检查力矩是否被正确执行（伺服层）。不要一上来就改 QP 权重，因为问题可能根本不在 WBC 层。

本章常见误解汇总¶

阅读至此，把全章最容易建立错误心智模型的点集中纠正一遍。左列是初学者常持有的误解，右列是正确理解——建议读完后逐条自检，能完整说出右列才算真正掌握。

常见误解	正确理解
LIPM 是"不够精确的近似模型"，工程上应尽量少用	LIPM 是"在明确假设下精确描述质心运动的数学工具"。它的价值不在精度而在降维——把 30+ 维压成 2 维，从而可用线性系统理论分析、写解析解（§92.2）。
ZMP 和 CoP 是一回事	平地无足底摩擦力矩时两者重合，但 CoP 是力传感器读出的力学测量量，ZMP 是由质心运动方程定义的动力学概念。倾斜地面或有足底扭矩时两者分离，其差可用于诊断接触模型（§92.3）。
ZMP 在支撑多边形内 = 不会摔	ZMP 在内只是必要条件。仍可能因摩擦不足、力矩饱和、状态估计错误或落脚不可达而摔。调试要同时看 ZMP 裕度、摩擦锥裕度、力矩裕度、DCM 误差（§92.3）。
Capture Point 和 DCM 是两个不同的量	是同一个量 $\xi=x+\dot x/\omega_0$ 的两个名字。Pratt 一系从 push recovery 命名为 Capture Point，Englsberger 一系从模态分离命名为 DCM（§92.5）。
平衡只有"要不要迈步"两种状态	平衡是分级的：0 步 / 1 步 / N 步可捕获 / 不可捕获。可捕获级别是一个连续可监测的余量，不是布尔量（§92.5 N 步可捕获性）。
CoM 位置控好了角动量自然就好	CoM 任务只控线动量 $l_G=m\dot r_{CoM}$，角动量 $k_G$ 需要单独的任务。受扰时不管角动量，它会在躯干累积直到 ZMP 出界（§92.7）。
加权 QP 里权重设得足够高就等于高优先级	高权重只是让误差"更贵"，仍可能被优先级泄漏牺牲。只有 HQP 的零空间投影/字典序最小化才是严格优先级（§92.8 HQP 零空间投影）。
WBIC 就是换了个 QP 求解器的 TSID	WBIC 是控制器结构的替换（分层 IK + 小 QP 两步），且第一性目标是复现 MPC 接触力而非跟踪轨迹，允许浮动基松弛。光换求解器得到的还是 TSID（§92.9.5）。
WBC 数学框架是平台无关的，所以参数也通用	数学框架（QP + 动力学约束）确实平台无关，但增益、力矩限幅、接触模型和故障表是平台特定的——液压 Atlas 和电动 TALOS 的工程权衡完全不同（§92.12）。
经典 WBC 会被 RL 取代	RL 是补充不是替代。好的人形 RL 奖励大量借用 ZMP/DCM/CMM；混合架构里底层 WBC 保安全、上层 RL 处理高维决策（§92.10、向下一章过渡）。

本章小结¶

知识点	核心内容	所在章节
支撑多边形	人形窄支撑使 ZMP 成为关键约束	92.1
LIPM	恒定高度和零角动量近似，线性倒立摆	92.2
ZMP 角动量修正	摆臂和扭腰通过 $\dot k_G$ 影响 ZMP	92.3
Preview Control	利用未来脚步信息提前调整质心	92.4
DCM / Capture Point	隔离发散模态，判断是否需要迈步	92.5
N 步可捕获性	把平衡升级为分级可计算余量	92.5
落脚规划	步长、步宽、转角约束下的脚印决策	92.6
CMM	全身速度到 6D 动量的线性映射	92.7
TSID / HQP	全身逆动力学的 QP 形式；零空间投影实现严格优先级	92.8
IHMC Atlas 架构	动量优先的 QP + RNEA 反推	92.9
WBIC	以复现 MPC 接触力为先、浮动基松弛的两步结构	92.9.5
DRC 历史	2013-2015 竞赛的 WBC 工程教训	92.10
多接触运动	楼梯、斜面和多接触的 WBC 扩展	92.11
TALOS 方案	欧洲人形 WBC 的力矩控制方案	92.12

本章与后续章节的关系¶

章节	关系	本章提供的铺垫知识
第93章人形全身 RL	互补/被复用	ZMP/DCM/CMM 作为奖励项与安全监控；WBC 作为底层安全层
复合机器人动力学（复合/20，前置）	被本章复用	浮动基动力学 $M\dot v+h=S^T\tau+J_c^T\lambda$、CMM 定义是本章 §7/§8 的直接工具
足式 WBC/HQP（足式/90，前置）	被本章扩展	QP 任务栈与 HQP 求解器；本章新增角动量任务与 ZMP/CoP 约束
腿足简化模型（足式/70，前置）	被本章特化	倒立摆/SRBD 思想；本章特化为窄支撑双足的 LIPM/DCM
多接触与操作章节	向后延伸	§92.11 的 Contact6d 接触约束与 CWC 判据可推广到手脚混合接触

承上启下：本章是"经典优化派"人形控制的集大成，往前它把复合动力学和足式 WBC 收束到双足这个最苛刻场景，往后它为数据驱动派（第93章）提供物理语言和安全底座。读完本章再看后续的 RL 章节，你会发现奖励函数里的每一项几乎都能在本章找到出处。

经典 WBC 与 RL 的关系——向下一章的过渡¶

本章详细介绍了经典人形 WBC 的数学框架和工程实现。这套方法在结构化任务中表现优异：每个约束都有明确的物理含义，每个权重都有可调的工程直觉，调试时可以通过 ZMP 裕度、DCM 误差、力矩饱和等指标精确定位问题。

但经典方法也有明显局限：

局限	具体表现	根本原因
任务栈设计困难	挥手+走路+避障需要大量手工权重调整	多任务优先级的组合爆炸
动作自然性差	机器人动作"正确但僵硬"	二次代价函数无法捕捉"自然"这个模糊概念
依赖精确模型	URDF 参数错误 5% 就可能导致摔倒	QP 假设模型准确
高维任务空间	全身 43+ 关节的任务定义极其复杂	人工无法在高维空间中穷举所有情况

关键过渡 第93章将介绍人形全身 RL，它试图用数据驱动的方式解决上述问题。但请注意：RL 不是经典 WBC 的替代品，而是补充。好的人形 RL 系统的奖励设计仍然大量借鉴本章的 ZMP、DCM、CMM 等概念。理解了经典 WBC，才能理解 RL 奖励函数中每一项的物理含义。反过来，RL 学到的策略也可以被 WBC 的分析工具诊断——例如检查 RL 策略是否隐式维持了 ZMP 可行性。

本章和下一章构成一对互补的视角：经典方法提供可解释性和物理保证，数据驱动方法提供灵活性和自然性。真正的工程系统越来越多地采用混合架构——底层 WBC 保证安全，上层 RL 处理高维决策。

延伸阅读¶

论文/资源	难度	内容
Kajita et al., "Biped walking pattern generation by using preview control of ZMP", ICRA 2003	⭐⭐	Preview 控制的原始论文
Englsberger et al., "Three-dimensional bipedal walking control using DCM", IROS 2011	⭐⭐⭐	DCM/Capture Point 的系统化框架
Koolen et al., "Design of a Momentum-Based Control Framework", IJHR 2016	⭐⭐⭐⭐	IHMC Atlas 动量 WBC 的完整设计
Caron et al., "Stability of surface contacts for humanoid robots", ICRA 2015	⭐⭐⭐⭐	多接触稳定性的数学分析
Romualdi et al., "Benchmarking Whole-Body Controllers on TALOS", Frontiers 2022	⭐⭐⭐	TALOS 上三种 WBC 的系统对比
Feng & Atkeson, "Optimization-based Full Body Control for the DRC", JFR 2015	⭐⭐⭐⭐	DRC 竞赛中的双层优化方案
Orin & Goswami, "Centroidal Momentum Matrix of a humanoid robot", IROS 2008	⭐⭐⭐	CMM 的原始提出和高效算法
Vukobratovic & Juricic, "Contribution to the synthesis of biped gait", 1969	⭐⭐	ZMP 概念的原始提出
TSID 库源码：github.com/stack-of-tasks/tsid	⭐⭐⭐	Python/C++ 实现的参考
Pinocchio 文档：gepettoweb.laas.fr/doc/stack-of-tasks/pinocchio	⭐⭐	动力学计算后端

术语对照表¶

在阅读不同论文时，同一概念可能使用不同术语。以下对照表帮助避免混淆：

本章术语	英文	其他常见名称	出处
质心动量矩阵	Centroidal Momentum Matrix (CMM)	Composite Rigid Body Algorithm	Orin & Goswami 2008
发散运动分量	Divergent Component of Motion (DCM)	Capture Point (CP)	Englsberger 2011 / Pratt 2006
零力矩点	Zero Moment Point (ZMP)	动态平衡点	Vukobratovic 1969
虚拟排斥点	Virtual Repellent Point (VRP)	eCMP (enhanced CMP)	部分论文
分层二次规划	Hierarchical QP (HQP)	Lexicographic QP / Cascaded QP	Escande 2014
加速度-力形式	AccForce formulation	$(\\dot v, \\lambda)$ 形式	TSID
接触不滑约束	Non-slipping constraint	Holonomic contact constraint
浮动基约束	Underactuation constraint	Free-floating dynamics

跨语言注意 中文文献中"动量"通常指线动量 $l_G = m\dot r_{CoM}$。"角动量"需要显式说明。英文中 "momentum" 在人形控制语境下通常指 6D 动量 $h_G = [l_G; k_G]$（包含线动量和角动量）。阅读英文论文时需要根据上下文判断。

开源工具链参考¶

搭建本章教学 demo 需要以下软件：

工具	用途	安装方式	版本建议
Pinocchio	动力学计算	`pip install pin` 或源码编译	3.x
TSID	WBC QP 组装	`pip install tsid`	与 Pinocchio 版本匹配
ProxQP / qpOASES	QP 求解器	`pip install proxsuite`	最新稳定版
MuJoCo	仿真环境	`pip install mujoco`	3.x
Meshcat	可视化	`pip install meshcat`	最新
example-robot-data	URDF 库	`pip install example-robot-data`	与 Pinocchio 匹配

注意事项：Pinocchio 3.x 改变了部分 API（如 example-robot-data 的加载方式从 EXAMPLE_ROBOT_DATA_MODEL_DIR 变为 example_robot_data.load()）。如果参考旧教程或论文中的代码，需要检查 API 兼容性。

92 巩固卡片 01：LIPM 假设¶

问题：为什么恒定高度能线性化模型？
直觉：竖直力固定，水平力矩平衡变成线性关系。
数学抓手：$\omega=\sqrt{g/z_c}$
工程接口：CoM planner
易错判断：不能直接用于跳跃。
自测动作：改变高度比较发散速度。

92 巩固卡片 02：ZMP 可行性¶

问题：为什么要在脚掌内？
直觉：压力中心不能超出接触面。
数学抓手：$A_pp\le b_p$
工程接口：ZMP-MPC
易错判断：ZMP 在内不代表不滑。
自测动作：同时检查摩擦锥。

92 巩固卡片 03：角动量修正¶

问题：摆臂如何影响平衡？
直觉：改变 $\dot k_G$ 等效移动 ZMP。
数学抓手：$+\dot k_y/(mg)$
工程接口：Momentum task
易错判断：角动量过大也会带来恢复难题。
自测动作：画出 ZMP 偏移。

92 巩固卡片 04：DCM¶

问题：为什么定义 $x+\dot x/\omega$？
直觉：隔离发散模态。
数学抓手：$\dot\xi=\omega(\xi-p)$
工程接口：落脚规划
易错判断：高度错会让 DCM 错。
自测动作：实时更新 $\omega$。

92 巩固卡片 05：CMM¶

问题：如何汇总全身动量？
直觉：每个连杆动量变换到质心后求和。
数学抓手：$h_G=A_Gv$
工程接口：Pinocchio ccrba
易错判断：只看 CoM 会漏角动量。
自测动作：比较手臂构型。

92 巩固卡片 06：HQP¶

问题：为什么不用全靠权重？
直觉：高优先级残差必须被保护。
数学抓手：字典序最小化
工程接口：TSID
易错判断：权重过大会污染稳定任务。
自测动作：降低手臂权重观察 ZMP。

研究实践建议¶

学完本章后如何把知识转化为研究/工程能力，按投入程度分三层：

层次	建议动作	产出
入门（理解）	把 §92.2/§92.5/§92.7 的 LIPM、DCM、CMM 用不超过 50 行 Python 各自复现一遍，画出发散曲线和零空间运动	对三个核心量的数值直觉
进阶（集成）	完成本章综合项目：脚印 + DCM + TSID 的迈步 demo，并把 §92.13.5 的接触检测/状态估计加进去	一套可复用的双足 WBC 积木
研究（推进）	选一个本章的"工程折中"做消融——例如把摩擦金字塔换成 SOCP、把 LIPM 换成 centroidal MPC，量化收益与代价	一份可写进论文的对比实验

给初学者的三条务实建议：

先让它站住，再让它走，最后才让它干活。 不要一上来就实现完整行走+操作。先做站立平衡（只有接触+CoM+姿态任务），确认 ZMP 裕度稳定；再加摆脚和 DCM 实现迈步；最后才加手臂操作任务。每一步都验证验收指标（§92.13），出问题立刻能定位是哪一层引入的。
把指标做成实时面板。 ZMP 裕度、DCM 误差、QP 求解时间、各接触力到摩擦锥/CoP 边界的距离——这些不要等出事才打印，要做成实时曲线。本章反复强调的"先看指标再改算法"，前提是指标一直可见。
仿真和实机的差距优先查估计层。 仿真跑通、实机翻车时，按 §92.13.5 的铁律"先确认输入对，再怀疑算法"：先核对 $q,v$ 和接触集是不是干净的，再去动 QP。

近年进展（2022-2025）¶

本章的经典框架仍是 2025 年人形控制的主干，但有三条值得关注的演进线，它们都建立在本章概念之上而非取代它：

Centroidal / 全身非线性 MPC 取代 LIPM。 越来越多工作用质心动力学甚至全身运动学作为 MPC 的预测模型，而不再停留在 LIPM 的两条强假设上。这类方法把手臂和腰的动力学直接放进规划模型，实现真正"全身协调"的行走（而非"行走+附加手臂动作"），并已在 Unitree G1/H1 等电驱人形上实机验证，典型架构是高层落脚规划约 40 Hz、中层 MPC 约 500 Hz 在板载小型 PC 上实时运行。它对本章的关系是：把 §92.2 的 LIPM 升级为更高保真的预测模型，但 §92.5 的 DCM/步调整、§92.8 的 WBC 下游执行依然原样保留。
学习增强的 WBC（残差策略）。 一类做法把本章的 WBC 当作底层基控制器，用 RL 学一个**残差策略**去微调 WBC 输出的关节指令，兼得 WBC 的物理安全和 RL 的鲁棒性/自然性；另一类则让 RL 输出高层目标（如关键点轨迹、动量参考）交给下游 WBC 执行。两类都印证了本章的判断——WBC 是混合系统的"安全底座"，而非被淘汰的旧方法。
接触失效感知的 WBC。 面向真实地形的不可预期接触失败（踩空、打滑），出现了把意外接触显式纳入加权 QP 的方案（如碰撞感知 WBC）。这本质是 §92.13.5 接触检测与 §92.8 接触约束的工程强化，思路与本章一脉相承。

本质洞察 这三条演进线没有一条丢掉了本章的核心量：它们要么把 LIPM 换成更准的模型（但仍算 ZMP/DCM 类指标），要么把 WBC 包进学习框架（但仍用它保安全），要么强化接触处理（但仍是接触约束那套）。这印证了本章开头的论断——经典人形 WBC 是"所有混合方法的安全底座"。学透本章，不是学一套会过时的旧算法，而是学懂这个领域所有新方法共享的物理语言。

前沿指引：经典全身控制的演进方向¶

本章介绍的 LIPM/ZMP/DCM + HQP/WBC 框架是人形全身控制的经典基础，但这一领域正在经历从"纯优化"到"优化+学习"的范式融合。学习者在掌握本章内容后，可关注以下方向：（1）用 RL 自动调优 WBC 的优先级权重和约束边界，替代人工调参；（2）用 Centroidal 动力学（而非 LIPM）作为 MPC 的预测模型，支持更激进的全身运动（如跑步、跳跃）；（3）将 Diffusion Policy 与 WBC 结合——用 Diffusion Policy 生成全身关键点轨迹，WBC 负责将轨迹转化为满足动力学约束的关节力矩。经典框架的价值不会消失，它是所有混合方法的"安全底座"。

一句话总结：经典人形全身控制的核心是"用优先级保护安全，用优化分配资源"。无论上层策略如何变化，这个底层原则不会过时。

—— 本章终 ——

阶段	内容	时间
入门	92.1-92.3，理解 LIPM/ZMP/DCM 的物理直觉	1 周
核心	92.4-92.8，掌握 Preview/DCM/TSID 的数学细节	1 周
进阶	92.9-92.12，理解 IHMC 架构和多接触扩展	0.5 周
实践	综合项目，搭建教学 demo	0.5 周

假设	数学表达	简化效果	代价
恒定质心高度	\(z=z_c,\ddot z=0\)	竖直力固定为 \(mg\)	不能描述下蹲与跳跃
点质量	质量集中于 CoM	关节细节消失	无法直接求力矩
角动量变化小	\(\dot k_G\approx0\)	ZMP 与 CoM 加速度直接相关	摆臂和扭腰被忽略
平面足底	\(p_{zmp}\in\mathcal P\)	稳定性变成几何约束	忽略足底柔性

约束	形式	意义
步长	\(\\|x_{next}-x_{stance}\\|\le L_{max}\)	髋关节可达
步宽	\(W_{min}\le \\|y_{next}-y_{stance}\\|\le W_{max}\)	避免交叉腿
转角	\(\\|\psi_{next}-\psi_{stance}\\|\le\psi_{max}\)	足底朝向连续
地形	\(A_{terrain}p\le b_{terrain}\)	避开不可落脚区域
时间	\(T_{swing}\ge T_{min}\)	摆腿轨迹可执行

任务	Jacobian	参考	优先级
支撑脚	\(J_c\)	零加速度	硬约束
CoM	\(J_{com}\)	LIPM/DCM 参考	高
角动量	\(A_G^{ang}\)	\(\dot k_G^d\)	高
摆动脚	\(J_{swing}\)	落脚轨迹	中高
双臂末端	\(J_{ee}\)	操作位姿	中
关节姿态	\(I\)	默认姿态	低

方案	控制级别	QP 变量	优点	局限
TSID-Acc	加速度级	\([\dot v, \lambda]\)	任务定义直观	力矩精度依赖逆动力学模型
TSID-Torque	力矩级	\([\tau, \lambda]\)	直接控制力矩	QP 维度更大
位置级 WBC	关节角级	\([\Delta q]\)	实现简单	不满足动力学约束

判据	原理	陷阱
足底力阈值	法向力 \(f_n>f_{thresh}\) 即判接触	阈值太低误触发、太高漏检；落地瞬间力有冲击尖峰
相位时钟	按步态相位表硬切接触	地形不平时实际落地早于/晚于时钟，导致"踩空"或"硬怼"
运动学高度	脚低于地面估计高度即接触	依赖地面高度已知；斜面/台阶易错

编号	公式	含义	使用位置
E1	\(\ddot x=\omega_0^2(x-p)\)	LIPM	CoM/ZMP 规划
E2	\(p_x=x-z\ddot x/(g+\ddot z)+\dot k_y/[m(g+\ddot z)]\)	含角动量 ZMP	摆臂与强扰动
E3	\(\xi=x+\dot x/\omega_0\)	DCM / Capture Point	落脚判断
E4	\(\dot\xi=\omega_0(\xi-p)\)	DCM 动力学	反馈控制
E5	\(h_G=A_G(q)v\)	CMM 定义	动量任务
E6	\(\dot h_G=A_G\dot v+\dot A_Gv\)	动量加速度形式	WBC
E7	\(M\dot v+h=S^T\tau+J_c^T\lambda\)	全身动力学	QP 硬约束
E8	\(J_c\dot v+\dot J_cv=0\)	接触不滑	支撑脚
E9	\(\tau=S(M\dot v+h-J_c^T\lambda)\)	反推力矩	TSID 输出
E10	\(p_{next}=p_{cur}+e^{\omega_0 T_{step}}(\xi_{now}-p_{cur})\)	解析 DCM 落脚	一步可捕获
E11	\(N_k=I-J_k^{+}J_k\)，下层在 \(N_{1:k-1}\) 内求解	HQP 零空间投影	严格优先级
E12	\(J^{+}_M=M^{-1}J^T(JM^{-1}J^T)^{-1}\)	动力学一致伪逆	浮动基投影

来源章节	本章复用的模块	在本章 demo 中的角色
复合/20	浮动基动力学 \(M,h,J_c\) 与 CMM 计算	demo 阶段 4 的 Pinocchio 调用直接复用
足式/90	WBC/QP 任务栈与 HQP 求解器封装	demo 阶段 5 的 QP 组装在其上扩展角动量任务
足式/70	倒立摆/SRBD 简化模型	demo 阶段 2 的 DCM 规划是其双足特化