基于輔助匹配的1點(diǎn)RANSAC單目視覺(jué)導(dǎo)航算法

齊乃新, 張勝修, 曹立佳, 楊小岡

(1. 火箭軍工程大學(xué)控制工程系, 陜西 西安 710025;2. 四川理工學(xué)院自動(dòng)化與信息工程學(xué)院, 四川 自貢 643000;3. 人工智能四川省重點(diǎn)實(shí)驗(yàn)室, 四川 自貢 643000)

0 引 言

隨著計(jì)算機(jī)視覺(jué)的發(fā)展,單目視覺(jué)導(dǎo)航技術(shù)已經(jīng)在機(jī)器人、可穿戴計(jì)算、增強(qiáng)實(shí)景以及無(wú)人駕駛汽車等眾多領(lǐng)域展現(xiàn)出了廣闊的應(yīng)用前景,成為人們研究的熱點(diǎn)。目前,單目視覺(jué)導(dǎo)航方法主要分為兩大類[1]:一類是基于濾波的估計(jì)方法,通過(guò)融合圖像序列的觀測(cè)量,不斷地更新特征點(diǎn)和攝像機(jī)的位姿參數(shù)來(lái)估計(jì)攝像機(jī)的運(yùn)動(dòng)信息[2-3];另一類則是基于光束平差(bundle adjustment,BA)的估計(jì)方法,通過(guò)光流場(chǎng)或特征點(diǎn)匹配實(shí)現(xiàn)對(duì)載體長(zhǎng)時(shí)間的無(wú)漂移運(yùn)動(dòng)估計(jì)[4-5]。

基于濾波的估計(jì)方法可以有效地利用濾波預(yù)測(cè),估計(jì)下一幀的特征點(diǎn)位置,采用主動(dòng)視覺(jué)匹配的方法來(lái)降低匹配的復(fù)雜度,能夠?qū)崿F(xiàn)對(duì)攝像機(jī)位姿參數(shù)的實(shí)時(shí)在線估計(jì)。帝國(guó)理工大學(xué)的Davison教授及其領(lǐng)導(dǎo)的研究團(tuán)隊(duì)在這一領(lǐng)域做出了比較突出的貢獻(xiàn),2003年首次提出了對(duì)自由運(yùn)動(dòng)攝像機(jī)的6D姿態(tài)估計(jì)方法[6]。該方法采用主動(dòng)視覺(jué)匹配,具有較好的實(shí)時(shí)性和魯棒性;采用基于粒子濾波的特征初始化算法,很好地解決了位姿估計(jì)的長(zhǎng)時(shí)間尺度漂移問(wèn)題,具有較高的精度。隨后,Davison教授等人又做了大量的研究工作,推動(dòng)了基于視覺(jué)同步定位與地圖構(gòu)建(simultaneous localization and mapping,SLAM)技術(shù)的發(fā)展[7]。在Davison教授的研究基礎(chǔ)上,Civera[8]提出了一種基于1點(diǎn)隨機(jī)抽樣一致性(random sample consensus,RANSAC)的單目視覺(jué)導(dǎo)航方法,實(shí)現(xiàn)了對(duì)運(yùn)動(dòng)攝像機(jī)的位姿估計(jì)。該方法結(jié)合了RANSAC和擴(kuò)展卡爾曼濾波器(extended Kalman filter,EKF),解決了視覺(jué)SLAM中的數(shù)據(jù)一致性問(wèn)題,較大程度上優(yōu)化了數(shù)據(jù)關(guān)聯(lián)的過(guò)程[9]。同時(shí),利用1點(diǎn)RANSAC[10]生成假設(shè)模型,減少了算法迭代的次數(shù),增強(qiáng)了系統(tǒng)的穩(wěn)定性。

雖然Civera提出的1點(diǎn)RANSAC算法在視覺(jué)導(dǎo)航的研究中受到了許多學(xué)者的青睞[11-13],但在實(shí)際實(shí)驗(yàn)過(guò)程中面臨一個(gè)匹配失效的問(wèn)題,尤其是在攝像機(jī)姿態(tài)或者環(huán)境的光照條件發(fā)生突變的情況下。這一問(wèn)題是由算法中的主動(dòng)視覺(jué)匹配和數(shù)據(jù)關(guān)聯(lián)原理決定的。當(dāng)攝像機(jī)的運(yùn)動(dòng)發(fā)生突變時(shí),主動(dòng)視覺(jué)匹配極易失效,找不到有效的匹配點(diǎn),對(duì)后續(xù)的EKF內(nèi)點(diǎn)更新過(guò)程產(chǎn)生較大的影響,系統(tǒng)濾波的穩(wěn)定性以及狀態(tài)估計(jì)的精度都會(huì)不同程度的降低。針對(duì)上述問(wèn)題,本文提出了基于尺度不變特征變換(scale invariant feature transform,SIFT)的輔助匹配方法,采用SIFT算法提取特征點(diǎn)、RANSAC算法解算特征點(diǎn)所在圖像與當(dāng)前圖像之間的基礎(chǔ)矩陣,然后利用此基礎(chǔ)矩陣求解匹配點(diǎn),解決攝像機(jī)姿態(tài)突變情況下主動(dòng)視覺(jué)匹配失效的問(wèn)題。

