基于ORB的多機(jī)器人SLAM地圖融合算法

衛(wèi) 恒， 呂 強(qiáng)， 梁 建， 王國勝， 梁 冰

(1. 陸軍裝甲兵學(xué)院兵器與控制系， 北京 100072； 2. 江西理工大學(xué)信息工程學(xué)院， 江西 贛州 341000)

在自主移動機(jī)器人領(lǐng)域，現(xiàn)階段的研究熱點(diǎn)是如何在GPS受到抑制的情況下進(jìn)行同時定位與地圖構(gòu)建(Simultaneous Localization And Mapping，SLAM)，單機(jī)器人SLAM研究已經(jīng)較為深入，然而當(dāng)轉(zhuǎn)移到多機(jī)器人系統(tǒng)時，SLAM又面臨很多新的挑戰(zhàn)。地圖融合算法是解決多機(jī)器人SLAM的一個方向，它將多機(jī)器人系統(tǒng)中各機(jī)器人構(gòu)建的局部地圖(子地圖)通過融合構(gòu)建成一張全局地圖[1-2]，通常分為2步：1) 各子地圖之間對準(zhǔn)信息的匹配；2) 通過整合對準(zhǔn)信息求得旋轉(zhuǎn)矩陣和平移矢量，達(dá)到地圖融合[3]。一般通過重復(fù)區(qū)域或位姿對子地圖之間的對準(zhǔn)信息進(jìn)行查找，從而解算出地圖之間的轉(zhuǎn)換關(guān)系[4]。目前，多機(jī)器人地圖融合主要從各機(jī)器人初始位姿、多機(jī)器人會合[5]、機(jī)器人間相對定位[6-7]和子地圖間重復(fù)區(qū)域[8]4個方面進(jìn)行研究。由于基于子地圖間重復(fù)區(qū)域的地圖融合算法不需要多機(jī)器人之間實(shí)時滿足視線內(nèi)觀測條件，也不需要機(jī)器人會合，因此得到了廣泛應(yīng)用，但目前該算法仍主要針對2D-SLAM，至于3D-SLAM，尚未發(fā)現(xiàn)有較好的算法，這也是未來研究的方向。

基于子地圖間重復(fù)區(qū)域的地圖融合算法的精度非常依賴于各機(jī)器人所構(gòu)建子地圖的精度,如：基于激光測距儀利用Howard提供的標(biāo)準(zhǔn)數(shù)據(jù)集[9]，F(xiàn)astSLAM 1.0相較于FastSLAM 2.0構(gòu)建的子地圖，在地圖融合時需要至少10倍的粒子數(shù)量才能達(dá)到相同效果[10]。另外，在歐式空間內(nèi)尋找子地圖間的重復(fù)區(qū)域也會比較困難，且計(jì)算量大，需占用大量資源。為此，CARPIN[11]提出了基于Hough譜的地圖融合算法，該算法將子地圖轉(zhuǎn)換到Hough空間后，利用歐式空間中轉(zhuǎn)換關(guān)系在Hough空間中是線性的特性,解算出子地圖之間的轉(zhuǎn)換關(guān)系,從而大大降低了算法復(fù)雜度，但該算法需對所有可能成立的轉(zhuǎn)換關(guān)系進(jìn)行分析，仍需占用較多的數(shù)據(jù)處理資源，且當(dāng)子地圖之間存在較少的重復(fù)區(qū)域時可能會失效。SAEEDI等[12]提出了一種基于Hough峰匹配的地圖融合算法，雖然大大提高了運(yùn)算速度，但由于僅利用了角點(diǎn)、線段等特征，在相似、單一的環(huán)境中仍會出現(xiàn)較大誤差。以上算法無論是在歐式空間還是Hough空間，都是基于角點(diǎn)、線段等特征的傳統(tǒng)地圖融合算法，所提取的特征沒有相對應(yīng)的描述子，在特征匹配及地圖融合階段誤差較大，甚至出現(xiàn)融合失效的情況，對整個系統(tǒng)魯棒性產(chǎn)生影響。

為此，筆者基于ORB(Oriented FAST and Rotated BRIEF)特征對傳統(tǒng)地圖融合算法進(jìn)行改進(jìn)，并搭建基于樹莓派的多機(jī)器人平臺，通過將算法應(yīng)用于實(shí)際環(huán)境，驗(yàn)證算法的有效性、實(shí)時性和魯棒性。

1 ORB算法

ORB算法是一種特征點(diǎn)快速提取并進(jìn)行描述的算法[13]，特征點(diǎn)快速提取是由FAST發(fā)展而來，特征點(diǎn)描述是基于BRIEF算法改進(jìn)而來。

1.1 oFAST特征點(diǎn)

