吳俊君 ，胡國生

（1.廣東食品藥品職業(yè)學(xué)院 基礎(chǔ)部，廣東 廣州510520；2.華南理工大學(xué) 機械與汽車工程學(xué)院，廣東 廣州510640）

0 引 言

同時定位與地圖構(gòu)建SLAM （simultaneous localization and mapping）技術(shù)是使移動機器人具有自主導(dǎo)航能力的關(guān)鍵技術(shù)。它確保移動機器人在未知環(huán)境中，從未知起點增量式地構(gòu)建環(huán)境地圖，同時利用環(huán)境地圖進行自我定位［1］。在視覺SLAM過程中，機器人利用配備的 “眼睛”觀測周邊自然環(huán)境特征，進行自身定位和構(gòu)建環(huán)境拓撲地圖［2］。此類地圖能有效地表達環(huán)境的拓撲結(jié)構(gòu)，對大范圍內(nèi)的移動機器人視覺導(dǎo)航尤為適用。

目前視覺SLAM一般是在貝葉斯框架下建立系統(tǒng)模型［3－5］，然后采用基于擴展卡爾曼濾波器或粒子濾波器對模型進行求解。研究者們致力于提升SLAM算法有效性和實時性能［6］，以及增強對感知混淆環(huán)境的適應(yīng)能力［7］。在視覺SLAM過程中，由于各種噪聲 （例如：傳感器測量誤差，機器人運動模型誤差等）的存在，機器人在執(zhí)行算法迭代過程中會不斷地積累誤差，導(dǎo)致SLAM收斂失敗。如何有效抑制各種噪聲和控制誤差的積累與傳播是視覺SLAM成功的保證。

錯誤的閉環(huán)探測是產(chǎn)生誤差積累的主要因素之一，它會降低地圖構(gòu)建的準(zhǔn)確率，因此機器人需要快速有效地進行閉環(huán)探測，即：判斷自己是否到過某些場景位置，以降低誤差積累概率。目前基于視覺傳感器的SLAM，閉環(huán)檢測主要分為概率計算方法［8］和圖像匹配方法［9］。此外，地圖節(jié)點生成方式關(guān)系著地圖構(gòu)建的質(zhì)量，它是視覺SLAM過程中另一個重要環(huán)節(jié)。

現(xiàn)有的視覺閉環(huán)探測方法存在：計算過程復(fù)雜繁瑣，效率欠佳等問題；而視覺地圖節(jié)點的探測方法沒能有效地控制地圖節(jié)點的冗余度，也進一步浪費了計算資源和存儲資源，制約了視覺導(dǎo)航系統(tǒng)的性能。針對目前閉環(huán)探測和地圖節(jié)點探測存在的問題，本文對此進行了改進，設(shè)計了一種新的視覺SLAM算法，它具有如下特點：①算法一方面從降低場景圖像相似性計算復(fù)雜度入手改進閉環(huán)探測效率，另一方面采用場景間共有信息量閾值來控制地圖節(jié)點的冗余度，在有效性、實時性和節(jié)約計算資源方面提升視覺定位與地圖構(gòu)建效果，力求算法簡潔實用；②采用具有二進制描述符的BRISK特征作為場景建模的局部特征點，有效地提高了計算效率，降低了存儲量；③基于Mini哈希進行場景模型的相似性計算，避免了繁雜的計算過程，整個過程快速有效。

1 改進的視覺SLAM算法

閉環(huán)探測和地圖節(jié)點生成這兩個環(huán)節(jié)密切影響著SLAM算法的性能。閉環(huán)探測錯誤會導(dǎo)致拓撲地圖構(gòu)建失敗，探測的實時性不佳也會制約SLAM的效率；地圖節(jié)點冗余度大，會制約閉環(huán)探測效率，而無效的地圖節(jié)點又進一步增加了閉環(huán)探測的失誤率。本節(jié)針對上述問題，對SLAM從閉環(huán)探測和地圖節(jié)點生成環(huán)節(jié)進行了改進。

