四元数与姿态插值

我们先从自由度的角度审视三维旋转的紧凑表示。如果旋转轴的方向已经固定，只需一个转角就能确定姿态；而在一般的三维转动中，旋转轴本身还可以在空间中任意指向。由于三维单位向量受模长为 1 的约束，确定转轴方向需要两个独立参数，再加上绕轴转过的角度，三维姿态一共具有三个独立自由度。接下来我们将看到，若把这三个自由度编码进四个实数分量，并附加一个单位长度约束，就能构造出一套封闭且无奇异的旋转代数；至于其中为何会出现“半角”结构，将在第 2 节分析向量旋转作用时水到渠成地导出。

将一个实标量 s\in\mathbb R 与一个三维实向量 \boldsymbol u\in\mathbb R^3 组合在一起，构成的有序四元组记作：

这样的四元组配合下文定义的加法与乘法运算，就称为四元数（Quaternion）。当四元数用于表示刚体姿态时，它的四个分量均为无量纲实数。其中 s 称为四元数的标量部（或实部），\boldsymbol u 称为向量部（或虚部）；特别地，标量部为零的四元数 (0,\boldsymbol u) 称为纯四元数（Pure Quaternion），三维空间向量正是通过写成纯四元数的形式参与四元数运算的。任给两个四元数 (s,\boldsymbol u) 与 (t,\boldsymbol v)，它们的乘积定义为：

式（3.2）称为四元数的 Hamilton 乘法（Hamilton Product），其中 t 是第二个因子的标量部，乘法单位元为 (1,\boldsymbol0)。注意到向量部中包含三维叉积项 \boldsymbol u\times\boldsymbol v，一旦交换两个因子的先后次序，叉积项就会变号，因此四元数乘法一般不满足交换律。

从式（3.2）可以看出，当两个纯四元数 (0,\boldsymbol u) 与 (0,\boldsymbol v) 相乘时，Hamilton 乘法巧妙地把三维向量的点积和叉积装进了同一个乘积结果中：点积 -\boldsymbol u^{\mathsf T}\boldsymbol v 构成标量部，叉积 \boldsymbol u\times\boldsymbol v 构成向量部。如果我们将 \boldsymbol u 和 \boldsymbol v 放在水平面内，乘积的向量部就会沿着竖直法线方向立起来。

在实验 01 中调节两向量的夹角可以看到：当两向量共线平行时，叉积为零，纯四元数乘积退化为纯标量；当两向量相互垂直时，点积为零，乘积仍是一个垂直于两者的纯四元数。一旦对调 \boldsymbol u 与 \boldsymbol v 的次序，点积读数丝毫不变，而垂直于平面的叉积箭头立即翻转 180^\circ——这正是四元数乘法不可交换的几何根源。

虽然四元数乘法不满足交换律，但它严格满足结合律（即在保持各因子左右先后次序的前提下，可以任意改变相乘的括号分组）：

结合律在多体运动学中至关重要：它保证了当我们对一个向量连续施加多次四元数旋转时，既可以一步一步依次更新向量，也可以先把一连串四元数累乘合并为一个总四元数再作用于向量。下面的折叠推导仅利用点积和二重叉积恒等式，直接验证这一性质。

共轭、模与逆

与复数类似，保持四元数 q=(s,\boldsymbol u) 的标量部不变、将其向量部取反，所得的四元数称为 q 的共轭四元数（Conjugate Quaternion），记作 q^*；四个分量的平方和开方则定义为四元数的模（或范数）\|q\|：

由于 qq^*=q^*q=(\|q\|^2,\boldsymbol 0)，当 q\ne 0 时，四元数的乘法逆元可以直接写为 q^{-1}=q^*/\|q\|^2。特别地，模长等于 1（即 \|q\|=1）的四元数称为单位四元数（Unit Quaternion）。对于单位四元数而言，它的逆元就是它的共轭元：q^{-1}=q^*，求逆只需将向量部三个分量变号，无需任何除法运算。物理引擎中所有用于表示刚体姿态的四元数都是单位四元数。

