秦國(guó)威, 孫新柱, 陳孟元

(安徽工程大學(xué)安徽省電氣傳動(dòng)與控制重點(diǎn)實(shí)驗(yàn)室, 安徽 蕪湖 241000)

引 言

同步定位與地圖構(gòu)建(Simultaneous Localization and Mapping, SLAM)是移動(dòng)機(jī)器人目前面臨的一項(xiàng)重大難題，此問(wèn)題的求解即是在一個(gè)未知環(huán)境中創(chuàng)建地圖的過(guò)程，要求機(jī)器人在一個(gè)陌生環(huán)境中首先要探索環(huán)境，從而了解環(huán)境，同步運(yùn)用地圖追蹤機(jī)器人在該環(huán)境中的位置[1-2]。然而，在實(shí)際的復(fù)雜場(chǎng)景下，解決SLAM鄰域的定位精度問(wèn)題存在一定的難度，因?yàn)橐苿?dòng)機(jī)器人實(shí)質(zhì)上就是移動(dòng)的傳感器平臺(tái)，傳感器雖然類型和能力各有不同，但是廣泛存在里程計(jì)漂移、不同的噪聲等問(wèn)題[3]。因此，SLAM過(guò)程中的定位精度得到了人們的廣泛關(guān)注。現(xiàn)行的室內(nèi)定位技術(shù)主要有WIFI定位技術(shù)、仿生模型定位技術(shù)等。

WIFI定位技術(shù)與其他定位技術(shù)相比具備價(jià)格低、易布設(shè)、范圍廣、受非視距影響小等眾多優(yōu)勢(shì)，但是考慮到WIFI定位技術(shù)易受到空間分辨率的限制，不能解決旋轉(zhuǎn)誤差累積問(wèn)題，也不能克服WIFI架構(gòu)的固有局限性，使得WIFI定位精確度受到很大的束縛。因此，相比較而言，基于嚙齒類動(dòng)物RatSLAM模型的仿生定位技術(shù)更為大眾所接受，此算法表現(xiàn)出完美的生物合理性和對(duì)自然環(huán)境的高度適應(yīng)性[4]，是一種純視覺的仿生SLAM算法。實(shí)驗(yàn)表明RatSLAM模型能夠滿足室內(nèi)靜態(tài)場(chǎng)景下的導(dǎo)航，目前廣泛應(yīng)用于定位導(dǎo)航等相關(guān)領(lǐng)域中[5-8]。然而，依靠純視覺的RatSLAM算法在光線變換等復(fù)雜環(huán)境下，其核心部分視覺里程計(jì)存在可靠性低、圖像匹配精度有限、環(huán)境適應(yīng)性較差等問(wèn)題[9-11]，因此RatSLAM模型仿生算法精度還有很高的提升空間。在文獻(xiàn)[12]中，Berkvens等人通過(guò)在RatSLAM模型中引入地磁定位的思想，目的在于提高定位的精度，但是由于電氣設(shè)備相互的影響，此方法并不能實(shí)現(xiàn)精準(zhǔn)定位。此外，對(duì)于常見的車載導(dǎo)航系統(tǒng)，由于單獨(dú)的RatSLAM算法無(wú)法修正里程計(jì)誤差累積，張瀟等[13]將光學(xué)雙軸傳感器和MIMU信息引入RatSLAM模型中，雖然精度得到些許的提升，但是隨之帶來(lái)系統(tǒng)的魯棒性變差，實(shí)時(shí)性降低等問(wèn)題。在文獻(xiàn)[14]中，針對(duì)原始的RatSLAM算法僅用一維向量描述視覺圖像的特征，及在視覺模板校正過(guò)程中易出現(xiàn)匹配效率很低和誤匹配的情況，提出將ORB算法與RatSLAM算法結(jié)合，用ORB算法提取RGB圖像的特征，完成特征描述子的匹配。

