理论推导不同旋转顺序对应欧拉角四元数转换
前言
欧拉角是我们最常用的姿态表达方式，也是人类能理解的最直观的表达方式，但在使用其来进行旋转变换时常会遇到万向锁问题，而使用四元数的方式进行旋转变换则不会，因此我们通常在旋转变换时会先将欧拉角转换成四元数来进行旋转变换，变换完成后再将变换后的四元数转换成欧拉角进行直观的显示。而规定不同的旋转顺序，其对应的欧拉角与四元数之间的相互转换也是不一样的，这也是为什么在查阅资料时会发现不同的博主写出来的转换公式各不相同的原因，因此，在这里对不同旋转顺序所对应的欧拉角与四元数之间的相互转换做一个总结。
欧拉角转四元数
对位姿进行旋转变换，按照不同的旋转顺序，本质上可以分解为绕三轴依次进行三次不同的旋转。 因此，使用四元数来进行旋转变换可以看作对原始位姿四元数依次乘三轴的四元数变换。设
q
a
q_{a}
qa,
q
b
q_{b}
qb定义如下:
q
a
=
[
w
a
,
x
a
,
y
a
,
z
a
]
T
q
b
=
[
w
b
,
x
b
,
y
b
,
z
b
]
T
q_{a} = [w_{a},x_{a},y_{a},z_{a}]^T \\ q_{b} = [w_{b},x_{b},y_{b},z_{b}]^T
qa=[wa,xa,ya,za]Tqb=[wb,xb,yb,zb]T
四元数乘法规则如下：
q
a
⊗
q
b
=
w
a
w
b
−
x
a
x
b
−
y
a
y
b
−
z
a
z
b
+
(
w
a
x
b
+
x
a
w
b
+
y
a
z
b
−
z
a
y
b
)
i
+
(
w
a
y
b
−
x
a
z
b
+
y
a
w
b
+
z
a
x
b
)
j
+
(
w
a
z
b
+
x
a
y
b
−
y
a
x
b
+
z
a
w
b
)
k
q_{a}\otimes q_{b} = w_aw_b - x_ax_b - y_ay_b - z_az_b \\ \qquad\qquad+(w_ax_b+x_aw_b+y_az_b-z_ay_b)i \\ \qquad\qquad+(w_ay_b-x_az_b+y_aw_b+z_ax_b)j \\ \qquad\qquad+(w_az_b+x_ay_b-y_ax_b+z_aw_b)k
qa⊗qb=wawb−xaxb−yayb−zazb+(waxb+xawb+yazb−zayb)i+(wayb−xazb+yawb+zaxb)j+(wazb+xayb−yaxb+zawb)k
本文规定绕X轴旋转角度为pitch(即俯仰角)，绕Y轴旋转角度为roll(即横滚角)，绕Z轴旋转角度为yaw(即偏航角)。依此这里给出绕各轴旋转欧拉角转四元数的公式：
q
x
=
[
c
o
s
p
i
t
c
h
2
,
s
i
n
p
i
t
c
h
2
,
0
,
0
]
q
y
=
[
c
o
s
r
o
l
l
2
,
0
,
s
i
n
r
o
l
l
2
,
0
]
q
z
=
[
c
o
s
y
a
w
2
,
0
,
0
,
s
i
n
r
o
l
l
2
]
q_x = [cos{\frac {pitch} 2},sin{\frac {pitch} 2},0,0]\\q_y = [cos{\frac {roll} 2},0,sin{\frac {roll} 2},0]\\q_z = [cos{\frac {yaw} 2},0,0,sin{\frac {roll} 2}]
qx=[cos2pitch,sin2pitch,0,0]qy=[cos2roll,0,sin2roll,0]qz=[cos2yaw,0,0,sin2roll]
以ZXY旋转顺序为例，依照上述分析变换可以分解为：
q
=
q
z
⊗
q
x
⊗
q
y
=
[
c
o
s
p
i
t
c
h
2
c
o
s
r
o
l
l
2
c
o
s
y
a
w
2
−
s
i
n
p
i
t
c
h
2
s
i
n
r
o
l
l
2
s
i
n
y
a
w
2
,
c
o
s
r
o
l
l
2
c
o
s
y
a
w
2
s
i
n
p
i
t
c
h
2
−
c
o
s
p
i
t
c
h
2
s
i
n
r
o
l
l
2
s
i
n
y
a
w
2
,
c
o
s
p
i
t
c
h
2
c
o
s
y
a
w
2
s
i
n
r
o
l
l
2
+
c
o
s
r
o
l
l
2
s
i
n
p
i
t
c
h
2
s
i
n
y
a
w
2
,
c
o
s
p
i
t
c
h
2
c
o
s
r
o
l
l
2
s
i
n
y
a
w
2
+
c
o
s
y
a
w
2
s
i
n
p
i
t
c
h
2
s
i
n
r
o
l
l
2
]
q = q_z\otimes q_x \otimes q_y \\ \qquad\\=[cos{\frac {pitch} 2}cos{\frac {roll} 2}cos{\frac {yaw} 2}-sin{\frac {pitch} 2}sin{\frac {roll} 2}sin{\frac {yaw} 2}, \\\qquad\\cos{\frac {roll} 2}cos{\frac {yaw} 2}sin{\frac {pitch} 2} - cos{\frac {pitch} 2}sin{\frac {roll} 2}sin{\frac {yaw} 2}, \\\qquad\\ cos{\frac {pitch} 2}cos{\frac {yaw} 2}sin{\frac {roll} 2} + cos{\frac {roll} 2}sin{\frac {pitch} 2}sin{\frac {yaw} 2}, \\\qquad\\ cos{\frac {pitch} 2}cos{\frac {roll} 2}sin{\frac {yaw} 2} + cos{\frac {yaw} 2}sin{\frac {pitch} 2}sin{\frac {roll} 2}]
q=qz⊗qx⊗qy=[cos2pitchcos2rollcos2yaw−sin2pitchsin2rollsin2yaw,cos2rollcos2yawsin2pitch−cos2pitchsin2rollsin2yaw,cos2pitchcos2yawsin2roll+cos2rollsin2pitchsin2yaw,cos2pitchcos2rollsin2yaw+cos2yawsin2pitchsin2roll]
这就是欧拉角到四元数的转换，其他的顺序也可以按照相同的思路，只需要将三轴的四元数相乘顺序换一下即可，文章末尾会给出六种常见的转换。四元数之间相乘手推比较麻烦，可以借助一下matlab来进行公式的推导，代码也放在文章末尾。
四元数转欧拉角
四元数转欧拉角也是相同的思路，这里需要借助一下旋转矩阵作为中转。欧拉角转换旋转矩阵按照相同的思路分解成绕三轴依次进行三次不同的转换，这里给出绕各轴旋转欧拉角转旋转矩阵的公式：
R
x
=
[
1
0
0
0
c
o
s
(
p
i
t
c
h
)
−
s
i
n
(
p
i
t
c
h
)
0
s
i
n
(
p
i
t
c
h
)
c
o
s
(
p
i
t
c
h
)
]
R
y
=
[
c
o
s
(
r
o
l
l
)
0
s
i
n
(
r
o
l
l
)
0
1
0
−
s
i
n
(
r
o
l
l
)
0
c
o
s
(
r
o
l
l
)
]
R
z
=
[
c
o
s
(
y
a
w
)
−
s
i
n
(
y
a
w
)
0
s
i
n
(
y
a
w
)
c
o
s
(
y
a
w
)
0
0
0
1
]
R_x = \begin{bmatrix} 1 & 0 & 0 \\ 0 & cos(pitch) & -sin(pitch) \\ 0 & sin(pitch) & cos(pitch) \end{bmatrix} \\\qquad\\ R_y = \begin{bmatrix} cos(roll) & 0 & sin(roll) \\ 0 & 1 & 0 \\ -sin(roll) & 0 & cos(roll) \end{bmatrix} \\\qquad\\ R_z = \begin{bmatrix} cos(yaw) & -sin(yaw) & 0 \\ sin(yaw) & cos(yaw) & 0 \\ 0 & 0 & 1 \end{bmatrix}
Rx=
1000cos(pitch)sin(pitch)0−sin(pitch)cos(pitch)
Ry=
cos(roll)0−sin(roll)010sin(roll)0cos(roll)
Rz=
cos(yaw)sin(yaw)0−sin(yaw)cos(yaw)0001
以ZXY旋转顺序为例，依照上述欧拉角转四元数思路变换可以分解为：
R
=
R
z
R
x
R
y
=
[
c
o
s
(
r
o
l
l
)
∗
c
o
s
(
y
a
w
)
−
s
i
n
(
p
i
t
c
h
)
∗
s
i
n
(
r
o
l
l
)
∗
s
i
n
(
y
a
w
)
−
c
o
s
(
p
i
t
c
h
)
∗
s
i
n
(
y
a
w
)
c
o
s
(
y
a
w
)
∗
s
i
n
(
r
o
l
l
)
+
c
o
s
(
r
o
l
l
)
∗
s
i
n
(
p
i
t
c
h
)
∗
s
i
n
(
y
a
w
)
c
o
s
(
r
o
l
l
)
∗
s
i
n
(
y
a
w
)
+
c
o
s
(
y
a
w
)
∗
s
i
n
(
p
i
t
c
h
)
∗
s
i
n
(
r
o
l
l
)
c
o
s
(
p
i
t
c
h
)
∗
c
o
s
(
y
a
w
)
s
i
n
(
r
o
l
l
)
∗
s
i
n
(
y
a
w
)
−
c
o
s
(
r
o
l
l
)
∗
c
o
s
(
y
a
w
)
∗
s
i
n
(
p
i
t
c
h
)
−
c
o
s
(
p
i
t
c
h
)
∗
s
i
n
(
r
o
l
l
)
s
i
n
(
p
i
t
c
h
)
c
o
s
(
p
i
t
c
h
)
∗
c
o
s
(
r
o
l
l
)
]
R = R_zR_xR_y \\\qquad\\= \begin{bmatrix} cos(roll)*cos(yaw) - sin(pitch)*sin(roll)*sin(yaw) & -cos(pitch)*sin(yaw) & cos(yaw)*sin(roll) + cos(roll)*sin(pitch)*sin(yaw)\\ cos(roll)*sin(yaw) + cos(yaw)*sin(pitch)*sin(roll) & cos(pitch)*cos(yaw) & sin(roll)*sin(yaw) - cos(roll)*cos(yaw)*sin(pitch) \\ -cos(pitch)*sin(roll) & sin(pitch)& cos(pitch)*cos(roll) \end{bmatrix}
R=RzRxRy=
cos(roll)∗cos(yaw)−sin(pitch)∗sin(roll)∗sin(yaw)cos(roll)∗sin(yaw)+cos(yaw)∗sin(pitch)∗sin(roll)−cos(pitch)∗sin(roll)−cos(pitch)∗sin(yaw)cos(pitch)∗cos(yaw)sin(pitch)cos(yaw)∗sin(roll)+cos(roll)∗sin(pitch)∗sin(yaw)sin(roll)∗sin(yaw)−cos(roll)∗cos(yaw)∗sin(pitch)cos(pitch)∗cos(roll)
这里手算也是比较复杂的，可以借助matlab进行计算推导。而四元数转旋转矩阵的公式如下：
R
=
[
q
0
2
+
q
1
2
−
q
2
2
−
q
3
2
2
q
1
q
2
−
2
q
0
q
3
2
q
0
q
2
+
2
q
1
q
3
2
q
0
q
3
+
2
q
1
q
2
q
0
2
−
q
1
2
+
q
2
2
−
q
3
2
2
q
2
q
3
−
2
q
0
q
1
2
q
1
q
3
−
2
q
0
q
2
2
q
0
q
1
+
2
q
2
q
3
q
0
2
−
q
1
2
−
q
2
2
+
q
3
2
]
R = \begin{bmatrix} q0^2 + q1^2 - q2^2 - q3^2 & 2q1q2 - 2q0q3 & 2q0q2 + 2q1q3\\ 2q0q3 + 2q1q2 & q0^2 - q1^2 + q2^2 - q3^2 & 2q2q3 - 2q0q1 \\ 2q1q3 - 2q0q2 & 2q0q1 + 2q2q3 & q0^2 - q1^2 - q2^2 + q3^2 \end{bmatrix}
R=
q02+q12−q22−q322q0q3+2q1q22q1q3−2q0q22q1q2−2q0q3q02−q12+q22−q322q0q1+2q2q32q0q2+2q1q32q2q3−2q0q1q02−q12−q22+q32
可以对比一下上述两式得出四元数转欧拉角的公式：
r
o
l
l
=
a
t
a
n
−
R
20
R
22
=
a
t
a
n
2
q
0
q
2
−
2
q
1
q
3
q
0
2
−
q
1
2
−
q
2
2
+
q
3
2
p
i
t
c
h
=
a
s
i
n
R
21
=
a
s
i
n
(
2
q
0
q
1
+
2
q
2
q
3
)
y
a
w
=
a
t
a
n
−
R
01
R
11
=
a
t
a
n
2
q
0
q
3
−
2
q
1
q
2
q
0
2
−
q
1
2
+
q
2
2
−
q
3
2
roll = atan{\frac {-R_{20}} {R_{22}}} = atan{\frac {2q0q2 - 2q1q3} {q0^2 - q1^2 - q2^2 + q3^2}} \\\qquad\\ pitch = asinR_{21} = asin(2q0q1+2q2q3) \\\qquad\\ yaw = atan{\frac {-R_{01}} {R_{11}}} = atan{\frac {2q0q3 - 2q1q2} {q0^2 - q1^2 + q2^2 - q3^2}}
roll=atanR22−R20=atanq02−q12−q22+q322q0q2−2q1q3pitch=asinR21=asin(2q0q1+2q2q3)yaw=atanR11−R01=atanq02−q12+q22−q322q0q3−2q1q2
其他的旋转顺序同理。
六种不同旋转顺序对应欧拉角与四元数间相互转换公式
XYZ
欧拉角转四元数
q = q x ⊗ q y ⊗ q z = [ c o s p i t c h 2 c o s r o l l 2 c o s y a w 2 − s i n p i t c h 2 s i n r o l l 2 s i n y a w 2 , c o s r o l l 2 c o s y a w 2 s i n p i t c h 2 + c o s p i t c h 2 s i n r o l l 2 s i n y a w 2 , c o s p i t c h 2 c o s y a w 2 s i n r o l l 2 − c o s r o l l 2 s i n p i t c h 2 s i n y a w 2 , c o s p i t c h 2 c o s r o l l 2 s i n y a w 2 + c o s y a w 2 s i n p i t c h 2 s i n r o l l 2 ] q = q_x\otimes q_y \otimes q_z \\ \qquad\\=[cos{\frac {pitch} 2}cos{\frac {roll} 2}cos{\frac {yaw} 2}-sin{\frac {pitch} 2}sin{\frac {roll} 2}sin{\frac {yaw} 2}, \\\qquad\\ cos{\frac {roll} 2}cos{\frac {yaw} 2}sin{\frac {pitch} 2} + cos{\frac {pitch} 2}sin{\frac {roll} 2}sin{\frac {yaw} 2}, \\\qquad\\ cos{\frac {pitch} 2}cos{\frac {yaw} 2}sin{\frac {roll} 2} - cos{\frac {roll} 2}sin{\frac {pitch} 2}sin{\frac {yaw} 2}, \\\qquad\\ cos{\frac {pitch} 2}cos{\frac {roll} 2}sin{\frac {yaw} 2} + cos{\frac {yaw} 2}sin{\frac {pitch} 2}sin{\frac {roll} 2}] q=qx⊗qy⊗qz=[cos2pitchcos2rollcos2yaw−sin2pitchsin2rollsin2yaw,cos2rollcos2yawsin2pitch+cos2pitchsin2rollsin2yaw,cos2pitchcos2yawsin2roll−cos2rollsin2pitchsin2yaw,cos2pitchcos2rollsin2yaw+cos2yawsin2pitchsin2roll]
四元数转欧拉角
r o l l = a s i n ( 2 q 0 q 2 + 2 q 1 q 3 ) p i t c h = a t a n 2 q 0 q 1 − 2 q 2 q 3 q 0 2 − q 1 2 − q 2 2 + q 3 2 y a w = a t a n 2 q 0 q 3 − 2 q 1 q 2 q 0 2 + q 1 2 − q 2 2 − q 3 2 roll = asin(2q0q2+2q1q3) \\\qquad\\ pitch = atan{\frac {2q0q1 - 2q2q3} {q0^2 - q1^2 - q2^2 + q3^2}} \\\qquad\\ yaw = atan{\frac {2q0q3 - 2q1q2} {q0^2 + q1^2 - q2^2 - q3^2}} roll=asin(2q0q2+2q1q3)pitch=atanq02−q12−q22+q322q0q1−2q2q3yaw=atanq02+q12−q22−q322q0q3−2q1q2
XZY
欧拉角转四元数
q = q x ⊗ q z ⊗ q y = [ c o s p i t c h 2 c o s r o l l 2 c o s y a w 2 + s i n p i t c h 2 s i n r o l l 2 s i n y a w 2 , c o s r o l l 2 c o s y a w 2 s i n p i t c h 2 − c o s p i t c h 2 s i n r o l l 2 s i n y a w 2 , c o s p i t c h 2 c o s y a w 2 s i n r o l l 2 − c o s r o l l 2 s i n p i t c h 2 s i n y a w 2 , c o s p i t c h 2 c o s r o l l 2 s i n y a w 2 + c o s y a w 2 s i n p i t c h 2 s i n r o l l 2 ] q = q_x\otimes q_z \otimes q_y \\ \qquad\\=[cos{\frac {pitch} 2}cos{\frac {roll} 2}cos{\frac {yaw} 2}+sin{\frac {pitch} 2}sin{\frac {roll} 2}sin{\frac {yaw} 2}, \\\qquad\\ cos{\frac {roll} 2}cos{\frac {yaw} 2}sin{\frac {pitch} 2} - cos{\frac {pitch} 2}sin{\frac {roll} 2}sin{\frac {yaw} 2}, \\\qquad\\ cos{\frac {pitch} 2}cos{\frac {yaw} 2}sin{\frac {roll} 2} - cos{\frac {roll} 2}sin{\frac {pitch} 2}sin{\frac {yaw} 2}, \\\qquad\\ cos{\frac {pitch} 2}cos{\frac {roll} 2}sin{\frac {yaw} 2} + cos{\frac {yaw} 2}sin{\frac {pitch} 2}sin{\frac {roll} 2}] q=qx⊗qz⊗qy=[cos2pitchcos2rollcos2yaw+sin2pitchsin2rollsin2yaw,cos2rollcos2yawsin2pitch−cos2pitchsin2rollsin2yaw,cos2pitchcos2yawsin2roll−cos2rollsin2pitchsin2yaw,cos2pitchcos2rollsin2yaw+cos2yawsin2pitchsin2roll]
四元数转欧拉角
r o l l = a t a n 2 q 0 q 2 + 2 q 1 q 3 q 0 2 + q 1 2 − q 2 2 − q 3 2 p i t c h = a t a n 2 q 0 q 1 + 2 q 2 q 3 q 0 2 − q 1 2 + q 2 2 − q 3 2 y a w = a s i n ( 2 q 0 q 3 − 2 q 1 q 2 ) roll = atan{\frac {2q0q2 + 2q1q3} {q0^2 + q1^2 - q2^2 - q3^2}} \\\qquad\\ pitch = atan{\frac {2q0q1 + 2q2q3} {q0^2 - q1^2 + q2^2 - q3^2}} \\\qquad\\ yaw = asin(2q0q3 - 2q1q2) roll=atanq02+q12−q22−q322q0q2+2q1q3pitch=atanq02−q12+q22−q322q0q1+2q2q3yaw=asin(2q0q3−2q1q2)
YXZ
欧拉角转四元数
q = q y ⊗ q x ⊗ q z = [ c o s p i t c h 2 c o s r o l l 2 c o s y a w 2 + s i n p i t c h 2 s i n r o l l 2 s i n y a w 2 , c o s r o l l 2 c o s y a w 2 s i n p i t c h 2 + c o s p i t c h 2 s i n r o l l 2 s i n y a w 2 , c o s p i t c h 2 c o s y a w 2 s i n r o l l 2 − c o s r o l l 2 s i n p i t c h 2 s i n y a w 2 , c o s p i t c h 2 c o s r o l l 2 s i n y a w 2 − c o s y a w 2 s i n p i t c h 2 s i n r o l l 2 ] q = q_y\otimes q_x \otimes q_z \\ \qquad\\=[cos{\frac {pitch} 2}cos{\frac {roll} 2}cos{\frac {yaw} 2}+sin{\frac {pitch} 2}sin{\frac {roll} 2}sin{\frac {yaw} 2}, \\\qquad\\ cos{\frac {roll} 2}cos{\frac {yaw} 2}sin{\frac {pitch} 2} + cos{\frac {pitch} 2}sin{\frac {roll} 2}sin{\frac {yaw} 2}, \\\qquad\\ cos{\frac {pitch} 2}cos{\frac {yaw} 2}sin{\frac {roll} 2} - cos{\frac {roll} 2}sin{\frac {pitch} 2}sin{\frac {yaw} 2}, \\\qquad\\ cos{\frac {pitch} 2}cos{\frac {roll} 2}sin{\frac {yaw} 2} - cos{\frac {yaw} 2}sin{\frac {pitch} 2}sin{\frac {roll} 2}] q=qy⊗qx⊗qz=[cos2pitchcos2rollcos2yaw+sin2pitchsin2rollsin2yaw,cos2rollcos2yawsin2pitch+cos2pitchsin2rollsin2yaw,cos2pitchcos2yawsin2roll−cos2rollsin2pitchsin2yaw,cos2pitchcos2rollsin2yaw−cos2yawsin2pitchsin2roll]
四元数转欧拉角
r o l l = a t a n 2 q 0 q 2 + 2 q 1 q 3 q 0 2 − q 1 2 − q 2 2 + q 3 2 p i t c h = a s i n ( 2 q 0 q 1 − 2 q 2 q 3 ) y a w = a t a n 2 q 0 q 3 + 2 q 1 q 2 q 0 2 − q 1 2 + q 2 2 − q 3 2 roll = atan{\frac {2q0q2 + 2q1q3} {q0^2 - q1^2 - q2^2 + q3^2}} \\\qquad\\ pitch = asin(2q0q1 - 2q2q3) \\\qquad\\ yaw = atan{\frac {2q0q3 + 2q1q2} {q0^2 - q1^2 + q2^2 - q3^2}} roll=atanq02−q12−q22+q322q0q2+2q1q3pitch=asin(2q0q1−2q2q3)yaw=atanq02−q12+q22−q322q0q3+2q1q2
YZX
欧拉角转四元数
q = q y ⊗ q z ⊗ q x = [ c o s p i t c h 2 c o s r o l l 2 c o s y a w 2 − s i n p i t c h 2 s i n r o l l 2 s i n y a w 2 , c o s r o l l 2 c o s y a w 2 s i n p i t c h 2 + c o s p i t c h 2 s i n r o l l 2 s i n y a w 2 , c o s p i t c h 2 c o s y a w 2 s i n r o l l 2 + c o s r o l l 2 s i n p i t c h 2 s i n y a w 2 , c o s p i t c h 2 c o s r o l l 2 s i n y a w 2 − c o s y a w 2 s i n p i t c h 2 s i n r o l l 2 ] q = q_y\otimes q_z \otimes q_x \\ \qquad\\=[cos{\frac {pitch} 2}cos{\frac {roll} 2}cos{\frac {yaw} 2}-sin{\frac {pitch} 2}sin{\frac {roll} 2}sin{\frac {yaw} 2}, \\\qquad\\ cos{\frac {roll} 2}cos{\frac {yaw} 2}sin{\frac {pitch} 2} + cos{\frac {pitch} 2}sin{\frac {roll} 2}sin{\frac {yaw} 2}, \\\qquad\\ cos{\frac {pitch} 2}cos{\frac {yaw} 2}sin{\frac {roll} 2} + cos{\frac {roll} 2}sin{\frac {pitch} 2}sin{\frac {yaw} 2}, \\\qquad\\ cos{\frac {pitch} 2}cos{\frac {roll} 2}sin{\frac {yaw} 2} - cos{\frac {yaw} 2}sin{\frac {pitch} 2}sin{\frac {roll} 2}] q=qy⊗qz⊗qx=[cos2pitchcos2rollcos2yaw−sin2pitchsin2rollsin2yaw,cos2rollcos2yawsin2pitch+cos2pitchsin2rollsin2yaw,cos2pitchcos2yawsin2roll+cos2rollsin2pitchsin2yaw,cos2pitchcos2rollsin2yaw−cos2yawsin2pitchsin2roll]
四元数转欧拉角
r o l l = a t a n 2 q 0 q 2 − 2 q 1 q 3 q 0 2 + q 1 2 − q 2 2 − q 3 2 p i t c h = a t a n 2 q 0 q 1 − 2 q 2 q 3 q 0 2 − q 1 2 + q 2 2 − q 3 2 y a w = a s i n ( 2 q 0 q 3 + 2 q 1 q 2 ) roll = atan{\frac {2q0q2 - 2q1q3} {q0^2 + q1^2 - q2^2 - q3^2}} \\\qquad\\ pitch = atan{\frac {2q0q1 - 2q2q3} {q0^2 - q1^2 + q2^2 - q3^2}} \\\qquad\\ yaw = asin(2q0q3 + 2q1q2) roll=atanq02+q12−q22−q322q0q2−2q1q3pitch=atanq02−q12+q22−q322q0q1−2q2q3yaw=asin(2q0q3+2q1q2)
ZXY
欧拉角转四元数：
q = q z ⊗ q x ⊗ q y = [ c o s p i t c h 2 c o s r o l l 2 c o s y a w 2 − s i n p i t c h 2 s i n r o l l 2 s i n y a w 2 , c o s r o l l 2 c o s y a w 2 s i n p i t c h 2 − c o s p i t c h 2 s i n r o l l 2 s i n y a w 2 , c o s p i t c h 2 c o s y a w 2 s i n r o l l 2 + c o s r o l l 2 s i n p i t c h 2 s i n y a w 2 , c o s p i t c h 2 c o s r o l l 2 s i n y a w 2 + c o s y a w 2 s i n p i t c h 2 s i n r o l l 2 ] q = q_z\otimes q_x \otimes q_y \\ \qquad\\=[cos{\frac {pitch} 2}cos{\frac {roll} 2}cos{\frac {yaw} 2}-sin{\frac {pitch} 2}sin{\frac {roll} 2}sin{\frac {yaw} 2}, \\\qquad\\ cos{\frac {roll} 2}cos{\frac {yaw} 2}sin{\frac {pitch} 2} - cos{\frac {pitch} 2}sin{\frac {roll} 2}sin{\frac {yaw} 2}, \\\qquad\\ cos{\frac {pitch} 2}cos{\frac {yaw} 2}sin{\frac {roll} 2} + cos{\frac {roll} 2}sin{\frac {pitch} 2}sin{\frac {yaw} 2}, \\\qquad\\ cos{\frac {pitch} 2}cos{\frac {roll} 2}sin{\frac {yaw} 2} + cos{\frac {yaw} 2}sin{\frac {pitch} 2}sin{\frac {roll} 2}] q=qz⊗qx⊗qy=[cos2pitchcos2rollcos2yaw−sin2pitchsin2rollsin2yaw,cos2rollcos2yawsin2pitch−cos2pitchsin2rollsin2yaw,cos2pitchcos2yawsin2roll+cos2rollsin2pitchsin2yaw,cos2pitchcos2rollsin2yaw+cos2yawsin2pitchsin2roll]
四元数转欧拉角
r o l l = a t a n 2 q 0 q 2 − 2 q 1 q 3 q 0 2 − q 1 2 − q 2 2 + q 3 2 p i t c h = a s i n ( 2 q 0 q 1 + 2 q 2 q 3 ) y a w = a t a n 2 q 0 q 3 − 2 q 1 q 2 q 0 2 − q 1 2 + q 2 2 − q 3 2 roll = atan{\frac {2q0q2 - 2q1q3} {q0^2 - q1^2 - q2^2 + q3^2}} \\\qquad\\ pitch = asin(2q0q1 + 2q2q3) \\\qquad\\ yaw = atan{\frac {2q0q3 - 2q1q2} {q0^2 - q1^2 + q2^2 - q3^2}} roll=atanq02−q12−q22+q322q0q2−2q1q3pitch=asin(2q0q1+2q2q3)yaw=atanq02−q12+q22−q322q0q3−2q1q2
ZYX
欧拉角转四元数：
q = q z ⊗ q y ⊗ q x = [ c o s p i t c h 2 c o s r o l l 2 c o s y a w 2 + s i n p i t c h 2 s i n r o l l 2 s i n y a w 2 , c o s r o l l 2 c o s y a w 2 s i n p i t c h 2 − c o s p i t c h 2 s i n r o l l 2 s i n y a w 2 , c o s p i t c h 2 c o s y a w 2 s i n r o l l 2 + c o s r o l l 2 s i n p i t c h 2 s i n y a w 2 , c o s p i t c h 2 c o s r o l l 2 s i n y a w 2 − c o s y a w 2 s i n p i t c h 2 s i n r o l l 2 ] q = q_z\otimes q_y \otimes q_x \\ \qquad\\=[cos{\frac {pitch} 2}cos{\frac {roll} 2}cos{\frac {yaw} 2}+sin{\frac {pitch} 2}sin{\frac {roll} 2}sin{\frac {yaw} 2}, \\\qquad\\ cos{\frac {roll} 2}cos{\frac {yaw} 2}sin{\frac {pitch} 2} - cos{\frac {pitch} 2}sin{\frac {roll} 2}sin{\frac {yaw} 2}, \\\qquad\\ cos{\frac {pitch} 2}cos{\frac {yaw} 2}sin{\frac {roll} 2} + cos{\frac {roll} 2}sin{\frac {pitch} 2}sin{\frac {yaw} 2}, \\\qquad\\ cos{\frac {pitch} 2}cos{\frac {roll} 2}sin{\frac {yaw} 2} - cos{\frac {yaw} 2}sin{\frac {pitch} 2}sin{\frac {roll} 2}] q=qz⊗qy⊗qx=[cos2pitchcos2rollcos2yaw+sin2pitchsin2rollsin2yaw,cos2rollcos2yawsin2pitch−cos2pitchsin2rollsin2yaw,cos2pitchcos2yawsin2roll+cos2rollsin2pitchsin2yaw,cos2pitchcos2rollsin2yaw−cos2yawsin2pitchsin2roll]
四元数转欧拉角
r o l l = a s i n ( 2 q 0 q 2 − 2 q 1 q 3 ) p i t c h = a t a n 2 q 0 q 1 + 2 q 2 q 3 q 0 2 − q 1 2 − q 2 2 + q 3 2 y a w = a t a n 2 q 0 q 3 + 2 q 1 q 2 q 0 2 + q 1 2 − q 2 2 − q 3 2 roll = asin(2q0q2 - 2q1q3) \\\qquad\\ pitch = atan{\frac {2q0q1 + 2q2q3} {q0^2 - q1^2 - q2^2 + q3^2}} \\\qquad\\ yaw = atan{\frac {2q0q3 + 2q1q2} {q0^2 + q1^2 - q2^2 - q3^2}} roll=asin(2q0q2−2q1q3)pitch=atanq02−q12−q22+q322q0q1+2q2q3yaw=atanq02+q12−q22−q322q0q3+2q1q2
代码
% 测试欧拉角到四元数之间的转换
syms pitch % x
syms roll % y
syms yaw % z
syms q0
syms q1
syms q2
syms q3
q_x=[cos(pitch/2),1*sin(pitch/2),0,0]; % x轴：[1,0,0] 绕x轴旋转pitch
q_y=[cos(roll/2),0,1*sin(roll/2),0]; % y轴：[0,1,0] 绕y轴旋转roll
q_z=[cos(yaw/2),0,0,1*sin(yaw/2)]; % z轴：[0,0,1] 绕z轴旋转yaw
mat_x = [1,0,0;
0,cos(pitch),-sin(pitch);
0,sin(pitch),cos(pitch)];
mat_y = [cos(roll),0,sin(roll);
0,1,0;
-sin(roll),0,cos(roll)];
mat_z = [cos(yaw),-sin(yaw),0;
sin(yaw),cos(yaw),0;
0,0,1];
quat_mat = [q0*q0+q1*q1-q2*q2-q3*q3, 2*q1*q2-2*q0*q3, 2*q1*q3+2*q0*q2;
2*q1*q2+2*q0*q3, q0*q0-q1*q1+q2*q2-q3*q3, 2*q2*q3-2*q0*q1;
2*q1*q3-2*q0*q2, 2*q2*q3+2*q0*q1, q0*q0-q1*q1-q2*q2+q3*q3];
q= quaternProd(quaternProd(q_z, q_y),q_x)
mat_z*mat_y*mat_x
quat_mat
% 下面函数是四元数乘法
% 四元数相乘
function ab = quaternProd(a, b)
ab(1) = a(1).*b(1)-a(2).*b(2)-a(3).*b(3)-a(4).*b(4);
ab(2) = a(1).*b(2)+a(2).*b(1)+a(3).*b(4)-a(4).*b(3);
ab(3) = a(1).*b(3)-a(2).*b(4)+a(3).*b(1)+a(4).*b(2);
ab(4) = a(1).*b(4)+a(2).*b(3)-a(3).*b(2)+a(4).*b(1);
end
扫一扫
272
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
