SE(3) 上的几何力学——Euler-Poincare 方程、浮基动力学与 Lie-Poisson 结构

所属：刚体动力学专题 4-4 | 前置：10_空间向量代数（Plucker 变换、motion/force 叉积）、20_Lagrange与Hamilton力学（Euler-Lagrange 方程）、30_ON动力学递推算法（RNEA body-frame 递推） 交叉引用：→ 50_动力学解析微分 (Carpentier-Mansard 导数前置)、→ 05_运动控制/90_DDP、→ 05_运动控制/120_MPC

---

前置自测

📋 前置自测（答不出 >= 2 题 → 先回前置专题复习）

1. 写出 \(SE(3)\) 的矩阵表示形式，并解释为什么 \(SE(3)\) 不是 \(SO(3)\) 和 \(\mathbb{R}^3\) 的直积而是半直积？
2. 给定 \(T = (R, p) \in SE(3)\)，写出 \(\operatorname{Ad}_T\) 的 \(6\times6\) 矩阵显式形式（区分 angular/linear 排列）。
3. 解释 Featherstone 的 spatial velocity (motion vector) 对应李代数 \(\mathfrak{se}(3)\) 的哪个元素？body frame 还是 spatial frame？
4. 从变分原理 \(\delta S = 0\) 出发，推导标准 Euler-Lagrange 方程的关键步骤是什么？
5. RNEA 后向递推中的 f = Ia + v x* (Iv) 各符号代表什么？

---

本章目标

学完本章后，你将能够： 1. 从 Hamilton 原理出发完整推导 Euler-Poincare 方程，理解约束变分 \(\delta\xi = \dot\eta + [\xi,\eta]\) 的几何本质 2. 写出浮基机器人在 \(SE(3) \times \mathbb{T}^n\) 上的完整动力学方程，理解 \(n_q \neq n_v\) 的根本原因 3. 建立 Featherstone 空间向量代数与 Lie 群几何的精确对应，读懂 Pinocchio 源码中的每一个 cross 操作 4. 理解 Lie-Poisson 结构与余伴随轨道，掌握 Casimir 函数和辛叶的物理意义 5. 掌握 \(SE_2(3)\) 扩展位姿群在 IMU 预积分中的应用

---

知识树

SE(3) 几何力学
├── 根问题：浮基机器人的配置空间不是 R^n，如何写动力学？
├── 主干 1：Left-invariant Lagrangian 与 body velocity
│   ├── body velocity xi = g^{-1} g_dot in g
│   ├── spatial velocity eta = g_dot g^{-1} in g
│   └── 关系：eta = Ad_g xi
├── 主干 2：约束变分与 Euler-Poincare 方程
│   ├── 两参数族 g(t, epsilon)
│   ├── 关键引理：delta xi = eta_dot + ad_xi eta
│   └── EP 方程：d/dt(dL/d xi) = ad*_xi (dL/d xi) + tau
├── 主干 3：SE(3) 退化与浮基动力学
│   ├── SO(3) → Euler 刚体方程
│   ├── SE(3) → 6D Newton-Euler
│   ├── SE(3) x T^n → 浮基多体
│   └── Centroidal dynamics
├── 分支 A：Lie-Poisson 结构
│   ├── g* 上的 Poisson 括号
│   ├── Coadjoint orbits = symplectic leaves
│   └── Casimir 函数
├── 分支 B：SE_2(3) 扩展位姿群
│   ├── 用于 IMU 预积分
│   └── InEKF 的 process model
└── 分支 C：与 Featherstone 的精确对应
    ├── spatial velocity <-> body twist
    ├── Plucker transform <-> Ad_T
    └── spatial inertia <-> locked inertia

---

4.1 为什么 SE(3) 几何力学不是"数学装饰" ⭐

动机

考虑一个问题：你要为一台四足机器人写全身动力学控制器。机器人有 12 个关节，加上浮动基座一共 18 个自由度。你打开教科书，准备写 Euler-Lagrange 方程……

困难立刻出现：基座的"位置"要怎么参数化？

如果你选欧拉角：3 个角度 + 3 个平移 = 6 个广义坐标，配合 12 个关节角度，总共 18 维。看起来很好。但是——当俯仰角接近 90 度时，万向锁出现，Christoffel 符号奇异，数值积分崩溃。

如果你选四元数：4 个分量 + 3 个平移 = 7 维表示，但切空间只有 6 维（因为四元数有 \(|q|=1\) 的约束）。这意味着**位形维度 \(7+n\) 不等于速度维度 \(6+n\)**——标准的 Euler-Lagrange 方程 \(\frac{d}{dt}\frac{\partial L}{\partial \dot{q}} - \frac{\partial L}{\partial q} = \tau\) 直接失效，因为 \(\dot{q}\) 和广义力 \(\tau\) 的维度不匹配。

本质洞察：浮基机器人的配置空间天然是 \(SE(3) \times \mathbb{T}^n\)，不是 \(\mathbb{R}^{6+n}\)。这不是表示的选择问题，而是拓扑结构的硬约束——\(SO(3)\) 不可能无奇异地被 \(\mathbb{R}^3\) 全覆盖（"毛球定理"的推论）。

如果不用几何力学会怎样

让我们具体看看"强行用欧拉角"会遇到什么：

问题 1：坐标奇异。设 ZYX 欧拉角为 (phi, theta, psi)，角速度与欧拉角速率的关系为：

\[\omega = \begin{bmatrix} 1 & 0 & -\sin\theta \\ 0 & \cos\phi & \sin\phi\cos\theta \\ 0 & -\sin\phi & \cos\phi\cos\theta \end{bmatrix} \begin{bmatrix}\dot\phi \\ \dot\theta \\ \dot\psi\end{bmatrix}\]

当 \(\theta = \pi/2\) 时，这个矩阵奇异！物理上角速度有限，但欧拉角速率趋于无穷。

问题 2：伪张量。用欧拉角写出的质量矩阵 \(M(q)\) 包含大量 sin/cos 项，Christoffel 符号 \(\Gamma^k_{ij}\) 在不同参数化下形式完全不同。你花三天手推出来的公式，换一种欧拉角约定就全部作废。

问题 3：维度不匹配。即使勉强用四元数避免奇异，\(q \in \mathbb{R}^{7+n}\) 但 \(v \in \mathbb{R}^{6+n}\)，你必须引入约束 \(|q_{\text{quaternion}}| = 1\)，把问题变成 DAE（微分代数方程）。Pinocchio 的解决方案是定义 integrate(model, q, v*dt) 使用 Lie 群指数映射，彻底绕过这个问题。

几何力学的"一劳永逸"方案

Euler-Poincare 方程直接在 body frame 中工作，用李代数元素 \(\xi = g^{-1}\dot{g} \in \mathfrak{se}(3)\) 代替 \(\dot{q}\)，用余伴随作用 \(\mathrm{ad}^*_\xi\) 代替 Christoffel 项。它**天然避免表示奇异**——因为根本不使用任何参数化。

用一个类比来说明：

跨领域类比：想象你要描述地球表面一个人的运动。方案 A：用经纬度——在北极（经度未定义）失效。方案 B：直接用人"脚下"的局部切平面——永远有效，因为切空间在每一点都是 \(\mathbb{R}^2\)。Euler-Poincare 方程就是方案 B 的力学版本：它始终在 body frame 的切空间（李代数）中工作。

这不是纯数学的优雅。**实际工程后果**包括： - InEKF（Barrau-Bonnabel 2017）需要 SE(3) 上的不变动力学作为 process model - Centroidal dynamics（Orin-Goswami-Lee 2013）本质上是 SE(3) 上的 reduction - Lee-Leok-McClamroch 2010 的无人机几何控制直接在 SE(3) 上写 Lyapunov 函数 - Featherstone 的空间向量代数就是 se(3)/se(3)* 的矩阵实现

---

4.2 Left-Invariant Lagrangian 与 Body Velocity ⭐⭐

动机

刚体动能只依赖于角速度和质心线速度——不依赖于刚体在空间中的具体位置和朝向。用数学语言说：动能 \(T(g, \dot{g})\) 在 \(G\) 的左平移作用下不变。这个对称性是 Euler-Poincare 约化（reduction）的起点。

4.2.1 Body velocity 与 Spatial velocity 的精确定义

对一个配置在 Lie 群 \(G\) 上的系统，设 \(g(t) \in G\) 是时间参数化曲线。定义：

Body velocity（左平凡化速度）： $\(\xi = g^{-1}\dot{g} \in \mathfrak{g}\)$

Spatial velocity（右平凡化速度）： $\(\eta = \dot{g}\,g^{-1} \in \mathfrak{g}\)$

两者的关系： $\(\eta = \dot{g}\,g^{-1} = g\,(g^{-1}\dot{g})\,g^{-1} = g\,\xi\,g^{-1} = \mathrm{Ad}_g\,\xi\)$

这里 \(\mathrm{Ad}_g: \mathfrak{g} \to \mathfrak{g}\) 是 \(G\) 的**伴随表示**（adjoint representation）。

本质洞察：body velocity 描述的是"站在刚体上观察自己的运动"——你感受到的角速度和线速度；spatial velocity 描述的是"站在固定参考系中观察刚体运动"。两者通过 \(\mathrm{Ad}_g\) 变换，不相等！区分它们是整个几何力学的第一条生命线。

4.2.2 SE(3) 的具体化

对 \(SE(3)\)，群元 \(g = (R, p)\) 用齐次矩阵表示：

\[g = \begin{bmatrix} R & p \\ 0 & 1 \end{bmatrix}, \quad R \in SO(3),\ p \in \mathbb{R}^3\]

计算 body velocity：

\[\xi = g^{-1}\dot{g} = \begin{bmatrix} R^T & -R^T p \\ 0 & 1 \end{bmatrix}\begin{bmatrix} \dot{R} & \dot{p} \\ 0 & 0 \end{bmatrix} = \begin{bmatrix} R^T\dot{R} & R^T\dot{p} \\ 0 & 0 \end{bmatrix} = \begin{bmatrix} \hat\omega_b & v_b \\ 0 & 0 \end{bmatrix}\]

其中 \(\omega_b = (R^T\dot{R})^\vee\) 是 body frame 角速度，\(v_b = R^T\dot{p}\) 是 body frame 线速度。

类似地，spatial velocity：

\[\eta = \dot{g}\,g^{-1} = \begin{bmatrix} \dot{R}R^T & \dot{p} - \dot{R}R^T p \\ 0 & 0 \end{bmatrix} = \begin{bmatrix} \hat\omega_s & v_s \\ 0 & 0 \end{bmatrix}\]

其中 \(\omega_s = (\dot{R}R^T)^\vee = R\omega_b\) 是 spatial angular velocity，\(v_s = \dot{p} - \omega_s \times p\) 是**空间参考点处的速度**（注意这不是质心线速度的空间表达！）。

4.2.3 SO(3) 特例

设 \(R \in SO(3)\)，则 \(\dot{R} = R\hat{\omega}_b\)（body frame）\(= \hat{\omega}_s R\)（spatial frame），即 \(\omega_s = R\omega_b\)。

刚体动能 \(T = \frac{1}{2}\omega_b^T I \omega_b\) 只用 body 角速度写出——这正是 left-invariant Lagrangian 的原型：\(T(R, \dot{R}) = T(e, R^{-1}\dot{R}) = T(e, \hat{\omega}_b) = \ell(\omega_b)\)。

4.2.4 Left-invariant Lagrangian 的约化

对一个配置在 Lie 群 \(G\) 上的系统，设完整 Lagrangian \(L: TG \to \mathbb{R}\)。若 \(L\) 是**左不变的**，即对所有 \(h \in G\)：

\[L(h \cdot g,\ h \cdot \dot{g}) = L(g,\ \dot{g})\]

则 \(L\) 只依赖于 body velocity \(\xi = g^{-1}\dot{g}\)，因此**descend**到李代数 \(\mathfrak{g}\) 上：

\[\ell: \mathfrak{g} \to \mathbb{R}, \quad \ell(\xi) := L(e, \xi) = L(g, g\xi)\]

这个 \(\ell\) 称为**约化 Lagrangian**（reduced Lagrangian）。

为什么这很重要？原始 Lagrangian \(L\) 定义在 \(TG\)（切丛）上——对 \(SE(3)\) 这是 12 维空间。约化后 \(\ell\) 定义在 \(\mathfrak{g}\)（李代数）上——只有 6 维。这就是"约化"的威力：利用对称性降维。

4.2.5 与 Pinocchio 的对应

Pinocchio 中： - pinocchio::MotionTpl<Scalar> 对应 \(\mathfrak{se}(3)\) 元素（\(\xi = (\omega, v)\)） - data.v[i] 存储的是关节 i 的 body frame velocity - SE3::act(Motion) 执行 \(\mathrm{Ad}_T\) 变换 - SE3::actInv(Motion) 执行 \(\mathrm{Ad}_{T^{-1}}\) 变换

// Pinocchio 中 body velocity 的获取
pinocchio::forwardKinematics(model, data, q, v);
// data.v[i] 是 joint i 的 body-frame spatial velocity
// 等价于 xi_i = g_i^{-1} dot{g_i} (in se(3))

// 如果需要 world-frame velocity：
pinocchio::Motion v_world = data.oMi[i].act(data.v[i]);
// 等价于 eta_i = Ad_{g_i} xi_i

⚠️ 常见陷阱

⚠️ 概念误区：认为 "body velocity 就是在 body frame 表达的速度向量"

新手想法：body velocity \(\xi = (\omega_b, v_b)\) 中的 \(v_b\) 就是质心线速度在 body frame 中的投影。

实际上：\(v_b = R^T\dot{p}\) 确实是质心线速度在 body frame 中的表达。但 spatial velocity 中的 \(v_s = \dot{p} - \omega_s \times p\) 并**不是**质心线速度在 world frame 中的表达！它是"空间参考点（原点固定）处的速度"。这是 Featherstone 6D spatial velocity 最容易混淆的地方。

正确理解：spatial velocity 是 \(\mathfrak{se}(3)^*\) 对偶配对意义下的对象，其物理意义是"对偶于 wrench 在计算功率时的对应量"。

⚠️ 编程陷阱：在 Pinocchio 中混淆 LOCAL 和 WORLD frame

data.v[i] 是 LOCAL (body) frame。如果你把它当作 WORLD frame 使用，所有下游计算都会错。特别是在 WBC/TSID 中，任务 Jacobian 的 reference frame 必须与速度表达一致。

---

4.3 李群上的变分原理——约束变分 ⭐⭐⭐

动机

在标准 Lagrange 力学中，Hamilton 原理要求 \(\delta S = \delta\int L\,dt = 0\)，其中变分 \(\delta q\) 是**自由的**（端点为零即可）。但在 Lie 群上，情况发生了根本变化：约化后的变分 \(\delta\xi\) 不是自由的——它被一个约束方程锁死。理解这个约束，是推导 Euler-Poincare 方程最精妙也最关键的一步。

4.3.1 两参数族与 Maurer-Cartan 形式

考虑 \(G\) 中的一个两参数族 \(g(t, \varepsilon)\)： - \(t\) 是时间参数 - \(\varepsilon\) 是变分参数（\(\varepsilon = 0\) 时为真实运动，\(\partial/\partial\varepsilon|_{\varepsilon=0}\) 给出变分方向）

定义两个 \(\mathfrak{g}\) 值函数： $\(\xi(t, \varepsilon) = g^{-1}\frac{\partial g}{\partial t} \in \mathfrak{g} \quad \text{(body velocity along time)}\)$ $\(\eta(t, \varepsilon) = g^{-1}\frac{\partial g}{\partial \varepsilon} \in \mathfrak{g} \quad \text{(body "velocity" along variation)}\)$

在 \(\varepsilon = 0\) 处：\(\xi(t, 0)\) 就是真实运动的 body velocity，\(\eta(t, 0)\) 就是变分方向（在 body frame 中表达）。

4.3.2 关键引理的完整证明

引理（Marsden-Ratiu Lemma 13.5.2）：

\[\delta\xi = \dot\eta + [\xi, \eta] = \dot\eta + \mathrm{ad}_\xi\,\eta\]

