旋转矩阵与欧拉角

旋转矩阵与欧拉角

参考文献：
[Computing Euler angles from a rotation matrix ——Gregory G. Slabaugh]

三个主轴的旋转矩阵

右手坐标系，逆时针转动角度为正（右手螺旋定则确定）。

关于绕 $x$ 轴旋转 $\psi$ 弧度 的主动旋转定义为

$$R_x(\psi )=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \psi & -\sin \psi \\ 0& \sin \psi & \cos \psi \end{array}\right]$$

关于绕 $y$ 轴旋转 $\theta$ 弧度 的主动旋转定义为

$$R_y(\theta )=\left[\begin{array}{ccc}\cos \theta & 0& \sin \theta \\ 0& 1& 0\\ -\sin \theta & 0& \cos \theta \end{array}\right]$$

关于绕 $z$ 轴旋转 $\psi$ 弧度 的主动旋转定义为

$$R_z(\varphi )=\left[\begin{array}{ccc}\cos \varphi & -\sin \varphi & 0\\ \sin \varphi & \cos \varphi & 0\\ 0& 0& 1\end{array}\right]$$

广义旋转矩阵

一般旋转矩阵具有如下形式，

$$R=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$$

旋转矩阵与转角

旋转矩阵可以认为是每个主轴都有一次旋转的共有三个旋转的序列，这也能与空间中的旋转是三个自由度对应。

将旋转矩阵拆解成 3 次旋转角度来做 3 次旋转连乘，有以下注意事项：

• 由于矩阵乘法是不可交换的，因此轴旋转先后的顺序将影响结果
• 旋转转轴也需要明确定义，是对“固定不动”的转轴旋转？还是对“转动的坐标系当下所在”的转轴旋转？

两种旋转转轴：

• 对方向“固定不动”的转轴旋转：Fixed angles
• 对“转动的坐标系当下所在”的转轴反向旋转：Euler angles

Fixed angles

Fixed angles 是以某个固定转轴来旋转坐标系的。

以绕固定轴X、Y、Z的顺序旋转为例，将其表示为旋转矩阵，

$$\begin{array}{rl}R& =R_z(\varphi )R_y(\theta )R_x(\psi )\\ & =\left[\begin{array}{ccc}\cos \theta \cos \varphi & \sin \psi \sin \theta \cos \varphi -\cos \psi \sin \varphi & \cos \psi \sin \theta \cos \varphi +\sin \psi \sin \varphi \\ \cos \theta \sin \varphi & \sin \psi \sin \theta \sin \varphi +\cos \psi \cos \varphi & \cos \psi \sin \theta \sin \varphi -\sin \psi \cos \varphi \\ -\sin \theta & \sin \psi \cos \theta & \cos \psi \cos \theta \end{array}\right]\end{array}$$

Euler angles

Fixed angles 是以自身坐标轴为转轴来旋转坐标系的。

求解角度

给定旋转矩阵 $R$，我们可以通过将 $R$ 中的每个元素与矩阵乘积 $R_z(\varphi )R_y(\theta )R_x(\psi )$ 中的相应元素相等来计算欧拉角 $\psi$、$\theta$ 和 $\varphi$，这产生了九个可用于求欧拉角的方程。

求解 $\theta$ 可能的两个角度

通过旋转矩阵的元素 $r_{31}$ ，可以得到：

$$r_{31}=-\sin \theta$$

再通过反三角函数就可以得出：

$$\begin{array}{}\text{(1)}& \theta =-\arcsin r_{31}\end{array}$$

若 $r_{31}\ne \pm 1$，由于 $\sin (\pi -\theta )=\sin \theta$，实际上有两个不同的 $\theta$ 值满足方程 $(1)$；因此，这两个值都是能作为有效解的：

$$\begin{array}{rl}{\theta }_1& =-\arcsin r_{31}\\ {\theta }_2& =\pi -{\theta }_1=\pi +\arcsin r_{31}\end{array}$$

若 $r_{31}=\pm 1$，将有特殊处理的地方，这个在稍后处理。

因此，使用旋转矩阵的元素 $r_{31}$，我们能够确定 $\theta$ 的两个可能值。

求解 $\psi$ 的角度

为了找到 $\psi$ 的值，观察旋转矩阵可以得到：

$$\begin{array}{}\text{(2)}& \frac{r_{32}}{r_{33}}=\frac{\sin \psi \cos \theta }{\cos \psi \cos \theta }=\tan \psi \end{array}$$

使用这个方程可以求解出 $\psi$：

$$\psi =atan2(r_{32},r_{33})$$

其中 $atan2(y,x)$ 是两个变量 $x$ 和 $y$ 的反正切。它类似于计算 $y/x$ 的反正切，只是两个参数的正负符号用于确定结果的象限，该象限位于 $[-\pi ，\pi ]$ 范围内。函数 $atan2(y,x)$ 在许多编程语言中都可用。

在解方程 $(2)$ 时必须小心：如果 $\cos \theta >0$，则 $\psi =atan2(r_{32},r_{33})$；然而，当 $\cos \theta <0$ 时，$\psi =atan2(-r_{32},-r_{33})$。处理此问题的简单方法是使用以下式子求解出 $\psi$：

$$\begin{array}{}\text{(3)}& \psi =atan2(\frac{r_{32}}{\cos \theta },\frac{r_{33}}{\cos \theta })\end{array}$$

除 $\cos \theta =0$ 之外，方程 $(3)$ 适用于所有情况，这也在稍后处理这个特殊情况。对于每个 $\theta$ 值，我们使用方程 $(3)$ 计算相应的 $\psi$ 值，得出：

