SLAM第二讲笔记

---

一、名词解释

1. IMU(Inertial Measurement Unit)：惯性测量单元，测量运动的角速度和加速度
2. Scale：尺度，物体的远近只是个相对值，无法确定真实尺度；单目SLAM估计的轨迹和地图与真实的轨迹与地图相差一个因子（场景的维度，也就是深度（或距离）），也就是所谓的尺度。
3. RGB-D：深度相机。
4. 位姿：位置和姿态，由两个位置和一个转角，即：
   $$x_k=[x_1,x_2,\theta {]}_k^T$$
   其中，$x_1$ $x_2$ 是两个轴上的位置而θ为转角。

---

二、定位与建图

为什么要定位和建图？

SLAM系统是为了使机器人具有自主运动能力，自主运动能力是许多高级功能的前提。为了使机器像人一样能够感知周边的环境，所以至少要明白自身的状态（即位置）和外在的环境（即地图）。如何实现定位和建图呢？分两步，首先在机器人上安装相机（激光等传感器）获取周边环境数据，然后通过SLAM系统完成机器人定位和建图。

定位和建图的解决方案

解决定位与建图的方法很多。大致可以分为两类

1. 安装于环境中的传感器，比如导轨、二维码标志等。安装于环境的传感设备，通常能够直接测量机器人的位置信息，简单有效的解决定位问题。然而，由于它们要求环境必须人工布置，在一定的程度上限制了机器人的使用范围。这类传感器约束了外部环境。只能在这些约束满足时，基于它们的定位方案才能工作。反之，当约束无法满足时，我们无法进行定位。虽然这类传感器简单可靠，但它们无法提供一个普遍的、通用的解决方案。
2. 在机器人上的传感器，比如激光、相机、轮式编码器、IMU等，它们测量的通常都是间接的物理量而不是直接的位置数据。例如。轮式编码器会测量轮子转动的角度，相机和激光传感器则读取外部环境的某种观测数据。我们可以通过一些间接的手段，从这些数据推算自己的位置。它们没有对环境提出任何要求，从而使这种定位可适用于未知环境。

SLAM要能够在未知环境中实现对机器人控制和规划，所以主要研究间接物理量完成定位与建图

---

三、 单目相机

只有一个摄像头进行SLAM的叫单目SLAM，照片是单目相机最常见的数据。照片的本质是拍摄的某个场景在相机的成像平面上留下的投影，以二维的形式记录三维的世界。二维的场景中丢失了深度，也就是距离。我们无法通过单张的图片计算场景中的物体与相机的距离。这个距离是SLAM中非常关键的信息。
单目相机拍摄的图像只是三维空间 的二维投影。所以，要想恢复三维结构，必须改变相机的视角。我们必须移动相机，才能估计它的运动，同时估计场景中物体的远近和大小，不妨称之为结构。怎么估计运动和结构呢？想象你座在火车上：

1. 列车往右移动，那我们看到的东西就会往左移动，这就给我们推测运动带来了信息。
2. 近处的物体移动快，远处的物体移动慢，极远处的物体（比如太阳）基本不动。

当相机移动时，这些物体在图像上的运动就形成了视差。通过视差判断物体远近。即使我们知道了远近，它们任然是个相对值。比如在看电影时，虽然能知道电影的场景哪些比另一些大，但无法确定电影里那些物体的真实尺度：是真的高楼还是桌上的模型。如果把相机的运动和场景同时放大两倍，单目相机看到的是一样的。说明单目SLAM估计的轨迹和地图与真实的轨迹与地图相差一个因子，也就是“尺度”，又称“尺度不确定"。
平移后才能计算深度，以及无法确定真实尺度。其根本原因通过单张相片无法确定深度，为了得到这个深度，人们开始使用双目相机和深度相机。

四、 双目相机和深度相机

为了克服单目相机无法知道距离的缺点，双目相机和深度相机通过算法等手段测量物体和相机之间的距离，可以消除尺度不确定性，场景的三维结构就可以在单个图片中恢复。双目相机和深度相机测量距离的原理不同。

