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

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

1 引言

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

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

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

2 特征點的檢測與匹配

2.1 ORB特征點

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

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

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

(1)

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

(2)

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

θ=arctan(m01/m10)

(3)

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

2.2 改進ORB特征算法

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

2.2.1 SIRB 特征點提取

1)尺度空間極值檢測

Lindeberg[12]等人已證明高斯卷積核是實現(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)鍵點的位置

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

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

(5)

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

圖1 極值檢測原理圖

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

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

(6)

其中

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

(7)

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

(8)

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

(9)

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

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

(10)

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

(11)

由式10與11得到：

(12)

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

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

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

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

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

2.2.2 SIRB 特征點匹配

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

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

圖2 PROSAC隨機采樣圖

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

(15)

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

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

圖3 SIRB特征點的選取

圖4 Brute-Force Matche匹配

圖5 Brute-Force Matche、FLANN匹配

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

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

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

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

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

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

3 位姿優(yōu)化

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

(16)

(17)

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

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

(18)

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

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

(19)

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

(20)

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

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

(21)

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

(22)

求得該誤差的平方和

(23)

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

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

2)求解第w次迭代

(24)

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

(25)

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

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

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

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

圖7 小強XQ5實物圖

在室內(nèi)實驗室對本文設(shè)計的單目視覺里程計進行實驗。小強XQ5勻速在室內(nèi)無規(guī)則行駛，不間斷檢測周圍環(huán)境且提取室內(nèi)圖像中的特征點，在相同配置的硬件下，特征提取及匹配時間短，有效提高了實時性。小強XQ5標(biāo)準(zhǔn)軌跡與視覺軌跡對比如圖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算法的定位誤差對比如圖9。

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

圖9 定位誤差對比圖

5 總結(jié)

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

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

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

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

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