殷新凱，茅 健，周玉鳳，陳曉平

（1. 上海工程技術(shù)大學(xué) 機(jī)械與汽車工程學(xué)院，上海 松江 201620；2. 寧波工程學(xué)院 械學(xué)院，浙江 寧波 315016）

0 引言

SLAM是同步定位與地圖構(gòu)建，是指攜帶傳感器的移動設(shè)備，在部分未知或全部未知的環(huán)境中，在運(yùn)動過程中建立環(huán)境的模型，同時估計自身運(yùn)動[1]。一般有視覺SLAM和激光SLAM兩種，激光SLAM技術(shù)已經(jīng)相對成熟，廣泛應(yīng)用到移動式機(jī)器人中，而視覺SLAM大多還在理論研究階段。本文利用視覺SLAM技術(shù)，研究排爆機(jī)器人自主導(dǎo)航定位與路徑規(guī)劃可行性，對視覺SLAM前端視覺里程計中特征匹配進(jìn)行優(yōu)化，為排爆機(jī)器人完成作業(yè)任務(wù)奠定基礎(chǔ)。

1 視覺SLAM

SLAM問題是為了同時解決移動設(shè)備的兩個問題：一是確定自身所處位置；二是了解自身所在環(huán)境的信息。SLAM問題實質(zhì)上就是一個估計狀態(tài)問題，移動設(shè)備通過自身傳感器獲得位置信息，估計自身位置和地圖環(huán)境。選擇相機(jī)作為機(jī)器人的主要傳感器，運(yùn)用SLAM的方法，就是視覺SLAM。我們往往用以下數(shù)學(xué)模型來視覺SLAM的過程，在一段時間 (t0～tn)內(nèi)，機(jī)器人自身位置用x1,x2,… ,xn表示，設(shè)定環(huán)境中存在一些路標(biāo)，記做 y1, y2,… ,ym，那么機(jī)器人通過自身傳感器獲得位置信息就被分成兩步，一是運(yùn)動，主要通過機(jī)器人自身位置變化表示機(jī)器人運(yùn)動；二是觀測，通過機(jī)器人感知路標(biāo)來表示機(jī)器人觀測。我們可以用數(shù)學(xué)模型表示為：

上式中，nu是機(jī)器人傳感器采集的數(shù)據(jù)，nw是噪聲，方程f是運(yùn)動方程，表示第一步。第二步的方程表示為：

式中jy表示機(jī)器在nx處觀測到的路標(biāo)，,njv 是觀測中的噪聲，方程h是觀測方程。上述兩個式中,,x y z都可根據(jù)自身攜帶的不同傳感器來用具體的參數(shù)信息來表示，例如常用的平面坐標(biāo)(,)x y，轉(zhuǎn)角θ等，視覺SLAM一般都是通過相機(jī)來觀測。我們可以將視覺SLAM總結(jié)為：

式（3）是視覺 SLAM 的狀態(tài)估計方程，求解這一方程之前的工作就是前段視覺里程計的內(nèi)容，之后就是后端優(yōu)化的過程。

狀態(tài)估計方程的求解，一般按照運(yùn)動和觀測方程是否為線性，噪聲是否服從高斯分布分為線性與非線性和高斯與非高斯系統(tǒng)[3]。線性高斯系統(tǒng)是比較簡單的狀態(tài)估計問題，此類問題時比較理想的情況，可以由卡爾曼濾波器求出，而非線性非高斯系統(tǒng)是實際情況中出現(xiàn)較多的情況，此類問題一般由擴(kuò)展卡爾曼濾波和非線性優(yōu)化去求出。視覺SLAM成熟的今天，我們更傾向用圖優(yōu)化方法進(jìn)行狀態(tài)估計[2]。

在明確狀態(tài)估計方程的求解后，視覺SLAM需要考慮的就是回環(huán)檢測，其目的是解決位置估計隨時間漂移產(chǎn)生的誤差問題。

