果園移動機器人激光雷達雙源信息融合實時導航方法

劉 慧 段云鵬 沈 躍

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

0 引言

植保機械作業(yè)是現(xiàn)代化農(nóng)業(yè)經(jīng)濟發(fā)展的前提,是林果園增效的重要保障[1-2]。林果園機械自主導航是農(nóng)業(yè)移動機器人研究的一個重要組成部分,對我國農(nóng)業(yè)機械智能化、農(nóng)業(yè)生產(chǎn)精細化有著重大的促進作用[3-6]。

機器人自主導航是根據(jù)自身傳感器所獲取的周圍環(huán)境信息,規(guī)劃出前進路線并對自身進行控制,實現(xiàn)自主行駛[7-8]。目前廣泛使用的導航傳感器有全球衛(wèi)星導航系統(tǒng)(Global navigation satellite system, GNSS)、慣性測量單元(Inertial measurement unit, IMU)、視覺相機、激光雷達等[9-12]。ZHANG等[13]使用北斗衛(wèi)星導航系統(tǒng)和IMU實現(xiàn)果園中定位,利用誤差狀態(tài)卡爾曼濾波方法提高定位精度,但是存在果樹遮擋導致定位信號丟失的問題。OPIYO等[14]使用RealSense相機獲取果樹信息,通過提取地面圖像的中軸線作為導航路徑。張振乾等[15]在香蕉園中采用改進K-means算法將道路兩側(cè)香蕉樹分離,基于最小包圍矩陣提取導航路徑。但機器視覺導航技術(shù)受光照強度、視角范圍等條件限制較大。

激光雷達傳感器能精準獲取環(huán)境的距離信息,相比于視覺相機傳感器,具有測距精度高、抗干擾能力強等優(yōu)點[16-18]。二維激光雷達因成本較低而被一些學者廣泛使用。ZHANG等[19]對二維激光點云建立了不同的過濾閾值,有效消除二維激光點云數(shù)據(jù)的離散點,并基于最小二乘法對導航路徑進行擬合。李秋潔等[20]采用二維激光雷達獲取樹干信息,提出兩步樹行分割法獲取相鄰樹行數(shù)據(jù),將樹行中心線作為導航路徑,但該方法只進行了仿真實驗。劉星星等[21]使用二維激光雷達融合姿態(tài)傳感器進行數(shù)據(jù)校正,利用最小二乘法擬合樹行,結(jié)合支持向量機(Support vector machine,SVM)算法,預(yù)測果園行間中心線。二維激光掃描儀在果園導航中存在感知信息量少、無法應(yīng)對枝葉茂密或果樹相連成片的復雜果園環(huán)境等問題[22-23],更多學者采用三維激光雷達作為導航傳感器。MENGOLI等[24]使用三維激光雷達檢測樹干信息并通過霍夫變換方法擬合樹行。LIU等[25]使用隨機采樣一致性方法擬合樹行。楊洋等[26]提出基于感興趣區(qū)域(Region of interest,ROI)的甘蔗壟間導航線實時提取算法,其中甘蔗壟行擬合采用最小二乘算法。劉偉洪等[27]使用挖空打斷后的樹墻體心等效樹干位置,根據(jù)左右樹行的最佳平行度對隨機采樣一致性算法與最小二乘法擬合的樹行進行互補融合并求其中心線得到導航線。以上研究多以檢測單株或作物行為主,規(guī)劃路徑為直線,不利于躲避行間果樹冠層與行人等障礙物。胡廣銳等[28]在直線路徑的基礎(chǔ)上加入了改進人工勢場法,但未對改進后的導航路徑進行優(yōu)化,不利于農(nóng)業(yè)機械行走跟蹤控制。