1 1點(diǎn)RANSAC單目視覺(jué)導(dǎo)航算法

1.1 預(yù)測(cè)和搜索匹配點(diǎn)

(1)

(2)

(3)

(4)

式中,Hi是hi關(guān)于xk|k-1的雅可比矩陣;Ri是觀測(cè)模型中高斯噪聲的協(xié)方差矩陣。

然后,采用主動(dòng)視覺(jué)匹配方法根據(jù)預(yù)測(cè)值在特征點(diǎn)出現(xiàn)概率較高(不低于99%)的區(qū)域內(nèi)搜索,找到匹配點(diǎn)zi,即為觀測(cè)量的真實(shí)值。

1.2 建立和估計(jì)1點(diǎn)狀態(tài)假設(shè)

圖1 1點(diǎn)RANSAC估計(jì)過(guò)程Fig.1 1-Point RANSAC estimation process

在建立假設(shè)時(shí),RANSAC算法首先為系統(tǒng)的模型參數(shù)建立假設(shè),并且挑選出獲得支持率最高的一個(gè)假設(shè)。假設(shè)的建立是通過(guò)隨機(jī)的從匹配點(diǎn)集zIC=(z1…zi…zn)T中選出m個(gè)點(diǎn)完成的,m的值是計(jì)算模型參數(shù)所需的匹配點(diǎn)數(shù)量的最小值。通過(guò)簡(jiǎn)單的計(jì)算統(tǒng)計(jì)所在閾值內(nèi)的點(diǎn)的數(shù)量得到每一個(gè)假設(shè)的支持投票數(shù),這里所用的閾值可以根據(jù)顯著性參數(shù)α=0.05的χ2分布得到。包含外點(diǎn)的假設(shè)會(huì)得到較少的投票數(shù),如圖1的假設(shè)3。建立的假設(shè)的數(shù)量nhyp至少能夠確保在測(cè)試概率p下有一個(gè)無(wú)虛假假設(shè),可以由式(5)計(jì)算得到。

(5)

式中,e表示點(diǎn)集中外點(diǎn)的占有比例;nhyp表示需要建立的假設(shè)數(shù)量;m表示滿足模型參數(shù)求解條件的最少匹配點(diǎn)數(shù)。

1.3 第1次部分更新

靠近假設(shè)模型較近的點(diǎn),對(duì)假設(shè)的支持性較大,被近似認(rèn)為是構(gòu)成實(shí)際模型的點(diǎn),這些特征點(diǎn)被稱為具有小觀測(cè)殘差的內(nèi)點(diǎn)集。除此之外的點(diǎn)稱為外點(diǎn),但是仍具有是內(nèi)點(diǎn)的可能性。

在RANSAC假設(shè)的建立過(guò)程中,攝像機(jī)的旋轉(zhuǎn)量假設(shè)主要依靠距離較遠(yuǎn)的特征點(diǎn)建立,攝像機(jī)的位移量假設(shè)主要依靠距離較近的特征點(diǎn)建立。由于估計(jì)的不準(zhǔn)確性,部分距離遠(yuǎn)的外點(diǎn)會(huì)呈現(xiàn)出小觀測(cè)殘差的特性,而距離較近的內(nèi)點(diǎn)也有可能呈現(xiàn)出大觀測(cè)殘差的特性。

在假設(shè)模型和內(nèi)定確定以后,利用具有小觀測(cè)殘差的內(nèi)點(diǎn)對(duì)系統(tǒng)的狀態(tài)矩陣和協(xié)方差矩陣完成第1次部分更新,如式(6)～式(8)所示。

(6)

Pk|k=(I-KkHk)Pk|k-1

(7)

(8)

1.4 第2次部分更新

EKF預(yù)測(cè)中的誤差和協(xié)方差中的誤差在第1次部分更新完成之后得到了有效的修正,此時(shí),存在于外點(diǎn)集中的具有大觀測(cè)殘差的內(nèi)點(diǎn)可以被有效地恢復(fù)。

2 SIFT輔助匹配

2.1 匹配失效問(wèn)題描述分析