視覺SLAM的整個過程可以概括為傳感器信息讀取、視覺里程計、后端優(yōu)化、回環(huán)檢測、建立地圖五步。其中視覺里程計又稱為前端，是狀態(tài)估計的過程，而后端優(yōu)化是狀態(tài)估計的求解和優(yōu)化過程。本文主要對前端里程計中的特征提取和匹配算法進(jìn)行優(yōu)化，提高匹配精確率，更好的匹配排爆機(jī)器人的導(dǎo)航定位過程，視覺SLAM框架如圖1所示。

視覺里程計目前分為特征點法和光流法，兩者的區(qū)別在于是否需要提前特征[3]?；谔卣鼽c法的前端視覺里程計，運(yùn)行穩(wěn)定，識別能力較強(qiáng)，是主流的里程計實現(xiàn)方法。

圖1 經(jīng)典視覺SLAM框架Fig.1 Classic visual SLAM framework

2 特征點法

特征點是指相機(jī)在運(yùn)動過程中，某一區(qū)域中，能反應(yīng)自身特點與其他區(qū)域區(qū)別開，而且運(yùn)動過程中未發(fā)生明顯化的點或者區(qū)塊。

特征點法的兩幀之間里程計研究過程包括[4]：（1）通過相機(jī)所采集的圖像，對圖像中特征點進(jìn)行提??；（2）對提取好的特征點進(jìn)行處理，完成特征匹配；（3）估計相機(jī)兩幀之間的相機(jī)運(yùn)動和場景構(gòu)建。

特征點分為關(guān)鍵點和描述子兩部分。關(guān)鍵點是圖像中能表示圖像局部特征的點，描述子是描述關(guān)鍵點附近像素信息的參數(shù)。當(dāng)兩幀圖像中，某一處特征點的關(guān)鍵點和描述子相似時，那么這兩個特征點相似，特征點法要做的就是找出所有圖像中的關(guān)鍵點，計算相應(yīng)的描述子，進(jìn)行特征匹配，進(jìn)而估計相機(jī)運(yùn)動和場景構(gòu)建。常見的關(guān)鍵點提取算法有Harris角點、FAST角點等。

2.1 FAST角點法

FAST角點是E.Rosten和T.Drummond在2006年提出[5]，是一種快速的角點提取算法。其檢測過程如圖2所示。FAST角點是根據(jù)圖像像素亮度大小來檢測角點，在左圖中選擇一像素P，亮度為PI，在其周圍選擇半徑為3的圓，將圓周上的區(qū)域標(biāo)記為1～16，設(shè)定閾值T，若圓周上存在連續(xù)大于設(shè)定值N個點的亮度滿足：

圖2 FAST角點檢測示意圖Fig.2 FAST corner detection diagram

那么該點就是FAST角點，N的取值一般有9、11、12三種，ORB算法的關(guān)鍵點檢測技術(shù)是利用FAST角點完成。

2.2 Harris角點

Harris角點是C Harris在1988年提出[6]，通過描述滑動窗口圖像像素灰度變化，可表示為：

式中 E ( u, v)是誤差函數(shù)， w ( x, y)是窗函數(shù)，一般取高斯函數(shù)， I ( x, y)和表示窗口平移前后的灰度變化值。將進(jìn)行泰勒展開得：

將式（6）化簡得：

取M表示：

用二級矩陣M的兩個特征值12,λ λ來判斷是否是角點。若特征值12,λ λ值都較大，則該點是角點；若特征值12,λ λ都較小，則該點附近都較為平坦；若特征值12,λ λ一大一小，則該點為邊緣點。給定閾值C大小，判斷角點。

其中，D e t( M )是矩陣M的行列式，T r ace( M )是矩陣M的跡，系數(shù)k一般取0.04～0.06。