其中 \(\delta\xi := \partial\xi/\partial\varepsilon|_{\varepsilon=0}\)，dot 表示 \(d/dt\)。

完整证明（对矩阵 Lie 群直接验证）：

Step 1：计算 \(\partial(g^{-1})/\partial\varepsilon\)。

由 \(g\,g^{-1} = I\) 两侧对 \(\varepsilon\) 求导： $\(\frac{\partial g}{\partial\varepsilon}\,g^{-1} + g\,\frac{\partial g^{-1}}{\partial\varepsilon} = 0\)$ $\(\Rightarrow\quad \frac{\partial g^{-1}}{\partial\varepsilon} = -g^{-1}\frac{\partial g}{\partial\varepsilon}\,g^{-1} = -\eta\,g^{-1}\)$

Step 2：对 \(\xi = g^{-1}\partial_t g\) 关于 \(\varepsilon\) 求导：

\[\delta\xi = \frac{\partial}{\partial\varepsilon}\bigl(g^{-1}\frac{\partial g}{\partial t}\bigr) = \frac{\partial g^{-1}}{\partial\varepsilon}\cdot\frac{\partial g}{\partial t} + g^{-1}\cdot\frac{\partial^2 g}{\partial\varepsilon\partial t}\]

Step 3：利用 Step 1 的结果：

第一项：\(\frac{\partial g^{-1}}{\partial\varepsilon}\cdot\frac{\partial g}{\partial t} = -\eta\,g^{-1}\cdot\frac{\partial g}{\partial t} = -\eta\,\xi\)

Step 4：处理混合偏导项。由于 g 是光滑函数，混合偏导交换：

\[\frac{\partial^2 g}{\partial\varepsilon\partial t} = \frac{\partial^2 g}{\partial t\partial\varepsilon}\]

因此： $\(g^{-1}\frac{\partial^2 g}{\partial\varepsilon\partial t} = g^{-1}\frac{\partial}{\partial t}\bigl(\frac{\partial g}{\partial\varepsilon}\bigr) = g^{-1}\frac{\partial}{\partial t}(g\eta) = g^{-1}(\dot{g}\eta + g\dot\eta)\)$ $\(= g^{-1}\dot{g}\,\eta + \dot\eta = \xi\eta + \dot\eta\)$

Step 5：合并 Step 3 和 Step 4：

\[\delta\xi = -\eta\xi + \xi\eta + \dot\eta = [\xi, \eta] + \dot\eta = \dot\eta + \mathrm{ad}_\xi\,\eta \quad \blacksquare\]

4.3.3 约束变分的物理意义

这个引理告诉我们一个深刻的事实：在 Lie 群上，变分 \(\eta\) 可以任意选取（只要端点为零），但由此引起的速度变分 \(\delta\xi\) 不是自由的——它被 \(\eta\) 通过 \(\delta\xi = \dot{\eta} + \mathrm{ad}_\xi\,\eta\) 完全确定。

跨领域类比：想象一根不可伸长的绳子约束了两个质点。你可以自由移动一个质点（类比于自由选取 eta），但另一个质点的运动（类比于 delta xi）被绳子约束锁死。在 Lie 群上，"绳子"就是群结构本身——它把位形变分和速度变分绑在一起。

反事实推理：如果 \(\delta\xi\) 是自由的（像 \(\mathbb{R}^n\) 中那样），那么 Hamilton 原理直接给出标准 Euler-Lagrange 方程 \(\frac{d}{dt}\frac{\partial\ell}{\partial\xi} - \ldots\) 但等等，约化后的 \(\ell(\xi)\) 不依赖于"位形"——所以如果 \(\delta\xi\) 自由，我们只会得到 \(\frac{d}{dt}\frac{\partial\ell}{\partial\xi} = 0\)，即动量守恒。这对自由刚体的**空间**角动量确实成立，但对 body 动量则不成立（body 动量 \(\pi = I\omega\) 的方向在变化！）。约束变分恰好补回了这个差异——它产生额外的 \(\mathrm{ad}^*_\xi\) 项，描述的正是 body frame 本身在旋转这个事实。

4.3.4 SO(3) 上的具体验证

在 \(SO(3)\) 上：\(g = R\)，\(\xi = \omega\)（body angular velocity），\(\eta\) 也是 \(\mathbb{R}^3\) 向量，Lie bracket \([\xi, \eta] = \omega \times \eta\)。

因此约束变分变为： $\(\delta\omega = \dot\eta + \omega \times \eta\)$

这正是 Marsden-Ratiu Ch. 15 中刚体问题的标准结果。物理直觉：即使 \(\eta\) 是固定的空间方向，body frame 中观察到的变分方向也会因旋转而变化——额外的 \(\omega \times \eta\) 正是这个"随体旋转"效应。

练习

1. **（手推）**对 SO(3)，取 R(t, epsilon) = R(t) exp(epsilon hat{eta(t)})。直接计算 xi = R^T R_dot 和 delta xi = d/d epsilon|_0，验证 delta omega = dot{eta} + omega x eta。
2. **（概念）**解释为什么端点条件 eta(t_0) = eta(t_1) = 0 在 Lie 群上的物理含义是"变分保持初末位形不变"。
3. **（进阶）**对 SE(3)，将 eta = (eta_omega, eta_v) 和 xi = (omega, v) 代入约束变分公式，写出 delta omega 和 delta v 的分量形式。

---

4.4 Euler-Poincare 方程的完整推导 ⭐⭐

动机

我们现在有了两个关键要素：(1) 约化 Lagrangian \(\ell(\xi)\)；(2) 约束变分 \(\delta\xi = \dot{\eta} + \mathrm{ad}_\xi\,\eta\)。把它们代入 Hamilton 原理，就能推出几何力学的"主方程"——Euler-Poincare 方程。

4.4.1 推导的起点：Hamilton 原理

Hamilton 原理要求对所有满足端点条件的变分，作用量取极值：

\[\delta S = \delta\int_{t_0}^{t_1} \ell(\xi)\,dt = 0\]

计算变分（\(\ell\) 只依赖 \(\xi\)）：

\[\delta S = \int_{t_0}^{t_1} \left\langle \frac{\partial\ell}{\partial\xi},\ \delta\xi \right\rangle dt\]

其中尖括号表示 \(\mathfrak{g}^*\) 与 \(\mathfrak{g}\) 之间的对偶配对（即把 \(\partial\ell/\partial\xi \in \mathfrak{g}^*\) 作用在 \(\delta\xi \in \mathfrak{g}\) 上）。

4.4.2 代入约束变分

将 \(\delta\xi = \dot{\eta} + \mathrm{ad}_\xi\,\eta\) 代入：

\[\delta S = \int_{t_0}^{t_1} \left\langle \frac{\partial\ell}{\partial\xi},\ \dot\eta + \mathrm{ad}_\xi\,\eta \right\rangle dt\]

拆成两个积分：

\[\delta S = \underbrace{\int_{t_0}^{t_1} \left\langle \frac{\partial\ell}{\partial\xi},\ \dot\eta \right\rangle dt}_{I_1} + \underbrace{\int_{t_0}^{t_1} \left\langle \frac{\partial\ell}{\partial\xi},\ \mathrm{ad}_\xi\,\eta \right\rangle dt}_{I_2}\]

4.4.3 处理第一个积分：分部积分

对 \(I_1\) 做分部积分：

\[I_1 = \left[\left\langle \frac{\partial\ell}{\partial\xi},\ \eta \right\rangle\right]_{t_0}^{t_1} - \int_{t_0}^{t_1} \left\langle \frac{d}{dt}\frac{\partial\ell}{\partial\xi},\ \eta \right\rangle dt\]

由端点条件 \(\eta(t_0) = \eta(t_1) = 0\)，边界项消失：

\[I_1 = -\int_{t_0}^{t_1} \left\langle \frac{d}{dt}\frac{\partial\ell}{\partial\xi},\ \eta \right\rangle dt\]

4.4.4 处理第二个积分：余伴随定义

对 \(I_2\)，利用 coadjoint action \(\mathrm{ad}^*_\xi\) 的定义。对任意 \(\mu \in \mathfrak{g}^*\)，\(\eta \in \mathfrak{g}\)：

\[\langle \mathrm{ad}^*_\xi\,\mu,\ \eta \rangle := \langle \mu,\ \mathrm{ad}_\xi\,\eta \rangle\]

因此：

\[I_2 = \int_{t_0}^{t_1} \langle \frac{\partial\ell}{\partial\xi},\ \mathrm{ad}_\xi\,\eta \rangle\,dt = \int_{t_0}^{t_1} \langle \mathrm{ad}^*_\xi\,\frac{\partial\ell}{\partial\xi},\ \eta \rangle\,dt\]

4.4.5 合并与基本引理

\[\delta S = \int_{t_0}^{t_1} \left\langle -\frac{d}{dt}\frac{\partial\ell}{\partial\xi} + \mathrm{ad}^*_\xi\,\frac{\partial\ell}{\partial\xi},\ \eta \right\rangle dt = 0\]

由于 \(\eta\) 在端点为零的前提下**可以任意选取**（这是变分原理的基本引理），被积函数的"系数"必须恒为零：

\[\boxed{\frac{d}{dt}\frac{\partial\ell}{\partial\xi} = \mathrm{ad}^*_\xi\left(\frac{\partial\ell}{\partial\xi}\right) + \tau}\]

这就是 Euler-Poincare 方程（加入外广义力 \(\tau \in \mathfrak{g}^*\) 后的形式）。

4.4.6 方程各项的物理意义

项	含义	R^n 类比
\(\partial\ell/\partial\xi \in \mathfrak{g}^*\)	body frame 广义动量 \(\mu\)	\(p = \partial L/\partial\dot{q}\)
\(\frac{d}{dt}(\partial\ell/\partial\xi)\)	body 动量的时间导数	\(\dot{p}\)
\(\mathrm{ad}^*_\xi(\partial\ell/\partial\xi)\)	科里奥利/离心力在 body frame 中的表现	Christoffel 项 \(\Gamma^k_{ij}\dot{q}^i\dot{q}^j\)
\(\tau \in \mathfrak{g}^*\)	外广义力/力矩	外力 \(Q\)

本质洞察：\(\mathrm{ad}^*_\xi(\partial\ell/\partial\xi)\) 这一项不是"新的物理"，而是 body frame 本身在旋转的几何效应。在 spatial frame 中，自由刚体的动量守恒（\(\dot{p} = 0\)）；但在 body frame 中，由于坐标系随体旋转，必须补偿这个旋转效应——这正是 \(\mathrm{ad}^*\) 项的角色。它和 \(\mathbb{R}^n\) 中的 Christoffel 符号扮演完全相同的角色，只是在 Lie 群语言中表达得更加优雅和坐标无关。

4.4.7 Euler-Poincare 方程的 Body/Spatial 两种形式

Body 形式（xi = g^{-1} g_dot，左约化）： $\(\frac{d\mu}{dt} = \mathrm{ad}^*_\xi\,\mu + \tau_b, \quad \mu = \frac{\partial\ell}{\partial\xi} \in \mathfrak{g}^*\)$

Spatial 形式（eta = g_dot g^{-1}，右约化）： $\(\frac{d\nu}{dt} = -\mathrm{ad}^*_\eta\,\nu + \tau_s, \quad \nu = \frac{\partial\ell}{\partial\eta} \in \mathfrak{g}^*\)$

注意**符号相反**！这来自左/右约化的差异。多数机器人学文献用 body 形式（Featherstone、Pinocchio），流体力学用 spatial 形式（Euler 流体方程就是右约化的结果）。

⚠️ 常见陷阱

⚠️ 思维陷阱：认为"Euler-Poincare 方程只是 Euler-Lagrange 的另一种写法"

新手想法：两个方程描述相同的物理，只是符号不同。

实际上：Euler-Lagrange 定义在切丛 TG 上（2n 维），而 Euler-Poincare 定义在李代数 g 上（n 维）——维度直接减半。这不只是"换记号"，而是利用了对称性的真正降维。代价是：从 EP 方程的解 xi(t) 恢复位形 g(t) 需要额外积分 g_dot = g xi（重构方程）。

⚠️ 概念误区：ad* 中的符号约定混乱

Marsden-Ratiu 定义 angle{ad*_xi mu, eta} = angle{mu, ad_xi eta}（正号）。某些文献（如 Holm 系列）用相反约定。Featherstone 的 force cross v x* f 对应**正号** ad*（在我们约定下）。阅读任何文献前先确认符号约定。

练习

1. **（核心手推）**从 Hamilton 原理出发，独立完成 Euler-Poincare 方程的完整推导。不看笔记，在草稿纸上从 delta integral l(xi) dt = 0 写到最终方程。限时 30 分钟。
2. **（对比）**对同一个自由刚体（SO(3) 上），分别用标准 Euler-Lagrange 方程（选欧拉角）和 Euler-Poincare 方程推导运动方程，比较推导复杂度。
3. **（概念）**解释为什么自由刚体的空间角动量 pi_s = R I omega 守恒，但 body 角动量 pi_b = I omega 不守恒。从 EP 方程的角度，守恒量对应什么？

---

4.5 SO(3) 退化：Euler 刚体方程 ⭐⭐

动机

让我们验证 Euler-Poincare 方程在最简单情况——SO(3) 上的自由刚体——确实退化为经典 Euler 方程。这不仅是 sanity check，也能让我们对 ad* 的具体矩阵形式建立手感。

4.5.1 SO(3) 上的具体计算

李代数：\(\mathfrak{so}(3)\) 同构于 \((\mathbb{R}^3, \times)\)，其中 \(\times\) 是标准叉积。Lie bracket \([\xi, \eta] = \xi \times \eta\)。

对偶配对：\(\langle\mu, \eta\rangle = \mu \cdot \eta\)（标准内积）。

ad 算子：\(\mathrm{ad}_\xi\,\eta = [\xi, \eta] = \xi \times \eta\)。用矩阵表示：\(\mathrm{ad}_\xi = \hat{\xi}\)（反对称矩阵）。

ad* 算子：由定义 \(\langle\mathrm{ad}^*_\xi\,\mu, \eta\rangle = \langle\mu, \mathrm{ad}_\xi\,\eta\rangle = \mu \cdot (\xi \times \eta) = (\mu \times \xi) \cdot \eta\)，因此：

\[\mathrm{ad}^*_\xi\,\mu = \mu \times \xi = -\xi \times \mu\]

约化 Lagrangian：\(\ell(\omega) = \frac{1}{2}\omega^T I\omega\)，其中 \(I\) 是 body frame 转动惯量张量。

广义动量：\(\mu = \partial\ell/\partial\omega = I\omega := \pi\)（body angular momentum）。

4.5.2 代入 Euler-Poincare

\[\frac{d}{dt}(I\omega) = \mathrm{ad}^*_\omega(I\omega) + \tau = (I\omega) \times \omega + \tau\]

即： $\(I\dot\omega = (I\omega) \times \omega + \tau = \pi \times \omega + \tau\)$

等价地（把交叉乘移到另一边）：

\[\boxed{I\dot\omega + \omega \times (I\omega) = \tau}\]

这就是经典 Euler 刚体方程！ 它不是一个独立的"物理定律"，而是 SO(3) 上 Euler-Poincare 方程的**自动推论**。

4.5.3 物理意义与能量守恒

对于自由旋转（\(\tau = 0\)），Euler 方程给出 \(\frac{d}{dt}(I\omega) = \pi \times \omega\)。用 \(\pi = I\omega\) 改写：

\[\dot\pi = \pi \times (I^{-1}\pi)\]

这说明 body angular momentum 的**模长守恒**： $\(\frac{d}{dt}|\pi|^2 = 2\pi \cdot \dot\pi = 2\pi \cdot (\pi \times I^{-1}\pi) = 0\)$

（因为 \(\mathbf{a} \cdot (\mathbf{a} \times \mathbf{b}) = 0\) 恒成立。）

同时动能也守恒： $\(T = \frac{1}{2}\omega^T I\omega = \frac{1}{2}\pi^T I^{-1}\pi\)$

