姿态矩阵的表示方式及应用

路线概览

欧拉角（最直觉） → 旋转矩阵（从局部到全局） → 轴角（本质揭示） → 四元数（完美插值） → 6D 表示（深度学习最爱）

每一步都是为了解决前一步的致命缺陷，而且数学上可以严格推导。

0. 预备：旋转群 SO(3)SO(3)SO(3)

三维空间中的所有旋转构成特殊正交群：
SO(3)={R∈R3×3∣RTR=I,det⁡R=+1} SO(3)=\{ \mathbf{R}\in\mathbb{R}^{3\times3} \mid \mathbf{R}^T\mathbf{R}=\mathbf{I}, \det\mathbf{R}=+1 \} SO(3)={R∈R3×3∣RTR=I,detR=+1}

任给 R∈SO(3)\mathbf{R}\in SO(3)R∈SO(3)，它把刚体连体坐标系的三根轴（各自单位正交）变换到世界坐标系，矩阵的列就是连体轴在世界系下的坐标。

旋转合成直接用矩阵乘法，结合律成立且不引入歧义。这是唯一无任何奇异性、无任何冗余的全局表示，但它有9个参数，自身不适合直接做插值。

1：欧拉角 ------ 最简单，最危险

直觉来源

你用手去拧一个夹爪，会自然地说"先绕 Z 轴偏航 30°，再绕 Y 轴俯仰 15°，再绕 X 轴滚转 10°"。这就是欧拉角------用三个独立转角按固定顺序描述姿态。

数学本质

任意旋转可分解为三个基本旋转的乘积：
R=Rx(ϕ) Ry(θ) Rz(ψ) \mathbf{R} = \mathbf{R}_x(\phi)\,\mathbf{R}_y(\theta)\,\mathbf{R}_z(\psi) R=Rx(ϕ)Ry(θ)Rz(ψ)

其中 ϕ,θ,ψ\phi,\theta,\psiϕ,θ,ψ 是角度，顺序可定义（如 XYZ 外旋或内旋）。三维向量 (ϕ,θ,ψ)(\phi,\theta,\psi)(ϕ,θ,ψ) 就是欧拉角，参数量最少（只有 3 个），也最符合人类直觉。

致命伤

万向节死锁 ：当俯仰角 θ=±90∘\theta = \pm 90^\circθ=±90∘ 时，绕 X 和绕 Z 的旋转轴在空间中对齐，丧失一个自由度，导致旋转轨迹在此处奇变。
周期性边界 ：角度在 ±180∘\pm 180^\circ±180∘ 附近表达不唯一，线性插值 179° → -179° 会绕 358° 长弧。这也是你末端姿态"剧烈抖动"的直接原因。

既然欧拉角只是旋转矩阵的一种参数化，我们自然要去看那个被当作"积木"的矩阵。

1.1 定义

选定一个旋转顺序（例如 Z-Y-X 内旋），任意旋转可分解为
R=Rz(ψ) Ry(θ) Rx(ϕ) \mathbf{R} = \mathbf{R}_z(\psi)\,\mathbf{R}_y(\theta)\,\mathbf{R}_x(\phi) R=Rz(ψ)Ry(θ)Rx(ϕ)

其中基本旋转矩阵为：
Rx(ϕ)=[1000cos⁡ϕ−sin⁡ϕ0sin⁡ϕcos⁡ϕ], Ry(θ)=[cos⁡θ0sin⁡θ010−sin⁡θ0cos⁡θ], Rz(ψ)=[cos⁡ψ−sin⁡ψ0sin⁡ψcos⁡ψ0001] \mathbf{R}_x(\phi)=\begin{bmatrix}1&0&0\\0&\cos\phi&-\sin\phi\\0&\sin\phi&\cos\phi\end{bmatrix},\; \mathbf{R}_y(\theta)=\begin{bmatrix}\cos\theta&0&\sin\theta\\0&1&0\\-\sin\theta&0&\cos\theta\end{bmatrix},\; \mathbf{R}_z(\psi)=\begin{bmatrix}\cos\psi&-\sin\psi&0\\\sin\psi&\cos\psi&0\\0&0&1\end{bmatrix} Rx(ϕ)= 1000cosϕsinϕ0−sinϕcosϕ ,Ry(θ)= cosθ0−sinθ010sinθ0cosθ ,Rz(ψ)= cosψsinψ0−sinψcosψ0001

