基于融合深度信息的自動噴霧車全局非線性軌跡優(yōu)化方法

劉 慧 劉加林 沈 躍 朱嘉慧 李尚龍

(江蘇大學電氣信息工程學院， 鎮(zhèn)江 212013)

0 引言

近年來，隨著農(nóng)業(yè)信息化、自動化的發(fā)展，林道間的植株施藥從早期主要依靠人工完成逐漸向無人化、智能化發(fā)展。因此，基于高精度定位的自主行駛車技術[1-4]在農(nóng)業(yè)施藥領域?qū)⒊蔀橹悄茏兞繃婌F的發(fā)展趨勢，有望解決人工噴霧過程中勞動強度大、危害施藥人員安全、浪費農(nóng)藥、污染環(huán)境等問題[5-6]。

目前自主行駛車輛定位方法主要有：基于全球定位技術(GPS)[7]的方法；基于激光雷達的定位方法[8-12]；基于單目、雙目、RGBD的視覺定位方法[13-19]。如文獻[20]使用GPS提供精準的實時經(jīng)緯度地理信息獲取車輛位置，但室內(nèi)和有枝葉遮蔽的果園林間GPS信號差，使得作業(yè)過程中經(jīng)常需要重定位。文獻[21]使用激光傳感器對溫室內(nèi)道路邊緣進行檢測與深度數(shù)據(jù)提取，實現(xiàn)車輛自身定位和自主行駛，但其僅依賴道路邊緣的深度信息檢測，使得車輛在定位過程中易出現(xiàn)深度信息丟失，且激光傳感器價格昂貴，成本較高。文獻[22]基于Kinect深度攝像機使機器人從一個起始位置出發(fā)，利用單目視覺里程計進行自身定位，從而依據(jù)位置調(diào)整前進的方向，但由于依賴機器人前一位置狀態(tài)，缺乏全局的非線性優(yōu)化，定位誤差大，行駛魯棒性差。

本文在前人研究的基礎上，提出一種融合深度信息的軌跡優(yōu)化方法。應用RealSense[23-24]二代傳感器獲得連續(xù)彩色圖像和深度圖像，提取關鍵幀，并通過全局非線性優(yōu)化的方法不斷迭代誤差，使噴霧車在田間行駛過程中具有高精度定位，并在優(yōu)化過程中融合深度信息對噴霧車林道間行駛進行實時定位，增強噴霧車實時獲取自身精確軌跡的魯棒性，從而避免依賴單一深度信息軌跡定位時，深度信息幀易丟失、無法進行實時定位、自身軌跡與真實軌跡偏離較大的問題。

1 試驗和方法

1.1 RealSense植株圖像數(shù)據(jù)采集平臺

試驗地點選擇室外林道，光照強度均為1 000 lx，將RealSense傳感器安裝在自主行駛噴霧車上，在林道間行駛并連續(xù)拍攝兩側植株。如圖1所示，使用RealSense傳感器獲取彩色信息幀和深度信息幀，并通過RealSense自帶的RealSense for Windows SDK對彩色信息幀和深度信息幀進行實時校準，獲取相匹配的彩色幀和深度幀。

圖1 試驗車示意圖Fig.1 Schematic of experimental vehicle1.主控制箱 2.可調(diào)支架 3.RealSense傳感器 4.搭載的嵌入式計算機 5.車水箱 6.噴霧噴頭

如圖2所示，本文采用的RealSense傳感器，可以同時獲得彩色信息和深度信息，性價比高，且其體積小巧，無需搭載220 V外接電源，通過自帶的USB接口與嵌入式計算機相連進行供電和數(shù)據(jù)傳輸，使用安全輕便。

圖2 RealSense傳感器示意圖Fig.2 Schematic of RealSense sensor1.深度攝像頭 2.彩色攝像頭 3.定位孔

1.2 ORB特征提取及匹配

1.2.1Oriented FAST特征點

圖3 FAST特征點Fig.3 FAST feature point

FAST[25-26]算法主要用于檢測灰度圖中像素灰度變化明顯的地方，如圖3所示，以像素點A為圓心，3為半徑的圓上取16個像素點，若選取的圓上有連續(xù)9個點的亮度大于A點像素值的20%,則這個點就為FAST特征點。由于是直接比較像素亮度的差值，所以計算速度非常迅速。但同時由于FAST特征點數(shù)量巨大，且不具備尺度不變性和旋轉(zhuǎn)不變性，所以本文對FAST特征點進行改進。

