空间速度、空间力与惯量

推动一扇旋转门时，远离门轴的把手移动得很快，门轴铰链处保持静止，而整块门板在任意时刻都共享同一个角速度。为了在多刚体机构中沿着连杆传递运动，首先需要明确：在已知刚体上某一个参考点的速度与加速度之后，如何求出同一刚体上另一个点的速度与加速度。

在刚体上任取两个固连的材料点 P 和 Q，记从点 P 指向点 Q 的世界系位移向量为 \boldsymbol d=\boldsymbol x_Q-\boldsymbol x_P。由于刚体内部任意两点间的距离固定不变，位移向量 \boldsymbol d 只能随刚体一起以角速度 \boldsymbol\omega 旋转，因此其时间导数以及两点的线速度关系分别为：

式中第二项 \boldsymbol\omega\times\boldsymbol d 是点 Q 相对于点 P 因刚体转动而产生的切向相对速度。由此可见，角速度 \boldsymbol\omega 是整块刚体共有的全局运动属性，而线速度则依赖于所选取的参考点位置。对上述速度关系继续求一次时间导数，即可得到两点间的加速度关系，其中自然分离出切向角加速度项与向心加速度项：

在下方实验中，可以在门板上分别选取靠近铰链和远离铰链的材料点，对比它们的速度箭头方向与长度：在同一角速度下，材料点到转轴的垂直距离翻倍，相对速度模长 \|\boldsymbol\omega\times\boldsymbol d\| 也随之翻倍；图中的固定空间探针则用于对比“空间固定采样点”与“随刚体运动的材料点”。

可以看到，随着门板转动，附着在材料点上的速度箭头始终沿着该点圆周轨迹的切线方向旋转。即便角速度 \boldsymbol\omega 保持恒定，材料点速度矢量的大小虽然不变，其方向却在持续偏转——这正是匀速圆周运动中向心加速度 \boldsymbol\omega\times(\boldsymbol\omega\times\boldsymbol d) 的几何来源。

进一步地，我们可以把两点速度公式 \boldsymbol v(\boldsymbol x)=\boldsymbol v_P+\boldsymbol\omega\times(\boldsymbol x-\boldsymbol x_P) 延拓到三维空间中的任意位置 \boldsymbol x（无论该位置是否落在刚体实体内部）：只需在位置 \boldsymbol x 处想象一个与刚体虚固连的伴随点，该公式便给出了此刻经过位置 \boldsymbol x 的伴随点速度。这样定义在整个三维空间上的向量场称为刚体的瞬时速度场（velocity field）。图中固定空间探针的位置不随门板移动，其读数反映的正是刚体速度场在空间固定点处的采样值。

虽然刚体速度场在空间不同位置有着无穷多个不同的速度箭头，但只要给定刚体的角速度 \boldsymbol\omega 以及某一个参考点 P 的线速度 \boldsymbol v_P，整个速度场就唯一确定了。空间速度正是把这两项核心信息打包成一个六维列向量。在后续的多体动力学递推中，每个刚体只需维护自身参考点处的这一个六维向量；当运动沿着关节从父刚体传递到子刚体时，只需通过一个 6\times6 变换矩阵同步完成参考点平移与坐标架旋转。

本节先将第 1 节的两点速度关系改写为六维分块矩阵，建立空间运动变换；后续小节再在此基础上依次导出空间力与空间加速度的变换法则。

选定参考点 P 后，将刚体的角速度向量 \boldsymbol\omega 与参考点 P 处的线速度向量 \boldsymbol v_P 上下拼接为一个六维列向量：

这一六维向量称为刚体在参考点 P 处的空间速度（spatial velocity / spatial motion vector），在旋量理论中也常称为运动旋量（twist）。其前三个分量的单位为 \mathrm{rad/s}，后三个分量的单位为 \mathrm{m/s}，六个分量共同完整刻画了刚体在当前时刻的速度场。

首先保持坐标轴方向不变，仅将参考点从 P 平移到 Q（位移向量为 \boldsymbol d=\boldsymbol x_Q-\boldsymbol x_P，对应反对称叉乘矩阵 D=[\boldsymbol d]_\times）。利用叉乘的反交换律 \boldsymbol\omega\times\boldsymbol d=-\boldsymbol d\times\boldsymbol\omega=-D\boldsymbol\omega，两点速度关系 \boldsymbol v_Q=\boldsymbol v_P+\boldsymbol\omega\times\boldsymbol d 可写成如下分块下三角矩阵乘法：