本文提出將ORB和SURF算法融合提取圖像的HSV特征進(jìn)行匹配，進(jìn)而完成視覺模板的校正，此融合算法不僅匹配的精度得到了明顯的提高，且匹配的效率、魯棒性皆顯著增強(qiáng)。

1 RatSLAM仿生SLAM算法

RatSLAM仿生SLAM算法的結(jié)構(gòu)框圖如圖1所示。其中，位姿細(xì)胞和經(jīng)歷圖在此算法中扮演著十分重要的角色。位姿細(xì)胞以(x,y,θ)的樣式排列，使得機(jī)器人能夠同時(shí)運(yùn)用x,y,θ表征其所在的位置和方向。位姿細(xì)胞的激活程度表示機(jī)器人在該位置和方向的可能性大小。視覺模板和視覺里程計(jì)信息分別修正和更新位姿細(xì)胞的活性，每一幅視覺模板是通過(guò)獲取局部場(chǎng)景的圖像信息特征得到。視覺里程計(jì)信息是通過(guò)對(duì)每幅圖像特定區(qū)域提取速度、角度等信息并經(jīng)過(guò)路徑積分加工，進(jìn)而實(shí)現(xiàn)位姿細(xì)胞的實(shí)時(shí)更新。視覺里程計(jì)的實(shí)時(shí)更新與視覺模板的修正機(jī)制共同作用能夠獲得機(jī)器人的空間經(jīng)歷圖。

圖1 RatSLAM模型結(jié)構(gòu)框圖

1.1 位姿表征

位姿細(xì)胞是一個(gè)三維結(jié)構(gòu)體，表示載體在三維空間里的位置和方向，機(jī)器人的位姿信息被編碼在一個(gè)名為位姿細(xì)胞中的單獨(dú)的競(jìng)爭(zhēng)型吸引子網(wǎng)絡(luò)(CAN)中。CAN模型是模擬頭部方向細(xì)胞和位置細(xì)胞的功能建立的模型，通過(guò)對(duì)位姿細(xì)胞激活程度進(jìn)行合理地刺激或抑制，達(dá)到始終控制位姿細(xì)胞內(nèi)部活動(dòng)的目的，保證位姿細(xì)胞激活程度的動(dòng)態(tài)穩(wěn)定。

可用一個(gè)三維離散高斯分布來(lái)創(chuàng)建刺激權(quán)重矩陣εdef：

(1)

因局部的刺激而使位姿感知細(xì)胞激活程度產(chǎn)生變化為：

(2)

其中：kxy和kθ分別為(x,y)平面和θ維的方差常數(shù)；d、e、f是x、y、θ的分布系數(shù)；Nx、Ny、Nθ是在(x,y,θ)空間中的位姿感知細(xì)胞三維矩陣元素。

對(duì)位姿細(xì)胞激活程度的抑制可表示為：

(3)

式中：δ是三維離散高斯分布的抑制權(quán)重矩陣。

(1) 路徑積分

路徑積分就是通過(guò)轉(zhuǎn)換每個(gè)位姿細(xì)胞基于里程計(jì)速度和(x,y,θ)坐標(biāo)的活動(dòng)來(lái)更新姿態(tài)細(xì)胞的活動(dòng)。在上述的三維競(jìng)爭(zhēng)型吸引子網(wǎng)絡(luò)中，不僅要保持穩(wěn)定的內(nèi)部活動(dòng)，還要具備良好的路徑積分特性，本文采用的路徑積分方法是轉(zhuǎn)換現(xiàn)有位姿細(xì)胞的活性而不是立即將當(dāng)前活性復(fù)制傳遞。此方法不僅能夠生成更加精確的機(jī)器人行走路線，并且中途不需要參數(shù)調(diào)整。在路徑積分之后，被更新的位姿感知細(xì)胞活性為：

(4)

式中：δx0,δy0,δθ0為在x,y,θ方向被四舍五入后的整數(shù)偏差，計(jì)算如下：