(1)為了提高檢測效率，通過暴力檢測，即直接檢測鄰域圓上1、5、9、13這4個像素的亮度，當這4個像素中有3個像素亮度值同時大于像素A亮度20%，則繼續(xù)對該點進行FAST檢測，否則直接排除。同時在第1遍檢測出FAST特征點后，原始的FAST特征點經(jīng)常會出現(xiàn)局部扎堆的現(xiàn)象，此時通過非極大值抑制的方法對FAST特征點進行進一步的篩選，即通過選取5×5的窗口，分別計算其中FAST特征點的Harris[27]響應值，并按照分值排序，選取其中排名靠前的6個特征點作為最終的特征點集合。

(2)由于FAST特征點不具備尺度不變性，所以通過構建多尺度空間進行改進。由于傳統(tǒng)圖像金字塔采用下采樣的方式對每一層進行Harris響應值計算，提取FAST特征點。本文采用基于OpenCV庫的高斯差分金字塔，通過利用高斯卷積構建非完整金字塔結構的圖像尺度空間，大大減少了計算Harris響應值的金字塔層數(shù)，增強了提取FAST特征點的魯棒性和實時性。其計算公式為

L(u,v,δ)=I(u,v)*G(δ)

(1)

式中I——圖像中的像素值

δ——高斯方差

G(δ)——高斯核函數(shù)

L(u,v,δ)——圖像尺度空間

(3)針對FAST特征點不具備旋轉(zhuǎn)不變性，采用灰度質(zhì)心法進行改進。通過選取計算FAST特征點所在鄰域圖像塊B的質(zhì)心，連接幾何中心O到質(zhì)心C構成一個方向向量OC。從而使得FAST特征點具有了良好的旋轉(zhuǎn)描述性，大大提升了不同圖像之間特征點表述的魯棒性。其計算公式為

(2)

(3)

θ=arctan(m01/m10)

(4)

式中mpq——圖像塊B的矩

θ——特征點的方向角

x、y——圖像塊中像素點坐標

p、q——系數(shù)

1.2.2BRIEF描述子

BRIEF[28]是一種二進制的描述子，其描述向量由特征點附近兩個像素的亮度關系決定，并使用二進制編碼來加速存儲。但原始的BRIEF描述子不具備旋轉(zhuǎn)不變性和尺度不變性。通過結合Oriented FAST特征點計算的BRIEF描述子，可很好地解決這兩個缺陷。其計算公式為

(5)

式中I(xp,yp)——圖像中p點的像素值

I(xq,yq)——圖像中q點的像素值

Vf——p到q的向量描述值

1.3 特征匹配算法

1.3.1粗匹配算法

在獲取了BRIEF描述子后，通過FLANN算法[29]創(chuàng)建許多棵隨機樹，然后從具有最高距離方差的維度中隨機選取若干維度，并使用這些維度對數(shù)據(jù)進行劃分。本文通過BRIEF向量串之間的漢明距離作為查詢點距離，將BRIEF描述子向量不斷進行維度劃分，建立樹形結構，并通過由上而下的遞歸方法，將查詢點和參考點按區(qū)域搜索，進行最終的匹配。FLANN算法相對于暴力匹配算法，在需要對大量特征點匹配時速度更快，魯棒性更好。

為進一步加速近鄰搜索，本文采用Nanoflann算法[30]加速，Nanoflann算法是從內(nèi)存和編譯的角度來對近鄰搜索進行優(yōu)化。此算法還可以在近鄰搜索前直接定義目標點云的最小包圍盒，避免了近鄰搜索中的重復計算，對不同類型的數(shù)據(jù)兼容性更高，適用性更廣泛。通過對多組圖像進行特征點提取，按特征點數(shù)量進行排序，分別對FLANN搜索算法和經(jīng)過Nanoflann加速過后的搜索算法進行快速性對比，如圖4所示。在特征點數(shù)量很大時，Nanoflann算法比FLANN算法快速性得到了較大提高。

圖4 Nanoflann算法和FLANN算法快速性對比Fig.4 Comparison of fastness between Nanoflann algorithm and FLANN algorithm

1.3.2剔除誤匹配點

由于噴霧車行駛過程中存在運動誤差和路標點觀測誤差，所以在特征匹配后，特征點之間存在著很多誤匹配點對。針對這些誤匹配點對，需要對匹配的特征點對進行篩選。

首先對匹配的特征點進行最小距離過濾的初始篩選。計算統(tǒng)計描述子之間的最小漢明距離和最大漢明距離，當描述子之間的距離大于兩倍的最小距離時，即認為匹配有誤。由于存在描述子之間的最小漢明距離非常小的情況，所以設定一個經(jīng)驗值作為下限，取漢明距離30作為經(jīng)驗值。計算公式為