1. 双目相机：两个相机之间的距离（基线）是已知的，我们通过基线来估计每个像素的空间位置。双目相机测量到的深度范围和基线相关，基线距离越大，能够看到的物体就越远。双目相机估机的距离估计是比较左右相机图像获取的。缺点是配置与标定复杂，深度量程和精度受到基线与分辨率限制，视差的计算非常消耗计算机资源，需要使用GPU、FPGA设备加速，才能实时输出整张的距离信息。
2. 深度相机：通过红外结构光或Time-of-Flight（tof）原理，像激光传感器那样，通过主动向物体发射光并接收返回的光，测出物体和相机之间的距离。不是双目相机那样通过软件计算距离，而是通过物理测量手段，所以可以减少大量的计算开销。缺点还存在测量范围窄，噪声大，视野小，易受日光干扰，无法测量透射材质等，主要用在室内。

五、SLAM框架

1.传感器读取信息

在视觉SLAM中主要为相机图像信息的读取和预处理。如果在机器人中，还可能有码盘、惯性传感器等信息的读取和同步。

2.前端视觉里程计

视觉里程计的任务是估算相邻间图像之间相机的运动，及其局部地图的样子。

1. 图像在计算里只有一个数值矩阵。在视觉SLAM中，我们只看到一个个像素，知道它们是某些空间点在相机的成像平面上的投影的结果。为了定量的估计相机运动，必须先了解相机与空间的几何关系。视觉里程计能够通过相邻帧间的图像估计相机运动，并恢复场景空间结构。称它为“里程计”是因为它和实际的里程计一样，只计算相邻时刻的运动，而和过去的信息没有关联。在这一点上，视觉里程计就像一种只有短时记忆的物种（不过可以不限于两帧，数量可以更多一些，例如5~10帧）。
2. 假设我们已有了一个视觉里程计，估计两张图像间的相机运动。那么，一方面，只要把相邻时刻的运动“串”起来，就构成了机器人的运动轨迹，从而解决了定位问题。另一方面，我们根据每个时刻的相机位置，计算出各像素对应的空间点的位置，就得到了地图。
3. 通过视觉里程计来估计轨迹，将不可避免出现累计漂移。在最简单的情况下只估算两个图像间的运动造成的。每次估计都带有一定的误差，而由于里程计的工作方式，会把先前时刻的误差将会传递到下一时刻，导致经过一段时间之后，估计的轨迹将不再准确。例如：机器人在原点往前走100cm,又往后走100cm,由于误差我们把前走100cm计算成99cm，机器人并没回到原点。多次计算出现误差，就会造成累计漂移。所以需要后端优化和回环检测。

3.后端（非线性）优化

后端接收不同时刻视觉里程计测量的相机位姿，以及回环检测信息，对它们进行优化，等到全局一致的轨迹和地图。

1. 数据都是由传感器得到，传感器带有噪声，还会受到磁场、温度等环境的影响。除了要如何从图像估计出相机的运动，还有关心这个估计带有多大噪声。这些噪声是如何从上一个时刻传递到下一个时刻的，就是如何从这些噪声的数据中估计整个系统的状态，以及这个状态估计的不确定性由多大，这称为最大后验概率估计（Maximum-a-Posteriori,MAP)。这里的状态即包括机器人自身运动轨迹，也包含地图。
2. 视觉里程计部分有时被称为“前端”。在SLAM框架中，前端给后端提供优化的数据，以及这些数据的初始值。后端负责整体的优化过程，它往往面对的是数据，不必关心这些数据，以及这些数据到底来至什么传感器。在视觉SLAM中，前端和计算机视觉研究领域更为相关，比如图像的特征提取与匹配等，后端则主要是滤波与非线性优化算法。
3. SLAM的本质：对运动主体自身和周围环境空间不确定性的估计。

4. 回环检测

回环检测判断机器人是否先到达过先前的位置。如果检测到回环，它会把信息提供给后端进行处理，又称闭环检测。

1. 主要解决位置估计随时间漂移的问题。比如：机器人经过一段时间的运动后回到原点，由于飘逸，它的位置值却没有回到原点。让机器人知道回到原点，或者把原点识别出来在把位置估计值拉过去，就可以解决漂移，这就是回环检测。
2. 地图存在的目的主要意义是让机器人知晓自己到过的地方，为了实现回环检测，我们需要机器人具有识别到过的场景的能力，可以通过判断图象间的相似性来完成回环检测，所有视觉回环检测实际上是一种计算图像数据相似性的算法。
3. 在检测到回环之后，会把A与B是同一个点这样的信息告诉后端优化算法。然后，后端根据这些新的信息，把轨迹和地图调整到符合回环检测结果的样子。如果我们有充分而且正确的回环检测，则可以消除累计误差，得到全局一致的轨迹和地图。