接着考虑在同一参考点 Q 处旋转表达坐标架：设正交矩阵 R 将新坐标架下的分量变换到旧坐标架，则从旧坐标架变到新坐标架时，角速度与线速度两组三维分量均需左乘 R^{\mathsf T}。将“先由 P 换点到 Q”与“再左乘 R^{\mathsf T} 换轴”两步矩阵相乘，即得一般的六维空间运动变换矩阵（spatial motion transform）：

需要特别留意式（5.3）中的约定：位移向量 \boldsymbol d=\boldsymbol x_Q-\boldsymbol x_P 始终从旧参考点 P 指向新参考点 Q，并且在旧坐标架下表示成叉乘矩阵 D=[\boldsymbol d]_\times。这一指向约定直接决定了左下角分块 -R^{\mathsf T}D 的负号。

由于参考点平移与坐标架旋转都是可逆的几何操作，若先在新坐标架下转回旧坐标轴方向（左乘 R），再沿反方向位移 -\boldsymbol d 移回旧参考点 P，就应当精确还原初始的六个分量。下方推导给出了逆矩阵 X^{-1} 的显式分块，并通过矩阵乘法验证 X^{-1}X=E_6（E_6 为六阶单位矩阵）。

借助空间速度的换点变换，我们可以在三维空间中找出一条具有特殊几何意义的直线：在这条直线上所有点的瞬时线速度都与角速度 \boldsymbol\omega 平行。于是，刚体在任意时刻的一般空间运动，都可以直观地分解为绕该直线的转动与沿该直线的滑动的叠加（即沙尔定理，Chasles' theorem）。这条直线称为刚体速度场的瞬时螺旋轴（instantaneous screw axis）。

当刚体的角速度非零（\boldsymbol\omega\ne\boldsymbol0）时，从当前参考点 P 指向螺旋轴上最近点（垂足）的垂直位移向量 \boldsymbol r，以及刚体每绕轴转过单位弧度所伴随的轴向平移量——螺距（pitch）h，分别由下式给出：

由此确定的瞬时螺旋轴穿过空间点 \boldsymbol x_P+\boldsymbol r，且方向平行于 \boldsymbol\omega；螺距 h 的单位为 \mathrm m（或 \mathrm{m/rad}），螺旋轴上各点的瞬时线速度均为沿轴向的 h\boldsymbol\omega。上述分解刻画的是当前瞬时的速度场分布；如果螺旋轴的位置、方向以及螺距 h 随时间保持恒定，刚体上各材料点就会在空间中描绘出标准的螺旋线轨迹，每旋转一弧度沿轴向推进距离 h。

在特殊情形下，若螺距 h=0（即 \boldsymbol\omega\perp\boldsymbol v_P），螺旋轴上各点的瞬时速度为零，刚体的瞬时运动退化为绕该轴的纯转动；若角速度本身为零（\boldsymbol\omega=\boldsymbol0），则速度场在空间各处完全相同，刚体作纯平移运动。

在下方实验中，可先将螺距调为零，在速度场中观察速度为零的瞬时转轴；随后逐步增大螺距 h，观察螺旋轴上的速度箭头如何严格沿着轴向伸长。由于图中保持螺旋轴与螺距恒定，播放动画时即可看到材料点轨迹如何从平面圆周演变为三维螺旋线。

可以看到，当 h=0 时材料点描绘出闭合圆周，当 h\ne0 时则叠加了速率恒为 |h|\|\boldsymbol\omega\| 的轴向推进。无论在哪个参考点观察，刚体在同一瞬间都共享同一个角速度 \boldsymbol\omega 与同一个轴向速度分量，改变参考点位置只会改变垂直于螺旋轴的切向速度分量。下一节分析空间力与力矩时，我们也将利用完全对偶的几何分解找出合力的中心轴。

用同样大小的力推门时，推在远离门轴的把手上或推在靠近门轴的位置，对门板产生的转动效果截然不同。因此，要完整描述一组外力对刚体运动的作用，需要把作用在刚体上的合力与关于选定参考点的合力矩合并在一起。

设作用在刚体上的总外力矢量为 \boldsymbol f，所有外力关于参考点 P 的合力矩为 \boldsymbol\tau_P。按照角分量在前、线分量在后的约定，定义刚体在参考点 P 处的六维空间力（spatial force vector）（在旋量理论中也常称为力旋量，wrench），并由其与空间速度 V_P 的六维欧氏内积给出外力对刚体做功的瞬时总功率 \mathcal P：