(5)

式中：v為機(jī)器人移動(dòng)速度，ω為旋轉(zhuǎn)角速度，θ為首選細(xì)胞方向，kx、ky、kθ為路徑積分常數(shù)。

(2) 視覺模板修正

在路徑積分過(guò)程中，由于外界因素的突變和阻礙，經(jīng)常會(huì)產(chǎn)生位置和方向上的累積誤差，這可以通過(guò)絕對(duì)差綜合模型(SAD)進(jìn)行修正。SAD模型將從局部場(chǎng)景中獲取的每個(gè)圖像與模板庫(kù)中的圖像進(jìn)行比較，與模板圖像充分相似的圖像被再次識(shí)別出來(lái)，同時(shí)有顯著差異的圖像就成為新的圖像模板并加入到模板庫(kù)中。對(duì)于一個(gè)新的視覺模板V，在模板庫(kù)中與模板之間建立聯(lián)系β：

(6)

(7)

將di與距離閾值d作比較：

(8)

如果距離di小于閾值d，當(dāng)前獲取的視覺場(chǎng)景作為新的視覺模板Vi保存在數(shù)據(jù)庫(kù)中，否則認(rèn)為是熟悉的模板，回到經(jīng)歷過(guò)的地方，由視覺模板引起的位姿細(xì)胞活動(dòng)的變化，即對(duì)位姿細(xì)胞激活程度的修正為：

(9)

1.2 經(jīng)歷圖

經(jīng)歷圖是由眾多的經(jīng)歷e構(gòu)成的，每個(gè)經(jīng)歷都具有一個(gè)活性水平，活性水平由位姿感知細(xì)胞的激活程度和視覺模板所決定。每個(gè)經(jīng)歷ei由經(jīng)歷活性水平Ei、位姿細(xì)胞激活程度Pi和視覺場(chǎng)景模板Vi構(gòu)成：

ei={Ei,Pi,Vi}

(10)

經(jīng)歷能級(jí)水平Ei按下式計(jì)算：

(11)

Exyθ=

式中：xpc、ypc和θpc為最大活性姿態(tài)細(xì)胞的坐標(biāo)；xi、yi、θi為與該經(jīng)歷相關(guān)的位姿感知細(xì)胞的坐標(biāo)；ra為(x,y)平面的區(qū)域常數(shù)；θa為θ維上的區(qū)域常數(shù)；Vcurr為當(dāng)前場(chǎng)景；Vi為與經(jīng)歷i相關(guān)的場(chǎng)景。

2 改進(jìn)型RatSLAM仿生SLAM算法

原始RatSLAM模型中采用絕對(duì)差總和(SAD)模型進(jìn)行RGB圖像視覺模板的校正，考慮到RGB圖像與人類的視覺感知差別較大，而且進(jìn)行視覺模板匹配的魯棒性較差，匹配的效率有待提高。本文使用H(色度)、S(飽和度)、V(亮度)表示圖像的內(nèi)容，首先利用HSV圖像特征進(jìn)行全局特征匹配，得到粗匹配圖像；再利用SURF與ORB融合算法進(jìn)一步進(jìn)行局部特征匹配，進(jìn)而得到更好的匹配效果。圖像匹配流程如圖2所示。

圖2 圖像匹配流程圖

2.1 全局特征匹配

全局特征匹配首先運(yùn)用顏色矩表示圖像的顏色特征，再利用漢明距離完成特征模板顏色匹配。將圖像中的顏色分布用矩表示即構(gòu)成顏色矩，其中，一階矩表示顏色平均值，二階矩表示顏色方差，三階矩表示顏色的偏斜度。利用顏色矩的表示方法具有無(wú)需量化描述特征和顏色向量維數(shù)低的優(yōu)勢(shì)。3個(gè)顏色矩的數(shù)學(xué)表示為：

(13)

(14)

(15)

