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

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

四元数概念

  四元数由实部和虚部组成，设 $q$ 为四元数，则可表述为 $q=w+xi+yj+zk$ ，其中 $w$ 为实部， $ijk$ 为虚单位，满足 $i^2=j^2=k^2=ijk=-1$ 。四元数的模计算公式$$\left|q\right|=\sqrt{w^2+x^2+y^2+z^2}$$当模等于1时，则为单位四元数，四元数也可写成向量形式 $q=\left[w,\vec{u}\right]$ ，其中 $w$ 为实部，$\vec{u}=\left[x,y,z\right]$ 为三个实数构成的虚部向量。单位四元数又可表示为轴对角的形式
$$q=\left[\cos \left(\theta /2\right),\sin \left(\theta /2\right)\ast v\right]$$它表示任意向量 $r$ 绕轴 $v$ 旋转角度 $\theta$ 得到的四元数。

四元数的运算

共轭：设 $q=\left[w,\vec{u}\right]$ 为四元数，则其共轭四元数为 $q^{\ast }=\left[w,-\vec{u}\right]$。
逆：单位四元数的逆 $q^{-1}=q^{\ast }$ 。
乘法：对应四元数 $q=\left[w_1,\overrightarrow{u_1}\right]$ 和 $q=\left[w_2,\overrightarrow{u_2}\right]$，则有 $q_1{q}_2=\left[w_1{w}_2-{\vec{u}}_1\ast {\vec{u}}_2,{\vec{u}}_1\times {\vec{u}}_2+w_1{\vec{u}}_2+w_2{\vec{u}}_1\right]$ ,其中*和×分别表示三维向量的内积和外积。并且四元数的乘法是不可交换的。
数乘：对于 $q$ 为四元数，$r$ 为实数，则数乘定义为 $rq=\left[r,0\right]q=\left[rw,r\vec{u}\right]$。

四元数转换为旋转矩阵

  一个绕轴 $v$ 旋转角度 $\theta$ 的操作可以用单位四元数 $q=\left[w,x,y,z\right]=\left[\cos \left(\theta /2\right),\sin \left(\theta /2\right)\ast \nu \right]$ 进行表示，其中 $\theta \in \left[0,\pi \right]$。对应旋转矩阵为 $$R\left(q\right)=\left[\begin{array}{ccc}1-2y^2-2z^2& 2xy-2zw& 2xz+2yw\\ 2xy+2zw& 1-2x^2-2z^2& 2yz-2xw\\ 2xz-2yw& 2yz+2xw& 1-2x^2-2y^2\end{array}\right]$$

旋转矩阵转换为四元数

有旋转矩阵 $${}^i{R}_j=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$$ ， 转换为单位四元数$q=\left[w,x,y,z\right]$

$$w=\frac{\sqrt{r_{11}+r_{22}+r_{33}+1}}{2}$$

$$x=\frac{r_{32}-r_{23}}{4w}$$

$$y=\frac{r_{13}-r_{31}}{4w}$$

$$z=\frac{r_{21}-r_{12}}{4w}$$