\[\frac{dT}{dt} = \pi^T I^{-1}\dot\pi = \omega \cdot (\pi \times \omega) = 0\]

这两个守恒量（\(|\pi|^2\) 和 \(T\)）把 3D 的相空间约束到 1D 曲线——这正是 Poinsot 构造（能量椭球与角动量球面的交线）。

跨领域类比：Euler 方程和陀螺仪的进动有着密切关系。想象你手持一个快速旋转的自行车轮——当你试图改变轮轴方向时，轮子会"反抗"你的力矩方向，产生 90 度的进动。Euler 方程中的 omega x (I omega) 项正是描述这种进动效应——它把施加的力矩"转"了一个方向，这是因为 body frame 本身在旋转。

⚠️ 常见陷阱

⚠️ 概念误区：认为 Euler 方程中的 omega x (I omega) 是"力矩"

新手想法：方程 I omega_dot = tau - omega x (I omega) 中，右边第二项是"某种力矩"（科里奥利力矩？）。

实际上：omega x (I omega) 不是真实的物理力矩——它是**由于选择了旋转参考系（body frame）而产生的数学项**。在 spatial frame 中，自由刚体的方程是 d(R I omega)/dt = 0（角动量守恒，无额外项）。额外项完全来自"坐标系本身在旋转"——这正是 ad*_xi mu 的几何含义。

---

4.6 SE(3) 退化：单刚体 6D Newton-Euler ⭐⭐

动机

从 SO(3) 到 SE(3) 的推广，对应从"纯旋转刚体"到"可平移+旋转的完整刚体"。这个推广直接给出 Featherstone 教科书 (2.3)-(2.4) 的 body-frame 单刚体方程。

4.6.1 se(3) 的 Lie bracket

取 \(\mathfrak{g} = \mathfrak{se}(3)\) 同构于 \(\mathbb{R}^6\)，body spatial velocity \(\xi = (\omega;\, v) \in \mathbb{R}^6\)。\(\mathfrak{se}(3)\) 的 Lie bracket（即 Featherstone 的 motion cross）：

\[[\xi_1, \xi_2] = \mathrm{ad}_{\xi_1}\xi_2 = \begin{bmatrix} \omega_1 \times \omega_2 \\ \omega_1 \times v_2 - \omega_2 \times v_1 \end{bmatrix}\]

用 \(6\times6\) 矩阵表示：

\[\mathrm{ad}_\xi = \begin{bmatrix} [\omega]_\times & 0 \\ [v]_\times & [\omega]_\times \end{bmatrix}\]

4.6.2 对偶作用 ad*（coadjoint）与 Featherstone force cross

对偶配对：对 \(\mu = (m;\, f) \in \mathfrak{se}(3)^*\)（角动量 \(m\)，线动量 \(f\)）和 \(\xi = (\omega;\, v) \in \mathfrak{se}(3)\)：

\[\langle \mu, \xi \rangle = m \cdot \omega + f \cdot v = \text{power}\]

数学定义（coadjoint）：由 \(\langle \mathrm{ad}^*_\xi \mu,\, \eta \rangle = \langle \mu,\, \mathrm{ad}_\xi \eta \rangle\) 推得：

\[\mathrm{ad}^*_\xi\,\mu = \mathrm{ad}_\xi^T\,\mu = \begin{bmatrix} -\omega \times m - v \times f \\ -\omega \times f \end{bmatrix}\]

矩阵形式：\(\mathrm{ad}^*_\xi = \mathrm{ad}_\xi^T = \begin{bmatrix} -[\omega]_\times & -[v]_\times \\ 0 & -[\omega]_\times \end{bmatrix}\)

Featherstone 的 force cross 算子定义为相反符号：

\[\xi \times^* \mu \;:=\; -\mathrm{ad}^*_\xi\,\mu \;=\; -\mathrm{ad}_\xi^T\,\mu = \begin{bmatrix} \omega \times m + v \times f \\ \omega \times f \end{bmatrix}\]

矩阵形式：\([\xi]_{\times^*} = -\mathrm{ad}_\xi^T = \begin{bmatrix} [\omega]_\times & [v]_\times \\ 0 & [\omega]_\times \end{bmatrix}\)

符号警告：两种记法在文献中均常见。Murray/Li/Sastry、Marsden/Ratiu 使用数学 \(\mathrm{ad}^* = \mathrm{ad}^T\)；Featherstone RBDA 使用 \(\times^* = -\mathrm{ad}^T\)。本文 Euler-Poincare 方程使用数学约定（\(\mathrm{ad}^*\)），空间向量递推算法中使用 Featherstone 约定（\(\times^*\)）。混用是最常见的符号错误来源。

4.6.3 Spatial inertia 与约化 Lagrangian

body frame 下的 spatial inertia（6x6 对称正定矩阵）：

\[G_b = \begin{bmatrix} I_c + m[c]_\times^T[c]_\times & m[c]_\times \\ m[c]_\times^T & mE_3 \end{bmatrix} = \begin{bmatrix} \bar{I} & m[c]_\times \\ m[c]_\times^T & mE_3 \end{bmatrix}\]

其中 \(I_c\) 是绕质心的转动惯量，\(\bar{I} = I_c - m[c]_\times^2 = I_c + m[c]_\times^T[c]_\times\) 是平行轴修正后的绕 body frame 原点的转动惯量，\(c\) 是 body frame 中质心位置矢量（从 body frame 原点到质心），\(m\) 是质量。此形式与空间向量代数（第 10 章）的 Featherstone 公式一致。

当 body frame 原点取在质心时（c = 0），简化为：

\[G_b = \begin{bmatrix} I_{cm} & 0 \\ 0 & mE_3 \end{bmatrix}\]

约化 Lagrangian：\(\ell(\xi) = \frac{1}{2}\xi^T G_b\,\xi\)。

广义动量：\(\mu = \partial\ell/\partial\xi = G_b\,\xi \in \mathfrak{se}(3)^*\)。

4.6.4 SE(3) 上的 Euler-Poincare

代入左不变 EP 方程 \(\dot\mu = \mathrm{ad}^*_\xi \mu + \mathcal{F}_{\mathrm{ext}}\)（其中 \(\mathrm{ad}^* = \mathrm{ad}^T\)，数学约定）：

\[G_b\dot\xi = \mathrm{ad}^*_\xi(G_b\xi) + \mathcal{F}_{\mathrm{ext}}\]

展开成分量（body frame 原点在质心，\(\mu = G_b\xi = (I_{cm}\omega;\, mv)\)）：

\[\mathrm{ad}^*_\xi(G_b\xi) = \mathrm{ad}_\xi^T \begin{bmatrix} I_{cm}\omega \\ mv \end{bmatrix} = \begin{bmatrix} -\omega \times (I_{cm}\omega) - v \times (mv) \\ -\omega \times (mv) \end{bmatrix}\]

注意 \(v \times (mv) = m(v \times v) = 0\)，因此 EP 方程变为：

\[\begin{bmatrix} I_{cm}\dot\omega \\ m\dot v \end{bmatrix} = \begin{bmatrix} -\omega \times (I_{cm}\omega) \\ -\omega \times (mv) \end{bmatrix} + \begin{bmatrix}\tau_{\mathrm{ext}} \\ f_{\mathrm{ext}}\end{bmatrix}\]

整理后得到经典形式：

\[\boxed{I_{cm}\dot\omega + \omega \times (I_{cm}\omega) = \tau_{\mathrm{ext}}}$$ $$\boxed{m\dot v + \omega \times (mv) = f_{\mathrm{ext}}}\]

第一个方程就是 Euler 旋转方程，第二个是 body frame 中的线动量方程（\(\omega \times (mv)\) 是由于 body frame 随体旋转产生的科里奥利/离心项）。

关键验证：如果错误地使用 Featherstone 的 force cross 符号 \(\xi \times^* = -\mathrm{ad}^T\) 代入 EP 方程，会得到 \(I\dot\omega - \omega \times I\omega = \tau\)（错号！）。这是混用两种 \(\mathrm{ad}^*\) 约定的经典后果。

当 body frame 原点不在质心时，\(G_b\) 有非对角块耦合，方程中出现额外的陀螺耦合项——这正是 Featherstone RBDA (2.3)-(2.4) 的完整形式。

本质洞察：整个 6D Newton-Euler 方程就是 SE(3) 上的 Euler-Poincare！Featherstone 没有用 Lie 群语言，但他的 spatial algebra 编码了完全相同的几何内容。"motion cross" = ad，"force cross" \(\xi\times^* = -\mathrm{ad}^T\)，"spatial inertia" = 从 \(\mathfrak{se}(3)\) 到 \(\mathfrak{se}(3)^*\) 的映射。用 Featherstone 记号重写 EP：\(\mathcal{I}\dot{\hat{v}} + \hat{v} \times^* (\mathcal{I}\hat{v}) = \hat{f}_{\mathrm{ext}}\)（注意此时 force cross 出现在等号左边）。

4.6.5 与 Lynch-Park Modern Robotics 的对应

Lynch-Park Ch. 8 的 (8.40) 式：

\[\mathcal{G}_b\dot{\mathcal{V}}_b - [\mathrm{ad}_{\mathcal{V}_b}]^T \mathcal{G}_b\mathcal{V}_b = \mathcal{F}_b\]

这里 G_b 是 spatial inertia，V_b 是 body twist，F_b 是 body wrench。与我们的 EP 方程完全一致：

\[G_b\dot\xi - \mathrm{ad}^*_\xi(G_b\xi) = \mathcal{F}_{\mathrm{ext}} \quad \Leftrightarrow \quad G_b\dot\xi = \mathrm{ad}^*_\xi(G_b\xi) + \mathcal{F}_{\mathrm{ext}}\]

（注意 \(-[-\mathrm{ad}^T_\xi\,G_b\xi] = +\mathrm{ad}^*_\xi\,G_b\xi\)，符号由 ad* = -ad^T 给出。）

练习

1. **（手推）**对 SE(3)，取 body frame 原点不在质心（c != 0），完整展开 G_b dot{xi} = ad*_xi (G_b xi) + F_ext 的 6 个分量方程，验证与 Featherstone RBDA (2.3)-(2.4) 一致。
2. **（概念）**解释为什么当 body frame 原点在质心时，旋转和平移方程解耦（只通过 omega x mv 弱耦合），但原点不在质心时强耦合。

---

4.7 浮基多体系统动力学 ⭐⭐

动机

单刚体的 6D Newton-Euler 只是起点。真正的挑战是：一个由多个刚体通过关节连接的**多体系统**，其中基座是自由浮动的（如四足、人形、水下机器人）。如何把 Euler-Poincare 的几何智慧融入 Featherstone 的 O(N) 递推中？

4.7.1 配置空间的拓扑

配置空间：\(Q = SE(3) \times \mathbb{T}^n\)

• 基座 \(g \in SE(3)\)：6-DoF（或用四元数表示时 7 维参数化）
• 关节 \(q_j \in \mathbb{T}^n\)：\(n\) 个关节角度（\(\mathbb{T}\) 是圆周群 \(S^1\) 或 \(\mathbb{R}\)）

广义速度：\(v = (\xi_b;\, \dot{q}_j) \in \mathbb{R}^{6+n}\)

• \(\xi_b = g^{-1}\dot{g}\) 是基座的 body velocity（6 维）
• \(\dot{q}_j\) 是关节速度（\(n\) 维）

关键不匹配：位形维度 \(= 7 + n\)（四元数 4 维 + 平移 3 维 + \(n\) 关节），但速度维度 \(= 6 + n\)。这是因为 \(SO(3)\) 的参数化（4 维四元数）比其切空间（3 维 \(\mathfrak{so}(3)\)）多一维。

反事实推理：如果你忽视 \(n_q \neq n_v\)，直接写 \(q_{k+1} = q_k + v_k \Delta t\)，会发生什么？四元数部分的模长会偏离 1（打破 \(SO(3)\) 约束），旋转矩阵不再正交，之后所有依赖 \(R\) 的计算全部错误。正确做法是 Lie 群积分：q_next = integrate(model, q, v * dt)，内部对基座部分使用 exp 映射。

4.7.2 混合形式动力学方程

对基座部分用 Euler-Poincare（因为 SE(3) 是 Lie 群），对关节部分用标准 Euler-Lagrange（因为 T^n 同构于 R^n），合起来的 (6+n) 维方程：

\[M(q)\dot v + C(q,v)v + g(q) = S^T\tau + J_c^T\lambda\]

符号	含义	维度
M(q)	广义质量矩阵	(6+n) x (6+n)
C(q,v)v	科里奥利+离心力	(6+n) x 1
g(q)	重力项	(6+n) x 1
S	选择矩阵 [0_{n x 6}, I_n]^T	(6+n) x n
tau	关节力矩	n x 1
J_c	接触 Jacobian	n_c x (6+n)
lambda	接触力	n_c x 1

S 矩阵的含义：基座的前 6 行是 unactuated（没有电机直接驱动浮动基座），所以关节力矩 tau 只出现在后 n 行。S = [0; I] 正是这个选择。

4.7.3 M 矩阵的结构

\[M(q) = \begin{bmatrix} M_{bb} & M_{bj} \\ M_{jb} & M_{jj} \end{bmatrix}\]

• \(M_{bb} \in \mathbb{R}^{6\times6}\)：基座的"锁定惯性"（locked inertia）——当所有关节锁死时，整机作为单刚体的 6D 惯性
• \(M_{bj} = M_{jb}^T\)：基座-关节耦合
• \(M_{jj}\)：关节空间质量矩阵

关键物理含义：当某个关节加速时（q_ddot_j != 0），通过 M_{bj} 会在基座上产生反力——这就是为什么人形机器人挥臂会影响躯干平衡。反过来，基座加速也通过 M_{jb} 影响关节。

4.7.4 Pinocchio 的实现

Pinocchio 在检测到 JointModelFreeFlyer 时，会在递推树的根节点插入 6-DoF 运动自由度：

pinocchio::Model model;
pinocchio::urdf::buildModel(urdf_path, pinocchio::JointModelFreeFlyer(), model);
// model.nq == 7 + n_joint (四元数 4 + 平移 3 + 关节)
// model.nv == 6 + n_joint (se(3) 切空间 3+3 + 关节)

// q 的前 7 维结构：
// q[0:3] = position (world frame)
// q[3:7] = unit quaternion (x, y, z, w) -- Eigen 约定
// 注意：Drake/MuJoCo 用 (w, x, y, z)，必须转换！

// v 的前 6 维结构：
// v[0:3] = linear velocity (body frame)
// v[3:6] = angular velocity (body frame)
// 注意：Pinocchio 内部存储是 (linear, angular)，但某些输出函数可能用 (angular, linear)

// 正确的积分方式：
Eigen::VectorXd q_next = pinocchio::integrate(model, q, v * dt);
// 错误：q_next = q + v * dt;  // 四元数部分会破坏 |q|=1

4.7.5 Euler-Lagrange 与 Euler-Poincare 混合推导的细节

让我们详细展开 SE(3) x T^n 上的混合方程是如何得到 M(q) v_dot + C(q,v) v + g(q) = S^T tau + J_c^T lambda 的。

Step 1：总 Lagrangian。设 L(g, q_j, xi_b, q_dot_j) = T - V。

动能分解为三部分： $\(T = \underbrace{\frac{1}{2}\xi_b^T M_{bb}(q_j)\,\xi_b}_{\text{基座纯动能}} + \underbrace{\xi_b^T M_{bj}(q_j)\,\dot q_j}_{\text{耦合动能}} + \underbrace{\frac{1}{2}\dot q_j^T M_{jj}(q_j)\,\dot q_j}_{\text{关节纯动能}}\)$

注意 \(M_{bb}\) 取决于关节角 \(q_j\)（因为改变关节角会改变各连杆相对于基座的位置，从而改变整机绕基座的等效惯性）。

Step 2：基座方程（EP 约化）。对基座的 SE(3) 部分应用 Euler-Poincare：

\[\frac{d}{dt}\frac{\partial T}{\partial\xi_b} = \mathrm{ad}^*_{\xi_b}\frac{\partial T}{\partial\xi_b} + \frac{\partial V}{\partial g}\bigg|_{\text{body frame}} + J_{c,b}^T\lambda\]