(6)

式中dB——匹配特征點對之間的漢明距離

dmin——匹配特征點對之間的最小漢明距離

G——匹配特征點正確匹配標志

在進行初始篩選后，為了進一步消除圖像背景紋理和相似物體干擾造成的誤匹配，采用基于本質(zhì)矩陣的RANSAC方法對匹配結果進行后驗。首先，從初始篩選后的點集D中，隨機采樣10組匹配點，通過對極約束(Perspective-n-point, PnP)[31]方程計算本質(zhì)矩陣E，由本質(zhì)矩陣對噴霧車進行運動恢復，解算出噴霧車的位姿變換矩陣T。再通過本質(zhì)矩陣[32]和位姿變換分別計算圖像I1和I2中點到各自極線的距離方差。如果每個匹配點對到極線的距離平方和小于初始閾值χ，則將這些匹配點對加入到內(nèi)點點集Din，并記錄距離方差；否則認為是錯誤匹配點對，從初始篩選點集D中移除。重復上述過程，直到初始篩選點集D中的點對都被訪問過，則將內(nèi)點點集Din作為D的更新點集，計算距離方差的均值作為閾值更新量，重復篩選過程，直到閾值χ近似收斂于一個常值。內(nèi)點中的點對集合即為正確的匹配點對集。

1.4 融合深度信息非線性全局軌跡優(yōu)化

1.4.1對極幾何

圖5 對極約束幾何圖Fig.5 Polar constraint geometries

通過上文得到的有效匹配點對結合對極約束計算重投影誤差

(7)

式中 (pi,pj)——圖像中用于求解本質(zhì)矩陣的觀測點的坐標向量

K——運動相機的內(nèi)部參數(shù)矩陣

1.4.2基于深度信息的車位置解算

圖6 噴霧車深度信息位置解算圖Fig.6 Position of depth information of spray vehicle

當噴霧車在行駛過程中，對已經(jīng)匹配好的有效特征點對進行深度信息的采集，通過計算相鄰兩關鍵幀獲取的匹配特征點到噴霧車的深度信息和位姿變換角即可計算出小車行駛過程中具體位置變化，如圖6所示，圖中C1、C2為車在相鄰兩關鍵幀中的位置，P1、P2、P3為相鄰關鍵幀中的有效特征點，P1C1為第1幀特征點到車的深度信息，P1C2為第2幀特征點到車的深度信息，C1C2為相鄰兩幀車的位置變化。噴霧車從C1行駛到C2過程中，RealSense傳感器采集到相鄰兩關鍵幀間噴霧車的位置變化，經(jīng)過特征點匹配后，觀測目標點P1場景位置不變，由對極約束獲取的本質(zhì)矩陣E解算出噴霧車旋轉(zhuǎn)矩陣R，從而獲得旋轉(zhuǎn)角α。最后通過三角形余弦定理即可求出噴霧車在相鄰關鍵幀間移動的距離lC1C2。其計算公式為

E=tR

(8)

(9)

式中t——運動相機的平移向量

lP1C1——第1幀特征點到噴霧車的距離

lP1C2——第2幀特征點到噴霧車的距離

將融合深度信息計算的位移lC1C2作為對極約束解算平移向量t的模長優(yōu)化量，將優(yōu)化后的平移向量t′和旋轉(zhuǎn)矩陣R回代入公式，獲得融合深度信息的本質(zhì)矩陣E′，并將該本質(zhì)矩陣作為特征點獲取重投影點的本質(zhì)矩陣，用于計算融合深度信息的重投影誤差。其計算公式為

(10)

式中β——位移長度比例參數(shù)

t′——融合深度信息計算的平移向量

1.4.3融合深度信息非線性全局優(yōu)化

在特征點觀測和噴霧車運動過程中存在著大量的觀測誤差和運動誤差，而傳統(tǒng)基于濾波的線性優(yōu)化方法對前一個時刻噴霧車狀態(tài)的依賴性強，噴霧車在行駛過程中關鍵幀丟失對其正確計算重投影誤差的影響極大，兩幀間重投影誤差迭代優(yōu)化易陷入局部最優(yōu)。因此本文算法通過獲取噴霧車整個運動過程中一定時間間隔的所有連續(xù)關鍵幀，對全部匹配點累積重投影誤差進行迭代優(yōu)化，獲得一個全局的最優(yōu)解。

(11)

