黃秀珍，王佳斌，高發(fā)欽，冉宇瑤

(1.浙江理工大學(xué) 科技與藝術(shù)學(xué)院，浙江 上虞 312369；2.浙江理工大學(xué)，浙江 杭州 315700)

0 引言

隨著時(shí)代進(jìn)步，人工智能、圖像處理及計(jì)算機(jī)視覺(jué)等技術(shù)迅速發(fā)展，基于這些技術(shù)的應(yīng)用，如智能機(jī)器人、無(wú)人機(jī)和無(wú)人駕駛汽車等已經(jīng)廣泛出現(xiàn)在人們生活的各個(gè)領(lǐng)域和方面[1]。無(wú)論智能機(jī)器人還是無(wú)人駕駛汽車，空間定位是最基本和關(guān)鍵的問(wèn)題。視覺(jué)圖像中含有豐富的位置和空間信息[2]，機(jī)器視覺(jué)主要通過(guò)攝像機(jī)來(lái)模擬人的視覺(jué)系統(tǒng)。相對(duì)慣性導(dǎo)航，機(jī)器視覺(jué)定位誤差對(duì)累積誤差的抑制能力更強(qiáng)，不會(huì)受到未知空間、環(huán)境和長(zhǎng)距離的影響[3]。

機(jī)器視覺(jué)里程計(jì)分為單目視覺(jué)里程計(jì)和立體視覺(jué)里程計(jì)，其中單目視覺(jué)里程計(jì)需要外部信息進(jìn)行約束才能獲取空間尺度，立體視覺(jué)里程計(jì)可以通過(guò)三角測(cè)量獲得特征點(diǎn)深度信息，根據(jù)深度信息和特征點(diǎn)的二維位置求出相機(jī)的位姿變化，最終實(shí)現(xiàn)空間定位。立體視覺(jué)里程計(jì)中雙目視覺(jué)容易實(shí)現(xiàn)，僅需2個(gè)相機(jī)[4]。

本文提出機(jī)器雙目視覺(jué)里程計(jì)定位算法，假定相機(jī)的初始位置已知，相機(jī)的實(shí)時(shí)位置信息可由初始位置和計(jì)算出的位姿變化通過(guò)遞推計(jì)算得到[5]。

1 里程計(jì)定位算法設(shè)計(jì)

本文研究機(jī)器雙目視覺(jué)里程計(jì)定位算法，主要經(jīng)過(guò)相機(jī)標(biāo)定、圖像處理、特征提取和匹配以及運(yùn)動(dòng)估計(jì)最終得到定位結(jié)果。

相機(jī)標(biāo)定采用Matlab自帶的基于張正友標(biāo)定法的APP實(shí)現(xiàn)。圖像處理主要是為了減小計(jì)算量，加快程序運(yùn)行速度，包含灰度化和圖像校正2步。特征提取本文選擇了基于自定義模板的特征點(diǎn)提取方法，能夠提取變化明顯的特征點(diǎn)，特征匹配采取了魯棒性較強(qiáng)的環(huán)形匹配。運(yùn)動(dòng)估計(jì)首先使用RANSAC隨機(jī)抽樣3組點(diǎn)，然后使用P3P算法計(jì)算它們各自在相機(jī)坐標(biāo)系下的三維位置，然后利用Bundle Adjustment進(jìn)行優(yōu)化，利用高斯牛頓法求解位姿，并將重投影誤差和最小的位姿變換作為最優(yōu)位姿變換。然后根據(jù)前一幀的位置即可計(jì)算出當(dāng)前幀的位姿完成定位。

1.1 相機(jī)標(biāo)定及圖像處理算法

相機(jī)標(biāo)定的目的是獲取2個(gè)相機(jī)的內(nèi)參模型和外參模型。綜合考慮標(biāo)定精確度、標(biāo)定快慢以及標(biāo)定物制作難易程度，本文使用張正友標(biāo)定法[6]。由于在Matlab 2016b已經(jīng)集成了該方法的標(biāo)定APP，故只需準(zhǔn)備好標(biāo)定板，拍攝不同角度的圖像，再導(dǎo)入計(jì)算機(jī)，利用Matlab中的Stereo camera calibrator進(jìn)行計(jì)算即可。

圖像處理包含灰度化和校正[7]。相機(jī)拍攝得到的圖像為彩色圖像，為了加快程序運(yùn)行的速度，需要對(duì)圖像進(jìn)行灰度化處理。本文選擇加權(quán)平均法對(duì)圖像進(jìn)行灰度化處理，該方法灰度化效果最好。圖像校正是為了使左右相機(jī)之間的像素行對(duì)準(zhǔn)，方便視差計(jì)算也能加快特征匹配。本文選擇常用的Bouguet算法進(jìn)行校正。

