Quaternion to Euler angle
Quaternion to Euler angle
Reference article: Mutual conversion between quaternion and Euler angle
The singular value problem of converting quaternion to Euler angle
When encountering singular value problems when converting quaternions to Euler angles, please refer to the mutual conversion between quaternions and Euler angles , which is explained in the article.
Then convert the ZYX Euler angle (or RPY angle: rotate α, β, γ angles in turn around the XYZ of the fixed coordinate system) into a quaternion:
q = [ cos γ 2 0 0 sin γ 2 ] [ cos β 2 0 sin β 2 0 ] [ cos α 2 sin α 2 0 0 ] = [ cos α 2 cos β 2 cos γ 2 + sin α 2 sin β sin γ 2 sin α 2 cos β 2 cos γ 2 + cos α 2 sin β 2 sin γ 2 cos α 2 sin β 2 cos γ 2 + sin α 2 cos β 2 sin γ 2 cos α 2 cos β 2 sin γ + sin α 2 sin β 2 cos γ 2 ] q = \left[ \begin{matrix} cos\frac{\gamma}{2} \\ 0 \\ 0 \\ sin\frac{\gamma}{2} \ end{matrix}\right]\left[\begin{matrix}cos\frac{\beta}{2}\\0\\sin\frac{\beta}{2}\\0 \end{matrix}\right ] \left[ \begin{matrix} cos\frac{\alpha}{2}\\sin\frac{\alpha}{2}\\ 0 \\ 0 \end{matrix}\right] = \left[\ begin{matrix} 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{matrix} \right]q=⎣ ⎡cos2c00sin2c⎦ ⎤⎣ ⎡cos2b0sin2b0⎦ ⎤⎣ ⎡cos2asin2a00⎦ ⎤=⎣ ⎡cos2acos2bcos2c+sin2asin2bsin2csin2acos2bcos2c+cos2asin2bsin2ccos2asin2bcos2c+sin2acos2bsin2ccos2acos2bsin2c+sin2asin2bcos2c⎦ ⎤
According to the above formula, the inverse solution can be obtained, that is, the quaternion q = ( q 0 , q 1 , q 2 , q 3 ) q=(q_0,q_1,q_2,q_3)q=(q0,q1,q2,q3)或 q = ( w , x , y , z ) q=(w,x,y,z) q=(w,x,y,z ) Let us give the following solution:
[ α β γ ] = [ arctan 2 ( q 0 q 1 + q 2 q 3 ) 1 − 2 ( q 1 2 + q 2 2 ) arcsine ( 2 ( q 0 q). 2 − q 1 q 3 ) ) arctan 2 ( q 0 q 3 + q 1 q 2 ) 1 − 2 ( q 2 2 + q 3 2 ) ] \left[ \begin{matrix} \alpha\\\beta\\ \gamma \end{matrix}\right] = \left[\begin{matrix} 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{matrix} \right]⎣
⎡abc⎦
⎤=⎣
⎡I 'm sorry _1−2(q12+q22)2(q0q1+q2q3)arcsin(2(q0q2−q1q3))I 'm sorry _1−2(q22+q32)2(q0q3+q1q2)⎦
⎤
Since the value range of arctanthe sum is − π 2 \frac{-\pi}{2}arcsin2− p和π 2 \frac{\pi}{2}2pBetween , there is only 180°, while the range of rotation around a certain axis is 360°, so a atan2function should be used instead of a arctanfunction:
[ α β γ ] = [ atan 2 ( 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 ) ) atan 2 ( 2 ( q 0 q 3 + q 1 q 2 ) , 1 − 2 ( q 2 2 + q 3 2 ) ) ] \left[ \begin{matrix} \alpha \\ \beta \\ \gamma \end{matrix} \right] = \left[ \begin{matrix} 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{matrix} \right]⎣
⎡abc⎦
⎤=⎣
⎡atan2(2(q0q1+q2q3),1−2(q12+q22))arcsin(2(q0q2−q1q3))atan2(2(q0q3+q1q2),1−2(q22+q32))⎦
⎤
For tan(θ) = y / x:
The value range of θ calculated by θ = ATan(y / x) is [-PI/2, PI/2]; the value range
of θ calculated by θ = ATan2(y, x) is The value range is [-PI, PI].
- When (x, y) is in the first quadrant, 0 < θ < PI/2
- When (x, y) is in the second quadrant PI/2 < θ≤PI
- When (x, y) is in the third quadrant, -PI < θ < -PI/2
- When (x, y) is in the fourth quadrant, -PI/2 < θ < 0