在空间力向量 F_P 中，前三个分量的单位为 \mathrm{N\,m}，后三个分量的单位为 \mathrm N；它与空间速度 V_P 的六维内积自然将转动功率 \boldsymbol\omega^{\mathsf T}\boldsymbol\tau_P 与参考点平动功率 \boldsymbol v_P^{\mathsf T}\boldsymbol f 相加，给出标量总功率（单位为 \mathrm W）。

当把参考点从 P 平移到 Q（位移为 \boldsymbol d=\boldsymbol x_Q-\boldsymbol x_P）时，每个外力作用点相对于新参考点 Q 的力臂都减少了 \boldsymbol d，因此关于新参考点 Q 的合力矩变为：

与此同时，合力 \boldsymbol f 本身与参考点无关。随着参考点移动，合力矩 \boldsymbol\tau 与参考点线速度 \boldsymbol v 同步发生改变，使转动功率项与平动功率项的变化量精确抵消，保证外力对刚体做功的物理总功率 \mathcal P 与参考点选取无关。

已知空间速度按式（5.3）变换为 V_{\mathrm{new}}=XV_{\mathrm{old}}，要求在任意空间速度下功率标量恒保持不变（V_{\mathrm{new}}^{\mathsf T}F_{\mathrm{new}}=V_{\mathrm{old}}^{\mathsf T}F_{\mathrm{old}}），即可直接导出空间力的对偶变换矩阵（spatial force transform）：

在下方实验中，将参考点从门轴铰链逐步移向推力的作用点，分别观察转动功率项 \boldsymbol\omega^{\mathsf T}\boldsymbol\tau 与平动功率项 \boldsymbol v^{\mathsf T}\boldsymbol f 的消长：随着参考点靠近推力作用线，力臂缩短使 \boldsymbol\omega^{\mathsf T}\boldsymbol\tau 减小，而参考点线速度增大则使 \boldsymbol v^{\mathsf T}\boldsymbol f 同步增大，两项之和沿整条移动路径恒定不变。

特别地，当参考点恰好移到推力的作用线上时，关于该点的力矩 \boldsymbol\tau 降为零，但外力依然通过该参考点的线速度 \boldsymbol v 对门板做正功。这说明在计算关节广义驱动力时，必须将完整的六维空间力投影到关节的六维运动子空间上——下面引入的内积 S^{\mathsf T}F 正是用来自动汇总转动与平动两部分功率的。

具体而言，若单自由度关节的标量速率 \dot q 沿着六维运动轴向量 S 产生空间速度 V=S\dot q，则空间力 F 在该关节运动上做功的功率为 \mathcal P=V^{\mathsf T}F=\dot q(S^{\mathsf T}F)。由此可知，与关节速率 \dot q 配对的标量广义力（generalized force）为：

例如，对于绕过点 O、单位轴向为 \boldsymbol n 的旋转铰链，其在任意参考点 P 处表达的六维关节运动轴为 S=[\boldsymbol n;\ \boldsymbol n\times(\boldsymbol x_P-\boldsymbol x_O)]；此时无论参考点 P 选在何处，六维内积 S^{\mathsf T}F_P 都会自动还原出外力关于铰链轴 O 的真实力矩。

与第 3 节寻找速度螺旋轴完全类似，当合力非零（\boldsymbol f\ne\boldsymbol0）时，我们也可以将力矩 \boldsymbol\tau_P 分解为平行于合力 \boldsymbol f 的分量 \boldsymbol\tau_\parallel 与垂直于 \boldsymbol f 的分量：

通过将参考点平移一段垂直于 \boldsymbol f 的位移 \boldsymbol d，即可利用附加力臂 -\boldsymbol d\times\boldsymbol f 完全抵消垂直于合力的力矩分量：

经过新参考点 Q=P+\boldsymbol d 且平行于合力 \boldsymbol f 的直线称为空间力系的中心轴（central axis / line of action）（即潘索中心轴，Poinsot's central axis）。若轴上剩余的平行力偶矩 \boldsymbol\tau_\parallel 也为零，则整个空间力系等价于沿该中心轴作用的单一纯力。

空间速度刻画了刚体的瞬时运动状态，而动量则刻画了这一运动状态所携带的惯性。既然平移参考点时空间速度的线分量会发生改变，为了在任意参考点下都能把“由空间速度求空间动量”写成统一的线性映射，我们需要把刚体的角动量与线动量组装成一个六维空间动量向量，并由此导出 6\times6 空间惯量矩阵。

