基于PROSAC算法與ORB-SLAM2的RGB-D相機室內視覺定位研究*

曹 蜜,胡凌燕,熊彭文,彭 杰,曾 雷

(南昌大學信息工程學院,南昌 330031)

同時定位和建圖(Simultaneous Localization and Mapping,SLAM)是指機器人在構建當前環境地圖時,同時定位自身位置和姿態的過程[1]。SLAM技術在過去30年中取得了驚人的進步,其實時性問題是一直是移動機器人導航控制研究過程中未解決的問題,它是當前的研究熱點,也是移動機器人在未知環境中真正實現自主導航的關鍵問題[2]。

近年來,隨著圖像處理技術的進步,以及視覺傳感器(攝像機)較激光傳感器的價格低廉的優勢,基于視覺信息完成定位建圖的視覺SLAM(Visual SLAM,VSLAM)逐漸成為當前移動機器人領域研究的熱點[3]。

視覺SLAM系統使用攝像頭作為主傳感器來估計機器人的位姿和構建環境地圖[4]。根據利用圖像信息的不同,視覺SLAM算法可分為基于直接法的SLAM方法和基于特征的SLAM方法[5]。基于直接法的SLAM方法中,Forster C等人[6]提出了SVO:Fast semi-direct monocular visual odometry,SVO算法可以在嵌入式系統中實時運行,但位姿估計存在累計誤差,并且丟失后不太容易進行重定位;為了構建稠密的三維環境地圖,Engel J等人[7]提出了 LSD-SLAM(large-scale direct SLAM)算法,與文獻[6]提出的直接的視覺里程計方法相比,該方法在提高相機估計位姿準確性的同時,能夠創建大規模的三維環境地圖,但無法完成稠密的建圖,且單目真實尺度未知,可能出現地圖大小與真實地圖大小偏差很大;Kinect-fusion[8]是第一個基于Kinect的能在GPU上實時構建稠密三維地圖的算法,但實時性差,精度低;RGBD-SLAMv2[9]結合彩色圖像和深度圖像,由g2o實現最優相機位姿估計,并提出了基于熵的關鍵幀提取及閉環檢方法,大大降低了軌跡誤差,但集成度高,計算量大,運用場景有限。基于特征的SLAM方法由濾波算法逐漸發展為關鍵幀算法[5],Strasdat等人[10]證明基于關鍵幀的方法比基于濾波的方法精度更高。基于關鍵幀方法的單目視覺中,最具代表性的是parallel tracking and mapping(PTAM)[11],將跟蹤和映射分成單目SLAM中的兩個平行線,但是場景小,跟蹤容易丟失;ORB-SLAM[12]基于PTAM,提出了一個單目視覺SLAM系統,ORB-SLAM2[13]在此基礎上擴展為雙目、RGB-D相機,二者均將整個SLAM過程分為定位、創建地圖、閉環檢測三個線程,是一種基于稀疏特征的輕量級SLAM定位方案,但無法實現機器人導航;且由于特征追蹤存在誤差,誤差不斷累積會導致后面幀的位姿離實際位姿越來越遠,最終影響系統整體的精度。

RANSAC(Progressive Sample Consensus,隨機抽樣一致性)[14]算法是一種魯棒的方法。在一組包含噪聲的具有對應關系的數據中,該算法通過隨機抽取樣本數據,計算模型參數。該算法可以避免錯誤信息,是一種功能強大的模型參數估計方法,它廣泛用于各個領域,如消除圖像特征不匹配和自動估計單應性等。但由于RANSAC數據處理具有隨機性的特點,算法的效率較低。PROSAC(PROgressive SAmple Consensus,順序抽樣一致性)[15]算法是一種基于RANSAC改進的算法,該算法首先對數據進行相似性排序,選擇相似性排名靠前的對應關系作為子集,從子集抽取樣本數據進行模型參數估計。與RANSAC算相比,是一種半隨機的參數估計方式。在圖像匹配誤差剔除過程中,求解單應性矩陣更加高效。

基于以上研究現狀,我們開展以Kinect 2.0作為圖像輸入,基于ORB-SLAM2的RANSAC誤差剔除改進算法的研究,同時為機器人導航未來工作建立稠密地圖奠定基礎。本文采用PROSAC算法對圖像匹配誤差進行剔除以及求解單應性矩陣H獲得相機估計位姿,以實現更加高效準確的實時相機定位跟蹤。再結合ORB-SLAM2,通過關鍵幀跟蹤,實時建圖,回環檢測三線程數據處理,得到準確的定位。圖像誤差剔除匹配實驗結果表明,PROSAC算法可以明顯提高運算效率,整個匹配過程相對于RANSAC算法效率提高一倍。將該算法結合ORB-SLAM2進行定位實驗結果表明,該算法能夠獲得與傳統ORB-SLAM2算法幾乎相同的定位精度,兩者的絕對軌跡誤差均方根誤差僅相差0.000 4 m,但該算法的效率明顯提高,保證了整個定位系統實時性。

