徐子鋒 石超 王永鋒 陳龍

摘? 要：在視覺(jué)SLAM前端特征點(diǎn)匹配過(guò)程中，采用RANSAC算法剔除誤匹配特征點(diǎn)存在迭代次數(shù)不穩(wěn)定、效率低、魯棒相差等問(wèn)題，從而對(duì)相機(jī)定位產(chǎn)生影響。與ORB算法結(jié)合，本文引入一種漸進(jìn)采樣一致性算法，即PROSAC（Progressive Sampling Consensus），來(lái)消除迭代次數(shù)不穩(wěn)定問(wèn)題。利用Kinect v2相機(jī)對(duì)改進(jìn)的RGB-D SLAM算法進(jìn)行實(shí)驗(yàn)，獲得三維點(diǎn)云地圖和相機(jī)軌跡，實(shí)現(xiàn)了ORB+PROSAC的誤匹配剔除算法。與ORB+RANSAC的結(jié)合方式相對(duì)比，本文算法驗(yàn)證魯棒性更好，實(shí)時(shí)性更強(qiáng)。

關(guān)鍵詞：視覺(jué)SLAM;特征點(diǎn)匹配;RANSAC算法;PROSAC算法

中圖分類(lèi)號(hào)：TP391.4? ? ?文獻(xiàn)標(biāo)識(shí)碼：A

Abstract：In the process of feature point matching in the front of visual SLAM，a series of problems have been found when using the RANSAC algorithm to eliminate the mismatched feature points，such as unstable iterations，low efficiency and robust phase difference，resulting in an impact on camera positioning.Combined with the ORB algorithm，this paper introduces a progressive sampling consensus algorithm，PROSAC （Progressive Sampling Consensus），to eliminate the instability of iteration times.Using the Kinect v2 camera with the improved RGB-D SLAM algorithm，the 3D point cloud map and camera trajectory can be obtained in the experiment and the mismatch elimination algorithm of ORB+PROSAC can be realized.Compared with the combination with ORB+RANSAC，the proposed algorithm verifies the robustness better with a stronger real-time performance.

Keywords：visual SLAM;feature point matching;RANSAC algorithm;PROSAC algorithm

1? ?引言（Introduction）

移動(dòng)機(jī)器人的同步定位與地圖構(gòu)建（Simultaneous Localization and Mapping，SLAM）[1]是實(shí)現(xiàn)移動(dòng)機(jī)器人自主定位導(dǎo)航的關(guān)鍵技術(shù)。視覺(jué)SLAM由于傳感器價(jià)格低廉，以及所能采集到的信息豐富等特點(diǎn)，越來(lái)越多地出現(xiàn)在工業(yè)應(yīng)用中。

視覺(jué)SLAM前端系統(tǒng)中，圖像在匹配過(guò)程中受到噪聲干擾，會(huì)導(dǎo)致特征點(diǎn)的誤匹配，影響相機(jī)定位。為了提高視覺(jué)SLAM前端特征點(diǎn)匹配的精確性，通常在匹配過(guò)程中加入隨機(jī)采樣一致性算法，即RANSAC（Random Sampling Consensus）[2]，對(duì)誤匹配點(diǎn)進(jìn)行剔除。RANSAC算法是隨機(jī)過(guò)程，會(huì)導(dǎo)致迭代次數(shù)不穩(wěn)定，且時(shí)間開(kāi)銷(xiāo)大，不能很好滿足實(shí)時(shí)性要求。另外，現(xiàn)在越來(lái)越多的研究人員嘗試著將機(jī)器學(xué)習(xí)的方法加入視覺(jué)SLAM前端中，使得圖像匹配更加魯棒，但是需要大量樣本的訓(xùn)練，計(jì)算量也會(huì)增加。

視覺(jué)SLAM前端圖像匹配既要滿足精確度要求，同時(shí)還需要滿足實(shí)時(shí)性要求。對(duì)此，文中提出在圖像匹配過(guò)程中引入一種漸進(jìn)采樣一致性算法，即PROSAC（Progressive Sampling Consensus）[3]，來(lái)消除迭代次數(shù)不穩(wěn)定問(wèn)題，將兩種誤匹配點(diǎn)剔除方式與ORB算法結(jié)合對(duì)比。利用Kinect v2相機(jī)對(duì)改進(jìn)的RGB-D SLAM[4]算法進(jìn)行實(shí)驗(yàn)，為了使結(jié)果更加準(zhǔn)確，利用圖優(yōu)化的方式對(duì)相機(jī)位姿進(jìn)行優(yōu)化，獲得三維點(diǎn)云地圖和相機(jī)軌跡，進(jìn)而證明了ORB+PROSAC算法的優(yōu)勢(shì)。

