基于改進(jìn)ORB算法的單目視覺里程計(jì)研究

陸文超，楊慧斌，閆 娟，亢程博

(上海工程技術(shù)大學(xué)機(jī)械與汽車工程學(xué)院，上海 201620)

1 引言

自動(dòng)引導(dǎo)小車(Automated Guided Vehicle，AGV)是組成現(xiàn)代生產(chǎn)物流的重要組成部分[1]。自主導(dǎo)航技術(shù)作為 AGV 核心技術(shù)之一，是 AGV 技術(shù)發(fā)展水平的最重要標(biāo)志。視覺導(dǎo)航憑借能夠在樓宇眾多、城市信號(hào)阻塞等不利環(huán)境下實(shí)現(xiàn)自主導(dǎo)航，已成為時(shí)下SLAM自主定位的最優(yōu)選擇[2，3]。視覺里程計(jì)(VO)是SLAM至關(guān)重要的一環(huán)，通過相鄰的兩張圖像來估計(jì)相機(jī)運(yùn)動(dòng)，連續(xù)的相機(jī)運(yùn)動(dòng)就規(guī)劃出了AGV的運(yùn)動(dòng)軌跡，實(shí)現(xiàn)精確自主定位[4-6]。

單目視覺系統(tǒng)與深度相機(jī)定位系統(tǒng)、雙目視覺系統(tǒng)相比，雖然定位精度較低但計(jì)算量較小，安裝便捷[7]。VO作為視覺系統(tǒng)的輸入，主要通過SIFT、SURF、ORB等特征點(diǎn)法提取特征因子。高云峰基于 Surf 算法的單目視覺里程計(jì)在一定程度上提高了定位精度，但存在實(shí)時(shí)性不足的問題[8]。王彭林研究的視覺里程計(jì)雖然具有實(shí)時(shí)性，但無法對(duì)定位過程進(jìn)行優(yōu)化，導(dǎo)致定位精度不高[9]。

視覺里程計(jì)是影響AGV整體運(yùn)行的關(guān)鍵因素，針對(duì)以上相關(guān)研究成果存在的問題，提出基于改進(jìn)ORB算法的單目視覺里程計(jì)技術(shù)研究方法，在保證ORB算法實(shí)時(shí)性的同時(shí)又增添SIFT算法的尺寸不變性，依據(jù)三種不同算法對(duì)特征點(diǎn)進(jìn)行優(yōu)化匹配，采用最小二乘法優(yōu)化AGV位姿。通過實(shí)驗(yàn)驗(yàn)證，提出方法可以有效提高AGV定位精度，降低了AGV對(duì)高配置的依賴，增強(qiáng)視覺里程計(jì)的魯棒性與實(shí)時(shí)性。

2 特征點(diǎn)的檢測與匹配

2.1 ORB特征點(diǎn)

Rublee等人于2011年提出一種創(chuàng)新型檢測與匹配特征點(diǎn)的方法—ORB算法[10]。ORB算法由關(guān)鍵點(diǎn)和描述子兩部分構(gòu)成。其中，ORB算法的關(guān)鍵點(diǎn)是基于原有的FAST角點(diǎn)進(jìn)行改進(jìn)而得到的“Oriented FAST”，描述子簡稱BRIEF。具體實(shí)現(xiàn)步驟為

1)ORB算法通過構(gòu)建圖像金字塔，在金字塔每一層中提取FASR特征點(diǎn)，解決尺度不變性的問題。

2)由灰度質(zhì)心法實(shí)現(xiàn)特征的旋轉(zhuǎn)。通過矩計(jì)算得出特征點(diǎn)的方向因子，圖像的矩定義為

(1)

其中上式(1)中，p，q均代表矩的階次，I(x，y)表示圖像的灰度表達(dá)式。由上式得到圖像的質(zhì)心

(2)

由上式(2)得出的質(zhì)心及與特征點(diǎn)的夾角即特征點(diǎn)的主方向

θ=arctan(m01/m10)

(3)

