三维旋转与姿态更新

为了单独研究转动，我们先将书本的中心固定在世界坐标系的原点。在书本上沿着长边、短边和封面法线建立三根相互垂直的单位坐标轴，随着书本转到某个姿态，这三根物体坐标轴在世界坐标系下各自对应一个三维单位向量，依次记为 \boldsymbol b_1,\boldsymbol b_2,\boldsymbol b_3。将这三个列向量按顺序并排组合，便构成一个 3\times 3 矩阵：

这个矩阵 R 就是描述刚体当前姿态的旋转矩阵。它不仅直接记录了三根物体坐标轴在世界系中的方向，还给出了从物体坐标系到世界坐标系的坐标变换关系。

具体而言，设书本上某一点在物体坐标系中的局部坐标为 \boldsymbol p=(p_1,p_2,p_3)^{\mathsf T}，在世界坐标系中的坐标为 \boldsymbol x。由于局部坐标的三个分量分别表示该点沿三根物体坐标轴的位移尺度，将该点的位置向量沿这三根轴展开，即可写出：

由此可见，用旋转矩阵 R 左乘局部坐标向量 \boldsymbol p，几何上就是以 p_1,p_2,p_3 为权重对三根物体坐标轴 \boldsymbol b_1,\boldsymbol b_2,\boldsymbol b_3 作线性组合。特别地，若依次取局部单位向量 \boldsymbol p=(1,0,0)^{\mathsf T}、(0,1,0)^{\mathsf T} 和 (0,0,1)^{\mathsf T} 代入式（2.2），乘积结果便分别提取出矩阵 R 的第一列、第二列和第三列。

方向变化与坐标换算

对于关系式 \boldsymbol x=R\boldsymbol p，我们可以从两个互补的视角来理解：如果考察的是固连在书本上的一个材料点，那么它的局部坐标 \boldsymbol p 恒定不变，当书本转动即 R 改变时，式（2.2）给出了该点在世界系中的新位置 \boldsymbol x；如果考察的是空间中固定不动的一个点，那么它的世界坐标 \boldsymbol x 恒定不变，当书本转动时，同一空间点在书本坐标系下的读数 \boldsymbol p 会随之改变。无论从哪个视角看待，矩阵 R 的三列始终是书本三根坐标轴在世界系中的方向向量。

这自然引出一个逆向问题：已知空间向量在世界系下的坐标 \boldsymbol x，如何反求它在物体坐标系下的三个分量 p_1,p_2,p_3？由于三根物体坐标轴彼此垂直且长度均为 1，要求 \boldsymbol x 沿第一根物体轴的分量，只需将 \boldsymbol x 向 \boldsymbol b_1 作正交投影，即计算内积 p_1=\boldsymbol b_1^{\mathsf T}\boldsymbol x；同理可得 p_2=\boldsymbol b_2^{\mathsf T}\boldsymbol x 与 p_3=\boldsymbol b_3^{\mathsf T}\boldsymbol x。注意到行向量 \boldsymbol b_1^{\mathsf T},\boldsymbol b_2^{\mathsf T},\boldsymbol b_3^{\mathsf T} 恰好组成了转置矩阵 R^{\mathsf T} 的三行，上述三次投影运算可以合并写为 \boldsymbol p=R^{\mathsf T}\boldsymbol x。在前面的例题中，将世界向量 (1,2,3)^{\mathsf T} 依次向旋转后的三根物体轴作内积投影，得到的分量正是 2,-1,3，恰好还原出最初的局部坐标。

旋转矩阵的性质

刚体在转动过程中不会发生拉伸或剪切变形，因此固连在书本上的三根坐标轴始终保持单位长度、彼此两两正交，并且维持右手系朝向不变。将这三条几何约束翻译成矩阵代数语言，就得到旋转矩阵必须满足的两项基本条件：

式中 I 为三阶单位矩阵。第一个等式称为正交性条件，它保证了旋转前后向量的长度与向量间的夹角保持不变；第二个等式保证了变换不会把右手坐标系翻转为左手镜像系。所有满足式（2.3）的三阶实矩阵在矩阵乘法下构成一个群，称为三维特殊正交群（Special Orthogonal Group），记作 \mathrm{SO}(3)。后文中写 R\in\mathrm{SO}(3)，即表示 R 是一个合法的三维旋转矩阵。

在物理引擎的实际场景中，刚体除了绕自身参考点转动外，参考点本身还会在空间中平移。设刚体参考点（如质心或局部原点）在世界系中的位置向量为 \boldsymbol c，刚体上某一点的局部坐标为 \boldsymbol p，则该点在世界系中的绝对位置就是先将局部相对位移旋转到世界系、再加上参考点位置，即 \boldsymbol x=\boldsymbol c+R\boldsymbol p。这一表达式将刚体的平移与转动统一为空间点的完整位姿变换。

在二维平面中，先后转过两个角度的结果与次序无关；但在三维空间里，情况截然不同。你可以拿手边的书本做一个简单的试验：先绕桌面横轴（x 轴）将平放的书本向上立起 90^\circ，再绕桌面纵轴（y 轴）转过 90^\circ；随后把书放回原位，调换这两步的先后次序重新操作一次。你会发现，书本最终的朝向完全不同。当我们用旋转矩阵记录连续多次转动时，这种对先后次序的敏感性就直接反映在矩阵乘法的不可交换性上。