在完成角點檢測后，需要對角點進(jìn)行描述子表示，構(gòu)建出誤差較小的特征向量。常見的描述子有兩類：一類是基于梯度直方圖；一類是基于二進(jìn)制位串。SIFT算法中的SIFT描述子和BRIEF描述子分別對應(yīng)兩類。

2.3 SIFT描述子

SIFT描述子是在SIFT關(guān)鍵點檢測后，對這些關(guān)鍵點局部特征用梯度表示[7]。首先計算DOG空間圖像 (,)L x y內(nèi)各點的梯度幅值m和方向θ：

用直方圖統(tǒng)計領(lǐng)域像素的梯度方向，接著構(gòu)造關(guān)鍵點特征向量圖，如圖3所示：圖a所示區(qū)域中，每個小格代表關(guān)鍵點所在尺度空間的一個像素，每個向量代表關(guān)鍵點附近的像素點(i, j)的梯度幅值mi,j和方向θi,j的高斯加權(quán)值，向量長度為G(σ,i, j)*mi,j，方向為θi,j。圖a中藍(lán)色圓為高斯加權(quán)區(qū)域，黑色圓點為關(guān)鍵點，圖b中4× 4窗口中都含有8個方向的向量，將所有的矢量結(jié)合起來構(gòu)成最終的SIFT描述子。

圖3 關(guān)鍵點特征向量圖Fig.3 Key point feature vector diagram

2.4 BRIEF描述子

BRIEF描述子是M. Calonder在2010年提出[9]，是一種基于二進(jìn)制位串的描述子方法。其原理是對圖像進(jìn)行分塊，比較灰度值，將比較結(jié)果進(jìn)行 0/1分類。其步驟如下：

（1）對采集到的圖像進(jìn)行預(yù)處理，使用方差為σ，窗口為qq×的高斯濾波；

（2）在圖像上選取大小為SS×的矩形區(qū)域 ，在P上進(jìn)行BRIEF描述過程；

（3）定義測試τ，其表達(dá)式為：

式中(,)x y是區(qū)域P內(nèi)的一對像素點， (,)I p x、(,)I p y是其對應(yīng)的灰度值，τ的輸出值為0或1；

（4）一個BRIEF描述是由無數(shù)個τ組成的二進(jìn)制比特串，將像素點排序后進(jìn)行τ測試，代入公式：

式中，nd對像素點(x, y)是有序的，經(jīng)實驗驗證，采用 nd= 1 28，方差的高斯分布時，BRIEF描述子的特征向量最優(yōu)，在特征提取和之后的特征匹配過程，此順序是不變的。

2.5 ORB算法

ORB特征算法則是將FAST角點法和BRIEF描述子結(jié)合得到的。針對邊緣反應(yīng)這一問題，計算了Harris響應(yīng)值，選取了前N個具有最大響應(yīng)值的角點作為角點集合；針對不具有方向性這一問題，引入灰度質(zhì)心法，質(zhì)心C是將圖像灰度值作為權(quán)重的中心，步驟如下[10]：

（1）在圖像B中，定義圖像塊的矩：

（2）通過矩可以找到圖像塊的質(zhì)心：

（3）連接圖像→塊的幾何中心O與質(zhì)心C，得到一個方向向量特征點的方向定義為：

2.6 優(yōu)化ORB算法

本文將FAST角點法與Harris角點法進(jìn)一步結(jié)合，同時引入SIFT算法關(guān)鍵點檢測，然后與BRIEF描述子結(jié)合，用暴力匹配算法完成不同圖像的匹配。FAST角點法提取出來的角點往往分布集中，忽視了其他區(qū)域出現(xiàn)關(guān)鍵點概率，因此引入Harris角點，然后考慮 FAST角點計算迅速，但是無法保證尺度不變性，用SIFT檢測中的高斯金字塔構(gòu)建尺度不變空間，結(jié)合BRIEF描述子，完成算法的優(yōu)化，優(yōu)化后的算法更加適合排爆機(jī)器人對算法精度的高要求的同時，減小了對自身所需CPU的高要求，減小了機(jī)器人自身負(fù)載，保障了危險物處理工作的有效解決。算法的具體流程如下：

