多传感器融合的移动机器人同步定位与建图

陈超 徐军 张伟伟

摘  要： 在移动机器人的同步定位与建图（SLAM）研究中，使用单一的Kinect传感器或激光传感器时，出现建图精度低、信息不完整、回环检测易出错、可靠性差等问题，为此提出一种方案，将这两种传感器进行融合，利用融合之后的数据来创建环境地图。该方法首先将Kinect采集的深度图转换成伪激光数据后，与激光传感器的数据进行融合，来构建环境的局部栅格地图，并利用融合后的数据进行激光回环检测。当检测到有回环时，再利用Kinect的RGB图的丰富的视觉信息进行视觉回环检测。通过双回环检测来过滤掉错误的回环信息，提高全局地图的精度和完整性。最后在机器人操作系统上进行试验，结果表明该方案能显著提高建图的精度和完整性。

关键词： 同步定位建图; 移动机器人; 多传感器融合; 环境地图; 回环检测; 系统试验

中图分类号： TN830?34; TP242.6                文献标识码： A                      文章编号： 1004?373X（2020）14?0164?06

Mobile robot simultaneous localization and mapping based on multi?sensor fusion

CHEN Chao， XU Jun， ZHANG Weiwei

（College of Mechanical Engineering， Jiangsu University of Science and Technology， Zhenjiang 212003， China）

Abstract： In the research of mobile robot simultaneous localization and mapping （SLAM）， there are some problems such as low accuracy of mapping， incomplete information， error loop closure and poor reliability when using a single Kinect sensor or laser sensor. A scheme is proposed to fuse these two sensors， and the environment map is constructed by means of the fusion data. In this method， the depth map acquired by means of the Kinect is converted into the pseudo laser data， which is fused with the data of the laser sensor， to create a local raster map of the environment and conduct the laser loopback detection with the fused data. When a loopback is detected， the visual loop detection will be performed by using the visual information of the Kinect′s RGB image. The double loop closure detection is used to filter out the wrong loop closure and improve the accuracy and integrity of the global map. The experiments on the robot operating system show that the method can significantly improve the accuracy and integrity of the map.

Keywords： simultaneous localization and mapping; mobile robot; multi?sensor fusion; environment map; loopback detection; system testing

0  引  言

随着SLAM研究的不断发展，各种传感器由于其自身的特点被广泛地应用于不同的场景。其中以激光传感器的研究和应用最为成熟。近年来随着计算机视觉技术的发展，视觉传感器在SLAM研究中也得到了越来越广泛的应用。当这两种传感器单独使用时，创建的地图精度和完整性难以满足实际的要求。因此，采用多传感器融合，充分利用这两种传感器的优势，获取环境的冗余信息，提高系统的鲁棒性和准确度，是当前SLAM研究的重要方向之一。

激光传感器能非常准确、快速地获取环境信息，计算复杂度低，能够很好地满足实时性要求，被广泛应用于传统的2D栅格地图的创建之中[1?2]。但是由于激光所获取环境的信息量极少，因此非常容易在回環上出错。而回环一旦出错，将会导致整个建图失败。因此，Wolfgang Hess等人提出对激光传感器采集的数据进行处理之后，再进行回环检测，以此提高回环的成功率[3]。但是当环境中存在大量几何形状相似的物体时，该方法仍然容易出错。

视觉传感器主要有单目相机、双目相机及深度相机。由于相机轻便价格便宜，而且能够获取丰富的环境信息，现在已经被广泛应用到SLAM研究中。如Raul Mur?Artal的ORB?SLAM2[4]能够分别使用单双目及深度相机来创建环境的稀疏点云地图。Felix Endres等人提出基于Kinect的RGB?D?SLAM[5]充分利用了深度相机的信息来创建环境地图。但是视觉传感器易受光照的影响，且精度低，计算复杂度高，实时性差。

针对单一传感器的这些固有的缺陷和不足，文献[6?9]提出了各种方案来对激光和视觉传感器进行融合。融合之后，利用两种传感器的冗余信息来创建环境地图。其结果表明使用多传感器融合，能取得比单一传感器更好的效果。但是，以上的各种融合方案中并未充分利用这两种传感器的有效信息。如：有的只使用了Kinect的深度信息，而舍弃了RGB信息;有的只使用Kinect的RGB图来对深度图的黑洞区域进行局部补充，而未充分利用RGB图的其余信息[10]。

