基于数据融合的SLAM系统研究与路径规划实现

赖秋玲　禹素萍　丁绅一

摘要：移动机器人同时定位与建图（SLAM- Simultaneous Locating And Mapping）与路径规划是实现智能化机器人的前提。该系统首先通过HCR（Home Care Robot）机器人搭载激光雷达和RGB-D传感器，在ROS的运行环境下，分别实现基于ORB-SLAM2和RGB-D SLAM算法框架的同时定位与建图，获取三维点云图。然后将三维点云图与激光雷达生成的二维平面图进行数据融合，得到含有较丰富环境信息的环境二维栅格地图，并利用A*算法路径搜索算法，对机器人进行路径规划。实验结果表明融合了环境三维点云信息的地图，更适合用于路径规划。

关键词：同时定位与建图；RGB-D传感器；激光雷达；路径规划

中图分类号：TP242 文献标识码：A 文章编号：1009-3044（2017）33-0024-03

Abstract： Mobile robot simultaneous locating and mapping （SLAM） and path planning are the prerequisites for realizing intelligent robots. In this system， the HCR （Home Care Robot） robot was equipped with Lidar and an RGB-D sensor. In the ROS operating environment， the SLAM Based on the ORB-SLAM2 and RGB-D SLAM algorithms were implemented respectively and three - dimensional point cloud map was generated. Then， the three - dimensional point cloud map was combined with the two - dimensional map generated by the Lidar to get a grid map with the rich environment information. Finally， the A * algorithm path search algorithm was applied to plan the path for the robot. Experiment results show that the grid map which integrates the three-dimensional cloud information is more suitable for path planning.

Key words： SLAM； RGB-D sensor； Lidar； path planning

1 概述

同时定位与建图（Simultaneous Localization and Mapping—SLAM）是移动机器人领域的热门研究课题[1]。文献[2]已对现有的SLAM算法做了较全面的总结。SLAM的发展主要分为两个阶段：第一阶段为经典阶段（1986-2004），在这个阶段学者们为实现SLAM提出了基于贝叶斯的滤波器方法，包括卡尔曼滤波（Kalman Filtering）、扩展卡尔曼滤波（Extended Kalman Filtering）[3]、Rao-Blackwellized粒子滤波[4]等。第二阶段为算法分析阶段（2004-2016），该阶段主要对SLAM的基本性质进行研究，包括可观察性、收敛性和一致性。在此时期，人们理解了稀疏性[5]对高效地实现SLAM的关键作用，并开源了较多的SLAM库，例如RGB-D SLAM和ORB-SLAM。ORB-SLAM2[6]是目前较优秀的稀疏性开源SLAM框架，它优化了ORB-SLAM[7]，支持标定后的RGB-D相机。

随着传感器技术的发展，RGB-D相机、激光雷达等传感器简化了SLAM的实现难度，而传感器信息融合[8]也不断地被应用到机器视觉领域。

因为激光雷达仅能检测到一定高度的平面信息，所以靠单一的激光雷达，并不能得到更全面的环境信息，而RGB-D相机则具有较大的视角，可以获取更丰富的环境信息。因此该系统利用RGB-D相机，构建基于ORB-SLAM2和RGB-D SLAM算法框架的SLAM系统，对比两种经典SLAM算法，将较好的三维点云图与激光雷达平面图进行环境信息融合，并在此融合地图上实现机器人路径规划。

2 软硬件平台与系统架构

该系统的软件部分使用ROS（Robot Operating System）系统[9]，机器人为HCR（Home Care Robot）机器人。电脑配置为Intel core I7處理器，操作系统为Ubuntu14.04。软件系统结构如图1所示。

软件系统结构的主要分为以下三部分：

（1） 解算机器人位姿：由RGB-D相机采集环境的彩色和深度信息，经由ORB-SLAM2算法或者RGB-D SLAM算法，解算出机器人的当前位姿[XmYmθm]。

（2） 获得环境地图：通过ORB-SLAM2算法或者RGB-D SLAM算法，拼接融合关键帧生成环境三维点云图，通过ROS的第三方库Octomap[10]得到三维网格地图。Octomap用八叉树来存储环境信息，遍历八叉树，将概率大于0.5的全部方块投影得到二维平面地图。然后将此平面地图与激光雷达生成的地图进行或操作，得到融合后的二维栅格地图。

（3） 实现路径规划：将解算出的当前位姿和机器人模型导入到生成的二维地图中，将当前位姿作为起始点，确定一个目标点，通过A*算法，搜索出一条路径，并生成机器人控制命令。endprint

