3D激光SLAM的回环检测方法

一、Scan-to-Scan

1、格地图或者说是概率占据栅格地图初步

在Matlab中创建一个1000*1000大小的二维数组，如果数组中每个元素取值为0到255的整数（灰度图），再把这个数组用Matlab中的图片转换函数把数组转化为BMP图片（也可以其它你自己觉得合适的格式），这样我们就手动构建了一张地图了。直接用Windows自带的画图软件打开，可以在图上自己手动修改，这样就是手动地图修改，保存后在用Matlab读取这张BMP图片，转换为1000*1000的二维数组，这样就是地图的读取。说明：你也可以生成500*500，或者10000*10000大小，根据实际需求设置。图片中每个像素点代表一个单位距离，假如一个像素点代表0.05m，那么1000*1000大小的图片代表50m*50m大小的地图，以此类推，一个像素点也可以代表1m单位距离（距离分辨率小了）或者0.01m单位距离（距离分辨率大了）。在灰度图中0值为黑色，255为白色，可以假定0值为障碍物和255值为空（也可以反过来，随自己喜好），其它值代表这个位置是障碍物的概率或者是空的概率的大小，比如128代表既不是空也不是障碍物或者说两者的情形概率相等，比如200代表是空的概率大点，比如50代表是障碍物的概率大点。

下图为全是0值的1000*1000像素大小的地图，这里假定0值为空。

下图为自己随意画的几条线代表障碍物，假定255白色为障碍物

下两幅图为实际利用激光雷达SLAM方法生成一个大的房间的地图，两幅地图本质一样，只是表达方式取值不一样，第二幅只显示了障碍物和空地（没探索到的地方也表示空了）

对栅格地图的进一步说明

栅格地图理论可以从 概率机器人这本书中或者网上很方便的搜索到，我这边在仔细说明一下，以我自己实际应用的方式班门弄斧。

 

二、Scan to map

1、Scan to map ，即激光雷达扫描数据直接与地图进行匹配，得到实际位置坐标[x,y,theta]。这种方式一边计算位置，一边把新扫描到的数据及时加入到先前地图中。我这边主要参考论文：A Flexible and Scalable SLAM System with Full 3D Motion Estimation。这里我关心的只是二维激光雷达数据定位和建图，文献中的3D姿态估计等不涉及，只关心二维的。其实这篇文献就是Hector的理论基础。Google的Cartographer中前半部分submap建立就是用的这个方法，唯一区别就是原文献的双线性插值变成了Google的双三次插值，这么做应该是让数据更加smooth，插值方式可以随自己喜好，也可以选用其它数值计算相关书籍中的方法，这个主要是为了让数值计算更加稳定，避免出现计算崩溃，数据越smooth，计算可靠性越好。本篇讲诉比较简短，因为实现太简单了，编程很容易实现，这个前提是你要掌握非线性优化理论相关知识，因为文献中用了高斯牛顿迭代法求解。这里又出现了优化迭代方法，我在第二篇中也提出了一种高斯牛顿法简化原文献的计算思路。

2、第二篇中的Scan to scan 主要用于精确的相对位置求解，那么scan to map用来做什么呢。因为Scan to map 对于地图表示要求较高，一旦地图建错，后面将越来越错，就算地图不建错，它也是有误差在的。但是假如一张精确的地图已知，我们可以用Scan to map 给个大致定位，这个才是主要目标。而且它的定位计算量较小，因此在车速较高时也是可以达到实时的。下图就是我在十几米长宽的房间实现的一张小地图，一边Scan to map，一边把新的激光雷达数据加入地图中。地图50m*50m。

在SLAM中相关方法没有太大的区别，而且Scan to map 在slam中不是特别有主导地位，比如说用来建图还是有误差，后端还是主要图优化来建图；它主要还是在前端作用，用来即时定位，这就是它的存在吧，因为计算量还是比较小的相比Scan to Scan。以后开始我要着重讲诉图优化原理方法，把图优化理论用到激光雷达slam中可以说是激光雷达slam到了成熟期，理论上基本已经可以不需要再去研究了，最多只是计算效率怎么提升下，因为它把地图误差消除处理的非常好，回环误差或者闭环误差在图优化理论下根本无处可躲。现在主流slam都转向了视觉，更是由于神经网络，深度学习等等这几年的再一次火热，视觉slam成为了最新研究热点，我目前不会讲诉相关视觉slam，主要还是以激光雷达为主​​​​​​​