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

第 49 章：空间向量代数与浮动基座动力学¶

本章定位：这是腿足机器人动力学的数学核心。从"为什么 3D 向量不够用"出发，构建 6D 空间向量代数的完整框架，最终推导出浮动基座的全身动力学方程。本章是 Ch53（WBC）和 Ch55（OCS2 MPC）的直接前置——不理解本章内容，后续所有控制算法都无法深入。

前置依赖：Ch11（Eigen 基础）、Ch23（李群与 manif，SE(3)/SO(3)/伴随表示）、Ch47（Pinocchio Model-Data 架构）

核心参考文献：Roy Featherstone, Rigid Body Dynamics Algorithms, Springer, 2008

建议学时：2 周（本课程最重数学推导的章节）

49.0 前置自测¶

📋 前置自测（答不出 $\geq 2$ 题 → 先回对应章节复习）

[Ch11] 给定 $3 \times 3$ 反对称矩阵 $[\boldsymbol{\omega}]_\times$，写出它的显式形式。它与叉积 $\boldsymbol{\omega} \times \boldsymbol{v}$ 的关系是什么？
[Ch23] $SE(3)$ 群的元素 $T = (R, t)$ 如何作用于一个点 $\boldsymbol{p} \in \mathbb{R}^3$？写出变换公式。
[Ch23] 什么是 $SE(3)$ 的伴随表示 $\operatorname{Ad}_T$？它是一个多大的矩阵？它把什么映射到什么？
[Ch47] 在 Pinocchio 中，model.nq 和 model.nv 分别表示什么？为什么对于含 FreeFlyer 关节的模型，nq != nv？
[刚体力学] 牛顿-欧拉方程写成什么形式？平动部分和转动部分分别是什么？

如果第 3 题答不出来——本章的数学核心就是 $\operatorname{Ad}_T$ 在 6D 空间中的具体矩阵形式。请务必先回 Ch23 复习李群伴随表示。

49.0.1 本章目标¶

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

**解释**为什么 Featherstone 的空间向量代数能把刚体动力学递归算法（如 RNEA、ABA）降到 $O(N)$——核心在于 6D 向量统一了参考点/坐标系变换（注：CRBA 仍为 $O(N^2)$）
**手写**一个 twist（6D 速度）和 wrench（6D 力）的变换，并解释它们的对偶关系
推导 Plucker 坐标变换矩阵 ${}^A\!X_B$ 的完整结构，说明它与 $\operatorname{Ad}_{T_{AB}}$ 的关系
构造 $6 \times 6$ 空间惯量矩阵 $\hat{I}$，并用它写出牛顿-欧拉方程的空间形式
**写出**浮动基座全身动力学方程 $M(q)\ddot{q} + h(q,\dot{q}) = S^T\tau + J_c^T\lambda_c$，解释每一项的物理含义
使用 Pinocchio 计算 Go2 四足机器人的 RNEA/ABA/CRBA，理解代码与数学公式的对应关系
理解 Centroidal Momentum Matrix 如何桥接全身动力学与简化模型（Ch51）

49.1 为什么需要空间向量 ⭐¶

动机：一个看似简单的问题¶

考虑一个最简单的场景：一根刚性连杆在空间中运动。你需要回答一个问题——这根连杆上某一点 $P$ 的速度是多少？

如果你用"朴素的 3D 向量"来描述，你需要：

连杆的**角速度** $\boldsymbol{\omega} \in \mathbb{R}^3$（描述转动）
连杆上某个参考点 $O$ 的**线速度** $\boldsymbol{v}_O \in \mathbb{R}^3$（描述平动）
点 $P$ 相对于 $O$ 的位移 $\boldsymbol{r}_{OP} \in \mathbb{R}^3$

然后：

\[\boldsymbol{v}_P = \boldsymbol{v}_O + \boldsymbol{\omega} \times \boldsymbol{r}_{OP}\]

这看起来很简单。但问题马上出现了——角速度和线速度的变换规则**不对称**，在多连杆系统中会引发一连串工程问题。

参考点依赖：3D 方法的根本缺陷¶

问题 1：参考点不同，线速度变换规则不同

角速度 $\boldsymbol{\omega}$ 是刚体的**全局属性**——无论你选哪个参考点，角速度都一样。但线速度 $\boldsymbol{v}_O$ 是**依赖参考点的**——连杆上不同点的线速度不同。

