旋转表达之四元数

旋转表达之四元数

  旋转矩阵用9个元素表示3自由度旋转，表达具有冗余性。而欧拉角(后面会提及)和旋转向量（前文提到罗德里格斯的$t$）是紧凑的，但具有奇异性。

  旋转向量用一个旋转轴$\omega$和旋转角$t$来描述一个旋转，所以也称轴角(Axis-Angle)。不过很明显，因为旋转角度有一定的周期性（$2\pi$一圈），所以这种表达方式具有奇异性。

  欧拉角有一个致命缺点：万向锁。也就是在俯仰角为±90°时，第一次和第三次旋转使用的是同一个坐标轴，会丢失一个自由度，引起奇异性【后面会另开博文，说明欧拉角】。

  表达三维旋转的不带奇异性的三维向量描述方式是不存在的，它是一个三维流型，想要无奇异性的表示，仅用3个量是不够的，所以引出了四元数。

  四元数是Hamilton找到的对于复数的扩展，它由一个实部和三个虚部组成，是一种非常紧凑、没有奇异的表达方式。然而四元数不够直观，且运算较为复杂。

  推荐一个四元数的文档。内容极度充实，不过本文不需要太多的复数概念，看完本文还有不懂的，可以去阅读该文档。

  本博文若有缺少的部分没有概述，会找时间不断补充。

四元数

  我们知道复数$\mathbb{C}$可以简单地定义为$\mathbb{C}=\mathbb{R}+{\mathbb{R}}_i$，其中$i^2=-1$。 四元数是类似于复进行扩展和推广。 由$\mathbb{H}$表示的四元数集定义为

$$\mathbb{H}=\mathbb{C}+{\mathbb{C}}_j，j^2=-1;i\cdot j=-j\cdot i$$

  所以$\mathbb{H}$中元素的形式如下：

$$q=q_0+q_{1}i+(q_2+i{q}_3)j=q_0+q_{i}i+q_{2}j+q_{3}ij,q_0,q_1,q_2,q_3\in \mathbb{R}$$

  我们后续简化为

$$q=q_0+q_{i}i+q_{2}j+q_{3}k，,q_0,q_1,q_2,q_3\in \mathbb{R}$$

  其中$i,j,k$是四元数的三个虚部，而且$k=ij$。

  我们把三个虚部的乘法结果以下标展示：

$\times$	$1$	$i$	$j$	$k$
1	1	$i$	$j$	$k$
$i$	$i$	-1	$k$	$-j$
$j$	$j$	$-k$	-1	$i$
$k$	$k$	$j$	$-i$	-1

  其实还可以推出$i^2=j^2=k^2=ijk=-1$

  事实上，$i,j,k$正如下图所示，摘自维基百科。

四元数单位乘积在4D空间平面内旋转90°的图形表示，由$1，i，j，k$中的两个张成。 左侧因子可以被视为右侧旋转。 例如：蓝色：$1/i$平面：$1\cdot i=i$，$i/k$平面：$i\cdot j=k$红色：$1/j$平面：$1\cdot j=j$，$j/k$平面： $j\cdot i=-k$。

  有些时候，我们可以用标量和向量表达四元数：

$$q=[s,v],s=q_0\in \mathbb{R},v=[q_1,q_2,q_3]\in {\mathbb{R}}^3$$

  其中$s$是四元数的实部，而$v$是四元数的虚部。如果虚部为0，则称为实四元数。若实部为0则称为虚四元数。

四元数运算

  四元数和复数类似，故而存在有常见四则运算，数乘和求逆以及共轭等。

加减法

  假设有两个四元数，我们设$q=a_1+b_{1}i+c_{1}j+d_{1}k$,$p=a_2+b_{2}i+c_{2}j+d_{2}k$。当然我们还可以将其记为$\mathrm{q}=[s_1,v_1],\mathrm{p}=[s_2,v_2]$。其中$s_1=a_1,v_1=[b_1,c_1,d_1],s_2=a_2,v_2=[b_2,c_2,d_2]$

  所以有$\mathrm{q}=s_1+v_1\left[\begin{array}{c}i\\ j\\ k\end{array}\right],\mathrm{p}=s_2+v_2\left[\begin{array}{c}i\\ j\\ k\end{array}\right]$

  目前定义加法为：

