欧拉角与四元数转换实例

假设我们有一个欧拉角（Pitch, Yaw, Roll）为（30, 45, 60）度，需要将其转换为四元数。

首先，我们需要将欧拉角转换为旋转矩阵。这可以通过以下公式完成：

$$
R = R_{roll} R_{yaw} R_{pitch}
$$

其中，$R_{roll}$、$R_{yaw}$ 和 $R_{pitch}$ 分别是以下旋转矩阵：

$$
R_{roll} =
\begin{bmatrix}
1 & 0 & 0 \\
0 & \cos\theta & -\sin\theta \\
0 & \sin\theta & \cos\theta
\end{bmatrix},
R_{yaw} =
\begin{bmatrix}
\cos\psi & 0 & \sin\psi \\
0 & 1 & 0 \\
-\sin\psi & 0 & \cos\psi
\end{bmatrix},
R_{pitch} =
\begin{bmatrix}
\cos\phi & -\sin\phi & 0 \\
\sin\phi & \cos\phi & 0 \\
0 & 0 & 1
\end{bmatrix}
$$

将角度值转换为弧度值后，我们有：

$$
R_{roll} =
\begin{bmatrix}
1 & 0 & 0 \\
0 & 0.866 & -0.5 \\
0 & 0.5 & 0.866
\end{bmatrix},
R_{yaw} =
\begin{bmatrix}
0.707 & 0 & 0.707 \\
0 & 1 & 0 \\
-0.707 & 0 & 0.707
\end{bmatrix},
R_{pitch} =
\begin{bmatrix}
0.866 & -0.5 & 0 \\
0.5 & 0.866 & 0 \\
0 & 0 & 1
\end{bmatrix}
$$

将三个矩阵相乘，我们得到旋转矩阵 $R$：

$$
R = R_{roll} R_{yaw} R_{pitch} =
\begin{bmatrix}
0.612 & -0.354 & 0.707 \\
0.866 & 0.354 & -0.354 \\
-0.707 & 0.866 & 0.354
\end{bmatrix}
$$

接下来，我们可以将 $R$ 转换为四元数 $q$，其中 $q = (w, x, y, z)$：

$$
q = \begin{bmatrix}
w \\ x \\ y \\ z
\end{bmatrix} =
\begin{bmatrix}
\frac{\sqrt{2}+\sqrt{6}}{4} \\
\frac{\sqrt{6}-\sqrt{2}}{4} \\
\frac{\sqrt{3}}{4} \\
\frac{\sqrt{3}}{4}
\end{bmatrix}
$$

这样，我们就完成了从欧拉角到四元数的转换。如果要将四元数转换回欧拉角，则可以使用以下公式：

$$
\begin{aligned}
\phi &= \mathrm{atan2}(2(xy + wz), 1 - 2(x^2 + y^2)) \\
\theta &= \mathrm{asin}(2(xy - wz)) \\
\psi &= \mathrm{atan2}(2(xz + wy), 1 - 2(x^2 + z^2))
\end{aligned}
$$

其中，$x, y, z, w$ 分别是四元数的四个分量。注意，这里的 $\mathrm{atan2}$ 函数可以返回任意角度的反正切值。