设刚体先后经历了两次旋转，对应的旋转矩阵分别为 A 和 B，且两次旋转的转轴都在固定的世界坐标系下指定。对于原处于 \boldsymbol x 处的向量，第一次旋转将其变为 A\boldsymbol x，第二次旋转再对所得结果作用矩阵 B，使其变为 B(A\boldsymbol x)=(BA)\boldsymbol x。因此，两次世界系旋转复合后的总旋转矩阵是 BA——按时间先后依次发生的固定轴旋转，在矩阵乘法中应当从右向左依次左乘，最先执行的旋转排在最右侧。

在实验 02 中将两个转角均设为 90^\circ，可以清晰地看到两种次序的分歧所在：若先绕世界横轴 x、再绕世界纵轴 y 旋转，书本封面法线在第一步后已经转到了负纵轴方向，恰好与第二步的旋转轴共线，因此第二步旋转不再改变法线方向，使其最终停留在负纵轴；反之，若先绕世界纵轴 y、再绕世界横轴 x 旋转，封面法线在第一步后转到了正横轴方向，从而在第二步绕横轴旋转时保持不动，最终指向正横轴。虽然两次操作使用了完全相同的世界转轴与转角，但第一次转动改变了物体与第二根转轴之间的相对位置，这正是 BA 与 AB 结果不同的几何根源。

坐标变换的复合

除了在固定世界系下连续施加旋转外，物理引擎中另一类极为常见的复合运算是沿着多体机构的父子层级逐级换算坐标。例如，机器人的手掌连接在前臂末端，手掌的姿态通常是相对于前臂坐标系给出的，而前臂的姿态又是相对于世界坐标系给出的。记前臂相对于世界系的旋转矩阵为 A，手掌相对于前臂系的旋转矩阵为 B。对于手掌上的一个局部方向向量 \boldsymbol p，先左乘 B 可得它在前臂坐标系中的表示 B\boldsymbol p；随后再左乘 A，即可将它从前臂系进一步变换到世界系：

由此可知，将父刚体的世界姿态矩阵 A 与子刚体相对于父刚体的局部姿态矩阵 B 按 AB 次序相乘，所得矩阵 R 就直接给出了子刚体在世界坐标系下的姿态。后续章节在运动学树上由根节点向叶节点递推计算各连杆的世界姿态时，核心运算正是式（2.4）所示的相对变换右乘复合。

上一节用三根随体坐标轴的方向拼成了含有九个元素的旋转矩阵。一个自然的问题随之产生：无论刚体经历了多么复杂的转动过程，从初始姿态到当前姿态的净效果，能否等价为绕空间中某一条固定轴的一次单轴旋转？三维欧拉旋转定理给出了肯定的回答：任意三维旋转矩阵 R\in\mathrm{SO}(3) 都存在一条保持方向不变的单位旋转轴 \boldsymbol n，满足 R\boldsymbol n=\boldsymbol n；与此同时，与 \boldsymbol n 垂直的平面在旋转前后仍映射回该平面自身，全部三维旋转都归结为该垂直平面内转过的一个角度 \theta。

有了旋转轴和转角的概念，我们再来推导：当空间向量 \boldsymbol x 绕单位轴 \boldsymbol n 转过角度 \theta 后，旋转所得的新向量 \boldsymbol x' 应当如何用 \boldsymbol n、\theta 和 \boldsymbol x 表示？（这里旋转前后的向量均在同一个固定坐标系下表示。）既然旋转轴方向上的分量不受转动影响，自然的思路是先将原向量 \boldsymbol x 正交分解为平行于转轴的轴向分量 \boldsymbol x_\parallel 与垂直于转轴的平面分量 \boldsymbol x_\perp：

在旋转过程中，轴向分量 \boldsymbol x_\parallel 始终保持不变，只有垂直分量 \boldsymbol x_\perp 在正交于 \boldsymbol n 的平面内绕轴画圆。为了写出圆周上的旋转结果，需要在该垂直平面内构造一对相互垂直且等长的基准向量：其中一个现成的方向就是 \boldsymbol x_\perp 本身，而将单位转轴 \boldsymbol n 与原向量 \boldsymbol x 作叉积所得的 \boldsymbol n\times\boldsymbol x，恰好给出了平面内与之正交且等长的第二个方向。

当 \boldsymbol x_\perp 在垂直平面内转过角度 \theta 时，它在原方向 \boldsymbol x_\perp 与正交方向 \boldsymbol n\times\boldsymbol x 上的投影系数分别为 \cos\theta 和 \sin\theta。将旋转后的垂直分量与保持不变的轴向分量相加，便得到了著名的 Rodrigues 旋转公式：

式（2.6）中，第一项是沿旋转轴保持不动的轴向分量；后两项的线性组合则描述了垂直分量在正交平面内的圆周运动。