$$\mathrm{p}\pm \mathrm{q}=(a_1\pm a_2)+(b_1\pm b_2)i+(c_1\pm c_2)j+(d_1\pm d_2)k$$

$$\iff \mathrm{p}\pm \mathrm{q}=(s_1\pm s_2)+(v_1\pm v_2)\left[\begin{array}{c}i\\ j\\ k\end{array}\right]$$

  所以我们标记$\mathrm{p}+\mathrm{q}=[s_1\pm s_1,v_1\pm v_2]$

乘法

标量乘法

  我们有一个四元数$q=a_1+b_{1}i+c_{1}j+d_{1}k$和一个标量$s$，则他们乘积为：

$$s\mathrm{q}=s(a+bi+cj+dk)=sa+sbi+scj+sdk$$

  而且标量乘法符合交换律，即$s\mathrm{q}=\mathrm{q}s$

四元数乘法

  四元数乘法并不符合交换律，这点后面会指出。

  假设$q=a_1+b_{1}i+c_{1}j+d_{1}k$,$p=a_2+b_{2}i+c_{2}j+d_{2}k$

  则两者乘积为

$$\begin{array}{rl}\mathrm{q}\mathrm{p}& =(a_1+b_{1}i+c_{1}j+d_{1}k)(a_2+b_{2}i+c_{2}j+d_{2}k)\\ & =a_1{a}_2+a_1{b}_{2}i+a_1{c}_{2}j+a_1{d}_{2}k+\\ & b_1{a}_{2}i+b_1{b}_2{i}^2+b_1{c}_{2}ij+b_1{d}_{2}ik+\\ & c_1{a}_{2}j+c_1{b}_{2}ji+c_1{c}_2{j}^2+c_1{d}_{2}jk+\\ & d_1{a}_{2}k+d_1{b}_{2}ki+d_1{c}_{2}kj+d_1{d}_2{k}^2\end{array}$$

  又上面给出的表可以知道

$i^2=j^2=k^2=-1$,$ij=k,ji=-k,ik=-j,ki=j,jk=i,kj=-i$

  所以上面可以换得

$$\begin{array}{rl}\mathrm{q}\mathrm{p}& =(a_1{a}_2-b_1{b}_2-c_1{c}_2-d_1{d}_2)\\ & +(a_1{b}_2+b_1{a}_2+c_1{d}_2-d_1{c}_2)i\\ & +(a_1{c}_2-b_1{d}_2+c_1{a}_2+d_1{b}_2)j\\ & +(a_1{d}_2+b_1{c}_2-c_1{b}_2+d_1{a}_2)k\end{array}$$

  两个四元数乘积相当于先旋转$a_2+b_{2}i+c_{2}j+d_{2}k$再旋转$a_1+b_{1}i+c_{1}j+d_{1}k$

  四元数相乘其实也算线性组合。我们可以将其写成下面形式

$$\mathrm{p}\mathrm{q}=\left[\begin{array}{cccc}{a}_1& -b_1& -c_1& -d_1\\ b_1& a_1& -d_1& c_1\\ c_1& d_1& a_1& -b_1\\ d_1& -c_1& b_1& a_1\end{array}\right]\left[\begin{array}{c}{a}_2\\ b_2\\ c_2\\ d_2\end{array}\right]\left[\begin{array}{c}1\\ i\\ j\\ k\end{array}\right]$$

  该变换相当于左乘$\mathrm{p}$，矩阵乘法不符合交换律。

  由此可以得到

$$\mathrm{q}\mathrm{p}=\left[\begin{array}{cccc}{a}_1& -b_1& -c_1& -d_1\\ b_1& a_1& d_1& -c_1\\ c_1& -d_1& a_1& b_1\\ d_1& c_1& -b_1& a_1\end{array}\right]\left[\begin{array}{c}{a}_2\\ b_2\\ c_2\\ d_2\end{array}\right]\left[\begin{array}{c}1\\ i\\ j\\ k\end{array}\right]$$

  如果换成标识为：