此外，四元数的共轭与模长对 Hamilton 乘法还具有两条极其优美的代数性质——乘积的共轭等于共轭的反序乘积，乘积的模等于模的乘积：

第一条性质与矩阵转置的反序律 (AB)^{\mathsf T}=B^{\mathsf T}A^{\mathsf T} 完全对应，用于求复合旋转的逆变换；第二条性质则保证了任意两个单位四元数的乘积依然是单位四元数。这两条性质是下面构造三维向量旋转作用的基石。

现在我们来解决核心问题：如何用单位四元数 q=(s,\boldsymbol u) 旋转一个三维空间向量 \boldsymbol x\in\mathbb R^3？合法的三维旋转必须满足两条基本要求：第一，旋转后的结果仍然是一个三维空间向量（写成四元数时标量部必须为零）；第二，旋转前后向量的长度保持不变。如果我们把三维向量 \boldsymbol x 写成纯四元数 (0,\boldsymbol x)，仅用 q 在左边单侧相乘，所得乘积 q(0,\boldsymbol x)=(-\boldsymbol u^{\mathsf T}\boldsymbol x,\,s\boldsymbol x+\boldsymbol u\times\boldsymbol x) 通常会出现非零标量部 -\boldsymbol u^{\mathsf T}\boldsymbol x，走出了纯四元数子空间。而如果我们将左乘 q 与右乘共轭 q^* 配成一对，就能恰好消去标量部，使输出重新落回纯四元数空间，同时由模长乘法性质自动保证 \|q(0,\boldsymbol x)q^*\|=\|q\|\,\|\boldsymbol x\|\,\|q^*\|=\|\boldsymbol x\|。这种“左乘 q、右乘 q^*”的双侧配对运算称为四元数的共轭作用（或形象地称为夹乘，Sandwich Product）。

接下来，我们把这一夹乘运算按分量完全展开，并与第 2 章式（2.6）的 Rodrigues 旋转公式逐项比对，看看单位四元数 q=(s,\boldsymbol u) 的四个分量究竟如何与旋转轴 \boldsymbol n 和转角 \theta 挂钩。

将三维向量 \boldsymbol x 嵌入为纯四元数 (0,\boldsymbol x)，定义单位四元数 q=(s,\boldsymbol u) 对它的旋转作用为：

计算式（3.4）时，既可以先算左侧乘积 q(0,\boldsymbol x)，再将所得的中间四元数（一般带有非零标量部）右乘 q^*，也可以先算右侧再左乘 q。将这两次 Hamilton 乘法彻底展开并化简后，旋转后的三维向量 \boldsymbol x' 可以完全用原向量 \boldsymbol x、内积投影与叉积表示出来：

观察式（3.5）右端的三项结构：它由原向量 \boldsymbol x、沿 \boldsymbol u 方向的投影项以及绕 \boldsymbol u 方向的叉积项线性组合而成，这与第 2 章的 Rodrigues 公式如出一辙！由此可知，四元数的向量部 \boldsymbol u 必然沿着旋转轴 \boldsymbol n 的方向。进一步将式（3.5）的各项系数与绕单位轴 \boldsymbol n 转过角度 \theta 的 Rodrigues 公式对比，便能确定单位四元数与轴角 (\boldsymbol n,\theta) 的精确对应关系：

为什么式（3.6）中出现的是半角 \theta/2，而最终旋转向量时却转过了完整角度 \theta？从代数上看，这是因为式（3.4）的夹乘让 q 和 q^* 各作用了一次，式（3.5）中的系数全部是 s 与 \boldsymbol u 的二次齐次组合，经二倍角公式正好把半角 \theta/2 还原为全角 \theta。

除了代数系数对比外，半角 \theta/2 还有一个非常直观的几何图像。将原向量 \boldsymbol x 分解为平行于转轴的分量 \boldsymbol x_\parallel 和垂直于转轴的分量 \boldsymbol x_\perp。若令 q=(\cos\alpha,\boldsymbol n\sin\alpha)，则在夹乘 q(0,\boldsymbol x)q^* 中，左乘 q 与右乘 q^* 对轴向分量 \boldsymbol x_\parallel 的作用彼此抵消，使其保持不动；而对于垂直平面内的分量 \boldsymbol x_\perp，由于叉积反号与共轭反号负负得正，右乘 q^* 竟然与左乘 q 产生了完全同向的旋转效果！两次半角 \alpha 的同向旋转叠加为 2\alpha，因此要实现总转角 \theta，单侧四元数只需取半角 \alpha=\theta/2。