左侧展开： $\(\frac{d}{dt}(M_{bb}\xi_b + M_{bj}\dot q_j) = M_{bb}\dot\xi_b + \dot M_{bb}\xi_b + M_{bj}\ddot q_j + \dot M_{bj}\dot q_j\)$

右侧的 \(\mathrm{ad}^*_{\xi_b}\) 项给出 Coriolis/centrifugal 力在 body frame 中的表现。

Step 3：关节方程（EL）。对关节部分用标准 Euler-Lagrange：

\[\frac{d}{dt}\frac{\partial T}{\partial\dot q_j} - \frac{\partial T}{\partial q_j} = \tau_j + J_{c,j}^T\lambda\]

展开后得到标准的 \(M_{jb}\dot\xi_b + M_{jj}\ddot q_j + ...\) 形式。

Step 4：合并。将 Step 2 和 Step 3 合并为 (6+n) 维方程：

\[\begin{bmatrix}M_{bb} & M_{bj} \\ M_{jb} & M_{jj}\end{bmatrix}\begin{bmatrix}\dot\xi_b \\ \ddot q_j\end{bmatrix} + \begin{bmatrix}C_{bb} & C_{bj} \\ C_{jb} & C_{jj}\end{bmatrix}\begin{bmatrix}\xi_b \\ \dot q_j\end{bmatrix} + \begin{bmatrix}g_b \\ g_j\end{bmatrix} = \begin{bmatrix}0 \\ \tau\end{bmatrix} + \begin{bmatrix}J_{c,b}^T \\ J_{c,j}^T\end{bmatrix}\lambda\]

这正是 M v_dot + C v + g = S^T tau + J_c^T lambda。

关键微妙之处：C 矩阵的基座-基座块 \(C_{bb}\) 包含两个来源——(1) 标准的 Christoffel 符号项（来自 \(\dot M_{bb}\) 对 \(q_j\) 的依赖），(2) Euler-Poincare 的 \(\mathrm{ad}^*_{\xi_b}\) 项（来自 body frame 的旋转效应）。在 Pinocchio 的 RNEA 中，这两个来源被自然合并——因为 RNEA 的递推已经在 body frame 中工作。

反事实推理：如果把基座也当作 6 个独立的广义坐标（3 个欧拉角 + 3 个平移），而不是用 EP 约化，会怎样？(1) 出现万向锁奇异（theta = pi/2 时 M 矩阵奇异）；(2) C 矩阵的 Christoffel 项在不同欧拉角约定下形式完全不同——换一种约定就要重新推导；(3) 数值积分需要处理 "角度 wrap-around"（2pi 周期性）。EP 约化彻底回避了这些问题。

4.7.6 nq != nv 的深层原因与工程影响

为什么维度不匹配：SO(3) 是一个 3 维流形，但最常用的无奇异表示（单位四元数）需要 4 个参数加一个约束 |q| = 1。这意味着：

• 参数化维度（nq 的 SO(3) 部分）= 4
• 切空间维度（nv 的 SO(3) 部分）= 3
• 差异 = 1（正好是约束的个数）

推广到 SE(3)：参数化 = 3（position）+ 4（quaternion）= 7，切空间 = 3 + 3 = 6。

工程影响：

1. 不能用 q_dot：q_dot in R^7 不是切空间的元素（四元数的 q_dot 还包括约束方向的分量）。必须用 v in R^6。
2. Jacobian 的维度：各种 Jacobian（运动学、接触等）都是 ? x nv 而非 ? x nq。
3. 积分必须用 Lie 群：q_{k+1} = integrate(model, q_k, v*dt)，内部对 SO(3) 部分用 exp 映射。
4. 配置空间距离：q_1 - q_2 在 R^7 中没有物理意义（两个四元数的 Euclidean 差不等于旋转差）。应用 pinocchio::difference(model, q1, q2) 返回 R^{nv} 的切向量。

# nq != nv 的具体演示
import pinocchio as pin

model = pin.buildSampleModelHumanoidRandom()
print(f"nq = {model.nq}, nv = {model.nv}")
# 典型输出：nq = 35, nv = 34（差 1 = 四元数约束）

# 正确计算两个 configuration 之间的"差"
q1 = pin.randomConfiguration(model)
q2 = pin.randomConfiguration(model)
dq = pin.difference(model, q1, q2)  # R^{nv}，不是 R^{nq}！
print(f"dq shape: {dq.shape}")  # (34,) 不是 (35,)

# 正确积分
v = np.random.randn(model.nv) * 0.01
q_next = pin.integrate(model, q1, v)
# 验证四元数仍是单位的
quat = q_next[3:7]
print(f"|quaternion| = {np.linalg.norm(quat):.15f}")  # 应精确为 1.0

4.7.7 典型例题：四足浮基动力学

以 Solo12（12 个关节的四足）为例：

• nq = 7 + 12 = 19（基座四元数+位移+12 关节角）
• nv = 6 + 12 = 18（基座 6D velocity + 12 关节速度）
• M in R^{18 x 18}

当四足站立时，4 条腿各有一个接触点，每个接触点有 3 维反力（或考虑摩擦锥约束后的力）： - J_c in R^{12 x 18}（4 个接触点 x 3D） - lambda in R^{12}

WBC/TSID 的核心 QP 正是对这个方程求解：给定期望加速度约束和力矩限制，找最优的 (v_dot, tau, lambda)。

⚠️ 常见陷阱

⚠️ 编程陷阱：四元数约定不统一

库	四元数顺序
Pinocchio / Eigen	(x, y, z, w)
Drake / MuJoCo / ROS tf	(w, x, y, z)
Ceres	(w, x, y, z)

跨库数据传递时必须显式重排！一个常见的 bug 是：从 MuJoCo 仿真读出四元数，直接丢给 Pinocchio 做控制——结果完全错误但不报错（因为四元数仍然是单位的，只是分量对应错了）。

⚠️ 编程陷阱：MuJoCo 的速度约定与 Pinocchio 不同

MuJoCo 的 qvel[0:3] 是基座**线速度在 global frame**，qvel[3:6] 是**角速度在 body frame**。Pinocchio 的 v[0:6] 全部在 body frame。如果不做转换直接混用，控制器会完全失效。

练习

1. **（编程）**用 Pinocchio 加载 example-robot-data 的 solo12，打印 nq 和 nv，验证 nq = 19, nv = 18。
2. **（手推）**对一个 2-link floating-base robot（基座 + 2 个旋转关节），写出 M 矩阵的分块结构，解释每个块的物理意义。
3. **（跨章综合）**结合 30_ON动力学递推算法 中的 RNEA，解释 Pinocchio 如何在 RNEA 的根节点处理 JointModelFreeFlyer——它多做了什么？少做了什么？

---

4.8 质心动量与 Centroidal Dynamics ⭐⭐

动机

浮基机器人有 18-30 个甚至更多自由度，但其质心的运动只有 6 维（3 维平移 + 3 维旋转的动量）。Centroidal dynamics 把整台复杂的多体系统压缩为一个等效的 6D 动量方程——这正是 humanoid 平衡控制和步态生成的物理基础。

4.8.1 Centroidal Momentum Matrix (CMM)

定义 CMM \(A_G(q) \in \mathbb{R}^{6 \times (6+n)}\)，使得：

\[h := \begin{bmatrix} k \\ l \end{bmatrix} = A_G(q)\,v \in \mathbb{R}^6\]

其中： - \(k \in \mathbb{R}^3\)：系统关于质心的总角动量 - \(l = m\,v_{\text{CoM}} \in \mathbb{R}^3\)：系统的总线动量 - \(v \in \mathbb{R}^{6+n}\)：广义速度

物理含义：CMM 把高维的关节空间速度"投影"到 6D 质心动量空间。每一列 \(A_G[:,j]\) 表示"单位速度 \(v_j\) 对质心动量的贡献"。

4.8.2 Centroidal Dynamics 主方程

对 h 求时间导数（Orin-Goswami-Lee 2013 核心方程）：

\[\dot h = A_G(q)\dot v + \dot A_G(q,v)v = \sum_i \mathcal{F}_{\mathrm{ext},i}\]

其中 \(\mathcal{F}_{\mathrm{ext},i}\) 是第 \(i\) 个外力（以 wrench 形式，作用点的力矩关于 CoM）。

展开为分量：

\[\begin{bmatrix}\dot k \\ \dot l\end{bmatrix} = \begin{bmatrix}\sum_i (p_i - p_{\mathrm{CoM}}) \times f_i + \sum_i \tau_i \\ \sum_i f_i + mg\end{bmatrix}\]

右边就是所有外力对质心产生的力矩和，以及所有外力加重力之和。

本质洞察：Centroidal dynamics 本质上就是"把整台机器人等效为一个质点+陀螺"的 Newton-Euler 方程。线动量变化率 = 合外力（含重力），角动量变化率 = 合力矩。但关键在于：通过 A_G(q)，我们可以精确地从关节空间控制质心动量。

4.8.3 A_G 与关节空间质量矩阵的关系

Wensing-Orin 2016 的重要结果：A_G 和 dot{A}_G 可以直接从关节空间 M(q) 和 C(q,v) 的前 6 行抽取：

\[A_G = \Phi^T M[1:6, :], \quad \dot A_G = \Phi^T C[1:6, :]\]

其中 Phi 是一个与质心位置相关的变换矩阵。这意味着不需要专门的算法计算 CMM——RNEA/CRBA 的中间结果已经包含了它。

Pinocchio 的实现对应：

// 计算 CMM + 质心动量 + centroidal composite rigid-body inertia
pinocchio::ccrba(model, data, q, v);
// data.Ag: CMM (6 x nv)
// data.hg: centroidal momentum (Force 类型)
// data.Ig: centroidal composite rigid-body inertia

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

// 验证：h = A_G * v
Force h_check = data.Ag * v;  // 应该等于 data.hg

// 验证：h_dot = A_G * a + dA_G * v = sum(F_ext)
Force hdot = data.Ag * a + data.dAg * v;

约定注记：注意 data.hg/data.Ag 按 Pinocchio 约定为 (linear, angular)，即前 3 行 = 线动量、后 3 行 = 角动量，与本节数学式 \(h=[k;l]\)（角在前）相反，跨用需重排。

4.8.4 在 WBC/TSID 中的应用

TSID（stack-of-tasks/tsid）中的 TaskAM（Angular Momentum Task）把质心动量作为 QP 等式约束：

\[A_G\,\dot v = \dot h_{\mathrm{desired}} - \dot A_G\,v\]

这确保多体系统跟踪期望的质心动量轨迹——对 humanoid 平衡控制至关重要。

与控制方向的交叉引用：在 05_运动控制/90_DDP 中，Crocoddyl 的 CostModelCentroidalMomentum 直接使用 A_G 和 data.hg 来惩罚质心动量偏差。在 05_运动控制/120_MPC 中，OCS2 的 centroidal model 把 h_dot = sum F_ext 作为简化动力学约束（相比完整的关节空间动力学，维度从 6+n 降到 6）。

4.8.5 Centroidal momentum frame 的注意事项

Pinocchio 的 data.hg 定义在一个**平移到 CoM 但方向对齐世界系**的 frame 中： - 不是 body frame（方向不随基座旋转） - 不是纯世界系（原点在 CoM 而非世界原点）

这个约定与 Orin-Goswami-Lee 2013 的原始定义一致。如果需要在其他 frame 中使用，必须用 Ad* 进行变换。

练习

1. **（编程）**用 Pinocchio 加载 talos，对随机 (q, v)：(a) 计算 ccrba 得到 h；(b) 施加一个已知外力到右手末端；(c) 前向仿真 1 步，验证 h_dot approx sum F_ext（数值误差 < 1e-4）。
2. **（概念）**解释为什么 humanoid 的质心加速度满足 m * a_CoM = sum f_ext + mg，但基座角加速度不满足 I_base * alpha_base = sum tau_ext。耦合项来自哪里？

---

4.9 Featherstone 空间向量与 Lie 群的精确对应 ⭐⭐

动机

学到这里，你可能有一个疑问：Featherstone 的空间向量代数（写在 1987 年的博士论文中，远早于机器人学界广泛使用 Lie 群语言）和本章的 Lie 群几何到底是什么关系？答案是：它们是同一个几何对象的两种记号。

4.9.1 完整对照表

Featherstone 概念	记号	Lie 群几何对应	记号	维度
spatial velocity (motion)	v_hat in M^6	body velocity	xi = g^{-1}g_dot in se(3)	6
spatial force (wrench)	f_hat in F^6	coadjoint momentum	mu in se(3)*	6
Plucker transform	X in GL(6)	Adjoint representation	Ad_T	6x6
dual Plucker	X* = X^{-T}	Coadjoint	Ad*_{T^{-1}}	6x6
motion cross	v x_M	adjoint action	ad_xi	6x6
force cross	v x*_F	coadjoint action	ad*_xi = -ad_xi^T	6x6
spatial inertia	I in R^{6x6}	body inertia	G_b: se(3) -> se(3)*	6x6
RNEA backward: f = Ia + v x* (Iv)	—	EP on SE(3)	G_b xi_dot = ad*_xi (G_b xi) + tau	—
motion frame change	v' = X v	change of body frame	xi' = Ad_T xi	—
force frame change	f' = X* f	coadjoint change	mu' = Ad*_{T^{-1}} mu	—

4.9.2 为什么 Featherstone 不用 Lie 群语言却算法等价

因为他的 6x6 矩阵乘法已经**显式编码了 ad 和 Ad 的所有几何内容**：

• Motion cross 的 4 个 3x3 块就是 ad_xi 的分块形式
• Plucker transform 的 4 个 3x3 块就是 Ad_T 的分块形式
• 力的传递用 X* = X^{-T}，对应 Ad*_{T^{-1}}

几何学家用抽象符号 (ad, Ad, ad*)，工程师用矩阵 (x, X, X*)。几何内容毫无差别——就像用极坐标和笛卡尔坐标描述同一条曲线。

4.9.3 Pinocchio 源码中的对应

理解了这个对照表，Pinocchio 源码变得透明：

// Motion::cross(const Motion& v2) → ad_{this} v2
// 实现：return Motion(w.cross(v2.w), w.cross(v2.v) + v.cross(v2.w))
// 这正是 se(3) Lie bracket 的分量形式！

// Motion::cross(const Force& f) → ad*_{this} f
// 实现：return Force(w.cross(f.angular()) + v.cross(f.linear()),
//                    w.cross(f.linear()))
// 这正是 coadjoint action 的分量形式！

// SE3::act(const Motion& m) → Ad_T m
// SE3::actInv(const Motion& m) → Ad_{T^{-1}} m

跨领域类比：Featherstone vs Lie 群几何学家的关系，类似于电气工程师 vs 物理学家。电气工程师用阻抗 Z = R + jX 解电路，物理学家用麦克斯韦方程。两者计算同一个物理现象，结果完全一致——只是抽象层次和通用性不同。Lie 群语言的优势在于统一性和可推广性（从 SO(3) 到 SE(3) 到 SE_2(3) 到任意 Lie 群），Featherstone 的优势在于计算效率和实现直觉。

4.9.4 与 Featherstone 的 "Spatial Velocity" 约定差异

重要注意：Featherstone 原始定义中 spatial velocity 的排列是 (omega, v)（angular first），这与 se(3) 的标准 hat 映射一致。但不同文献/库可能有不同排列：

来源	排列
Featherstone RBDA	(omega, v) — angular first
Pinocchio 内部	(angular, linear) 或 (linear, angular)，取决于版本
Lynch-Park	(omega, v) — angular first
Murray-Li-Sastry	(v, omega) — linear first
Siciliano et al.	(v, omega) — linear first

**Pinocchio 的 Motion 类**内部存储为 .linear() 和 .angular() 两个 R^3 向量，对外 API 保证一致性。但在手推公式和看其他代码时，务必确认排列约定。

练习

