歐明華，李 翔

(桂林電子科技大學(xué) 電子工程與自動(dòng)化學(xué)院，廣西 桂林 541004)

不同于GPS等衛(wèi)星導(dǎo)航技術(shù)，視覺(jué)導(dǎo)航技術(shù)能夠在城市信號(hào)阻塞、樓宇眾多等不利環(huán)境下實(shí)現(xiàn)導(dǎo)航。視覺(jué)里程計(jì)[1](Visual Odometry，VO)已經(jīng)成為智能系統(tǒng)導(dǎo)航的最佳選擇，是SLAM(Simultaneous Localization and Mapping)技術(shù)[2]重要的組成部分，也是計(jì)算機(jī)視覺(jué)的一個(gè)重要應(yīng)用。VO僅使用圖像作為輸入即可確定系統(tǒng)位置，不需要有關(guān)環(huán)境的任何先驗(yàn)知識(shí)。因此視覺(jué)導(dǎo)航已成為當(dāng)下移動(dòng)機(jī)器人導(dǎo)航[3]的研究熱點(diǎn)。

現(xiàn)有視覺(jué)定位系統(tǒng)包括雙目(立體)視覺(jué)系統(tǒng)、單目視覺(jué)系統(tǒng)及深度相機(jī)定位系統(tǒng)。主流的視覺(jué)定位系統(tǒng)研究主要集中在雙目視覺(jué)定位，但是雙目視覺(jué)系統(tǒng)設(shè)備安裝、相機(jī)標(biāo)定等環(huán)節(jié)過(guò)于繁瑣；另一方面，雙目系統(tǒng)計(jì)算量大，難以達(dá)到實(shí)時(shí)效果。單目視覺(jué)定位系統(tǒng)安裝便捷、計(jì)算量較小，但單目有其固有缺點(diǎn)，即定位精度較低?；谔卣鼽c(diǎn)方法進(jìn)行視覺(jué)運(yùn)動(dòng)估計(jì)是視覺(jué)SLAM的主要方法。

文獻(xiàn)[4]提出的基于Surf的單目視覺(jué)里程計(jì)在一定程度上提高了定位精度，但是其應(yīng)用于特征較為豐富的場(chǎng)景時(shí)會(huì)出現(xiàn)實(shí)時(shí)性不足的問(wèn)題。文獻(xiàn)[5]提出的視覺(jué)里程計(jì)具有一定的實(shí)時(shí)性，但缺少對(duì)定位過(guò)程的優(yōu)化，導(dǎo)致精度不夠。文獻(xiàn)[6]提出了一種基于卷積神經(jīng)網(wǎng)絡(luò)的算法，通過(guò)優(yōu)化神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu)，從卷積神經(jīng)網(wǎng)絡(luò)的每一層去圖像描述子，進(jìn)行動(dòng)態(tài)環(huán)境的位置識(shí)別，在一定程度上提高了定位的準(zhǔn)確性，但該算法時(shí)間復(fù)雜度較高、實(shí)時(shí)性不足。針對(duì)以上問(wèn)題，提出一種基于ORB特征(Oriented FAST and Rotated BRIEF)[7]的單目視覺(jué)里程計(jì)及其優(yōu)化方法：根據(jù)特征點(diǎn)重復(fù)率選取關(guān)鍵幀，減少特征匹配過(guò)程的計(jì)算量，通過(guò)構(gòu)建李代數(shù)上的優(yōu)化問(wèn)題，提高視覺(jué)里程計(jì)實(shí)時(shí)性及可靠性。

1 ORB特征檢測(cè)與匹配

1.1 特征檢測(cè)

ORB算法是Rublee等人[7]在2011年的提出的一種特征點(diǎn)提取和匹配的方法，由兩部分組成。ORB算法的關(guān)鍵點(diǎn)稱為“Oriented FAST”，是一種改進(jìn)的FA ST角點(diǎn)。描述子稱為BRIEF(Binary Robust Independent Elementary Feature)。ORB特征的提取分為兩個(gè)步驟：