乘出后，得到矩阵的显式（略）。给定三元组 (ϕ,θ,ψ)(\phi,\theta,\psi)(ϕ,θ,ψ)，可唯一计算出 R\mathbf{R}R。反向从 R\mathbf{R}R 提取欧拉角：
θ=arcsin⁡(−r31)ϕ=atan2⁡(r32/cos⁡θ, r33/cos⁡θ)ψ=atan2⁡(r21/cos⁡θ, r11/cos⁡θ) \begin{aligned} \theta &= \arcsin(-r_{31}) \\ \phi &= \operatorname{atan2}(r_{32}/\cos\theta,\; r_{33}/\cos\theta) \\ \psi &= \operatorname{atan2}(r_{21}/\cos\theta,\; r_{11}/\cos\theta) \end{aligned} θϕψ=arcsin(−r31)=atan2(r32/cosθ,r33/cosθ)=atan2(r21/cosθ,r11/cosθ)

（当 cos⁡θ≠0\cos\theta\neq0cosθ=0）

1.2 问题：万向节死锁

当 θ=±π2\theta = \pm\frac{\pi}{2}θ=±2π，则 cos⁡θ=0\cos\theta=0cosθ=0，上面公式失效。此时矩阵退化为
R=[0sin⁡(ϕ±ψ)cos⁡(ϕ±ψ)0cos⁡(ϕ±ψ)∓sin⁡(ϕ±ψ)−100] \mathbf{R} = \begin{bmatrix} 0 & \sin(\phi\pm\psi) & \cos(\phi\pm\psi)\\ 0 & \cos(\phi\pm\psi) & \mp\sin(\phi\pm\psi)\\ -1 & 0 & 0 \end{bmatrix} R= 00−1sin(ϕ±ψ)cos(ϕ±ψ)0cos(ϕ±ψ)∓sin(ϕ±ψ)0

可见第一行和第二行的元素只依赖于 (ϕ±ψ)(\phi\pm\psi)(ϕ±ψ) 这一个组合，无法解出 ϕ\phiϕ 和 ψ\psiψ 独立值------丧失了一个自由度，这就是万向节死锁。在参数空间中，这一条线对应 SO(3) 的一个二维子集，映射不是局部微分同胚。

1.3 问题：周期性边界与非最短弧插值

欧拉角是周期函数，将 ψ\psiψ 从 179∘179^\circ179∘ 变到 −179∘-179^\circ−179∘ 实际上对应于 SO(3) 中几乎重合的两个姿态（只差 2∘2^\circ2∘），但在参数空间它们相差 358∘358^\circ358∘。对欧拉角直接线性插值：
θ(t)=(1−t)θstart+tθend \boldsymbol{\theta}(t) = (1-t)\boldsymbol{\theta}{\text{start}} + t\boldsymbol{\theta}{\text{end}} θ(t)=(1−t)θstart+tθend

得到的路径在参数空间是一条直线，映射回 SO(3) 后路径会穿过 180∘180^\circ180∘ 的边界，导致物理上夹爪旋转接近一整圈。这正是你遇到的抖动根源。

所以欧拉角只适合作为"路标"（单点表示），绝不能用于规划连续的姿态轨迹。

2：旋转矩阵 ------ 无歧义的全局描述

怎么从欧拉角推出来

你把三个基本旋转矩阵乘起来，就得到了一个 3×33\times33×3 的正交矩阵：
R=[r11r12r13r21r22r23r31r32r33]∈SO(3) \mathbf{R} = \begin{bmatrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{bmatrix} \in SO(3) R= r11r21r31r12r22r32r13r23r33 ∈SO(3)

这个矩阵的每一列就是末端坐标系的 X、Y、Z 轴在世界坐标系下的单位向量。它没有死锁 ，不受边界困扰，旋转合成直接矩阵乘法。

致命伤

9 个参数，有 6 个约束（正交性），冗余且不适合直接做插值。
如果把矩阵的每个元素做线性插值，得到的中间矩阵通常不再是正交矩阵，强行"掰正"后旋转路径不匀速，甚至不是最短弧。

但我们直觉知道：任何刚性旋转都应该可以用 绕某个空间固定轴转一个角度 来描述。这个想法直接导致轴角的诞生。

2.1 从旋转矩阵提取轴与角

定理（欧拉旋转定理） ：任何旋转 R≠I\mathbf{R}\neq\mathbf{I}R=I 有唯一的旋转轴单位向量 u\mathbf{u}u 和转角 θ∈(0,π)\theta\in(0,\pi)θ∈(0,π)，使得 R\mathbf{R}R 可写成罗德里格斯公式：
R=I+sin⁡θ [u]×+(1−cos⁡θ) [u]×2(1) \mathbf{R} = \mathbf{I} + \sin\theta\,[\mathbf{u}]\times + (1-\cos\theta)\,[\mathbf{u}]\times^2 \tag{1} R=I+sinθ[u]×+(1−cosθ)[u]×2(1)

其中反对称矩阵