融合顯著特征和互信息熵的SLAM閉環(huán)檢測算法

石祥濱，耿 凱，劉翠微

1(遼寧大學(xué) 信息學(xué)院,沈陽 110036)2(沈陽航空航天大學(xué) 計算機學(xué)院,沈陽 110136)

1 引 言

同時定位與地圖構(gòu)建(simultaneous localization and mapping，SLAM)指機器人處于未知環(huán)境下，在運動過程中建立環(huán)境的地圖，同時估計自身的位置，是實現(xiàn)機器人路徑規(guī)劃和導(dǎo)航等自主任務(wù)執(zhí)行的關(guān)鍵技術(shù)，閉環(huán)檢測是SLAM中關(guān)鍵環(huán)節(jié)和基礎(chǔ)問題，對于消除機器人位姿累積誤差，構(gòu)建全局一致地圖和提高整體系統(tǒng)的魯棒性至關(guān)重要[1,2].

最簡單實現(xiàn)閉環(huán)檢測的方法是對所有關(guān)鍵幀做遍歷匹配，根據(jù)正確的特征匹配點數(shù)目決定哪幅圖像存在關(guān)聯(lián)，但隨著關(guān)鍵幀的增加，計算復(fù)雜度過高，對于大規(guī)模SLAM系統(tǒng)不實用，另一種方法是隨機抽取關(guān)鍵幀進行檢測，雖然能在一些系統(tǒng)中使用且保持常數(shù)時間的復(fù)雜度，但其選擇過于盲目，抽取到正確的閉環(huán)幀數(shù)量較低，總的檢測效率并不高.

直接根據(jù)特征匹配比較兩幅圖像的相似性，對光照變化不穩(wěn)定且特征提取比較費時，主流方法采用視覺詞袋模型(bag of visual word，BoVW)解決這一問題，其是在文本檢索領(lǐng)域的詞袋(bag of word，BoW)[3]模型上發(fā)展而來，通過特征聚類將一幅圖像表示為差異性的單詞，利用單詞差別判定不同圖像相似性，能有效節(jié)省時間.基于此，Cummins 等[4]通過Chow-Liu 樹探討了詞袋中不同單詞之間的關(guān)聯(lián)性，提出了基于Chow-Liu 樹的閉環(huán)觀測概率模型，Angeli 等[5]通過建立增量式的詞典結(jié)構(gòu)進行閉環(huán)檢測，Labbe′ 等[6，7]采用離散貝葉斯估計不同場景可能出現(xiàn)閉環(huán)的概率，不對所有場景進行閉環(huán)檢測，只對可能發(fā)生閉環(huán)概率較大的場景進行檢測，提高了閉環(huán)檢測的實時性.文獻[4]和文獻[5]的研究能在一定程度上解決閉環(huán)檢測問題，但其將當(dāng)前圖像與所有歷史場景進行對比，耗費時間與計算資源過大，效率較低.文獻[6，7]使用部分概率大的歷史場景判斷閉環(huán)，提高了實時性，但對于過長距離相似場景會出現(xiàn)誤匹配，影響全局地圖的構(gòu)建.

詞袋模型廣泛應(yīng)用于各種SLAM系統(tǒng)中，但詞袋單詞特征的提取不關(guān)注單詞的空間位置與排列順序，只在乎單詞有無，僅使用詞袋進行閉環(huán)檢測，容易產(chǎn)生感知偏差，造成閉環(huán)錯誤判定.針對以上問題，提出了一種基于顯著特征互信息熵的閉環(huán)檢測算法.

2 主要思想

本文通過控制閉環(huán)幀數(shù)量以及提取顯著特征來提高閉環(huán)檢測的實時性與準(zhǔn)確性，結(jié)構(gòu)如圖1所示，主要工作如下：

圖1 結(jié)構(gòu)流程圖Fig.1 Structural flow chart

1)關(guān)鍵幀提取.根據(jù)改進的ORB[8]特征采用PNP算法計算出前后幀相對位姿變化，然后計算不同幀的信息熵差異，將滿足兩者條件的作為關(guān)鍵幀，能夠減少數(shù)據(jù)量，提高實時性.

2)閉環(huán)候選幀篩選.提取關(guān)鍵幀的單詞特征，根據(jù)詞袋模型逆序索引將與當(dāng)前幀相似的關(guān)鍵幀作為閉環(huán)候選幀.

3)顯著圖生成.利用圖像幀顏色與深度信息提取顯著特征生成顯著圖.

