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

第 51 章：腿足简化模型理论——从 LIPM 到 Centroidal Model¶

本章定位：这是腿足运动控制的物理基石。全身动力学（Ch49）维度太高无法实时求解 MPC，简化模型通过物理假设将 48 维系统降至 2-12 维，使实时规划成为可能。本章覆盖从最简单的 LIPM（2 维）到最精确的 Centroidal Model（12 维）的完整模型层级。

前置依赖：Ch11（Eigen）、Ch47（Pinocchio）、Ch49（浮动基座动力学/Centroidal Momentum）、Ch50（QP/NLP 建模）

关键文献：Kajita 2003 ICRA, Pratt 2006 (Capture Point), Koolen 2012 (Capturability), Orin 2013 (Centroidal), Di Carlo 2018 (MIT Cheetah 3 SRBD)

51.0 前置自测¶

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

[Ch49] 写出浮动基座全身动力学方程 $M(q)\ddot{q} + h(q,\dot{q}) = S^T\tau + J_c^T\lambda$。Go2 的 $n_v$ 是多少？
[Ch49] 什么是质心动量矩阵（CMM）$A_G(q)$？它与广义速度的关系是什么？
[Ch50] QP 标准形式是什么？写出 KKT 条件。
[物理] 什么是"零力矩点"（ZMP）？为什么 ZMP 在支撑多边形内是稳定行走的充分条件？
[控制] 什么是"质心"（CoM）？如何用 Pinocchio 计算一个多体系统的 CoM 位置？

51.0.1 本章目标¶

学完本章后，你应该能够：

**解释**为什么全身动力学无法直接用于实时 MPC，简化模型通过哪些物理假设实现维度降阶
推导 LIPM 的运动方程 $\ddot{x} = \omega^2(x - p)$，解释 ZMP/CoP/DCM 三者的物理含义与数学关系
构造 SRBD 模型的状态空间方程并将其离散化为 QP 约束——这是四足 trot MPC（Di Carlo 2018）的核心
区分 SRBD 与 Centroidal 模型的建模差异，理解为什么 Centroidal 模型能处理高动态运动（如跳跃、翻转）
使用 Pinocchio 计算 Go2 的 CoM、CMM，验证简化模型输出与全身动力学的一致性

51.1 维度灾难与简化模型的动机 ⭐¶

这一节解决什么问题：为什么不能直接用全身动力学做实时 MPC？简化模型的核心思想是什么？

全身动力学的维度挑战¶

Go2 四足机器人的全身状态： - 配置：$q \in \mathbb{R}^{19}$（7 浮基 + 12 关节） - 速度：$v \in \mathbb{R}^{18}$ - 完整状态：$(q, v) \in \mathbb{R}^{37}$ - 控制输入：$\tau \in \mathbb{R}^{12}$（12 个电机） - 接触力：$\lambda \in \mathbb{R}^{12}$（4 脚 × 3D 力）

MPC 需要在每个时间步优化所有这些变量。对于 $N=20$ 步 horizon： - 决策变量总数：$20 \times (18 + 12 + 12) = 840$ - QP 的 Hessian 矩阵：$840 \times 840$ - 单步 QP 求解时间：~50-100 ms（远超 10 ms 预算）

平台	状态维度	MPC 决策变量 (N=20)	全身 QP 求解	可行？
Go2 (18-DOF)	37	840	~80 ms	❌ 不实时
H1 (25-DOF)	51	1200	~300 ms	❌ 完全不可行
LIPM (2-DOF)	4	80	~0.1 ms	✅ 轻松
SRBD (6-DOF)	13	260	~3 ms	✅ 可行
Centroidal (6-DOF)	12	240	~5 ms	✅ 可行

简化模型的核心思想¶

不建模关节——只建模质心。这好比公司管理中的"只关心 KPI，不管具体操作"——CEO（MPC 规划层）只需要知道"营收目标是多少"（质心应到哪里），不需要知道每个员工具体怎么干活（每个关节怎么动），执行细节交给部门经理（WBC）。

步态规划和平衡控制本质上只关心**整体动量**（质心在哪里、往哪里动），而不关心每个关节的具体角度。关节角度是"如何实现这个动量目标"的执行细节——这交给下层 WBC 处理。

决策层级:
  规划层 → "质心应该从这里移到那里"（简化模型 MPC）
  控制层 → "为了实现这个质心轨迹，各关节应该施加多大力矩"（WBC/Ch53）

模型层级总览¶

模型	状态维度	核心假设	捕获的物理	典型用途
LIPM	2-4	恒定 CoM 高度、点质量	线性平动	步行规划
DCM	2	LIPM + 发散分量分离	一阶不稳定性	步距规划
SLIP	4	弹簧腿、飞行相	垂直弹跳	跑步/跳跃
SRBD	12-13	整体单刚体	平动+转动	四足 trot
Centroidal	12	精确质心动量	完整 6D 动量	通用腿足

⚠️ 常见陷阱：简化模型只适用于 MPC 规划层。不要试图直接用简化模型的输出驱动电机——还需要 WBC 将"期望质心力/力矩"转换为"关节力矩"。这是初学者最常犯的错误。

练习¶

[估算题] Atlas 人形机器人有 28 个关节。如果直接做全身 MPC（horizon=20），估算 QP 的决策变量数量和求解时间。
[概念题] 为什么飞行相（如跑步的腾空期）不能用 LIPM 建模？需要什么模型？

51.2 LIPM——线性倒立摆模型 ⭐⭐⭐¶

这一节解决什么问题：推导所有步态规划的数学基石——LIPM。从"为什么倒立摆不稳定"出发，得到质心运动的线性 ODE。

物理设置¶

想象最简单的双足步行模型： - 一个点质量 $m$ 代表机器人全部质量（集中在质心 CoM） - 一条无质量的刚性支撑腿连接 CoM 和地面支撑点 $p$ - 假设 CoM 高度恒定 $z = h$（不考虑上下起伏）

           m (CoM)
          /|
         / |  h = const
        /  |
       /   |
      p────┘ (支撑点/CoP)
─────────────── 地面

从牛顿定律到 LIPM 方程¶

Step 1：对点质量 $m$ 在 $x$ 方向列牛顿第二定律。

作用在质量上的力只有重力 $mg$ 和支撑腿的约束力 $F$。由于假设 $z = h = \text{const}$（竖直方向加速度为零），竖直力平衡给出：

\[F_z = mg\]

支撑力沿腿方向（从 $p$ 指向 $m$），因此水平分量：

\[F_x = F \cdot \frac{x - p}{l} = mg \cdot \frac{x - p}{h}\]

（其中 $l$ 是腿长，$h/l \approx 1$ 在小角度近似下）

Step 2：牛顿第二定律 $F_x = m\ddot{x}$：

\[m\ddot{x} = mg \cdot \frac{x - p}{h}\]

化简：

\[\boxed{\ddot{x} = \frac{g}{h}(x - p) = \omega^2(x - p)}\]

其中 $\omega = \sqrt{g/h}$ 是 LIPM 的**自然频率**。

Step 3：这是一个关于 CoM 位置 $x$ 的**线性 ODE**（因为 $p$ 是控制输入）。

为什么这个方程描述"不稳定"¶

LIPM 方程的齐次解（$p = 0$）：

\[x(t) = A e^{\omega t} + B e^{-\omega t}\]

两个模态： - $e^{\omega t}$：发散模态（exponentially growing）— 摔倒！ - $e^{-\omega t}$：收敛模态（exponentially decaying）— 自稳定

任何微小扰动都会激发发散模态 → 倒立摆本质不稳定 → 必须主动控制。

这就是为什么双足行走需要实时控制——与轮式机器人的本征稳定性完全不同。倒立摆的不稳定性就像在手指尖上立一根棍子：你必须不断调整手指位置（CoP）来抵消棍子（CoM）的发散趋势，一旦停止控制，棍子立刻倒下。轮式机器人则像把球放在碗底——即使不控制，球也会自然回到平衡点。假如没有这种主动平衡控制，双足机器人在任何微小扰动下都会在不到一秒内倒地——$e^{\omega t}$ 的指数增长在 $\omega \approx 3.5$ rad/s 时，0.5 秒内扰动就会放大 6 倍。

状态空间形式¶

定义状态 $\mathbf{x} = (x, \dot{x})^T$，控制 $u = p$（CoP 位置）：

\[\dot{\mathbf{x}} = \begin{pmatrix} 0 & 1 \\ \omega^2 & 0 \end{pmatrix} \mathbf{x} + \begin{pmatrix} 0 \\ -\omega^2 \end{pmatrix} u\]

矩阵 $A$ 的特征值为 $\pm\omega$：一正一负，确认系统不稳定。

离散化（MPC 必需）¶

Zero-order hold 离散化，采样时间 $\Delta t$：

\[\mathbf{x}_{k+1} = A_d \mathbf{x}_k + B_d u_k\]

\[A_d = e^{A\Delta t} = \begin{pmatrix} \cosh(\omega\Delta t) & \frac{1}{\omega}\sinh(\omega\Delta t) \\ \omega\sinh(\omega\Delta t) & \cosh(\omega\Delta t) \end{pmatrix}\]

\[B_d = \begin{pmatrix} 1 - \cosh(\omega\Delta t) \\ -\omega\sinh(\omega\Delta t) \end{pmatrix}\]

历史与意义¶

1991：Kajita 首次将 LIPM 应用于步行机器人
2001：Kajita et al. 提出 3D-LIPM（添加 $y$ 方向，仍然解耦）
2003：Kajita ICRA — 预览控制（→ 51.4 节详述），在 HRP-2 上实现稳定行走
至今：LIPM 仍是几乎所有双足/四足步态规划器的基础模型

LIPM 的局限性¶

局限	后果	解决方案
恒定 CoM 高度	不能跳跃/下蹲	SLIP 模型 (51.6)
忽略角动量	不能甩臂/翻跟头	SRBD/Centroidal (51.7-8)
点质量	不能预测关节力矩	WBC (Ch53)
平面运动	$x$ 和 $y$ 解耦	3D-LIPM（简单扩展）

⚠️ 常见陷阱：LIPM 假设 CoM 高度恒定，这意味着它**不适用于**奔跑（有飞行相）、跳跃（CoM 大幅变化）、或爬楼梯（$h$ 变化）。对这些场景需要 SLIP 或 SRBD。

Pinocchio 中的 CoM 计算¶

#include <pinocchio/algorithm/center-of-mass.hpp>

// 计算 CoM 位置
pinocchio::centerOfMass(model, data, q);
Eigen::Vector3d com = data.com[0];  // 整体 CoM

// 计算 CoM 位置 + 速度
pinocchio::centerOfMass(model, data, q, v);
Eigen::Vector3d com_vel = data.vcom[0];  // CoM 速度

// 计算 CoM 的 Jacobian: v_com = J_com * dq
pinocchio::jacobianCenterOfMass(model, data, q);
Eigen::MatrixXd J_com = data.Jcom;  // 3 x nv

C++ 实现：LIPM 离散模拟¶

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

// LIPM 离散系统
// 实现 Zero-Order Hold 离散化并模拟质心轨迹
struct LIPMDiscrete {
    double omega;   // 自然频率 sqrt(g/h)
    double dt;      // 采样时间
    Eigen::Matrix2d Ad;  // 离散状态转移矩阵
    Eigen::Vector2d Bd;  // 离散输入矩阵

    LIPMDiscrete(double h, double g, double dt)
        : omega(std::sqrt(g / h)), dt(dt)
    {
        double ch = std::cosh(omega * dt);
        double sh = std::sinh(omega * dt);
        // 状态转移矩阵 Ad = exp(A*dt)
        Ad << ch,           sh / omega,
              omega * sh,   ch;
        // 输入矩阵 Bd
        Bd << 1.0 - ch,
              -omega * sh;
    }

