一種基于動態(tài)時間規(guī)整的閉環(huán)檢測算法

朱 田，彭之川，張智騰，朱澤敏，謝勇波，王文明

（長沙中車智馭新能源科技有限公司， 湖南 長沙 410000）

0 引言

機器人在未知環(huán)境中自主工作的第一步是環(huán)境感知和建立環(huán)境模型。同步定位與建圖(simultaneous localization and mapping, SLAM)是研究此步驟的重要方法，即機器人在傳感器數(shù)據(jù)基礎(chǔ)上，自主移動建立環(huán)境模型并同時完成自身定位[1]。閉環(huán)檢測是SLAM中的一個關(guān)鍵問題[2]，其通過機器人的當(dāng)前位置圖像與先前位置圖像進行配準(zhǔn)來判斷機器人是否曾經(jīng)到達過先前位置，因此閉環(huán)檢測也可以被視為是一類場景識別問題。正確的閉環(huán)檢測可以為位姿圖添加邊約束，減小機器人運動估計的累計誤差，并能構(gòu)建一致性地圖；而錯誤的閉環(huán)檢測會對已有位姿圖帶來干擾，導(dǎo)致地圖創(chuàng)建的失敗。因此，閉環(huán)檢測的研究意義重大，設(shè)計一個好的閉環(huán)檢測算法，對于構(gòu)圖一致性乃至整個SLAM系統(tǒng)都是至關(guān)重要的。

目前主流的方法是基于單張圖像特征進行匹配來檢測閉環(huán)，比較經(jīng)典的方法有基于詞袋模型[3]的視覺閉環(huán)檢測算法，其可以將場景圖像中的視覺特征聚類成許多個“單詞”，然后基于這些“單詞”用向量的形式對整幅圖像進行描述，進而將視覺閉環(huán)檢測問題轉(zhuǎn)化為兩張圖像的描述向量的相似度度量問題。由于機器人所在的工作環(huán)境是變化的，受光照變化、物體移動或者攝像頭位置等因素的影響，即使來自同一個地方的兩張圖片，其絕對相似度很可能也不滿足正確匹配的要求。另外，對于外表相似的一些地點，其圖片的相似度可能很高，因而造成誤匹配的現(xiàn)象。所以，在復(fù)雜環(huán)境下利用單張圖像匹配進行閉環(huán)檢測可能存在比較大的誤差。由于在閉環(huán)檢測問題中，圖片數(shù)據(jù)集在時間上與空間上是關(guān)聯(lián)的，基于這一特性，本文利用圖片序列匹配取代單張圖片匹配，提出了一種基于動態(tài)時間規(guī)整的閉環(huán)檢測算法，其圖像序列匹配不再是比較兩張圖片的絕對相似度，而是更看重圖片在序列中的相對相似度。該方法首先通過深度神經(jīng)網(wǎng)絡(luò)來提取每張圖像的高級抽象特征，然后構(gòu)造出圖像序列的相似度矩陣，最后基于動態(tài)時間規(guī)整算法(dynamic time warping, DTW)[4]計算出序列的相對相似度進行閉環(huán)檢測。通過與詞袋法進行實驗對比，結(jié)果顯示本文所提方法的準(zhǔn)確率和召回率更高，算法性能更好。

1 基于動態(tài)時間規(guī)整閉環(huán)檢測算法

動態(tài)時間規(guī)整算法包含4個關(guān)鍵組成部分：特征提取、余弦相似度矩陣的構(gòu)建、基于DTW的序列相似度求解以及最終匹配結(jié)果報告。

1.1 特征提取

