slam14讲 |二、三维空间刚体运动

参考资料

高翔slam14讲

1. 旋转矩阵

1.1 内积、外积

对于$a,b\in {\mathbb{R}}^3$，内积可以写成

内积可以描述向量间的投影关系。而外积呢是这个样子:

外积的方向垂直于这两个向量,大小为$|a||b|sin(a,b)$,是两个向量张成的四边形的有向面积。对于外积,我们引入了^符号,把a写成一个矩阵，事实上是一个反对称矩阵(Skew-symmetric)。这样就把外积$axb$,写成了矩阵与向量的乘法a^b,把它变成了线性运算。这个符号将在后文经常用到,请记住它。外积只对三维向量存在定义,我们还能用外积表示向量的旋转。

为什么外积可以表示旋转呢?

考虑两个不平行的向量a,b,我们要描述从a到b之间是如何旋转的,如图3-1所示。我们可以用一个向量来描述三维空间中两个向量的旋转关系。在右手法则下,我们用右手的四个指头从a转向b,其大拇指朝向就是旋转向量的方向,事实上也是$a\times b$的方向。它的大小则由a和b的夹角决定。通过这种方式,我们构造了从a到b的一个旋转向量。这个向量同样位于三维空间中,在此坐标系下,可以用三个实数来描述它。

1.2 坐标变换

• 与向量间的旋转类似,我们同样可以描述两个坐标系之间的旋转关系,再加上平移,统称为坐标系之间的变换关系。

• 一个欧氏变换由一个旋转和一个平移两部分组成。

1.2.1 旋转

首先来考虑旋转。我们设某个单位正交基$(e_1,e_2,e_3)$经过一次旋转,变成了$(e_1^{\prime },e_2^{\prime },e_3^{\prime })$。那么,对于同一个向量a(注意该向量并没有随着坐标系的旋转而发生运动),它在两个坐标系下的坐标为$[a_1,a_2,a_3{]}^T$和$[a_1^{\prime },a_2^{\prime },a_3^{\prime }{]}^T$,根据坐标的定义,有:

为了描述两个坐标之间的关系,我们对上面等式左右同时左乘$[e_1,e_2,e_3{]}^T$，那么左边的系数变成了单位矩阵,所以:

我们把中间的阵拿出来,定义成一个矩阵R。这个矩阵由两组基之间的内积组成,刻画了旋转前后同一个向量的坐标变换关系。只要旋转是一样的,那么这个矩阵也是一样的。可以说,矩阵R描述了旋转本身。因此它又称为旋转矩阵。

旋转矩阵有一些特别的性质。事实上,它是一个行列式为1的正交矩阵。反之,行列式为1的正交矩阵也是一个旋转矩阵。所以,我们可以把旋转矩阵的集合定义如下:

SO(n)是特殊正交群(Special Orthogonal Group)的意思。

1.2.2 平移

在欧氏变换中,除了旋转之外还有一个平移。考虑世界坐标系中的向量$a$,经过一次旋转(用R描述)和一次平移$t$后,得到了$a^{\prime }$,那么把旋转和平移合到一起,有：

其中,$t$称为平移向量。相比于旋转,平移部分只需把这个平移量加到旋转之后的坐标上,显得非常简洁。

1.2.3 变换矩阵与齐次坐标

式(3.8)完整地表达了欧氏空间的旋转与平移,不过还存在一个小问题:这里的变换关系不是一个线性关系。假设我们进行了两次变换: $R_1,t_1$和$R_2,t_2$,满足:

这样的形式在变换多次之后会过于复杂。因此,我们要引入齐次坐标和变换矩阵重写式(3.8):

这是一个数学技巧:我们把一个三维向量的末尾添加1,变成了四维向量,称为齐次坐标。对于这个四维向量,我们可以把旋转和平移写在一个矩阵里面,使得整个关系变成了线性关系。该式中,矩阵T称为变换矩阵(Transform Matrix)。我们暂时用$\tilde{a}$表示a的齐次坐标。

齐次坐标

它是射影几何里的概念。通过添加最后一维,我们用四个实数描述了一个三维向量,这显然多了一个自由度,但允许我们把变换写成线性的形式。在齐次坐标中,某个点$x$的每个分量同乘一个非零常数k后,仍然表示的是同一个点。因此,一个点的具体坐标值不是唯一的。如$[1,1,1,1{]}^T$和$[2,2,2,2{]}^T$是同一个点。但当最后一项不为零时,我们总可以把所有坐标除以最后一项,强制最后一项为1,从而得到一个点唯一的坐标表示(也就是转换成非齐次坐标):

这时,忽略掉最后一项,这个点的坐标和欧氏空间就是一样的。依靠齐次坐标和变换矩阵,两次变换的累加就可以有很好的形式:

变换矩阵
关于变换矩阵T,它具有比较特别的结构:左上角为旋转矩阵,右侧为平移向量,左下角为0向量,右下角为1。这种矩阵又称为特殊欧氏群(Special Euclidean Group):

