崔大成，曾連蓀

（上海海事大學(xué)信息工程學(xué)院，上海201306）

基于視覺(jué)字典的移動(dòng)機(jī)器人閉環(huán)檢測(cè)方法研究

崔大成，曾連蓀

（上海海事大學(xué)信息工程學(xué)院，上海201306）

針對(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é)果顯示該方法是有效的。

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í)的幫助下，依賴(lài)自身的傳感器實(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ì)其分類(lèi)構(gòu)建視覺(jué)字典樹(shù)[9]。任何一幅圖像都可以通過(guò)視覺(jué)字典中的單詞集合進(jìn)行表示。在視覺(jué)閉環(huán)檢測(cè)方面，Cummins等人提出了基于拓?fù)渫庥^的概率閉環(huán)檢測(cè)FABMAP[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é)單詞，通常采用的表示方法有：KMeans算法[11]、近似K-Means算法和Mean-Shift算法等，其主要就是對(duì)局部特征點(diǎn)聚類(lèi)，每個(gè)聚類(lèi)中心代表一個(gè)視覺(jué)單詞。該算法對(duì)于高維局部特征，效率下降，穩(wěn)定性也降低。Moosmann等人則借鑒隨機(jī)決策樹(shù)和隨機(jī)森林算法思想，提出了隨機(jī)聚類(lèi)森林方法，減弱了視覺(jué)字典生成過(guò)程中的隨機(jī)性，但其時(shí)間復(fù)雜度高且占用內(nèi)存大。為了獲得更加準(zhǔn)確的視覺(jué)字典分類(lèi)和提高視覺(jué)字典的生成效率，本文基于隨機(jī)聚類(lèi)森林算法，使用隨機(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）。

式（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)訪(fǎ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è)：

測(cè)量更新：

依據(jù)貝葉斯得到t時(shí)刻的后驗(yàn)概率分布為：其中，η是歸一化因子；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)，具體可以分為以下四種情況：

（1）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；

（4）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)室取景軌跡。

圖1 微軟Kinect與實(shí)驗(yàn)室環(huá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算法更有效。

圖2 圖像處理時(shí)間

圖3 圖像特征提取

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)境等因素決定，但都要滿(mǎn)足系統(tǒng)處理每張圖像所需的時(shí)間。一般情況下，采樣頻率設(shè)為1 Hz，Ttime為600 ms～800 ms。相關(guān)閾值設(shè)置如表1所示。

表1 實(shí)驗(yàn)閾值設(shè)置

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)。

圖4 場(chǎng)景圖像處理過(guò)程

根據(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 閉環(huán)檢測(cè)結(jié)果

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)一步探索。

[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，F(xiàn)ILLIAT 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，F(xiàn)ILLIAT 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].24thInternational 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．

Loop closure detection based on visual dictionary for mobile robots

Cui Dacheng，Zeng Liansun
（College of Information Engineering，Shanghai Maritime University，Shanghai 201306，China）

Aiming at the problem of loop closure detection in simultaneous localization and mapping（SLAM）for mobile robots，this paper presents a novel detection algorithm based on visual dictionary.Firstly，it uses SURF algorithm to extract features to generate visual words for each required image，and builds a visual vocabulary tree.Then，it builds a scene model based on bag of words（BoW）algorithm.The similarity between the images will be estimated by the degree of matching in visual words.In order to improve the continuity of the loop closure detection，the Bayesian filter method and similarity is applied to estimate the posteriori probability distribution of loop closure detection.Meanwhile，in order to improve the system′s real-time and process speed，the paper introduces memory management mechanisms.The experimental results demonstrate that this method is effective.

SLAM；visual word；loop closure detection；memory management

TP302.1

A

1674-7720（2015）09-0085-04

2014-12-03）

崔大成（1987-），男，碩士生，主要研究方向：移動(dòng)機(jī)器人導(dǎo)航。

曾連蓀（1962-），男，教授，碩士生導(dǎo)師，主要研究方向：無(wú)線(xiàn)接入技術(shù)。