Udacity机器人软件工程师课程笔记（三十五) - SLAM - 基于网格的FastSLAM

一.SLAM介绍

即使定位和建图问题（simultaneous localization and mapping），一般简称为SLAM， 也称作（Concurrent Mapping and Localization, CML）。当机器人没有环境地图或者不清楚自身的位置时就会产生SLAM问题。机器人所具有的信息就只有历次测量值$z_{1:t}$和控制量$u_{1:t}$。 在SLAM问题中，机器人构建地图的同时确定自生相对于地图的位置。SLAM问题远比我们目前讨论过的问题困难的多。SLAM 问题比到目前为止讨论过的机器人领域的所有其他问题都要难解决得多。该问题比定位问题更困难，是因为地图是未知的，并且不得不沿着路径估计该地图。该问题也比给定位姿构建地图要困难，因为位姿是未知的，并且不得不沿路径估计姿态。

从概率学的角度来看，SLAM问题有两种主要的形式，它们具有同等重要的地位。其一称为在线SLAM问题online SLAM problem，它估计的是瞬时位姿以及地图的后验概率:
$$p(x_t,m|z_{1:t},u_{1:t})\text{\hspace{1em}(1.1)}$$
其中$x_t$是时刻t的位姿，m则是地图，而$z_{1:t}$和$u_{1:t}$分别是历次测量值和控制量。因为只包含了时刻t 的变最估计问题，该问题被称为在线SLAM 问题。在线SLAM 问题的许多算法是增量的。在这些算法中，过去的测量值和控制量一旦被处理即被丢弃。

第二种SLAM问题成为完全SLAM问题full SLAM problem。在完全SLAM中，我们尝试计算整个路径$x_{1:t}$以及地图的后验概率，而不仅仅是当前的位置$x_t$，如图10.2所示。 $$p(x_{1:t},m|z_{1:t},u_{1:t})\text{\hspace{1em}(1.2)}$$ 在线和完全SLAM问题之间的细微差异在于，他们使用了不同类型的算法。特别的，在线SLAM问题是对完全SLAM问题的历史位姿的积分 $$\begin{gathered} p(x_t,m|z_{1:t},u_{1:t}) \\ =\int \int \cdots \int p(x_{1:t},m|z_{1:t},u_{1:t})d{x}_{1}d{x}_2\cdots d{x}_{t-1}\text{\hspace{1em}(1.3)} \end{gathered}$$

上图为在线SLAM 问题的图示模型（在线SLAM 的目标是估计机器人当前位姿和地图的后验）

上图为全SLAM 问题的图示模型（计算关千整个机器人路径和地图的联合后验）

SLAM 问题的第二个关键特点，与估计问题的本质有关。SLAM 问题拥有连
续和离散的要素。连续的估计问题涉及地图中物体的定位和机器人自身位姿变量。在基于特征的表示方式中，物体可能是地标，或者是由测距传感器探测到的某些部分。离散特性与一致性有关：当物体被检测到时， SLAM 算法推理该物体与之前被检测到的物体之间的联系。该推理过程是离散的：该物体要么与之前探测到的物体是同一个，要么不是。

有时，明确一致性变量是非常有用的，在线SLAM 后验由下式给定：
$$p(x_t,m,c_t|z_{1:t},u_{1:t})\text{\hspace{1em}(1.4)}$$

全SLAM 后验由下式给定：
$$p(x_{1:t},m,c_{1:t}|z_{1:t},u_{1:t})\text{\hspace{1em}(1.5)}$$

在线后验由全后验对过去机器人位姿积分和对所有过去一致性求和得到：
$$\begin{gathered} p(x_t,m,c_t|z_{1:t},u_{1:t}) \\ =\int \int \cdots \int \sum _{c_1}\sum _{c_2}\cdots \sum _{c_{t-1}}p(x_{1:t},m,c_{1:t}|z_{1:t},u_{1:t})d{x}_{1}d{x}_2\cdots d{x}_{t-1}\text{\hspace{1em}(1.6)} \end{gathered}$$

