激光 2D SLAM 学习

激光 2D SLAM

本文主要介绍的是基于激光雷达的 2D SLAM，以及我最近看的粒子滤波、GMapping、Cartographer 等内容。

什么是 SLAM

图 1. cartographer 建的地图

全称 simultaneous localization and mapping，顾名思义，就是在机器人构建周围环境的地图的同时，定位它在地图中的位置。

算法的输入一般是：机器人对周围环境的观测信息 z (激光雷达的扫描点、摄像头的图像等) 和 机器人的控制信息 u (运动过程中的前进转向等信息，一般对其积分后用里程计 odometry 来表示)；

算法的输出是：机器人的运动轨迹 x 和 周围的地图 m 。

而我们要做的就是在给定 z 和 u 的条件下估计出 x 和 m：

$$p(x,m|z,u)$$

这里我觉得可以稍微说一下为什么要用一个条件概率以及为什么可以用一个条件概率来表示我们的问题，因为在一开始接触机器人的时候我自己就有这样的疑惑。

首先为什么要用一个条件概率而不是一个封闭式的函数？因为我们无法对整个系统精确建模，或者说精确建模的代价太高，比如说传感器自身的测量误差（一般由厂家提供）、传感器与环境交互时的误差（激光雷达遇强光或黑色物体时的测量误差、摄像头在黑暗条件下的图像不清晰）等等，这都使我们很难用一个封闭式的函数求得一个解析解，所以我们只能用概率形式来表示在存在各种误差的情况下，使用 z 和 u 来估计 x 和 m 的分布。并且在 2d SLAM 中， x 就是一个 $[x,y,$\theta$]$ 的三维随机变量， 如果 m 是栅格地图， m 就是一个 $r*c$ 维的随机变量，其中 $r$ 是地图的宽度， $c$ 是地图的长度，所以我们可以用一个条件概率来解决我们的问题。实际上，在与传感器相关的问题中，很多问题都可以通过构建一个这样的条件概率、后验概率来求解。

难点

我们现在已经知道了 SLAM 问题应该如何表示了，那么还有什么难点在其中呢？难点就在于 x 和 m 之间是相互依赖的，也就是说：

1. 要想建图，我们需要知道机器人在各个时刻的精确轨迹信息
2. 要想获取精确的轨迹信息，我们需要借助精确的地图来定位

这也就是鸡生蛋问题。那这如何解决呢？对我们之前的 SLAM 后验分解一下看看：

图 2. 条件概率

其中建图那一项根据马尔科夫性质省略了 $u_{0:t-1}$ ，因为在已知轨迹和观测信息的情况下，控制信息对于我们的建图而言是没有影响的。这样分解的实际意义就是，先根据测量信息和控制信息计算出机器人的轨迹，然后根据轨迹和测量信息建图。其实目前大部分 SLAM 算法都按照这个思路来实现的，并且主要关注于对运动轨迹的估计和优化上，因为有了精确的轨迹，建图就成了自然而然的事了。

地图的种类

在继续介绍 SLAM 的求解之前，我们先来介绍一下 SLAM 构建的地图的种类。

我所知道的 SLAM 构建的地图一般有两类：volumetric map 和 feature map

图 3. 地图种类

其中 feature map 我没有接触过，我理解的就是在测量信息中提取特征，将这些特征设置为 landmark ，并标示这些 landmark 的位置。这个地图的好处是比较适合动态的场景，因为当 landmark 移动到另一处时，我们只需要改变这个 landmark 的坐标就可以了。

虽然我接触的一直是 volumetric map ，但我还真不知道这该怎么翻译，不过在 2d 中， volumetric map 就是 grid map，也就是栅格地图， 3d 中就是 voxel map，也就是体素地图。因为本文主要讲的是 2d SLAM ，所以我还是主要讲一下栅格地图。

图 4. 栅格地图

栅格地图主要就是把地图离散化为一个个的单元(cell)，每个单元占据 $r*r$ 个像素，所以 $r$ 的大小就代表了地图的分辨率，位置在 $(x_i, y_i)$ 的 cell 的 index 为 $i=x_i * y + y_i$。

Assumption：

1. 每个 cell 有两种状态：free、occupied
2. 每个 cell 之间的状态相互独立

当 cell 为 free 时，该 cell 的灰度值就设为 255，也就是白色，当 cell 为 occupied 时，该 cell 的灰度值就设为 0 ，也就是黑色。初始时我们不知道 cell 具体的状态，我们就将其设为 100 ，也就是灰色。

接下来我们设 $m_i$ 为第 $i$ 个 cell 是否occupied 的随机变量，那么 p(mi=1