在刚体上任取参考点 O，记从参考点 O 指向刚体质心 C 的位移向量为 \boldsymbol d=\boldsymbol x_C-\boldsymbol x_O（对应反对称矩阵 D=[\boldsymbol d]_\times），刚体总质量为 m，关于质心的 3\times3 惯量矩阵为 I_C（以上各量均在同一表达坐标系下写出）。

刚体的总线动量 \boldsymbol p 等于总质量乘以质心线速度 \boldsymbol v_C=\boldsymbol v_O+\boldsymbol\omega\times\boldsymbol d；而刚体关于参考点 O 的总角动量 \boldsymbol\ell_O，则等于关于质心的相对角动量 I_C\boldsymbol\omega 加上质心线动量关于点 O 的矩 \boldsymbol d\times\boldsymbol p：

将角动量 \boldsymbol\ell_O 与线动量 \boldsymbol p 上下拼接为六维空间动量（spatial momentum）H_O=[\boldsymbol\ell_O;\ \boldsymbol p]，并将上述两式改写为关于空间速度 V_O=[\boldsymbol\omega;\ \boldsymbol v_O] 的分块矩阵乘法，即得到 6\times6 空间惯量矩阵（spatial inertia matrix）\mathcal I_O：

在式（5.5）的分块结构中，左上角 I_O=I_C-mD^2 正是第四章平行轴定理给出的关于点 O 的三维转动惯量矩阵；右下角 mE 为各向同性的平动质量矩阵；右上角 mD 与左下角 -mD 则是因参考点偏离质心而产生的平动–转动交叉耦合块。四个 3\times3 分块的国际单位依次为 \mathrm{kg\,m^2}（左上）、\mathrm{kg\,m}（右上与左下）和 \mathrm{kg}（右下）。

借助空间惯量矩阵 \mathcal I_O，刚体的总动能（包含平动动能与转动动能）可以直接写成一个紧凑的六维二次型：

当参考点恰好选在质心 C 时，位移 \boldsymbol d=\boldsymbol0（即 D=0），右上与左下两个非对角耦合块消失，空间惯量矩阵简化为块对角阵 \operatorname{diag}(I_C,mE)；当参考点移离质心时，虽然矩阵的四个分块随之改变，但二次型所代表的物理总动能保持不变。

在下方实验中，可以先将参考点置于质心，确认右上和左下两个耦合分块全为零；随后沿着门板移动参考点，观察非对角耦合块如何随质心偏移 \boldsymbol d 线性浮现，并核对在新旧参考点下通过 \tfrac12V^{\mathsf T}\mathcal IV 算出的总动能始终恒等。

可以看到，刚体的总线动量 \boldsymbol p=m\boldsymbol v_C 完全由质心速度决定，不随参考点移动而改变；而关于参考点的角动量 \boldsymbol\ell_O 则随新力臂相应变化，矩阵中的非对角耦合块 \pm mD 恰在平动与转动之间传递这一力臂效应。当需要把多个刚体的空间惯量累加在一起时，必须先将它们变换到同一个参考点与同一组表达坐标轴下。

设同一刚体的空间速度在新旧两个坐标系下满足 V_{\rm new}=XV_{\rm old}，为了使动能二次型在变换前后恒等（或使空间动量按空间力规则变换），空间惯量矩阵必须满足如下合同变换法则：\mathcal I_{\rm old}=X^{\mathsf T}\mathcal I_{\rm new}X。从操作顺序看，右端算子 X^{\mathsf T}\mathcal I_{\rm new}X 先通过 X 将旧参考点速度映射为新参考点速度，再左乘 \mathcal I_{\rm new} 算出新参考点动量，最后通过 X^{\mathsf T} 将动量归算回旧参考点。第八章的复合刚体算法（CRBA）正是利用这一合同变换，将子树的合成惯量逐级向上累加到父刚体上。

在多刚体机构中递推加速度时，如果直接把角加速度 \boldsymbol\alpha 与材料点的真实线加速度 \boldsymbol a_P 拼在一起，每次平移参考点都会因式（5.1）中的向心项 \boldsymbol\omega\times(\boldsymbol\omega\times\boldsymbol d) 而破坏线性矩阵变换。为了使六维加速度与空间速度共享完全相同的空间变换矩阵 X，空间向量代数对线加速度分量作了一项速度叉乘修正。本节先给出这一空间加速度（spatial acceleration）的定义及还原真实材料点加速度的方法，再从移动探针与随体坐标求导两个角度阐明其物理意义（在匀速旋转门中即可直观看到两者的区别：门板上各材料点的速度方向持续偏转、具有非零向心加速度，但门板在随体坐标系下的空间速度分量恒定、空间加速度为零）。