3)ORB算法使用改進(jìn)的BRIEF算法對(duì)提取出的特征點(diǎn)計(jì)算描述子。ORB在FAST 特征點(diǎn)提取過程中，通過構(gòu)建圖像金字塔以及灰度質(zhì)心法增添了尺度與旋轉(zhuǎn)不變性，由此計(jì)算經(jīng)旋轉(zhuǎn)后的“Steer BRIEF”特征，所得到的ORB算法描述子的旋轉(zhuǎn)不變性效果良好。

2.2 改進(jìn)ORB特征算法

ORB 特征算法使用 FAST 提取特征點(diǎn)，在具備快速、高效等優(yōu)點(diǎn)的同時(shí)也缺乏尺度與旋轉(zhuǎn)不變性。考慮到AGV在行駛過程中，由于自身傳感器的誤差而導(dǎo)致的抖動(dòng)以及不斷根據(jù)需求調(diào)整前后運(yùn)動(dòng)，目標(biāo)的尺寸將發(fā)生變化，因此所使用的算法需具備尺度不變性。SIFT是David G Lowe 在 1999 年提出的一種基于不變量描述子的匹配算法，對(duì)尺度平移、縮放、旋轉(zhuǎn)、亮度變化等具有良好的不變性[11]，是目前較為成熟的特征點(diǎn)提取的算法。為有效解決此問題，本文提出將 SIFT算法所具有的尺度不變性特點(diǎn)和 ORB 算法快速、高效特點(diǎn)相結(jié)合，使用 SIFT 算法求出特征點(diǎn)和其質(zhì)心尺度、方向后，用ORB算子描述特征點(diǎn)，并在特征匹配階段利用均方根誤差進(jìn)行約束。因此，使得在保留了ORB算子運(yùn)算快速度的優(yōu)點(diǎn)的同時(shí)，解決ORB算法不具備尺度不變性的缺陷，將改進(jìn)的 ORB算法自定義為 SIRB 算法。

2.2.1 SIRB 特征點(diǎn)提取

1)尺度空間極值檢測

Lindeberg[12]等人已證明高斯卷積核是實(shí)現(xiàn)尺度變換的唯一變換核，并且是唯一的線性核。輸入圖像設(shè)為I(x，y)，G(x，y，σ)表示可變尺度的高斯函數(shù)，兩者通過卷積得到圖像的尺度空間：L(x，y，σ)，即

L(x，y，σ)=G(x，y，σ)*I(x，y)

(4)

使用差分高斯函數(shù)D(x，y，σ)如式(5)，高效得到

關(guān)鍵點(diǎn)的位置

D(x，y，σ)=G(x，y，kσ)-G(x，y，σ)*I(x，y)

=L(x，y，kσ)-L(x，y，σ)

(5)

上式D表示兩個(gè)相鄰的尺度差，初始圖像與不同σ值的高斯函數(shù)卷積，將得到的一垛模糊圖像臨近兩兩相減得到對(duì)應(yīng)的DOG。將DOG中的某個(gè)像素和本尺度的8個(gè)相鄰像素以及上下相鄰尺度各9個(gè)像素比較，如果該像素點(diǎn)在這所有參與比較的點(diǎn)中有最大值或者最小值，則認(rèn)為該像素是尺度空間的極值點(diǎn)之一，極值檢測原理如圖1。

圖1 極值檢測原理圖

2)精度特征關(guān)鍵點(diǎn)定位

為得到最終參與匹配的極值點(diǎn)，采用三維二次曲線展開的方法剔除對(duì)比度較低的邊緣極值點(diǎn)。在像素點(diǎn)處用三維泰勒級(jí)數(shù)展開到2次方項(xiàng)，然后再精確定位極值的位置至亞像素級(jí)，展開式如下

(6)

其中

通過上式(6)求對(duì)x的偏導(dǎo)數(shù)，即可求得局部的極值，此時(shí)極值點(diǎn)對(duì)于展開點(diǎn)的偏移量x滿足

(7)

將式(7)帶入式(6)，得

(8)

設(shè)定閾值為0.03，將|D(X)|<0.03的對(duì)比度偏低的點(diǎn)忽略，進(jìn)而通過計(jì)算主曲率的比值去除不穩(wěn)定的點(diǎn)，獲取最終值。主曲率依據(jù)2×2的Hessian矩陣H來計(jì)算，其中：

(9)

