【机器人学】四元数与旋转矩阵的相互转换

1. 从四元数到旋转矩阵

推导过程利用以下矩阵（该矩阵的推导过程链接），它能计算绕任意轴的旋转：
$$\text{R}(\text{n},\theta )=\left[\begin{array}{ccc}(1-cos\theta )n_x^2+cos\theta & (1-cos\theta )n_x{n}_y-n_{z}sin\theta & (1-cos\theta )n_x{n}_z+n_{y}sin\theta \\ (1-cos\theta )n_x{n}_y+n_{z}sin\theta & (1-cos\theta )n_y^2+cos\theta & (1-cos\theta )n_y{n}_z-n_{x}sin\theta \\ (1-cos\theta )n_x{n}_z-n_{y}sin\theta & (1-cos\theta )n_y{n}_z+n_{x}sin\theta & (1-cos\theta )n_z^2+cos\theta \end{array}\right]$$
该矩阵用 $\text{n}$ 和 $\theta$ 表示，但其中四元数的分量是：
$$\begin{array}{rl}w& =cos(\theta /2)\\ x& ={\text{n}}_{x}sin(\theta /2)\\ y& ={\text{n}}_{y}sin(\theta /2)\\ z& ={\text{n}}_{z}sin(\theta /2)\end{array}$$
将矩阵变形以带入$w,x,y,z$。首先考虑矩阵对角线上的元素：
$$\begin{array}{rl}{m}_{11}& ={\text{n}}_x^2(1-cos\theta )+cos\theta \\ & ={\text{n}}_x^2-{\text{n}}_x^{2}cos\theta +cos\theta \\ & =1-1+{\text{n}}_x^2-{\text{n}}_x^{2}cos\theta +cos\theta \\ & =1-(1-{\text{n}}_x^2+{\text{n}}_x^{2}cos\theta -cos\theta )\\ & =1-(1-cos\theta -{\text{n}}_x^2+{\text{n}}_x^{2}cos\theta )\\ & =1-(1-{\text{n}}_x^2)(1-cos\theta )\end{array}$$
根据三角函数倍角公式有$cos\theta =1-2si{n}^2(\theta /2)$，带入上式：
$$\begin{array}{rl}{m}_{11}& =1-(1-{\text{n}}_x^2)(1-cos\theta )\\ & =1-(1-{\text{n}}_x^2)(2si{n}^2(\theta /2))\\ & =1-2si{n}^2(\theta /2)+2{\text{n}}_x^{2}si{n}^2(\theta /2)\\ & =1-2(1-co{s}^2(\theta /2))+2{\text{n}}_x^{2}si{n}^2(\theta /2)\\ & =1-2+2co{s}^2(\theta /2))+2{\text{n}}_x^{2}si{n}^2(\theta /2)\\ & =-1+2co{s}^2(\theta /2))+2{\text{n}}_x^{2}si{n}^2(\theta /2)\end{array}$$
带入$w,x$即可求得，$m_{11}=-1+2w^2+2x^2$，等价于$$m_{11}=1-2y^2-2z^2$$$$m_{11}=w^2+x^2-y^2-z^2$$
对角元素$m_{22},m_{33}$可以用同样的方式求得。

以$m_{12}$为例，使用三角形的倍角公式 $sin\theta =2sin(\theta /2)cos(\theta /2)$ 推导非对角线元素，其他非对角线元素可用类似方式推导：
$$\begin{array}{rl}{m}_{12}& ={\text{n}}_x{\text{n}}_y(1-cos\theta )-{\text{n}}_{z}sin\theta \\ & ={\text{n}}_x{\text{n}}_y(1-(1-2^{2}sin(\theta /2)))-{\text{n}}_{z}2sin(\theta /2)cos(\theta /2)\\ & ={\text{n}}_x{\text{n}}_y(2si{n}^2(\theta /2))-2{\text{n}}_{z}sin(\theta /2)cos(\theta /2)\\ & =2({\text{n}}_{x}sin(\theta /2){\text{n}}_{y}sin(\theta /2))-2cos(\theta /2)({\text{n}}_{z}sin(\theta /2))\\ & =2xy-2wz\end{array}$$
最终推导出的四元数构造的完整旋转矩阵为以下形式：
$$R(n,\theta )=\left[\begin{array}{ccc}1-2y^2-2z^2& 2xy-2wz& 2xz+2wy\\ 2xy+2wz& 1-2x^2-2z^2& 2yz-2wx\\ 2xz-2wy& 2yz+2wx& 1-2x^2-2y^2\end{array}\right]\text{\hspace{1em}(2)}$$

