基于改進ORB算法的單目視覺里程計研究

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

(上海工程技術大學機械與汽車工程學院，上海 201620)

1 引言

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

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

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

2 特征點的檢測與匹配

2.1 ORB特征點

Rublee等人于2011年提出一種創新型檢測與匹配特征點的方法—ORB算法[10]。ORB算法由關鍵點和描述子兩部分構成。其中，ORB算法的關鍵點是基于原有的FAST角點進行改進而得到的“Oriented FAST”，描述子簡稱BRIEF。具體實現步驟為

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

2)由灰度質心法實現特征的旋轉。通過矩計算得出特征點的方向因子，圖像的矩定義為

(1)

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

(2)

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

θ=arctan(m01/m10)

(3)

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

2.2 改進ORB特征算法

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

2.2.1 SIRB 特征點提取

1)尺度空間極值檢測

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

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

(4)

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

關鍵點的位置

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

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

(5)

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

圖1 極值檢測原理圖

2)精度特征關鍵點定位

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

(6)

其中

通過上式(6)求對x的偏導數，即可求得局部的極值，此時極值點對于展開點的偏移量x滿足

(7)

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

(8)

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

(9)

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

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

(10)

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

(11)

由式10與11得到：

(12)

3)關鍵點主方向分配

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

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)

關鍵點的主方向是通過統計以關鍵點為中心的一個鄰域之內所有點梯度方向來確定。通過將梯度直方圖分為36個柱形，每個柱形10度，關鍵點的主方向就是梯度方向峰值點的位置。

4)關鍵點描述子生成

依據上述得到帶有位置、尺度，主方向的關鍵點信息，使用改進的Steer BRIEF算法快速對特征點計算描述子，保有快速的同時，所得到的特征子旋轉、尺度不變性效果良好。

2.2.2 SIRB 特征點匹配

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

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

圖2 PROSAC隨機采樣圖

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

(15)

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

為驗證基于ORB算法改進的SIRB算法對于特征點的提取，以及在結合Brute-Force Matcher算法與FLANN 算法的基礎上，依據PROSAC算法在圖像匹配過程中剔除誤匹配點的有效性，運用Open CV等工具進行實驗，實驗證明此方法有效提高了特征點匹配的質量，特征點提取與匹配如圖3、圖4、圖5、圖6。

圖3 SIRB特征點的選取

圖4 Brute-Force Matche匹配

圖5 Brute-Force Matche、FLANN匹配

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

上述實驗中，用RMSE、RMSEx與RMSEy分別表示已知特征點坐標與檢測出的特征點坐標間的整體均方根誤差，X和Y向的均方根誤差。提取的特征點間誤差比較見表1。

表1 各算法提取特征點比較

三種算法提取的特征點花費的時間見表2。

表2 各算法檢測特征點時間比較

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

3 位姿優化

由于噪音誤差等帶來的影響，非線性優化使用所有時刻采集到的數據進行狀態估計，以此對相機位姿進行優化。經典的SLAM模型由運動方程和觀測方程構成，如式(16)所示

(16)

(17)

假設運動和觀測方程的兩個噪聲項wk，vk，j均滿足零均值的高斯分布：

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

(18)

通過引入最小二乘法求解上述構成的狀態估計問題，所得到的觀測數據的條件概率是：

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

(19)

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

(20)

由于對原分布求解最大化相當于對上式負對數求最小化x，因此只需求解右側的二次型項，將所得到對狀態的最大似然估計帶入觀測模型，同時定義數據與估計值之間的誤差

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

(21)

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

(22)

求得該誤差的平方和

(23)

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

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

2)求解第w次迭代

(24)

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

(25)

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

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

4 實驗結果與分析

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

圖7 小強XQ5實物圖

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

圖8 小強XQ5標準軌跡與視覺軌跡對比圖

圖9 定位誤差對比圖

5 總結

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

1)采用SIRB算法對特征點進行快速高效的提取。

2)結合暴力匹配算法與FLANN 算法以及PROSAC算法進行快速匹配并剔除誤匹配，快速高效估計機器人位姿狀態。

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

以小強XQ5為實驗載體驗證得出，本文方法能有效增強視覺里程計的魯棒性，提高AGV定位精度。