基于雙目相機的SLAM回環檢測改進算法研究

朱 晨，王宇杰，郭 杭

(南昌大學 信息工程學院，江西 南昌 330031)

隨著社會與經濟快速發展，定位和導航在大眾生活和國家重要戰略中發揮著越來越重要的作用。例如災難救援[1-3]、工業生產[4-5]、家庭服務[6-8]等。但是由于衛星導航系統在室內等場景下，無法進行高精度定位，對于室內和室外衛星信號較弱的場景，同步定位與建圖(Simultaneous Localization and Mapping,SLAM)技術能夠有效地解決這一問題[9]。隨著近些年計算機視覺技術的不斷發展，以相機為傳感器的視覺SLAM技術受到了越來越多的關注[10]。視覺SLAM具有成本低、精度較高、采集的圖像信息豐富等優點，在室內外復雜和未知的環境下具有很強的適應性[10]，因為視覺定位技術利用豐富的信息在定位的同時還能夠進行環境感知(即為建圖)，可以將未知環境變成已知環境進行進一步處理。視覺定位技術系統具有圖像處理、跟蹤(Tracking)、建圖(Mapping)和回環檢測(Loop Closing)4個模塊[11]。

回環檢測是視覺定位技術中較為關鍵的一個部分[9]。視覺定位技術雖然能夠進行當前載體的實時位姿估計，但是從本質上來說是一種航跡推算算法，存在位姿誤差累積的問題。所以需要回環檢測算法消除與抑制累計誤差[10]。基于詞袋模型(Bag of Words，BoW)的回環檢測技術，在目前各種開源的視覺SLAM算法中被廣泛使用，其本質上是基于外觀的回環檢測算法[12]，通過圖像信息判斷當前場景是否是之前到達過的地點，實現位姿糾正。

隨著視覺SLAM在小型移動載體上應用越來越廣泛，近年來回環檢測出現了一些新的問題。主要問題是小型移動載體對視角變化的要求較高[13]。以地面移動機器人為例，如果機器人在原地旋轉一定的角度，雖然還是在原地，但是卻很難正確匹配。為解決這一問題，部分學者提出使用全景相機來代替普通的相機[14]、利用激光SLAM來輔助視覺SLAM[15]、直接基于RGB-D相機來解決回環檢測的問題[16]。雖然這些方法能夠解決一些視角變換帶來的問題，但是卻增加了額外的成本。

針對傳統視覺SLAM閉環檢測過程中，因視角變化導致難以正確閉環匹配的問題，本文對傳統的基于BoW閉環檢測算法提出改進，采用低成本的雙目相機進行環境感知。具體方法是：①利用透視變換算法對當前圖像關鍵幀進行視角變換；②采用基于迭代最近點(Iterative Closest Point，ICP)算法求出當前關鍵幀與閉環候選關鍵幀的空間信息，進行幾何檢驗。在算法耗時要求不甚嚴格的前提下，采用本文的算法，可以提高回環檢測的準確率和召回率。

1 回環檢測算法

圖1為本文回環檢測算法流程。判斷當前圖像關鍵幀是否滿足回環條件，首先，對該關鍵幀進行左右各30°視角的透視變換，目的是減少因視角變化過大導致誤匹配和漏匹配的影響；然后，提取變換后該關鍵幀中的特征點與描述子，將提取到的每一個特征點和描述子通過多級聚類的方式轉化為詞匯，計算出該關鍵幀的詞袋向量。相似度計算是計算當前關鍵幀與歷史關鍵幀的詞袋向量的相似情況，如果小于設定的相似度閾值，歷史關鍵幀中的關鍵幀即為閉環候選關鍵幀，進行下一步的操作。基于ICP算法與空間信息幾何檢驗可以判斷閉環候選關鍵幀是否真的構成回環，如果滿足距離閾值，則認為是正確回環幀，從而糾正位姿，消除累積誤差。

圖1 本文回環檢測算法流程圖

2 基于詞袋模型的回環檢測方法

基于詞袋模型的方法忽略單詞出現的順序和語法規則，只需要統計圖像中的文本單詞出現的次數，能夠極大地提高單詞的檢索速度并減少內存開銷。實現過程是將每張圖片關鍵幀轉為單詞向量，即為詞袋，然后計算當前關鍵幀與歷史關鍵幀的詞袋向量相似度，確定是否經過同一地點[12]。