首先选取固连在刚体上的材料参考点 P，并在世界坐标系下表示速度与加速度。由于两点间的真实加速度关系式（5.1）含有二次向心项，我们从点 P 的真实加速度 \boldsymbol a_P 中减去 \boldsymbol\omega\times\boldsymbol v_P，将参考点 P 处的六维空间加速度定义为：

式（5.6）的下半部分称为空间加速度的线分量（记为 (A_P)_{\mathrm{lin}}=\boldsymbol a_P-\boldsymbol\omega\times\boldsymbol v_P）。反过来，一旦通过递推求出了空间加速度 A_P 与空间速度 V_P，只需把叉乘项 \boldsymbol\omega\times\boldsymbol v_P 加回线分量，即可立即恢复材料点 P 的真实经典线加速度：

为什么减去 \boldsymbol\omega\times\boldsymbol v_P 会如此自然地消去向心项？如果在原点固定于点 P、坐标轴随刚体一起旋转的随体坐标系中观察，空间加速度 A_b 恰好就是随体空间速度分量 V_b 对时间的普通导数 \dot V_b：对随体线速度分量 \boldsymbol v_b=R^{\mathsf T}\boldsymbol v_P 求时间导数时，坐标轴自身的旋转自动生成了修正项 -\boldsymbol\omega\times\boldsymbol v_P。

更一般地，在多体引擎（如 MuJoCo 以子树质心为公共参考点）中，采样速度场的参考点 P 本身可能沿着任意给定的空间路径 \boldsymbol x_P(t) 移动。此时需要严格区分两个速度：\boldsymbol u=\dot{\boldsymbol x}_P 是探针（参考点位置）在空间中移动的路径速度，而 \boldsymbol v_P 则是刚体瞬时速度场在当前探针位置 \boldsymbol x_P 处的采样值。在方向固定的世界坐标轴下，空间加速度的线分量与探针读数时间导数 \dot{\boldsymbol v}_P 之间满足如下普遍关系：

这一公式将常见的两种参考点统一为特例：当探针固定在世界空间不动时，路径速度 \boldsymbol u=\boldsymbol0，故 (A_P)_{\rm lin}=\dot{\boldsymbol v}_P，即空间加速度直接等于固定点速度场读数的时间导数；当探针固连在刚体材料点上随刚体运动时，路径速度 \boldsymbol u=\boldsymbol v_P 且读数导数 \dot{\boldsymbol v}_P=\boldsymbol a_P，代入后即回到本节开头的材料点定义式（5.6）。

进一步地，探针路径 \boldsymbol x_P(t) 决定了在空间何处采样，而表达坐标架则决定了将向量投影到哪组基底上。设表达坐标架相对于世界系的姿态矩阵为 R(t)，该坐标架自身以世界角速度 \boldsymbol\nu 旋转。对于任意世界系向量 \boldsymbol z，其在表达坐标架下的分量 \boldsymbol z^c=R^{\mathsf T}\boldsymbol z 对时间的导数满足：

式（5.2）右侧第二项 -\boldsymbol\nu^c\times\boldsymbol z^c 来自表达坐标轴自身的旋转。将探针路径移动（\boldsymbol u）与表达坐标轴旋转（\boldsymbol\nu）结合起来，即可写出任意运动参考系下的空间加速度分量表达式（当取方向固定的世界坐标轴时 \boldsymbol\nu=\boldsymbol0，直接回到上面的世界轴探针公式）。

在下方实验中，先将表达坐标架的转速设为零（\boldsymbol\nu=\boldsymbol0），单独对比探针移动路径的影响：在初始位置 \boldsymbol x_P=(0.4,0,0)^{\mathsf T}\,\mathrm m 处，随刚体转动的材料点具有向心加速度 -1.6\boldsymbol e_x\,\mathrm{m/s^2}，而固定空间探针的速度读数导数 \dot{\boldsymbol v}_P 则为零；若切换为沿直线匀速移动的探针，读数导数 \dot{\boldsymbol v}_P 便等于迁移变化率 \boldsymbol\omega\times\boldsymbol u。

由于本实验中刚体绕固定原点作匀角速度转动，整块刚体的空间加速度恒为零，因此三条不同探针路径在减去各自的 \boldsymbol\omega\times\boldsymbol u 后，得到的空间加速度线分量 (A_P)_{\rm lin} 均为零：其中随材料点运动的探针读数导数等于真实向心加速度，固定位置探针的读数导数为零。最后再调节表达坐标架的角速度 \boldsymbol\nu，即可单独观察基底旋转按式（5.2）引入的分量导数修正。