圖1 PROSAC算法與ORB_SLAM2系統框架

1 系統結構框架

ORB-SLAM2包括特征跟蹤、局部建圖、回環檢測以及在回環檢測后的全局BA[16]優化這3+1線程,可實現地圖重用、回環檢測以及重新定位等功能。在本文中,我們將PROSAC算法取代原ORB-SLAM2中的RANSAC算法,實現特征匹配誤差剔除,以求解單應性矩陣H獲得相機估計位姿,應用于ORB-SLAM2的特征跟蹤線程。PROSAC算法與ORB-SLAM2系統結合框架圖,如圖1所示。

該系統框架具體實施過程如下:

①在跟蹤線程,首先使用詞袋模型[17]對前后幀圖像的特征點進行預分類,實現特征點快速匹配。針對特征匹配點對,采用PROSAC算法進行更加高效準確的誤匹配剔出處理,以計算單應性矩陣H并估計相機位姿。然后跟蹤局部地圖,通過投影在當前幀與局部地圖之間查找對應關系以對當前幀的位姿進行優化。最后判斷是否需要插入新的關鍵幀。

②在局部建圖線程,將新的關鍵幀作為新節點加入共視圖后,更新共視圖、邊以及關鍵幀生長樹,并插入到地圖中。然后剔除不符合約束測試的地圖點,以保證只保留高質量的地圖點,防止外點影響建圖。接下來檢查關鍵幀中ORB特征點的正向景深、視差、反投影誤差和尺度一致性,無誤后創建一個新的地圖點。接下來對關鍵幀和地圖點執行局部BA優化,最后剔除冗余關鍵幀以降低BA復雜度。

③在閉環檢測線程,計算加入回環檢測線程中的回環候選關鍵幀與當前關鍵幀的相似度,若判斷確實有回環,則融合重復的地圖點,并在共視圖中插入新的邊以連接閉環。最后執行全局的位姿和地圖點優化,并更新關鍵幀位姿和地圖點坐標。

2 圖像誤匹配剔除算PROSAC算法

本文提出改進的PROSAC算法剔出誤差匹配,求解參數模型的方法。該方法基于“置信度更高的點是內點的概率更高”這一假設[15],在選取樣本點時置信度高(質量好)的數據將被優先選擇。PROSAC算法在盡可能短的時間內,找到所有初步對應集合中的內點,并且盡可能保證所有內點被找到。RANSAC算法是隨機抽取樣本,沒有考慮到樣本之間的差異性,PROSAC算法則先將樣本質量好壞進行降序排列,再從具有較高質量的樣本子集中抽取樣本,求解模型參數,PROSAC算法的魯棒性和計算效率比RANSAC更高[18]。

該方法引入質量函數q(uj),以及數據點是內點的概率P{uj},有以下關系:

當i

P{ui}≥P{uj}

(1)

根據特征匹配點歐氏距離的比值β,對特征點匹配質量進行估計,將β作為質量函數q(uj)的定量表示,β為:

β=D1/D2

(2)

式中:D表示ORB特征點描述子的歐式距離,D1表示歐式距離最小值,D2表示次小值,比值β越小,匹配質量越好,匹配正確的概率越高。

PROSAC算法需要解決以下兩個問題:

①假設生成集大小的選擇

準則增長函數n=g(t)定義了集合Un中的質量排名前n的對應關系,這n個對應關系將被進行t次采樣。定義增長函數為:

g(t)=min{n:T′n≥t}

(3)

式中:T′n表示Tn次采樣中,樣本點來源于Un的次數;n表示采樣次數大于t時的最小n。

②采樣停止標準

當集合Un中內點的數量In滿足以下兩個標準:非隨機性——In是非正確模型的內點的概率低于ψ(一般設為5%);極大性——在集合Un中,有超過In個內點,且經過K次抽樣仍未被找到的概率低于η0(一般設為5%)。采樣將會停止。

隨機類內點集合元素有以下二項分布:

(4)

式中:β是與一個錯誤模型相對應的點不在采樣點集合中的概率,并且該采樣點集合包含一個類外點。

(5)

在Un中發現一個非隨機內點須滿足:

(6)

極大性定義了需要多少次采樣才能保證解的置信度,定義PIn表示從Un中采樣得到的m個點都來自內點的概率,εn表示質量排名前n的數據中內點比例大小,有:

(7)

(8)

η表示經過K次采樣后,集合Un中丟失一組內點的概率,有以下關系:

η=(1-PIn)k

(9)

Kn*(η0)≥log(η0)/log(1-PIn*)

(10)

式中:n*表示使得Kn*(n0)采樣終止的長度,η0=5%。

PROSAC算法對質量q進行降序排序,質量排名較前的匹配點對用于求取單應性矩陣H。對質量因子進行排序時,雖然增加了計算,但在迭代過程中大大減小了迭代次數,從而在整個求解模型參數過程中,減少了運算量,提高了算法效率。PROSAC算法流程圖如圖2所示。

圖2 PROSAC算法流程圖

3 實驗及其結果分析

3.1 特征點匹配對比實驗

為了驗證PROSAC算法的高效性,本文對PROSAC算法與RANSAC算法進行對比實驗。本文將6張圖片分為三組,三組特征點對數量梯度分別為:特征多、特征較多與特征少,進行誤匹配剔除時間對比實驗,并對結果進行了分析比較。實驗平臺在英特爾四核i5 CPU,4G RAM和256G SDD的LENOVO YOGA710筆記本上進行實驗數據處理。實驗中我們以0.7倍最大歐式距離比值作為閾值進行粗匹配,設置最大迭代次數為2000次。

如表1比較了PROSAC算法與RANSAC算法在進行特征點匹配實驗時的各項性能指標。當內點數量比外點數量多時,在特征點數量較少的情況下,使用PROSAC算法剔除外點求解模型參數,在效率上仍要優于RANSAC算法。

表1 PROSAC算法與RANSAC算法各項指標比較

由表1中兩種算法之間誤匹配剔除時間的差異可知,PROSAC算法雖然在質量排序計算上增加了耗時,但在特征匹配及誤匹配剔除求解單應性矩陣H整個過程中總耗時下降。當特征點數量較多時,計算機使用PROSAC算法可以以20幀/s的速度進行圖像匹配處理,而RANSAC算法只能處理大概11幀,速度提高了近1倍。當ORB-SLAM系統不斷采集圖像數據,不斷進行圖像誤匹配剔除求解模型參數最優解計算,PROSAC算法將會大大提高ORB-SLAM線程處理數據的速度。

由于三組情況下的誤匹配剔除效果相似,本文選取特征點較多的一組圖片進行展示。我們使用ORB特征進行特征提取,ORB特征原始匹配如圖3(a)所示,存在較多錯誤匹配;PROSAC算法處理后如圖3(b),圖像剔除了錯誤匹配,效果良好。

圖3 特征點匹配結果

3.2 定位實驗

PROSAC算法結合ORB-SLAM2后的系統定位精度與傳統方法有幾乎相同的效果。我們基于 RGBD-benchmark的兩個數據集和真實環境的定位實驗進行驗證,并對本文方法與傳統方法的軌跡進行分析比較。在實驗中,當我們將本文方法中的PROSAC算法迭代次數設置為100次,傳統方法中的RANSAC算法迭代次數設置為300次不變時,發現定位效果幾乎一致。

3.2.1 基于RGBD-benchmark數據集

RBGD-benchmark所有數據集序列由高精度運動捕獲系統提供,其軌跡可認為是真實軌跡。實驗平臺在英特爾四核i5 CPU,4G RAM和256G SDD的、筆記本上進行實驗數據處理。由于ORB-SLAM2在紋理特征不明顯的環境中,易出現特征跟蹤丟失無法形成閉環的情況,本文以rgbd_dataset_freiburg1-desk(以下簡稱Desk)和rgbd_dataset_freiburg2_pioneer_slam3(以下簡稱Slam3)作為實驗數據集,分析對比本文方法與傳統方法在形成閉環與無閉環情況下的軌跡精度,其中Desk數據集長度為9.263 m,Slam3數據集長度為18.135 m。并采用絕對軌跡誤差的均方根誤差來評價估計的軌跡偏差。絕對軌跡均方根誤差計算公式:

(11)

圖4是本文方法與傳統方法針對Desk與Slam3兩個數據集,在有回環與無回環情況下的絕對軌跡誤差對比實驗結果。圖4(a)、4(b)表示在ORB-SLAM2在特征跟蹤過程中,一直保持在SLAM模式下,形成閉環,與真實軌跡相比較,可以看出均表現出了良好的定位跟蹤效果。圖4(c)、4(d)表示ORB-SLAM2在定位過程中,由于光照和環境特征不明顯等原因,出現特征跟蹤丟失的情況,沒有形成閉環。但在SLAM模式階段,本文方法與傳統方法均保持了較好的定位效果。