$\mathrm{q}=[s_1,v_1],\mathrm{p}=[s_2,v_2]$。其中$s_1=a_1,v_1=[b_1,c_1,d_1],s_2=a_2,v_2=[b_2,c_2,d_2]$

  很容易推算得到

$$\mathrm{q}\mathrm{p}=[s_1{s}_2-v_1^T{v}_2,s_1{v}_1+s_2{v}_2+v_1{v}_2]$$

  由此可见，如果要四元数相乘可交换，则除非$v_1,v_2$共线，此时外积项为0。

  这个标识的结果也被称为Graßmann积。

共轭

  定义一个四元数为$q=a+bi+cj+dk$，则其共轭为

$$\bar{\mathrm{q}}=a-bi-cj-dk$$

在文献中，共轭$\hat{q}$的符号还可以标记为${\mathrm{q}}^{\ast },{\mathrm{q}}^{\mathrm{t}}$等。博文采用$\bar{\mathrm{q}}$

  共轭是对合函数，也就是说它是它自己的反转，意味着$f(f(x))=x$。意味着一个元素共轭两次就会是原始元素。两个四元数乘积的共轭是他们的共轭相反顺序的乘积。也就是说:

$$\forall \mathrm{p},\mathrm{q}\in \mathbb{H},\overline{\mathrm{p}\mathrm{q}}=\bar{\mathrm{q}}\bar{\mathrm{p}}$$

  四元数的共轭还可以用四元数的乘法和加法表示：

$$\bar{\mathrm{q}}=-\frac{1}{2}(\mathrm{q}+i\mathrm{q}i+j\mathrm{q}j+k\mathrm{q}k)$$

  我们还可以用共轭去提取出四元数中的实部和虚部。即

$$a=\frac{\mathrm{q}+\bar{\mathrm{q}}}{2}$$

$$bi+cj+dk=\frac{\mathrm{q}-\bar{\mathrm{q}}}{2}$$

模与范数

  四元数和其共轭的乘积的平方根称为其范数，表示为$||\mathrm{q}||$，表示如下：

$$||\mathrm{q}||=\sqrt{\mathrm{q}\bar{\mathrm{q}}}=\sqrt{\bar{\mathrm{q}}\mathrm{q}}=\sqrt{a^2+b^2+c^2+d^2}$$

  该范数也被称为四元数的模，模为非负数，这与$\mathbb{H}$上的欧氏范数相同，被视为向量空间${\mathbb{R}}^4$。

  在共轭中，如果我们标记$\mathrm{q}=[s,v]$，则

$$\bar{q}=[s,-v]$$

  然后共轭乘积表示为：

$$\bar{q}q=q\bar{q}=[s^2+v^{T}v,0]$$

  将四元数乘以一个实数后的范数相当于这个四元数的范数乘以这个实数的绝对值。即若$\alpha$为实数，则

$$||\alpha \mathrm{q}||=|\alpha |||\mathrm{q}||$$

  这是下面范数乘法的特例：

$$||\mathrm{p}\mathrm{q}||=||\mathrm{p}||||\mathrm{q}||,\forall \mathrm{p},\mathrm{q}\in \mathbb{H}$$

  乘法是乘积共轭公式的一个结果。或者，它从恒等式开始

$$\det (\begin{array}{cc}a+ib& id+c\\ id-c& a-ib\end{array})=a^2+b^2+c^2+d^2$$

  其中$i$通常被认为是虚数单位。因此，因此形成了方阵行列式的乘法性质。

  下面这个范数可以用来定义$\mathrm{p}$和$\mathrm{q}$之间的距离$d(\mathrm{p},\mathrm{q})$,作为它们差异的范数：

$$d(\mathrm{p},\mathrm{q})=||\mathrm{p}-\mathrm{q}||$$

  这使$\mathbb{H}$成为度量空间。 度量拓扑中的加法和乘法是连续的。 实际上，对于任何标量，正数$a$使得

$$(\mathrm{p}+a{\mathrm{p}}_1+\mathrm{q}+a{\mathrm{q}}_1)-(\mathrm{p}+\mathrm{q})=a||{\mathrm{p}}_1+{\mathrm{q}}_1||$$

  连续性从极限值变为零。 乘法的连续性同样如此。

四元数的逆

  我们定义${\mathrm{q}}^{-1}$是$\mathrm{q}$的逆。而且满足

