單目視覺里程計設(shè)計及優(yōu)化

歐明華，李 翔

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

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

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

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

1 ORB特征檢測與匹配

1.1 特征檢測

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

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

(1)

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

(2)

θ=arctan(m01/m10)

(3)

所以FAST角點就有了尺度不變性。

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

1.2 特征點的幀間配準

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

2 特征點誤匹配剔除與運動估計

2.1 利用RANSAC算法進行剔除誤匹配

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

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

圖2 篩選后的匹配

2.2 基礎(chǔ)矩陣估計

基礎(chǔ)矩陣F表示兩幅圖像的運動關(guān)系，是兩幅圖像幾何關(guān)系的代數(shù)描述。由于相機的旋轉(zhuǎn)和平移各有3個自由度，所以基礎(chǔ)矩陣共6自由度，但由于單目的尺度等價性，F(xiàn)實際上5個自由度，所以用5對點對就可以求解F。根據(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是兩個像素點的歸一化平面上的坐標，^為反對稱矩陣符號。K為已知相機內(nèi)參，R、t為相機運動。然后通過對F奇異值分解(SVD)[16]求得R、t，也就是相機位姿。同理，對其他點對，也有類似表示。

3 優(yōu)化

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

(4)

寫成矩陣形式

siui=Kexp(ξ^)Pi

(5)

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

圖3 重投影誤差示意圖

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

(6)

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

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

(7)

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

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

(8)

相機投影模型相對于P′為

su=KP′

(9)

展開

(10)

可以得到相機模型

(11)

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

(12)

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

(13)

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

(14)

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

(15)

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

(16)

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

(17)

按照定義

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

(18)

(19)

于是有

(20)

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

4 實驗結(jié)果及分析

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

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

圖4 機器人運行時的圖像

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

圖5 機器人視覺定位軌跡

圖6 機器人定位軌跡與標準軌跡對比

5 結(jié)束語

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