基于雙目相機(jī)的SLAM回環(huán)檢測(cè)改進(jìn)算法研究

朱 晨，王宇杰，郭 杭

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

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

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

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

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

1 回環(huán)檢測(cè)算法

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

圖1 本文回環(huán)檢測(cè)算法流程圖

2 基于詞袋模型的回環(huán)檢測(cè)方法

基于詞袋模型的方法忽略單詞出現(xiàn)的順序和語(yǔ)法規(guī)則，只需要統(tǒng)計(jì)圖像中的文本單詞出現(xiàn)的次數(shù)，能夠極大地提高單詞的檢索速度并減少內(nèi)存開銷。實(shí)現(xiàn)過程是將每張圖片關(guān)鍵幀轉(zhuǎn)為單詞向量，即為詞袋，然后計(jì)算當(dāng)前關(guān)鍵幀與歷史關(guān)鍵幀的詞袋向量相似度，確定是否經(jīng)過同一地點(diǎn)[12]。

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

(1)

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

(2)

式中：η為歸一化的相似度分?jǐn)?shù)；vj-1為和該地點(diǎn)最接近的另一個(gè)關(guān)鍵幀詞袋向量。若η高于相似度閾值t，則關(guān)鍵幀j是當(dāng)前關(guān)鍵幀i的閉環(huán)候選幀，在幾何檢驗(yàn)步驟中通過檢驗(yàn)，則確認(rèn)為正確回環(huán)。

3 利用透視變換對(duì)圖片進(jìn)行視角變換

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

在視覺SLAM過程中，地面機(jī)器人重新回到閉環(huán)地點(diǎn)處，相較于初始姿態(tài)，其當(dāng)前姿態(tài)難免會(huì)發(fā)生變化。根據(jù)參考文獻(xiàn)[13]，當(dāng)相機(jī)的視角旋轉(zhuǎn)30°時(shí)，其相似度將衰減至原來的20%，當(dāng)相似度為20%時(shí)則不可避免會(huì)產(chǎn)生錯(cuò)誤的回環(huán)檢測(cè)。所以本文提出采用透視變換的方式對(duì)當(dāng)前關(guān)鍵幀的視角進(jìn)行變換，從而減少因視角變化而產(chǎn)生的影響，提高算法召回率。

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

圖2 透視變換原理示意圖

(3)

透視變換后相機(jī)視角示意圖如圖3所示。Z軸方向的視角為當(dāng)前相機(jī)視角，通過透視變換矩陣對(duì)相機(jī)的當(dāng)前視角下獲取的關(guān)鍵幀圖片進(jìn)行左右各30°的視角變換，獲得相機(jī)在發(fā)生向左(正方向)或向右(負(fù)方向)旋轉(zhuǎn)30°的視角下拍攝到的當(dāng)前場(chǎng)景的圖片關(guān)鍵幀。在室內(nèi)環(huán)境下實(shí)際相機(jī)視角拍攝的圖片如圖4所示。經(jīng)過向左和向右透視變換30°后視角的圖片如圖5和圖6所示。

圖3 透視變換后相機(jī)視角示意圖

圖4 實(shí)際相機(jī)視角拍攝圖片

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

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

4 基于ICP算法空間信息的幾何檢驗(yàn)

通過相似度閾值篩選出的閉環(huán)候選關(guān)鍵幀仍然存在誤匹配、當(dāng)前關(guān)鍵幀對(duì)應(yīng)到多個(gè)閉環(huán)候選關(guān)鍵幀的情況，所以需要幾何驗(yàn)證進(jìn)一步去篩選。

由于本文回環(huán)檢測(cè)改進(jìn)算法是基于雙目相機(jī)的，在相機(jī)坐標(biāo)系中，通過雙目相機(jī)模型可以計(jì)算出當(dāng)前關(guān)鍵幀和閉環(huán)候選關(guān)鍵幀中單詞相對(duì)應(yīng)的特征點(diǎn)和描述子的三維點(diǎn)云信息，基于ICP算法就可以求出關(guān)鍵幀與閉環(huán)候選關(guān)鍵幀的相對(duì)位姿轉(zhuǎn)換關(guān)系，即為空間信息，用于幾何檢驗(yàn)判斷是否構(gòu)成回環(huán)，然后再糾正位姿，消除累積誤差。

ICP算法是3D空間點(diǎn)配準(zhǔn)算法,該算法思想簡(jiǎn)單、需要的信息少、容易實(shí)現(xiàn)并能夠獲得高精度的配準(zhǔn)結(jié)果。ICP算法的實(shí)質(zhì)是基于最小二乘最優(yōu)匹配思想的匹配算法。ICP算法的核心思想是：首先，基于點(diǎn)到點(diǎn)的歐式距離最近原則，在當(dāng)前點(diǎn)云和目標(biāo)點(diǎn)云中尋找多組對(duì)應(yīng)的點(diǎn)對(duì)，組成對(duì)應(yīng)點(diǎn)集；然后，在這個(gè)對(duì)應(yīng)點(diǎn)集的基礎(chǔ)上，計(jì)算出這2個(gè)點(diǎn)云間的變換矩陣，更新當(dāng)前點(diǎn)云；最后，通過迭代的方式，直至誤差小于某一閾值或者達(dá)到迭代次數(shù)，確定出最終的變換關(guān)系[18-19]。

假設(shè)當(dāng)前點(diǎn)云為P={p1,p2,…，pm}；目標(biāo)點(diǎn)云為Q={q1,q2,…，qn}；m和n為當(dāng)前點(diǎn)云和目標(biāo)點(diǎn)云的點(diǎn)數(shù)；設(shè)最大迭代次數(shù)為kmax；單次迭代的均方差閾值為ξ；兩次迭代的均方誤差差值的閾值為ε。ICP算法的實(shí)現(xiàn)過程如下。

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