在第四章的转动坐标系求导中，三维叉乘 \boldsymbol\omega\times\boldsymbol y 刻画了坐标架旋转引起的向量变化率；而在六维空间向量代数中，除了坐标架旋转，还需要同时计入参考点平移带来的变化率。为此，我们将三维叉乘推广为两类六维空间叉乘（spatial cross product）算子，并结合式（5.5）的空间惯量与式（5.6）的空间加速度，把第四章的 Euler 转动方程与牛顿平动方程统一组装为六维 Newton–Euler 方程。

设刚体的空间速度为 V=[\boldsymbol\omega;\ \boldsymbol v]，另一个六维空间运动向量为 U=[\boldsymbol\eta;\ \boldsymbol u]，定义作用于运动向量的空间运动叉乘（记为 V\times U 或左乘 6\times6 矩阵 V\times）：

与之相对应，作用于对偶空间力向量 F=[\boldsymbol\tau;\ \boldsymbol f]（或空间动量 H）的空间力叉乘定义为 V\times 的负转置：

空间运动叉乘 V\times U 的几何意义可以通过一个随体运动方向的运输求导问题来理解：设六维运动轴向量 U（例如子关节的运动轴）固连在刚体上，随着刚体以空间速度 V 运动，在同一个参考点与坐标架下观察到的 U 的时间变化率正是 \dot U=V\times U。下方推导逐项展开这一导数，说明左下角分块为何会出现线速度叉乘项 \boldsymbol v\times\boldsymbol\eta。

由于空间运动向量 U 与空间力向量 F 的内积 U^{\mathsf T}F 代表标量功率，当 U 与 F 随同一个运动系 V 一起旋转和平移时，功率标量保持不变，因此两类空间叉乘对内积的变化率贡献必然正负抵消。这一对偶性质也可作为代码实现的快速校验：任取三个六维向量 V,U,F，两项内积 (V\times U)^{\mathsf T}F 与 U^{\mathsf T}(V\times^*F) 之和恒等于零。

有了空间惯量、空间加速度与空间力叉乘，我们就可以将单个刚体的平动与转动动力学合并为一条统一的六维方程。在固连于刚体的坐标系中（空间惯量 \mathcal I 为常矩阵），刚体的六维空间 Newton–Euler 方程写为：

在式（5.7）中，第一项 \mathcal I A 与空间加速度成正比，第二项 V\times^*(\mathcal I V) 则是关于空间速度的二次偏置力项（统括了离心力、科氏力与陀螺力矩）。更重要的是，只要将 F,\mathcal I,A,V 统一变换到空间中任意选定的同一个参考点与同一组表达坐标轴下，式（5.7）在形式上完全保持不变。

作为重要检验，当把参考点选在刚体质心 C 处时，空间惯量的两个非对角耦合块消失（\mathcal I_C=\operatorname{diag}(I_C,mE)）。此时将式（5.7）按上下两个三维分块展开，空间加速度线分量中的修正项 -m(\boldsymbol\omega\times\boldsymbol v_C) 恰好与速度叉乘项中的 +\boldsymbol\omega\times(m\boldsymbol v_C) 正负相消，从而同时还原出第四章的 Euler 转动方程 \boldsymbol\tau_C=I_C\boldsymbol\alpha+\boldsymbol\omega\times(I_C\boldsymbol\omega) 与质心平动方程 \boldsymbol f=m\boldsymbol a_C。

为了把六维 Newton–Euler 方程的各项落到具体数值上，我们考察一块均匀薄门板：质量为 m=2\,\mathrm{kg}，宽度为 a=0.8\,\mathrm m，高度为 c=1.2\,\mathrm m（厚度忽略不计）。旋转铰链 O 沿竖轴穿过门板左侧边缘中点，门板质心 C 距铰链 0.4\,\mathrm m；随体坐标系的横轴沿门板宽度方向，竖轴沿门板高度方向。

由第四章的长方体主惯量公式（4.6）可算出门板关于质心的惯量矩阵 I_C，再由平行轴定理求出绕铰链竖轴的转动惯量 I_{O,z}：

考察门板恰好转至沿世界横轴正向伸出的瞬间：设此时门板的角速度为 \boldsymbol\omega=2\boldsymbol e_z\,\mathrm{rad/s}、角加速度为 \boldsymbol\alpha=3\boldsymbol e_z\,\mathrm{rad/s^2}。由于铰链 O 固定在空间不动，以铰链 O 为参考点时，空间速度 V_O 与空间加速度 A_O 的线分量（下三行）全为零。

