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

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

0 引言

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

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

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

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

1.1 特征提取

本文利用卷積神經(jīng)網(wǎng)絡(luò)模型VGGNet[5]的前5層完成對(duì)場景圖像的特征提取。VGGNet網(wǎng)絡(luò)的輸入是像素大小為224×224、經(jīng)過均值化處理的RGB圖片。網(wǎng)絡(luò)的前5層包含13個(gè)卷積層和5個(gè)最大池化層。卷積層中，卷積核的大小為3×3，步長為1。池化層中的池化窗口為2×2，步長為2，并且均采用最大池化的方式。同時(shí)，在所有隱含層后面均采用ReLU激活函數(shù)以加入非線性因素，提高神經(jīng)網(wǎng)絡(luò)對(duì)模型的表達(dá)能力。相較于其他卷積網(wǎng)絡(luò)，例如AlexNet[6]，VGGNet網(wǎng)絡(luò)層數(shù)更多，并且所有卷積層使用同樣大小的濾波器。更深的網(wǎng)絡(luò)能夠提升圖像識(shí)別性能，學(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個(gè)大小為7×7的特征圖，這些特征圖即為輸入圖像的抽象特征。為了計(jì)算兩張圖片的相似度，將7×7的特征圖轉(zhuǎn)為一維向量的表達(dá)形式：

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

1.4 匹配結(jié)果

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

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

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

2 實(shí)驗(yàn)結(jié)果分析

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

2.1 實(shí)驗(yàn)數(shù)據(jù)集

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

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

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

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

通過設(shè)定不同的閾值，可以得到一組準(zhǔn)確率和召回率的值。以R為橫坐標(biāo)，P為縱坐標(biāo)，可以得到算法的P-R曲線。P-R曲線越偏向右上方，表明算法越好。與此同時(shí)，P=100%時(shí)的召回率大小和R=50%時(shí)的準(zhǔn)確率大小都可以作為算法的評(píng)價(jià)指標(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ù)得到的實(shí)驗(yàn)結(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%時(shí)，F(xiàn)abMap方法的準(zhǔn)確率只有28%，SeqSLAM算法的準(zhǔn)確率為58%，而本文方法能達(dá)到100%。說明本文所提算法明顯優(yōu)于傳統(tǒng)詞袋法(FabMap)以及SeqSLAM算法。同時(shí)由于目標(biāo)序列是白天拍攝的圖片，而所選取的閉環(huán)數(shù)據(jù)選取的是同一地點(diǎn)晚上拍攝的圖像，說明該方法在光照變化時(shí)魯棒性較好。

2.3 真陽率與假陽率

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

通常，ROC曲線越偏向左上方，表明算法越好，將其量化為曲線下面積 (area under curve, AUC)，則面積越大代表分類的性能越好，達(dá)到理想情況時(shí)AUC為1。利用2.1節(jié)所介紹的測試數(shù)據(jù)集分別對(duì)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曲線最靠近左上方，真陽率的值始終比其他兩種方法要高。通過計(jì)算3種方法的ROC曲線下面積，可以得到SeqSLAM的AUC值為0.56，F(xiàn)abMap的AUC值為0.61，而本文所提算法的AUC值為0.96，遠(yuǎn)遠(yuǎn)高于其他兩種算法的，說明該方法檢測閉環(huán)的精度更高，算法性能更好。

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

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

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

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

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

3 結(jié)語

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

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