结合实验 03 中的几个特殊角度，可以直观地检验式（2.6）中各项的作用：当 \theta=0 时，\cos 0=1、\sin 0=0，后两项还原为 \boldsymbol x_\perp，与 \boldsymbol x_\parallel 相加正好恢复原向量 \boldsymbol x；当转角达到四分之一周（\theta=\pi/2）时，余弦项归零，垂直分量完全转到 \boldsymbol n\times\boldsymbol x 方向；当转角达到半周（\theta=\pi）时，正弦项归零而余弦项取 -1，垂直分量反向为 -\boldsymbol x_\perp，轴向分量依然不变。对于一般转角 \theta，余弦和正弦系数则在这两个正交方向之间平滑地分配权重，使向量端点沿空间圆周连续转动。

叉乘矩阵

式（2.6）以向量运算的形式给出了单个向量的旋转结果。如果能将它改写为旋转矩阵左乘 \boldsymbol x 的形式，就可以直接得到轴角 (\boldsymbol n,\theta) 所对应的旋转矩阵 R(\boldsymbol n,\theta)。观察式（2.6）可以发现，当单位轴 \boldsymbol n=(n_1,n_2,n_3)^{\mathsf T} 固定时，叉积运算 \boldsymbol n\times\boldsymbol x 对输入向量 \boldsymbol x 是线性的，因而可以用一个三阶矩阵左乘 \boldsymbol x 来表示。这个矩阵称为 \boldsymbol n 的叉乘矩阵（或反对称矩阵）：

由式（2.7）的元素排布一眼可以看出，它的主对角线全为零，上下三角关于主对角线互为相反数，满足 N^{\mathsf T}=-N。借助叉乘矩阵 N，不仅叉积项可写为 N\boldsymbol x，垂直分量 \boldsymbol x_\perp 也可表示为 -N^2\boldsymbol x。将其代回式（2.6）并把右端的公因式 \boldsymbol x 提取出来，便得到 Rodrigues 公式的矩阵形式：

为了检验式（2.8）的计算过程，我们可以考察一个三根坐标轴同时发生变化的具体例子：将刚体绕第一卦限的立方体主对角线方向 (1,1,1)^{\mathsf T} 逆时针旋转三分之一周（即 \theta=2\pi/3）。从空间对称性不难看出，这次旋转应当使 x,y,z 三根坐标轴发生循环轮换（x\to y\to z\to x），因此最终算出的旋转矩阵应该是一个每行每列只有一个 1 的置换矩阵。下面展开式（2.8）的各项予以验证，计算结果也可直接与实验 01 中的斜轴旋转对照。

前几节讨论的旋转矩阵和轴角公式，描述的都是两个离散姿态之间的有限转动。而在动力学仿真中，刚体的姿态随时间连续演化，我们需要进一步刻画姿态在每一瞬间的变化率。想象刚体正在绕过原点的某一瞬时转轴转动，固连在刚体上的任意一点都会沿着垂直于转轴的圆弧切线方向运动：该点离转轴越远，线速度越大；速度的切向方向则由右手定则确定。

为此，我们引入角速度向量：它的方向沿着瞬时旋转轴的正方向，它的模长表示单位时间内转过的弧度数（单位为 \mathrm{rad/s}）。先在世界坐标系下表示这一角速度向量，记作 \boldsymbol\omega。在极小的时间步长 h 内，刚体绕单位轴 \boldsymbol n=\boldsymbol\omega/\|\boldsymbol\omega\| 转过的微元角度约为 \theta=h\|\boldsymbol\omega\|。将 Rodrigues 公式（2.6）按时间步长 h 展开并保留到一阶项（此时 \cos\theta\approx 1、\sin\theta\approx h\|\boldsymbol\omega\|），可得刚体上固定向量 \boldsymbol x(t) 在 t+h 时刻的位置：

将等式两边减去 \boldsymbol x(t) 后同除以时间间隔 h，再令 h\to 0 取极限，便得到随刚体一同转动的向量对时间的导数公式：

式（2.9）中的叉积 \boldsymbol\omega\times\boldsymbol x 既垂直于转轴 \boldsymbol\omega，又垂直于位置向量 \boldsymbol x，因而严格指向端点圆周轨迹的切线方向；其模长 \|\boldsymbol\omega\|\|\boldsymbol x_\perp\| 则等于角速度大小与点到转轴距离（圆周半径）的乘积。在前面的轴向分解与平面旋转实验中勾选“切向方向”，即可直观地观察这一瞬时速度方向如何始终与圆轨迹相切。

既然固连在刚体上的任意向量都满足式（2.9），构成旋转矩阵 R=[\boldsymbol b_1\ \boldsymbol b_2\ \boldsymbol b_3] 的三根随体坐标轴自然也分别满足 \dot{\boldsymbol b}_i=\boldsymbol\omega\times\boldsymbol b_i=[\boldsymbol\omega]_\times\boldsymbol b_i。将这三列的变化率重新并排拼成一个 3\times 3 矩阵，便得到了旋转矩阵对时间的微分方程。此外，在动力学计算和传感器测量（如陀螺仪）中，角速度常常是在随刚体运动的物体坐标系下给出的，记作体坐标角速度 \boldsymbol\omega_b。这两种角速度表示下的姿态微分方程分别为：