式中ri——車運動過程中采集每幀的時間間隔

i——車運動過程中采集幀序號

e——車當前運行的時間段序號

γ——車運動過程中采集的幀數(shù)

噴霧車從初始位置到當前運動位置時間段e內(nèi)，按照式(11)計算相應時間間隔ri，并按此時間間隔采集全局軌跡的連續(xù)關鍵幀作為參考幀，進行特征點提取和特征匹配從而估計出各個連續(xù)關鍵幀間的本質(zhì)矩陣E，并解算本質(zhì)矩陣獲得位移向量t和旋轉(zhuǎn)矩陣R。然后通過RealSense傳感器采集深度信息計算噴霧車實際位移，優(yōu)化位移向量t，再結合旋轉(zhuǎn)矩陣反求本質(zhì)矩陣，獲得融合深度信息的本質(zhì)矩陣E′，再將特征點通過新的本質(zhì)矩陣進行重投影，計算重投影誤差。

(12)

式中ξ——車運動過程中重投影疊加誤差

ηi——重投影點像素坐標

si——尺度因子

ξ^——相機運動過程中的擾動分量

最后綜合全局各關鍵幀間的重投影疊加誤差，構建最小二乘問題，并基于G2O庫進行圖優(yōu)化，使總投影誤差迭代收斂于一個全局最小誤差值。其中在優(yōu)化過程中，噴霧車位姿和觀測路標點作為圖頂點，重投影誤差ξ作為圖邊，不斷迭代更新噴霧車位姿和路標點觀測值，獲取噴霧車最優(yōu)位姿和路標觀測點最優(yōu)位置。另外，針對迭代優(yōu)化過程中的梯度下降策略，本文選取Dogleg算法進行更新值的求解，使其具有更高的效率和魯棒性。

圖7 融合深度信息和非線性位姿估計算法流程圖Fig.7 Flow chart of fusion depth information and nonlinear pose estimation algorithm

融合深度信息非線性全局軌跡優(yōu)化算法流程如圖7所示。

2 結果與分析

2.1 FAST特征點獲取

對RealSense傳感器采集到的彩色信息關鍵幀提取FAST特征點。如圖8所示，紅色的點為FAST特征點。由圖可知，圖8a的FAST特征點密集冗余，在后期的特征點匹配過程中會占用大量計算時間，因此，圖8b中在經(jīng)過非極大值抑制后，減少了大量冗余點，消除了扎堆的現(xiàn)象，極大提高了匹配效率。

圖8 FAST特征點提取圖Fig.8 Extraction graphs of FAST feature points

2.2 特征匹配

在非極大值抑制后獲得FAST特征點后，通過基于高斯差分金字塔分層提取特征點，灰度質(zhì)心法計算特征點方向向量。獲取具有尺度不變性和旋轉(zhuǎn)不變性的特征點，并計算BRIEF描述子。使用FLANN快速最近鄰算法進行粗匹配，并利用Nanoflann算法加速匹配時間，獲取匹配結果后，對匹配點對進行篩選，移除誤匹配點對。相對于傳統(tǒng)ORB(Oriented FAST and rotated BRIEF)特征匹配過程中使用圖像金字塔計算尺度不變性，暴力匹配算法對特征點進行特征匹配，并使用單一最小距離過濾誤匹配點對進行篩選。本文算法在特征點提取上通過非極大值抑制減少冗余點，使用高斯差分金字塔計算尺度不變性，采用FLANN快速最近鄰算法進行匹配，并增加了基于本質(zhì)矩陣的匹配校驗。使特征匹配在效率和精度上得到了很大的提升。匹配效果如圖9所示。

圖9 特征點匹配結果Fig.9 Matching graphs of feature points

圖9a由于未對匹配點對進行篩選，其水平方向匹配線密集扎堆，匹配消耗大量時間，并出現(xiàn)許多傾斜的誤匹配線。圖9b為傳統(tǒng)ORB匹配結果圖，其誤匹配線已經(jīng)減少了一部分，但仍有較多誤匹配對。圖9c為本文算法匹配效果圖，經(jīng)過進一步篩選和校驗后，可以明顯看到匹配線稀疏得多，準確率也得到了很大提升。為驗證試驗結果的準確性，通過多次匹配試驗，比較傳統(tǒng)ORB匹配算法與本文算法，結果如表1所示。本文算法相對于傳統(tǒng)ORB匹配算法，消除了63對誤匹配對，匹配準確率提高了9個百分點，在時間上減少了3.33 s，使準確率和效率得到了較大的提高。

