,,

(1.中國(guó)科學(xué)院上海微系統(tǒng)與信息技術(shù)研究所,上海 200050; 2.中國(guó)科學(xué)院大學(xué),北京 100049)

0 概述

實(shí)時(shí)精確的自我定位是移動(dòng)機(jī)器人實(shí)現(xiàn)自動(dòng)導(dǎo)航的關(guān)鍵。視覺(jué)里程計(jì)[1]利用一個(gè)或多個(gè)攝像機(jī)獲取圖像序列并計(jì)算移動(dòng)機(jī)器人的相對(duì)運(yùn)動(dòng)位姿,是一種相對(duì)定位的方法。與傳統(tǒng)的輪式里程計(jì)相比,視覺(jué)里程計(jì)不受車輪打滑或路面不平等環(huán)境限制,且獲取的視覺(jué)信息可用于避障和目標(biāo)識(shí)別等視覺(jué)應(yīng)用,具有更高的魯棒性和更廣的應(yīng)用場(chǎng)景。

早期針對(duì)視覺(jué)里程計(jì)的研究都服務(wù)于星球探測(cè)車。文獻(xiàn)[2]采用單目相機(jī)設(shè)計(jì)了一種“滑動(dòng)立體”的裝置,每當(dāng)機(jī)器人停下時(shí),相機(jī)水平滑行并等距拍攝9幀圖像,然后通過(guò)三角測(cè)量法重建特征點(diǎn)的三維坐標(biāo),并使用這些三維點(diǎn)求解運(yùn)動(dòng)信息。在文獻(xiàn)[2]研究的基礎(chǔ)上,文獻(xiàn)[3]使用雙目立體視覺(jué)系統(tǒng),并把重建的三維點(diǎn)的協(xié)方差矩陣用于運(yùn)動(dòng)估計(jì),一定程度上提高了精度。文獻(xiàn)[4]正式提出了Visual Odometry(VO),實(shí)現(xiàn)了魯棒去除外點(diǎn)的實(shí)時(shí)VO系統(tǒng),并首次在數(shù)百米距離上對(duì)VO算法進(jìn)行了測(cè)試。后期VO的總體框架基本依照文獻(xiàn)[4]方法,通過(guò)不斷改進(jìn)特征提取算法、特征匹配和跟蹤算法、運(yùn)動(dòng)估計(jì)算法等,來(lái)獲取更準(zhǔn)確、快速、魯棒的定位結(jié)果。視覺(jué)里程計(jì)最成功的應(yīng)用是火星探測(cè)機(jī)器人“勇氣號(hào)”和“機(jī)遇號(hào)”[5],它們采用的視覺(jué)里程計(jì)系統(tǒng)在火星上出色地完成了任務(wù),為視覺(jué)里程計(jì)的發(fā)展帶來(lái)極大動(dòng)力。近年來(lái)視覺(jué)里程計(jì)開(kāi)始被廣泛應(yīng)用在室內(nèi)外導(dǎo)航、自動(dòng)駕駛、增強(qiáng)現(xiàn)實(shí)(Augmented Reality,AR)等領(lǐng)域。

根據(jù)所用的傳感器不同,視覺(jué)里程計(jì)可以分為單目、雙目和RGBD。單目視覺(jué)里程計(jì)缺少尺度信息,不能獲得絕對(duì)的位置信息;RGBD視覺(jué)里程計(jì)受深度相機(jī)的影響較大,深度誤差和有效距離限制導(dǎo)致其自我定位精度較低;雙目視覺(jué)里程計(jì)能夠通過(guò)左右相機(jī)立體匹配獲取深度信息,恢復(fù)機(jī)器人的絕對(duì)的位置信息并且精度較高。

按照深度信息重建的方式,雙目視覺(jué)里程計(jì)又可以分為基于特征的方法[6]和直接法[7]。基于特征的方法通過(guò)提取圖像的特征點(diǎn)進(jìn)行特征匹配和稀疏三維重建,實(shí)時(shí)性較好;直接法對(duì)圖像中的所有像素進(jìn)行匹配,得到稠密的三維重建點(diǎn)云,很難保證實(shí)時(shí)性。因此,本文從實(shí)時(shí)性的角度考慮,設(shè)計(jì)基于特征點(diǎn)的雙目視覺(jué)里程計(jì)。