    // 单步前向传播
    // state = [x, xdot], u = CoP 位置
    Eigen::Vector2d step(const Eigen::Vector2d& state, double u) const {
        return Ad * state + Bd * u;
    }

    // 多步模拟
    // 返回 (N+1) 个状态向量，每个是 [x, xdot]
    std::vector<Eigen::Vector2d> simulate(
        const Eigen::Vector2d& x0,
        const std::vector<double>& cop_sequence)
    {
        std::vector<Eigen::Vector2d> trajectory;
        trajectory.reserve(cop_sequence.size() + 1);
        trajectory.push_back(x0);

        Eigen::Vector2d state = x0;
        for (double u : cop_sequence) {
            state = step(state, u);
            trajectory.push_back(state);
        }
        return trajectory;
    }
};

// 使用示例
int main() {
    LIPMDiscrete lipm(0.8, 9.81, 0.01);  // h=0.8m, g=9.81, dt=10ms
    Eigen::Vector2d x0(0.0, 0.1);        // x=0, xdot=0.1 m/s
    std::vector<double> cop(200, 0.0);    // CoP 固定原点, 模拟 2 秒
    auto traj = lipm.simulate(x0, cop);
    // omega~3.5, 2秒后 CoM 发散到 ~15.7 m (指数增长!)
    std::cout << "Final CoM: " << traj[200](0) << " m\n";
    return 0;
}

练习¶

[推导题] 对 $y$ 方向重复上述推导，验证 3D-LIPM 中 $x$ 和 $y$ 完全解耦（独立的两个二阶 ODE）
[编程题] 实现 LIPM 离散化，设 $h=0.8$ m, $\Delta t=10$ ms。给定初始 $x_0=0, \dot{x}_0=0.1$ m/s，CoP 固定在原点，模拟 2 秒，观察 CoM 的指数发散
[思考题] 为什么四足机器人（Go2）也能用 LIPM，尽管它有 4 条腿？提示：考虑"等效支撑点"的概念

51.3 CoM、CoP 与 ZMP——三个关键概念 ⭐⭐¶

这一节解决什么问题：精确定义三个容易混淆的概念（CoM/CoP/ZMP），推导它们的数学关系，以及 ZMP 稳定性判据。

CoM（质心, Center of Mass）¶

\[\mathbf{r}_{\text{CoM}} = \frac{\sum_i m_i \mathbf{r}_i}{\sum_i m_i}\]

对于多刚体系统（机器人），每个连杆 $i$ 贡献其质量 $m_i$ 和质心位置 $\mathbf{r}_i$。Pinocchio 的 centerOfMass() 精确计算这个量。

CoP（压力中心, Center of Pressure）¶

CoP 定义为地面反力（GRF）的合力作用点——在该点，地面对机器人的总力矩的水平分量为零：

\[\mathbf{p}_{\text{CoP}} = \frac{\sum_i \mathbf{p}_i f_{z,i}}{\sum_i f_{z,i}}\]

其中 $\mathbf{p}_i$ 是第 $i$ 个接触点，$f_{z,i}$ 是该点的法向力。

物理直觉：如果你站在一块板上，CoP 就是板对你施力的"加权平均位置"。双脚站立时 CoP 在两脚之间；单脚站立时 CoP 在脚掌范围内。

ZMP（零力矩点, Zero Moment Point）¶

ZMP 概念由 Vukobratovic 与 Juricic 于 1968-1969 年提出，正式术语"零力矩点"确立于 1972 年。其定义为：地面上使得合力矩的水平分量为零的点。

从 Newton-Euler 在 ZMP 点列力矩平衡（水平分量）：

\[\sum_i (\mathbf{p}_i - \mathbf{p}_{\text{ZMP}}) \times f_i + \mathbf{n}_i = 0 \quad (\text{水平分量})\]

ZMP = CoP 的条件¶

定理：当所有接触点在同一平面上（平地行走）时，ZMP = CoP。

这是因为：在平地上，所有接触力都是竖直的（法向力），力矩只由水平力臂产生。CoP 的定义正好使水平力矩为零——这与 ZMP 的定义相同。

ZMP 稳定性判据¶

定理（Vukobratovic & Juricic, 1968-1972）：如果 ZMP 位于支撑多边形内部，机器人不会翻倒。

直觉理解： - ZMP 在支撑多边形**内部** → 存在合法的接触力分布可以平衡机器人 → 稳定 - ZMP 在支撑多边形**边界** → 某些脚的法向力为零（即将抬起）→ 临界 - ZMP 在支撑多边形**外部** → 不存在合法的接触力 → 必然翻倒

ZMP 公式推导¶

从 CoM 动力学推导 ZMP 位置（x 方向）：

\[p_{\text{ZMP},x} = x_{\text{CoM}} - \frac{z_{\text{CoM}} \cdot \ddot{x}_{\text{CoM}} - \dot{L}_y / m}{g + \ddot{z}_{\text{CoM}}}\]

其中 $\dot{L}_y$ 是绕 $y$ 轴的角动量变化率，$m$ 是总质量。注意 $\dot{L}_y$ 的量纲为 $\text{kg}\cdot\text{m}^2/\text{s}^2$，除以 $m$ 后才与 $z \ddot{x}$（量纲 $\text{m}^2/\text{s}^2$）量纲一致。

在 LIPM 假设下（$z = h = \text{const}$, $\ddot{z} = 0$, $\dot{L}_y = 0$）：

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

这正是 LIPM 方程的另一种写法！（代入 $\ddot{x} = \omega^2(x-p)$ 可验证 $p_{\text{ZMP}} = p_{\text{CoP}}$）

C++ 实现：ZMP 计算¶

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

// 从接触力计算 CoP（平地）: CoP = sum(p_i * fz_i) / sum(fz_i)
Eigen::Vector2d computeCoP(
    const std::vector<Eigen::Vector3d>& contacts,
    const std::vector<Eigen::Vector3d>& forces)
{
    double total_fz = 0.0;
    Eigen::Vector2d cop = Eigen::Vector2d::Zero();
    for (size_t i = 0; i < contacts.size(); ++i) {
        double fz = forces[i].z();
        if (fz > 1e-6) {
            cop += contacts[i].head<2>() * fz;
            total_fz += fz;
        }
    }
    return (total_fz > 1e-6) ? cop / total_fz : cop;
}

// 从 CoM 动力学计算 ZMP（含角动量项）
// 注意：L_dot 是角动量变化率 (kg·m²/s²)，需要除以总质量 m 才能与 z·ẍ 量纲一致
Eigen::Vector2d computeZMP(const Eigen::Vector3d& com,
                           const Eigen::Vector3d& com_ddot,
                           const Eigen::Vector3d& L_dot,
                           double mass,
                           double g = 9.81)
{
    double denom = g + com_ddot.z();  // 自由落体时无定义
    Eigen::Vector2d zmp;
    zmp.x() = com.x() - (com.z()*com_ddot.x() - L_dot.y()/mass) / denom;
    zmp.y() = com.y() - (com.z()*com_ddot.y() + L_dot.x()/mass) / denom;
    return zmp;
}

⚠️ 常见陷阱：ZMP 稳定性判据是**必要条件而非充分条件**。ZMP 在支撑多边形内只保证"不翻倒"，不保证"不摔倒"（滑动、关节力矩饱和等其他失败模式仍可能发生）。

练习¶

[推导题] 从 Newton-Euler 方程出发，推导含角动量项的完整 ZMP 公式
[编程题] 用 Pinocchio 计算 Go2 在 trot 步态中的实时 ZMP 位置（需要 CoM 加速度和角动量），与支撑多边形对比绘图

51.4 ZMP 预览控制——Kajita 2003 ⭐⭐⭐¶

这一节解决什么问题：如何利用 LIPM + ZMP 约束实现稳定行走？Kajita 的预览控制是第一个实用的答案。

问题设定¶

目标：给定**期望的 ZMP 轨迹** $p_{\text{ref}}(t)$（由步态规划器生成），求 CoM 轨迹 $x(t)$ 使得实际 ZMP 跟踪参考。

这是一个**逆问题**：已知输出（ZMP），求输入（CoM 运动）。

Cart-Table 模型¶

Kajita 使用一个等价的物理直觉——"推车-桌子"模型：

想象一个质量为 $m$ 的推车在桌子上运动。桌子的 ZMP 就是桌面施力在推车上的等效作用点。

Cart-Table 模型的状态方程（用 CoM 的 jerk $\dddot{x}$ 作为输入）：

\[\frac{d}{dt}\begin{pmatrix} x \\ \dot{x} \\ \ddot{x} \end{pmatrix} = \begin{pmatrix} 0 & 1 & 0 \\ 0 & 0 & 1 \\ 0 & 0 & 0 \end{pmatrix} \begin{pmatrix} x \\ \dot{x} \\ \ddot{x} \end{pmatrix} + \begin{pmatrix} 0 \\ 0 \\ 1 \end{pmatrix} u\]

输出方程（ZMP）：

\[p = x - \frac{h}{g}\ddot{x} = \begin{pmatrix} 1 & 0 & -h/g \end{pmatrix} \begin{pmatrix} x \\ \dot{x} \\ \ddot{x} \end{pmatrix}\]

输入 $u = \dddot{x}$ 是 CoM 的 jerk（加速度的导数）。

Kajita 的增广系统¶

离散化后（采样时间 $\Delta t$），定义误差 $e_k = p_k - p_{\text{ref},k}$。

构建**增广状态**来包含误差积分：

\[\tilde{\mathbf{x}}_k = \begin{pmatrix} e_k \\ x_k \\ \dot{x}_k \\ \ddot{x}_k \end{pmatrix}\]

增广系统：

\[\tilde{\mathbf{x}}_{k+1} = \tilde{A} \tilde{\mathbf{x}}_k + \tilde{B} u_k + \tilde{F} p_{\text{ref},k+1}\]

LQR + 预览控制律¶

代价函数：

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

最优控制律（含预览项）：

\[u_k = -K_I \sum_{i=0}^{k} e_i - K_x \mathbf{x}_k - \sum_{j=1}^{N_p} G_j p_{\text{ref},k+j}\]

其中： - $K_I$：误差积分增益 - $K_x = (K_{x1}, K_{x2}, K_{x3})$：状态反馈增益 - $G_j$：第 $j$ 步的**预览增益**

预览增益的计算¶

预览增益通过离散 Riccati 方程的解 $P$ 求得：

\[G_j = (R + \tilde{B}^T P \tilde{B})^{-1} \tilde{B}^T (\tilde{A} - \tilde{B} K)^{j-1} P \tilde{F}\]

关键性质：$G_j$ 随 $j$ 指数衰减——越远的未来信息影响越小。通常取预览窗口 $N_p \approx 1.5$ 秒即足够（更远的 $G_j \approx 0$）。

为什么预览控制是革命性的¶

1991 之前：双足机器人只能静态站立或极慢行走
2003 Kajita：HRP-2 首次实现动态行走（利用 ZMP 预览控制）
意义：证明了 LIPM + LQR + preview 足以实现稳定行走

代码实现¶

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

// Kajita 预览控制器
// 基于 Cart-Table 模型 + LQR + Preview
class KajitaPreviewController {
public:
    double h_;    // CoM 高度
    double g_;    // 重力
    double dt_;   // 采样周期
    int Np_;      // 预览步数

