一種基于改進(jìn)閉環(huán)檢測(cè)的視覺SLAM算法

趙 磊 李振偉 楊曉利 張 衛(wèi)

(河南科技大學(xué)醫(yī)學(xué)與技術(shù)工程學(xué)院 河南 洛陽 471000)

0 引 言

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

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

近些年來,視覺SLAM發(fā)展很快,在兼顧處理速度的同時(shí)還取得了不錯(cuò)的精度,但通過特征提取進(jìn)行相機(jī)位姿估計(jì)仍占用了整個(gè)系統(tǒng)不少的時(shí)間?；谝陨戏治?本文利用深度相機(jī)進(jìn)行圖像采集,減少前端的計(jì)算量,對(duì)關(guān)鍵幀以及候選閉環(huán)幀進(jìn)行合理刪選,并改進(jìn)詞袋模型的相似性得分方法,減少感知歧義,最后利用ORB-SLAM2系統(tǒng)和標(biāo)準(zhǔn)數(shù)據(jù)集進(jìn)行算法驗(yàn)證。

1 算法框架

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

圖1 SLAM系統(tǒng)框架

2 改進(jìn)閉環(huán)檢測(cè)算法

視覺SLAM前端僅僅通過圖像匹配估計(jì)相機(jī)的大致運(yùn)動(dòng)軌跡,由于傳感器本身會(huì)存在誤差,加之部分場(chǎng)景中圖像特征不明顯,導(dǎo)致特征點(diǎn)對(duì)特征點(diǎn)的約束關(guān)系使得前端估計(jì)產(chǎn)生誤差。該誤差會(huì)隨著時(shí)間的推移不斷累積至下一幀圖像,最后導(dǎo)致系統(tǒng)估計(jì)的地圖與真實(shí)的環(huán)境相差甚遠(yuǎn)。而閉環(huán)檢測(cè)則通過判斷當(dāng)前圖像與歷史圖像的相似關(guān)系,為后端優(yōu)化提供一種更強(qiáng)、時(shí)間更久遠(yuǎn)的約束,通過這種約束關(guān)系,可以有效地減少累積誤差,得到更加準(zhǔn)確的相機(jī)軌跡和地圖。

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

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

2.1 關(guān)鍵幀的篩選

相機(jī)采集圖像的過程是連續(xù)的,而且采集頻率較高,以Kinect為例,可以達(dá)到每秒30幀。如果沒有對(duì)短時(shí)間內(nèi)選取的關(guān)鍵幀進(jìn)行合理篩選,會(huì)導(dǎo)致圖像相似度過高,進(jìn)而影響閉環(huán)檢測(cè)的準(zhǔn)確性。本文在ORB_SLAM2的關(guān)鍵幀選取機(jī)制的基礎(chǔ)上,引入幀間位姿變化,選出滿足條件的圖像作為關(guān)鍵幀。

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

(1)

(2)

2.2 改進(jìn)的相似性得分匹配方法

詞袋模型包括視覺單詞字典的創(chuàng)建和相似度比較。利用Kmeans++算法對(duì)圖像數(shù)據(jù)集進(jìn)行特征分層聚類生成單詞向量,但平面結(jié)構(gòu)的詞袋模型會(huì)導(dǎo)致特征搜索時(shí)間過長(zhǎng),因此本文采用k叉樹結(jié)構(gòu)來描述單詞字典,詞匯樹結(jié)構(gòu)如圖2所示。

圖2 視覺詞匯樹結(jié)構(gòu)示意圖

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

(3)

單詞字典的容納能力會(huì)隨著層數(shù)的增加急劇增強(qiáng),這樣的字典樹足以表征絕大部分圖像。

(4)

(5)

(6)

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

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

(7)

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

(8)

(9)

(10)

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

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

(11)

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

(12)

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

(13)

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

(14)

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

(15)

2.3 閉環(huán)候選幀的剔除

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

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

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

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

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

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

相似度計(jì)算公式為:

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

(16)

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

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

(17)

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

3 實(shí)驗(yàn)結(jié)果與分析

本文實(shí)驗(yàn)使用的計(jì)算機(jī)配置為:CPU為Intel i5-3210M,主頻2.5 GHz,內(nèi)存4 GB,操作系統(tǒng)為Ubuntu 16.04。使用TUM標(biāo)準(zhǔn)數(shù)據(jù)集中的RGBD SLAM數(shù)據(jù)進(jìn)行實(shí)驗(yàn),RGB圖像大小為640×480像素,本文算法中n為100,Sp為64,q為10。

3.1 改進(jìn)的均值哈希算法測(cè)試結(jié)果

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

(a) fre1_desk (b) fre1_floor

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

表1 閉環(huán)檢測(cè)統(tǒng)計(jì)結(jié)果

由表1的統(tǒng)計(jì)數(shù)據(jù)可知,對(duì)于fre2_loop和fre1_loop,由于場(chǎng)景中的低頻信息占據(jù)了圖像的絕大部分,隨著相機(jī)的移動(dòng),圖像低頻信息變化不明顯,導(dǎo)致均值哈希算法出現(xiàn)了較多的誤判。而對(duì)于本文算法,除了fre1_floor中有一幅圖像判別錯(cuò)誤,其他幾個(gè)數(shù)據(jù)集的相似圖像都實(shí)現(xiàn)了準(zhǔn)確的判別,而且算法耗時(shí)也略優(yōu)。

3.2 軌跡誤差實(shí)驗(yàn)

為了評(píng)判本文算法在實(shí)際SLAM系統(tǒng)中的運(yùn)行效果,本文從TUM數(shù)據(jù)集中選取了4組深度相機(jī)數(shù)據(jù)集進(jìn)行實(shí)驗(yàn),每個(gè)數(shù)據(jù)集中都有真實(shí)的軌跡文件groundtruth.txt,便于進(jìn)行算法優(yōu)劣的對(duì)比。均方根誤差(Root Mean Squared Error,RMSE)是評(píng)價(jià)SLAM算法的重要指標(biāo),文獻(xiàn)[21]提供了計(jì)算RMSE的評(píng)估工具。

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

表2 均方根誤差對(duì)比 單位:m

(a) fre1_room

(b) fre2_desk

(c) fre1_floor

(d) fre1_desk2圖4 真實(shí)軌跡與估計(jì)軌跡對(duì)比

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

表3 時(shí)間對(duì)比 單位:s

4 結(jié) 語

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