初识SLAM

初识SLAM

1、什么是SLAM？

• SLAM 是 Simultaneous Localization and Mapping 的缩写，中文译作 “同时定位与地图构建” 。它是指搭载特定传感器的主体，在没有环境先验信息的情况下，于运动过程中建立环境的模型，同时估计自己的运动。如果这里的传感器主要为相机，那就称为 “视觉SLAM”。
• SLAM 的目的是解决 “定位” 与 “地图构建” 这两个问题。也就是说，一边要估计传感器自身的位置，一边要建立周围环境的模型。
• SLAM 的典型过程是这样的：当某种移动设备（如机器人/无人机、手机、汽车等）从一个未知环境里的未知地点出发，在运动过程中通过传感器（如激光雷达、摄像头等）观测定位自身在三维空间中的位置和姿态，再根据自身位置进行增量式的三维地图构建，从而达到同时定位和地图构建的目的。

2、两类传感器

• 安装于环境中的：
  • 二维码 Marker
  • GPS
  • 导轨、磁条
• 携带于机器人本体上的
  • IMU
  • 激光
  • 相机

环境中的传感器限制了应用环境，如需要环境允许使用GPS、允许贴marker等，而 SLAM 强调未知环境，更重视携带式传感器。

3、相机

3.1 简介

• 当讨论视觉 SLAM 时，我们主要是指如何用相机解决定位和建图问题。

• 视觉 SLAM 中使用的相机与我们平时见到的单反摄像头并不是同一个东西。它更加简单，通常不携带昂贵的镜头，而是以一定速率拍摄周围的环境，形成一个连续的视频流。

3.2 相机的本质

• 以二维投影形式记录了三维世界的信息。
• 此过程丢掉了一个维度：深度（或距离）。

3.3 单目相机

按照工作方式的不同，相机可以分为单目（Monocular）相机、双目（Stereo）相机和深度（RGB-D）相机三大类。

• 单目相机：没有深度，必须通过移动相机产生深度 Moving View Stereo
• 双目相机：通过视差计算深度 Stereo
• RGB-D：通过物理方法测量深度

4、经典视觉SLAM框架

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

• 传感器信息读取。在视觉 SLAM 中主要为相机图像信息的读取和预处理。如果是在机器人中，还可能有码盘、惯性传感器等信息的读取和同步。
• 前端视觉里程计（Visual Odometry，VO）。视觉里程计的任务是估算相邻图像间相机的运动，以及局部地图的样子。VO 又称为前端（Front End）。
  • 视觉里程计能够通过相邻帧（不一定限于两帧）间的图像估计相机运动，并恢复场景的空间结构。称它为 “里程计” 是因为它和实际的里程计一样，只计算相邻时刻的运动，而和过去的信息没有关联。
  • 视觉里程计在最简单的情况下只估计两个图像间的运动，每次估计都带有一定的误差，而由于里程计的工作方式，先前时刻的误差将会传递到下一时刻，导致经过一段时间之后，估计的轨迹将不再准确，因此，仅通过视觉里程计来估计轨迹，将不可避免地出现累积漂移（Accumulating Drift）。为了解决漂移问题，需要用到两种技术：后端优化和回环检测。回环检测负责把 “机器人回到原始位置” 的事情检测出来，而后端优化则根据该信息，校正整个轨迹的形状。
• 后端（非线性）优化（Optimization）。后端接受不同时刻视觉里程计测量的相机位姿，以及回环检测的信息，对它们进行优化，得到全局一致的轨迹和地图。由于在 VO 之后，又称为后端（Back end）。
  • 后端优化主要指处理 SLAM 过程中的噪声问题。后端优化要考虑的问题，就是如何从这些带有噪声的数据中估计整个系统的状态，以及这个状态估计的不确定性有多大—这称为最大后验概率估计（Maximum-a-Posteriori, MAP）。这里的状态既包括机器人自身的轨迹，也包地图。
  • 在 SLAM 框架中，前端给后端提供待优化的数据，以及这些数据的初始值。而后端负责整体的优化过程，它往往面对的只有数据，不必关心这些数据到底来自什么传感器。
