基于ORB的多机器人SLAM地图融合算法

卫 恒， 吕 强， 梁 建， 王国胜， 梁 冰

(1. 陆军装甲兵学院兵器与控制系， 北京 100072； 2. 江西理工大学信息工程学院， 江西 赣州 341000)

在自主移动机器人领域，现阶段的研究热点是如何在GPS受到抑制的情况下进行同时定位与地图构建(Simultaneous Localization And Mapping，SLAM)，单机器人SLAM研究已经较为深入，然而当转移到多机器人系统时，SLAM又面临很多新的挑战。地图融合算法是解决多机器人SLAM的一个方向，它将多机器人系统中各机器人构建的局部地图(子地图)通过融合构建成一张全局地图[1-2]，通常分为2步：1) 各子地图之间对准信息的匹配；2) 通过整合对准信息求得旋转矩阵和平移矢量，达到地图融合[3]。一般通过重复区域或位姿对子地图之间的对准信息进行查找，从而解算出地图之间的转换关系[4]。目前，多机器人地图融合主要从各机器人初始位姿、多机器人会合[5]、机器人间相对定位[6-7]和子地图间重复区域[8]4个方面进行研究。由于基于子地图间重复区域的地图融合算法不需要多机器人之间实时满足视线内观测条件，也不需要机器人会合，因此得到了广泛应用，但目前该算法仍主要针对2D-SLAM，至于3D-SLAM，尚未发现有较好的算法，这也是未来研究的方向。

基于子地图间重复区域的地图融合算法的精度非常依赖于各机器人所构建子地图的精度,如：基于激光测距仪利用Howard提供的标准数据集[9]，FastSLAM 1.0相较于FastSLAM 2.0构建的子地图，在地图融合时需要至少10倍的粒子数量才能达到相同效果[10]。另外，在欧式空间内寻找子地图间的重复区域也会比较困难，且计算量大，需占用大量资源。为此，CARPIN[11]提出了基于Hough谱的地图融合算法，该算法将子地图转换到Hough空间后，利用欧式空间中转换关系在Hough空间中是线性的特性,解算出子地图之间的转换关系,从而大大降低了算法复杂度，但该算法需对所有可能成立的转换关系进行分析，仍需占用较多的数据处理资源，且当子地图之间存在较少的重复区域时可能会失效。SAEEDI等[12]提出了一种基于Hough峰匹配的地图融合算法，虽然大大提高了运算速度，但由于仅利用了角点、线段等特征，在相似、单一的环境中仍会出现较大误差。以上算法无论是在欧式空间还是Hough空间，都是基于角点、线段等特征的传统地图融合算法，所提取的特征没有相对应的描述子，在特征匹配及地图融合阶段误差较大，甚至出现融合失效的情况，对整个系统鲁棒性产生影响。

为此，笔者基于ORB(Oriented FAST and Rotated BRIEF)特征对传统地图融合算法进行改进，并搭建基于树莓派的多机器人平台，通过将算法应用于实际环境，验证算法的有效性、实时性和鲁棒性。

1 ORB算法

ORB算法是一种特征点快速提取并进行描述的算法[13]，特征点快速提取是由FAST发展而来，特征点描述是基于BRIEF算法改进而来。

1.1 oFAST特征点

ORB算法在特征点提取部分对FAST特征点进行了改进，即oFAST(oriented FAST)，基本思想是在特征点提取之后，对其添加一个方向，从而实现特征点的旋转不变性。旋转不变性利用灰度质心法来实现，具体过程如下：

1) 选取包含特征点O的一部分地图B，定义局部地图的矩

(1)

式中：x,y∈[-r,r]，其中r为特征点O的邻域半径;I(x,y)为点(x，y)处灰度值；z,q∈{0,1}。

2) 通过矩求得地图B质心

C=(m10/m00,m01/m00)

。

(2)

3) 连接特征点O与质心C，求得特征点方向角

(3)

1.2 rBRIEF描述子

ORB算法特征点描述子rBRIEF是基于BRIEF描述子改进而来的，BRIEF描述子对旋转不敏感，rBRIEF应用了oFAST特征点方向，从而使其具备旋转不变性，实现过程如下：

1) 在特征点邻域p内，经RBF(Radial Basis Function)平滑处理，定义一个二进制串

(4)

式中：p(x)、p(y)为分别x、y处的灰度值。