针对以上情况，本文提出一种新的方案来融合激光和Kinect传感器进行环境地图创建。该方案首先通过Kinect来获取环境的深度图和RGB图，将深度图转换成激光格式的数据，与激光传感器采集的数据进行融合，构建环境的二维栅格地图。然后在回环部分先利用融合后激光格式的数据来进行回环预检。当激光数据检测到有回环存在时，会发送一个信号给视觉部分，然后利用丰富的视觉信息来进行视觉回环检测，以确定是否存在回环。通过使用双回环检测来提高建图的精度和完整性。最后通过实验来对该方案的可行性和效果进行验证。

1  SLAM问题描述

移动机器人SLAM是一个关于状态估计的问题。机器人在不断运动过程中，通过自身所携带的传感器来获取环境的观测数据，然后通过观测的数据来估计自身的位姿并不斷地迭代下去如图1所示。由于传感器通常是在一定的时间间隔内采集数据，所以结果是把一段连续时间内的运动变成了离散的各时刻的运动[11]。将其抽象成数学模型，总结如下：

[xk+1=f（xk，uk，wk）] （1）

[zk，j=g（xk，yj，vk，j）] （2）

用[xi]表示机器人在[i]时刻的位置，于是移动机器人在各时刻的位置就表示为[x1，x2，…，xi，…，xn]，它们一起组成机器人的运动轨迹。[yi]为移动机器人上相机所观测到的第[i]个环境路标，于是[y1，y2…，yi，…yN]构成了环境的地图。[zk，j]为相机在[xk]处观测到路标[yj]的观测数据。[uk]为输入控制数据，[wk]为位置噪声，[vk，j]为观测噪声。

式（1）为运动模型，式（2）为观测模型。函数[f，g]在不同的传感器中具体有不同的形式，根据传感器的不同，有着不同的参数化形式。这两个方程描述了最基本的SLAM问题：已知控制输入量[u]，及观测的数据[z]时，如何求机器人的位置[x]，并构建环境的地图[y]。这样就把SLAM问题建模成一个状态估计问题，即如何通过带有噪声的测量数据，来估计机器人的状态变量。

2  多传感器融合

2.1  传感器模型

激光传感器如图2所示。其型号为Hokuyo UST?10LX，测量精度为±4 mm，检测视角为270°，有效测量距离可达8 m。

视觉传感器为Kinect V1，如图3所示。它是微软公司开发的一款深度相机，能同时获取环境的深度信息和彩色信息。其在水平方向视角为57°，竖直方向视角为43°，检测范围为0.8～4 m，帧率为30 f/s。

本文针对这两种传感器的优缺点，对其进行融合。同时使用激光传感器和Kinect传感器，来获得大的视角、丰富的环境信息和准确的测量数据，如图4所示。

2.2  融合方案

将激光传感器与Kinect传感器采集的数据进行融合，来创建环境的二维栅格地图。其融合方案见图5。

2.3  Kinect深度图转换成激光数据

在开源机器人操作系统ROS（Robot Operating System）上，提供的用来创建环境二维栅格地图的SLAM算法，只能接收一种激光扫描类型数据作为输入。因此，Kinect传感器所采集的数据必须先转换成激光类型的数据，也称作伪激光数据。然后再和激光传感器所采集的数据融合成为一种数据，才能输入到系统中进行处理、建图[12]。Kinect采集的RGB图和对应的深度图如图6所示，其中深度图的分辨率为320×240。然后将Kinect的深度图转换成激光数据的格式。在转换之前需要先将深度图进行压缩处理，选取每列中距离成像平面最近的点为：

[ri=min（ri1，ri2，…，rij，…，ri240）] （3）

以[ri] 作为伪激光所检测到距离Kinect最近的障碍物。从而实现既保留障碍物的高度信息，又有效地减少了数据量的大小。

2.4  栅格地图的创建与更新