这里 \boldsymbol\omega 与 \boldsymbol\omega_b 描述的是同一个物理角速度向量在不同坐标系下的分量，满足坐标变换关系 \boldsymbol\omega=R\boldsymbol\omega_b。值得特别留意的是式（2.10）中叉乘矩阵的位置规律：使用世界系角速度 \boldsymbol\omega 时，叉乘矩阵 [\boldsymbol\omega]_\times 左乘在 R 的左侧；使用物体坐标系角速度 \boldsymbol\omega_b 时，叉乘矩阵 [\boldsymbol\omega_b]_\times 右乘在 R 的右侧。

由姿态变化恢复角速度

反过来，如果已知刚体在某时刻的姿态矩阵 R 及其时间导数 \dot R，我们也可以在式（2.10）两边右乘 R^{\mathsf T}，直接反解出世界系角速度的叉乘矩阵 \Omega=[\boldsymbol\omega]_\times。这一逆向关系同时揭示了一个重要事实：尽管 \dot R 含有九个导数元素，但由于正交性约束，\dot R R^{\mathsf T} 必然是一个只有三个独立分量的反对称矩阵：

最后，我们将刚体参考点的平移运动也纳入进来。设刚体参考点在世界系中的位置为 \boldsymbol c(t)，线速度为 \boldsymbol v=\dot{\boldsymbol c}。对于固连在刚体上的任意材料点，其局部坐标 \boldsymbol p 不随时间改变，对世界位置关系式 \boldsymbol x(t)=\boldsymbol c(t)+R(t)\boldsymbol p 两边关于时间求导，并代入 \dot R=[\boldsymbol\omega]_\times R，即得刚体上任意一点的世界速度公式：

该式表明，刚体上任一点的绝对速度等于参考点的平移速度 \boldsymbol v 与绕参考点转动所产生的切向速度 \boldsymbol\omega\times(R\boldsymbol p) 的矢量和。后续章节推导关节空间雅可比矩阵和接触点速度约束时，都将以这一公式作为起点。

下面的交互实验将参考点平移速度 \boldsymbol v、转动切向速度 \boldsymbol\omega\times(R\boldsymbol p) 以及两者的合速度 \dot{\boldsymbol x} 放在同一三维场景中，便于直观比较材料点的到轴距离与沿轴高度对速度的影响。

在实验 04 中调节滑块可以看到：当材料点仅沿着平行于旋转轴的方向上下移动时，相对位置向量 R\boldsymbol p 的轴向分量与 \boldsymbol\omega 平行，在叉积 \boldsymbol\omega\times(R\boldsymbol p) 中恰好消去，因此转动速度的大小和方向都保持不变；而当增大材料点到转轴的垂直距离时，转动速度的大小随之成正比增大。第 4 章将进一步从圆弧弧长与转角的关系出发，利用这一速度分布计算整个刚体的转动动能。

姿态微分方程（2.10）刻画了旋转矩阵在某一时刻的瞬时变化率。要在物理引擎中将刚体姿态从当前时刻 t 推进到下一时刻 t+h，就需要对这一微分方程在时间步长 h 上积分。我们先从角速度在一个时间步内保持恒定的情形入手。

在微积分中，一阶常系数线性标量微分方程 \dot y=ay 的解析解可以写成指数函数 y(t+h)=e^{ha}y(t)。与之完全类似，当世界系角速度 \boldsymbol\omega 为常向量时，线性矩阵微分方程 \dot R=[\boldsymbol\omega]_\times R 的解析解也可以用矩阵指数（Matrix Exponential）写出，其中三阶矩阵的指数函数由收敛的无穷幂级数定义：

将式（2.11）给出的旋转增量左乘在当前姿态 R(t) 的左侧，就把恒定世界角速度在时间步 h 内产生的有限转动累积到了姿态上；反之，如果给定的是在一个时间步内保持恒定的体坐标角速度 \boldsymbol\omega_b，则对应于右乘微分方程 \dot R=R[\boldsymbol\omega_b]_\times，旋转增量应当右乘在当前姿态的右侧：

在真实的多体动力学仿真中，刚体的角速度通常会随时间连续变化。此时我们在每个离散时间步 h 内取该步的角速度代表值（如步末或步初的角速度），将这一小段时间近似视为恒定角速度运动，再调用式（2.12）更新姿态。虽然步内角速度取常值是一种时间离散近似，会影响旋转轨迹的时间精度，但由矩阵指数构造出的增量本身始终是一个严格的正交旋转矩阵。

这里需要注意，式（2.12）的左乘与右乘两种更新方式，分别要求角速度在世界坐标系下恒定或在物体坐标系下恒定：当转轴固定在世界空间中时使用左乘，当转轴固连在刚体上随体转动时使用右乘。在一般的动力学步进循环中，求解器先根据当前步的受力算出新的角速度，随后通过式（2.12）生成当前步的有限旋转增量并完成姿态更新，从而把动力学速度求解与几何姿态推进衔接起来。