5.建图

它根据估计的轨迹，建立与任务要求对应的地图。地图是对环境的描述，但这个描述不是固定的，需要根据SLAM的应用场合而定的。

度量地图

强调精确地表示地图中物体的位置关系，通常用稀疏（Sparse)和稠密（Dense)对其分类。稀疏地图进行一定程度的抽象，并不需要表达所有的物体。例如，我们选择一部分具有代表意义的东西，称之为路标。那么一张稀疏地图就是由路标组成的地图，而不是路标的部分就可以忽略。相对的稠密地图着重于建模所有看到的东西。定位时用稀疏地标就足够了。而用于导航时，则往往需要稠密地图（否则撞上两个路标的墙怎么办？）。稠密地图通常按照某种分辨率，由许多小块组成，在二维度量地图中体现为许多个小格子（Grid）,而三维度量地图中则体现为许多小方块（Voxel）。通常，一个小块含有占据、空闲、未知三种状态，以表达该表格内是否有物体。当查询某个空间位置时地图能够给出该位置是否通过的信息。这样的地图可以用于各种导航算法。

拓扑地图

相比于度量地图的精确性，拓扑地图更强调地图元素之间的关系。拓扑地图是一个图（Graph），由节点和边组成，只考虑节点间的连通性，例如只关注A、B点是连通的，而不考虑如何从A点到达B点。它放松了地图对精确位置的需要，去掉了地图的细节，是一种更为紧凑的表达方式。然而，拓扑地图不擅长表达具有复杂结构的地图。如何对地图进行分割，形成节点与边，如何使用拓扑地图进行导航与路径规划。

六、SLAM中的数学

假设机器人在陌生的环境中运动，相机是时刻采集数据的。这些时刻的位置和地图，连续时间的运动可以在离散时刻 t = 1,2,3…n。位置用X表示，地图路标用Y表示，则这些时刻对应的位置为X1，X2…Xn，路标为Y1，Y2…Yn
通过这样的设定，我们可以描述以下信息：

1. 运动：从（n-1）时刻到n时刻，机器人的位置x的变化。
   机器人会携带测量自身运动的传感器（如码盘、惯性传感器等）。当我们给机器人发送指令时，比如前进1米。可以用一个通用、抽象的函数表达：
   $$x_n=f(x_{n-1},u_k,w_k)$$ 这里的$u_k$ 是运动传感器的读数或者输入。$w_k$是噪声。我们用一般的函数$f$来描述这个过程，不指明$f$的具体作用方式。整个函数可以指代任意的运动传感器/输入，成为一个通用的方程，而不必限定于某个特定的传感器上。我们把它称为运动方程。
2. 噪声的存在使得这个模型变的随机性。例如，我们下达了前进1米的指令，实际行走了0.9米或者1.1米。每次运动中大的噪声是随机的。与运动相对应的是观测方程。观测方程描述的是，当机器人在$x_i$位置上看到某个路标$y_j$时，产生一个观测数据$z_{ij}$。同样，用一个抽象的函数h来描述这个关系：
   $$z_{i,j}=h(y_j,x_i,v_{i,j})$$
   这里$v_{i,j}$是这次观测的噪声。由于观测的传感器形式很多，这里的观测数据$z_{i,j}$和$h$也有许多不同的形式。

我们用函数$f$,$h$，并没有具体的说明运动和观测是怎么回事。机器人的真实运动和传感器的种类，存在着若干种参数化的方式。例如：机器人在平面运动中，它的位姿由两个位置和一个转角来描述，即$x_k=[x_1,x_2,\theta {]}_k^T$，其中，$x_1$ $x_2$ 是两个轴上的位置而θ为转角。同时，输入的指令是两个时间间隔位置和转角的变化量$u_k=[\Delta x_1,\Delta x_2,\Delta {\theta }_1,{]}_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$$
这是简单的线性关系。并不是所有的输入都是位移和角度的变化量。比如速度和加速度。其他更复杂的形式，就可能需要动力学分析了。