本文利用卷積神經(jīng)網(wǎng)絡(luò)模型VGGNet[5]的前5層完成對場景圖像的特征提取。VGGNet網(wǎng)絡(luò)的輸入是像素大小為224×224、經(jīng)過均值化處理的RGB圖片。網(wǎng)絡(luò)的前5層包含13個卷積層和5個最大池化層。卷積層中，卷積核的大小為3×3，步長為1。池化層中的池化窗口為2×2，步長為2，并且均采用最大池化的方式。同時，在所有隱含層后面均采用ReLU激活函數(shù)以加入非線性因素，提高神經(jīng)網(wǎng)絡(luò)對模型的表達能力。相較于其他卷積網(wǎng)絡(luò)，例如AlexNet[6]，VGGNet網(wǎng)絡(luò)層數(shù)更多，并且所有卷積層使用同樣大小的濾波器。更深的網(wǎng)絡(luò)能夠提升圖像識別性能，學(xué)習(xí)更多圖像的細(xì)微特征。VGGNet網(wǎng)絡(luò)參數(shù)見表1。

表1 VGGNet前5層網(wǎng)絡(luò)參數(shù)表Tab. 1 The first 5-network parameters of VGGNet

本文提取VGGNet的第5層輸出作為圖片的特征，由表1可知Maxpool2層提取到的特征的形式是維度為（1，7，7，512）的張量。

1.2 余弦相似度矩陣

將一張224×224×3的場景圖像經(jīng)過訓(xùn)練好的VGGNet模型提取特征之后，可以得到512個大小為7×7的特征圖，這些特征圖即為輸入圖像的抽象特征。為了計算兩張圖片的相似度，將7×7的特征圖轉(zhuǎn)為一維向量的表達形式：

兩個向量的夾角余弦值越接近1，即兩向量夾角越小，說明兩張圖片相似度越高。傳統(tǒng)的基于單張圖像進行閉環(huán)檢測的原理就是利用兩張圖片的絕對相似度sij與設(shè)定的閾值進行比較來判斷是否形成閉環(huán)。由于單張圖片特征的地點識別無法應(yīng)對復(fù)雜場景，因此本文采用圖片序列來進行比對。

假設(shè)待檢測的場景圖像是Y1，目標(biāo)圖像幀為(X1,X2,X3, …,Xn)，本文選取Y1后的(m-1)幀圖像與該場景圖像組成測試圖像序列(Y1,Y2,Y3, …,Ym)，記該待測序列為T，其中T的長度為m且可調(diào)。為了實現(xiàn)同等長度的序列匹配，本文在目標(biāo)圖像幀中隨機提取連續(xù)的m幀圖像構(gòu)成作為一個候選序列，可以得到(n-m+1)段候選序列，記為(L1,L2,L3, …,Ln-m+1)，如圖1所示。

圖1 候選序列的選取示意圖Fig. 1 Schematic diagram of candidate sequence selection

在進行序列相似度計算之前，需要先構(gòu)造待測序列與每個候選序列的余弦相似度矩陣，以待測序列T與候選序列L1為例：

1.3 基于DTW的序列相似度求解

動態(tài)時間規(guī)整DTW是一個典型的優(yōu)化問題。在測試模板和參考模板時間關(guān)系基礎(chǔ)上對優(yōu)化目標(biāo)求最優(yōu)解，其中優(yōu)化目標(biāo)為兩者的匹配距離。最開始該算法是運用在語言匹配當(dāng)中，用來匹配兩個長度不一致的語音序列的相似度，以識別測試語音對應(yīng)參考語音中的哪個詞?？紤]到這種語言識別與閉環(huán)檢測在匹配時的相似性，采用動態(tài)規(guī)整算法來識別測試圖片序列對應(yīng)參考圖片序列的哪一段，以實現(xiàn)閉環(huán)檢測。

本文在時間關(guān)系基礎(chǔ)上尋找兩個圖像序列中對齊的圖片，即在式(3)所示相似度矩陣中尋找一條通過對齊點的最優(yōu)路徑，路徑通過的格點即為兩個序列進行計算的對齊的點，如圖2所示，黑色點線為最優(yōu)路徑。將該路徑用W來表示。

圖2 規(guī)整路徑示例Fig. 2 An example of regular path

W的第k個元素定義為wk=(i,j)k，路徑要滿足以下的3個條件：

（1）邊界條件。測試圖像序列和參考圖像序列是隨機選擇的，但是每個序列內(nèi)部的時間關(guān)系是一致的，因此滿足邊界條件，目標(biāo)左下角為起始點，右上角為終止點。