4)互信息熵計算.將顯著圖分塊，分別計算不同顯著圖不同塊區(qū)域的互信息熵，判斷圖像的相似度，用于幾何判定是否產(chǎn)生閉環(huán).當(dāng)產(chǎn)生閉環(huán)時，利用圖優(yōu)化求解器g2o[9]迭代優(yōu)化全局位姿，建立一致的3D點云地圖.

3 閉環(huán)檢測

閉環(huán)檢測用于檢測機器人是否經(jīng)過之前相同的位置，通過查找歷史相同場景圖像建立整體地圖的閉環(huán)關(guān)系，優(yōu)化地圖與位姿.閉環(huán)檢測可以顯著消除累計誤差，但檢測錯誤同樣會破壞地圖結(jié)構(gòu)，且損害程度遠(yuǎn)大于未檢測出閉環(huán)時的影響.閉環(huán)需要謹(jǐn)慎選擇，以下為本文的閉環(huán)選取策略.

3.1 關(guān)鍵幀選取

目前大多SLAM系統(tǒng)根據(jù)位姿的相對變化選取關(guān)鍵幀，選擇簡單有效，但魯棒性不強，一些系統(tǒng)在此基礎(chǔ)上采用時空一致性進行了改進，以一定時間與一定位移選擇關(guān)鍵幀，仍然存在冗余，本文考慮相對位姿變化和幀間信息熵差異，將具有一定位移與幀間信息差別的圖像作為關(guān)鍵幀.

運動中的相機位姿可以用一個4×4的矩陣T來表示如式(1)：

(1)

其中R為3×3的旋轉(zhuǎn)矩陣，t為1×3的平移向量.

在SLAM前端的局部地圖中會產(chǎn)生一個參考關(guān)鍵幀Tref，根據(jù)式(2)得到當(dāng)前幀Tcur與參考關(guān)鍵幀Tref的相對位姿變化Tdel.

(2)

然后通過式(3)-式(4)計算當(dāng)前幀相對位移矩陣Tdel的旋轉(zhuǎn)余弦距離Norm(R)和平移歐式距離Norm(t)：

Norm(R)=cos-1(0.5×(R(0,0)+R(1,1)+R(2,2)-1))

(3)

(4)

利用Norm(R) ,Norm(t)值判斷當(dāng)前圖像是否滿足閾值條件，對于滿足條件的圖像根據(jù)式(5)計算信息熵E，設(shè)當(dāng)前幀信息熵為Ei，通過式(6)剔除冗余的關(guān)鍵幀.

(5)

(6)

式(5)中pi為某個灰度在該圖像中出現(xiàn)的概率，式(6)表示當(dāng)前幀與前一幀的信息熵差異是否大于當(dāng)前幀與前m幀差異的均值，ε為均值系數(shù).

3.2 閉環(huán)候選幀選擇

文獻[10]中通過直接遍歷所有幀檢測閉環(huán)，文獻[11]隨機抽取一些幀判斷是否出現(xiàn)閉環(huán)，閉環(huán)幀的選擇盲目，檢測實時性較差，效率較低，相比這兩種方法，先在關(guān)鍵幀中選擇具有可能出現(xiàn)閉環(huán)的圖像作為閉環(huán)候選幀，然后對閉環(huán)候選幀進行檢測，可以避免遍歷所有歷史數(shù)據(jù)，減少比較次數(shù)，節(jié)省時間.

本文采用詞袋模型中逆序索引獲取閉環(huán)候選幀.詞袋模型將所有特征通過聚類形成樹形結(jié)構(gòu)，使類內(nèi)具有較高的相似度，類間相似度較低.樹形結(jié)構(gòu)中根節(jié)點為所有特征，第一層為所有特征根據(jù)k-means算法聚為K類的結(jié)果，然后對于第一層每個節(jié)點繼續(xù)聚為K類，以此類推，直至最后一層的葉子節(jié)點，每個葉子節(jié)點中包含一個逆序索引結(jié)構(gòu)，存儲了當(dāng)前節(jié)點的圖像索引號，以及該單詞對應(yīng)的權(quán)值.

查找相似圖像時，首先提取當(dāng)前圖像特征M，通過查找樹形結(jié)構(gòu)將每個特征m∈M與根節(jié)點逐層比較，然后將其表示為葉子單詞，利用單詞的逆序索引查找出包含當(dāng)前單詞節(jié)點的圖像，將得到的所有圖像作為閉環(huán)候選幀.

3.3 顯著特征提取

