【机器人】四元数与旋转矩阵的转换关系

导语：最近在搭建电机与机械臂的联合仿真，闲暇之余顺便看了一下姿态轨迹规划的文章，对姿态的插补有点感兴趣，发现用四元数表示姿态然后来进行姿态的插补非常方便，且不会出现奇异。这里对四元数与常规的旋转矩阵的转换做一下总结。

四元数概念

四元数由实部和虚部组成，设 qqq 为四元数，则可表述为 q=s+xi+yj+zkq=s+xi+yj+zkq=s+xi+yj+zk ，其中 sss 为实部， ijkijkijk 为虚单位，满足 i2=j2=k2=ijk=−1i^2=j^2=k^2=ijk=-1i2=j2=k2=ijk=−1 。四元数的模计算公式∣q∣=w2+x2+y2+z2\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] ，其中 sss 为实部，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]它表示任意向量 rrr 绕轴 vvv 旋转角度 θ\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=[s1,u1⃗]q=\left[ s_1,\vec{u_1} \right]q=[s1,u1] 和 q=[s2,u2⃗]q=\left[ s_2,\vec{u_2} \right]q=[s2,u2]，则有 q1q2=[s1s2−u⃗1∗u⃗2,u⃗1×u⃗2+s1u⃗2+s2u⃗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] ,其中*和×分别表示三维向量的内积和外积。并且四元数的乘法是不可交换的。
数乘：对于 qqq 为四元数，rrr 为实数，则数乘定义为 rq=[r,0]q=[rs,rv⃗]rq=\left[ r,0 \right] q=\left[ rs,r\vec{v} \right]rq=[r,0]q=[rs,rv]。

四元数转换为旋转矩阵

一个绕轴 vvv 旋转角度 θθθ 的操作可以用单位四元数 q=[q1,q2,q3,q4]=[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(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]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⎦⎤

旋转矩阵转换为四元数

有旋转矩阵 iRj=[snα]=[sxnxαxsynyαysznzα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=[q1,q2,q3,q4]q=\left[ q_1, q_2, q_3, q_4 \right]q=[q1,q2,q3,q4]

q1=12sx+ηy+αz+1q_1=\frac{1}{2}\sqrt{s_x+\eta _y+\alpha _z+1} q1=21sx+ηy+αz+1

q2=12sign(nz−αy)sx−ηy−αz+1q_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

q3=12sign(αx−sz)−sx+ηy−αz+1q_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

q4=12sign(sy−nx)−sx−ηy+αz+1q_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