2. 从旋转矩阵到四元数

利用公式(2)，将对角线元素求和可以得到：
$$\begin{array}{rl}tr(\text{M})& =m11+m22+m33\\ & =(1-2y^2-2z^2)+(1-2x^2-2z^2)+(1-2x^2-2y^2)\\ & =3-4(x^2+y^2+z^2)\\ & =3-4(1-w^2)\\ & =4w^2-1\end{array}$$
因此可以推导出：$$w=\frac{\sqrt{m11+m22+m33+1}}{2}$$
通过使轨迹中三个元素中的两个为负，可以用类似的方法求得其他三个元素：
$$\begin{array}{rl}m11-m22-m33& =(1-2y^2-2z^2)-(1-2x^2-2z^2)-(1-2x^2-2y^2)\\ & =4x^2-1\\ -m11+m22-m33& =-(1-2y^2-2z^2)+(1-2x^2-2z^2)-(1-2x^2-2y^2)\\ & =4y^2-1\\ -m11-m22+m33& =-(1-2y^2-2z^2)-(1-2x^2-2z^2)+(1-2x^2-2y^2)\\ & =4z^2-1\end{array}$$
这种方法是存在一定问题的：平方根的结果总是正值（没有选择是负根还是正根的依据）。$q$和$-q$代表相同的方位，因此我们可以任意选择用非负根作为4个分量中的一个并仍能得到正确的四元数，只是不能对四元数的所有4个数都用这种方法。

可以考虑计算相对于对角线的对称位置上元素的和与差：
$$\begin{array}{rl}m21+m12& =(2xy+2wz)+(2xy-2wz)=4xy\\ m21-m12& =(2xy+2wz)-(2xy-2wz)=4wz\\ m13+m31& =(2xz+2wy)+(2xz-2wy)=4xz\\ m13-m31& =(2xz+2wy)-(2xz-2wy)=4wy\\ m32+m23& =(2yz+2wx)+(2yz-2wx)=4yz\\ m32-m23& =(2yz+2wx)-(2yz-2wx)=4wx\end{array}$$
因此，一旦用对角线元素和/差的平方根解得了4个值中的一个，就能用以下方法计算其他三个：
$$\begin{array}{rl}w=\frac{\sqrt{m11+m22+m33+1}}{2}⟹& x=\frac{m32-m23}{4w}y=\frac{m13-m31}{4w}z=\frac{m21-m12}{4w}\\ x=\frac{\sqrt{m11-m22-m33+1}}{2}⟹& w=\frac{m32-m23}{4x}y=\frac{m21+m12}{4x}z=\frac{m13+m31}{4x}\\ y=\frac{\sqrt{-m11+m22-m33+1}}{2}⟹& w=\frac{m13-m31}{4y}x=\frac{m12+m21}{4y}z=\frac{m23+m32}{4y}\\ z=\frac{\sqrt{-m11-m22+m33+1}}{2}⟹& w=\frac{m21-m12}{4z}x=\frac{m13+m31}{4z}y=\frac{m23+m32}{4z}\end{array}$$
我们可以先计算四个分量中的其中一个，比如$w$，然后在计算$x,y,z$。这样也会出现问题，如果$w=0$，除法就没有意义了，如果$w$非常小，将会出现数值不稳定。所以我们可以采取的策略是，先判断$w,x,y,z$中哪个元素最大，用矩阵对角线计算该元素，再计算其他三个。