将上述状态代入式（5.7），即可算出此刻维持门板运动所需的合外力为 \boldsymbol f=(-3.2,2.4,0)^{\mathsf T}\,\mathrm N，关于铰链的合力矩为 \boldsymbol\tau_O=1.28\boldsymbol e_z\,\mathrm{N\,m}。在合外力的两个非零分量中，横向负分量 -3.2\,\mathrm N 用于提供质心做圆周运动的向心力，纵向正分量 2.4\,\mathrm N 则用于产生切向角加速。下方计算分别列出了加速度项 \mathcal I_O A_O 与速度叉乘项 V_O\times^*(\mathcal I_O V_O) 的贡献，可以清楚看到这两部分力的来源。

对比计算过程可知：维持圆周运动的向心合力 -3.2\boldsymbol e_x\,\mathrm N 完全来自速度叉乘项 V_O\times^*(\mathcal I_O V_O)，而切向加速力 2.4\boldsymbol e_y\,\mathrm N 与绕轴力矩 1.28\boldsymbol e_z\,\mathrm{N\,m} 则来自惯量乘空间加速度项 \mathcal I_O A_O。若将参考点从铰链 O 移至质心 C，所需合外力 \boldsymbol f 保持不变，而关于质心的合力矩则变为 \boldsymbol\tau_C=I_{C,z}\alpha_z=(8/75)\times3=0.32\boldsymbol e_z\,\mathrm{N\,m}（与第 1 节例题算出的质心真实加速度 \boldsymbol a_C=(-1.6,1.2,0)^{\mathsf T}\,\mathrm{m/s^2} 乘以质量 2\,\mathrm{kg} 完全吻合）。

在实际物理系统中，式（5.7）求出的空间力 F 是驱动刚体按给定运动演化所需的总外力旋量；它由重力、关节轴承的约束反力以及电机的驱动力矩共同提供，后续章节将利用关节运动子空间投影进一步把已知驱动力矩与未知约束反力分离开来。

在下方实验中，可先将角加速度设为零（\alpha=0），验证匀速转动下向心合力依然存在；随后增大角加速度，观察切向合力与驱动力矩的出现。最后在“铰链参考点”与“质心参考点”之间切换，对比两处保持不变的合外力 \boldsymbol f 以及按换点规律变化的力矩与分项贡献。

可以看到，当门板匀速转动（\alpha=0）时，铰链参考点处的空间加速度 A_O 全为零，但为了拉住质心做圆周运动，铰链仍需提供由 V_O\times^*(\mathcal I_O V_O) 算出的向心拉力。这一具体算例直观展示了空间偏置力项 V\times^*(\mathcal I V) 的物理作用；在第八章的多体树递推中，每个刚体都会先按式（5.7）算出各自的六维惯性力，再沿树向根部汇总并投影到各个关节轴上。

以下源码分析对应 MuJoCo 3.10.0。MuJoCo 的六维空间向量同样采用“角分量在前三项、线分量在后三项”的排列顺序：在 mjdata.h 中，cvel 保存各刚体的空间速度，cdof 保存各自由度的六维运动子空间基向量，cinert 则保存各刚体压缩后的空间惯量。

在参考点与表达坐标的选择上，mj_comPos 统一采用世界坐标系方向，并将同一棵运动学树中所有刚体的空间量全部平移到同一个共同参考位置 subtree_com[body_rootid[i]]（即该连通运动学树根子树的整体质心）。每个刚体自身的质心位置 xipos 与这一共同参考点之差，即被用作偏移向量 \boldsymbol d 来组装该刚体在共同参考点处的空间惯量 cinert。

值得注意的是，这一共同参考位置通常位于单个刚体的实体之外，因此 cvel 的下三行表示的是该刚体的刚体速度场延伸到共同参考点处的线速度值。当整棵运动学树运动导致共同参考点随时间移动时，空间加速度与 cvel 读数的时间导数之间便严格遵循第 6 节推导的移动参考点换算关系。

在底层工具函数中，mju_transformSpatial 接收旧参考位置 oldpos、新参考位置 newpos 以及将新坐标系变到旧坐标系的旋转矩阵，依次执行“先换参考点、再用旋转矩阵转置换坐标轴”的两步变换。下方摘录了其换点分支的核心语句（省略了局部数组声明与复制）：

mji_sub3(dif, newpos, oldpos);
if (flg_force) {
  mji_cross(cros, dif, vec+3);
  mji_sub3(tran, vec, cros);
} else {
  mji_cross(cros, dif, vec);
  mji_sub3(tran+3, vec+3, cros);
}