为什么矩阵指数生成的增量一定能严格保持正交性？我们可以从导数的角度给出一个简洁的证明。记反对称矩阵 \Omega=[\boldsymbol\omega]_\times，并设 E(h)=\exp(h\Omega)。由于 E(h) 对步长 h 的导数满足 E'=\Omega E，结合反对称性质 \Omega^{\mathsf T}+\Omega=0，对乘积 E^{\mathsf T}E 求导可得：

这说明 E(h)^{\mathsf T}E(h) 不随时间步长 h 变化。又因为初始时刻 E(0)=I 满足 E(0)^{\mathsf T}E(0)=I，故对任意步长 h 恒有 E(h)^{\mathsf T}E(h)=I；再由行列式随 h 的连续性以及 \det E(0)=1，可知 \det E(h)=1 始终成立。下面的折叠推导从幂级数逐项求导出发，严格验证上面用到的求导性质 E'=\Omega E。

指数映射与 Rodrigues 公式的关系

式（2.11）是用无穷级数定义的，在实际编写程序时显然不能直接累加无穷多项。为了得到有限项的闭合计算式，我们将单位旋转轴 \boldsymbol n 与转角 \theta 合并记为一个三维旋转向量 \boldsymbol\phi=\theta\boldsymbol n（在一步姿态积分中即取 \boldsymbol\phi=h\boldsymbol\omega 或 h\boldsymbol\omega_b），它的方向代表转轴，模长 \|\boldsymbol\phi\|=\theta 代表转角。利用第 3 节导出过的叉乘矩阵递推性质 N^3=-N（其中 N=[\boldsymbol n]_\times），矩阵指数级数中所有三次及以上的高次幂都可以归并到 N 和 N^2 两项上，合并同类项后恰好还原为 Rodrigues 公式：

这一结果说明，计算矩阵指数 \exp([\boldsymbol\phi]_\times) 只需做右侧的三项矩阵加法。至此，第 3 节基于空间向量正交分解的几何推导，与本节基于姿态微分方程积分的分析推导殊途同归：从反对称矩阵 [\boldsymbol\phi]_\times 到旋转矩阵 R\in\mathrm{SO}(3) 的映射 \exp([\boldsymbol\phi]_\times) 称为三维旋转群的指数映射（Exponential Map）。

小角度下的数值计算

在仿真器的单步更新中，时间步长 h 往往只有毫秒量级，因此单步转角 \theta=\|\boldsymbol\phi\| 通常非常接近于零（甚至在刚体静止时严格等于零）。如果直接先用 \boldsymbol n=\boldsymbol\phi/\theta 归一化求单位转轴，当 \theta\to 0 时就会遇到除以零或浮点下溢的问题。为了直接以未归一化的旋转向量 \boldsymbol\phi（及其叉乘矩阵 A=[\boldsymbol\phi]_\times=\theta N）作为输入，我们可以将 N=A/\theta 代入式（2.13），把指数映射改写为：

观察式（2.14）中的两个标量系数 a(\theta)=\frac{\sin\theta}{\theta} 和 b(\theta)=\frac{1-\cos\theta}{\theta^2}：当 \theta\to 0 时，两者的分子与分母同时趋于零，但在数学上各自存在光滑的有限极限 a(0)=1 和 b(0)=1/2。因此，在 \theta 很小时改用这两个系数的泰勒级数求值，就能彻底避开 0/0 型浮点除法，保证零角附近的数值稳定性。

在实际代码实现中，我们先根据 \theta=\|\boldsymbol\phi\| 的大小独立算出标量系数 a(\theta) 和 b(\theta)，再组合出 R=I+a(\theta)A+b(\theta)A^2。尤其方便的是，即便输入严格为零向量 \boldsymbol\phi=\boldsymbol 0，此时虽然 a(0)=1、b(0)=1/2，但由于 A=0，式（2.14）依然能平滑地给出单位矩阵 R=I，无需为静止刚体单独编写跳过归一化的特判逻辑。

从实验 05 的曲线可以看到，a(\theta) 与 b(\theta) 在 \theta=0 附近都是关于 \theta 的偶函数，图像在零点处十分平缓，偏离极限值的主导项均为二次项 \theta^2。以 \theta=0.1\,\mathrm{rad} 为例，两个系数的二阶泰勒近似分别为 1-0.01/6 和 1/2-0.01/24，而被截断的下一个四次项分别为 10^{-4}/120\approx 8.3\times 10^{-7} 和 10^{-4}/720\approx 1.4\times 10^{-7}。将这一截断余项的数量级与双精度浮点数的机器精度对比，便能清楚地确定切换泰勒展开的角度阈值与所需的展开阶数。

姿态积分的正交误差

既然单步转角很小，如果我们在幂级数（2.11）中只保留前两项（即一阶线性项），直接对旋转矩阵作显式欧拉（Euler）步进，结果会怎样？以体坐标角速度为例，单步显式 Euler 更新写为：