1.1 SLAM算法框架

貝葉斯濾波器對 “離散時間非線性動態(tài)系統(tǒng)”的狀態(tài)變化具有較強的描述能力。SLAM過程具有明顯的離散時間非線性動態(tài)系統(tǒng)的特征，SLAM過程的貝葉斯模型如式（1），連續(xù)定位和構(gòu)圖過程分為2步：預(yù)測和更新

式中：p （zt｜ xt）——觀測模型，p （xt｜ ut，xt－1）——運動模型。雖然貝葉斯模型能較好地描述SLAM系統(tǒng)，但是直接求解比較困難。

本文采用粒子濾波器方法對SLAM過程的貝葉斯模型進行求解。機器人按照本文提出的地圖節(jié)點探測方法，連續(xù)地構(gòu)建拓撲節(jié)點。針對當(dāng)前最新探測的拓撲節(jié)點，機器人會檢測自己是否到過這個節(jié)點，這就是閉環(huán)探測過程。如圖1所示，根據(jù)閉環(huán)探測的結(jié)果算法選擇不同的地圖構(gòu)建方式：如果存在閉環(huán)，則更新現(xiàn)有節(jié)點的連接關(guān)系 （n－＞2）；如果不存在閉環(huán)，則新增拓撲節(jié)點 （n－＞n＋1）。

圖1 拓撲地圖構(gòu)建與閉環(huán)探測

SLAM算法流程，如圖2所示。閉環(huán)探測和地圖節(jié)點探測方法的具體過程在1.3和1.4小節(jié)詳述。

圖2 算法流程

1.2 場景的相似性測量方法

兩個場景的相似性測量是本文算法中閉環(huán)探測和地圖節(jié)點生成的基礎(chǔ)。本小節(jié)提出一種快速的場景相似性測量方法：首先提取圖像的BRISK特征，然后基于Minhash求解離散特征點集合的相似性，將圖像的相似性問題轉(zhuǎn)化為集合相似性問題。

1.2.1 BRISK特征點描述與場景圖像刻畫

算法采用BRISK特征［10］作為表征場景圖像的局部不變性特征點。首先在圖像金字塔內(nèi)的多個尺度上探測圖像的極值點作為有效的特征點P：位置 ｛x，y｝，尺度k。特征點P鄰域的像素點對形成的集合A被分為兩部分，如公式（2）。距離較遠的像素點對集合L用來計算灰度梯度方向，距離較近的像素點對集合S，其元素的灰度大小關(guān)系被用來生成該特征點的二進制描述符

算法采用二進制向量的集合S（p1，p2，p3…pn）來刻畫一幅場景圖像I（提取到n個特征點），集合元素與提取的特征點描述符相對應(yīng)。如圖3所示。

圖3 場景表示：特征向量

1.2.2 場景圖像的相似性測量

在上一節(jié)中圖像被刻畫為特征點集合，因此兩幅圖像的相似性測量被轉(zhuǎn)化為兩個集合的相似性計算。算法采用Minhash方法生成簽名矩陣計算集合的相似度。下面以四幅圖像為例來詳述計算過程：

四幅圖像分別對應(yīng)集合A （p1，p2，p3，p4）；B （p3，p4，p5，p6）；C （p3，p7）；D （p2，p6），其中集合元素pi是特征點描述向量。首先生成集合的特征矩陣 （如表1），然后利用哈希函數(shù)生成壓縮的簽名矩陣 （如表2），最后統(tǒng)計簽名矩陣中各列出現(xiàn)相同Minhash值的概率H。概率H便是兩個集合的相似系數(shù)，即：兩幅圖像的相似度S。

表1 特征矩陣

表2 簽名矩陣

1.3 閉環(huán)探測過程

閉環(huán)探測依賴于圖像的相似度測量。本文基于上一節(jié)提出的場景相似性測量方法，在粒子濾波器框架下，進行閉環(huán)探測。