$${\mathrm{q}\mathrm{q}}^{-1}={\mathrm{q}}^{-1}\mathrm{q}=1(\mathrm{q}̸=0)$$

  也就是说：

$$(\mathrm{p}\mathrm{q}){\mathrm{q}}^{-1}=\mathrm{p}(\mathrm{q}{\mathrm{q}}^{-1})=\mathrm{p}\cdot 1=\mathrm{p}$$

$${\mathrm{q}}^{-1}(\mathrm{q}\mathrm{p})=({\mathrm{q}}^{-1}\mathrm{q})\mathrm{p}=1\cdot \mathrm{p}=\mathrm{p}$$

  这分别是右乘${\mathrm{q}}^{-1}$和左乘${\mathrm{p}}^{-1}$，与矩阵性质相似。

  那么如何求四元数的逆？

  这里我们引入单位四元数，这是范数的四元数。它是通过非零四元数$\mathrm{q}$除以其范数而得到的单位四元数${\mathrm{U}}_{\mathrm{q}}$,称为$\mathrm{q}$的规范化四元数(Versor)。

$${\mathrm{U}}_{\mathrm{q}}=\frac{\mathrm{q}}{||\mathrm{q}||}$$

  每个范数都有一个极分解$\mathrm{q}=||\mathrm{q}||\cdot {\mathrm{U}}_{\mathrm{q}}$。

  使用共轭和范数可以定义非零四元数的倒数。四元数和其倒数的乘积应该等于1。这里我们把倒数认为是四元数的逆。

  其实这就意味着$\mathrm{q}$和$\bar{\mathrm{q}}/||\mathrm{q}||$的乘积结果是1(不论顺序）。

回想我们定义四元数的模的时候的范数定义

  所以四元数的逆定义为：

$${\mathrm{q}}^{-1}=\frac{\bar{\mathrm{q}}}{||\mathrm{q}||}$$

  如果$\mathrm{q}$是单位四元数，那么${\mathrm{q}}^{-1}=\bar{\mathrm{q}}$。

  值得注意的是四元数乘积的逆和矩阵的逆相似性质，即：

$$(\mathrm{p}\mathrm{q}{)}^{-1}={\mathrm{q}}^{-1}{\mathrm{p}}^{-1}$$

四元数的数乘

  与向量类似，可以定义四元数的数乘。

  我们标记四元数$\mathrm{q}=[s,v]$，则数乘：

$$k\mathrm{q}=k[s,v]$$

四元数的点乘（点积）

  点乘指的是四元数的每个位置上的数值对应相乘。

  假设$q=a_1+b_{1}i+c_{1}j+d_{1}k$,$p=a_2+b_{2}i+c_{2}j+d_{2}k$

  则二者点乘的结果为：

$$\mathrm{p}\cdot \mathrm{q}=a_1{a}_2+b_1{b}_2+c_1{c}_2+d_1{d}_2$$

  还可以不用分量的形式表示为：

$$\mathrm{p}\cdot \mathrm{q}=\frac{1}{2}(\bar{\mathrm{p}}\mathrm{q}+\bar{\mathrm{q}}\mathrm{p})=\frac{1}{2}(\mathrm{p}\bar{\mathrm{q}}+\mathrm{q}\bar{\mathrm{p}})$$

可以验证计算成立

四元数的叉积

  假设$q=a_1+b_{1}i+c_{1}j+d_{1}k$,$p=a_2+b_{2}i+c_{2}j+d_{2}k$

  则

$$\mathrm{p}\times \mathrm{q}=\frac{1}{2}(\bar{\mathrm{p}}\mathrm{q}-\bar{\mathrm{q}}\mathrm{p})=\frac{1}{2}(\mathrm{p}\bar{\mathrm{q}}-\mathrm{q}\bar{\mathrm{p}})$$

  如果我们标记$\mathrm{q}=[s_1,v_1],\mathrm{p}=[s_2,v_2]$，则

$$p\times q=[0,v_1\times v_2]$$

  具体来说，叉积后实部为0。

$$\begin{array}{rl}p\times q& =(c_1{d}_2-d_1{c}_2)i\\ & +(d_1{b}_2-b_1{d}_2)j\\ & +(b_1{c}_2-c_1{b}_2)k\end{array}$$

四元数指数，对数和幂函数

  给定四元数$\mathrm{q}=a+bi+cj+dk=a+\vec{v}$

  则

$$\exp (\mathrm{q})=\sum _{\mathrm{n}=0}^{\infty }\frac{{\mathrm{q}}^{\mathrm{n}}}{\mathrm{n}!}={\mathrm{e}}^{\mathrm{a}}\left(\cos ||\vec{\mathrm{v}}||+\frac{\vec{\mathrm{v}}}{||\vec{\mathrm{v}}||}\sin ||\vec{\mathrm{v}}||\right)$$

$$\ln (\mathrm{q})=\ln ||\mathrm{q}||+\frac{\vec{v}}{||\vec{v}||}\arccos \frac{a}{||\mathrm{q}||}$$

  由此可以写成四元数的极分解

$$\mathrm{q}=||\mathrm{q}||e^{\hat{n}\theta }=||\mathrm{q}||(\cos \theta +\hat{n}\sin \theta )$$

  其中角度为$\theta$，单位矢量$\hat{n}$定义为

$$a=||\mathrm{q}||\cos \theta$$

  和

$$\vec{v}=\hat{n}||\vec{v}||=\hat{n}||\mathrm{q}||\sin \theta$$

  任何四元数都可以由极形式表示为$e^{\hat{n}\theta }$。

  四元数的幂提到任意（实）指数由下式给出：

$${\mathrm{q}}^{\alpha }=||\mathrm{q}|{|}^{\alpha }(\cos (\alpha \theta )+\hat{n}\sin (\alpha \theta ))$$

  四元数的其他函数则不描述（有需要则评论中人提出再添加）。

---

四元数表示旋转

  四元数用于旋转时被称为旋转四元数，当用于方向（相对于参考坐标系的旋转）时，被称为方向四元数或位姿四元数。

  在三维空间中，根据欧拉旋转定理，刚体或坐标系围绕固定点的任何旋转或旋转顺序相当于围绕固定轴（称为欧拉轴）的给定角度$\theta$的单一旋转。 贯穿固定点。 欧拉轴通常由单位矢量$\vec{u}$表示。 因此，三维中的任何旋转都可以表示为矢量$\vec{u}$和标量$\theta$的组合。

图来源于维基百科。对于欧拉轴和角度的可视化旋转表示。

  四元数提供了一种简单的方法来将此轴角表示为四个数字，并且可用于将相应的旋转应用于位置向量，表示相对于${\mathbb{R}}^3$中的原点的点。

  四元数要表示旋转，则需要用到虚四元数，即实部为0的四元数。

  用$i,j,k$分别为笛卡尔坐标系的三个轴的单位向量。把旋转定义为绕着以某个单位向量$\vec{u}$为轴旋转$\theta$角。

$$\vec{u}=(u_x,u_y,u_z)=u_{x}i+u_{y}j+u_{z}k$$

  可以被四元数描述。这就可以用于欧拉公式的扩展：

$$\mathrm{q}=e^{\frac{\theta }{2}(u_{x}i+u_{y}j+u_{z}k)}=\cos \frac{\theta }{2}+(u_{x}i+u_{y}j+u_{z}k)\sin \frac{\theta }{2}$$

  所以得到:

$$\mathrm{q}=[q_0,q_1,q_2,q_3]=[\cos \frac{\theta }{2},u_x\sin \frac{\theta }{2},u_y\sin \frac{\theta }{2},u_z\sin \frac{\theta }{2}]$$

  在这里我们给出由此公式得到了旋转向量和四元数的转换关系式：

$$\{\begin{array}{l}\theta =2\arccos (q_0)\\ [u_x,u_y,u_z{]}^T=[q_1,q_2,q_3{]}^T/\sin \frac{\theta }{2}\end{array}$$

  我们知道，一个点，比如点$p$，如果用旋转矩阵来表示旋转，则为

$$p^{\prime }=Rp$$

  现在这个点被用虚四元数描述。即$p=[0,x,y,z]=[0,v]$

  然后我们用四元数$\mathrm{q}=[\cos \frac{\theta }{2},v\sin \frac{\theta }{2}]$表示这个旋转。

  采用Hamilton积来表示：

$$p^{\prime }=\mathrm{q}p{\mathrm{q}}^{-1}$$

  可以验证得出，计算结果的实部为0，是纯虚四元数。

四元数与旋转矩阵的转换

  由四元数旋转$p^{\prime }=\mathrm{q}p{\mathrm{q}}^{-1}$,我们假设$\mathrm{q}=q_r+q_{i}i{q}_{j}j+q_{k}k$,可以得到旋转矩阵$R$。

$$\mathbf{R}=\left[\begin{array}{ccc}1-2s(q_j^2+q_k^2)& 2s(q_i{q}_j-q_k{q}_r)& 2s(q_i{q}_k+q_j{q}_r)\\ 2s(q_i{q}_j+q_k{q}_r)& 1-2s(q_i^2+q_k^2)& 2s(q_j{q}_k-q_i{q}_r)\\ 2s(q_i{q}_k-q_j{q}_r)& 2s(q_j{q}_k+q_i{q}_r)& 1-2s(q_i^2+q_j^2)\end{array}\right]$$

  这里$s=||q|{|}^{-2}$，如果$\mathrm{q}$是单位四元数，则$s=1$。

  如果从旋转矩阵到四元数，则是：

  假设矩阵$R=m_{ij}，i,j\in [1,2,3]$,$\mathrm{q}$是单位四元数，则对应四元数的各个分量为：

$q_r=\frac{\sqrt{\mathrm{t}\mathrm{r}\mathrm{a}\mathrm{c}\mathrm{e}(R)+1}}{2}$，$q_i=\frac{m_{23}-m_{32}}{4q_r}$,$q_j=\frac{m_{31}-m_{13}}{4q_r}$,$q_k=\frac{m_{12}-m_{21}}{4q_r}$

   值得注意的是$\mathrm{q},-\mathrm{q}$是同一旋转，即$R$对应的四元数并不唯一。

  当$q_r$接近于0，$q_i,q_j,q_k$会变得十分大，导致解不稳定。

四元数恢复轴角表示

  若已知旋转四元数为$\mathrm{q}=q_r+q_{i}i{q}_{j}j+q_{k}k$，则旋转轴$u$和旋转角$\theta$为：

$$(u_x,u_y,u_z)=\frac{(q_i,q_j,q_k)}{\sqrt{q_i^2+q_j^2+q_k^2}}$$

$$\theta =2\mathrm{a}\mathrm{t}\mathrm{a}\mathrm{n}2(\sqrt{q_i^2+q_j^2+q_k^2},q_r)$$

  其中$\mathrm{a}\mathrm{t}\mathrm{a}\mathrm{n}2$是双参数反正切函数。

---

四元数的特殊子群

  特殊的$\mathbb{H}$子群，即所谓的单位四元数（前文已经描述）

S 3 = { q ∈ H ∣ ∣ ∣ q ∣ ∣ 2 = q 0 2 + q 1 2 + q 2 2 + q 3 2 = 1 } \mathbb{S}^3=\{q\in \mathbb{H}| ||q||^2=q_0^2+q_1^2+q_2^2+q_3^2=1\} S3={q∈H∣∣∣q∣∣2=q02​+q12​+q22​+q32​=1}

  很明显，所有单位四元数的集合只是${\mathbb{R}}^4$中的单位球。很容易证明该单位四元数符合群的性质。

  刚刚我们说过，对于$\mathrm{q},-\mathrm{q}$是同一旋转，即$R$对应的四元数并不唯一。

由于欧拉公式展开可以知道，当四元数取复数时，$\theta$和$\vec{v}$对应反向，而乘积不变。

  可以由下图可以看出。

单位球面${\mathbb{S}}^3\subset {\mathbb{R}}^4$上的对应单位四元数$\mathrm{q}$和$-\mathrm{q}$对应于相同的旋转矩阵。

  因此，在拓扑学中，${\mathbb{S}}^3$是$SO(3)$的双覆盖(double-covering)。因此$SO(3)$在拓扑上与三维投影平面${\mathbb{R}\mathbb{P}}^3$相同。

​

​