劉 慧 段云鵬 沈 躍

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

0 引言

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

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

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

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

1 材料與方法

1.1 果園移動(dòng)機(jī)器人導(dǎo)航系統(tǒng)

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

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

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

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

圖2 移動(dòng)機(jī)器人運(yùn)動(dòng)控制系統(tǒng)原理圖Fig.2 Mobile robot motion control system schematic

1.2 點(diǎn)云預(yù)處理

(1)

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

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

1.3 插值導(dǎo)航線擬合

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

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

(2)

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

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

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

圖3 插值導(dǎo)航線擬合Fig.3 Fitting of interpolated navigation line

(3)

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

ylefti——左側(cè)第i個(gè)樹行點(diǎn)y軸坐標(biāo)

yrighti——右側(cè)第i個(gè)樹行點(diǎn)y軸坐標(biāo)

1.4 NSVM導(dǎo)航線擬合

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

為減少訓(xùn)練時(shí)間,對(duì)預(yù)處理后點(diǎn)云再次進(jìn)行下采樣和離群點(diǎn)去除。對(duì)二次處理后的每個(gè)點(diǎn)Pi(xPi,yPi)添加標(biāo)簽ti

(4)

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

(5)

式中ypk——點(diǎn)pk的y軸坐標(biāo)

tk——點(diǎn)pk的預(yù)測(cè)標(biāo)簽,為0

xpk——點(diǎn)pk的x軸坐標(biāo)

1.5 導(dǎo)航線融合

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

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

(6)

(7)

然后對(duì)兩導(dǎo)航線上對(duì)應(yīng)導(dǎo)航點(diǎn)的橫向距離進(jìn)行歸一化,有

(8)

式中ρ——?dú)w一化后權(quán)重

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

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

(9)

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

1.6 導(dǎo)航線優(yōu)化

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

(10)

(11)

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

ywt1i——隊(duì)列中第1組融合后導(dǎo)航線上第i個(gè)導(dǎo)航點(diǎn)的y軸坐標(biāo)

(12)

對(duì)融合后導(dǎo)航線進(jìn)行平滑處理,本文采用三次準(zhǔn)均勻B樣條算法。若平面中有4個(gè)點(diǎn)p0、p1、p2、p3,則三次準(zhǔn)均勻B樣條插值的方程為

(13)

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

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

(14)

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

1.7 軌跡跟蹤

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

圖4 差速運(yùn)動(dòng)學(xué)模型示意圖Fig.4 Schematic of differential kinematics model

(15)

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

化簡(jiǎn)后有

(16)

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

(17)

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

(18)

2 實(shí)驗(yàn)

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

圖5 實(shí)驗(yàn)場(chǎng)景Fig.5 Experimental scene

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

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

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

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

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

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

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

2.2 插值導(dǎo)航線

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

圖8 點(diǎn)云聚類效果Fig.8 Clustering effects of point cloud

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

圖9 插值導(dǎo)航線Fig.9 Interpolated navigation line

2.3 NSVM導(dǎo)航線

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

圖10 NSVM導(dǎo)航線Fig.10 NSVM navigation line

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

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

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

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

表2 導(dǎo)航線曲率Tab.2 Curvature of navigation line m-1

圖12 導(dǎo)航點(diǎn)平均曲率Fig.12 Average curvature of navigation point

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

表3 導(dǎo)航線跟蹤橫向誤差Tab.3 Lateral error of navigation line tracking

圖13 軌跡跟蹤Fig.13 Trajectory tracking

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

3 結(jié)論

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

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

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