• 回环检测（Loop Closure Detection）。回环检测判断机器人是否到达先前的位置。如果检测到回环，它会把信息提供给后端进行处理。
  • 回环检测，又称闭环检测，主要解决位置估计随时间漂移的问题。
  • 在检测到回环之后，我们会把 “A与B是同一个点” 这样的信息告诉后端优化算法。然后，后端根据这些新的信息，把轨迹和地图调整到符合回环检测结果的样子。这样，如果我们有充分而且正确的回环检测，则可以消除累积误差，得到全局一致的轨迹和地图。
• 建图（Mapping）。它根据估计的轨迹，建立与任务要求对应的地图。
  • 度量地图（Metric Map）：度量地图强调精确地表示地图中物体的位置关系，通常用稀疏（Sparse）与稠密（Dense）对其分类。稀疏地图进行了一定程度的抽象，并不需要表达所有的物体。例如，我们选择一部分具有代表意义的东西，称之为路标（Landmark），那么一张稀疏地图就是由路标组成的地图，而不是路标的部分就可以忽略。相对地，稠密地图着重于建模所有看到的东西。
  • 拓扑地图（Topological Map）：相比于度量地图的精确性，拓扑地图更强调地图元素之间的关系。拓扑地图是一个图（Graph），由节点和边组成，只考虑节点间的连通性。

5、SLAM 问题的数学表达

假设有一台机器人正携带着某种传感器在未知环境里运动，现在我们将用数学语言来描述这件事。

由于传感器（如相机等）通常是在某些时刻采集数据的，因此我们可以把一段连续时间的运动变成离散时刻 \(t = 1, 2, ..., K\) 当中发生的事情。

在这些时刻，用 \(x\) 表示机器人自身的位置，于是各时刻的位置就记为 \(x_1, x_2, ..., x_K\) ，它们构成了机器人的轨迹。

地图方面，我们假设地图是由许多个路标组成的，而每个时刻，传感器会测量到一部分路标点，得到它们的观测数据。不妨设路标点一共有 \(N\) 个，用 \(y_1, y_2, ... , y_N\) 表示它们。

5.1 运动方程

通常，机器人会携带一个测量自身运动的传感器，例如码盘或惯性传感器。这个传感器可以测量有关运动的读数，但不一定直接就是位置之差，还可能是加速度、角速度等信息。有时我们也可以给机器人发送指令，例如 “前进1米” “向左转” ，或者 “油门踩到底” “刹车” 等。无论是何种情况，我们都能使用一个通用的、抽象的数学模型来说明此事：

\[x_k = f(x_{k-1},\ u_k,\ w_k) \]

这里，\(u_k\) 是运动传感器的读数或者输入，\(w_k\) 为该过程中加入的噪声。

注意，我们用一个一般函数 \(f\) 来描述这个方程，而不指明 \(f\) 具体的作用方式。这使得整个函数可以指代任意的运动传感器/输入，成为一个通用的方程，而不必限定于某个特殊的传感器上。我们把它称为运动方程。

5.2 观测方程

观测方程描述的是，当机器人在 \(x_k\) 位置上看到某个路标点 \(y_j\) 时，产生了一个观测数据 \(z_{k,\ j}\) 。同样，用一个抽象的函数 \(h\) 来描述这个关系：

\[z_{k, j} = h(y_j,\ x_k,\ v_{k, j}) \]

这里，\(v_{k, j}\) 是这次观测里的噪声。由于观测所用的传感器形式更多，这里的观测数据 \(z\) 及观测方程 \(h\) 也有许多不同的形式。

5.3 小结

针对不同的传感器，这两个方程有不同的参数化形式。如果我们保持通用性，把它们取成通用的抽象形式，那么 SLAM 过程可总结为两个基本方程：

\[\begin{cases} x_k = f(x_{k-1},\ u_k,\ w_k),\ \ \ k = 1, 2,..., K \\ z_{k, j} = h(y_j,\ x_k,\ v_{k, j}),\ \ \ (k, j) \in \Omicron \end{cases} \]

其中\(\Omicron\)是一个集合，记录着在哪个时刻观察到了哪个路标（通常不是每个路标在每个时刻都能看到的—我们在单个时刻很可能只看到一小部分）。

这两个方程描述了最基本的 SLAM 问题：当知道运动测量的读数 \(u\) ，以及传感器的读数 \(z\) 时，如何求解定位问题（估计$ x$ ）和建图问题（估计$ y$ ）？这时，我们就把 SLAM 问题建模成了一个状态估计问题：如何通过带有噪声的测量数据，估计内部的、隐藏着的状态变量。