与SO(3)一样,求解该矩阵的逆表示一个反向的变换:

最后,为了保持符号的简洁,在不引起歧义的情况下,我们以后不区别齐次坐标与普通的坐标的符号,默认我们使用的是符合运算法则的那一种。

回顾一下我们介绍的内容:首先,我们说了向量和它的坐标表示,并介绍了向量间的运算;然后,坐标系之间的运动由欧氏变换描述,它由平移和旋转组成。旋转可以由旋转矩阵$SO(3)$描述,而平移直接由一个${\mathbb{R}}^3$向量描述。最后,如果将平移和旋转放在一个矩阵中,就形成了变换矩阵$SE(3)$。

Eigen是一个C++开源线性代数库。它提供了快速的有关矩阵的线性代数运算,还
包括解方程等功能。许多上层的软件库也使用Eigen进行矩阵运算,包括g2o,Sophus等。可自行学习

2. 旋转向量和欧拉角

2.1 旋转向量

旋转矩阵表示方式至少有以下几个缺点:

1. SO(3)的旋转矩阵有九个量,但一次旋转只有三个自由度。因此这种表达方式是冗余的。同理,变换矩阵用十六个量表达了六自由度的变换。
2. 旋转矩阵自身带有约束：它必须是个正交矩阵,且行列式为1。变换矩阵也是如此。当我们想要估计或优化一个旋转矩阵/变换矩阵时,这些约束会使得求解变得更困难。

任意旋转都可以用一个旋转轴和一个旋转角来刻画。于是,我们可以使用一个向量,其方向与旋转轴一致,而长度等于旋转角。这种向量,称为旋转向量(或轴角, Axis-Angle)。这种表示法只需一个三维向量即可描述旋转。同样,对于变换矩阵,我们使用一个旋转向量和一个平移向量即可表达一次变换。这时的维数正好是六维。

旋转向量和旋转矩阵之间是如何转换的呢?
假设有一个旋转轴为$n$,角度为$\theta$的旋转,显然,它对应的旋转向量为$\theta n$。由旋转向量到旋转矩阵的过程由罗德里格斯公式(Rodrigues’s Formula)表明，由于推导过程比较复杂,我们不作描述,只给出转换的结果:

符号KaTeX parse error: Expected group after '^' at position 1: ^̲是向量到反对称的转换符,见式(3.3)。反之,我们也可以计算从一个旋转矩阵到旋转向量的转换。对于转角$\theta$有:

因此,转轴$n$是矩阵$R$特征值1对应的特征向量。求解此方程,再归一化,就得到了旋转轴。读者也可以从“旋转轴经过旋转之后不变”的几何角度看待这个方程。

2.2 欧拉角

无论是旋转矩阵、旋转向量,虽然它们能描述旋转,但对我们人类是非常不直观的。
当我们看到一个旋转矩阵或旋转向量时,很难想象出来这个旋转究竟是什么样的。当它们变换时,我们也不知道物体是向哪个方向在转动。而欧拉角则提供了一种非常直观的方式来描述旋转——它使用了三个分离的转角,把一个旋转分解成三次绕不同轴的旋转。
当然,由于分解方式有许多种,所以欧拉角也存在着不同的定义方法。比如说,当我先绕X轴旋转,再绕Y轴,最后绕Z轴,就得到了一个XYZ轴的旋转。同理,可以定义,ZYZ、 ZYX等等旋转方式。
欧拉角当中比较常用的一种,便是用“偏航-俯仰-滚转”(yaw-pitch-roll)三个角度来描述一个旋转的。由于它等价于ZYX轴的旋转,我们就以ZYX为例。假设一个刚体的前方(朝向我们的方向)为X轴,右侧为Y轴,上方为Z轴,见图3-3。那么,ZYX转角相当于把任意旋转分解成以下三个轴上的转角:

1. 绕物体的Z轴旋转,得到偏航角yaw；
2. 绕旋转之后的Y轴旋转,得到俯仰角pitch；
3. 绕旋转之后的X轴旋转,得到滚转角roll。

欧拉角的一个重大缺点是会碰到著名的万向锁问题(Gimbal Lock):在俯仰角为$\pm 90°$时,第一次旋转与第三次旋转将使用同一个轴,使得系统丢失了一个自由度(由三次旋转变成了两次旋转)。这被称为奇异性问题,在其他形式的欧拉角中也同样存在。

3. 四元数

旋转矩阵用九个量描述三自由度的旋转,具有冗余性；

欧拉角和旋转向量是紧凑的，但具有奇异性。

事实上,我们找不到不带奇异性的三维向量描述方式。这有点类似于,当我们想用两个坐标表示地球表面时(如经度和纬度),必定存在奇异性(纬度为$\pm 90°$时经度无意义)。

