欧拉角、四元数和旋转矩阵
欧拉角、四元数和旋转矩阵
旋转变换
旋转变换最为直观的表示方法是“轴-角”：绕着某一个过原点轴，旋转某一角度。
轴可以用一个单位长度的点
[
w
1
,
w
2
,
w
3
]
[w_1,w_2,w_3]
[w1,w2,w3]表示：原点到该点的射线即为此轴。 一个旋转变换可以用用欧拉角、四元数或者旋转矩阵表示。以下讨论不同表示方法之间的关系，以及旋转变换的合成、取逆等操作。
旋转矩阵
旋转可以看做一种特殊的坐标变换，而坐标变换可以用用
3
×
3
3\times 3
3×3矩阵
R
R
R来表示。对一个坐标施加旋转的结果是
x
′
=
R
x
x'=Rx
x′=Rx。 旋转矩阵是正交矩阵，即
∣
R
∣
=
1
|R|=1
∣R∣=1，旋转变换不改变向量的长度。
欧拉角的物理意义
任何一个旋转可以表示为依次绕着三个旋转轴旋三个角度的组合。这三个角度称为欧拉角。 本文中提到的欧拉角指：绕着世界坐标系的x,y,z轴，依次旋转的结果。其取值范围如下：
θ
x
∈
(
−
π
,
π
)
,
θ
y
∈
(
−
π
2
,
π
2
)
,
θ
z
∈
(
−
π
,
π
)
\theta_x\in (-\pi,\pi), \theta_y \in (-\frac{\pi}{2}, \frac{\pi}{2}), \theta_z \in (-\pi, \pi)
θx∈(−π,π),θy∈(−2π,2π),θz∈(−π,π)
欧拉角
→
\to
→旋转矩阵
单独绕一个轴旋转
θ
\theta
θ角度的旋转矩阵为：<br> <img src="https://img-blog.csdnimg.cn/img_convert/d8d711bb7a8f0ab024bf52998eb71ea3.png" alt="这里写图片描述">
如果依次绕x轴、y轴、z轴旋转，该变换的旋转矩阵为：
R
=
R
z
⋅
R
y
⋅
R
x
R=R_z \cdot R_y \cdot R_x
R=Rz⋅Ry⋅Rx
记三个轴欧拉角的正弦和余弦函数为
s
x
,
c
x
,
s
y
,
c
y
,
s
z
,
c
z
s_x, c_x, s_y, c_y, s_z, c_z
sx,cx,sy,cy,sz,cz。使用matlab的syms功能可以轻松推导旋转矩阵
R
R
R：
[
c
y
c
z
c
z
s
x
s
y
−
c
x
s
z
s
x
s
z
+
c
x
c
z
s
y
c
y
s
z
c
x
c
z
+
s
x
s
y
s
z
c
x
s
y
s
z
−
c
z
s
x
−
s
y
c
y
s
x
c
x
c
y
]
\left[ \begin{matrix} c_yc_z & c_z s_x s_y - c_x s_z & s_x s_z + c_x c_z s_y \\ c_y s_z & c_x c_z + s_x s_y s_z & c_x s_y s_z - c_z s_x \\ -s_y & c_y s_x & c_x c_y \end{matrix}\right]
⎣⎡cyczcysz−syczsxsy−cxszcxcz+sxsyszcysxsxsz+cxczsycxsysz−czsxcxcy⎦⎤
旋转矩阵
→
\to
→欧拉角
设旋转矩阵i行j列元素为
r
i
j
r_{ij}
rij。根据旋转矩阵的表达式，利用三角函数可以推导出欧拉角取值：
θ
x
=
a
t
a
n
2
(
r
32
,
r
33
)
\theta_x = atan2(r_{32},r_{33})
θx=atan2(r32,r33)
θ
y
=
a
t
a
n
2
(
−
r
31
,
r
32
2
+
r
33
2
)
\theta_y = atan2(-r_{31}, \sqrt{r_{32}^2+r_{33}^2})
θy=atan2(−r31,r322+r332
<svg width="400em" height="1.8800000000000001em" viewbox="0 0 400000 1944" preserveaspectratio="xMinYMin slice">
<path d="M1001,80H400000v40H1013.1s-83.4,268,-264.1,840c-180.7,
572,-277,876.3,-289,913c-4.7,4.7,-12.7,7,-24,7s-12,0,-12,0c-1.3,-3.3,-3.7,-11.7, -7,-25c-35.3,-125.3,-106.7,-373.3,-214,-744c-10,12,-21,25,-33,39s-32,39,-32,39 c-6,-5.3,-15,-14,-27,-26s25,-30,25,-30c26.7,-32.7,52,-63,76,-91s52,-60,52,-60 s208,722,208,722c56,-175.3,126.3,-397.3,211,-666c84.7,-268.7,153.8,-488.2,207.5, -658.5c53.7,-170.3,84.5,-266.8,92.5,-289.5c4,-6.7,10,-10,18,-10z M1001 80H400000v40H1013z"></path> </svg>)
θ
z
=
a
t
a
n
2
(
r
21
,
r
11
)
\theta_z = atan2(r_{21},r_{11})
θz=atan2(r21,r11)
四元数的物理意义
设有一个通过原点
[
0
,
0
,
0
]
[0,0,0]
[0,0,0]的旋转轴，该轴上单位长度的点为
[
w
1
,
w
2
,
w
3
]
[w_1,w_2,w_3]
[w1,w2,w3]。绕此轴旋转
θ
\theta
θ角的变换可以用一个向量表示：<br>
[
cos
θ
2
,
w
1
sin
θ
2
,
w
2
sin
θ
2
,
w
3
sin
θ
2
]
[\cos \frac{\theta}{2}, w_1 \sin \frac{\theta}{2}, w_2 \sin \frac{\theta}{2},w_3 \sin \frac{\theta}{2}]
[cos2θ,w1sin2θ,w2sin2θ,w3sin2θ]
也记为
q
=
[
q
0
,
q
1
,
q
2
,
q
3
]
q=[q_0,q_1,q_2,q_3]
q=[q0,q1,q2,q3]，或者
q
=
q
0
+
q
1
i
+
q
2
j
+
q
3
k
q=q_0+q_1i+q_2j+q_3k
q=q0+q1i+q2j+q3k。四元数的模长为1：<br>
q
0
2
+
q
1
2
+
q
2
2
+
q
3
2
=
cos
2
θ
2
+
sin
2
θ
2
(
w
1
2
+
w
2
2
+
w
3
3
)
=
cos
2
θ
2
+
sin
2
θ
2
=
1
q_0^2+q_1^2+q_2^2+q_3^2 = \cos^2 \frac{\theta}{2} + \sin^2 \frac{\theta}{2} (w_1^2+w_2^2+w_3^3)=\cos^2 \frac{\theta}{2} + \sin^2 \frac{\theta}{2} = 1
q02+q12+q22+q32=cos22θ+sin22θ(w12+w22+w33)=cos22θ+sin22θ=1
四元数
→
\to
→旋转矩阵
此段公式暂不确定，请您参看评论讨论
利用罗德里格旋转公式可以获得绕某过原点一轴
[
w
1
,
w
2
,
w
3
]
[w_1,w_2,w_3]
[w1,w2,w3]旋转某一角度
θ
\theta
θ的旋转矩阵
R
R
R：<br>
[
cos
θ
+
w
1
2
(
1
−
cos
θ
)
w
1
w
2
(
1
−
cos
θ
)
w
2
sin
θ
w
1
w
2
(
1
−
cos
θ
)
cos
θ
+
w
2
2
(
1
−
cos
θ
)
−
w
1
sin
θ
−
w
2
sin
θ
w
1
sin
θ
cos
θ
]
\left[ \begin{matrix} \cos \theta + w_1^2(1-\cos\theta) & w_1w_2(1-\cos \theta) & w_2 \sin \theta \\ w_1w_2(1-\cos \theta) & \cos \theta + w_2^2(1-\cos \theta) & -w_1 \sin \theta \\ -w_2 \sin \theta & w_1 \sin \theta & \cos \theta \end{matrix} \right]
⎣⎡cosθ+w12(1−cosθ)w1w2(1−cosθ)−w2sinθw1w2(1−cosθ)cosθ+w22(1−cosθ)w1sinθw2sinθ−w1sinθcosθ⎦⎤
代入四元组的表示方法，可得：
[
1
−
2
q
2
2
−
2
q
3
2
2
q
1
q
2
−
2
q
3
q
0
2
q
1
q
3
+
2
q
2
q
0
2
q
1
q
2
+
2
q
3
q
0
1
−
2
q
1
2
−
2
q
3
2
2
q
2
q
3
−
2
q
1
q
0
2
q
1
q
3
−
2
q
2
q
0
2
q
2
q
3
+
2
q
1
q
0
1
−
2
q
1
2
−
2
q
2
2
]
\left[ \begin{matrix} 1-2q_2^2-2q_3^2 & 2 q_1 q_2 - 2 q_3 q_0 & 2q_1q_3 + 2q_2q_0 \\ 2q_1q_2 + 2q_3q_0 & 1-2q_1^2-2q_3^2 & 2q_2q_3-2q_1q_0 \\ 2q_1q_3 - 2q_2q_0 & 2q_2q_3 + 2q_1q_0 & 1-2q_1^2-2q_2^2 \end{matrix} \right]
⎣⎡1−2q22−2q322q1q2+2q3q02q1q3−2q2q02q1q2−2q3q01−2q12−2q322q2q3+2q1q02q1q3+2q2q02q2q3−2q1q01−2q12−2q22⎦⎤
旋转矩阵
→
\to
→四元数
此段公式暂不确定，请您参看评论讨论
根据旋转矩阵的表达式，利用三角函数性质，可以由旋转矩阵得到四元数：
q
0
=
1
+
r
11
+
r
22
+
r
33
2
q_0 = \frac {\sqrt{1+r_{11}+r_{22}+r_{33}}}{2}
q0=21+r11+r22+r33
<svg width="400em" height="1.08em" viewbox="0 0 400000 1080" preserveaspectratio="xMinYMin slice">
<path d="M95,702c-2.7,0,-7.17,-2.7,-13.5,-8c-5.8,-5.3,-9.5,
-10,-9.5,-14c0,-2,0.3,-3.3,1,-4c1.3,-2.7,23.83,-20.7,67.5,-54c44.2,-33.3,65.8, -50.3,66.5,-51c1.3,-1.3,3,-2,5,-2c4.7,0,8.7,3.3,12,10s173,378,173,378c0.7,0, 35.3,-71,104,-213c68.7,-142,137.5,-285,206.5,-429c69,-144,104.5,-217.7,106.5, -221c5.3,-9.3,12,-14,20,-14H400000v40H845.2724s-225.272,467,-225.272,467 s-235,486,-235,486c-2.7,4.7,-9,7,-19,7c-6,0,-10,-1,-12,-3s-194,-422,-194,-422 s-65,47,-65,47z M834 80H400000v40H845z"></path> </svg>
q
1
=
r
32
−
r
23
4
q
0
q_1 = \frac {r_{32}-r_{23}}{4q_0}
q1=4q0r32−r23
q
2
=
r
13
−
r
31
4
q
0
q_2 = \frac {r_{13}-r_{31}}{4q_0}
q2=4q0r13−r31
q
3
=
r
21
−
r
12
4
q
0
q_3 = \frac {r_{21}-r_{12}}{4q_0}
q3=4q0r21−r12
开根号要求
1
+
T
r
(
R
)
>
0
1+Tr(R)>0
1+Tr(R)>0（其中
T
r
Tr
Tr表示矩阵的迹，等于对角元素之和，也等于特征值之和），分式要求
q
0
≠
0
q_0\neq0
q0=0。<br> 某些情况下（例如
θ
=
π
\theta=\pi
θ=π），
q
0
q_0
q0接近零，
T
r
(
R
)
Tr(R)
Tr(R)接近-1，需要使用以下方式求解（来源于[此处](http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm)）。
如果
r
11
,
r
22
,
r
33
r_{11},r_{22},r_{33}
r11,r22,r33中，
r
11
r_{11}
r11最大：<br>
S
=
1
+
r
11
−
r
22
−
r
33
S=\sqrt{1+r_{11}-r_{22}-r_{33}}
S=1+r11−r22−r33
<svg width="400em" height="1.08em" viewbox="0 0 400000 1080" preserveaspectratio="xMinYMin slice">
<path d="M95,702c-2.7,0,-7.17,-2.7,-13.5,-8c-5.8,-5.3,-9.5,
-10,-9.5,-14c0,-2,0.3,-3.3,1,-4c1.3,-2.7,23.83,-20.7,67.5,-54c44.2,-33.3,65.8, -50.3,66.5,-51c1.3,-1.3,3,-2,5,-2c4.7,0,8.7,3.3,12,10s173,378,173,378c0.7,0, 35.3,-71,104,-213c68.7,-142,137.5,-285,206.5,-429c69,-144,104.5,-217.7,106.5, -221c5.3,-9.3,12,-14,20,-14H400000v40H845.2724s-225.272,467,-225.272,467 s-235,486,-235,486c-2.7,4.7,-9,7,-19,7c-6,0,-10,-1,-12,-3s-194,-422,-194,-422 s-65,47,-65,47z M834 80H400000v40H845z"></path> </svg>
q
0
=
r
32
−
r
23
S
q_0 = \frac{r_{32}-r_{23}}{S}
q0=Sr32−r23
q
1
=
S
/
4
q_1 = S/4
q1=S/4
q
2
=
r
12
+
r
21
S
q_2 = \frac{r_{12}+r_{21}}{S}
q2=Sr12+r21
q
3
=
r
21
+
r
12
S
q_3 = \frac {r_{21}+r_{12}}{S}
q3=Sr21+r12
如果
r
11
,
r
22
,
r
33
r_{11},r_{22},r_{33}
r11,r22,r33中，
r
22
r_{22}
r22最大：<br>
S
=
1
−
r
11
+
r
22
−
r
33
S=\sqrt{1-r_{11}+r_{22}-r_{33}}
S=1−r11+r22−r33
<svg width="400em" height="1.08em" viewbox="0 0 400000 1080" preserveaspectratio="xMinYMin slice">
<path d="M95,702c-2.7,0,-7.17,-2.7,-13.5,-8c-5.8,-5.3,-9.5,
-10,-9.5,-14c0,-2,0.3,-3.3,1,-4c1.3,-2.7,23.83,-20.7,67.5,-54c44.2,-33.3,65.8, -50.3,66.5,-51c1.3,-1.3,3,-2,5,-2c4.7,0,8.7,3.3,12,10s173,378,173,378c0.7,0, 35.3,-71,104,-213c68.7,-142,137.5,-285,206.5,-429c69,-144,104.5,-217.7,106.5, -221c5.3,-9.3,12,-14,20,-14H400000v40H845.2724s-225.272,467,-225.272,467 s-235,486,-235,486c-2.7,4.7,-9,7,-19,7c-6,0,-10,-1,-12,-3s-194,-422,-194,-422 s-65,47,-65,47z M834 80H400000v40H845z"></path> </svg>
q
0
=
r
13
−
r
31
S
q_0 = \frac{r_{13}-r_{31}}{S}
q0=Sr13−r31
q
1
=
r
12
+
r
21
S
q_1 = \frac {r_{12}+r_{21}}{S}
q1=Sr12+r21
q
2
=
S
/
4
q_2 = S/4
q2=S/4
q
3
=
r
23
+
r
32
S
q_3 = \frac {r_{23}+r_{32}}{S}
q3=Sr23+r32
如果
r
11
,
r
22
,
r
33
r_{11},r_{22},r_{33}
r11,r22,r33中，
r
33
r_{33}
r33最大：<br>
S
=
1
−
r
11
−
r
22
+
r
33
S=\sqrt{1-r_{11}-r_{22}+r_{33}}
S=1−r11−r22+r33
<svg width="400em" height="1.08em" viewbox="0 0 400000 1080" preserveaspectratio="xMinYMin slice">
<path d="M95,702c-2.7,0,-7.17,-2.7,-13.5,-8c-5.8,-5.3,-9.5,
-10,-9.5,-14c0,-2,0.3,-3.3,1,-4c1.3,-2.7,23.83,-20.7,67.5,-54c44.2,-33.3,65.8, -50.3,66.5,-51c1.3,-1.3,3,-2,5,-2c4.7,0,8.7,3.3,12,10s173,378,173,378c0.7,0, 35.3,-71,104,-213c68.7,-142,137.5,-285,206.5,-429c69,-144,104.5,-217.7,106.5, -221c5.3,-9.3,12,-14,20,-14H400000v40H845.2724s-225.272,467,-225.272,467 s-235,486,-235,486c-2.7,4.7,-9,7,-19,7c-6,0,-10,-1,-12,-3s-194,-422,-194,-422 s-65,47,-65,47z M834 80H400000v40H845z"></path> </svg>
q
0
=
r
21
−
r
12
S
q_0 = \frac {r_{21}-r_{12}}{S}
q0=Sr21−r12
q
1
=
r
13
+
r
31
S
q_1 = \frac {r_{13}+r_{31}}{S}
q1=Sr13+r31
q
2
=
r
23
−
r
32
S
q_2 = \frac {r_{23}-r_{32}}{S}
q2=Sr23−r32
q
3
=
S
/
4
q_3 = S/4
q3=S/4
变换的逆
使用欧拉角表示时，必须颠倒三个旋转轴的顺序，同时对旋转角度取反。
使用旋转矩阵表示时，求矩阵的逆即可：
R
∗
=
R
−
1
R^*=R^{-1}
R∗=R−1。<br> 使用四元组表示时，考虑其物理意义，对后三位取反即可：
q
−
1
=
[
q
0
,
−
q
1
,
−
q
2
,
−
q
3
]
q^{-1}=[q_0,-q_1,-q_2,-q_3]
q−1=[q0,−q1,−q2,−q3]。
向量的叉乘
接下去讨论之前，需要先复习向量的叉乘。
三维空间中的一个点可以表示为向量
[
x
i
,
y
j
,
z
k
]
[xi,yj,zk]
[xi,yj,zk]，其中
i
,
j
,
k
i,j,k
i,j,k是三个坐标轴：
[
1
,
0
,
0
]
,
[
0
,
1
,
0
]
,
[
0
,
0
,
1
]
[1,0,0],[0,1,0],[0,0,1]
[1,0,0],[0,1,0],[0,0,1]。<br> 两个向量
a
,
b
a,b
a,b叉乘的结果是一个向量，其长度为
∣
a
∣
∣
b
∣
sin
θ
|a||b| \sin \theta
∣a∣∣b∣sinθ。
θ
\theta
θ表示从向量
a
a
a到
b
b
b的小于180°的角度。其方向垂直于
a
,
b
a,b
a,b所在平面，遵循右手定则：四指从
a
a
a转向
b
b
b，拇指方向为叉乘结果方向。
四元组作为一种向量，其叉乘涉及到虚数单位的乘法，遵循以下原则：
i
×
i
=
−
1
i\times i = -1
i×i=−1
i
×
j
=
−
j
×
i
i\times j = -j\times i
i×j=−j×i
i
×
j
=
k
,
j
×
k
=
i
,
k
×
i
=
j
i\times j = k, j\times k = i, k \times i = j
i×j=k,j×k=i,k×i=j
叉乘满足反交换律：
a
×
b
=
−
b
×
a
a\times b = -b\times a
a×b=−b×a<br> 联想叉乘的物理意义：交换了
a
,
b
a,b
a,b顺序，则拇指方向反转。
叉乘满足加法分配律：
(
a
+
b
)
×
c
=
a
×
c
+
b
×
c
(a+b)\times c = a\times c + b\times c
(a+b)×c=a×c+b×c
特别要注意，叉乘不满足结合律：
(
a
×
b
)
×
c
≠
a
×
(
b
×
c
)
(a\times b) \times c \neq a \times ( b\times c )
(a×b)×c=a×(b×c)
四元数的叉乘
四元数既不是矢量也不是标量。 可以看做一个标量
q
0
q_0
q0和一个三维矢量
[
q
1
,
q
2
,
q
3
]
[q_1,q_2,q_3]
[q1,q2,q3]的结合体：
q
=
q
0
+
q
1
i
+
q
2
j
+
q
3
k
q=q_0+q_1i+q_2j+q_3k
q=q0+q1i+q2j+q3k。<br> 两个四元数
a
1
+
a
2
i
+
a
3
j
+
a
4
k
a_1+a_2i+a_3j+a_4k
a1+a2i+a3j+a4k，
b
1
+
b
2
i
+
b
3
j
+
b
4
k
b_1+b_2i+b_3j+b_4k
b1+b2i+b3j+b4k，相乘的结果
q
=
a
×
b
q=a \times b
q=a×b：<br>
q
0
=
a
1
b
1
−
a
2
b
2
−
a
3
b
3
−
a
4
b
4
q_0 = a_1b_1-a_2b_2-a_3b_3-a_4b_4
q0=a1b1−a2b2−a3b3−a4b4
q
1
=
a
1
b
2
+
a
2
b
1
+
a
3
b
4
−
a
4
b
3
q_1 = a_1b_2+a_2b_1+a_3b_4-a_4b_3
q1=a1b2+a2b1+a3b4−a4b3
q
2
=
a
1
b
3
−
a
2
b
4
+
a
3
b
1
+
a
4
b
2
q_2 = a_1b_3-a_2b_4+a_3b_1+a_4b_2
q2=a1b3−a2b4+a3b1+a4b2
q
3
=
a
1
b
4
+
a
2
b
3
−
a
3
b
2
+
a
4
b
1
q_3 = a_1b_4+a_2b_3-a_3b_2+a_4b_1
q3=a1b4+a2b3−a3b2+a4b1
四元数叉乘不满足交换律、结合律。
变换的组合
使用欧拉角表示时，很难直接组合两个变换。
使用旋转矩阵表示时，依次做矩阵相乘即可：
R
=
R
2
R
1
R=R_2R_1
R=R2R1。<br> 使用四元组表示时，需要对两个四元组叉乘：
q
=
q
2
×
q
1
q=q_2 \times q_1
q=q2×q1。 由于叉乘不满足结合律，当有一系列变化
q
1
,
q
2
,
q
3
.
.
.
q_1,q_2,q_3...
q1,q2,q3...陆续发生时，要用括号保证乘法发生的顺序：<br>
q
=
q
4
×
(
q
3
×
(
q
2
×
q
1
)
)
q = q_4 \times (q_3\times (q_2\times q_1))
q=q4×(q3×(q2×q1))
使用四元数
首先把待旋转的点
(
x
,
y
,
z
)
(x,y,z)
(x,y,z)表示成四元数形式：
p
=
[
0
,
x
,
y
,
z
]
p=[0,x,y,z]
p=[0,x,y,z]。这里的四元数不再表示一个旋转，所以模长不一定为1。<br> 对该点
p
p
p施加旋转变换
q
q
q，通过四元组的两次叉乘实现：
p
′
=
q
×
p
×
q
−
1
=
[
0
,
x
′
,
y
′
,
z
′
]
p'= q \times p \times q^{-1} = [0,x',y',z']
p′=q×p×q−1=[0,x′,y′,z′]。
利用matlab的syms功能（参看附录），可以得到对三维点
[
x
,
y
,
z
]
[x,y,z]
[x,y,z]施加变换
[
q
0
,
q
1
,
q
2
,
q
3
]
[q_0,q_1,q_2,q_3]
[q0,q1,q2,q3]的结果：<br>
x
′
=
x
(
q
0
2
+
q
1
2
−
q
2
2
−
q
3
2
)
+
2
y
(
q
1
q
2
−
q
0
q
3
)
+
2
z
(
q
0
q
2
+
q
1
q
3
)
x' = x(q_0^2 + q_1^2 - q_2^2 - q_3^2) + 2y(q_1q_2 - q_0q_3)+ 2z(q_0q_2 + q_1q_3)
x′=x(q02+q12−q22−q32)+2y(q1q2−q0q3)+2z(q0q2+q1q3)
y
′
=
2
x
(
q
0
q
3
+
q
1
q
2
)
+
y
(
q
0
2
−
q
1
2
+
q
2
2
−
q
3
2
)
+
2
z
(
−
q
0
q
1
+
q
2
q
3
)
y'= 2x(q_0q_3+ q_1q_2)+y(q_0^2- q_1^2+ q_2^2- q_3^2) + 2z(-q_0q_1 + q_2q_3)
y′=2x(q0q3+q1q2)+y(q02−q12+q22−q32)+2z(−q0q1+q2q3)
z
′
=
2
x
(
−
q
0
q
2
+
q
1
q
3
)
+
2
y
(
q
0
q
1
+
q
2
q
3
)
+
z
(
q
0
2
−
q
1
2
−
q
2
2
+
q
3
2
)
z'=2x(-q_0q_2+ q_1q_3)+ 2y(q_0q_1+ q_2q_3)+z(q_0^2 - q_1^2 - q_2^2 + q_3^2)
z′=2x(−q0q2+q1q3)+2y(q0q1+q2q3)+z(q02−q12−q22+q32)
附录
以下代码推导对三维点
[
x
,
y
,
z
]
[x,y,z]
[x,y,z]<br> 施加变换
[
q
0
,
q
1
,
q
2
,
q
3
]
[q_0,q_1,q_2,q_3]
[q0,q1,q2,q3]的结果。
clear;clc;close all;
syms a1 a2 a3 a4 b1 b2 b3 b4
syms q0 q1 q2 q3
syms x y z
% a: rotation, b: 3D vector
a1 = q0;
a2 = q1;
a3 = q2;
a4 = q3;
b1 = 0;
b2 = x;
b3 = y;
b4 = z;
% c = a (*) b
c1 = a1*b1-a2*b2-a3*b3-a4*b4;
c2 = a1*b2+a2*b1+a3*b4-a4*b3;
c3 = a1*b3-a2*b4+a3*b1+a4*b2;
c4 = a1*b4+a2*b3-a3*b2+a4*b1;
% d = conj(a)
d1 = a1;
d2 = -a2;
d3 = -a3;
d4 = -a4;
% e = a (*) b (*) conj(a) = c (*) d
e1 = c1*d1-c2*d2-c3*d3-c4*d4;
e2 = c1*d2+c2*d1+c3*d4-c4*d3;
e3 = c1*d3-c2*d4+c3*d1+c4*d2;
e4 = c1*d4+c2*d3-c3*d2+c4*d1;
expand(e1)
expand(e2)
expand(e3)
expand(e4)
浙公网安备 33010602011771号