正因为式（3.6）使用的是半角 \theta/2，当刚体的真实转角 \theta 从 0 连续增加到两整周（4\pi）时，四元数分量呈现出独特的双周期现象。在下面的实验 02 中，可先将旋转轴设为世界竖轴，依次观察转过半周、一周和两周时的物体姿态，再切换到分量曲线对比两者的周期差异。

从实验 02 可以清晰地看到：当刚体绕固定轴 \boldsymbol n 依次转到 \theta=0,\pi,2\pi,4\pi 时，半角依次为 0,\pi/2,\pi,2\pi，对应的单位四元数依次变为 (1,\boldsymbol0)、(0,\boldsymbol n)、(-1,\boldsymbol0) 和 (1,\boldsymbol0)。三维空间中的刚体每转满一周（\theta=2\pi）就已经回到初始姿态，但此时的四元数却变成了 (-1,\boldsymbol 0)；必须再继续转过第二周（\theta=4\pi），四元数的四个分量才会走完一个完整周期回到 (1,\boldsymbol 0)。下一节我们将把式（3.5）写成矩阵形式，从代数上进一步阐明为什么 q 与 -q 对应同一个三维姿态。

在物理引擎中，虽然姿态本身以单位四元数的形式保存，但在进行碰撞检测或网格渲染时，往往需要把四元数转换成 3\times 3 旋转矩阵以批量变换顶点坐标；反过来，在读取外部姿态数据或初始化模型时，又需要从旋转矩阵反解出对应的单位四元数。同时，这一双向转换还会从代数上回答上一节留下的问题：一个三维旋转矩阵究竟对应多少个单位四元数？

由于式（3.5）对输入向量 \boldsymbol x 是线性的，将其中轴向内积投影项 (\boldsymbol u^{\mathsf T}\boldsymbol x)\boldsymbol u 改写为外积矩阵左乘 (\boldsymbol u\boldsymbol u^{\mathsf T})\boldsymbol x，将叉积项 \boldsymbol u\times\boldsymbol x 改写为叉乘矩阵左乘 [\boldsymbol u]_\times\boldsymbol x，即可提取出与单位四元数 q=(s,\boldsymbol u) 等价的旋转矩阵 R(q)：

观察式（3.7）右端的三项可以发现：每一项都是关于四元数分量 s 与 \boldsymbol u 的二次齐次式（s^2、u_iu_j 或 su_i）。因此，将单位四元数整体取反（即把 q 替换为 -q=(-s,-\boldsymbol u)）时，所有二次乘积项的负号全部两两抵消，给出的旋转矩阵完全相同：R(-q)=R(q)。这种“四维单位球面 S^3 上一对径向相对的单位四元数 \pm q 映射为同一个三维旋转矩阵 R\in\mathrm{SO}(3)”的二对一关系，在拓扑与李群理论中称为单位四元数对三维旋转群的双重覆盖（Double Cover）。这就解释了实验 02 中的现象：当刚体从零角连续转过一整周（\theta=2\pi）时，四元数从北极点 (1,\boldsymbol 0) 走到了南极点 (-1,\boldsymbol 0)，虽然四元数符号相反，但对应的旋转矩阵 R 已经回到了单位矩阵 I。

从矩阵恢复四元数

