视觉SLAM中的数学基础 第二篇 四元数

转自：http://www.cnblogs.com/gaoxiang12/p/5120175.html

致敬原作者，请移步原作者博文详细浏览

什么是四元数

　　相比欧拉角，四元数(Quaternion)则是一种紧凑、易于迭代、又不会出现奇异值的表示方法。它在程序中广为使用，例如ROS和几个著名的SLAM公开数据集、g2o等程序都使用四元数记录机器人的姿态。因此，理解四元数的含义与用法，对学习SLAM来说是必须的。本节我们就来讲讲四元数。

　　首先，请读者不要对四元数有什么神秘的感觉。四元数仅是3D姿态的一种表达方式，我们用一个单位四元数表达原本用旋转矩阵表示的三维旋转。这样做一个直接的好处是省空间。一个旋转阵有9个分量，但只有三个自由度。那么，能不能用三个数来描述呢？可以是可以的，但不可避免会出现奇异的情况，欧拉角就是一个例子。而四元数，比三维向量多了一个分量，从而可以无奇异地表示各种姿态。下面我们来详细讲讲四元数。

　　四元数是Hamilton找到的一种扩展的复数。一个四元数拥有一个实部和三个虚部（故事上说他原先找了很久带两个虚部的，结果怎么也找不到，最后豁然开朗找到了三虚部的四元数）：
　　

q=q0+q1i+q2j+q3kq=q0+q1i+q2j+q3k

　　其中i,j,ki,j,k为四元数的三个虚部。这三个虚部满足关系式：