相較于二維激光雷達,三維激光雷達具有檢測距離遠(100 m)、感知信息豐富等優(yōu)點,三維點云數(shù)據(jù)可同時用于導航線提取、障礙物檢測與識別、對靶噴霧和即時定位與地圖構(gòu)建(Simultaneous localization and mapping,SLAM)等,因此,本文以三維激光雷達作為導航傳感器,提出一種果園行間路徑提取方法。對果園三維點云數(shù)據(jù)進行直通濾波、降采樣和離群點去除等預(yù)處理,使用牛頓插值方法和非線性支持向量機方法提取行間導航線;為了提升系統(tǒng)的魯棒性與容錯率,將高頻更新的插值導航線與低頻更新的NSVM導航線進行互補融合,并對融合后導航線進行優(yōu)化;基于履帶式差速模型,采用純跟蹤算法對最終導航線進行跟蹤。

1 材料與方法

1.1 果園移動機器人導航系統(tǒng)

本研究以履帶式移動機器人為實驗平臺,其外形尺寸(長×寬×高)為1.25 m×0.85 m×1.90 m,如圖1a所示。果園移動機器人主要由履帶式移動底盤(bunker型,深圳市松靈機器人)、中央計算機(英特爾酷睿八代i7-8565u)、三維激光雷達(VLP-16,Velodyne公司)、組合導航系統(tǒng)(高精度組合導航系統(tǒng)X1,北云科技)和變量噴霧系統(tǒng)組成。其中履帶式移動底盤使用無刷直流電機驅(qū)動,符合差速運動模型,軸距為0.68 m,最大行駛速度為1.5 m/s。三維激光雷達具體參數(shù)如表1所示,安裝于機器人前方,距離地面1.25 m,激光雷達坐標系xlylzl與車體坐標系xbybzb如圖1b所示。

表1 Velodyne VLP-16型三維激光雷達參數(shù)Tab.1 Parameters of Velodyne VLP-16 LiDAR

圖1 移動機器人平臺Fig.1 Platform of mobile robot1.變量噴霧系統(tǒng)框架 2.VLP-16激光雷達 3.中央計算機 4.高精度組合導航X1 5.GPS接收天線

本研究使用中央計算機裝載Ubuntu 18.04 LTS操作系統(tǒng),并基于機器人操作系統(tǒng)(Robot operating system,ROS)與點云庫(Point cloud library,PCL)進行開發(fā)。果園移動機器人運動控制系統(tǒng)如圖2所示,使用三維激光雷達實時獲取樹木點云數(shù)據(jù),通過以太網(wǎng)口傳輸至中央計算機,進行點云處理、導航線規(guī)劃和軌跡跟蹤等運算,將控制指令通過CAN通信發(fā)送至下位機,實現(xiàn)移動機器人行走控制。

圖2 移動機器人運動控制系統(tǒng)原理圖Fig.2 Mobile robot motion control system schematic

1.2 點云預(yù)處理

(1)

式中 (Δx,Δy,Δz)——平移向量

三維激光雷達每幀原始點云數(shù)據(jù)理論上有28 800個點,為減少計算量和無關(guān)點對路徑規(guī)劃的影響,需要對點云數(shù)據(jù)進行預(yù)處理。調(diào)用點云庫中的相應(yīng)函數(shù),設(shè)定合理的閾值參數(shù),對三維點云進行感興趣區(qū)域(Region of interest,ROI)劃分、下采樣和去除離群點等處理。

1.3 插值導航線擬合

使用DBSCAN聚類方法對預(yù)處理后的三維點云進行聚類,使用果樹點云的中心位置表示果樹位置,對左右兩側(cè)果樹位置進行牛頓插值,得到兩側(cè)樹行曲線,取樹行曲線中心線為插值導航線。