1. **（手推验证）**写出 se(3) 上 ad_xi 的 6x6 矩阵（xi = (omega; v)），验证 ad_xi eta = [xi, eta]_se(3)。
2. **（源码阅读）**打开 Pinocchio 的 motion.hpp，找到 cross(Motion) 和 cross(Force) 的实现，与上述公式对比。
3. **（概念）**解释为什么 Plucker transform 不是正交矩阵（X^T X != I），但 Ad_T 保持 Killing form（在 se(3) 上的特定二次形式）。

---

4.10 Lie-Poisson 结构与余伴随轨道 ⭐⭐⭐

动机

到目前为止，我们一直在 Lagrangian（速度-位形）框架中工作。但力学还有一个 Hamiltonian（动量-位形）框架，它在定性分析（守恒量、稳定性、可积性）方面更加强大。当把 Hamiltonian 力学也"约化"到 Lie 群上时，我们得到**Lie-Poisson 方程**——它住在余李代数 g* 上，带有一个天然的（但秩亏的）Poisson 结构。

4.10.1 g* 上的 Lie-Poisson 括号

在余李代数 g* 上定义**Lie-Poisson 括号**（Marsden-Ratiu Ch.13）：

\[\{F, G\}_\pm(\mu) = \pm \langle \mu, \left[\frac{\delta F}{\delta\mu},\ \frac{\delta G}{\delta\mu}\right] \rangle, \quad \mu \in \mathfrak{g}^*\]

其中 \(\delta F/\delta\mu \in \mathfrak{g}\) 是函数 \(F: \mathfrak{g}^* \to \mathbb{R}\) 的"功能导数"（functional derivative），定义为 \(dF(\mu) \cdot \delta\mu = \langle\delta\mu,\, \delta F/\delta\mu\rangle\)。

"+" 和 "-" 分别对应右不变约化和左不变约化。大多数机器人学文献用左约化（body frame），对应"-"号。

4.10.2 Lie-Poisson 方程

对任意 Hamiltonian \(H: \mathfrak{g}^* \to \mathbb{R}\)，动力学由 Lie-Poisson 方程给出：

\[\dot\mu = \mathrm{ad}^*_{\delta H/\delta\mu}\,\mu\]

（使用"-"号 Poisson 括号时）。

与 Euler-Poincare 的关系：通过 Legendre 变换 \(H(\mu) = \langle\mu, \xi\rangle - \ell(\xi)\)（其中 \(\mu = \partial\ell/\partial\xi\)），Lie-Poisson 方程等价于 Euler-Poincare 方程。两者是同一个动力学的 Lagrangian 和 Hamiltonian 两面。

4.10.3 SO(3) 上的 Lie-Poisson

取 \(\mathfrak{g}^* = \mathfrak{so}(3)^* \cong \mathbb{R}^3\)（通过 Killing form 认同），\(\mu = \pi\)（body angular momentum）。

Lie-Poisson 括号： $\(\{F, G\}(\pi) = -\pi \cdot (\nabla F \times \nabla G)\)$

Hamiltonian：\(H(\pi) = \frac{1}{2}\pi^T I^{-1}\pi\)（动能用动量表达）。

计算 \(\delta H/\delta\pi = I^{-1}\pi = \omega\)，代入：

\[\dot\pi = \mathrm{ad}^*_\omega\,\pi = \pi \times \omega\]

这又是 Euler 方程！但现在是在**纯动量空间** R^3 中表达——不需要显式的位形变量 R。

4.10.4 Casimir 函数

定义：函数 \(C: \mathfrak{g}^* \to \mathbb{R}\) 称为 Casimir，如果对所有 \(F\) 有 \(\{C, F\} = 0\)。

物理意义：Casimir 是**对任何 Hamiltonian 都守恒**的量——它是 Poisson 结构本身的约束，不依赖于具体的 \(H\)。

SO(3) 的 Casimir：\(C(\pi) = |\pi|^2\)（角动量模长平方）。

验证：\(\{C, F\} = -\pi \cdot (2\pi \times \nabla F) = -2(\pi \times \pi) \cdot \nabla F = 0\)（因为 \(\pi \times \pi = 0\)）。

物理含义：无论刚体的惯量张量 \(I\) 如何（即无论 \(H\) 是什么），\(|\pi|^2\) 总是守恒。这反映了 \(SO(3)\) 的一个深刻的几何事实。

4.10.5 余伴随轨道作为辛叶

Lie-Poisson 流形 (g*, {,}) 是 Poisson 而非辛——因为 Poisson 张量的秩不满（存在 Casimir 方向）。

辛叶定理（Kirillov-Kostant-Souriau）：g* 的 symplectic leaves 恰好是 coadjoint orbits：

\[\mathcal{O}_\mu = \{\mathrm{Ad}^*_g\,\mu : g \in G\} \subset \mathfrak{g}^*\]

SO(3) 具体化：\(\mathrm{Ad}^*\) 就是 \(SO(3)\) 对 \(\mathbb{R}^3\) 的标准旋转作用，其轨道是**同心球面** \(S^2_r = \{\pi \in \mathbb{R}^3 : |\pi| = r\}\)。

• Casimir \(C = |\pi|^2\) 在每个轨道上为常数（因为 \(r\) 是轨道的半径）
• Hamiltonian \(H = \frac{1}{2}\pi^T I^{-1}\pi\) 的等值面（能量椭球）与球面 \(S^2_r\) 的交线就是 Euler 刚体的**轨迹曲线**

这就是 **Poinsot 构造**的现代几何解释！

本质洞察：Euler 刚体的运动被两个守恒量（|pi|^2 = const 和 H = const）约束在球面上的一条曲线——这条曲线是能量椭球与角动量球面的交线。从 Lie-Poisson 视角看，这不是巧合：Casimir 定义了辛叶（球面），Hamiltonian 在辛叶上给出运动。2D 辛流形上的 1D 等能面就是轨迹。

4.10.6 SE(3) 的 Lie-Poisson 与 Casimir

\(SE(3)\) 的余伴随轨道结构更复杂。设 \(\mu = (m, f) \in \mathfrak{se}(3)^*\)（angular momentum, linear momentum），两个独立的 Casimir：

\[C_1(\mu) = m \cdot f = \|m\| \|f\| \cos\theta, \quad C_2(\mu) = |f|^2\]

其中 \(C_1\) 是"螺旋参数"（angular 和 linear momentum 的投影），\(C_2\) 是线动量模长平方。余伴随轨道因此是 4 维的（6 维 \(\mathfrak{se}(3)^*\) 减去 2 个 Casimir 方向）。

练习

1. **（手推）**验证 SO(3) 上 {F, G}(pi) = -pi . (nabla F x nabla G) 满足 Jacobi 恒等式 {{F,G},H} + cyclic = 0。
2. **（概念）**解释 Casimir 函数的"几何含义"——为什么它对任何 H 都守恒？（提示：它的梯度与 Poisson 张量的核有什么关系？）
3. **（进阶）**对 SE(3)，验证 C_1 = m . f 确实是 Casimir：计算 {C_1, G} 对任意 G，验证为零。

---

4.11 SE_2(3) 扩展位姿群与 IMU 预积分 ⭐⭐⭐

动机

在视觉-惯性导航（VIO）和足式机器人状态估计中，IMU 预积分是核心技术。Barrau-Bonnabel 2017 的 Invariant EKF (InEKF) 表明：如果把状态空间扩展为**SE_2(3)**（包含旋转、速度、位移的"扩展位姿群"），IMU 的运动学方程具有完美的 group-affine 结构——这使得估计误差动力学自主（不依赖于状态本身），从而保证滤波器的一致性。

4.11.1 SE_2(3) 的定义

SE_2(3)（也记为 SE_2(3) 或 extended special Euclidean group）是 5x5 矩阵群：

\[T = \begin{bmatrix} R & v & p \\ 0_{1\times 3} & 1 & 0 \\ 0_{1\times 3} & 0 & 1 \end{bmatrix} \in \mathbb{R}^{5\times 5}\]

其中 \(R \in SO(3)\) 是旋转，\(v \in \mathbb{R}^3\) 是速度，\(p \in \mathbb{R}^3\) 是位移。

群乘法：

\[T_1 T_2 = \begin{bmatrix} R_1 R_2 & R_1 v_2 + v_1 & R_1 p_2 + p_1 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix}\]

逆元：

\[T^{-1} = \begin{bmatrix} R^T & -R^T v & -R^T p \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix}\]

4.11.2 为什么需要 SE_2(3)

标准 IMU 运动学（world frame）：

\[\dot R = R\,[\omega_m - b_g]_\times, \quad \dot v = R(a_m - b_a) + g, \quad \dot p = v\]

其中 \(\omega_m, a_m\) 是 IMU 测量，\(b_g, b_a\) 是陀螺/加速度计偏置，\(g\) 是重力。

关键观察：如果把 \(X = (R, v, p)\) 视为 \(SE_2(3)\) 的群元，则上述方程可以写成**左不变形式**：

\[\dot X = X\,\mathcal{U} + W\,X\]

其中 \(\mathcal{U}\) 和 \(W\) 只依赖于测量和已知量（重力），不依赖于 \(X\) 本身。这就是 Barrau-Bonnabel 的 group-affine 结构。

4.11.3 与 InEKF 的连接

当动力学是 group-affine 时，左不变误差 \(e = X_{\text{true}}^{-1} X_{\text{est}}\)（或右不变误差 \(e = X_{\text{est}} X_{\text{true}}^{-1}\)）的动力学方程**不依赖于真实状态** \(X_{\text{true}}\)。这意味着：

1. 误差动力学的线性化是**精确的**（不是近似！）
2. EKF 的一致性得到保证（协方差不会低估）
3. 收敛性可以严格证明

这就是 InEKF 相比标准 EKF 的核心优势——不是"更好的线性化"，而是"线性化本身就是精确的"。

4.11.4 Hartley et al. 2020 的 Contact-Aided InEKF

Hartley-Ghaffari-Eustice-Grizzle 2020 (IJRR) 将 InEKF 应用于足式机器人：把接触脚的位置也纳入 SE_2(3) 的扩展状态（每个接触脚多加一个"p_foot"列），形成 SE_{2+k}(3)。

这直接建立在本章的 SE(3) 几何力学基础之上——process model 需要浮基动力学（本章 4.7 节），观测模型需要 SE(3) 上的 Lie 群导数。

4.11.5 se_2(3) 李代数

se_2(3) 的李代数元素：

\[\xi = \begin{bmatrix} [\omega]_\times & a & v \\ 0 & 0 & 0 \\ 0 & 0 & 0 \end{bmatrix}, \quad \xi^\vee = (\omega, a, v) \in \mathbb{R}^9\]

其中 omega 是角速度，a 是线加速度，v 是线速度。指数映射：

\[\exp(\xi) = \begin{bmatrix} \exp([\omega]_\times) & J(\omega)\,a & J(\omega)\,v \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix}\]

其中 J(omega) 是 SO(3) 的左 Jacobian。

⚠️ 常见陷阱

⚠️ 概念误区：\(SE_2(3)\) 和 \(SE(3) \times \mathbb{R}^3\) 是同一个东西

新手想法：\(SE_2(3)\) 就是把 \(SE(3)\) 加一个 \(\mathbb{R}^3\) 速度分量，和直积 \(SE(3) \times \mathbb{R}^3\) 没区别。

实际上：虽然作为集合它们同构（都是 \(R, v, p\) 的三元组），但**群结构不同**！\(SE_2(3)\) 的乘法是 \(v_1 + R_1 v_2\)，而直积的乘法是 \(v_1 + v_2\)。这个差异导致 adjoint representation 完全不同，进而影响 InEKF 的误差动力学是否自主。

练习

1. **（手推）**验证 \(SE_2(3)\) 的群乘法满足结合律 \((T_1 T_2) T_3 = T_1 (T_2 T_3)\)。
2. **（概念）**解释为什么标准 EKF（在 \(\mathbb{R}^9\) 上做 ESKF）对 IMU 积分不一致，但 InEKF（在 \(SE_2(3)\) 上）一致。关键差异在哪一步？
3. **（进阶）**写出 \(SE_2(3)\) 的 \(9\times9\) Adjoint matrix \(\mathrm{Ad}_T\) 的显式形式。

---

4.12 典型例题 ⭐⭐

4.12.0 EP 方程在不同 Lie 群上的统一总结

在进入具体例题之前，让我们用一张表格总结 Euler-Poincare 方程在不同 Lie 群上的退化形式。这彰显了 EP 方程作为"元方程"的统一力量。

Lie 群 G	李代数 g	约化 Lagrangian l(xi)	EP 方程具体形式	物理系统
SO(3)	R^3, [,] = x	1/2 omega^T I omega	I omega_dot + omega x I omega = tau	刚体旋转
SE(3)	R^6, motion cross	1/2 xi^T G_b xi	G_b xi_dot = ad*_xi(G_b xi) + F	单刚体 6D
SE(3) x T^n	R^{6+n}	T(xi_b, q_dot_j)	M v_dot + Cv + g = S^T tau + J_c^T lambda	浮基多体
Diff(M)_vol	散度为零的矢量场	1/2 integral	v	^2 dV
SO(3) x R^3 (半直积)	R^6	T(omega) - V(Gamma)	I omega_dot + omega x I omega = mgl Gamma x e_3	Heavy top
SE_2(3)	R^9	IMU kinematic energy	结构化 IMU 方程	IMU 预积分

统一结构：在所有情况下，EP 方程的形式都是：

\[\frac{d\mu}{dt} = \mathrm{ad}^*_\xi\,\mu + \text{external force}\]

变化的只是：g 的维度、ad* 的具体矩阵形式、以及 l(xi) 的具体表达式。学会一个，学会所有——这就是几何力学的威力。

跨领域类比：这就像学了矩阵乘法 C = AB 之后，发现标量乘法 c = ab、向量内积 c = a . b、矩阵-向量乘 c = Ax 都是它的特例。EP 方程就是 Lie 群力学的"矩阵乘法"——一个足够抽象的框架，包含了所有具体情形。

4.12.1 例题一：自由浮体卫星

一颗卫星在无重力环境中自由旋转（无外力矩），其 body frame 转动惯量为 I = diag(A, B, C)，A < B < C。

问题：绕中间轴（B 轴）旋转的稳定性如何？

解：由 Euler 方程（tau = 0）： - A omega_dot_1 = (B - C) omega_2 omega_3 - B omega_dot_2 = (C - A) omega_3 omega_1 - C omega_dot_3 = (A - B) omega_1 omega_2

对绕 B 轴的平衡点 omega = (0, Omega, 0) 做线性化，设小扰动 omega = (epsilon_1, Omega + epsilon_2, epsilon_3)：

• A dot{epsilon}_1 = (B - C) Omega epsilon_3
• C dot{epsilon}_3 = (A - B) Omega epsilon_1

联立：A C epsilon_ddot_1 = (B-C)(A-B) Omega^2 epsilon_1

由于 A < B < C：(B-C) < 0，(A-B) < 0，乘积 > 0。因此 epsilon_1 满足**指数增长**——中间轴旋转不稳定！

这就是著名的**中间轴定理**（Tennis Racket Theorem / Dzhanibekov Effect），在 Lie-Poisson 视角下，对应能量椭球与角动量球面在中间轴附近交出的是**鞍点**（而非极值点）。

4.12.2 例题二：四足浮基动力学验证

问题：用 Pinocchio 数值验证浮基 Solo12 的 centroidal dynamics h_dot = sum F_ext。

import pinocchio as pin
import numpy as np
from example_robot_data import load

# 加载 Solo12 四足模型（浮基）
robot = load("solo12")
model, data = robot.model, robot.data

# 随机状态
q = pin.randomConfiguration(model)
v = np.random.randn(model.nv) * 0.1  # 小速度避免数值问题
a = np.zeros(model.nv)  # 零加速度（纯自由落体）

# 计算 centroidal momentum
pin.ccrba(model, data, q, v)
h0 = data.hg.copy()  # 初始质心动量 (Force 类型)

# 施加重力：使用 RNEA 计算重力项
tau_gravity = pin.rnea(model, data, q, np.zeros(model.nv), np.zeros(model.nv))
# tau_gravity 是抵消重力所需的力矩（负重力效应）