代码中先算出从旧参考点指向新参考点的位移 dif = newpos - oldpos（即 \boldsymbol d）。当 flg_force 为真时，力分支计算 \boldsymbol\tau_{\mathrm{new}}=\boldsymbol\tau_{\mathrm{old}}-\boldsymbol d\times\boldsymbol f，对应式（5.3）；否则运动分支计算 \boldsymbol v_{\mathrm{new}}=\boldsymbol v_{\mathrm{old}}-\boldsymbol d\times\boldsymbol\omega，由叉乘反交换律可知这与两点速度关系 \boldsymbol v_{\mathrm{old}}+\boldsymbol\omega\times\boldsymbol d 完全一致。

在代数运算方面，mji_crossMotion 与 mji_crossForce 逐分量展开了第 7 节定义的六维运动叉乘与力叉乘；而 mju_mulInertVec 则利用 10 个浮点数压缩存储的空间惯量直接左乘六维向量，这 10 个分量的排列顺序为：

其中前 6 项是刚体关于共同参考点的对称转动惯量矩阵的独立分量，中间 3 项是质量一阶矩 m\boldsymbol d，最后 1 项为刚体质量 m；这 10 个数恰好足以完整还原式（5.5）中 6\times6 空间惯量矩阵的四个 3\times3 分块。

在正运动学速度递推阶段，mj_comVel 沿运动学树从根向叶累加各关节运动方向与关节速度的乘积，并调用运动叉乘生成运动子空间基向量的时间导数 cdof_dot，供后续加速度递推计算科氏与离心速度偏置项。进入逆动力学函数 mj_rne 后，单个刚体所需的六维空间力直接由式（5.7）的空间惯量乘加速度项与速度叉乘项相加得出：

mju_mulInertVec(loc_cfrc_body+6*i, d->cinert+10*i, loc_cacc+6*i);
mju_mulInertVec(tmp, d->cinert+10*i, d->cvel+6*i);
mjtNum tmp1[6];
mji_crossForce(tmp1, d->cvel+6*i, tmp);
mju_addTo(loc_cfrc_body+6*i, tmp1, 6);

这里的刚体空间加速度 loc_cacc 由基座加速度、关节加速度以及速度偏置项沿树递推而来；通过将世界基座的初始线加速度设为负重力向量 -\boldsymbol g，算出的刚体受力便自动包含了克服重力所需的等效惯性力。至于刚体间内力的向根部汇总与向关节轴的投影，将在第八章详细展开。

若需进一步对照不同文献的空间向量记号，可参阅 Featherstone 的空间向量软件说明，以及 MuJoCo 官方计算文档中关于递归动力学与子树公共参考点的说明。在比对不同文献与源码时，只需先核对各自采用的“角/线排列次序”以及“坐标变换方向”，即可在各套记号之间无障碍转换。

配套脚本 spatial.py 使用平铺的行主序矩阵实现了本章的空间运动与力变换、空间惯量组装、六维运动与力叉乘、材料点实际加速度恢复以及单刚体逆动力学计算。其中变换函数的输入参数 rotation 表示从新坐标系到旧坐标系的旋转矩阵 R，displacement 表示从旧参考点指向新参考点并在旧坐标轴下表达的位移向量 \boldsymbol d。

from spatial import DOOR, spatial_inertia, required_wrench

I = spatial_inertia(DOOR["mass"], DOOR["com"], DOOR["inertia_com"])
V = [0, 0, 2, 0, 0, 0]  # angular velocity, hinge velocity
A = [0, 0, 3, 0, 0, 0]  # angular acceleration, corrected linear part
F = required_wrench(I, V, A)
print(F)  # [0, 0, 1.28, -3.2, 2.4, 0]

配套的测试脚本会将六维空间代数算出的结果与离散质点系逐点累加的动能、动量及实际加速度做交叉比对，并验证换参考点前后的功率守恒、动能不变量与空间力变换规律。可在仓库根目录或按相对路径运行：

python3 examples/check_spatial.py

from rotation import identity
from spatial import *

V = [0, 0, 2, 0, 0, 0]
A = [0, 0, 3, 0, 0, 0]
X = motion_transform(identity(), DOOR["com"])
Vc, Ac = matvec(X, V), matvec(X, A)
Ic = spatial_inertia(DOOR["mass"], [0, 0, 0], DOOR["inertia_com"])
Fc = required_wrench(Ic, Vc, Ac)
print(Fc)  # [0, 0, 0.32, -3.2, 2.4, 0]