Převod quaternionu na Eulerovy úhly

V tomto příspěvku odvodím vztahy pro převod quaternionu na Elerovy úhly používané v letectví, tedy úhly, které postupně reprezentují rotaci okolo osy $z$ , $y$ a $x$. Tyto úhly značím postupně jako $\alpha$, $\beta$ a $\gamma$. Prvek matice $\mathbf{R}$ na řádku $i$ a ve sloupci $j$ značím jako $\mathbf{R}_{ij}$.

Nejprve převedeme quaternion na rotační matici a z té posléze získáme Eulerovy úhly. Rotační matici z quaternionu $\mathbf{q}=(q_w, q_x, q_y, q_z)$ získáme pomocí následujícího vztahu [1]:
$$
\mathbf{R}_q =

\begin{bmatrix}
    1 - 2 q_y^2 - 2 q_z^2 & 2 q_x q_y - 2 q_z q_w & 2 q_x q_z + 2 q_y q_w \\
    2 q_x q_y + 2 q_z q_w & 1 - 2 q_x^2 - 2 q_z^2 & 2 q_y q_z - 2 q_x q_w \\
    2 q_x q_z - 2 q_y q_w & 2 q_y q_z + 2 q_x q_w & 1 - 2 q_x^2 - 2 q_y^2
\end{bmatrix}
$$ Rotační matice pro Eulerovy úhly definované v úvodním odstavci při značení $\sin\!\left(\alpha\right)=s_\alpha$ (pro kosinus a další úhly analogicky) má následující podobu
$$ \mathbf{R}_E =
\begin{bmatrix} c_\alpha\, c_\beta & c_\alpha\, s_\beta\, s_\gamma - c_\gamma\, s_\alpha & s_\alpha\, s_\gamma + c_\alpha\, c_\gamma\, s_\beta\\ c_\beta\, s_\alpha & c_\alpha\, c_\gamma + s_\alpha\, s_\beta\, s_\gamma & c_\gamma\, s_\alpha\, s_\beta - c_\alpha\, s_\gamma\\ - s_\beta & c_\beta\, s_\gamma & c_\beta\, c_\gamma \end{bmatrix}
$$ Z matice $\mathbf{R}_E$ určíme nejprve úhel $\beta$ jako
$$ \beta = \mathrm{asin}\,\left(-\mathbf{R}_{31} \right).
$$ Dále se postup v závislosti na hodnotě prvku $\mathbf{R}_{31}$ dělí na tři případy.

1. Pokud platí, že $\mathbf{R}_{31} = 1$, pak má rotační matice nutně následující podobu

$$
\mathbf{R}_E' =
\begin{bmatrix} 0 & -c_\alpha\, s_\gamma - c_\gamma\, s_\alpha & s_\alpha\, s_\gamma - c_\alpha\, c_\gamma\\
0 & c_\alpha\, c_\gamma - s_\alpha\,  s_\gamma & -c_\gamma\, s_\alpha\, - c_\alpha\, s_\gamma\\
1 & 0 & 0 \end{bmatrix}
=
\begin{bmatrix} 0 & -s_{\gamma + \alpha} & c_{\gamma + \alpha}\\
0 & c_{\gamma + \alpha} & -s_{\gamma + \alpha}\\
1 & 0 & 0 \end{bmatrix}.
$$ Řešení  pro úhly $\alpha$ a $\gamma$ je nekonečně mnoho. Je možné získat pouze součet těchto úhlů. Z nekonečně mnoha řešení tedy vybereme jedno. \begin{align*}
\alpha &= 0\\
\gamma &= \mathrm{atan2}\!\left( \mathrm{-\mathbf{R}_{12}}\mathrm{\mathbf{R}_{22}} \right)
\end{align*}

2. Pokud platí, že $\mathbf{R}_{31} = -1$, pak má rotační matice nutně následující podobu

$$
\mathbf{R}_E' =
\begin{bmatrix} 0 & c_\alpha\, s_\gamma - c_\gamma\, s_\alpha & s_\alpha\, s_\gamma + c_\alpha\, c_\gamma\\
0 & c_\alpha\, c_\gamma + s_\alpha\,  s_\gamma & c_\gamma\, s_\alpha\, - c_\alpha\, s_\gamma\\
-1 & 0 & 0 \end{bmatrix}
=
\begin{bmatrix} 0 & s_{\gamma - \alpha} & c_{\gamma - \alpha}\\
0 & c_{\gamma - \alpha} & -s_{\gamma - \alpha}\\
-1 & 0 & 0 \end{bmatrix}.
$$ Obdobně jako v předchozím případě je řešení  pro úhly $\alpha$ a $\gamma$ nekonečně mnoho. Opět vybereme jedno řešení, které má například následující podobu \begin{align*}
\alpha &= 0,\\
\gamma &= \mathrm{atan2}\!\left( \mathrm{\mathbf{R}_{12}}, \mathrm{\mathbf{R}_{22}} \right).
\end{align*}

3. Pro jiné hodnoty $\mathbf{R}_{31}$ určíme zbývající úhly podle následujících vztahů

\begin{align*} \alpha &= \mathrm{atan2}\!\left(\mathrm{\mathbf{R}_{21}}, \mathrm{\mathbf{R}_{11}} \right),\\
\gamma &= \mathrm{atan2}\!\left( \mathrm{\mathbf{R}_{32}}, \mathrm{\mathbf{R}_{33}} \right).
\end{align*} Rotační matici s konkrétními hodnotami jsme získali převodem z quaternionu a známe vztahy pro výpočet jednotlivých Eulerových úhlů z této matice, které jsme odvodili z obecného tvaru rotační matice pro tyto úhly. Již tedy zbývá jen do těchto vztahů dosadit. Pro jednoduchost zde uvádím jen dosazené vztahy pro poslední případ.\begin{align*} \alpha &= \mathrm{atan2}\!\left( 2 q_x q_y + 2 q_z q_w, 1 - 2 q_x^2 - 2 q_y^2 \right),\\
\beta &= \mathrm{asin}\,\left(2 q_y q_w - 2 q_x q_z\right),\\
\gamma &= \mathrm{atan2}\!\left( 2 q_y q_z + 2 q_x q_w, 1 - 2 q_y^2 - 2 q_z^2 \right).
\end{align*} Převod quaternionů na Eulerovy úhly implementovaný v programovacím jazyce C# vypadá následovně:

        public void ToEulerAngles(out double yaw, out double pitch, out double roll)
        {
            double test = q[1] * q[3] - q[0] * q[2];

            if (test > 0.49999)
            {
                roll = Math.Atan2(2 * q[1] * q[0] - 2 * q[2] * q[3], 2 * q[1] * q[3] + 2 * q[2] * q[0]);
                pitch = -Math.PI / 2;
                yaw = 0;
                return;
            }

            if (test < -0.49999)
            {
                roll = Math.Atan2(2 * q[2] * q[3] - 2 * q[1] * q[0], 2 * q[1] * q[3] + 2 * q[2] * q[0]);
                pitch = Math.PI / 2;
                yaw = 0;
                return;
            }

            double sqx = q[1] * q[1];
            double sqy = q[2] * q[2];
            double sqz = q[3] * q[3];

            roll = Math.Atan2(2 * q[2] * q[3] + 2 * q[0] * q[1], 1 - 2 * sqx - 2 * sqy);
            pitch = Math.Asin(-2 * test);
            yaw = Math.Atan2(2 * q[1] * q[2] + 2 * q[3] * q[0], 1 - 2 * sqy - 2 * sqz);
        }

Zdroje

[1] Maths - Conversion Quaternion to Matrix