唐 浩, 王紅旗, 茹淑慧

(河南理工大學(xué) 電氣工程與自動(dòng)化學(xué)院,河南 焦作 454000)

0 引 言

隨著移動(dòng)機(jī)器人的快速發(fā)展,移動(dòng)機(jī)器人需要完成的任務(wù)多種多樣,例如物體抓取、空間探索、農(nóng)業(yè)植保、搜索救援等,這些任務(wù)對(duì)移動(dòng)機(jī)器人的定位提出了更高要求[1]。對(duì)于距離傳感器,激光傳感器價(jià)格昂貴,應(yīng)用門(mén)檻太高,超聲波傳感器較大的誤差也限制其應(yīng)用；輪式里程計(jì)結(jié)合慣性測(cè)量單元(inertial measurement unit,IMU)的應(yīng)用得到了一定的推廣,但輪式里程計(jì)需要構(gòu)建準(zhǔn)確的車(chē)輪數(shù)學(xué)模型,存在遠(yuǎn)距離運(yùn)動(dòng)后誤差累計(jì)的問(wèn)題。視覺(jué)里程計(jì)僅僅利用攝像機(jī)采集到的圖片來(lái)估計(jì)出相對(duì)運(yùn)動(dòng),提供了精確的軌跡估計(jì),相對(duì)位置誤差范圍為0.1 %～2 %[2],能夠在信號(hào)遮擋,GPS導(dǎo)航系統(tǒng)不能用的地方如水下,室內(nèi)等進(jìn)行位姿定位。單目視覺(jué)里程計(jì)與雙目視覺(jué)里程計(jì)相比,因?yàn)閮H僅使用了一個(gè)攝像頭,成本更低,圖像處理速度更快[3],并且在現(xiàn)有的硬件基礎(chǔ)上也更容易配置,因此單目視覺(jué)里程計(jì)具有更高的應(yīng)用前景[4]。Nister D等人[5]對(duì)單目視覺(jué)里程計(jì)進(jìn)行了早期研究,采用Harris算法提取特征點(diǎn),結(jié)合5點(diǎn)和隨機(jī)抽樣一致性算法來(lái)進(jìn)行運(yùn)動(dòng)估計(jì),該方法對(duì)圖形的旋轉(zhuǎn)、縮放等變換適應(yīng)性較差。Pretto A[6]采用尺度不變特征變換(scale invariant feature transform,SIFT)算法提取特征點(diǎn),結(jié)合5點(diǎn)算法完成運(yùn)動(dòng)估計(jì),采用SIFT算法對(duì)于圖像的平移、旋轉(zhuǎn)等有較好的適應(yīng)性。文獻(xiàn)[7] 基于平面假設(shè),將光流跟蹤與特征點(diǎn)匹配進(jìn)行融合,通過(guò)卡爾曼濾波更新機(jī)器人的位置和姿態(tài),該方法提高了定位精度,采用SURF算法對(duì)圖像的平移、旋轉(zhuǎn)、縮放等有較好的魯棒性,但SURF算法計(jì)算量比較大。經(jīng)過(guò)改進(jìn)的定向二進(jìn)制(orientated binary,ORB)算法計(jì)算速度快、精度高,可以與光流法進(jìn)行融合,提高視覺(jué)里程計(jì)的性能。

1 光流法與改進(jìn)ORB融合方法

1.1 基于光流的運(yùn)動(dòng)估計(jì)

1.1.1 基于FAST算子角點(diǎn)檢測(cè)

在圖像中以像素點(diǎn)p為圓心,以3個(gè)像素單位為半徑畫(huà)圓,通過(guò)求出p點(diǎn)灰度值與該圓周上各像素點(diǎn)灰度值的差值,并統(tǒng)計(jì)該差值的絕對(duì)值大于某個(gè)閾值εd的點(diǎn)的個(gè)數(shù),以此判斷p點(diǎn)是否為角點(diǎn),則角點(diǎn)響應(yīng)函數(shù)N為

(1)

式中I(i)為p點(diǎn)的圓周上第i個(gè)像素點(diǎn)的灰度值,對(duì)于給定的閾值N0(這里取N0為9),當(dāng)N>N0時(shí),認(rèn)為p點(diǎn)為角點(diǎn)[8]。

1.1.2 基于LK法光流跟蹤

