【SLAM】00_初识SLAM

1. 经典视觉SLAM框架

整个视觉SLAM流程包括以下步骤。

1. 传感器信息读取。
   在视觉SLAM中主要为相机图像信息的读取和预处理。如果是在机器人中，还可能有码盘、惯性传感器等信息的读取和同步。
2. 前端视觉里程计(Visual Odometry,VO)。
   视觉里程计的任务是估算相邻图像间相机的运动，以及局部地图的样子。VO又称为前端(Front End)。
3. 后端（非线性）优化(Optimization)。
   后端接受不同时刻视觉里程计测量的相机位姿，以及回环检测的信息，对它们进行优化，得到全局一致的轨迹和地图。由于接在VO之后，又称为后端(Back End)。
4. 回环检测(Loop Closure Detection)。
   回环检测判断机器人是否到达过先前的位置。如果检测到回环，它会把信息提供给后端进行处理。
5. 建图(Mapping)。
   它根据估计的轨迹，建立与任务要求对应的地图。
   

1.1 视觉里程计

VO是SLAM的关键问题，相对于后端优化，VO被称为“前端”
和实际的里程计相同，只计算相邻时刻的运动，和过去信息没有关联。
仅通过VO就会出现累积漂移(Accumulating Drift)。
为了解决漂移(Drift)，则需要后端优化和回环检测：

• 回环检测负责把“机器人回到原始位置”的事情检测出来；
• 而后端优化则根据该信息，校正整个轨迹的形状。

1.2 后端优化

主要用来处理SLAM过程中的噪声问题
后端优化要考虑的问题，就是如何从带有噪声的数据中估计整个系统的状态，以及这个状态估计的不确定性有多大——这称为最大后验概率估计(Maximum-a-Posteriori, MAP)。
这里的状态既包括机器人自身的轨迹，也包含地图。

1.3 回环检测

回环检测，又称闭环检测，主要解决位置估计随时间漂移的问题。

假设实际情况下机器人经过一段时间的运动后回到了原点，但是由于漂移，它的位置估计值却没有回到原点。怎么办呢？如果有某种手段，让机器人知道“回到了原点”这件事，或者把“原点”识别出来，我们再把位置估计值“拉”过去，就可以消除漂移了。

故机器人需要具备识别当前场景是否经历过的能力

可以通过判断图片之间的相似性来完成回环检测

检测到回环后，“A与B是同一点”的信息告知后端优化算法，后端将轨迹与地图调整到符合回环检测结果的样子。则消除累积误差。

1.4 建图

即构建地图的过程。

地图大致分为两类：

• 度量地图(Metric Map)：主要强调地图中物体的位置关系。
  度量地图强调精确地表示地图中物体的位置关系，通常用稀疏(Sparse)与稠密(Dense)对其分类。稀疏地图进行了一定程度的抽象，并不需要表达所有的物体。
  选择一部分具有代表意义的东西，称之为路标(Landmark),那么一张稀疏地图就是由路标组成的地图，而不是路标的部分就可以忽略。相对地，稠密地图着重于建模所有看到的东西。定位时用稀疏路标地图就足够。导航则需要稠密地图。
• 拓扑地图(Topological Map)：更加强调地图元素之间的关系。

2. SLAM问题的数学表达

使用离散时间表述连续运动，即$t=1,...,K$

各时刻位置使用$x_1,...,x_K$表示，构成运动轨迹

假设地图是由多个路标组成，设N个路标，即$y_1,...,y_N$

2.1 运动方程

运动方程表述的是机器人$k-1$到$k$时刻，位置$x$的变化，即：
$$x_k=f(x_{k-1},u_k,w_k)$$
$u_k$为运动传感器的信息输入，$w_k$为噪声，$f$为运动方程。

2.2 观测方程

表述的当机器人在$x_k$位置上看到路标$y_j$，产生一个观测数据$z_{k,j}$，即：
$$z_{x,j}=h(y_j,x_k,v_{k,j})$$
$v_{k,j}$为此次观测噪声。

2.3 位姿

由两个位置与一个转角来表述，即：
$$x_k={\left[x_1,x_2,\theta \right]}_k^T$$
输入运动指令为两个时刻之间的变化量，即：
$$u_k={\left[\Delta x_1,\Delta x_2,\Delta \theta \right]}_k^T$$
则运动方程可以具体化为：

$${\left[\begin{array}{c}{x}_1\\ x_2\\ \theta \end{array}\right]}_k={\left[\begin{array}{c}{x}_1\\ x_2\\ \theta \end{array}\right]}_{k-1}+{\left[\begin{array}{c}\Delta x_1\\ \Delta x_2\\ \Delta \theta \end{array}\right]}_k+w_k$$

2.4 状态估计问题

两个方程描述了最基本的SLAM问题：当知道运动测量的读数$u$,以及传感器的读数$z$时，如何求解定位问题(估计$x$)和建图问题(估计$y$)?

这时，我们就把SLAM问题建模成了一个状态估计问题：如何通过带有噪声的测量数据，估计内部的、隐藏着的状态变量？

2.5 状态估计问题求解

求解与方程形式与噪声分布有关

按照运动和观测方程是否为线性，噪声是否服从高斯分布进行分类，分为线性/非线性和高斯/非高斯系统。

其中线性高斯系统(Linear Gaussian,LG系统)是最简单的，它的无偏的最优估计可以由卡尔曼滤波器(Kalman Filter,KF)给出。
而在复杂的非线性非高斯系统（Non-Linear Non-Gaussian, NLNG）中，我们会使用以扩展卡尔曼滤波器(Extended Kalman Filter, EKF)和非线性优化两大类方法求解。

时至今日，主流视觉SLAM使用以图优化(Graph Optimization) 为代表的优化技术进行状态估计。优化技术已经明显优于滤波器技术，只要计算资源允许，通常都偏向于使用优化方法。