2.1 基于ORB-SLAM2和RGB-D SLAM算法框架的实时定位与建图

随着传感器技术不断地发展，各类SLAM算法框架也应运而生。ORB-SLAM2和RGB-D SLAM是近几年较受关注的SLAM算法框架。

ORB-SLAM2是目前较优秀的SLAM算法系统框架，如图2所示，该系统主要由三个平行线程组成。

（1） 相机位姿跟踪线程：首先对输入帧进行预处理。然后相机位姿跟踪，若不插入关键帧则不启动局部地图线程。局部地图跟踪是将当前帧与局部地图进行匹配，利用最小化投影误差来优化位姿。最后依据是否很长时间没有插入关键帧或者跟踪失败等因素来判断是否生成关键帧。

（2） 局部地图线程：该线程主要是负责处理新生成的关键帧，剔除地图中新添加的质量不高的地图点。而后进行地图点的融合并进行局部BA（Bundle Adjustment）[11]优化，最后将那些地图点超过90%以上能被其他至少3个关键帧共同检测到的关键帧进行剔除，以减少重复率，节省存储空间。

（3） 闭环检测及矫正线程：利用基于DBoW2[12]的场景识别进行闭环检测，而后通过优化位姿图来实现闭环矫正。

在闭环检测及矫正线程结束之后，触发第四个线程进行全局的BA优化，并更新地图。该算法利用ORB[13]特征点完成跟踪，建图，以及场景识别的任务。这些ORB特征点对于旋转和尺度变换有较强鲁棒性，不受光线变换的影响并且提取速度较快，可以满足实时性的需求。

而RGB-D SLAM[14]主要由前端与后端两部分组成，如图3所示，前端部分处理深度相机获取的环境信息，解算出环境信息的空间几何位置，后端用g2o库将前端部分得到的机器人位姿进行优化。最终得到环境三维点云图和机器人位姿。

2.2 基于A*算法的路径规划

该系统采用A*算法来对机器人进行路径规划。A*算法[15]是一种启发式的算法，它被广泛地用于解决各类搜索问题，例如机器人路径规划，网络游戏以及UAV（Unmanned Aerial Vehicles）避障。A*算法的基本思想是结合Dijkstra算法所用的离起始点最近的顶点信息和Greedy best-first search算法采用的離目标点最近的顶点信息。通常用公式（1）来说明A*算法，g（n）表示从起始点到任意顶点n的实际消耗，即走的路径长度，h（n）表示从顶点n到目标点的启发式预测消耗。在每一次循环中，算法找到一个n使得f（n）最小，从而搜索到目标点的最小消耗，即最短路径。

[fn=gn+h（n）] （1）

3 实验结果与分析

3.1 室内ORB-SLAM2，RGB-D SLAM与激光雷达建图结果

通过ORB-SLAM2算法框架和RGB-D SLAM算法，实验结果如图4所示，（a）为实验场景图，（b） 激光雷达生成的二维平面图，（c）为ORB-SLAM2生成的三维点云图， （d）为RGB-D SLAM生成的三维点云图。从实验结果看出，ORB-SLAM2算法框架的闭环检测效果优于RGB-D SLAM算法。RGB-D SLAM算法通过计算关键帧中的特征点描述子来进行回环检测，而ORB-SLAM2是通过基于DBoW2的场景识别来进行闭环检测，而且具有闭环修正模块，较好地修正了地图。在实验过程中发现，ORB-SLAM2算法的实时性也较RGB-D SLAM好。ORB-SLAM2算法采用的是多线程方式，而RGB-D SLAM是单线程，在同样的硬件设备条件下，ORB-SLAM2运行速度较快，可以满足实际应用中对实时性的要求。ORB-SLAM2算法的基于DBoW2的场景识别模型具有全局重定位能力，在跟踪丢失之后，可以进行重新定位，鲁棒性更优。

<

其中左上角（a）图为场景图；右上角（b）图激光雷达平面图；左下角（c）图为ORB-SLAM2生成的三维点云图；右下角（d）图为RGB-SLAM生成的三维点云图。

从以上的实验结果图分析，该系统选用ORB-SLAM2生成的三维点云图经过Octomap处理，得到如图5所示的三维网格环境地图。Octomap使用八叉树的数据结构存储地图，首先遍历八叉树节点，删除值小于0.5的节点（大于0.5表示该区域被占概率较大），然后将其投影得到二维平面地图，为了便于使用A*算法，将障碍物方格化，地图中标示1的为障碍物，0为可移动空间（灰色区域），最后将投影得到的平面图与激光雷达平面图进行或操作，得到融合了环境三维点云信息和激光雷达信息的平面图。