    // 离散系统矩阵 (Cart-Table)
    Eigen::Matrix3d Ad_;
    Eigen::Vector3d Bd_;
    Eigen::RowVector3d Cd_;  // ZMP 输出

    // 增益
    double K_I_;             // 误差积分增益
    Eigen::RowVector3d Kx_;  // 状态反馈增益
    Eigen::VectorXd Gp_;    // 预览增益

    // 内部状态
    double error_sum_;       // 误差积分
    Eigen::Vector3d state_;  // [x, xdot, xddot]

    KajitaPreviewController(double h, double g, double dt, int Np,
                           double Qe = 1.0, double R = 1e-6)
        : h_(h), g_(g), dt_(dt), Np_(Np), error_sum_(0.0)
    {
        state_.setZero();

        // 离散化 Cart-Table 模型
        Ad_ << 1.0, dt_, dt_*dt_/2.0,
               0.0, 1.0, dt_,
               0.0, 0.0, 1.0;
        Bd_ << dt_*dt_*dt_/6.0, dt_*dt_/2.0, dt_;
        Cd_ << 1.0, 0.0, -h_/g_;

        // 求解 Riccati 方程得到增益
        solveRiccatiAndGains(Qe, R);
    }

    // 计算控制输入（jerk）
    double compute(double zmp_error,
                  const std::vector<double>& zmp_ref_future) {
        error_sum_ += zmp_error;

        // u = -K_I * sum_e - Kx * x - sum(Gp * p_ref_future)
        double u = -K_I_ * error_sum_ - Kx_ * state_;

        // 预览项
        int preview_steps = std::min((int)zmp_ref_future.size(), Np_);
        for (int j = 0; j < preview_steps; ++j) {
            u -= Gp_(j) * zmp_ref_future[j];
        }
        return u;
    }

    // 更新状态
    void updateState(double u) {
        state_ = Ad_ * state_ + Bd_ * u;
    }

    // 获取当前 CoM 位置
    double getCoMPosition() const { return state_(0); }

    // 获取当前 ZMP
    double getZMP() const { return Cd_ * state_; }

private:
    void solveRiccatiAndGains(double Qe, double R) {
        // 构建增广系统 [e; x; xdot; xddot]
        // At(4x4), Bt(4x1) 包含误差积分动力学
        Eigen::Matrix4d At = Eigen::Matrix4d::Zero();
        At(0,0) = 1.0;
        At.block<1,3>(0,1) = Cd_ * Ad_;
        At.block<3,3>(1,1) = Ad_;
        Eigen::Vector4d Bt;
        Bt << (Cd_ * Bd_)(0), Bd_;

        // 迭代 Riccati: P = Q + At'*P*At - At'*P*Bt*(R+Bt'*P*Bt)^-1*Bt'*P*At
        Eigen::Matrix4d Qt = Eigen::Matrix4d::Zero();
        Qt(0,0) = Qe;
        Eigen::Matrix4d P = Qt;
        for (int i = 0; i < 5000; ++i) {
            auto P_new = Qt + At.transpose()*P*At
                - At.transpose()*P*Bt * (1.0/(R + Bt.dot(P*Bt)))
                  * Bt.transpose()*P*At;
            if ((P_new - P).norm() < 1e-12) break;
            P = P_new;
        }

        // 提取 K_I, Kx, 预览增益 Gp
        double d = R + Bt.dot(P * Bt);
        Eigen::Vector4d K = (1.0/d) * (Bt.transpose() * P * At).transpose();
        K_I_ = K(0);  Kx_ = K.tail<3>().transpose();
        Gp_.resize(Np_);
        Eigen::Matrix4d Acl = At - Bt * K.transpose();
        Eigen::Vector4d Ft = Eigen::Vector4d::Zero(); Ft(0) = -1.0;
        Eigen::Vector4d tmp = P * Ft;
        for (int j = 0; j < Np_; ++j) {
            Gp_(j) = (1.0/d) * Bt.dot(tmp);
            tmp = Acl.transpose() * tmp;
        }
    }
};

使用示例¶

// 使用预览控制器
KajitaPreviewController ctrl(0.8, 9.81, 0.01, 150);  // h, g, dt, Np

// 生成方波 ZMP 参考（左右脚交替，每 0.5 秒切换）
std::vector<double> zmp_ref(650, 0.0);
for (int k = 0; k < 650; ++k)
    zmp_ref[k] = ((k / 50) % 2 == 0) ? 0.05 : -0.05;

// 控制循环
for (int k = 0; k < 500; ++k) {
    double error = ctrl.getZMP() - zmp_ref[k];
    std::vector<double> future(zmp_ref.begin()+k+1, zmp_ref.begin()+k+151);
    double u = ctrl.compute(error, future);
    ctrl.updateState(u);
}

练习¶

[推导题] 推导增广系统的完整状态空间方程，验证增广矩阵 $\tilde{A}, \tilde{B}, \tilde{F}$ 的表达式
[编程题] 实现完整的预览控制器：生成方波 ZMP 参考（模拟左右脚交替），模拟 CoM 轨迹，观察预览窗口大小对跟踪性能的影响
[对比题] 将预览窗口从 1.5 秒减少到 0（即纯反馈，无预览）。对比 ZMP 跟踪误差，解释为什么"预知未来"如此重要

51.5 DCM——发散分量与 Capture Point ⭐⭐⭐¶

这一节解决什么问题：将 LIPM 的二阶系统分解为"发散"和"收敛"两个一阶系统。DCM 是现代步态规划器的核心状态变量。

从 LIPM 到 DCM¶

回顾 LIPM 的解：$x(t) = A e^{\omega t} + B e^{-\omega t}$

定义 DCM（Divergent Component of Motion）：

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

代入 LIPM 方程 $\ddot{x} = \omega^2(x - p)$：

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

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

这是一阶线性 ODE！ 将 LIPM 的二阶系统简化为一阶。

DCM 的物理意义¶

DCM $\xi$ 代表"如果从现在开始停止控制（$p = 0$），CoM 最终会发散到哪里"。

更精确地说： - DCM 是 LIPM 齐次解中**发散模态的系数** - $\xi$ 完全编码了系统的不稳定成分

几何直觉：$\xi = x + \dot{x}/\omega$ 可以理解为"CoM 当前位置 + 按自然频率预测的未来偏移"。

DCM 的动力学分析¶

如果 $\xi = p$（DCM 等于 CoP）→ $\dot{\xi} = 0$ → 系统平衡
如果 $\xi > p$ → $\dot{\xi} > 0$ → DCM 继续发散（向正方向加速远离 CoP）
如果 $\xi < p$ → $\dot{\xi} < 0$ → DCM 向负方向发散

关键：DCM 总是**远离** CoP！这是倒立摆不稳定性的一阶表达。如果不引入 DCM 分解会怎样？你需要直接处理 LIPM 的二阶方程，其中稳定和不稳定模态耦合在一起，难以分别设计控制律。DCM 分解的本质贡献是把"平衡问题"从二阶降为一阶——你只需要控制发散分量 $\xi$，收敛分量会自动衰减。

本质洞察：DCM 的数学结构揭示了一个深刻的控制原理——对于不稳定系统，控制资源应该集中在不稳定模态上。LIPM 有一个稳定模态（$e^{-\omega t}$，自动衰减）和一个不稳定模态（$e^{+\omega t}$，需要控制）。DCM 精确地隔离了不稳定模态，使控制器可以"对症下药"。这个思想在控制论中有普遍意义：任何线性系统都可以分解为稳定和不稳定子空间，只需要对不稳定子空间施加反馈——这就是"反馈稳定化"的本质。

收敛分量（VRP）¶

相应地定义**收敛分量**：

\[\zeta = x - \frac{\dot{x}}{\omega}\]

其动力学：$\dot{\zeta} = -\omega(\zeta - p)$

收敛分量自然衰减——即使不控制也不会发散。这就是为什么控制器只需关注 DCM。

Capture Point（Pratt et al. 2006; Koolen et al. 2012）¶

Capture Point 概念由 Pratt et al. 2006 提出，Koolen et al. 2012 将其扩展为 Capturability 分析框架。

Capture Point = 当前 DCM 值 $\xi$。它的物理意义：

如果机器人立即将脚踩到 Capture Point $\xi$ 的位置，它可以停下来不摔倒。

证明：如果 $p_{\text{new}} = \xi$，则 $\dot{\xi} = \omega(\xi - \xi) = 0$。DCM 停止发散，然后 CoM 以 $e^{-\omega t}$ 收敛到 $\xi$。

实际意义： - 如果 Capture Point 在可达范围内 → 机器人可以通过迈一步恢复平衡 - 如果 Capture Point 超出可达范围 → 必须多步恢复 → 进入"步态" - 如果 Capture Point 远超所有可能步态 → 不可恢复 → 摔倒

DCM 步态规划¶

对于离散步态（每步持续时间 $T_s$），利用 DCM 方程 $\dot{\xi} = \omega(\xi - p)$ 的解析解：

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

在每步结束时：

\[\xi_{\text{end}} = p + (\xi_{\text{start}} - p)e^{\omega T_s}\]

反解起始 DCM：

\[\xi_{\text{start},i} = p_i + e^{-\omega T_s}(\xi_{\text{end},i} - p_i)\]

反向递推算法：从最后一步（期望停止）向前递推

设最后一步结束时 $\xi_{\text{end},N} = p_N$（停在最后一个脚步位置）
反向：$\xi_{\text{start},i} = p_i + e^{-\omega T_s}(\xi_{\text{end},i} - p_i)$
连续性：$\xi_{\text{start},i} = \xi_{\text{end},i-1}$

为什么 DCM 简化了控制¶

方面	LIPM (二阶)	DCM (一阶)
系统阶数	2 (x, x_dot)	1 (xi)
规划变量	CoM 轨迹	DCM 轨迹
步规划	需要解 ODE	解析递推公式
稳定性判断	需要同时检查 x, x_dot	只检查 xi vs 支撑多边形
在线调整	困难（二阶耦合）	简单（一阶线性）

C++ 实现：DCM 步态规划器¶

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

// DCM 步态规划器
// 基于 Capture Point 反向递推
class DCMPlanner {
public:
    double omega_;  // sqrt(g/h)
    double Ts_;     // 步态周期（每步持续时间）

    DCMPlanner(double h, double g, double Ts)
        : omega_(std::sqrt(g / h)), Ts_(Ts) {}

    // 每步的 DCM 起止值
    struct StepDCM {
        Eigen::Vector2d dcm_start;
        Eigen::Vector2d dcm_end;
    };

    // 反向递推 DCM 轨迹
    // footsteps: 脚步位置序列（2D: x, y）
    std::vector<StepDCM> planDCM(
        const std::vector<Eigen::Vector2d>& footsteps)
    {
        int N = footsteps.size();
        std::vector<StepDCM> dcm_plan(N);

        // 最后一步结束时：DCM = 最后脚步位置（停下来）
        dcm_plan[N - 1].dcm_end = footsteps[N - 1];

        // 反向递推
        double alpha = std::exp(-omega_ * Ts_);
        for (int i = N - 1; i >= 0; --i) {
            // xi_start = p + alpha * (xi_end - p)
            dcm_plan[i].dcm_start = footsteps[i]
                + alpha * (dcm_plan[i].dcm_end - footsteps[i]);

            // 上一步的结束 DCM = 这一步的开始 DCM（连续性）
            if (i > 0) {
                dcm_plan[i - 1].dcm_end = dcm_plan[i].dcm_start;
            }
        }
        return dcm_plan;
    }

