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

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

0 引言

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

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

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

1 試驗(yàn)和方法

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

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

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

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

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

1.2 ORB特征提取及匹配

1.2.1Oriented FAST特征點(diǎn)

圖3 FAST特征點(diǎn)Fig.3 FAST feature point

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

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

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

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

(1)

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

δ——高斯方差

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

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

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

(2)

(3)

θ=arctan(m01/m10)

(4)

式中mpq——圖像塊B的矩

θ——特征點(diǎn)的方向角

x、y——圖像塊中像素點(diǎn)坐標(biāo)

p、q——系數(shù)

1.2.2BRIEF描述子

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

(5)

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

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

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

1.3 特征匹配算法

1.3.1粗匹配算法

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

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

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

1.3.2剔除誤匹配點(diǎn)

由于噴霧車行駛過程中存在運(yùn)動誤差和路標(biāo)點(diǎn)觀測誤差，所以在特征匹配后，特征點(diǎn)之間存在著很多誤匹配點(diǎn)對。針對這些誤匹配點(diǎn)對，需要對匹配的特征點(diǎn)對進(jìn)行篩選。

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

(6)

式中dB——匹配特征點(diǎn)對之間的漢明距離

dmin——匹配特征點(diǎn)對之間的最小漢明距離

G——匹配特征點(diǎn)正確匹配標(biāo)志

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

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

1.4.1對極幾何

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

通過上文得到的有效匹配點(diǎn)對結(jié)合對極約束計算重投影誤差

(7)

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

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

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

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

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

E=tR

(8)

(9)

式中t——運(yùn)動相機(jī)的平移向量

lP1C1——第1幀特征點(diǎn)到噴霧車的距離

lP1C2——第2幀特征點(diǎn)到噴霧車的距離

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

(10)

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

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

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

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

(11)

式中ri——車運(yùn)動過程中采集每幀的時間間隔

i——車運(yùn)動過程中采集幀序號

e——車當(dāng)前運(yùn)行的時間段序號

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

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

(12)

式中ξ——車運(yùn)動過程中重投影疊加誤差

ηi——重投影點(diǎn)像素坐標(biāo)

si——尺度因子

ξ^——相機(jī)運(yùn)動過程中的擾動分量

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

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

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

2 結(jié)果與分析

2.1 FAST特征點(diǎn)獲取

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

圖8 FAST特征點(diǎn)提取圖Fig.8 Extraction graphs of FAST feature points

2.2 特征匹配

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

圖9 特征點(diǎn)匹配結(jié)果Fig.9 Matching graphs of feature points

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

表1 傳統(tǒng)ORB匹配算法和本文改進(jìn)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

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

2.4 行駛軌跡誤差分析

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

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

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

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

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

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

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