假定α和β是H的較大和較小特征值，則α=rβ，且

Tr(H)=Dxx+Dyy=α+β

(10)

Det(H)=DxxDyy-(Dxy)2=αβ

(11)

由式10與11得到：

(12)

3)關(guān)鍵點(diǎn)主方向分配

關(guān)鍵點(diǎn)通過分配一個(gè)主方向，并將主方向納入關(guān)鍵點(diǎn)的描述子特性之中，具有旋轉(zhuǎn)不變性。描述主方向主要依據(jù)像素點(diǎn)的梯度。梯度的模和方向以像素差分的方法定義，如下所示

m(x，y)=(L(x+1，y)-L(x-1，y))2+

(L(x，y+1)-L(x，y-1))2

(13)

θ(x，y)=tan-1(L(x，y+1)-

L(x，y-1))/(L(x+1，y)-L(x-1，y))

(14)

關(guān)鍵點(diǎn)的主方向是通過統(tǒng)計(jì)以關(guān)鍵點(diǎn)為中心的一個(gè)鄰域之內(nèi)所有點(diǎn)梯度方向來確定。通過將梯度直方圖分為36個(gè)柱形，每個(gè)柱形10度，關(guān)鍵點(diǎn)的主方向就是梯度方向峰值點(diǎn)的位置。

4)關(guān)鍵點(diǎn)描述子生成

依據(jù)上述得到帶有位置、尺度，主方向的關(guān)鍵點(diǎn)信息，使用改進(jìn)的Steer BRIEF算法快速對(duì)特征點(diǎn)計(jì)算描述子，保有快速的同時(shí)，所得到的特征子旋轉(zhuǎn)、尺度不變性效果良好。

2.2.2 SIRB 特征點(diǎn)匹配

由于噪聲等外界環(huán)境因素的影響，VO系統(tǒng)在圖像特征點(diǎn)的匹配過程容易產(chǎn)生大量的誤匹配點(diǎn)，影響相機(jī)定位。本文依據(jù)Brute-Force Matcher算法對(duì)相鄰兩個(gè)圖像進(jìn)行特征點(diǎn)的匹配，若滿足Hamming dist < 2Min dist，則將兩者視為正確匹配的特征點(diǎn)。在暴力匹配(Brute-Force Matcher，BF)算法的基礎(chǔ)上，再采用快速近似最近鄰( Fast Library for Ap-proximate Nearest Neighbors，F(xiàn)LANN)算法進(jìn)行特征點(diǎn)匹配[13]。此方法在一定程度上減少了特征誤匹配，同時(shí)視覺SLAM前端可估計(jì)相機(jī)采集相鄰時(shí)刻圖像的相對(duì)位姿。

為提高視覺SLAM前端特征點(diǎn)匹配的精確性，同時(shí)將機(jī)器學(xué)習(xí)的方法加入視覺前端[14，15]，提高圖像匹配的魯棒性、精確度、實(shí)時(shí)性是時(shí)下的發(fā)展趨勢，但存在計(jì)算量較大的新難題。對(duì)此，文中Brute-Force Matcher算法FLANN算法基礎(chǔ)上，提出在圖像匹配過程中引入漸進(jìn)采樣一致性(Progressive Sampling Consensus，PROSAC)[16]。PROSAC 算法首先對(duì)所有特征匹配點(diǎn)的結(jié)果進(jìn)行降序排序，不間斷循環(huán)選取最佳結(jié)果進(jìn)行迭代，消除了迭代次數(shù)不穩(wěn)定問題，并得到最優(yōu)解，提高了圖像匹配精度。在二維平面通過選取隨機(jī)點(diǎn)擬合直線過程中，先將聚集密集的點(diǎn)分為兩個(gè)部分，然后設(shè)定距離閾值，選取最佳內(nèi)點(diǎn)，最后擬合出最佳直線，PROSAC隨機(jī)采樣如圖2。

圖2 PROSAC隨機(jī)采樣圖

PROSAC算法在圖像匹配過程中可有效地剔除誤匹配點(diǎn)，同時(shí)對(duì)每一對(duì)特征點(diǎn)建立歐式距離的比值β，比值越小，則說明距離越小，特征點(diǎn)匹配的質(zhì)量也就越好。計(jì)算公式如下