式中 R_E 表示一步 Euler 更新后的矩阵。从几何上看，增量 h[\boldsymbol\omega_b]_\times\boldsymbol p 沿着圆周的切线方向迈步，而切线上的步进点必然落在圆周外侧，因此每走一步都会使垂直于转轴的随体坐标轴被拉长一点。固定角速度大小和总仿真时长后，我们可以在下面的实验中调节时间步长 h，直观对比显式 Euler 更新与指数更新的长期表现。

实验 06 使用 Frobenius 范数 \varepsilon=\|R^{\mathsf T}R-I\|_F 来定量衡量矩阵偏离正交群 \mathrm{SO}(3) 的程度（其中下标 F 表示将矩阵全部九个元素的平方求和后再开方，同时反映了坐标轴长度偏离 1 以及轴间夹角偏离直角的误差）。可以看到，指数映射更新的正交误差始终維持在 10^{-15} 量级的浮点舍入误差附近；而显式 Euler 更新则会不断累积来自一阶截断的几何变形误差。

具体而言，对于垂直于固定转轴的初始单位向量，显式 Euler 更新每走一步都会像直角三角形求斜边一样将其长度放大为原来的 \sqrt{1+h^2\omega^2} 倍；连续执行 k 步后，该轴的长度便膨胀为 r_k=(1+h^2\omega^2)^{k/2}，在图上表现为端点沿向外发散的螺线飞离单位圆。当固定总时长 T 并缩小时间步长 h 时，虽然总步数 k=T/h 增加了，但每一步的伸长因子 1+h^2\omega^2 以 h^2 的高阶速度趋于 1，将 k=T/h 代入即可看出累积膨胀随步长缩小而减弱。

在姿态控制与运动规划中，我们经常需要比较两个姿态之间的偏差：设刚体的当前姿态为 R，期望到达的目标姿态为 S（两者均表示从物体坐标系到世界坐标系的旋转）。如果要在世界坐标系下施加一次旋转 Q，把当前姿态 R 直接转到目标姿态 S，那么按世界系旋转左乘的复合规则应有 QR=S。在等式两边同时右乘 R^{\mathsf T}，即可解出：

矩阵 Q 就称为从当前姿态 R 到目标姿态 S 的相对旋转（在世界系下表示）。一旦从矩阵 Q 中反解出对应的旋转轴和转角，就能用三维旋转向量定量刻画当前姿态与目标姿态之间的朝向误差。

同样地，如果希望在当前刚体的物体坐标系下表示这一姿态偏差，则按相对变换右乘的复合规则应满足 RQ_b=S，两边左乘 R^{\mathsf T} 即得体坐标下的相对旋转矩阵 Q_b=R^{\mathsf T}S。由于两种相对旋转描述的是同一次空间转动，它们所对应的世界系旋转向量 \boldsymbol\phi 与物体坐标系旋转向量 \boldsymbol\phi_b 满足坐标变换关系 \boldsymbol\phi=R\boldsymbol\phi_b。下一章介绍四元数时，“目标四元数右乘当前四元数之逆”与“当前四元数之逆左乘目标四元数”这两种姿态差公式，分别对应的正是这里的左乘增量 Q=SR^{\mathsf T} 与右乘增量 Q_b=R^{\mathsf T}S。

从旋转矩阵求轴角

给定一个旋转矩阵 Q\in\mathrm{SO}(3)，如何反过来求出它的旋转角 \theta 与单位旋转轴 \boldsymbol n？观察 Rodrigues 公式 Q=I+\sin\theta\,[\boldsymbol n]_\times+(1-\cos\theta)[\boldsymbol n]_\times^2 可以发现：右侧三项中，I 和 [\boldsymbol n]_\times^2 都是对称矩阵，唯独 \sin\theta\,[\boldsymbol n]_\times 是反对称矩阵。因此，对矩阵 Q 分别取迹（即主对角线三个元素之和 \operatorname{tr}Q）和反对称部分 Q-Q^{\mathsf T}，就能把 \cos\theta 与 \sin\theta\,[\boldsymbol n]_\times 干净地分离出来：

将主值转角 \theta 约定在闭区间 [0,\pi] 内，由式（2.16）第一式即可通过反余弦函数求出转角 \theta=\arccos((\operatorname{tr}Q-1)/2)；由第二式的三个独立非对角元差值则可提取出旋转轴 \boldsymbol n 的三个分量。当 0<\theta<\pi 时，\sin\theta>0，由此合成的旋转向量 \boldsymbol\phi=\theta\boldsymbol n 为：

式中 Q_{ij} 表示矩阵 Q 的第 i 行、第 j 列元素。这一从旋转矩阵 Q 反求反对称矩阵 [\boldsymbol\phi]_\times（或旋转向量 \boldsymbol\phi）的运算，称为旋转群上的对数映射（Logarithmic Map），它是第 5 节指数映射的逆运算：

之所以需要先规定主值区间 \theta\in[0,\pi]，是因为从旋转向量到旋转矩阵的指数映射是多对一的：绕同一根轴多转整数圈（\theta+2k\pi），或者把转轴反向的同时把转角取负（-\boldsymbol n,-\theta），得到的都是同一个旋转矩阵：