1點(diǎn)RANSAC算法要求攝像機(jī)的運(yùn)動(dòng)是平穩(wěn)連續(xù)的,這樣才能夠保證特征點(diǎn)在預(yù)測(cè)的區(qū)域中。但在實(shí)際情況下，攝像機(jī)并不能保證平穩(wěn)連續(xù)運(yùn)動(dòng),比如飛行器在遇到強(qiáng)風(fēng)、障礙,車輛發(fā)生急轉(zhuǎn)彎、緊急剎車等情況下,載體的速度或者姿態(tài)會(huì)發(fā)生突變。此時(shí),特征點(diǎn)落在預(yù)測(cè)的區(qū)域之外或有較少的點(diǎn)落在預(yù)測(cè)區(qū)域之中,主動(dòng)視覺(jué)匹配無(wú)法搜索到落在預(yù)測(cè)區(qū)域外的點(diǎn)。因此,在這種情況下會(huì)找不到有效的匹配點(diǎn),即使找到,數(shù)量也很少。1點(diǎn)RANSAC算法做出假設(shè)至少需要一對(duì)匹配點(diǎn),此時(shí)的主動(dòng)匹配失效,不能提供有效的匹配點(diǎn)對(duì)來(lái)建立假設(shè),也就不能完成后續(xù)的更新過(guò)程。當(dāng)找到的匹配點(diǎn)數(shù)量過(guò)少時(shí),不能滿足概率p下確保有一個(gè)無(wú)虛假假設(shè)的條件,此時(shí)的RANSAC過(guò)程處于一種病態(tài)假設(shè),對(duì)模型參數(shù)的估計(jì)存在較大的誤差。

1點(diǎn)RANSAC算法中采用主動(dòng)視覺(jué)匹配作為特征點(diǎn)的匹配策略,通過(guò)式(3)對(duì)特征點(diǎn)的位置進(jìn)行預(yù)測(cè),在特征點(diǎn)以99%概率出現(xiàn)的區(qū)域內(nèi)進(jìn)行搜索。當(dāng)攝像機(jī)的姿態(tài)發(fā)生突變時(shí),相鄰兩幀圖像之間會(huì)有較大的差異,此時(shí)特征點(diǎn)將會(huì)落在預(yù)測(cè)的區(qū)域之外或有極少數(shù)的特征點(diǎn)落在搜索區(qū)域內(nèi)。當(dāng)全部的特征點(diǎn)都落在搜索區(qū)域外時(shí),搜索失效,主動(dòng)視覺(jué)匹配將無(wú)法找到匹配點(diǎn),發(fā)生匹配失效的問(wèn)題。當(dāng)有極少數(shù)的特征點(diǎn)落在搜索區(qū)域時(shí),由于特征點(diǎn)過(guò)少在后續(xù)的內(nèi)點(diǎn)更新過(guò)程中會(huì)代入較大的誤差,影響濾波估計(jì)的精度。

上述問(wèn)題在本文的主動(dòng)視覺(jué)匹配失效分析實(shí)驗(yàn)部分,通過(guò)對(duì)Civera提供的“rawoutput”測(cè)試數(shù)據(jù)集的實(shí)驗(yàn)分析得到了充分說(shuō)明。為此,本文提出輔助匹配策略,采用SIFT特征點(diǎn)匹配方法解決主動(dòng)視覺(jué)匹配失效的問(wèn)題。

2.2 SIFT輔助匹配方法

匹配失效情況的判斷依據(jù)是主動(dòng)視覺(jué)匹配得到的匹配特征點(diǎn)數(shù)。首先,根據(jù)式(3)和式(4)預(yù)測(cè)出特征點(diǎn)所在的區(qū)域;然后,采用主動(dòng)視覺(jué)匹配方法搜索匹配點(diǎn)zi;其次,對(duì)匹配點(diǎn)zi的數(shù)量進(jìn)行統(tǒng)計(jì),得到匹配點(diǎn)總數(shù)Nz。最后,根據(jù)匹配點(diǎn)總數(shù)和設(shè)定的閾值(本文中取值為5)進(jìn)行判斷,如果匹配點(diǎn)數(shù)Nz小于設(shè)定的閾值則進(jìn)行輔助匹配。輔助匹配的具體過(guò)程描述如下。

2.2.1 獲取SIFT特征點(diǎn)

SIFT算法是由Lowe等人在2004年提出的[15],該算法具有豐富的信息量,能夠很好地處理尺度變換、亮度變換、旋轉(zhuǎn)等情況下的圖像匹配問(wèn)題。為了提高SIFT算法的效率,本文采用柵格的形式在關(guān)鍵點(diǎn)的鄰域內(nèi)選取SIFT特征點(diǎn)[16],對(duì)SIFT算法的描述子進(jìn)行適當(dāng)?shù)貎?yōu)化,如圖2所示。

將關(guān)鍵點(diǎn)16×16大小區(qū)域按圖2所示分成4×4的16個(gè)小區(qū)域,從中選取彼此不相鄰的8個(gè)小區(qū)域(圖中灰色小區(qū)域)對(duì)SIFT特征點(diǎn)進(jìn)行描述。如此,能夠?qū)IFT特征點(diǎn)128維的特征描述向量壓縮為64維,降低了算法的復(fù)雜度。

圖2 SIFT特征點(diǎn)選取方式Fig.2 SIFT feature point selection method

然后利用式(9)剔除響應(yīng)值小的點(diǎn)

(9)