算法采用60個基本粒子作為運動樣本，初始化時用粒子的均勻分布概率表示機器人最初的位置估計。每次重采樣后，粒子的當(dāng)前位置概率分布g，如公式 （3）。機器人根據(jù)當(dāng)前的觀測值估計自身的位置概率分布f，如式 （4）

閉環(huán)探測的位置與粒子最終的收斂位置相關(guān)，粒子的收斂過程如下：利用上一節(jié)提出的相似性測量方法，測量當(dāng)前觀測圖像與地圖節(jié)點中已有的圖像的相似度M。利于相似度M和預(yù)設(shè)的相似閾值A(chǔ)，估計機器人處于當(dāng)前位置的分布概率f。算法利用機器人上一次位置的后驗概率分布g和當(dāng)前位置的先驗概率分布f，估算每個粒子的權(quán)重w，如式 （5）

根據(jù)粒子權(quán)重進行重采樣，若粒子位置最終被收斂，則該位置即為閉環(huán)探測的位置；若粒子位置未收斂則表明無閉環(huán)現(xiàn)象發(fā)生。相似閾值A(chǔ)的設(shè)定在第2節(jié)進行。

1.4 地圖節(jié)點的生成

一幅好的拓撲地圖應(yīng)該以盡可能少的拓撲節(jié)點較完整地描述環(huán)境區(qū)域的拓撲結(jié)構(gòu)。拓撲節(jié)點過于密集會產(chǎn)生大量冗余的信息導(dǎo)致計算量倍增，過于稀疏又無法較好地反映環(huán)境拓撲結(jié)構(gòu)，因此地圖節(jié)點的生成方式對地圖質(zhì)量有較大的影響。機器人在視覺SLAM過程中，可能在某個地點或小范圍區(qū)域停留，若基于時間步長或距離步長生成拓撲節(jié)點可能會產(chǎn)生上述的冗余信息。

本文算法采用基于信息減量的方式生成拓撲節(jié)點。機器人捕獲場景圖像的頻率為25fps，為了保證計算效率，機器人每隔10幀取一幅圖像作為有效幀的關(guān)鍵幀進行測量，過程描述如下：算法采用文中提出的相似性測量方法，連續(xù)地計算候選關(guān)鍵幀與最新的節(jié)點之間的相似度，若相似度低于節(jié)點生成閾值B，則認為機器人已經(jīng)到達另外一個場景，然后進行閉環(huán)探測，如果不存在閉環(huán)現(xiàn)象則將該新場景作為拓撲節(jié)點加入；若存在閉環(huán)現(xiàn)象則直接對現(xiàn)有的節(jié)點連接關(guān)系進行升級。該探測方法既保證了關(guān)鍵幀節(jié)點對環(huán)境的覆蓋率又降低了信息冗余度，有利于提升SLAM算法性能。節(jié)點生成閾值B在第二節(jié)設(shè)定。

2 仿真實驗分析

本小節(jié)通過實驗確定閉環(huán)探測過程中粒子權(quán)重閾值A(chǔ)和地圖節(jié)點的生成閾值B，以滿足后續(xù)室外環(huán)境下移動機器人實驗的需要。

算法分析了機器人在室外采集的圖像數(shù)據(jù)集G （分辨率640x480）。首先將該數(shù)據(jù)集G中的80幅圖像分類為4個子集：不同場景集A1，相似場景集A2，較小近似場景集A3，相同場景集A4.利用1.2小節(jié)提出的相似性測量方法，分別對4個數(shù)據(jù)子集里的圖像進行兩兩測試，相似系數(shù)范圍：H （A1）∈ ｛S｜0＜＝S＜＝0.05｝；H （A2～3）∈ ｛S｜0.15＜＝S＜＝0.9｝；H （A4）∈ ｛S｜S＝1｝；測試結(jié)果部分數(shù)據(jù)，如圖4所示。根據(jù)實驗結(jié)果：粒子權(quán)重閾值A(chǔ)設(shè)為0.15，地圖節(jié)點探測閾值B設(shè)為0.05。

圖4 場景相似系數(shù)