式中：ui、σi、si分別表示顏色分量i的均值、方差和斜度，N表示圖像的總像素?cái)?shù)，pi,j表示圖像中第j個(gè)像素的第i個(gè)顏色分量。圖像的3個(gè)顏色分量的3階顏色矩組成一個(gè)9維向量，則圖像的顏色特征表示為：

Fcolor=[uH,σH,sH,uS,σS,sS,uL,σL,sL]

(16)

由移動(dòng)機(jī)器人獲取圖像，輸出最相似和次相似的兩幅模板圖像。顏色矩向量生成后，利用歐式距離計(jì)算當(dāng)前圖像與模板兩幅圖像的相似度。歐式距離計(jì)算公式為：

(17)

式中：CI(k)和CJ(k)分別表示圖模板庫(kù)中圖像I和當(dāng)前圖像J的顏色矩分量。當(dāng)歐式距離D(I,J)在既定的閾值內(nèi)，則匹配成功；反之，則匹配失敗。

2.2 局部特征匹配

全局特征匹配之后得到的僅為粗匹配圖像，因此，仍需進(jìn)一步驗(yàn)證局部特征匹配情況，在此采用SURF與ORB融合算法來(lái)完成。

STEP1：積分圖像。積分圖像的概念是由Viola和Jones提出的。積分圖像中任意一點(diǎn)(i,j)的值為原圖像左上角到任意點(diǎn)(i,j)相應(yīng)的對(duì)焦區(qū)域的灰度值的總和，任取一幅灰度原始圖像G，其積分圖像為GΣ(x,y)，二者關(guān)系如下：

(18)

STEP2：Hession矩陣探測(cè)。為避免從圖像中獲取的特征點(diǎn)是重復(fù)的，SURF算法通過(guò)將高斯微分模板和圖像的卷積轉(zhuǎn)化為方框?yàn)V波，進(jìn)而利用不同尺寸大小的方框?yàn)V波器對(duì)待處理圖像進(jìn)行濾波處理，構(gòu)成圖像金字塔，之后運(yùn)用Hession矩陣探測(cè)器對(duì)金字塔每層的特征點(diǎn)進(jìn)行極值點(diǎn)檢測(cè)。Hession矩陣就是利用二階微分來(lái)進(jìn)行斑點(diǎn)檢測(cè)，給定圖像G中一個(gè)點(diǎn)(x,y)，在(x,y)處，尺度為σ的Hession矩陣H(x,σ)定義為：

(19)

式中：Lxx(x,σ)是高斯二階微分?2g(σ)/?(x2)在點(diǎn)x處與圖像G的卷積，Lxy(x,σ)與Lyy(x,σ)的意義非常相似。

對(duì)Hession矩陣的行列式進(jìn)行簡(jiǎn)化，近似可得：

|H(x,σ)|=DxxDyy-0.9Dxy

(20)

式中：Dxx、Dyy和Dxy分別是Lxx、Lyy和Lxy的近似取值。

STEP3：選取特征點(diǎn)。利用上述方法獲取圖像相關(guān)區(qū)域的極值后，實(shí)現(xiàn)以此極值點(diǎn)為中心的3×3×3立體區(qū)域內(nèi)的非極大值抑制。若某一點(diǎn)的極值相對(duì)于同層與上下兩層所有像素點(diǎn)的極值是最大或者最小的，則稱此極值點(diǎn)成為候選特征點(diǎn)，之后分別在圖像和相應(yīng)的尺度空間中進(jìn)行一系列的插值運(yùn)算以最終確定特征點(diǎn)，以及特征點(diǎn)在圖像中的區(qū)域分布。

STEP4：特征點(diǎn)描述子。得到特征點(diǎn)后需要以某種方式描述這些特征點(diǎn)的屬性，這些屬性的輸出稱之為該特征點(diǎn)的描述子，在此使用具有旋轉(zhuǎn)不變性的BRIEF即rBRIEF。它的思想為在特征點(diǎn)周圍以一定模式遴選出指定數(shù)量的點(diǎn)對(duì)，把這些點(diǎn)對(duì)的比較結(jié)果組合起來(lái)作為描述子。