则n个二进制串

(5)

2) 将式(3)求得的特征点方向加入描述子中。设生成特征点描述子的n个测试点对为(xi,yi)(i=1,2,…,n),构造一个2×n的矩阵

(6)

设特征点方向角θ对应的旋转矩阵为Rθ，则特征点对应的旋转矩阵

Sθ=RθQ。

(7)

带有方向的描述子称为Steer BRIEF：

gn(p,θ)=fn(p)|(xi,yi)∈Sθ。

(8)

3) 对Steer BRIEF描述子进行完善，提取出其中相关性低且均值为0.5附近的特征点对。过程如下(假设n=256)：

(1) 选取一个图像邻域，计算所有可能的二进制串τ。

(2) 对比τ的均值与0.5的差值，按差值大小重新排序，构成单应矩阵T。

(3) 进行贪婪搜索：①从T中选取第一个τ添加到R中，并在T中删除;②在T中按序再取出下一个τ，与R中所有的值进行对比，若相关性大于设定阈值，则在T中删除，否则添加到R中；③重复上述步骤，直到R中有256个τ，得到最后的描述子。

2 基于ORB的地图融合算法

算法的基本思路如下：首先提取栅格地图的ORB特征并找到最优的匹配点；然后计算地图最优匹配点集的单应矩阵，找到点集之间的最优仿射变换参数；最后完成子地图的融合。基于ORB的地图融合算法流程如图1所示。由于该算法在进行地图融合时，首先将各个机器人构建的局部地图每2幅分成一组进行融合，然后再将融合后的地图继续分组进行融合，直至形成最终的全局地图，因此在此只对2个机器人的地图融合过程进行分析。

假设机器人R1、R2分别对区域L1、L2应用Hector-SLAM算法构建的局部栅格地图为m1、m2。对m1和m2提取ORB特征，得到oFAST特征点集合o1={ki|1≤i≤m,m∈N}，o2={kj|1≤j≤n,n∈N}，将每个特征点依据描述子rBRIEF的方向向量化，用坐标表示为ki=(ai,bi)和kj=(aj,bj)，由于ORB的rBRIEF描述子是基于BRIEF描述子改进后的二进制描述子，因此选择Hanming距离来匹配相似特征点。采用3步提纯手段对误匹配点进行剔除：第1步，对BruteForce匹配后的特征点进行提纯，采用基于阈值的方法，首先将Hanming距离>30的特征点剔除，然后找到2幅地图中距离差Δd相同的特征点对，定义为

(9)

第2步，对Δd>5的特征点对进行剔除提纯；第3步， 利用RANSAC(RANdon SAmple Consensus)算法剔除剩余的特征点对误匹配点。

为了求解2幅子地图的转换关系，即单应矩阵T，计算最终匹配的特征点对偏仿射变换，定义旋转矩阵Rφ、尺度变量S和平移矢量t分别如下：

(10)

(11)

(12)

则

(13)

由于T中只有4个变量，只需输入2组匹配点对即可求解，因此采用最小二乘算法求解最优变换。得到单应矩阵T后，设定融合比例，将m1与m2融合，从而得到全局栅格地图。

3 基于树莓派的多机器人平台

笔者搭建了基于树莓派的自主移动机器人平台，如图2所示，用于构建二维栅格地图。单个机器人车身选用3轮全向移动机器人，以树莓派2为微处理器，中央处理机硬件为微型电脑NUC V100_CS，搭载Intel I7 HD5500处理器，16 GB内存，软件运行Ubuntu14.04系统和开源图像处理库opencv。同时，构建了多机器人网络架构，解决了机器人之间和机器人与中央处理机之间的通信问题[14]。

4 试验验证

为验证本文算法的有效性、实时性和鲁棒性，选取特征相对相似、单一的楼道环境，这样在地图构建完成后，关键点特征总量相对较少，且相似特征相对较多，能更好地验证算法性能。具体验证步骤如下：

1) 多机器人平台的2个自主移动机器人采用漫游的形式，通过Hector-SLAM算法各自构建局部地图，并将数据传输到云端。机器人漫游区域及其构建的局部地图如图3所示。

由图3可以看出：自主移动机器人运行稳定，Hector-SLAM算法构建的局部地图特征清晰，能够用来完成地图融合，其中红色线条圈住部分为重复区域，2幅局部地图的重叠率为14.38%，且为特征较为单一、相似的楼道环境。

