Pitch = ±90° 时的四元数计算与校核

1. 欧拉角到旋转矩阵 (Z-Y-X)

即使在 $\text{Pitch} = \pm 90^\circ$ 时存在无数多组欧拉角解，这无数多组解对应的旋转矩阵 $R$ 却是唯一的。公式如下（$c$ 代表 $\cos$，$s$ 代表 $\sin$）：

$$ R = R_z(\psi) R_y(\theta) R_x(\phi) = \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} $$

2. 矩阵提取四元数 (鲁棒算法)

为了避免在 $\theta = \pm 90^\circ$ 时欧拉角造成的奇异性，机器人学中通常直接从矩阵 $R$ 提取四元数 $q = [q_0, q_1, q_2, q_3]^T$（标量在前）。通过计算矩阵的迹 (Trace, $tr = R_{11} + R_{22} + R_{33}$)，算法分为四种情况以防除以 0：

情况 A (常用)： 若 $tr > 0$：

$$ S = 2\sqrt{tr + 1} $$ $$ q_0 = \frac{S}{4}, \quad q_1 = \frac{R_{32} - R_{23}}{S}, \quad q_2 = \frac{R_{13} - R_{31}}{S}, \quad q_3 = \frac{R_{21} - R_{12}}{S} $$

情况 B： 若 $tr \le 0$ 且 $R_{11}$ 是对角线上的最大值：

$$ S = 2\sqrt{1 + R_{11} - R_{22} - R_{33}} $$ $$ q_0 = \frac{R_{32} - R_{23}}{S}, \quad q_1 = \frac{S}{4}, \quad q_2 = \frac{R_{12} + R_{21}}{S}, \quad q_3 = \frac{R_{13} + R_{31}}{S} $$

（情况 C 和 D 分别对应 $R_{22}$ 和 $R_{33}$ 为最大值时的轮换公式，可见讲义。）

3. 四元数转回旋转矩阵 (用于校核)

将求得的单位四元数 $q$ 转回等效旋转矩阵 $R(q)$，用于验证推导的准确性，公式与讲义完全对应：

$$ R' = \begin{bmatrix} 1 - 2(q_2^2 + q_3^2) & 2(q_1q_2 - q_0q_3) & 2(q_1q_3 + q_0q_2) \\ 2(q_1q_2 + q_0q_3) & 1 - 2(q_1^2 + q_3^2) & 2(q_2q_3 - q_0q_1) \\ 2(q_1q_3 - q_0q_2) & 2(q_2q_3 + q_0q_1) & 1 - 2(q_1^2 + q_2^2) \end{bmatrix} $$