摘  要： 針對移動機(jī)器人同步定位和地圖構(gòu)建（SLAM）的閉環(huán)檢測問題，提出了一種基于視覺字典的閉環(huán)檢測方法。該方法首先使用SURF算法對每一幀圖像進(jìn)行特征提取，生成視覺單詞，構(gòu)建視覺字典樹，再基于“詞袋”（Bag of Words，BoW）對場景建模，通過計算圖像視覺單詞的匹配度估計圖像間的相似度。為提高閉環(huán)檢測的成功率，運(yùn)用貝葉斯濾波與相似度來計算閉環(huán)假設(shè)的后驗(yàn)概率分布。同時，為提高系統(tǒng)的實(shí)時性，引入了內(nèi)存管理機(jī)制。實(shí)驗(yàn)結(jié)果顯示該方法是有效的。

　　關(guān)鍵詞： SLAM；視覺單詞；閉環(huán)檢測；內(nèi)存管理

0 引言

　　機(jī)器人學(xué)是一門綜合學(xué)科，代表著目前高技術(shù)的發(fā)展前沿，移動機(jī)器人作為機(jī)器人研究領(lǐng)域的一個重要分支，在軍事、生產(chǎn)自動化等許多領(lǐng)域都有廣泛的應(yīng)用，具有重要的實(shí)用價值，已經(jīng)成為機(jī)器人領(lǐng)域的研究熱點(diǎn)之一[1-2]。自主導(dǎo)航作為移動機(jī)器人的關(guān)鍵技術(shù)，是機(jī)器人在沒有外部環(huán)境先驗(yàn)知識的幫助下，依賴自身的傳感器實(shí)現(xiàn)地圖創(chuàng)建和精確定位。閉環(huán)檢測是機(jī)器人研究的又一重要內(nèi)容，用以判斷機(jī)器人當(dāng)前的位置是否為之前已經(jīng)經(jīng)歷過的歷史環(huán)境區(qū)域。有效的場景模型是機(jī)器人視覺SLAM閉環(huán)檢測[3-6]的關(guān)鍵。BoW[7-8]是一種非常有效的場景建模方法，它通過提取圖像局部特征，并對其分類構(gòu)建視覺字典樹[9]。任何一幅圖像都可以通過視覺字典中的單詞集合進(jìn)行表示。在視覺閉環(huán)檢測方面，Cummins等人提出了基于拓?fù)渫庥^的概率閉環(huán)檢測FAB-MAP[10]方法。本文首先使用SURF算法提取圖像的局部特征，再基于BoW方法構(gòu)建場景模型，創(chuàng)建視覺字典樹，并根據(jù)圖像間的相似度進(jìn)行圖像預(yù)處理，最后基于貝葉斯濾波更新閉環(huán)檢測的后驗(yàn)概率，提高閉環(huán)檢測的準(zhǔn)確性；同時引入內(nèi)存管理機(jī)制，提高閉環(huán)檢測的實(shí)時性，增強(qiáng)系統(tǒng)的執(zhí)行效率。

1 基于視覺字典的場景模型表示

　　借鑒文本處理領(lǐng)域中的“詞袋法”，使用BoW方法將圖像的連續(xù)特征離散化，用一組無序的局部特征集合表示圖像。BoW只強(qiáng)調(diào)用維數(shù)相同而權(quán)重不同的向量描述來表示視覺特征，忽略它們之間的空間位置關(guān)系，是視覺SLAM場景表示的一種有效的建模方法。

　　本文提出了基于BoW的場景表示方法，其具體過程可分為3步：（1）使用SURF算法提取圖像局部特征；（2）使用KD-Tree索引方法創(chuàng)建視覺字典樹；（3）投影圖像特征到字典樹，計算圖像間的相似度。基于BoW，一幅圖像就可以通過對應(yīng)的視覺單詞進(jìn)行表示，這不僅保存了圖像的局部特征，也節(jié)約了圖像的存儲空間。

　　1.1 字典的創(chuàng)建

　　創(chuàng)建BoW的目的是為了將相似的特征描述向量劃分為相同的視覺單詞，通常采用的表示方法有：K-Means算法[11]、近似K-Means算法和Mean-Shift算法等，其主要就是對局部特征點(diǎn)聚類，每個聚類中心代表一個視覺單詞。該算法對于高維局部特征，效率下降，穩(wěn)定性也降低。Moosmann等人則借鑒隨機(jī)決策樹和隨機(jī)森林算法思想，提出了隨機(jī)聚類森林方法，減弱了視覺字典生成過程中的隨機(jī)性，但其時間復(fù)雜度高且占用內(nèi)存大。為了獲得更加準(zhǔn)確的視覺字典分類和提高視覺字典的生成效率，本文基于隨機(jī)聚類森林算法，使用隨機(jī)KD-Tree索引方法，具體實(shí)現(xiàn)過程如下所述：

