欧拉角旋转：正向计算与逆向反解 (Z-Y-X 顺序)

已知 Z-Y-X 顺序（绕动轴旋转）的完整旋转矩阵乘法展开如下：

$$ \begin{aligned} R_{zyx}(\psi, \theta, \phi) &= R_z(\psi) \, R_y(\theta) \, R_x(\phi) \\ \\ &= \begin{bmatrix} \cos\psi & -\sin\psi & 0 \\ \sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} \cos\theta & 0 & \sin\theta \\ 0 & 1 & 0 \\ -\sin\theta & 0 & \cos\theta \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\phi & -\sin\phi \\ 0 & \sin\phi & \cos\phi \end{bmatrix} \\ \\ &= \begin{bmatrix} c_\psi c_\theta & c_\psi s_\theta s_\phi - s_\psi c_\phi & c_\psi s_\theta c_\phi + s_\psi s_\phi \\ s_\psi c_\theta & s_\psi s_\theta s_\phi + c_\psi c_\phi & s_\psi s_\theta c_\phi - c_\psi s_\phi \\ -s_\theta & c_\theta s_\phi & c_\theta c_\phi \end{bmatrix} \end{aligned} $$

在实际应用中，我们经常需要从已知的旋转矩阵 $R$ 中提取出欧拉角 $(\psi, \theta, \phi)$。设已知矩阵 $R$ 的各个元素为：

$$ R = \begin{bmatrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{bmatrix} $$

通过对比上面的 $R_{zyx}$ 展开式，反解过程需要根据 $\cos\theta$ 是否为 $0$ 来进行分情况讨论：

3.1 常规情况：$\cos\theta \neq 0$

通常我们限制俯仰角 $\theta \in (-\frac{\pi}{2}, \frac{\pi}{2})$，此时 $\cos\theta > 0$。

• 求 Pitch ($\theta$)：由 $r_{31} = -s_\theta$，可得 $\theta = \text{Atan2}(-r_{31}, \sqrt{r_{11}^2 + r_{21}^2})$。
• 求 Yaw ($\psi$)：由 $r_{11} = c_\psi c_\theta$ 和 $r_{21} = s_\psi c_\theta$，可得 $\psi = \text{Atan2}(r_{21}, r_{11})$。
• 求 Roll ($\phi$)：由 $r_{32} = c_\theta s_\phi$ 和 $r_{33} = c_\theta c_\phi$，可得 $\phi = \text{Atan2}(r_{32}, r_{33})$。

注：使用双变量反正切函数 $\text{Atan2}(y, x)$ 可以正确处理象限问题并避免除以零的错误。

3.2 奇异情况 (万向节死锁)：$\cos\theta = 0$

当俯仰角 $\theta = \pm \frac{\pi}{2}$ 时，此时 $\cos\theta = 0$，系统丢失一个自由度（Gimbal Lock）。

情况 A：当 $\theta = \frac{\pi}{2}$ 时，$\sin\theta = 1, \cos\theta = 0$。代入后，旋转矩阵退化为：

$$ R = \begin{bmatrix} 0 & c_\psi s_\phi - s_\psi c_\phi & c_\psi c_\phi + s_\psi s_\phi \\ 0 & s_\psi s_\phi + c_\psi c_\phi & s_\psi c_\phi - c_\psi s_\phi \\ -1 & 0 & 0 \end{bmatrix} = \begin{bmatrix} 0 & \sin(\phi-\psi) & \cos(\phi-\psi) \\ 0 & \cos(\phi-\psi) & -\sin(\phi-\psi) \\ -1 & 0 & 0 \end{bmatrix} $$

此时，从矩阵元素中我们只能解出 $\phi - \psi$ 的值，而无法唯一确定 $\phi$ 和 $\psi$ 各自的值。因为有无数多组解，通常在编程时我们会强制令 $\phi = 0$，然后由 $\sin(-\psi) = r_{12}, \cos(-\psi) = r_{22}$ 求解得到 $\psi = \text{Atan2}(-r_{12}, r_{22})$。

情况 B：当 $\theta = -\frac{\pi}{2}$ 时，$\sin\theta = -1, \cos\theta = 0$。代入后，旋转矩阵退化为：

$$ R = \begin{bmatrix} 0 & -c_\psi s_\phi - s_\psi c_\phi & -c_\psi c_\phi + s_\psi s_\phi \\ 0 & -s_\psi s_\phi + c_\psi c_\phi & -s_\psi c_\phi - c_\psi s_\phi \\ 1 & 0 & 0 \end{bmatrix} = \begin{bmatrix} 0 & -\sin(\phi + \psi) & -\cos(\phi + \psi) \\ 0 & \cos(\phi + \psi) & -\sin(\phi + \psi) \\ 1 & 0 & 0 \end{bmatrix} $$

此时，从矩阵元素中我们只能解出 $\phi + \psi$ 的值，同样无法唯一确定各自的值。若强制令 $\phi = 0$，则可以通过 $\sin(\psi) = -r_{12}, \cos(\psi) = r_{22}$ 求解得到 $\psi = \text{Atan2}(-r_{12}, r_{22})$。