Kinect和激光传感器融合后的数据，以及创建的栅格地图主要有三种状态：空、占据、未知，每一种状态都有对应的区域。由于传感器本身存在误差，其测量并不准确，常常带有一定的噪声。因此，只能计算传感器前方给定位置处，存在障碍物的概率是多少。对于这种不确定性的环境，通过引入贝叶斯模型，用概率来对其进行描述，并用栅格地图来表示。栅格地图将环境划分成一系列的网格，每个网格的取值在0～1之间。不同的取值代表着每个网格的状态，0表示网格为空，1表示网格被占据，0.5表示网格状态未知[13]。

贝叶斯公式被用于更新栅格地图的状态，提高建图的精度。用[p（mij）]表示[（i，j）]位置处存在障碍物的概率。对于一个栅格点，[p（m=0）]表示该栅格为空，[p（m=1）]表示该栅格被占据，且

[p（m=0）=1-p（m=1）] （4）

将两者的比值作为该点的状态：

[Odd（m）=p（m=1）p（m=0）] （5）

对于一个网格点，现在有一个新的测量值[z∈{0，1}]，需要更新该点的状态。未更新之前的状态为[Odd（m）]，更新之后的状态为：

[Odd（m|z）=p（m=1|z）p（m=0|z）] （6）

表示在有新的观测值[z]之后，[m]的概率。然后根据贝叶斯公式得到测量之后的后验概率为：

[p（m|z）=p（z|m）p（m）p（z）] （7）

代入到更新状态方程中有：

[Odd（m|z）=p（m=1|z）p（m=0|z）=p（z|m=1）p（m=1）p（z）p（z|m=0）p（m=0）p（z）             =p（z|m=1）p（m=1）p（z|m=0）p（m=0）=p（z|m=1）p（z|m=0）Odd（m）] （8）

对式（8）两边同时取对数，得：

[log[Odd（m|z）]=logp（z|m=1）p（z|m=0）+log[Odd（m）]] （9）

式（9）中含有新的测量值的项只有[logp（z|m=1）p（z|m=0）]，将其记为[logmeans]，即：

[logmeans=logp（z|m=1）p（z|m=0）] （10）

式中[logmeans]只有两种状态，分别为：

[logfree=logp（z=0|m=1）p（z=0|m=0）] （11）

[logoccu=logp（z=1|m=1）p（z=1|m=0）] （12）

用[S+]來表示[log[Odd（m|z）]]，指通过测量值，更新后的状态;用[S-]来表示[log[Odd（m）]]，指更新前的状态。则式（9）可以表示为：

[S+=logmeans+S-] （13）

即更新后的状态值为测量的状态值加上未更新前的状态值。在没有任何测量值的初始状态下，栅格的状态为：

[p（m=0）=p（m=1）=0.5] （14）

此时，

[S0=log[Odd（m）]=logp（m=1）p（m=0）     =log0.50.5=0] （15）

经过上面的建模和转化后，栅格地图中任意一点的状态都可以通过传感器的测量值，进行加减来更新地图的状态。一个点的状态值越大，就越有可能表示它是被占据的状态;值越小，就越有可能表示它是空的状态。

2.5  激光回环检测

激光回环检测主要有三种方法：

1） scan?to?scan，即当前扫描数据帧与历史扫描帧间的匹配。

2） scan?to?map，即当前数据与历史局部地图之间的匹配，以确认回环。

3） map?to?map，即将当前连续帧激光数据先构建成子地图，再和之前的子地图进行匹配。

方法1）由于信息量太少，常常出现匹配错误;方法2）的匹配精度较高，且匹配速到快，实时性好;方法3）由于需要建立大量的子地图，来进行相互匹配，虽然匹配准确性高，但是计算量大，实时性差，且对硬件要求高。因此，本文的激光回环检测将采用方法2），即scan?to?map的方式。

由于激光只能扫描到其安装平面上的信息，在该平面上下的障碍物都会被其忽略。因此，激光扫描能获取的环境信息极少。在回环检测时，常常由于信息的不足而导致回环失败。同时，激光只能获取障碍物的几何信息，而不能获取障碍物表面的纹理信息。因此，当环境中存在大量与几何外形相似的障碍物时，激光将难以仅仅根据环境物体的形状来判断是否存在回环。

