剛體姿態運動學08 尤拉角、四元數與旋轉矩陣之間的轉換(Conversion Between Quaternions And Euler Angles)

四元數轉換尤拉角（Convert Quaternion to Euler Angles）

本節尤拉角以 $X_0 - Y_0 - Z_0$ 順序為例，推導出尤拉角、旋轉矩陣與四元數的轉換關係。
首先是尤拉角與旋轉矩陣的轉換，將尤拉角各角度的旋轉矩陣
$\mathbf{R}(\varphi_z)$、$\mathbf{R}(\varphi_y)$ 和 $\mathbf{R}(\varphi_x)$ 相乘，
$\mathbf{R}_E$ 表示由尤拉角計算的旋轉矩陣，如下：

\[\begin{aligned} \mathbf{R}_E &= \mathbf{R}(\varphi_z)\mathbf{R}(\varphi_y)\mathbf{R}(\varphi_x) \\ &= \begin{bmatrix} \cos \varphi_z & -\sin \varphi_z & 0 \\ \sin \varphi_z & \cos \varphi_z & 0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} \cos \varphi_y & 0 & \sin \varphi_y \\ 0 & 1 & 0 \\ -\sin \varphi_y & 0 & \cos \varphi_y \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos \varphi_x & -\sin \varphi_x \\ 0 & \sin \varphi_x & \cos \varphi_x \end{bmatrix} \end{aligned}\] \[\mathbf{R}_E = \begin{bmatrix} \cos \varphi_z \cos \varphi_y & -\sin \varphi_z \cos \varphi_x + \cos \varphi_z \sin \varphi_y \sin \varphi_x & \sin \varphi_z \sin \varphi_x + \cos \varphi_z \cos \varphi_x \sin \varphi_y \\ \sin \varphi_z \cos \varphi_y & \cos \varphi_z \cos \varphi_x + \sin \varphi_x \sin \varphi_y \sin \varphi_z & -\cos \varphi_z \sin \varphi_x + \sin \varphi_y \sin \varphi_z \cos \varphi_x \\ -\sin \varphi_y & \cos \varphi_y \sin \varphi_x & \cos \varphi_y \cos \varphi_x \end{bmatrix}\]

令

\[\mathbf{R}_E = \begin{bmatrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{bmatrix}\]

即可由矩陣元素反推出尤拉角：

\[\begin{bmatrix} \tan^{-1}\!\left(\dfrac{r_{32}}{r_{33}}\right) \\ \sin^{-1}(-r_{31}) \\ \tan^{-1}\!\left(\dfrac{r_{21}}{r_{11}}\right) \end{bmatrix} = \begin{bmatrix} \varphi_x \\ \varphi_y \\ \varphi_z \end{bmatrix}\]

---

四元數轉尤拉角

根據先前的尤拉參數推導，姿態四元數的旋轉矩陣為：

\[\mathbf{R}_E(\mathbf{q}) = \begin{bmatrix} q_0^2 + q_1^2 - q_2^2 - q_3^2 & 2(q_1 q_2 - q_3 q_0) & 2(q_1 q_3 + q_0 q_2) \\ 2(q_2 q_1 + q_3 q_0) & q_0^2 - q_1^2 + q_2^2 - q_3^2 & 2(q_2 q_3 - q_0 q_1) \\ 2(q_3 q_1 - q_2 q_0) & 2(q_3 q_2 + q_1 q_0) & q_0^2 - q_1^2 - q_2^2 + q_3^2 \end{bmatrix}\]

因此四元數轉尤拉角為：

\[\begin{bmatrix} \varphi_x \\ \varphi_y \\ \varphi_z \end{bmatrix} = \begin{bmatrix} \tan^{-1}\!\left( \dfrac{2(q_3 q_2 + q_1 q_0)} {q_0^2 - q_1^2 - q_2^2 + q_3^2} \right) \\ \sin^{-1}\!\left(-2(q_3 q_1 - q_2 q_0)\right) \\ \tan^{-1}\!\left( \dfrac{2(q_2 q_1 + q_3 q_0)} {q_0^2 + q_1^2 - q_2^2 - q_3^2} \right) \end{bmatrix}\]

---

尤拉角轉換四元數（Convert Euler Angles to Quaternion）

姿態四元數可寫為：

\[\mathbf{q} = \cos \theta + \mathbf{u} \sin \theta\]

對應三軸旋轉四元數為：

\[\begin{aligned} \mathbf{q}_x &= \begin{bmatrix} \cos \dfrac{\varphi_x}{2} & \sin \dfrac{\varphi_x}{2} & 0 & 0 \end{bmatrix}^T \\ \mathbf{q}_y &= \begin{bmatrix} \cos \dfrac{\varphi_y}{2} & 0 & \sin \dfrac{\varphi_y}{2} & 0 \end{bmatrix}^T \\ \mathbf{q}_z &= \begin{bmatrix} \cos \dfrac{\varphi_z}{2} & 0 & 0 & \sin \dfrac{\varphi_z}{2} \end{bmatrix}^T \end{aligned}\]

依 $X_0 - Y_0 - Z_0$ 順序相乘：

\[\mathbf{q}_E = \mathbf{q}_z \mathbf{q}_y \mathbf{q}_x\]

整理後得到：

\[\begin{bmatrix} q_{E0} \\ q_{E1} \\ q_{E2} \\ q_{E3} \end{bmatrix} = \begin{bmatrix} \cos \frac{\varphi_z}{2}\cos \frac{\varphi_y}{2}\cos \frac{\varphi_x}{2} + \sin \frac{\varphi_z}{2}\sin \frac{\varphi_y}{2}\sin \frac{\varphi_x}{2} \\ \cos \frac{\varphi_z}{2}\cos \frac{\varphi_y}{2}\sin \frac{\varphi_x}{2} - \sin \frac{\varphi_z}{2}\sin \frac{\varphi_y}{2}\cos \frac{\varphi_x}{2} \\ \sin \frac{\varphi_z}{2}\cos \frac{\varphi_y}{2}\sin \frac{\varphi_x}{2} + \cos \frac{\varphi_z}{2}\sin \frac{\varphi_y}{2}\cos \frac{\varphi_x}{2} \\ \sin \frac{\varphi_z}{2}\cos \frac{\varphi_y}{2}\cos \frac{\varphi_x}{2} - \cos \frac{\varphi_z}{2}\sin \frac{\varphi_y}{2}\sin \frac{\varphi_x}{2} \end{bmatrix}\]

---

參考資料

• 林昱廷，
  飛彈姿態運動之適應控制研究，
  碩士論文，機械工程學系，國立臺灣科技大學，2021。