DBSCAN聚類是一種基于密度的聚類算法,能夠把具有足夠高密度的區(qū)域劃分為簇[29];KD-Tree(K-dimension tree)是一種平衡二叉樹,基于KD-Tree的近鄰查詢算法可以加快聚類時的搜索速度[30-31]。使用一幀三維點云數(shù)據(jù)構(gòu)建KD-Tree,將點云數(shù)據(jù)中同一棵樹的點云數(shù)據(jù)存放在一個容器中,因為激光雷達的掃描特性,同一棵果樹單次掃描只能獲取果樹單側(cè)輪廓的點云信息,本文采用單棵果樹點云中心表示果樹位置以減小偏差,果樹的等效位置Oi(xOi,yOi,zOi)為

(2)

式中xmax、xmin——單棵果樹x軸最大、最小值

ymax、ymin——單棵果樹y軸最大、最小值

為了減少計算量,將果樹位置投影至車體坐標系的xbobyb平面。根據(jù)果樹位置y軸坐標的正負可以劃分為左側(cè)果樹和右側(cè)果樹。以單側(cè)果樹位置為節(jié)點,建立差商表,進行牛頓插值,如圖3所示,其中x軸方向均勻插入樹行點,y軸方向使用牛頓插值法進行計算。取左右兩側(cè)樹行曲線的中心線為插值導航線Wp={wpi|i=0,1,2,…},導航線第i個導航點wpi(xwpi,ywpi)坐標為

圖3 插值導航線擬合Fig.3 Fitting of interpolated navigation line

(3)

式中δ——間隔系數(shù)

ylefti——左側(cè)第i個樹行點y軸坐標

yrighti——右側(cè)第i個樹行點y軸坐標

1.4 NSVM導航線擬合

SVM屬于機器學習監(jiān)督算法中的統(tǒng)計分類方法,是一種二分類模型[21,32]。本文采用非線性支持向量機的方法,對左右果樹冠層點云進行二分類,在樹行中間尋找一條與兩側(cè)樹冠點云間隔最大化的最優(yōu)分類線,以此分割線為NSVM導航線。

為減少訓練時間,對預(yù)處理后點云再次進行下采樣和離群點去除。對二次處理后的每個點Pi(xPi,yPi)添加標簽ti

(4)

在經(jīng)過訓練之后,對于車體坐標系xbobyb平面中的任一點pk(xpk,ypk),都可以預(yù)測其標簽tk,若|tk|=0,則pk位于最大間隔分隔線上。在x軸方向上均勻地選擇最大間隔分隔線上的點,將其連接成NSVM導航線Ws={wsi|i=0,1,2,…},NSVM導航線中第i個導航點wsi(xwsi,ywsi)坐標為

(5)

式中ypk——點pk的y軸坐標

tk——點pk的預(yù)測標簽,為0

xpk——點pk的x軸坐標

1.5 導航線融合

插值導航線是以果樹中心位置信息生成的導航線,未考慮果樹邊緣點信息,NSVM導航線是以果樹邊緣點信息生成的導航線,未考慮果樹中心位置信息,因此兩種方法在果園樹行中形成互補。牛頓插值算法計算量較小,計算頻率較高,NSVM算法計算量較大,計算頻率較低,本文將高頻更新的插值導航線與低頻更新的NSVM導航線進行融合,融合方法為:接收到一幀點云新數(shù)據(jù)時,更新插值導航線,若此時上一幀NSVM導航線還未計算完畢,則不更新NSVM導航線,若已計算完畢,則更新NSVM導航線。記融合后導航線為Wt={wti|i=0,1,2,…},融合后導航線上第i個導航點為wti(xwti,ywti),在計算插值導航線與NSVM導航線時,設(shè)置兩者在x軸方向的間隔系數(shù)δ相等,即

xwti=xwpi=xwsi=δi(i=0,1,2,…)

(6)

(7)

然后對兩導航線上對應(yīng)導航點的橫向距離進行歸一化,有

(8)

式中ρ——歸一化后權(quán)重

以歸一化后結(jié)果計算權(quán)重

ywti=(1-0.5ρ)ywpi+0.5ρywsi

(9)

