视觉SLAM十四讲:运动方程

355
0
2020年11月30日 09时14分

 

SLAM:同时定位和建图(Simultaneous Localization and Mapping)。

 

希望机器人从未知环境的未知地点出发,在运动过程中通过重复观测到的地图特征(比如,墙角,柱子等)定位自身位置和姿态,再根据自身位置增量式的构建地图,从而达到同时定位和地图构建的目的。

 

所用传感器:激光雷达,相机(单目,双目,RGBD,事件)

 

SLAM框架:
传感器信息提取—前端视觉里程计—后端优化—建图
传感器信息提取—回环检测—后端优化(这两条线路之间形成一个闭环)

 

传感器信息提取:SLAM中传感器信息的提取和预处理,往往是多种传感器信息的融合。

 

前端视觉里程计(VO):通过估算相邻图像间的关系来推导相机的运动,以及局部地图的样子。

 

后端优化:接受不同时刻视觉里程计测量的相机位姿以及回环检测的信息,并对他们进行优化(往往是非线性优化),进而得到全局地图和轨迹。

 

回环检测:用于判断机器人是否到达先前预定的位置,用于解决位置估计随时间漂移的问题。

 

建图:根据估计的轨迹,建立相应要求的地图(栅格地图,拓扑地图,点云地图,网格地图)。

综上,定位与建图便完成了。

 

1

 

SLAM数学表达
此处分为4个部分进行阐述:运动方程及李代数优化,观测方程,非线性优化。

 

**1. 运动方程:**从K-1时刻运动到K时刻,位姿X变化的数学表示。
可以表述为一个方程:X_k = F(X_(k-1), U_k, W_k)
其中U_k表示传感器输入,W_k表示过程中的噪声,F往往表示非线性函数。

 

为了进一步从数学角度来理解空间变换,需要了解一下旋转矩阵、变换矩阵。

 

1.1 旋转矩阵
我们用一个3X1的矩阵来表示物体的位置,3X3的矩阵来表示物体的姿态。
物体在空间里的运动可以分解为平移加旋转,平移操作只需要坐标的简单相加,旋转则涉及到坐标系的变换,下面对旋转矩阵进行分析:
设某个单位正交基(e1,e2,e3) 经过一次旋转,变成了(e1, e2, e3)。那么,对于同一个向量a(该向量并没有随着坐标系的旋转而发生运动),它在两个坐标系下的坐标为[a1, a2, a3]T 和[a1, a2, a3]。根据坐标的定义,有:

 

2

 

上面等式左右同乘以[a1, a2, a3`]T,变成:

 

3

 

我们把R称为旋转矩阵,它描述了旋转前后同一个向量的坐标变换关系。通过旋转矩阵,我们便可以求解物体的旋转变换了。

 

1.2 变换矩阵
有了旋转矩阵和平移矩阵,便可以描述一个物体的空间变换了,不过还有一个遗憾,便是操作的复杂性,因为目前变换前后的坐标不是一个线性关系,而是两次变换(平移和旋转)。为了使其变为一次线性操作,我们提出了变换矩阵的概念:
将平移矩阵和旋转矩阵按照如下规律合在一起,便称之为变换矩阵T:

 

4

 

式中[a, i]T为齐次坐标(四维向量)。

 

如此,变换前后的坐标便成了线性关系。

 

但是,这样的表示还不够,因为旋转矩阵一共有9个量,但是它只描述了3个自由度的旋转,这便产生了冗余。同理,变换矩阵有16个量,但只描述了6个自由度。于是,我们就寻找更加紧凑的表达。

 

书中给出了很多种表达,如旋转向量、欧拉角和四元数等,但是由于前两种三维向量描述方式都带有奇异性(所谓奇异性,即在某些情况下这种描述方式不管用),所以常用的还是四元数,此处仅介绍一下四元数:

 

四元数是一种类似于复数的代数,由四个部分组成,即是紧凑的,也没有奇异性。
一个四元数q 拥有一个实部和三个虚部。q = q0 + q1i + q2j + q3k;
其中i; j; k 为四元数的三个虚部。这三个虚部满足关系式:

 

4

 

假设某个旋转是绕单位向量n = [nx; ny; nz]T 进行了角度为 theta 的旋转,那么这个旋转的四元数形式为:

 

5

 

具体表示:假设一个空间三维点p = [x; y; z],以及一个由轴角n; theta 指定的旋转。三维点p 经过旋转之后变成为p′。如果使用矩阵描述,那么有p′ = Rp。如果用四元数描述旋转:
首先,把三维空间点用一个虚四元数来描述: p = [0; x; y; z]
用四元数q 表示这个旋转:

 

7

 

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

 

8

 

至此,我们已经可以用四元数和一个平移矩阵来表示物体的空间运动了。

2. 运动方程的李代数优化

目前,我们已经知道了三维世界中刚体运动的表示方式,但是在SLAM 中,除了表示之
外,我们还要对它们进行估计和优化。因为在 SLAM 中位姿是未知的,而我们需要解决什
么样的相机位姿最符合当前观测数据这样的问题。

 

一种典型的方式是把它构建成一个优化问题,求解最优的R; t,使得误差最小化。

 

但是,由于旋转矩阵自身是带有约束的(正交且行列式为1)。所以它们作为优化变量时,会引入额外的约束,使优化问题变得困难。于是我们提出了通过李群——李代数间的转换关系,把位姿估计变成无约束的优化问题,简化求解方式。

 

李群:下节再讲,好累啊,感觉在做无用功···

 

3. 观测方程:在X_k位置上观测到路标点Y_i时,产生的观测数据Z_(k,j).
Z_(k,j) = H(Y_i, X_k, V_(k,j))
其中V_(k,j)表示此时的噪声

发表评论

后才能评论