而在Kinect的深度图转换的伪激光数据中，包含障碍物的高度信息，其信息更加丰富。先用融合后的激光类型数据来构建环境的子地图，再利用以后时刻的融合数据与子地图进行匹配，根据环境的几何特征来初步判断是否存在可能的回环，从而提高激光回环的成功率。当激光检测到有回环存在时，将会向视觉回环发出一个信号，来启动视觉回环进行检测。

2.6  视觉回环检测

视觉传感器获取的信息十分丰富，使用视觉进行回环检测的成功率要远远高于激光的回环。因此，利用Kinect的RGB图中含有丰富的环境信息，首先从RGB图中选取关键帧，再从这些关键帧中提取特征点。本文提取的是ORB特征点[14]，如图7所示。通过这些关键帧间的特征点进行回环检测，来验证激光所检测到的回环是否存在，从而提高建图的准确性和全局一致性。

由于直接进行特征点的匹配会比较耗时，当光照变化时，特征的描述也可能不稳定，所以，为了减少系统的运算量，需要在选取的关键帧之间设定合理的间隔。同时，采用词袋模型，即Bag?of?Words（Bow），通过离线训练来构建环境的字典，加快ORB特征的匹配速度[15]。

在这里Kinect的视觉信息，即RGB图，只是用来建立词袋，用于视觉回环检测使用，并不用于建立包含视觉信息的三维点云地图。这样能够有效地避免视觉SLAM创建地图时，需要进行的高复杂度计算，提高了系统的实时性和运行效率。

最后用回环检测的信息来对栅格地图进行校正、优化，并创建出全局一致性的地图。

3  实  验

3.1  实验平台及环境

本文采用的实验平台是改装后的Turtlebot 2移动机器人，其搭载了微软的Kinect V1传感器、Hokuyo激光传感器和一台笔记本电脑，如图8所示。电脑采用的是Ubuntu 14.04 LTS系统， 并在ROS indigo上运行开源SLAM算法Gmapping。

实验环境如图9所示，其为人工搭建室内场景。场地尺寸大小为5 m×2.5 m。在实验场地中设置如图9中所示的障碍物，其中1号障碍物的高度高于激光雷达的安装平面;2号、3号障碍物高度低于激光雷达的安装平面，且2号、3号障碍物的几何外形有着明显的区别，便于激光回环检测时的准确识别。通过这些障碍物的设置来对本文所提出的融合方案的可行性和效果进行验证与分析。

3.2  实验结果

首先只使用激光传感器来对环境进行建图，效果如图10所示。从图中可以看出，在激光所创建的二维栅格地图中，比激光传感器安装位置低的障碍物2、障碍物3在图中都未显示;而比其安装位置高的障碍物1则出现在地图中。表明激光只能检测到其安装平面上的障碍物。

只使用Kinect传感器所创建的环境地图，如图11所示，从图中可以看出，Kinect传感器能够将环境中，激光传感器检测不到的小障碍物2、障碍物3都检测出来。这主要是由于Kinect传感器在竖直方向上有着43°的视角，让其检测范围不只限于水平面上。但是可以看出，Kinect传感器所创建的栅格地图的精度，明显低于激光传感器，存在很大误差。这主要是因为，Kinect传感器在水平方向上只有57°的视角。在小视角的情况下，Kinect传感器将不能进行准确地扫描匹配。同时，Kinect传感器的分辨率也低于激光传感器，从而导致Kinect传感器所创建的二维栅格地图，精度低于激光传感器。

最后再同时使用激光传感器和Kinect传感器来构建环境的地图，其效果如图12所示。从图中可以看出，之前被激光所忽略掉的障碍物，被检测出来，并且，创建地图的精度和完整性相比于单一的传感器，有很大的改善。

4  结  语