(15)

式中，dmin表示最小歐式距離，dmin2代表次小歐式距離。

為驗(yàn)證基于ORB算法改進(jìn)的SIRB算法對(duì)于特征點(diǎn)的提取，以及在結(jié)合Brute-Force Matcher算法與FLANN 算法的基礎(chǔ)上，依據(jù)PROSAC算法在圖像匹配過程中剔除誤匹配點(diǎn)的有效性，運(yùn)用Open CV等工具進(jìn)行實(shí)驗(yàn)，實(shí)驗(yàn)證明此方法有效提高了特征點(diǎn)匹配的質(zhì)量，特征點(diǎn)提取與匹配如圖3、圖4、圖5、圖6。

圖3 SIRB特征點(diǎn)的選取

圖4 Brute-Force Matche匹配

圖5 Brute-Force Matche、FLANN匹配

圖6 Brute-Force Matche、FLANN、 PROSAC匹配

上述實(shí)驗(yàn)中，用RMSE、RMSEx與RMSEy分別表示已知特征點(diǎn)坐標(biāo)與檢測出的特征點(diǎn)坐標(biāo)間的整體均方根誤差，X和Y向的均方根誤差。提取的特征點(diǎn)間誤差比較見表1。

表1 各算法提取特征點(diǎn)比較

三種算法提取的特征點(diǎn)花費(fèi)的時(shí)間見表2。

表2 各算法檢測特征點(diǎn)時(shí)間比較

為避免檢測實(shí)驗(yàn)出現(xiàn)偶然性，采取多組實(shí)驗(yàn)進(jìn)行驗(yàn)證，仿真結(jié)果表明，提出的SIRB算法相比SIFT算法和ORB算法具有較低的特征點(diǎn)提取誤差，相比SIFT算法具有較高的特征點(diǎn)提取速度，并且相比ORB算法具有較高的特征點(diǎn)提取精度，保有快速提取同時(shí)，解決了ORB算子不具備尺度不變性的缺點(diǎn)。AGV對(duì)于不同路況的實(shí)時(shí)變化，不僅對(duì)尺度不變性有所要求，對(duì)實(shí)時(shí)性也提出較高要求。將改進(jìn)的SIRB算法用于AGV特征點(diǎn)的特征提取上，在有效減少時(shí)間的基礎(chǔ)上，還獲得較高的魯棒性。

3 位姿優(yōu)化

由于噪音誤差等帶來的影響，非線性優(yōu)化使用所有時(shí)刻采集到的數(shù)據(jù)進(jìn)行狀態(tài)估計(jì)，以此對(duì)相機(jī)位姿進(jìn)行優(yōu)化。經(jīng)典的SLAM模型由運(yùn)動(dòng)方程和觀測方程構(gòu)成，如式(16)所示

(16)

(17)

假設(shè)運(yùn)動(dòng)和觀測方程的兩個(gè)噪聲項(xiàng)wk，vk，j均滿足零均值的高斯分布：

wk～N(0，Rk)，vk～N(0，Qk，j)

(18)

通過引入最小二乘法求解上述構(gòu)成的狀態(tài)估計(jì)問題，所得到的觀測數(shù)據(jù)的條件概率是：

P(zj，k|xk，yj)=N(h(yj，xk)，Qk，j)

(19)

如上式依舊滿足高斯分布，使用最小化負(fù)對(duì)數(shù)求出其最大似然，得到使它最大化的xk，yj。假設(shè)任意高維高斯分布x～N(μ，ψ)，對(duì)其概率密度函數(shù)展開式取負(fù)對(duì)數(shù)，則得到

(20)

由于對(duì)原分布求解最大化相當(dāng)于對(duì)上式負(fù)對(duì)數(shù)求最小化x，因此只需求解右側(cè)的二次型項(xiàng)，將所得到對(duì)狀態(tài)的最大似然估計(jì)帶入觀測模型，同時(shí)定義數(shù)據(jù)與估計(jì)值之間的誤差