ORB算法在特征點(diǎn)提取部分對FAST特征點(diǎn)進(jìn)行了改進(jìn)，即oFAST(oriented FAST)，基本思想是在特征點(diǎn)提取之后，對其添加一個方向，從而實(shí)現(xiàn)特征點(diǎn)的旋轉(zhuǎn)不變性。旋轉(zhuǎn)不變性利用灰度質(zhì)心法來實(shí)現(xiàn)，具體過程如下：

1) 選取包含特征點(diǎn)O的一部分地圖B，定義局部地圖的矩

(1)

式中：x,y∈[-r,r]，其中r為特征點(diǎn)O的鄰域半徑;I(x,y)為點(diǎn)(x，y)處灰度值；z,q∈{0,1}。

2) 通過矩求得地圖B質(zhì)心

C=(m10/m00,m01/m00)

。

(2)

3) 連接特征點(diǎn)O與質(zhì)心C，求得特征點(diǎn)方向角

(3)

1.2 rBRIEF描述子

ORB算法特征點(diǎn)描述子rBRIEF是基于BRIEF描述子改進(jìn)而來的，BRIEF描述子對旋轉(zhuǎn)不敏感，rBRIEF應(yīng)用了oFAST特征點(diǎn)方向，從而使其具備旋轉(zhuǎn)不變性，實(shí)現(xiàn)過程如下：

1) 在特征點(diǎn)鄰域p內(nèi)，經(jīng)RBF(Radial Basis Function)平滑處理，定義一個二進(jìn)制串

(4)

式中：p(x)、p(y)為分別x、y處的灰度值。

則n個二進(jìn)制串

(5)

2) 將式(3)求得的特征點(diǎn)方向加入描述子中。設(shè)生成特征點(diǎn)描述子的n個測試點(diǎn)對為(xi,yi)(i=1,2,…,n),構(gòu)造一個2×n的矩陣

(6)

設(shè)特征點(diǎn)方向角θ對應(yīng)的旋轉(zhuǎn)矩陣為Rθ，則特征點(diǎn)對應(yīng)的旋轉(zhuǎn)矩陣

Sθ=RθQ。

(7)

帶有方向的描述子稱為Steer BRIEF：

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

(8)

3) 對Steer BRIEF描述子進(jìn)行完善，提取出其中相關(guān)性低且均值為0.5附近的特征點(diǎn)對。過程如下(假設(shè)n=256)：

(1) 選取一個圖像鄰域，計(jì)算所有可能的二進(jìn)制串τ。

(2) 對比τ的均值與0.5的差值，按差值大小重新排序，構(gòu)成單應(yīng)矩陣T。

(3) 進(jìn)行貪婪搜索：①從T中選取第一個τ添加到R中，并在T中刪除;②在T中按序再取出下一個τ，與R中所有的值進(jìn)行對比，若相關(guān)性大于設(shè)定閾值，則在T中刪除，否則添加到R中；③重復(fù)上述步驟，直到R中有256個τ，得到最后的描述子。

2 基于ORB的地圖融合算法

算法的基本思路如下：首先提取柵格地圖的ORB特征并找到最優(yōu)的匹配點(diǎn)；然后計(jì)算地圖最優(yōu)匹配點(diǎn)集的單應(yīng)矩陣，找到點(diǎn)集之間的最優(yōu)仿射變換參數(shù)；最后完成子地圖的融合?；贠RB的地圖融合算法流程如圖1所示。由于該算法在進(jìn)行地圖融合時，首先將各個機(jī)器人構(gòu)建的局部地圖每2幅分成一組進(jìn)行融合，然后再將融合后的地圖繼續(xù)分組進(jìn)行融合，直至形成最終的全局地圖，因此在此只對2個機(jī)器人的地圖融合過程進(jìn)行分析。

假設(shè)機(jī)器人R1、R2分別對區(qū)域L1、L2應(yīng)用Hector-SLAM算法構(gòu)建的局部柵格地圖為m1、m2。對m1和m2提取ORB特征，得到oFAST特征點(diǎn)集合o1={ki|1≤i≤m,m∈N}，o2={kj|1≤j≤n,n∈N}，將每個特征點(diǎn)依據(jù)描述子rBRIEF的方向向量化，用坐標(biāo)表示為ki=(ai,bi)和kj=(aj,bj)，由于ORB的rBRIEF描述子是基于BRIEF描述子改進(jìn)后的二進(jìn)制描述子，因此選擇Hanming距離來匹配相似特征點(diǎn)。采用3步提純手段對誤匹配點(diǎn)進(jìn)行剔除：第1步，對BruteForce匹配后的特征點(diǎn)進(jìn)行提純，采用基于閾值的方法，首先將Hanming距離>30的特征點(diǎn)剔除，然后找到2幅地圖中距離差Δd相同的特征點(diǎn)對，定義為