D(x,y,σ)=(G(x,y,kσ)-G(x,y,σ))?I(x,y)=

L(x,y,kσ)-L(x,y,σ)

(10)

L(x,y,σ)=G(x,y,σ)?I(x,y)

(11)

式中,I(x,y)表示一幅圖像;(x,y)表示圖像的像素坐標(biāo);σ是尺度空間因子;L(x,y,σ)表示圖像變換后的尺度空間;G(x,y,σ)為高斯核函數(shù); ?為卷積運(yùn)算。

最后,通過(guò)如式(12)中2×2的Hessian矩陣H剔除邊緣點(diǎn),得到帶有描述子的SIFT特征點(diǎn)。

(12)

2.2.2 匹配

分別提取到兩幅圖像的SIFT特征點(diǎn)后,采用式(13)歐式距離進(jìn)行相似性度量,獲取圖像間潛在的匹配。

(13)

式中,向量X和Y分別定義為X=(x1,x2,…,xn)和Y=(y1,y2,…,yn)。

根據(jù)相似性度量得到的初始匹配點(diǎn)對(duì)中存在一定程度的誤匹配,為了剔除這些誤匹配點(diǎn),提高算法的魯棒性,在算法中添加了主方向約束條件[16],其具體步驟如下:

步驟1按式(14)計(jì)算匹配點(diǎn)集zIC=(z1…zi…zn)T的主方向差方值,即

D(i)=|O1(i)-O2(i)|2，i=1,2,…,n

(14)

式中,D(i)代表zIC=(z1…zi…zn)T的主方向差方值;O1(i)為圖像1中的特征點(diǎn)主方向;O2(i)為圖像2中的特征點(diǎn)主方向;n為匹配點(diǎn)集的大小;

步驟2計(jì)算誤匹配點(diǎn)的剔除閾值To,即

(15)

步驟3剔除初始匹配點(diǎn)集zIC=(z1…zi…zn)T中的誤匹配點(diǎn),即

(16)

2.2.3 解算基礎(chǔ)矩陣及匹配點(diǎn)求解

基礎(chǔ)矩陣反映了兩幅圖像之間的變換關(guān)系,其與兩幅圖像的匹配特征點(diǎn)對(duì)存在如式(17)所示的約束關(guān)系。

u′Fu=0

(17)

假設(shè)匹配點(diǎn)集的大小為n,則兩幅圖像之間滿足的約束方程為

Af=

(18)

式中,f=(f11,f12,f13,f21,f22,f23,f31,f32,f33)；A為系數(shù)矩陣。基礎(chǔ)矩陣的求解算法對(duì)外點(diǎn)比較敏感,為此文中在RANSAC算法的基礎(chǔ)上引入了Sampson加權(quán)算子來(lái)劃分局內(nèi)點(diǎn)和局外點(diǎn),用以減小噪聲對(duì)特征點(diǎn)的影響,防止由于少量誤匹配所產(chǎn)生的誤差影響匹配精度的問(wèn)題產(chǎn)生。解算得到基本矩陣之后,根據(jù)初始特征點(diǎn)求解對(duì)應(yīng)的匹配點(diǎn),具體步驟如下:

步驟2通過(guò)加權(quán)算子

(19)

計(jì)算Sampson距離,將zIC=(z1…zi…zn)T分為局內(nèi)點(diǎn)和局外點(diǎn),對(duì)基本子集進(jìn)行投票;

步驟3將當(dāng)前投票數(shù)與最大投票數(shù)進(jìn)行比較,保存更新對(duì)應(yīng)投票數(shù)最多的基礎(chǔ)矩陣和局內(nèi)點(diǎn);

步驟5根據(jù)對(duì)應(yīng)投票數(shù)最大的局內(nèi)點(diǎn),重新計(jì)算基礎(chǔ)矩陣F;

步驟6根據(jù)步驟5得到的基礎(chǔ)矩陣F解算用于狀態(tài)更新的有效匹配特征點(diǎn)。

2.2.4 EKF狀態(tài)更新

輔助匹配找到有效匹配特征點(diǎn)之后對(duì)狀態(tài)方程和協(xié)方差矩陣進(jìn)行一次更新，更新方程如式(20)～式(22)所示。

(20)

Pk|k=(I-KkHk)Pk|k-1

(21)

(22)

2.2.5 建立狀態(tài)假設(shè)

最后建立狀態(tài)假設(shè),完成1點(diǎn)RANSAC單目視覺(jué)導(dǎo)航算法的兩次內(nèi)點(diǎn)更新,得到狀態(tài)方程的最終估計(jì)量。

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

3.1 主動(dòng)視覺(jué)匹配失效分析實(shí)驗(yàn)

為充分說(shuō)明1點(diǎn)RANSAC算法中的主動(dòng)視覺(jué)匹配失效問(wèn)題,本文采用Civera提供的rawoutput測(cè)試數(shù)據(jù)集進(jìn)行了實(shí)驗(yàn)分析,實(shí)驗(yàn)結(jié)果如圖3～圖5所示。

