SLAM从入门到放肆——SLAM简述

SLAM

同时定位与建图（simultaneous localization and mapping，SLAM）是自动驾驶与增强现实领域中常用的技术。主要研究装置通过各种传感器在未知环境中的感知与定位问题。

归根结底，SLAM是一个状态估计问题，即根据当前的观测（图像、IMU传感器读数等），判断当前自身所处的状态（自身的6自由度位姿，所在的地点的地图结构）。

假设当前时刻k的状态为$X_k$，得到的观测为$Z_k$。则我们对于当前状态的最大后验估计如下：

$${\hat{X}}_k=\arg \underset{X_k}{\max }p(X_k)p(Z_k|X_k)$$

其中$p(X_k)$为$X_k$的先验概率，$p(Z_k|X_k)$为$Z_k$关于$X_k$的条件概率。观测与状态存在映射（比如对于场景中的三维点可以通过投影方程得到二维点作为观测），考虑观测过程中存在噪声，我们有$Z_k=h(X_k)+\delta$，其中$\delta$为噪声，根据中心极限定律，我们认为噪声是高斯分布的，于是$Z_k$符合均值为$h(X_k)$的高斯分布。同样的，假设$X_k$的先验分布为均值为${\bar{X}}_k$的高斯分布，我们可以把上式变为：

$${\hat{X}}_k=\arg \underset{X_k}{\max }[\alpha \exp (-\frac{(X_k-{\bar{X}}_k{)}^2}{2P_k})\beta \exp (-\frac{Z_k-h(X_k{)}^2}{2R_k})]$$

其中$P_k,R_k$为两个高斯分布的协方差，进而有:

$${\hat{X}}_k=\arg \underset{X_k}{\min }(||X_k-{\bar{X}}_k|{|}_{P_k}^2+||Z_k-h(X_k)|{|}_{R_k}^2)$$

我们通常称$E(X_k)=||X_k-{\bar{X}}_k|{|}_{P_k}^2+||Z_k-h(X_k)|{|}_{R_k}^2$为能量函数或代价函数。根据选取的观测及其表示方法的不同，我们往往会得到不同的能量函数。因此，SLAM技术需要解决两个问题：

1. 如何得到和组织观测量？
2. 如何最优化能量函数？

根据这两个问题，我们可以将整个SLAM问题分为前端和后端，并简单的认为前端即处理传感器的观测使之可用的过程，后端即根据观测的表示构建最优化问题并进行求解的过程。

SLAM方法分类

一般来说，我们需要对一段时间内的系统状态进行估计，从而得到场景的地图和装置的轨迹。假设从$t_0$到$t_3$时刻系统观测到了一些landmark如下图a：

其中上面的圆圈表示某一时刻的姿态，下面的圆圈表示场景中的landmark。连线表示在某一时刻对某一个landmark进行了观测。为充分的利用所有信息，正确的方法是使用所有的连线对上方所有的状态进行优化，而这显然在要求实时的情况下是不可能的，因此我们需要对问题进行简化。常用做法分为滤波式框架(b)和关键帧式的框架(c)。

基于滤波的框架仅仅保留最后一帧的系统状态和观测，并通过一些手段将此前的观测信息尽可能的融合到最后一帧的状态，从而达到实时。基于关键帧的方法通过选取一些有代表性的帧作为关键帧，只对这些关键帧进行状态估计，从而大大减少运算时间，以达到实时。鉴于目前学术界以基于关键帧的方法为主流，以下均介绍基于关键帧的方法。

前端

可以看到，对于不同的传感器我们往往有不同的前端，但是总体来说，前端主要是从传感器中提取出有用的信息并以恰当的方法进行组织的过程。
常用的传感器包括相机、IMU、GPS等。多个传感器的融合往往可以起到互补的作用。比如单目相机无法恢复场景的尺度，同时无法应对无纹理的场景。通过与IMU数据进行融合可以解决这个问题。再比如GPS可以得到装置在地球上的经纬度，这是其他传感器无法解算的。新的传感器往往可能促进新的SLAM技术的发展。

当使用多个传感器时，我们往往要对传感器之间的坐标系变换进行标定，对它们的时钟进行同步。

除此之外，对于关键帧的选择，地图点的生成等也属于前端要处理的范畴。

后端

在确定了观测之后，后端只需要求解如下的最小二乘问题：
$${\hat{X}}_k=\arg \underset{X_k}{\min }(||X_k-{\bar{X}}_k|{|}_{P_k}^2+||Z_k-h(X_k)|{|}_{R_k}^2)$$

对于线性最小二乘问题的求解已经足够成熟。参见上文，我们只需要求解如下形式的标准方程:

$$Hx=b$$

对该方程可以直接求解$x=H^{-1}b$，也可以通过矩阵分解的方法利用H矩阵的稀疏性来加快运算，常用的求解分解方法有QR分解、Cholesky分解、LDLT分解等。

然而在SLAM问题中，$h(X_k)$往往不是一个线性函数，于是我们往往需要求解一个非线性最小二乘问题。

对于非线性最小二乘的求解往往采用线性化——迭代的方法进行，常用的迭代方法有牛顿法，高斯牛顿法，LM算法，DogLeg算法等。

结束语

事实上，在实际使用中无论是前端还是后端都有更加复杂的细节。一个SLAM系统往往由很多模块组成，包括Tracking、Mapping、Loop closure、Global BA等。