    // 在步内插值 DCM（给定步开始时间 t=0）
    Eigen::Vector2d interpolateDCM(
        const Eigen::Vector2d& dcm_start,
        const Eigen::Vector2d& cop,
        double t) const
    {
        // xi(t) = p + (xi_0 - p) * exp(omega * t)
        return cop + (dcm_start - cop) * std::exp(omega_ * t);
    }

    // 从 DCM 恢复 CoM 位置（前向积分）
    // CoM 动力学: x_dot = -omega * (x - xi)
    Eigen::Vector2d integrateCoM(
        const Eigen::Vector2d& com_prev,
        const Eigen::Vector2d& dcm_current,
        double dt) const
    {
        // 前向 Euler: x_new = x + dt * (-omega) * (x - xi)
        return com_prev + dt * (-omega_) * (com_prev - dcm_current);
    }
};

// 使用示例: 5 步直线行走
void dcmPlanningDemo() {
    DCMPlanner planner(0.8, 9.81, 0.4);  // h=0.8m, g=9.81, Ts=0.4s

    std::vector<Eigen::Vector2d> footsteps = {
        {0.0, 0.1}, {0.3, -0.1}, {0.6, 0.1}, {0.9, -0.1}, {1.2, 0.1}
    };

    auto dcm_plan = planner.planDCM(footsteps);

    // 前向模拟 CoM: 对每步内 DCM 插值，积分 CoM
    Eigen::Vector2d com = dcm_plan[0].dcm_start;
    for (int step = 0; step < 5; ++step) {
        for (int k = 0; k < 40; ++k) {  // 40 steps * 10ms = 0.4s
            double t = k * 0.01;
            auto dcm = planner.interpolateDCM(
                dcm_plan[step].dcm_start, footsteps[step], t);
            com = planner.integrateCoM(com, dcm, 0.01);
        }
    }
}

DCM 反馈控制¶

实际中 DCM 规划需要闭环反馈来补偿扰动：

\[p_{\text{cmd}} = \xi_{\text{ref}} - \frac{1}{\omega}\dot{\xi}_{\text{ref}} + K_{\xi}(\xi_{\text{ref}} - \xi_{\text{actual}})\]

其中 $K_{\xi} > 0$ 是反馈增益。当 $K_{\xi} = 1$ 时，对应"死敲"（deadbeat）控制——一步修正所有误差。

⚠️ 常见陷阱：DCM 公式 $\xi = x + \dot{x}/\omega$ 中的 $\omega = \sqrt{g/h}$ 依赖于 CoM 高度 $h$。如果机器人在不平地面行走（$h$ 变化），需要用**变高度 DCM** 公式或切换到 VRPM（Variable-height Robust Point Mass）模型。

练习¶

[推导题] 证明 LIPM 的 CoM 位置可以用 DCM 表示为 $x = \xi - \dot{\xi}/\omega$，并解释其物理意义（"CoM 总是追随 DCM"）
[编程题] 实现 DCM 步态规划器：给定 5 步的脚步位置序列，反向递推 DCM 轨迹，前向积分得到 CoM 轨迹。绘制 CoM、DCM、CoP 三条曲线
[分析题] 如果步态周期 $T_s$ 增大（走得慢），DCM 在步内的发散量 $e^{\omega T_s}$ 如何变化？对步距规划有什么影响？
[设计题] 一个人形机器人被推了一下，DCM 偏离参考 5 cm。设计一个 DCM 反馈控制器，计算需要的 CoP 调整量。如果 CoP 调整受限于脚掌大小（8 cm），什么情况下需要改变步规划？

51.6 SLIP——弹簧倒立摆模型 ⭐⭐¶

这一节解决什么问题：LIPM 假设 CoM 高度恒定，无法建模跑步/跳跃。SLIP 引入弹簧腿和飞行相。

为什么 LIPM 无法建模跑步¶

跑步与步行的本质区别：存在**飞行相**（所有脚离地）。飞行相中没有接触力，唯一的力是重力。CoM 做抛体运动 → 高度变化 → LIPM 的 $z = \text{const}$ 假设破裂。LIPM 和 SLIP 的关系就像刚体力学和弹性力学的关系：前者假设物体不变形（CoM 高度不变），后者引入弹性（弹簧腿允许 CoM 上下起伏），适用范围因此大幅扩展。假如不引入弹簧腿模型，跑步和跳跃中的垂直能量循环就完全无法描述——控制器会因为缺少对飞行相动力学的预测而在着地瞬间产生巨大的冲击力。

SLIP 物理模型¶

点质量 $m$ 作为机器人质量
一条弹簧腿（刚度 $k$，自然长 $L_0$）
两个相交替：
支撑相（stance）：弹簧压缩-回弹，$m\ddot{r} = -k(r-L_0)\hat{r} + mg$
飞行相（flight）：自由抛体，$m\ddot{r} = mg$

   飞行相          支撑相          飞行相
     m               m               m
     |              /|              |
     | (抛体)      / | (弹簧)      | (抛体)
     |            /  |              |
     ↓           ●───┘              ↓
─────────────────────────────────────────

为什么 SLIP 没有解析解¶

支撑相的运动方程（极坐标 $(r, \theta)$）：

\[m\ddot{r} - mr\dot{\theta}^2 = -k(r - L_0) - mg\cos\theta$$ $$mr\ddot{\theta} + 2m\dot{r}\dot{\theta} = mg\sin\theta\]

这是一个**非线性耦合 ODE**——没有闭式解。只能数值积分。

推导过程：从笛卡尔到极坐标¶

设足端为坐标原点，质心位置用极坐标 $(r, \theta)$ 描述（$\theta$ 从竖直方向量起）。

笛卡尔坐标下弹簧力 $\mathbf{F}_s = -k(r - L_0)\hat{r}$，其中 $\hat{r}$ 是从足端指向质心的单位向量。

极坐标加速度分量： - 径向：$a_r = \ddot{r} - r\dot{\theta}^2$ - 切向：$a_\theta = r\ddot{\theta} + 2\dot{r}\dot{\theta}$

径向力平衡（弹簧力 + 重力径向分量）： $$m(\ddot{r} - r\dot{\theta}^2) = -k(r - L_0) - mg\cos\theta$$

切向力平衡（仅重力切向分量）： $$m(r\ddot{\theta} + 2\dot{r}\dot{\theta}) = mg\sin\theta$$

非线性体现在：$r\dot{\theta}^2$ 项（离心力）、$2\dot{r}\dot{\theta}$ 项（科里奥利力）、$\cos\theta/\sin\theta$ 的耦合。

Poincaré 映射¶

由于没有解析解，分析 SLIP 稳定性的方法是 Poincaré 映射： - 定义截面：apex（最高点，$\dot{z} = 0$） - 状态：$(z_{\text{apex}}, \dot{x}_{\text{apex}})$（高度和水平速度） - 映射 $P$：从 apex 到下一个 apex 的状态转移 - 不动点 $x^* = P(x^*)$：周期跳跃/奔跑

稳定性判据：计算 Jacobian $J = \frac{\partial P}{\partial x}\bigg|_{x^*}$

若 $|\lambda_i(J)| < 1$ 对所有特征值成立 → 渐近稳定
若存在 $|\lambda_i(J)| > 1$ → 不稳定

SLIP 的一个惊人性质：在特定参数范围内，SLIP 具有**被动自稳定性**——无需主动控制（固定落脚角度），即可对小扰动自动恢复。这与 passive walker 的自稳定行走异曲同工。

Raibert 三部分解耦（1986）¶

Raibert 在没有 SLIP 理论分析的情况下，凭工程直觉提出了惊人有效的控制策略：

跳跃高度控制：调节弹簧推力 → 控制飞行高度
前进速度控制：调节落脚点（Raibert heuristic） $$p_{\text{foot}} = \frac{v_x T_s}{2} + k_v(v_x - v_{\text{ref}})$$
姿态控制：支撑相中髋关节力矩 → 保持身体水平

Raibert heuristic 的物理解释：

$\frac{v_x T_s}{2}$：中性点（neutral point）——如果落在这里，前半段减速 = 后半段加速，速度不变
$k_v(v_x - v_{\text{ref}})$：反馈校正——速度太快则落脚偏前（多减速），太慢则偏后

Raibert heuristic 至今仍是最常用的落脚规划公式。Ch58 将在此基础上引入基于优化的落脚点规划——把 Raibert 的线性反馈公式作为初始猜测，再通过考虑地形约束和步态时序的优化框架求解更优的落脚位置，兼顾了 Raibert 的鲁棒直觉和优化方法的全局最优性。

RK4 数值实现¶

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

// SLIP 模型参数
struct SLIPParams {
    double m = 80.0;      // 质量 [kg]
    double k = 10000.0;   // 弹簧刚度 [N/m]
    double L0 = 1.0;      // 自然腿长 [m]
    double g = 9.81;      // 重力加速度
    double theta_td = 68.0 * M_PI / 180.0;  // 落脚角度 [rad]
};

// 状态：[x, z, dx, dz]（笛卡尔坐标，足端在原点）
using State = Eigen::Vector4d;

enum class Phase { FLIGHT, STANCE };

// 飞行相动力学：仅重力
State flightDynamics(const State& s, const SLIPParams& p) {
    // dx/dt = vx, dz/dt = vz, dvx/dt = 0, dvz/dt = -g
    return State(s(2), s(3), 0.0, -p.g);
}

// 支撑相动力学：弹簧力 + 重力
// foot_pos: 足端世界坐标
State stanceDynamics(const State& s, const Eigen::Vector2d& foot_pos,
                     const SLIPParams& p) {
    // 质心相对足端的位置
    double rx = s(0) - foot_pos(0);
    double rz = s(1) - foot_pos(1);
    double r = std::sqrt(rx*rx + rz*rz);

    // 弹簧力（仅在压缩时，r < L0）
    double F_spring = 0.0;
    if (r < p.L0) {
        F_spring = p.k * (p.L0 - r);  // 指向质心方向
    }

    // 力分解到 x, z 方向
    double fx = F_spring * rx / r;
    double fz = F_spring * rz / r;

    return State(s(2), s(3), fx / p.m, fz / p.m - p.g);
}

// RK4 积分一步
State rk4Step(const State& s, double dt, Phase phase,
              const Eigen::Vector2d& foot_pos, const SLIPParams& p) {
    auto f = [&](const State& state) -> State {
        if (phase == Phase::FLIGHT) return flightDynamics(state, p);
        else return stanceDynamics(state, foot_pos, p);
    };

    State k1 = f(s);
    State k2 = f(s + 0.5*dt*k1);
    State k3 = f(s + 0.5*dt*k2);
    State k4 = f(s + dt*k3);
    return s + (dt/6.0) * (k1 + 2*k2 + 2*k3 + k4);
}

SLIP 在现代四足中的应用¶

MIT Cheetah 的 bounding/galloping 步态本质上是 3D-SLIP： - 前后两组腿交替 = 两个弹簧 - 四足 trot ≈ 两个对角 SLIP

步态	SLIP 等效	飞行相	典型平台
步行 (walk)	LIPM (无弹簧)	无	HRP-2, Atlas
小跑 (trot)	双 SLIP	无/短	Go2, Spot
奔跑 (bound)	单/双 SLIP	有	MIT Cheetah
跳跃 (pronk)	单 SLIP	有	ANYmal

⚠️ 常见陷阱：SLIP 的弹簧刚度 $k$ 不是物理弹簧——它是**等效刚度**，由腿的关节阻抗/力控策略产生。实际机器人通过 PD 控制模拟弹簧行为（impedance control → Ch53 WBC）。

练习¶

