一種基于改進閉環檢測的視覺SLAM算法

趙 磊 李振偉 楊曉利 張 衛

(河南科技大學醫學與技術工程學院 河南 洛陽 471000)

0 引 言

SLAM是指機器人利用相機、雷達等傳感器探測周圍環境信息,估計自身位姿,并對環境地圖進行描述的一種技術[1]。機器人和計算機視覺的許多相關應用都要求能夠快速獲取環境的三維模型,并根據該模型估計相機的姿態,因此基于視覺的SLAM技術(vSLAM)引起了眾多研究者的關注,vSLAM是未來機器人在復雜環境中能夠完成諸多任務,實現智能化發展的重要技術。閉環檢測作為SLAM系統的關鍵部分,能夠有效消除前端里程計進行位姿估計時引起的誤差,對長時間大范圍內的機器人導航定位極為重要[2-4]。目前,對于基于視覺SLAM框架,國內外學者提出了許多閉環檢測算法。

最直接也最準確的閉環檢測方法是:對所有關鍵幀一一進行特征匹配,根據匹配結果得到最相似的圖像。但該方法計算量大,在大規模的SLAM系統中無法滿足實時性的要求。為此,基于外觀的閉環檢測方法被提出,該方法擺脫了前后端的相機位姿估計,使其相對獨立于整個系統中的其他模塊,已經成為了視覺SLAM中主流的方法。Mur-Artal等[5-6]提出的ORB-SLAM便是該方法的突出代表,其引入的閉環檢測功能以及改進的關鍵幀選取策略使得該算法取得了非常不錯的效果。王遠配等[7]通過引入能夠實現尺度不變性的ORB特征點,實現了較高的相似圖像匹配率。李弋星等[8]提出一種關鍵幀的選擇方法,對新插入的關鍵幀設定嚴格的閾值要求,提高了系統的處理速度和建圖精度。Cummins等[9]通過Chow-Liu樹對詞袋模型中不同單詞之間的相互聯系進行了分析,提出了一種利用外觀數據進行導航的概率模型。這些方法一定程度上都通過可以代表局部環境信息的關鍵幀進行整體優化,而關鍵幀的合理選擇又能夠為閉環檢測模塊提供更好的數據依據。對于目前基于外觀的閉環檢測算法,絕大多數都建立在詞袋模型框架上,該框架為我們快速高效地查找可能存在閉環的圖像提供了一個有效的工具。利用詞袋模型及其對應的相似性度量方法進行候選閉環幀的選取對SLAM系統構建全局一致的地圖有著十分重要的意義,但在這方面進行改進的工作并不多。

近些年來,視覺SLAM發展很快,在兼顧處理速度的同時還取得了不錯的精度,但通過特征提取進行相機位姿估計仍占用了整個系統不少的時間。基于以上分析,本文利用深度相機進行圖像采集,減少前端的計算量,對關鍵幀以及候選閉環幀進行合理刪選,并改進詞袋模型的相似性得分方法,減少感知歧義,最后利用ORB-SLAM2系統和標準數據集進行算法驗證。

1 算法框架

本文的視覺SLAM算法整體框架如圖1所示。相比SIFT和SURF特征,ORB在取得不錯的精度的同時,還具有較高的計算速度,因此本文利用ORB進行圖像特征提取。為了避免誤匹配引起的誤差,利用RANRAC算法[10]檢測錯誤的特征點并進行剔除,并通過深度數據獲取特征點的3D位置,結合PnP(Perspective-n-Point)算法[11]實現前端的相機位置估計。然后在詞袋模型的基礎上,利用本文改進的閉環檢測方法識別閉環幀,并剔除錯誤幀,最后使用g2o優化工具對前端計算的相機軌跡進行全局優化,得到更為精確的軌跡和點云地圖。

圖1 SLAM系統框架

2 改進閉環檢測算法