将转角 \theta 限制在主值区间 [0,\pi] 后，在开区间 (0,\pi) 内的任意普通转角都对应唯一确定的单位转轴 \boldsymbol n。而在区间的两个端点处则出现几何退化：当 \theta=0（即 Q=I）时，没有发生转动，旋转轴可以任意选取，旋转向量始终唯一地取为零向量 \boldsymbol\phi=\boldsymbol 0；当 \theta=\pi（半周旋转）时，绕 \boldsymbol n 转过 \pi 与绕 -\boldsymbol n 转过 \pi 完全等价，因此存在一对互为相反数的旋转向量 \pm\pi\boldsymbol n 对应同一个半周姿态（当旋转连续跨过 \theta=\pi 时，主值旋转向量会在这对反向端点之间切换，但所表示的三维姿态本身依然连续）。接下来我们就专门讨论这两个端点附近的数值求值策略。

零角与半周附近的数值处理

在编写通用对数映射函数时，直接套用式（2.17）会在主值区间的两个端点 \theta\to 0 和 \theta\to\pi 处遇到分母 2\sin\theta\to 0 的数值困难。针对两端不同的退化机理，我们需要分别采用对应的稳定算法：在 \theta=0 处直接返回零向量 \boldsymbol 0，在接近零角的小角度区间内利用泰勒展开计算比例系数 \theta/\sin\theta\to 1；而在转角接近半周（\theta\to\pi）时，反对称部分 Q-Q^{\mathsf T} 因 \sin\theta\to 0 而趋于零矩阵，此时需要改从矩阵的对称部分 Q+Q^{\mathsf T} 中提取旋转轴 \boldsymbol n。

为什么连转角 \theta 本身的求法也需要改进？这源于余弦函数 c=\cos\theta 在 \theta=0 和 \theta=\pi 两个端点处的导数 -\sin\theta 均为零，函数曲线在两端极其平缓：哪怕矩阵的迹因为浮点舍入混入了极微小的余弦误差 \delta，反余弦函数 \arccos(c+\delta) 也会在端点附近将角度误差放大到 \sqrt{|\delta|} 量级。反之，如果先从反对称部分算出 s=\sin\theta\ge 0，再把正弦 s 与余弦 c 一起送入双参数反正切函数 \theta=\operatorname{atan2}(s,c)，就能充分利用正弦函数在两端斜率最大的优势，在整个 [0,\pi] 区间内准确地恢复转角。

在实验 07 中，我们记 s=\sin\theta、c=\cos\theta，并通过滑块向余弦值人为注入一个可控扰动 \delta（同时保持正弦值 s 不变，并在求反余弦前将 c+\delta 截断到 [-1,1] 定义域内），以便单独观察余弦误差向转角传播的放大效应。虽然实际浮点计算中反对称部分也会带有微小舍入误差，但图中这一对照清晰地展示了为什么在端点附近不能单靠 \arccos c 定角。

定出转角 \theta 之后，下一步是恢复单位转轴 \boldsymbol n。对照实验 07 中的棕线与绿线可以看到：反对称部分所含的因子 \sin\theta（棕线）在半周 \theta=\pi 处衰减为零，导致式（2.17）完全丧失轴向信噪比；与此同时，对称部分所含的因子 1-\cos\theta（绿线）却在 \theta=\pi 处达到最大值 2。因此，当转角逼近 \pi 时，改从对称矩阵中提取轴向就能获得最充沛的数值信号。

综合实验 07 的三条曲线，完整的旋转对数映射算法可以清晰地归纳为两步：第一步，先从反对称部分 Q-Q^{\mathsf T} 构造向量 \boldsymbol a=\sin\theta\,\boldsymbol n，以其模长 s=\|\boldsymbol a\| 作为正弦、以 c=(\operatorname{tr}Q-1)/2 作为余弦，调用 \theta=\operatorname{atan2}(s,c) 求出稳定转角；第二步，根据 \theta 所在的区间分三支恢复旋转向量——小角度区间用 \theta/\sin\theta 的泰勒级数缩放 \boldsymbol a，中间普通角度区间直接按式（2.17）计算 (\theta/s)\boldsymbol a，而在接近 \pi 的半周区间则改从对称矩阵 Q+Q^{\mathsf T} 提取单位轴 \boldsymbol n。这套分支策略的核心思想，就是在每一个角度区间都挑选数值幅度最大、条件数最好的矩阵部分来提取几何信息。

前面建立的旋转矩阵坐标变换、相对旋转复合以及指数映射积分，在物理引擎底层都有直接对应的实现。本节对照 MuJoCo 3.10.0 的 C 语言源码（版本号声明见 mujoco/CMakeLists.txt 第 32 行），看看这些数学运算在引擎内核中是如何组织与调用的。

旋转矩阵的存储方式

在内存中，MuJoCo 使用一个长度为 9 的一维连续浮点数组，按行优先（Row-Major）次序存储 3\times 3 旋转矩阵。因此，数学上代表第一根随体坐标轴的第一列 (r_{11},r_{21},r_{31})^{\mathsf T}，在数组中的下标依次为 0, 3, 6；要读取某一根随体坐标轴在世界系中的方向向量，只需按步长 3 跨行取数即可：