　　首先，假設(shè)在t時刻獲取圖像It，通過SURF算法提取到圖像的M個視覺特征集合{Fm}（0<m≤M），集合中的每個視覺特征Fm都是64維。由于系統(tǒng)受外部噪聲等因素影響，需要對視覺特征進(jìn)行預(yù)篩選；再根據(jù)篩選結(jié)果，使用KD-Tree方法創(chuàng)建視覺字典樹；最后使用最近鄰距離比算法，更新視覺字典。設(shè)此時視覺字典中有N個視覺單詞，集合為{Vn}（0<n≤N）。

　　‖F(xiàn)m‖2≥Tresponse，（0<m≤M且M≤TmaxFeatures）（1）

　　式（1）是對視覺特征的預(yù)篩選，只提取響應(yīng)強(qiáng)度不小于Tresponse的視覺特征。同時，如果提取當(dāng)前場景圖像的視覺特征與平均每幅場景圖像的視覺特征之比小于Tbad，則認(rèn)為該場景表示不合格。TmaxFeatures是提取一幅場景圖像最大的視覺特征數(shù)。

　　式（2）表示，基于視覺字典對當(dāng)前視覺特征使用最近距離比算法，如果RNNDR<TNNDR（最近距離比閾值），則將當(dāng)前兩個視覺特征看成是一個視覺單詞，更新視覺字典。

　　1.2 基于視覺字典的圖像相似度計算方法

　　基于BoW圖像表示，如果兩幅圖像相似，則說明有相同的視覺單詞，圖像相似度可以通過圖像間視覺單詞的匹配度來估計。設(shè)Nt和Nc分別為SURF算法提取兩幅圖像相應(yīng)的視覺單詞數(shù)，Npair為圖像間匹配的視覺單詞數(shù)，則圖像間的相似度Sim（It，Ic）為：

　　通過式（3），將圖像間的相似度轉(zhuǎn)化為視覺單詞的匹配度，大大減少了計算的復(fù)雜度。

2 基于貝葉斯濾波閉環(huán)檢測方法

　　閉環(huán)檢測過程可以分為圖像預(yù)處理、貝葉斯濾波更新和閉環(huán)獲取三個過程。對于圖像預(yù)處理過程，如果在圖像獲取過程中，獲取圖像的頻率較高，則相鄰圖像的相似度較高，即表示同一場景的可能性較大。因此，需要在圖像預(yù)處理時，剔除與前一時刻相似度高的場景圖像，通過不斷更新當(dāng)前圖像與歷史圖像的后驗(yàn)概率獲取閉環(huán)。

　　2.1 圖像預(yù)處理

　　在機(jī)器人獲取場景圖像過程中，相鄰圖像間的相似度一般都較大。如果對所有圖像都處理，勢必會增加系統(tǒng)負(fù)擔(dān)。通過添加圖像間的相似度閾值Tsim，可以有效減少數(shù)據(jù)量處理。在本文中，為了后續(xù)程序更好地檢測閉環(huán)，引入圖像權(quán)重概念，權(quán)重越高表示在該圖像發(fā)生閉環(huán)的可能性越大。設(shè)t時刻獲取的圖像為It，具體圖像預(yù)處理過程如下所示：

　　1.It←對新圖像特征提取和篩選，設(shè)權(quán)重Wt=0；

　　2.if It不合格

　　3.刪除It；

　　4.else

　　5.Ic←獲取前一個時刻保存的圖像

　　6.Sim（It，Ic）←計算圖像It與Ic的相似度；

　　7.If  Sim（It，Ic）大于Tsim

　　8.It←合并圖像It與圖像Ic；

　　9.Weight（It）=Weight（Ic）+1；

　　10.保存It，刪除Ic

　　11.end if；

　　2.2 貝葉斯濾波更新

　　基于貝葉斯濾波理論，在沒有完全情報的情況下，先對未知狀態(tài)進(jìn)行主觀概率估計，再利用貝葉斯修正事件發(fā)生的概率，最后依據(jù)后驗(yàn)概率做最優(yōu)決策，即將閉環(huán)檢測假設(shè)看成是一個迭代的貝葉斯估計問題，通過估計當(dāng)前圖像與已經(jīng)訪問過的歷史圖像相匹配的后驗(yàn)概率來檢測閉環(huán)。

　　設(shè)Xt為t時刻發(fā)生閉環(huán)假設(shè)的一個隨機(jī)變量，X=i表示當(dāng)前圖像It與歷史圖像Ii相匹配，而Xt=-1表示當(dāng)前圖像It為新的場景圖像，此刻沒有發(fā)生閉環(huán)。貝葉斯濾波主要包含兩部分，狀態(tài)預(yù)測和測量更新，通過計算所有時刻i=-1，…，t發(fā)生閉環(huán)的概率來估計后驗(yàn)概率分布p（Xt|It）。