这意味着：当你需要把一个连杆的运动从一个坐标系变换到另一个坐标系时，角速度只需要旋转（$\boldsymbol{\omega}' = R\boldsymbol{\omega}$），但线速度需要旋转**加上**一个耦合项（$\boldsymbol{v}'_{O'} = R\boldsymbol{v}_O + \boldsymbol{t} \times R\boldsymbol{\omega}$）。

问题 2：力和力矩也有类似的不一致

力 $\boldsymbol{f}$ 是不依赖参考点的（平移刚体不改变力），但力矩 $\boldsymbol{\tau}$ 依赖参考点：

\[\boldsymbol{\tau}_A = \boldsymbol{\tau}_B + \boldsymbol{r}_{BA} \times \boldsymbol{f}\]

这又是一个不对称的变换规则。

问题 3：编程中的灾难——如果不用 6D 空间向量，多连杆动力学代码就好比用手工记账而不是电子表格管理财务：每笔交易（坐标变换）都需要手动区分"本金"（力/角速度）和"利息"（力矩修正/线速度耦合），稍有疏忽就全盘皆错。

在写多连杆机器人的动力学代码时，每个连杆都有自己的坐标系。你需要在不同坐标系之间反复变换速度和力。如果用 3D 向量分开处理：

// 朴素写法：角速度和线速度分开变换
Vector3d omega_A = R_AB * omega_B;                           // 角速度只旋转
Vector3d v_A = R_AB * v_B + t_AB.cross(R_AB * omega_B);     // 线速度旋转+耦合

// 力的变换——注意力和力矩的变换方向与速度不同！
Vector3d f_A = R_AB * f_B;                                   // 力只旋转
Vector3d tau_A = R_AB * tau_B + t_AB.cross(R_AB * f_B);     // 力矩旋转+耦合

对于一个有 $N$ 个连杆的机器人，在 RNEA 算法中你需要做 $2N$ 次这样的变换。每次都要写两行代码（角速度/线速度分开），并且耦合项符号容易搞错。这是 bug 的温床：

常见错误	后果
忘记耦合项 $\boldsymbol{t} \times R\boldsymbol{\omega}$	线速度变换错误，动力学结果完全错误
力的耦合项符号写反	力矩方向反了，机器人仿真中"飞"起来
角速度和线速度的排列顺序搞混	在不同库之间移植代码时最常见
忘记区分 body frame 和 spatial frame	所有结果都错，但数值"差不多"，极难 debug

Featherstone 的洞察（1987）¶

Roy Featherstone 在 1987 年的博士论文中提出了一个关键洞察：

把角速度和线速度组合成一个 6D 向量（twist），把线力和力矩组合成一个 6D 向量（wrench），所有的变换规则就统一为一个 $6 \times 6$ 矩阵乘法。

这不是简单的"把两个 3D 向量拼在一起"——6D 向量有自己的代数结构（它们是李代数 $\mathfrak{se}(3)$ 和余切空间 $\mathfrak{se}(3)^*$ 的元素），变换矩阵有自己的群结构（它们是 $SE(3)$ 的伴随表示）。

Featherstone 空间向量代数的三个核心贡献：

贡献	具体内容	工程价值
统一变换	所有坐标系变换规约为 $6 \times 6$ 矩阵乘法	代码简洁，bug 少
统一运算	叉积、求导、牛顿-欧拉方程全在 6D 空间	公式对称，推导简单
O(N) 算法	RNEA/ABA/CRBA 都是线性时间	实时控制可行

历史脉络：Plucker → Ball → Featherstone¶

空间向量的概念并不是 Featherstone 凭空发明的。它有一段悠久的数学历史：

年代	人物	贡献
1865	Julius Plucker	提出 Plucker 坐标：用 6 个数描述 3D 空间中的一条直线
1876	Robert Ball	The Theory of Screws：用螺旋（screw）统一描述刚体的运动和力
1964	Brand, Dimentberg	将螺旋理论应用于机构学
1978	Yuan, Freudenstein	首次在机器人学中使用 6D 向量
1987	Roy Featherstone	博士论文：建立完整的空间向量代数 + O(N) 动力学算法
2008	Roy Featherstone	Rigid Body Dynamics Algorithms：空间向量代数的"圣经"
2019	Carpentier, Mansard	Pinocchio 库：空间向量代数的现代 C++ 实现

Plucker 坐标描述的是空间中的一条**直线**——用方向向量 $\boldsymbol{d}$ 和矩向量 $\boldsymbol{m} = \boldsymbol{p} \times \boldsymbol{d}$ 来表示（其中 $\boldsymbol{p}$ 是直线上的任意一点）。注意到 $\boldsymbol{m}$ 的变换规则和线速度的变换规则完全一样——这不是巧合。刚体的瞬时运动可以用一条**螺旋轴**（screw axis）来描述，而螺旋轴就是一条 Plucker 直线加上一个沿轴的平移量。

本质洞察：空间向量代数不是"花哨的数学包装"，而是把刚体动力学从"手工处理 3D 向量的耦合关系"升级到"自动化的 6D 代数运算"——就像从手写汇编升级到用高级语言编程。3D 世界中角速度和线速度的不对称变换规则，在 6D 空间中变成了一个统一的矩阵乘法——不对称性并没有消失，而是被**编码进了变换矩阵的结构**中，由代数机制自动处理。

固定基座 vs 浮动基座：为什么本章如此重要¶

在进入 6D 代数之前，我们先明确一个关键区别——这是理解本章后半部分的基础。

固定基座（Fixed Base）：

典型例子：工业机械臂（如 Franka Panda、UR5）
基座焊接在桌面/地面上，位姿固定不变
广义坐标 $q = (q_1, q_2, \ldots, q_n)^T \in \mathbb{R}^n$，只包含关节角
每个关节都有电机驱动——全驱动（fully-actuated）
动力学方程：$M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = \tau$

浮动基座（Floating Base）：

典型例子：四足机器人（Go2）、人形机器人（Atlas、Talos）
基座可以在 3D 空间中自由运动（6 自由度：3 平动 + 3 转动）
广义坐标 $q = (q_{\text{base}}, q_{\text{joints}})^T$
$q_{\text{base}}$：基座位姿，通常用 $(x, y, z, q_w, q_x, q_y, q_z)$ 表示，维度 7
$q_{\text{joints}}$：关节角，维度 $n$（Go2: $n=12$）
nq = 7 + n，nv = 6 + n（因为四元数 4 维但角速度 3 维）
基座的 6 个自由度没有电机——欠驱动（under-actuated）
基座运动只能通过**足端接触力**间接控制

属性	固定基座	浮动基座
基座自由度	0	6
广义坐标维度 nq	$n$	$7 + n$
广义速度维度 nv	$n$	$6 + n$
nq = nv ?	是	否（差 1，因为四元数）
驱动性	全驱动	欠驱动
动力学等式右端	$\tau$（关节扭矩）	$S^T\tau + J_c^T\lambda$（扭矩 + 接触力）

⚠️ 常见陷阱：很多初学者在第一次使用 Pinocchio 时，把 model.nq 当成状态维度来分配 q_dot 向量，结果维度不匹配。记住：对于浮动基座机器人，nq ≠ nv。位形空间维度（nq）比切空间维度（nv）多 1，因为四元数的 4 个分量在切空间中对应 3 个角速度分量。在 Pinocchio 中，Go2 的 model.nq = 19，model.nv = 18。

练习 49.1.1：对于 Unitree Go2（12 个关节 + FreeFlyer 基座），写出 nq 和 nv 的值。如果改用欧拉角表示基座姿态，nq 和 nv 分别是多少？为什么 Pinocchio 选择四元数而不是欧拉角？

练习 49.1.2：解释为什么固定基座机器人的动力学方程中没有 $J_c^T\lambda$ 项。如果一个固定基座机器人的末端也与环境接触（例如抛光任务），方程应该怎么修改？

49.2 Twist（空间速度）⭐⭐¶

从直觉出发：为什么需要 6D 速度¶

在上一节我们看到，描述一个刚体的运动需要两个 3D 量：角速度 $\boldsymbol{\omega}$ 和线速度 $\boldsymbol{v}$。Featherstone 的核心想法是把它们打包成一个 6D 向量——这就像把电压和电流合并为"复功率"来统一处理交流电路：表面上只是"拼在一起"，实际上 6D 向量拥有自己独立的代数结构和变换规则，远非简单拼接。但"怎么打包"有两种约定，这是理解文献时最大的混乱来源之一。

Twist 的定义¶

Twist（空间速度，或广义速度）是一个 6D 向量，表示刚体在某个坐标系中的瞬时运动：

\[\mathbf{V} = \begin{pmatrix} \boldsymbol{\omega} \\ \boldsymbol{v} \end{pmatrix} \in \mathbb{R}^6\]

这里 $\boldsymbol{\omega} \in \mathbb{R}^3$ 是角速度，$\boldsymbol{v} \in \mathbb{R}^3$ 是线速度。

⚠️ 常见陷阱——约定之争：Featherstone 原书使用 $(\boldsymbol{\omega}, \boldsymbol{v})$ 顺序（角速度在上），本章正文公式沿用此约定。但有些教材（如 Murray, Li, Sastry 的 Mathematical Introduction to Robotic Manipulation）和某些 SLAM 库使用 $(\boldsymbol{v}, \boldsymbol{\omega})$ 顺序（线速度在上）。注意：Pinocchio 的数学文献沿用 Featherstone 记号 $(\boldsymbol{\omega}, \boldsymbol{v})$，但其 C++ API 中 Motion/Force 对象的内部存储顺序为 $(\boldsymbol{v}, \boldsymbol{\omega})$（linear 在前，angular 在后），getFrameJacobian 返回的 Jacobian 前 3 行 = 线速度、后 3 行 = 角速度。使用 Pinocchio API 时务必注意行索引与本章公式的对应关系。

约定	排列	使用者
Featherstone（本章公式）	$(\boldsymbol{\omega}, \boldsymbol{v})^T$	Featherstone 2008, RBDL, Drake
Pinocchio API 存储	$(\boldsymbol{v}, \boldsymbol{\omega})^T$	Pinocchio C++ `Motion`/`Force` 内存布局
MLS / Lie group	$(\boldsymbol{v}, \boldsymbol{\omega})^T$	Murray-Li-Sastry 1994, Manif, 部分 SLAM 文献

在代码中遇到 6D 速度时，第一件事就是确认约定。差一个 swap 就导致完全错误的结果。

Body Twist vs Spatial Twist¶

对于同一个刚体的运动，我们可以用不同的坐标系来表达 twist。两种最常用的表达方式：

Body Twist（体坐标系速度，也称 body-fixed velocity）：

\[{}^B\mathbf{V} = \begin{pmatrix} {}^B\boldsymbol{\omega} \\ {}^B\boldsymbol{v} \end{pmatrix}\]

角速度和线速度都在**刚体自身坐标系** $B$ 中表达
线速度是刚体坐标系原点在刚体坐标系中的速度
物理直觉：你坐在刚体上，用你自己的坐标轴来测量运动

Spatial Twist（空间坐标系速度，也称 spatial velocity）：

\[{}^S\mathbf{V} = \begin{pmatrix} {}^S\boldsymbol{\omega} \\ {}^S\boldsymbol{v} \end{pmatrix}\]

角速度和线速度都在**世界坐标系（或空间坐标系）** $S$ 中表达
线速度是**刚体坐标系原点瞬时叠合在世界坐标系原点的那个点**的速度
物理直觉：你站在原地，看着刚体运动，用世界坐标系来测量

这两个量的数学关系，用李群的语言表达非常清楚。设 $T_{SB}(t) \in SE(3)$ 是刚体坐标系 $B$ 相对于空间坐标系 $S$ 的齐次变换矩阵：

\[T_{SB} = \begin{pmatrix} R_{SB} & \boldsymbol{p}_{SB} \\ 0 & 1 \end{pmatrix}\]

那么：

Body twist 来自左不变向量场（右乘求导）：

\[[{}^B\mathbf{V}]_\wedge = T_{SB}^{-1} \dot{T}_{SB} \in \mathfrak{se}(3)\]

Spatial twist 来自右不变向量场（左乘求导）：

\[[{}^S\mathbf{V}]_\wedge = \dot{T}_{SB} T_{SB}^{-1} \in \mathfrak{se}(3)\]

这里 $[\cdot]_\wedge$ 表示从 $\mathbb{R}^6$ 到 $\mathfrak{se}(3)$（$4 \times 4$ 矩阵）的映射：

\[\left[\begin{pmatrix} \boldsymbol{\omega} \\ \boldsymbol{v} \end{pmatrix}\right]_\wedge = \begin{pmatrix} [\boldsymbol{\omega}]_\times & \boldsymbol{v} \\ 0 & 0 \end{pmatrix}\]

Body twist 与 Spatial twist 的关系推导¶

我们来**完整推导**两者之间的关系。这是理解 Plucker 变换矩阵的基础。

从 body twist 出发：

\[[{}^B\mathbf{V}]_\wedge = T_{SB}^{-1} \dot{T}_{SB}\]

从 spatial twist 出发：

\[[{}^S\mathbf{V}]_\wedge = \dot{T}_{SB} T_{SB}^{-1}\]

将第一个式子代入第二个。从 $[{}^B\mathbf{V}]_\wedge = T_{SB}^{-1} \dot{T}_{SB}$ 可得 $\dot{T}_{SB} = T_{SB} [{}^B\mathbf{V}]_\wedge$，代入：

\[[{}^S\mathbf{V}]_\wedge = T_{SB} [{}^B\mathbf{V}]_\wedge T_{SB}^{-1}\]

这正是 $SE(3)$ 的**伴随作用**（Adjoint action）：

\[[{}^S\mathbf{V}]_\wedge = \operatorname{Ad}_{T_{SB}} [{}^B\mathbf{V}]_\wedge\]

写成 6D 向量形式：

\[{}^S\mathbf{V} = \operatorname{Ad}_{T_{SB}} \cdot {}^B\mathbf{V}\]

其中伴随矩阵 $\operatorname{Ad}_T$ 是一个 $6 \times 6$ 矩阵。具体形式我们在下一节详细推导。

螺旋运动解释¶

Twist 不仅仅是"角速度和线速度的拼接"——它有更深的几何含义。

Chasles 定理（1830 年）：任何刚体的瞬时运动都可以分解为**绕某条轴的旋转加上沿该轴的平移**——即一个**螺旋运动**（screw motion）。

一个 twist $\mathbf{V} = (\boldsymbol{\omega}, \boldsymbol{v})^T$ 对应的螺旋参数为：

螺旋轴方向：$\hat{\boldsymbol{\omega}} = \boldsymbol{\omega} / \|\boldsymbol{\omega}\|$（当 $\boldsymbol{\omega} \neq 0$ 时）
螺旋轴位置：轴经过的点 $\boldsymbol{q} = \frac{\boldsymbol{\omega} \times \boldsymbol{v}}{\|\boldsymbol{\omega}\|^2}$
角速度大小：$\|\boldsymbol{\omega}\|$
螺距（pitch）：$h = \frac{\boldsymbol{\omega}^T \boldsymbol{v}}{\|\boldsymbol{\omega}\|^2}$（沿轴平移速度与角速度之比）

特殊情况： - $h = 0$（纯旋转）：旋转关节的运动 - $h = \infty$（纯平移，$\boldsymbol{\omega} = 0$）：棱柱关节的运动 - $0 < h < \infty$（螺旋运动）：螺旋关节的运动

⚠️ 常见陷阱：当 $\boldsymbol{\omega} = 0$ 时（纯平移），螺旋轴的方向变为 $\boldsymbol{v} / \|\boldsymbol{v}\|$，螺距为 $\infty$，螺旋轴"退化"为无穷远处的一条直线。在数值计算中，当 $\|\boldsymbol{\omega}\|$ 很小时，螺旋参数的计算会变得不稳定——这就是为什么实际代码中一般不显式计算螺旋参数，而是直接用 6D 向量运算。

与 $\mathfrak{se}(3)$ 的对应¶

在 Ch23 中我们学过，$\mathfrak{se}(3)$ 是 $SE(3)$ 的李代数，由 $4 \times 4$ 矩阵组成：

\[\mathfrak{se}(3) = \left\{ \begin{pmatrix} [\boldsymbol{\omega}]_\times & \boldsymbol{v} \\ 0 & 0 \end{pmatrix} \;\middle|\; \boldsymbol{\omega}, \boldsymbol{v} \in \mathbb{R}^3 \right\}\]

Twist 就是 $\mathfrak{se}(3)$ 的一个元素，用 6D 向量 $(\boldsymbol{\omega}, \boldsymbol{v})^T$ 来表示。映射关系：

\[(\boldsymbol{\omega}, \boldsymbol{v})^T \xrightarrow{[\cdot]_\wedge} \begin{pmatrix} [\boldsymbol{\omega}]_\times & \boldsymbol{v} \\ 0 & 0 \end{pmatrix} \in \mathfrak{se}(3)\]

反过来，从 $4 \times 4$ 矩阵提取 6D 向量：

\[\begin{pmatrix} [\boldsymbol{\omega}]_\times & \boldsymbol{v} \\ 0 & 0 \end{pmatrix} \xrightarrow{[\cdot]_\vee} (\boldsymbol{\omega}, \boldsymbol{v})^T \in \mathbb{R}^6\]

Pinocchio 中的 Twist¶

在 Pinocchio 中，运行正向运动学后，每个连杆的 body twist 存储在 data.v[i] 中：

import pinocchio as pin
import numpy as np

# 加载 Go2 模型（假设 URDF 已准备好）
model = pin.buildModelFromUrdf("go2.urdf", pin.JointModelFreeFlyer())
data = model.createData()

# 设置位形和速度
q = pin.neutral(model)  # 零位形
v = np.zeros(model.nv)  # 零速度
v[0] = 1.0  # 基座沿 x 平移速度 1 m/s

# 运行正向运动学
pin.forwardKinematics(model, data, q, v)

# 查看基座的 body twist
print(f"Base body twist: {data.v[0]}")
# 输出形如: (1, 0, 0, 0, 0, 0)  即线速度沿 x 方向，角速度为零
# Pinocchio 的 Motion 类型内部用 (linear, angular) 顺序

# 查看某个关节的 body twist
joint_id = model.getJointId("FR_hip_joint")  # 右前髋关节
print(f"FR hip body twist: {data.v[joint_id]}")

Pinocchio 的 Motion 类型封装了 twist，提供了 .angular 和 .linear 访问器：

twist = data.v[joint_id]
omega = twist.angular  # 角速度部分 (3D)
v_lin = twist.linear   # 线速度部分 (3D)

# 也可以当作 numpy 数组使用
twist_np = twist.np  # 返回 6D numpy 数组 [linear; angular]

练习 49.2.1：用 Pinocchio 加载 Go2 模型，设置基座绕 z 轴以角速度 $\omega_z = 0.5$ rad/s 旋转（其他速度为零）。运行 forwardKinematics 后，检查右前脚（FR_foot）的 body twist。解释为什么即使只有基座在转动，脚的 twist 也有非零的线速度分量。

练习 49.2.2：数学推导——给定 body twist ${}^B\mathbf{V} = (0, 0, \omega_z, v_x, 0, 0)^T$（绕 z 轴旋转同时沿 x 平移），写出对应的 $\mathfrak{se}(3)$ 矩阵 $[{}^B\mathbf{V}]_\wedge$，并用矩阵指数 $e^{[{}^B\mathbf{V}]_\wedge t}$ 计算 $t=1$ 时的 $SE(3)$ 变换矩阵（精确解，不要用近似）。

49.3 Plucker 变换矩阵 ⭐⭐⭐¶

动机：我们需要一个统一的坐标系变换¶

在上一节，我们知道了 body twist 和 spatial twist 之间通过伴随矩阵 $\operatorname{Ad}_T$ 联系。现在我们来**完整推导**这个 $6 \times 6$ 矩阵的具体形式——Featherstone 称之为 Plucker 变换矩阵（Plucker transform），记作 ${}^A\!X_B$。

这个矩阵是空间向量代数的**核心工具**。一旦掌握了它，所有 6D 量的坐标变换都变成矩阵乘法。

从 $SE(3)$ 伴随表示的完整推导¶

设坐标系 $A$ 和 $B$ 之间的齐次变换矩阵为：

\[T_{AB} = \begin{pmatrix} R & \boldsymbol{p} \\ 0 & 1 \end{pmatrix}\]

其中 $R \in SO(3)$ 是旋转矩阵，$\boldsymbol{p} \in \mathbb{R}^3$ 是 $B$ 的原点在 $A$ 坐标系中的位置。

目标：找到 $6 \times 6$ 矩阵 ${}^A\!X_B$，使得对于任意 twist $\mathbf{V}$：

\[{}^A\mathbf{V} = {}^A\!X_B \cdot {}^B\mathbf{V}\]

第一步：写出 $\mathfrak{se}(3)$ 元素的伴随作用

由上一节的推导：

\[[{}^A\mathbf{V}]_\wedge = T_{AB} [{}^B\mathbf{V}]_\wedge T_{AB}^{-1}\]

设 ${}^B\mathbf{V} = (\boldsymbol{\omega}, \boldsymbol{v})^T$，我们来展开右端。

第二步：计算 $T_{AB}^{-1}$

\[T_{AB}^{-1} = \begin{pmatrix} R^T & -R^T \boldsymbol{p} \\ 0 & 1 \end{pmatrix}\]

第三步：展开矩阵乘法

\[T_{AB} [{}^B\mathbf{V}]_\wedge T_{AB}^{-1} = \begin{pmatrix} R & \boldsymbol{p} \\ 0 & 1 \end{pmatrix} \begin{pmatrix} [\boldsymbol{\omega}]_\times & \boldsymbol{v} \\ 0 & 0 \end{pmatrix} \begin{pmatrix} R^T & -R^T \boldsymbol{p} \\ 0 & 1 \end{pmatrix}\]

先算前两个矩阵的乘积：

\[\begin{pmatrix} R & \boldsymbol{p} \\ 0 & 1 \end{pmatrix} \begin{pmatrix} [\boldsymbol{\omega}]_\times & \boldsymbol{v} \\ 0 & 0 \end{pmatrix} = \begin{pmatrix} R[\boldsymbol{\omega}]_\times & R\boldsymbol{v} \\ 0 & 0 \end{pmatrix}\]

再乘右边的矩阵：

\[\begin{pmatrix} R[\boldsymbol{\omega}]_\times & R\boldsymbol{v} \\ 0 & 0 \end{pmatrix} \begin{pmatrix} R^T & -R^T \boldsymbol{p} \\ 0 & 1 \end{pmatrix} = \begin{pmatrix} R[\boldsymbol{\omega}]_\times R^T & -R[\boldsymbol{\omega}]_\times R^T \boldsymbol{p} + R\boldsymbol{v} \\ 0 & 0 \end{pmatrix}\]

第四步：利用旋转矩阵与反对称矩阵的恒等式

这里有一个关键恒等式：

\[R [\boldsymbol{\omega}]_\times R^T = [R\boldsymbol{\omega}]_\times\]

这个恒等式的证明：对任意向量 $\boldsymbol{a}$，

\[R[\boldsymbol{\omega}]_\times R^T \boldsymbol{a} = R(\boldsymbol{\omega} \times R^T\boldsymbol{a}) = (R\boldsymbol{\omega}) \times (RR^T\boldsymbol{a}) = (R\boldsymbol{\omega}) \times \boldsymbol{a} = [R\boldsymbol{\omega}]_\times \boldsymbol{a}\]

其中第二个等号用了旋转矩阵保持叉积的性质：$R(\boldsymbol{a} \times \boldsymbol{b}) = (R\boldsymbol{a}) \times (R\boldsymbol{b})$。

代入得到：

\[T_{AB} [{}^B\mathbf{V}]_\wedge T_{AB}^{-1} = \begin{pmatrix} [R\boldsymbol{\omega}]_\times & -[R\boldsymbol{\omega}]_\times \boldsymbol{p} + R\boldsymbol{v} \\ 0 & 0 \end{pmatrix}\]

第五步：改写线速度部分

\[-[R\boldsymbol{\omega}]_\times \boldsymbol{p} + R\boldsymbol{v} = -(R\boldsymbol{\omega}) \times \boldsymbol{p} + R\boldsymbol{v} = \boldsymbol{p} \times (R\boldsymbol{\omega}) + R\boldsymbol{v} = [\boldsymbol{p}]_\times (R\boldsymbol{\omega}) + R\boldsymbol{v}\]

这里用了反对称矩阵的性质：$-\boldsymbol{a} \times \boldsymbol{b} = \boldsymbol{b} \times \boldsymbol{a}$，即 $-[R\boldsymbol{\omega}]_\times \boldsymbol{p} = \boldsymbol{p} \times (R\boldsymbol{\omega}) = [\boldsymbol{p}]_\times (R\boldsymbol{\omega})$。

所以：

\[-[R\boldsymbol{\omega}]_\times \boldsymbol{p} + R\boldsymbol{v} = [\boldsymbol{p}]_\times R \boldsymbol{\omega} + R \boldsymbol{v}\]

第六步：读出伴随矩阵

从上面的结果，$A$ 坐标系中的 twist 为：

\[{}^A\boldsymbol{\omega} = R\boldsymbol{\omega}, \quad {}^A\boldsymbol{v} = [\boldsymbol{p}]_\times R \boldsymbol{\omega} + R \boldsymbol{v}\]

写成矩阵形式：

\[\begin{pmatrix} {}^A\boldsymbol{\omega} \\ {}^A\boldsymbol{v} \end{pmatrix} = \underbrace{\begin{pmatrix} R & 0 \\ [\boldsymbol{p}]_\times R & R \end{pmatrix}}_{{}^A\!X_B = \operatorname{Ad}_{T_{AB}}} \begin{pmatrix} \boldsymbol{\omega} \\ \boldsymbol{v} \end{pmatrix}\]

Plucker 变换矩阵的显式形式¶

\[\boxed{{}^A\!X_B = \operatorname{Ad}_{T_{AB}} = \begin{pmatrix} R & 0 \\ [\boldsymbol{p}]_\times R & R \end{pmatrix} \in \mathbb{R}^{6 \times 6}}\]

其中： - $R = R_{AB} \in SO(3)$：从 $B$ 到 $A$ 的旋转 - $\boldsymbol{p} = \boldsymbol{p}_{AB}$：$B$ 的原点在 $A$ 坐标系中的位置 - $[\boldsymbol{p}]_\times$ 是 $\boldsymbol{p}$ 对应的 $3 \times 3$ 反对称矩阵

矩阵结构分析：

         ┌─────────────────────────┐
         │     R       │    0      │  ← 角速度只需旋转
    X =  │─────────────│───────────│
         │ [p]× R      │    R      │  ← 线速度需要旋转 + 耦合
         └─────────────────────────┘
              3×3           3×3

注意左下角的 $[\boldsymbol{p}]_\times R$ 正是我们在 49.1 节中看到的"耦合项"——线速度变换时额外出现的交叉项。空间向量代数把它"吸收"到了 $6 \times 6$ 矩阵中，不需要单独处理。

逆变换¶

${}^A\!X_B$ 的逆矩阵：

\[{}^B\!X_A = ({}^A\!X_B)^{-1} = \begin{pmatrix} R^T & 0 \\ -R^T[\boldsymbol{p}]_\times & R^T \end{pmatrix}\]

验证 ${}^A\!X_B \cdot {}^B\!X_A = I_6$：

\[\begin{pmatrix} R & 0 \\ [\boldsymbol{p}]_\times R & R \end{pmatrix} \begin{pmatrix} R^T & 0 \\ -R^T[\boldsymbol{p}]_\times & R^T \end{pmatrix} = \begin{pmatrix} RR^T & 0 \\ [\boldsymbol{p}]_\times RR^T - RR^T[\boldsymbol{p}]_\times & RR^T \end{pmatrix} = \begin{pmatrix} I & 0 \\ 0 & I \end{pmatrix}\]

中间利用了 $RR^T = I$，所以 $[\boldsymbol{p}]_\times - [\boldsymbol{p}]_\times = 0$。

力变换矩阵 $X^*$¶

Twist 的变换矩阵是 $X$，那 wrench（力/力矩组合）的变换矩阵是什么？

关键约束是**功率守恒**：功率 $P = \mathbf{F}^T \mathbf{V}$ 不依赖于表达坐标系。

\[P = {}^A\mathbf{F}^T \cdot {}^A\mathbf{V} = {}^B\mathbf{F}^T \cdot {}^B\mathbf{V}\]

将 ${}^A\mathbf{V} = {}^A\!X_B \cdot {}^B\mathbf{V}$ 代入左端：

\[{}^A\mathbf{F}^T ({}^A\!X_B \cdot {}^B\mathbf{V}) = {}^B\mathbf{F}^T \cdot {}^B\mathbf{V}\]

\[({}^A\!X_B^T \cdot {}^A\mathbf{F})^T \cdot {}^B\mathbf{V} = {}^B\mathbf{F}^T \cdot {}^B\mathbf{V}\]

由于这对任意 ${}^B\mathbf{V}$ 都成立：

\[{}^B\mathbf{F} = {}^A\!X_B^T \cdot {}^A\mathbf{F}\]

即 wrench 从 $A$ 变换到 $B$ 用 $X^T$。等价地，wrench 从 $B$ 变换到 $A$ 用：

\[{}^A\mathbf{F} = {}^A\!X_B^{-T} \cdot {}^B\mathbf{F}\]

Featherstone 将力变换矩阵记为 $X^*$：

\[\boxed{{}^A\!X_B^* = {}^A\!X_B^{-T} = \begin{pmatrix} R & [\boldsymbol{p}]_\times R \\ 0 & R \end{pmatrix}}\]

注意 $X^*$ 与 $X$ 的**转置对偶**关系：$X$ 的左下角非零，$X^*$ 的右上角非零。

量	从 B 变换到 A	矩阵
Motion（twist）	${}^A\mathbf{V} = {}^A\!X_B \cdot {}^B\mathbf{V}$	$X = \operatorname{Ad}_T$
Force（wrench）	${}^A\mathbf{F} = {}^A\!X_B^* \cdot {}^B\mathbf{F}$	$X^* = \operatorname{Ad}_T^{-T}$

⚠️ 常见陷阱：注意 $X^* = X^{-T}$，不是 $X^T$。很多初学者直觉上觉得"对偶就是转置"，于是力变换用 $X^T$——这是错的。正确的关系是 $X^* = X^{-T} = (X^{-1})^T = (X^T)^{-1}$。在数学上，$X$ 是 $\operatorname{Ad}_T$（伴随表示），$X^*$ 是 $\operatorname{Ad}_T^{-T}$（余伴随表示 coadjoint）。

Go2 腿链实例¶

让我们用 Go2 的右前腿（FR leg）来具体化这些变换。FR 腿有 3 个关节：hip（髋）→ thigh（大腿）→ calf（小腿）→ foot（脚）。

设各连杆的坐标系变换为：

\[T_{\text{base} \to \text{hip}} = \begin{pmatrix} R_1 & \boldsymbol{p}_1 \\ 0 & 1 \end{pmatrix}, \quad T_{\text{hip} \to \text{thigh}} = \begin{pmatrix} R_2 & \boldsymbol{p}_2 \\ 0 & 1 \end{pmatrix}\]

对应的 Plucker 变换矩阵：

\[X_{\text{base} \to \text{hip}} = \begin{pmatrix} R_1 & 0 \\ [\boldsymbol{p}_1]_\times R_1 & R_1 \end{pmatrix}\]

链式变换——把 thigh 坐标系的 twist 变换到 base 坐标系：

\[{}^{\text{base}}\mathbf{V} = X_{\text{base} \to \text{hip}} \cdot X_{\text{hip} \to \text{thigh}} \cdot {}^{\text{thigh}}\mathbf{V}\]

这就是 RNEA 正向传递的核心操作——逐关节地用 $6 \times 6$ 矩阵乘法传递 twist。

# Pinocchio 中查看 Plucker 变换
pin.forwardKinematics(model, data, q)

# 获取 world 到 FR_thigh 的 SE3 变换
joint_id = model.getJointId("FR_thigh_joint")
oMi = data.oMi[joint_id]  # SE3: world -> joint_i

# oMi 就是 T_{world, joint_i}
print(f"Position: {oMi.translation}")
print(f"Rotation:\n{oMi.rotation}")

# 获取 6x6 伴随矩阵 (= Plucker 变换矩阵)
Ad_T = oMi.action  # 6x6 numpy array
print(f"Plucker transform X:\n{Ad_T}")

# 验证结构：左上角和右下角应该是同一个 R
R = oMi.rotation
p = oMi.translation
assert np.allclose(Ad_T[:3, :3], R)     # 左上 = R
assert np.allclose(Ad_T[3:, 3:], R)     # 右下 = R
assert np.allclose(Ad_T[:3, 3:], 0)     # 右上 = 0
# 左下 = [p]x R
px = pin.skew(p)
assert np.allclose(Ad_T[3:, :3], px @ R)
print("Structure verified!")

练习 49.3.1：手算——给定纯平移 $T = (I, \boldsymbol{p})$（只有平移，无旋转），写出对应的 $X$ 和 $X^*$。验证 $X X^{-1} = I_6$。

练习 49.3.2：给定纯旋转 $T = (R, 0)$（只有旋转，无平移），写出对应的 $X$。与 $\operatorname{blkdiag}(R, R)$ 比较——什么时候 Plucker 变换退化为"分别旋转角速度和线速度"？

练习 49.3.3：证明 Plucker 变换矩阵的链式法则：${}^A\!X_C = {}^A\!X_B \cdot {}^B\!X_C$。提示：利用 $T_{AC} = T_{AB} T_{BC}$ 和伴随表示的同态性质 $\operatorname{Ad}_{T_1 T_2} = \operatorname{Ad}_{T_1} \operatorname{Ad}_{T_2}$。

49.4 Wrench（空间力）⭐⭐¶

从牛顿第三定律到 6D 力¶

在 3D 世界中，描述作用在刚体上的力需要两个量： - 力 $\boldsymbol{f} \in \mathbb{R}^3$（平动效应） - 力矩 $\boldsymbol{\tau} \in \mathbb{R}^3$（转动效应，关于某参考点）

与 twist 类似，我们把它们打包成一个 6D 向量——wrench（空间力）：

\[\mathbf{F} = \begin{pmatrix} \boldsymbol{\tau} \\ \boldsymbol{f} \end{pmatrix} \in \mathbb{R}^6\]

⚠️ 常见陷阱——排列顺序：在 Featherstone 约定（本章公式）中，wrench 的排列是 $(\boldsymbol{\tau}, \boldsymbol{f})$（力矩在上，力在下），与 twist $(\boldsymbol{\omega}, \boldsymbol{v})$ 对应——"旋转分量在上，平移分量在下"。有些文献用 $(\boldsymbol{f}, \boldsymbol{\tau})$ 顺序。注意 Pinocchio API 的 Force 对象存储顺序为 $(\boldsymbol{f}, \boldsymbol{\tau})$（linear 在前，angular 在后），与本章公式相反。 使用前务必确认约定。

对偶关系：功率不变量¶

Wrench 和 twist 之间有一个深刻的数学关系——它们是**对偶**的。用物理的语言说，功率是它们的内积：

\[P = \mathbf{F}^T \mathbf{V} = \boldsymbol{\tau}^T \boldsymbol{\omega} + \boldsymbol{f}^T \boldsymbol{v}\]

这就是为什么我们把力矩 $\boldsymbol{\tau}$ 和角速度 $\boldsymbol{\omega}$ 放在同一位置（都在上面），力 $\boldsymbol{f}$ 和线速度 $\boldsymbol{v}$ 放在同一位置（都在下面）——内积的形式最简洁。

用李群的语言： - Twist $\in \mathfrak{se}(3)$（李代数） - Wrench $\in \mathfrak{se}(3)^*$（余李代数，即对偶空间） - 功率是自然配对 $\langle \mathbf{F}, \mathbf{V} \rangle = \mathbf{F}^T \mathbf{V}$

Wrench 的变换规则¶

在 49.3 节中，我们已经推导过：力变换用 $X^*$（余伴随矩阵）。

设一个 wrench 在 $B$ 坐标系中表达为 ${}^B\mathbf{F}$，那么在 $A$ 坐标系中：

\[{}^A\mathbf{F} = {}^A\!X_B^* \cdot {}^B\mathbf{F} = \begin{pmatrix} R & [\boldsymbol{p}]_\times R \\ 0 & R \end{pmatrix} \begin{pmatrix} {}^B\boldsymbol{\tau} \\ {}^B\boldsymbol{f} \end{pmatrix}\]

展开得：

\[{}^A\boldsymbol{\tau} = R \cdot {}^B\boldsymbol{\tau} + [\boldsymbol{p}]_\times R \cdot {}^B\boldsymbol{f} = R \cdot {}^B\boldsymbol{\tau} + \boldsymbol{p} \times (R \cdot {}^B\boldsymbol{f})\]

\[{}^A\boldsymbol{f} = R \cdot {}^B\boldsymbol{f}\]

物理解释： - 力只需旋转（${}^A\boldsymbol{f} = R \cdot {}^B\boldsymbol{f}$）——力不依赖参考点 - 力矩需要旋转 + 力臂修正（$\boldsymbol{p} \times \boldsymbol{f}$）——力矩依赖参考点

这正好与 49.1 节中看到的"3D 向量分开处理"的规则一致，只不过现在统一为一个 $6 \times 6$ 矩阵乘法。如果不用 6D wrench 统一表示，力和力矩的变换就必须分别处理，每次都手动加上力臂修正项——在多连杆系统的 RNEA 反向递推中，这种手动处理极易出错，特别是当力矩的参考点在不同连杆之间反复切换时。

本质洞察：twist 和 wrench 的对偶关系不是巧合——它反映了一个深层的物理原理：功率守恒。速度空间（李代数 $\mathfrak{se}(3)$）和力空间（余切空间 $\mathfrak{se}(3)^*$）天然成对出现，它们的内积给出功率这个物理量。变换矩阵 $X$ 和 $X^*$ 的"互逆转置"关系，正是为了保证在任何坐标系下功率的计算结果都一致。这个对偶结构贯穿了整个刚体动力学——理解它，就理解了为什么 RNEA 的正向传递用 $X$（传播速度），反向传递用 $X^*$（传播力）。

接触 Wrench 在腿足机器人中的应用¶

对于四足/双足机器人，最重要的 wrench 来自**足端与地面的接触**。

当 Go2 的一只脚接触地面时，地面对脚施加的接触力可以分解为：

\[\mathbf{F}_{\text{contact}} = \begin{pmatrix} \boldsymbol{\tau}_c \\ \boldsymbol{f}_c \end{pmatrix}\]

其中： - $\boldsymbol{f}_c = (f_x, f_y, f_z)^T$：接触力（法向力 $f_z$ + 摩擦力 $f_x, f_y$） - $\boldsymbol{\tau}_c$：接触力矩（对于点接触，$\boldsymbol{\tau}_c = 0$；对于面接触如人的脚底，$\boldsymbol{\tau}_c$ 包含绕法向量的扭矩）

点接触模型（Go2 适用）：假设接触区域很小，力矩为零，只有 3D 接触力：

\[\mathbf{F}_{\text{contact}}^{\text{point}} = \begin{pmatrix} \boldsymbol{0} \\ \boldsymbol{f}_c \end{pmatrix} \in \mathbb{R}^6\]

面接触模型（人形机器人适用）：脚底有一个面积，可以产生力矩：

\[\mathbf{F}_{\text{contact}}^{\text{surface}} = \begin{pmatrix} \boldsymbol{\tau}_c \\ \boldsymbol{f}_c \end{pmatrix} \in \mathbb{R}^6\]

接触模型	力自由度	约束	典型机器人
点接触	3 ($f_x, f_y, f_z$)	摩擦锥 $\sqrt{f_x^2+f_y^2} \leq \mu f_z$	Go2, Spot
面接触	6 ($\boldsymbol{\tau}_c, \boldsymbol{f}_c$)	摩擦锥 + COP 约束	Atlas, Digit

在 WBC（Ch53）和 MPC（Ch55）中，接触 wrench 通过接触 Jacobian $J_c$ 映射到广义力空间：

\[\boldsymbol{\tau}_{\text{contact}} = J_c^T \boldsymbol{\lambda}\]

其中 $\boldsymbol{\lambda}$ 是所有接触点的力堆叠。对于 Go2（4 条腿，每腿 1 个点接触），$\boldsymbol{\lambda} \in \mathbb{R}^{12}$。

Wrench 的螺旋解释¶

与 twist 的螺旋解释对偶，一个 wrench 也可以解释为沿一条螺旋轴的力-力矩组合：

力轴方向：$\hat{\boldsymbol{f}} = \boldsymbol{f} / \|\boldsymbol{f}\|$
力轴位置：轴经过的点 $\boldsymbol{q} = \frac{\boldsymbol{f} \times \boldsymbol{\tau}}{\|\boldsymbol{f}\|^2}$
力的大小：$\|\boldsymbol{f}\|$
力矩螺距：$h_F = \frac{\boldsymbol{f}^T \boldsymbol{\tau}}{\|\boldsymbol{f}\|^2}$

当 $h_F = 0$ 时，wrench 退化为纯力（通过力轴的力，无附加力矩）。当 $h_F \neq 0$ 时，除了力之外还有一个沿力轴方向的"耦合力矩"（类似拧螺丝时的力和扭矩）。

功率的螺旋解释：当一个 wrench 作用在一个以 twist 运动的刚体上时，功率 $P = \mathbf{F}^T \mathbf{V}$ 等于力沿速度方向的分量乘以速度大小，加上力矩绕角速度轴的分量乘以角速度大小。这两项分别是 $\boldsymbol{f}^T \boldsymbol{v}$（平动功率）和 $\boldsymbol{\tau}^T \boldsymbol{\omega}$（转动功率）。

练习 49.4.1：一个质量为 12 kg 的 Go2 机器人站在水平地面上，四只脚均匀承重。每只脚的接触力是多少？用 wrench 的形式写出一只脚在脚坐标系中的接触力（假设点接触）。

练习 49.4.2：推导——一个点接触的 wrench 在接触点坐标系中力矩为零（$\boldsymbol{\tau}_c = 0$）。将这个 wrench 变换到世界坐标系后，力矩分量一般不再为零。写出 ${}^W\boldsymbol{\tau} = \boldsymbol{p} \times (R \boldsymbol{f}_c)$，解释为什么——参考点从接触点移到了世界坐标系原点，力臂 $\boldsymbol{p}$ 产生了附加力矩。

练习 49.4.3：对于面接触的人形机器人，解释 COP（Center of Pressure）约束：$-d_y \leq \tau_x / f_z \leq d_y$，$-d_x \leq \tau_y / f_z \leq d_x$，其中 $d_x, d_y$ 是脚底的半长半宽。为什么 COP 必须在支撑多边形内？

49.5 6×6 空间惯量矩阵 ⭐⭐⭐¶

动机：把牛顿-欧拉方程写成 6D 形式¶

经典的牛顿-欧拉方程分两部分：

牛顿方程（平动）：

\[\boldsymbol{f} = m \boldsymbol{a}_c\]

其中 $m$ 是质量，$\boldsymbol{a}_c$ 是质心加速度。

欧拉方程（转动，关于质心）：

\[\boldsymbol{\tau}_c = I_c \dot{\boldsymbol{\omega}} + \boldsymbol{\omega} \times (I_c \boldsymbol{\omega})\]

其中 $I_c \in \mathbb{R}^{3 \times 3}$ 是关于质心的惯量张量。

假如没有空间惯量矩阵的 6D 统一表示，每次写动力学方程都要分别处理平动（牛顿）和转动（欧拉），然后手动添加质心偏移带来的耦合项——对于 12 个连杆的四足机器人，这意味着 24 个耦合方程，人工推导几乎不可能不出错。空间惯量矩阵的意义正在于此：把耦合项编码进矩阵结构，让计算机自动处理。

我们的目标是把这两个方程统一为一个 6D 方程：

\[\mathbf{F} = \hat{I} \dot{\mathbf{V}} + \mathbf{V} \times^* (\hat{I} \mathbf{V})\]

其中 $\hat{I} \in \mathbb{R}^{6 \times 6}$ 是**空间惯量矩阵**（spatial inertia matrix），$\times^*$ 是空间力叉积（我们稍后定义）。

空间惯量矩阵的完整构造¶

第一步：刚体参数¶

设一个刚体的参数为： - 质量 $m$ - 质心在体坐标系中的位置 $\boldsymbol{c} \in \mathbb{R}^3$ - 关于质心的惯量张量 $I_c \in \mathbb{R}^{3 \times 3}$（在体坐标系中表达）

第二步：推导空间动量¶

6D 空间中，我们需要关于**体坐标系原点** $O$（不一定在质心）的动量。

体坐标系原点为 $O$，质心为 $C$，$\boldsymbol{c} = \overrightarrow{OC}$。刚体的 body twist 为 $\mathbf{V} = (\boldsymbol{\omega}, \boldsymbol{v}_O)^T$。

质心速度：

\[\boldsymbol{v}_C = \boldsymbol{v}_O + \boldsymbol{\omega} \times \boldsymbol{c}\]

线动量：

\[\boldsymbol{p} = m \boldsymbol{v}_C = m \boldsymbol{v}_O + m \boldsymbol{\omega} \times \boldsymbol{c} = m \boldsymbol{v}_O + m [\boldsymbol{c}]_\times^T \boldsymbol{\omega}\]

注意 $\boldsymbol{\omega} \times \boldsymbol{c} = [\boldsymbol{\omega}]_\times \boldsymbol{c} = -[\boldsymbol{c}]_\times \boldsymbol{\omega}$，所以：

\[\boldsymbol{p} = m \boldsymbol{v}_O - m[\boldsymbol{c}]_\times \boldsymbol{\omega}\]

关于 $O$ 的角动量：

角动量关于 $O$ 包含两部分——关于质心的自转角动量，以及质心轨道运动带来的角动量：

\[\boldsymbol{L}_O = I_c \boldsymbol{\omega} + m \boldsymbol{c} \times \boldsymbol{v}_C\]

代入 $\boldsymbol{v}_C = \boldsymbol{v}_O + \boldsymbol{\omega} \times \boldsymbol{c}$：

\[\boldsymbol{L}_O = I_c \boldsymbol{\omega} + m \boldsymbol{c} \times \boldsymbol{v}_O + m \boldsymbol{c} \times (\boldsymbol{\omega} \times \boldsymbol{c})\]

展开最后一项，使用向量三重积 $\boldsymbol{a} \times (\boldsymbol{b} \times \boldsymbol{a}) = (\boldsymbol{a}^T \boldsymbol{a}) \boldsymbol{b} - (\boldsymbol{a}^T \boldsymbol{b}) \boldsymbol{a}$：

\[m \boldsymbol{c} \times (\boldsymbol{\omega} \times \boldsymbol{c}) = m[(\boldsymbol{c}^T \boldsymbol{c})I - \boldsymbol{c}\boldsymbol{c}^T]\boldsymbol{\omega}\]

用反对称矩阵的性质 $[\boldsymbol{c}]_\times^2 = \boldsymbol{c}\boldsymbol{c}^T - (\boldsymbol{c}^T\boldsymbol{c})I$，所以：

\[m \boldsymbol{c} \times (\boldsymbol{\omega} \times \boldsymbol{c}) = -m[\boldsymbol{c}]_\times^2 \boldsymbol{\omega}\]

因此：

\[\boldsymbol{L}_O = (I_c - m[\boldsymbol{c}]_\times^2) \boldsymbol{\omega} + m [\boldsymbol{c}]_\times \boldsymbol{v}_O\]

第三步：组装 6D 矩阵¶

把空间动量 $\mathbf{h} = (\boldsymbol{L}_O, \boldsymbol{p})^T$ 写成 $\hat{I} \mathbf{V}$ 的形式：

\[\underbrace{\begin{pmatrix} \boldsymbol{L}_O \\ \boldsymbol{p} \end{pmatrix}}_{\mathbf{h}} = \underbrace{\begin{pmatrix} I_c - m[\boldsymbol{c}]_\times^2 & m[\boldsymbol{c}]_\times \\ -m[\boldsymbol{c}]_\times & mI \end{pmatrix}}_{\hat{I}} \underbrace{\begin{pmatrix} \boldsymbol{\omega} \\ \boldsymbol{v}_O \end{pmatrix}}_{\mathbf{V}}\]

空间惯量矩阵的显式形式¶

\[\boxed{\hat{I} = \begin{pmatrix} I_c - m[\boldsymbol{c}]_\times^2 & m[\boldsymbol{c}]_\times \\ -m[\boldsymbol{c}]_\times & mI \end{pmatrix} \in \mathbb{R}^{6 \times 6}}\]

验证对称性：

左上角：$(I_c - m[\boldsymbol{c}]_\times^2)^T = I_c^T - m([\boldsymbol{c}]_\times^2)^T = I_c - m[\boldsymbol{c}]_\times^2$（因为 $I_c$ 对称，$[\boldsymbol{c}]_\times^2$ 也对称）

右上角的转置 = 左下角：$(m[\boldsymbol{c}]_\times)^T = -m[\boldsymbol{c}]_\times$，正好等于左下角。

所以 $\hat{I}^T = \hat{I}$。

关键性质总结：

性质	说明
对称	$\hat{I} = \hat{I}^T$
正定	对非零 twist，$\mathbf{V}^T \hat{I} \mathbf{V} = 2T > 0$（$T$ 是动能）
10 个独立参数	$m$（1）+ $\boldsymbol{c}$（3）+ $I_c$（6，对称矩阵上三角）= 10
特殊情况	当 $\boldsymbol{c} = 0$（原点在质心）：$\hat{I} = \operatorname{blkdiag}(I_c, mI)$

⚠️ 常见陷阱：左上角 $I_c - m[\boldsymbol{c}]_\times^2$ 中的减号容易弄错方向。注意 $[\boldsymbol{c}]_\times^2 = \boldsymbol{c}\boldsymbol{c}^T - \|\boldsymbol{c}\|^2 I$ 是一个**负半定**矩阵（特征值 $\leq 0$），所以 $-m[\boldsymbol{c}]_\times^2$ 是**正半定**的。因此 $I_c - m[\boldsymbol{c}]_\times^2 = I_c + m(\|\boldsymbol{c}\|^2 I - \boldsymbol{c}\boldsymbol{c}^T)$，这正是经典的**平行轴定理**——关于原点 $O$ 的惯量 = 关于质心的惯量 + 质心偏移带来的附加项。

空间叉积运算符¶

在 3D 中，欧拉方程有一个 $\boldsymbol{\omega} \times (I_c \boldsymbol{\omega})$ 项——这是因为我们在旋转坐标系中求导，出现了科里奥利/陀螺效应。

在 6D 中，类似的项变成了**空间力叉积** $\mathbf{V} \times^*$。首先定义空间运动叉积矩阵。

对于 twist $\mathbf{V} = (\boldsymbol{\omega}, \boldsymbol{v})^T$，运动叉积矩阵：

\[[\mathbf{V}]_\times = \begin{pmatrix} [\boldsymbol{\omega}]_\times & 0 \\ [\boldsymbol{v}]_\times & [\boldsymbol{\omega}]_\times \end{pmatrix} \in \mathbb{R}^{6 \times 6}\]

这个定义来自于李括号（Lie bracket）：对于 $\mathfrak{se}(3)$ 中的两个元素 $\xi_1, \xi_2$，李括号 $[\xi_1, \xi_2] = \xi_1 \xi_2 - \xi_2 \xi_1$ 对应的 6D 向量运算就是空间叉积 $\mathbf{V}_1 \times \mathbf{V}_2$。

**力叉积矩阵**是运动叉积的对偶：

\[[\mathbf{V}]_{\times^*} = -[\mathbf{V}]_\times^T = \begin{pmatrix} [\boldsymbol{\omega}]_\times & [\boldsymbol{v}]_\times \\ 0 & [\boldsymbol{\omega}]_\times \end{pmatrix}\]

验证反对称性：$-[\mathbf{V}]_\times^T$ 的左上角 $= -[\boldsymbol{\omega}]_\times^T = [\boldsymbol{\omega}]_\times$（因为反对称矩阵的转置等于负矩阵），右上角 $= -[\boldsymbol{v}]_\times^T = [\boldsymbol{v}]_\times$，左下角 $= -0^T = 0$，右下角 $= -[\boldsymbol{\omega}]_\times^T = [\boldsymbol{\omega}]_\times$。

两种叉积的**对偶关系**：

\[(\mathbf{V}_1 \times \mathbf{V}_2)^T \mathbf{F} = -\mathbf{V}_2^T (\mathbf{V}_1 \times^* \mathbf{F})\]

这保证了功率的一致性——坐标变换不改变功率。

6D 牛顿-欧拉方程¶

最终，单个刚体的动力学方程在空间向量代数中写成：

\[\boxed{\mathbf{F} = \hat{I} \dot{\mathbf{V}} + \mathbf{V} \times^* (\hat{I} \mathbf{V})}\]

这一个方程同时包含了牛顿方程（平动）和欧拉方程（转动）。

验证（取 $\boldsymbol{c} = 0$，即原点在质心时）：

此时 $\hat{I} = \operatorname{blkdiag}(I_c, mI)$，twist $\mathbf{V} = (\boldsymbol{\omega}, \boldsymbol{v}_c)^T$。

\[\hat{I}\mathbf{V} = \begin{pmatrix} I_c \boldsymbol{\omega} \\ m\boldsymbol{v}_c \end{pmatrix}\]

\[\mathbf{V} \times^* (\hat{I}\mathbf{V}) = \begin{pmatrix} [\boldsymbol{\omega}]_\times & [\boldsymbol{v}_c]_\times \\ 0 & [\boldsymbol{\omega}]_\times \end{pmatrix} \begin{pmatrix} I_c \boldsymbol{\omega} \\ m\boldsymbol{v}_c \end{pmatrix} = \begin{pmatrix} \boldsymbol{\omega} \times (I_c \boldsymbol{\omega}) + \boldsymbol{v}_c \times (m\boldsymbol{v}_c) \\ \boldsymbol{\omega} \times (m\boldsymbol{v}_c) \end{pmatrix}\]

注意 $\boldsymbol{v}_c \times (m\boldsymbol{v}_c) = m(\boldsymbol{v}_c \times \boldsymbol{v}_c) = 0$，所以：

角动量方程（前 3 行）：$\boldsymbol{\tau}_c = I_c \dot{\boldsymbol{\omega}} + \boldsymbol{\omega} \times (I_c \boldsymbol{\omega})$——正是经典欧拉方程。

线动量方程（后 3 行）：$\boldsymbol{f} = m\dot{\boldsymbol{v}}_c + \boldsymbol{\omega} \times (m\boldsymbol{v}_c)$——第二项 $\boldsymbol{\omega} \times (m\boldsymbol{v}_c)$ 是旋转参考系中的科里奥利效应。在世界坐标系（非旋转）中表达时，$\boldsymbol{f} = m\boldsymbol{a}_c$——正是牛顿第二定律。

平行轴定理的 6D 形式¶

经典的平行轴定理：如果知道关于质心的惯量 $I_c$，那么关于另一个点 $O$（距质心 $\boldsymbol{d}$）的惯量为：

\[I_O = I_c + m(\|\boldsymbol{d}\|^2 I - \boldsymbol{d}\boldsymbol{d}^T)\]

在 6D 中，平行轴定理变成空间惯量的坐标变换。如果 $T_{AB}$ 是从坐标系 $B$ 到 $A$ 的变换：

\[{}^A\hat{I} = {}^A\!X_B^{*} \cdot {}^B\hat{I} \cdot {}^A\!X_B^{-1}\]

展开写就是：

\[{}^A\hat{I} = {}^A\!X_B^{-T} \cdot {}^B\hat{I} \cdot {}^A\!X_B^{-1}\]

这是一个**合同变换**（congruence transformation），保持对称性和正定性——与 3D 中惯量张量的变换规则 $I' = R I R^T$ 完全类比。

复合惯量（Composite Inertia）¶

这是 CRBA 算法的基础概念。对于两个刚体 $A$ 和 $B$，如果它们被刚性连接（看作一个复合刚体），复合刚体的空间惯量等于两个空间惯量在同一坐标系下的**直接相加**：

\[\hat{I}_{\text{composite}}^A = \hat{I}_A + {}^A\!X_B^{-T} \hat{I}_B \, {}^A\!X_B^{-1}\]

先把 $B$ 的惯量变换到 $A$ 的坐标系，然后加法。在 3D 中，合并两个刚体的惯量需要分别处理质量、质心位置、惯量张量——三步繁琐操作。在 6D 中，就是一次坐标变换加一次矩阵加法。

这就是空间向量代数的优雅之处——复杂的物理组合操作变成了简单的线性代数。

Pinocchio 中的空间惯量¶

import pinocchio as pin
import numpy as np

model = pin.buildModelFromUrdf("go2.urdf", pin.JointModelFreeFlyer())
data = model.createData()

# 查看某个连杆的空间惯量
link_id = 3  # 某个连杆
inertia = model.inertias[link_id]

print(f"Mass: {inertia.mass}")
print(f"CoM (lever): {inertia.lever}")  # 质心在体坐标系中的位置 c
print(f"Rotational inertia (at CoM):\n{inertia.inertia}")  # 3x3 I_c

# 获取完整的 6x6 空间惯量矩阵
I_spatial = inertia.matrix()  # 返回 6x6 numpy array
print(f"Spatial inertia (6x6):\n{I_spatial}")

# 验证对称性和正定性
assert np.allclose(I_spatial, I_spatial.T), "空间惯量应对称"
eigenvalues = np.linalg.eigvalsh(I_spatial)
assert np.all(eigenvalues > 0), "空间惯量应正定"
print(f"Eigenvalues: {eigenvalues}")

# 手动构造并验证
m = inertia.mass
c = inertia.lever
Ic = inertia.inertia
cx = pin.skew(c)  # [c]_x, 3x3 反对称矩阵

I_manual = np.zeros((6, 6))
I_manual[:3, :3] = Ic - m * cx @ cx      # 左上
I_manual[:3, 3:] = m * cx                 # 右上
I_manual[3:, :3] = -m * cx               # 左下
I_manual[3:, 3:] = m * np.eye(3)         # 右下

assert np.allclose(I_spatial, I_manual), "手动构造应与 Pinocchio 一致"
print("Manual construction matches Pinocchio!")

练习 49.5.1：手算——一个质量 $m = 2$ kg、质心在 $\boldsymbol{c} = (0, 0, 0.1)^T$ m、惯量张量 $I_c = \operatorname{diag}(0.01, 0.01, 0.005)$ kg$\cdot$m$^2$ 的连杆，写出其 $6 \times 6$ 空间惯量矩阵 $\hat{I}$。验证它是对称正定的。

练习 49.5.2：验证——用 Pinocchio 加载 Go2 模型，取出基座（body 0）的空间惯量矩阵，与手动用 $(m, \boldsymbol{c}, I_c)$ 构造的结果进行逐元素比较。

练习 49.5.3：推导——证明当体坐标系原点在质心（$\boldsymbol{c} = 0$）时，空间惯量矩阵变为块对角矩阵 $\hat{I} = \operatorname{blkdiag}(I_c, mI)$。这意味着在质心坐标系中，转动惯量和平动惯量是**解耦的**。这是许多算法选择质心作为参考点的原因。

49.6 Featherstone RNEA（递归牛顿-欧拉算法）⭐⭐⭐¶

动机：从 $O(N^3)$ 到 $O(N)$¶

在 Lagrange 方法中，要计算给定 $(q, \dot{q}, \ddot{q})$ 对应的关节扭矩 $\tau$（即**逆动力学**），需要：

构造质量矩阵 $M(q)$——$O(N^2)$
构造科氏/离心力项 $C(q,\dot{q})\dot{q}$——$O(N^2)$
计算 $\tau = M\ddot{q} + C\dot{q} + g$——$O(N^2)$ 矩阵-向量乘法

总复杂度 $O(N^2)$（如果用解析方法甚至 $O(N^3)$）。

Featherstone 的 RNEA（Recursive Newton-Euler Algorithm）用完全不同的思路：沿运动树递归，逐连杆计算。不需要显式构造 $M(q)$ 矩阵。总复杂度 $O(N)$。

对于 Go2（$N = 18$ 自由度），RNEA 比 Lagrange 方法快约 $18$ 倍。对于人形机器人（$N = 30+$），差距更大。

算法结构：两遍递归¶

RNEA 由两遍（pass）组成：

Pass 1: Forward (从基座到末端)
  ──►  逐连杆向外传递 twist 和 spatial acceleration

Pass 2: Backward (从末端到基座)
  ◄──  逐连杆向内累积 wrench（力/力矩）

树形结构记号： - 连杆 $i$ 的父连杆：$\lambda(i)$ - 连杆 $i$ 的关节轴（在连杆 $i$ 的坐标系中）：$\mathbf{S}_i \in \mathbb{R}^6$（unit motion vector） - 连杆 $i$ 相对于父连杆的 Plucker 变换：${}^i\!X_{\lambda(i)}$ - 关节 $i$ 的速度和加速度：$\dot{q}_i, \ddot{q}_i$

完整推导¶

Pass 1：正向传递——计算每个连杆的 twist 和 spatial acceleration¶

**连杆 $i$ 的 body twist**由两部分组成：从父连杆传递过来的运动，加上关节 $i$ 本身的运动。

\[\mathbf{V}_i = {}^i\!X_{\lambda(i)} \mathbf{V}_{\lambda(i)} + \mathbf{S}_i \dot{q}_i\]

第一项 ${}^i\!X_{\lambda(i)} \mathbf{V}_{\lambda(i)}$ 是把父连杆的 twist 变换到连杆 $i$ 的坐标系——如果关节 $i$ 不动（$\dot{q}_i = 0$），连杆 $i$ 和父连杆刚性连接，twist 就等于这一项。第二项 $\mathbf{S}_i \dot{q}_i$ 是关节 $i$ 贡献的附加运动。

对于旋转关节（绕 z 轴），$\mathbf{S}_i = (0, 0, 1, 0, 0, 0)^T$；对于棱柱关节（沿 z 轴），$\mathbf{S}_i = (0, 0, 0, 0, 0, 1)^T$。

连杆 $i$ 的 spatial acceleration——对 twist 求时间导数：

\[\dot{\mathbf{V}}_i = {}^i\!X_{\lambda(i)} \dot{\mathbf{V}}_{\lambda(i)} + \mathbf{S}_i \ddot{q}_i + \mathbf{V}_i \times \mathbf{S}_i \dot{q}_i\]

最后一项 $\mathbf{V}_i \times \mathbf{S}_i \dot{q}_i$ 是**科里奥利加速度**——连杆 $i$ 的整体旋转与关节 $i$ 的局部运动之间的耦合效应。这一项在 3D 方法中需要手动处理；在 6D 空间向量代数中，它自然地以空间叉积的形式出现。

Pass 2：反向传递——累积 wrench¶

每个连杆 $i$ 上作用的净 wrench（由 6D 牛顿-欧拉方程给出）：

\[\mathbf{F}_i = \hat{I}_i \dot{\mathbf{V}}_i + \mathbf{V}_i \times^* (\hat{I}_i \mathbf{V}_i)\]

但连杆 $i$ 上的 wrench 还要加上所有子连杆传递过来的反作用力（牛顿第三定律）：

\[\mathbf{F}_i \leftarrow \mathbf{F}_i + \sum_{j : \lambda(j)=i} {}^i\!X_j^* \mathbf{F}_j\]

最终，关节 $i$ 的扭矩等于净 wrench 在关节轴方向的投影：

\[\tau_i = \mathbf{S}_i^T \mathbf{F}_i\]

完整伪代码¶

Algorithm: RNEA(model, q, q_dot, q_ddot)
─────────────────────────────────────────
Input:  model (robot), q, q_dot, q_ddot
Output: tau (joint torques)

# 初始化：基座（连杆 0）
V[0] = 0  (固定基座) 或 V_base (浮动基座)
a[0] = -g  (重力加速度，取负号因为等效于基座在"向上"加速)

# Pass 1: Forward (i = 1, 2, ..., N)
for i = 1 to N:
    # Plucker 变换：父坐标系 → 连杆 i 坐标系
    X[i] = X_J(q[i]) * X_tree[i]    # 关节变换 × 树结构变换

    # Twist 传递
    V[i] = X[i] * V[parent[i]] + S[i] * q_dot[i]

    # Spatial acceleration 传递
    a[i] = X[i] * a[parent[i]] + S[i] * q_ddot[i] + V[i] ×  (S[i] * q_dot[i])
    #                                                  ↑ Coriolis term

# Pass 2: Backward (i = N, N-1, ..., 1)
for i = N downto 1:
    # 连杆 i 的净 wrench (6D Newton-Euler)
    F[i] = I_hat[i] * a[i] + V[i] ×* (I_hat[i] * V[i])

    # 关节扭矩 = wrench 在关节轴上的投影
    tau[i] = S[i]^T * F[i]

    # 把 wrench 传递给父连杆（牛顿第三定律）
    F[parent[i]] += X[i]^* * F[i]

return tau

固定基座 vs 浮动基座的处理差异¶

方面	固定基座	浮动基座
$\mathbf{V}_0$	$\mathbf{0}$（基座不动）	基座的 body twist
$\dot{\mathbf{V}}_0$	$-{}^0\mathbf{g}$（只有重力）	基座加速度 + $-{}^0\mathbf{g}$
输出维度	$\tau \in \mathbb{R}^n$	$\tau \in \mathbb{R}^{6+n}$（前 6 维是基座虚拟力）
基座虚拟力	不存在	$\mathbf{F}_0$：维持基座运动所需的假想力

对于浮动基座，RNEA 输出的前 6 维是"维持基座按给定加速度运动所需的外力"。如果机器人是自由飘浮的（如太空中），这 6 维应为零——这就是动力学方程中基座行对应的等式。

⚠️ 常见陷阱：在浮动基座 RNEA 中，重力的处理很微妙。不是给每个连杆加 $m\boldsymbol{g}$，而是设置基座的"虚拟加速度" $\dot{\mathbf{V}}_0 = -\mathbf{g}_{spatial}$（空间坐标系中向上的加速度）。这利用了等效原理：所有连杆受重力 = 基座在"向上"加速。这个技巧避免了在每个连杆上单独加重力项。

Pinocchio RNEA API¶

import pinocchio as pin
import numpy as np

model = pin.buildModelFromUrdf("go2.urdf", pin.JointModelFreeFlyer())
data = model.createData()

q = pin.neutral(model)
v = np.zeros(model.nv)
a = np.zeros(model.nv)  # 零加速度

# 逆动力学: 给定 (q, v, a)，计算 tau
tau = pin.rnea(model, data, q, v, a)
print(f"tau shape: {tau.shape}")  # (18,) = 6 (base) + 12 (joints)
print(f"tau[:6] (base wrench): {tau[:6]}")  # 应约等于重力 wrench
print(f"tau[6:] (joint torques): {tau[6:]}")  # 关节抗重力扭矩

# 验证：当 v=0, a=0 时，RNEA 输出的就是重力项 g(q)
g = pin.computeGeneralizedGravity(model, data, q)
assert np.allclose(tau, g), "v=0, a=0 时 RNEA = g(q)"
print("Verified: rnea(q, 0, 0) = g(q)")

# 计算非线性项 h(q, v) = C(q,v)v + g(q)
# 方法：设 a=0，RNEA 给出 h = M*0 + C*v + g = C*v + g
v_nonzero = np.random.randn(model.nv) * 0.1
h = pin.rnea(model, data, q, v_nonzero, np.zeros(model.nv))
print(f"Nonlinear term h(q,v): {h[:6]}")

# RNEA 完整调用：tau = M*a + h
a_nonzero = np.random.randn(model.nv) * 0.1
tau_full = pin.rnea(model, data, q, v_nonzero, a_nonzero)

RNEA 的计算复杂度分析：

操作	每连杆	总计 ($N$ 连杆)
Plucker 变换 ${}^i\!X_{\lambda(i)}$	$O(1)$（利用稀疏结构）	$O(N)$
Twist 传递	$6 \times 6$ 矩阵-向量乘	$O(N)$
空间叉积	$6 \times 6$ 矩阵-向量乘	$O(N)$
Wrench 计算	$6 \times 6$ 矩阵-向量乘	$O(N)$
Wrench 反向传递	$6 \times 6$ 矩阵-向量乘	$O(N)$
总计		$O(N)$

在 Pinocchio 中，Go2 的 RNEA 耗时约 2-5 微秒（单次调用），远快于构造 $M(q)$ 矩阵。

练习 49.6.1：用 Pinocchio 验证 RNEA 的一致性：设随机的 $q, \dot{q}, \ddot{q}$，分别计算 $\tau_{\text{rnea}} = \texttt{rnea}(q, \dot{q}, \ddot{q})$ 和 $\tau_{\text{matrix}} = M(q)\ddot{q} + h(q,\dot{q})$（其中 $M$ 由 CRBA 得到，$h$ 由 RNEA 在 $\ddot{q}=0$ 时得到），验证两者相等。

练习 49.6.2：解释为什么 RNEA 中重力不是作为外力加到每个连杆上，而是通过设置 $\dot{\mathbf{V}}_0 = -\mathbf{g}_{spatial}$ 来处理。手动推导这两种方式的等价性。

练习 49.6.3：对于 Go2，运行 RNEA 在 $v=0, a=0$ 状态下，检查输出的前 6 维（基座 wrench）。它应该等于整个机器人的总重力——验证 $\tau_0 = (0, 0, 0, 0, 0, -mg)^T$（当基座坐标系与世界坐标系对齐时）。

49.7 Featherstone ABA（Articulated Body Algorithm）⭐⭐⭐¶

动机：正向动力学——从力到加速度¶

RNEA 解决的是**逆动力学**（给定运动求力）。但在仿真和某些控制场景中，我们需要**正向动力学**（给定力求加速度）：

\[\ddot{q} = M(q)^{-1}(\tau - h(q,\dot{q}))\]

朴素方法：先用 CRBA 算 $M(q)$（$O(N^2)$），再求逆或做 Cholesky 分解（$O(N^3)$）。

Featherstone 的 ABA 算法直接计算 $\ddot{q}$，复杂度只有 $O(N)$。它避免了显式构造和求逆 $M$ 矩阵。

核心概念：Articulated Body Inertia¶

ABA 的关键洞察是引入**铰接体惯量**（Articulated Body Inertia）$\hat{I}_i^A$——连杆 $i$ 及其所有子树在关节 $i$ 处"看到"的等效惯量。

想象你站在关节 $i$ 处，给这个关节施加一个单位扭矩。连杆 $i$ 及其所有子连杆会一起加速——但不同连杆的加速度不同（因为中间有活动关节）。$\hat{I}_i^A$ 就是描述这种"带关节的复合体"对力的响应的等效惯量。

与"复合刚体惯量" $\hat{I}_i^C$（CRBA 中用的）的区别：

概念	假设	用途
复合刚体惯量 $\hat{I}_i^C$	子树关节锁死（rigid）	CRBA：计算 $M(q)$
铰接体惯量 $\hat{I}_i^A$	子树关节自由（free）	ABA：计算 $\ddot{q}$

三遍递归结构¶

ABA 比 RNEA 多一遍——共三遍：

Pass 1: Forward (基座 → 末端)
  ──►  计算 twist、spatial acceleration bias

Pass 2: Backward (末端 → 基座)
  ◄──  递推计算 articulated body inertia + bias force

Pass 3: Forward (基座 → 末端)
  ──►  计算关节加速度 q_ddot 和连杆 spatial acceleration

完整推导¶

Pass 1：正向传递¶

与 RNEA Pass 1 类似，计算每个连杆的 twist：

\[\mathbf{V}_i = {}^i\!X_{\lambda(i)} \mathbf{V}_{\lambda(i)} + \mathbf{S}_i \dot{q}_i\]

以及**偏置加速度**（不含 $\ddot{q}$ 项的加速度分量）：

\[\mathbf{c}_i = \mathbf{V}_i \times \mathbf{S}_i \dot{q}_i\]

这是科里奥利加速度项，只依赖于 $(q, \dot{q})$。

Pass 2：反向传递——构建铰接体惯量¶

从叶子连杆开始，叶子连杆没有子连杆，其铰接体惯量就是自身的空间惯量：

\[\hat{I}_i^A = \hat{I}_i \quad \text{(叶子连杆)}\]

对于有子连杆的连杆 $i$，铰接体惯量通过递推计算：

\[\hat{I}_i^A = \hat{I}_i + \sum_{j : \lambda(j) = i} {}^i\!X_j^{*} \left[ \hat{I}_j^A - \frac{\hat{I}_j^A \mathbf{S}_j \mathbf{S}_j^T \hat{I}_j^A}{\mathbf{S}_j^T \hat{I}_j^A \mathbf{S}_j} \right] {}^i\!X_j^{-1}\]

中间那个减法项 $\frac{\hat{I}_j^A \mathbf{S}_j \mathbf{S}_j^T \hat{I}_j^A}{\mathbf{S}_j^T \hat{I}_j^A \mathbf{S}_j}$ 是 Schur 补——它"减掉"了关节 $j$ 自由运动所吸收的惯量。

简化记号。定义：

\[U_i = \hat{I}_i^A \mathbf{S}_i, \quad D_i = \mathbf{S}_i^T U_i, \quad u_i = \tau_i - \mathbf{S}_i^T \mathbf{p}_i^A\]

其中 $\mathbf{p}_i^A$ 是偏置力（bias force）：

\[\mathbf{p}_i^A = \mathbf{V}_i \times^* (\hat{I}_i \mathbf{V}_i) - \hat{I}_i^A (\mathbf{V}_i \times \mathbf{S}_i \dot{q}_i) + \sum_{j:\lambda(j)=i} {}^i\!X_j^* \mathbf{p}_j^A\]

初始时（叶子连杆）：$\mathbf{p}_i^A = \mathbf{V}_i \times^* (\hat{I}_i \mathbf{V}_i)$（即陀螺效应）。

铰接体惯量的递推更新为：

\[\hat{I}_{\lambda(i)}^A \mathrel{+}= {}^{\lambda(i)}\!X_i^{*} \left(\hat{I}_i^A - \frac{U_i U_i^T}{D_i}\right) {}^{\lambda(i)}\!X_i^{-1}\]

\[\mathbf{p}_{\lambda(i)}^A \mathrel{+}= {}^{\lambda(i)}\!X_i^{*} \left(\mathbf{p}_i^A + \hat{I}_i^A \mathbf{c}_i + \frac{U_i u_i}{D_i}\right)\]

Pass 3：正向传递——计算加速度¶

从基座开始：

\[\ddot{q}_i = \frac{u_i - U_i^T \mathbf{a}_{\lambda(i)}}{D_i}\]

\[\mathbf{a}_i = {}^i\!X_{\lambda(i)} \mathbf{a}_{\lambda(i)} + \mathbf{S}_i \ddot{q}_i + \mathbf{c}_i\]

其中基座加速度 $\mathbf{a}_0 = -\mathbf{g}_{spatial}$（固定基座）或由铰接体惯量方程求解（浮动基座）。

ABA 伪代码¶

Algorithm: ABA(model, q, q_dot, tau)
─────────────────────────────────────
Input:  model, q, q_dot, tau
Output: q_ddot (joint accelerations)

# Pass 1: Forward
for i = 1 to N:
    V[i] = X[i] * V[parent[i]] + S[i] * q_dot[i]
    c[i] = V[i] × (S[i] * q_dot[i])
    pA[i] = V[i] ×* (I_hat[i] * V[i])   # gyroscopic bias
    IA[i] = I_hat[i]                       # init articulated inertia

# Pass 2: Backward
for i = N downto 1:
    U[i] = IA[i] * S[i]
    D[i] = S[i]^T * U[i]          # scalar for 1-DOF joint
    u[i] = tau[i] - S[i]^T * pA[i]

    # Propagate to parent (Schur complement update)
    Ia = IA[i] - U[i] * U[i]^T / D[i]
    pa = pA[i] + Ia * c[i] + U[i] * u[i] / D[i]

    IA[parent[i]] += X[i]^* * Ia * X[i]^{-1}
    pA[parent[i]] += X[i]^* * pa

# Pass 3: Forward
a[0] = -g_spatial   # 基座 (固定基座)
for i = 1 to N:
    a[i] = X[i] * a[parent[i]] + c[i]
    q_ddot[i] = (u[i] - U[i]^T * a[i]) / D[i]
    a[i] += S[i] * q_ddot[i]

return q_ddot

Pinocchio ABA API¶

import pinocchio as pin
import numpy as np

model = pin.buildModelFromUrdf("go2.urdf", pin.JointModelFreeFlyer())
data = model.createData()

q = pin.neutral(model)
v = np.random.randn(model.nv) * 0.1
tau = np.zeros(model.nv)  # 零输入扭矩

# 正向动力学: 给定 (q, v, tau)，计算 q_ddot
q_ddot = pin.aba(model, data, q, v, tau)
print(f"q_ddot shape: {q_ddot.shape}")  # (18,)
print(f"Base acceleration: {q_ddot[:6]}")
print(f"Joint accelerations: {q_ddot[6:]}")

# 验证 ABA 与 M^{-1}(tau - h) 的一致性
pin.crba(model, data, q)  # 计算 M
M = data.M.copy()
M = M + M.T - np.diag(M.diagonal())  # CRBA 只填上三角，需要对称化

h = pin.rnea(model, data, q, v, np.zeros(model.nv))  # h = C*v + g
q_ddot_check = np.linalg.solve(M, tau - h)  # M^{-1} (tau - h)

print(f"ABA vs M^{{-1}}(tau-h) error: {np.max(np.abs(q_ddot - q_ddot_check)):.2e}")
# 应该接近机器精度 ~1e-12

⚠️ 常见陷阱：Pinocchio 的 crba 函数只填充 data.M 的**上三角**部分。使用前必须手动对称化：M = data.M + data.M.T - np.diag(data.M.diagonal())。或者使用 data.M 直接与 np.linalg.solve 配合（它内部会处理对称性），但打印或做逐元素比较时需要先对称化。

练习 49.7.1：用 Pinocchio 计算 Go2 在 $\tau = 0$（自由落体 + 关节无扭矩）时的 $\ddot{q}$。验证基座加速度的线性部分约等于 $-g = (0, 0, -9.81)$ m/s$^2$。

练习 49.7.2：比较 ABA 和"先 CRBA 再 solve"的计算时间。用 %timeit 测量 100 次调用的平均耗时，画出对比图。对于 Go2（18 自由度），差异有多大？

49.8 CRBA（Composite Rigid Body Algorithm）⭐⭐¶

动机：显式构造质量矩阵¶

虽然 RNEA 和 ABA 可以避免显式构造质量矩阵 $M(q)$，但在很多控制算法中（特别是 WBC 和基于 QP 的控制器），我们**确实需要** $M(q)$ 矩阵本身。

CRBA 是构造 $M(q)$ 最高效的算法，复杂度 $O(N^2)$（因为 $M$ 本身有 $O(N^2)$ 个元素）。

算法思想¶

CRBA 利用了**复合刚体惯量**的概念：假设从连杆 $i$ 往后的所有关节都锁死，那么连杆 $i$ 及其子树形成一个复合刚体，其空间惯量为 $\hat{I}_i^C$。

质量矩阵的第 $(i, j)$ 元素可以表示为：

\[M_{ij} = \mathbf{S}_i^T \hat{I}_i^C \mathbf{S}_j \quad \text{(当 $i$ 是 $j$ 的祖先或 $i = j$ 时)}\]

完整推导¶

从动能出发。系统的总动能为：

\[T = \frac{1}{2} \dot{q}^T M(q) \dot{q} = \frac{1}{2} \sum_{i=1}^{N} \mathbf{V}_i^T \hat{I}_i \mathbf{V}_i\]

将每个连杆的 twist 展开为关节速度的函数：

\[\mathbf{V}_i = \sum_{j \in \text{path}(0, i)} {}^i\!X_j \mathbf{S}_j \dot{q}_j = J_i(q) \dot{q}\]

其中求和遍历从基座到连杆 $i$ 路径上的所有关节。代入动能表达式并整理，可得：

\[M(q) = \sum_{i=1}^{N} J_i^T \hat{I}_i J_i\]

CRBA 的巧妙之处在于不显式构造 $J_i$，而是通过递归累积复合惯量。

CRBA 伪代码¶

Algorithm: CRBA(model, q)
─────────────────────────
Input:  model, q
Output: M (mass matrix, upper triangle)

# 初始化：每个连杆的复合惯量 = 自身惯量
for i = 1 to N:
    IC[i] = I_hat[i]

# Backward pass: 累积复合惯量
for i = N downto 1:
    # 对角元素
    M[i, i] = S[i]^T * IC[i] * S[i]

    # 把复合惯量传递给父连杆
    IC[parent[i]] += X[i]^* * IC[i] * X[i]^{-1}

    # 非对角元素：沿路径向上传递
    F = IC[i] * S[i]       # 6D wrench
    j = i
    while parent[j] != 0:  # 沿树向上
        F = X[j]^* * F     # 变换到父坐标系
        j = parent[j]
        M[j, i] = S[j]^T * F   # off-diagonal element
        M[i, j] = M[j, i]      # 对称

return M

质量矩阵的结构与稀疏性¶

对于 Go2（FreeFlyer + 12 关节），$M \in \mathbb{R}^{18 \times 18}$：

          base(6)   FL(3)    FR(3)    RL(3)    RR(3)
base(6)  [  ████    ███      ███      ███      ███  ]
FL(3)    [  ███     ███       0        0        0   ]
FR(3)    [  ███      0       ███       0        0   ]
RL(3)    [  ███      0        0       ███       0   ]
RR(3)    [  ███      0        0        0       ███  ]

关键观察： - 基座行/列是**稠密**的——基座与所有连杆都有惯性耦合 - 不同腿之间是**稀疏**的——FL 和 RR 之间没有直接的惯性耦合（只通过基座间接耦合） - 每条腿内部是 $3 \times 3$ 稠密块

Pinocchio CRBA API¶

import pinocchio as pin
import numpy as np

model = pin.buildModelFromUrdf("go2.urdf", pin.JointModelFreeFlyer())
data = model.createData()

q = pin.neutral(model)

# 计算质量矩阵
pin.crba(model, data, q)
M = data.M.copy()

# CRBA 只填上三角，手动对称化
M_full = M + M.T - np.diag(M.diagonal())

print(f"M shape: {M_full.shape}")  # (18, 18)
print(f"M is symmetric: {np.allclose(M_full, M_full.T)}")
print(f"M is positive definite: {np.all(np.linalg.eigvalsh(M_full) > 0)}")

# 可视化稀疏结构
import matplotlib.pyplot as plt
plt.figure(figsize=(8, 8))
plt.imshow(np.abs(M_full) > 1e-6, cmap='binary')
plt.title("Go2 Mass Matrix Sparsity Pattern")
plt.xlabel("Generalized coordinate index")
plt.ylabel("Generalized coordinate index")
plt.colorbar(label="|M_ij| > 1e-6")
plt.show()

练习 49.8.1：用 Pinocchio 计算 Go2 在零位形下的 $M(q)$，打印左上角 $6 \times 6$ 块（基座惯量块）。解释为什么它不是对角的——基座的惯量与整个机器人的质心位置有关。

练习 49.8.2：验证 $M(q)$ 的维度。对于 Go2，$M$ 应该是 $18 \times 18$。改用固定基座加载同一个 URDF（不加 FreeFlyer），$M$ 变成多少维？

49.9 浮动基座全身动力学方程 ⭐⭐⭐¶

完整形式¶

一个浮动基座机器人的动力学方程为：

\[\boxed{M(q)\ddot{q} + h(q, \dot{q}) = S^T \tau + J_c^T \lambda}\]

其中： - $q \in \mathcal{Q}$：广义坐标（nq 维，含基座位姿 + 关节角） - $\dot{q} \in \mathbb{R}^{n_v}$：广义速度（nv 维，$n_v = 6 + n$） - $M(q) \in \mathbb{R}^{n_v \times n_v}$：广义质量矩阵（由 CRBA 计算） - $h(q, \dot{q}) \in \mathbb{R}^{n_v}$：非线性项（科氏力 + 离心力 + 重力，由 RNEA 在 $\ddot{q}=0$ 时计算） - $S \in \mathbb{R}^{n \times n_v}$：选择矩阵（Selection matrix） - $\tau \in \mathbb{R}^n$：关节扭矩 - $J_c \in \mathbb{R}^{n_c \times n_v}$：接触 Jacobian - $\lambda \in \mathbb{R}^{n_c}$：接触力 - $n_c$：接触约束的总维度

选择矩阵 $S$ 的详细解释¶

选择矩阵 $S$ 把关节扭矩映射到广义力空间。它的结构很简单：

\[S = \begin{pmatrix} \mathbf{0}_{n \times 6} & I_{n \times n} \end{pmatrix} \in \mathbb{R}^{n \times n_v}\]

即 $S$ 选出广义速度中"关节"的部分，跳过"基座"的部分。

\[S^T \tau = \begin{pmatrix} \mathbf{0}_{6 \times 1} \\ \tau \end{pmatrix} \in \mathbb{R}^{n_v}\]

物理含义：关节电机只能对关节施加扭矩，不能直接对基座施加力。基座的 6 个自由度只能通过接触力 $J_c^T \lambda$ 来间接驱动。

块分解¶

将广义速度分为基座和关节两部分：$\dot{q} = (\dot{q}_b, \dot{q}_j)^T$，动力学方程变为：

\[\begin{pmatrix} M_{bb} & M_{bj} \\ M_{jb} & M_{jj} \end{pmatrix} \begin{pmatrix} \ddot{q}_b \\ \ddot{q}_j \end{pmatrix} + \begin{pmatrix} h_b \\ h_j \end{pmatrix} = \begin{pmatrix} \mathbf{0} \\ \tau \end{pmatrix} + \begin{pmatrix} J_{c,b}^T \\ J_{c,j}^T \end{pmatrix} \lambda\]

基座行（前 6 行）：

\[M_{bb} \ddot{q}_b + M_{bj} \ddot{q}_j + h_b = J_{c,b}^T \lambda\]

这 6 个方程没有关节扭矩 $\tau$——基座运动**完全由接触力决定**。这就是"欠驱动"的数学表达。

关节行（后 $n$ 行）：

\[M_{jb} \ddot{q}_b + M_{jj} \ddot{q}_j + h_j = \tau + J_{c,j}^T \lambda\]

这些方程中 $\tau$ 出现了——关节电机可以直接影响关节加速度。

Go2 的具体维度¶

量	符号	维度	Go2 值
广义坐标	$q$	nq	19 ($= 7 + 12$)
广义速度	$\dot{q}$	nv	18 ($= 6 + 12$)
质量矩阵	$M$	$n_v \times n_v$	$18 \times 18$
非线性项	$h$	$n_v$	18
选择矩阵	$S$	$n \times n_v$	$12 \times 18$
关节扭矩	$\tau$	$n$	12
接触力（4 腿站立）	$\lambda$	$n_c$	12 ($= 4 \times 3$)
接触 Jacobian	$J_c$	$n_c \times n_v$	$12 \times 18$

接触 Jacobian $J_c$¶

接触 Jacobian 将广义速度映射到接触点速度：

\[\boldsymbol{v}_{\text{contact}} = J_c(q) \dot{q}\]

对于 Go2 的 4 个足端（点接触，每个足端 3D 速度），$J_c \in \mathbb{R}^{12 \times 18}$。

在 Pinocchio 中获取接触 Jacobian：

import pinocchio as pin
import numpy as np

model = pin.buildModelFromUrdf("go2.urdf", pin.JointModelFreeFlyer())
data = model.createData()
q = pin.neutral(model)

# 必须先更新运动学
pin.forwardKinematics(model, data, q)
pin.computeJointJacobians(model, data, q)

# 获取某个足端的 Jacobian (6D, in world frame)
foot_frame_id = model.getFrameId("FR_foot")
J_foot = pin.getFrameJacobian(model, data, foot_frame_id, pin.LOCAL_WORLD_ALIGNED)
print(f"FR foot Jacobian shape: {J_foot.shape}")  # (6, 18)

# 对于点接触，只需要线速度部分（前 3 行）
J_foot_linear = J_foot[:3, :]  # (3, 18)

# 堆叠所有 4 个足端的 Jacobian
foot_names = ["FL_foot", "FR_foot", "RL_foot", "RR_foot"]
J_c = np.zeros((12, model.nv))
for i, name in enumerate(foot_names):
    fid = model.getFrameId(name)
    J = pin.getFrameJacobian(model, data, fid, pin.LOCAL_WORLD_ALIGNED)
    J_c[3*i:3*(i+1), :] = J[:3, :]  # 取线速度部分（Pinocchio: 前3行=linear）

print(f"Contact Jacobian J_c shape: {J_c.shape}")  # (12, 18)

⚠️ 常见陷阱：Pinocchio 的 getFrameJacobian 返回的是 $6 \times n_v$ 的完整 Jacobian（包含角速度和线速度部分）。对于**点接触**，只需要线速度部分（前 3 行，即行索引 0:3）。如果错误地使用完整的 6D Jacobian，维度会对不上，QP 求解会失败。另外注意 LOCAL_WORLD_ALIGNED 参考系的选择——它在接触点位置，但轴与世界坐标系对齐，这是 WBC 中最常用的选择。

⚠️ 约定差异：Featherstone 教材用 $[\boldsymbol{\omega};\; \boldsymbol{v}]$（角速度在上），Pinocchio 内部 Motion/Force 的存储顺序为 $[\boldsymbol{v};\; \boldsymbol{\omega}]$（线速度在上）。因此 getFrameJacobian 返回的 $J$ 中 topRows(3)（行索引 0:3）= **线速度**部分，bottomRows(3)（行索引 3:6）= **角速度**部分。上面代码中 J[3:, :] 取的是角速度行——对于点接触应改用 J[:3, :] 取线速度。本章正文公式沿用 Featherstone $(\boldsymbol{\omega}, \boldsymbol{v})$ 约定，与 Pinocchio API 行顺序相反，使用时请注意转换。

验证动力学方程的 Pinocchio 实现¶

import pinocchio as pin
import numpy as np

model = pin.buildModelFromUrdf("go2.urdf", pin.JointModelFreeFlyer())
data = model.createData()

# 随机状态
q = pin.randomConfiguration(model)
v = np.random.randn(model.nv) * 0.1
a = np.random.randn(model.nv) * 0.1
tau_joints = np.random.randn(12) * 1.0  # 12 个关节扭矩

# 方法 1: RNEA 直接计算 M*a + h
tau_rnea = pin.rnea(model, data, q, v, a)

# 方法 2: CRBA + RNEA 分开计算
pin.crba(model, data, q)
M = data.M + data.M.T - np.diag(data.M.diagonal())
h = pin.rnea(model, data, q, v, np.zeros(model.nv))
tau_separate = M @ a + h

# 验证一致性
print(f"RNEA vs M*a+h error: {np.max(np.abs(tau_rnea - tau_separate)):.2e}")
# 应该接近机器精度 ~1e-12

# 构造选择矩阵 S
n_joints = model.nv - 6
S = np.hstack([np.zeros((n_joints, 6)), np.eye(n_joints)])
print(f"S shape: {S.shape}")  # (12, 18)

# S^T tau
generalized_tau = S.T @ tau_joints
print(f"S^T tau shape: {generalized_tau.shape}")  # (18,)
print(f"S^T tau[:6] = {generalized_tau[:6]}")  # 应全为 0

练习 49.9.1：写出 Go2 在静止站立时（$\dot{q} = 0, \ddot{q} = 0$）的动力学方程。它简化为什么形式？解释为什么此时接触力必须平衡重力。

练习 49.9.2：对于 Go2，手动构造选择矩阵 $S$（$12 \times 18$），验证 $S^T \tau$ 的前 6 个元素为零。

练习 49.9.3：解释为什么基座行方程 $M_{bb}\ddot{q}_b + M_{bj}\ddot{q}_j + h_b = J_{c,b}^T\lambda$ 在 WBC 中被称为"动力学约束"（dynamics constraint）——控制器必须找到满足这个等式的 $(\ddot{q}, \tau, \lambda)$ 组合。

49.10 Centroidal Momentum 与 CCRBA ⭐⭐⭐¶

动机：从全身动力学到简化模型的桥梁¶

在 Ch51 中，我们将学习 LIPM（线性倒立摆模型）和 SRBM（单刚体模型）等简化模型。这些模型把整个机器人视为一个"质点"或"刚体"，用质心的运动来规划步态。

但问题是：简化模型和全身动力学之间怎么连接？

答案就是 Centroidal Momentum——系统在质心（Center of Mass, CoM）处的总动量。它是全身动力学的一个"低维投影"，保留了最关键的物理量。

定义¶

Centroidal Momentum 是整个机器人在质心处的 6D 动量：

\[\mathbf{h}_G = \begin{pmatrix} \boldsymbol{k}_G \\ \boldsymbol{l}_G \end{pmatrix} \in \mathbb{R}^6\]

其中： - $\boldsymbol{k}_G \in \mathbb{R}^3$：关于质心的**角动量** - $\boldsymbol{l}_G = m \boldsymbol{v}_G \in \mathbb{R}^3$：线动量（$m$ 是总质量，$\boldsymbol{v}_G$ 是质心速度）

约定提示：本章公式沿用 Featherstone/Orin 的 $(\text{angular}, \text{linear})$ 顺序。Pinocchio 的 computeCentroidalMomentum 返回的 Force 类型内部存储顺序为 $(\text{linear}, \text{angular})$——即 .np[:3] 是线动量，.np[3:] 是角动量。使用时注意对应关系。

Centroidal Momentum Matrix (CMM)¶

CMM $A_G(q)$ 将广义速度线性映射到 centroidal momentum：

\[\boxed{\mathbf{h}_G = A_G(q) \dot{q}}\]

其中 $A_G \in \mathbb{R}^{6 \times n_v}$。

$A_G$ 的每一列表示：当对应的广义速度分量变化 1 单位时，centroidal momentum 变化多少。

CMM 的构造：

\[A_G(q) = \sum_{i=1}^{N} {}^G\hat{I}_i J_i(q)\]

其中 ${}^G\hat{I}_i$ 是连杆 $i$ 的空间惯量变换到质心坐标系后的结果，$J_i$ 是连杆 $i$ 的 body Jacobian。

质心处的 Newton-Euler 方程¶

centroidal momentum 的时间导数等于外力之和——这是牛顿-欧拉方程在质心处的形式：

\[\dot{\mathbf{h}}_G = \begin{pmatrix} \dot{\boldsymbol{k}}_G \\ \dot{\boldsymbol{l}}_G \end{pmatrix} = \begin{pmatrix} \sum \boldsymbol{\tau}_{G,\text{ext}} \\ \sum \boldsymbol{f}_{\text{ext}} \end{pmatrix}\]

展开为：

线动量定理：

\[m \ddot{\boldsymbol{p}}_G = \sum_{k} \boldsymbol{f}_k + m\boldsymbol{g}\]

其中 $\boldsymbol{f}_k$ 是第 $k$ 个接触力，$\boldsymbol{g}$ 是重力加速度。

角动量定理（关于质心）：

\[\dot{\boldsymbol{k}}_G = \sum_{k} (\boldsymbol{p}_k - \boldsymbol{p}_G) \times \boldsymbol{f}_k\]

其中 $\boldsymbol{p}_k$ 是第 $k$ 个接触点的位置，$\boldsymbol{p}_G$ 是质心位置。

这两个方程是 Ch51 中所有简化模型的出发点： - LIPM：假设 $\dot{\boldsymbol{k}}_G \approx 0$（角动量变化很小） - SRBM：保留完整的 6D centroidal 动力学

CMM 的时间导数¶

\[\dot{\mathbf{h}}_G = A_G(q) \ddot{q} + \dot{A}_G(q, \dot{q}) \dot{q}\]

其中 $\dot{A}_G \dot{q}$ 是由位形变化引起的附加项。在 Pinocchio 中，这个量通过 computeCentroidalMomentumTimeVariation 计算。

CCRBA 算法¶

CCRBA（Centroidal Composite Rigid Body Algorithm）是一个高效计算 $A_G$ 的 $O(N)$ 算法。它的思想与 CRBA 类似，但把惯量累积到质心坐标系而不是关节坐标系。

算法步骤：

Forward pass：计算所有关节的位形（forwardKinematics）
计算质心：$\boldsymbol{p}_G = \frac{1}{m}\sum_i m_i \boldsymbol{p}_i$
Backward pass：类似 CRBA，但把复合惯量变换到质心坐标系
投影：$A_G$ 的每一列 = 复合惯量在质心处对关节轴的响应

Pinocchio Centroidal API¶

import pinocchio as pin
import numpy as np

model = pin.buildModelFromUrdf("go2.urdf", pin.JointModelFreeFlyer())
data = model.createData()

q = pin.neutral(model)
v = np.random.randn(model.nv) * 0.1

# 方法 1: 计算 centroidal momentum
pin.forwardKinematics(model, data, q, v)
pin.computeCentroidalMomentum(model, data, q, v)

h_G = data.hg  # 6D centroidal momentum
print(f"Centroidal momentum: {h_G}")
print(f"Angular momentum (k_G): {h_G.angular}")
print(f"Linear momentum (l_G): {h_G.linear}")

# 方法 2: 计算 CMM (Centroidal Momentum Matrix)
pin.computeCentroidalMap(model, data, q)
A_G = data.Ag  # 6 x nv
print(f"CMM shape: {A_G.shape}")  # (6, 18)

# 验证 h_G = A_G * v
h_G_check = A_G @ v
print(f"h_G vs A_G*v error: {np.linalg.norm(h_G.np - h_G_check):.2e}")

# 计算质心位置
pin.centerOfMass(model, data, q)
com = data.com[0]
print(f"CoM position: {com}")

# 计算 centroidal momentum 的时间导率
a = np.zeros(model.nv)
pin.computeCentroidalMomentumTimeVariation(model, data, q, v, a)
dh_G = data.dhg  # 6D, = A_G * a + dA_G * v
print(f"dh_G/dt: {dh_G}")

# 验证：dh_G 应等于外力之和
# 在零加速度、零扭矩情况下，dh_G 应等于重力
pin.computeCentroidalMomentumTimeVariation(model, data, q, np.zeros(model.nv), a)
print(f"dh_G at rest (should be gravity wrench): {data.dhg}")

CMM 与简化模型的连接¶

简化模型	使用的 centroidal 量	假设
质心动力学	$m\ddot{\boldsymbol{p}}_G = \sum \boldsymbol{f}_k + m\boldsymbol{g}$	无（精确）
LIPM	上述 + $\dot{\boldsymbol{k}}_G = 0$	角动量恒定
SRBM	$\dot{\mathbf{h}}_G = \sum \mathbf{F}_k^G$	保留 6D，忽略内部运动
全身动力学	$\mathbf{h}_G = A_G \dot{q}$	无假设（精确）

从上到下，模型越来越精确，计算代价也越来越高。CMM 是"全身→简化"的**精确投影**，没有任何近似。

从 CMM 到基座行的等价关系¶

基座行方程 $M_{bb}\ddot{q}_b + M_{bj}\ddot{q}_j + h_b = J_{c,b}^T\lambda$ 的前 6 行描述的正是 centroidal dynamics（在适当坐标变换后）。具体来说：

\[\dot{\mathbf{h}}_G = A_G \ddot{q} + \dot{A}_G \dot{q} = \sum_{k} \mathbf{F}_k^G\]

这可以通过左乘适当的变换矩阵从动力学方程的基座行得到。在实际 WBC 实现中，有些框架直接用基座行（如 TSID），有些用 centroidal 形式（如 IHWBC）。两者是等价的。

练习 49.10.1：用 Pinocchio 计算 Go2 站立时的 CMM $A_G$。检查 $A_G$ 的行数（6）和列数（18）。将其按基座/关节块分解为 $A_G = (A_{G,b}, A_{G,j})$，分析 $A_{G,b}$ 的结构。

练习 49.10.2：验证线动量——在 Pinocchio API 中，从 CMM 的前 3 行提取线动量部分，验证 $\boldsymbol{l}_G = m\boldsymbol{v}_G$。其中 $\boldsymbol{v}_G$ 由 pin.centerOfMass(model, data, q, v) 后从 data.vcom[0] 获取。如果你把 CMM 重排到本章 Featherstone 公式的 $[\text{angular};\text{linear}]$ 约定，再从最后 3 行取线动量。

练习 49.10.3：解释为什么 LIPM 假设 $\dot{\boldsymbol{k}}_G = 0$ 在慢速行走时是合理的，但在快速跑步或翻跟头时不成立。提示：快速运动时四肢的摆动会产生显著的角动量变化。

49.11 Pinocchio 代码-数学对应实操 ⭐⭐¶

目标：打通"公式→代码"的完整链路¶

本节用 Go2 四足机器人做一个完整的动力学计算流程，逐一验证本章推导的每个公式。

完整示例代码¶

"""
Ch49 完整实操：Go2 空间向量代数与动力学验证
要求：pip install pinocchio numpy matplotlib
"""
import pinocchio as pin
import numpy as np
np.set_printoptions(precision=4, suppress=True)

# ============================================================
# 1. 加载模型
# ============================================================
urdf_path = "go2.urdf"  # 替换为你的 Go2 URDF 路径
model = pin.buildModelFromUrdf(urdf_path, pin.JointModelFreeFlyer())
data = model.createData()

print("=" * 60)
print(f"Model: {model.name}")
print(f"nq = {model.nq}, nv = {model.nv}")
print(f"nq - nv = {model.nq - model.nv} (应为 1, 因为四元数)")
print(f"Number of joints: {model.njoints}")
print(f"Number of frames: {model.nframes}")
print("=" * 60)

# ============================================================
# 2. 设置随机状态
# ============================================================
q = pin.randomConfiguration(model)
v = np.random.randn(model.nv) * 0.3
a = np.random.randn(model.nv) * 0.1

# ============================================================
# 3. 验证 RNEA: tau = M*a + h
# ============================================================
# RNEA 直接算
tau_rnea = pin.rnea(model, data, q, v, a)

# CRBA + RNEA 分开算
pin.crba(model, data, q)
M = data.M + data.M.T - np.diag(data.M.diagonal())  # 对称化

h = pin.rnea(model, data, q, v, np.zeros(model.nv))  # h = C*v + g

tau_manual = M @ a + h
error_rnea = np.max(np.abs(tau_rnea - tau_manual))

print(f"\n[Test 1] RNEA vs M*a + h")
print(f"  Max error: {error_rnea:.2e} (should be ~1e-12)")

# ============================================================
# 4. 验证 ABA: q_ddot = M^{-1}(tau - h)
# ============================================================
tau_input = np.random.randn(model.nv)
tau_input[:6] = 0  # 基座无直接扭矩输入

qddot_aba = pin.aba(model, data, q, v, tau_input)
qddot_manual = np.linalg.solve(M, tau_input - h)
error_aba = np.max(np.abs(qddot_aba - qddot_manual))

print(f"\n[Test 2] ABA vs M^{{-1}}(tau - h)")
print(f"  Max error: {error_aba:.2e} (should be ~1e-11)")

# ============================================================
# 5. 验证空间惯量矩阵构造
# ============================================================
for link_id in [1, 3, 5]:
    inertia = model.inertias[link_id]
    m_link = inertia.mass
    c = inertia.lever
    Ic = inertia.inertia

    # 手动构造 6x6 空间惯量
    cx = pin.skew(c)
    I_manual = np.zeros((6, 6))
    I_manual[:3, :3] = Ic - m_link * cx @ cx
    I_manual[:3, 3:] = m_link * cx
    I_manual[3:, :3] = -m_link * cx
    I_manual[3:, 3:] = m_link * np.eye(3)

    I_pin = inertia.matrix()
    error_inertia = np.max(np.abs(I_manual - I_pin))
    print(f"\n[Test 3] Spatial inertia link {link_id}: error = {error_inertia:.2e}")

# ============================================================
# 6. 验证 Plucker 变换矩阵
# ============================================================
pin.forwardKinematics(model, data, q)

for joint_id in [1, 4, 7]:
    oMi = data.oMi[joint_id]
    R = oMi.rotation
    p = oMi.translation

    # 手动构造 Ad_T
    px = pin.skew(p)
    Ad_manual = np.zeros((6, 6))
    Ad_manual[:3, :3] = R
    Ad_manual[:3, 3:] = np.zeros((3, 3))
    Ad_manual[3:, :3] = px @ R
    Ad_manual[3:, 3:] = R

    Ad_pin = oMi.action
    error_ad = np.max(np.abs(Ad_manual - Ad_pin))
    print(f"\n[Test 4] Plucker transform joint {joint_id}: error = {error_ad:.2e}")

# ============================================================
# 7. 验证 Centroidal Momentum
# ============================================================
pin.forwardKinematics(model, data, q, v)
pin.computeCentroidalMomentum(model, data, q, v)
h_G = data.hg.np  # 6D vector

pin.computeCentroidalMap(model, data, q)
A_G = data.Ag
h_G_check = A_G @ v
error_cmm = np.max(np.abs(h_G - h_G_check))
print(f"\n[Test 5] h_G vs A_G @ v: error = {error_cmm:.2e}")

# 验证线动量 = m * v_com
pin.centerOfMass(model, data, q, v)
v_com = data.vcom[0]
m_total = sum(model.inertias[i].mass for i in range(1, model.njoints))
l_G = h_G[3:]  # 线动量 (后3维)
l_G_check = m_total * v_com
error_momentum = np.max(np.abs(l_G - l_G_check))
print(f"\n[Test 6] Linear momentum: l_G vs m*v_com error = {error_momentum:.2e}")

# ============================================================
# 8. 接触 Jacobian
# ============================================================
pin.computeJointJacobians(model, data, q)
pin.framesForwardKinematics(model, data, q)

foot_names = ["FL_foot", "FR_foot", "RL_foot", "RR_foot"]
n_contacts = len(foot_names)
J_c = np.zeros((3 * n_contacts, model.nv))

for i, name in enumerate(foot_names):
    frame_id = model.getFrameId(name)
    J_full = pin.getFrameJacobian(model, data, frame_id, pin.LOCAL_WORLD_ALIGNED)
    J_c[3*i:3*(i+1), :] = J_full[3:, :]  # 只取线速度部分

print(f"\n[Test 7] Contact Jacobian J_c shape: {J_c.shape}")
print(f"  J_c rank: {np.linalg.matrix_rank(J_c)}")

# ============================================================
# 9. 性能基准
# ============================================================
import time

N_iter = 10000
q_test = pin.neutral(model)
v_test = np.zeros(model.nv)
a_test = np.zeros(model.nv)

# RNEA benchmark
t0 = time.perf_counter()
for _ in range(N_iter):
    pin.rnea(model, data, q_test, v_test, a_test)
t_rnea = (time.perf_counter() - t0) / N_iter * 1e6

# ABA benchmark
t0 = time.perf_counter()
for _ in range(N_iter):
    pin.aba(model, data, q_test, v_test, a_test)
t_aba = (time.perf_counter() - t0) / N_iter * 1e6

# CRBA benchmark
t0 = time.perf_counter()
for _ in range(N_iter):
    pin.crba(model, data, q_test)
t_crba = (time.perf_counter() - t0) / N_iter * 1e6

print(f"\n[Benchmark] (averaged over {N_iter} iterations)")
print(f"  RNEA:  {t_rnea:.1f} us")
print(f"  ABA:   {t_aba:.1f} us")
print(f"  CRBA:  {t_crba:.1f} us")
print(f"  (1 kHz control loop budget: 1000 us)")

代码-数学对应速查表¶

数学公式	Pinocchio API	输入	输出
$\tau = M\ddot{q} + h$	`pin.rnea(model, data, q, v, a)`	$q, \dot{q}, \ddot{q}$	$\tau$
$\ddot{q} = M^{-1}(\tau - h)$	`pin.aba(model, data, q, v, tau)`	$q, \dot{q}, \tau$	$\ddot{q}$
$M(q)$	`pin.crba(model, data, q)` → `data.M`	$q$	$M$ (上三角)
$g(q)$	`pin.computeGeneralizedGravity(model, data, q)`	$q$	$g$
$h = C\dot{q} + g$	`pin.rnea(model, data, q, v, 0)`	$q, \dot{q}$	$h$
$\mathbf{V}_i$ (body twist)	`data.v[i]` (after `forwardKinematics`)	$q, \dot{q}$	$\mathbf{V}_i$
${}^0 T_i$ (SE3)	`data.oMi[i]`	$q$	$T$
$\operatorname{Ad}_T$ ($6 \times 6$)	`oMi.action`	—	$X$
$\hat{I}_i$ ($6 \times 6$)	`model.inertias[i].matrix()`	—	$\hat{I}$
$J_i$ (frame Jacobian)	`pin.getFrameJacobian(...)`	$q$	$J$
$A_G$ (CMM)	`data.Ag` (after `computeCentroidalMap`)	$q$	$A_G$
$\mathbf{h}_G$ (centroidal mom.)	`data.hg` (after `computeCentroidalMomentum`)	$q, \dot{q}$	$\mathbf{h}_G$
$\boldsymbol{p}_G$ (CoM)	`data.com[0]` (after `centerOfMass`)	$q$	CoM

典型性能数据参考¶

以下是 Go2（18 自由度）在现代笔记本 CPU（Intel i7-12700H）上的典型耗时：

算法	复杂度	耗时 (Go2)	1 kHz 控制循环占比
RNEA	$O(N)$	~3 us	0.3%
ABA	$O(N)$	~5 us	0.5%
CRBA	$O(N^2)$	~4 us	0.4%
CRBA + Cholesky	$O(N^2) + O(N^3)$	~8 us	0.8%
CMM	$O(N)$	~3 us	0.3%
Frame Jacobian	$O(N)$	~1 us	0.1%

所有动力学计算在 1 kHz 控制循环（1000 us）中只占不到 5%——剩余时间留给 QP 求解器和通信。

练习 49.11.1：运行完整示例代码，确认所有 7 个测试的误差都在 $10^{-10}$ 量级。如果某个测试失败，检查 URDF 路径和 Pinocchio 版本。

练习 49.11.2：修改基准测试代码，比较 Go2（18 自由度）和一个虚拟的 30 自由度人形机器人的 RNEA 耗时差异。验证 $O(N)$ 线性增长规律。

练习 49.11.3：在 Pinocchio 中计算 Go2 静止站立时的所有量：$M(q)$, $g(q)$, $J_c$, $A_G$，写出完整的静力平衡方程 $g(q) = J_c^T \lambda$ 并用最小二乘法求解 $\lambda$。验证每只脚的法向力约为 $mg/4$。

常见故障与排查¶

现象	可能原因	排查方法
空间惯量矩阵 $\hat{I}$ 出现负特征值	惯量参数设置错误（如 URDF 中 `<inertia>` 标签的 `ixx/iyy/izz` 为零或负值），或参考点设定不正确	用 `model.inertias[i].matrix()` 打印每个连杆的 $6\times6$ 惯量矩阵，检查正定性；核对 URDF 惯量参数与 CAD 模型一致
RNEA 计算的重力补偿力矩与实际电机力矩差异很大	浮动基座的四元数未归一化，导致重力方向在机体坐标系下的投影错误	调用 `pinocchio::normalize(model, q)` 后重新计算；打印 `q.head<4>().norm()` 确认接近 1.0
Centroidal Momentum $\mathbf{h}_G$ 在静止站立时不为零	输入速度向量 $\dot{q}$ 中的浮动基座速度分量非零（如 IMU 漂移引入的微小速度）	打印 `v.head<6>()`，确认静止时浮基速度为零；检查状态估计器是否有零速校准
接触 Jacobian $J_c$ 的秩不足，QP 求解失败	机器人处于奇异位型（如膝关节完全伸直），导致 Jacobian 秩亏	打印 $J_c$ 的奇异值（`JacobiSVD`），确认最小奇异值 $> 10^{-6}$；在初始化时避免关节角为 0 或 $\pi$
`pinocchio::computeCentroidalMap()` 返回的 CMM 维度与预期不符	Pinocchio 版本差异——v2.x 和 v3.x 对 centroidal 相关 API 有接口变化	确认 Pinocchio 版本 $\geq$ 3.0；查阅对应版本的 API 文档，注意函数名可能从 `ccrba` 变更为 `computeCentroidalMap`

本章小结¶

核心概念速查¶

概念	符号	维度	物理含义
Twist	$\mathbf{V} = (\boldsymbol{\omega}, \boldsymbol{v})^T$	$\mathbb{R}^6$	空间速度 = 角速度 + 线速度
Wrench	$\mathbf{F} = (\boldsymbol{\tau}, \boldsymbol{f})^T$	$\mathbb{R}^6$	空间力 = 力矩 + 力
Plucker 变换	$X = \operatorname{Ad}_T$	$\mathbb{R}^{6 \times 6}$	Motion 坐标变换
力变换	$X^* = \operatorname{Ad}_T^{-T}$	$\mathbb{R}^{6 \times 6}$	Force 坐标变换
空间惯量	$\hat{I}$	$\mathbb{R}^{6 \times 6}$	6D 惯量（10 参数）
质量矩阵	$M(q)$	$\mathbb{R}^{n_v \times n_v}$	广义惯量
CMM	$A_G(q)$	$\mathbb{R}^{6 \times n_v}$	广义速度 → centroidal momentum

算法复杂度总结¶

算法	功能	复杂度	核心思想
RNEA	逆动力学 $\tau = f(q,\dot{q},\ddot{q})$	$O(N)$	两遍递归：正向传 twist，反向传 wrench
ABA	正向动力学 $\ddot{q} = f(q,\dot{q},\tau)$	$O(N)$	三遍递归：铰接体惯量 + Schur 补
CRBA	质量矩阵 $M(q)$	$O(N^2)$	反向累积复合惯量
CCRBA	CMM $A_G(q)$	$O(N)$	累积到质心坐标系

本章数学工具链路¶

Plucker 坐标 (1865)
    ↓
螺旋理论 (1876, Ball)
    ↓
空间向量代数 (1987, Featherstone)
    ├── Twist (se(3))  ←→  Wrench (se(3)*)    [对偶]
    ├── Plucker Transform X = Ad_T              [坐标变换]
    ├── Spatial Inertia I                        [6D 惯量]
    └── 6D Newton-Euler: F = I·V̇ + V ×* (I·V)   [单体动力学]
         ↓
    递归算法 (RNEA / ABA / CRBA)               [多体动力学]
         ↓
    浮动基座方程: Mq̈ + h = S^Tτ + Jc^Tλ       [全身动力学]
         ↓
    Centroidal Momentum: hG = AG q̇              [简化投影]
         ↓
    简化模型 (LIPM / SRBM → Ch51)              [步态规划]

两周学习计划¶

天	内容	任务
第 1 天	49.0-49.1	前置自测 + 空间向量动机，读 Featherstone Ch2 前半
第 2 天	49.2	Twist 定义与两种约定，body vs spatial twist，Pinocchio `data.v`
第 3 天	49.3 前半	Plucker 变换推导（Ad_T 展开），手推矩阵形式
第 4 天	49.3 后半	力变换 $X^*$，Go2 腿链实例，练习
第 5 天	49.4	Wrench 定义，对偶关系，接触力模型
第 6 天	49.5 前半	空间惯量矩阵构造，10 参数，Pinocchio 验证
第 7 天	49.5 后半	空间叉积，6D Newton-Euler 方程，平行轴定理
第 8 天	49.6	RNEA 推导 + 伪代码 + Pinocchio `rnea()`
第 9 天	49.7	ABA 推导 + 铰接体惯量 + Pinocchio `aba()`
第 10 天	49.8	CRBA 推导 + 质量矩阵稀疏结构
第 11 天	49.9	浮动基座方程，选择矩阵 $S$，接触 Jacobian
第 12 天	49.10	Centroidal momentum，CMM，与简化模型的桥梁
第 13 天	49.11	完整 Pinocchio 实操，所有验证代码跑通
第 14 天	复习 + 习题	总结笔记，完成所有练习，预习 Ch50（QP/NLP）

下一章预告：Ch50 将介绍 QP 和 NLP 建模——掌握了本章的动力学方程后，Ch50 将把它们作为优化问题的约束条件，为 Ch53（WBC）和 Ch55（OCS2 MPC）做最后的数学准备。

累积项目：四足控制器——空间向量计算 + 浮动基座全身动力学模块¶

本模块是"累积项目：四足控制器"的动力学基础层。完成后你将拥有一套可复用的空间向量工具库和完整的全身动力学计算管线，为后续 WBC（Ch53）和 MPC（Ch55）提供核心接口。

阶段 1：空间向量工具库（2-3 天） - 实现 SpatialVector 类：封装 twist（$\mathbf{V} \in \mathbb{R}^6$）和 wrench（$\mathbf{F} \in \mathbb{R}^6$）的创建、加减、对偶运算 - 实现 Plucker 变换 $X = \operatorname{Ad}_T$：给定 $SE(3)$ 变换矩阵，构造 $6 \times 6$ 的 motion 变换和 force 变换 $X^*$ - 实现空间叉积 $\mathbf{V} \times^* \mathbf{F}$：验证与 Pinocchio 内部计算一致 - 单元测试：与 Pinocchio 的 data.oMi、data.v、data.f 对比，误差 $< 10^{-12}$

阶段 2：全身动力学计算管线（3-4 天） - 加载 Go2 URDF，构建 Pinocchio model/data - 封装 RNEA/ABA/CRBA 调用接口：输入 $(q, \dot{q}, \ddot{q})$，输出 $\tau$ / $\ddot{q}$ / $M(q)$ - 实现浮动基座方程的矩阵组装：$M(q)\ddot{q} + h(q,\dot{q}) = S^T\tau + J_c^T\lambda_c$ - 计算并缓存接触 Jacobian $J_c$（四脚端点）和非线性项 $h(q,\dot{q})$

阶段 3：Centroidal 动量接口（2-3 天） - 调用 pinocchio::computeCentroidalMap() 获取 CMM $A_G(q)$ - 实现 centroidal momentum 计算：$\mathbf{h}_G = A_G(q) \dot{q}$ - 验证 $\dot{\mathbf{h}}_G$ 与接触力之间的关系（Ch51 简化模型的数学基础） - 导出统一接口：getDynamics(q, v) → 返回 $M$, $h$, $J_c$, $A_G$

阶段 4：集成验证（1-2 天） - 静力平衡验证：站立状态下 $g(q) = J_c^T \lambda$，求解 $\lambda$ 并检查每脚法向力 $\approx mg/4$ - 动态一致性验证：给定随机 $(q, \dot{q}, \tau)$，ABA 输出的 $\ddot{q}$ 代入 RNEA 应还原 $\tau$ - 性能基准：单次全身动力学计算 $< 0.1$ ms（Go2 18-DOF）

延伸阅读¶

资料	类型	难度	推荐理由
Featherstone R. Rigid Body Dynamics Algorithms, Springer, 2008	专著	⭐⭐⭐	空间向量代数与递归动力学算法的权威教材，本章全部理论的原始来源
Featherstone R. "A Beginner's Guide to 6-D Vectors (Part 1 & 2)", IEEE RAM, 2010	教程	⭐⭐	空间向量入门最佳阅读材料，用直觉解释 6D 向量的物理含义
Carpentier J., Mansard N. "Analytical Derivatives of Rigid Body Dynamics Algorithms", RSS 2018	论文	⭐⭐⭐⭐	Pinocchio 的解析微分理论基础，理解 Pinocchio 源码的关键
Carpentier J. et al. "The Pinocchio C++ library", IEEE/SICE SII 2019	论文	⭐⭐	Pinocchio 架构设计论文，理解 model/data 分离和算法实现
Carpentier J., Mansard N. "Pinocchio: Efficient and versatile dynamics algorithms", T-RO 2025	论文	⭐⭐⭐⭐	Pinocchio v3 最新综述，覆盖 proximal/constrained dynamics 等前沿扩展
Orin D. et al. "Centroidal dynamics of a humanoid robot", Autonomous Robots 2013	论文	⭐⭐⭐	CMM 的原始定义论文，本章 49.10 节的核心参考

方面	固定基座	浮动基座
\(\mathbf{V}_0\)	\(\mathbf{0}\)（基座不动）	基座的 body twist
\(\dot{\mathbf{V}}_0\)	\(-{}^0\mathbf{g}\)（只有重力）	基座加速度 + \(-{}^0\mathbf{g}\)
输出维度	\(\tau \in \mathbb{R}^n\)	\(\tau \in \mathbb{R}^{6+n}\)（前 6 维是基座虚拟力）
基座虚拟力	不存在	\(\mathbf{F}_0\)：维持基座运动所需的假想力

操作	每连杆	总计 (\(N\) 连杆)
Plucker 变换 \({}^i\!X_{\lambda(i)}\)	\(O(1)\)（利用稀疏结构）	\(O(N)\)
Twist 传递	\(6 \times 6\) 矩阵-向量乘	\(O(N)\)
空间叉积	\(6 \times 6\) 矩阵-向量乘	\(O(N)\)
Wrench 计算	\(6 \times 6\) 矩阵-向量乘	\(O(N)\)
Wrench 反向传递	\(6 \times 6\) 矩阵-向量乘	\(O(N)\)
总计		\(O(N)\)

概念	假设	用途
复合刚体惯量 \(\hat{I}_i^C\)	子树关节锁死（rigid）	CRBA：计算 \(M(q)\)
铰接体惯量 \(\hat{I}_i^A\)	子树关节自由（free）	ABA：计算 \(\ddot{q}\)

量	符号	维度	Go2 值
广义坐标	\(q\)	nq	19 (\(= 7 + 12\))
广义速度	\(\dot{q}\)	nv	18 (\(= 6 + 12\))
质量矩阵	\(M\)	\(n_v \times n_v\)	\(18 \times 18\)
非线性项	\(h\)	\(n_v\)	18
选择矩阵	\(S\)	\(n \times n_v\)	\(12 \times 18\)
关节扭矩	\(\tau\)	\(n\)	12
接触力（4 腿站立）	\(\lambda\)	\(n_c\)	12 (\(= 4 \times 3\))
接触 Jacobian	\(J_c\)	\(n_c \times n_v\)	\(12 \times 18\)

数学公式	Pinocchio API	输入	输出
\(\tau = M\ddot{q} + h\)	`pin.rnea(model, data, q, v, a)`	\(q, \dot{q}, \ddot{q}\)	\(\tau\)
\(\ddot{q} = M^{-1}(\tau - h)\)	`pin.aba(model, data, q, v, tau)`	\(q, \dot{q}, \tau\)	\(\ddot{q}\)
\(M(q)\)	`pin.crba(model, data, q)` → `data.M`	\(q\)	\(M\) (上三角)
\(g(q)\)	`pin.computeGeneralizedGravity(model, data, q)`	\(q\)	\(g\)
\(h = C\dot{q} + g\)	`pin.rnea(model, data, q, v, 0)`	\(q, \dot{q}\)	\(h\)
\(\mathbf{V}_i\) (body twist)	`data.v[i]` (after `forwardKinematics`)	\(q, \dot{q}\)	\(\mathbf{V}_i\)
\({}^0 T_i\) (SE3)	`data.oMi[i]`	\(q\)	\(T\)
\(\operatorname{Ad}_T\) (\(6 \times 6\))	`oMi.action`	—	\(X\)
\(\hat{I}_i\) (\(6 \times 6\))	`model.inertias[i].matrix()`	—	\(\hat{I}\)
\(J_i\) (frame Jacobian)	`pin.getFrameJacobian(...)`	\(q\)	\(J\)
\(A_G\) (CMM)	`data.Ag` (after `computeCentroidalMap`)	\(q\)	\(A_G\)
\(\mathbf{h}_G\) (centroidal mom.)	`data.hg` (after `computeCentroidalMomentum`)	\(q, \dot{q}\)	\(\mathbf{h}_G\)
\(\boldsymbol{p}_G\) (CoM)	`data.com[0]` (after `centerOfMass`)	\(q\)	CoM

常见错误	后果
忘记耦合项 \(\boldsymbol{t} \times R\boldsymbol{\omega}\)	线速度变换错误，动力学结果完全错误
力的耦合项符号写反	力矩方向反了，机器人仿真中"飞"起来
角速度和线速度的排列顺序搞混	在不同库之间移植代码时最常见
忘记区分 body frame 和 spatial frame	所有结果都错，但数值"差不多"，极难 debug

贡献	具体内容	工程价值
统一变换	所有坐标系变换规约为 \(6 \times 6\) 矩阵乘法	代码简洁，bug 少
统一运算	叉积、求导、牛顿-欧拉方程全在 6D 空间	公式对称，推导简单
O(N) 算法	RNEA/ABA/CRBA 都是线性时间	实时控制可行

约定	排列	使用者
Featherstone（本章公式）	\((\boldsymbol{\omega}, \boldsymbol{v})^T\)	Featherstone 2008, RBDL, Drake
Pinocchio API 存储	\((\boldsymbol{v}, \boldsymbol{\omega})^T\)	Pinocchio C++ `Motion`/`Force` 内存布局
MLS / Lie group	\((\boldsymbol{v}, \boldsymbol{\omega})^T\)	Murray-Li-Sastry 1994, Manif, 部分 SLAM 文献

量	从 B 变换到 A	矩阵
Motion（twist）	\({}^A\mathbf{V} = {}^A\!X_B \cdot {}^B\mathbf{V}\)	\(X = \operatorname{Ad}_T\)
Force（wrench）	\({}^A\mathbf{F} = {}^A\!X_B^* \cdot {}^B\mathbf{F}\)	\(X^* = \operatorname{Ad}_T^{-T}\)

接触模型	力自由度	约束	典型机器人
点接触	3 (\(f_x, f_y, f_z\))	摩擦锥 \(\sqrt{f_x^2+f_y^2} \leq \mu f_z\)	Go2, Spot
面接触	6 (\(\boldsymbol{\tau}_c, \boldsymbol{f}_c\))	摩擦锥 + COP 约束	Atlas, Digit

性质	说明
对称	\(\hat{I} = \hat{I}^T\)
正定	对非零 twist，\(\mathbf{V}^T \hat{I} \mathbf{V} = 2T > 0\)（\(T\) 是动能）
10 个独立参数	\(m\)（1）+ \(\boldsymbol{c}\)（3）+ \(I_c\)（6，对称矩阵上三角）= 10
特殊情况	当 \(\boldsymbol{c} = 0\)（原点在质心）：\(\hat{I} = \operatorname{blkdiag}(I_c, mI)\)

简化模型	使用的 centroidal 量	假设
质心动力学	\(m\ddot{\boldsymbol{p}}_G = \sum \boldsymbol{f}_k + m\boldsymbol{g}\)	无（精确）
LIPM	上述 + \(\dot{\boldsymbol{k}}_G = 0\)	角动量恒定
SRBM	\(\dot{\mathbf{h}}_G = \sum \mathbf{F}_k^G\)	保留 6D，忽略内部运动
全身动力学	\(\mathbf{h}_G = A_G \dot{q}\)	无假设（精确）

现象	可能原因	排查方法
空间惯量矩阵 \(\hat{I}\) 出现负特征值	惯量参数设置错误（如 URDF 中 `<inertia>` 标签的 `ixx/iyy/izz` 为零或负值），或参考点设定不正确	用 `model.inertias[i].matrix()` 打印每个连杆的 \(6\times6\) 惯量矩阵，检查正定性；核对 URDF 惯量参数与 CAD 模型一致
RNEA 计算的重力补偿力矩与实际电机力矩差异很大	浮动基座的四元数未归一化，导致重力方向在机体坐标系下的投影错误	调用 `pinocchio::normalize(model, q)` 后重新计算；打印 `q.head<4>().norm()` 确认接近 1.0
Centroidal Momentum \(\mathbf{h}_G\) 在静止站立时不为零	输入速度向量 \(\dot{q}\) 中的浮动基座速度分量非零（如 IMU 漂移引入的微小速度）	打印 `v.head<6>()`，确认静止时浮基速度为零；检查状态估计器是否有零速校准
接触 Jacobian \(J_c\) 的秩不足，QP 求解失败	机器人处于奇异位型（如膝关节完全伸直），导致 Jacobian 秩亏	打印 \(J_c\) 的奇异值（`JacobiSVD`），确认最小奇异值 \(> 10^{-6}\)；在初始化时避免关节角为 0 或 \(\pi\)
`pinocchio::computeCentroidalMap()` 返回的 CMM 维度与预期不符	Pinocchio 版本差异——v2.x 和 v3.x 对 centroidal 相关 API 有接口变化	确认 Pinocchio 版本 \(\geq\) 3.0；查阅对应版本的 API 文档，注意函数名可能从 `ccrba` 变更为 `computeCentroidalMap`

概念	符号	维度	物理含义
Twist	\(\mathbf{V} = (\boldsymbol{\omega}, \boldsymbol{v})^T\)	\(\mathbb{R}^6\)	空间速度 = 角速度 + 线速度
Wrench	\(\mathbf{F} = (\boldsymbol{\tau}, \boldsymbol{f})^T\)	\(\mathbb{R}^6\)	空间力 = 力矩 + 力
Plucker 变换	\(X = \operatorname{Ad}_T\)	\(\mathbb{R}^{6 \times 6}\)	Motion 坐标变换
力变换	\(X^* = \operatorname{Ad}_T^{-T}\)	\(\mathbb{R}^{6 \times 6}\)	Force 坐标变换
空间惯量	\(\hat{I}\)	\(\mathbb{R}^{6 \times 6}\)	6D 惯量（10 参数）
质量矩阵	\(M(q)\)	\(\mathbb{R}^{n_v \times n_v}\)	广义惯量
CMM	\(A_G(q)\)	\(\mathbb{R}^{6 \times n_v}\)	广义速度 → centroidal momentum

算法	功能	复杂度	核心思想
RNEA	逆动力学 \(\tau = f(q,\dot{q},\ddot{q})\)	\(O(N)\)	两遍递归：正向传 twist，反向传 wrench
ABA	正向动力学 \(\ddot{q} = f(q,\dot{q},\tau)\)	\(O(N)\)	三遍递归：铰接体惯量 + Schur 补
CRBA	质量矩阵 \(M(q)\)	\(O(N^2)\)	反向累积复合惯量
CCRBA	CMM \(A_G(q)\)	\(O(N)\)	累积到质心坐标系

第 49 章：空间向量代数与浮动基座动力学¶

49.0 前置自测¶

49.0.1 本章目标¶

49.1 为什么需要空间向量 ⭐¶

动机：一个看似简单的问题¶

参考点依赖：3D 方法的根本缺陷¶

Featherstone 的洞察（1987）¶

历史脉络：Plucker → Ball → Featherstone¶

固定基座 vs 浮动基座：为什么本章如此重要¶

49.2 Twist（空间速度）⭐⭐¶

从直觉出发：为什么需要 6D 速度¶

Twist 的定义¶

Body Twist vs Spatial Twist¶

Body twist 与 Spatial twist 的关系推导¶

螺旋运动解释¶

与 \(\mathfrak{se}(3)\) 的对应¶

Pinocchio 中的 Twist¶

49.3 Plucker 变换矩阵 ⭐⭐⭐¶

动机：我们需要一个统一的坐标系变换¶

从 \(SE(3)\) 伴随表示的完整推导¶

Plucker 变换矩阵的显式形式¶

逆变换¶

力变换矩阵 \(X^*\)¶

Go2 腿链实例¶

49.4 Wrench（空间力）⭐⭐¶

从牛顿第三定律到 6D 力¶

对偶关系：功率不变量¶

Wrench 的变换规则¶

接触 Wrench 在腿足机器人中的应用¶

Wrench 的螺旋解释¶

49.5 6×6 空间惯量矩阵 ⭐⭐⭐¶

动机：把牛顿-欧拉方程写成 6D 形式¶

空间惯量矩阵的完整构造¶

第一步：刚体参数¶

第二步：推导空间动量¶

第三步：组装 6D 矩阵¶

空间惯量矩阵的显式形式¶

空间叉积运算符¶

6D 牛顿-欧拉方程¶

平行轴定理的 6D 形式¶

复合惯量（Composite Inertia）¶

Pinocchio 中的空间惯量¶

49.6 Featherstone RNEA（递归牛顿-欧拉算法）⭐⭐⭐¶

动机：从 \(O(N^3)\) 到 \(O(N)\)¶

算法结构：两遍递归¶

完整推导¶

Pass 1：正向传递——计算每个连杆的 twist 和 spatial acceleration¶

Pass 2：反向传递——累积 wrench¶

完整伪代码¶

固定基座 vs 浮动基座的处理差异¶

Pinocchio RNEA API¶

49.7 Featherstone ABA（Articulated Body Algorithm）⭐⭐⭐¶

动机：正向动力学——从力到加速度¶

核心概念：Articulated Body Inertia¶

三遍递归结构¶

完整推导¶

Pass 1：正向传递¶

Pass 2：反向传递——构建铰接体惯量¶

Pass 3：正向传递——计算加速度¶

ABA 伪代码¶

Pinocchio ABA API¶

49.8 CRBA（Composite Rigid Body Algorithm）⭐⭐¶

动机：显式构造质量矩阵¶

算法思想¶

完整推导¶

CRBA 伪代码¶

质量矩阵的结构与稀疏性¶

Pinocchio CRBA API¶

49.9 浮动基座全身动力学方程 ⭐⭐⭐¶

完整形式¶

选择矩阵 \(S\) 的详细解释¶

块分解¶

Go2 的具体维度¶

接触 Jacobian \(J_c\)¶

验证动力学方程的 Pinocchio 实现¶

49.10 Centroidal Momentum 与 CCRBA ⭐⭐⭐¶

动机：从全身动力学到简化模型的桥梁¶

定义¶

Centroidal Momentum Matrix (CMM)¶

质心处的 Newton-Euler 方程¶