视觉SLAM_04_四元数

说明：转发请注明作者和出处；以下文章内容为精简版，详细版以pdf格式的幻灯片形式呈现，请见链接：视觉SLAM_04_四元数.pdf

0.目录

1.四元数的定义

  旋转矩阵用9个量描述3个自由度的旋转，具有冗余性；欧拉角和旋转向量是紧凑的，但具有奇异性。事实上，找不到不带奇异性的三维向量描述方式。

  四元数是Hamilton找到的一种扩展的复数，优点是紧凑且没有奇异性，缺点是不够直观、运算复杂。

  一个四元数$\mathbf{q}$拥有一个实部和三个虚部：
$$\mathbf{q}=q_0+q_1\mathbf{i}+q_2\mathbf{j}+q_3\mathbf{k}\text{\hspace{1em}(1)}$$其中，$\mathbf{i}$，$\mathbf{j}$，$\mathbf{k}$为四元数的三个虚部，并且满足如下关系：
$$\{\begin{array}{ll}{\mathbf{i}}^2={\mathbf{j}}^2={\mathbf{k}}^2=-1& \\ \mathbf{i}\mathbf{j}=\mathbf{k},\mathbf{j}\mathbf{i}=-\mathbf{k}& \\ \mathbf{j}\mathbf{k}=\mathbf{i},\mathbf{k}\mathbf{j}=-\mathbf{i}& \\ \mathbf{k}\mathbf{i}=\mathbf{j},\mathbf{i}\mathbf{k}=-\mathbf{j}& \end{array}\text{\hspace{1em}(2)}$$

  也可以用一个标量和一个向量来表达四元数：
$$\mathbf{q}=[s,\mathbf{v}{]}^T$$
其中，$s=q_0\in \mathbb{R}$称为四元数的实部，$\mathbf{v}=[q_1,q_2,q_3{]}^T\in {\mathbb{R}}^3$称为四元数的虚部。如果一个四元数的虚部为0，则称为实四元数；如果一个四元数的实部为0，则称虚四元数。

2.四元数的运算

  设有两个四元数：
$${\mathbf{q}}_{\mathbf{a}}=s_a+x_a\mathbf{i}+y_a\mathbf{j}+z_a\mathbf{k}=[s_a,{\mathbf{v}}_{\mathbf{a}}{]}^T,{\mathbf{q}}_{\mathbf{b}}=s_b+x_b\mathbf{i}+y_b\mathbf{j}+z_b\mathbf{k}=[s_b,{\mathbf{v}}_{\mathbf{b}}{]}^T$$
1.加减法
$${\mathbf{q}}_{\mathbf{a}}\pm {\mathbf{q}}_{\mathbf{b}}=[s_a\pm s_b,{\mathbf{v}}_{\mathbf{a}}\pm {\mathbf{v}}_{\mathbf{b}}{]}^T\text{\hspace{1em}(3)}$$

2.乘法
  乘法是把${\mathbf{q}}_{\mathbf{a}}$的每一项与${\mathbf{q}}_{\mathbf{b}}$的每一项相乘，最后相加，虚部要按照(2)式进行。
$$\begin{array}{r}{\mathbf{q}}_{\mathbf{a}}{\mathbf{q}}_{\mathbf{b}}=s_a{s}_b-x_a{x}_b-y_a{y}_b-z_a{z}_b\\ +(s_a{x}_b+x_a{s}_b+y_a{z}_b-z_a{y}_b)\mathbf{i}\\ +(s_a{y}_b-x_a{z}_b+y_a{s}_b+z_a{x}_b)\mathbf{j}\\ +(s_a{z}_b+x_a{y}_b-y_a{x}_b+z_a{s}_b)\mathbf{k}\end{array}\text{\hspace{1em}(4)}$$
如果写成向量形式并利用内外积运算，表达会更加简洁：
$${\mathbf{q}}_{\mathbf{a}}{\mathbf{q}}_{\mathbf{b}}=[s_a{s}_b-{{\mathbf{v}}_{\mathbf{a}}}^T{\mathbf{v}}_{\mathbf{b}},s_a{\mathbf{v}}_{\mathbf{b}}+s_b{\mathbf{v}}_{\mathbf{a}}+{\mathbf{v}}_{\mathbf{a}}\times {\mathbf{v}}_{\mathbf{b}}{]}^T\text{\hspace{1em}(5)}$$

3.模长
  四元数的模长定义为：
$$\Vert \begin{array}{c}{\mathbf{q}}_{\mathbf{a}}\end{array}\Vert =\sqrt{s_a^2+x_a^2+y_a^2+z_a^2}\text{\hspace{1em}(6)}$$
可以验证，两个四元数乘积的模等于模的乘积：
$$\Vert \begin{array}{c}{\mathbf{q}}_{\mathbf{a}}{\mathbf{q}}_{\mathbf{b}}\end{array}\Vert =\Vert \begin{array}{c}{\mathbf{q}}_{\mathbf{a}}\end{array}\Vert \Vert \begin{array}{c}{\mathbf{q}}_{\mathbf{b}}\end{array}\Vert \text{\hspace{1em}(7)}$$

4.共轭
  四元数的共轭是把虚部取成相反数：
$${\mathbf{q}}_{\mathbf{a}}^{\ast }=s_a-x_a\mathbf{i}-y_a\mathbf{j}-z_a\mathbf{k}=[s_a,-{\mathbf{v}}_{\mathbf{a}}{]}^T\text{\hspace{1em}(8)}$$   四元数共轭与其本身相乘，会得到一个实四元数，其实部为模长的平方：
$${\mathbf{q}}^{\ast }\mathbf{q}=\mathbf{q}{\mathbf{q}}^{\ast }=[s_a^2+{\mathbf{v}}^T\mathbf{v},0]\text{\hspace{1em}(9)}$$

5.逆
  一个四元数的逆为：
$${\mathbf{q}}^{-1}=\frac{{\mathbf{q}}^{\ast }}{{\Vert \begin{array}{c}{\mathbf{q}}_{\mathbf{a}}\end{array}\Vert }^2}\text{\hspace{1em}(10)}$$
按此定义，四元数和自己的逆的乘积为实四元数$1$：
$$\mathbf{q}{\mathbf{q}}^{-1}={\mathbf{q}}^{-1}\mathbf{q}=1\text{\hspace{1em}(11)}$$6.数乘
$$k\mathbf{q}=[ks,k\mathbf{v}{]}^T\text{\hspace{1em}(12)}$$

3.用四元数表示旋转

  用四元数旋转点$\mathbf{p}=[x,y,z]\in {\mathbb{R}}^3$。

• 构造四元数：
  $$\mathbf{p}=[0,x,y,z{]}^T=[0,\mathbf{v}{]}^T$$
• 确定旋转四元数：
  设单位长度旋转轴$\mathbf{n}=(n_x,n_y,n_z)$，和旋转角$\theta$，则对应的旋转四元数为：
  $$\mathbf{q}=[\cos \frac{\theta }{2},x\sin \frac{\theta }{2},y\sin \frac{\theta }{2},z\sin \frac{\theta }{2}]$$
• 代入四元数旋转公式：
  $${\mathbf{q}}^{{}^{\prime }}=\mathbf{q}\mathbf{p}{\mathbf{q}}^{-1}\text{\hspace{1em}(13)}$$这里的乘法均为四元数乘法，其中，${\mathbf{p}}^{{}^{\prime }}$为$\mathbf{p}$经过旋转之后的四元数。取出${\mathbf{p}}^{{}^{\prime }}$的虚部即为旋转之后的点坐标。

4.参考文献

• 高翔等. 视觉SLAM十四讲：从理论到实践第二版. 北京：电子工业出版社，2019.8.
• 赫恩. 计算机图形学第三版. 北京：电子工业出版社，2005.6.