|期刊分享|SLAM|不到200行C代码的CoreSLAM

编者序：本文给出了不到200行代码的激光雷达SLAM算法，令人惊奇的是效果还挺好，源码可在OpenSLAM上下载，也可在该文献末尾处查看。机器人传感器的配置、计算资源及软件配置都值得参考，有多少扫地机器人参考了该配置呢。

推荐指数☆☆☆☆☆

一、引言

本文提出了一种名为CoreSLAM的新SLAM算法，是为Mines ParisTech的CoreBots机器人系统开发的。之所以叫CoreSLAM，是由于它的C代码实现不到200行，且在试验中取得了很好的性能。

二、前期工作

SLAM有大量的研究者，可根据使用的传感器类型或采用的计算方法进行分类。有基于单个相机或几个相机的基于视觉的SLAM算法和基于激光传感器或声呐的SLAM算法。基于视觉的算法又可分为基于卡尔曼滤波的算法、基于粒子滤波的算法，还有像FastSLAM中结合二者的算法。DP-SLAM是最有名的基于粒子滤波的激光SLAM算法，利用粒子滤波维持机器人位姿和地图联合分布。但DP-SLAM问题是很难集成已有的粒子滤波定位子系统。

回顾SLAM算法时发现，大多数的算法都是在慢速机器人上测试，它们速度几乎没有超过1m/s，并且使用了非常昂贵的激光雷达，如作用距离100米的SICK和IBEO激光雷达等。我们的机器人速度达到3m/s，所使用的激光雷达Hokuyo URG04 作用距离。还有，大多数的SLAM算法使用了复杂的数学工具需要编写很多行代码，需要大量的调参等工作。我们的工作目标是：设计简单算法，理解起来简单但是性能很好，并且更重要的是能够与已有的粒子滤波定位框架简单集成。还有最后一个要求：该算法嵌入在终端内，需要占用内存上且在关键循环代码中使用整数计算。

三、算法

相比DP-SLAM，我们决定仅使用一副地图。DP-SLAM由于CoreSLAM的地方在于在较长走廊不丢的理论能力，这确实是每个粒子地图理念的目标，而不是DP-SLAM无法实现的闭环检测。事实上，我们认为该能力引入的复杂度不值，因为我们平台上有可靠的里程计，我们的目标是小闭环检测。

由于CoreSLAM思想是在基于粒子滤波的定位子系统中集成激光扫描信息，我们写了两个主要函数：扫描地图距离函数ts_distance_scan_to_map，计算滤波器中每个状态位置假设的相似性，仅仅包括在所有扫描点的map值得简单相加，仅仅使用了整数运算，速度很快，实时运行时并不需要约束粒子的数量，然而，该估计相似性方法意味着地图以特殊方式重建。地图更新函数，用于随机器人移动构建地图。

构建与粒子滤波兼容的地图并不简单，似然函数的峰值非常重要应该易于控制，我们通过灰度级地图实现，地图更新小孔宽度由似然函数峰值决定。对于障碍物检测，算法并不画出单个点，而是在障碍物处挖个坑，如下图所示。因此，构建的地图鱼传统的地图不同，而是看起来像带有孔的表面。地图集成通过alpha beta滤波器实现，使得地图收敛到新的状态。在粒子滤波的最后（似然函数计算后所有粒子加权平均）调用地图更新，延迟latency可选。

ts_map_update函数使用了最有技巧性的ts_map_laser_ray函数，后者使用Bresenham算法在地图上画出激光线，内含了另一种增强bresenham算法以计算正确的形状。关键部分使用整数计算且没有整数除法。由于每一步（10Hz）都计算，要求该过程很快，因为地图分辨率很高（0.01m）.

CoreSLAM很容易集成在粒子滤波中，也可独立运行。重建的带有小孔和斜坡的地图，可以使用任何梯度下降算法。由于小孔的存在，匹配算法很快朝着障碍物收敛。我们的stand-alone版本使用简单的蒙特卡洛算法匹配最新扫描和地图，计算机器人位置更新。Stand-alone版本是调参用的。

基于粒子滤波的版本需要管理模糊情况，因此需要重定位。该方法是里程计集成的最佳方式，因为我们可以通过集成非线性误差模型处理slippage等非系统误差。除了里程计和激光雷达外，粒子滤波框架还可以集成其它传感器，我们继承了GPS和指南针。

下面讨论CoreSLAM定位精度和集成延迟。尽管地图分辨率是1cm，仍可实现亚像素级位置测量，因为ts_distance_scan_to_map函数使用若干点进行计算，1mm的移动都可以测量。集成延迟是指激光扫描与集成在地图直接的时间间隔，引起的位置偏移需要小于地图分辨率。

CoreSLAM没有闭环检测，但可以集成任何闭环检测算法。

四、平台描述及实验

实验使用的自制的MinesRover机器人，如下图所示，有四个驱动轮和两个里程计轮，里程计轮理论上一直接触地面，不会打滑，没有减震器。处理平台是ARM9+Xilinx FPGA的Qwerk模块。操作系统是Linux，传感器有激光雷达、摄像头、GPS、IMU、指南针、超声波传感器。

实验结果如下图所示。

英文名称：CoreSLAM: a SLAM Algorithm in less than 200 lines of C code

链接：https://www.researchgate.net/publication/228374722_CoreSLAM_a_SLAM_Algorithm_in_less_than_200_lines_of_C_code

源码：https://svn.openslam.org/data/svn/tinyslam

死磕自律，遇见更好的自己；自斩双臂，方能长出强两翼。

败而不弃，潜龙勿用待时机；执着坚毅，飞升上神创奇迹！

关注该公众号，一起创造奇迹。