本文主要的改进在于，一是将Kinect的深度图与激光的扫描数据进行充分融合后，来创建局部栅格地图，使得地图具有高度方向信息;二是利用融合后的激光类型数据，进行激光回环检测，来快速预判是否存在回环;三是充分利用Kinect的RGB信息来进行视觉回环检测，对激光回环的结果进行确定，去除错误的回环信息。实验结果证明，这些改进相对于单一传感器的使用，能够显著地提高全局地图的精度和完整性。

本文的双回环检测部分，是由激光回环的结果来启动视觉的回环检测。不能根据某一环境的特征，动态地使用对应的信息进行回环检测。因此，下一步主要改进方向是让回环部分根据环境的不同，而采用对应的检测方式。如：当环境中各处有明显的几何外形差异，且缺少纹理特征时，只使用激光来进行回环检测;当环境中存在大量的几何外形相似，且有明显的纹理特征时，只使用视觉来进行回环检测。通过这种方式，来加速回环检测，提高回环效率。

参考文献

[1] LIANG X， CHEN H Y， LI Y J， et al. Visual laser?SLAM in large?scale indoor environments [C]// IEEE International Conference on Robotics & Biomimetics. Qingdao： IEEE， 2017： 134?137.

[2] WU Y， DING Z. Research on laser navigation mapping and path planning of tracked mobile robot based on hector SLAM [C]// International Conference on Intelligent Informatics & Biomedical Sciences. Bangkok： IEEE， 2018： 210?215.

[3] HESS W， KOHLER D， RAPP H， et al. Real?time loop closure in 2D LIDAR SLAM [C]// IEEE International Conference on Robotics and Automation. Stockholm： IEEE， 2016： 101?106.

[4] MUR?ARTAL R， TARDOS J D. ORB?SLAM2： an open?source SLAM system for monocular， stereo and RGB?D cameras [J]. IEEE transactions on robotics， 2017， 33（5）： 1255?1262.

[5] ENDRES F， HESS J， STURM J， et al. 3D mapping with an RGB?D camera [J]. IEEE transactions on robotics， 2017， 30（1）： 177?187.

[6] LIU Z X， XIE C X， XIE M， et al. Mobile robot positioning method based on multi?sensor information fusion laser SLAM [J]. Cluster computing， 2018， 22（2）： 55?61.

[7] KAMARUDIN K， MAMDUH S M， YEON A S A. Improving performance of 2D SLAM methods by complementing Kinect with laser scanner [C]// IEEE International Symposium on Robotics & Intelligent Sensors. Langkawi： IEEE， 2015： 47?61.

[8] YEON A S A， KAMARUDIN K， VISVANATHAN R， et al. Feasibility analysis of 2D?SLAM using combination of kinect and laser scanner [J]. Sciences & engineering， 2015， 76（12）： 9?15.

[9] TUBMAN R， POTGIETER J， ARIF K M. Efficient robotic SLAM by fusion of RatSLAM and RGBD?SLAM [C]// 2016 23rd International Conference on Mechatronics and Machine Vision in Practice. Nanjing： IEEE， 2016： 47?53.

[10] 公维思，周绍磊，吴修振，等.基于改进FAST特征检测的ORB?SLAM方法[J].现代电子技术，2018，41（6）：53?56.

[11] 高翔，张涛.视觉SLAM十四讲：从理论到实践[M].北京：电子工业出版社，2017.

[12] CHEN M X， YANG S W， YI X D， et al. Real?time 3D mapping using a 2D laser scanner and IMU?aided visual SLAM [C]// IEEE International Conference on Real?time Computing & Robotics. Okinawa： IEEE， 2018： 106?118.

[13] FILATOV A， FILATOV A， KRINKIN K， et al. 2D SLAM quality evaluation methods [C]// Processing of the 21th Conference of Fruct Association. Petersburg： IEEE， 2017： 54?67.

[14] FANG W K， ZHANG Y J， YU B， et al. FPGA?based ORB feature extraction for real?time visual SLAM [C]// 2017 International Conference on Field Programmable Technology. Melbourne： IEEE， 2018： 32?41.

[15] RONGBO H B， WU W， TING H， et al. Indoor robot localization and 3D dense mapping based on ORB?SLAM [J]. Journal of computer applications， 2017（5）： 1439?1444.