LK光流法基于三個(gè)假設(shè):圖像光照變化不大；目標(biāo)在幀間的位移比較??；局部小鄰域內(nèi)像素點(diǎn)運(yùn)動(dòng)參數(shù)相同,依照前2個(gè)假設(shè)建立光流方程[9]

Ixu+Iyv+It=0

(2)

式中I為整幅圖像,Ix,Iy和It分別為圖像對(duì)x,y,t方向的導(dǎo)數(shù),u,v分別為x,y方向的速度[10],利用超定方程組求解得到中心點(diǎn)的運(yùn)動(dòng)參數(shù),其方程可表示為

(3)

如果圖像中的紋理區(qū)域最少包含兩個(gè)方向,就可以得到式(3)的定解,光流法跟蹤如圖1所示。

圖1 光流法跟蹤

1.1.3 位姿恢復(fù)

由對(duì)極幾何約束[11]可知,任意相鄰兩幀圖像上的對(duì)應(yīng)點(diǎn)x,x′,滿足

x′TFx=0

(4)

式中F為基礎(chǔ)矩陣。得到基礎(chǔ)矩陣F后,用式(5)計(jì)算本質(zhì)矩陣E

E=KTFK

(5)

式中K為相機(jī)內(nèi)參矩陣,對(duì)本質(zhì)矩陣E奇異值(SVD)分解,可以得到機(jī)器人相鄰幀間的旋轉(zhuǎn)矩陣R和平移向量T。

1.2 基于改進(jìn)ORB法的運(yùn)動(dòng)估計(jì)

1.2.1 Hessian矩陣特征點(diǎn)檢測(cè)

(6)

式中 (x,y)為圖像坐標(biāo)點(diǎn),σ為尺度因子,Lxx(x,y,σ)為高斯拉普拉斯算子LOG,在此用DOG算子D(x,y,σ),取代LOG算子來(lái)近似的表達(dá)。則近似Hessian矩陣的行列式可表示為:det(Happrox)=DxxDyy-(wDxy)2,其中w為比例系數(shù),在此取為0.9,如果近似Hessian矩陣的行列式det(Happrox)的符號(hào)為正,則該點(diǎn)為極值點(diǎn)[12]。

1.2.2 生成特征描述子

采用灰度質(zhì)心法,給特征點(diǎn)增加方向信息,然后使用ORB構(gòu)造特征描述子的方法,計(jì)算出特征描述子,進(jìn)而得到了既具有旋轉(zhuǎn)不變性又有尺度不變性的特征描述子。

1.2.3 漢明距離匹配

由于使用ORB構(gòu)造的是二進(jìn)制描述子,所以可以利用漢明距離對(duì)特征點(diǎn)進(jìn)行匹配,為了設(shè)定域值來(lái)篩選匹配點(diǎn),本文的設(shè)置是當(dāng)描述子之間的距離大于2倍的最小距離時(shí),即視為匹配有誤,但最小距離有時(shí)會(huì)非常小,為此設(shè)置一個(gè)經(jīng)驗(yàn)值30作為下限。

1.2.4 剔除誤匹配對(duì)

找到兩幅圖像之間單應(yīng)性矩陣,通過(guò)單應(yīng)矩陣建立兩幅圖像間的關(guān)聯(lián),采用RANSAC算法使用比較少的點(diǎn)對(duì)模型估算,再用余下的點(diǎn)去驗(yàn)證模型的正確性,找到較為準(zhǔn)確的模型,根據(jù)此模型篩選誤匹配點(diǎn)。改進(jìn)ORB法特征匹配圖如圖2所示。

圖2 改進(jìn)ORB法特征匹配

1.2.5 位姿恢復(fù)

同1.1.3位姿恢復(fù)方法類(lèi)似,由相鄰幀的匹配點(diǎn)對(duì),計(jì)算出相鄰幀間的本質(zhì)矩陣,然后對(duì)獲得的本質(zhì)矩陣使用奇異值分解得到旋轉(zhuǎn)矩陣R和平移向量T。

1.3 光流法與改進(jìn)ORB融合

光流法計(jì)算速度快,定位頻率高,適合車(chē)輛的實(shí)時(shí)定位,但是誤差會(huì)隨著跟蹤點(diǎn)數(shù)的丟失而增加。改進(jìn)ORB算法定位法精度較高,但定位頻率低,兩者的性能具有較好的互補(bǔ)性。本文提出一種光流法與改進(jìn)ORB融合方法,具體步驟如圖3所示。

