摘  要： 針對(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é)是一門(mén)綜合學(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ù)；

　?。?）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．