# 前向仿真一步（dt = 1e-4）
dt = 1e-4
# 在无外力、无关节力矩下，ABA 给出加速度（只有重力）
a_free = pin.aba(model, data, q, v, np.zeros(model.nv))
v_next = v + a_free * dt
q_next = pin.integrate(model, q, v_next * dt)

# 计算下一步的质心动量
pin.ccrba(model, data, q_next, v_next)
h1 = data.hg.copy()

# 数值 h_dot
hdot_numerical = (h1 - h0) / dt  # 注意 Force 类型的减法

# 理论 h_dot：只有重力
total_mass = sum([model.inertias[i].mass for i in range(model.njoints)])
hdot_theory_linear = total_mass * np.array([0, 0, -9.81])  # mg
hdot_theory_angular = np.zeros(3)  # 自由落体无外力矩

print(f"数值 h_dot (linear): {hdot_numerical.linear}")
print(f"理论 h_dot (linear): {hdot_theory_linear}")
print(f"相对误差: {np.linalg.norm(hdot_numerical.linear - hdot_theory_linear) / np.linalg.norm(hdot_theory_linear):.2e}")

4.12.3 例题三：无人机 SE(3) 几何控制验证

问题：实现 Lee-Leok-McClamroch 2010 的 SE(3) 几何控制器，控制一个 quadrotor 从任意初始姿态恢复到悬停。

控制器设计：

位置环（外环）： $\(f = -k_x(x - x_d) - k_v(\dot x - \dot x_d) + mg\,e_3 + m\ddot x_d\)$

其中 f 是期望推力方向和大小。

姿态环（内环，直接在 SO(3) 上）： $\(e_R = \frac{1}{2}(R_d^T R - R^T R_d)^\vee \quad \text{(旋转误差向量)}\)$ $\(e_\Omega = \omega - R^T R_d\,\omega_d \quad \text{(角速度误差)}\)$ $\(M = -k_R\,e_R - k_\Omega\,e_\Omega + \omega \times (I\omega) - I(\hat\omega\,R^T R_d\,\omega_d - R^T R_d\,\dot\omega_d)\)$

关键优势： - 无万向锁：e_R 直接从旋转矩阵差异导出 - 几乎全局稳定：除了 R = -R_d（对径点）处的不稳定平衡 - 大姿态机动：即使 quadrotor 翻转 180 度也能恢复

import numpy as np
from scipy.spatial.transform import Rotation

def geometric_attitude_controller(R, omega, R_d, omega_d, domega_d, I, kR, kOmega):
    """
    Lee-Leok-McClamroch 2010 SE(3) geometric controller (attitude part)

    Args:
        R: 当前旋转矩阵 (3x3)
        omega: 当前 body angular velocity (3,)
        R_d: 期望旋转矩阵 (3x3)
        omega_d: 期望 angular velocity (3,)
        domega_d: 期望 angular acceleration (3,)
        I: 惯量张量 (3x3)
        kR, kOmega: 增益

    Returns:
        M: body frame 控制力矩 (3,)
    """
    # 旋转误差（vee map of skew-symmetric part）
    eR_hat = 0.5 * (R_d.T @ R - R.T @ R_d)  # 反对称矩阵
    eR = np.array([eR_hat[2,1], eR_hat[0,2], eR_hat[1,0]])  # vee map

    # 角速度误差
    eOmega = omega - R.T @ R_d @ omega_d

    # 控制力矩
    omega_hat = np.array([[0, -omega[2], omega[1]],
                          [omega[2], 0, -omega[0]],
                          [-omega[1], omega[0], 0]])
    feedforward = omega_hat @ (I @ omega)  # 陀螺补偿
    desired_term = I @ (omega_hat @ R.T @ R_d @ omega_d - R.T @ R_d @ domega_d)

    M = -kR * eR - kOmega * eOmega + feedforward - desired_term
    return M

数值验证：从 R_0 = R_z(pi)（翻转 180 度）出发，几何控制器能在 ~3 秒内恢复正常悬停。而 Euler 角 PID 在这个初始条件下会因为万向锁而失控。

4.12.4 例题四：水下机器人的 SE(3) 动力学

问题：水下机器人（ROV）的动力学方程在 body frame 中怎么写？

水下机器人的 SE(3) 动力学（Fossen 2011）：

\[M_{\text{total}}\,\dot\xi + C(\xi)\,\xi + D(\xi)\,\xi + g(\eta) = \tau\]

其中： - \(M_{\text{total}} = M_{\text{rigid}} + M_{\text{added}}\)：包含附加质量效应 - \(C(\xi)\)：Coriolis + 离心力（含附加质量贡献） - \(D(\xi)\)：水动力阻尼（线性 + 二次） - \(g(\eta)\)：浮力/重力恢复力

与自由空间的区别： 1. 附加质量 \(M_{\text{added}}\) 使得等效惯性变为非对角（体现了水与刚体的耦合） 2. 阻尼项 \(D(\xi)\) 中的二次阻力 \(D_q |\xi| \xi\) 使方程非线性 3. 浮力-重力力矩依赖于姿态 R（通过 g(eta) = buoyancy - gravity 在 body frame 中的表达）

这正是 SE(3) 上 Euler-Poincare 方程加入耗散和外力后的完整形式。Fossen 教材用的记号与 Featherstone 不同（Fossen 把线速度放前面），但几何内容一致。

4.12.5 例题五：四足浮基动力学中的基座-关节耦合

问题：ANYmal 在站立时抬起前右腿，基座会怎么运动？

分析：抬腿时关节加速度 q_ddot_FR != 0。通过 M 矩阵的基座-关节耦合块 M_{bj}：

\[M_{bb}\dot\xi_b + M_{bj}\ddot q_j = -C_b(q,v)v - g_b(q) + J_{c,b}^T\lambda\]

即使没有外力矩，关节运动通过 M_{bj} q_ddot_j 项"反冲"到基座。这就是为什么 humanoid 挥臂会影响躯干平衡——也是 CMM 任务在 WBC 中如此重要的原因。

工程对策： 1. 在 WBC/TSID 中加入 centroidal momentum task，约束 h_dot 在允许范围内 2. 规划关节轨迹时考虑 CMM 约束（离线轨迹优化用 Crocoddyl 的 CostModelCentroidalMomentum） 3. 用 whole-body MPC 联合优化所有关节+接触力（OCS2 路线）

---

4.13 动量映射 J: T*G -> g* 与 Noether 定理 ⭐⭐⭐

动机

Noether 定理是理论物理最深刻的定理之一：每一个连续对称性对应一个守恒量。在 Lie 群力学中，它有一个极其优雅的几何表述——通过**动量映射**（momentum map）。

4.13.1 动量映射的定义

设 G 作用在辛流形 (P, omega_symplectic) 上，生成无穷小作用 xi_P（对任意 xi in g）。动量映射 J: P -> g* 满足：

\[d\hat{J}(\xi) = \iota_{\xi_P}\,\omega_{\mathrm{symplectic}}\]

其中 \(\hat{J}(\xi)(z) := \langle J(z), \xi \rangle\) 是 J 在 xi 方向的分量。

直觉：J 把辛流形上的点映射到"动量空间" g*，使得 G 的每个生成元 xi 对应一个函数 J_hat(xi)，它的 Hamilton 流恰好是 xi 生成的对称变换。

4.13.2 Cotangent lift 的动量映射

对 G 作用在自身上（左平移）的 cotangent lift T*L_g: T*G -> T*G：

\[J(g, p) = T^*_e L_g \cdot p \in \mathfrak{g}^*\]

这就是**body momentum**——把余切向量 p 从 g 处"左平凡化"到 e 处。

4.13.3 Noether 定理（几何版）

定理（Marsden-Ratiu Th. 11.4.1）：若 Hamiltonian H 是 G-不变的（\(H(\Phi_g(z)) = H(z)\) for all g in G），则 J 沿 Hamilton 流守恒：

\[\frac{d}{dt}J(\phi_t(z)) = 0\]

SO(3) 特例： - G = SO(3)，作用在 T*SO(3) 上 - J = spatial angular momentum pi_s = R I omega - H 对 SO(3) 不变 <=> 势能 V = 0（自由刚体） - Noether: pi_s 守恒 <=> 自由刚体的空间角动量守恒

SE(3) 特例： - G = SE(3)，作用在 T*SE(3) 上 - J = spatial momentum (m_s, f_s) = 6 维 - H 对 SE(3) 不变 <=> 自由刚体在无重力环境 - Noether: 6 个空间动量分量全部守恒

4.13.4 对称性破缺与守恒量丧失

当 H 不是完全 G-不变时，部分守恒量消失：

对称性	不变群	守恒量	维数
完全自由（无重力）	SE(3)	6 个空间动量	6
有重力（竖直方向）	SE(2) x R	z 角动量 + 水平线动量	3
有重力 + 平面约束	SO(2)	z 角动量	1

本质洞察：Noether 定理告诉我们，守恒量不是偶然的——它们是对称性的**必然**后果。理解这一点，可以帮助你在设计控制器时预判哪些量是守恒的（不需要主动控制）、哪些不是（需要驱动器输出）。

练习

1. **（手推）**对 SO(3) 自由刚体，从 Noether 定理出发证明 pi_s = R I omega 守恒。然后直接对 pi_s 求时间导数验证 (d/dt)(R I omega) = 0。
2. **（概念）**解释为什么有重力的四足站立时，z 方向角动量仍然守恒（如果没有外部水平力矩），但 x/y 方向不守恒。

---

4.15 Lie 群 MPC 与 Crocoddyl 的连接 ⭐⭐⭐

动机

本章的几何力学理论不只是理论优美——它直接决定了 Crocoddyl 和 OCS2 等现代 MPC 框架的状态空间设计。理解这个连接，是从"学了几何力学"到"用于机器人控制"的关键桥梁。

4.15.1 为什么 MPC 需要 Lie 群状态空间

标准 MPC 假设状态空间是 R^n：x_{k+1} = f(x_k, u_k)。但浮基机器人的状态包含旋转——而 SO(3) 不是 R^3。

如果强行用 R^n： - 方案 1（Euler 角）：万向锁 → MPC 在 pitch=90 度附近发散 - 方案 2（四元数 + Euclidean 差）：||q1 - q2|| 不等于旋转距离（对径四元数表示同一旋转但 Euclidean 距离 = 2） - 方案 3（rotation vector）：在 pi 弧度附近不唯一

Lie 群 MPC 的正确做法： - 状态空间是 SE(3) x R^n（Lie 群 x Euclidean） - 状态差异用 difference(x1, x2) = log(x1^{-1} x2)（切空间表达） - 积分用 integrate(x, delta) = x . exp(delta)（Lie 群指数） - Cost function 用 Lie 群上的度量（如 tr(I - R_d^T R)）

4.15.2 Crocoddyl 的 StateMultibody

import crocoddyl

# 创建 Lie 群状态空间
state = crocoddyl.StateMultibody(model)
# state.nx = 2 * model.nv (位形切空间 + 速度)
# state.ndx = 2 * model.nv (状态差异维度 = nx，因为切空间维度 = nv != nq)

# 状态操作
x0 = state.rand()  # 随机状态（自动满足 SO(3) 约束）
dx = np.random.randn(state.ndx) * 0.01
x1 = state.integrate(x0, dx)  # Lie 群积分
dx_back = state.diff(x0, x1)  # Lie 群差分
# 验证：dx_back approx dx（一阶精确）

4.15.3 Geometric MPC 的前沿

2023-2026 年的前沿工作： - Saccon-Hauser 的 geometric MPC 在 SE(3) 上做轨迹优化 - Teng-Gahlawat-Mueller 的 quadrotor geometric MPC - Crocoddyl 3 的 IntegratedActionModelImplicit + Lie 群变分积分器

这些工作的共同数学基础就是本章的 Euler-Poincare 方程 + SE(3) 几何。

跨领域类比：Lie 群 MPC 之于标准 MPC，就像曲面上的测地线之于平面上的直线。在平面上最短路径是直线——算起来简单。在球面上最短路径是大圆弧——需要微分几何。浮基机器人的"最优控制"也不是在 R^n 上的简单优化，而是在 SE(3) 上的测地运动——本章提供了正确的数学框架。

---

4.16 Kelvin-Noether 定理与 Locomotion 几何相位 ⭐⭐⭐⭐

动机

蛇形机器人通过周期性扭动身体前进，游泳者通过划臂蹬腿移动，猫从空中翻身着地——这些运动有一个共同特征：通过**内部形状变化**产生**外部位移**，而不需要外力推动。这个"净位移"是**几何相位**（geometric phase）——它是纤维丛上的 holonomy，不是坐标奇异的产物，而是一个拓扑/几何不变量。

4.15.1 纤维丛与形状空间

把 locomotion 系统的配置空间分解为： - 形状空间 M（内部关节角度）：如蛇的各节身体的相对角度 - 位姿空间 G（外部位移/旋转）：如蛇的质心位置和朝向 - 总配置空间 Q = M x G（近似；更精确地是 G-principal bundle over M）

关键约束：如果没有外力，系统的动量 J 守恒。这个守恒律在形状空间 M 上定义了一个**连接**（connection）——它告诉你：给定一个形状变化 delta_r（如某个关节转了 10 度），body 需要做多少"补偿运动" delta_g 以保持动量守恒。

4.15.2 几何相位

当机器人做一个**闭合的**形状变化回路（从形状 r_0 出发，回到 r_0），外部位姿一般**不回到原位**。这个净位移就是几何相位：

\[g_{\text{net}} = \text{Holonomy}(\gamma) = \mathcal{P}\exp\left(-\oint_\gamma A(r)\,dr\right)\]

其中 A(r) 是动量守恒定义的连接 1-form，P exp 是路径序指数。

Stokes 定理版本：对小回路，净位移正比于连接的曲率（Berry 曲率）在回路围成面积上的积分。回路面积越大，净位移越大——这就是为什么蛇的扭动幅度越大，前进得越远。

4.15.3 与 Berry 相位、Aharonov-Bohm 效应的同源性

几何相位不只出现在机器人学中。它与量子力学的 Berry 相位、电磁学的 Aharonov-Bohm 效应在数学上**完全同源**——都是纤维丛上连接的 holonomy。

系统	纤维丛	连接	Holonomy
蛇形机器人	SE(2)-bundle over shape space	角动量=0 约束	净位移/旋转
猫翻身	SO(3)-bundle over shape space	角动量=0 约束	净旋转 (360 度翻身)
Berry 相位	U(1)-bundle over parameter space	量子绝热连接	波函数相位
Aharonov-Bohm	U(1)-bundle over R^3	电磁势 A	电子相位

本质洞察：猫从空中翻身不违反角动量守恒——它通过改变身体形状（弯曲脊柱、伸缩四肢），在零角动量约束下，利用几何相位实现了净旋转。这不是"力矩"的结果，而是**拓扑/几何结构**的必然推论。用欧拉角去分析猫翻身会觉得"不可思议"，但用几何相位就是 Stokes 定理的直接应用。

练习

1. **（概念）**解释为什么宇航员在太空中（角动量守恒）可以通过摆动手臂改变身体朝向。这是几何相位的实例吗？
2. **（进阶）**对一个平面蛇形机器人（3 节身体，2 个关节），形状空间是 T^2（两个角度）。画出形状空间中的一个闭合回路（正弦波形），估算对应的净位移方向和大小。

---

4.17 半直积 SE(3) = SO(3) ⋉ R^3 的 Lie-Poisson ⭐⭐⭐⭐

动机

到目前为止，我们假设 Lagrangian 完全左不变。但对有重力的机器人，势能 V(g) = mgh(g) 破坏了 SE(3) 的完全不变性——只保留绕重力轴（z 轴）的 SO(2) 对称性。如何处理这种"部分对称性"？

答案是**半直积**约化理论。

4.13.1 为什么纯 Euler-Poincare 不够

考虑一个有重力的刚体（"heavy top"）。动能 T = 1/2 omega^T I omega 是 SO(3) 左不变的，但势能 V = mgl cos(theta)（theta 是重力方向与对称轴的夹角）不是 SO(3) 不变的——只有绕重力轴 z 的旋转保持 V 不变。