(4)

③ 更新源點(diǎn)云。使用上一步計(jì)算出的變換參數(shù)Rk、Tk對(duì)當(dāng)前點(diǎn)云Pk進(jìn)行變換得到新的當(dāng)前點(diǎn)云Pk+1。

Pk+1=RkPk+Tk

(5)

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

(6)

(7)

⑤ 配準(zhǔn)點(diǎn)云。以最后一次迭代計(jì)算的變換參數(shù)為最終的變換參數(shù)，通過此變換參數(shù)將當(dāng)前點(diǎn)云配準(zhǔn)到目標(biāo)點(diǎn)云，從而完成點(diǎn)云匹配。

根據(jù)已知的雙目相機(jī)的參數(shù)和數(shù)學(xué)模型，由當(dāng)前關(guān)鍵幀和閉環(huán)候選關(guān)鍵幀中的單詞對(duì)應(yīng)的特征點(diǎn)，可以求出這兩個(gè)關(guān)鍵幀圖片特征點(diǎn)對(duì)應(yīng)的點(diǎn)云。根據(jù)ICP算法可以求出當(dāng)前關(guān)鍵幀和候選關(guān)鍵幀的變換參數(shù)。

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

圖7 空間信息的幾何驗(yàn)證

5 回環(huán)檢測(cè)評(píng)價(jià)指標(biāo)與實(shí)驗(yàn)分析

5.1 回環(huán)檢測(cè)評(píng)價(jià)指標(biāo)

對(duì)于基于外觀的回環(huán)檢測(cè)算法，存在可能在同一地點(diǎn)因?yàn)楣饩€、天氣或動(dòng)態(tài)物體的影響導(dǎo)致兩張圖片在外觀上存在較大的差異，算法可能會(huì)無法檢測(cè)出其中存在的閉環(huán)匹配關(guān)系。如果拍攝的場(chǎng)景過于單一，導(dǎo)致整個(gè)場(chǎng)景中的景物非常相像，極有可能將不是回環(huán)地點(diǎn)的場(chǎng)景認(rèn)作同一場(chǎng)景。

對(duì)于前一種實(shí)際上構(gòu)成閉環(huán)但是算法認(rèn)為不是閉環(huán)的情況，稱之為感知變異(Perceptual Variability)或者假陰性(False Negative,FN)。而實(shí)際上不構(gòu)成閉環(huán)但是算法認(rèn)為是閉環(huán)的情況，稱之為感知偏差(Perceptual Aliasing)，或者假陽(yáng)性(False Positive)。回環(huán)檢測(cè)結(jié)果分類如表1所示。

表1 回環(huán)檢測(cè)結(jié)果分類

在回環(huán)檢測(cè)算法中，分別使用回環(huán)檢測(cè)的準(zhǔn)確率(Precision)和召回率(Recall)這兩個(gè)性能指標(biāo)[12-20],如式(8)所示。

(8)

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

5.2 實(shí)驗(yàn)分析

本文實(shí)驗(yàn)中的詞匯樹，采用室內(nèi)環(huán)境下采集的Bicocca_2009-02-26a數(shù)據(jù)集訓(xùn)練生成。本文使用雙目相機(jī)在室內(nèi)環(huán)境下采集3個(gè)數(shù)據(jù)集，圖8～圖10分別為3個(gè)數(shù)據(jù)集的真實(shí)軌跡，其中數(shù)據(jù)集traj_1、traj_2、traj_3的軌跡長(zhǎng)度分別為338.249 m、175.861 m、56.178 m，然后通過高精度的光學(xué)捕捉系統(tǒng)獲取到當(dāng)前實(shí)際位姿作為參考值。

圖8 traj_1數(shù)據(jù)集的真實(shí)軌跡

圖9 traj_2數(shù)據(jù)集的真實(shí)軌跡

圖10 traj_3數(shù)據(jù)集的真實(shí)軌跡

利用該數(shù)據(jù)集進(jìn)行實(shí)驗(yàn)，通過調(diào)節(jié)關(guān)鍵幀相似度閾值t=[0,1]，得到準(zhǔn)確率和召回率的變化結(jié)果如圖11～圖13所示。

圖11 traj_1的召回率和準(zhǔn)確率實(shí)驗(yàn)結(jié)果

圖12 traj_2的召回率和準(zhǔn)確率實(shí)驗(yàn)結(jié)果

圖13 traj_3的召回率和準(zhǔn)確率實(shí)驗(yàn)結(jié)果

由圖11～圖13可以看出，3個(gè)數(shù)據(jù)集在準(zhǔn)確率都為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%。本文的算法在準(zhǔn)確率相同的基礎(chǔ)上，獲得比DBoW算法更好的召回率，效果比傳統(tǒng)基于DBoW算法更好。

6 結(jié)束語(yǔ)

針對(duì)相機(jī)視角變化使相似度評(píng)分變小，導(dǎo)致回環(huán)檢測(cè)誤匹配和漏匹配的問題，基于雙目相機(jī)對(duì)傳統(tǒng)的基于詞袋模型閉環(huán)檢測(cè)算法提出改進(jìn)。經(jīng)過實(shí)驗(yàn)證明，與傳統(tǒng)的閉環(huán)檢測(cè)算法相比，所提出的算法效果更好。但所提出的算法適用于對(duì)實(shí)時(shí)要求不甚嚴(yán)格的場(chǎng)景，后續(xù)可以在此基礎(chǔ)上進(jìn)行進(jìn)一步優(yōu)化，提高實(shí)時(shí)性能。