点云配准（四） 四元数与旋转变换

四元数与旋转变换

参考资料：

• 三维空间刚体运动4-1：四元数表示旋转
• 如何形象地理解四元数？
• Quaternion.pdf (krasjet.github.io)

一.四元数的定义

1.1 四元数的基本概念

​ 四元数（Quaternions）是由爱尔兰数学家哈密顿（William Rowan Hamilton）在1843年引入的数学概念，可以看作是一种新的数值类型。四元数与复数的联系比较密切，可以视为是一种复数的拓展（复数是实数的复数，四元数是复数的复数）。四元数的定义和复数非常类似，唯一的区别就是四元数一共有三个虚部，而复数只有一个。所有的四元数 $q$ 都可以写成如下形式：
$$q=a+bi+cj+dk,(a,b,c,d\in R)$$
​ 其中 $i,j,k$ 为四元数的三个虚数单位，$b,c,d$为四元数的三个虚部，$a$为四元数的实部。与复数类似，四元数其实就是对于基 ${1, , , } $ 的线性组合，所以四元数也可以写成如下向量的形式：
$$q=\left[\begin{array}{c}a\\ b\\ c\\ d\end{array}\right]$$
​ 除此之外，我们经常将四元数的实部与虚部分开，并用一个三维的向量来表示虚部，将它表示为标量$s$和向量$v$的有序对形式如下：
$$q=[s,v{]}^T,(s=a,v=[b,c,d{]}^T且a,b,c,d\in R)$$
​ 如果一个四元数的虚部全为0，即 $q=[s,0{]}^T$ ，则该四元数被称为实四元数。反之，若一个四元数的实部为0，即 $q=[0,v{]}^T$ ，则该四元数被称为虚四元数或纯四元数。我们可以将任意的 3D 向量 $v$ 转换为纯四元数 [0,$v$] ，也可以将任意实数 $s$ 转化为实四元数 [$s$,0].

1.2 四元数的基本性质