在两个SLAM 问题版本（即在线SLAM 和全SLAM) 中，估计全后验的
式(1. 4) 或式(1. 5) 是SLAM 问题的黄金定律。全后验捕捉所有关千地图
和位姿或路径的已知内容。
在实际应用中，计算全后验常是不可实现的。其原因有两个：①连续参数空
间的高维数；②离散一致性变量的数量巨大。许多先进的SLAM 算法用数以万计或更多的特征来构建地图。即使在已知一致性的情况下，只是这些地图的后验就包含了$10^5$甚至更多维的概率分布。这与定位问题形成了鲜明的对比，定位问题的后验是在三维连续空间上的估计。而且，在多数应用中，一致性是未知的。一致性变量$c_{1:t}$的可能分配的数量随时间呈指数增长。因此，可用的能处理一致性问题的SLAM 算法必须依赖近似。

二.SLAM的性质

• 形式
  该特征与它的两种形式有关。在线SLAM问题与地图一起计算当前姿势的后验，而完整SLAM问题与地图一起计算整个路径的后验。

• 性质
  SLAM问题的第二个关键特征与其性质有关。SLAM问题通常具有连续和离散的元素。

  • 连续
    让我们从SLAM问题的连续部分开始。
    在SLAM期间，机器人会不断收集测距信息以估算机器人的姿势，并不断感应环境以估算物体或地标的位置。因此，机器人的姿势和物体位置都是SLAM问题的连续方面。

  • 离散
    现在，转到SLAM问题的第二部分。
    正如之前提到的，机器人不断地感知环境以估计对象的位置，这样做时，SLAM算法必须识别在任何新检测到的对象与先前检测到的对象之间是否存在关系。这有助于机器人了解它之前是否曾经在同一位置。在每个时刻，机器人都必须回答以下问题：“我以前来过这里吗？”。这个问题的答案是二进制-是或否-这就是使对象之间的关系成为SLAM问题的离散组成部分的原因，即对应关系已知对象之间的这种离散关系。

SLAM挑战

在SLAM下，计算由机器人姿态，地图和对应关系组成的整个后验对机器人技术提出了很大的挑战，这主要是由于连续和离散的部分。

连续
由机器人姿势和对象位置组成的连续参数空间是高维的。在绘制环境并进行自身定位时，机器人会遇到许多物体，并且必须跟踪其中的每个物体。因此，变量的数量将随时间增加，这使问题具有高度的维度，并且难以计算后验。

离散
接下来，离散参数空间由对应值组成，并且由于大量的对应变量而具有很高的维数。不仅如此，由于机器人将继续感测环境并将新检测到的对象与先前检测到的对象相关联，因此对应值随时间呈指数增长。即使假设已知的对应值，在地图上的后验也仍然是高维的。

因此，SLAM算法在估计后验时必须依赖于近似，以节省计算内存。

三、FastSLAM

1.FastSLAM简介

FastSLAM算法解决了全部问题SLAM已知对应。

• 估计轨迹： FastSLAM使用粒子过滤器方法估计轨迹的后验。这将为SLAM提供一个优势，以解决已知姿势的映射问题。
• 估计地图： FastSLAM使用低维扩展卡尔曼滤波器来求解使用局部高斯模型建模的地图的独立特征。

用Rao-Blackwellized粒子过滤器方法已知用粒子过滤器和高斯表示后验的惯用方法。

我们已经看到，FastSLAM算法可以解决具有已知对应关系的完整SLAM问题。由于FastSLAM使用粒子滤波器方法来解决SLAM问题，因此一些机器人专家认为它是一种能够解决Full SLAM和Online SLAM问题的强大算法。

FastSLAM估计完整的机器人路径，因此可以解决Full SLAM问题。
另一方面，FastSLAM中的每个粒子都估计瞬时姿势，因此FastSLAM也解决了在线SLAM问题。

现在，存在FastSLAM算法的三个不同实例。

• FastSLAM 1.0
  FastSLAM 1.0算法既简单又易于实现，但是由于粒子滤波器会导致样本效率低下，因此该算法效率不高。

• FastSLAM 2.0
  FastSLAM 2.0算法通过施加不同的分布来克服FastSLAM 1.0的效率低下问题，这导致了较少的粒子数量。请记住，FastSLAM 1.0和2.0算法都使用低维扩展卡尔曼滤波器来估计地图特征上的后验。