(1) 確定特征點(diǎn)方向。為了使算法具有旋轉(zhuǎn)不變性，需要提前為每個(gè)特征點(diǎn)分配方向。為此，以特征點(diǎn)為中心，也即為坐標(biāo)系的坐標(biāo)原點(diǎn)，在其周圍鄰域S內(nèi)利用積分計(jì)算質(zhì)心位置。以特征點(diǎn)為起點(diǎn)、質(zhì)心為終點(diǎn)構(gòu)造出的向量作為特征點(diǎn)的方向。計(jì)算過(guò)程如下：

(21)

(22)

(23)

(2) rBRIEF為一種二進(jìn)制碼串形式的描述子，在特征點(diǎn)周圍的鄰域內(nèi)選取的點(diǎn)對(duì)可以用灰度值對(duì)比來(lái)表達(dá)。定義操作準(zhǔn)則T為：

(24)

式中：p(x)為鄰域內(nèi)x處的灰度函數(shù)。

(3)當(dāng)在鄰域內(nèi)選擇m個(gè)(x,y)點(diǎn)對(duì)時(shí)，描述子就是一個(gè)包含m個(gè)二進(jìn)制準(zhǔn)則的碼串，如下式所示：

(25)

(4) 將第(1)步計(jì)算出的特征點(diǎn)方向信息添加至描述子中，使描述子包含有特征點(diǎn)的方向信息，在點(diǎn)對(duì)位置(x,y)處，由下式定義一個(gè)2×m的矩陣：

(26)

式中：每個(gè)(xi,yi)表示一個(gè)測(cè)試點(diǎn)對(duì)，利用特征點(diǎn)方向θ所對(duì)應(yīng)的旋轉(zhuǎn)矩陣Rθ，構(gòu)造的特征點(diǎn)對(duì)矩陣為Qθ=RθQ,這樣就可以得到具備旋轉(zhuǎn)不變性的描述子：

gm(p,θ)=fm(p)|(xi,yi)∈Qθ

(27)

(5) 采用貪婪搜索的方法，找出256個(gè)像素塊對(duì)，使其相關(guān)性最低，并構(gòu)成所需的描述子。

STEP5：特征點(diǎn)匹配。利用融合算法快速提取兩幅圖像的特征點(diǎn)和特征描述子，運(yùn)用特征描述子描述的圖片之間的相似度判斷兩個(gè)關(guān)鍵點(diǎn)之間是否匹配。

3 實(shí)驗(yàn)及分析

為了驗(yàn)證本文算法的可行性與正確性，在基于Windows7操作系統(tǒng)、處理器為i3-3240、運(yùn)行內(nèi)存為3 GHz的PC機(jī)上進(jìn)行多次仿真實(shí)驗(yàn)。實(shí)驗(yàn)場(chǎng)景為一9 m×6 m的會(huì)議室，真實(shí)環(huán)境如圖3所示，平面結(jié)構(gòu)如圖4所示。實(shí)驗(yàn)中運(yùn)用輪式移動(dòng)機(jī)器人，如圖5所示。通過(guò)機(jī)器人上裝有的攝像頭采集場(chǎng)景信息，并將其通過(guò)無(wú)線傳輸給上位機(jī)，并用MATLAB2015a進(jìn)行仿真實(shí)驗(yàn)。

圖3 會(huì)議室環(huán)境

圖4 會(huì)議室平面圖

圖5 移動(dòng)機(jī)器人

3.1 經(jīng)歷節(jié)點(diǎn)模板匹配圖