（1）基本等式关系
$$\{\begin{array}{c}{i}^2=j^2=k^2=ijk=-1\\ ij=k,ji=-k\\ jk=i,kj=-i\\ ki=j,ik=-j\end{array}$$
（2）模长（范数）

​ 仿照复数的定义，我们可以将一个四元数 $q=a+bi+cj+dk$ （$q=[s,v{]}^T$）的模长（或者说范数（Norm））定义如下：
$$||q||=\sqrt{a^2+b^2+c^2+d^2}=\sqrt{s^2+||v|{|}^2}$$
​ 特别的，如果 $||q||=1$，则称 $q$ 为单位四元数。

二.四元数的基本运算

​ 在介绍之前，我们先定义三个用来运算的基本四元数分别为 $q,q_a,q_b$ ，其中 ：
$$\begin{array}{rl}& q=s+xi+yj+zk(向量有序对形式:q=[s,v],v=xi+yj+zk)\\ & q_a=s_a+x_{a}i+y_{a}j+z_{a}k(q_a=[s_a,v_a],v_a=x_{a}i+y_{a}j+z_{a}k)\\ & q_b=s_b+x_{b}i+y_{b}j+z_{b}k(q_b=[s_b,v_b],v_b=x_{b}i+y_{b}j+z_{b}k)\end{array}$$

2.1 四元数加减法

​ 与复数类似，四元数的加减法只需要将对应分量相加减就可以了，运算可以表示如下：
$$\begin{array}{rl}{q}_a\pm q_b& =(s_a\pm s_b)+(x_a\pm x_b)i+(y_a\pm y_b)j+(z_a\pm z_b)k\\ & =[s_a\pm s_b,v_a\pm v_b]\end{array}$$

2.2 标量乘法

​ 标量乘法又称数乘，指四元数可以与实数r ∈ R 相乘。四元数与标量的乘法是遵守交换律的，也就是说 $rq=qr$
$$\begin{array}{rl}& rq=r(s+xi+yj+zk)=rs+rxi+ryj+rzk\\ & rq=r[s,v]=[rs,rv]\end{array}$$

2.3 四元数乘法

（1）乘法过程推导

​ 四元数之间的乘法比较特殊，它们是不遵守交换律的，即 $q_a{q}_b\ne q_b{q}_a$ 。但经常使用的结合律和分配律在四元数内都是成立的。四元数乘法 $q_a{q}_b$ 是把$q_a$的每一项与 $q_b$ 的每一项相乘，最后再相加，乘积可以表示为（整理合并后）：
$$\begin{array}{rl}{q}_a{q}_b& =(s_a+x_{a}i+y_{a}j+z_{a}k)(s_b+x_{b}i+y_{b}j+z_{b}k)\\ & =s_a{s}_b+s_a{x}_{b}i+s_a{y}_{b}j+s_a{z}_{b}k+\\ & x_a{s}_{b}i-x_a{x}_b+x_a{y}_{b}k-x_a{z}_{b}j+\\ & y_a{s}_{b}j-y_a{x}_{b}k-y_a{y}_b+y_a{z}_{b}i+\\ & z_a{s}_{b}k+z_a{x}_{b}j-z_a{y}_{b}i-z_a{z}_b\\ & =(s_a{s}_b-x_a{x}_b-y_a{y}_b-z_a{z}_b)+\\ & (x_a{s}_b+s_a{x}_b-z_a{y}_b+y_a{z}_b)i+\\ & (y_a{s}_b+z_a{x}_b+s_a{y}_b-x_a{z}_b)j+\\ & (z_a{s}_b-y_a{x}_b+x_a{y}_b+s_a{z}_b)k\end{array}$$
​ 或者说也可以表示为：

（2）乘法的矩阵形式

​ 可以看到，四元数的相乘结果其实也是一个线性组合，我们同样可以将它写成矩阵的形式（$q_b$左乘$q_a$或$q_a$右乘$q_b$）：
$$q_a{q}_b=\left[\begin{array}{cccc}{s}_a& -x_a& -y_a& -z_a\\ x_a& s_a& -z_a& y_a\\ y_a& z_a& s_a& -x_a\\ z_a& -y_a& x_a& s_a\end{array}\right]\left[\begin{array}{c}{s}_b\\ x_b\\ y_b\\ z_b\end{array}\right]$$
（3）Graßmann 积 (Graßmann Product)

​ 上述乘法的推导结果过于繁杂，于是我们重新进行一下整理与化简。已知 ：

• 根据定义： $v_a=[x_a,y_a,z_a{]}^T,v_b=[x_b,y_b,z_b{]}^T$
• 根据向量点乘： $v_a\cdot v_b=x_a{x}_b+y_a{y}_b+z_a{z}_b$
• 根据向量叉乘：$v_a\times v_b=\left|\begin{array}{ccc}i& j& k\\ x_a& y_a& z_a\\ x_b& y_b& z_b\end{array}\right|=(y_a{z}_b-z_a{y}_b)i-(x_a{z}_b-z_a{x}_b)j+(x_a{y}_b-y_a{x}_b)k$

​ 写成这种形式结果应该就非常清楚了，如果使用标量向量有序对形式来表示，$q_a{q}_b$ 的结果可以用向量点乘和叉乘的形式表示出来，将乘法结果进一步整理化简。则对于任意四元数$q_a=[s_a,v_a],q_b=[s_b,v_b]$， $q_a{q}_b$的相乘结果可以表示如下，这个结果也被叫做Graßmann 积：
$$\begin{array}{rl}{q}_a{q}_b& =[s_a{s}_b-v_a\cdot v_b,s_a{v}_b+s_b{v}_a+v_a\times v_b]\\ & =[s_a{s}_b-v_a^T{v}_b,s_a{v}_b+s_b{v}_a+v_a\times v_b]\end{array}$$
​ 该定理是四元数与旋转变换联系起来的关键桥梁。

2.4 四元数的共轭

​ 对于四元数 $q=s+xi+yj+zk=[s,v]$ 来说，四元数的共轭是把虚部取成相反数，我们将其表示为 $q^{\ast }$ ( $q$ star )，即：
$$q^{\ast }=s-xi-yj-zk=[s,-v]$$
​ 共轭四元数的三个非常有用的性质如下：

• $q{q}^{\ast }=q^{\ast }q=[s^2+v\cdot v,0]=[||q|{|}^2,0]$ ，其实部正是该四元数模长的平方。证明如下：

$$\begin{array}{rl}q{q}^{\ast }& =[s,v]\cdot [s,-v]\\ & =[s^2-v\cdot (-v),s(-v)+sv+v\times (-v)]\\ & =[s^2+v\cdot v,0]\\ & =[||q|{|}^2,0](q^{\ast }q同)\end{array}$$

• $(q^{\ast }{)}^{\ast }=q$ ，四元数共轭的共轭等于本身
• $(q_a{q}_b{)}^{\ast }=q_b^{\ast }{q}_a^{\ast }$，乘积的共轭等于相反共轭的乘积

2.5 四元数的逆

​ 对于四元数 $q$ 来说，我们定义 $q^{-1}$ 为 $q$ 的逆，并规定：
$$q{q}^{-1}=q^{-1}q=1(q\ne 0)$$
​ 四元数逆的两个非常有用的性质如下：

• 共轭与四元数逆的关系：$q^{-1}=\frac{q^{\ast }}{||q|{|}^2}$ (计算四元数的逆)，证明如下：

$$\begin{array}{rl}& q{q}^{-1}=1\\ & q^{\ast }(q{q}^{-1})=q^{\ast }\\ & (q^{\ast }q)q^{-1}=q^{\ast }\\ & ||q|{|}^2\cdot q^{-1}=q^{\ast }\\ & q^{-1}=\frac{q^{\ast }}{||q|{|}^2}\end{array}$$

• 如果$||q||=1$， 也就是说 $q$ 是一个单位四元数 (Unit Quaternion)，那么则有：$q^{-1}=\frac{q^{\ast }}{1^2}=q^{\ast }$ 。即其逆和共轭就是同一个量。

三.四元数与旋转的关系

3.1 四元数旋转公式

​ 我们可以使用四元数来表达对一个点/向量的旋转变换。之前所学的旋转矩阵表示使用九个量来描述三个自由度的旋转，具有一定的冗余性；欧拉角和旋转向量表示虽然比较紧凑，但是存在万向锁问题，具有奇异性。而四元数是一种四维的表示方式，它既是紧凑的，也没有奇异性。如果说缺点，四元数不够直观，其运算稍复杂些。

（1）基本旋转

​ 假设有一个空间三维点 $v=[x,y,z]$ ，其对应的虚四元数形式为 $V=[0,x,y,z]=[0,v]$。现有一个由单位四元数 $q$ 指定的旋转，三维点 $v$ 经过 $q$ 的旋转后变为 $v^{\prime }$，我们可以将这个旋转变换过程表示如下：
$$V^{\prime }=qV{q}^{-1}=qV{q}^{\ast }$$
​ 其中，表示旋转的四元数 $q$ 一定是个单位四元数；这里的乘法均为四元数乘法，结果 $V^{\prime }$ 也是一个虚四元数（实部为0）；最后把结果的虚部取出，即得旋转之后点坐标 $v^{\prime }$。

（2）复合旋转

​ 假设有两个表示沿着不同轴，不同角度旋转的四元数 $q_1,q_2$，我们先对 $v$ 进行 $q_1$ 的变换，再进行 $q_2$ 的变换，复合变换的结果是什么呢？我们不妨将这两次变换分步进行。首先，我们对初始 $v$ 实施 $q_1$ 的变换，变换之后的 $v^{\prime }=q_{1}v{q}_1^{\ast }$

​ 接下来，我们对 $v^{\prime }$ 继续进行 $q_2$ 的变换，得到 $v^{\prime \prime }=q_2(q_{1}v{q}_1^{\ast })q_2^{\ast }=q_2{q}_{1}v{q}_1^{\ast }{q}_2^{\ast }$ ，根据性质 2.4-3 ，原式可以改写为 $v^{\prime \prime }=(q_2{q}_1)v(q_2{q}_1{)}^{\ast }=q_{net}v{q}_{net}^{\ast }$ ，其中 $q_{net}=q_2{q}_1$ 。注意复合旋转的四元数乘法顺序，这和矩阵、复数的复合运算非常相似，都是从右向左叠加。

​ 要注意的是，$q_1$ 与 $q_2$ 的等价旋转 $q_{net}$ 并不是分别沿着 $q_1$ 和 $q_2$ 的两个旋转轴进行的两次旋转。它是沿着一个全新的旋转轴进行的一次等价旋转，仅仅只有旋转的结果相同。

3.2 推导证明过程

推导证明过程与旋转向量中罗德里格斯公式的推导过程类似，具体请参考文章开头所列出的参考资料，此处不再赘述。

3.3 四元数与其他旋转表示的转换

3.3.1 旋转向量

（1）旋转向量=>四元数

​ 绕坐标轴的多次旋转可以等效为绕某一转轴旋转一定的角度。假设已知等效旋转轴方向单位旋转向量 $u=[u_x,u_y,u_z{]}^T$ ，旋转角度为 $\theta$ 。则旋转四元数 $q$ 表示为：
$$q=[cos(\frac{1}{2}\theta ),sin(\frac{1}{2}\theta )u]$$
​ 即：
$$\{\begin{array}{l}{q}_0=cos(\frac{1}{2}\theta )\\ q_1=u_{x}sin(\frac{1}{2}\theta )\\ q_2=u_{y}sin(\frac{1}{2}\theta )\\ q_3=u_{z}sin(\frac{1}{2}\theta )\end{array}$$
​ 换句话说，如果我们有 $q=[cos(\theta ),sin(\theta )u]$ ，那么 $v^{\prime }=qv{q}^{\ast }$ 可以将 $v$ 沿 着 $u$ 旋转 $2\theta$ 度。

（2）四元数=>旋转向量

​ 假设有一个单位四元数 $q=[a,bb]=[a,b_0,b_1,b_2]$，如果我们想要提取它所对应旋转的角度和旋转轴向量，则可直接逆表示如下：
$$\{\begin{array}{l}\theta =2arccos(a)\\ uu=\frac{bb}{sin(\frac{1}{2}\theta )}=\frac{bb}{sin(arccos(a))}\end{array}$$

3.3.2 旋转矩阵

（1）四元数 => 旋转矩阵

• 转换方法一：四元数 => 旋转向量 => 旋转矩阵（罗德里格斯公式）

  已知单位四元数 $q=[q_0,q_1,q_2,q_3]$， 根据之前推导的旋转向量表示中罗德里格斯公式展开式以及上述旋转向量和四元数转换的关系，将单位旋转向量$u$及旋转角 $\theta$ 替换为单位四元数$q$的形式，省去计算过程，便可以得到单位四元数到旋转矩阵的转换公式：
  $$R=\left[\begin{array}{ccc}1-2q_2^2-2q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& 1-2q_1^2-2q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& 1-2q_1^2-2q_2^2\end{array}\right]$$

• 转换方法二：四元数 => 矩阵形式 => 旋转矩阵
  
  

（2）旋转矩阵 => 四元数

​ 假设已知旋转变换矩阵 $R$ 如下：
$$R=\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]$$
​ 则根据上述四元数 => 旋转矩阵的转换公式，推导计算对应的单位四元数为：
$$\{\begin{array}{l}{q}_0=\frac{1}{2}\cdot \sqrt{1+r_{11}+r_{22}+r_{33}}\\ q_1=\frac{r_{32}-r_{23}}{4q_0}\\ q_2=\frac{r_{13}-r_{31}}{4q_0}\\ q_3=\frac{r_{21}-r_{12}}{4q_0}\end{array}$$

3.4 双倍覆盖

​ 四元数与 3D 旋转的关系并不是一对一的，同一个 3D 旋转可以使用两个不同的四元数来表示．对任意的单位四元数 $q=[cos(\frac{1}{2}\theta ),sin(\frac{1}{2}\theta )u]$，$q$ 与 $-q$ 代表的是同一个旋转。如果 $q$ 表示的是沿着旋转轴 $u$ 旋转 $\theta$ 度，那么 $-q$ 代表的是沿着相反的旋转轴 $-u$ 旋转 $(2\pi -\theta )$ 度，二者的最终旋转结果是一样的：

​ 其实从四元数的旋转公式中也能推导出相同的结果：
$$(-q)v(-q{)}^{\ast }=(-1{)}^{2}qv{q}^{\ast }=qv{q}^{\ast }$$
​ 所以，我们经常说单位四元数与 3D 旋转有一个 「2 对 1 满射同态」 (2-1 Surjective Homomorphism) 关系，或者说单位四元数双倍覆盖 (Double Cover) 了 3D 旋转。有一点需要注意的是，虽然 与 − 是两个不同的四元数，但是由于旋转矩阵中的每一项都包含了四元数两个分量的乘积，它们的旋转矩阵是完全相同的。旋转矩阵并不会出现双倍覆盖的问题。