　　狀態(tài)預(yù)測：

　　其中，η是歸一化因子；It=I-1，…；It為在t時刻獲取的整個圖像序列。

　　在使用后驗(yàn)概率進(jìn)行閉環(huán)檢測時，最重要的兩個環(huán)節(jié)就是對觀測模型和運(yùn)動模型的估計。在本文中，使用似然函數(shù)對觀測模型進(jìn)行評估。如果當(dāng)前時刻發(fā)生閉環(huán)，似然函數(shù)L（Xt|It）計算公式為：

　　如果當(dāng)前時刻沒有發(fā)生閉環(huán)，則似然函數(shù)L（Xt|Lt）為：

　　其中Sim表示當(dāng)前圖像It與發(fā)生閉環(huán)的歷史圖像Ii的相似度，μ和σ分別表示當(dāng)前圖像與歷史圖像相似度的平均值和標(biāo)準(zhǔn)差。如果計算獲得的似然值L（Xt|It）較大，意味著當(dāng)前圖像與歷史圖像的相似度較小，即當(dāng)前圖像為新場景圖像的可能性較大。

　　運(yùn)動模型預(yù)測閉環(huán)的狀態(tài)，具體可以分為以下四種情況：

　?。?）p（Xt=-1│Xt-1=-1）=0.9，如果在t-1時刻沒有發(fā)生閉環(huán)，那么在t時刻也沒有發(fā)生閉環(huán)的概率為0.9；

　?。?）p（Xt=i│Xt-1=-1）=0.1/n，i∈[0，t-1]，如果在t-1時刻沒有發(fā)生閉環(huán)，那么t時刻發(fā)生閉環(huán)的概率為  0.1/n，其中n為獲取圖像的總數(shù)；

　?。?）p（Xt=-1│Xt-1=i）=0.1，i∈[0，t-1]，如果在t-1時刻發(fā)生閉環(huán)，那么在t時刻圖像i處發(fā)生閉環(huán)的概率為0.1；

　　（4）p（Xt=i│Xt-1=j），i，j∈[0，t]，表示在t時刻和t-1時刻都發(fā)生閉環(huán)的概率。定義該概率是以j為中心的非空離散高斯函數(shù)，并且j與其相鄰8個（i=j-4，…，j+4）領(lǐng)域值之和為0.9。

　　2.3 閉環(huán)獲取

　　閉環(huán)檢測過程是一個不斷預(yù)測和更新的過程，通過上一時刻的預(yù)測和更新來估計當(dāng)前的后驗(yàn)概率p（Xt│It）。如果當(dāng)前估計的后驗(yàn)概率p（Xt│It）大于閉環(huán)閾值Tloop，則認(rèn)為發(fā)生閉環(huán)，否則繼續(xù)獲取場景圖像It進(jìn)行新的閉環(huán)檢測。

3 實(shí)時性的實(shí)現(xiàn)

　　為提高系統(tǒng)閉環(huán)檢測的實(shí)時性，本文引入短期內(nèi)存、工作內(nèi)存和長期內(nèi)存三種內(nèi)存管理機(jī)制。短期內(nèi)存主要用來合并相似度較高圖像并更新其權(quán)重，其遵循先進(jìn)先出原則。當(dāng)短期內(nèi)存中儲存的圖像數(shù)目大于設(shè)定儲存空間大小時，將最先添加到短期內(nèi)存中的圖像轉(zhuǎn)移到工作內(nèi)存中，進(jìn)行后續(xù)處理。工作內(nèi)存主要是實(shí)現(xiàn)貝葉斯濾波更新和實(shí)時閉環(huán)檢測。在工作內(nèi)存中保存著權(quán)重較大的圖像，權(quán)重小的圖像或處理時間超過設(shè)定時間閾值的圖像被實(shí)時地轉(zhuǎn)存到長期內(nèi)存中。長期內(nèi)存的作用相當(dāng)于數(shù)據(jù)庫，保存大量的圖像。這種通過短期內(nèi)存激活工作內(nèi)存，再激活長期內(nèi)存的機(jī)制，大大減少了數(shù)據(jù)量的處理，提高了系統(tǒng)的實(shí)時性能。

4 實(shí)驗(yàn)結(jié)果

　　本文提出的閉環(huán)檢測算法基于處理器Intel Core T6600，2.2 GHz主頻，2 GB內(nèi)存，通過手持微軟Kinect XBOX在一個相對較小的環(huán)境區(qū)域，以1 Hz/s的采樣速率，共采集到120張320×240場景圖像。圖1展示的是微軟Kinect XBOX和在實(shí)驗(yàn)室取景軌跡。

　　4.1 圖像特征提取算法選取