插值導航線權(quán)重一直保持在0.5以上,若插值導航線與NSVM導航線的橫向距離較小,則插值導航線權(quán)重較重,融合后導航線更接近插值導航線;反之,兩者權(quán)重接近0.5,融合后導航線接近兩者中間;在插值導航線與NSVM導航線橫向距離最大處,兩者權(quán)重都為0.5,融合后導航線在兩者中間。

1.6 導航線優(yōu)化

在不考慮機器人初始位置時可以計算出行間導航線,但是在后續(xù)進行軌跡跟蹤時,需要對導航線進行優(yōu)化,使導航線起點與機器人位置重合,即導航線經(jīng)過車體坐標系原點,本文使用線性化的方法,重新計算融合后導航線前2 m內(nèi)導航點的坐標:記x=2 m處的導航點為wtd2(xwtd2,ywtd2),優(yōu)化后的每個導航點x軸坐標值不變,y軸坐標值ywti為

(10)

(11)

式中n——5組導航線剔除發(fā)生跳變導航線后,保留下的導航線數(shù)量

ywt1i——隊列中第1組融合后導航線上第i個導航點的y軸坐標

(12)

對融合后導航線進行平滑處理,本文采用三次準均勻B樣條算法。若平面中有4個點p0、p1、p2、p3,則三次準均勻B樣條插值的方程為

(13)

式中t——插值的間隔比例,為插值數(shù)量的倒數(shù)

本文僅對導航點進行優(yōu)化,不進行插值,所以設(shè)置t=0。要使三次B樣條優(yōu)化后導航點的數(shù)量不會減少,且優(yōu)化后的導航線經(jīng)過優(yōu)化前的導航線的起點和終點,需要構(gòu)造新的點p′0=2p0-p1與p′n=2pn-pn-1,其中n為融合后導航點的數(shù)量,分別將p′0添加到p0之前,將p′n添加到pn之后。平滑后導航點為

(14)

至此,導航線融合與優(yōu)化全部完成。

1.7 軌跡跟蹤

機器人移動底盤為履帶式結(jié)構(gòu),滿足差速運動學模型,如圖4所示,采用純跟蹤算法進行軌跡跟蹤,在前視距離l處的行駛目標軌跡上選取跟蹤點P,機器人從當前位置O前進至點P的路徑可以近似看作一段半徑為R的圓弧,R即機器人轉(zhuǎn)彎半徑,根據(jù)三角函數(shù)關(guān)系有

圖4 差速運動學模型示意圖Fig.4 Schematic of differential kinematics model

(15)

式中l(wèi)——前視距離α——航向偏差

化簡后有

(16)

機器人前進線速度v為恒定速度,轉(zhuǎn)彎角速度ω與前進線速度v之間的關(guān)系為

(17)

結(jié)合式(16)與式(17)可求得

(18)

2 實驗

本研究在江蘇省鎮(zhèn)江市丹徒區(qū)林果園內(nèi)(32.12°N,119.27°E)和江蘇大學校園內(nèi)(32.22°N,119.51°E)進行行間自主導航實驗,如圖5所示,丹徒區(qū)林果園內(nèi)兩側(cè)為桂花樹,行距約6.2 m,株距3.0～3.2 m,隨機選擇5棵桂花樹進行測量,可知樹高為3.0～3.5 m,樹寬為1.2～1.6 m,樹木大小基本一致,樹行呈直線,路面較崎嶇;江蘇大學內(nèi)實驗場景兩側(cè)植株為櫻花樹,行距為5.5 m,株距為2.5～3.5 m,經(jīng)測量可知樹高為2.1～3.0 m,樹寬為1.5～2.2 m,樹木大小略有差距,樹行呈“S”形曲線,路面較平坦;符合本研究實驗要求。實驗時選擇相同的起始位置,分別使用插值導航線、NSVM導航線和融合后導航線進行導航,并使用純跟蹤算法進行跟蹤,每種方法重復3次,記錄導航線與行駛軌跡。

圖5 實驗場景Fig.5 Experimental scene