圖4 不同數據集絕對軌跡誤差對比

針對以上數據集實驗結果,我們分別計算了兩組數據集在不同方法下的均方根誤差、中值以及位姿對比對數,如表2所示。

表2 基于數據集各項指標比較

如表2所示,對于定位形成閉環的Desk1數據集,傳統方法與本文方法的均方根誤差僅相差0.000 552 m,中值僅相差0.000 524 m,而位姿對比對數均達500對以上,二者僅相差1對;對于定位未形成閉環的Slam3數據集,傳統方法與本文方法的均方根誤差僅相差0.000 372 m,中值僅相差0.001 892 m,而位姿對比對數均達1 500對以上,二者僅相差9對。

圖4和表2的實驗結果表明,采用RANSAC算法的傳統ORB-SLAM2系統與采用PROSAC算法降低迭代次數后的本文方法的系統,在定位效果及精度上,均保持了較好的定位效果,且差別很小。

3.2.2 基于真實環境的實時定位

基于實驗室真實環境,我們再次驗證了采用本文方法系統的定位精度仍能保持良好的實時定位效果。本實驗以Kinect2.0和移動機器人Summit為數據采集平臺,在英特爾四核i5 CPU,4G RAM和256G SDD的筆記本上進行實驗數據處理,圖像數據以ROS(機器人操作系統)主題的形式發布進行實驗。為盡可能保證圖像質量,我們對Kinect2.0傳感器進行了標定,同時在明亮且光照均勻實驗室內進行試驗[19-20]。

將Kinect2.0安放在移動機器人Summit上,控制機器人圍繞長3.5 m,寬2 m的標準矩形行走一圈。機器人先以0.1 m/s的線速度沿直線運動2.5 m,再以0.26 rad/s(15 °/s)的角速度,0.5 m/s的線速度沿著半徑為0.5 m圓弧轉彎,行走1/4圓后繼續沿直線以0.1 m/s的速度運動1.5 m,如此往復,行走一圈。

圖5(a)為實驗平臺,移動機器人Summit搭載相機Kinect2.0獲取數據,并用筆記本進行實驗數據處理;圖5(b)代表ORB-SLAM2定位與地圖構建過程,其中藍色代表相機在運動過程中存儲的關鍵幀,綠色幀表示當前關鍵幀位置,綠色線條代表相機運動軌跡,黑色和紅色的點代表儲存的地圖點。

本文方法與傳統方法的軌跡對比如圖6所示,軌跡絕對誤差對比如圖7所示。

圖5 實驗平臺及實驗過程

圖6 移動機器人定位軌跡對比

由圖7可知,實驗采集的2 680幀數據中,有四個階段出現絕對誤差峰值,這是由于相機快速旋轉帶來場景圖像變換,使得圖像特征匹配時誤匹配較多,導致絕對誤差較大。在誤差較小時刻,相機在做平穩的直線運動,匹配效果較好。基于PROSAC的本文方法與基于RANSAC的傳統方法雖存在差別,但差別較小,跟蹤效果良好。分別對兩種方法的絕對軌跡誤差均方根誤差進行計算,本文方法值為0.023 9 m,傳統方法值為0.024 3 m,二者僅相差0.000 4 m。機器人在實時場景運動過程中,使用本文方法定位軌跡與使用傳統方法二者軌跡的誤差區別很小,幾乎沒有區別。但PROSAC在誤匹配剔除方面效果明顯提高,實時性比較好。

圖7 絕對誤差對比

4 結論

本文提出用PROSAC算法取代ORB-SLAM2中的RANSAC算法的室內視覺定位方法。PROSAC算法通過將樣本質量好壞進行降序排列,再從具有較高質量的樣本子集中抽取樣本,最后對誤匹配數據進行剔除求解模型參數,可以明顯提高運算效率。

實驗結果表明,相對于RANSAC算法在誤匹配剔除計算模型參數上效率提高一倍,將該算法結合ORB-SLAM2進行定位,該算法能夠獲得與傳統ORB-SLAM2算法幾乎相同的定位精度,兩者的軌跡誤差均方根誤差僅相差0.000 4 m。用PROSAC算法取代ORB-SLAM2中的RANSAC算法可以在不降低整個定位準確度的情況下,提高明顯提高算法效率,保證實際定位過程的實時性和流暢性。

由于ORB-SLAM2是一種基于稀疏特征的輕量級定位方案,無法利用其稀疏地圖進行機器人導航[21]。未來我們將利用Kinect2.0,基于PROSAC算法與ORB-SLAM2,進一步進行稠密地圖構建,實現機器人導航[22]。