1.2 特征提取及匹配算法

特征提取是為了提取圖像中一些特別的地方即特征[8]，本文選擇較易提取和信息量大的點(diǎn)特征。圖像特征點(diǎn)提取的過(guò)程可以分為2步：首先是找關(guān)鍵點(diǎn)，灰度值發(fā)生劇烈變化的點(diǎn)可以作為關(guān)鍵點(diǎn)，然后計(jì)算描述子，描述子描述了關(guān)鍵點(diǎn)及其周圍像素的信息。

選擇基于模板的特征點(diǎn)檢測(cè)方法，計(jì)算速度較快且魯棒性較高[9]。選用的5×5的Blod模板和5×5的Corner模板，如圖1所示。輸入圖像進(jìn)行計(jì)算，計(jì)算Blod算子和Corner算子的核函數(shù)響應(yīng)，獲得候選特征點(diǎn)[10]。

接下來(lái)計(jì)算描述子。本文所選描述子計(jì)算方法是計(jì)算特征點(diǎn)周圍11×11的窗口中16個(gè)點(diǎn)的Sobel濾波響應(yīng)，如圖2所示。

圖1 Blod模板和Corner模板

圖2 描述子和環(huán)形匹配

選擇最小平方差(SSD)來(lái)計(jì)算匹配代價(jià)。計(jì)算方法如式(1)：

(1)

式中，i，j表示圖像物理坐標(biāo)系中某點(diǎn)到特征點(diǎn)(x,y)的水平和垂直距離；w表示16個(gè)點(diǎn)的位置集合；L表示圖2(a)中某點(diǎn)的Sobel濾波響應(yīng)；R表示圖2(b)中某點(diǎn)的Sobel濾波響應(yīng)；D表示視差。

環(huán)形匹配如圖2(b)所示。從前一幀的左圖像中的一個(gè)特征點(diǎn)Pl1出發(fā)，找到該點(diǎn)在前一幀右圖像中的對(duì)應(yīng)點(diǎn)Pr1，左右2幅圖像之間采用極線搜索和塊匹配來(lái)尋找對(duì)應(yīng)點(diǎn)，采用最小平方差(SSD)來(lái)計(jì)算匹配代價(jià)。然后根據(jù)前一幀右圖像中的點(diǎn)Pr1，找到當(dāng)前幀右圖像中的對(duì)應(yīng)點(diǎn)Pr2。再使用極線搜索和塊匹配找到當(dāng)前幀左圖像中匹配點(diǎn)Pl2的位置。然后計(jì)算Pl1和Pl2的匹配代價(jià)，若代價(jià)基本一致，則表示實(shí)現(xiàn)了環(huán)形匹配，否則失敗，選另一個(gè)特征點(diǎn)進(jìn)行環(huán)形匹配。

1.3 運(yùn)動(dòng)估計(jì)算法

本文使用的運(yùn)動(dòng)估計(jì)方法先用P3P算法[11]計(jì)算出一個(gè)初始位姿變換，然后用Bundle Adjustment算法進(jìn)行優(yōu)化。P3P算法只需要3個(gè)點(diǎn)對(duì)就可以估計(jì)得到位姿變化[12]。

如圖3所示，A,B,C三點(diǎn)為世界坐標(biāo)系下特征點(diǎn)位置，世界坐標(biāo)系是真實(shí)世界的坐標(biāo)系也是系統(tǒng)的絕對(duì)坐標(biāo)系。a,b,c為對(duì)應(yīng)點(diǎn)在圖像像素坐標(biāo)系下的位置。由已知像素坐標(biāo)，采用余弦定理計(jì)算出A,B,C三點(diǎn)在相機(jī)坐標(biāo)系下的三維位置。假設(shè)相機(jī)坐標(biāo)系中心點(diǎn)為O。

圖3 投影模型

記x=OA/OC,y=OB/OC,v=AB2/OC2,uv=BC2/OC2,wv=AC2/OC2，x表示投影模型OA邊和OC邊的比值，其他類似，這樣方便根據(jù)余弦定理推出：

(1-u)y2-ux2-cos〈b,c〉y+2uxycos〈a,b〉+1=0，(1-w)x2-wy2-cos〈a,c〉x+2wxycos〈a,b〉+1=0 ，

(2)