如果强行对完整 SO(3) 做 Euler-Poincare 约化，势能项 V(R) 无法用 body velocity omega 表达——因为 V 依赖于位形 R 本身。

4.13.2 Advected parameter 方法

解决方案：引入一个"被平流的参数"（advected parameter）Gamma = R^T e_3 in S^2，表示 body frame 中看到的重力方向。

则 V = mgl (Gamma . e_3_body)，只依赖于 Gamma（而非完整的 R）。

Gamma 的演化方程：Gamma_dot = -omega x Gamma（即 Gamma 被 body angular velocity "平流"）。

现在整个系统可以用 (omega, Gamma) 来描述——这就是**半直积**约化的结果。

4.13.3 半直积 Lie-Poisson

扩展 Poisson 括号到 (so(3)* x S^2) 上（Marsden-Ratiu-Weinstein 1984）：

\[\{F, G\}(\pi, \Gamma) = -\pi \cdot (\nabla_\pi F \times \nabla_\pi G) - \Gamma \cdot (\nabla_\pi F \times \nabla_\Gamma G - \nabla_\pi G \times \nabla_\Gamma F)\]

对应的运动方程（heavy top）：

\[\dot\pi = \pi \times \omega + mgl\,\Gamma \times e_3, \quad \dot\Gamma = \Gamma \times \omega\]

第一个方程就是带重力力矩的 Euler 方程；第二个是 advected parameter 的运动学。

4.13.4 机器人中的应用

浮基 humanoid + 固定方向的重力正是这种**半直积结构**： - Lie 群是 SE(3)（全 6D 运动） - 重力破坏了 SE(3) 的完全不变性，只保留 SO(2)_z x R^3（绕 z 轴旋转 + 平移） - Advected parameter 是 body frame 中的重力方向 Gamma = R^T g / |g|

这解释了为什么 Pinocchio RNEA 中重力是通过 model.gravity 传入，而不是作为"外力"——它在约化理论中扮演特殊角色。

练习

1. **（手推）**对 heavy top，验证上述半直积 Lie-Poisson 方程确实给出正确的旋转和进动方程。
2. **（概念）**解释为什么陀螺仪的进动可以理解为余伴随轨道上的运动。

---

4.18 Lie 群变分积分器 ⭐⭐⭐⭐

动机

标准 RK4 在 SO(3) 上**不保持正交性**——长时仿真后 R^T R 会漂移出 I。每步做 SVD 投影回 SO(3) 虽然可行，但破坏了数值精度。**Lie 群变分积分器**通过离散化 Hamilton 原理本身（而非微分方程），天然保持 Lie 群结构。

4.14.1 核心思想

离散 Euler-Poincare (DEP) 的核心更新：

\[g_{k+1} = g_k \cdot \exp(h\,\xi_k)\]

其中 h 是时间步长，xi_k 是离散的 body velocity。配合 xi_k 的离散 Legendre 变换（对 free rigid body 有闭式解），得到一个**精确保持 SO(3) 结构**的积分器。

4.14.2 性能优势

积分器	能量漂移	结构保持	动量守恒
RK4（不投影）	O(t)，最终发散	否	否
RK4 + SVD 投影	O(t)（线性漂移）	是（每步投影）	近似
DEP / Lie group variational	O(1)（有界振荡）	精确	精确

对 1000 秒仿真：RK4 的能量漂移可能达到 1%，DEP 的漂移保持在 10^{-10} 量级。

4.14.3 与 Crocoddyl 的连接

Crocoddyl 的 IntegratedActionModelEuler + StateMultibody 实际上就是一个 Lie 群积分器：它对关节空间用 Euler/RK4，但对基座的 SE(3) 部分使用 exp 映射积分：

// Crocoddyl 内部（简化版）
q_next_base = q_base * exp(v_base * dt);  // SE(3) 指数映射
q_next_joints = q_joints + v_joints * dt;  // R^n 直接加

这保证了浮基不会积分到 SE(3) 外面。

练习

1. **（编程）**实现 SO(3) 上的 DEP 积分器，仿真 free rigid body 1000 秒。对比 RK4（有/无投影）的能量漂移和 |pi|^2 漂移。
2. **（概念）**解释为什么"离散化 Hamilton 原理"比"离散化微分方程"能更好地保持守恒量。

---

本章小结

知识点	核心公式/结论	难度	工程对应
Body/Spatial velocity	xi = g^{-1}g_dot, eta = g_dot g^{-1}, eta = Ad_g xi	⭐⭐	data.v[i] (body)
约束变分	delta xi = dot{eta} + ad_xi eta	⭐⭐⭐	—
Euler-Poincare 方程	d/dt(dl/d xi) = ad*_xi(dl/d xi) + tau	⭐⭐	隐含于 RNEA/ABA
SO(3) 退化	I omega_dot + omega x I omega = tau	⭐⭐	陀螺仪、卫星
SE(3) 退化	G_b xi_dot = ad*_xi (G_b xi) + F	⭐⭐	Newton-Euler 6D
浮基动力学	M v_dot + Cv + g = S^T tau + J_c^T lambda	⭐⭐	JointModelFreeFlyer
Centroidal dynamics	h_dot = sum F_ext	⭐⭐	ccrba / dccrba
Featherstone 对应	motion cross = ad, force cross = ad*, Plucker = Ad	⭐⭐	源码中的 cross
Lie-Poisson	{F,G}(mu) = angle{mu, [dF/dmu, dG/dmu]}	⭐⭐⭐	—
Casimir		pi	^2 守恒（SO(3)）
SE_2(3)	IMU 预积分的 group-affine 结构	⭐⭐⭐	InEKF
半直积	有重力时的部分约化	⭐⭐⭐⭐	heavy top
Lie 群变分积分器	g_{k+1} = g_k exp(h xi_k)，能量 O(1)	⭐⭐⭐⭐	Crocoddyl

---

累积项目：本章新增模块

项目：从零构建浮基四足控制器

本章新增模块： - 模块 4-4A：加载 Solo12 浮基模型，打印 nq/nv，理解四元数约定 - 模块 4-4B：实现 ccrba/dccrba 调用，数值验证 h_dot = sum F_ext - 模块 4-4C：可视化 Euler 刚体的 Poinsot 构造（能量椭球 + 角动量球面交线）

上一章（30_ON递推算法）的模块：RNEA/ABA 在固定基座上的验证。 下一章（50_动力学解析微分）将新增：计算 dtau/dq 并用于 iLQR。

---

延伸阅读

资源	难度	内容
Marsden & Ratiu, Intro to Mechanics and Symmetry Ch.13	⭐⭐⭐	EP 方程的黄金标准
Lynch & Park, Modern Robotics Ch.8	⭐⭐	SE(3) 动力学工程写法
Featherstone, RBDA Ch.2	⭐⭐	空间向量代数原始定义
Orin-Goswami-Lee 2013, Autonomous Robots	⭐⭐	Centroidal dynamics
Barrau-Bonnabel 2017, IEEE TAC	⭐⭐⭐	InEKF 与 Lie 群滤波
Hartley et al. 2020, IJRR	⭐⭐⭐	Contact-aided InEKF
Lee-Leok-McClamroch 2010, CDC	⭐⭐⭐	SE(3) 几何控制
Holm-Schmah-Stoica, Geometric Mechanics	⭐⭐⭐	从 Euler top 到 falling cat
Bloch, Nonholonomic Mechanics and Control	⭐⭐⭐⭐	约束 EP + locomotion
Arnold, Math Methods of Classical Mechanics	⭐⭐⭐⭐	刚体 = so(3) 测地流
Carpentier-Mansard RSS 2018	⭐⭐⭐	解析导数（下一章前置）
Wensing-Orin 2016, IJHR	⭐⭐⭐	CMM 高效计算

---

🔧 故障排查手册

症状	可能原因	排查步骤	相关章节
浮基仿真中旋转矩阵不再正交	使用了 q += v*dt 而非 Lie 群积分	1. 检查 q_next = integrate(model, q, v*dt) 2. 打印 det(R) 和 R^T R - I 的范数	本章 4.7
centroidal momentum 验证不通过	忘记 dAg*v 项	1. 确认 hdot = Ag*a + dAg*v 2. 检查 v 不为零时是否包含了 dAg	本章 4.8
Pinocchio/MuJoCo 间数据不一致	四元数约定不同	1. 打印两库的 q[3:7] 2. 确认 (x,y,z,w) vs (w,x,y,z) 3. 做一个简单旋转验证	本章 4.7
body velocity 和 world velocity 混用	忘记 Ad_T 变换	1. 确认 Jacobian 的 reference frame (LOCAL/WORLD) 2. 检查下游 cost/约束的 frame 一致性	本章 4.2
自由刚体仿真能量漂移	使用 RK4 不投影	1. 检查积分器类型 2. 改用 Lie 群变分积分器或加 SVD 投影 3. 画能量-时间曲线	本章 4.14

---

附录 A：Pinocchio 浮基 API 完整清单

A.1 建模

pinocchio::Model model;
pinocchio::urdf::buildModel(urdf_path, pinocchio::JointModelFreeFlyer(), model);
// nq = 7 + n_joint (位置3 + 四元数4 + 关节)
// nv = 6 + n_joint (线速度3 + 角速度3 + 关节)

A.2 Centroidal API

// CMM 计算
pinocchio::ccrba(model, data, q, v);
// data.Ag: CMM (R^{6 x nv})
// data.hg: centroidal momentum (Force)
// data.Ig: centroidal composite rigid-body inertia

// CMM 时间导数
pinocchio::dccrba(model, data, q, v);
// data.dAg: dA_G/dt (R^{6 x nv})

// 直接计算 h_dot
pinocchio::computeCentroidalMomentumTimeVariation(model, data, q, v, a);

A.3 SE(3) 数据结构

类型	几何对应	操作
SE3Tpl<Scalar>	SE(3) 群元	act, actInv, inverse
MotionTpl<Scalar>	se(3) 元素	cross(Motion) = ad, cross(Force) = ad*
ForceTpl<Scalar>	se(3)* 元素	dot(Motion) = 对偶配对 = power
InertiaTpl<Scalar>	se(3) -> se(3)* 映射	vxiv(Motion) = v x* (I v)

A.4 Drake 对比

// Drake 四元数约定：(w, x, y, z) -- 与 Pinocchio 相反！
// Drake spatial momentum：
drake::multibody::SpatialMomentum<double> h =
    plant.CalcSpatialMomentumInWorldAboutPoint(context, com_position);

A.5 MuJoCo 对比

<freejoint/>  <!-- MuJoCo 浮基 -->
<!-- qpos[0:3] = position (global), qpos[3:7] = quaternion (w,x,y,z) -->
<!-- qvel[0:3] = linear velocity (GLOBAL!), qvel[3:6] = angular velocity (LOCAL!) -->

---

附录 B：核心公式速查

名称	公式	适用
Body velocity	xi = g^{-1} g_dot	所有 Lie 群
Spatial velocity	eta = g_dot g^{-1} = Ad_g xi	所有 Lie 群
约束变分	delta xi = dot{eta} + [xi, eta]	EP 推导
Euler-Poincare	d/dt(dl/d xi) = ad*_xi (dl/d xi) + tau	左不变 L
Euler 刚体	I omega_dot + omega x I omega = tau	SO(3)
6D Newton-Euler	G_b xi_dot = ad*_xi (G_b xi) + F	SE(3)
Centroidal dynamics	h_dot = sum F_ext	浮基多体
Lie-Poisson	{F,G}(mu) = pm angle{mu, [dF/dmu, dG/dmu]}	Hamiltonian 约化
Casimir (SO(3))	C(pi) = |pi|^2	自由刚体
SE_2(3) 群乘法	T1 T2 对 v: R_1 v_2 + v_1	IMU 预积分

---

附录 C：GitHub 代码仓库

仓库	用途
stack-of-tasks/pinocchio	浮基 CMM + 解析导数
stack-of-tasks/tsid	TaskAM 质心动量任务
loco-3d/crocoddyl	Lie 群 DDP
RobotLocomotion/drake	MultibodyPlant 浮基
google-deepmind/mujoco	freejoint + subtree angmom
Gepetto/example-robot-data	talos/solo12/anymal URDF
artivis/manif	轻量 SE(3)/SO(3) C++ 库
fdcl-gwu/uav_geometric_control	SE(3) 几何控制器
geomstats/geomstats	Python 几何 ML

---

附录 D：与后续专题的桥梁

本节明确说明本章知识如何直接服务于后续学习内容：

→ 50_动力学解析微分：浮基 RNEA/CCRBA 的解析导数（Carpentier-Mansard 2018）需要 SE(3) 切空间微积分——本章的 ad、Ad、ad* 是直接前置。没有理解 "placement 对 q 的微分 = 与 S 的叉乘"（这本质上就是 Ad 对群参数的导数），就无法理解 Carpentier 的核心恒等式。

→ 60_约束动力学：浮基 + 接触力 = 约束 EP on SE(3) x T^n。接触 Jacobian J_c 在 SE(3) 上的几何含义（它把关节空间速度映射到接触点的空间速度，其 kernel 正是接触约束的零空间）由本章给出。

→ 05_运动控制/90_DDP：Crocoddyl 的 StateMultibody 定义了 SE(3) x R^n 上的状态空间，其 integrate/diff 操作正是本章的 Lie 群积分。DDP 的 cost 函数（如 CostModelFramePlacement）使用 Psi(R, R_d) = 1/2 tr(I - R_d^T R) 避开 Euler 角奇异——这就是 4.12 附录 G 中 Lee 几何控制器的 Lyapunov 函数。

→ 05_运动控制/120_MPC：OCS2 的 centroidal model 把 h_dot = sum F_ext 作为简化动力学约束。CMM A_G(q) 的计算依赖于本章 4.8 节的 centroidal dynamics 理论，其导数 dA_G/dq 则连接到 50_动力学解析微分。

→ InEKF 与状态估计：Barrau-Bonnabel 2017 的 InEKF 要求 process model 写在 SE_2(3) 上（4.11 节）。等变观测器要求动力学是 G-equivariant——这直接在本章语言内表达。

---

附录 E：Euler-Poincare 方程的 Spatial 形式详细推导 ⭐⭐⭐

本附录给出 Euler-Poincare 方程的**右约化（spatial frame）**完整推导，与正文的左约化形成对照。

D.1 右平凡化与 Spatial 变分

定义 spatial velocity eta = g_dot g^{-1} in g。对两参数族 g(t, epsilon)，定义： - zeta(t, epsilon) = (partial_t g) g^{-1}（spatial "velocity"） - sigma(t, epsilon) = (partial_epsilon g) g^{-1}（spatial "variation"）

右约化变分恒等式：

\[\delta\zeta = \dot\sigma - [\zeta, \sigma] = \dot\sigma - \mathrm{ad}_\zeta\,\sigma\]

注意**符号与左约化相反**（左约化是 +ad，右约化是 -ad）！这来自 Lie bracket 在左/右平凡化下的不同表现。

证明 sketch： - delta zeta = d/d epsilon (g_dot g^{-1}) = (d/d epsilon g_dot) g^{-1} + g_dot (d/d epsilon g^{-1}) - 利用 d/d epsilon g^{-1} = -g^{-1} (d g/d epsilon) g^{-1} - 混合偏导交换 + 整理 → delta zeta = dot{sigma} - [zeta, sigma]

D.2 右约化 Euler-Poincare

对右不变 Lagrangian l_R(eta) = L(g, g_dot)（L 关于 G 的右平移不变），Hamilton 原理给出：

\[\frac{d}{dt}\frac{\partial\ell_R}{\partial\eta} = -\mathrm{ad}^*_\eta\left(\frac{\partial\ell_R}{\partial\eta}\right) + \tau_s\]

与左约化的区别仅在于 ad* 前的符号。

D.3 左约化 vs 右约化的物理对应

约化类型	velocity 定义	EP 方程	符号约定	常用领域
左约化 (body)	xi = g^{-1} g_dot	d/dt(dl/d xi) = +ad*_xi ...	Featherstone, Pinocchio	机器人学
右约化 (spatial)	eta = g_dot g^{-1}	d/dt(dl/d eta) = -ad*_eta ...	Euler 流体方程	流体力学

