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

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

0 引言

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

1 基于視覺(jué)字典的場(chǎng)景模型表示

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

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

　　1.1 字典的創(chuàng)建

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

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

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

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

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

　　1.2 基于視覺(jué)字典的圖像相似度計(jì)算方法

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

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

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

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

　　2.1 圖像預(yù)處理

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

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

　　2.if It不合格

　　3.刪除It；

　　4.else

　　5.Ic←獲取前一個(gè)時(shí)刻保存的圖像

　　6.Sim（It，Ic）←計(jì)算圖像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 貝葉斯濾波更新

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

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

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

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

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

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

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

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

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

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

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

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

　　2.3 閉環(huán)獲取

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

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

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

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

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

　　4.1 圖像特征提取算法選取

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

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

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

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

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

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

5 結(jié)論

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

　　參考文獻(xiàn)

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

　　[2] 王麗楊，陳明.移動(dòng)機(jī)器人的導(dǎo)航地位和地圖構(gòu)建技術(shù)綜述[J].中國(guó)新技術(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] 梁志偉，陳燕燕，朱松豪，等．基于視覺(jué)字典的單目視覺(jué)閉環(huán)檢測(cè)算法[J]．模式識(shí)別與人工智能，2013，26（6）：561-570．

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