⎧⎩⎨⎪⎪⎪⎪⎪⎪i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ik=−j(1)(1){i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ik=−j

　　由于它的这种特殊表示形式，有时人们也用一个标量和一个向量来表达四元数：

q=[s,v],s=q0∈R,v=[q1,q2,q3]∈R3.q=[s,v],s=q0∈R,v=[q1,q2,q3]∈R3.

　　这里，标量ss称为四元数的实部，而向量vv称为它的虚部。如果一个四元数虚部为00，称之为实四元数。反之，若它的实部为00，称之为虚四元数。该定义和复数是相似的。

　　四元数可以表示三维空间中任意一个旋转。与旋转矩阵中类似，我们仍假设某个旋转是绕单位向量n=[nx,ny,nz]Tn=[nx,ny,nz]T进行了角度为θθ的旋转，那么这个旋转的四元数形式为：

q=[cosθ2,nxsinθ2,nysinθ2,nzsinθ2]T(2)(2)q=[cos⁡θ2,nxsin⁡θ2,nysin⁡θ2,nzsin⁡θ2]T

　　事实上，这还是一个模长为1的四元数，称为单位四元数。反之，我们亦可通过任意一个长度为1的四元数，计算对应旋转轴与夹角：

{θ=2arccosq0[nx,ny,nz]T=[q1,q2,q3]T/sinθ2(3)(3){θ=2arccos⁡q0[nx,ny,nz]T=[q1,q2,q3]T/sin⁡θ2

　　若某个四元数长度不为1，我们可以通过归一化将它转换为一个模长为1的四元数。

　　对式22的θθ加上2π2π，我们得到一个相同的旋转，但此时对应的四元数变成了−q−q。因此，在四元数中，任意的旋转都可以由两个互为相反数的四元数表示。同理，取θθ为00，则得到一个没有任何旋转的四元数：

q0=[±1,0,0,0]T(4)(4)q0=[±1,0,0,0]T

---

 

四元数的运算

　　四元数和通常复数一样，可以进行一系列的运算。常见的有四则运算、内积、求逆、共轭、求指数／对数等等。表示姿态时，它还可以进行插值。下面我们分别介绍。

　　现有两个四元数qa,qbqa,qb，它们的向量表示为[sa,va],[sb,vb][sa,va],[sb,vb]，或者原始四元数表示为：

sa+xai+yaj+zak,sb+xbi+ybj+zbk.sa+xai+yaj+zak,sb+xbi+ybj+zbk.

那么，它们的运算可表示如下。

• 加法和减法

　　四元数qa,qbqa,qb的加减运算为：

qa±qb=[sa±sb,va±vb].(5)(5)qa±qb=[sa±sb,va±vb].

• 乘法

　　乘法是把qaqa的每一项与qbqb每项相乘，最后相加，虚部要按照式~11~进行：

qaqb=sasb−xaxb−yayb−zazb+(saxb+xasb+yazb−zayb)i+(sayb−xazb+yasb+zabb)j+(sazb+xayb−xbya+zasb)k(6)(6)qaqb=sasb−xaxb−yayb−zazb+(saxb+xasb+yazb−zayb)i+(sayb−xazb+yasb+zabb)j+(sazb+xayb−xbya+zasb)k

　　虽然稍为复杂，但形式上也是整齐有序的。如果写成向量形式并利用内外积运算，该表达会更加简洁：

qaqb=[sasb−va⋅vb,savb+sbva+va×vb](7)(7)qaqb=[sasb−va⋅vb,savb+sbva+va×vb]

　　这里我们就不帮读者复习什么叫外积了。在该乘法定义下，两个实的四元数乘积仍是实的，这与复数也是一致的。然而，注意到，由于最后一项外积的存在，该乘法通常是不可交换的，除非vava和vbvb在R3R3中共线。

• 共轭

　　四元数的共轭为：

q∗a=sa−xai−yaj−zak=[sa,−va](8)(8)qa∗=sa−xai−yaj−zak=[sa,−va]

　　即把虚部取成相反数。四元数共轭与自己本身相乘，会得到一个实四元数，其实部为模长的平方:

q∗q=qq∗=[s2a+vTv,0]=s2a+vTv(9)(9)q∗q=qq∗=[sa2+vTv,0]=sa2+vTv

• 模长

　　四元数的模长定义为：

∥qa∥=s2a+x2a+y2a+z2a−−−−−−−−−−−−−−√=q∗Taqa−−−−−√(10)(10)∥qa∥=sa2+xa2+ya2+za2=qa∗Tqa

　　可以验证，两个四元数乘积的模即为模的乘积。这保证单位四元数相乘后仍是单位四元数。

∥qaqb∥=∥qa∥∥qb∥(11)(11)∥qaqb∥=∥qa∥∥qb∥

• 逆

　　一个四元数的逆为：

q−1=q∗/∥q∥2(12)(12)q−1=q∗/∥q∥2

　　按此定义，四元数和自己的逆的乘积为实四元数的1：

qq−1=q−1q=1(13)(13)qq−1=q−1q=1

　　同时，乘积的逆有和矩阵相似的性质：

(qaqb)−1=q−1bq−1a(14)(14)(qaqb)−1=qb−1qa−1

　　对于单位四元数，即∥q∥=1∥q∥=1，它的逆即是它的共轭四元数。

• 数乘与点乘

　　和向量相似，四元数可以与数相乘：

kq=[ks,kv](15)(15)kq=[ks,kv]

　　点乘是指两个四元数每个位置上的数值分别相乘：

qa⋅qb=sasb+xaxbi+yaybj+zazbk(16)(16)qa⋅qb=sasb+xaxbi+yaybj+zazbk

---

 

用四元数表示旋转

　　在复数域CC，我们可以用一个复数eiθeiθ表示2D的旋转，类似的，3D空间也可以用单位四元数表示旋转。假设一个空间三维点v=[x,y,z]∈R3v=[x,y,z]∈R3，以及一个由旋转轴和夹角n,θn,θ 指定的旋转，下面讨论如何用四元数表示它们。

　　首先，我们把三维空间点用一个虚四元数来描述：

p=[0,x,y,z]=[0,v].p=[0,x,y,z]=[0,v].

　　然后，参照式22，用另一个四元数qq表示这个旋转：

q=[cosθ2,nsinθ2].q=[cos⁡θ2,nsin⁡θ2].

　　那么，旋转后的点p′p′即可表示为这样的乘积：

p′=qpq−1(17)(17)p′=qpq−1

　　可以验证，计算结果的实部为nT(n×v)=0nT(n×v)=0，故计算结果为纯虚四元数。其虚部的三个分量表示旋转后3D点的坐标。

---

 

四元数到旋转矩阵的转换

　　由于任意单位四元数都可表示为一个3D旋转，即SO(3)SO(3)中的元素，我们可以找到一个旋转矩阵与之对应。最简单的方式是由四元数qq解出旋转角θθ和旋转轴nn，但那样要计算一个arccosarccos函数，代价较大。实际上这个计算是可以通过一定的计算技巧绕过的。为省略篇幅，我们直接给出四元数到旋转矩阵的转换方式。

　　设四元数q=q0+q1i+q2j+q3kq=q0+q1i+q2j+q3k，对应的旋转矩阵RR为：

R=⎡⎣⎢⎢1−2q22−2q232q1q2−2q0q32q1q3+2q0q22q1q2+2q0q31−2q21−2q232q2q3−2q0q12q1q3−2q0q22q2q3+2q0q11−2q21−2q22⎤⎦⎥⎥(18)(18)R=[1−2q22−2q322q1q2+2q0q32q1q3−2q0q22q1q2−2q0q31−2q12−2q322q2q3+2q0q12q1q3+2q0q22q2q3−2q0q11−2q12−2q22]

　　反之，由旋转矩阵到四元数的转换如下。假设矩阵为R={mij},i,j∈[1,2,3]R={mij},i,j∈[1,2,3]，其对应的四元数qq由下式给出：

q0=tr(R)+1−−−−−−−−√2,q1=m23−m324q0,q2=m31−m134q0,q3=m12−m214q0(19)(19)q0=tr(R)+12,q1=m23−m324q0,q2=m31−m134q0,q3=m12−m214q0

　　值得一提的是，由于q和−qq和−q表示同一个旋转，事实上一个RR的四元数表示并不是惟一的。存在其他三种与上式类似的计算方式，而本书省略了。实际编程中，当q0q0接近0时，其余三个分量会非常大，导致解不稳定，此时会考虑使用剩下的几种方式计算。

---

 

其他几种变换

　　3D空间中的变换，除了欧氏变换之外，还存在其他几种变换（事实上欧氏变换是最简单的）。它们有一部分和测量几何有关，我们之后的讲解中会提到，在此先罗列出来。

• 相似变换

　　相似变换比欧氏变换多了一个自由度，它允许物体进行自由地缩放。

TS=[sR0Tt1](20)(20)TS=[sRt0T1]

　　注意到旋转部分多了一个缩放因子ss，它在x,y,zx,y,z三个坐标上形成均匀的缩放。类似的，相似变换的乘法也构成群，称为Sim(3)Sim(3)。由于含有缩放，相似变换不再保持图形的面积不变。

• 仿射变换

　　仿射变换的矩阵形式如下：

TA=[A0Tt1](21)(21)TA=[At0T1]

　　与欧氏变换不同的是，仿射变换只要求AA是一个可逆矩阵，而不必是正交矩阵。在仿射变换下，直线的夹角会发生改变，但平行性质不变。这即是说，仿射变换把平行四边形变为平行四边形。

• 射影变换

　　射影变换是最一般的变换，它的矩阵形式为:

TP=[AaTtv](22)(22)TP=[AtaTv]

　　它左上角为可逆矩阵AA，右上为平移tt，左下缩放aTaT。由于采用齐坐标，当v≠0v≠0时，我们可以对整个矩阵除以vv得到一个右下角为1的矩阵； 否则，则得到右下角为00的矩阵。因此，这个矩阵在2D中一共有8个自由度，而在3D中一共有15个自由度，是现在提到的变换中最为一般的。

　　下表总结了目前讲到的几种变换的性质。注意在“不变性质”中，从上到下是有包含关系的。例如，欧氏变换除了保体积之外，也具有保平行、相交等性质。