Graph-based SLAM的Cartographer系统的实现与分析

1 背景简介

Cartographer是一个Google于2016年开源的项目，它是一种在多平台和传感器配置下，提供2D和3D实时同时定位与建图（SLAM）的系统。
论文《Real-Time Loop Closure in 2D LIDAR SLAM》对Cartographer对系统进行了详细的阐述。

2 系统概述

2.1 系统总览

Cartographer主要理论是通过闭环检测来消除构图过程中产生的累积误差。用于闭环检测的基本单元是submap。一个submap是由一定数量的laser scan构成。将一个laser scan插入其对应的submap时，会基于submap已有的laser scan及其它传感器数据估计其在该submap中的最佳位置。submap的创建在短时间内的误差累积被认为是足够小的。然而随着时间推移，越来越多的submap被创建后，submap间的误差累积则会越来越大。因此需要通过闭环检测适当的优化这些submap的位姿进而消除这些累积误差，这就将问题转化成一个位姿优化问题。
当一个submap的构建完成时，也就是不会再有新的laser scan插入到该submap时，该submap就会加入到闭环检测中。闭环检测会考虑所有的已完成创建的submap。当一个新的laser scan加入到地图中时，如果该laser scan的估计位姿与地图中某个submap的某个laser scan的位姿比较接近的话，那么通过某种 scan match策略就会找到该闭环。
Cartographer中的scan match策略通过在新加入地图的laser scan的估计位姿附近取一个窗口，进而在该窗口内寻找该laser scan的一个可能的匹配，如果找到了一个足够好的匹配，则会将该匹配的闭环约束加入到位姿优化问题中。Cartographer的重点内容就是融合多传感器数据的局部submap创建以及用于闭环检测的scan match策略的实现。
总体流程图如图1所示：

2.2 局部二维的实时定位和建图

系统实现2D SLAM结合了局部的和全局的方法，局部和全局的方法都对Lidar观测到的位置进行了优化。这个位置包括(x,y)坐标的转化，以及角度的旋转，实际上就是对scan点集的优化。平台采用的是IMU测量方向，然后将其映射到2D平面上。
在局部方法中，每个连续的点集被拿来和整个地图的一部分进行匹配，就是和submap进行匹配。使用一种非线性的优化方法将submap和scan点集联系起来，这也是scan matching的过程。局部方法积累的误差在之后的全局方法去掉。
如图2所示，灰色的网格代表的是一次scan点集的范围，黑色的点表示hit（障碍物）的网格。

2.3闭环的实现

因为一个submap是由少数的几个连续scan点集生成的，所以误差很小。论文中优化所有点集和submap位置的方法使用的是Sparse Pose Adjustment。一个scan点集被插入submap中的相对位置会被存储下来，用于之后的闭环优化。除了这些位置信息，还有的包含scan点集和submap，而且这个submap不再变化的时候，都会被用来作为闭环检测。一个scan matcher一直会在后台不断的运行，当一个好的scan match被找到之后，这个相应的位置也会被加入到优化问题中。
求解方法如图3所示。

3 安装

注：这里基本是按照官方文档操作的，喜欢官方文档和英文的请移步。

3.1 系统要求

• 64-bit, modern CPU (e.g. 3rd generation i7)
• 16 GB RAM
• Ubuntu 14.04 (Trusty) and 16.04 (Xenial)
• gcc version 4.8.4 and 5.4.0
• ROS Indigo or Kinetc

3.2 建立和安装

3.2.1 安装wstool和rosdep

$ sudo apt-get update
$ sudo apt-get install -y python-wstool python-rosdep ninja-build

3.2.2 在catkin_ws文件夹中创建一个新的工作空间

$ mkdir catkin_ws
$ cd catkin_ws
$ wstool init src

3.2.3 安装proto3

$ src/cartographer/scripts/install_proto3.sh

3.2.4 合并cartographer_ros.rosinstall文件并获取依赖关系的代码

$ wstool merge -t src https://raw.githubusercontent.com/googlecartographer/cartographer_ros/master/cartographer_ros.rosinstall
$ wstool update -t src

3.2.4 安装deb依赖项

$ sudo rosdep init
$ rosdep update
$ rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y

3.2.5 构建并安装

$ catkin_make_isolated --install --use-ninja
$ source install_isolated/setup.bash

4 测试

实验数据来源为Google工作人员通过装有激光雷达、IMU和里程计设备的背包采集的数据，可在指定网站下载。

4.1 下载2D背包示例包

$ wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/cartographer_paper_deutsches_museum.bag

4.2 启动2D背包数据进行演示

$ roslaunch cartographer_ros demo_backpack_2d.launch bag_filename:=${HOME}/Downloads/cartographer_paper_deutsches_museum.bag

4.3 运行效果

5 分析与总结

从图4可以看出此系统的总体建图效果比较理想，但是与论文的效果（如图5所示）相比，部分细节效果较差，初步猜测为电脑（Lenovo Y450）性能有限，导致回环检测环节减小累积误差的性能降低，具体原因有待考证。

Cartographer是Google的开源SLAM库，并且包含了ROS接口，主要依赖的传感器是激光雷达（Lidar）和惯性导航仪（IMU），用于2D和3D的室内SLAM。
它的设计目标是为了解决在有限的计算资源下，能够实时地获取相对较高精度的2D地图，考虑到基于模拟策略的粒子滤波方法在较大环境时对内存和计算资源的高需求，Cartographer采用基于图网络的优化方法。
论文中指出，它主要的贡献并不在算法层面，而是提供了工程上高效率、Robust的代码实现。它的一个关键点在于将branch and bound（分支限界算法）引入到 loop closure的过程中，提高与之前submap比较的效率。
这个系统的目标应用场景：室内服务机器人（如扫地机器人）、无人机等计算资源有限、对精度要求不高、且需要实时避障和寻路的应用。
在无人车领域，可以首先探索在地下车库进行无人驾驶的应用。

声明

1. 这是之前的实验报告，现在只是整理成博客，参考（甚至是Copy）了很多资料，印象中有泡泡机器人微信公众号的文章以及知乎用户的评论，在此表达感谢。我以后会养成引用的习惯，尊重知识版权。
2. 去年9月份做的实验，而且Cartographer一直在更新，本教程不一定完全适用，有问题请百度或Google。