(1)FAST角點(diǎn)提取。相對(duì)于原始FAST角點(diǎn)，ORB中計(jì)算了特征點(diǎn)的主方向，為后續(xù)的BRIEF描述子增加了旋轉(zhuǎn)不變特性。特征點(diǎn)的主方向通過(guò)矩計(jì)算而來(lái)，圖像塊B的矩定義為

(1)

通過(guò)矩找到圖像的質(zhì)心

(2)

θ=arctan(m01/m10)

(3)

所以FAST角點(diǎn)就有了尺度不變性。

(2)描述子。對(duì)前一步提取出特征點(diǎn)的周?chē)鷪D像區(qū)域進(jìn)行描述。BRIEF是一種二進(jìn)制描述子，ORB在FAST特征點(diǎn)提取階段計(jì)算了關(guān)鍵點(diǎn)的方向，利用方向信息，計(jì)算旋轉(zhuǎn)之后的“Steer BRIEF”特征使ORB的描述子具有較好的旋轉(zhuǎn)不變性。

1.2 特征點(diǎn)的幀間配準(zhǔn)

特征匹配過(guò)程是VO的關(guān)鍵步驟，特征匹配是路標(biāo)與路標(biāo)之間的對(duì)應(yīng)關(guān)系，圖像與圖像的準(zhǔn)確匹配將為后續(xù)的姿態(tài)估計(jì)、優(yōu)化等操作減輕大量負(fù)擔(dān)。由于室內(nèi)環(huán)境復(fù)雜、圖像特征點(diǎn)較多，特征匹配過(guò)程占用了大量的計(jì)算資源。而相機(jī)采集圖像的頻率很高(60 frame·s-1)，為了節(jié)省計(jì)算資源，達(dá)到實(shí)時(shí)效果，本文采取關(guān)鍵幀策略對(duì)特征點(diǎn)進(jìn)行配準(zhǔn)：在進(jìn)行特征提取時(shí)，當(dāng)特征點(diǎn)A、B的描述子相似度大于某個(gè)設(shè)定閾值(設(shè)定為90%)，認(rèn)為A、B是同一特征點(diǎn)；當(dāng)前幀特征點(diǎn)與上一幀特征點(diǎn)重復(fù)率超過(guò)設(shè)定閾值時(shí)，則舍棄當(dāng)前幀，繼而采用快速近似最近鄰(Fast Library for Approximate Nearest Neighbors，F(xiàn)LANN)[8]算法進(jìn)行特征點(diǎn)數(shù)量較多情況下的匹配。

2 特征點(diǎn)誤匹配剔除與運(yùn)動(dòng)估計(jì)

2.1 利用RANSAC算法進(jìn)行剔除誤匹配

提取相機(jī)運(yùn)動(dòng)過(guò)程中的兩幀圖像的特征點(diǎn)之后，首先進(jìn)行人工篩選：對(duì)相鄰兩個(gè)圖像的ORB特征向量進(jìn)行距離判斷，如果漢明距離小于既定閾值(根據(jù)工程經(jīng)驗(yàn)，閾值設(shè)定為最小距離的兩倍)，那么將兩者視為正確匹配的特征點(diǎn)。這種方法在一定程度上減少了特征誤匹配，VO能夠估計(jì)出相機(jī)在相鄰時(shí)刻采集圖像時(shí)的相對(duì)位姿。由于是單目視覺(jué)里程計(jì)，相鄰圖像的特征匹配點(diǎn)必須滿足空間的對(duì)極約束。因此，在人工篩選的基礎(chǔ)上，使用隨機(jī)抽樣一致性(Random Sample Consensus，RANSAC)[9]算法再次篩選，獲得的特征匹配點(diǎn)滿足空間的對(duì)極約束[10]，特征匹配精度也有所提高。圖1和圖2分別為篩選前后的特征匹配。

圖1 未經(jīng)篩選的匹配

圖2 篩選后的匹配

2.2 基礎(chǔ)矩陣估計(jì)