視覺(jué)里程計(jì)的幀間運(yùn)動(dòng)估計(jì)存在誤差,并且這種誤差會(huì)累積,如何減小這種誤差提高定位精度成為視覺(jué)里程計(jì)的研究重點(diǎn),可以根據(jù)是否引入概率模型將其分為基于概率的方法和基于非概率的方法?；诟怕实姆椒ㄖ饕谢跀U(kuò)展卡爾曼濾波器(Extended Kalman Filter,EKF)[8]的方式和基于粒子濾波器(Particle Filter,PF)[9]的方式等?；诟怕实姆椒ㄊ茉肼暦植加绊懘?會(huì)帶來(lái)非線性誤差,在大規(guī)模場(chǎng)景下很難保證精度和魯棒性。基于非概率的方法比較著名是基于關(guān)鍵幀的單目即時(shí)定位與地圖構(gòu)建算法PATM[10],該算法是非概率方法的基本框架。

本文設(shè)計(jì)一種基于特征點(diǎn)的雙目視覺(jué)里程計(jì)算法,根據(jù)關(guān)鍵幀的關(guān)系,針對(duì)幀間運(yùn)動(dòng)估計(jì)進(jìn)行局部?jī)?yōu)化,并采用回環(huán)檢測(cè)對(duì)累積誤差進(jìn)行全局優(yōu)化。

1 基于特征點(diǎn)的雙目視覺(jué)里程計(jì)

基于特征點(diǎn)的雙目視覺(jué)里程計(jì)首先在雙目相機(jī)獲取的左右圖像上進(jìn)行特征點(diǎn)匹配,通過(guò)匹配結(jié)果計(jì)算特征點(diǎn)深度信息;再根據(jù)前后幀的特征點(diǎn)跟蹤進(jìn)行運(yùn)動(dòng)估計(jì)求解當(dāng)前幀的位姿,包括旋轉(zhuǎn)矩陣R和平移矩陣T。

1.1 雙目相機(jī)模型

相機(jī)模型使用針孔模型,雙目相機(jī)在使用之前會(huì)經(jīng)過(guò)立體標(biāo)定[11],完成畸變矯正和平行校正。經(jīng)過(guò)標(biāo)定后的雙目相機(jī)模型如圖1所示。

圖1 算法整體流程

1.1.1 坐標(biāo)系定義

攝像機(jī)坐標(biāo)系:OciXciYciZci(i=1,2)。i=1表示左相機(jī);i=2表示右相機(jī);光軸方向Oc1Zc1和Oc2Zc1平行;光心之間相距Oc1Oc2=Tx表示基線。

圖像坐標(biāo)系:Oiuivi(i=1,2)。以像素為單位,左右相機(jī)具有相同的主點(diǎn)坐標(biāo)(u0,v0)。

世界坐標(biāo)系:OwXwYwZw。以左相機(jī)的攝像機(jī)坐標(biāo)系為世界坐標(biāo)系,并且第1幀圖像的攝像機(jī)坐標(biāo)系作為全局的世界坐標(biāo)系,所有圖像序列的位姿均相對(duì)于此世界坐標(biāo)系表示。

1.1.2 投影模型與三維重建

世界坐標(biāo)系中的點(diǎn)P(xw,yw,zw)投影到雙目相機(jī)中得到左右圖像中的P1(u1,v1)和P2(u2,v2)。設(shè)相機(jī)的內(nèi)參為K,以左相機(jī)為追蹤相機(jī),則在齊次坐標(biāo)系下投影方程表示為:

(1)

其中,s為比例因子,設(shè)為1。

對(duì)于空間中點(diǎn)P,通過(guò)三角計(jì)算恢復(fù)三維世界坐標(biāo)示意圖,如圖2所示。

圖2 雙目相機(jī)模型圖

P點(diǎn)在左右圖像中的視差d=u1-u2,由三角關(guān)系可得:

(2)

將式(2)代入式(1),得到:

(3)

1.2 特征點(diǎn)提取與匹配

雙目視覺(jué)里程計(jì)中的特征點(diǎn)匹配包括左右圖像之間的稀疏立體匹配和幀間運(yùn)動(dòng)的特征追蹤。為得到準(zhǔn)確的特征點(diǎn)三維坐標(biāo),需要特征點(diǎn)算子具有可重復(fù)性和穩(wěn)定性,包括光照不變性、旋轉(zhuǎn)不變性、尺度不變性等,同時(shí)兼顧實(shí)時(shí)性。本文選擇了一種基于GPU的加速尺度不變特征變換(Scale Invariant Feature Transform,SIFT)[12]算法SiftGPU[13]。SiftGPU縮短了SIFT算法的時(shí)間消耗,使其用于實(shí)時(shí)圖像處理成為可能。SIFT算子是一種提取局部特征的算法,在尺度空間尋找極值點(diǎn),提取位置、尺度、旋轉(zhuǎn)不變量,并對(duì)視角變化、仿射變換、噪聲也保持一定程度的穩(wěn)定性。