（2）連續(xù)性。每一個圖像序列是按照時間排列，時間是連續(xù)關(guān)系，因此圖像序列也將是連續(xù)關(guān)系，路徑計算中可以包含圖像序列中每一個節(jié)點。

（3）單調(diào)性。時間是單調(diào)遞增關(guān)系，圖像序列根據(jù)時間先后順序排列，路徑計算中沿著特定的3個方向，不會出現(xiàn)交叉等現(xiàn)象。

在連續(xù)性和單調(diào)性條件約束下，每一個點（i，j）的前進方向只有3個，即(i+1,j), (i,j+1), (i+1,j+1)。但是，滿足這些約束條件的路徑可以有指數(shù)個，優(yōu)化目標(biāo)是求得代價最小的路徑，即滿足以下條件：

式中：T——目標(biāo)序列；Li——候選序列；wk——對應(yīng)點距離；P——路徑W的元素個數(shù)，其作用是對不同長度的規(guī)整路徑做補償。

本文算法是基于圖像序列的相似度匹配，目標(biāo)是尋找一條相似度最高的規(guī)整路徑，因此定義一個累加距離為

其中，d(Xi,Yj)為當(dāng)前格點相似度，為這3個方向中到此格點累加距離最大的格點。從(0, 0)點開始匹配這兩個序列T和Li，到達終點(m,m)后，這個累積距離就是最后總的相似度。最佳路徑是使得沿路徑的累積距離達到最大值的路徑，即此時序列T和Li的相似度最高。

1.4 匹配結(jié)果

為了得到最終的匹配結(jié)果，本文定義了一個序列相似度向量：

式(7)中，DTW(T,Li)是利用式(5)求解得到的兩序列相似度，SVector代表序列相似度向量。將序列相似度向量中最大元素值與設(shè)定的閾值σ進行比較，判斷待測場景是否形成了閉環(huán)：

其中，max(SVector)指序列相似度向量中最大的元素值，若該值不小于設(shè)定的相似度閾值，則說明待測序列可以在候選序列找到其匹配對象，即待測場景形成了閉環(huán)；若該值小于設(shè)定的相似度閾值，則說明待測場景不形成閉環(huán)。

2 實驗結(jié)果分析

為了驗證基于DTW的閉環(huán)檢測算法的性能和有效性，本文將其與傳統(tǒng)的利用單張圖像進行匹配的FABMAP方法和基于圖像序列匹配的SeqSLAM方法進行了比較，并且討論了主要參數(shù)序列長度對算法造成的影響。

2.1 實驗數(shù)據(jù)集

本文使用Gardens Point Walking閉環(huán)數(shù)據(jù)集對算法進行測試，該數(shù)據(jù)集被廣泛應(yīng)用于閉環(huán)檢測算法性能的評測。Gardens Point Walking是針對澳大利亞布里斯班昆士蘭科技大學(xué)Gardens Point校區(qū)的單一路線收集的視覺數(shù)據(jù)集。作者用前向手持?jǐn)z像頭收集視覺數(shù)據(jù)，且在此路線一共采集3組數(shù)據(jù)，分別為白天兩組，夜晚一組。在路徑的左側(cè)遍歷一天路線，并且在路徑的右側(cè)遍歷另一天路線和夜間路線，用相機捕獲不同時間、方向路線中環(huán)境的姿態(tài)和狀況變化。圖片幀之間的采集間隔取得較大，以避免在相同的時刻有大量重復(fù)圖片。Gardens Point Walking數(shù)據(jù)集由3個部分組成，白天沿路線左側(cè)拍攝的200張圖像、白天沿路線右側(cè)拍攝的200張圖像以及晚上沿路線右側(cè)拍攝的200張圖像。圖像按照名稱順序在空間位置上是一一對應(yīng)的。圖3給出了該閉環(huán)數(shù)據(jù)集的真實軌跡以及示例圖像。

圖3 數(shù)據(jù)集軌跡及閉環(huán)數(shù)據(jù)示例Fig. 3 Examples of trajectory and closed-loop data

2.2 準(zhǔn)確率和召回率