2.1 點云預(yù)處理結(jié)果

對原始激光點云進行坐標變換,手動測量車體坐標系到激光坐標系之間的平移向量(Δx,Δy,Δz)=(0.35 m,0,1.25 m),代入式(1),將點云轉(zhuǎn)換到車體坐標系下,后續(xù)所有計算都是在車體坐標系下進行。通過PCL點云庫中直通濾波算法劃分感興趣區(qū)域,林果園場景設(shè)置濾波范圍在x軸方向上為(0,12.0 m),在y軸方向上為(-4.0 m,4.0 m),在z軸方向上為(-0.7 m,2.25 m);江蘇大學內(nèi)場景設(shè)置濾波范圍在x軸方向上為(0,12.0 m),在y軸方向上為(-3.5 m,3.5 m),在z軸方向上為(-0.7 m,2.25 m),保留范圍內(nèi)點云數(shù)據(jù)。使用體素化網(wǎng)格方法實現(xiàn)下采樣,在減少點云數(shù)量的同時保持點云的形狀特性,本文兩種場景中都設(shè)置體素為邊長0.1 m的立方體。使用統(tǒng)計濾波方法移除噪聲點與離群點,本文上述場景中都設(shè)置k臨近搜索點數(shù)為50個,判定閾值為1.0倍標準差。林果園場景點云預(yù)處理結(jié)果如圖6所示。

圖6 林果園場景點云預(yù)處理結(jié)果Fig.6 Results of point cloud preprocessing in field of orchard

江蘇大學校園內(nèi)場景點云預(yù)處理結(jié)果如圖7所示。

圖7 江蘇大學內(nèi)場景點云預(yù)處理結(jié)果Fig.7 Results of point cloud preprocessing in field of UJS

三維激光原始點云為每幀約包含28 800個點,林果園場景中去除無效點后約包含17 000個點,感興趣區(qū)域內(nèi)包含3 000～5 500個點,下采樣后包含1 100～1 300個點,去除離群點后包含800～1 000個點;江蘇大學內(nèi)場景中去除無效點后約包含15 000個點,感興趣區(qū)域內(nèi)包含1 000～3 000個點,下采樣后包含400～600個點,去除離群點后包含300～500個點。

感興趣區(qū)域劃分后,x軸保留機器人前方12 m范圍內(nèi)點云信息,去除較遠距離點云與機器人后方點云,y軸范圍保留左右兩側(cè)樹行點云信息,去除隔壁樹行的點云,z軸去除地面點云,激光安裝高度為1.25 m,因此可以保留0.55 m以上樹木點云,針對本文中兩種實驗場景,都可以有效保留樹木冠層點云。下采樣和統(tǒng)計濾波后點云數(shù)量可以減少至1 000以下,下降約96.5%,極大地減少了計算量,并成功保留了樹木冠層輪廓信息,為后續(xù)導航線提取做準備。

2.2 插值導航線

使用DBSCAN方法對預(yù)處理后點云進行聚類,設(shè)置聚類近鄰搜索的搜索半徑為0.5 m,在三維點云數(shù)據(jù)預(yù)處理后,林果園場景中一幀點云的數(shù)量有800～1 000點,一棵桂花樹的平均點云數(shù)量為100～140點,因此設(shè)置DBSCAN算法中每一類的點云數(shù)目最少為80,最多為200;按照同樣的方法,江蘇大學內(nèi)場景中設(shè)置每一類的點云數(shù)目最少為40,最多為150,如圖8所示,在感興趣區(qū)域內(nèi)每棵櫻花樹的點云都可以成功聚為一類,使用外接立方體將每一類進行標記。

圖8 點云聚類效果Fig.8 Clustering effects of point cloud

