旋转矩阵与欧拉角
参考文献：
[Computing Euler angles from a rotation matrix ——Gregory G. Slabaugh]
三个主轴的旋转矩阵
右手坐标系，逆时针转动角度为正（右手螺旋定则确定）。
关于绕 \(x\) 轴旋转 \(\psi\) 弧度 的主动旋转定义为
\[ R_x(\psi) = \begin{bmatrix}
1 & 0 & 0 \\
0 & \cos\psi & -\sin\psi \\
0 & \sin\psi & \cos\psi
\end{bmatrix}
\]
关于绕 \(y\) 轴旋转 \(\theta\) 弧度 的主动旋转定义为
\[ R_y(\theta) = \begin{bmatrix}
\cos\theta & 0 & \sin\theta \\
0 & 1 & 0 \\
-\sin\theta & 0 & \cos\theta
\end{bmatrix}
\]
关于绕 \(z\) 轴旋转 \(\psi\) 弧度 的主动旋转定义为
\[ R_z(\phi) = \begin{bmatrix}
\cos\phi & -\sin\phi & 0 \\
\sin\phi & \cos\phi & 0 \\
0 & 0 & 1
\end{bmatrix}
\]
广义旋转矩阵
一般旋转矩阵具有如下形式，
\[ R = \begin{bmatrix}
r_{11} & r_{12} & r_{13} \\
r_{21} & r_{22} & r_{23} \\
r_{31} & r_{32} & r_{33}
\end{bmatrix}
\]
旋转矩阵与转角
旋转矩阵可以认为是每个主轴都有一次旋转的共有三个旋转的序列，这也能与空间中的旋转是三个自由度对应。
将旋转矩阵拆解成 3 次旋转角度来做 3 次旋转连乘，有以下注意事项：
- 由于矩阵乘法是不可交换的，因此轴旋转先后的顺序将影响结果
- 旋转转轴也需要明确定义，是对“固定不动”的转轴旋转？还是对“转动的坐标系当下所在”的转轴旋转？
两种旋转转轴：
- 对方向“固定不动”的转轴旋转：Fixed angles
- 对“转动的坐标系当下所在”的转轴反向旋转：Euler angles
Fixed angles
Fixed angles 是以某个固定转轴来旋转坐标系的。
以绕固定轴X、Y、Z的顺序旋转为例，将其表示为旋转矩阵，
\[\begin{aligned}
R & = R_z(\phi)R_y(\theta)R_x(\psi) \\
& = \begin{bmatrix}
\cos\theta\cos\phi & \sin\psi\sin\theta\cos\phi − \cos\psi\sin\phi &\cos\psi\sin\theta\cos\phi + \sin\psi\sin\phi \\
\cos\theta\sin\phi & \sin\psi\sin\theta\sin\phi + \cos\psi\cos\phi & \cos\psi\sin\theta \sin\phi − \sin\psi\cos\phi \\
−\sin\theta & \sin\psi\cos\theta & \cos\psi\cos\theta
\end{bmatrix}
\end{aligned}
\]
Euler angles
Fixed angles 是以自身坐标轴为转轴来旋转坐标系的。
求解角度
给定旋转矩阵 \(R\)，我们可以通过将 \(R\) 中的每个元素与矩阵乘积 \(R_z(\phi)R_y(\theta)R_x(\psi)\) 中的相应元素相等来计算欧拉角 \(\psi\)、\(\theta\) 和 \(\phi\)，这产生了九个可用于求欧拉角的方程。
求解 \(\theta\) 可能的两个角度
通过旋转矩阵的元素 \(r_{31}\) ，可以得到：
\[ r_{31} = −\sin\theta
\]
再通过反三角函数就可以得出：
\[ \theta = −\arcsin{r_{31}} \tag{1}
\]
若 \(r_{31} \not= {\pm1}\)，由于 \(\sin(\pi−\theta) = \sin\theta\)，实际上有两个不同的 \(\theta\) 值满足方程 \((1)\)；因此，这两个值都是能作为有效解的：
\[\begin{aligned}
\theta_1 & = −\arcsin{r_{31}} \\
\theta_2 & = \pi − \theta_1 = \pi + \arcsin{r_{31}}
\end{aligned}
\]
若 \(r_{31} = {\pm1}\)，将有特殊处理的地方，这个在稍后处理。
因此，使用旋转矩阵的元素 \(r_{31}\)，我们能够确定 \(\theta\) 的两个可能值。
求解 \(\psi\) 的角度
为了找到 \(\psi\) 的值，观察旋转矩阵可以得到：
\[ \frac{r_{32}}{r_{33}} = \frac{\sin\psi\cos\theta}{\cos\psi\cos\theta} = \tan\psi \tag{2}
\]
使用这个方程可以求解出 \(\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\)：
\[ \psi = atan2(\frac{r_{32}}{\cos\theta}, \frac{r_{33}}{\cos\theta}) \tag{3}
\]
除 \(\cos\theta = 0\) 之外，方程 \((3)\) 适用于所有情况，这也在稍后处理这个特殊情况。对于每个 \(\theta\) 值，我们使用方程 \((3)\) 计算相应的 \(\psi\) 值，得出：
\[\begin{align}
\psi_1 & = atan2(\frac{r_{32}}{\cos\theta_1}, \frac{r_{33}}{\cos\theta_1}) \tag{4} \\
\psi_2 & = atan2(\frac{r_{32}}{\cos\theta_2}, \frac{r_{33}}{\cos\theta_2}) \tag{5}
\end{align}
\]
求解 \(\phi\) 的角度
类似求解 \(\psi\) 的分析也适用于求解 \(\phi\)，观察旋转矩阵可以得到：
\[ \frac{r_{21}}{r_{11}} = \frac{\cos\theta\sin\phi}{\cos\theta\cos\phi } = \tan\phi
\]
使用这个方程可以求解出 \(\phi\)：
\[ \phi = atan2(\frac{r_{21}}{\cos\theta}, \frac{r_{11}}{\cos\theta}) \tag{6}
\]
同样，除了 \(\cos\theta = 0\) 之外，方程 \((6)\) 对所有情况都有效。这也在稍后处理这个特殊情况。对于每个 \(\theta\) 值，我们使用方程 \((3)\) 计算相应的 \(\psi\) 值，得出：
\[\begin{align}
\phi_1 = atan2(\frac{r_{21}}{\cos\theta_1}, \frac{r_{11}}{\cos\theta_1}) \tag{7} \\
\phi_2 = atan2(\frac{r_{21}}{\cos\theta_2}, \frac{r_{11}}{\cos\theta_2}) \tag{8}
\end{align}
\]
当 \(\cos\theta \not= 0\) 的两组解
对于\(\cos\theta \not= 0\)的情况，通过上述我们可以得出两组欧拉角的解，它们再现了旋转矩阵，即：
\[ (\psi_1, \theta_1, \phi_1) \\
(\psi_2, \theta_2, \phi_2)
\]
这两个解都是有效的。
处理特殊情况 \(\cos\theta = 0\)
如果旋转矩阵的元素 \(r_{31} = {\pm1}\)，也就是分别对应于 \(\theta = \pm \pi/2\)，也就是 \(\cos\theta = 0\)，上述方法将不起作用。
当我们尝试使用上述方法求解 \(\psi\) 和 \(\phi\) 的可能值时，会出现问题，因为旋转矩阵的元素 \(r_{11}\)、\(r_{21}\)、\(r_{32}\) 和 \(r_{33}\) 的值都将等于 \(0\)，也就是方程 \((3)\) 和 \((6)\) 将变为：
\[\begin{aligned}
\psi & = atan2(\frac{0}{0}, \frac{0}{0}) \\
\phi & = atan2(\frac{0}{0}, \frac{0}{0})
\end{aligned}
\]
在这种情况下，\(r_{11}\)、\(r_{21}\)、\(r_{32}\) 和 \(r_{33}\) 不限制 \(\psi\) 和 \(\phi\) 的值。因此，我们必须使用其它旋转矩阵的元素来计算 \(\psi\) 和 \(\phi\) 的值。
\(\theta = \pi/2\) 的情况：考虑 \(\theta = \pi/2\)，那么，
\[\begin{aligned}
r_{12} & = \sin\psi\cos\phi − \cos\psi\sin\phi = \sin(\psi − \phi) \\
r_{13} & = \cos\psi\cos\phi + \sin\psi\sin\phi = \cos(\psi - \phi) \\
r_{22} & = \sin\psi\sin\phi + \cos\psi\cos\phi = \cos(\psi − \phi) = r_{13} \\
r_{23} & = \cos\psi\sin\phi − \sin\psi\cos\phi = −\sin(\psi − \phi) = -r_{12}
\end{aligned}
\]
使用 \(r_{12}\) 和 \(r_{13}\) 的方程，我们发现:
\[\begin{aligned}
(\psi − \phi) & = atan2(r_{12}, r_{13}) \\
\psi & = \phi + atan2(r_{12}, r_{13})
\end{aligned}
\]
\(\theta = -\pi/2\) 的情况：毫不奇怪，当 \(\theta = -\pi/2\)，类似的结果也成立，
\[\begin{aligned}
r_{12} & = -\sin\psi\cos\phi − \cos\psi\sin\phi = -\sin(\psi + \phi) \\
r_{13} & = -\cos\psi\cos\phi + \sin\psi\sin\phi = -\cos(\psi + \phi) \\
r_{22} & = -\sin\psi\sin\phi + \cos\psi\cos\phi = \cos(\psi + \phi) = -r_{13} \\
r_{23} & = -\cos\psi\sin\phi − \sin\psi\cos\phi = −\sin(\psi + \phi) = r_{12}
\end{aligned}
\]
同样，使用 \(r_{12}\) 和 \(r_{13}\) 的方程，我们发现:
\[\begin{aligned}
(\psi + \phi) & = atan2(-r_{12}, -r_{13}) \\
\psi & = -\phi + atan2(-r_{12}, -r_{13})
\end{aligned}
\]
至此，就解决了从旋转矩阵求解所有情况欧拉角。
在 \(\theta = \pm \pi/2\) 的情况下，可以发现 \(\psi\) 和 \(\phi\) 是相连的，也就是 \(\psi\) 和 \(\phi\) 本应该有 2 个自由度，却降为了 1 个自由度，这种现象被称为万向锁。
尽管在这种情况下，\(\psi\) 和 \(\phi\) 有无限多组的解，但在实践中，人们往往对找到一个解的方案感兴趣。对此，可以设置 \(\phi = 0\) 并方便地如上所述计算 \(\psi\)。
这里只是对于矩阵乘积 \(R_z(\phi)R_y(\theta)R_x(\psi)\) 做出的例子，其它旋转顺序和旋转方式会有与其对应的矩阵乘积的表达式，要按照实际情况，类似地依上所述去求解欧拉角。