• 基于网格的FastSLAM
  FastSLAM的第三个实例实际上是对FastSLAM的扩展，称为基于网格的FastSLAM算法，该算法使FastSLAM适应于网格图。在本课程中，您将学习基于网格的FastSLAM。

2.基于网格的FastSLAM技术

为了使FastSLAM适应网格映射，我们需要三种不同的技术：

1. Sampling Motion- $p(x_t|x_{t-1}^{[k]},u_t)$：给定第k个粒子的先前姿态，并根据当前控制$u$估算当前姿态。
2. Map Estimation- $p(m_t|z_t,x_t^{[k]},m_{t-1}^{[k]})$：根据当前测量值，当前第k个粒子姿态和先前的第k个粒子图估计当前图
3. 重要性权重- $p(z_t|x_t^{[k]},m^{[k]})$：根据当前第k个粒子姿态和当前第k个粒子图，估计当前测量的可能性。

基于网格的FastSLAM算法

3.gmapping ROS包

gmapping提供基于激光的SLAM。这意味着可以向其节点提供机器人激光测量值和里程计值，并期望它提供环境的2D占用栅格图。当机器人移动并使用其激光测距仪传感器收集感官信息时，地图将更新。

该软件包包含用于OpenSlam Gmapping的ROS包装器。gmapping软件包提供了基于激光的SLAM（同时定位和映射），作为称为slam_gmapping的ROS节点。使用slam_gmapping，可以根据激光和移动机器人收集的数据来创建二维占用栅格图（如建筑物平面图）。

更多参考此处。

创建catkin_ws

$ mkdir -p ~/catkin_ws/src
$ cd ~/catkin_ws/src
$ catkin_init_workspace
$ cd ..
$ catkin_make

执行系统更新/升级

$ apt-get update
$ apt-get upgrade -y

报错：

W: GPG error: http://packages.ros.org trusty InRelease: The following signatures couldn't be verified because the public key is not available: NO_PUBKEY F42ED6FBAB17C654

解决办法：

sudo apt-key adv --keyserver keyserver.ubuntu.com --recv-keys <PUBKEY>

这里的<PUBKEY>就是丢失的public key。例如F42ED6FB17C654。

在src中应用turtlebot_gazebo和turtlebot_teleop

$ cd src/
$ git clone https://github.com/turtlebot/turtlebot_simulator
$ git clone https://github.com/turtlebot/turtlebot

安装软件包依赖项

$ cd ..
$ source devel/setup.bash
$ rosdep -i install turtlebot_gazebo
$ rosdep -i install turtlebot_teleop

编译软件包

$ catkin_make
$ source devel/setup.bash

在Willow Garage环境中部署Turltebot

$ roslaunch turtlebot_gazebo turtlebot_world.launch world_file:=worlds/willowgarage.world

现在，将gmapping包与机器人对接以执行SLAM。

解决办法：

$ cd ~/
$ hg clone https://bitbucket.org/osrf/gazebo_models

下载完成后讲gazebo_models复制到~/.gazebo文件夹中，重命名为models

Terminal 1
在Willow环境中启动Turtlebot

$ cd ~/catkin_ws
$ source devel/setup.bash
$ roslaunch turtlebot_gazebo turtlebot_world.launch world_file:=worlds/willowgarage.world 

Turtlebot should now appear in a Willow Garage environment.

Terminal 2
运行 keyboard teleop node

$ cd ~/catkin_ws
$ source devel/setup.bash
$ roslaunch turtlebot_teleop keyboard_teleop.launch

Don’t move your robot yet!

Terminal 3
运行 slam_gmapping node

$ cd ~/catkin_ws
$ source devel/setup.bash
$ rosrun gmapping slam_gmapping

Terminal 4
运行rviz并订阅不同的已发布主题以可视化地图

$ rosrun rviz rviz

如下编辑rviz配置：

将固定框架更改为map
保持参考系为默认
添加机器人模型
添加相机并选择/camera/rgb/image_raw主题
添加地图并选择/map主题

Terminal 5

$ cd /home/workspace/
$ rosrun map_server map_saver -f myMap

运行map_server将生成map.pgm和map.yaml文件：

但是我发现我的rviz一直无法获得地图。猜测可能是tf信息转换有问题，我使用了他给的整合包无法解决这个问题。

整合包在此处。