采用式(2)中的方法計算每棵樹木在車體坐標系下的位置,設(shè)置式(3)中的間隔系數(shù)δ=0.2,即插值導航線上相臨兩導航點間的x軸距離為0.2 m,感興趣區(qū)域在x軸方向為0～12 m,所以插值導航線共包含61個導航點。經(jīng)過測試,插值算法的代碼運行時間為0.022 s,所以設(shè)置插值算法的代碼運行頻率為10 Hz,與激光雷達頻率相同。林果園場景中樹行擬合與插值導航線計算結(jié)果如圖9a所示,導航線與左右兩側(cè)桂花樹中心的距離相同,但在感興趣區(qū)域內(nèi)單側(cè)只能檢測到3～5棵桂花樹,以此為節(jié)點進行插值,所得到的樹行曲線曲率變化較大,在樹行曲線的首尾端曲率變化最大,因此,單獨使用牛頓插值方法生成的導航線在首尾端存在曲率過大的問題。如圖9b所示,在櫻花樹大小不均勻的校園內(nèi)場景中,插值導航線與較大櫻花樹(樹1)間的距離小于機器人寬度的一半,會發(fā)生機器人與櫻花樹冠層碰撞的問題。

圖9 插值導航線Fig.9 Interpolated navigation line

2.3 NSVM導航線

對預(yù)處理后的點云再次進行降采樣和統(tǒng)計濾波,體素網(wǎng)格降采樣方法中設(shè)置網(wǎng)格為0.2 m×0.2 m×1 m的立方體,統(tǒng)計濾波器中k臨近搜索點數(shù)為30個,判定閾值為1.0倍標準差,二次處理后點云數(shù)量減少至80左右。本文設(shè)置NSVM的高斯核帶寬σ=7,式(5)中的間隔系數(shù)δ=0.2,與式(3)中相同。每幀點云訓練和NSVM導航線提取時間約為0.3 s,所以設(shè)置NSVM部分的代碼運行頻率為3 Hz。林果園場景中NSVM導航線提取結(jié)果如圖10a所示,導航線與左右兩側(cè)桂花樹冠層邊緣的距離基本相同。在櫻花樹大小不均勻的校園內(nèi)場景中,如圖10b所示,根據(jù)NSVM原理可知,較大櫻花樹的邊緣點會被選定為支持向量,由支持向量來計算最大分割線,會導致圖中NSVM導航線與樹1和樹2之間的距離相同,但與樹3和樹4之間的距離不同的問題。本實驗所采用的平臺為果園噴霧機器人,若單獨使用非線性支持向量機的方法生成導航線,會導致機器人對不同櫻花樹(例如樹3和樹4)的噴霧量與噴霧效果不同的問題。

圖10 NSVM導航線Fig.10 NSVM navigation line

2.4 融合優(yōu)化導航線

設(shè)置式(6)中的間隔系數(shù)δ=0.2,將插值導航線與NSVM導航線進行融合,并對其進行優(yōu)化,結(jié)果如圖11所示,圖中黃色曲線為插值導航線,藍色曲線為NSVM導航線,紅色曲線為融合優(yōu)化后導航線,林果園場景中,融合后導航線位于插值導航線與NSVM導航線中間,且更靠近插值導航線。在櫻花樹大小不均勻的場景下,如圖11b所示,融合優(yōu)化后導航線解決了僅使用插值導航線或NSVM導航線時所存在問題,保證噴霧機器人在樹行間行駛時,既不會與櫻花樹發(fā)生碰撞,又可以對不同體積的櫻花樹進行均勻噴灑。

圖11 融合優(yōu)化后導航線Fig.11 Integrated optimized navigation line

