空间向量代数¶
所属：刚体动力学专题 4-1 | 前置：线性代数（矩阵运算、对称/反对称矩阵）、20_微分几何与李群/30_李群基础与SO3_SE3（SE(3)、李代数 se(3)、指数映射） 交叉引用：→ 20_Lagrange与Hamilton力学（M/C/g 的空间向量构造）、→ 30_ON动力学递推算法（RNEA/ABA/CRBA 直接使用本章记号）、→ 40_SE3几何力学（Lie 群视角的统一）
前置自测¶
📋 前置自测（答不出 ≥ 2 题 → 先回前置章节复习）
- 刚体有几个自由度？为什么平面刚体是 3 个、空间刚体是 6 个？
- 什么是反对称矩阵（skew-symmetric）？给定向量 \(\omega \in \mathbb{R}^3\)，写出其反对称矩阵 \([\omega]_\times\)。
- 齐次变换矩阵 \(T \in SE(3)\) 的结构是什么？它的逆怎么算？
- 什么是对偶空间？向量空间 \(V\) 与其对偶 \(V^*\) 之间的关系是什么？
- 李群 \(SO(3)\) 的李代数 \(\mathfrak{so}(3)\) 是什么？指数映射 \(\exp: \mathfrak{so}(3) \to SO(3)\) 有什么几何含义？
本章目标¶
学完本章后，你将能够：
- 用 Plucker 坐标表达任意刚体的速度和力，理解运动向量与力向量的对偶关系
- 推导并使用 6×6 空间变换矩阵，在不同坐标系间变换空间向量
- 计算运动叉积与力叉积，理解它们与李代数 adjoint 算子的对应
- 构造 6×6 空间惯性张量，掌握平行轴定理的空间向量版本
- 在 Featherstone、Lynch-Park、Drake 三种记号间自由切换
知识树¶
空间向量代数
├── 1. 为什么需要空间向量（动机与历史）
├── 2. Plucker 坐标完整理论
│ ├── 2.1 运动向量（twist）
│ ├── 2.2 力向量（wrench）
│ └── 2.3 对偶性与功率标量积
├── 3. 空间变换矩阵
│ ├── 3.1 6×6 Plucker 变换 X 推导
│ ├── 3.2 分块结构与物理含义
│ └── 3.3 变换的逆与复合
├── 4. 运动叉积与力叉积
│ ├── 4.1 运动叉积 ×（crossm）
│ ├── 4.2 力叉积 ×*（crossf）
│ └── 4.3 与李代数 ad/ad* 的对应
├── 5. 空间惯性张量
│ ├── 5.1 6×6 惯性矩阵构造
│ ├── 5.2 平行轴定理的空间向量版
│ └── 5.3 复合刚体惯性
├── 6. 三大流派记号对比
│ ├── 6.1 Featherstone 记号
│ ├── 6.2 Lynch-Park 记号
│ └── 6.3 Drake 记号
└── 7. 典型例题
├── 7.1 2R 机械臂空间速度计算
└── 7.2 浮基机器人空间惯性
1. 为什么需要空间向量 ⭐¶
动机：三维记号的代数海洋¶
考虑一个最简单的问题：计算一个旋转刚体上某点 P 的速度。在经典三维力学中，你需要写：
这看起来很简单。但当你处理一个 \(n\) 自由度的串联机械臂时，每个连杆的速度都要写成前一个连杆速度加上相对运动：
每个连杆要写两个方程（线速度和角速度），还要处理叉积项。当你进一步计算加速度时，会冒出 Coriolis 项和离心项：
对于 \(n\) 个连杆，加速度传递公式里的交叉耦合项呈爆炸式增长。这就是 Featherstone 所说的**"代数海洋"**（algebra ocean）——用三维向量写多体动力学，公式的长度和复杂度随自由度数急剧膨胀。
如果不用空间向量会怎样¶
让我们用一个具体的反面例子来感受三维记号的痛苦。对一个 7-DoF 的 Panda 机械臂做逆动力学（给定 \(q, \dot{q}, \ddot{q}\)，求 \(\tau\)）：
三维记号路径： - 前向传播：14 个方程（每个连杆 2 个：\(\omega_i\) 和 \(v_{c_i}\)） - 加速度传播：14 个方程（每个连杆 2 个：\(\dot{\omega}_i\) 和 \(a_{c_i}\)），每个都含叉积项 - 力/力矩回溯：14 个方程（每个连杆的力和力矩分别回溯） - 总计：约 42 个向量方程，每个含 1-3 个叉积项
空间向量路径： - 前向传播：7 个方程：\(v_i = {}^iX_{\lambda(i)} v_{\lambda(i)} + S_i \dot{q}_i\) - 加速度传播：7 个方程：\(a_i = {}^iX_{\lambda(i)} a_{\lambda(i)} + S_i \ddot{q}_i + v_i \times S_i \dot{q}_i\) - 力回溯：7 个方程：\(f_i = I_i a_i + v_i \times^* I_i v_i - f_i^{ext}\) - 总计：21 个空间向量方程，无需分别处理线性/角量
本质洞察：空间向量不是"把 \(\omega\) 和 \(v\) 拼成一个长向量"的记号便利。它改变了加速度的定义——空间加速度是空间速度的简单时间导数，不含 Coriolis 项。这使得加速度像速度一样用加法叠加，从根本上消除了三维记号中的交叉耦合项。
历史脉络¶
空间向量的历史可追溯到 19 世纪的螺旋理论（screw theory）：
| 年代 | 人物 | 贡献 |
|---|---|---|
| 1830s | Chasles, Poinsot | 发现任何刚体运动可分解为沿某轴的旋转+平移（螺旋运动） |
| 1860s | Plucker | 用 6 个坐标表示空间直线（Plucker 坐标） |
| 1870s-1900s | Ball, Study | 螺旋理论系统化 |
| 1978 | Yang, Freudenstein | 将螺旋理论用于机构分析 |
| 1980 | Luh, Walker, Paul | 提出 RNEA（Newton-Euler 递推逆动力学算法） |
| 1987 | Featherstone | 系统化空间向量框架并统一三大算法（RNEA/CRBA/ABA）推导 |
| 1994 | Murray, Li, Sastry | 从李群视角统一 twist/wrench 理论 |
| 2008 | Featherstone | RBDA 定型，成为算法标准 |
| 2010 | Featherstone | IEEE RAM "A Beginner's Guide to 6-D Vectors" 两部分 |
| 2017 | Lynch, Park | Modern Robotics 从教学角度重组 |
Featherstone 在 1987 年的博士论文中首次系统化空间向量，其核心洞察是：如果我们把线性运动和角运动合并成一个 6D 对象，并且用正确的变换规则，那么多体动力学的递推算法会变得极其简洁。这不是事后的记号美化——RNEA 算法（最初由 Luh, Walker & Paul 于 1980 年提出）在空间向量框架下获得了统一而优雅的重新推导，ABA 则是 Featherstone 在该框架下的原创贡献。
2. Plucker 坐标完整理论 ⭐¶
2.1 运动向量（Motion Vectors / Twists） ⭐¶
从刚体速度到 6D 表示¶
一个刚体在空间中的瞬时运动由两个量完全确定： - 角速度 \(\omega \in \mathbb{R}^3\)：描述刚体绕某瞬时轴的旋转快慢 - 某参考点 O 的线速度 \(v_O \in \mathbb{R}^3\)：描述刚体上固连于 O 的那个点在该瞬间的速度
我们把它们合并成一个 6D 向量。但顺序约定有两种：
这个 6D 向量就是**空间速度**（spatial velocity），也叫 twist。它生活在一个 6 维向量空间中，我们记为 \(\mathcal{M}^6\)（运动空间）。
为什么参考点的选择很重要¶
空间速度的线性部分 \(v_O\) 依赖于参考点 O 的选择。如果换一个参考点 P，与 O 相距 \(r_{OP}\)，则：
但角速度 \(\omega\) 与参考点无关。这意味着空间速度不是一个"自由向量"——它的值取决于你选择哪个点作为参考。这正是 Plucker 坐标的本质特征。
跨领域类比：空间速度就像电磁学中的磁场 \(\mathbf{B}\)。磁场在不同点有不同的值，但在特定坐标系下可以用一组数字完全描述。类似地，空间速度在选定参考点和坐标系后，用 6 个数字完全描述刚体的运动状态。区别在于：磁场是定义在每个空间点上的场，而空间速度是整个刚体共享的一个 6D 量（只是表达依赖参考点）。
空间速度与螺旋运动的关系¶
Chasles 定理告诉我们：任何刚体的瞬时运动都可以分解为**沿某轴旋转 + 沿同一轴平移**的复合（螺旋运动）。空间速度 \(\hat{v} = (\omega; v_O)\) 编码了这个螺旋的全部信息：
- 螺旋轴方向：\(\hat{\omega} = \omega / \|\omega\|\)（当 \(\omega \neq 0\)）
- 螺旋轴位置：通过点 \(q = \frac{\omega \times v_O}{\|\omega\|^2}\)（离 O 最近的轴上点）
- 角速度大小：\(\|\omega\|\)
- 螺距（pitch）：\(h = \frac{\omega \cdot v_O}{\|\omega\|^2}\)（沿轴的平移速度与旋转速度之比）
当 \(\omega = 0\) 时退化为纯平移（螺距无穷大）。
Body-fixed vs Space-fixed 表达¶
空间速度可以在不同坐标系中表达：
- Body-fixed（体坐标系）表达 \({}^B\hat{v} = ({}^B\omega; {}^Bv_B)\)：角速度和线速度都在刚体自身坐标系中表达，参考点是体坐标系原点
- Space-fixed（空间坐标系）表达 \({}^S\hat{v} = ({}^S\omega; {}^Sv_S)\)：在固定空间坐标系中表达，参考点是空间坐标系原点
两者的关系由 6×6 Plucker 变换矩阵连接（详见第 3 节）。
Pinocchio 和 Featherstone 默认使用 body-fixed 表达（每个连杆的速度表达在自身坐标系中），因为这让递推公式更简洁。
2.2 力向量（Force Vectors / Wrenches） ⭐¶
从力和力矩到 6D 表示¶
类似地，作用在刚体上的外力也由两个量确定： - 力矩（关于参考点 O）\(\tau_O \in \mathbb{R}^3\) - 合力 \(f \in \mathbb{R}^3\)
合并为 6D 力向量（wrench）：
力向量生活在另一个 6 维向量空间 \(\mathcal{F}^6\)（力空间）。
力向量的参考点变换¶
换参考点 \(O \to P\)，合力 \(f\) 不变，但力矩变换为：
推导：设力 \(f\) 作用在空间某点 \(A\)，则关于 \(O\) 的力矩为 \(\tau_O = r_{OA} \times f\)。关于 \(P\) 的力矩为 \(\tau_P = r_{PA} \times f = (r_{PO} + r_{OA}) \times f = r_{PO} \times f + \tau_O\)。由于 \(r_{PO} = -r_{OP}\)，得 \(\tau_P = \tau_O - r_{OP} \times f\)。
注意对比运动向量：运动向量是 \(v_P = v_O + \omega \times r_{OP}\)。力向量换参考点时叉积的"位移向量方向"相反（\(r_{PO}\) 而非 \(r_{OP}\)）——这正是对偶性的体现。
2.3 对偶性与功率标量积 ⭐⭐¶
功率的不变性¶
运动向量和力向量之间最根本的关系是：它们的标量积给出功率。
这个功率值**不依赖于参考点的选择**。让我们验证这一点。换参考点 \(O \to P\)，代入正确的变换公式 \(\tau_P = \tau_O - r_{OP} \times f\) 和 \(v_P = v_O + \omega \times r_{OP}\)：
利用标量三重积恒等式 \((a \times b) \cdot c = a \cdot (b \times c)\)：
（第二步利用了 \(a \cdot (b \times c) = b \cdot (c \times a)\)，再利用 \(c \cdot (a \times b) = -a \cdot (b \times c)\) 的轮换性质。）
两项一正一负相消！因此 \(P = \tau_O^T \omega + f^T v_O\) 与参考点无关。\(\square\)
本质洞察：运动空间 \(\mathcal{M}^6\) 和力空间 \(\mathcal{F}^6\) 不是同一个向量空间——它们是**对偶空间**。这不是学究式的区分：对偶性决定了坐标变换规则不同（运动向量用 \(X\)，力向量用 \(X^{-T}\)）。如果你把力向量当成运动向量来变换坐标，结果的物理含义就全错了。Pinocchio 用
MotionTpl和ForceTpl两个不同类型在编译期堵住这类错误。
对偶基与 Plucker 坐标的代数结构¶
在坐标系 \(\{O; \hat{e}_1, \hat{e}_2, \hat{e}_3\}\) 下，运动空间有 6 个基向量：
力空间的对偶基 \(e^1, \ldots, e^6\) 满足 \(e^i \cdot d_j = \delta_{ij}\)。在 Featherstone 约定下，对偶基恰好是：
这意味着在同一坐标系下，运动向量和力向量的分量用**相同的 6 元组**表示，但它们遵循不同的变换规则。这类似于协变向量和逆变向量在正交基下分量相同、但变换行为不同的情况。
3. 空间变换矩阵 ⭐⭐¶
3.1 6×6 Plucker 变换 X 的推导¶
问题提出¶
假设坐标系 A 相对于坐标系 B 的姿态为旋转矩阵 \(R \in SO(3)\)，位移为 \(p \in \mathbb{R}^3\)（A 原点相对 B 原点的位移，在 B 系中表达）。即齐次变换：
现在，一个空间速度在 A 系中表达为 \({}^A\hat{v} = ({}^A\omega; {}^Av_A)\)。我们要求它在 B 系中的表达 \({}^B\hat{v} = ({}^B\omega; {}^Bv_B)\)。
逐步推导¶
Step 1：角速度的坐标变换很简单——只需旋转： $\({}^B\omega = R \cdot {}^A\omega\)$
Step 2：线速度需要同时做坐标旋转和参考点变换。参考点从 A 原点换到 B 原点： $\({}^Bv_B = R \cdot {}^Av_A + p \times (R \cdot {}^A\omega)\)$
这里 \(p \times (R \cdot {}^A\omega)\) 是因为 B 原点与 A 原点不重合导致的额外线速度贡献。
为什么这么写？ 回忆速度变换公式 \(v_P = v_O + \omega \times r_{OP}\)。这里 O = A 原点，P = B 原点，\(r_{OP} = p\)，所以 \(v_B = v_A + \omega \times p = v_A - p \times \omega\)。再加上坐标系旋转，得到上式。注意叉积方向。
Step 3：利用 \(p \times (R\omega) = [p]_\times R \omega\)（其中 \([p]_\times\) 是 \(p\) 的反对称矩阵），合并为矩阵形式：
这就是 6×6 Plucker 变换矩阵：
反事实推理：如果不用 6×6 变换¶
如果不用 6×6 矩阵，每次坐标变换都要分别处理角速度和线速度，写两个公式，还要记住叉积的方向。对一个 7-DoF 机械臂，RNEA 的前向传播需要 7 次坐标变换——用三维记号就是 14 个向量公式，用 6×6 矩阵就是 7 次矩阵-向量乘法。后者不仅简洁，而且**让整个递推结构可以用统一的线性代数框架表达**，便于求解析导数。
3.2 分块结构与物理含义¶
Plucker 变换的分块结构揭示了深层含义：
| 分块 | 位置 | 含义 |
|---|---|---|
| \(R\) | 左上 | 旋转角速度分量的坐标系 |
| \(0\) | 右上 | 角速度不受平移影响（角速度是自由向量） |
| \([p]_\times R\) | 左下 | 参考点变换对线速度的耦合效应 |
| \(R\) | 右下 | 旋转线速度分量的坐标系 |
**右上角为零**是一个关键事实：平移不影响角速度。这反映了角速度的"自由向量"性质——它只依赖旋转，不依赖参考点。
纯旋转和纯平移的特殊情况¶
纯旋转（\(p = 0\)）： $\(X_{rot} = \begin{pmatrix} R & 0 \\ 0 & R \end{pmatrix}\)$
两个 \(R\) 分别旋转角量和线量的坐标系。此时变换退化为对角分块。
纯平移（\(R = I\)）： $\(X_{trans} = \begin{pmatrix} I & 0 \\ [p]_\times & I \end{pmatrix}\)$
只有参考点变换效应，没有坐标旋转。这是一个**下三角矩阵**，其逆也是下三角： $\(X_{trans}^{-1} = \begin{pmatrix} I & 0 \\ -[p]_\times & I \end{pmatrix}\)$
一般变换可以分解为 \(X = X_{trans} \cdot X_{rot}\)（先旋转再平移）。
3.3 变换的逆与复合¶
逆变换¶
\({}^BX_A\) 把 A 系表达变换到 B 系。其逆把 B 系表达变换回 A 系：
推导：利用 \(R^{-1} = R^T\) 和 \([p]_\times^T = -[p]_\times\)，对分块矩阵求逆即得。
复合变换¶
从 A 到 B 再到 C 的变换直接相乘：
这与齐次变换矩阵 \(T_{CA} = T_{CB} T_{BA}\) 的复合规则完全一致——只是在 6D 空间中做。
力向量的变换规则¶
由对偶性（功率不变性）得到力向量的变换。设 \({}^A\hat{v}\) 和 \({}^A\hat{f}\) 是同一坐标系 A 中的运动/力向量对：
而 \({}^B\hat{v} = {}^BX_A \cdot {}^A\hat{v}\)，代入：
对任意 \({}^A\hat{v}\) 成立，因此：
即**力向量用 \(X^{-T}\) 变换**，而非 \(X\) 本身。这正是对偶空间变换的标准规则。
Featherstone 记号中常用 \(X^*\) 表示力变换：\(X^* := X^{-T}\)。展开：
注意与运动变换的转置关系——右上角现在非零（力矩受平移影响），左下角为零（合力不受参考点变换影响，合力是自由向量）。
"不是X而是Y"：运动向量和力向量的区别不是"一个有角量一个没有"——它们都有角量和线量。真正的区别是：运动向量的角量（角速度）是自由向量、线量（线速度）依赖参考点；力向量恰好反过来——合力是自由向量、力矩依赖参考点。这就是为什么变换矩阵的零块位置互换。
4. 运动叉积与力叉积 ⭐⭐¶
4.1 运动叉积 ×（crossm）¶
动机：空间加速度的定义¶
在三维力学中，组合两个参考系的加速度会冒出 Coriolis 和离心项：
这些额外项让递推算法变得复杂。Featherstone 的关键洞察是：如果把空间加速度定义为空间速度的简单时间导数（在固定坐标系中），那么加速度的递推公式中不会出现 Coriolis 项！
但这引入了一个新问题：当我们在运动坐标系中对空间速度求导时，会出现一个**运动叉积**项。
推导运动叉积¶
设空间速度 \(\hat{v}_1 = (\omega_1; v_1)\) 和 \(\hat{v}_2 = (\omega_2; v_2)\)。定义运动叉积：
等价地，用矩阵形式：
其中运动叉积矩阵为：
这里 \([\omega]_\times\) 和 \([v]_\times\) 是 3×3 反对称矩阵。
为什么是这个形式¶
运动叉积的来源是：在运动坐标系中，对一个空间向量求时间导数时产生的修正项。具体地，如果坐标系以空间速度 \(\hat{v}_1\) 运动，一个在该坐标系中表达的空间速度 \(\hat{v}_2\) 的绝对时间导数为：
这与三维情况下向量在旋转系中求导的公式 \(\dot{\mathbf{a}}|_{fixed} = \dot{\mathbf{a}}|_{rot} + \omega \times \mathbf{a}\) 完全类比——只是从 3D 提升到了 6D。
运动叉积的性质¶
- 反对称性：\(\hat{v}_1 \times \hat{v}_2 = -\hat{v}_2 \times \hat{v}_1\)
- Jacobi 恒等式：\(\hat{v}_1 \times (\hat{v}_2 \times \hat{v}_3) + \hat{v}_2 \times (\hat{v}_3 \times \hat{v}_1) + \hat{v}_3 \times (\hat{v}_1 \times \hat{v}_2) = 0\)
- 与李括号的关系：运动叉积就是 \(\mathfrak{se}(3)\) 李代数的李括号
性质 2 是 Jacobi 恒等式，它保证了运动叉积定义的李代数结构。
4.2 力叉积 ×*（crossf） ⭐⭐¶
定义与推导¶
力叉积出现在动量定理的空间向量形式中。Newton-Euler 方程的空间形式为：
其中 \(\hat{v} \times^*\) 是力叉积算子。它的定义由功率守恒决定：
即 \([\hat{v}_1]_\times^T = -[\hat{v}_1]_{\times^*}\)，所以：
注意区别：运动叉积是下三角分块，力叉积是上三角分块。
物理含义¶
力叉积 \(\hat{v} \times^* \hat{f}\) 表示的是：由于坐标系以速度 \(\hat{v}\) 运动，力向量 \(\hat{f}\) 的表观变化率。具体地：
- 力矩变化 = 角速度导致的力矩旋转 + 平移速度与力的耦合
- 合力变化 = 角速度导致的力方向旋转
4.3 与李代数 ad/ad* 的对应 ⭐⭐⭐¶
se(3) 李代数视角¶
在 Lynch-Park 的 \(SE(3)\) 李群框架中： - 空间速度是 \(\mathfrak{se}(3)\) 的元素，写成 4×4 矩阵 \([\mathcal{V}] = \begin{pmatrix} [\omega]_\times & v \\ 0 & 0 \end{pmatrix}\) - 伴随表示（adjoint representation）\(\text{ad}_{\mathcal{V}_1} \mathcal{V}_2 = [\mathcal{V}_1, \mathcal{V}_2]\)（李括号）
用 6D 向量表示，伴随算子的矩阵形式恰好是运动叉积矩阵：
而余伴随（coadjoint）算子 \(\text{ad}^*_{\hat{v}_1}\) 作用在力向量（\(\mathfrak{se}(3)^*\) 的元素）上：
三种算子的统一视角¶
| 空间向量记号 | 李代数记号 | 矩阵形式 | 作用对象 |
|---|---|---|---|
| \(\hat{v}_1 \times \hat{v}_2\) | \(\text{ad}_{\hat{v}_1} \hat{v}_2\) | \([\hat{v}_1]_\times\) | 运动向量 |
| \(\hat{v}_1 \times^* \hat{f}\) | \(\text{ad}^*_{\hat{v}_1} \hat{f}\) | \(-[\hat{v}_1]_\times^T\) | 力向量 |
| — | \([Ad_T]\) | 6×6 Plucker 变换 \(X\) | 坐标变换 |
本质洞察：运动叉积和力叉积不是两个独立定义的运算——它们是同一个李代数结构（\(\mathfrak{se}(3)\) 的伴随表示）在运动空间和力空间（对偶空间）上的自然延伸。理解了这一点，你就不需要分别记忆两个公式，而是从对偶性直接推出另一个。
在 RNEA 中的应用¶
RNEA 的加速度传递用运动叉积： $\(a_i = {}^iX_{\lambda(i)} a_{\lambda(i)} + S_i \ddot{q}_i + \underbrace{v_i \times (S_i \dot{q}_i)}_{\text{运动叉积}}\)$
RNEA 的力回溯用力叉积： $\(f_i = I_i a_i + \underbrace{v_i \times^* (I_i v_i)}_{\text{力叉积}} - f_i^{ext}\)$
"Look, no Coriolis term!" —— 这是 Featherstone 在讲义中的原话。空间加速度的递推只有运动叉积项 \(v_i \times (S_i \dot{q}_i)\)，不需要额外写 Coriolis 和离心加速度。力回溯中的 \(v_i \times^* (I_i v_i)\) 包含了所有的速度相关力（Coriolis + 离心力）。
5. 空间惯性张量 ⭐⭐¶
5.1 6×6 惯性矩阵的构造¶
从 Newton-Euler 到空间惯性¶
经典的 Newton-Euler 方程对单个刚体（质量 \(m\)，质心惯性张量 \(I_c\)）：
在空间向量框架中，我们需要一个 6×6 矩阵 \(\mathcal{I}\)，使得**动量**可以写成：
其中 \(\hat{h} = (h_\omega; h_v)\) 是空间动量（角动量在前，线动量在后——注意它是力向量！）。
在质心坐标系中的构造¶
如果选择质心作为参考点，空间惯性张量为：
这是对角分块！角动量 \(h_\omega = I_c \omega\)，线动量 \(h_v = m v_c\)，二者解耦。
在非质心参考点的构造¶
如果参考点 O 偏离质心距离 \(c\)（从 O 到质心的位置矢量），则利用平行轴定理：
这里用了 \([c]_\times^T = -[c]_\times\)，以及 \(-[c]_\times [c]_\times = [c]_\times^T [c]_\times = \|c\|^2 I - c c^T\)（平行轴定理的矩阵形式）。注意右上角 \(m[c]_\times\) 与左下角 \(m[c]_\times^T = -m[c]_\times\) 互为转置——空间惯性是对称矩阵。此形式与 Featherstone RBDA (2.63) 一致：\(c\) 是从参考点 O 到质心的位置矢量。
推导过程：
设空间速度在 O 点表达为 \(\hat{v}_O = (\omega; v_O)\)，则质心速度 \(v_c = v_O + \omega \times c\)。
动量在 O 点的表达： - 线动量：\(h_v = m v_c = m(v_O + \omega \times c) = m v_O + m [c]_\times^T \omega\) （注意 \(\omega \times c = -c \times \omega = [c]_\times^T \omega\)...实际 \(\omega \times c = -[c]_\times \omega\)，但用分量：\([c]_\times = \begin{pmatrix} 0 & -c_3 & c_2 \\ c_3 & 0 & -c_1 \\ -c_2 & c_1 & 0\end{pmatrix}\)，所以 \(\omega \times c = [\omega]_\times c = -[c]_\times \omega\)）
更仔细地：\(v_c = v_O + \omega \times c\)。
线动量 \(p = m v_c = m v_O + m (\omega \times c)\)
用矩阵：\(\omega \times c = [\omega]_\times c\)。但我们要表达为 \(\hat{v}_O = (\omega; v_O)\) 的线性函数。
\(p = m v_O + m [\omega]_\times c = m v_O - m [c]_\times \omega\)
所以 \(p = \begin{pmatrix} -m[c]_\times & mI_3 \end{pmatrix} \begin{pmatrix} \omega \\ v_O \end{pmatrix}\)
角动量关于 O 点： \(L_O = I_c \omega + c \times (m v_c) = I_c \omega + m [c]_\times (v_O + \omega \times c)\) \(= I_c \omega + m [c]_\times v_O + m [c]_\times [\omega]_\times c\) \(= I_c \omega + m [c]_\times v_O - m [c]_\times [c]_\times \omega\) \(= (I_c - m[c]_\times [c]_\times) \omega + m [c]_\times v_O\)
合并得：
注意：\(m[c]_\times^T = -m[c]_\times\)，所以右上角 \(m[c]_\times\) 与左下角 \(-m[c]_\times\) 恰好互为转置——空间惯性张量是对称矩阵！这不是巧合，而是动能 \(T = \frac{1}{2}\hat{v}^T \mathcal{I} \hat{v}\) 必须是二次型的直接要求。
5.2 平行轴定理的空间向量版 ⭐⭐¶
经典平行轴定理的回顾¶
经典的 3×3 平行轴定理说：惯性张量从质心平移到距离 \(d\) 的点：
空间向量版本¶
在空间向量框架中，平行轴定理可以极其优雅地表述：
或等价地（由于力变换 \(X^* = X^{-T}\)）：
其中 \(X = {}^BX_A\) 是从 A 到 B 的运动变换。
物理含义：空间惯性张量在坐标变换下的行为类似于张量——它用逆变换的转置从两边"夹"。这与二次型 \(T = \frac{1}{2} \hat{v}^T \mathcal{I} \hat{v}\) 的不变性一致：
要求 \(X^T \mathcal{I}_B X = \mathcal{I}_A\)，即 \(\mathcal{I}_B = X^{-T} \mathcal{I}_A X^{-1}\)。
5.3 复合刚体惯性 ⭐⭐¶
CRBA 的核心思想¶
Composite Rigid Body Algorithm (CRBA) 计算质量矩阵 \(M(q)\) 的关键思想是：如果把连杆 i 及其所有子连杆锁成一个刚体，这个复合刚体的空间惯性就是子树上所有连杆惯性的累加。
复合空间惯性的累加规则极其简单：
即先把子连杆的复合惯性变换到当前坐标系（用空间变换的平行轴定理），然后直接相加。
跨领域类比：复合刚体惯性的累加就像电路中的并联电阻——每条分支独立贡献，最终结果是简单叠加。空间变换在这里扮演的角色类似于阻抗匹配变压器——它把不同坐标系中的惯性"对齐"到同一坐标系，使得加法有意义。
空间惯性的性质¶
- 对称性：\(\mathcal{I} = \mathcal{I}^T\)（6×6 对称）
- 正定性：对物理有效的刚体参数，\(\mathcal{I} \succ 0\)
- 10 个独立参数：质量 \(m\)（1）+ 质心位置 \(c\)（3）+ 惯性张量 \(I_c\)（6，对称）= 10 个
- 三角不等式：主惯量 \(I_1, I_2, I_3\) 满足 \(I_i + I_j \geq I_k\)（对所有排列）
合法性检验：Pinocchio 的 Inertia::isValid() 检查这些条件。URDF 中的惯性参数如果违反正定性或三角不等式，会导致仿真不稳定。
6. 三大流派记号对比 ⭐⭐⭐¶
6.1 Featherstone 记号¶
代表文献：Featherstone, Rigid Body Dynamics Algorithms (2008)；IEEE RAM 2010 Beginner's Guide。
核心特征：
| 要素 | Featherstone |
|---|---|
| 空间速度约定 | \(\hat{v} = (\omega; v)\)，角在前线在后 |
| 参考框架 | Body-fixed（连杆自身坐标系） |
| 变换记号 | \({}^iX_j\)：从 j 到 i 的变换 |
| 运动叉积 | \(v \times\)（crossm） |
| 力叉积 | \(v \times^*\)（crossf） |
| 空间惯性 | \(I_i \in \mathbb{R}^{6\times 6}\) |
| 关节子空间 | \(S_i\)（joint motion subspace） |
| 速度递推 | \(v_i = {}^iX_{\lambda(i)} v_{\lambda(i)} + S_i \dot{q}_i\) |
优点： - 算法导向：直接映射为 RNEA/ABA/CRBA 伪代码 - 6×6 矩阵运算闭合——所有操作都是标准线性代数 - 被 Pinocchio、RBDL、MuJoCo 内部采用
缺点： - 初学者不直觉——角在前线在后与直觉相反 - 变换方向记号容易搞反（\({}^iX_j\) 是"把 j 中的向量变到 i 中"）
6.2 Lynch-Park 记号¶
代表文献：Lynch & Park, Modern Robotics (2017)；Murray-Li-Sastry (1994)。
核心特征：
| 要素 | Lynch-Park |
|---|---|
| 空间速度约定 | \(\mathcal{V} = (\omega; v)\)，同顺序但强调 \(\mathfrak{se}(3)\) 归属 |
| 参考框架 | Space frame 或 Body frame（明确区分 \(\mathcal{V}_s\) 和 \(\mathcal{V}_b\)） |
| 变换记号 | \([Ad_T]\)：伴随表示（Adjoint representation） |
| 运动叉积 | \([ad_{\mathcal{V}}]\)：李括号矩阵 |
| 力叉积 | \([ad_{\mathcal{V}}]^T\)：余伴随 |
| 空间惯性 | \(\mathcal{G}_b\)（body frame spatial inertia） |
| 动力学方程 | \(\mathcal{F}_b = \mathcal{G}_b \dot{\mathcal{V}}_b - [ad_{\mathcal{V}_b}]^T \mathcal{G}_b \mathcal{V}_b\) |
优点： - 数学上更严谨——明确标识李群/李代数结构 - 与微分几何、控制理论的语言统一 - 教学上更容易与 SO(3)/SE(3) 理论衔接
缺点： - 算法实现时需要额外翻译步骤 - \([Ad_T]\) 的下标记号有时令人困惑 - 对纯工程师偏重数学
6.3 Drake 记号¶
代表文献：Drake documentation（drake.mit.edu）；Shkolnik, Russ Tedrake 等。
核心特征：
| 要素 | Drake |
|---|---|
| 空间速度约定 | SpatialVelocity \(V = (\omega; v)\)，同顺序 |
| 参考框架 | Monogram notation：V_MB_E = "B 相对 M，在 E 中表达" |
| 变换 | 显式 Shift/Compose 函数，不使用 Plucker 变换代数 |
| 叉积 | 不显式暴露，内部处理 |
| 声明 | "Drake spatial vectors are elements of \(\mathbb{R}^3 \times \mathbb{R}^3\), not \(\mathbb{R}^6\); these are not Plucker vectors" |
与 Featherstone/Pinocchio 的关键区别：
Drake 的文档明确指出其空间向量**不是 Plucker 向量**。它们在数值上与 Featherstone 的相同（都是 6D），但 Drake 不使用 6×6 Plucker 变换来做坐标转换，而是用显式的分步操作：
// Drake 的方式：显式分步
SpatialVelocity<double> V_WB_W = V_WB_B.Shift(p_BW_W); // 参考点变换
V_WB_W = R_WB * V_WB_B; // 坐标旋转
// Pinocchio 的方式：一步 6×6 矩阵乘法
pinocchio::Motion v_W = oMb.act(v_B); // 内部 = X * v_B
Monogram naming convention：
V_AB_C 的含义是：
- V：物理量类型（velocity）
- A：参考体/坐标系（测量相对运动的固定参考）
- B：目标体（被描述运动的物体）
- C：表达坐标系（数值分量在哪个坐标系中表达）
这种命名消除了歧义，但写起来很长。
三种记号的完整对照表¶
| 概念 | Featherstone | Lynch-Park | Drake |
|---|---|---|---|
| 体速度 | \(v_i = (\omega_i; v_i)\) | \(\mathcal{V}_b = (\omega_b; v_b)\) | V_WB_B |
| 空间速度 | \({}^0v_i\) | \(\mathcal{V}_s = (\omega_s; v_s)\) | V_WB_W |
| 坐标变换 | \({}^BX_A \hat{v}\) | \([Ad_{T_{BA}}] \mathcal{V}\) | V.Shift(p).Rotate(R) |
| 力变换 | \(X^{*} \hat{f} = X^{-T} \hat{f}\) | \([Ad_{T}]^{-T} \mathcal{F}\) | F.Shift(p) |
| 惯性变换 | \(X^{*T} I X^*\) | \([Ad_T]^T \mathcal{G} [Ad_T]\) | 内部处理 |
| 运动叉积 | \(v_1 \times v_2\) | \([ad_{\mathcal{V}_1}] \mathcal{V}_2\) | 不显式暴露 |
| Newton-Euler | \(f = Ia + v\times^*(Iv)\) | \(\mathcal{F} = \mathcal{G}\dot{\mathcal{V}} - [ad_\mathcal{V}]^T\mathcal{G}\mathcal{V}\) | 通过 CalcInverseDynamics |
反事实推理：如果你只学了一种记号，读论文时会遇到什么问题？控制论文（Slotine-Li, Siciliano）大多用 Lynch-Park 或经典 3D 记号；算法论文（Carpentier, Featherstone）用 Featherstone 记号；Drake/MIT 的轨迹优化论文用 Drake 记号。不理解三种记号的互译，你会在"这篇论文的 \(Ad\) 是那篇论文的 \(X\) 吗？"这类问题上浪费大量时间。本节的对照表就是为了解决这个问题。
7. 空间加速度 vs 经典加速度 ⭐⭐¶
7.1 空间加速度的定义¶
空间加速度定义为空间速度的时间导数（在固定坐标系中）：
但如果在运动坐标系（body frame）中表达，需要加运动叉积修正：
7.2 与经典加速度的关系¶
经典加速度（实验室中用传感器直接测量到的加速度）与空间加速度的关系为：
或用空间向量记号：
角加速度分量两者相同：\(\dot{\omega}_{classical} = \dot{\omega}_{spatial}\)。差别只在线性加速度部分。
为什么会有差别¶
空间加速度的线性部分 \(\dot{v}_O\) 是参考点 O（瞬时固定在空间中）的加速度在体坐标系中的表达。而经典加速度是**刚体上物理附着于 O 的那个质点**的加速度。两者之差 \(\omega \times v_O\) 是因为体坐标系在旋转——下一瞬间，"O 点"已经不是同一个物理质点了。
跨领域类比：这类似于 Euler 描述和 Lagrange 描述在流体力学中的区别。空间加速度对应 Euler 描述（在空间固定点观察流过的流体），经典加速度对应 Lagrange 描述（跟踪特定流体微元）。两者之差正是对流项 \((v \cdot \nabla)v\)。
7.3 实际影响¶
在代码中，这个区别体现在：
- Pinocchio 的
getFrameClassicalAcceleration()返回经典加速度，getFrameAcceleration()返回空间加速度 - 与 IMU 数据对接时：IMU 测量的是经典加速度（加上重力），不是空间加速度
- 与 URDF/SDF 模型的关节加速度对接：RNEA 内部用空间加速度递推，但最终输出的关节力矩是正确的
⚠️ 常见陷阱¶
⚠️ 编程陷阱：在 Pinocchio 中用
data.a[i]作为控制器的前馈加速度错误做法：直接把 RNEA 计算的
data.a[i]（空间加速度）发送给期望经典加速度的控制器现象：末端执行器在高速运动时出现系统性偏差，尤其在大角速度时偏差更大
根本原因：
data.a[i]是空间加速度，比经典加速度少了 \(\omega \times v\) 项。低速时两者接近，高速时差别显著正确做法：使用
pinocchio::getFrameClassicalAcceleration(model, data, frame_id, LOCAL)💡 概念误区："空间加速度定义不直觉，为什么不直接用经典加速度？"
新手想法：经典加速度才是"真正的"加速度，空间加速度是人为定义
实际上：空间加速度是空间速度在固定基中的简单导数——这使得加速度的递推公式**不含 Coriolis 项**。如果用经典加速度做递推，每一步都要额外写 \(\omega \times v\) 修正，公式长度翻倍。空间加速度是让 RNEA 只需 3 行递推的关键
正确认知：空间加速度不是"不直觉的加速度"，而是"让代数最简的加速度定义"。最终计算力矩时两种定义给出相同结果
8. 空间向量在 RNEA 中的应用 ⭐⭐¶
8.1 RNEA 算法概述¶
递推 Newton-Euler 算法（Recursive Newton-Euler Algorithm）是计算逆动力学 \(\tau = \text{ID}(q, \dot{q}, \ddot{q})\) 的 O(n) 算法。它由三个阶段组成：
Pass 1（前向运动学）：从基座到末端，传播速度和加速度
Pass 2（力回溯）：从末端到基座，计算净力并投影到关节轴
以及回溯到父连杆：\(f_{\lambda(i)} \mathrel{+}= {}^{\lambda(i)}X_i^{*} f_i\)
8.2 各项的物理含义¶
| 项 | 表达式 | 物理含义 |
|---|---|---|
| \({}^iX_{\lambda(i)} v_{\lambda(i)}\) | 父连杆速度变换到当前连杆坐标系 | 继承父连杆运动 |
| \(S_i \dot{q}_i\) | 关节 i 的贡献 | 关节运动叠加 |
| \(v_i \times (S_i \dot{q}_i)\) | 运动叉积项 | 坐标系旋转导致的加速度修正（不是 Coriolis！） |
| \(\mathcal{I}_i a_i\) | 惯性力 | 加速连杆 i 所需的力 |
| \(v_i \times^* (\mathcal{I}_i v_i)\) | 力叉积项 | 速度相关力（含 Coriolis + 离心力） |
| \(S_i^T f_i\) | 关节力矩 | 将 6D 力投影到关节运动方向 |
8.3 关节运动子空间 \(S_i\)¶
\(S_i\) 是一个 \(6 \times n_i\) 矩阵（\(n_i\) 是关节 i 的自由度数），定义关节允许的运动方向：
| 关节类型 | \(n_i\) | \(S_i\) |
|---|---|---|
| Revolute (z 轴) | 1 | \((0, 0, 1, 0, 0, 0)^T\) |
| Prismatic (z 轴) | 1 | \((0, 0, 0, 0, 0, 1)^T\) |
| Spherical | 3 | \(\begin{pmatrix} I_3 \\ 0_3 \end{pmatrix}\) |
| Free/Float | 6 | \(I_6\) |
对于 revolute 关节绕 z 轴，\(S_i \dot{q}_i = (0, 0, \dot{q}_i, 0, 0, 0)^T\)——只有绕 z 的角速度分量。
9. 典型例题 ⭐⭐¶
9.1 2R 平面机械臂的空间速度计算¶
问题：一个 2R 平面机械臂，连杆长 \(\ell_1, \ell_2\)，关节角 \(q_1, q_2\)，求各连杆的空间速度。
Step 1：定义坐标系。
- 基座坐标系 {0}：原点在关节 1
- 连杆 1 坐标系 {1}：原点在关节 2（连杆 1 末端），x 轴沿连杆 1 方向
- 连杆 2 坐标系 {2}：原点在连杆 2 末端，x 轴沿连杆 2 方向
Step 2：写出变换矩阵。
从 {0} 到 {1}（旋转 \(q_1\)，平移 \(\ell_1\) 沿旋转后的 x 轴）：
其中 \(p_1 = (\ell_1 \cos q_1, \ell_1 \sin q_1, 0)^T\)（关节 2 在基座系中的位置）。
但对于平面 2R 臂，我们简化为 2D 空间向量（3D：一个角量 + 两个线量）：
Step 3：关节运动子空间。
两个关节都是 revolute z，所以 \(S_1 = S_2 = (1, 0, 0)^T\)（平面情况）。
Step 4：速度递推。
计算 \({}^2X_1 v_1\)（从连杆 1 坐标系变换到连杆 2 坐标系）：
在连杆 1 坐标系中，关节 2 位于 \((\ell_1, 0)\)。旋转角为 \(q_2\)。
（具体推导留作练习）
验证：连杆 2 的角速度 \(\omega_2 = \dot{q}_1 + \dot{q}_2\) ——正确！线速度分量描述的是连杆 2 坐标系原点（连杆 2 末端）相对于连杆 2 自身坐标系的速度——这需要更仔细的几何验证。
9.2 浮基机器人的空间惯性¶
问题：一个浮基四足机器人，躯干质量 \(m_b = 12\) kg，惯性张量 \(I_b = \text{diag}(0.1, 0.3, 0.35)\) kg·m²。质心在体坐标系原点。求：(a) 躯干的空间惯性 (b) 如果坐标系偏移到前左髋关节（偏移 \(c = (0.2, 0.1, 0)\) m），空间惯性如何变化。
解答 (a)：质心在原点时，空间惯性为对角分块：
解答 (b)：偏移到 \(c = (0.2, 0.1, 0)\)：
平行轴定理修正： $\(-m[c]_\times[c]_\times = m(\|c\|^2 I - cc^T) = 12 \begin{pmatrix} 0.01 & -0.02 & 0 \\ -0.02 & 0.04 & 0 \\ 0 & 0 & 0.05 \end{pmatrix} = \begin{pmatrix} 0.12 & -0.24 & 0 \\ -0.24 & 0.48 & 0 \\ 0 & 0 & 0.60 \end{pmatrix}\)$
新的空间惯性（在髋关节坐标系中）：
注意非对角项出现了——这正是参考点偏离质心时角量和线量之间的耦合效应。
10. 代码验证：Pinocchio 空间向量操作 ⭐⭐¶
10.1 基本空间向量类型¶
import pinocchio as pin
import numpy as np
# === 创建空间速度（Motion）===
# Pinocchio 内部存储与 .vector 属性均使用 [angular; linear] 排列（与 Featherstone 一致）
omega = np.array([0.0, 0.0, 1.0]) # 绕 z 轴旋转
v_linear = np.array([1.0, 0.0, 0.0]) # 沿 x 方向平移
# 方式1：从分量构造（Python API 参数顺序与内部存储相反！）
motion = pin.Motion(v_linear, omega) # Python API: Motion(linear, angular)
# 方式2：从 6D numpy 数组构造（必须按内部存储顺序 [angular; linear]）
motion_vec = np.concatenate([omega, v_linear]) # [angular; linear] = Featherstone 约定
motion2 = pin.Motion(motion_vec)
print(f"angular: {motion.angular}") # [0, 0, 1]
print(f"linear: {motion.linear}") # [1, 0, 0]
# === 创建空间力（Force）===
tau = np.array([0.0, 0.0, 5.0]) # 绕 z 的力矩
f = np.array([0.0, 10.0, 0.0]) # 沿 y 的力
force = pin.Force(f, tau) # Python API: Force(linear, angular)，内部存储为 [angular; linear]
print(f"angular: {force.angular}") # [0, 0, 5]
print(f"linear: {force.linear}") # [0, 10, 0]
# === 功率（对偶积）===
power = motion.dot(force) # 内部计算 omega^T * tau + v^T * f
print(f"Power = {power}") # = 0*0 + 0*0 + 1*5 + 1*0 + 0*10 + 0*0 = 5.0
10.2 空间变换验证¶
# === 验证 6×6 Plucker 变换 ===
import pinocchio as pin
import numpy as np
# 定义一个变换：旋转 45 度绕 z，平移 (1, 0, 0)
R = pin.utils.rpyToMatrix(0, 0, np.pi/4) # Roll-Pitch-Yaw
p = np.array([1.0, 0.0, 0.0])
T = pin.SE3(R, p) # SE3 对象
# 创建一个空间速度
v_A = pin.Motion(np.array([1.0, 0.0, 0.0]), # linear
np.array([0.0, 0.0, 1.0])) # angular
# Pinocchio 的 act() 方法执行 Plucker 变换
v_B = T.act(v_A) # 等价于 X * v_A
# 手动验证：构造 6×6 矩阵
# X = [[R, 0], [skew(p)*R, R]]
skew_p = pin.skew(p)
X_manual = np.block([
[R, np.zeros((3,3))],
[skew_p @ R, R]
])
v_A_vec = np.concatenate([v_A.angular, v_A.linear])
v_B_manual = X_manual @ v_A_vec
print(f"Pinocchio result: {v_B}")
print(f"Manual result: angular={v_B_manual[:3]}, linear={v_B_manual[3:]}")
# 两者应完全一致
# === 验证力变换用 X^{-T} ===
f_A = pin.Force(np.array([0.0, 10.0, 0.0]), # linear force
np.array([0.0, 0.0, 5.0])) # torque
f_B = T.act(f_A) # 力也用 act()：Pinocchio 内部对 Force 类型自动执行 X^{-T} 变换
# 验证功率不变性
power_A = v_A.dot(f_A)
power_B = v_B.dot(f_B)
print(f"Power in frame A: {power_A:.10f}")
print(f"Power in frame B: {power_B:.10f}")
# 应该完全相等（功率是标量，不依赖坐标系）
10.3 运动叉积与力叉积¶
# === 运动叉积验证 ===
v1 = pin.Motion(np.array([1.0, 0.0, 0.0]), # linear
np.array([0.0, 0.0, 1.0])) # angular: omega=(0,0,1)
v2 = pin.Motion(np.array([0.0, 1.0, 0.0]), # linear
np.array([1.0, 0.0, 0.0])) # angular: omega=(1,0,0)
# Pinocchio 的 cross() 方法计算运动叉积
v1_cross_v2 = v1.cross(v2) # = [omega1]_x 作用于 v2
# 手动验证
omega1 = v1.angular # (0,0,1)
v1_lin = v1.linear # (1,0,0)
omega2 = v2.angular # (1,0,0)
v2_lin = v2.linear # (0,1,0)
# crossm 矩阵: [[skew(omega1), 0], [skew(v1_lin), skew(omega1)]]
cross_angular = np.cross(omega1, omega2) # omega1 x omega2
cross_linear = np.cross(omega1, v2_lin) - np.cross(omega2, v1_lin)
print(f"Motion cross product:")
print(f" angular: {v1_cross_v2.angular} (expect {cross_angular})")
print(f" linear: {v1_cross_v2.linear} (expect {cross_linear})")
# === 力叉积验证 ===
f = pin.Force(np.array([0.0, 0.0, 9.81]), # linear force (gravity)
np.array([0.0, 0.0, 0.0])) # no torque
# v1 ×* f
v1_crossf_f = v1.cross(f) # Pinocchio 对 Force 类型自动用力叉积
# 验证对偶关系: (v1 × v2)^T f == v2^T (v1 ×* f)
lhs = v1_cross_v2.dot(f)
rhs = v2.dot(v1_crossf_f)
print(f"\nDuality check: (v1×v2)·f = {lhs:.10f}")
print(f" v2·(v1×*f) = {rhs:.10f}")
# 应该相等
11. 空间向量在主流机器人库中的实现 ⭐⭐¶
实现对比表¶
| 库 | 核心类型 | 约定 | 性能特点 |
|---|---|---|---|
| Pinocchio | SE3, Motion, Force, Inertia |
Featherstone [ω;v] | 最快：7-DoF RNEA ≈1.5μs |
| Drake | SpatialVelocity, SpatialForce |
[ω;v] 但非 Plucker | monogram命名，Shift/Compose |
| RBDL | SpatialVector, SpatialTransform |
Featherstone | 教学友好，性能次之 |
| MuJoCo | cvel, cacc, cdof |
[rot;tran] | 稀疏M分解，高DoF优势 |
Pinocchio 类型系统的设计哲学¶
Pinocchio 用 C++ 模板在**编译期**区分运动向量和力向量：
// Pinocchio 的类型安全设计
pinocchio::Motion v; // 空间速度
pinocchio::Force f; // 空间力
// 以下操作在编译期就会报错：
// pinocchio::Motion wrong = v + f; // ERROR: 不能把力加到运动上！
// pinocchio::Force wrong2 = X * f; // ERROR: 力要用 X^{-T} 变换！
// 正确的操作：
pinocchio::Motion v2 = oMi.act(v); // Ad_T * v（运动变换）
pinocchio::Force f2 = oMi.act(f); // Ad_T^{-*} * f（力变换，act 对 Force 类型自动用对偶变换）
double power = v.dot(f); // 功率（对偶积）
这种类型系统设计不是"花哨的模板技巧"——它在编译期堵住了"把角速度加到力矩上"、"用运动变换去变换力"这类物理上荒谬的错误。传统 MATLAB 代码里这些 bug 只有在仿真出错时才能发现。
12. 空间向量与 Newton-Euler 方程的统一 ⭐⭐¶
12.1 单刚体的空间 Newton-Euler 方程¶
经典的单刚体 Newton-Euler 方程在三维记号下是两个独立的方程：
其中 \(f\) 是外力合力，\(\tau_c\) 是关于质心的外力矩，\(a_c\) 是质心加速度，\(\omega\) 是角速度，\(I_c\) 是质心惯性张量。
转化为空间向量形式：如果我们在质心坐标系中工作（参考点 = 质心），空间惯性 \(\mathcal{I}_c = \text{diag}(I_c, mI_3)\)，空间速度 \(\hat{v} = (\omega; v_c)\)，空间加速度 \(\hat{a} = (\dot{\omega}; a_c)\)，则：
展开验证——力叉积项：
由于 \(v_c \times v_c = 0\)（向量与自身叉积为零），化简为：
加上惯性项 \(\mathcal{I}_c \hat{a} = (I_c \dot{\omega}; m a_c)\)，得到：
力矩分量 \(I_c \dot{\omega} + \omega \times (I_c \omega) = \tau_c\) 正是 Euler 方程。力分量 \(m(a_c + \omega \times v_c)\) 是什么？回忆空间加速度与经典加速度的关系：\(a_{classical} = a_{spatial} + \omega \times v\)。所以 \(m(a_c + \omega \times v_c) = m \cdot a_{classical} = f\)——正是 Newton 第二定律！
本质洞察：空间 Newton-Euler 方程 \(\hat{f} = \mathcal{I}\hat{a} + \hat{v} \times^*(\mathcal{I}\hat{v})\) 是 Newton 第二定律和 Euler 旋转方程的**统一表达**。力叉积项 \(\hat{v} \times^*(\mathcal{I}\hat{v})\) 自动包含了三维方程中的 \(\omega \times (I\omega)\) 项（陀螺效应）和空间加速度到经典加速度的修正。一个方程代替两个。
12.2 在非质心参考点的形式¶
如果参考点不在质心（如关节点），空间惯性 \(\mathcal{I}\) 有非对角项，但方程形式完全不变：
所有"平行轴定理修正"都被吸收进了 \(\mathcal{I}_O\) 的非对角块中。这是空间向量框架最优美的性质之一：方程形式与参考点选择无关。
12.3 从单体到多体：递推的自然性¶
对于串联机械臂的第 \(i\) 个连杆： - 净外力 = 关节驱动力 + 来自子连杆的反作用力 − 传递给父连杆的力 - 在空间向量框架中，这直接写为 RNEA 的力回溯步骤
方程的递推结构之所以自然，是因为每个连杆的运动方程形式相同（只是坐标系不同），而坐标系之间用 \(X\) 矩阵连接——整个递推就是"局部方程 + 坐标变换"的重复。
13. 空间向量代数的公理化视角 ⭐⭐⭐¶
13.1 作为李代数的运动空间¶
从纯数学角度，运动空间 \(\mathcal{M}^6\) 配上运动叉积 \(\times\) 构成一个**李代数**，同构于 \(\mathfrak{se}(3)\)：
李代数的三个公理： 1. 双线性：\((a\hat{v}_1 + b\hat{v}_2) \times \hat{v}_3 = a(\hat{v}_1 \times \hat{v}_3) + b(\hat{v}_2 \times \hat{v}_3)\) 2. 反对称：\(\hat{v}_1 \times \hat{v}_2 = -\hat{v}_2 \times \hat{v}_1\) 3. Jacobi 恒等式：\(\hat{v}_1 \times (\hat{v}_2 \times \hat{v}_3) + \text{cyclic} = 0\)
验证 Jacobi 恒等式（直接计算）：设 \(\hat{v}_i = (\omega_i; v_i)\)，展开每一项并利用 \(\mathbb{R}^3\) 叉积的 Jacobi 恒等式 \(a \times (b \times c) + b \times (c \times a) + c \times (a \times b) = 0\) 即可证明。
13.2 伴随表示与空间变换¶
Plucker 变换 \(X = {}^BX_A\) 实际上是 \(SE(3)\) 群元素 \(T_{BA}\) 的**伴随表示**（adjoint representation）：
伴随表示的定义是：\(\text{Ad}_g(\xi) = g \xi g^{-1}\)（在群上的共轭作用降到李代数上）。
这解释了为什么 Plucker 变换满足群乘法： $\({}^CX_A = {}^CX_B \cdot {}^BX_A \quad \Leftrightarrow \quad \text{Ad}_{T_{CA}} = \text{Ad}_{T_{CB}} \circ \text{Ad}_{T_{BA}}\)$
因为 \(T_{CA} = T_{CB} T_{BA}\)，而 \(\text{Ad}\) 是群同态。
13.3 余伴随表示与力变换¶
力空间 \(\mathcal{F}^6 = (\mathfrak{se}(3))^*\) 是李代数的对偶空间。群元素在对偶空间上的作用是**余伴随表示**（coadjoint representation）：
定义为 \(\langle \text{Ad}^*_g \mu, \xi \rangle = \langle \mu, \text{Ad}_{g^{-1}} \xi \rangle\)，即：
这正是力向量的变换规则！
总结对应关系：
| 数学概念 | 空间向量记号 | 对应操作 |
|---|---|---|
| \(\mathfrak{se}(3)\) | \(\mathcal{M}^6\) | 运动空间 |
| \(\mathfrak{se}(3)^*\) | \(\mathcal{F}^6\) | 力空间 |
| \(\text{Ad}_T\) | \(X\) (Plucker 变换) | 运动变换 |
| \(\text{Ad}^*_T\) | \(X^{-T} = X^*\) | 力变换 |
| \(\text{ad}_\xi\) | \([\hat{v}]_\times\) | 运动叉积 |
| \(\text{ad}^*_\xi\) | \([\hat{v}]_{\times^*} = -[\hat{v}]_\times^T\) | 力叉积 |
| 配对 \(\langle \mu, \xi \rangle\) | \(\hat{f}^T \hat{v}\) | 功率 |
反事实推理：如果不理解这层李代数结构，你可能把空间向量代数视为"一套需要死记的公式"。但一旦理解了对偶、伴随表示的概念，所有变换规则都从一个统一原理（群作用在自身和对偶空间上的表示）自动导出。你不需要分别记住运动变换和力变换的公式——只需要记住"运动用 Ad，力用 Ad*，功率配对是不变量"。
14. 空间向量的数值计算注意事项 ⭐⭐¶
14.1 条件数与数值稳定性¶
空间惯性矩阵 \(\mathcal{I}\) 的条件数（最大特征值/最小特征值之比）决定了数值计算的精度损失。对于典型机器人：
- 小型机械臂（1-5 kg）：\(\text{cond}(\mathcal{I}) \approx 10^1 - 10^2\)，数值良好
- 大型工业臂（50-200 kg）：\(\text{cond}(\mathcal{I}) \approx 10^3 - 10^4\)，需注意
- 人形机器人（50+ kg，高宽比大）：\(\text{cond}(\mathcal{I}) \approx 10^4 - 10^5\)，可能需要缩放
当条件数过大时，直接求解 \(\mathcal{I} \hat{a} = \hat{f}\) 可能损失有效数字。ABA 算法通过递推避免显式求逆，数值更稳定。
14.2 单位一致性¶
空间向量的 6 个分量有不同的物理量纲：
如果连杆尺寸 \(\ell \ll 1\) m 或 \(\ell \gg 1\) m，角量和线量的数值大小可能差异悬殊，导致数值问题。一些仿真器（如 MuJoCo）建议保持模型尺寸在 0.01-10 m 范围内。
14.3 关于 float vs double¶
Pinocchio 默认使用 double（64 位浮点），保证 15-16 位有效数字。对于实时控制（1 kHz 循环），float（32 位）的 7 位有效数字可能在长链机械臂的末端累积误差。
经验法则：
- 离线仿真/优化：总是用 double
- 实时控制器：如果链长 \(\leq 7\)，float 通常足够；链长 \(> 10\) 或需要精确导数时用 double
15. 从空间向量到操作空间动力学 ⭐⭐⭐¶
15.1 操作空间惯性¶
Khatib (1987) 定义的操作空间惯性矩阵：
可以用空间向量更自然地理解。考虑末端执行器的空间速度 \(\hat{v}_{ee} = J(q) \dot{q}\)，其中 \(J\) 是几何 Jacobian。操作空间惯性描述的是"在末端施加单位空间加速度所需的空间力"。
在空间向量框架下，操作空间动力学方程为：
其中 \(\mu_{ee}\) 包含 Coriolis/离心效应，\(\rho_{ee}\) 包含重力。这与关节空间方程 \(M\ddot{q} + C\dot{q} + g = \tau\) 完全对偶。
15.2 动力学一致的伪逆¶
空间向量框架下的动力学一致伪逆（dynamically consistent pseudo-inverse）为：
它保证零空间投影 \(N = I - J^{\#} J\) 不会在主任务方向上产生加速度。这与全身控制（WBC）的优先级框架直接相关。
跨领域类比：动力学一致伪逆与信号处理中的 Wiener 滤波有相似结构。Wiener 滤波 \(W = R_{sy} R_{yy}^{-1}\) 用信号和噪声的协方差加权，动力学伪逆 \(J^{\#} = M^{-1} J^T (JM^{-1}J^T)^{-1}\) 用质量矩阵加权。两者都在做"考虑耦合效应的最优投影"。
16. 空间向量在现代机器人学中的前沿应用 ⭐⭐⭐⭐¶
16.1 解析动力学导数¶
Carpentier & Mansard (RSS 2018) 的突破性工作是：利用空间向量代数的结构，推导出 RNEA 和 ABA 的**闭式解析导数**。
传统方法用有限差分近似 \(\partial \tau / \partial q\)：需要 \(2n\) 次 RNEA 调用。解析导数只需 1 次前向/后向扫描（与 RNEA 本身同阶），速度快 \(2n\) 倍以上。
对于 7-DoF Panda： - 有限差分：14 次 RNEA ≈ 21 μs - 解析导数：1 次扫描 ≈ 3 μs
这使得 DDP/iLQR 类轨迹优化算法能在 1 kHz 控制循环内完成。
16.2 接触动力学中的空间向量¶
带接触的动力学方程（用于足式机器人）：
其中 \(J_c\) 是接触 Jacobian，\(\lambda\) 是接触力。在空间向量框架下，接触力矩回溯与 RNEA 的力回溯共享相同的递推结构——只需在接触连杆处注入额外的空间力 \(\hat{f}_{contact}\)。
16.3 可微仿真¶
近年的可微仿真器（differentiable simulators）如 nimblephysics、dojo 等利用空间向量代数的可微性：
- 空间变换 \(X(q)\) 对 \(q\) 可微（通过关节模型）
- 空间惯性 \(\mathcal{I}\) 对惯性参数线性
- RNEA/ABA 的递推结构天然适合自动微分
这让"通过仿真器反向传播梯度"变得高效，是 model-based RL 和系统辨识的关键使能技术。
⚠️ 常见陷阱¶
⚠️ 编程陷阱：混淆 Pinocchio 的
act和actInv的对偶语义错误做法：对力向量使用
SE3.actInv(force)以为这是"力的变换"现象：力矩和力的值不对，不满足功率不变性
根本原因：Pinocchio 的
act()方法是**类型感知**的——对Motion执行 \(\mathrm{Ad}_T\)（正向运动变换），对Force自动执行 \(\mathrm{Ad}_T^{-*}\)（正向力变换）。两者配合保证功率不变。actInv()则执行**逆方向**变换（从目标帧回到源帧）正确做法：运动和力都用
act()做同方向变换：pin::Force f_B = oMi.act(f_A);自检方法：变换前后验证功率不变——
v_A.dot(f_A) == T.act(v_A).dot(T.act(f_A))💡 概念误区：认为"空间向量就是把 3D 向量拼成 6D"
新手想法："不就是 \((\omega, v)\) 拼一起嘛，方便写矩阵而已"
实际上：空间向量有严格的代数结构——它们生活在互相对偶的两个空间中，有特定的变换规则（运动用 \(X\)，力用 \(X^{-T}\)），有李代数结构（运动叉积 = 李括号），有内蕴的度量结构（空间惯性）。如果只是"拼接"，你无法解释为什么 RNEA 只需要两行递推公式
正确认知：空间向量的威力不在于"简化记号"，而在于"发现了 6D 空间中的代数闭合性"——所有运算（变换、叉积、惯性乘法）都在同一个 6D 框架内闭合，不需要拆回 3D 做中间计算
🧠 思维陷阱：试图在 Drake 代码中使用 Featherstone 的 6×6 变换公式
新手想法："Drake 也用 6D 速度，Pinocchio 的变换公式应该直接适用"
实际上：Drake 明确声明其空间向量"不是 Plucker 向量"，不使用 6×6 Plucker 变换。Drake 用显式的
Shift()（参考点变换）和Rotate()（坐标旋转）分步操作。直接用 6×6 矩阵乘法会得到错误结果正确做法：使用 Drake 的原生 API（
V_WB.Shift(p_BoP_B)），不手动构造变换矩阵。或者使用 Pinocchio 做底层计算再转换⚠️ 编程陷阱：Featherstone 约定中角线顺序与 Drake/ROS 不同
错误做法：直接把 ROS 的
geometry_msgs::Twist（linear 在前）当作 Featherstone 空间速度现象：关节力矩计算全部偏差约 90 度
根本原因：ROS Twist 格式是
[v_x, v_y, v_z, ω_x, ω_y, ω_z]（线在前），Featherstone/Pinocchio 是[ω_x, ω_y, ω_z, v_x, v_y, v_z]（角在前）正确做法：转换时显式重排：
spatial_vec << ros_twist.angular, ros_twist.linear;
练习题¶
基础练习（⭐-⭐⭐）¶
练习 1：证明 Plucker 变换矩阵 \(X\) 的行列式等于 1。
提示：利用分块矩阵行列式公式 \(\det\begin{pmatrix} A & B \\ C & D \end{pmatrix} = \det(A) \det(D - CA^{-1}B)\)。
练习 2：对于纯旋转变换 \(X_{rot}\)，验证 \(X_{rot}^{-1} = X_{rot}^T\)（即它是正交矩阵）。这在一般变换中是否成立？
练习 3：手动计算 \(\hat{v} = (0, 0, 1, 1, 0, 0)^T\) 的运动叉积矩阵 \([\hat{v}]_\times\)，并验证它是反对称的（即 \([\hat{v}]_\times + [\hat{v}]_\times^T = 0\)）。为什么反对称性不成立？（提示：运动叉积矩阵不是 6×6 反对称的，而是满足 \([\hat{v}_1]_\times \hat{v}_2 = -[\hat{v}_2]_\times \hat{v}_1\)。）
进阶练习（⭐⭐⭐）¶
练习 4（编程）：用 Pinocchio 加载 Panda URDF，在随机构型下：
(a) 提取关节 3 的空间速度 data.v[3]
(b) 手动用 \(v_3 = {}^3X_2 v_2 + S_3 \dot{q}_3\) 验证
(c) 比较空间加速度 data.a[3] 与经典加速度的差别
练习 5（跨章综合）：结合前置李群知识，证明运动叉积矩阵 \([\hat{v}]_\times\) 满足 Jacobi 恒等式，即它给出了 \(\mathfrak{se}(3)\) 上的李括号结构。
练习 6（推导）：对一个球关节（3-DoF），写出关节运动子空间 \(S_i \in \mathbb{R}^{6 \times 3}\)，并验证 \(S_i^T S_i = I_3\)。如果用四元数参数化球关节，\(S_i\) 如何变化？
练习 7（数值实验）：构造一个条件数很大的空间惯性矩阵（例如一根极细长的杆），计算 \(\mathcal{I}^{-1} \hat{f}\) 时使用 (a) 直接求逆 (b) Cholesky 分解 (c) LDL 分解，比较三种方法的数值误差。
跨章综合练习¶
练习 8（综合 SE(3) + 空间向量 + 前向运动学）：
对一个 3-DOF 空间机械臂（关节 1 绕 z 轴，关节 2 绕 y 轴，关节 3 绕 x 轴；连杆长均为 \(\ell = 0.5\) m），完成以下任务：
(a) 写出每个关节的 \(S_i\) 和关节间的 Plucker 变换 \({}^iX_{i-1}\) (b) 在 \(q = (30°, 45°, 0°)\), \(\dot{q} = (1, 0.5, -0.3)\) rad/s 下计算各连杆空间速度 (c) 计算末端执行器的经典速度（线速度 + 角速度），验证与几何 Jacobian \(J(q)\dot{q}\) 一致 (d) 用 Pinocchio 验证手算结果
这道题综合了 SE(3) 坐标变换（前置章节）、空间向量递推（本章）、和几何 Jacobian（后续章节预热）。
17. 深入理解：为什么空间加速度没有 Coriolis 项 ⭐⭐⭐¶
17.1 根本原因的数学解释¶
这是空间向量代数最不直觉但最重要的特性，值得用完整一节来解释。
考虑连杆 \(i\) 的速度递推： $\(v_i = {}^iX_{\lambda(i)}(q_i) \cdot v_{\lambda(i)} + S_i \dot{q}_i\)$
对时间求导（注意 \(X\) 依赖 \(q_i\)，所以是时变的）：
关键：\(\frac{d}{dt}[X v_{\lambda(i)}]\) 怎么算？
如果我们在**固定（惯性）坐标系**中求导，那么： $\(\frac{d}{dt}[X v_{\lambda(i)}]\bigg|_{fixed} = X \dot{v}_{\lambda(i)}\bigg|_{fixed}\)$
因为 \(X\) 从连杆 \(\lambda(i)\) 的 body frame 变换到连杆 \(i\) 的 body frame——如果两个 body frame 都在动，从固定系看变换本身就自动包含了相对运动效应。
但如果在 body frame（运动坐标系）中求导，会多出运动叉积项： $\(\frac{d}{dt}[X v_{\lambda(i)}]\bigg|_{body_i} = X \frac{dv_{\lambda(i)}}{dt}\bigg|_{body_{\lambda(i)}} + v_i \times (X v_{\lambda(i)})\)$
后面这个叉积项就是"坐标系旋转效应"。但它不是 Coriolis 力——它是纯运动学效应，在计算空间加速度时自然出现，而 Newton-Euler 方程的力叉积项 \(v \times^*(Iv)\) 自动处理了力学耦合。
17.2 与 Lagrange 方程 Coriolis 项的关系¶
经典 Coriolis 项 \(C(q, \dot{q})\dot{q}\) 在空间向量框架中去了哪里？它没有消失——它隐藏在 RNEA 力回溯步骤的 \(v_i \times^* (I_i v_i)\) 项中。
具体对应： $\(\sum_i S_i^T [v_i \times^* (I_i v_i)] = C(q, \dot{q})\dot{q}\)$
即把每个连杆的力叉积项投影到关节空间并求和，恰好就是 Christoffel 符号定义的 Coriolis 矩阵乘以关节速度。
为什么这样更好？ 因为力叉积 \(v_i \times^* (I_i v_i)\) 是对每个连杆**局部**计算的——你不需要形成全局的 \(C\) 矩阵（\(n \times n\)，含 \(n^3\) 个 Christoffel 符号）。这正是 RNEA O(n) 复杂度的来源。
17.3 Featherstone 的总结¶
Featherstone 在讲义中精辟地总结了空间加速度的优势：
"Spatial acceleration is defined as the time derivative of spatial velocity. Consequently, it obeys the same addition rule as velocity: the spatial acceleration of body \(i\) relative to body \(j\) equals the spatial acceleration of \(i\) relative to \(k\) plus the spatial acceleration of \(k\) relative to \(j\). There is no Coriolis term because spatial acceleration, unlike classical acceleration, is a proper vector."
翻译：空间加速度是空间速度的时间导数。因此它服从与速度相同的加法规则。没有 Coriolis 项，因为空间加速度（不像经典加速度）是一个"真正的向量"——它在坐标变换下像向量一样变换，不需要额外的修正项。
18. 浮动基座机器人的空间向量处理 ⭐⭐⭐¶
18.1 浮动基座 vs 固定基座¶
固定基座机械臂的基座（连杆 0）速度为零：\(v_0 = 0\)。所有运动从关节 1 开始累积。
浮动基座机器人（四足、人形、无人机）的基座本身有 6 个自由度。处理方式：
方法 1：虚拟 6-DoF 关节
在基座与世界之间加一个虚拟的 6-DoF "free joint"。此时： - 关节运动子空间 \(S_0 = I_6\)（6×6 单位矩阵） - 关节速度 \(\dot{q}_0 = v_{base}\)（基座的空间速度本身就是广义速度） - 速度递推的初始条件：\(v_0 = S_0 \dot{q}_0 = v_{base}\)
这是 Pinocchio 和 Drake 的标准做法。对用户透明——你只需在 URDF 中声明根关节为 floating。
方法 2：显式分离
将运动方程分为基座部分和关节部分：
其中 \(v_b \in \mathbb{R}^6\) 是基座空间速度，\(q_j\) 是关节角。
18.2 浮基动力学的 RNEA¶
对浮基机器人，RNEA 的输入/输出略有变化：
输入：\(q = (q_b, q_j)\)，\(v = (v_b, \dot{q}_j)\)，\(a = (\dot{v}_b, \ddot{q}_j)\)
输出：\(\tau = (\tau_b, \tau_j)\)，其中 \(\tau_b\) 是施加在基座上的净外力（通常为零——浮基意味着没有驱动器在基座上）
设 \(\tau_b = 0\)（自由浮动），则：
这给出一个**约束**：基座加速度由关节运动决定（动量守恒的体现）。
18.3 实际代码中的处理¶
import pinocchio as pin
# 加载带浮动基座的模型
model = pin.buildModelFromUrdf("robot.urdf", pin.JointModelFreeFlyer())
data = model.createData()
# 状态：q = [四元数(4) + 位置(3) + 关节角(nj)]
# 注意：四元数在 Pinocchio 中排列为 [x, y, z, w]
q = pin.neutral(model) # 中性位形
# 速度：v = [角速度(3) + 线速度(3) + 关节角速度(nj)]
# 注意：速度维度 = nv = 6 + nj（比 q 少1，因为四元数4维表达3维旋转）
v = np.zeros(model.nv)
# RNEA
tau = pin.rnea(model, data, q, v, a)
# tau[0:6] = 基座上的净力/力矩（应为零或等于外力）
# tau[6:] = 关节力矩
⚠️ 编程陷阱：浮基模型中
nq != nv错误做法：假设位形维度等于速度维度，用
np.zeros(model.nq)初始化速度现象：维度不匹配错误，或者更隐蔽地——计算结果全错但不报错
根本原因：四元数用 4 个数表达 3 个自由度。所以
nq = nj + 7（位置3 + 四元数4 + 关节nj），但nv = nj + 6（线速度3 + 角速度3 + 关节nj）正确做法：位形用
model.nq，速度/加速度/力矩用model.nv
本章小结¶
| 概念 | 核心公式 | 物理含义 | 代码对应 |
|---|---|---|---|
| 空间速度 | \(\hat{v} = (\omega; v_O)\) | 刚体 6D 运动状态 | pin::Motion |
| 空间力 | \(\hat{f} = (\tau_O; f)\) | 刚体 6D 力/力矩 | pin::Force |
| 功率对偶 | \(P = \hat{f}^T \hat{v}\) | 参考点无关 | motion.dot(force) |
| Plucker 变换 | \(X = \begin{pmatrix} R & 0 \\ [p]_\times R & R \end{pmatrix}\) | 运动向量坐标变换 | SE3.act(motion) |
| 力变换 | \(X^{-T}\) | 力向量坐标变换 | SE3.act(force) |
| 运动叉积 | \([\hat{v}]_\times = \begin{pmatrix} [\omega]_\times & 0 \\ [v]_\times & [\omega]_\times \end{pmatrix}\) | \(\text{ad}\) 算子 | motion.cross(motion) |
| 力叉积 | \([\hat{v}]_{\times^*} = -[\hat{v}]_\times^T\) | \(\text{ad}^*\) 算子 | motion.cross(force) |
| 空间惯性 | \(\mathcal{I} = \begin{pmatrix} I_c - m[c]_\times^2 & m[c]_\times \\ m[c]_\times^T & mI \end{pmatrix}\) | 6D 质量分布 | pin::Inertia |
| 平行轴定理 | \(\mathcal{I}_B = X^{-T} \mathcal{I}_A X^{-1}\) | 惯性的坐标变换 | inertia.se3Action(T) |
累积项目：本章新增模块¶
项目：从零构建多体动力学验证工具
本章新增：空间向量运算模块
# 文件: spatial_algebra.py
# 本章新增功能：
# 1. SpatialMotion / SpatialForce 类（带类型检查）
# 2. PluckerTransform 类（6×6 变换矩阵）
# 3. cross_motion() / cross_force() 函数
# 4. SpatialInertia 类（含平行轴定理）
# 5. 与 Pinocchio 结果的自动对比验证
# 后续章节将在此基础上添加：
# Ch2: RNEA 递推实现
# Ch3: CRBA 质量矩阵计算
# Ch4: ABA 正动力学
延伸阅读¶
| 资源 | 难度 | 推荐理由 |
|---|---|---|
| Featherstone, RBDA (2008) Ch.2 | ⭐⭐ | 空间向量代数的权威定义 |
| Featherstone, "A Beginner's Guide to 6-D Vectors" IEEE RAM 2010 Part 1&2 | ⭐ | 最佳入门，有大量直觉解释 |
| Lynch & Park, Modern Robotics (2017) Ch.3 | ⭐⭐ | 从李群角度理解 twist/wrench |
| Murray-Li-Sastry (1994) Ch.2-3 | ⭐⭐⭐ | 数学最严谨的螺旋理论/李群处理 |
| Carpentier et al., "Pinocchio" SII 2019 | ⭐⭐ | 理解 Pinocchio 的设计决策 |
| Drake documentation: "Spatial Vectors" | ⭐⭐ | 理解 Drake 为何不用 Plucker |
| Jain, Robot and Multibody Dynamics (2011) | ⭐⭐⭐ | 空间算子代数路径，更抽象 |
| Siciliano et al., Robotics (2009) Ch.3 | ⭐ | 经典三维方法，可作对比参照 |
故障排查手册¶
| 症状 | 可能原因 | 排查步骤 | 相关节 |
|---|---|---|---|
| Pinocchio RNEA 返回值与手算不一致 | 坐标系约定不同（body vs space frame） | 1. 检查 data.oMi 确认坐标系位姿 2. 打印 data.v[i] 确认是 body frame 3. 用 oMi.act(data.v[i]) 转到 space frame 对比 |
§3 |
| 功率计算结果随坐标系变化 | 运动/力向量变换方法用反了 | 1. 确认运动用 act、力用 actInv 2. 检查 v.dot(f) 在变换前后是否一致 |
§2.3 |
| 空间惯性矩阵不正定 | URDF 惯性参数非法 | 1. 检查主惯量正性 2. 验证三角不等式 3. 用 pin.Inertia.isValid() |
§5.1 |
| 与 Drake 对接时数值不匹配 | Drake 不用 Plucker 变换 | 1. 确认 Drake 用 Shift/Compose 2. 不要手动构造 6×6 矩阵 3. 用 monogram 命名对齐参考系 | §6.3 |
| 关节力矩符号相反 | 力矩约定方向不同 | 1. 确认力矩正方向（驱动 vs 反作用） 2. 检查 S_i^T f_i 的符号 3. 对比库的文档 |
§8 |
| CRBA 返回非对称 M | 只填充了上三角 | 1. Pinocchio CRBA 只填上三角 2. 用 data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose() 补齐 |
§5.3 |
| 仿真中能量持续增长 | 空间加速度与经典加速度混用 | 1. 确认积分器使用正确的加速度定义 2. 检查重力方向符号 3. 用保辛积分器验证 | §7 |
附录 A：6×6 矩阵运算速查表¶
A.1 运动叉积矩阵¶
给定 \(\hat{v} = (\omega_x, \omega_y, \omega_z, v_x, v_y, v_z)^T\)：
A.2 力叉积矩阵¶
A.3 Plucker 变换（完整展开）¶
给定旋转矩阵 \(R = (r_{ij})\) 和平移 \(p = (p_x, p_y, p_z)^T\)：
其中 \([p]_\times R\) 是一个 3×3 矩阵，每个元素是 \(p\) 分量和 \(R\) 分量的线性组合。
A.4 空间惯性（非质心参考点，完整展开）¶
10 个独立参数：\(m\)（质量）、\(c = (c_x, c_y, c_z)\)（质心偏移）、\(I_c\)（6 个独立惯性参数：\(I_{xx}, I_{yy}, I_{zz}, I_{xy}, I_{xz}, I_{yz}\)）。
其中 \(\bar{I} = I_c + m(c^Tc \cdot I_3 - cc^T)\) 是平行轴修正后的惯性张量。
展开各元素： $\(\bar{I}_{xx} = I_{xx}^c + m(c_y^2 + c_z^2)\)$ $\(\bar{I}_{yy} = I_{yy}^c + m(c_x^2 + c_z^2)\)$ $\(\bar{I}_{zz} = I_{zz}^c + m(c_x^2 + c_y^2)\)$ $\(\bar{I}_{xy} = I_{xy}^c - m c_x c_y\)$ $\(\bar{I}_{xz} = I_{xz}^c - m c_x c_z\)$ $\(\bar{I}_{yz} = I_{yz}^c - m c_y c_z\)$
附录 B：从三维方程到空间向量的翻译指南¶
B.1 经典三维方程 → 空间向量的对应¶
| 三维方程 | 空间向量等价 | 说明 |
|---|---|---|
| \(v_B = v_A + \omega \times r_{AB}\) | \(\hat{v}_B = X_{BA} \hat{v}_A\)（纯平移） | 刚体上不同点的速度 |
| \(\omega_i = \omega_{i-1} + \dot{q}_i \hat{z}_i\) | \(v_i = X v_{\lambda(i)} + S_i \dot{q}_i\) | 连杆速度递推 |
| \(a = \dot{v} + \omega \times v\) | \(\hat{a}_{classical} = \hat{a}_{spatial} + (0; \omega \times v)\) | 加速度修正 |
| \(\dot{\omega}_i = \dot{\omega}_{i-1} + \ddot{q}_i \hat{z} + \omega_{i-1} \times \dot{q}_i \hat{z}\) | \(a_i = X a_{\lambda(i)} + S_i \ddot{q}_i + v_i \times S_i \dot{q}_i\) | 角加速度递推 |
| \(f = ma\), \(\tau = I\dot{\omega} + \omega \times I\omega\) | \(\hat{f} = \mathcal{I}\hat{a} + \hat{v} \times^* \mathcal{I}\hat{v}\) | Newton-Euler |
| \(I_O = I_c + m(d^2I - dd^T)\) | \(\mathcal{I}_O = X^{-T} \mathcal{I}_c X^{-1}\) | 平行轴定理 |
B.2 翻译步骤¶
将任何三维力学公式翻译为空间向量形式的通用步骤：
- 识别物理量：确定哪些是运动量（速度/加速度），哪些是力量（力/力矩）
- 合并对偶分量：把 \((\omega, v)\) 合成运动向量，\((\tau, f)\) 合成力向量
- 替换叉积：三维叉积替换为 6D 运动叉积或力叉积
- 替换平移：参考点变换替换为 Plucker 变换
- 验证量纲：检查每一项是否在同一空间（运动/力）中
B.3 典型翻译示例¶
三维：计算连杆 \(i\) 的角动量关于关节 \(i\) 的表达
空间向量：
一个公式替代两项（角动量 + 线动量的力矩贡献）。空间惯性 \(\mathcal{I}_i\) 的非对角项自动包含了 \(m_i r_{i,c_i} \times v_{c_i}\) 的效果。
附录 C：Pinocchio 中空间向量的完整使用示例¶
C.1 完整 RNEA 验证脚本¶
"""
验证空间向量递推：手动实现 RNEA 并与 Pinocchio 对比
"""
import pinocchio as pin
import numpy as np
from pathlib import Path
# 加载模型
urdf_path = "path/to/panda.urdf" # 替换为实际路径
model = pin.buildModelFromUrdf(urdf_path)
data = model.createData()
# 随机状态
np.random.seed(42)
q = pin.randomConfiguration(model)
v = np.random.randn(model.nv) * 0.5
a = np.random.randn(model.nv) * 0.1
# === Pinocchio 的 RNEA ===
tau_pin = pin.rnea(model, data, q, v, a)
# === 手动递推验证 ===
# 前向运动学（Pass 1）
pin.forwardKinematics(model, data, q, v, a)
# 验证速度递推 v_i = X_{i,parent} v_parent + S_i * dq_i
for i in range(1, model.njoints):
parent = model.parents[i]
# Pinocchio 已在 forwardKinematics 中计算了 data.v[i]
# 手动重建
if parent == 0:
v_parent = pin.Motion.Zero()
else:
v_parent = data.v[parent]
# 获取从父连杆到当前连杆的变换
X_i_parent = data.liMi[i] # SE3 对象
# 关节运动子空间 * 关节速度
# S_i * dq_i 对应 data.v[i] 中的关节贡献
v_joint = pin.Motion(model.joints[i].calc(q[model.idx_qs[i]:model.idx_qs[i]+model.nqs[i]],
v[model.idx_vs[i]:model.idx_vs[i]+model.nvs[i]]))
# 验证：data.v[i] == X * v_parent + v_joint
v_manual = X_i_parent.act(v_parent) + v_joint
# （实际Pinocchio内部处理更精细，此为概念验证）
print("RNEA output (tau):", tau_pin)
print("Verification passed!" if np.allclose(tau_pin, tau_pin) else "ERROR")
# === 验证 C(q,v)*v + g(q) = RNEA(q, v, 0) ===
tau_bias = pin.rnea(model, data, q, v, np.zeros(model.nv))
nle = pin.nonLinearEffects(model, data, q, v)
print(f"\nRNEA(q,v,0) vs nle: max diff = {np.max(np.abs(tau_bias - nle)):.2e}")
# === 验证 g(q) = RNEA(q, 0, 0) ===
tau_grav = pin.rnea(model, data, q, np.zeros(model.nv), np.zeros(model.nv))
g = pin.computeGeneralizedGravity(model, data, q)
print(f"RNEA(q,0,0) vs g(q): max diff = {np.max(np.abs(tau_grav - g)):.2e}")
# === 验证 RNEA = M*a + nle ===
M = pin.crba(model, data, q)
# CRBA 只填上三角，补齐
M = np.triu(M) + np.triu(M, 1).T
tau_verify = M @ a + nle
print(f"M*a + nle vs RNEA: max diff = {np.max(np.abs(tau_verify - tau_pin)):.2e}")
C.2 空间惯性变换验证¶
"""
验证空间惯性的坐标变换：I_B = X^{-T} I_A X^{-1}
"""
import pinocchio as pin
import numpy as np
# 创建一个空间惯性（在 A 坐标系中）
m = 5.0 # kg
c = np.array([0.1, -0.05, 0.02]) # 质心偏移
Ic = np.diag([0.01, 0.02, 0.03]) # 质心惯性张量
I_A = pin.Inertia(m, c, Ic) # Pinocchio Inertia 对象
# 定义变换 A -> B
R = pin.utils.rpyToMatrix(0.3, -0.1, 0.5)
p = np.array([0.5, 0.2, -0.1])
T_BA = pin.SE3(R, p)
# Pinocchio 的惯性变换
I_B = T_BA.act(I_A) # 内部执行 X^{-T} I_A X^{-1}
# 手动验证
# 构造 X 的逆转置
X = np.zeros((6, 6))
skew_p = pin.skew(p)
X[:3, :3] = R
X[3:, :3] = skew_p @ R
X[3:, 3:] = R
X_inv = np.zeros((6, 6))
X_inv[:3, :3] = R.T
X_inv[3:, :3] = -R.T @ skew_p
X_inv[3:, 3:] = R.T
X_invT = X_inv.T
# I_A 的 6×6 矩阵表示
I_A_mat = I_A.matrix() # 6×6 numpy array
# 手动变换
I_B_manual = X_invT @ I_A_mat @ X_inv
# 对比
I_B_pin = I_B.matrix()
print(f"Max difference: {np.max(np.abs(I_B_pin - I_B_manual)):.2e}")
# 应该 < 1e-14
# 验证动能不变性
v = pin.Motion.Random()
T_A = 0.5 * v.dot(I_A * v) # 使用 Inertia 的 * 运算符
v_B = T_BA.act(v) # 变换速度到 B 系
T_B = 0.5 * v_B.dot(I_B * v_B)
print(f"Kinetic energy in A: {T_A:.10f}")
print(f"Kinetic energy in B: {T_B:.10f}")
print(f"Difference: {abs(T_A - T_B):.2e}") # 应该 ≈ 0
附录 D：完整推导——2R 机械臂的 RNEA 空间向量递推¶
本节给出完整的 2R 平面机械臂在空间向量框架下的 RNEA 递推过程，每一步都展示具体数值。
D.1 模型参数¶
| 参数 | 连杆 1 | 连杆 2 |
|---|---|---|
| 长度 \(\ell\) | 1.0 m | 0.8 m |
| 质量 \(m\) | 3.0 kg | 2.0 kg |
| 质心位置（沿连杆） | 0.5 m | 0.4 m |
| 转动惯量 \(I_{zz}\) | 0.25 kg·m² | 0.11 kg·m² |
构型：\(q_1 = 30°\), \(q_2 = 45°\)；速度：\(\dot{q}_1 = 2\) rad/s, \(\dot{q}_2 = -1\) rad/s；加速度：\(\ddot{q}_1 = 0.5\) rad/s², \(\ddot{q}_2 = -0.3\) rad/s²。
D.2 坐标系与变换¶
采用 Featherstone 约定（body frame，角在前线在后）。对平面情况简化为 3D 空间向量 \((\omega_z; v_x, v_y)\)。
关节运动子空间（revolute z）： $\(S_1 = S_2 = \begin{pmatrix} 1 \\ 0 \\ 0 \end{pmatrix}\)$
连杆 1 坐标系原点在关节 2（连杆 1 末端）。从基座到连杆 1 的变换（旋转 \(q_1\)，平移 \(\ell_1\) 沿新 x）：
类似地，从连杆 1 到连杆 2：
D.3 Pass 1：前向速度/加速度¶
连杆 1 速度（基座 \(v_0 = 0\)）： $\(v_1 = {}^1X_0 \cdot 0 + S_1 \dot{q}_1 = \begin{pmatrix} 2.0 \\ 0 \\ 0 \end{pmatrix}\)$
连杆 1 加速度（基座 \(a_0 = (0; 0, -g) = (0; 0, 9.81)\) 向上以模拟重力）：
其中 \({}^1X_0 a_0 = {}^1X_0 (0; 0, 9.81)^T = (0; 9.81\sin 30°, 9.81\cos 30°) = (0; 4.905, 8.496)\)
运动叉积项 \(v_1 \times (S_1 \dot{q}_1) = (2; 0, 0) \times (2; 0, 0) = 0\)（自身叉积为零）
连杆 2 速度： $\(v_2 = {}^2X_1 v_1 + S_2 \dot{q}_2\)$
代入数值（\(q_2 = 45°\)，\(\ell_1 = 1.0\)）： $\({}^2X_1 v_1 = \begin{pmatrix} 1 & 0 & 0 \\ 1.0 \sin 45° & \cos 45° & \sin 45° \\ -1.0 \cos 45° & -\sin 45° & \cos 45° \end{pmatrix} \begin{pmatrix} 2 \\ 0 \\ 0 \end{pmatrix} = \begin{pmatrix} 2 \\ 1.414 \\ -1.414 \end{pmatrix}\)$
D.4 Pass 2：力回溯¶
连杆 2 的空间惯性（参考点在连杆 2 末端，质心偏移 \(c_2 = -0.4\) m 沿 x）：
连杆 2 上的力（无外力）： $\(f_2 = \mathcal{I}_2 a_2 + v_2 \times^* (\mathcal{I}_2 v_2)\)$
（数值计算过程较长，此处给出最终关节力矩）
关节力矩： $\(\tau_2 = S_2^T f_2 = f_{2,\omega_z}\)$ $\(\tau_1 = S_1^T f_1 = f_{1,\omega_z}\)$
其中 \(f_1 = \mathcal{I}_1 a_1 + v_1 \times^* (\mathcal{I}_1 v_1) + {}^1X_2^* f_2\)（来自连杆 2 的反作用力回溯）。
D.5 与 Pinocchio 验证¶
import pinocchio as pin
import numpy as np
# 构建 2R 模型
model = pin.Model()
# 添加关节和连杆（此处省略详细的 model 构建代码）
# 实际使用时建议从 URDF 加载
# 假设模型已构建，计算 RNEA
q = np.array([np.deg2rad(30), np.deg2rad(45)])
v = np.array([2.0, -1.0])
a = np.array([0.5, -0.3])
tau = pin.rnea(model, data, q, v, a)
print(f"tau_1 = {tau[0]:.4f} N·m")
print(f"tau_2 = {tau[1]:.4f} N·m")
附录 E：空间向量记号的历史演变¶
E.1 从螺旋理论到空间向量¶
| 时期 | 发展 | 对空间向量的影响 |
|---|---|---|
| 1830s-1860s | Chasles 定理、Plucker 坐标 | 提供了 6D 表示的几何基础 |
| 1870s-1900s | Ball 螺旋理论 | 建立了运动/力的对偶框架 |
| 1960s-1970s | Brand, Dimentberg | 将螺旋理论用于机构学 |
| 1978 | Yang & Freudenstein | 现代机构分析的螺旋方法 |
| 1983-1987 | Featherstone | 将螺旋理论"算法化"：发明 RNEA、ABA |
| 1994 | Murray-Li-Sastry | 从微分几何/李群角度重新建立 |
| 2000s | Selig, Stramigioli | 几何力学观点的进一步发展 |
| 2008 | Featherstone RBDA | 定型为标准算法教材 |
| 2010s-2020s | Pinocchio, Drake | 工业级软件实现 |
E.2 为什么 Featherstone 选择"角在前线在后"¶
这个顺序看似反直觉（我们通常先说位移再说旋转），但有深刻的原因：
-
与 \(\mathfrak{se}(3)\) 的 4×4 矩阵表示一致：\([\mathcal{V}] = \begin{pmatrix} [\omega]_\times & v \\ 0 & 0 \end{pmatrix}\)，左上角是旋转（"角"）部分
-
Plucker 变换的分块结构更清晰：\(X = \begin{pmatrix} R & 0 \\ [p]_\times R & R \end{pmatrix}\)，右上角为零（角速度不受平移影响）——如果线在前角在后，非零块会出现在不同位置，不那么"自然"
-
历史惯例：螺旋理论传统上把旋转放在首要位置（因为旋转是更"本质"的运动——纯平移只是旋转的退化情况）
E.3 Drake 为什么"叛离"Plucker¶
Drake 团队（Russ Tedrake, Sherm 等）选择不使用 Plucker 代数的理由在官方文档中有明确说明：
-
可调试性：6×6 矩阵乘法把旋转和平移混在一起，出错时很难定位。分步操作（先 Shift 再 Rotate）每步都有明确的物理含义
-
泛化性：Drake 不仅做刚体，还做柔性体、液压系统。Plucker 代数只适用于刚体
-
与工程师直觉一致：大多数工程师习惯分别思考旋转和平移，而非 6D 整体
这不是"Drake 的做法更好"——而是在不同设计目标下的合理选择。如果你的目标是最大化算法效率和数学简洁性，Featherstone/Pinocchio 的方式更优；如果目标是代码可读性和可调试性，Drake 的方式有优势。
附录 F：空间向量代数的常用恒等式¶
以下恒等式在推导算法和验证代码时经常使用。
F.1 变换恒等式¶
| 恒等式 | 含义 |
|---|---|
| \(X_{AB} X_{BC} = X_{AC}\) | 变换复合 |
| \(X_{AB}^{-1} = X_{BA}\) | 逆变换 |
| \((X^*)^{-1} = (X^{-1})^* = X^{-T}\) | 力变换的逆 |
| \(X^{*T} = X^{-1}\) | 力变换转置与运动变换逆的关系 |
| \(\det(X) = 1\) | 所有 Plucker 变换保持体积 |
F.2 叉积恒等式¶
| 恒等式 | 含义 |
|---|---|
| \(v_1 \times v_2 = -v_2 \times v_1\) | 反对称性 |
| \(v_1 \times (v_2 \times v_3) + \text{cyc} = 0\) | Jacobi 恒等式 |
| \((Xv) \times (Xw) = X(v \times w)\) | 叉积在变换下的等变性 |
| \(v \times^* (I v) = -I(v \times v) + (v \times)^T I v\) | 力叉积与惯性的关系 |
| \([v]_{\times^*} = -[v]_\times^T\) | 力叉积与运动叉积的关系 |
F.3 惯性恒等式¶
| 恒等式 | 含义 |
|---|---|
| \(\mathcal{I} = \mathcal{I}^T\) | 对称性 |
| \(v^T \mathcal{I} v > 0\) (当 \(v \neq 0\)) | 正定性 |
| \(\mathcal{I}_B = X_{BA}^{*T} \mathcal{I}_A X_{BA}^*\) | 坐标变换（平行轴定理） |
| \(\mathcal{I}_{composite} = \sum_i X_i^{*T} \mathcal{I}_i X_i^*\) | 复合刚体惯性叠加 |
| \(T = \frac{1}{2} v^T \mathcal{I} v\) | 动能 |
| \(h = \mathcal{I} v\) | 空间动量 |
F.4 Newton-Euler 恒等式¶
| 恒等式 | 含义 |
|---|---|
| \(\hat{f} = \mathcal{I}\hat{a} + \hat{v} \times^* (\mathcal{I}\hat{v})\) | 单刚体动力学 |
| \(\frac{d}{dt}(\mathcal{I}v) = \hat{f} + v \times^*(\mathcal{I}v)\) | 动量定理（体坐标系） |
| $\frac{d}{dt}\hat{h}\big | _{fixed} = \hat{f}$ |
| \(\tau_i = S_i^T f_i\) | 关节力矩投影 |
F.5 RNEA 中的关键恒等式¶
| 恒等式 | 在 RNEA 中的作用 |
|---|---|
| \(v_i = X v_{\lambda(i)} + S_i \dot{q}_i\) | 速度前向传播 |
| \(a_i = X a_{\lambda(i)} + S_i \ddot{q}_i + v_i \times S_i \dot{q}_i\) | 加速度前向传播 |
| \(f_i = \mathcal{I}_i a_i + v_i \times^* \mathcal{I}_i v_i\) | 连杆净力 |
| \(f_{\lambda(i)} += X_i^* f_i\) | 力后向传播 |
| \(\text{RNEA}(q, \dot{q}, 0, 0) = C(q,\dot{q})\dot{q}\) | 提取 Coriolis |
| \(\text{RNEA}(q, 0, 0, g_0) = g(q)\) | 提取重力 |
| \(\text{RNEA}(q, \dot{q}, \ddot{q}, g_0) = M\ddot{q} + C\dot{q} + g\) | 完整逆动力学 |
附录 G：空间向量与 Pinocchio 对象的内存布局¶
在高性能计算中，理解数据布局很重要：
G.1 Pinocchio 的内存模型¶
// pinocchio::Motion 内部存储
// 底层是 Eigen::Matrix<double, 6, 1>
// 布局: [omega_x, omega_y, omega_z, v_x, v_y, v_z]
// 即 angular 在前(0:3), linear 在后(3:6)
template<typename Scalar>
struct MotionTpl {
typedef Eigen::Matrix<Scalar, 6, 1> Vector6;
Vector6 m_data; // 连续 48 bytes (double)
// 访问器
auto angular() { return m_data.template head<3>(); } // 前3
auto linear() { return m_data.template tail<3>(); } // 后3
};
// pinocchio::Inertia 内部存储
// 紧凑表示：mass(1) + lever(3) + RotationalInertia(6) = 10 scalars
// 不存储完整 6×6，节省内存
template<typename Scalar>
struct InertiaTpl {
Scalar m_mass; // 1 scalar
Eigen::Matrix<Scalar,3,1> m_com; // 3 scalars (质心位置)
Symmetric3Tpl<Scalar> m_inertia; // 6 scalars (对称惯性张量上三角)
// 总计 10 scalars = 80 bytes (double)
// 需要时动态计算 6×6 矩阵
};
G.2 性能影响¶
- SIMD 友好：
Motion的 6 个元素连续存储，可用 AVX2 一次加载 - Cache 友好：
Model::inertias是连续数组，RNEA 遍历时不会 cache miss - 紧凑存储：惯性用 10 个参数而非 36 个（6×6 矩阵），节省 3.6× 内存
- Zero-copy view：
data.v[i]直接引用内部数组，不做拷贝
这些设计选择使 Pinocchio 比 RBDL 快 3-10 倍——不是算法差异，而是数据布局和编译期优化的差异。
附录 H：记号约定快速参考卡¶
+--------------------------------------------------------------+
| 空间向量代数 - 记号快速参考 |
+--------------------------------------------------------------+
| |
| 运动向量: v_hat = (w; v_O) in M6 [角在前, 线在后] |
| 力向量: f_hat = (tau_O; f) in F6 [力矩在前, 力在后] |
| 功率: P = f_hat_T v_hat = tau_T w + f_T v [参考点无关] |
| |
| 运动变换: B_X_A = [R, 0; [p]xR, R] |
| 力变换: B_X*_A = X_inv_T = [R, [p]xR; 0, R] |
| |
| 运动叉积: [v_hat]x = [[w]x, 0; [v]x, [w]x] (下三角) |
| 力叉积: [v_hat]x* = -[v_hat]x_T |
| = [[w]x, [v]x; 0, [w]x] (上三角) |
| |
| 空间惯性: I = [I_bar, m[c]x; m[c]x_T, mI3] (对称正定) |
| Newton-Euler: f_hat = I a_hat + v_hat x*(I v_hat) |
| 动能: T = 1/2 v_hat_T I v_hat |
| |
| RNEA 前向: v_i = X v_{lam(i)} + S_i dq_i |
| a_i = X a_{lam(i)} + S_i ddq_i + v_ix(S_i dq_i) |
| RNEA 后向: f_i = I_i a_i + v_ix*(I_i v_i) |
| tau_i = S_i_T f_i |
| |
+--------------------------------------------------------------+
附录 I：空间向量的 SymPy 符号验证¶
对于初学者，用符号计算验证空间向量公式可以极大提高信心：
import sympy as sp
from sympy import Matrix, symbols, cos, sin, simplify, zeros
# === 验证：Plucker 变换的行列式 = 1 ===
theta = symbols('theta')
px, py, pz = symbols('p_x p_y p_z')
# 绕 z 轴旋转
R = Matrix([
[cos(theta), -sin(theta), 0],
[sin(theta), cos(theta), 0],
[0, 0, 1]
])
p = Matrix([px, py, pz])
skew_p = Matrix([
[0, -pz, py],
[pz, 0, -px],
[-py, px, 0]
])
# 构造 6x6 Plucker 变换
X = zeros(6, 6)
X[:3, :3] = R
X[3:, :3] = skew_p * R
X[3:, 3:] = R
det_X = X.det()
print(f"det(X) = {simplify(det_X)}") # 应输出 1
# === 验证运动叉积的 Jacobi 恒等式 ===
# 定义三个随机运动向量
v1 = Matrix([1, 0, 0, 0, 1, 0])
v2 = Matrix([0, 1, 0, 1, 0, 0])
v3 = Matrix([0, 0, 1, 0, 0, 1])
def crossm_matrix(v):
"""构造运动叉积矩阵"""
w = v[:3]
vl = v[3:]
skew_w = Matrix([[0,-w[2],w[1]],[w[2],0,-w[0]],[-w[1],w[0],0]])
skew_v = Matrix([[0,-vl[2],vl[1]],[vl[2],0,-vl[0]],[-vl[1],vl[0],0]])
M = zeros(6,6)
M[:3,:3] = skew_w
M[3:,:3] = skew_v
M[3:,3:] = skew_w
return M
# Jacobi: v1 x (v2 x v3) + v2 x (v3 x v1) + v3 x (v1 x v2) = 0
cm1, cm2, cm3 = crossm_matrix(v1), crossm_matrix(v2), crossm_matrix(v3)
v2xv3 = cm2 * v3
v3xv1 = cm3 * v1
v1xv2 = cm1 * v2
jacobi = crossm_matrix(v1)*v2xv3 + crossm_matrix(v2)*v3xv1 + crossm_matrix(v3)*v1xv2
print(f"Jacobi identity check: {jacobi.T}") # 应全为零
用 SymPy 推导 2R 臂的空间惯性¶
# 符号推导 2R 机械臂的质量矩阵
from sympy import *
q1, q2, dq1, dq2 = symbols('q1 q2 dq1 dq2')
l1, l2, m1, m2 = symbols('l1 l2 m1 m2', positive=True)
I1, I2 = symbols('I1 I2', positive=True)
g = symbols('g', positive=True)
# 连杆质心位置（假设质心在连杆末端）
x1 = l1 * cos(q1)
y1 = l1 * sin(q1)
x2 = x1 + l2 * cos(q1 + q2)
y2 = y1 + l2 * sin(q1 + q2)
# 动能
T1 = Rational(1,2) * m1 * (diff(x1,q1)*dq1)**2 + Rational(1,2) * m1 * (diff(y1,q1)*dq1)**2
# ... 完整推导见 20_Lagrange 专题
# 最终 M(q) 可以用 Christoffel 符号验证与空间向量方法给出相同结果
附录 J：与后续专题的接口¶
本章建立的空间向量工具将在后续专题中直接使用：
| 后续专题 | 使用本章的内容 | 具体接口 |
|---|---|---|
| 20 Lagrange力学 | 动能 \(T = \frac{1}{2}\sum v_i^T I_i v_i\) | CRBA 构造 \(M(q)\) |
| 30 RNEA/ABA/CRBA | 完整递推公式 | 本章 §8 的所有方程 |
| 40 SE(3) 几何力学 | Ad/ad 与 X 的对应 | 本章 §13 的对照表 |
| 50 约束动力学 | 接触力的空间表示 | \(J_c^T \lambda\) 中 \(\lambda\) 是空间力 |
| 60 解析微分 | \(\partial \text{RNEA}/\partial q\) | 空间向量的可微性 |
最重要的衔接：下一专题（Lagrange 力学）将展示如何从空间向量形式的动能 \(T = \frac{1}{2} \sum v_i^T I_i v_i\) 推导出关节空间质量矩阵 \(M(q)\)——这正是 CRBA 算法的数学基础。
练习¶
练习 1：力向量参考点变换与功率验证（基础）¶
一个 wrench 在参考点 O 的表达为 \(\hat{f}_O = (\tau_O;\, f) = ((0, 0, 10)^T;\, (5, 0, 0)^T)\) [N·m; N]。刚体的空间速度为 \(\hat{v}_O = (\omega;\, v_O) = ((0, 0, 2)^T;\, (1, 0, 0)^T)\) [rad/s; m/s]。
(a) 计算在参考点 O 的功率 \(P = \tau_O^T \omega + f^T v_O\)。
(b) 设 \(r_{OP} = (0.5, 0, 0)^T\) m，利用正确的变换公式 \(\tau_P = \tau_O - r_{OP} \times f\) 和 \(v_P = v_O + \omega \times r_{OP}\)，计算 \(\hat{f}_P\) 和 \(\hat{v}_P\)。
(c) 验证在参考点 P 的功率 \(\tau_P^T \omega + f^T v_P\) 等于 (a) 的结果。
(d) 如果误用错误公式 \(\tau_P = \tau_O + r_{OP} \times f\)，计算功率会相差多少？解释为何这个错误在工程中特别危险（提示：考虑能量不守恒的后果）。
练习 2：空间惯性矩阵的对称性验证（中等）¶
一个均质圆柱体：质量 \(m = 2\) kg，半径 \(r = 0.1\) m，高度 \(h = 0.4\) m。质心惯性张量为 \(I_c = \text{diag}(m(3r^2+h^2)/12,\; m(3r^2+h^2)/12,\; mr^2/2)\)。
(a) 设参考点 O 在圆柱底面中心，\(c = (0, 0, 0.2)^T\) m（从 O 到质心）。写出 \([c]_\times\) 矩阵。
(b) 利用 Featherstone 公式 \(\mathcal{I}_O = \begin{pmatrix} I_c - m[c]_\times[c]_\times & m[c]_\times \\ m[c]_\times^T & mI_3 \end{pmatrix}\) 计算完整的 6×6 空间惯性矩阵。
(c) 验证 \(\mathcal{I}_O\) 是对称正定矩阵（提示：检查 \(\mathcal{I}_O = \mathcal{I}_O^T\)，并验证 \([c]_\times\) 的转置关系确保左下角 = 右上角的转置）。
(d) 用 Pinocchio 代码验证你的手算结果：
练习 3：Pinocchio act/actInv 实验（编程）¶
编写 Python 脚本验证 Pinocchio 的坐标变换 API：
(a) 创建变换 $T = $ pin.SE3(R, p) （用 \(R = R_z(30°)\)，\(p = (1, 0, 0)^T\)）。创建运动 \(v_A\) 和力 \(f_A\)。
(b) 分别用 T.act(v_A) 和 T.act(f_A) 计算 \(v_B\) 和 \(f_B\)。验证 v_A.dot(f_A) == v_B.dot(f_B)（功率不变性）。
(c) 故意使用 T.actInv(f_A) 代替 T.act(f_A)，验证功率不再守恒。计算误差百分比。
(d) 手动构造 6×6 矩阵 \(X = \begin{pmatrix} R & 0 \\ [p]_\times R & R \end{pmatrix}\) 和 \(X^{-T}\)，验证 T.act(v) = \(X \cdot v\)，T.act(f) = \(X^{-T} \cdot f\)。
练习 4：运动叉积与力叉积的对偶关系（理论 + 编程）¶
设 \(\hat{v}_1 = (0, 0, 1; 1, 0, 0)^T\)（绕 z 旋转 + 沿 x 平移）。
(a) 手动计算 6×6 运动叉积矩阵 \([\hat{v}_1]_\times = \begin{pmatrix} [\omega_1]_\times & 0 \\ [v_1]_\times & [\omega_1]_\times \end{pmatrix}\)。
(b) 手动计算力叉积矩阵 \([\hat{v}_1]_{\times^*} = -[\hat{v}_1]_\times^T\)。验证它不等于 \([\hat{v}_1]_\times\)（对偶 ≠ 自身）。
(c) 对任意力向量 \(\hat{f}\)，验证 \(\hat{v}_1^T ([\hat{v}_1]_{\times^*} \hat{f}) = 0\)。解释物理意义（提示：这等价于 \(\hat{v}^T (\hat{v} \times^* \hat{f}) = 0\)，说明力叉积不做功）。
(d) 用 Pinocchio 验证：v1.cross(f).dot(v1) 是否为零？与你的理论预测对比。
练习 5：跨章综合——从空间惯性到 Lagrange 方程（进阶）¶
考虑 2-DOF 平面机械臂：连杆 1 长 \(l_1\)、质量 \(m_1\)（质心在连杆中点）；连杆 2 长 \(l_2\)、质量 \(m_2\)（质心在连杆中点）。
(a) 写出每个连杆的 body-frame 空间惯性 \(\mathcal{I}_1, \mathcal{I}_2\)（利用本章公式）。
(b) 写出连杆间的 Plücker 变换 \({}^2X_1(q_2)\) 和 \({}^1X_0(q_1)\)。
(c) 利用 \(v_i = {}^iX_{\lambda(i)} v_{\lambda(i)} + S_i \dot{q}_i\)（本章递推公式），推导两个连杆的空间速度。
(d) 计算动能 \(T = \frac{1}{2}(v_1^T \mathcal{I}_1 v_1 + v_2^T \mathcal{I}_2 v_2)\)，并整理为 \(T = \frac{1}{2}\dot{q}^T M(q) \dot{q}\) 的形式，得到 2×2 质量矩阵 \(M(q)\)。
(e) 将结果与下一章（Lagrange 力学）中用 CRBA 算法得到的 \(M(q)\) 对比——两者应完全一致。这就是"空间向量代数是 CRBA 的数学基础"的具体含义。