根據事先訓練好的詞匯樹，計算出關鍵幀之間的相似度。根據關鍵幀的詞袋，使用 TF-IDF 算法計算當前關鍵幀Ii以及所有歷史關鍵幀Ij中每個單詞的權值，每張關鍵幀中的所有詞匯信息用一個詞袋向量vt表示；最后將當前關鍵幀在之前的所有關鍵幀中檢索，計算當前關鍵幀與候選關鍵幀的相似度分數，并對其進行歸一化處理，如式(1)和式(2)所示。

(1)

式中：s(vi,vj)為關鍵幀i和關鍵幀j的詞袋向量vi和vj的相似度值。

(2)

式中：η為歸一化的相似度分數；vj-1為和該地點最接近的另一個關鍵幀詞袋向量。若η高于相似度閾值t，則關鍵幀j是當前關鍵幀i的閉環候選幀，在幾何檢驗步驟中通過檢驗，則確認為正確回環。

3 利用透視變換對圖片進行視角變換

傳統的視覺SLAM中主要采用的是基于詞袋模型的圖像檢索方式，但是在視角差異較大情況下難以匹配到正確的圖像。

在視覺SLAM過程中，地面機器人重新回到閉環地點處，相較于初始姿態，其當前姿態難免會發生變化。根據參考文獻[13]，當相機的視角旋轉30°時，其相似度將衰減至原來的20%，當相似度為20%時則不可避免會產生錯誤的回環檢測。所以本文提出采用透視變換的方式對當前關鍵幀的視角進行變換，從而減少因視角變化而產生的影響，提高算法召回率。

在實際生活中的景物，由于觀察距離與方位的不同，導致在視覺上引起不同的反映，即為透視現象。透視變換的本質是將在原來平面上的圖像向一個新的平面投影，得到圖像在新的平面上的坐標[17]，如式(3)所示。透視變換原理示意圖如圖2所示。

圖2 透視變換原理示意圖

(3)

透視變換后相機視角示意圖如圖3所示。Z軸方向的視角為當前相機視角，通過透視變換矩陣對相機的當前視角下獲取的關鍵幀圖片進行左右各30°的視角變換，獲得相機在發生向左(正方向)或向右(負方向)旋轉30°的視角下拍攝到的當前場景的圖片關鍵幀。在室內環境下實際相機視角拍攝的圖片如圖4所示。經過向左和向右透視變換30°后視角的圖片如圖5和圖6所示。

圖3 透視變換后相機視角示意圖

圖4 實際相機視角拍攝圖片

圖5 向左透視變換30°視角圖片

圖6 向右透視變換30°視角圖片

4 基于ICP算法空間信息的幾何檢驗

通過相似度閾值篩選出的閉環候選關鍵幀仍然存在誤匹配、當前關鍵幀對應到多個閉環候選關鍵幀的情況，所以需要幾何驗證進一步去篩選。

由于本文回環檢測改進算法是基于雙目相機的，在相機坐標系中，通過雙目相機模型可以計算出當前關鍵幀和閉環候選關鍵幀中單詞相對應的特征點和描述子的三維點云信息，基于ICP算法就可以求出關鍵幀與閉環候選關鍵幀的相對位姿轉換關系，即為空間信息，用于幾何檢驗判斷是否構成回環，然后再糾正位姿，消除累積誤差。

ICP算法是3D空間點配準算法,該算法思想簡單、需要的信息少、容易實現并能夠獲得高精度的配準結果。ICP算法的實質是基于最小二乘最優匹配思想的匹配算法。ICP算法的核心思想是：首先，基于點到點的歐式距離最近原則，在當前點云和目標點云中尋找多組對應的點對，組成對應點集；然后，在這個對應點集的基礎上，計算出這2個點云間的變換矩陣，更新當前點云；最后，通過迭代的方式，直至誤差小于某一閾值或者達到迭代次數，確定出最終的變換關系[18-19]。

假設當前點云為P={p1,p2,…，pm}；目標點云為Q={q1,q2,…，qn}；m和n為當前點云和目標點云的點數；設最大迭代次數為kmax；單次迭代的均方差閾值為ξ；兩次迭代的均方誤差差值的閾值為ε。ICP算法的實現過程如下。

② 求解變換參數。求出如式(4)所示的最小值，然后使用四元數法計算出變換參數Rk、Tk。

(4)

③ 更新源點云。使用上一步計算出的變換參數Rk、Tk對當前點云Pk進行變換得到新的當前點云Pk+1。

Pk+1=RkPk+Tk

(5)