獲取左右圖像的SIFT特征點(diǎn)后,采用特征向量的歐式距離作為相似性度量。對(duì)于立體匹配,極線校正后的圖像匹配點(diǎn)基本處于同一行。對(duì)每個(gè)特征點(diǎn),搜索其在另一幅圖像中同一行和相鄰2行的特征點(diǎn)計(jì)算歐式距離。比較每組特征的最近和次近距離的比值,當(dāng)比值小于給定閾值,最近距離點(diǎn)判為匹配點(diǎn),否則判為無(wú)效匹配點(diǎn)。降低閾值能提高匹配精度,增大閾值能獲得更多的匹配點(diǎn)。本文通過(guò)對(duì)大量變換場(chǎng)景的左右圖像匹配實(shí)驗(yàn),選擇0.6作為匹配閾值。分別以左右圖為基準(zhǔn)進(jìn)行匹配,滿足左右一致性才能成為為正確的匹配點(diǎn)。幀間運(yùn)動(dòng)的特征追蹤以左圖為基準(zhǔn),需要遍歷更多的特征點(diǎn),匹配準(zhǔn)則和稀疏立體匹配相同。

1.3 增量式的運(yùn)動(dòng)估計(jì)

每一幀含有左右2張圖像,以左圖為基準(zhǔn),經(jīng)過(guò)左右立體匹配和前后幀間特征追蹤之后,得到了上一幀Ik-1和當(dāng)前幀Ik的3D坐標(biāo)匹配點(diǎn)集。假設(shè)從Ik-1到Ik相機(jī)經(jīng)過(guò)旋轉(zhuǎn)矩陣R和平移矩陣T,如圖3所示。

圖3 三維重建示意圖

1.3.1 幀間運(yùn)動(dòng)估計(jì)求解

(4)

由于每一組匹配點(diǎn)存在誤差(如式(5)所示),因此通過(guò)3D到3D的匹配點(diǎn)集求解旋轉(zhuǎn)矩陣R和平移矩陣T,使所有匹配點(diǎn)的誤差平方和最小化(如式(6)所示)。

(5)

(6)

求解上述方程,首先對(duì)3D匹配點(diǎn)集坐標(biāo)分別去質(zhì)心化,令兩組匹配點(diǎn)集質(zhì)心分別為:

(7)

因此,去質(zhì)心后的匹配點(diǎn)坐標(biāo)為:

(8)

將式(8)代入式(6)并展開(kāi)得:

(9)

式(9)表示的優(yōu)化問(wèn)題可以等價(jià)于式(10)和式(11):

(10)

‖p′-Rp-T‖2=0

(11)

R=UVT

(12)

將式(12)代入式(11)得到:

T=p′-UVTp

(13)

(14)

雙目視覺(jué)里程計(jì)通過(guò)這種增量的方式從第1幀開(kāi)始不斷獲得每幀相對(duì)于第1幀的位姿,實(shí)現(xiàn)定位的功能。

1.3.2 RANSAC運(yùn)動(dòng)估計(jì)

由于幀間運(yùn)動(dòng)的特征追蹤存在誤匹配點(diǎn),錯(cuò)誤的匹配點(diǎn)會(huì)導(dǎo)致式(10)的求解誤差增大。為降低幀間錯(cuò)誤匹配點(diǎn)給運(yùn)動(dòng)估計(jì)帶來(lái)的影響,本文采用隨機(jī)采樣一致性方法(RANSAC)[15]去除誤匹配點(diǎn)。主要步驟如下:

1)從匹配點(diǎn)集中隨機(jī)選擇m(m≥3)個(gè)匹配點(diǎn),利用1.3.1節(jié)的求解方法求解初始R、T。

2)根據(jù)初始R、T按式(5)計(jì)算誤差,如果小于給定閾值,該匹配點(diǎn)判斷為內(nèi)點(diǎn),否則為外點(diǎn)。

3)重復(fù)上述步驟S次,找到內(nèi)點(diǎn)數(shù)量最多的點(diǎn)集。

4)使用上述內(nèi)點(diǎn)集重新計(jì)算R、T,并且剔除外點(diǎn)內(nèi)的匹配點(diǎn)。

2 局部位姿優(yōu)化