　　SIFT算法是圖像特征提取與匹配中常用且有效的方法，其在仿射變換、噪聲等情況下有良好的匹配性能，但由于其生成的特征描述維數(shù)較高，計算量大和時間復(fù)雜度高，系統(tǒng)的實(shí)時性能低。SURF算法是在積分圖像的基礎(chǔ)上，利用方框?yàn)V波近似代替二階高斯濾波，大大提高了運(yùn)算效率。如圖2與圖3所示，是使用SIFT和SURF算法對圖像的處理。結(jié)果顯示，使用SURF算法更有效。

　　4.2 實(shí)驗(yàn)閾值的設(shè)置

　　實(shí)驗(yàn)中各個閾值的選取不僅關(guān)系到系統(tǒng)的檢測，也影響系統(tǒng)的性能。首先，對于場景圖像的采樣速率R，如果設(shè)置較小，會影響系統(tǒng)的實(shí)時性；若設(shè)置過大，會影響閉環(huán)檢測的準(zhǔn)確性。根據(jù)經(jīng)驗(yàn)，采樣頻率R設(shè)為1 Hz。其次，對于圖像的處理時間閾值Ttime，主要由配置、運(yùn)行環(huán)境等因素決定，但都要滿足系統(tǒng)處理每張圖像所需的時間。一般情況下，采樣頻率設(shè)為1 Hz，Ttime為600 ms~800 ms。相關(guān)閾值設(shè)置如表1所示。

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

　　利用本文提供的方法對數(shù)據(jù)進(jìn)行處理，圖4是實(shí)驗(yàn)室環(huán)境下對圖像的處理過程。前者是通過SURF算法提取場景圖像的視覺特征過程，后者表示在第84幀場景圖像處與第2幀場景圖像處發(fā)生了閉環(huán)。

　　根據(jù)表1參數(shù)，實(shí)驗(yàn)室環(huán)境下閉環(huán)檢測的結(jié)果如圖5所示。菱形表示系統(tǒng)沒有檢測到閉環(huán)的圖像序列，圓圈表示正確檢測到閉環(huán)的圖像序列，而三角形則表示檢測到閉環(huán)但拒絕閉環(huán)的圖像序列。

5 結(jié)論

　　本文提出的基于視覺字典的圖像表示與圖像間的相似度計算方法，極大地提高了系統(tǒng)閉環(huán)檢測效率，體現(xiàn)了系統(tǒng)的實(shí)時處理能力。通過對實(shí)驗(yàn)室環(huán)境的檢測，本方法得到了很好的驗(yàn)證，為后續(xù)機(jī)器人的研究奠定了基礎(chǔ)。目前，實(shí)驗(yàn)環(huán)境還比較局限，還要對其作進(jìn)一步探索。

　　參考文獻(xiàn)

　　[1] 劉寶平，張紅梅.移動機(jī)器人研究現(xiàn)狀和未來發(fā)展的分析[J].科技信息，2009（29）：65．

　　[2] 王麗楊，陳明.移動機(jī)器人的導(dǎo)航地位和地圖構(gòu)建技術(shù)綜述[J].中國新技術(shù)新產(chǎn)品，2011（17）：13．

　　[3] ANGELI A， FILLIAT D． A fast and incremental method for loop-closure detection using bags of visual words[J]．IEEE Transactions on Robotics， 2008，24（5）：204-226．

　　[4] ANGELI A， FILLIAT D. Real time visual loop closure detection[C]. IEEE International Conference on Robotics and Automation（ICRA）， Pasadena， 2008：1842-1847．

　　[5] Liu Yang， Zhang Hong. Indexing visual features： real-time loop closure detection using a tree structure[C]. International Conference on Robotics and Automation， 2012：3613-3618．

　　[6] MATHIEU L，F(xiàn)RANCOIS M. Appearance-based loop closure detection for online large-scale and long-term operation[J]. IEEE Transaction on Robotics， 2013，9（3）：734．

　　[7] 劉紅光，魏小敏.Bag of Words算法框架的研究[J].艦船電子工程，2011，31（9）：125-128．

　　[8] BOTTERILL T， GREEN R， MILLS S. A Bag-of-Words speedometer for single camera SLAM[C]. 24th International Conference Image and Vision Computing New Zealand（IVCNZ 2009），2009：91-96．

　　[9] CUMMINS M， NEWMAN P． Accelerating FAB-MAP with concentration inequalities[J]． IEEE Trans on Robotics， 2010，26（6）：1042-1050．

　　[10] 梁志偉，陳燕燕，朱松豪，等．基于視覺字典的單目視覺閉環(huán)檢測算法[J]．模式識別與人工智能，2013，26（6）：561-570．

　　[11] 李博，楊丹，鄧林．移動機(jī)器人閉環(huán)檢測的視覺字典樹金字塔TF-IDF得分匹配方法[J]．自動化學(xué)報，2011，37（6）：665-673．