反过来，给定一个满足 R^{\mathsf T}R=I、\det R=1 的旋转矩阵 R，如何稳健地反解出对应的一个单位四元数代表元 q=(s,u_1,u_2,u_3)？观察上面展开的九分量矩阵：它的三个主对角元只含四个分量的平方项 s^2,u_1^2,u_2^2,u_3^2，利用单位模长条件 s^2+u_1^2+u_2^2+u_3^2=1 对三个主对角元作加减组合，就能直接解出四个分量各自的平方（例如 4s^2=1+\operatorname{tr}R）。由于四个分量的平方和等于 1，其中最大的一个平方值必然不小于 1/4（即对应分量的绝对值不小于 1/2）。先对这四个平方中最大的一个开正平方根确定主分量，再以该主分量为除数、利用非对角元的和差关系算出其余三个分量，就能确保除数始终远离零，这一经典算法称为 Shepperd 方法。在转角较小时，标量部 s 最大，通常直接从 s 分支恢复；当转角接近半周（\theta\to\pi）导致 s\to 0 时，则自动切换到绝对值最大的向量分量分支。

两种代表元与连续姿态

前面我们已经看到 R(q)=R(-q)。事实上，不仅 q 与 -q 给出相同的旋转矩阵，而且对应于同一个三维旋转的单位四元数只有 q 和 -q 这两个——它们在四维单位球面 S^3 上处于对径点位置，却代表三维空间中完全相同的物理朝向。

在记录或插值一条连续变化的姿态轨迹时，双重覆盖特性需要我们格外留意相邻两帧之间的符号一致性。将两个单位四元数 q_0,q_1 视为四维欧氏空间中的单位向量，它们与正负代表元之间的四维距离平方分别为：

由此可见，当相邻两帧的四维内积 q_0^{\mathsf T}q_1<0 时，-q_1 比 q_1 离 q_0 更近（落在同一个四维半球内）。此时将后一帧 q_1 整体变号为 -q_1，既不会改变它所表示的物理姿态，又能保证四元数分量序列随时间平滑过渡。第 6 节在计算两个姿态之间的最短转角时，也会利用这一准则将相对四元数的标量部选为非负主值。但请注意：对整条时间序列消跳时，应当拿当前帧与上一帧作内积比较，切忌孤立地强行规定每一帧的标量部 s\ge 0——否则当刚体连续转过半周（\theta=\pi、s 穿过零点）时，强制 s\ge 0 反而会在四元数曲线中人为制造出整体变号的阶跃跳变。

如果刚体先经历由单位四元数 q_1 描述的第一次旋转，再经历由单位四元数 q_2 描述的第二次旋转（两次旋转均在固定的世界系下指定），复合后的总四元数应当怎样写？将两次夹乘作用依次嵌套起来，利用第 1 节证明的结合律与共轭反序律，立即可知合成四元数为 q_2q_1，与旋转矩阵左乘复合 R(q_2)R(q_1) 的左右次序完全一致：

在下面的实验 03 中，我们将两次绕固定世界轴的转角均设为 90^\circ。先把动画进度停在 0.5 处：左侧刚体刚完成绕世界横轴 x 的旋转，右侧刚体刚完成绕世界纵轴 y 的旋转；随后把进度推到终点 1.0，追踪两侧刚体上同一根随体竖轴（蓝色轴）的最终指向，就能把四元数乘积的次序与三维姿态的变化一一对应起来。

对比实验 03 左右两侧的终态可以看到：尽管两边从同一个初始姿态出发，且使用的两根世界转轴与转角完全相同，但“先绕横轴、后绕纵轴”使随体竖轴最终指向负纵轴 -\boldsymbol e_y，而“先绕纵轴、后绕横轴”则使随体竖轴最终指向正横轴 \boldsymbol e_x。下面我们直接用 Hamilton 乘法算出这两种次序的合成四元数，核对实验读数。

在动力学仿真中，刚体的姿态四元数 q(t) 随时间连续变化，并将物体系方向实时变换到世界系。设刚体在体坐标系下的角速度为 \boldsymbol\omega_b，时间步长为 h。与第二章用旋转向量构造矩阵增量的方法相对应，我们先将步内的局部角位移旋转向量 \boldsymbol\phi=h\boldsymbol\omega_b 转换为单位四元数增量 \delta q，再从右侧乘到当前姿态上：

在式（3.8）中，第一行将体坐标角速度在步长 h 内集成为角位移向量，第二行根据半角公式将该角位移写为单位四元数增量，第三行则通过右乘把局部增量与当前姿态复合。当 \theta=0 时，向量部的比例系数 \sin(\theta/2)/\theta 取连续极限 1/2，此时 \delta q=(1,\boldsymbol0) 为恒等四元数。