(ev，k=xk-f(xk-1，uk)

(21)

ey，j，k=zk，j-h(xk，yj)

(22)

求得該誤差的平方和

(23)

對(duì)得到的最小二乘方程進(jìn)行求解，得到導(dǎo)數(shù)為零的極值，它們可能是極大、極小或鞍點(diǎn)處的值，然后逐個(gè)比較它們的函數(shù)值大小即可。對(duì)于難求解的最小二乘問題，采用迭代的方式，即從一個(gè)初始值出發(fā)，不斷地更新當(dāng)前的優(yōu)化變量，使目標(biāo)函數(shù)下降。這個(gè)過程只要找到迭代點(diǎn)的梯度方向即可，進(jìn)而通過如下改良的非線性優(yōu)化列文伯格—馬夸爾特方法不斷迭代優(yōu)化求解。

1)給定初始值與初始優(yōu)化半徑x0、μ；

2)求解第w次迭代

(24)

其中μ是信賴區(qū)域的半徑，列文伯格把D取成單位陣I，馬夸爾特提出將D取成非負(fù)數(shù)對(duì)角陣，ρ的計(jì)算如式(25)所示

(25)

5)如果ρ大于某閾值，則認(rèn)為可行，令xk+1=xk+Δxk；

6)判斷算法是否收斂，若不收斂則返回2)，若收斂則結(jié)束。

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

本文實(shí)驗(yàn)采用內(nèi)置機(jī)載 PC(i7 處理器、1.8GHz CPU 、睿頻 3.0GHZ)的小強(qiáng)XQ5輪式機(jī)器人。小強(qiáng)XQ5配置60fps 178度廣角攝像頭廣角單目攝像頭，在 Ubuntu 系統(tǒng)中依據(jù)機(jī)器人操作系統(tǒng) ROS，以及解決圖像處理的 Open CV 庫和用于實(shí)現(xiàn)優(yōu)化算法的 g2o 庫，以此實(shí)現(xiàn)小強(qiáng)XQ5能夠自主定位，小強(qiáng)XQ5如圖7。

圖7 小強(qiáng)XQ5實(shí)物圖

在室內(nèi)實(shí)驗(yàn)室對(duì)本文設(shè)計(jì)的單目視覺里程計(jì)進(jìn)行實(shí)驗(yàn)。小強(qiáng)XQ5勻速在室內(nèi)無規(guī)則行駛，不間斷檢測周圍環(huán)境且提取室內(nèi)圖像中的特征點(diǎn)，在相同配置的硬件下，特征提取及匹配時(shí)間短，有效提高了實(shí)時(shí)性。小強(qiáng)XQ5標(biāo)準(zhǔn)軌跡與視覺軌跡對(duì)比如圖8，標(biāo)準(zhǔn)軌跡里程約16.70m，視覺軌跡里程約為16. 86m，最大誤差約為0.16m，誤差為0. 95% ，兩者軌跡基本擬合。SIRB算法整體的誤差較ORB檢測方法減小0.04m，定位精度提高0. 24%，相比較SIFT檢測方法誤差減小0.03m，提高0. 18%的定位精度。SIRB算法可最大化減小誤差，有效提高定位精度，效果更佳。SIRB算法、ORB算法以及SIFT算法的定位誤差對(duì)比如圖9。

圖8 小強(qiáng)XQ5標(biāo)準(zhǔn)軌跡與視覺軌跡對(duì)比圖

圖9 定位誤差對(duì)比圖

5 總結(jié)

本文提出的基于改進(jìn)ORB算法的單目視覺里程計(jì)方法：

1)采用SIRB算法對(duì)特征點(diǎn)進(jìn)行快速高效的提取。

2)結(jié)合暴力匹配算法與FLANN 算法以及PROSAC算法進(jìn)行快速匹配并剔除誤匹配，快速高效估計(jì)機(jī)器人位姿狀態(tài)。

3)構(gòu)建最小二乘問題，利用改良的列文伯格—馬夸爾特方法以及非線性優(yōu)化庫g2o 對(duì)AGV位姿進(jìn)行優(yōu)化。

以小強(qiáng)XQ5為實(shí)驗(yàn)載體驗(yàn)證得出，本文方法能有效增強(qiáng)視覺里程計(jì)的魯棒性，提高AGV定位精度。