實時性分析：1.2小節(jié)中圖像相似測量的時間消耗是影響閉環(huán)探測乃至整個SLAM算法計算效率的主要因素，為此文本在PC （Intel core （TM）2Duo CPU E7500，2.93GHz，1G內(nèi)存）上對該方法的實時性進行了測試，如圖5所示。與當(dāng)前經(jīng)典的BOW （Bag of Words）方法相比，該方法無需離線構(gòu)建單詞本，最快每秒可處理40幀圖像 （平均33幀），可以進行在線測量，因此它具有更高的計算效率。

圖5 每兩幅圖像相似測量時間

3 室外環(huán)境下移動機器人實驗

3.1 閉環(huán)探測的有效性實驗

機器人在室外校園環(huán)境中，沿著如圖6所示的路徑拍攝一組視頻 （593幀），隨后又在這段場景里隨機拍攝了10組圖像，每組包含5幅圖像作為機器人在不同位置的觀測值。下面以一組圖像為例，利用本文的閉環(huán)探測方法，在視頻流中對這5幅圖像進行相似性計算，將最相似的場景作為每幅圖像的閉環(huán)位置，然后檢查探測結(jié)果是否與主觀判斷結(jié)果相吻合，從而驗證閉環(huán)探測方法的有效性。

圖6 機器人移動路徑

圖7展示了對其中2個場景的閉環(huán)探測結(jié)果：左邊2幅圖像表示機器人到過的位置，針對這些位置，在視頻流里每隔10幀做一次閉環(huán)探測計算，根據(jù)粒子權(quán)重閾值A(chǔ)確定閉環(huán)位置 （右邊2幅場景）。上述兩對閉環(huán)位置的場景相似度分別為：0.21和0.17。通過人眼也可以看到：左右兩列場景圖像相似度很高。在593幀的圖像集里，其他閉環(huán)位置均被準(zhǔn)確地探測出來，結(jié)果與人眼判斷完全吻合。

圖7 閉環(huán)場景位置探測

3.2 視覺SLAM算法驗證

在室外環(huán)境下 （見圖8），采用先鋒輪式機器人平臺（Pioneer 3－DX）進一步驗證了SLAM算法的性能，包括以下2個指標(biāo)：視覺定位的準(zhǔn)確性及地圖節(jié)點的冗余度。驗證方法如下：讓機器人按兩條不同的路徑移動，先后采集圖像數(shù)據(jù)集 （見圖9），兩條路徑有一段重疊的部分以檢驗閉環(huán)探測效果。機器人在定位與構(gòu)圖過程中需要處理兩條路徑的共同部分才能正確構(gòu)建這一區(qū)域的拓撲地圖。

算法首先處理機器人沿路徑1行走所捕獲的視頻：按文中提出的減量式方法構(gòu)建拓撲節(jié)點并不斷進行閉環(huán)探測，機器人共構(gòu)建拓撲節(jié)點45個，閉環(huán)數(shù)目為0。隨后算法處理機器人按路徑2行走時捕獲的視頻：繼續(xù)執(zhí)行SLAM算法，每10幀做一次閉環(huán)探測，并對發(fā)生閉環(huán)的節(jié)點進行標(biāo)注，在未發(fā)生閉環(huán)的地方，升級拓撲地圖。閉環(huán)探測結(jié)果顯示的場景區(qū)域，如圖9（b）所示，與實際情況完全吻合。

此外，對本文的節(jié)點生成方法進行了驗證：將實驗生成的拓撲地圖與基于距離和基于時間步長生成的地圖從信息冗余度上進行了對比分析。機器人在同一個區(qū)域分別采集3段視頻流，并按3種方式生成地圖節(jié)點，然后統(tǒng)計地圖節(jié)點數(shù)目，并利用場景相似測量方法計算節(jié)點之間的信息冗余度，返回冗余節(jié)點數(shù)。結(jié)果表明 （見表4）：在確保對環(huán)境區(qū)域的有效覆蓋率前提下，基于時間步長的方式對機器人產(chǎn)生的冗余節(jié)點機率最高，距離步長次之，基于信息減量的方式產(chǎn)生的地圖節(jié)點冗余度最低。