式（3.8）的四元数右乘更新与第二章的矩阵右乘更新 R(t+h)=R(t)\exp([\boldsymbol\phi]_\times) 完全对应：右乘代表先在刚体自身的局部坐标系内施加微小转动，再由当前姿态送入世界系。若改用世界系角速度 \boldsymbol\omega=R(q)\boldsymbol\omega_b，则增量四元数由 h\boldsymbol\omega 构造并从左侧相乘，代表在当前世界姿态外侧追加一次绕世界轴的转动。只要在步长起点将角速度变换到对应的坐标系并在步内保持恒定，左乘与右乘描述的就是同一个有限转动；当角速度随时间变化时，式（3.8）使用的是步内选定的近似角速度，若要进一步提高时间积分的阶数，则需改进对步内角速度变化率的估计。

令步长 h\to0，对有限更新公式取极限，即可得到四元数对时间的导数在两种角速度表达下的统一形式：

导数公式中的系数 1/2 直接源于四元数的半角构造。当角速度为零时，\dot q=0 且增量退化为单位元，姿态保持静止；在极小转角下，直接对比例系数使用幂级数连续极限，即可避免因除以接近零的转角而无法确定旋转轴向。

Euler 更新与归一化

在数值积分中，如果直接对四元数微分方程采用一阶显式 Euler 更新 q_E=q+h\dot q，几何上相当于从四维单位球面上的当前点 q 出发，沿球面切线方向走出一步。由于切线延伸到了球面外侧，新点 q_E 的模长必然大于 1。即便事后通过除以模长将 q_E 沿径向投回单位球面，投影点与起点之间的球面弧长也会小于精确增量走过的弧长。对于单位四元数起点和步内恒定角速度，模长偏离与归一化后的实际转角 \theta_E 可精确量化为：

式中 \theta=h\|\boldsymbol\omega_b\| 为本步应当转过的精确角度：第一式给出了未归一化四元数的模方增长，第二式则表明归一化 Euler 更新在每一步都会系统性地少转一个小角度。

例如，取恒定角速度 2\,\mathrm{rad/s}、时间步长 h=0.4\,\mathrm s，精确增量每步转过 \theta=0.8\,\mathrm{rad}，而归一化 Euler 更新每步仅转过 2\arctan 0.4\approx0.76101\,\mathrm{rad}。下方实验左侧在刚体模型上直观对比了逐渐滞后的 Euler 姿态，右侧用曲线记录随时间累积的转角误差；可以看到，当刚体转满整周后，累计角度相位差仍随步数持续增大。

在上述参数下，仿真运行 6\,\mathrm s 共经历 15 个时间步，归一化 Euler 累计少转了约 15\times(0.8-0.76101)\simeq0.58481\,\mathrm{rad}。缩短步长 h 时，两种方法的姿态轨迹逐渐靠拢；值得注意的是，归一化虽然使四元数的模长始终严格等于 1，但相位滞后依然存在并清楚地反映在转角误差曲线上。因此，检验姿态积分器的精度时，需要同时核查单位模长条件与实际的旋转相位。

Euler 角与万向节锁

除了旋转矩阵和四元数，工程上也常使用绕坐标轴连续旋转三次的 Euler 角来记录姿态。以滚转角（roll）\varphi、俯仰角（pitch）\theta 和偏航角（yaw）\psi 为例，约定按如下次序合成旋转矩阵：

式中 R_x,R_y,R_z 分别表示绕对应坐标轴的右手基本旋转矩阵，按从右向左的次序依次作用。在这一约定下，当中间的俯仰角到达直角（\theta=\pm\pi/2）时，前后的滚转轴与偏航轴在空间中重合，滚转角 \varphi 与偏航角 \psi 对姿态的贡献退化为仅依赖于它们的差值或和值。由于两个角度参数的瞬时作用方向重合，这种坐标表示的奇异性称为万向节锁（gimbal lock）。需要区分的是，此时刚体在三维空间中依然可以朝任意方向产生角速度，只是所选的三个 Euler 角参数中有两个产生了相互共线的瞬时转动，导致从角度变化率 (\dot\varphi,\dot\theta,\dot\psi)^{\mathsf T} 到世界角速度 \boldsymbol\omega 的线性映射丢失了一个独立维度。