由3.2節(jié)得到閉環(huán)候選幀后，對產(chǎn)生的候選幀提取顯著特征，顯著特征是人類視覺機制對圖像感興趣的特征，通過顯著特征可以將有限的計算資源用于處理圖像中更重要的信息.文獻[12-14]中直接利用底層特征根據(jù)全局或區(qū)域?qū)Ρ榷扔嬎銏D像顯著性.文獻[15-17]將圖像分割為超像素，然后在其基礎(chǔ)上，根據(jù)顏色、紋理、多尺度特征提取顯著區(qū)域.文獻[18]結(jié)合了深度信息，經(jīng)過超像素分割后利用神經(jīng)網(wǎng)絡(luò)訓(xùn)練提取顯著特征.文獻[12-17]均利用圖像的顏色、紋理、多尺度等傳統(tǒng)特征，在紋理少或者顏色沒有明顯對比度時會經(jīng)常失效，文獻[18]引入深度信息以彌補不足，但采用神經(jīng)網(wǎng)絡(luò)訓(xùn)練比較耗時，需要GPU加速.本文在加入深度信息后同時考慮實時性，利用局部-全局深度距離對比度計算深度顯著性，然后融合顏色顯著性得到最終顯著圖.

首先，將彩色信息轉(zhuǎn)換到Lab顏色空間如式(7)所示，Lab空間包括L、a、b三個通道，L表示亮度空間，a和b表示顏色空間，相比于RGB顏色空間，Lab空間具有更廣的色域，能更精確的處理顏色信息.

(7)

在得到Lab空間圖像后計算三通道均值Iμ={Lμ,aμ,bμ}如式(8)所示：

(8)

利用當(dāng)前像素與均值像素歐式距離根據(jù)式(9)獲取顏色顯著值：

(9)

然后，利用式(10)計算深度顯著值，式中α和β為權(quán)重系數(shù)，深度顯著特征提取包括三種機制:

1)距離近的物體比遠(yuǎn)的物體更能引起人的注意，認(rèn)為顯著性強, 即當(dāng)前像素的深度值dcur.

2)當(dāng)前深度與鄰域?qū)Ρ榷炔町惔?，認(rèn)為顯著性強，如式(11)所示，其中R為以當(dāng)前像素位置為中心，一定距離為半徑的圓形區(qū)域，di表示當(dāng)前像素深度值，dj表示圓形區(qū)域內(nèi)其他像素深度值.

3)當(dāng)前深度與全局深度對比度差異大，認(rèn)為顯著性強，如式(12)所示，考慮實時性，不對圖像所有像素進行對比，只利用最大、最小以及平均深度作為全局深度進行對比.

(10)

(11)

dall=|di-dmax|+|di-dmin|+|di-dmean|

(12)

最后，結(jié)合顏色與深度顯著值并賦予權(quán)重，由式(13)得最終顯著圖，圖2為根據(jù)彩色圖像與深度圖像生成的顯著圖.

(13)

3.4 互信息熵計算

閉環(huán)候選幀采用聚類方法得到的單詞對空間位置不敏感，當(dāng)一幅圖像聚類為人、車和樹三個單詞時，詞袋模型只知道圖像中存在這些單詞，不知道它們確切的空間分布，此時兩幅相似圖像會產(chǎn)生誤匹配，影響后續(xù)閉環(huán)檢測效果.

針對這一問題，本文在顯著圖基礎(chǔ)上進行分塊處理，將圖像均勻分為四部分I={I1,I2,I3,I4}，對于每部分分別求互信息熵，得到表示兩幅圖像相似度的四維向量，根據(jù)向量距離判斷相似性，進而檢測閉環(huán).

圖2 處理后生成的顯著圖Fig.2 Saliency map generated after processing

設(shè)閉環(huán)候選幀為Fcan，當(dāng)前幀為Fcur，首先計算兩幀中對應(yīng)塊的信息熵E1，E2，E3，E4，由式(14)計算兩幅圖像每塊對應(yīng)的聯(lián)合信息熵UnE1(Img1,Img2)，UnE2(Img1,Img2)，UnE3(Img1,Img2)，UnE4(Img1,Img2)：

(14)

其中X,Y是隨機變量的取值集合.

然后根據(jù)式(15)計算兩幅圖像對應(yīng)的互信息熵Mut1(Img1,Img2),Mut2(Img1,Img2),Mut3(Img1,Img2),Mut4(Img1,Img2)，得到表示兩張圖像相似度的四維向量Mut(Img1,Img2)={Mut1,Mut2,Mut3,Mut4}.

