SLAM中Gammping算法的优化

耿志文 邱慧丽 姜飞

本文简述了移动机器人运动同步定位与地图构建（Simultaneous Localization AndMapping，SLAM）模块的基本原理，介绍了几种常用的算法，并对现在最常用的Gammping算法进行了解释和优化，提出了对Gammping算法的优化方案，使用Robot Operation System（ROS）软件平台，使移动机器人拥有了更高的建图效率和更加准确的路径规划。

Perface

SLAM在移动机器人地图构建中被经常使用，使用类型和算法主要有激光和视觉2种。二者原理不同，激光SLAM依赖于激光雷达通过对周围环境进行探测从而通过珊格建图;视觉SLAM依靠前端视觉里程计，本文研究的主要是激光SLAM建图算法。Gamappmg算法相对于其他算法的优势在于可以依靠里程计更精准的建图，并且对激光传感器频率要求相对较低。当移动机器人构建现实空间较小场景时拥有高计算精度且计算量不大，所以本文重点研究对Gamapping的调优改进方案。

SLAM建图常用算法

Gmapping算法

Gmapping算法由Giorgio等人首次提出，关于算法求解的主要问题在由map坐标系计算得到odom坐标系并推算出base link坐标系与laser坐标系。Gmapping算法相较于其他算法的特点表现在其利用里程计位姿信息来减少构建小场景地图时的计算量。其中最常见的算法之一就是粒子滤波算法（Rao-Blackwelised Particle Filter，RBPF），把定位与建图时的整个过程进行了分离，直接进行定位然后再建图。RBPFSLAM的一个核心理念就是如何利用激光雷达检测信息。

Gmapping算法依靠激光雷达和里程计获取环境位姿信息，优化粒子分布并且引入自适应重采样机制，提高了算法效率。

Karto SLAM算法

Karto SLAM是Karto Robotics开发，主要思想是图优化方法。

Hector SLAM算法

Hector SLAM是一种鲁棒性比较好的算法。使用激光雷达的高更新精度来对图形进行构建，这种算法不需要里程计数据，但是对激光雷达的性能有很高的要求，需要高更新率且噪声比较低的多线激光雷达。

Hector算法通过构建栅格地图来模拟真实环境，可对任意的环境进行地图构建。当对栅格地图完成构建后还会对后续的激光雷达扫描地图来匹配扫描，Hector SLAM的匹配算法通过迭代来完成对地图的精确构建。通过确定机器人到验地图对最佳转换矩阵，ζ=[pxpyΨ]T使传感器数据和环境地图达到最佳匹配。

Cartographer SLAM算法

Cartographer是基于图优化对方法的激光SLAM算法，通过获取传感器测量数据生成分辨率为厘米级的实时栅格地图。前端包括子图扫描匹配和回环检测，同时生成子图且数据输入完成局部回环检测。后端则进行位姿估计优化实现全局闭环检测。

SLAM自主导航系统

路径规划算法

激光SLAM的几种算法通过构建栅格地图来模拟真实场景，并通过确定任意起点和任意终点来进行路径规划。路径规划分为全局和局部，通过构建全局路径来确定机器人到达终点的最优算法，局部路径规划算法对全局路径来实时动态调节，对路线进行细微的修正且检测路径中的障碍并进行避障。

全局路径规划

全局路径规划能在确定环境地图信息和初始位姿前提下来对路径进行规划。A*算法结合多种算法思想，广泛应用于各种路径规划场景，它使用贪婪最佳优先搜索（Greedy Best-First Search，GBFS）提高效率，同时使用评估函数计算最优路径。A*算法的规划路径为：

f（n）= g（n）+ h（n）

其中，g（n）表示从起始节点nstart到节点n到实际代价;h（n）表示节点n到目标节点start的估算代价;f（n）表示从起始节点n到目标节点的估算代价。

Gammping算法改进

算法优点

Gmapping的主要优势体现在对里程计位姿信息的有效利用，在坐标转换和获取最优粒子位姿势上可以极大提升效率。算法开始接受数据对地图进行初始化并获取粒子位姿计算map to odom，并进行第二次处理激光数据来进行优化，如果满足时间间隔就对地图进行更新，并用流程化处理对地图进行不断优化。而在地图初始化上首先获取激光坐标系名称然后来获得机器人坐标系下激光位姿，计算激光中心坐标作为laser系下的坐标参考从而设置激光扫描角度和激光传感器位姿、角度数组。再根据laser系下激光中心位姿得到odom下激光位姿get odom pose，此时才获得到真正有用的位姿信息，从而对地图参数进行初始化。

算法缺点

算法的主要缺点表现在构建大场景地图时往往需要的内存和计算量都会倍增，并且由于缺失回环检测可能导致地图错位。

算法改进

真实分布不能直接得到，但可以通过其他分布计算真实分布，评价标准为权重f/g，f为目标分布，g表示提议分布。粒子权值的方差会不断增加，而激光具有单峰特性会导致方差更小，可以改善基本粒子滤波算法退化问题，所以采取以下措施优化：

①增加采样点，更多的采样可以获得更多粒子，从而改善退化问题;

②增加重采樣，本质是增加粒子多样性;

③选择合理的建议密度。

本文介绍了几种常用SLAM算法，并提出了对Gmappmg算法的改进建议，通过更多采样来延缓退化并更准确地构建地图，增加重采样来增加粒子多样性，并选择合理的建议密度来减少粒子滤波器中不必要的重采样次数。