三维空间中的旋转

这篇文章总结了用旋转矩阵 / 旋转向量 / 欧拉角 / 四元数对三维点进行旋转的方式以及他们之间的转换关系。

旋转矩阵 #

定义 #

旋转矩阵 $R$ 是一个行列式为 $1$ 的正交矩阵（反过来说，行列式为 $1$ 的正交矩阵是一个旋转矩阵也成立）。旋转矩阵的集合可以定义为

$$\text{SO}(n) = \{\textbf{R} \in \mathbb{R}^{n \times n} | \textbf{RR}^T = \textbf{I}, \text{det}(\textbf{R}) = 1\}.$$

$\text{SO}$ 为特殊正交群（Special Orthogonal Group），$\text{SO}(3)$ 中的元素就描述了三维空间内的旋转。

转换为旋转向量 #

根据罗德里格斯公式 ^[1] 有

$$\text{tr}(\textbf{R}) = 1 + 2\cos{\theta}.$$

故旋转角为

$$\theta = \arccos(\frac{\text{tr}(\textbf{R}) - 1}{2}).$$

由于旋转轴经过旋转矩阵 $\textbf{R}$ 旋转后不变，故有

$$\textbf{Rn} = \textbf{n}.$$

即转轴 $\textbf{n}$ 为矩阵 $\textbf{R}$ 特征值 $1$ 对应的特征向量。求解方程后归一化即得到旋转轴 $\textbf{n}$。

转换为四元数 #

求出对应的旋转角 $\theta$ 和旋转向量 $\textbf{n}$ 后，使用旋转向量 $\rightarrow$ 四元数进行转换。

旋转向量 #

定义 #

在三维中，旋转矩阵用 9 个量描述一个三自由度的旋转，这种表达方式是冗余的，且旋转矩阵要求正交且行列式为 $1$，这些约束会使矩阵的估计和优化变得困难。旋转向量使用 3 个量描述一个三维空间中的旋转，其形式为 $\theta \textbf{n}$，其中 $\theta$ 为旋转的角度，$\textbf{n}$ 为旋转轴的方向。

转换为旋转矩阵 #

使用罗德里格斯公式 ^[1:1]

$$\textbf{R} = \cos{\theta}\textbf{I} + (1 - \cos{\theta})\textbf{nn}^T + \sin{\theta}n^{\wedge}.$$

其中 $\wedge$ 为向量到反对称矩阵的转换符：

$$\textbf{n}^{\wedge} = \left [ \begin{array}{ccc} 0 & -n_3 & n_2 \\ n_3 & 0 & -n_1 \\ -n_2 & n_1 & 0 \end{array} \right ].$$

转换为四元数 #

记旋转轴方向为 $\textbf{n}$，旋转角度为 $\theta$，对应的四元数 $\textbf{q}$ 为

$$\textbf{q} = [ \cos{\frac{\theta}{2}}, n_1\sin{\frac{\theta}{2}}, n_2\sin{\frac{\theta}{2}}, n_3\sin{\frac{\theta}{2}} ].$$

欧拉角 #

定义 #

欧拉角用直观的方式描述旋转。它使用三个角度 yaw pitch roll 来描述旋转，三个角度分别对应于刚体的 $z$ 轴，$y$ 轴和 $x$ 轴。对于 $ZYX$ 转角，旋转顺序可以描述为

1. 绕 $z$ 轴旋转角度 yaw （偏航角）
2. 绕旋转后的 $y$ 轴旋转角度 pitch （俯仰角）
3. 绕旋转后的 $x$ 轴旋转 roll （滚转角）

此时可以使用一个向量 $[r, p, y]^T$ 来描述任意旋转。

存在的问题是具有奇异性 ^[2] （俯仰角为 $\frac{\pi}{2}$ 时，第一次旋转和第三次旋转的旋转轴是同一个轴，从而丢失了一个自由度）。

四元数 #

定义 #

旋转矩阵用 9 个量描述一个三自由度的旋转，这种表达方式是冗余的；旋转向量和欧拉角是紧凑的，但是具有奇异性（找不到不带奇异性的用三维向量描述三维旋转的方式^[3]）。回忆复数集 $\mathbb{C}$ 中的元素曾被用于表示复平面上的向量，复数的乘法则可以表示复平面上的旋转。三维空间中的旋转有一种类似的表示方式：四元数。四元数的运算此处暂时不详细列举。

一个虚四元数 $\textbf{p} = [0, \textbf{v}_0]$ 可以表示一个三维空间中的点，其虚部 $\textbf{v}_0$ 即为点的三维坐标。

一个单位四元数 $\textbf{q} = [s, \textbf{v}]$ 可以表示三维空间中的旋转，计算过程为

$$\textbf{p}' = \textbf{qpq}^{-1}.$$

此处使用四元数的乘法（而不是四元数的数乘或点乘）。四元数的乘法具有如下形式

$$\textbf{q}_a\textbf{q}_b = [ s_as_b + \textbf{v}_a\cdot\textbf{v}_b, s_a\textbf{v}_b + s_b\textbf{v}_a + \textbf{v}_a \times \textbf{v}_b ].$$

可以验证 $\textbf{p}'$ 的实部为 $0$，是三维空间中的一个点。

注意四元数 $\textbf{q}$ 和 $-\textbf{q}$ 表示的是同一个旋转。$\textbf{q}$ 表示的旋转操作的逆操作用 $\textbf{q}^{-1}$ 表示，其中

$$\textbf{q}^{-1} = \frac{\textbf{q}^*}{||\textbf{q}||} = \textbf{q}^*.$$

由于表示旋转操作的四元数是一个单位四元数（$||\textbf{q}||=1$），故其逆等于共轭，即其对应的逆操作也可以用它的共轭 $\textbf{q}^*$ 来表示。

转换为旋转矩阵 #

先转换为旋转向量，然后 [从旋转向量转换到旋转矩阵](#转换为旋转矩阵)。

转换为旋转向量 #

参考旋转向量转四元数的操作，记旋转轴方向为 $\textbf{n}$，旋转 角度为 $\theta$，单位四元数 $\textbf{q} = [s, \textbf{v}]$ 对应的 $\textbf{n}$ 和 $\theta$ 为

$$\theta = 2\arccos{s}, \\ \textbf{n} = \frac{1}{\sin{\frac{\theta}{2}}}\textbf{v}.$$