$$\begin{array}{}\text{(4)}& {\psi }_1& =atan2(\frac{r_{32}}{\cos {\theta }_1},\frac{r_{33}}{\cos {\theta }_1})\text{(5)}& {\psi }_2& =atan2(\frac{r_{32}}{\cos {\theta }_2},\frac{r_{33}}{\cos {\theta }_2})\end{array}$$

求解 $\varphi$ 的角度

类似求解 $\psi$ 的分析也适用于求解 $\varphi$，观察旋转矩阵可以得到：

$$\frac{r_{21}}{r_{11}}=\frac{\cos \theta \sin \varphi }{\cos \theta \cos \varphi }=\tan \varphi$$

使用这个方程可以求解出 $\varphi$：

$$\begin{array}{}\text{(6)}& \varphi =atan2(\frac{r_{21}}{\cos \theta },\frac{r_{11}}{\cos \theta })\end{array}$$

同样，除了 $\cos \theta =0$ 之外，方程 $(6)$ 对所有情况都有效。这也在稍后处理这个特殊情况。对于每个 $\theta$ 值，我们使用方程 $(3)$ 计算相应的 $\psi$ 值，得出：

$$\begin{array}{}\text{(7)}& {\varphi }_1=atan2(\frac{r_{21}}{\cos {\theta }_1},\frac{r_{11}}{\cos {\theta }_1})\text{(8)}& {\varphi }_2=atan2(\frac{r_{21}}{\cos {\theta }_2},\frac{r_{11}}{\cos {\theta }_2})\end{array}$$

当 $\cos \theta \ne 0$ 的两组解

对于$\cos \theta \ne 0$的情况，通过上述我们可以得出两组欧拉角的解，它们再现了旋转矩阵，即：

$$\begin{gathered} ({\psi }_1,{\theta }_1,{\varphi }_1) \\ ({\psi }_2,{\theta }_2,{\varphi }_2) \end{gathered}$$

这两个解都是有效的。

处理特殊情况 $\cos \theta =0$

如果旋转矩阵的元素 $r_{31}=\pm 1$，也就是分别对应于 $\theta =\pm \pi /2$，也就是 $\cos \theta =0$，上述方法将不起作用。

当我们尝试使用上述方法求解 $\psi$ 和 $\varphi$ 的可能值时，会出现问题，因为旋转矩阵的元素 $r_{11}$、$r_{21}$、$r_{32}$ 和 $r_{33}$ 的值都将等于 $0$，也就是方程 $(3)$ 和 $(6)$ 将变为：

$$\begin{array}{rl}\psi & =atan2(\frac{0}{0},\frac{0}{0})\\ \varphi & =atan2(\frac{0}{0},\frac{0}{0})\end{array}$$

在这种情况下，$r_{11}$、$r_{21}$、$r_{32}$ 和 $r_{33}$ 不限制 $\psi$ 和 $\varphi$ 的值。因此，我们必须使用其它旋转矩阵的元素来计算 $\psi$ 和 $\varphi$ 的值。

$\theta =\pi /2$ 的情况：考虑 $\theta =\pi /2$，那么，

$$\begin{array}{rl}{r}_{12}& =\sin \psi \cos \varphi -\cos \psi \sin \varphi =\sin (\psi -\varphi )\\ r_{13}& =\cos \psi \cos \varphi +\sin \psi \sin \varphi =\cos (\psi -\varphi )\\ r_{22}& =\sin \psi \sin \varphi +\cos \psi \cos \varphi =\cos (\psi -\varphi )=r_{13}\\ r_{23}& =\cos \psi \sin \varphi -\sin \psi \cos \varphi =-\sin (\psi -\varphi )=-r_{12}\end{array}$$

使用 $r_{12}$ 和 $r_{13}$ 的方程，我们发现:

$$\begin{array}{rl}(\psi -\varphi )& =atan2(r_{12},r_{13})\\ \psi & =\varphi +atan2(r_{12},r_{13})\end{array}$$

$\theta =-\pi /2$ 的情况：毫不奇怪，当 $\theta =-\pi /2$，类似的结果也成立，

$$\begin{array}{rl}{r}_{12}& =-\sin \psi \cos \varphi -\cos \psi \sin \varphi =-\sin (\psi +\varphi )\\ r_{13}& =-\cos \psi \cos \varphi +\sin \psi \sin \varphi =-\cos (\psi +\varphi )\\ r_{22}& =-\sin \psi \sin \varphi +\cos \psi \cos \varphi =\cos (\psi +\varphi )=-r_{13}\\ r_{23}& =-\cos \psi \sin \varphi -\sin \psi \cos \varphi =-\sin (\psi +\varphi )=r_{12}\end{array}$$

同样，使用 $r_{12}$ 和 $r_{13}$ 的方程，我们发现:

$$\begin{array}{rl}(\psi +\varphi )& =atan2(-r_{12},-r_{13})\\ \psi & =-\varphi +atan2(-r_{12},-r_{13})\end{array}$$

至此，就解决了从旋转矩阵求解所有情况欧拉角。

在 $\theta =\pm \pi /2$ 的情况下，可以发现 $\psi$ 和 $\varphi$ 是相连的，也就是 $\psi$ 和 $\varphi$ 本应该有 2 个自由度，却降为了 1 个自由度，这种现象被称为万向锁。

尽管在这种情况下，$\psi$ 和 $\varphi$ 有无限多组的解，但在实践中，人们往往对找到一个解的方案感兴趣。对此，可以设置 $\varphi =0$ 并方便地如上所述计算 $\psi$。

这里只是对于矩阵乘积 $R_z(\varphi )R_y(\theta )R_x(\psi )$ 做出的例子，其它旋转顺序和旋转方式会有与其对应的矩阵乘积的表达式，要按照实际情况，类似地依上所述去求解欧拉角。