圖3 主動(dòng)視覺(jué)匹配失效幀的圖像Fig.3 Image in active vision matching failure frame

圖4 特征點(diǎn)的統(tǒng)計(jì)情況Fig.4 Statistics of feature points

圖5 主動(dòng)視覺(jué)匹配找到的匹配點(diǎn)少于5個(gè)時(shí)的統(tǒng)計(jì)情況Fig.5 Statistics of the matching points under 5 found by the active visual matching

圖3給出了主動(dòng)視覺(jué)匹配得到的匹配點(diǎn)數(shù)在5個(gè)之內(nèi)(包括5個(gè))的幾幀圖像,從第1～6行分別是0～5個(gè)匹配點(diǎn)的情況,圖中藍(lán)色表示未能找到匹配點(diǎn)的特征點(diǎn)、粉紅色表示找到的匹配點(diǎn)是外點(diǎn)的特征點(diǎn)、紅色表示找到的匹配點(diǎn)是內(nèi)點(diǎn)的特征點(diǎn)。從圖3中可以看出,雖然具有較多的特征點(diǎn),但由于攝像機(jī)的運(yùn)動(dòng)突變以及外界環(huán)境的干擾,導(dǎo)致主動(dòng)視覺(jué)匹配沒(méi)有找到有效的匹配點(diǎn),或者找到的有效匹配點(diǎn)不足以建立一個(gè)良態(tài)假設(shè)。

圖4對(duì)整個(gè)圖像序列每一幀的特征點(diǎn)總數(shù)、主動(dòng)視覺(jué)匹配找到的匹配點(diǎn)總數(shù)、具有小觀測(cè)殘差的內(nèi)點(diǎn)數(shù)以及具有大觀測(cè)殘差的內(nèi)點(diǎn)數(shù)進(jìn)行了統(tǒng)計(jì)。從統(tǒng)計(jì)情況來(lái)看,主動(dòng)視覺(jué)找到的匹配點(diǎn)數(shù)在10個(gè)以內(nèi)的情況出現(xiàn)次數(shù)較多,而匹配點(diǎn)數(shù)少于10時(shí),RANSAC假設(shè)的建立以及利用內(nèi)點(diǎn)更新的過(guò)程會(huì)出現(xiàn)病態(tài)情況,影響了算法的估計(jì)精度。由此可以知道,在1點(diǎn)RANSAC單目視覺(jué)導(dǎo)航算法中主動(dòng)視覺(jué)匹配失效問(wèn)題比較嚴(yán)重,并且會(huì)對(duì)估計(jì)精度造成一定的影響,需要采取必要的措施加以解決。

圖5是對(duì)匹配點(diǎn)數(shù)在5個(gè)之內(nèi)出現(xiàn)的幀數(shù)的一個(gè)統(tǒng)計(jì)情況,從圖中可以分析出主動(dòng)視覺(jué)失效是階段性的,存在于攝像機(jī)運(yùn)動(dòng)發(fā)生突變,或者環(huán)境發(fā)生突變時(shí)的情況下。在本實(shí)驗(yàn)中主動(dòng)視覺(jué)匹配失效問(wèn)題在第800～1 700幀時(shí)出現(xiàn)比較密集,原因在于這一段中攝像機(jī)的運(yùn)動(dòng)突變情況較多,平滑穩(wěn)定性較差,并且多次出現(xiàn)角度和運(yùn)動(dòng)方向逆轉(zhuǎn)的情況。

為了進(jìn)一步說(shuō)明1點(diǎn)RANSAC算法中的匹配失效問(wèn)題對(duì)整個(gè)算法的影響程度,表1對(duì)主動(dòng)視覺(jué)匹配找到的匹配點(diǎn)少于10個(gè)時(shí)的幀數(shù),以及在圖像序列中的占比情況作了統(tǒng)計(jì)。

表1 匹配點(diǎn)數(shù)在10個(gè)以內(nèi)的幀數(shù)統(tǒng)計(jì)結(jié)果

整個(gè)數(shù)據(jù)集總共2 169幀圖像,沒(méi)有找到匹配點(diǎn)的情況共44幀,找到一個(gè)匹配點(diǎn)共61幀,找到2個(gè)匹配點(diǎn)共75幀,找到3個(gè)匹配點(diǎn)共101幀,找到4個(gè)匹配點(diǎn)共106幀,找到5個(gè)匹配點(diǎn)共117幀。從統(tǒng)計(jì)結(jié)果來(lái)看,完全沒(méi)有找到匹配點(diǎn)的情況占2.03%,匹配點(diǎn)在5個(gè)以內(nèi)(包含5個(gè))占23.25%,匹配點(diǎn)在10個(gè)以內(nèi)(包含10個(gè))占46.26%?？梢?jiàn)主動(dòng)視覺(jué)匹配失效的情況發(fā)生的次數(shù)較多,對(duì)算法的影響比較大,如果不采取一定的措施來(lái)解決這一問(wèn)題,會(huì)較大程度上影響單目視覺(jué)導(dǎo)航算法的估計(jì)精度和穩(wěn)定性。為此,本文提出一種基于SIFT的輔助匹配策略,解決了1點(diǎn)RANSAC單目視覺(jué)導(dǎo)航算法中的匹配失效問(wèn)題。