視覺SLAM前端僅僅通過圖像匹配估計相機的大致運動軌跡,由于傳感器本身會存在誤差,加之部分場景中圖像特征不明顯,導致特征點對特征點的約束關系使得前端估計產生誤差。該誤差會隨著時間的推移不斷累積至下一幀圖像,最后導致系統估計的地圖與真實的環境相差甚遠。而閉環檢測則通過判斷當前圖像與歷史圖像的相似關系,為后端優化提供一種更強、時間更久遠的約束,通過這種約束關系,可以有效地減少累積誤差,得到更加準確的相機軌跡和地圖。

目前普遍利用計算機視覺進行閉環檢測的方法主要有三類,分別為:地圖對地圖[12]、圖像對地圖[13]和圖像對圖像[14]的匹配方法,Williams等[15]通過比較三種方法,得出最后一種方法的匹配性能最好。但將當前圖像與所有歷史圖像進行對比的時間復雜度太高,不利于實時地圖構建,因此詞袋模型得到了廣泛使用。該方法通過視覺單詞對圖像進行描述,能夠有效地檢測出閉環圖像。但由于特征的分層投影步驟,使得傳統的相似性得分方法存在一定的缺陷。另外,通過詞袋模型聚類得到的單詞向量只注重單詞的存在與否,忽略了圖像幾何信息,容易發生相似圖像誤匹配的情況,影響閉環檢測的準確率。

針對以上存在的問題,首先對前端提取的關鍵幀進行差分信息熵判斷,剔除不符合條件的冗余幀,減少前端計算量,而且為后端優化提供更好的數據;然后利用詞袋模型建立視覺單詞樹,并在文獻[16]的基礎上改進得分函數,得到大于相似度評分閾值的閉環候選幀;并利用改進的感知哈希算法對出現誤匹配的閉環圖像進行剔除,最終得到正確的閉環。

2.1 關鍵幀的篩選

相機采集圖像的過程是連續的,而且采集頻率較高,以Kinect為例,可以達到每秒30幀。如果沒有對短時間內選取的關鍵幀進行合理篩選,會導致圖像相似度過高,進而影響閉環檢測的準確性。本文在ORB_SLAM2的關鍵幀選取機制的基礎上,引入幀間位姿變化,選出滿足條件的圖像作為關鍵幀。

文獻[17]通過比較當前幀與前一幀的信息熵差異和當前幀與前n幀差異的均值進行冗余幀的判斷,該方法只比較了當前幀和歷史幀,忽略了各歷史幀之間的聯系,容易出現誤判,因此本文在此基礎上提出新的冗余幀判斷方法。設當前幀圖像為Ii+1,上一幀圖像為Ii,用當前幀減去上一幀,得到圖像Ii+1-Ii,通過式(1)求得E(Ii+1-Ii)作為當前幀圖像的差分信息熵,再根據式(2)判斷該差分信息熵的大小程度,對具有較小信息熵的關鍵幀進行剔除。式(1)為信息熵計算公式,pi表示某像素灰度值為i時,該像素在整幅圖像中的頻率大小,式(2)表示當前幀差分信息熵是否小于之前的n幀圖像中兩兩相鄰圖像之間的差分信息熵的均值,σ為均值系數。

(1)

(2)

2.2 改進的相似性得分匹配方法

詞袋模型包括視覺單詞字典的創建和相似度比較。利用Kmeans++算法對圖像數據集進行特征分層聚類生成單詞向量,但平面結構的詞袋模型會導致特征搜索時間過長,因此本文采用k叉樹結構來描述單詞字典,詞匯樹結構如圖2所示。

圖2 視覺詞匯樹結構示意圖

一顆分支因子為k,層數為d的字典樹擁有的視覺單詞個數為:

(3)

單詞字典的容納能力會隨著層數的增加急劇增強,這樣的字典樹足以表征絕大部分圖像。

(4)

(5)

(6)