[仿真题] 用 RK4 数值积分实现 2D-SLIP 模型，找到一个稳定的周期跳跃轨迹（固定弹簧刚度和落脚角度）
[分析题] 对你找到的周期轨迹，数值计算 Poincaré 映射的 Jacobian $\partial P / \partial x$，判断稳定性（特征值是否在单位圆内）

51.7 SRBD——单刚体模型 ⭐⭐⭐¶

这一节解决什么问题：LIPM 忽略旋转动力学，SRBD 将整个机器人视为一个刚体，同时考虑平动和转动。MIT Cheetah 3 的核心模型。

动机：为什么需要旋转¶

LIPM 只建模质心平动 ($x, y$)。但真实机器人在快速运动时**旋转**也很重要： - 转弯时需要预测偏航角 (yaw) - 坡面行走需要控制俯仰/横滚 (pitch/roll) - 四足 trot 步态中身体有明显的 roll/pitch 振荡

SRBD 方程¶

将整个机器人视为一个刚体，位于 CoM $\mathbf{r}_G$ 处，总质量 $m$，惯性张量 $I_G$：

平动（Newton）：

\[m\ddot{\mathbf{r}}_G = \sum_{i=1}^{n_c} \mathbf{f}_i + m\mathbf{g}\]

转动（Euler）：

\[I_G \dot{\boldsymbol{\omega}} + \boldsymbol{\omega} \times (I_G \boldsymbol{\omega}) = \sum_{i=1}^{n_c} (\mathbf{p}_i - \mathbf{r}_G) \times \mathbf{f}_i\]

其中 $\mathbf{f}_i$ 是第 $i$ 个接触点的地面反力，$\mathbf{p}_i$ 是该接触点的世界坐标位置。

方程推导¶

平动部分——直接是牛顿第二定律对质心的应用：

\[m\ddot{\mathbf{r}}_G = \mathbf{F}_{\text{ext}} = \sum_{i=1}^{n_c} \mathbf{f}_i + m\mathbf{g}\]

转动部分——从角动量定理出发：

质心处的角动量 $\mathbf{L}_G = I_G \boldsymbol{\omega}$（刚体假设下）。

角动量定理：$\dot{\mathbf{L}}_G = \boldsymbol{\tau}_{\text{ext}}$

在体坐标系中求导（transport theorem）： $$\frac{d\mathbf{L}_G}{dt}\bigg|_{\text{inertial}} = \frac{d\mathbf{L}_G}{dt}\bigg|_{\text{body}} + \boldsymbol{\omega} \times \mathbf{L}_G$$

\[= I_G \dot{\boldsymbol{\omega}} + \boldsymbol{\omega} \times (I_G \boldsymbol{\omega})\]

外力矩：$\boldsymbol{\tau}_{\text{ext}} = \sum_{i=1}^{n_c} (\mathbf{p}_i - \mathbf{r}_G) \times \mathbf{f}_i$

注意：$\boldsymbol{\omega} \times (I_G \boldsymbol{\omega})$ 是**陀螺力矩**（gyroscopic term）。当 $\omega$ 小时可忽略（线性化时通常丢弃此项）。

💡 工程判断：为什么可以忽略陀螺项 $\boldsymbol{\omega} \times (I_G\boldsymbol{\omega})$

完整欧拉方程 $\dot{\mathbf{L}} = I_G\dot{\boldsymbol{\omega}} + \boldsymbol{\omega} \times (I_G\boldsymbol{\omega})$ 中的陀螺项使方程非线性耦合，在 MPC 等需要在线快速求解的场景中会显著增加计算负担。忽略该项的工程正当性： 1. 在低角速度工况下（如步行），$\|\boldsymbol{\omega}\|$ 较小，$\boldsymbol{\omega} \times (I_G\boldsymbol{\omega})$ 量级远小于 $I_G\dot{\boldsymbol{\omega}}$ 2. 对于近似对称的惯性张量，$I_G\boldsymbol{\omega}$ 与 $\boldsymbol{\omega}$ 近似同向，叉乘趋近于零 3. 高频反馈控制（WBC at 500-1000 Hz）可以补偿因模型简化引入的误差

状态与控制¶

状态：物理状态维度为 12（位置3+速度3+姿态3+角速度3），Di Carlo 实现中添加重力常数作为第13维以简化矩阵形式： $$\mathbf{x} = (\mathbf{r}_G, \boldsymbol{\Theta}, \dot{\mathbf{r}}_G, \boldsymbol{\omega}, g)^T \in \mathbb{R}^{13}$$

其中 $\boldsymbol{\Theta} = (\phi, \theta, \psi)^T$ 是欧拉角（或四元数 → 14 维），末尾的 $g$ 是重力常数，用于将仿射系统 $\mathbf{x}_{k+1} = A\mathbf{x}_k + B\mathbf{u}_k + \mathbf{d}$ 改写为纯线性形式 $\mathbf{x}_{k+1} = \hat{A}\mathbf{x}_k + B\mathbf{u}_k$（把常数项吸收进 $\hat{A}$ 的最后一列）。

控制（不是关节力矩！）： $$\mathbf{u} = (\mathbf{f}_1, \mathbf{f}_2, \ldots, \mathbf{f}_{n_c})^T \in \mathbb{R}^{3 n_c}$$

关键思路：接触力作为优化变量，而不是关节力矩。然后用 WBC 将期望接触力映射到关节力矩。

本质洞察：SRBD 模型的真正威力不在于"简化"，而在于它把决策变量从关节空间（12 维力矩）转移到了接触力空间（同样 12 维力，但含义完全不同）。接触力直接对应牛顿-欧拉方程的右端项——优化器可以"看到"力如何影响质心运动，而不需要经过关节动力学这个中间层。这就是为什么 SRBD + QP 能做到 3 ms 求解的根本原因。

为什么 SRBD MPC 可以是凸 QP¶

将 SRBD 方程线性化（绕当前状态 Taylor 展开到一阶）：

\[\mathbf{x}_{k+1} = A_k \mathbf{x}_k + B_k \mathbf{u}_k + \mathbf{d}_k\]

加上约束： - 摩擦锥：$|f_{x,i}| \leq \mu f_{z,i}$，$|f_{y,i}| \leq \mu f_{z,i}$（线性化锥） - 法向力非负：$f_{z,i} \geq 0$ - 力矩限制（可选）：基于电机能力

MPC 问题变为：

\[\min_{\mathbf{u}_0, \ldots, \mathbf{u}_{N-1}} \sum_{k=0}^{N} ||\mathbf{x}_k - \mathbf{x}_{\text{ref}}||_Q^2 + \sum_{k=0}^{N-1} ||\mathbf{u}_k||_R^2$$ $$\text{s.t.} \quad \mathbf{x}_{k+1} = A_k \mathbf{x}_k + B_k \mathbf{u}_k + \mathbf{d}_k$$ $$\quad\quad\quad |f_{x,i}| \leq \mu f_{z,i}, \quad f_{z,i} \geq 0\]

线性约束 + 二次代价 = 凸 QP → 全局最优解 + 快速求解！

线性化的关键近似：

丢弃陀螺力矩 $\boldsymbol{\omega} \times (I_G \boldsymbol{\omega}) \approx 0$（$\omega$ 小）
欧拉角到角速度的映射线性化：$\dot{\boldsymbol{\Theta}} \approx \boldsymbol{\omega}$（小角度）
$I_G$ 视为恒定（刚体假设的核心）

近似 1 的物理依据：完整的欧拉方程为 $\dot{\mathbf{L}} = I_G \dot{\boldsymbol{\omega}} + \boldsymbol{\omega} \times (I_G \boldsymbol{\omega})$。忽略陀螺项 $\boldsymbol{\omega} \times (I_G \boldsymbol{\omega})$ 是将模型线性化和解耦的关键一步，使得 MPC 的在线滚动求解变得可行。该近似成立的条件是：roll 和 pitch 角较小（使得惯性张量在世界系下近似恒定）且角速度较小（使得陀螺项远小于 $I_G \dot{\boldsymbol{\omega}}$）。在正常行走的工况下这些条件通常成立，且反馈控制可以有效补偿近似误差。

SRBD 连续状态空间方程的显式构造¶

将上述线性化后的动力学方程——包括欧拉角微分映射 $\dot{\boldsymbol{\Theta}} \approx R_z^T(\psi) \boldsymbol{\omega}$（保留 yaw 角的旋转修正）、牛顿第二定律 $m\ddot{\mathbf{r}}_G = \sum f_i + m\mathbf{g}$、以及简化后的欧拉方程 $I_G \dot{\boldsymbol{\omega}} = \sum (\mathbf{p}_i - \mathbf{r}_G) \times \mathbf{f}_i$——合并为标准状态空间形式。

初始合并会产生一个仿射系统（因为重力项 $m\mathbf{g}$ 是常数）：$\dot{\mathbf{x}} = A_c \mathbf{x} + B_c \mathbf{u} + \mathbf{d}$，其中 $\mathbf{d} = [0,\ldots,0, g_z]^T$。为了消除仿射项得到纯线性系统 $\dot{\mathbf{x}} = A_c \mathbf{x} + B_c \mathbf{u}$，将重力加速度 $g_z = -9.81$ 追加为第 13 个状态变量（其导数为零），从而得到增广状态空间方程：

\[\frac{d}{dt}\begin{pmatrix} \boldsymbol{\Theta} \\ \mathbf{r}_G \\ \boldsymbol{\omega} \\ \dot{\mathbf{r}}_G \\ g_z \end{pmatrix} = \begin{pmatrix} 0 & 0 & R_z^T(\psi) & 0 & 0 \\ 0 & 0 & 0 & I_3 & 0 \\ 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & \mathbf{e}_3 \\ 0 & 0 & 0 & 0 & 0 \end{pmatrix} \begin{pmatrix} \boldsymbol{\Theta} \\ \mathbf{r}_G \\ \boldsymbol{\omega} \\ \dot{\mathbf{r}}_G \\ g_z \end{pmatrix} + \begin{pmatrix} 0 & \cdots & 0 \\ 0 & \cdots & 0 \\ I_G^{-1}[\mathbf{r}_1]_\times & \cdots & I_G^{-1}[\mathbf{r}_4]_\times \\ I_3/m & \cdots & I_3/m \\ 0 & \cdots & 0 \end{pmatrix} \begin{pmatrix} \mathbf{f}_1 \\ \vdots \\ \mathbf{f}_4 \end{pmatrix}\]

其中 $[\mathbf{r}_i]_\times$ 是足端相对于质心的力臂向量 $(\mathbf{p}_i - \mathbf{r}_G)$ 的反对称矩阵，$\mathbf{e}_3 = [0,0,1]^T$。

离散化采用零阶保持（Zero-Order Hold），采样时间 $\Delta t$（通常为 MPC 步长，如 25 ms）：

\[A_k \approx I + A_c \Delta t, \quad B_k \approx B_c \Delta t\]

注意 $A_c$ 和 $B_c$ 都依赖于当前 yaw 角 $\psi$ 和当前落足点位置 $\mathbf{p}_i$——它们在每个 MPC horizon 步都可能变化，因此这是一个**时变线性系统**（LTV），而非 LTI。

MIT Cheetah 3（Di Carlo 2018）¶

Di Carlo 的关键贡献：证明 SRBD + 凸 QP 对四足 trot 足够好。

状态：12 维（位置3 + 速度3 + 欧拉角3 + 角速度3），Di Carlo 实现中添加重力常数作为第13维以简化矩阵形式
控制：12 维（4 脚 × 3D 力）
QP 求解器：qpOASES，<1 ms / solve
MPC 频率：20-30 Hz（每 33-50 ms 重规划一次），底层腿控制器运行在 500 Hz-1 kHz
效果：MIT Cheetah 3 在多种地形上实现稳定 trot + push recovery