在姿态反馈控制与运动规划中，已知刚体的当前姿态和目标姿态，往往需要求出将当前姿态转到目标姿态所需的旋转轴与转角。在第二章的旋转矩阵表示下，这一相对变换通过目标矩阵乘以当前矩阵的转置求得；推广到单位四元数，只需沿用相同的复合重建关系，即可直接用共轭乘法解出相对四元数。

设当前姿态四元数为 q，目标姿态四元数为 r，两者均表示从物体系到世界系的姿态变换。若在世界系下度量从 q 到 r 的相对旋转 d，则复合关系为左乘 dq=r；若在当前物体系下度量相对旋转 d_b，则复合关系为右乘 qd_b=r。利用单位四元数的逆等于共轭（q^{-1}=q^*），可立即解出两种坐标系下的相对四元数：

世界系相对四元数 d 与物体系相对四元数 d_b 具有相同的标量部，因而对应同一个相对转角，两者的旋转轴则通过当前姿态 R(q) 相互转换。为了从相对四元数中提取最短转角对应的主值旋转向量，先将相对四元数写为 d=(s,\boldsymbol u)，并在必要时对 d 整体取反以选取满足 s\ge0 的短弧代表元。当向量部 \boldsymbol u\ne\boldsymbol0 时，主值转角为 \theta=2\operatorname{atan2}(\|\boldsymbol u\|,s)\in[0,\pi]，对应的旋转向量为 \boldsymbol\phi=\theta\boldsymbol u/\|\boldsymbol u\|，其方向给出了姿态修正的旋转轴，模长给出了尚待转过的角度。当 \theta=0 时直接取零向量；当 \theta=\pi（半周旋转）时，两条相反的轴向均可沿半周到达同一目标姿态。

给定刚体的起始朝向和目标朝向，当需要在两者之间平滑补出中间姿态时，首先要确定连接两端的旋转轴，再确定沿该轴转动的角速度规律。第 6 节已经给出了从起点转到终点的相对旋转轴与相对转角；如果让这段相对转角随时间均匀增加，就能得到一条绕固定轴以恒定角速度转动的姿态轨迹。为此，先把两个单位四元数 q_0,q_1 视为四维欧氏空间中的单位向量，并在必要时对终点取反，以选择与起点四维内积非负的短弧代表元：

这里 \Omega 是两个四元数在四维单位球面上的夹角，而三维空间中实际姿态差的转角为 2\Omega。用归一化参数 \tau\in[0,1] 表示插值进度，令四元数沿四维单位球面上的大圆弧匀速行进，便得到球面线性插值（Spherical Linear Interpolation, SLERP）。当两端点不重合（\Omega>0）时，其加权公式为：

当 \Omega=0 时，已选定的两个端点代表元完全重合，插值路径恒取 q(\tau)=q_0。SLERP 中的“线性”是指三维相对转角随插值进度 \tau 呈线性变化，即 \theta(\tau)=2\Omega\tau；若整个插值过程持续 T 秒且时间进度为 \tau=t/T，则刚体在整个插值过程中的角速度大小恒为 2\Omega/T。

线段归一化与近重合端点

除了直接按三角函数权重沿圆弧插值，另一种简便做法是先在四维空间中对两个端点作普通线性插值，再将所得线段上的点沿径向归一化投回单位球面：

这种方法称为归一化线性插值（Normalized Linear Interpolation, NLERP），插值前同样需要先为终点选取满足 q_0^{\mathsf T}q_1\ge0 的短弧代表元。由于线段 (1-\tau)q_0+\tau q_1 完全位于由 q_0 和 q_1 张成的二维平面内，将其沿径向投影到单位球面后，扫过的几何轨迹与 SLERP 的大圆弧完全重合，只是相同进度参数 \tau 对应圆弧上的不同位置。经过短弧符号选择后，线段上任意一点的模方至少为 1/2，从而保证了归一化分母始终远离零点。特别地，当两个端点非常接近（\Omega\to0）时，NLERP 与 SLERP 的进度差异趋于消失，此时直接用式（3.11）即可稳定地计算 SLERP 在零夹角附近的连续极限，避免式（3.10）出现除以 \sin\Omega\to0 的数值奇异。