3.2 輔助匹配效果分析實(shí)驗(yàn)

本文采用Civera提供的rawoutput測(cè)試數(shù)據(jù)集進(jìn)行了匹配實(shí)驗(yàn)。取第892幀為當(dāng)前圖像,此時(shí)1點(diǎn)RANSAC算法發(fā)生了匹配失效問(wèn)題,采用主動(dòng)視覺(jué)匹配未能找到匹配點(diǎn)。取892幀時(shí)的特征點(diǎn)第一次被觀測(cè)到時(shí)所在圖像為待匹配圖像,采用本文提出的輔助匹配算法查找匹配點(diǎn)。

實(shí)驗(yàn)統(tǒng)計(jì)可知第892幀時(shí),特征點(diǎn)總數(shù)為40,SIFT匹配找到的特征點(diǎn)總數(shù)為35。如圖6所示,分別是第98、679、739和862幀上的特征點(diǎn)的SIFT匹配結(jié)果,這4組圖像發(fā)生了較大程度的平移、旋轉(zhuǎn)、尺度縮放以及視角變化,從實(shí)驗(yàn)結(jié)果來(lái)看,匹配精度較高,能夠有效地找到匹配點(diǎn)。

圖6 SIFT匹配結(jié)果Fig.6 Matching results of SIFT algorithm