架构分层：MPC 以 20-30 Hz 求解最优接触力轨迹（QP 求解时间 <1 ms），底层关节 PD 控制器以 500 Hz-1 kHz 追踪期望力。500 Hz/1 kHz 是低层控制器频率，而非 MPC 频率。

SRBD vs LIPM¶

维度	LIPM	SRBD
状态维度	2-4	12-13
控制维度	1-2 (CoP)	12 (接触力)
旋转	❌	✅
优化问题	极简 LQR/QP	中等 QP
求解时间	<0.1 ms	1-5 ms
适用场景	双足步行	四足 trot/push recovery
代表系统	HRP-2, ASIMO	MIT Cheetah, Go2

Pinocchio + QP 实现框架¶

#include <pinocchio/algorithm/center-of-mass.hpp>
#include <pinocchio/algorithm/crba.hpp>
#include <OsqpEigen/OsqpEigen.h>
#include <Eigen/Dense>

// SRBD MPC 核心结构
struct SRBDState {
    Eigen::Vector3d pos;    // 质心位置
    Eigen::Vector3d vel;    // 质心速度
    Eigen::Vector3d euler;  // 欧拉角 (roll, pitch, yaw)
    Eigen::Vector3d omega;  // 角速度
};

struct SRBDControl {
    std::array<Eigen::Vector3d, 4> forces;  // 4 脚的 GRF
};

// 动力学线性化：构建 A(13x13), B(13x12) 矩阵
void linearize(const SRBDState& x0, const SRBDControl& u0,
               double dt, double mass,
               const Eigen::Matrix3d& I_body,
               const std::array<Eigen::Vector3d, 4>& foot_positions,
               Eigen::MatrixXd& A, Eigen::MatrixXd& B) {
    A = Eigen::MatrixXd::Identity(13, 13);
    B = Eigen::MatrixXd::Zero(13, 12);

    // 位置 <- 速度
    A.block<3,3>(0, 6) = Eigen::Matrix3d::Identity() * dt;

    // 速度 <- 力（牛顿方程）
    for (int i = 0; i < 4; ++i) {
        B.block<3,3>(6, 3*i) = Eigen::Matrix3d::Identity() * dt / mass;
    }

    // 欧拉角 <- 角速度（小角度近似）
    A.block<3,3>(3, 9) = Eigen::Matrix3d::Identity() * dt;

    // 角速度 <- 力矩（欧拉方程，忽略陀螺项）
    Eigen::Matrix3d I_inv = I_body.inverse();
    for (int i = 0; i < 4; ++i) {
        // tau_i = (p_i - r_G) x f_i
        // d(omega)/d(f_i) = I_inv * skew(p_i - r_G) * dt
        Eigen::Vector3d r = foot_positions[i] - x0.pos;
        Eigen::Matrix3d skew;
        skew << 0, -r(2), r(1),
                r(2), 0, -r(0),
                -r(1), r(0), 0;
        B.block<3,3>(9, 3*i) = I_inv * skew * dt;
    }
}

// 摩擦锥约束矩阵（每脚 5 个不等式）
// |fx| <= mu*fz, |fy| <= mu*fz, fz >= 0
void buildFrictionConstraints(double mu, int n_feet,
                              Eigen::MatrixXd& C, Eigen::VectorXd& d) {
    int n_con = 5 * n_feet;
    C = Eigen::MatrixXd::Zero(n_con, 3 * n_feet);
    d = Eigen::VectorXd::Zero(n_con);

    for (int i = 0; i < n_feet; ++i) {
        int row = 5 * i, col = 3 * i;
        C(row, col) = 1;   C(row, col+2) = -mu;    // fx - mu*fz <= 0
        C(row+1, col) = -1; C(row+1, col+2) = -mu;  // -fx - mu*fz <= 0
        C(row+2, col+1) = 1; C(row+2, col+2) = -mu; // fy - mu*fz <= 0
        C(row+3, col+1) = -1; C(row+3, col+2) = -mu;// -fy - mu*fz <= 0
        C(row+4, col+2) = -1;                        // -fz <= 0
    }
}

⚠️ 常见陷阱：SRBD 假设惯性张量 $I_G$ 在整个 horizon 内近似恒定（不随关节角度变化）。对于四足 trot 这个近似很好（腿质量远小于躯干），但对于人形机器人（大型手臂摆动改变惯性）需要更频繁地更新 $I_G$，或直接使用 Centroidal 模型。

练习¶

[推导题] 将 SRBD 的旋转方程改写为四元数形式 $\dot{q} = \frac{1}{2}\Omega(\boldsymbol{\omega})q$，避免欧拉角万向锁
[编程题] 实现 Go2 的 SRBD MPC（horizon=10，dt=25ms），用 OSQP 求解，在 MuJoCo 中验证站立 push recovery

51.8 Centroidal Model——LIPM 的终极泛化 ⭐⭐⭐¶

这一节解决什么问题：SRBD 用恒定惯性近似真实系统。Centroidal Model 精确建模 6D 动量变化，是最精确的简化模型。

从 SRBD 到 Centroidal¶

SRBD 的局限：假设 $I_G$ 恒定。但当机器人大幅改变姿态时（如人形机器人甩臂），$I_G$ 显著变化。

Centroidal Dynamics 不做 $I_G = \text{const}$ 假设，而是直接建模 6D 质心动量 $\mathbf{h}_G$：

\[\dot{\mathbf{h}}_G = \begin{pmatrix} \dot{\mathbf{k}}_G \\ \dot{\mathbf{l}}_G \end{pmatrix} = \begin{pmatrix} \sum_i (\mathbf{p}_i - \mathbf{r}_G) \times \mathbf{f}_i \\ \sum_i \mathbf{f}_i + m\mathbf{g} \end{pmatrix}\]

其中 $\mathbf{k}_G$ 是绕 CoM 的角动量，$\mathbf{l}_G = m\dot{\mathbf{r}}_G$ 是线动量。

与 Ch49 CCRBA 的关系¶

回忆 Ch49：$\mathbf{h}_G = A_G(q)\dot{q}$，其中 $A_G \in \mathbb{R}^{6 \times n_v}$ 是 CMM。

Centroidal dynamics 的"精确"在于：它不假设 $I_G$ 恒定，而是通过 CMM 精确计算 $\dot{\mathbf{h}}_G$。但 MPC 层面只优化 $\mathbf{h}_G$ 的轨迹——具体哪些关节角度产生这个动量，交给 WBC。

关键区别的数学表达¶

SRBD：$\mathbf{k}_G = I_G^{\text{const}} \boldsymbol{\omega}_{\text{body}}$

Centroidal：$\mathbf{k}_G = A_G^{\text{angular}}(q) \dot{q}$

当机器人关节角度 $q$ 变化时： - SRBD 认为 $I_G$ 不变 → $\dot{\mathbf{k}}_G = I_G \dot{\boldsymbol{\omega}}$（近似，四足 trot 等小幅运动下合理，人形大幅运动时误差显著） - Centroidal 正确计算 $\dot{\mathbf{k}}_G = \dot{A}_G^{\text{angular}} \dot{q} + A_G^{\text{angular}} \ddot{q}$

状态空间¶

\[\mathbf{x} = (\mathbf{r}_G, \dot{\mathbf{r}}_G, \mathbf{k}_G, \boldsymbol{\Theta})^T \in \mathbb{R}^{12}\]

注意：线动量 $\mathbf{l}_G = m\dot{\mathbf{r}}_G$ 不需要单独作为状态（已被 $\dot{\mathbf{r}}_G$ 包含）。

为什么 Centroidal 是"终极"简化¶

特性	LIPM	SRBD	Centroidal
精确性	低	中	高（精确 6D 动量）
$I_G$ 假设	无转动	恒定	无假设（精确）
维度	2-4	12-13	12
凸 QP？	✅	✅（线性化后）	⚠️ NLP（非线性）
实时性	极高	高	中（需 SQP）

为什么 Centroidal MPC 是非线性优化¶

两个非线性来源：

交叉乘积：$(\mathbf{p}_i - \mathbf{r}_G) \times \mathbf{f}_i$——质心位置和力的双线性耦合
CMM 依赖构型：$A_G(q)$ 是关节角度的非线性函数

因此不能直接写成 QP。常用方法： - SQP (Sequential QP)：逐步线性化求解 - DDP (Differential Dynamic Programming)：利用二阶信息 - 交替优化：先固定 $\mathbf{r}_G$ 求力，再固定力求轨迹

Orin 2013 的关键洞察¶

Orin et al. (2013) 的贡献是揭示了 centroidal dynamics 的结构：

$\dot{\mathbf{h}}_G$ 只依赖于**接触力**和**质心位置**——不直接依赖关节角度
这意味着 centroidal MPC 的动力学约束是"低维的"（12 维），尽管完整系统有 30+ 维
关节角度通过 $A_G(q)$ 影响 $\mathbf{h}_G$，但 MPC 可以将这个耦合放在代价函数中而非硬约束

OCS2 中的 Centroidal Model¶

OCS2 使用 Kino-centroidal 模型——在 centroidal dynamics 基础上加上运动学约束（足端必须在地面上）：

// OCS2 CentroidalModelRobotics 核心结构
// 文件：ocs2_centroidal_model/CentroidalModelRbdConversions.h

// 状态组成：
// [0:5]   - normalized centroidal momentum (h_G / m)
// [6:8]   - base position
// [9:11]  - base orientation (ZYX Euler)
// [12:N]  - joint positions

// 输入组成：
// [0:11]  - contact forces (4 legs x 3D)
// [12:N]  - joint velocities

// 动力学约束：
// 1. centroidal dynamics (6 eqs)
// 2. kinematics: q_dot = v (integration)
// 3. foot contact: J_foot * v = 0 (no slip)

// OCS2 解决非线性问题使用 SLQ（Sequential Linear Quadratic）
// 本质是 iLQR/DDP 的变体

Pinocchio Centroidal API¶

#include <pinocchio/algorithm/centroidal.hpp>
#include <pinocchio/algorithm/center-of-mass.hpp>

// 计算 centroidal momentum matrix (CMM)
pinocchio::ccrba(model, data, q, v);
// 结果：
//   data.Ag   : CMM, 6 x nv 矩阵
//   data.hg   : centroidal momentum h_G (6D spatial vector)
//   data.hg.linear()  : 线动量 l_G = m * v_CoM
//   data.hg.angular() : 角动量 k_G

// 计算 CMM 的时间导数
pinocchio::dccrba(model, data, q, v);
// 结果：
//   data.dAg : dA_G/dt, 6 x nv 矩阵

// 质心加速度与动量变化率的关系
// h_dot_G = A_G * a + dA_G * v
// 其中 a = joint accelerations (nv 维)

// 验证 centroidal dynamics：
// h_dot_G 应该等于接触力扳手在 CoM 的等效
pinocchio::Force h_dot = pinocchio::Force::Zero();
for (int i = 0; i < n_contacts; ++i) {
    Eigen::Vector3d r = foot_pos[i] - data.com[0];
    h_dot.linear() += contact_forces[i];
    h_dot.angular() += r.cross(contact_forces[i]);
}
h_dot.linear() += model.gravity.linear() * data.mass[0];
// 此时 h_dot 应等于 data.dhg（如果正确积分的话）

练习¶

[对比题] 对同一个 Go2 trot 轨迹，分别用 SRBD（恒定 $I_G$）和 Centroidal（精确 $A_G(q)$）预测角动量轨迹，比较两者误差
[编程题] 用 Pinocchio 计算 Go2 在 trot 步态中 CMM 随时间的变化，验证 $I_G$ 近似恒定的程度（$||A_G(q_t) - A_G(q_0)||_F / ||A_G(q_0)||_F$）

