cartographer提供了实时并且可视化的建图，能够帮助操作员很好的评估建图质量和查看构建地图的范围，文章提供了一种5cm分辨率、实时建图和闭环的一种方法。

一. 介绍INTRODUCTION

本文的贡献：提供了一种新颖的计算闭环约束的方法，能够降低对大量激光数据的计算要求。该方法可以构建数万平方米的地图。

二. 相关工作

scan-to-scan

常用方法，但是容易积累误差。

scan-to-map

可以有效的减少累计误差。
其中一个代表性方法为利用高斯牛顿法查找最佳插值，详见下面的论文：

S. Kohlbrecher, J. Meyer, O. von Stryk, and U. Klingauf, “A flexible and scalable SLAM system with full 3D motion estimation,” in Proc. IEEE International Symposium on Safety, Security and Rescue Robotics (SSRR). IEEE, November 2011.

如果地面不够平坦，可以使用IMU单元对重力进行估计，然后投影到水平面建图。

pixel-accurate

像素级的扫描匹配，进一步的减少了局部的累计误差。尽管增加了计算量，但是对于闭环检测还是很有用的。该方法可以参考下面的论文：

E. Olson, “M3RSM: Many-to-many multi-resolution scan matching,” in Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), June 2015.

一些方法对计算量进行了改进，比如提取特征点进行匹配：

F. Mart´ın, R. Triebel, L. Moreno, and R. Siegwart, “Two different tools for three-dimensional mapping: DE-based scan matching and feature-based loop detection,” Robotica, vol. 32, no. 01, pp. 19–41, 2014.

还有一些方法基于直方图的匹配方式进行闭环检测，比如：

M. Himstedt, J. Frost, S. Hellbach, H.-J. Bohme, and E. Maehle, ¨ “Large scale place recognition in 2D LIDAR scans using geometrical landmark relations,” in Intelligent Robots and Systems (IROS 2014), 2014 IEEE/RSJ International Conference on. IEEE, 2014, pp. 5030– 5035.

还有一些闭环检测是基于特征检测并结合机器学习，比如：

K. Granstrom, T. B. Sch ¨ on, J. I. Nieto, and F. T. Ramos, “Learning to ¨ close loops from range data,” The International Journal of Robotics Research, vol. 30, no. 14, pp. 1728–1754, 2011.

粒子滤波和图优化

粒子滤波中每个粒子必须都携带整个系统地图，对于基于网格的方式，如果地图很大的话，将会消耗大量的资源。

图优化中，根据观测结果生成约束，可以利用不同的优化方法对所有的约束进行优化，来减少约束带来的误差。

三. 系统总览

cartographer可以构建一个5cm分辨率的2D栅格地图，在移动过程中实时构建地图，雷达扫描数据(scans)经过位置估计后被插入子图(submap)，这个过程在短时间内被认为精度足够高。扫描匹配只发生在最近的子图，所以它依赖于附近的扫描数据，这个过程会产生累计误差。

为了解决累计误差，我们定期运行位姿优化。当一个子图完成之后，就不会再有新的扫描数据插入，这个子图将被加入到闭环检测当中。所有的子图和扫描数据（submaps and scans）将自动加入到闭环检测。如果待检测的值距离现在的位姿估计足够近，则扫描匹配器将会在submap中找出相应的scan，如果找到了，将会增加一个闭环约束到优化问题中。

所以闭环的扫描匹配必须比新扫描数据加入得更快一些（被加进来了，还怎么识别相同位置?），文章中加入了分支定界法和几个预先计算好的网格来实现这个功能。

四. 局部SLAM

局部slam和全局slam都是优化位姿pose： $ξ = (ξ_x, ξ_y, ξ_θ)$ 。位姿表示激光观测值的平移和旋转，也是指scans。在3D中我们还会增加imu进行重力估计，来进行2D的投影。

在局部的扫描匹配中，每个scan都会对子图 $M$ 进行匹配，并且使用非线性优化方法使得scan和submap对齐。在扫描匹配过程中会积累误差，误差将会在全局方法中解决。

4.1 Scans

scan的原点在 $0 ∈ R^2$ ，那么scan的每个点坐标为 $H ={h_k}_{k=1,...,K}, h_k ∈ R^2$ 。假设scan坐标系的位姿 $ξ$ 在submap坐标系表示为 $T_ξ$ ，其实也就是将scan points从scan坐标系转换到submap坐标系：
在这里插入图片描述

4.2 submaps

连续的scans组成了一个submap，子图由概率网格组成 $M：rZ*rZ\rightarrow [p_{min},p_{max}]$ ，其中分辨率 $r = 5 c m$ 。概率小于 $p_{min}$ 则认为无障碍物，大于 $p_{max}$ 则认为有障碍物，在之间则认为未知。定义网格中的所有点为 $p i x e l$ 。
在这里插入图片描述

对于每一个添加到概率网格的scan都会计算 $h i t s$ 和 $m i s s e s$ 集合。例如网格点 $x$ 已经被观测到了，则：
在这里插入图片描述
$m i s s e s$ 也会有同样的等式。

4.3 ceres scan matching

将scan插入子图之前，scan的位姿pose $ξ$ 都会使用ceres的扫描匹配器进行优化。扫描匹配器的作用是使得scan位姿在子图中的概率最大，这是一个非线性优化问题：
在这里插入图片描述
其中 $T_ξ$ 是 $h_k$ 从scan坐标系到子图坐标系的变换，这个和scan位姿坐标系变换是一样的。 $M_{smooth} :R^2 → R$ 是一个局部子图的概率值的平滑形式。这里我们使用双三插值。