Mut(Img1,Img2)=E(Img1)+E(Img2)-UnE(Img1,Img2)

(15)

最后通過式(16)判斷兩張圖像相似度，將符合要求的M值進行驗證.

(16)

圖3是當(dāng)前關(guān)鍵幀與詞袋模型得到的部分閉環(huán)候選幀，其中(a)是當(dāng)前關(guān)鍵幀，(b)、(c)、(d)是獲得的閉環(huán)候選幀，表1中是根據(jù)閉環(huán)候選幀顯著圖計算得到的對應(yīng)熵值數(shù)據(jù).

圖3 詞袋模型產(chǎn)生的部分閉環(huán)候選幀F(xiàn)ig.3 Partial closed-loop candidate frames generated by word bag model

表1 閉環(huán)候選幀與當(dāng)前關(guān)鍵幀相關(guān)熵值Table 1 Closed-loop correlation entropy between candidate frames and current key frames

4 算法描述

本文利用顏色與深度信息提取顯著特征，并通過互信息熵對詞袋模型產(chǎn)生的閉環(huán)候選幀二次篩選，簡化參與閉環(huán)優(yōu)化數(shù)據(jù)，過程如算法1所示，首先將閉環(huán)候選幀KF={f1,f2,…,fn}轉(zhuǎn)換為Lab顏色空間，計算三通道均值，以此提取顏色顯著性，然后對于深度圖像，分別獲取當(dāng)前深度dcur、當(dāng)前深度與周圍深度對比度darea以及當(dāng)前深度的全局對比度dall，通過三者提取深度顯著性，最后融合彩色顯著性與深度顯著性得到最終的顯著圖，通過計算顯著圖的互信息熵進行閉環(huán)驗證.

算法 1.Mutual information entropy extraction of significant features

輸入：n個閉環(huán)候選幀圖像KF={f1,f2,…,fn}及其對應(yīng)深度圖像KF_D={fd1,fd2,…,fdn}當(dāng)前幀F(xiàn)_cur

輸出：s個閉環(huán)驗證幀Ver_F={v1,v2,…,vs}

1.KF_Lab←F_cur;

2.for i=1 to n in KF_Lab do

6.end for

7.Iμ←KF_Lab

11.for i=1 to n in Salient do

12. Divide_four_part(Salienti,F_cur);

13. for j=1 to 4 inSalientido

14. Cal_Entropy(Salienti);

15. Cal_Joint_Entropy(Salienti,F_cur);

16. Cal_Mutual_Entropy(Salienti,F_cur);

17. end for

18. Mutual_Entropy←{part1,part2,part3,part4}

19. if(L2_Norm(Mutual_Entropy)>T)

20. Ver_F.push(Salienti);

21. end if

22.end for

5 實驗結(jié)果與分析

本文采用的視覺傳感器為微軟的深度相機Kinect，實驗計算機配置：CPU為i5處理器，主頻2.6GHz，內(nèi)存4G，無GPU加速，系統(tǒng)為Ubuntu14.04.分別使用標(biāo)準(zhǔn)RGB-D SLAM數(shù)據(jù)集[19]和實際場景進行實驗.

5.1 RGB-D SLAM數(shù)據(jù)集實驗

為了檢驗本文算法在SLAM系統(tǒng)上位姿估計的精確度，從慕尼黑大學(xué)公共數(shù)據(jù)集中選取了五組數(shù)據(jù)集進行實驗，其中fre1_360是RGBD相機旋轉(zhuǎn)360度得到的室內(nèi)環(huán)境數(shù)據(jù)集，fre1_floor是追蹤平面地板得到的室內(nèi)數(shù)據(jù)集，兩者均不存在閉環(huán)，用于檢驗無閉環(huán)條件下SLAM系統(tǒng)的精確度，fre1_desk與fre2_desk是經(jīng)過多次掃描室內(nèi)四張桌子所得數(shù)據(jù)集，其中包含局部閉環(huán)，fre1_room軌跡包含了整個室內(nèi)環(huán)境，存在全局閉環(huán).fre1_desk、fre2_desk和fre1_room用于檢驗具有閉環(huán)檢測算法的SLAM系統(tǒng)精確度.以上五組數(shù)據(jù)集慕尼黑大學(xué)給出了RGBD相機的真實軌跡，是由高精度設(shè)備采集得到，可以通過均方根誤差(Root Mean Squared Error，RMSE)表示真實軌跡與估計位姿之間的差別，評判閉環(huán)檢測的效果.

表2 均方根誤差與時間對比Table 2 Comparison of root mean square error and time