式中:TFi表示投影到樹節點i上的特征數mi占特征點總數m的比例;IDFi表示節點i的權重;Mi表示至少有一個特征投影到節點i的圖像數;M表示圖像總數。由以上公式可得圖像X在字典樹的整體得分W(X)為Wl(X)的一個組合向量,而Wl(X)則是wi(X)的組合向量,Wl(X)表示X在字典樹的各層投影得分(l∈{1,2,…,d},0層為根節點,無表征能力),wi(X)表示X在某層的各節點投影得分(i∈{1,2,…,kl})。

W(X)=(W1(X),W2(X),…,Wd(X))

(7)

Wl(X)=(w1(X),w2(X),…,wkl(X))

(8)

(9)

(10)

該得分函數通過計算兩幅圖像在同一層的各個節點的得分向量之間的歐氏距離來比較二者的相似性,采用分母加1再求倒數的方法,實現了歸一化處理,而且得分越大,圖像越相像。這樣既減少了感知歧義,又保證了相似性計算的準確性。

根據文獻[16],為了消除視覺字典樹的相似性繼承特性引起的得分繼承誤差,引入得分增量ΔS,并將第l層的增量ΔSl定義為:

(11)

文獻[16]和文獻[18]將金字塔匹配核定義為式(12),并由此得到最終的核函數:

(12)

K(X,Y)=K(W(X),W(Y))=Sd(X,Y)+

(13)

式中:1/pd-l為l層的匹配系數,用于調節不同層之間的匹配差異。但文獻[16]和文獻[18]并沒有對p的取值進行詳細分析,只是說明了p的大小和底層空間的相似性占比呈正比例關系。將式(13)展開可得:

(14)

K(X,Y)=Sd(X,Y)+

(15)

2.3 閉環候選幀的剔除

詞袋模型通過逆序索引的方式可以得到閉環候選幀,但獲得的候選幀中存在誤匹配,錯誤的閉環會嚴重影響整個地圖的結構,因此閉環幀的選擇極為重要。均值感知哈希是通過將圖像進行尺寸壓縮,進而利用圖像的低頻信息來進行哈希值提取與圖像相似性比較的一種算法[19-20],其在獲得較高的準確率的同時,還具有極快的處理速度,可以用于閉環相似圖像的判別。由于均值哈希算法壓縮后的圖像會失去絕大部分高頻信息,對于高頻信息變化更為明顯的兩幅圖像來說會出現相似性誤判,因此本文利用改進的均值感知哈希算法對錯誤的閉環候選幀進行剔除,具體步驟為:

(1) 圖像尺寸縮小:將圖像下采樣至原來的1/n,并保留原來的縱橫比。

(2) 將下采樣后得到的圖像變換為灰度圖像。

(3) 求灰度圖像各列的像素平均值pi,對pi相加求和,并計算平均值Mpi。

(4) 比較pi與Mpi的大小:大于Mpi的將其記為1,否則記為0。

(5) 哈希指紋生成:將第4步中得到的所有的0和1按照固定的順序進行排列,得到哈希指紋。

相似度計算公式為:

1-H(hash(X),hash(Y))/Sp

(16)

式中:H(hash(X),hash(Y))表示圖像X和Y生成的哈希值的漢明距離,Sp表示圖像縮小后的列數。為了增強算法的魯棒性,本文在相似度計算之前,增加了一項判定步驟:

|MPi(X)-MPi(Y)|>Q

(17)

若Q≥q,則表示圖像相似度太小,直接剔除候選閉環幀;否則依據式(16)剔除相似度較小的候選幀。

3 實驗結果與分析

本文實驗使用的計算機配置為:CPU為Intel i5-3210M,主頻2.5 GHz,內存4 GB,操作系統為Ubuntu 16.04。使用TUM標準數據集中的RGBD SLAM數據進行實驗,RGB圖像大小為640×480像素,本文算法中n為100,Sp為64,q為10。