上述雙目視覺(jué)里程計(jì)的運(yùn)動(dòng)估計(jì)只用了2幀之間的特征匹配,幀間約束較少,當(dāng)2幀匹配點(diǎn)較少或誤匹配太多時(shí),計(jì)算出的位姿誤差較大。本文在上述視覺(jué)里程計(jì)的基礎(chǔ)上提出了一種基于可變滑動(dòng)窗口的局部?jī)?yōu)化方法,增加幀間約束,提高幀間運(yùn)動(dòng)估計(jì)的精度和魯棒性。

2.1 關(guān)鍵幀與非關(guān)鍵幀

為了提升計(jì)算效率,本文算法將圖像序列分為關(guān)鍵幀和非關(guān)鍵幀。每個(gè)非關(guān)鍵幀和上一個(gè)關(guān)鍵幀一一對(duì)應(yīng),非關(guān)鍵幀的位姿相對(duì)于該關(guān)鍵幀表示,關(guān)鍵幀的位姿相對(duì)于第一個(gè)關(guān)鍵幀即全局世界坐標(biāo)系表示,如圖4所示。其中,第1幀設(shè)為關(guān)鍵幀,每隔fps幀設(shè)一個(gè)關(guān)鍵幀。由于旋轉(zhuǎn)過(guò)程中圖像變換過(guò)快,如果當(dāng)前幀相對(duì)于關(guān)鍵幀新出現(xiàn)的特征點(diǎn)占比超過(guò)10%,則當(dāng)前幀設(shè)為關(guān)鍵幀。

圖4 幀間運(yùn)動(dòng)估計(jì)圖

2.2 可變滑動(dòng)窗口位姿優(yōu)化

當(dāng)新的關(guān)鍵幀到來(lái)時(shí),會(huì)有一個(gè)可變滑動(dòng)窗口形成,如圖5所示。其中,路標(biāo)點(diǎn){P}是關(guān)鍵幀所觀察到特征點(diǎn)對(duì)應(yīng)的3D世界坐標(biāo)點(diǎn)集,zk是路標(biāo)點(diǎn)在第k個(gè)關(guān)鍵幀KFk上的投影坐標(biāo)?；瑒?dòng)窗口內(nèi)包含(m+1)個(gè)關(guān)鍵幀。窗口大小由當(dāng)前幀所觀察到的路標(biāo)點(diǎn)決定,當(dāng)前幀確定為關(guān)鍵幀后,會(huì)尋找和當(dāng)前幀觀察到相同路標(biāo)點(diǎn)的關(guān)鍵幀,如果相同路標(biāo)點(diǎn)的數(shù)量超過(guò)當(dāng)前幀的一半,關(guān)鍵幀進(jìn)入滑動(dòng)窗口,滑動(dòng)窗口大小加1。

圖5 關(guān)鍵幀和非關(guān)鍵幀示意圖

經(jīng)過(guò)增量式運(yùn)動(dòng)估計(jì)后的關(guān)鍵幀KFk的初始位姿設(shè)為Xk,第i個(gè)路標(biāo)點(diǎn)設(shè)為Pi,當(dāng)在關(guān)鍵幀KFk上能夠觀測(cè)到Pi時(shí),設(shè)觀測(cè)到的像素坐標(biāo)為zk,i,則可以用觀測(cè)方程表示:

zk,i=h(Xk,Pi)+ek,i

(15)

因此,觀測(cè)誤差表示為:

ek,i=zk,i-h(Xk,Pi)

(16)

用x=[X1,X2,…,XN,P1,P2,…,PM]表示滑動(dòng)窗口內(nèi)的N個(gè)關(guān)鍵幀位姿和M個(gè)路標(biāo)點(diǎn)3D坐標(biāo),則x是需要優(yōu)化的變量。假設(shè)路標(biāo)點(diǎn){P}在N個(gè)關(guān)鍵幀里的投影坐標(biāo)共有S個(gè),即滿足zk,i有值的(k,i)共有S對(duì),那么局部位姿優(yōu)化將通過(guò)調(diào)整Pi和Xk的值來(lái)使得所有的觀測(cè)誤差平方和最小,即最小化以下目標(biāo)函數(shù):

(17)

其中,Ωj為信息矩陣,是觀測(cè)誤差協(xié)方差矩陣的逆。

對(duì)式(17)采用高斯-牛頓[16]方法求解,對(duì)每一個(gè)誤差項(xiàng)ej(x)和微小增量Δx,有:

(18)

將式(18)代入(17)式并作差ΔJ=J(x⊕Δx)-J(x),得到:

(19)

(20)