表4 地圖節(jié)點生成方式對比

分析發(fā)現(xiàn)：冗余節(jié)點主要發(fā)生在機器人轉(zhuǎn)彎或停留的位置。由于基于時間和基于距離的方法分別只與時間閾值和距離閾值相關(guān)，它們不關(guān)注場景節(jié)點的區(qū)分度，所以當(dāng)機器人速度減慢或在原地停留、打轉(zhuǎn)的時候必然增加了節(jié)點冗余度發(fā)生的概率；而本文的節(jié)點生成方法本質(zhì)上是利用場景間的區(qū)分度來生成地圖節(jié)點，只有場景達到一定的可區(qū)分度，節(jié)點才會被作為候選對象加入到地圖中，因此，無論機器人以多少速度、沿著何種路徑構(gòu)建地圖，節(jié)點冗余度的發(fā)生概率都會維持在一個較低的水平。

4 結(jié)束語

SLAM算法是保證移動機器人自主導(dǎo)航的重要技術(shù)。視覺SLAM生成環(huán)境拓撲地圖供視覺導(dǎo)航使用，其中閉環(huán)探測關(guān)系著地圖生成的正確性，地圖節(jié)點的探測方法決定著地圖的質(zhì)量。針對視覺SLAM算法中的閉環(huán)探測效率實時性不高，計算過程繁瑣；地圖節(jié)點生成方法所產(chǎn)生的地圖信息量冗余度大等問題。本文對視覺SLAM算法進行了改進。首先提出了一種簡潔有效的場景相似性測量方法，該方法首次采用了BRISK作為場景圖像的局部特征點，并基于Minihash方法完成了場景的相似性計算，然后將該方法應(yīng)用于閉環(huán)探測和地圖節(jié)點的生成等關(guān)鍵環(huán)節(jié)，改善了視覺SLAM性能。

在后續(xù)的研究工作中，將進一步研究大范圍環(huán)境下閉環(huán)探測的效率以及動態(tài)環(huán)境下視覺SLAM算法的魯棒性等問題。

［1］Durrant Whyte H，Bailey T.Simultaneous localization and mapping：Part I［J］.IEEE Robotics ＆ Automation Magazine，2006，13 （2）：99－110.

［2］Ranganathan A，Dellaert F.Online probabilistic topological mapping ［J］.International Journal of Robotics Research，2011，30 （6）：755－771.

［3］LIU Yang，ZHANG Hong.Visual loop closure detection with a compact image descriptor ［C］／／International Conference on Intelligent Robots and Systems.Vilamoura－Algarve，Portugal：IEEE Press，2012：1051－1056.

［4］Angeli A.Visual topological SLAM and global localization［C］／／IEEE International Conference on Robotics and Automation.Kobe：IEEE Press，2009：4300－4305.

［5］Angeli A，Doncieux S.Incremental vision－based topological SLAM ［C］／／International Conference on Intelligent Robots and Systems.Nice：IEEE Press，2008：1031－1036.

［6］Davison A J，Reid I D.MonoSLAM：Real－time single camera SLAM ［J］.IEEE Transactions on Pattern Analysis and Machine Intelligence，2007 （29）：1052－1067.

［7］Cummins M，Newman P.Fab－map：Probabilistic localization and mapping in the space of appearance［J］.The International Journal of Robotics Research，2008 （27）：647－665.

［8］Cummins M M，Newman P.Probabilistic appearance based navigation and loop closing［C］／／IEEE International Conference on Robotics and Automation.Rome：IEEE Press，2007：2042－2048.

［9］Ho K L，Newman P.Detecting loop closure with scene sequences［J］.International Journal of Computer Vision，2007，74 （3）：261－286.

［10］Leutenegger S，Chli M.Binary robust invariant scalable keypoints［C］／／IEEE International Conference on Computer Vision.Barcelona：IEEE Press，2011：2548－2555.