(9)

第2步，對Δd>5的特征點(diǎn)對進(jìn)行剔除提純；第3步， 利用RANSAC(RANdon SAmple Consensus)算法剔除剩余的特征點(diǎn)對誤匹配點(diǎn)。

為了求解2幅子地圖的轉(zhuǎn)換關(guān)系，即單應(yīng)矩陣T，計(jì)算最終匹配的特征點(diǎn)對偏仿射變換，定義旋轉(zhuǎn)矩陣Rφ、尺度變量S和平移矢量t分別如下：

(10)

(11)

(12)

則

(13)

由于T中只有4個變量，只需輸入2組匹配點(diǎn)對即可求解，因此采用最小二乘算法求解最優(yōu)變換。得到單應(yīng)矩陣T后，設(shè)定融合比例，將m1與m2融合，從而得到全局柵格地圖。

3 基于樹莓派的多機(jī)器人平臺

筆者搭建了基于樹莓派的自主移動機(jī)器人平臺，如圖2所示，用于構(gòu)建二維柵格地圖。單個機(jī)器人車身選用3輪全向移動機(jī)器人，以樹莓派2為微處理器，中央處理機(jī)硬件為微型電腦NUC V100_CS，搭載Intel I7 HD5500處理器，16 GB內(nèi)存，軟件運(yùn)行Ubuntu14.04系統(tǒng)和開源圖像處理庫opencv。同時，構(gòu)建了多機(jī)器人網(wǎng)絡(luò)架構(gòu)，解決了機(jī)器人之間和機(jī)器人與中央處理機(jī)之間的通信問題[14]。

4 試驗(yàn)驗(yàn)證

為驗(yàn)證本文算法的有效性、實(shí)時性和魯棒性，選取特征相對相似、單一的樓道環(huán)境，這樣在地圖構(gòu)建完成后，關(guān)鍵點(diǎn)特征總量相對較少，且相似特征相對較多，能更好地驗(yàn)證算法性能。具體驗(yàn)證步驟如下：

1) 多機(jī)器人平臺的2個自主移動機(jī)器人采用漫游的形式，通過Hector-SLAM算法各自構(gòu)建局部地圖，并將數(shù)據(jù)傳輸?shù)皆贫?。機(jī)器人漫游區(qū)域及其構(gòu)建的局部地圖如圖3所示。

由圖3可以看出：自主移動機(jī)器人運(yùn)行穩(wěn)定，Hector-SLAM算法構(gòu)建的局部地圖特征清晰，能夠用來完成地圖融合，其中紅色線條圈住部分為重復(fù)區(qū)域，2幅局部地圖的重疊率為14.38%，且為特征較為單一、相似的樓道環(huán)境。

2) 當(dāng)2個局部地圖出現(xiàn)重復(fù)區(qū)域時，中央處理機(jī)整合各子地圖信息并對各子地圖進(jìn)行特征提取，然后對提取的特征進(jìn)行匹配?；贠RB的地圖融合算法特征點(diǎn)匹配結(jié)果如圖4所示。

由圖4可以看出：在單純的特征點(diǎn)匹配過程中，特征點(diǎn)并不是一一對應(yīng)的關(guān)系，而是出現(xiàn)了一對多或多對多的情況，因此要求地圖融合算法能夠在眾多匹配關(guān)系中找到正確的匹配點(diǎn)對。

3) 在特征提取并找到匹配點(diǎn)對后，即可求得2幅局部地圖之間的單應(yīng)矩陣

旋轉(zhuǎn)角度φ=13.816 8°，尺度變換s=1.004 49，平移矢量

利用上述已構(gòu)建好的局部地圖，采用傳統(tǒng)的基于角點(diǎn)地圖融合算法進(jìn)行數(shù)據(jù)處理，得到融合結(jié)果如圖6所示。融合用時0.835 76 s,誤差e=27.619%，融合失敗，表明在特征單一、相似的樓道環(huán)境中，基于ORB的地圖融合算法具有較強(qiáng)的魯棒性和實(shí)時性。

5 結(jié)論

基于ORB特征提取及匹配算法，筆者提出了一種多機(jī)器人SLAM地圖融合算法，搭建了基于樹莓派的多機(jī)器人平臺，通過實(shí)際應(yīng)用對算法進(jìn)行了驗(yàn)證。結(jié)果表明：該算法能夠在特征單一且相似特征較多的環(huán)境中完成地圖融合。然而，隨著局部地圖的增大，特征點(diǎn)增多，耗時也大幅增加，導(dǎo)致全局地圖無法實(shí)時改變，下一步將繼續(xù)對算法進(jìn)行改進(jìn)，在確保魯棒性的前提下，進(jìn)一步提高其運(yùn)算速度。

參考文獻(xiàn)：

[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.