圖3 光流法和改進(jìn)ORB特征點(diǎn)匹配融合算法流程

如圖3所示,在2幀圖像之間利用光流法獲得旋轉(zhuǎn)矩陣R和平移向量T,當(dāng)?shù)趌n+1幀跟蹤特征點(diǎn)數(shù)小于設(shè)定的閾值時(shí),對(duì)于ln幀和ln+1幀利用改進(jìn)ORB特征點(diǎn)匹配法獲得兩幀間的旋轉(zhuǎn)矩陣R和平移向量T,同時(shí)重新檢測(cè)ln幀圖像并在ln幀和ln+1幀圖像間進(jìn)行跟蹤,并通過(guò)地面的平面單應(yīng)性矩陣來(lái)估計(jì)具體平移尺度[13],對(duì)之前得到的T進(jìn)行更新,然后對(duì)于光流法的結(jié)果和特征點(diǎn)匹配得到的結(jié)果進(jìn)行累積,不斷循環(huán),得到最終的R和T。

2 仿真結(jié)果分析

為了驗(yàn)證本文算法的有效性,在公開(kāi)數(shù)據(jù)集KITTI上進(jìn)行了測(cè)試,本文選取數(shù)據(jù)集序列5作為測(cè)試對(duì)象。實(shí)驗(yàn)計(jì)算機(jī)參數(shù)為:Pentium(R) Dual-Core CPU,2 GB內(nèi)存,64位操作系統(tǒng)。實(shí)驗(yàn)所用的程序是在Visual Studio2015中使用C++編寫(xiě),并借助OpenCV3.0開(kāi)源庫(kù)和Cmake3.8.0編譯工具。序列5中的800幀屬于靜態(tài)場(chǎng)景,同時(shí)含有相機(jī)校正文件,從中可讀出相機(jī)的焦距和主點(diǎn)坐標(biāo)。在Visual Studio上進(jìn)行仿真測(cè)試后,記錄下結(jié)果數(shù)據(jù)并用Matlab處理,得到與傳統(tǒng)光流法對(duì)比的定位結(jié)果比較如圖4(a)所示。

圖4 仿真結(jié)果對(duì)比

為了保證光流計(jì)算的精度,本文中對(duì)每幀檢測(cè)的最少特征點(diǎn)數(shù)設(shè)為800,當(dāng)跟蹤特征點(diǎn)數(shù)低于這一閾值時(shí),采用改進(jìn)ORB算法進(jìn)行幀間位移定位。另外本文算法平均每23幀執(zhí)行一次循環(huán),同光流法一樣具有較快的計(jì)算速度,從圖像幀中選取的8幀及其對(duì)應(yīng)的運(yùn)行時(shí)間如表1所示。

表1 里程計(jì)運(yùn)行時(shí)間

從圖4(a)中可以看出,本文方法估計(jì)的軌跡更加接近真實(shí)軌跡,因?yàn)樵诠諒澨幐欬c(diǎn)數(shù)快速減少,使用光流法估計(jì)的精度下降,所以在拐彎處兩種方法的差距更加明顯。從圖4(b)中可以看出,在前二百多幀時(shí)光流法定位精度稍高于本文方法,但隨著誤差的累積,之后光流法定位誤差越來(lái)越大,而本文方法因?yàn)槿诤狭烁倪M(jìn)ORB算法精度得到提高,累積誤差導(dǎo)致的定位誤差比光流法小的多,在800幀范圍內(nèi)誤差曲線始終在2 m范圍內(nèi)波動(dòng)。

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

本文設(shè)計(jì)了一種光流法和改進(jìn)ORB特征點(diǎn)匹配算法融合的單目視覺(jué)里程計(jì)方法,采用光流和改進(jìn)ORB基于閾值循環(huán)運(yùn)行,突出了光流法處理時(shí)間短和改進(jìn)ORB特征點(diǎn)匹配算法精度高的優(yōu)點(diǎn),相比于傳統(tǒng)的光流法視覺(jué)里程計(jì),精度得到了提高。實(shí)驗(yàn)結(jié)果表明,本文算法有著比較好的定位結(jié)果,對(duì)視覺(jué)里程計(jì)的開(kāi)發(fā)有一定的指導(dǎo)意義。