RatSLAM模型經(jīng)歷節(jié)點(diǎn)模板匹配圖如圖6所示。在圖6中，一個(gè)經(jīng)歷就是經(jīng)歷圖中的一個(gè)節(jié)點(diǎn)，由基于傳感器和位姿數(shù)據(jù)的位姿細(xì)胞產(chǎn)生。當(dāng)前的視覺場(chǎng)景與儲(chǔ)存的場(chǎng)景模板足夠地相似時(shí)，就發(fā)生了一次模板匹配，每個(gè)點(diǎn)表示在此時(shí)創(chuàng)建或匹配了一個(gè)經(jīng)歷節(jié)點(diǎn)。位姿細(xì)胞更新以后，進(jìn)一步進(jìn)行位姿細(xì)胞活性的修正，完成經(jīng)歷的匹配。

實(shí)驗(yàn)進(jìn)行三次，分別在260 s、520 s走完了場(chǎng)景的第一圈和第二圈。起點(diǎn)和終點(diǎn)為同一點(diǎn)的位置A，在模板匹配圖中已進(jìn)行了標(biāo)注，走完場(chǎng)景一圈時(shí)，將其定位到初始時(shí)刻點(diǎn)的位置，重新跟隨先前看到的場(chǎng)景進(jìn)行圖像信息匹配。從圖6可以看出，當(dāng)出現(xiàn)熟悉模板時(shí)，圖6(b)幾乎可以準(zhǔn)確無(wú)誤地與模板進(jìn)行匹配，誤匹配變少，匹配的精度明顯更高。

圖6 經(jīng)歷節(jié)點(diǎn)匹配圖

3.2 經(jīng)歷圖對(duì)比

原始RatSLAM與改進(jìn)型RatSLAM產(chǎn)生的經(jīng)歷圖如圖7所示。對(duì)比兩種算法的經(jīng)歷圖可以看出，運(yùn)用新穎圖像匹配算法改進(jìn)的RatSLAM算法對(duì)于復(fù)雜環(huán)境適應(yīng)性明顯更強(qiáng)，魯棒性更強(qiáng)。

圖7 經(jīng)歷圖

3.3 定位誤差的比較

為了直觀比較RatSLAM改進(jìn)前后定位的誤差因素，選擇25個(gè)測(cè)試點(diǎn)測(cè)試其性能。本文用均方根誤差作為標(biāo)準(zhǔn)，假設(shè)待定位點(diǎn)的實(shí)際位置為(xa,ya)，估計(jì)位置為(xe,ye)，則實(shí)際位置與測(cè)量位置的均方根誤差e表示為：

(28)

兩種算法的誤差累計(jì)分布圖(CDF)如圖8所示，其在定位累計(jì)誤差概率為30%、60%、90%時(shí)所對(duì)應(yīng)的定位精度見表1。從表1可知：在定位累計(jì)誤差概率分別為30%、60%、90%時(shí)，相較于原始的RatSLAM算法，改進(jìn)型算法的精度分別提升了7.5%、23.9%、39.8%，說(shuō)明改進(jìn)后的匹配算法精度明顯高于原始的匹配算法。

圖8 定位結(jié)果誤差累計(jì)分布圖

表1 不同定位誤差概率精度對(duì)比

4 結(jié)束語(yǔ)

本文系統(tǒng)研究了基于HSV圖像匹配的改進(jìn)型RatSLAM算法，采用一種新穎的圖像匹配算法，替代原有的絕對(duì)差總和(SAD)匹配方法，更好地完成了視覺模板的匹配，匹配的準(zhǔn)確率顯著增強(qiáng)，最終產(chǎn)生更為準(zhǔn)確的經(jīng)歷圖。既改進(jìn)了原有RatSLAM對(duì)于室內(nèi)復(fù)雜環(huán)境適應(yīng)性差、視覺里程計(jì)可靠性低的問(wèn)題，又使得系統(tǒng)的魯棒性變強(qiáng)，定位的性能也得到了很大的改善。但是本文對(duì)于算法實(shí)時(shí)性的作用并無(wú)太大改觀，這是接下來(lái)要重點(diǎn)研究的方向。