式中，x,y為未知量，用吳消元法求解得到A,B,C在相機(jī)坐標(biāo)系下的3D坐標(biāo)。如圖4所示。

圖4 前后圖像投影比較

圖4中，Pi表示匹配特征點(diǎn)集，A,B,C屬于Pi，ui表示Pi的投影點(diǎn)集，a,b,c屬于ui。因?yàn)橥籔i在前后幀的相機(jī)坐標(biāo)系下的三維位置已知，可以計(jì)算Pi的位姿變換，得到相機(jī)的位姿變換。

為了提高數(shù)據(jù)利用率，本文采取RANSAC算法隨機(jī)選擇所使用的特征點(diǎn)，然后求解最小化重投影誤差[13]。重投影是相對(duì)于相機(jī)第一次采集圖像而言，所謂重投影是將P點(diǎn)再次投影到成像平面上，如圖5所示。

圖5 重投影模型

圖5中，P1是P點(diǎn)在像素坐標(biāo)系中的實(shí)際位置，P2是相機(jī)位姿變化后的P點(diǎn)投影到成像平面上的位置，P2和P1之間的距離就是重投影誤差，要不斷調(diào)整投影角度，使P2和P1之間的距離e最小。

考慮到多個(gè)空間點(diǎn)P和它們對(duì)應(yīng)的投影點(diǎn)，將旋轉(zhuǎn)矩陣R和平移向量t用李代數(shù)表示為ξ，假設(shè)某個(gè)空間點(diǎn)表示為Pi=[Xi,Yi,Zi]T，其投影的像素坐標(biāo)為Ui=(ui,vi)T，式(3)表示對(duì)n個(gè)投影點(diǎn)誤差求和，單位是像素，最終確定誤差和最小時(shí)的變換矩陣。

(3)

式中，si為深度距離，可由三角測(cè)量和視差原理得到；K為內(nèi)參矩陣由標(biāo)定得到；exp(ξ∧)是從世界坐標(biāo)系到相機(jī)坐標(biāo)系的變換矩陣。

最小二乘優(yōu)化[14]計(jì)算過(guò)程主要分成2步，第1步用RANSAC采樣選取特征點(diǎn)，第2步用高斯牛頓法求解位姿變換Ti。

位姿計(jì)算的輸入是前面采樣得到的一組匹配特征點(diǎn)的像素坐標(biāo)中的點(diǎn)，一組點(diǎn)有4個(gè)像素坐標(biāo)系下的位置，分別是(u1c,v1c),(u2c,v2c),(u1p,v1p),(u2p,v2p) 。1表示左，2表示右，c表示當(dāng)前幀，p表示前一幀。(X1c,Y1c,Z1c),(X1p,Y1p,Z1p)由P3P算法計(jì)算得到。(X2c,Y2c,Z2c),(X2p,Y2p,Z2p) 由對(duì)極幾何原理得到：

(4)

旋轉(zhuǎn)矩陣和平移向量可以表示載體的位置和姿態(tài)變化。為了簡(jiǎn)化計(jì)算，用歐拉角來(lái)表示旋轉(zhuǎn)矩陣，用Ti={αi,βi,γi,txi,tyi,tzi}表示迭代中第i次的位姿變換。前3個(gè)參數(shù)是相機(jī)間關(guān)于X,Y,Z軸各自的旋轉(zhuǎn)，后3個(gè)是關(guān)于X,Y,Z軸方向上各自的平移。旋轉(zhuǎn)矩陣用歐拉角可表示為:

(5)

Ti通過(guò)RANSAC迭代，在每次迭代中都從匹配點(diǎn)中隨機(jī)抽取3個(gè)點(diǎn)通過(guò)高斯牛頓法求出。

將(X1c,Y1c,Z1c),(X2c,Y2c,Z2c)變換二維坐標(biāo)的過(guò)程就是重投影。依據(jù)重投影后的二維坐標(biāo)(u,v)，其雅克比計(jì)算方式為:

(6)

進(jìn)行高斯牛頓迭代，通過(guò)計(jì)算3組點(diǎn)中的雅克比矩陣，最終，可以得到：

(7)

式中，N為抽樣組數(shù)，本文N為3，J為6×12的矩陣。

(8)

往復(fù)迭代，直到T收斂(|T(i+1)-T(i)|<ε)。

(9)

式中，τ為人為設(shè)定。

得到最優(yōu)位姿T后再將其變換成R和t，由式(10)計(jì)算得到世界坐標(biāo)系下當(dāng)前幀的位置。

(10)