我们可以通过二维截面内圆弧与弦的几何关系来理解 NLERP 的速度变化：连接两端点的弦在中点 \tau=1/2 处距离圆心最近，因此在弦上移动相同长度的微小步长时，中点附近对圆心张开的角位移比两端更大。这导致 NLERP 在插值中段的转动角速度偏快、两端偏慢，而 SLERP 的角速度则自始至终保持均匀。将线段点 l 在截面正交基 \{q_0,e\} 下的坐标记为 a=1-\tau+\tau\cos\Omega 和 b=\tau\sin\Omega，由于径向归一化不改变向量的极角，NLERP 的实际转角与角速率可写为：

下面先在三维刚体上对比两种插值的运动过程。将总目标转角设为 120^\circ，并在进度 \tau=1/4 处暂停：此时匀速的 SLERP 已经准确转过 30^\circ，而起步较慢的 NLERP 仅转过约 27.80^\circ；继续推进到半程 \tau=1/2 时，NLERP 加速追上，两者同时到达 60^\circ。继续播放后半程，则可以看到 NLERP 先超前、再减速并与 SLERP 同时抵达终点。

由此可见，NLERP 在前半程落后于 SLERP、在正中点追平、在后半程超前并于终点再次重合；如果只检验 \tau=0,1/2,1 三个特殊点，就会忽略沿途的转速波动。下方实验进一步用转角曲线、角速度曲线以及四维球面的二维截面视图展示同一过程，可以直观看到弦上均匀移动的点在径向投影到圆弧后如何产生疏密变化。

当目标转角为 120^\circ（即 \Omega=\pi/3）时，SLERP 转角曲线的斜率恒为 2\pi/3\simeq2.0944\,\mathrm{rad} 每单位进度；而 NLERP 在两端点（a^2+b^2=1）处的斜率仅为 2\sin(\pi/3)=\sqrt3\simeq1.7321，在正中点（a^2+b^2=3/4）处的斜率则升至 4\sqrt3/3\simeq2.3094。转角曲线的 S 形弯曲、速度曲线的中部拱起，以及二维截面中弦中点向圆心的靠拢，从三个不同角度刻画了同一个几何事实。若整个插值过程持续 2\,\mathrm s，将上述斜率除以 2 即为以 \mathrm{rad/s} 为单位的实际角速度。

最后补充两种边界情形：当两个输入四元数表示同一个空间姿态但符号相反（q_1=-q_0）时，短弧符号选择会将终点翻转为 q_0，从而使插值路径保持静止；当两个端点的真实姿态差恰好为半周（2\Omega=\pi，即 q_0^{\mathsf T}q_1=0）时，正负两个终点代表元对应的球面大圆弧长度相等，所选的端点符号将决定刚体沿两条反向轴中的哪一条转过半周。

本节对照本地 MuJoCo 3.10.0 源码，梳理物理引擎中的四元数实现。相关函数集中在 engine_util_spatial.c 中，四元数数组统一按标量在前、三个向量分量在后的顺序存储，与本章约定完全一致。

乘法与向量旋转

函数 mju_mulQuat 严格按式（3.2）计算 Hamilton 乘积。为了支持原地更新（如 mju_mulQuat(q, q, dq)），实现时先将四个乘积分量写入局部临时数组，计算完毕后再拷贝回输出指针，从而允许输入与输出共用同一块内存。

函数 mju_rotVecQuat 在旋转三维向量时，将式（3.5）改写为如下仅含两次叉乘的紧凑形式：

共轭与相对旋转

函数 mju_negQuat 的实现保留第 0 个分量（标量部），并将后三个分量（向量部）取反：

res[0] = quat[0];
res[1] = -quat[1];
res[2] = -quat[2];
res[3] = -quat[3];

