四元数欧拉角相互转换与奇异值问题

四元数转欧拉角

参考文章：四元数和欧拉角的相互转换

四元数转欧拉角的奇异值问题

在四元数转欧拉角时遇到奇异值问题，可参考四元数和欧拉角的相互转换,在文章里面说明了。

那么将Z-Y-X欧拉角（或RPY角：绕固定坐标系的X-Y-Z依次旋转α,β,γ角）转换为四元数：

$$q=\left[\begin{array}{c}cos\frac{\gamma }{2}\\ 0\\ 0\\ sin\frac{\gamma }{2}\end{array}\right]\left[\begin{array}{c}cos\frac{\beta }{2}\\ 0\\ sin\frac{\beta }{2}\\ 0\end{array}\right]\left[\begin{array}{c}cos\frac{\alpha }{2}\\ sin\frac{\alpha }{2}\\ 0\\ 0\end{array}\right]=\left[\begin{array}{c}cos\frac{\alpha }{2}cos\frac{\beta }{2}cos\frac{\gamma }{2}+sin\frac{\alpha }{2}sin\frac{\beta }{2}sin\frac{\gamma }{2}\\ sin\frac{\alpha }{2}cos\frac{\beta }{2}cos\frac{\gamma }{2}+cos\frac{\alpha }{2}sin\frac{\beta }{2}sin\frac{\gamma }{2}\\ cos\frac{\alpha }{2}sin\frac{\beta }{2}cos\frac{\gamma }{2}+sin\frac{\alpha }{2}cos\frac{\beta }{2}sin\frac{\gamma }{2}\\ cos\frac{\alpha }{2}cos\frac{\beta }{2}sin\frac{\gamma }{2}+sin\frac{\alpha }{2}sin\frac{\beta }{2}cos\frac{\gamma }{2}\end{array}\right]$$

根据上面的公式可以求出逆解，即由四元数$q=(q_0,q_1,q_2,q_3)$或$q=(w,x,y,z)$。到欧拉角的转换为：
$$\left[\begin{array}{c}\alpha \\ \beta \\ \gamma \end{array}\right]=\left[\begin{array}{c}arctan\frac{2(q_0{q}_1+q_2{q}_3)}{1-2(q_1^2+q_2^2)}\\ arcsin(2(q_0{q}_2-q_1{q}_3))\\ arctan\frac{2(q_0{q}_3+q_1{q}_2)}{1-2(q_2^2+q_3^2)}\end{array}\right]$$

由于arctan和arcsin的取值范围在$\frac{-\pi }{2}$和$\frac{\pi }{2}$之间，只有180°，而绕某个轴旋转时范围是360°，因此要使用atan2函数代替arctan函数：
$$\left[\begin{array}{c}\alpha \\ \beta \\ \gamma \end{array}\right]=\left[\begin{array}{c}atan2(2(q_0{q}_1+q_2{q}_3),1-2(q_1^2+q_2^2))\\ arcsin(2(q_0{q}_2-q_1{q}_3))\\ atan2(2(q_0{q}_3+q_1{q}_2),1-2(q_2^2+q_3^2))\end{array}\right]$$

对于tan(θ) = y / x ：
θ = ATan(y / x)求出的θ取值范围是[-PI/2, PI/2]；
θ = ATan2(y, x)求出的θ取值范围是[-PI, PI]。

• 当 (x, y) 在第一象限, 0 < θ < PI/2
• 当 (x, y) 在第二象限 PI/2 < θ≤PI
• 当 (x, y) 在第三象限, -PI < θ < -PI/2
• 当 (x, y) 在第四象限, -PI/2 < θ < 0