51.9 模型层级与选型指南 ⭐⭐¶

这一节解决什么问题：面对具体任务，如何选择合适的简化模型？

完整选型表¶

模型	状态维度	计算	旋转	跳跃	四足	双足	人形
LIPM	2-4	极低	❌	❌	⚠️ 步行可	✅	⚠️
DCM	2	极低	❌	❌	⚠️	✅	⚠️
SLIP	4	中（数值积分）	❌	✅	✅	⚠️	❌
SRBD	12-13	中（凸 QP）	✅	⚠️	✅	⚠️	⚠️
Centroidal	12	高（NLP/SQP）	✅	✅	✅	✅	✅
全身动力学	2n	极高	✅	✅	⚠️离线	⚠️离线	⚠️离线

决策流程图¶

需要实时 MPC？
├── 是 → 只需平动（步行）？
│       ├── 是 → LIPM/DCM（最简最快）
│       └── 否 → 惯性变化大？
│               ├── 否（四足 trot）→ SRBD（凸 QP，3ms）
│               └── 是（人形/复合）→ Centroidal（SQP，5-10ms）
└── 否（离线规划/训练）→ 全身动力学

各模型的"信息丢失"¶

理解每个简化模型丢失了什么信息，是正确使用它们的关键：

模型	丢失的信息	后果
LIPM	旋转、CoM 高度变化、腿动力学	无法预测跌倒方向、无法跑步
DCM	同 LIPM + 连续时间信息（离散化）	步间无法修正
SLIP	旋转、多体耦合	无法控制身体姿态
SRBD	关节级动力学、惯性变化	大幅运动时力分配不准
Centroidal	关节级约束（力矩/速度限制）	可能生成物理不可执行的轨迹

现代趋势：模型学习 vs 物理模型¶

方法	代表	优势	劣势
物理简化模型	SRBD/Centroidal + MPC	可解释、安全保证	建模误差
端到端 RL	PPO/SAC policy	无需建模	不可解释、安全难保证
混合	RL + MPC residual	兼取两者	工程复杂

2024-2025 趋势：RL 策略作为"万能近似器"正在替代传统简化模型（尤其是 sim-to-real 成功后）。但在安全关键场景（如手术机器人、载人机器人），基于物理模型的 MPC 仍是首选。

51.9.5 虚拟模型控制（VMC）——直觉驱动的力控方法 ⭐⭐¶

这一节解决什么问题：在 MPC/WBC 等优化框架出现之前，如何用直觉化的方式为足式机器人设计力控策略？VMC 提供了一种不依赖逆动力学求解的轻量力控方法，至今仍在许多嵌入式平台和教学场景中广泛使用。

核心思想¶

虚拟模型控制（Virtual Model Control, VMC）是一种**直觉控制方法**，其核心思想是利用虚拟的机械部件（如弹簧、阻尼器、轴承等）连接机器人内部的作用点或连接作用点与外部环境，由虚拟构件产生**虚拟力**来驱动机器人运动。虚拟力并非执行机构的真实作用力，而是对期望运动效果的直觉化描述——通过雅可比矩阵的转置关系，将工作空间中的虚拟力转换为关节空间中的真实关节力矩：

\[\boldsymbol{\tau} = J^T(\mathbf{q}) \cdot \mathbf{F}_{\text{virtual}}\]

关节电机输出的真实力矩所产生的运动效果，与虚拟力产生的效果等价，均可驱使机器人按期望运动。

VMC 的独特优势¶

VMC 的威力在于**简化了复杂动态系统的控制设计**：

无需逆动力学求解：不需要构建完整的动力学模型并求逆，只需设计直觉上合理的虚拟力
综合考虑多个控制目标：可以同时考虑机身高度、姿态和前进速度等，通过在不同自由度上构造不同的虚拟构件实现
计算效率高：只需要正运动学和雅可比矩阵，适合嵌入式平台实时运行
物理直觉清晰：用户可以直观地理解"弹簧把身体往上拉"或"阻尼器在减速"，而不需要深入处理底层动力学方程

应用示例¶

以足式机器人的二自由度单腿结构为例，A、B 两个转动关节由电机驱动，两个连杆长度 $L_1$, $L_2$ 已知。通过正运动学获取足端 C 点的位置 $\mathbf{p} = f(\mathbf{q})$，求导得到雅可比矩阵 $J(\mathbf{q})$。

**设计过程**分两步：

构造虚拟力：在足端位置构造虚拟弹簧-阻尼器系统，产生指向期望位置的虚拟力 $\mathbf{F}_{\text{virtual}} = K_p(\mathbf{p}_d - \mathbf{p}) + K_d(\dot{\mathbf{p}}_d - \dot{\mathbf{p}})$
映射到关节力矩：通过静力学关系 $\boldsymbol{\tau} = J^T \mathbf{F}_{\text{virtual}}$ 计算各关节所需的电机力矩

VMC 的应用关键在于两点：一是在每个需要控制的自由度上构造恰当的虚拟构件以产生合适的虚拟力；二是在不同的相位状态（支撑相/摆动相）利用相应的雅可比矩阵计算期望的关节力矩。

与 MPC/WBC 的关系：VMC 可以看作是 WBC 的一种特化——在单腿非冗余系统中，VMC 的雅可比转置力映射与 WBC 的接触力分配本质相同。VMC 的局限在于它无法处理多任务优先级和复杂约束，这正是 WBC 框架（Ch53）要解决的问题。在 MPC + WBC 的现代控制框架中，VMC 的角色被 WBC 的力映射层替代，但 VMC 的直觉设计思想——用虚拟构件描述期望行为——至今仍然是控制器调参的重要心智模型。

51.10 C++ 实现与 OCS2/Crocoddyl 集成 ⭐⭐¶

这一节解决什么问题：将上述模型实际接入 MPC 框架。

LIPM 作为 CasADi 模型¶

import casadi as ca
import numpy as np

# LIPM 离散动力学（CasADi 符号表达）
g = 9.81; h = 0.8; omega = ca.sqrt(g/h)
dt = 0.01

x = ca.SX.sym('x', 2)  # 状态：[pos, vel]
u = ca.SX.sym('u', 1)  # 控制：CoP position

# 连续动力学：ddx = omega^2 * (x - p)
xdot = ca.vertcat(x[1], omega**2 * (x[0] - u))

# 一阶欧拉离散化（演示用；实际应使用 RK4）
x_next = x + dt * xdot
f_dynamics = ca.Function('f', [x, u], [x_next])

# 构建 N 步 MPC
N = 20  # horizon
opti = ca.Opti()
X = opti.variable(2, N+1)  # 状态轨迹
U = opti.variable(1, N)    # 控制轨迹

# 代价函数
cost = 0
x_ref = np.array([0.5, 0.0])  # 目标：位置0.5，零速
for k in range(N):
    cost += ca.sumsqr(X[:, k] - x_ref) + 0.01 * ca.sumsqr(U[:, k])
cost += 10 * ca.sumsqr(X[:, N] - x_ref)  # 终端代价加权

opti.minimize(cost)

# 动力学约束
for k in range(N):
    opti.subject_to(X[:, k+1] == f_dynamics(X[:, k], U[:, k]))

# CoP 必须在支撑多边形内（简化为脚长范围）
opti.subject_to(opti.bounded(-0.1, U, 0.1))

# 初始条件
opti.subject_to(X[:, 0] == [0.0, 0.3])

opti.solver('ipopt')
sol = opti.solve()

SRBD 作为 OCS2 SystemDynamics¶

OCS2 的 legged_robot 示例本质上就是 SRBD + WBC： - MPC 层优化接触力（SRBD 或 kino-centroidal） - WBC 层将接触力映射到关节力矩

// OCS2 中 SRBD 流映射（简化示意）
// 对应文件：ocs2_robotic_examples/ocs2_legged_robot/

#include <ocs2_core/dynamics/SystemDynamicsBase.h>

class SRBDDynamics : public ocs2::SystemDynamicsBase {
public:
    ocs2::vector_t computeFlowMap(
        ocs2::scalar_t time,
        const ocs2::vector_t& state,   // 13 维
        const ocs2::vector_t& input    // 12 维 (4 legs x 3D force)
    ) override {
        ocs2::vector_t dxdt(13);

        Eigen::Vector3d pos = state.segment<3>(0);
        Eigen::Vector3d euler = state.segment<3>(3);
        Eigen::Vector3d vel = state.segment<3>(6);
        Eigen::Vector3d omega = state.segment<3>(9);

        // 平动：m*a = sum(f_i) + m*g
        Eigen::Vector3d sum_forces = Eigen::Vector3d::Zero();
        Eigen::Vector3d sum_torques = Eigen::Vector3d::Zero();
        for (int i = 0; i < 4; ++i) {
            Eigen::Vector3d fi = input.segment<3>(3*i);
            sum_forces += fi;
            sum_torques += (footPositions_[i] - pos).cross(fi);
        }

        dxdt.segment<3>(0) = vel;
        dxdt.segment<3>(3) = omega;  // 小角度近似
        dxdt.segment<3>(6) = sum_forces / mass_ + gravity_;
        dxdt.segment<3>(9) = I_inv_ * (sum_torques - omega.cross(I_ * omega));
        dxdt(12) = 0.0;  // 重力常数

        return dxdt;
    }

private:
    double mass_;
    Eigen::Matrix3d I_, I_inv_;
    Eigen::Vector3d gravity_{0, 0, -9.81};
    std::array<Eigen::Vector3d, 4> footPositions_;
};

Crocoddyl 中的 Centroidal MPC¶

Crocoddyl 使用 DDP/FDDP 求解非线性轨迹优化：

⚠️ 注意：当前 Crocoddyl API 不直接提供 DifferentialActionModelCentroidal，
以下为概念伪代码，展示 centroidal MPC 的构建思路。
实际实现需要通过 DifferentialActionModelFreeFwdDynamics 或自定义 ActionModel 实现。

// 概念伪代码：Centroidal MPC via Crocoddyl
// 状态: x = [com, h_G]  (CoM 位置 + centroidal momentum, 12D)
// 控制: u = [f_1, ..., f_nc]  (各接触点力)

// 1. 自定义 DifferentialActionModel（继承 DifferentialActionModelAbstract）
//    calc(): 实现 centroidal dynamics  ḣ_G = sum(contact wrenches)
//    calcDiff(): 实现对应 Jacobians

// 2. 构建 shooting problem
for k = 0 to N-1:
    intModel = IntegratedActionModelEuler(centroidalModel, dt)
    models.push_back(intModel)

// 3. FDDP 求解
problem = ShootingProblem(x0, models, terminalModel)
solver = SolverFDDP(problem)
solver.solve(xs_init, us_init, 100);  // 最多 100 次迭代

完整 pipeline 概览¶

┌─────────────────────────────────────────────────────┐
│                   高层规划器                          │
│  输入：目标速度/位置、地形信息                        │
│  输出：步态时序、落脚点序列                           │
└────────────────────────┬────────────────────────────┘
                         ↓
┌─────────────────────────────────────────────────────┐
│              简化模型 MPC (30-50 Hz)                  │
│  LIPM/SRBD/Centroidal                                │
│  输入：当前状态 + 参考轨迹                           │
│  输出：期望接触力 + 期望 CoM 轨迹                    │
└────────────────────────┬────────────────────────────┘
                         ↓
