坐标系旋转关系描述 -- 四元数

在看ROS的TF时发现其描述旋转关系有2种方式:欧拉角和四元数

感觉四元数和罗德里格斯变换有点像,都是直接用一个向量描述旋转轴,不过后者用向量的模表示旋转角度,因此只有3个值。


以下内容转自 半闲居士的视觉SLAM中的数学基础 第二篇 四元数

*******************************************************************************************

什么是四元数

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

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

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

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

  其中 i,j,k i,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=q0R,v=[q1,q2,q3]R3. q=[s,v],s=q0∈R,v=[q1,q2,q3]∈R3.

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

  四元数可以表示三维空间中任意一个旋转。与旋转矩阵中类似,我们仍假设某个旋转是绕单位向量 n=[nx,ny,nz]T n=[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的四元数。

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

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


 

四元数的运算

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

  现有两个四元数 qa,qb qa,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,qb qa,qb的加减运算为:

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

  • 乘法

  乘法是把 qa qa的每一项与 qb qb每项相乘,最后相加,虚部要按照式~ 1 1~进行:

qaqb=sasbxaxbyaybzazb+(saxb+xasb+yazbzayb)i+(saybxazb+yasb+zabb)j+(sazb+xaybxbya+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=[sasbvavb,savb+sbva+va×vb](7) (7)qaqb=[sasb−va⋅vb,savb+sbva+va×vb]

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

  • 共轭

  四元数的共轭为:

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

  即把虚部取成相反数。四元数共轭与自己本身相乘,会得到一个实四元数,其实部为模长的平方:
qq=qq=[s2a+vTv,0]=s2a+vTv(9) (9)q∗q=qq∗=[sa2+vTv,0]=sa2+vTv

  • 模长

  四元数的模长定义为:

qa=s2a+x2a+y2a+z2a=qTaqa(10) (10)∥qa∥=sa2+xa2+ya2+za2=qa∗Tqa

  可以验证,两个四元数乘积的模即为模的乘积。这保证单位四元数相乘后仍是单位四元数。
qaqb=qaqb(11) (11)∥qaqb∥=∥qa∥∥qb∥

  一个四元数的逆为:

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

  按此定义,四元数和自己的逆的乘积为实四元数的1:
qq1=q1q=1(13) (13)qq−1=q−1q=1

  同时,乘积的逆有和矩阵相似的性质:
(qaqb)1=q1bq1a(14) (14)(qaqb)−1=qb−1qa−1

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

  • 数乘与点乘

  和向量相似,四元数可以与数相乘:

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


  点乘是指两个四元数每个位置上的数值分别相乘:
qaqb=sasb+xaxbi+yaybj+zazbk(16) (16)qa⋅qb=sasb+xaxbi+yaybj+zazbk


 

用四元数表示旋转

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

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

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

  然后,参照式 2 2,用另一个四元数 q q表示这个旋转:

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

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

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

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


 

四元数到旋转矩阵的转换

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

  设四元数 q=q0+q1i+q2j+q3k q=q0+q1i+q2j+q3k,对应的旋转矩阵 R R为:

R=12q222q232q1q22q0q32q1q3+2q0q22q1q2+2q0q312q212q232q2q32q0q12q1q32q0q22q2q3+2q0q112q212q22(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],其对应的四元数 q q由下式给出:

q0=tr(R)+12,q1=m23m324q0,q2=m31m134q0,q3=m12m214q0(19) (19)q0=tr(R)+12,q1=m23−m324q0,q2=m31−m134q0,q3=m12−m214q0

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


 

其他几种变换

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

  • 相似变换

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

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

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

  • 仿射变换

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

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

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

  • 射影变换

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

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

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

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

坐标系旋转关系描述 -- 四元数_第1张图片


你可能感兴趣的:(机器人ROS)