與傳統(tǒng)的基于單張圖片進行匹配的方式不同，本文匹配的是一定長度的圖片序列，并且通過DTW算法計算兩段圖片序列的相似度大小S，然后比較S和設(shè)定閾值σ的關(guān)系來判斷兩張圖片是否屬于閉環(huán)。當(dāng)滿足閾值要求時，則判定兩張圖片構(gòu)成閉環(huán)。在實驗中，得到的結(jié)果可以分為4種情況：真陽性TP、真陰性TN、假陽性FP和假陰性FN。其中，假陽性FP和假陰性FN為常見的兩種錯誤的閉環(huán)。在閉環(huán)檢測實驗中，假陽性指識別兩個不同地點為同一地點，假陰性指未識別出兩個相同地點。為了綜合評價閉環(huán)檢測算法的質(zhì)量，統(tǒng)計假陽性和假陰性數(shù)量，計算其準(zhǔn)確率(precision，P)和召回率(recall，R)值，并作出一條準(zhǔn)-召曲線precision-recall曲線(簡稱“P-R曲線”)。通過統(tǒng)計測試過程中TP，TN，F(xiàn)N和FP出現(xiàn)的次數(shù)，可以計算得到閉環(huán)檢測算法的準(zhǔn)確率P和召回率R：

通過設(shè)定不同的閾值，可以得到一組準(zhǔn)確率和召回率的值。以R為橫坐標(biāo)，P為縱坐標(biāo)，可以得到算法的P-R曲線。P-R曲線越偏向右上方，表明算法越好。與此同時，P=100%時的召回率大小和R=50%時的準(zhǔn)確率大小都可以作為算法的評價指標(biāo)。

將2.1節(jié)介紹的Gardens Point Walking數(shù)據(jù)集中白天沿路線右側(cè)拍攝的200張圖像作為目標(biāo)圖像序列。測試圖像序列由作為閉環(huán)數(shù)據(jù)的50張晚上沿路線右側(cè)拍攝的圖像和150張非閉環(huán)圖像組成。利用該測試數(shù)據(jù)得到的實驗結(jié)果如圖4所示。

圖4 算法在測試數(shù)據(jù)集上的P-R曲線Fig. 4 P-R curve of the algorithm on the test data set

從圖4可以看出，本文提出的基于DTW的閉環(huán)檢測算法的P-R曲線偏向于右上方，具有較高的準(zhǔn)確率和召回率。當(dāng)R=50%時，F(xiàn)abMap方法的準(zhǔn)確率只有28%，SeqSLAM算法的準(zhǔn)確率為58%，而本文方法能達到100%。說明本文所提算法明顯優(yōu)于傳統(tǒng)詞袋法(FabMap)以及SeqSLAM算法。同時由于目標(biāo)序列是白天拍攝的圖片，而所選取的閉環(huán)數(shù)據(jù)選取的是同一地點晚上拍攝的圖像，說明該方法在光照變化時魯棒性較好。

2.3 真陽率與假陽率

除了對算法的準(zhǔn)確率和召回率進行分析之外，本文還引入了接受者操作特征曲線(receiver operating characteristic, ROC)來對算法的性能進行評價。ROC曲線是另外一種常用的閉環(huán)檢測算法的評估方式，它顯示的是算法的真陽率(TPR)和假陽率(FPR)之間的關(guān)系。該方法簡單、直觀，觀察者通過圖示可分析方法的準(zhǔn)確性，并可用肉眼作出判斷。ROC曲線計算公式為

通常，ROC曲線越偏向左上方，表明算法越好，將其量化為曲線下面積 (area under curve, AUC)，則面積越大代表分類的性能越好，達到理想情況時AUC為1。利用2.1節(jié)所介紹的測試數(shù)據(jù)集分別對FabMap方法、SeqSLAM算法以及本文提出的閉環(huán)檢測算法繪制ROC曲線，得到的結(jié)果如圖5所示。

圖5 算法在測試數(shù)據(jù)集上的ROC曲線Fig. 5 ROC curves of the algorithm on the test data set

