基于視覺字典的移動機器人閉環(huán)檢測方法研究

崔大成，曾連蓀

（上海海事大學信息工程學院，上海201306）

基于視覺字典的移動機器人閉環(huán)檢測方法研究

崔大成，曾連蓀

（上海海事大學信息工程學院，上海201306）

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

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

0 引言

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

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

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

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

1.1 字典的創(chuàng)建

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

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

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

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

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

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

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

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

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

2.1 圖像預處理

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

1.It←對新圖像特征提取和篩選，設權重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)進行主觀概率估計，再利用貝葉斯修正事件發(fā)生的概率，最后依據(jù)后驗概率做最優(yōu)決策，即將閉環(huán)檢測假設看成是一個迭代的貝葉斯估計問題，通過估計當前圖像與已經(jīng)訪問過的歷史圖像相匹配的后驗概率來檢測閉環(huán)。

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

狀態(tài)預測：

測量更新：

依據(jù)貝葉斯得到t時刻的后驗概率分布為：其中，η是歸一化因子；It=I-1，…；It為在t時刻獲取的整個圖像序列。

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

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

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

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

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

（2）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ù)；

（3）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）領域值之和為0.9。

2.3 閉環(huán)獲取

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

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

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

4 實驗結(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和在實驗室取景軌跡。

圖1 微軟Kinect與實驗室環(huán)境

4.1 圖像特征提取算法選取

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

圖2 圖像處理時間

圖3 圖像特征提取

4.2 實驗閾值的設置

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

表1 實驗閾值設置

4.3 實驗結(jié)果與分析

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

圖4 場景圖像處理過程

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

圖5 閉環(huán)檢測結(jié)果

5 結(jié)論

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

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

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

[11]李博，楊丹，鄧林．移動機器人閉環(huán)檢測的視覺字典樹金字塔TF-IDF得分匹配方法[J]．自動化學報，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-），男，碩士生，主要研究方向：移動機器人導航。

曾連蓀（1962-），男，教授，碩士生導師，主要研究方向：無線接入技術。