例如，在 engine_util_blas.c 第 153 行定义的 mju_mulMatVec3 函数实现了正向坐标变换 \boldsymbol x=R\boldsymbol p，其内部直接用矩阵的每一行与输入向量作内积，计算出三个输出分量：

// mju_mulMatVec3 中的三个输出分量
mat[0]*vec[0] + mat[1]*vec[1] + mat[2]*vec[2]
mat[3]*vec[0] + mat[4]*vec[1] + mat[5]*vec[2]
mat[6]*vec[0] + mat[7]*vec[1] + mat[8]*vec[2]

紧随其后的 mju_mulMatTVec3 函数则按列跨步取数，用矩阵的每一列与输入向量作内积以计算 \boldsymbol p=R^{\mathsf T}\boldsymbol x。当传入的矩阵为旋转矩阵时，该函数便直接完成了从世界坐标系到物体坐标系的逆向坐标变换。

世界姿态的计算与存储

在每个仿真步的前向运动学阶段，MuJoCo 沿着运动学树从根节点向叶节点递推复合各层变换，并将算出的每个刚体的世界位姿缓存在数据结构 mjData 中：其中 mjData.xquat 存放各刚体的世界姿态四元数，mjData.xmat 存放由四元数转换而来的 3\times 3 世界姿态旋转矩阵，mjData.xpos 则存放刚体局部原点在世界系下的三维位置。在 engine_core_smooth.c 第 182 行附近，可以看到将第 i 个刚体的计算结果写入全局缓存并同步生成旋转矩阵的代码：

// 每个刚体分别占四个、三个、九个数
mji_copy4(d->xquat + 4*i, xquat);
mji_copy3(d->xpos + 3*i, xpos);
mju_quat2Mat(d->xmat + 9*i, xquat);

之所以在保存四元数的同时预先算出旋转矩阵 xmat，是因为后续的碰撞检测与渲染模块需要将大量局部几何顶点快速变换到世界系；直接调用缓存好的 xpos 与 xmat 执行 \boldsymbol x=\boldsymbol c+R\boldsymbol p，可以避免对同一个刚体重复进行四元数到矩阵的转换。

体坐标角速度与右乘更新

关于三维转动的速度参考系，doc/overview.rst 自第 1013 行起明确规定了自由关节（Free Joint）与球关节（Ball Joint）的速度约定：刚体的线速度分量在世界坐标系下表示，而角速度分量则在随刚体运动的物体坐标系下表示（即本章的 \boldsymbol\omega_b）。当时间积分函数 mj_integratePos 推进自由关节或球关节的姿态时，会在内部调用 mju_quatIntegrate 完成单步旋转更新。

打开 engine_util_spatial.c 第 234 行可以看到，mju_quatIntegrate 先根据体坐标角速度与时间步长 h 构造出单步旋转增量 qrot，随后调用 mju_mulQuat(quat, quat, qrot) 将该增量右乘在当前姿态四元数 quat 的右侧。转换成矩阵语言，这一右乘次序对应的正是式（2.12）的第二式：

有关这两个函数的接口参数说明，可进一步查阅官方文档中的 mj_integratePos 与 mju_quatIntegrate。

将本章推导的指数映射装入代码后，单步姿态更新函数只需接收当前姿态矩阵 R、体坐标角速度 omega_body 和时间步长 h，先算出单步旋转向量 \boldsymbol\phi=h\boldsymbol\omega_b，再将指数映射生成的增量矩阵右乘到 R 上即可，直接对应式（2.12）：

def step_body(R, omega_body, h):
    phi = [h * w for w in omega_body]
    return mul(R, exp_so3(phi))

本章配套的示例脚本 rotation.py 完整实现了三阶矩阵乘法、叉乘矩阵、指数映射、对数映射以及世界系和物体坐标系下的姿态步进；测试脚本 check_rotation.py 则使用解析已知的特殊轴旋转、随机轴角样本以及零角和半周边界角度对实现进行逐项校验。两个脚本均仅依赖 Python 标准库。

在学习笔记根目录下运行以下命令即可执行全部校验：

python3 examples/check_rotation.py

建议读者动手依次实现 skew、mul 和 exp_so3，先用正文中的直角旋转与立方体对角线旋转两个例题检验旋转方向是否正确，再实现单步姿态更新；最后实现包含小角与半周分支的 log_so3，验证从任意旋转矩阵提取出旋转向量 \boldsymbol\phi 后，能否通过 \exp([\boldsymbol\phi]_\times) 精确重建原矩阵。

from math import sqrt
from rotation import identity, mul, transpose, skew, exp_so3

def compare(h):
    steps = round(6 / h)
    K = skew([0, 0, 2])
    increments = [
        [a + h * b for a, b in zip(identity(), K)],
        exp_so3([0, 0, 2 * h]),
    ]
    for increment in increments:
        R = identity()
        for _ in range(steps):
            R = mul(R, increment)
        gram = mul(transpose(R), R)
        error = sqrt(sum((a - b) ** 2
                         for a, b in zip(gram, identity())))
        print(f"{error:.6g}")

compare(0.1)
compare(0.01)