导语：最近在搭建电机与机械臂的联合仿真，闲暇之余顺便看了一下姿态轨迹规划的文章，对姿态的插补有点感兴趣，发现用四元数表示姿态然后来进行姿态的插补非常方便，且不会出现奇异。这里对四元数与常规的旋转矩阵的转换做一下总结。
四元数概念
四元数由实部和虚部组成，设
q
q
q 为四元数，则可表述为
q
=
s
+
x
i
+
y
j
+
z
k
q=s+xi+yj+zk
q=s+xi+yj+zk ，其中
s
s
s 为实部，
i
j
k
ijk
ijk 为虚单位，满足
i
2
=
j
2
=
k
2
=
i
j
k
=
−
1
i^2=j^2=k^2=ijk=-1
i2=j2=k2=ijk=−1 。四元数的模计算公式
∣
q
∣
=
w
2
+
x
2
+
y
2
+
z
2
\left| q \right|=\sqrt{w^2+x^2+y^2+z^2}
∣q∣=w2+x2+y2+z2当模等于1时，则为单位四元数，四元数也可写成向量形式
q
=
[
s
,
u
⃗
]
q=\left[ s,\vec{u} \right]
q=[s,u] ，其中
s
s
s 为实部，
u
⃗
=
[
x
,
y
,
z
]
\vec{u}=\left[ x,y,z \right]
u=[x,y,z] 为三个实数构成的虚部向量。单位四元数又可表示为
q
=
[
cos
(
θ
/
2
)
,
sin
(
θ
/
2
)
∗
v
]
q=\left[ \cos \left( \theta /2 \right) ,\sin \left( \theta /2 \right) *v \right]
q=[cos(θ/2),sin(θ/2)∗v]它表示任意向量
r
r
r 绕轴
v
v
v 旋转角度
θ
\theta
θ 得到的四元数。
四元数的运算
共轭：设
q
=
[
s
,
u
⃗
]
q=\left[ s,\vec{u} \right]
q=[s,u] 为四元数，则其共轭四元数为
q
∗
=
[
s
,
−
u
⃗
]
q^*=\left[ s,-\vec{u} \right]
q∗=[s,−u]。
逆：单位四元数的逆
q
−
1
=
q
∗
q^{-1}=q^*
q−1=q∗ 。
乘法：对应四元数
q
=
[
s
1
,
u
1
⃗
]
q=\left[ s_1,\vec{u_1} \right]
q=[s1,u1] 和
q
=
[
s
2
,
u
2
⃗
]
q=\left[ s_2,\vec{u_2} \right]
q=[s2,u2]，则有
q
1
q
2
=
[
s
1
s
2
−
u
⃗
1
∗
u
⃗
2
,
u
⃗
1
×
u
⃗
2
+
s
1
u
⃗
2
+
s
2
u
⃗
1
]
q_1q_2=\left[ s_1s_2-\vec{u}_1*\vec{u}_2,\vec{u}_1\times \vec{u}_2+s_1\vec{u}_2+s_2\vec{u}_1 \right]
q1q2=[s1s2−u1∗u2,u1×u2+s1u2+s2u1] ,其中*和×分别表示三维向量的内积和外积。并且四元数的乘法是不可交换的。
数乘：对于
q
q
q 为四元数，
r
r
r 为实数，则数乘定义为
r
q
=
[
r
,
0
]
q
=
[
r
s
,
r
v
⃗
]
rq=\left[ r,0 \right] q=\left[ rs,r\vec{v} \right]
rq=[r,0]q=[rs,rv]。
四元数转换为旋转矩阵
一个绕轴 v v v 旋转角度 θ θ θ 的操作可以用单位四元数 q = [ q 1 , q 2 , q 3 , q 4 ] = [ s , x , y , z ] = [ cos ( θ / 2 ) , sin ( θ / 2 ) ∗ ν ] q=\left[ q_1, q_2, q_3, q_4 \right] =\left[ s,x,y,z \right] =\left[ \cos \left( \theta /2 \right) ,\sin \left( \theta /2 \right) *\nu \right] q=[q1,q2,q3,q4]=[s,x,y,z]=[cos(θ/2),sin(θ/2)∗ν] 进行表示，其中 θ ∈ [ 0 , π ] \theta \in \left[ 0,\pi \right] θ∈[0,π]。对应旋转矩阵为 R ( q ) = [ 2 ( q 1 2 + q 2 2 ) − 1 2 ( q 2 q 3 − q 1 q 4 ) 2 ( q 2 q 4 + q 1 q 3 ) 2 ( q 2 q 3 + q 1 q 4 ) 2 ( q 1 2 + q 3 2 ) − 1 2 ( q 3 q 4 − q 1 q 2 ) 2 ( q 2 q 4 − q 1 q 3 ) 2 ( q 3 q 4 + q 1 q 2 ) 2 ( q 1 2 + q 4 2 ) − 1 ] R\left( q \right) =\left[ \begin{matrix} 2\left( q_{1}^{2}+q_{2}^{2} \right) -1& 2\left( q_2q_3-q_1q_4 \right)& 2\left( q_2q_4+q_1q_3 \right)\\ 2\left( q_2q_3+q_1q_4 \right)& 2\left( q_{1}^{2}+q_{3}^{2} \right) -1& 2\left( q_3q_4-q_1q_2 \right)\\ 2\left( q_2q_4-q_1q_3 \right)& 2\left( q_3q_4+q_1q_2 \right)& 2\left( q_{1}^{2}+q_{4}^{2} \right) -1\\ \end{matrix} \right] R(q)= 2(q12+q22)−12(q2q3+q1q4)2(q2q4−q1q3)2(q2q3−q1q4)2(q12+q32)−12(q3q4+q1q2)2(q2q4+q1q3)2(q3q4−q1q2)2(q12+q42)−1
旋转矩阵转换为四元数
有旋转矩阵 i R j = [ s n α ] = [ s x n x α x s y n y α y s z n z α z ] ^iR_j=\left[ \begin{matrix} s& n& \alpha\\ \end{matrix} \right] =\left[ \begin{matrix} s_x& n_x& \alpha _x\\ s_y& n_y& \alpha _y\\ s_z& n_z& \alpha _z\\ \end{matrix} \right] iRj=[snα]= sxsysznxnynzαxαyαz ， 转换为单位四元数 q = [ q 1 , q 2 , q 3 , q 4 ] q=\left[ q_1, q_2, q_3, q_4 \right] q=[q1,q2,q3,q4]
q 1 = 1 2 s x + η y + α z + 1 q_1=\frac{1}{2}\sqrt{s_x+\eta _y+\alpha _z+1} q1=21sx+ηy+αz+1
q 2 = 1 2 s i g n ( n z − α y ) s x − η y − α z + 1 q_2=\frac{1}{2}sign\left( n_z-\alpha_y \right) \sqrt{s_x-\eta _y-\alpha _z+1} q2=21sign(nz−αy)sx−ηy−αz+1
q 3 = 1 2 s i g n ( α x − s z ) − s x + η y − α z + 1 q_3=\frac{1}{2}sign\left( \alpha_x-s_z \right) \sqrt{-s_x+\eta _y-\alpha _z+1} q3=21sign(αx−sz)−sx+ηy−αz+1
q 4 = 1 2 s i g n ( s y − n x ) − s x − η y + α z + 1 q_4=\frac{1}{2}sign\left( s_y-n_x \right) \sqrt{-s_x-\eta _y+\alpha _z+1} q4=21sign(sy−nx)−sx−ηy+αz+1
588
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