通過(guò)求解式(20)這個(gè)線性方程得到目標(biāo)函數(shù)的梯度Δx,并對(duì)關(guān)鍵幀的位姿Xk和路標(biāo)點(diǎn)坐標(biāo)Pi進(jìn)行更新:

x′=x⊕Δx

(21)

重復(fù)式(18)～式(21)直到優(yōu)化變量x收斂,此時(shí)關(guān)鍵幀的位姿Xk和路標(biāo)點(diǎn)Pi局部?jī)?yōu)化完成。非關(guān)鍵幀的全局位姿會(huì)隨著關(guān)鍵幀的位姿局部?jī)?yōu)化而得到更新。

3 全局位姿優(yōu)化

局部位姿優(yōu)化能夠減小雙目視覺(jué)里程計(jì)的幀間運(yùn)動(dòng)估計(jì)的誤差,隨著相機(jī)的運(yùn)動(dòng),幀間運(yùn)動(dòng)估計(jì)的誤差會(huì)因?yàn)槔鄯e而變大,導(dǎo)致軌跡漂移。為了減小累積誤差,本文采用了一種基于回環(huán)檢測(cè)的全局位姿優(yōu)化方法,一旦回環(huán)檢測(cè)成功,將對(duì)回環(huán)內(nèi)的所有關(guān)鍵幀位姿和路標(biāo)點(diǎn)進(jìn)行全局優(yōu)化。

3.1 回環(huán)檢測(cè)

回環(huán)檢測(cè)需要在圖像序列中找到出現(xiàn)在同一位置的關(guān)鍵幀,可以通過(guò)模式識(shí)別的方式來(lái)實(shí)現(xiàn)。本文采用詞袋模型[18]進(jìn)行回環(huán)檢測(cè)。

詞袋模型最早出現(xiàn)在信息檢索領(lǐng)域中,忽略文本的語(yǔ)法與詞序,用無(wú)序的單詞表示文檔。在計(jì)算機(jī)視覺(jué)中,圖像類比于文檔,圖像塊中的特征向量類比于單詞,則圖像的詞袋模型可用圖像中所有圖像塊的特征向量表示。本算法使用SiftGPU提取圖像中的局部不變特征向量作為視覺(jué)詞匯,并構(gòu)建相應(yīng)單詞表,用單詞表中的單詞來(lái)表示每一個(gè)關(guān)鍵幀。具體步驟如下:

1)利用SiftGPU對(duì)大量的圖像數(shù)據(jù)集進(jìn)行特征提取和描述,獲得作為視覺(jué)詞匯的特征向量集。

2)利用K-means[19]聚類方法對(duì)特征向量集進(jìn)行聚類,得到K個(gè)聚類中心,并且用聚類中心構(gòu)造sift單詞表。

3)對(duì)關(guān)鍵幀提取特征點(diǎn)和特征向量后,統(tǒng)計(jì)特征向量在單詞表中出現(xiàn)的次數(shù),將關(guān)鍵幀表示成關(guān)于sift單詞表的K維向量。

對(duì)每一個(gè)關(guān)鍵幀會(huì)記錄其Sift詞袋模型向量,當(dāng)新的關(guān)鍵幀到來(lái)時(shí),通過(guò)計(jì)算詞袋模型向量之間的距離來(lái)查詢之前關(guān)鍵幀中相似的關(guān)鍵幀。將當(dāng)前幀和相似度超過(guò)閾值的關(guān)鍵幀進(jìn)行特征匹配和幀間運(yùn)動(dòng)估計(jì),如果相似關(guān)鍵幀相對(duì)于當(dāng)前關(guān)鍵幀具有微小的位移t和足夠多的匹配點(diǎn),且相似關(guān)鍵幀到當(dāng)前關(guān)鍵幀之間初始位姿存在較大的位移,則判斷回環(huán)成功。

3.2 全局優(yōu)化窗口位姿優(yōu)化

當(dāng)前關(guān)鍵幀KFk與關(guān)鍵幀KFj回環(huán)檢測(cè)成功,如圖6所示。

圖6 局部?jī)?yōu)化中的可滑動(dòng)窗口示意圖

(22)

同時(shí)在KFk處構(gòu)造可變滑動(dòng)窗口,利用2.2節(jié)的方法對(duì)當(dāng)前關(guān)鍵幀KFk和滑動(dòng)窗口內(nèi)的關(guān)鍵幀KFj～KFj+m的位姿進(jìn)行局部?jī)?yōu)化。