这个平滑函数的数学优化往往比网格的精度更高。因为这是一个局部优化的问题，所以我们需要一个很好的初始估计。imu得出的角速度可以很好的估计扫描匹配中的旋转角 $θ$ 。在没有imu的情况下，高频的扫描匹配或者 $p i x e l$ - $a c c u r a t e$ 扫描匹配方法也可以使用。

五. 回环

因为scans仅仅匹配包含了一些scans的submap，在短时间内被认为误差足够小，但是在随着建图的进行误差也会慢慢变大。这里我们使用SPA(Sparse Pose Adjustment)的方法对scans和submaps的位姿进行优化，这个方法可以查看以下论文：

K. Konolige, G. Grisetti, R. Kummerle, W. Burgard, B. Limketkai, ¨ and R. Vincent, “Sparse pose adjustment for 2D mapping,” in IROS, Taipei, Taiwan, 10/2010 2010.

被插入的scans的相对位姿被保存(stored in memory)下来用于闭环优化。扫描匹配器在后台运行，一旦找到了闭环，相应的相对位姿将被加入到闭环优化问题中。

5.1 优化问题

闭环优化问题也是一种非线性最小二乘问题，每过几秒，我们使用ceres计算下面的值：
在这里插入图片描述
其中：
$Ξ^m = {ξ^m_i}_{i=1,...,m}$ ：submap在世界坐标系的位姿。
$Ξ^s = {ξ^s_j}_{j=1,...,m}$ ：scan在世界坐标系的位姿。
$ξ_{ij}$ ：submap和scan的约束。
$Σ_{ij}$ ：协方差矩阵。
对于scan $j$ 和submap $i$ ， $ξ_{ij}$ 描述了submap坐标系下的scan扫描匹配的位姿。协方差矩阵可通过下面的论文计算，也可以通过Ceres的（CS）协方差估计功能。

E. B. Olson, “Real-time correlative scan matching,” in Robotics and Automation, 2009. ICRA’09. IEEE International Conference on. IEEE, 2009, pp. 4387–4393.

残差 $E$ 通过下面的公式计算：

在这里插入图片描述
损失函数 $\rho$ ，例如 $Huber\ loss$ ,它的作用是，当扫描匹配加入错误的约束到优化问题时，可以减小SPA中的离群值( outliers)影响。例如办公室的对称环境。离群值的替代方案包括下面论文中的方法：

P. Agarwal, G. D. Tipaldi, L. Spinello, C. Stachniss, and W. Burgard, “Robust map optimization using dynamic covariance scaling,” in Robotics and Automation (ICRA), 2013 IEEE International Conference on. IEEE, 2013, pp. 62–69.

5.2 分支定界扫描匹配

在优化的时候，我们使用 $pix\ accurate$ 的方式进行匹配搜索， $W$ 表示搜索框， $M_{nearest}$ 表示最近的网格数据。
在这里插入图片描述
搜索匹配的质量也可以通过(CS)进行优化。

在这里搜索的角度步长用 $δ_θ$ 表示，扫描点之间的最大距离 $d_{max}$ 不大于分辨率 $r$ ,使用cos法则，我们得到：
在这里插入图片描述
对搜索框（linear和angular的搜索框）进行积分，比如搜索框大小为 $W_x=W_y=7m$ , $W_θ=30^o$ 。其中：

这样我们就得到了一个在中心位置估计 $ξ_0$ 附近的有限搜索集 $W$ ：

当然我们可以对BBS进行暴力搜索，但是这个算法太慢了。这个算法如下：
在这里插入图片描述

我们这里不使用Algorithm 1，使用的是分支定界法对 $ξ^*$ 进行计算，算法步骤参考Algorithm 2。这个算法是在下面的论文被第一次提出的：

A. H. Land and A. G. Doig, “An automatic method of solving discrete programming problems,” Econometrica, vol. 28, no. 3, pp. 497–520, 1960.

在这里插入图片描述
主要思想：树中的节点表示可能性的子集，根节点表示所有可能性，子节点是父节点的一部分。只要保证在节点c的内部， $s c o r e (c)$ 是它的得分上限，只要一个节点有界，那么就不存在比父节点更好的子节点。
下面介绍具体流程：

1. 节点选择

本文使用深度优先搜索算法（DFS,depth-first search）,算法的效率取决于树修建的部分，这主要依赖于两部分：好的上界和好的解决方案。这里的解决方案为DFS，它可以快速的对叶节点进行评估。为了不增加坏的匹配约束，我们引进了分数阈值(score threshold)。DFS的分支节点扫描匹配算法如Argorithm 3.
在这里插入图片描述

2. 分支规则

每个节点都由证书数组构成： $c = (c_x, c_y, c_θ, c_h) ∈ Z^4$ 。节点的高度为 $c_h$ 有 $2^{c_h} ×2^{c_h}$ 的旋转可能：
在这里插入图片描述
叶子节点的高度 $c_h=0$ ，并且对应的可行解为 $\ni ξ_c = ξ_0 + (rc_x, rc_y, δ_θc_θ)$ 。

在算法3中，包含所有可行解的根节点没有显示，而是出现在高度为 $h_0$ 的初始节点 $C_0$ 中，它的搜索窗口为：
在这里插入图片描述
对于任意一个高度 $c_h>1$ 的节点 $c$ ，都会有四个高度为 $c_h-1$ 的孩子节点。

3. 计算上界
在内部计算上界的方法：

为了最大化效率，我们使用预计算网格 $M_{precomp}^{c_h}$ ，同时 $\overset{-}{W_c}$ 也会被取代。
在这里插入图片描述