對(duì)于運(yùn)動(dòng)過(guò)程中采集的連續(xù)圖像序列可以采取遞進(jìn)的方式來(lái)計(jì)算位置，只要知道初始幀的坐標(biāo)，之后的坐標(biāo)都可以在前一幀的基礎(chǔ)上得到。

2 仿真結(jié)果及分析

選取了KITTI數(shù)據(jù)集，該數(shù)據(jù)集由德國(guó)卡爾斯魯厄理工學(xué)院和豐田美國(guó)技術(shù)研究院聯(lián)合創(chuàng)辦，是較大的計(jì)算機(jī)視覺(jué)算法評(píng)測(cè)數(shù)據(jù)集，取其中不同街道下的5組80幀連續(xù)圖像作為輸入，運(yùn)用本文算法進(jìn)行計(jì)算，并和汽車自帶IMU(慣性測(cè)量單元)測(cè)得數(shù)據(jù)進(jìn)行比較。

將KITTI數(shù)據(jù)集中的標(biāo)號(hào)為19和20的數(shù)據(jù)集作為本文數(shù)據(jù)集1和數(shù)據(jù)集2，各選取連續(xù)80幀圖像，在Matlab下運(yùn)行本文程序得到圖6。

圖6 不同數(shù)據(jù)集下汽車軌跡圖展示

圖6展示了不同數(shù)據(jù)集下的汽車運(yùn)動(dòng)路徑及最終位置，其中曲線表示汽車的運(yùn)動(dòng)軌跡，以初始幀為初始狀態(tài)，此時(shí)汽車位于(0,0)處，橫軸表示汽車較初始位置的左右變化，縱軸表示汽車較初始位置的前后變化，單位為m，高度看作不變。

任選數(shù)據(jù)集1中的某一幀，展示前后幀的位姿變換矩陣，如下所示：

計(jì)算該位姿變換時(shí)所用的匹配特征點(diǎn)為241對(duì)，局內(nèi)點(diǎn)為87.6%，精確度較高。本文將汽車初始位置設(shè)為(0,0,0)，則最終的位置為(8.39,133.5，-17.1)。

采用絕對(duì)軌跡誤差A(yù)TE進(jìn)行誤差評(píng)估，絕對(duì)軌跡誤差A(yù)TE的計(jì)算如式(11)所示。

(11)

式中，Ei為第i幀的誤差，xi,yi為通過(guò)視覺(jué)計(jì)算得到的第i幀相機(jī)的水平位置，Xi,Yi表示IMU測(cè)量得到的第i幀相機(jī)的水平位置。

統(tǒng)計(jì)所有時(shí)刻的絕對(duì)軌跡誤差，然后求平均就可以得到平均絕對(duì)誤差MATE：

(12)

終點(diǎn)軌跡誤差FATE由IMU測(cè)量終點(diǎn)和由視覺(jué)里程計(jì)測(cè)量終點(diǎn)依據(jù)式(11)計(jì)算出。

本文選取了5組不同數(shù)據(jù)集，來(lái)自KITTI數(shù)據(jù)集中的標(biāo)號(hào)為19～23的數(shù)據(jù)集，在這5組數(shù)據(jù)集中各選取80幀連續(xù)圖像進(jìn)行計(jì)算，結(jié)果如表1所示。

表1 不同數(shù)據(jù)集定位精度比較

數(shù)據(jù)集MATE/mFATE/m134.667.2218.336.137.426.245.18.7568129平均26.6853.44

由表1可知，在這5組數(shù)據(jù)集下，本文算法平均MATE在26.68 m，平均FATE為53.44 m。精度最高的是第4組數(shù)據(jù)，第4組中汽車接近直線行駛，且馬路上沒(méi)有其他車輛，只有路邊建筑和地面在變化，因此準(zhǔn)確度較高。第5組誤差最大，但也是接近直線行駛，不同之處在于馬路上汽車較多，因此推測(cè)其他車輛對(duì)視覺(jué)定位精度影響較大。

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

本文通過(guò)研究實(shí)現(xiàn)了雙目視覺(jué)里程計(jì)的定位算法，主要經(jīng)過(guò)相機(jī)標(biāo)定、圖像處理、特征提取和匹配以及運(yùn)動(dòng)估計(jì)算法，最終得到行駛汽車的運(yùn)動(dòng)軌跡，精度較高。但適應(yīng)性不強(qiáng)，在馬路上沒(méi)有其他車輛的情況下精度才比較高。不過(guò)，可以使用視覺(jué)與其他導(dǎo)航方法相結(jié)合，如慣性、GPS等結(jié)合，以此來(lái)提高精確度。