④ 進行迭代計算判定。根據式(6)計算本次迭代變換的均方誤差dk，若迭代達到最大次數kmax、dk+1-dk<ε或dk<ξ，則停止迭代，否則重復上述步驟。

(6)

(7)

⑤ 配準點云。以最后一次迭代計算的變換參數為最終的變換參數，通過此變換參數將當前點云配準到目標點云，從而完成點云匹配。

根據已知的雙目相機的參數和數學模型，由當前關鍵幀和閉環候選關鍵幀中的單詞對應的特征點，可以求出這兩個關鍵幀圖片特征點對應的點云。根據ICP算法可以求出當前關鍵幀和候選關鍵幀的變換參數。

在求出變換參數之后，還需要進一步篩選和確認候選關鍵幀，求出當前關鍵幀的正確閉環候選關鍵幀。本文采取的方法是設置當前關鍵幀的距離半徑閾值，如圖7所示。若當前關鍵幀與當前關鍵幀閉環候選關鍵幀的之間的距離小于設定的閾值，則判定閉環檢測成功，由于基于雙目相機的視覺SLAM位姿漂移不嚴重[11]，可以通過此方法進行驗證。

圖7 空間信息的幾何驗證

5 回環檢測評價指標與實驗分析

5.1 回環檢測評價指標

對于基于外觀的回環檢測算法，存在可能在同一地點因為光線、天氣或動態物體的影響導致兩張圖片在外觀上存在較大的差異，算法可能會無法檢測出其中存在的閉環匹配關系。如果拍攝的場景過于單一，導致整個場景中的景物非常相像，極有可能將不是回環地點的場景認作同一場景。

對于前一種實際上構成閉環但是算法認為不是閉環的情況，稱之為感知變異(Perceptual Variability)或者假陰性(False Negative,FN)。而實際上不構成閉環但是算法認為是閉環的情況，稱之為感知偏差(Perceptual Aliasing)，或者假陽性(False Positive)。回環檢測結果分類如表1所示。

表1 回環檢測結果分類

在回環檢測算法中，分別使用回環檢測的準確率(Precision)和召回率(Recall)這兩個性能指標[12-20],如式(8)所示。

(8)

在實際過程中，應該盡可能地同時提高回環檢測算法的準確率和召回率。但二者存在矛盾性，即回環檢測的召回率和準確率是反比變化的，隨著準確率的降低，召回率變得越來越高，這是不可避免的。當選取的相似度評分閾值較高時，準確率就會更高，同時召回率則會由于通過篩選的圖像數量降低而下降；而當閾值設置得較低時，條件放寬后可以得到更高的召回率，但是準確率則會下降。所以在多數情況下，回環檢測算法在保證準確率的同時要盡可能地提高召回率。

5.2 實驗分析

本文實驗中的詞匯樹，采用室內環境下采集的Bicocca_2009-02-26a數據集訓練生成。本文使用雙目相機在室內環境下采集3個數據集，圖8～圖10分別為3個數據集的真實軌跡，其中數據集traj_1、traj_2、traj_3的軌跡長度分別為338.249 m、175.861 m、56.178 m，然后通過高精度的光學捕捉系統獲取到當前實際位姿作為參考值。

圖8 traj_1數據集的真實軌跡

圖9 traj_2數據集的真實軌跡

圖10 traj_3數據集的真實軌跡

利用該數據集進行實驗，通過調節關鍵幀相似度閾值t=[0,1]，得到準確率和召回率的變化結果如圖11～圖13所示。

圖11 traj_1的召回率和準確率實驗結果

圖12 traj_2的召回率和準確率實驗結果

圖13 traj_3的召回率和準確率實驗結果

由圖11～圖13可以看出，3個數據集在準確率都為100%的情況下，本文算法的召回率比DBoW分別高出(0.247-0.183)/0.183 = 34.9%，(0.098-0.086)/0.086 = 13.9%，(0.351-0.292)/0.292= 20.2%。本文的算法在準確率相同的基礎上，獲得比DBoW算法更好的召回率，效果比傳統基于DBoW算法更好。

6 結束語

針對相機視角變化使相似度評分變小，導致回環檢測誤匹配和漏匹配的問題，基于雙目相機對傳統的基于詞袋模型閉環檢測算法提出改進。經過實驗證明，與傳統的閉環檢測算法相比，所提出的算法效果更好。但所提出的算法適用于對實時要求不甚嚴格的場景，后續可以在此基礎上進行進一步優化，提高實時性能。