機器人在樹行間采集100幀點云數(shù)據(jù),分別計算插值導航線、NSVM導航線和融合后導航線,統(tǒng)計每個導航點的平均曲率,如圖12所示,導航線前端1/3部分,插值導航線和NSVM導航線曲率都較大,原因有兩點:算法自身的原因?qū)е聦Ш骄€首尾部分的曲率相較于中間部分偏大;對導航線前端部分進行了調(diào)整,使其經(jīng)過車體坐標系原點;導航線中間1/3部分,3條導航線的曲率及曲率變化幅度都較小;導航線后端1/3部分,插值導航線與NSVM導航線曲率較大,原因是由算法自身導致?？傮w來看,融合后的導航線曲率及曲率變化都小于插值導航線和NSVM導航線,且沒有發(fā)生鋸齒狀波動。計算每條導航線的曲率最大值、絕對平均值以及標準偏差,結(jié)果如表2所示,融合后導航線曲率最大值為0.048 m-1,平均值為0.018 m-1,標準偏差為0.011 m-1,相較于插值導航線分別下降81%、54%、74%,相較于NSVM導航線分別下降62%、55%、66%,表明融合后導航線更適合作為行間導航線。

表2 導航線曲率Tab.2 Curvature of navigation line m-1

圖12 導航點平均曲率Fig.12 Average curvature of navigation point

隨機選取1幀點云數(shù)據(jù),分別計算插值導航線、NSVM導航線和融合后導航線,噴霧機器人以0.5 m/s和1.0 m/s不同的行駛速度對每條導航線進行軌跡跟蹤實驗,實驗中設(shè)置前視距離為2.0 m;機器人的位姿通過RTK信號獲取,實驗結(jié)果如圖13所示,圖中黃色曲線為插值導航線,藍色曲線為NSVM導航線,紅色曲線為融合優(yōu)化后導航線,綠色曲線為跟蹤曲線,即軌跡跟蹤時記錄的RTK值。記錄機器人行駛軌跡與導航線之間的橫向誤差,結(jié)果如圖14所示,整體來看,機器人行駛速度較慢時,跟蹤效果較好。統(tǒng)計各跟蹤曲線橫向誤差的最大值、絕對平均值和標準偏差,結(jié)果如表3所示,機器人行駛速度為0.5 m/s時,融合后導航線軌跡跟蹤橫向誤差最大值為0.104 m,平均值為0.053 m,標準偏差為0.026 m-1,相較于插值導航線分別下降47%、39%、40%,相較于NSVM導航線分別下降58%、41%、51%;行駛速度為1.0 m/s時,融合后導航線軌跡跟蹤橫向誤差最大值為0.130 m,平均值為0.049 m,標準偏差為0.039 m,相較于插值導航線分別下降41%、36%、34%,相較于NSVM導航線分別下降46%、54%、46%。機器人在低速行駛時,RTK定位精度更準確,機器人底盤控制效果更好,因此機器人軌跡跟蹤更準確。機器人以不同行駛速度對融合后導航線跟蹤的最大橫向誤差為0.13 m,表明融合后導航線更有利于軌跡跟蹤,該算法能夠滿足果園行間自主導航的需求。

表3 導航線跟蹤橫向誤差Tab.3 Lateral error of navigation line tracking

圖13 軌跡跟蹤Fig.13 Trajectory tracking

圖14 跟蹤橫向誤差Fig.14 Tracking lateral error

3 結(jié)論

(1)對激光雷達獲取的點云進行預(yù)處理以減小計算量,分別通過濾波、聚類、曲線擬合的方法計算插值導航線,通過非線性支持向量機的方法計算NSVM導航線,動態(tài)計算兩者權(quán)重,將高頻更新的插值導航線與低頻更新的NSVM導航線進行融合與優(yōu)化。

(2)融合優(yōu)化后導航線平均曲率為0.018 m-1,較插值導航線平均曲率下降54%,較NSVM導航線平均曲率下降55%,表明融合后導航線更適合作為行間導航線。

(3)軌跡跟蹤實驗表明,行駛速度為0.5 m/s時,融合后導航線軌跡跟蹤橫向誤差最大值為0.104 m,行駛速度為1.0 m/s時,其橫向誤差最大值為0.130 m,證明融合優(yōu)化后導航線滿足果園行間自主導航的需求,可為果園地面裝備的行間自主導航提供技術(shù)參考。