2? ORB特征點(diǎn)匹配算法（ORB feature point matchingalgorithm）

2.1? ?特征點(diǎn)提取

2.3? ?特征點(diǎn)匹配

如圖2所示為兩張圖片間的特征點(diǎn)匹配，特征匹配解決了SLAM中的前端數(shù)據(jù)關(guān)聯(lián)問(wèn)題。假設(shè)在圖像中提取特征點(diǎn)，m=1，2，…，M，在圖像中提取特征點(diǎn)，n=1，2，…，N進(jìn)行特征點(diǎn)匹配。最簡(jiǎn)單的特征匹配方法就是暴力匹配（Brute-Force Matcher），即對(duì)每一個(gè)特征點(diǎn)，與所有的測(cè)量描述子的距離進(jìn)行排序，取最近的一個(gè)作為匹配點(diǎn)。描述子距離表示了兩個(gè)特征之間的相似程度，在實(shí)際運(yùn)用中可以取不同的距離度量范數(shù)。對(duì)于BRIEF描述子，用漢明距離（Hamming distance）[8]為度量。兩個(gè)二進(jìn)制串之間的漢明距離指的是它們不同位數(shù)的個(gè)數(shù)。當(dāng)特征點(diǎn)數(shù)量很大時(shí)，暴力匹配法運(yùn)算量將變大，尤其是當(dāng)匹配一個(gè)幀和一張地圖的時(shí)，不能滿足SLAM中的實(shí)時(shí)性需求，因此可以用快速近似最近鄰（FLANN）[9]算法進(jìn)行匹配。

3? 基于 PROSAC 的誤匹配剔除算法（Mismatch-based culling algorithm based on PROSAC）

3.1? ?RANSAC算法原理

RANSAC算法用于二維平面上對(duì)隨機(jī)點(diǎn)進(jìn)行采樣并擬合出一條直線。具體算法：隨機(jī)從二維平面中選取兩個(gè)點(diǎn)，將兩點(diǎn)進(jìn)行連線。設(shè)定距離閾值，分別計(jì)算平面中其他點(diǎn)和該直線的距離，當(dāng)距離小于設(shè)定閾值時(shí)，則被認(rèn)為是內(nèi)點(diǎn);當(dāng)距離大于設(shè)定閾值時(shí)，則被認(rèn)為是外點(diǎn)，不斷重復(fù)隨機(jī)采樣的過(guò)程，找出所有最佳距離點(diǎn)，擬合成直線，如圖3所示為RANSAC隨機(jī)采樣過(guò)程。

3.2? ?PROSAC算法原理

RANSAC算法中選取的樣本點(diǎn)是隨機(jī)的，需要不斷的迭代才能選取出盡量多的內(nèi)點(diǎn)，因此整個(gè)計(jì)算過(guò)程時(shí)間增加，迭代次數(shù)不穩(wěn)定。PROSAC算法和RANSAC算法有所不同，首先是將樣本點(diǎn)進(jìn)行預(yù)排序，篩選出符合的樣本點(diǎn)，然后對(duì)匹配模型進(jìn)行估計(jì)。這樣減少模型的迭代次數(shù)，且模型正確率高。在二維平面隨機(jī)點(diǎn)擬合直線過(guò)程中，首先將樣本集中點(diǎn)部分分為兩個(gè)部分，在進(jìn)行距離閾值設(shè)定，選取最佳內(nèi)點(diǎn)，從而最后擬合出最佳直線，如圖4所示為PROSAC隨機(jī)采樣過(guò)程。

3.3? ?ORB+PROSAC與ORB+RANSAC特征點(diǎn)檢測(cè)實(shí)驗(yàn)

為了驗(yàn)證ORB算法分別和RANSAC算法和PROSAC算法結(jié)合的有效性，運(yùn)用OpenCV工具進(jìn)行對(duì)比實(shí)驗(yàn)。實(shí)驗(yàn)結(jié)果如圖6—圖10所示。

4? 基于RGB-D傳感器的3D SLAM（3D SLAM based on RGB-D sensor）