因此，尽管函数名为 negQuat，它实际计算的是四元数的共轭 q^*（对应于反向旋转）。在此基础上，函数 mju_subQuat(res, qa, qb) 先计算相对四元数 q_{\mathrm{rel}}=q_b^*q_a，再将其转换为三维旋转向量输出；对应的姿态重建关系为右乘 q_b\,q_{\mathrm{rel}}=q_a，因此输出的旋转向量是在当前姿态 q_b 的体坐标系下表达的：

// mju_subQuat 中的相对旋转
mji_negQuat(qneg, qb);
mji_mulQuat(qdif, qneg, qa);
mji_quat2Vel(res, qdif, 1);

其中调用的 mju_quat2Vel 使用两参数反正切 atan2 恢复转角，并将所得旋转向量除以给定的时间步长 dt 转换为角速度；当遇到转角超过半周的分支时，该函数会自动将轴向取反以输出短弧转角。

微分、更新与矩阵转换

在姿态微分与积分方面，函数 mju_derivQuat 直接实现体坐标导数公式 \dot q=q(0,\boldsymbol\omega_b)/2。函数 mju_quatIntegrate 则按式（3.8）由体坐标角速度构造有限轴角增量四元数，先对输入姿态执行一次归一化，再将增量四元数从右侧相乘：

// mju_quatIntegrate 中的核心步骤
mji_copy3(tmp, vel);
angle = scale * mju_normalize3(tmp);
mji_axisAngle2Quat(qrot, tmp, angle);
mju_normalize4(quat);
mju_mulQuat(quat, quat, qrot);

在矩阵与四元数互转方面，函数 mju_quat2Mat 直接展开式（3.7）的九个矩阵元。逆转换函数 mju_mat2Quat 则采用第 3 节推导的分支策略：当矩阵的迹 \operatorname{tr}(R)>0 时，先开方恢复绝对值大于 1/2 的标量部 s；否则挑出三个对角元中最大的一个，先开方恢复对应的向量分量，随后利用非对角元的和差关系恢复其余三个分量，最后再执行一次归一化。

在 MuJoCo 的广义坐标与广义速度数组中，球关节（ball joint）在位置数组 qpos 中占用 4 个浮点数以存储单位四元数，而在速度数组 qvel 中仅占用 3 个浮点数以存储局部体坐标角速度。自由关节（free joint）在此基础上再增加 3 个世界系位置分量和 3 个世界系线速度分量，因此在 qpos 和 qvel 中分别占用 7 维和 6 维。第 5 节推导的正交关系 q^{\mathsf T}\dot q=0 从几何上解释了这一维度差异：受单位球面约束限制，四个四元数分量只具有三个独立的瞬时变化自由度。第六章的配置与速度将进一步把这一维度映射用于系统雅可比矩阵与广义坐标积分。

更多接口细节可参考 MuJoCo 官方文档中的四元数积分与相对姿态说明；后续章节分析关节运动学时将继续调用这些基础函数。

编写四元数姿态内核时，每一项运算都有明确的几何性质可供校验：Hamilton 乘法应与旋转矩阵乘法保持同态，共轭旋转应保持向量模长与夹角，姿态积分需区分角速度所在的坐标系，相对旋转应能准确重建目标朝向，球面插值则需同时保持单位模长与端点姿态。对照这些几何不变量阅读和调试函数，就能把四分量代数运算稳固地对应到三维刚体的空间运动上。

本章配套的 quaternion.py 实现了四元数乘法、旋转向量互转、旋转矩阵互转、左右角速度更新以及 SLERP 与 NLERP 插值，与第二章的 rotation.py 共同构成完整的三维姿态计算内核。

def step_body(q, omega_body, h):
    delta = from_rotvec([h * w for w in omega_body])
    return normalize(mul(normalize(q), delta))

动手实现时，可先编写 Hamilton 乘法与共轭向量旋转，并用绕坐标轴的 90^\circ 旋转核对旋转方向；随后实现由旋转向量构造有限增量，并与旋转矩阵的指数更新结果交叉比对；最后实现两种插值函数，检验端点一致性、单位模长以及沿途转角的变化规律。

python3 examples/check_quaternion.py