┌─────────────────────────────────────────────────────┐
│           全身控制器 WBC (200-1000 Hz)                │
│  输入：期望力 + 期望运动                             │
│  输出：关节力矩 / 关节位置指令                       │
│  方法：QP (任务优先级) 或逆动力学                    │
└────────────────────────┬────────────────────────────┘
                         ↓
┌─────────────────────────────────────────────────────┐
│              底层电机驱动 (1-10 kHz)                  │
│  PD 力矩控制 / 电流环                                │
└─────────────────────────────────────────────────────┘

常见故障与排查¶

现象	可能原因	排查方法
LIPM 仿真中 CoM 轨迹发散（指数增长）	未正确施加 ZMP 控制或 DCM 反馈，系统运行在不稳定极点上	检查 DCM 反馈增益 $k_\xi > \omega$（不稳定极点需要足够大的增益来镇定）；打印 DCM 轨迹确认是否在 capture point 附近收敛
SRBD MPC 输出的接触力全为零或分布极不均匀	摩擦锥约束与代价函数权重设置不平衡——力正则化权重过大会压制所有力，过小则优化器不关心力的大小	将力正则化权重 $R$ 从 $10^{-6}$ 开始逐步增大，观察力分布变化；检查接触调度矩阵是否正确标记了当前支撑脚
Centroidal 模型与全身动力学的 CoM 轨迹不一致	简化模型使用了固定惯量 $I_G$（SRBD 假设），但实际运动中腿部摆动引起的惯量变化不可忽略	对比 `pinocchio::computeCentroidalMap()` 的实时 CMM 与固定 $I_G$ 的差异；在大幅摆腿场景（如跳跃）下差异最显著
SLIP 模型仿真中弹簧压缩量出现负值（腿被拉长）	RK4 积分步长过大，或落脚角 $\theta_{td}$ 设置导致足端在支撑相中移出地面	减小积分步长至 $\leq 0.001$ s；添加 `L >= L0` 的约束检测，在腿恢复自然长度时切换到飞行相
站立平衡 MPC 中机器人缓慢侧移或旋转	SRBD 模型的 CoM 位置估计有偏差，或重力补偿项中的质心位置与模型不一致	对比 Pinocchio `computeCenterOfMass()` 的结果与 SRBD 中使用的 CoM 常量；检查 URDF 连杆质量参数是否完整

本章小结¶

公式速查¶

模型	核心方程
LIPM	$\ddot{x} = \omega^2(x - p)$, $\omega = \sqrt{g/h}$
DCM	$\dot{\xi} = \omega(\xi - p)$, $\xi = x + \dot{x}/\omega$
ZMP	$p_{\text{ZMP}} = x_{\text{CoM}} - \frac{z \ddot{x} - \dot{L}_y / m}{g + \ddot{z}}$
SRBD (平动)	$m\ddot{\mathbf{r}}_G = \sum \mathbf{f}_i + m\mathbf{g}$
SRBD (转动)	$I_G\dot{\boldsymbol{\omega}} + \boldsymbol{\omega}\times(I_G\boldsymbol{\omega}) = \sum(\mathbf{p}_i - \mathbf{r}_G)\times\mathbf{f}_i$
Centroidal	$\dot{\mathbf{h}}_G = \sum$ contact wrenches at CoM
Capture Point	$\xi_{\text{capture}} = x + \dot{x}/\omega$

各模型的"一句话记忆"¶

LIPM：弹球在无摩擦桌面上滚，桌子可以倾斜（CoP 控制）
DCM：LIPM 的"发散方向"——只要盯住它就不会摔倒
SLIP：跑步的弹力球——压缩-弹起-飞行-落地
SRBD：整个机器人是一块会飞的砖头，脚是推力器
Centroidal：SRBD 但砖头的惯性矩会变（因为里面有东西在动）

学习时间估算¶

2 周（40-50 小时）：

内容	时间
LIPM 推导 + 编程验证	5 小时
CoM/CoP/ZMP 概念 + 推导	4 小时
Kajita 预览控制	6 小时
DCM + 步态规划	5 小时
SLIP + 数值仿真	4 小时
SRBD + 凸 QP MPC	8 小时
Centroidal + OCS2	6 小时
综合练习 + 论文阅读	6 小时

下游章节导引¶

Ch52 接触力学：摩擦锥线性化 → SRBD QP 的约束来源
Ch53 WBC：将简化模型的"期望力"转换为关节力矩
Ch55 OCS2 MPC：完整 legged_robot 示例 = Centroidal + WBC
Ch63 RL 训练栈：RL 替代简化模型 → 端到端控制

延伸阅读¶

#	资料	难度	推荐理由
1	Kajita et al. (2003) "Biped Walking Pattern Generation by using Preview Control of ZMP" ICRA	⭐⭐	LIPM + 预览控制的经典论文，双足步行规划的起点
2	Koolen et al. (2012) "Capturability-based analysis and control of legged locomotion" IJRR	⭐⭐⭐	Capture Point 理论的系统化阐述，DCM 稳定性分析的核心参考
3	Orin et al. (2013) "Centroidal dynamics of a humanoid robot" Autonomous Robots	⭐⭐⭐	Centroidal 模型的原始定义，CMM 与全身动力学的桥梁
4	Di Carlo et al. (2018) "Dynamic locomotion in the MIT Cheetah 3 through convex MPC" IROS	⭐⭐	SRBD + 凸 QP MPC 的工程经典，四足控制必读
5	Raibert M. (1986) Legged Robots That Balance, MIT Press	⭐	腿足机器人领域的奠基之作，直觉驱动的控制哲学
6	Geyer et al. (2006) "Compliant leg behaviour explains basic dynamics of walking and running" Proc. R. Soc. B	⭐⭐	SLIP 模型的生物力学验证，理解弹簧-质量模型为何有效
7	Wensing et al. (2016) "Improved computation of the humanoid centroidal dynamics" IJHR	⭐⭐⭐	CMM 的高效计算方法，Pinocchio CCRBA 实现的理论基础

累积项目：四足控制器——SRBD/Centroidal 简化模型 MPC 模块¶

本模块是"累积项目：四足控制器"的规划层核心。完成后你将拥有一个可实时运行的简化模型 MPC，输出期望接触力和 CoM 轨迹，为下游 WBC（Ch53）提供跟踪目标。

阶段 1：SRBD 模型实现（3-4 天） - 实现 SRBD 连续状态空间方程：状态 $\mathbf{x} = [\theta, \mathbf{r}_G, \boldsymbol{\omega}, \dot{\mathbf{r}}_G]^T \in \mathbb{R}^{13}$，输入 $\mathbf{u} = [\mathbf{f}_1, \mathbf{f}_2, \mathbf{f}_3, \mathbf{f}_4]^T \in \mathbb{R}^{12}$ - 实现欧拉离散化：$\mathbf{x}_{k+1} = A_k \mathbf{x}_k + B_k \mathbf{u}_k + \mathbf{d}_k$，提取离散矩阵 $A_k$, $B_k$ - 用 Pinocchio 计算 Go2 的整体惯量参数（总质量 $m$、质心惯量 $I_G$），与手动设定值对比 - 验证：给定恒力输入（四脚均匀支撑），仿真 CoM 轨迹应为匀速直线运动

阶段 2：凸 QP MPC 组装（3-4 天） - 按 Di Carlo 2018 的方法，将 $N$ 步 SRBD 动力学堆叠为大 QP：$\min \|\mathbf{X} - \mathbf{X}_{\text{ref}}\|_Q^2 + \|\mathbf{U}\|_R^2$ s.t. 动力学 + 摩擦锥 + 接触调度 - 接入 Ch50 模块的 QP 求解器（OSQP 或 ProxQP），warm-start 上一步的解 - 实现步态调度表（gait scheduler）：trot / walk / bound 的接触序列生成 - 性能目标：horizon $N=10$, 单步求解 $< 5$ ms

阶段 3：Centroidal 模型扩展（2-3 天） - 将 SRBD 的固定惯量 $I_G$ 替换为配置相关的 CMM $A_G(q)$（调用 Ch49 模块） - 对比 SRBD vs Centroidal 在大幅度运动（跳跃、快速转向）下的轨迹差异 - 理解非线性项的处理：Centroidal 模型需要 SQP 或逐步线性化

阶段 4：闭环仿真验证（2 天） - 构建 MPC 闭环：参考轨迹 → 简化模型 MPC → 期望力/CoM → 重新线性化 → 下一步 - 测试场景：原地站立、前进行走（trot）、45 度转弯 - 导出接口：solveMPC(state, reference, gait) → 返回期望接触力 $\mathbf{f}_i^*$ 和 CoM 轨迹 $\mathbf{r}_G^*$，供 Ch53 WBC 调用

特性	LIPM	SRBD	Centroidal
精确性	低	中	高（精确 6D 动量）
\(I_G\) 假设	无转动	恒定	无假设（精确）
维度	2-4	12-13	12
凸 QP？	✅	✅（线性化后）	⚠️ NLP（非线性）
实时性	极高	高	中（需 SQP）

现象	可能原因	排查方法
LIPM 仿真中 CoM 轨迹发散（指数增长）	未正确施加 ZMP 控制或 DCM 反馈，系统运行在不稳定极点上	检查 DCM 反馈增益 \(k_\xi > \omega\)（不稳定极点需要足够大的增益来镇定）；打印 DCM 轨迹确认是否在 capture point 附近收敛
SRBD MPC 输出的接触力全为零或分布极不均匀	摩擦锥约束与代价函数权重设置不平衡——力正则化权重过大会压制所有力，过小则优化器不关心力的大小	将力正则化权重 \(R\) 从 \(10^{-6}\) 开始逐步增大，观察力分布变化；检查接触调度矩阵是否正确标记了当前支撑脚
Centroidal 模型与全身动力学的 CoM 轨迹不一致	简化模型使用了固定惯量 \(I_G\)（SRBD 假设），但实际运动中腿部摆动引起的惯量变化不可忽略	对比 `pinocchio::computeCentroidalMap()` 的实时 CMM 与固定 \(I_G\) 的差异；在大幅摆腿场景（如跳跃）下差异最显著
SLIP 模型仿真中弹簧压缩量出现负值（腿被拉长）	RK4 积分步长过大，或落脚角 \(\theta_{td}\) 设置导致足端在支撑相中移出地面	减小积分步长至 \(\leq 0.001\) s；添加 `L >= L0` 的约束检测，在腿恢复自然长度时切换到飞行相
站立平衡 MPC 中机器人缓慢侧移或旋转	SRBD 模型的 CoM 位置估计有偏差，或重力补偿项中的质心位置与模型不一致	对比 Pinocchio `computeCenterOfMass()` 的结果与 SRBD 中使用的 CoM 常量；检查 URDF 连杆质量参数是否完整

模型	核心方程
LIPM	\(\ddot{x} = \omega^2(x - p)\), \(\omega = \sqrt{g/h}\)
DCM	\(\dot{\xi} = \omega(\xi - p)\), \(\xi = x + \dot{x}/\omega\)
ZMP	\(p_{\text{ZMP}} = x_{\text{CoM}} - \frac{z \ddot{x} - \dot{L}_y / m}{g + \ddot{z}}\)
SRBD (平动)	\(m\ddot{\mathbf{r}}_G = \sum \mathbf{f}_i + m\mathbf{g}\)
SRBD (转动)	\(I_G\dot{\boldsymbol{\omega}} + \boldsymbol{\omega}\times(I_G\boldsymbol{\omega}) = \sum(\mathbf{p}_i - \mathbf{r}_G)\times\mathbf{f}_i\)
Centroidal	\(\dot{\mathbf{h}}_G = \sum\) contact wrenches at CoM
Capture Point	\(\xi_{\text{capture}} = x + \dot{x}/\omega\)