4.1? ?幀間相機(jī)運(yùn)動(dòng)恢復(fù)

完成特征提取與匹配之后，可根據(jù)已經(jīng)匹配的特征對(duì)相機(jī)進(jìn)行運(yùn)動(dòng)估計(jì)[13]。根據(jù)特征的信息的不同，可以分為2D坐標(biāo)信息和3D坐標(biāo)信息。2D坐標(biāo)信息可以從圖像中直接獲取，3D坐標(biāo)深度信息可以通過(guò)其他方式獲取，單雙目相機(jī)可以通過(guò)三角化的方法獲取，RGB-D傳感器可以直接通過(guò)深度圖像與RGB圖像配準(zhǔn)獲得。求解相機(jī)運(yùn)動(dòng)有三種方式：2D-2D、3D-2D和3D-3D。2D-2D相機(jī)位姿估計(jì)計(jì)算兩幀圖像之間的本質(zhì)矩陣，通過(guò)構(gòu)建最小二乘法和SVD分解方法將本質(zhì)矩陣分解，求解R和t，再乘計(jì)算的t的相對(duì)尺度，最終獲得相機(jī)運(yùn)動(dòng)。本質(zhì)矩陣有五個(gè)自由度，三個(gè)旋轉(zhuǎn)加上三個(gè)平移，減去一個(gè)尺度化因子。本質(zhì)矩陣的計(jì)算至少需要五組圖像對(duì)應(yīng)點(diǎn)，通常采用五點(diǎn)算法和八點(diǎn)算法計(jì)算。3D-2D的方法是最小化重投影誤差的方法計(jì)算，而3D-3D的方法則是最小化三維位姿誤差，因此3D-2D方法比3D-3D方法更精確，最小化重投影誤差的形式是為了找到式（9）中最小的Tk：

4.2? ?ICP算法

迭代最鄰近點(diǎn)（Iterative Closest Points，ICP）算法常用于對(duì)齊兩組點(diǎn)云。假設(shè)存在兩組名為Pr和Pl的點(diǎn)云，通過(guò)計(jì)算方程的最小值來(lái)獲得兩組點(diǎn)云的剛性變換，為了計(jì)算使方程最小的旋轉(zhuǎn)和平移矩陣R和t，在兩組點(diǎn)云Pr和Pl中根據(jù)一定的約束條件，尋找最鄰近點(diǎn)，然后計(jì)算最優(yōu)匹配參數(shù)R和t，則誤差函數(shù)為：

4.3? ?位姿優(yōu)化

傳統(tǒng)的SLAM基于濾波和非線性優(yōu)化的方式進(jìn)行優(yōu)化，效率較低。引入圖優(yōu)化[14]方式的優(yōu)化方法可以完成相機(jī)局部和全局的位姿優(yōu)化。圖由節(jié)點(diǎn)和邊組，pose節(jié)點(diǎn)代表相機(jī)位姿，point節(jié)點(diǎn)代表相機(jī)觀測(cè)到的路標(biāo)點(diǎn)，邊代表兩個(gè)節(jié)點(diǎn)間的約束。完全構(gòu)建圖后，應(yīng)調(diào)整相機(jī)的姿勢(shì)以滿足邊界約束。用誤差函數(shù)表達(dá)兩個(gè)節(jié)點(diǎn)滿足約束的程度?；趫D優(yōu)化的SLAM系統(tǒng)分為兩個(gè)模塊，即前端和后端。在前端中，需要從傳感器數(shù)據(jù)中提取幾何約束，并解決數(shù)據(jù)關(guān)聯(lián)的問(wèn)題。最后，構(gòu)造圖結(jié)構(gòu)的節(jié)點(diǎn)和邊。在后端模塊中，尋找狀態(tài)向量，該狀態(tài)向量可以使幾何約束的似然函數(shù)最大化。將SLAM抽象成圖的形式之后節(jié)點(diǎn)表示待優(yōu)化變量，邊表示誤差項(xiàng)約束條件，包括相機(jī)節(jié)點(diǎn)間約束，以及相機(jī)位姿和路標(biāo)點(diǎn)的約束。前者依賴于系統(tǒng)閉環(huán)檢測(cè)產(chǎn)生的約束，如圖12和圖13所示。后者依賴于相機(jī)對(duì)路標(biāo)點(diǎn)觀測(cè)產(chǎn)生的約束，如圖11所示。通過(guò)構(gòu)建圖優(yōu)化的方式可以縮小系統(tǒng)的誤差累積，從而構(gòu)建信息一致的地圖。