**Euler 流体方程**就是无穷维 Lie 群 Diff(M)（体积保持微分同胚群）上的右约化 EP 方程：

\[\frac{\partial v}{\partial t} + (v \cdot \nabla)v = -\nabla p\]

其中 (v . nabla)v 正是 -ad*_v 在无穷维下的具体化。

D.4 从 EP 恢复位形：重构方程

EP 方程的解 xi(t) 只给出 body velocity 的演化。要恢复完整的位形 g(t)，需要积分**重构方程**：

\[\dot g = g\,\xi \quad \text{（body）} \quad \text{或} \quad \dot g = \eta\,g \quad \text{（spatial）}\]

这是一个定义在 Lie 群 G 上的 ODE，初始条件 g(0) = g_0。数值积分时必须使用 Lie 群积分器（如 exp 映射）——不能直接用 RK4 on matrix entries（除非每步投影回 G）。

---

附录 E：SE(3) 上 ad 和 Ad 的显式矩阵 ⭐⭐

为方便参考和验证，本附录列出 SE(3) 相关算子的完整 6x6 矩阵形式。

E.1 Ad_T（伴随表示）

对 T = (R, p) in SE(3)：

\[\mathrm{Ad}_T = \begin{bmatrix} R & 0 \\ [p]_\times R & R \end{bmatrix} \in \mathbb{R}^{6\times 6}\]

排列约定：xi = (omega; v)，angular first。

验证：Ad_T xi = (R omega; [p]_x R omega + R v) = (R omega; p x (R omega) + R v)。

这正是"把 body twist 变换到 spatial twist"的操作：angular part 直接旋转，linear part 需要加上旋转+平移的耦合项。

E.2 ad_xi（adjoint action on algebra）

对 xi = (omega; v)：

\[\mathrm{ad}_\xi = \begin{bmatrix} [\omega]_\times & 0 \\ [v]_\times & [\omega]_\times \end{bmatrix}\]

验证：ad_xi eta = ([omega]_x eta_omega; [v]_x eta_omega + [omega]_x eta_v) = (omega x eta_omega; v x eta_omega + omega x eta_v)。

这正是 se(3) Lie bracket 的分量形式。

E.3 ad*_xi（coadjoint action on coalgebra）

\[\mathrm{ad}^*_\xi = -\mathrm{ad}_\xi^T = \begin{bmatrix} [\omega]_\times & [v]_\times \\ 0 & [\omega]_\times \end{bmatrix}\]

作用在 mu = (m; f) in se(3)* 上：

\[\mathrm{ad}^*_\xi\,\mu = \begin{bmatrix} \omega \times m + v \times f \\ \omega \times f \end{bmatrix}\]

E.4 数值验证脚本

import numpy as np

def hat(v):
    """R^3 -> so(3) hat map"""
    return np.array([[0, -v[2], v[1]],
                     [v[2], 0, -v[0]],
                     [-v[1], v[0], 0]])

def Ad_SE3(R, p):
    """SE(3) Adjoint representation, 6x6"""
    Ad = np.zeros((6, 6))
    Ad[:3, :3] = R
    Ad[3:, :3] = hat(p) @ R
    Ad[3:, 3:] = R
    return Ad

def ad_se3(xi):
    """se(3) adjoint action, 6x6"""
    omega, v = xi[:3], xi[3:]
    ad = np.zeros((6, 6))
    ad[:3, :3] = hat(omega)
    ad[3:, :3] = hat(v)
    ad[3:, 3:] = hat(omega)
    return ad

# 验证：Ad 满足 Ad_{T1 T2} = Ad_{T1} Ad_{T2}
from scipy.spatial.transform import Rotation
R1 = Rotation.random().as_matrix()
R2 = Rotation.random().as_matrix()
p1 = np.random.randn(3)
p2 = np.random.randn(3)

# T1 * T2 = (R1 R2, R1 p2 + p1)
R12 = R1 @ R2
p12 = R1 @ p2 + p1

Ad1 = Ad_SE3(R1, p1)
Ad2 = Ad_SE3(R2, p2)
Ad12 = Ad_SE3(R12, p12)

print(f"Ad 乘法一致性: {np.max(np.abs(Ad1 @ Ad2 - Ad12)):.2e}")  # ~1e-15

---

附录 F：Noether 定理的几何版本与守恒量 ⭐⭐⭐⭐

F.1 动量映射的定义

设 G 辛地作用在 (P, omega) 上，生成无穷小作用 xi_P（对任意 xi in g）。动量映射 J: P -> g* 满足：

\[d\hat{J}(\xi) = \iota_{\xi_P}\,\omega\]

其中 J_hat(xi)(z) := angle{J(z), xi}（J 在 xi 方向的分量）。

Noether 定理（几何版，Marsden-Ratiu Th.11.4.1）：若 Hamiltonian H 是 G-不变的，则 J 沿 Hamilton 流守恒：\(\frac{d}{dt}J(\phi_t(z)) = 0\)。

F.2 SO(3) 自由刚体的守恒量

• 空间角动量 pi_s = R I omega 守恒（H 对 SO(3) 左平移不变）
• Body 角动量 pi_b = I omega 不守恒（但其模长 |pi_b| = |pi_s| 守恒——Casimir）
• 能量 H = 1/2 omega^T I omega 守恒

三个守恒量把 6D 相空间 T*SO(3) 约束到 0D 流形——系统**完全可积**（Liouville 意义下）。

F.3 SE(3) 自由刚体的守恒量

• Spatial momentum (m_s, f_s) = Ad*_{T^{-1}} (I omega + ...) 守恒（6 个）
• Casimir C_1 = m . f, C_2 = |f|^2（2 个）
• 能量 H（1 个）

总守恒量：6 + 1 = 7（其中 6 个 spatial momentum 包含 2 个 Casimir）。独立守恒量：6 - 1 = 5（减去 Casimir 约束的 1 个）。12D 相空间 T*SE(3) 被约束到 12 - 2*5 = 2D——仍然可积。

F.4 对称性破缺与守恒量丧失

当加入重力（V = mgh）时，SE(3) 的完全对称性破缺为 SO(2)_z x R^2（绕 z 轴旋转 + 水平平移）。此时只有 z 方向角动量和水平线动量守恒——3 个守恒量。这意味着 heavy top（对称陀螺）不再完全可积（除非有额外对称性，如轴对称）。

---

附录 G：几何控制理论简介 ⭐⭐⭐⭐

G.1 Lee-Leok-McClamroch 2010 的 SE(3) 几何控制

这篇 CDC 论文提出了无人机在 SE(3) 上的几何跟踪控制器，已成为 PX4/ArduPilot 的事实标准。

Lyapunov 函数： $\(\Psi(R, R_d) = \frac{1}{2}\mathrm{tr}(I - R_d^T R)\)$

性质： - Psi >= 0，当且仅当 R = R_d 时 Psi = 0 - Psi 在 R = R_d 的对径点（antipodal point）处有最大值 2 - 没有 Euler 角奇异——直接在 SO(3) 上定义

控制律： $\(\tau = -k_R e_R - k_\Omega e_\Omega + \omega \times I\omega\)$

其中 e_R 是旋转误差向量（从 Psi 的梯度导出），e_Omega 是角速度误差，最后一项抵消 Euler 方程中的 gyroscopic term。

稳定性结果：几乎全局指数稳定（almost globally exponentially stable）——"几乎"是因为对径点是不稳定平衡，measure-zero 的初始条件才会不收敛。

G.2 与 Euler 角控制器的对比

维度	Euler 角 PID	SE(3) 几何控制
奇异性	万向锁（pitch = 90 度）	无
稳定域	局部（小角度）	几乎全局
大机动	不稳定（翻转恢复失败）	稳定
实现复杂度	简单	中等（需要 SO(3) 运算）

工程后果：在特技飞行、紧急恢复（如被风吹翻后自主恢复正常姿态）等场景，几何控制器的优势是决定性的。

G.3 与 MPC 的结合

Crocoddyl 的 ActionModelFreeFwdDynamics 在 SE(3) 上做轨迹优化时，cost function 可以直接使用 Psi(R, R_d) 作为旋转跟踪代价——不需要转换为 Euler 角。这与几何控制在 Lyapunov 分析中的优势完全一致。

---

附录 H：跨章综合练习

练习 H.1（综合 4-1 + 4-3 + 4-4）

题目：对 KUKA iiwa 7-DoF 臂（固定基座），用三种方式计算 body frame 末端 wrench 对应的关节力矩 tau = J^T F： 1. 直接用 Pinocchio 的 computeJointJacobians + 转置乘 2. 用 RNEA（设 q_dot = q_ddot = 0，施加 F 作为外力） 3. 手动用 spatial vector 逐连杆反向传递 F，验证结果一致

目标：理解 RNEA 的"静力学特例"、Jacobian 转置法、和空间力传递的等价性。

练习 H.2（综合 4-2 + 4-4 → 控制方向 90_DDP）

题目：对一个浮基 2-link 机器人（基座 SE(3) + 2 个 revolute joint）： 1. 写出 Lagrangian L(q, v)，其中 q = (g, theta_1, theta_2)，v = (xi_b, omega_1, omega_2) 2. 用 Euler-Poincare（基座部分） + Euler-Lagrange（关节部分）推导 8D 运动方程 3. 把方程写成 M(q) v_dot + C(q,v)v + g(q) = [0; tau] 的形式 4. 用 Pinocchio 数值验证

目标：理解 EP + EL 混合形式如何生成浮基动力学方程。

练习 H.3（综合 4-4 + 4-5 → 控制方向 120_MPC）

题目：对 Solo12 四足，设计一个最简 MPC 控制器： 1. 用 centroidal dynamics h_dot = sum F_ext 作为简化动力学 2. 用 ccrba/dccrba 计算 A_G 和 dA_G 3. 线性化后用 LQR 求解 4. 验证：站立平衡时，MPC 能抵抗基座上的小扰动

---

附录 I：关键论文列表

论文	年份	核心贡献	与本章的关系
Marsden & Ratiu, Intro to Mechanics and Symmetry	1999	EP 方程的黄金标准	全章理论基础
Orin, Goswami, Lee, Autonomous Robots	2013	Centroidal dynamics	4.8 节核心
Wensing & Orin, IJHR	2016	CMM 高效计算	4.8 节 API 对应
Lee, Leok, McClamroch, 49th CDC	2010	SE(3) 几何控制	4.12 例题三
Barrau & Bonnabel, IEEE TAC	2017	InEKF	4.11 节核心
Hartley et al., IJRR	2020	Contact-aided InEKF	4.11 节扩展
Carpentier & Mansard, RSS	2018	解析导数	→ 下一章
Kelly & Murray, J. Robotic Systems	1995	Locomotion 几何相位	4.16 节
Bou-Rabee & Marsden, FoCM	2009	Lie 群变分积分器	4.17 节
Mastalli et al., ICRA	2020	Crocoddyl FDDP	4.15 节 MPC
Fossen, Marine Craft Control	2011	水下 SE(3) 动力学	4.12 例题四

---

附录 J：术语对照表（中英文）

英文	中文	本章首次出现
Euler-Poincare equation	欧拉-庞加莱方程	4.4
body velocity / left-trivialized velocity	体速度 / 左平凡化速度	4.2
spatial velocity / right-trivialized velocity	空间速度 / 右平凡化速度	4.2
adjoint representation	伴随表示	4.2
coadjoint action	余伴随作用	4.4
constrained variation	约束变分	4.3
reduced Lagrangian	约化拉格朗日量	4.2
centroidal momentum matrix (CMM)	质心动量矩阵	4.8
Lie-Poisson bracket	李-泊松括号	4.10
Casimir function	卡西米尔函数	4.10
coadjoint orbit	余伴随轨道	4.10
symplectic leaf	辛叶	4.10
momentum map	动量映射	4.13
geometric phase / holonomy	几何相位 / 完整性	4.16
variational integrator	变分积分器	4.17
advected parameter	平流参数	4.16 节
semi-direct product	半直积	4.16 节

---

附录 K：学习时间预算

档位 3（必学，30-40 小时）： - 第 1 周（8h）：4.1-4.2 节，body/spatial velocity 完全吃透；手推 SO(3) 上的 delta omega = eta_dot + omega x eta - 第 2 周（10h）：4.3-4.4 节，完成 EP 完整推导；SO(3)/SE(3) 退化亲手验证 - 第 3 周（8h）：4.7-4.8 节，浮基 + centroidal dynamics；读 Orin-Goswami-Lee 2013 全文 - 第 4 周（10-14h）：4.9 节对照表 + Pinocchio 编程验证 h_dot = sum F_ext

里程碑：能独立对 Solo12 跑完 ccrba/dccrba，并验证浮基 RNEA 与 EP 方程一致性；能在白板上 30 分钟内完整复现 EP 推导。

档位 4（进阶，额外 25-35 小时）： - 第 5-6 周（12h）：4.10-4.13 节，Lie-Poisson + 动量映射 + coadjoint orbits - 第 7 周（6h）：4.16 节，Kelvin-Noether + 半直积 - 第 8 周（8-10h）：4.17 节，Lie 群变分积分器 + DEP vs RK4 能量漂移对比编程

里程碑：能向纯数学 PhD 讲清 "为什么 Euler 方程是 SO(3) 上的 Lie-Poisson"；能实现 Lie 群积分器并量化其相对 RK4 的优势。

---

附录 L：本章 Pinocchio 最小可运行示例

以下代码片段可直接运行，验证本章核心概念：

#!/usr/bin/env python3
"""SE(3) 几何力学 - 本章核心概念的数值验证"""
import pinocchio as pin
import numpy as np
from example_robot_data import load

# === 加载浮基四足模型 ===
robot = load("solo12")
model, data = robot.model, robot.data
print(f"Solo12: nq={model.nq}, nv={model.nv} (差异={model.nq - model.nv}=四元数约束)")

# === 验证 1: nq != nv ===
assert model.nq == 19 and model.nv == 18, "浮基 Solo12 应该有 nq=19, nv=18"

# === 验证 2: Lie 群积分 vs 直接加法 ===
q = pin.randomConfiguration(model)
v = np.random.randn(model.nv) * 0.001
q_lie = pin.integrate(model, q, v)          # 正确：Lie 群积分
q_bad = q.copy(); q_bad[:model.nv] += v[:model.nv]  # 错误：直接加
quat_lie = q_lie[3:7]
quat_bad = q_bad[3:7] if len(q_bad) > 6 else None
print(f"Lie 积分后 |quat| = {np.linalg.norm(quat_lie):.15f}")  # = 1.0
# q_bad 的四元数范数会偏离 1

# === 验证 3: centroidal dynamics h_dot = sum F_ext ===
q = pin.randomConfiguration(model)
v = np.zeros(model.nv)  # 静止
pin.ccrba(model, data, q, v)
h0 = np.concatenate([data.hg.angular, data.hg.linear])
# 静止时质心动量应为零
print(f"静止时 |h| = {np.linalg.norm(h0):.2e} (应接近 0)")

print("\n所有验证通过！本章核心概念正确。")

---

附录 M：视频与中文资源

视频： - Jerrold Marsden Caltech CDS 202/205 (Geometric Mechanics) - Taeyoung Lee (GWU FDCL) YouTube 频道：SE(3) 几何控制 - Russ Tedrake Underactuated Robotics (underactuated.mit.edu) - ETH Robot Dynamics (151-0851-00L) Marco Hutter - Stanford CS223A (Khatib) 操作空间控制

中文： - 深蓝学院《机器人动力学》《最优控制与 MPC》 - 知乎：搜索 "Euler-Poincare"、"浮基机器人"、"质心动量" - 《视觉 SLAM 十四讲》第 4 讲（SO(3)/SE(3) 中文入门） - B 站：搜索 "李群 刚体动力学"、"Pinocchio 教程" - 清华大学高云峰《理论力学》（B 站全套）涉及 Euler 方程经典推导 - 梅凤翔《分析力学》、刘延柱《高等动力学》——中文分析力学经典

---