三维旋转是一个三维流形,想要无奇异性地表达它,用三个量是不够的。

回忆我们以前学习过的复数。我们用复数集C表示复平面上的向量,而复数的乘法则能表示复平面上的旋转:例如,乘上复数i相当于逆时针把一个复向量旋转90度。类似的,在表达三维空间旋转时,也有一种类似于复数的代数:四元数(Quaternion)。四元数是Hamilton找到的一种扩展的复数.它既是紧凑的,也没有奇异性。如果说缺点的话,四元数不够直观,其运算稍为复杂一些。

一个四元数q拥有一个实部和三个虚部：

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

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

这里, $s$称为四元数的实部,而$v$称为它的虚部。如果一个四元数虚部为0,称之为实四元数。反之,若它的实部为0,称之为虚四元数。

3.1 旋转向量与四元数

假设某个旋转是绕单位向量$n=[n_x,n_y,n_z{]}^T$进行了角度为0的旋转,那么这个旋转的四元数形式为:

反之,我们亦可从单位四元数中计算出对应旋转轴与夹角:

这式子给我们一种微妙的“转了一半”的感觉。同样,对式(3.19)的0加上2m,我们得到一个相同的旋转,但此时对应的四元数变成了$-q$。因此,在四元数中,任意的旋转都可以由两个互为相反数的四元数表示。同理,取$\theta$为0,则得到一个没有任何旋转的实四元数:

3.2 四元数的运算

四元数和通常复数一样,可以进行一系列的运算。常见的有四则运算、数乘、求逆、共轭等等。
现有两个四元数$q_a,q_b$,它们的向量表示为$[s_a,v_a],[s_b,v_b]$,或者原始四元数表示为:

1. 加法和减法
   四元数$q_a,q_b$的加减运算为:
   

2. 乘法
   乘法是把$q_a$的每一项与$q_b$每项相乘,最后相加,虚部要按照式(3.18)进行。
   

   虽然稍为复杂,但形式上是整齐有序的。如果写成向量形式并利用内外积运算,该表达会更加简洁:
   
   在该乘法定义下,两个实的四元数乘积仍是实的,这与复数也是一致的。然而,注意到,由于最后一项外积的存在,四元数乘法通常是不可交换的,除非$v_a$和$v_b$在${\mathbb{R}}^3$中共线,那么外积项为零。

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

4. 模长
   四元数的模长定义为:
   
   可以验证,两个四元数乘积的模即为模的乘积。这保证单位四元数相乘后仍是单位四元数。
   

5. 逆
   一个四元数的逆为:
   
   按此定义,四元数和自己的逆的乘积为实四元数的1:
   
   如果q为单位四元数,逆和共轭就是同一个量。同时,乘积的逆有和矩阵相似的性质：
   

6. 数乘与点乘
   和向量相似,四元数可以与数相乘:
   
   点乘是指两个四元数每个位置上的数值分别相乘:
   

3.3 用四元数表示旋转

我们可以用四元数表达对一个点的旋转。假设一个空间三维点$p=[r,y,z]\in {\mathbb{R}}^3$,以及一个由轴角$n,\theta$指定的旋转。三维点$p$经过旋转之后变成为$p^{\prime }$。如果使用矩阵描述，那么有$p^{\prime }=Rp$。如果用四元数描述旋转,它们的关系如何来表达呢?

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

这相当于我们把四元数的三个虚部与空间中的三个轴相对应。然后,参照式(3.19),用四元数$q$表示这个旋转:

那么,旋转后的点$p^{\prime }$即可表示为这样的乘积:

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

3.4 四元数与旋转矩阵

设四元数$q=q_0+q_{1}i+q_{2}j+q_{3}k$,对应的旋转矩阵R为:

反之,由旋转矩阵到四元数的转换如下。假设矩阵为$R=m_{ij}，i,j\in [1,2,3]$,其对应的四元数$q$由下式给出:

值得一提的是,由于$q$和$-q$表示同一个旋转,事实上一个R对应的四元数表示并不是惟一的。同时,除了上面给出的转换方式之外,还存在其他几种计算方法,这里不再赘述。

4. 相似、仿射、射影变换

4.1 相似变换

相似变换比欧氏变换多了一个自由度,它允许物体进行均匀的缩放,其矩阵表示为:

注意到旋转部分多了一个缩放因子s,表示我们在对向量旋转之后,可以在$x,y,z$三个坐标上进行均匀的缩放。由于含有缩放,相似变换不再保持图形的面积不变。可以想象一个边长为1的立方体通过相似变换后,变成边长为10的样子(但仍然是立方体)。

4.2 仿射变换

仿射变换矩阵形式如下：

与欧氏变换不同的是,仿射变换只要求A是一个可逆矩阵,而不必是正交矩阵。仿射变换也叫正交投影。经过仿射变换之后,立方体就不再是方的了,但是各个面仍然是平行四边形。

4.3 射影变换

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