3.1 改進的均值哈希算法測試結果

為了驗證改進的候選幀剔除算法對于相似圖像的有效檢測性,分別從RGBD SLAM的fre1_desk、fre1_floor、fre2_large_with_loop(以下簡稱fre2_loop)、fre2_desk2數據集中選取20幅圖像作為檢測對象(數據集的部分圖像如圖3所示),其中:前5幅圖像的順序間隔相同,從中選取3幅圖像作為閉環圖像;后15幅圖像中包含了部分與閉環圖像相同的外觀特征,順序間隔大小不一。然后利用式(16)計算閉環圖像與其余19幅圖像的相似度,并按照由大到小的順序選出前5幅圖像,對該5幅圖像進行判別,統計正確的圖像個數,結果見表1。

(a) fre1_desk (b) fre1_floor

(c) fre2_loop (d) fre2_desk2圖3 TUM數據集部分圖像

表1 閉環檢測統計結果

由表1的統計數據可知,對于fre2_loop和fre1_loop,由于場景中的低頻信息占據了圖像的絕大部分,隨著相機的移動,圖像低頻信息變化不明顯,導致均值哈希算法出現了較多的誤判。而對于本文算法,除了fre1_floor中有一幅圖像判別錯誤,其他幾個數據集的相似圖像都實現了準確的判別,而且算法耗時也略優。

3.2 軌跡誤差實驗

為了評判本文算法在實際SLAM系統中的運行效果,本文從TUM數據集中選取了4組深度相機數據集進行實驗,每個數據集中都有真實的軌跡文件groundtruth.txt,便于進行算法優劣的對比。均方根誤差(Root Mean Squared Error,RMSE)是評價SLAM算法的重要指標,文獻[21]提供了計算RMSE的評估工具。

四組數據集中,除了fre1_floor不存在閉環外,其他三組都存在局部閉環或全局閉環。表2是本文算法與ORB-SLAM2和文獻[22]中提出的RGBD-SLAM的均方根誤差對比,可以看出,對于存在閉環的三組數據集,本文提出的算法都明顯優于前兩種算法,對于不存在閉環的數據集,本文算法也略優于前兩者。圖4是軌跡對比圖,其中實線是高精度設備記錄的相機運動時的真實軌跡,而虛線則表示本文算法估計的軌跡,可以明顯看出本文算法得到的軌跡與真實軌跡是非常接近的。對于fre1_room,在特征點跟蹤時出現了較長時間內的跟蹤失敗,對于fre2_desk,全局優化階段出現了錯誤擬合,因此這兩個數據集的軌跡誤差相比fre1_floor和fre1_desk2有明顯的增大。

表2 均方根誤差對比 單位:m

(a) fre1_room

(b) fre2_desk

(c) fre1_floor

(d) fre1_desk2圖4 真實軌跡與估計軌跡對比

表3是三種方法的時間對比。文獻[21]采用了SIFT特征描述子進行特征提取,使得整個算法的耗時最長,實時性很差。而本文算法是基于ORB-SLAM2進行改進的,特征提取采用ORB描述子,相比ORB-SLAM2,本文算法在有效減少軌跡誤差的同時,在處理速度上依然有略微的優勢,完全能夠滿足系統的實時性需求。

表3 時間對比 單位:s

4 結 語

本文提出了一種基于改進閉環檢測的視覺SLAM方法。該方法首先利用差分信息熵對視覺前端的冗余關鍵幀進行預處理,然后通過改進的金字塔相似性得分方法選取閉環候選幀,提升閉環識別率;并利用改進的均值感知哈希算法識別并剔除出現誤判的閉環候選幀,提高閉環準確率。通過TUM標準數據集中的幾組RGB-D數據集進行算法驗證,計算軌跡誤差和算法耗時,并與主流的視覺SLAM算法進行了對比,證明了本文算法對于消除累積誤差,提高機器人定位精度的有效性和優越性。