歐明華，李 翔

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

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

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

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

1 ORB特征檢測與匹配

1.1 特征檢測

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特征的提取分為兩個步驟：

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

(1)

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

(2)

θ=arctan(m01/m10)

(3)

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

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

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

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

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

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

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

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

圖2 篩選后的匹配

2.2 基礎(chǔ)矩陣估計

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

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

(1)

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

(2)

F=K-Tt^RK-1

(3)

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

3 優(yōu)化

相機(jī)的位姿優(yōu)化是一個非線性問題。相機(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)

寫成矩陣形式

siui=Kexp(ξ^)Pi

(5)

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

圖3 重投影誤差示意圖

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

(6)

利用李代數(shù)構(gòu)建無約束的優(yōu)化問題，需要每個誤差項(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是一個2×6的矩陣。變換到相機(jī)坐標(biāo)系下的空間點(diǎn)坐標(biāo)記為P′，取其前3維

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

(8)

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

su=KP′

(9)

展開

(10)

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

(11)

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

(12)

符號“⊕”為李代數(shù)上的左乘擾動，第一項(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ī)觀測方程關(guān)于相機(jī)位姿與特征點(diǎn)的導(dǎo)數(shù)矩陣，為優(yōu)化提供了梯度方向，進(jìn)而通過高斯牛頓法[13]不斷迭代優(yōu)化求解。

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

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

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

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

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

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

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

5 結(jié)束語

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