圖7對(duì)SIFT匹配的精度進(jìn)行了統(tǒng)計(jì)分析,從分析結(jié)果來(lái)看，35個(gè)特征點(diǎn)中有3個(gè)點(diǎn)的誤差超出了10個(gè)像素,其他點(diǎn)的誤差均在5個(gè)像素之內(nèi),分析原因?yàn)檎`差超過(guò)10個(gè)像素的特征點(diǎn)所在的圖像與當(dāng)前圖像的重疊區(qū)域較少,SIFT匹配過(guò)程中找到的匹配點(diǎn)數(shù)少于15,此時(shí)RANSAC算法解算基礎(chǔ)矩陣時(shí)不能有效地剔除誤匹配,引入了較大的誤差。此問(wèn)題可以在SIFT輔助匹配過(guò)程,通過(guò)設(shè)定SIFT匹配點(diǎn)數(shù)的閾值來(lái)解決。

圖7 SIFT匹配解算的匹配點(diǎn)誤差Fig.7 Errors of matching points calculated by SIFT algorithm

采用添加輔助匹配后的1點(diǎn)RANSAC算法重新對(duì)Civera提供的rawoutput測(cè)試數(shù)據(jù)集進(jìn)行了導(dǎo)航估計(jì),并重新對(duì)整個(gè)數(shù)據(jù)集中每一幀的特征點(diǎn)總數(shù)、SIFT輔助找到的匹配點(diǎn)總數(shù)、具有小觀測(cè)殘差的內(nèi)點(diǎn)數(shù)以及具有大觀測(cè)殘差的內(nèi)點(diǎn)數(shù)進(jìn)行了統(tǒng)計(jì),結(jié)果如圖8和圖9所示。從圖8輔助匹配后特征點(diǎn)的統(tǒng)計(jì)情況來(lái)看,不存在匹配點(diǎn)數(shù)在5個(gè)以下的情況。圖9對(duì)應(yīng)的是圖2的幀數(shù),從匹配點(diǎn)的顯示結(jié)果來(lái)看,每一幀都能夠找到有效的特征點(diǎn)進(jìn)行算法后續(xù)的狀態(tài)更新,較好地解決了1點(diǎn)RANSAC單目視覺(jué)導(dǎo)航算法中存在的主動(dòng)視覺(jué)匹配失效問(wèn)題。

圖8 輔助匹配后的特征點(diǎn)統(tǒng)計(jì)情況Fig.8 Statistics of feature points with aided matching

圖9 主動(dòng)視覺(jué)匹配失效幀的輔助匹配結(jié)果Fig.9 Results of the images in active visual matching failure frame with aided matching

3.3 智能實(shí)驗(yàn)平臺(tái)驗(yàn)證實(shí)驗(yàn)

為了驗(yàn)證輔助匹配方案對(duì)提高EKF算法估計(jì)精度的有效性,本文設(shè)計(jì)了一組驗(yàn)證性實(shí)驗(yàn),實(shí)驗(yàn)采用載有單目攝像機(jī)的智能實(shí)驗(yàn)平臺(tái),如圖10所示。本實(shí)驗(yàn)平臺(tái)是根據(jù)需要自主改裝的由DrRobot公司生產(chǎn)的X80 Pro型WiFi智能控制小車。該智能實(shí)驗(yàn)平臺(tái)包含單目攝像機(jī)和通信控制模塊,能夠通過(guò)編程預(yù)定行駛軌跡模擬平臺(tái)的運(yùn)動(dòng)軌跡,并且能夠通過(guò)WiFi實(shí)時(shí)傳輸圖像序列,用于估計(jì)平臺(tái)的位姿參數(shù)。車載攝像頭采用電荷耦合器件(charge coupled device,CCD)針孔攝像頭,其模型為針孔攝像頭模型,焦距:fx=330.524,fy=329.791;中心點(diǎn):Cx=159.5,Cy=119.5;畸變參數(shù):k1=-0.103,k2=0.138;采集到的圖像大小:320像素×240像素。

圖10 智能實(shí)驗(yàn)平臺(tái)Fig.10 Intelligent experimental platform

本驗(yàn)證實(shí)驗(yàn)是在室內(nèi)環(huán)境下完成的,環(huán)境相對(duì)比較復(fù)雜,包括實(shí)驗(yàn)儀器、辦公桌、電腦等靜態(tài)物體,以及人員走動(dòng)、光照的變化等動(dòng)態(tài)環(huán)境。實(shí)驗(yàn)中,機(jī)器人平臺(tái)的運(yùn)動(dòng)軌跡分為3個(gè)階段:第1階段先往前直行8 m,然后進(jìn)入第2階段,進(jìn)行一次90°轉(zhuǎn)彎,第3階段再往前直行6 m。分別采用本文改進(jìn)算法和標(biāo)準(zhǔn)的1點(diǎn)RANSAC算法(原算法)對(duì)攝像機(jī)的姿態(tài)角進(jìn)行估計(jì),實(shí)驗(yàn)結(jié)果如圖11～圖17所示。機(jī)器人運(yùn)行軌跡如圖18所示。

圖11 攝像機(jī)航向角的估計(jì)結(jié)果Fig.11 Estimation results of camera’s course angle

圖12 攝像機(jī)俯仰角的估計(jì)結(jié)果Fig.12 Estimation results of camera’s pitch angle

圖13 攝像機(jī)滾動(dòng)角的估計(jì)結(jié)果Fig.13 Estimation results of camera’s roll angle

圖14 攝像機(jī)航向角的估計(jì)誤差Fig.14 Estimation errors of camera’s course angle

圖15 攝像機(jī)俯仰角的估計(jì)誤差Fig.15 Estimation errors of camera’s pitch angle

圖16 攝像機(jī)滾動(dòng)角的估計(jì)誤差Fig.16 Estimation errors of camera’s roll angle

圖17 算法對(duì)單幀圖像的計(jì)算時(shí)間Fig.17 Computational times for single frame of the algorithms

圖18 機(jī)器人運(yùn)行軌跡示意圖Fig.18 Sketch map of robot’s motion trail

圖11～圖13為本文算法和原算法對(duì)攝像機(jī)姿態(tài)角的估計(jì)結(jié)果對(duì)比圖,從圖中可以看出,本文改進(jìn)算法的估計(jì)精度要高于原算法。圖14～圖16為本文算法和原算法對(duì)攝像機(jī)姿態(tài)角的估計(jì)誤差對(duì)比圖,改進(jìn)算法估計(jì)的攝像機(jī)航向角、俯仰角和滾動(dòng)角的平均誤差為6.73°、3.25°和9.01°,標(biāo)準(zhǔn)的1點(diǎn)RANSAC算法估計(jì)的攝像機(jī)航向角、俯仰角和滾動(dòng)角的平均誤差為11.77°、4.46°和12.04°。由此可知,添加輔助匹配后的EKF算法提高了對(duì)攝像機(jī)的俯仰、偏航以及滾動(dòng)3個(gè)姿態(tài)角的估計(jì)精度,其中航向角平均誤差減小5.04°,俯仰角平均誤差減小1.21°,滾動(dòng)角平均誤差減小3.03°。

為了比較本文算法和標(biāo)準(zhǔn)的1點(diǎn)RANSAC算法的計(jì)算效率,圖17給出了本文算法和原算法對(duì)單幀圖像的運(yùn)算時(shí)間。其中，本文算法的運(yùn)算時(shí)間中不包括SIFT匹配的時(shí)間,因?yàn)镾IFT算法固有的復(fù)雜性,其運(yùn)算比較耗時(shí),文中仿真平臺(tái)為Matlab,其效率較低,算法仿真成熟后將移植到C++平臺(tái),然后采用多線程并行處理SIFT匹配問(wèn)題,不影響EKF濾波的過(guò)程,因此暫時(shí)不將SIFT算法的運(yùn)算時(shí)間考慮在內(nèi)。本文算法的平均耗時(shí)為0.32 s,原算法的平均耗時(shí)為0.31 s,由此可見(jiàn),本文改進(jìn)算法EKF濾波估計(jì)階段保持了原算法的計(jì)算效率。

上述3組實(shí)驗(yàn)可以說(shuō)明,標(biāo)準(zhǔn)的1點(diǎn)RANSAC算法中存在主動(dòng)視覺(jué)匹配失效的問(wèn)題,影響估計(jì)的精度,本文提出的輔助匹配方法能夠找到精度較高的匹配特征點(diǎn),有效地解決了1點(diǎn)RANSAC單目視覺(jué)導(dǎo)航算法中的匹配失效問(wèn)題,在較大程度上提高了EKF算法對(duì)攝像機(jī)姿態(tài)角的估計(jì)精度。

4 結(jié) 論

本文從理論與實(shí)驗(yàn)相結(jié)合的角度出發(fā),分析了1點(diǎn)RANSAC單目視覺(jué)導(dǎo)航算法中存在的匹配失效問(wèn)題,提出了一種基于輔助匹配的1點(diǎn)RANSAC單目視覺(jué)導(dǎo)航算法。該算法采用SIFT算法提取特征點(diǎn),RANSAC算法求解特征點(diǎn)所在圖像與當(dāng)前圖像之間的基礎(chǔ)矩陣,然后利用基礎(chǔ)矩陣求解匹配點(diǎn),有效地解決了攝像機(jī)姿態(tài)突變時(shí)由于數(shù)據(jù)關(guān)聯(lián)失敗引起的匹配失效問(wèn)題,提高了EKF算法對(duì)攝像機(jī)運(yùn)動(dòng)姿態(tài)角的估計(jì)精度。

參考文獻(xiàn)：

[1] STRASDAT H, MONTIEL J M M, DAVISON A J. Visual SLAM: why filter[J]. Image and Vision Computing, 2012, 30(2): 65-77.

[2] CIVERA J, GRASA O G, DAVISON A J, et al. 1-point RANSAC for EKF-based structure from motion[C]∥Proc.of the IEEE/RSJ International Conference on Intelligent Robots and Systems, 2009: 3498-3504.

[3] PIRE T, FISCHER T, CIVERA J, et al. Stereo parallel tracking and mapping for robot localization[C]∥Proc.of the IEEE/RSJ International Conference on Intelligent Robots and Systems, 2015: 1373-1378.

[4] LIM J, FRAHM J M, POLLEFEYS M. Online environment mapping[C]∥Proc.of the IEEE Conference on Computer Vision and Pattern Recognition, 2011: 3489-3496.

[5] GRASA O G, BERNAL E, CASADO S, et al. Visual SLAM for handheld monocular endoscope[J]. IEEE Tran.on Medical Imaging, 2014, 33(1): 135-146.

[6] DAVISON A J. Real-time simultaneous localisation and mapping with a single camera[C]∥Proc.of the IEEE Computer Vision, 2003: 1403-1410.

[7] HANDA A, CHLI M, STRASDAT H, et al. Scalable active matching[C]∥Computer Visionand Pattern Recognition, 2010: 1546-1553.

[8] CIVERA J, GRASA O G, DAVISON A J, et al. 1-Point RANSAC for extended Kalman filtering: application to real-time structure from motion and visual odometry[J]. Journal of Field Robotics, 2010, 27(5): 609-631.

[9] CIVERA J, DAVISON A J, MONTIEL J M M. Inverse depth parametrization for monocular SLAM[J]. IEEE Trans.on Robotics, 2008, 24(5): 932-945.

[10] FRAUNDORFER F, SCARAMUZZA D. Visual odometry: part II: matching, robustness, optimization, and applications[J]. IEEE Robotics & Automation Magazine, 2012, 19(2): 78-90.

[11] 李捐. 基于單目視覺(jué)的移動(dòng)機(jī)器人 SLAM 問(wèn)題的研究[D]. 哈爾濱:哈爾濱工業(yè)大學(xué), 2013.

LI J. Reserch on SLAM problem of mobile robot with monocular vision[D]. Harbin: Harbin Institute of Technology, 2013.

[12] CONCHA A, DREWS-JR P, CAMPOS M, et al. Real-time localization and dense mapping in underwater environments from a monocular sequence[J].Oceans, 2015：1-5.

[13] SENGUPTA A, ELANATTIL S. New feature detection mechanism for extended Kalman filter based monocular SLAM with 1-Point RANSAC[C]∥Proc.of the International Conference on Mining Intelligence and Knowledge Exploration, 2015: 29-36.

[15] LOWE D G.Distinctive image features from scale-invariant keypoint[J].International Journal of Computer Vision,2004,60(2):91-110.

[16] 齊乃新, 曹立佳, 楊小岡, 等. 基于方向約束的改進(jìn)SIFT匹配算法[J]. 計(jì)算機(jī)科學(xué), 2014, 41(S1): 125-128.

QI N X, CAO L J, YANG X G, et al. The improved SIFT matching algorithm based on orientation constraint[J]. Computer Science, 2014, 41(S1): 125-128.