4.4? ?地圖構(gòu)建

地圖的構(gòu)建[15]可以分為局部地圖和全局地圖兩種。局部地圖保留了當(dāng)前位置附近特征點(diǎn)信息，這些特征點(diǎn)用來(lái)與當(dāng)前幀進(jìn)行匹配求解當(dāng)前相機(jī)位姿，隨著相機(jī)的運(yùn)動(dòng)新的幀不斷被加進(jìn)來(lái)，數(shù)據(jù)維護(hù)將會(huì)變大，因此需要丟棄之前的幀以保證系統(tǒng)實(shí)時(shí)性。全局地圖則是記錄所有特征點(diǎn)的信息，后期通過(guò)地圖的加載和匹配可用于機(jī)器人的定位導(dǎo)航和路徑規(guī)劃。系統(tǒng)在傳感器數(shù)據(jù)中提取關(guān)鍵幀，估計(jì)關(guān)鍵幀匹配關(guān)系?？梢杂茫˙undle Adjustment，BA）方式優(yōu)化關(guān)鍵幀匹配點(diǎn)的重投影誤差，由于地圖點(diǎn)和關(guān)鍵幀是冗余的，需要對(duì)關(guān)鍵幀進(jìn)行剔除，才能獲得魯棒的地圖。到目前為止，已經(jīng)可以獲得全局一致的相機(jī)軌跡，通過(guò)引入深度信息，跟蹤在全局3D坐標(biāo)系中投影所有測(cè)量點(diǎn)坐標(biāo)，可生成環(huán)境的點(diǎn)云地圖。

5? ?基于kinect v2的 SLAM實(shí)驗(yàn)設(shè)計(jì)與分析（Design?and analysis of SLAM experiment based onkinect v2）

5.1? ?基于ORB+PROSAC特征提取的SLAM系統(tǒng)框架

如圖14所示為RGB-D SLAM系統(tǒng)框架，共五個(gè)主要模塊，包括特征檢測(cè)、運(yùn)動(dòng)估計(jì)、ICP算法、基于圖優(yōu)化的位姿優(yōu)化和地圖構(gòu)建。利用Kinect傳感器獲取深度圖像和RGB圖像，使用ORB算法進(jìn)行特征檢測(cè)，同時(shí)利用PROSAC算法在圖像特征點(diǎn)匹配中剔除外點(diǎn)，獲得更精確的匹配點(diǎn)，準(zhǔn)確估計(jì)相機(jī)運(yùn)動(dòng)。為了獲得相機(jī)的全局最優(yōu)姿態(tài)和環(huán)境地圖，通過(guò)將圖優(yōu)化方法應(yīng)用于系統(tǒng)，提取深度信息，可生成實(shí)驗(yàn)室環(huán)境3D點(diǎn)云地圖。

5.2? ?實(shí)驗(yàn)硬件平臺(tái)

為檢驗(yàn)ORB+PROSAC特征點(diǎn)匹配算法實(shí)際運(yùn)行效果，使用事先標(biāo)定好的Kinect V2相機(jī)，以及內(nèi)存4GB、CPU為Intel i5 4210M、2.6GHz、顯卡為Intel HD4600集成顯卡筆記本電腦作為硬件平臺(tái)，在實(shí)驗(yàn)室環(huán)境下對(duì)算法進(jìn)行實(shí)際測(cè)試。如圖15所示為Kinect V2相機(jī)。

5.3? ?實(shí)驗(yàn)結(jié)果及分析

由于缺少外部設(shè)備對(duì)相機(jī)進(jìn)行跟蹤，無(wú)法計(jì)算得到各時(shí)刻相機(jī)位置與真實(shí)相機(jī)位置之間的誤差。從運(yùn)行結(jié)果來(lái)看，RGB-D SLAM算法能夠很好地進(jìn)行初始化，且高效實(shí)時(shí)運(yùn)行，如圖16所示相機(jī)在特征點(diǎn)跟蹤階段比較穩(wěn)定，無(wú)丟幀或者失幀的情況。此外，在回環(huán)檢測(cè)階段能夠?qū)ο鄼C(jī)軌跡進(jìn)行全局調(diào)整，配準(zhǔn)傳感器深度信息，建立如圖17和圖18所示實(shí)驗(yàn)室環(huán)境特征點(diǎn)地圖和3D點(diǎn)云地圖，因此實(shí)驗(yàn)證明ORB+PROSAC方式效率高，魯棒性好。

