融合顯著特征和互信息熵的SLAM閉環檢測算法

石祥濱，耿 凱，劉翠微

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

1 引 言

同時定位與地圖構建(simultaneous localization and mapping，SLAM)指機器人處于未知環境下，在運動過程中建立環境的地圖，同時估計自身的位置，是實現機器人路徑規劃和導航等自主任務執行的關鍵技術，閉環檢測是SLAM中關鍵環節和基礎問題，對于消除機器人位姿累積誤差，構建全局一致地圖和提高整體系統的魯棒性至關重要[1,2].

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

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

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

2 主要思想

本文通過控制閉環幀數量以及提取顯著特征來提高閉環檢測的實時性與準確性，結構如圖1所示，主要工作如下：

圖1 結構流程圖Fig.1 Structural flow chart

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

2)閉環候選幀篩選.提取關鍵幀的單詞特征，根據詞袋模型逆序索引將與當前幀相似的關鍵幀作為閉環候選幀.

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

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

3 閉環檢測

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

3.1 關鍵幀選取

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

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

(1)

其中R為3×3的旋轉矩陣，t為1×3的平移向量.

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

(2)

然后通過式(3)-式(4)計算當前幀相對位移矩陣Tdel的旋轉余弦距離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)值判斷當前圖像是否滿足閾值條件，對于滿足條件的圖像根據式(5)計算信息熵E，設當前幀信息熵為Ei，通過式(6)剔除冗余的關鍵幀.

(5)

(6)

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

3.2 閉環候選幀選擇

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

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

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

3.3 顯著特征提取

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

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

(7)

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

(8)

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

(9)

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

1)距離近的物體比遠的物體更能引起人的注意，認為顯著性強, 即當前像素的深度值dcur.

2)當前深度與鄰域對比度差異大，認為顯著性強，如式(11)所示，其中R為以當前像素位置為中心，一定距離為半徑的圓形區域，di表示當前像素深度值，dj表示圓形區域內其他像素深度值.

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

(10)

(11)

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

(12)

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

(13)

3.4 互信息熵計算

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

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

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

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

(14)

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

然后根據式(15)計算兩幅圖像對應的互信息熵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是當前關鍵幀與詞袋模型得到的部分閉環候選幀，其中(a)是當前關鍵幀，(b)、(c)、(d)是獲得的閉環候選幀，表1中是根據閉環候選幀顯著圖計算得到的對應熵值數據.

圖3 詞袋模型產生的部分閉環候選幀Fig.3 Partial closed-loop candidate frames generated by word bag model

表1 閉環候選幀與當前關鍵幀相關熵值Table 1 Closed-loop correlation entropy between candidate frames and current key frames

4 算法描述

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

算法 1.Mutual information entropy extraction of significant features

輸入：n個閉環候選幀圖像KF={f1,f2,…,fn}及其對應深度圖像KF_D={fd1,fd2,…,fdn}當前幀F_cur

輸出：s個閉環驗證幀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 實驗結果與分析

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

5.1 RGB-D SLAM數據集實驗

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

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

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

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

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

5.2 真實場景實驗

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

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

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

6 結 語

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