2) 当2个局部地图出现重复区域时，中央处理机整合各子地图信息并对各子地图进行特征提取，然后对提取的特征进行匹配。基于ORB的地图融合算法特征点匹配结果如图4所示。

由图4可以看出：在单纯的特征点匹配过程中，特征点并不是一一对应的关系，而是出现了一对多或多对多的情况，因此要求地图融合算法能够在众多匹配关系中找到正确的匹配点对。

3) 在特征提取并找到匹配点对后，即可求得2幅局部地图之间的单应矩阵

旋转角度φ=13.816 8°，尺度变换s=1.004 49，平移矢量

利用上述已构建好的局部地图，采用传统的基于角点地图融合算法进行数据处理，得到融合结果如图6所示。融合用时0.835 76 s,误差e=27.619%，融合失败，表明在特征单一、相似的楼道环境中，基于ORB的地图融合算法具有较强的鲁棒性和实时性。

5 结论

基于ORB特征提取及匹配算法，笔者提出了一种多机器人SLAM地图融合算法，搭建了基于树莓派的多机器人平台，通过实际应用对算法进行了验证。结果表明：该算法能够在特征单一且相似特征较多的环境中完成地图融合。然而，随着局部地图的增大，特征点增多，耗时也大幅增加，导致全局地图无法实时改变，下一步将继续对算法进行改进，在确保鲁棒性的前提下，进一步提高其运算速度。

参考文献：

[1] HOWARD A，SUKHATME G S，MATARIC M J. Multirobot simultaneous localization and mapping using manifold representations[J]. Proceedings of the IEEE，2006，94(7):1360-1369.

[2] SAEEDI S，PAULL L，TRENTINI M，et al. Group mapping:a topological approach to map merging for multiple robots[J].Robotics & automation magazine IEEE，2014，21(2):60-72.

[3] PORTUGAL D，RUI P R.Cooperative multi-robot patrol with Bayesian learning[J].Autonomous robots，2016，40(5):929-953.

[4] KAPOUTSIS A C，CHATZICHRISTOFIS S A，DOITSIDIS L，et al.Real-time adaptive multi-robot exploration with application to underwater map construction[J].Autonomous robots，2016，40(6):987-1015.

[5] ZHOU X S，ROUMELIOTIS S I.Multi-robot SLAM with unknown initial correspondence:the robot rendezvous case[C]∥ IEEE/RSJ International Conference on Intelligent Robots and Systems.New York:IEEE，2006:1785-1792.

[6] AHMAD A，LAWLESS G，LIMA P.An online scalable approach to unified multirobot cooperative localization and object tracking[J].IEEE transactions on robotics，2017，19(7):1-16.

[7] WU P，LIU Y，YE M，et al.Geometry guided multi-scale depth map fusion via graph optimization[J].IEEE transactions on image processing，2017，26(3):1315-1329.

[8] LEI J，WU M，ZHANG C，et al.Depth-preserving stereo image retargeting based on pixel fusion[J].IEEE transactions on multimedia，2017，19(7):1442-1453.

[9] HOWARD A，PARKER L E，SUKHATME G S.Experiments with a large heterogeneous mobile robot team:exploration，mapping，deployment and detection[J].International journal of robotics research，2005，25(5):431-447.

[10] GRISETTI G，STACHNISS C，BURGARD W.Improved techniques for grid mapping with rao-blackwellized particle filters[J].IEEE transactions on robotics，2007，23(1):34-46.

[11] CARPIN S.Fast and accurate map merging for multi-robot systems[J].Autonomous robots，2008，25(3):305-316.

[12] SAEEDI S，PAULL L，TRENTINI M，et al.Map merging using hough peak matching[C]∥ IEEE/RSJ International Conference on Intelligent Robots and Systems.New York:IEEE，2012:4683-4688.

[13] RUBLEE E，RABAUD V，KONOLIGE K，et al.ORB:an efficient alternative to SIFT or SURF[C]∥IEEE International Conference on Computer Vision.New York:IEEE，2012:2564-2571.

[14] LV Q，WEI H，LIN H，et al.Design and implementation of multi robot research platform based on UWB[C]∥The 29th Chinese Control and Decision Conference.Shenyang,China:Northeastern University Press,2017:7246-7251.