當(dāng)可變滑動(dòng)窗口中的關(guān)鍵幀局部位姿優(yōu)化完畢,可變滑動(dòng)窗口擴(kuò)大至整個(gè)回環(huán)變成全局優(yōu)化窗口,開(kāi)始進(jìn)行全局優(yōu)化。全局優(yōu)化的計(jì)算方法和式(17)～式(21)相同,不同的是在優(yōu)化的過(guò)程中保持關(guān)鍵幀KFk和可變滑動(dòng)窗口內(nèi)的關(guān)鍵幀KFj～KFj+m的位姿不變,此時(shí)的優(yōu)化變量變?yōu)閤=[Xj+m+1,Xj+m+2,…,Xk-1,P1,P2,…,PM],目標(biāo)函數(shù)變?yōu)?

(23)

全局優(yōu)化完成對(duì)全局優(yōu)化窗口內(nèi)的所有關(guān)鍵幀位姿和路標(biāo)點(diǎn)坐標(biāo)的優(yōu)化,同時(shí)更新非關(guān)鍵幀的位姿。經(jīng)過(guò)全局優(yōu)化后的移動(dòng)機(jī)器人累積位姿誤差得到顯著消除,軌跡漂移減少。

至此,基于局部和全局優(yōu)化的雙目視覺(jué)里程計(jì)算法的流程如圖7所示。

圖7 全局優(yōu)化示意圖

4 實(shí)驗(yàn)與結(jié)果分析

4.1 實(shí)驗(yàn)數(shù)據(jù)與運(yùn)行環(huán)境

本文實(shí)驗(yàn)所用電腦型號(hào)為MS-7885(CPU酷睿i7-5820k,主頻3.30 GHz,內(nèi)存16 GB),運(yùn)行系統(tǒng)為L(zhǎng)inux。測(cè)試數(shù)據(jù)選擇計(jì)算機(jī)視覺(jué)算法測(cè)評(píng)平臺(tái)KITTI[20]的雙目圖像數(shù)據(jù)集。該數(shù)據(jù)集是由搭載PointGray Flea2雙目相機(jī)和OXTS RT3003 IMU/GPS導(dǎo)航系統(tǒng)的車輛在城鎮(zhèn)街道行駛獲取的,其中雙目圖像序列作為算法的輸入,IMU/GPS的數(shù)據(jù)作為參考真實(shí)值評(píng)價(jià)算法的的精度。實(shí)驗(yàn)數(shù)據(jù)相關(guān)參數(shù)如表1和表2所示,實(shí)驗(yàn)環(huán)境的真實(shí)場(chǎng)景如圖8所示。

表1 雙目相機(jī)參數(shù)

表2 圖像序列參數(shù)

圖8 實(shí)驗(yàn)環(huán)境真實(shí)場(chǎng)景

4.2 算法性能分析與比較

4.2.1 算法性能分析

為清楚地看到局部?jī)?yōu)化和全局優(yōu)化對(duì)位姿誤差的消除效果,本文分別對(duì)算法在不做優(yōu)化、只做局部?jī)?yōu)化、只做全局優(yōu)化、局部和全局優(yōu)化結(jié)合的定位效果進(jìn)行了實(shí)驗(yàn)分析。圖9是算法在4種情況下計(jì)算出的相機(jī)運(yùn)動(dòng)軌跡和實(shí)際參考運(yùn)動(dòng)軌跡的比較?？梢钥吹?算法在不做優(yōu)化時(shí)計(jì)算出的運(yùn)動(dòng)軌跡出現(xiàn)明顯漂移,經(jīng)過(guò)局部和全局優(yōu)化后,運(yùn)動(dòng)軌跡和參考值基本重合。

圖9 局部?jī)?yōu)化和全局優(yōu)化對(duì)相機(jī)運(yùn)動(dòng)軌跡的影響

實(shí)驗(yàn)分別比較了不同的距離和速度下位姿誤差,結(jié)果如圖10所示。其中距離范圍為50 m～800 m,每間隔50 m計(jì)算相應(yīng)的旋轉(zhuǎn)誤差和平移誤差,速度從6 m/s～16 m/s,每間隔2 m/s計(jì)算相應(yīng)的旋轉(zhuǎn)誤差和平移誤差,表3是相應(yīng)的統(tǒng)計(jì)誤差范圍。

圖10 局部?jī)?yōu)化和全局優(yōu)化對(duì)位姿誤差的影響

誤差項(xiàng)旋轉(zhuǎn)誤差/(deg·m-1)平移誤差/%不做優(yōu)化0.0210～0.0451.10～6.80局部?jī)?yōu)化0.0016～0.0110.60～0.90全局優(yōu)化0.0020～0.0140.20～1.40局部+全局優(yōu)化0.0006～0.0070.09～0.45