表1 傳統(tǒng)ORB匹配算法和本文改進ORB匹配算法對比Tab.1 Comparison of traditional method based on ORB feature points matching algorithm and presented method

2.3 噴霧車行駛軌跡

圖10 車行駛軌跡路線Fig.10 Track map of car

在獲取正確的特征匹配點對后，融合RealSense傳感器深度信息對車位姿進行全局非線性軌跡優(yōu)化，其行駛軌跡曲線如圖10所示。圖中，散點為車行駛過程中提取的特征點。方框為經(jīng)過本文算法優(yōu)化后噴霧車的具體位姿。直線為噴霧車行駛軌跡。

2.4 行駛軌跡誤差分析

由于室外林道光線強度大，RealSense深度信息傳感器對光線敏感，單一使用RealSense深度傳感器對噴霧車行駛軌跡進行優(yōu)化，易出現(xiàn)深度信息丟失，噴霧車偏航誤差較大的問題，如圖11所示。噴霧車在林道間行駛過程中，單一基于深度信息幀定位的軌跡在第61幀開始出現(xiàn)大幅偏離真實軌跡的現(xiàn)象。而本文算法，通過加入基于全局的非線性軌跡優(yōu)化后，補償了噴霧車深度信息幀丟失情況下偏離的行駛軌跡，使噴霧車行駛軌跡能夠繼續(xù)波動在真實軌跡附近。

圖11 深度信息幀丟失軌跡誤差分析Fig.11 Analysis of tracking error of depth information frame loss

此外，在正常行駛過程中，本文算法通過融合深度信息幀對噴霧車行駛軌跡進行基于全局的非線性優(yōu)化，相較于單一基于RealSense傳感器深度信息，行駛軌跡估計具有更高的魯棒性。其行駛軌跡如圖12所示，其在第21幀到91幀和第171幀到231幀過程中，本文算法獲得軌跡曲線更加貼近于噴霧車真實軌跡曲線。

為驗證本文算法的穩(wěn)定性，經(jīng)過多次試驗并與單一基于RealSense傳感器深度信息行駛軌跡優(yōu)化方法進行對比，結果如表2所示，在10組單次最大采集300幀關鍵幀試驗中，單一依賴RealSense傳感器深度信息對噴霧車進行軌跡定位出現(xiàn)了13幀關鍵幀的丟失，極易出現(xiàn)圖11中大幅偏離真實軌跡的情況，不利于噴霧車對自身的精準定位。而本文算法由RealSense傳感器獲取彩色信息幀，通過特征匹配點對的觀測數(shù)據(jù)在深度信息幀丟失時進行噴霧車的精準定位，并且融合深度信息進一步提高了噴霧車位姿估計的魯棒性和準確性，使噴霧車獲取的實時軌跡偏離真實軌跡誤差均值下降了1.07 cm，方差下降了2.14 cm，超調(diào)量降低了2.13 cm。

圖12 單一深度信息軌跡優(yōu)化方法和融合深度信息的全局非線性軌跡優(yōu)化方法優(yōu)化軌跡誤差對比Fig.12 Comparison of trajectory errors between single depth information trajectory optimization method and global nonlinear depth information fusion trajectory optimization method

表2 單一深度信息幀軌跡優(yōu)化算法和本文軌跡優(yōu)化算法對比Tab.2 Comparison of single depth frame trajectory optimization algorithm and presented algorithm

3 結論

(1)提出了改進的ORB特征提取篩選算法，使用高斯差分金字塔，減少了計算Harris響應值的金字塔層數(shù)，加快了FAST特征點的提取速度。

(2)在獲取FAST特征點后，通過近鄰算法進行特征匹配，并使用Nanoflann算法進行加速匹配過程。隨后采用最小距離過濾和基于本質(zhì)矩陣的RANSAC算法校驗誤匹配點對。相對于傳統(tǒng)ORB特征匹配算法，匹配準確率提高了9個百分點，時間縮短了3.33 s。

(3)相對基于單一RealSense傳感器深度信息定位噴霧車位置，本文算法通過RealSense傳感器采集的彩色信息幀中特征匹配點對的觀測數(shù)據(jù)進行定位補償，并融合深度信息對噴霧車進行非線性全局軌跡優(yōu)化，使噴霧車獲取的實時軌跡始終波動于真實軌跡附近，相對于單一依賴深度信息幀的軌跡定位具有更高的精確性和魯棒性，其噴霧車估計軌跡偏離真實軌跡誤差均值下降了1.07 cm，方差下降了2.14 cm，超調(diào)量降低了2.13 cm。