3.2 机器人路径规划结果

分别在激光雷达栅格地图和融合三维环境点云信息的栅格地图上，使用A*路径搜索算法，对机器人路径进行规划。图中所绘路线即为A*算法结果，黑色方块为终点。实验结果如图6所示。由路径规划结果看出，在激光雷达地图上进行导航出现失误，即规划的线路中经过了障碍物占有的区域，而在融合了点云信息的环境地图上，规划的路线正确。这在一定程度上说明融合了三维环境点云信息的激光雷达地图比未融合信息的激光雷达地图包含较丰富的环境信息，有助于对机器人路径规划的实现。

4 结论

该系统实现了室内移动机器人SLAM与路径规划，并比较了两种经典SLAM算法框架即ORB-SLAM2和RGB-D SLAM算法， 融合三维点云信息和激光雷达信息，实现机器人路径规划。实验结果显示ORB-SLAM2算法总体来说优于RGB-D SLAM。融合了环境三维点云信息的激光雷达地图，更有利于对机器人进行路径规划。但该系统在以下几个方面不足，首先，将三维网格地图投影生成二维平面地图时存在噪声影响，会对障碍物判断产生一定误差。其次，当运行RGB-D SLAM算法的时候，光照对其影响较大，所以有一定的误差产生。endprint

参考文献：

[1] 李贵亚. 基于立体视觉的机器人SLAM算法研究[D]. 哈尔滨工业大学， 2015.

[2] Cadena C， Carlone L， Carrillo H， et al. Simultaneous Localization And Mapping： Present， Future， and the Robust-Perception Age[J]. 2016， 32（6）.

[3] 时也， 吴怀宇， 徐文霞，等. 基于扩展卡尔曼滤波器的移动机器人SLAM研究[J]. 电子设计工程， 2012， 20（1）：104-106.

[4] 罗元， 傅有力， 程铁凤. 基于改进Rao-Blackwellized粒子滤波器的同时定位与地图构建[J]. 控制理论与应用， 2015， 32（2）：267-272. （下转第37页）

（上接第26页）

[5] Bichucher V， Walls J M， Ozog P， et al. Bathymetric factor graph SLAM with sparse point cloud alignment[C]// Oceans. IEEE， 2015：1-7.

[6] Murartal R， Tardos J D. ORB-SLAM2： an Open-Source SLAM System for Monocular， Stereo and RGB-D Cameras[J]. 2016.

[7] Mur-Artal R， Montiel J M M， Tardós J D. ORB-SLAM： A Versatile and Accurate Monocular SLAM System[J]. IEEE Transactions on Robotics， 2015， 31（5）：1147-1163.

[8] Belter D， Labcki P， Skrzypczynski P. Estimating terrain elevation maps from sparse and uncertain multi-sensor data[C]// IEEE International Conference on Robotics and Biomimetics. IEEE， 2012：715-722.

[9] 张建伟， 张立伟， 胡颖，等. 开源机器人操作系统——ROS[M].北京：科学出版社，2012.

[10] 王允华， 李建胜. 基于Octomap的无人机室内导航可飞行区域分析[J].测绘与空间地理信息，2017（2）：95-97.

[11] Zhao L， Huang S， Yan L， et al. Large-scale monocular SLAM by local bundle adjustment and map joining[C]// International Conference on Control Automation Robotics & Vision. IEEE， 2010：431-436. [12] Galvez-López D， Tardos J D. Bags of Binary Words for Fast Place Recognition in Image Sequences[J]. IEEE Transactions on Robotics， 2012， 28（5）：1188-1197.

[12] Galvez-López D， Tardos J D. Bags of Binary Words for Fast Place Recognition in Image Sequences[J]. IEEE Transactions on Robotics， 2012， 28（5）：1188-1197.

[13] 白雪冰， 车进， 牟晓凯，等. 结合快速鲁棒性特征改进ORB的特征点匹配算法[J].计算机应用，2016，36（7）：1923-1926.

[14] Elghor E C， Roussel D， Ababsa F， et al. 3D Planar RGB-D SLAM System[J]. 2016.

[15] 郝會龙， 任鸿翔， 肖方兵. 基于A算法的虚拟船员路径规划仿真[J].计算机工程与设计，2015（3）：733-737.endprint