可以看出,算法在不做優(yōu)化時(shí),旋轉(zhuǎn)誤差和平移誤差較大。旋轉(zhuǎn)誤差最大達(dá)到0.045 deg/m,平移誤差最大達(dá)到6.8%,同時(shí)圖10(b)和圖10(d)顯示,隨著運(yùn)動(dòng)距離和速度的增加,平移誤差有逐漸增大的過(guò)程,這是算法在不做優(yōu)化時(shí)位姿誤差累積的結(jié)果。同時(shí)可以看到局部?jī)?yōu)化和全局優(yōu)化分別能夠單獨(dú)減小位姿誤差,而且局部?jī)?yōu)化對(duì)位姿誤差的消除效果稍微要好于全局優(yōu)化,這是因?yàn)榫植績(jī)?yōu)化始終在進(jìn)行,而全局優(yōu)化只有在回環(huán)檢測(cè)成功的時(shí)候進(jìn)行。

從圖10(b)中可以看到,距離達(dá)到600 m時(shí),全局優(yōu)化的平移誤差開(kāi)始小于局部?jī)?yōu)化。這是由于此時(shí)場(chǎng)景中出現(xiàn)回環(huán),全局優(yōu)化開(kāi)始進(jìn)行,減小位姿誤差的累積,定位更加精確。當(dāng)局部?jī)?yōu)化和全局優(yōu)化同時(shí)進(jìn)行時(shí),隨著運(yùn)動(dòng)距離和速度的增加,旋轉(zhuǎn)誤差和平移誤差基本保持最小并下降至趨于穩(wěn)定,定位誤差最小。

表4顯示了本文算法的時(shí)間消耗?？梢钥闯?基于局部?jī)?yōu)化和全局優(yōu)化的雙目視覺(jué)里程計(jì)沒(méi)有顯著增加每幀的處理時(shí)間,每幀耗時(shí)約90 ms,能夠達(dá)到10 frame/s以上,基本滿足實(shí)時(shí)性要求。

表4 平均每幀耗時(shí)比較 ms

4.2.2 算法性能比較

為了比較本文算法和其他雙目視覺(jué)里程計(jì)算法精度,本文同時(shí)實(shí)驗(yàn)了Andreas Geiger等人開(kāi)發(fā)的視覺(jué)里程計(jì)C++庫(kù)libviso2[21]。libviso2是基于特征點(diǎn)匹配的雙目視覺(jué)里程計(jì),但其算法沒(méi)有本文算法中的局部?jī)?yōu)化和全局優(yōu)化。

圖11是本文算法和libviso2算法分別在實(shí)驗(yàn)數(shù)據(jù)集運(yùn)行后的平面軌跡圖。可以明顯看出,libviso2算法的運(yùn)動(dòng)軌跡在較長(zhǎng)時(shí)間后開(kāi)始偏離參考值,且偏移量越來(lái)越大。而本文算法運(yùn)動(dòng)軌跡基本和參考值保持一致,沒(méi)有明顯的偏移。

圖11 本文算法和libviso2算法的相機(jī)運(yùn)動(dòng)軌跡圖

圖12是本文算法和libviso2算法計(jì)算出的位姿誤差分析圖,從中可以明顯看出,本文的算法在不同速度和距離下旋轉(zhuǎn)誤差和平移誤差都明顯低于libviso2算法。libviso2算法的旋轉(zhuǎn)誤差最大達(dá)到0.018 deg/m,平移誤差最大達(dá)到1.2%,而本文算法旋轉(zhuǎn)誤差最大為0.007 deg/m,平移誤差最大為0.45%。從圖12(b)中可以看到,隨著距離的增加,libviso2算法的平移誤差逐漸增加,而本文算法的平移誤差逐漸減小,說(shuō)明局部?jī)?yōu)化和全局優(yōu)化能夠顯著減小幀間運(yùn)動(dòng)估計(jì)的誤差和累積誤差,定位更精確。

綜上所述,本文算法在滿足實(shí)時(shí)性要求的基礎(chǔ)上,能夠顯著減小位姿誤差,提高定位精度。

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