表2是本文算法與文獻[20](RGBD-SLAM-V1)和文獻[11](RGBD-SLAM-V2)算法在五組數(shù)據(jù)集下的均方根誤差(RMSE)和每幀處理時間(TIME)對比.文獻[20]是慕尼黑大學(xué)完成的RGBD-SLAM系統(tǒng)，該系統(tǒng)不包括閉環(huán)檢測模塊，文獻[11]是同一作者在文獻[20]基礎(chǔ)上進行的改進工作，包括利用GPU加速技術(shù)進行加速，加入了基于詞袋模型閉環(huán)檢測模塊.由表2知，本文算法在精度和時間上都明顯優(yōu)于文獻[20]算法，精度上，文獻[20]采用了匹配性能優(yōu)于本文ORB的SIFT[21]特征描述子，但由于本文具有閉環(huán)檢測模塊，能消除累積誤差，所以最終精確度優(yōu)于文獻[20]，驗證了閉環(huán)檢測在SLAM系統(tǒng)中的重要性.時間上，文獻[20]采用效果更好的SIFT描述子，特征提取非常耗時，實時性較差.相比于文獻[11]，本文算法整體上精度和時間略優(yōu)，精度上，文獻[11]采用了詞袋模型進行閉環(huán)檢測，本文先利用詞袋模型提取閉環(huán)候選幀，然后計算閉環(huán)候選幀的顯著特征互信息熵進行閉環(huán)檢測，過濾掉差異性較大的候選幀，所以精度上優(yōu)于文獻[11]算法.時間上，文獻[11]利用GPU對SIFT特征進行并行加速，相比于文獻[20]速度有明顯的提升，但仍略差于本文算法，原因是本文采用的ORB特征比SIFT特征提取速度快兩個數(shù)量級，顯著提高了系統(tǒng)實時性.

圖4是以上四組數(shù)據(jù)集在本文算法下的的絕對軌跡誤差圖，能直觀的感受到真實軌跡與估計位姿的誤差值.其中，實線表示真實軌跡，重虛線表示估計軌跡，輕虛線表示兩者差別.

圖4 真實軌跡與估計位姿對比Fig.4 Comparison of real trajectory and estimated posture

5.2 真實場景實驗

為體現(xiàn)本文算法的實用性，在真實場景下進行了建圖，如圖5所示，實驗場景為室內(nèi)辦公室，包括書柜、桌子、沙發(fā)等實物.圖6為重建后的三維點云圖，其中，圖6(a)和圖6(b)是采用傳統(tǒng)閉環(huán)檢測生成的點云地圖，兩幅圖像均能重建出室內(nèi)環(huán)境的整體結(jié)構(gòu)與內(nèi)容，但圖6(a)上方的沙發(fā)與圖6(b)上方的書柜出現(xiàn)了重疊模糊現(xiàn)象，而且地圖的整體結(jié)構(gòu)不夠規(guī)整，主要原因是多次檢測出不準(zhǔn)確的閉環(huán)，導(dǎo)致優(yōu)化后的位姿不夠精準(zhǔn)，地圖產(chǎn)生偏差.圖6(c)，圖6(d)是采用本文算法構(gòu)建的地圖，圖中的沙發(fā)、書柜無重影與模糊現(xiàn)象，原因是本文算法通過提高閉環(huán)檢測的準(zhǔn)確性，減少了錯誤閉環(huán)約束信息，提高了位姿精度，產(chǎn)生較好的地圖構(gòu)建效果.

圖5 辦公室真實場景Fig.5 Real office scene

圖6 辦公室重建點云圖Fig.6 Office reconstruction point cloud

6 結(jié) 語

本文提出了一種基于顯著性特征互信息熵的閉環(huán)檢測算法，旨在增強同一場景圖像的正確匹配率，提高閉環(huán)檢測的正確率.首先，設(shè)計了融合位姿與信息熵變化的關(guān)鍵幀選擇模型.然后，根據(jù)圖像顏色與深度信息給出了新的顯著特征提取方法.最后，基于顯著圖提出了顯著特征互信息熵的閉環(huán)判定架構(gòu).實驗數(shù)據(jù)表明，在無GPU加速情況下，本文算法能保證SLAM系統(tǒng)的準(zhǔn)確性與實時性，下一步考慮將生成的三維地圖應(yīng)用到室內(nèi)機器人的導(dǎo)航避障需求中，將研究如何在三維地圖下進行路徑規(guī)劃，實現(xiàn)機器人的自主導(dǎo)航.