從圖5可以看出，本文提出的算法得到的ROC曲線最靠近左上方，真陽率的值始終比其他兩種方法要高。通過計算3種方法的ROC曲線下面積，可以得到SeqSLAM的AUC值為0.56，F(xiàn)abMap的AUC值為0.61，而本文所提算法的AUC值為0.96，遠(yuǎn)遠(yuǎn)高于其他兩種算法的，說明該方法檢測閉環(huán)的精度更高，算法性能更好。

2.4 序列長度的不同對結(jié)果造成的影響

在利用動態(tài)時間規(guī)整算法求取兩段序列的相似度時，實驗發(fā)現(xiàn)序列長度的不同會影響到相似度的結(jié)果，并且對最終閉環(huán)檢測的精度造成間接影響。因此，序列長度的選取是基于DTW的閉環(huán)檢測算法的一個關(guān)鍵因素。為了驗證相似度計算時序列長度調(diào)整對閉環(huán)檢測造成的影響，本文將圖片序列長度Length分別設(shè)置為5, 10, 15及20，通過式(1)計算對應(yīng)序列長度的相似度，并且在第2.1節(jié)介紹的測試數(shù)據(jù)集下繪制出不同長度的P-R曲線以及ROC曲線來進行對比，實驗結(jié)果如圖6所示。

圖6 不同序列長度的結(jié)果分析示意圖Fig. 6 Result analysis in the condition of different sequence lengths

根據(jù)圖6可以發(fā)現(xiàn)，將閉環(huán)檢測匹配的方式設(shè)置為一定長度的序列匹配時，P-R曲線基本上都偏向于右上角，而ROC曲線基本上偏向于左上角，這說明了序列匹配性能的優(yōu)越性。

同時，當(dāng)R較高時，序列長度為10時的準(zhǔn)確率比序列長度為5，15及20時的準(zhǔn)確率要高。例如，在圖6(a)中，當(dāng)召回率為90%時，序列長度為10的準(zhǔn)確率達到了98%，而序列長度為5，15和20的準(zhǔn)確率分別為88%，94%和96%。在圖6(b)中，計算得到序列長度為5時的AUC值為0.976；當(dāng)序列長度增大到10時，得到的AUC值增長到了0.993；序列長度繼續(xù)增大到15時，AUC值并沒有增加，反而下降到了0.990；當(dāng)序列長度為20時AUC的值繼續(xù)下降，其值變成了0.988。結(jié)果證明，匹配序列長度會影響閉環(huán)檢測的結(jié)果，序列長度被設(shè)置得過長或過短都會降低算法的性能，而選取合適的序列長度不僅可以提高視覺閉環(huán)檢測算法的準(zhǔn)確率和召回率，同時還可減少假陽性出現(xiàn)的概率，更有利于檢測閉環(huán)。

3 結(jié)語

本文針對單張圖像匹配的閉環(huán)檢測算法存在的缺陷，利用單張圖片的時間連續(xù)性的特點，提出一種基于DTW的閉環(huán)檢測算法。此算法利用序列圖片匹配可以提高閉環(huán)檢測的動態(tài)環(huán)境適應(yīng)能力。文中首先選擇在ImageNet數(shù)據(jù)集上預(yù)訓(xùn)練好的深度神經(jīng)網(wǎng)絡(luò)VGGNet的前5個卷積層來提取每張圖像的高級抽象特征，然后構(gòu)造出圖像序列的相似度矩陣，最后基于動態(tài)規(guī)整算法計算出序列的相對相似度進行閉環(huán)檢測；同時，詳細(xì)比較了不同序列長度下得到的P-R曲線以及ROC曲線以確定最合適的匹配序列長度。

從實驗結(jié)果可以看到，本文所提方法具有較高的準(zhǔn)確率和召回率，并且假陽性概率較低，相較于主流方法FabMap及SeqSLAM算法，其性能更好。同時，序列長度分析實驗結(jié)果表明，雖然不同長度的序列精度稍有差別，但只要選取合適的序列長度就可以提高閉環(huán)檢測性能。因此如何選取合適的序列長度是我們未來的研究方向。