6? ?結(jié)論（Conclusion）

本文從RANSAC算法和PROSAC算法的對(duì)比分析入手，理論分析可知RANSAC算法迭代次數(shù)不穩(wěn)定、匹配效率低，PROSAC算法引入了質(zhì)量因子，通過(guò)質(zhì)量因子分級(jí)質(zhì)量好的內(nèi)點(diǎn)用于估計(jì)模型的方式，可減少算法的迭代次數(shù)、降低計(jì)算和時(shí)間復(fù)雜度。通過(guò)ORB特征點(diǎn)匹配結(jié)合兩種算法實(shí)驗(yàn)對(duì)比，得出PROSAC算法優(yōu)于RANSAC算法。用Kinect v2相機(jī)對(duì)改進(jìn)的RGB-D SLAM算法進(jìn)行實(shí)際運(yùn)行，獲得三維點(diǎn)云地圖和相機(jī)軌跡，進(jìn)而驗(yàn)證了ORB+PROSAC算法在視覺(jué)SLAM的應(yīng)用魯棒性好，實(shí)時(shí)性強(qiáng)。

參考文獻(xiàn)（References）

[1] 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，2017，31（05）：1147-1163.

[2] Bahraini MS，Bozorg M，RadAB.SLAM in dynamic environments via ML-RANSAC[J].Mechatronics，2018，49（08）：105-118.

[3] Panpan Z，Derui D，Yongxiong W，et al.An improved GMS-PROSAC algorithm for image mismatch elimination[J].Systems Science & Control Engineering，2018，6（01）：220-229.

[4] Ma L，Kerl C，Stuckler J，et al.CPA-SLAM：Consistent plane-model alignment for direct RGB-D SLAM[C].2016 IEEE International Conference on Robotics and Automation（ICRA）.IEEE，2016，8（02）：120-124.

[5] 吳金津，王鵬程，龍永新.基于FAST角點(diǎn)檢測(cè)的圖像配準(zhǔn)算法[J].湖南工業(yè)大學(xué)學(xué)報(bào)，2014，28（04）：71-75.

[6] 張陽(yáng)，許鋼，張星宇.一種改進(jìn)的ORB特征點(diǎn)匹配算法[J].重慶工商大學(xué)學(xué)報(bào)（自然科學(xué)版），2018，179（03）：73-78.

[7] 高翔，張濤.視覺(jué)SLAM十四講：從理論到實(shí)踐[M].北京：電子工業(yè)出版社，2017：133-182.

[8] 邸凱昌，萬(wàn)文輝，趙紅穎.視覺(jué)SLAM技術(shù)的進(jìn)展與應(yīng)用[J].測(cè)繪學(xué)報(bào)，2018，47（06）：84-93.

[9] 許芬，王振.基于Kinect傳感器和ORB特征的視覺(jué)SLAM算法設(shè)計(jì)與實(shí)現(xiàn)[J].計(jì)算機(jī)工程與科學(xué)，2018，40（05）：76-81.

[10] 邢凱盛，凌有鑄，陳孟元.ORB特征匹配的誤匹配點(diǎn)剔除算法研究[J].電子測(cè)量與儀器學(xué)報(bào)，2016，30（08）：1255-1262.

[11] 鄭顧平，閆勃勃，李剛.基于機(jī)器學(xué)習(xí)的多車(chē)牌識(shí)別算法應(yīng)用研究[J].計(jì)算機(jī)技術(shù)與發(fā)展，2018，254（06）：135-138.

[12] 李倫.基于PROSAC算法的室內(nèi)定位算法研究[D].哈爾濱：哈爾濱工業(yè)大學(xué)，2018：36-50.

[13] 辛冠希.基于RGB-D攝像機(jī)的同步定位與建圖研究[D].哈爾濱：哈爾濱工業(yè)大學(xué)，2016：29-54.

[14] 張毅，沙建松.基于圖優(yōu)化的移動(dòng)機(jī)器人視覺(jué)SLAM[J].智能系統(tǒng)學(xué)報(bào)，2018，13（02）：290-295.

[15] 楊林志.基于視覺(jué)的移動(dòng)機(jī)器人SLAM算法研究[D].成都：西南交通大學(xué)，2018：42-66.