（1）如上式（4）所示，設(shè)定閾值 對圖像進(jìn)行FAST-12角點檢測，得到總數(shù)為N的初始點集合0P;

（2）對于集合0P中的每一點(,)x y，判斷在該點附近半徑為3的圓周內(nèi)是否存在Harris響應(yīng)值大于該點，提出后保留剩余點集1P；

（3）隨后引入 SIFT關(guān)鍵點檢測方法，根據(jù)剩余點集1P，建立圖像金字塔，由式（16）、（17）確定1P內(nèi)所有點對應(yīng)的質(zhì)心和方向；

（4）用優(yōu)化后的BRIEF描述算法，在上一步的基礎(chǔ)上，為了保證描述子的方向不變性，用2dn×的矩陣表示確定方向的點集1P為：

本文取 nd= 1 28。根據(jù)方向角θ，確定旋轉(zhuǎn)矩陣：

將點集 P2與Rθ相乘，有：

則有描述子：

產(chǎn)生的描述子既保留了關(guān)鍵點的方向不變性與少邊緣響應(yīng)，同時有ORB算法中rBRIEF算法對噪聲的抑制；

（5）用漢明距離進(jìn)行匹配，特征匹配方法和仿真結(jié)果將在下一節(jié)詳細(xì)描述。

3 特征匹配

特征匹配是視覺里程計模塊中重要的一步，在特征點法的基礎(chǔ)上，通過圖像與圖像之間的描述子進(jìn)行準(zhǔn)確匹配，為后續(xù)后端優(yōu)化鋪墊。特征匹配的算法已經(jīng)成熟，常用的有暴力匹配與快速近似最近鄰算法[11]。

4 仿真分析

4.1 仿真環(huán)境

本文所選的場景圖是實驗環(huán)境，如圖4所示。

圖4 實驗環(huán)境圖Fig.4 Experimental environment diagram

仿真用的兩圖如圖5所示。

4.2 仿真過程

分布用SIFT算法、ORB算法與優(yōu)化ORB算法對實驗圖進(jìn)行特征匹配，對比圖如圖6所示。其中，圖(a)、(b)、(c)對應(yīng)著SIFT算法、ORB算法與優(yōu)化后的ORB算法的結(jié)果圖，比較三個算法圖，結(jié)果如表1所示，SIFT算法耗1.63 s，ORB算法耗時1.14 s，優(yōu)化后的ORB算法耗時0.93 s。

圖5 仿真實驗圖Fig.5 Simulation experiment diagram

圖6 特征匹配算法匹配結(jié)果圖Fig.6 Feature matching algorithm matching result graphs

表1 特征匹配算法對比Tab.1 Feature matching algorithm comparison

由表1可知，SIFT算法可提取較多的特征點，ORB算法的特征點雖然少，但是匹配率更高，同時SIFT算法耗時相對較多。由圖6(a)可知，SIFT算法存在許多錯誤，檢測到的特征點不能大概率地匹配上，而對ORB算法來說，由圖6(b)、(c)及表1可以看出，優(yōu)化后的ORB算法在匹配率和耗時上都得到了一定優(yōu)化，為排爆機(jī)器人下一步的路徑規(guī)劃做鋪墊，保證了機(jī)器人行走的安全性。

5 總結(jié)

本文結(jié)合 FAST角點法、Harris角點法、SIFT關(guān)鍵點檢測、BRIEF描述子，對ORB算法進(jìn)行優(yōu)化，以便得到適用于排爆機(jī)器人的視覺里程計特征匹配方法。通過仿真，驗證了算法的可行性，為下一步排爆機(jī)器人路徑規(guī)劃奠定了基礎(chǔ)。