基礎(chǔ)矩陣F表示兩幅圖像的運(yùn)動(dòng)關(guān)系，是兩幅圖像幾何關(guān)系的代數(shù)描述。由于相機(jī)的旋轉(zhuǎn)和平移各有3個(gè)自由度，所以基礎(chǔ)矩陣共6自由度，但由于單目的尺度等價(jià)性，F(xiàn)實(shí)際上5個(gè)自由度，所以用5對(duì)點(diǎn)對(duì)就可以求解F。根據(jù)針孔相機(jī)模型[10-15]，以及對(duì)極幾何約束可求得基礎(chǔ)矩陣F

s1p1=KP,s2p2=K(RP+t)

(1)

x1=K-1p1,x2=K-1p2

(2)

F=K-Tt^RK-1

(3)

其中x1,x2是兩個(gè)像素點(diǎn)的歸一化平面上的坐標(biāo)，^為反對(duì)稱矩陣符號(hào)。K為已知相機(jī)內(nèi)參，R、t為相機(jī)運(yùn)動(dòng)。然后通過(guò)對(duì)F奇異值分解(SVD)[16]求得R、t，也就是相機(jī)位姿。同理，對(duì)其他點(diǎn)對(duì)，也有類似表示。

3 優(yōu)化

相機(jī)的位姿優(yōu)化是一個(gè)非線性問(wèn)題。相機(jī)位姿R，t的李代數(shù)[11]表示為ξ。某空間點(diǎn)坐標(biāo)為Pi=[Xi,Yi,Zi]T，其投影像素坐標(biāo)為Ui=[ui,vi]T。像素位置和空間坐標(biāo)關(guān)系的齊次坐標(biāo)表示如下

(4)

寫(xiě)成矩陣形式

siui=Kexp(ξ^)Pi

(5)

將重投影誤差最小化[12]，如圖3所示，將位姿和三維特征點(diǎn)P同時(shí)優(yōu)化。

圖3 重投影誤差示意圖

由于相機(jī)位姿及觀測(cè)點(diǎn)噪聲，式(5)存在誤差，將誤差求和，構(gòu)建最小二乘問(wèn)題[13]，使其最小化

(6)

利用李代數(shù)構(gòu)建無(wú)約束的優(yōu)化問(wèn)題，需要每個(gè)誤差項(xiàng)關(guān)于優(yōu)化變量的導(dǎo)數(shù)，即線性化

e(x+Δx)≈e(x)+JΔx

(7)

其中，e為像素坐標(biāo)誤差(2維)，x為相機(jī)位姿(6維)，解析導(dǎo)數(shù)J是一個(gè)2×6的矩陣。變換到相機(jī)坐標(biāo)系下的空間點(diǎn)坐標(biāo)記為P′，取其前3維

P′=(exp(ξ^)P)1:3=[X′,Y′,Z′]

(8)

相機(jī)投影模型相對(duì)于P′為

su=KP′

(9)

展開(kāi)

(10)

可以得到相機(jī)模型

(11)

對(duì)式(6)左乘擾動(dòng)量δξ，然后考慮e的變化關(guān)于擾動(dòng)量的導(dǎo)數(shù)，利用鏈?zhǔn)椒▌t

(12)

符號(hào)“⊕”為李代數(shù)上的左乘擾動(dòng)，第一項(xiàng)為誤差關(guān)于投影點(diǎn)的導(dǎo)數(shù)，根據(jù)式(11)得

(13)

其中第2項(xiàng)為變換后的點(diǎn)關(guān)于李代數(shù)的導(dǎo)數(shù)，得到

(14)

根據(jù)式(8)，取前3維，得到

(15)

將式(12)和式(15)相乘得到2×6的雅克比矩陣[14]

(16)

等式(16)描述了重投影誤差關(guān)于相機(jī)位姿李代數(shù)的一階變化關(guān)系。另一方面除了優(yōu)化位姿還要優(yōu)化特征點(diǎn)的空間位置。因此需要討論e關(guān)于空間點(diǎn)P的導(dǎo)數(shù)，利用鏈?zhǔn)椒▌t

(17)

按照定義

P′=exp(ξ^)P=BP+t

(18)

(19)

于是有

(20)

由此得到了相機(jī)觀測(cè)方程關(guān)于相機(jī)位姿與特征點(diǎn)的導(dǎo)數(shù)矩陣，為優(yōu)化提供了梯度方向，進(jìn)而通過(guò)高斯牛頓法[13]不斷迭代優(yōu)化求解。

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