本文提出基于局部和全局優(yōu)化的雙目視覺(jué)里程計(jì)算法,設(shè)計(jì)了利用加速SIFT特征點(diǎn)匹配和RANSAC運(yùn)動(dòng)估計(jì)的雙目視覺(jué)里程計(jì)。通過(guò)可變滑動(dòng)窗口對(duì)位姿進(jìn)行局部?jī)?yōu)化,減小幀間運(yùn)動(dòng)估計(jì)的誤差,并采用回環(huán)檢測(cè)對(duì)位姿進(jìn)行全局優(yōu)化減小誤差累積。實(shí)驗(yàn)結(jié)果表明,本文算法在滿足實(shí)時(shí)性要求的同時(shí)具有較高的定位精度。目前多傳感器融合是移動(dòng)機(jī)器人導(dǎo)航的趨勢(shì),因此,下一步將設(shè)計(jì)雙目和慣性導(dǎo)航融合的視覺(jué)里程計(jì),提高定位算法的魯棒性和精度。

[1] SCARAMUZZA D,FRAUNDORFER F.Visual Odometry[J].IEEE Robotics & Automation Magazine,2011,18(4):80-92.

[2] MORAVEC H P.Obstacle Avoidance and Navigation in the Real World by a Seeing Robot Rover[M].Stanford,USA:Stanford University,1980.

[3] MATTHIES L,SHAFER S A.Error Modeling in Stereo Navigation[J].IEEE Journal of Robotics & Automation,1987,3(3):239-248.

[4] NISTER D,NARODITSKY O,BERGEN J.Visual Odometry[C]//Proceedings of 2004 IEEE Computer Society Conference on Computer Vision and Pattern Recognition.Washington D.C.,USA:IEEE Press,2004:652-659.

[5] MAIMONE M,CHENG Y,MATTHIES L.Two Years of Visual Odometry on the Mars Exploration Rovers[J].Journal of Field Robotics,2007,24(3):169-186.

[7] ENGEL J,SCH?PS T,CREMERS D.LSD-SLAM:Large-scale Direct Monocular SLAM[C]//Proceedings of European Conference on Computer Vision.Berlin,Germany:Springer,2014:834-849.

[8] DAVISON A J,REID I D,MOLTON N D,et al.MonoSLAM:Real-time Single Camera SLAM[J].IEEE Transactions on Pattern Analysis and Machine Intelligence,2007,29(6):1052-1067.

[9] MONTEMERLO M,THRUN S,KOLLER D,et al.FastSLAM:A Factored Solution to the Simultaneous Localization and Mapping Problem[C]//Proceedings of National Conference on Artificial Intelligence.New York,USA:ACM Press,2002:593-598.

[10] KLEIN G,MURRAY D.Parallel Tracking and Mapping for Small AR Workspaces[C]//Proceedings of the 6th IEEE and ACM International Symposium on Mixed and Augmented Reality.Washington D.C.,USA:IEEE Press,2007:225-234.

[11] 劉俸材,謝明紅,王 偉.雙目視覺(jué)的立體標(biāo)定方法[J].計(jì)算機(jī)工程與設(shè)計(jì),2011,32(4):1508-1512.

[12] LOWE D G.Distinctive Image Features from Scale-invariant Keypoints[J].International Journal of Computer Vision,2004,60(2):91-110.

[13] WU Changchang.SiftGPU:A GPU Implementation of Scale Invariant Feature Transform(SIFT)[D].Chapel Hill,USA:University of North Carolina at Chapel Hill,2013.

[14] 張賢達(dá).矩陣分析與應(yīng)用[M].北京:清華大學(xué)出版社,2004.

[15] RAGURAM R,CHUM O,POLLEFEYS M,et al.USAC:A Universal Framework for Random Sample Consensus[J].IEEE Transactions on Pattern Analysis and Machine Intelligence,2013,35(8):2022-2038.

[16] DENNIS J E,SCHNABEL R B.Numerical Methods for Unconstrained Optimization and Nonlinear Equations(Classics in Applied Mathematics,16)[M].[S.l.]:Prentice-Hall,1983.

[17] VARADARAJAN V S.Lie Groups,Lie Algebras,and Their Representations[M].Berlin,Germany:Springer,2013.

[19] AGARWAL S.Data Mining:Data Mining Concepts and Techniques[C]//Proceedings of International Conference on Machine Intelligence and Research Advancement.Washington D.C.,USA:IEEE Press,2013:203-207.

[20] GEIGER A,LENZ P,STILLER C,et al.Vision Meets Robotics:The KITTI Dataset[J].International Journal of Robotics Research,2013,32(11):1231-1237.

[21] GEIGER A,ZIEGLER J,STILLER C.StereoScan:Dense 3d Reconstruction in Real-time[C]//Proceedings of Intelligent Vehicles Symposium.Washington D.C.,USA:IEEE Press,2011:963-968.