驗(yàn)證實(shí)驗(yàn)采用內(nèi)置機(jī)載PC(處理器為i7，7500U，3.2 GHz)的BlueWhale輪式機(jī)器人。在一個(gè)4.5 m×4.5 m的室內(nèi)試驗(yàn)，最終目的是使機(jī)器人能夠進(jìn)行自主定位，機(jī)器人配置單目攝像頭(640×480)，機(jī)器人在Ubuntu中利用機(jī)器人操作系統(tǒng)ROS，及用于圖像處理的OpenCV庫(kù)和用于實(shí)現(xiàn)優(yōu)化算法的g2o庫(kù)。

在室內(nèi)場(chǎng)景下對(duì)本文設(shè)計(jì)的單目視覺(jué)里程計(jì)進(jìn)行測(cè)試。移動(dòng)機(jī)器人以0.2 m·s-1的速度在室內(nèi)無(wú)規(guī)則行駛，運(yùn)行狀態(tài)如圖4所示(左圖為原始圖像，右圖為特征點(diǎn)圖像)，機(jī)器人底盤(pán)里程計(jì)視作標(biāo)準(zhǔn)軌跡。機(jī)器人在運(yùn)行過(guò)程中，不斷的檢測(cè)周?chē)h(huán)境，提取圖像中的特征點(diǎn),在相同配置下的PC上，特征提取及匹配時(shí)間為15 ms，有效提高了實(shí)時(shí)性。

圖4 機(jī)器人運(yùn)行時(shí)的圖像

從實(shí)驗(yàn)結(jié)果圖5中可以看到，在直線行駛時(shí)，由于場(chǎng)景特征重復(fù)性較高，視覺(jué)里程計(jì)選取關(guān)鍵幀較為稀疏；在曲線行駛時(shí)，由于場(chǎng)景特征豐富，視角重疊較小，特征點(diǎn)較多，視覺(jué)里程計(jì)選取關(guān)鍵幀較為稠密。標(biāo)準(zhǔn)軌跡與機(jī)器人定位軌跡對(duì)比如圖6所示，視覺(jué)定位軌跡和標(biāo)準(zhǔn)軌跡的定位精度最大誤差約為0.15 m，標(biāo)準(zhǔn)軌跡計(jì)算機(jī)器人里程約17.6 m，視覺(jué)軌跡機(jī)器人里程約為17.75 m，誤差為0.85%，可見(jiàn)兩者軌跡基本一致，誤差較小。

圖5 機(jī)器人視覺(jué)定位軌跡

圖6 機(jī)器人定位軌跡與標(biāo)準(zhǔn)軌跡對(duì)比

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

本文提出的視覺(jué)里程計(jì)方法能夠在室內(nèi)較小環(huán)境下實(shí)現(xiàn)自定位，采用提取ORB特征點(diǎn)的方式，并通過(guò)檢測(cè)特征點(diǎn)重復(fù)率的方式選取關(guān)鍵幀，大幅減少了運(yùn)算量，保證了視覺(jué)里程計(jì)的實(shí)時(shí)性，降低了機(jī)器人對(duì)高配置的依賴。通過(guò)RANSAC算法對(duì)幀間特征點(diǎn)進(jìn)行匹配，進(jìn)而估計(jì)基礎(chǔ)矩陣，計(jì)算機(jī)器人位姿，通過(guò)最小化重投影誤差，在李代數(shù)上構(gòu)建無(wú)約束的優(yōu)化問(wèn)題，利用圖優(yōu)化算法處理連續(xù)的關(guān)鍵幀并估計(jì)相機(jī)的運(yùn)動(dòng)，實(shí)現(xiàn)移動(dòng)機(jī)器人的定位。實(shí)驗(yàn)中，視覺(jué)里程計(jì)與標(biāo)準(zhǔn)里程計(jì)的軌跡對(duì)比結(jié)果表明，本文設(shè)計(jì)的單目視覺(jué)里程計(jì)具有一定的魯棒性和實(shí)用性。