果園行間3D LiDAR導(dǎo)航方法

劉偉洪，何雄奎,3,4※，劉亞佳,3,4，武志明， 袁常健，劉理民，齊 鵬，李 天

（1. 中國(guó)農(nóng)業(yè)大學(xué)藥械與施藥技術(shù)研究中心，北京 100193；2. 中國(guó)農(nóng)業(yè)大學(xué)工學(xué)院，北京 100083；3. 中國(guó)農(nóng)業(yè)大學(xué)理學(xué)院，北京 100193；4. 中國(guó)農(nóng)業(yè)大學(xué)農(nóng)業(yè)無人機(jī)系統(tǒng)研究院，北京 100193；5. 山西農(nóng)業(yè)大學(xué)農(nóng)業(yè)工程學(xué)院，太谷 030801）

0 引言

中國(guó)的水果種植面積與產(chǎn)量均居世界第一，然而落后的果園機(jī)械化水平無法滿足當(dāng)下的水果產(chǎn)業(yè)需求。為了促進(jìn)水果產(chǎn)業(yè)的綠色無公害、標(biāo)準(zhǔn)化與產(chǎn)業(yè)化的進(jìn)程，提高農(nóng)業(yè)勞動(dòng)的生產(chǎn)效率和農(nóng)產(chǎn)品商品率，保持水果生產(chǎn)質(zhì)量的穩(wěn)定性，提高我國(guó)水果生產(chǎn)的機(jī)械化與自動(dòng)化成為亟待解決的問題[1-2]。果園機(jī)械自主導(dǎo)航技術(shù)是實(shí)現(xiàn)果園作業(yè)機(jī)械化、自動(dòng)化、管理精細(xì)化的關(guān)鍵，也是實(shí)現(xiàn)果園精準(zhǔn)施藥作業(yè)中人機(jī)分離的基礎(chǔ)[3]，相比于傳統(tǒng)的人工駕駛，該方法能夠有效解決水果生產(chǎn)中人力缺乏、勞動(dòng)強(qiáng)度大等問題[4-8]。

目前，國(guó)內(nèi)外針對(duì)果園導(dǎo)航方面的研究方法主要有全球衛(wèi)星導(dǎo)航系統(tǒng)（Global Navigation of Satellite System，GNSS）、機(jī)器視覺導(dǎo)航、多傳感器融合導(dǎo)航以及激光導(dǎo)航等[4,9-13]。郭成洋等[14]設(shè)計(jì)了一種基于RTK-BDS的自動(dòng)導(dǎo)航控制系統(tǒng)，利用卡爾曼濾波技術(shù)提高了該系統(tǒng)在果園中的定位精度，并在農(nóng)用車輛上進(jìn)行了直線跟蹤試驗(yàn)，在0.5 m/s的速度下，最大橫向偏差不大于0.086 m，平均偏差不大于0.036 m。Li等[15]基于低成本雙目視覺實(shí)現(xiàn)了丘陵地區(qū)野外山區(qū)道路3D自主導(dǎo)航線的提取，并分別在直路、多曲率復(fù)雜道路和起伏道路上進(jìn)行了試驗(yàn)，擬合的導(dǎo)航線與真實(shí)中線間最大偏差分別為0.133、0.195和0.216 m。彭順正等[16]針對(duì)復(fù)雜的矮化密植棗園，提出一種基于圖像處理的棗園導(dǎo)航基準(zhǔn)線生成算法，通過對(duì)多種天氣與噪聲多元疊加等條件進(jìn)行試驗(yàn)，視頻檢測(cè)結(jié)果表明，單一工況條件下算法動(dòng)態(tài)檢測(cè)準(zhǔn)確率達(dá)81.3%以上，每幀圖像處理平均耗時(shí)低于1.7 s。Bayar等[17]通過融合平面激光掃描儀、里程計(jì)和轉(zhuǎn)向編碼器的數(shù)據(jù)實(shí)現(xiàn)了機(jī)具在果園中的自動(dòng)導(dǎo)航，并通過李雅普諾夫（Lyapunov）函數(shù)證明了所設(shè)計(jì)的控制器的穩(wěn)定性。

激光導(dǎo)航，主要借助激光測(cè)距（Light Detecting and Ranging，LiDAR）傳感器對(duì)周圍的環(huán)境進(jìn)行實(shí)時(shí)感知，相比于傳統(tǒng)的圖像傳感器，LiDAR傳感器具有測(cè)距精度高、主動(dòng)發(fā)光，受光線影響小等優(yōu)點(diǎn)[18-20]。常見的基于LiDAR的導(dǎo)航主要包括平面激光掃描儀（Planer Laser，PL），通過PL對(duì)周圍環(huán)境進(jìn)行感知，提取樹干的位置信息，進(jìn)而實(shí)現(xiàn)作業(yè)機(jī)具的自主定位[21]。Zhang等[22]通過考慮標(biāo)準(zhǔn)化果園中的株距、行距和樹干直徑等農(nóng)藝要求，建立了不同的過濾閾值以消除PL點(diǎn)云數(shù)據(jù)的離散點(diǎn)，有效提高了果園環(huán)境下自動(dòng)導(dǎo)航的精度，實(shí)現(xiàn)了果園機(jī)具的自主運(yùn)行。薛金林等[23]以農(nóng)業(yè)機(jī)器人為平臺(tái)，基于LMS291-05激光掃描儀研究了果園單側(cè)存在行缺失情況下機(jī)器人的導(dǎo)航性能，試驗(yàn)分別在行距不均的冬青樹與梨樹中進(jìn)行，在冬青樹間的最大橫向偏差為17.5 cm，梨樹間為28 cm。牛潤(rùn)新等[20]通過二維激光雷達(dá)采集果園原始點(diǎn)云數(shù)據(jù)，經(jīng)過預(yù)處理、自適應(yīng)設(shè)定聚類閾值、二次曲線擬合、干擾剔除等步驟有效降低丘陵山區(qū)果園中的斜坡及雜草對(duì)果樹樹干檢測(cè)精度的影響，綜合樹干檢測(cè)試驗(yàn)得到95.5%的平均準(zhǔn)確率。周俊等[24]以密植果園為研究對(duì)象，針對(duì)GNSS在導(dǎo)航中衛(wèi)星信號(hào)被遮擋，單純的里程計(jì)存在累計(jì)誤差等問題，首先以激光雷達(dá)掃描樹行并使用圓弧聚類檢測(cè)樹干中心點(diǎn)，然后記錄在世界坐標(biāo)系中，導(dǎo)航作業(yè)時(shí)再次掃描樹行尋找樹干中心點(diǎn)并同先前記錄在世界坐標(biāo)系下的樹干中心點(diǎn)進(jìn)行匹配，利用匹配結(jié)果矯正里程計(jì)計(jì)算出機(jī)器人的位置和航向，10次重復(fù)試驗(yàn)表明，該方法在世界坐標(biāo)系x和y方向的定位誤差標(biāo)準(zhǔn)差均為8 cm。

基于二維激光掃描儀的方法能夠很好地適應(yīng)樹干明顯、管理相對(duì)規(guī)則的果園，然而對(duì)于樹冠茂密、樹干被遮擋、樹枝相連成片等復(fù)雜三維果園環(huán)境，二維激光雷達(dá)無法很好表征其三維場(chǎng)景特征，所提取的用于導(dǎo)航的信息容易受到較大的干擾[25]。因此，本文提出一種基于3D LiDAR的果園樹行識(shí)別與導(dǎo)航線擬合的方法，使用直通濾波算法縮小興趣區(qū)域（Region of Interest，ROI），提出挖空算法應(yīng)對(duì)樹枝相連成片，根據(jù)果樹在豎直方向滿足軸對(duì)稱的分布特征，提出一種體心等效樹干位置的方法；分別使用隨機(jī)采樣一致性（Random Sampling Consensus，RANSAC）算法與最小二乘法（Least Square Method，LSM）對(duì)機(jī)具左右樹行所在直線進(jìn)行擬合；為了提升系統(tǒng)的魯棒性與容錯(cuò)性，基于左右樹行平行的假設(shè)，提出一種基于平行度的互補(bǔ)融合（Complementary Fusion, CF）策略實(shí)現(xiàn)RANSAC與LSM擬合結(jié)果的互補(bǔ)融合，并求其中線作為目標(biāo)導(dǎo)航線；基于移動(dòng)機(jī)器人的差速運(yùn)動(dòng)學(xué)模型，對(duì)純跟蹤算法進(jìn)行改進(jìn)，實(shí)現(xiàn)機(jī)器人對(duì)樹行的跟蹤。

1 導(dǎo)航系統(tǒng)

1.1 硬件組成

系統(tǒng)主要由3D LiDAR（中國(guó)北科天繪公司R-Fans-16）、 RTK GNSS定位系統(tǒng)（中國(guó)華測(cè)導(dǎo)航公司P3-DU）、裝有Ubuntu18.04操作系統(tǒng)的工控機(jī)（處理器i7 10510U）和移動(dòng)機(jī)器人（中國(guó)松靈機(jī)器人公司Scout-Mini）組成。

3D LiDAR為一款16線機(jī)械激光雷達(dá)，安裝在移動(dòng)機(jī)器人正前方，距離地面垂直高度為0.50 m，其水平視場(chǎng)角為360°，在幀頻率為5～20 Hz時(shí)水平角分辨率為0.09°～0.36°，垂直視場(chǎng)角為±15°，垂直角分辨率為2°，點(diǎn)頻率為320 kHz，最大測(cè)距為200 m，測(cè)距精度為2 cm；RTK GNSS定位系統(tǒng)在固定解下的水平定位精度為±1 cm，該系統(tǒng)主要用于記錄機(jī)器人的實(shí)時(shí)軌跡，作為參考真值，對(duì)系統(tǒng)的定位與跟蹤精度進(jìn)行評(píng)價(jià)。

移動(dòng)機(jī)器人為四輪驅(qū)動(dòng)差速運(yùn)動(dòng)底盤，可直接通過CAN總線接收來自上位機(jī)的目標(biāo)線速度v與目標(biāo)角速度ω，實(shí)現(xiàn)任意轉(zhuǎn)彎半徑的軌跡跟蹤。定義滿足右手定則的機(jī)體坐標(biāo)系{V}如下：以3D LiDAR中心為坐標(biāo)原點(diǎn)o，x軸指向機(jī)器人的正前方，y軸平行于機(jī)器人的輪軸且指向左方，z軸垂直指向正上方，圖1為系統(tǒng)的硬件平臺(tái)與機(jī)體坐標(biāo)系{V}。

1.2 軟件組成

系統(tǒng)的軟件模塊框圖如圖2所示。

在第k幀下，LiDAR產(chǎn)生{V}下的原始三維點(diǎn)云數(shù)據(jù)集Pk，分別使用點(diǎn)云庫(kù)（Point Cloud Library，PCL）[26]中的“PassThrough filter”與“VoxelGrid filter”進(jìn)行ROI的裁剪與降采樣得到數(shù)據(jù)集Qk；使用PCL中的歐式聚類算法對(duì)Qk進(jìn)行聚類，同時(shí)求得每一類別的體心并將其投影至給定平面等效代替樹干在{V}下的位置，得到數(shù)據(jù)集Ek；分別使用RANSAC與LSM算法對(duì)Ek進(jìn)行擬合，得到左右樹行在{V}下直線方程的表達(dá)式Ll-RANSAC、Lr-RANSAC、Ll-LSM、Lr-LSM；基于余弦定理提出一種兩直線間的平行度計(jì)算方法，得到4組表示平行度的余弦值與最佳余弦值cos(α0)、cos(α1)、cos(α2)、cos(α3)與cos(αbest)；根據(jù)4組余弦值與最佳余弦值對(duì)Ll-RANSAC、Lr-RANSAC、Ll-LSM、Lr-LSM進(jìn)行互補(bǔ)融合，得到最終的左右樹行直線方程Lfit-l、Lfit-r，計(jì)算左右樹行的中線得到目標(biāo)導(dǎo)航線Lfit-nav；對(duì)純跟蹤算法（Pure Pursuit）進(jìn)行改進(jìn)，分別輸出目標(biāo)線速度（v）、目標(biāo)角速度（ω），用于控制移動(dòng)機(jī)器人跟蹤目標(biāo)軌跡。

2 導(dǎo)航方法

2.1 果樹定位

使用PCL中的直通濾波函數(shù)“PassThrough filter”對(duì)Pk進(jìn)行x、y、z三個(gè)維度的裁剪濾波以提取合適的ROI。設(shè)定成員函數(shù)“setFilterLimits()”中x軸方向的2個(gè)閾值參數(shù)為（-2dtre，5dtre），y軸方向?yàn)椋?1.25drow，1.25drow），z軸方向?yàn)椋?0.8hlidar，htre），其中drow為行距（4 m），dtre為株距（1.5 m），hlidar為3D LiDAR中心距離地面的高度（0.5 m），htre為樹高（4 m），將成員函數(shù)“setFilterLimitsNegative()”設(shè)置為“假”用于舍棄局外點(diǎn)而保留局內(nèi)點(diǎn)，將該算法處理后的點(diǎn)云保存至Pk'中，ROI選擇如圖3a所示。

對(duì)于果樹枝條相連成片，造成無法識(shí)別單棵果樹的情況，基于PCL中的“PassThrough filter”函數(shù)提出一種挖空算法，對(duì)Pk'進(jìn)行挖空、打斷。提前設(shè)定并初始化x軸上的2個(gè)邊界值xn、xp以及平移距離Δd，并定義中間變量xn′、xp′。當(dāng)滿足條件xn′

為降低后續(xù)處理的計(jì)算量，使用PCL中的“VoxelGrid filter”對(duì)點(diǎn)云集Pk″進(jìn)行降采樣。設(shè)置體素尺寸為（0.08, 0.08, 0.08），即將邊長(zhǎng)為0.08 m的立方體內(nèi)的點(diǎn)等效成一個(gè)點(diǎn)，將濾波結(jié)果存放在Qk中。

為實(shí)現(xiàn)果樹識(shí)別，使用PCL中的歐式聚類算法對(duì)經(jīng)過稀疏處理的點(diǎn)云Qk進(jìn)行聚類。由于距離傳感器較近的果樹點(diǎn)云較密（通常不超過5 000個(gè)），距離傳感器較遠(yuǎn)的果樹點(diǎn)云較為稀疏（通常不少于12個(gè)），為保證感知范圍內(nèi)的果樹被準(zhǔn)確識(shí)別，將該算法的主要參數(shù)聚類目標(biāo)點(diǎn)云設(shè)置為Qk、最小距離閾值設(shè)置為dtre/3、聚類點(diǎn)的最小數(shù)目設(shè)置為12、最大數(shù)目設(shè)置為5 000。為求得每一棵果樹在{V}下的位置，根據(jù)理想果樹樹形分布滿足中間集中、周圍發(fā)散，且從其正視圖可以看出樹冠呈軸對(duì)稱[25]分布的特點(diǎn)，使用該截面的幾何中心等效代替樹干的中心Otre（圖3b），將第k幀下第i棵果樹的體心記為itreO，其具體求解方法如下：首先求解聚類后第i棵果樹點(diǎn)云集沿x、y、z軸向的最值坐標(biāo)(xmax,xmin,ymax,ymin,zmax,zmin)，然后根據(jù)式（4）～（6）分別求解3個(gè)軸向最值坐標(biāo)的中點(diǎn)，得到該果樹的體心位置坐標(biāo)。由于果園導(dǎo)航主要關(guān)注果樹相對(duì)于移動(dòng)機(jī)器人的平面位置，因此，令式（6）為0得到式（7），將三維體心投影至二維平面，得到最終體心，將第k幀下的所有果樹的體心itreO組成的集合記為Ek，如式（8）所示。

2.2 導(dǎo)航線擬合

以x軸為中線，將2.1節(jié)中求得的點(diǎn)云集Ek分成左右2個(gè)組Ek-l與Ek-r且滿足式（9）。使用RANSAC算法[26-27]分別對(duì)點(diǎn)集Ek-l、Ek-r進(jìn)行擬合，結(jié)果分別表示為式（10）、（11），使用LSM算法[28]分別對(duì)點(diǎn)集Ek-l、Ek-r進(jìn)行擬合，結(jié)果分別表示為式（12）、（13）。

RANSAC具有較強(qiáng)的抗干擾能力，能夠在樣本數(shù)據(jù)較少時(shí)有效排除局外點(diǎn)的干擾，LSM在給出的所有點(diǎn)中進(jìn)行權(quán)衡，取綜合最優(yōu)，數(shù)據(jù)點(diǎn)較多時(shí)，結(jié)果較好，但在樣本數(shù)據(jù)集較少時(shí)容易受到局外點(diǎn)的干擾[29-30]?？梢娫跇颖緮?shù)據(jù)較少時(shí)，二者的擬合結(jié)果剛好互補(bǔ)，同時(shí)基于實(shí)際果園左右樹行平行的假設(shè)，取得最佳擬合結(jié)果的條件為左右樹行存在最佳平行度，故可使用平行度對(duì)2種算法的擬合結(jié)果進(jìn)行互補(bǔ)融合。圖4為平行度定義，在{V}下，存在2條直線Ll、Lr，且與y軸分別交于點(diǎn)B、C，于無窮遠(yuǎn)處交于點(diǎn)A，由此得到△ABC，點(diǎn)A、B、C分別對(duì)應(yīng)邊a、b、c，點(diǎn)A所對(duì)應(yīng)夾角為α，角α的余弦值cos(α)如式（14）所示。當(dāng)α趨于0時(shí)，cos(α)趨于1，其幾何意義表示Ll與Lr的平行程度最高，故使用該值表示兩直線的平行程度。鑒于實(shí)際情況下Ll、Lr的夾角不允許超過，故設(shè)定α取值區(qū)間為[0,]，則對(duì)應(yīng)的cos(α)取值區(qū)間為[0,1]。

根據(jù)平行度將左右樹行的不同表達(dá)式進(jìn)行互補(bǔ)融合。由互補(bǔ)濾波原理[31]定義式（16）、（17）的2個(gè)互補(bǔ)融合參數(shù)coef0、coef1，根據(jù)式（18）～（21）可求得最終融合結(jié)果。根據(jù)式（14）分別計(jì)算4個(gè)平行度cos(α0)、cos(α1)、cos(α2)、cos(α3)，定義并計(jì)算式（15）所示的最佳平行度cos(αbest)，設(shè)定容忍閾值cos(αth)=0.95。當(dāng)cos(αbest)>cos(αth)時(shí)，通過式（22）～（23）分4種情況對(duì)左右樹行的表達(dá)式進(jìn)行融合，否則執(zhí)行式（24）～（25）舍棄當(dāng)次融合并保留上次融合結(jié)果。4種情況具體為：1）當(dāng)cos(αbest)=cos(α0)，執(zhí)行式（18）和式（20）；2）當(dāng)cos(αbest)=cos(α1)，執(zhí)行式（18）和式（21）；3）當(dāng)cos(αbest)=cos(α2)，執(zhí)行式（19）～（20）；4）當(dāng)cos(αbest)=cos(α3)，執(zhí)行式（19）和式（21）。最后，根據(jù)式（26）求取左右樹行的中線，得到目標(biāo)導(dǎo)航線Lfit-nav。求解時(shí)，使用直線方程斜率k與截距b分別代替式（18）～（26）中相應(yīng)的直線標(biāo)識(shí)L，如由式（18）分別求得融合后的左樹行的斜率fitlk-與截距fitlb-分別為式（27）～（28），同理可求得其他直線的具體參數(shù)。最終求得的左樹行直線為式（29），右樹行直線為式（30），擬合的導(dǎo)航線所在直線為式（31）。

2.3 樹行跟蹤

由于移動(dòng)機(jī)器人為線控底盤，且滿足差速運(yùn)動(dòng)學(xué)模型，故可直接接收來自上位機(jī)發(fā)送的線速度（v）與角速度（ω）指令，實(shí)現(xiàn)任意半徑的軌跡跟蹤。

圖5為移動(dòng)機(jī)器人軌跡跟蹤示意圖，結(jié)合純跟蹤算法[32]可知，在目標(biāo)軌跡S上選擇一個(gè)點(diǎn)P作為跟蹤點(diǎn)，則機(jī)器人從當(dāng)前位置到達(dá)點(diǎn)P的軌跡可以視為一段半徑為R（m）的圓弧軌跡，對(duì)點(diǎn)P跟蹤的關(guān)鍵在于對(duì)圓弧軌跡半徑R的求解。在△EoP中，由正弦定理可得式（32），式中θ為航向偏差（rad），l為前視距離（m），進(jìn)一步展開得到式（33），進(jìn)而可以求出式（34）所示的跟蹤軌跡半徑R。過點(diǎn)P作PD垂直x軸于點(diǎn)D，在直角△oDP中，由正弦定理可得式（35），將其代入式（34）可得式（36）。

要控制移動(dòng)機(jī)器人跟蹤半徑為R的圓弧軌跡，在線速度v給定的情況下，只需根據(jù)式（37）求出目標(biāo)角速度ω，將式（36）代入式（37）得到式（38）。

由式（38）可以看出，ω與l成反比，當(dāng)l越大，ω越小，調(diào)節(jié)越平穩(wěn)，反之，越劇烈。在此，充分考慮前視距離與線速度的關(guān)系，并假設(shè)l與v滿足線性關(guān)系，表示為式（39），式中K1為比例系數(shù)，e為初始前視距離（m），表示移動(dòng)機(jī)器人在線速度為零時(shí)的前視距離。將式（39）代入式（38）得到式（40）。將航向偏差加以考慮，設(shè)定一個(gè)橫向偏差閾值dth（m），當(dāng)橫向跟蹤偏差達(dá)到該閾值以內(nèi)時(shí)，在式（40）中加入航向偏差負(fù)反饋K2θ對(duì)橫向調(diào)節(jié)加以抑制，提前對(duì)移動(dòng)機(jī)器人的航向進(jìn)行修正，防止移動(dòng)機(jī)器人在橫向偏差上調(diào)節(jié)過度進(jìn)而造成反復(fù)調(diào)節(jié)，由此得到ω的表達(dá)式（41），由式（35）和式（39）求得式（42）的θ，將式（42）代入式（41）求得ω的最終表達(dá)式（43）。

將目標(biāo)線速度v與目標(biāo)角速度ω發(fā)送給線控底盤，即可實(shí)現(xiàn)對(duì)目標(biāo)軌跡的跟蹤。

3 試驗(yàn)結(jié)果與分析

3.1 定位試驗(yàn)

在中國(guó)農(nóng)業(yè)大學(xué)校園內(nèi)搭建了一個(gè)長(zhǎng)8 m，寬4 m的人工籬壁式仿真果園，如圖6a所示，為了排除試驗(yàn)的偶然因素，定位試驗(yàn)共進(jìn)行3次重復(fù)。使用RTK定位系統(tǒng)建立仿真果園地圖（包括左右樹行與樹行中心線），試驗(yàn)時(shí)遙控移動(dòng)機(jī)器人沿仿真果園的中心線以0.33 m/s的速度勻速前進(jìn)，同時(shí)運(yùn)行本文所述的樹行擬合算法，實(shí)時(shí)輸出航向偏差與橫向偏差。圖6b為定位點(diǎn)云示意圖，可以看出，樹墻被挖空算法打斷成多個(gè)立方體，RANSAC與LSM對(duì)立方體的體心的擬合結(jié)果幾乎無差，CF融合后的樹行也接近重合。圖7a為RTK定位系統(tǒng)記錄的3次試驗(yàn)的實(shí)時(shí)軌跡，可以看出實(shí)時(shí)軌跡均勻分布在真實(shí)樹行的中心線附近。圖7b為3種擬合算法與RTK輸出的橫向偏差對(duì)比典型結(jié)果，以第2次試驗(yàn)為例，可以看出，3種擬合算法輸出的橫向偏差與RTK輸出的橫向偏差真值變化趨勢(shì)一致，RANSAC與LSM的偏差基本重合，CF的結(jié)果明顯優(yōu)于RANSAC與LSM。圖7c為CF輸出的橫向定位偏差誤差，表示3次試驗(yàn)中CF輸出的橫向偏差與RTK輸出的橫向偏差真值之間的誤差，可以看出3次試驗(yàn)的最大誤差不超過6.1 cm，滿足果園導(dǎo)航作業(yè)的橫向定位精度要求。

由于移動(dòng)機(jī)器人沿著果園的中心線行走，故可將航向定位偏差的真值當(dāng)作0，CF輸出的結(jié)果就是真實(shí)的航向偏差。表1為CF輸出的航向定位偏差統(tǒng)計(jì)結(jié)果，規(guī)定以導(dǎo)航線為基準(zhǔn)線，機(jī)體坐標(biāo)系的x軸與基準(zhǔn)線逆時(shí)針?biāo)蓨A角為正，順時(shí)針?biāo)蓨A角為負(fù)，可以看出，3次航向定位偏差正向最大均值為1.24°，負(fù)向最大均值為1.65°，平均偏差為0.84°，標(biāo)準(zhǔn)差均值為0.42°，基本滿足果園作業(yè)航向定位要求。

表1 CF輸出的航向定位偏差統(tǒng)計(jì)結(jié)果 Table 1 Heading positioning deviation statistical results of complementary fusion output (°)

表2為橫向定位偏差統(tǒng)計(jì)結(jié)果，規(guī)定以導(dǎo)航線為基準(zhǔn)線，移動(dòng)機(jī)器人在基準(zhǔn)線左側(cè)的橫向偏差記為負(fù)，移動(dòng)機(jī)器人在基準(zhǔn)線右側(cè)的橫向偏差記為正，可以看出，3次試驗(yàn)中，RANSAC與LSM輸出的正向最大橫向定位偏差均值均為0.052 m，負(fù)向最大橫向定位偏差均值均為0.050 m，偏差均值均為0.028 m，標(biāo)準(zhǔn)差均值均為0.016 m，這主要是因?yàn)楣麍@較為標(biāo)準(zhǔn)，RANSAC與LSM算法性能相當(dāng)，導(dǎo)致擬合結(jié)果幾乎沒有什么差別；CF與RTK輸出的正向最大橫向定位偏差均值分別為0.034、0.036 m，負(fù)向最大橫向定位偏差均值分別為0.044、0.039 m，偏差均值均為-0.028 m，標(biāo)準(zhǔn)差均值均為0.016 m，CF的輸出結(jié)果與RTK輸出的真值幾乎相同，且CF的輸出較RANSAC與LSM有所提高，說明互補(bǔ)融合算法具有一定的抗干擾能力。

表2 橫向定位偏差統(tǒng)計(jì)結(jié)果 Table 2 Statistical results of lateral positioning deviation m

3.2 樹行跟蹤試驗(yàn)

3.2.1 籬壁式仿真果園樹行跟蹤

樹行跟蹤試驗(yàn)中K1=10，K2=-1，v=0.43 m/s，e=1.05 m，dth=0.08 m；同樣進(jìn)行3次重復(fù)。圖8a為實(shí)時(shí)跟蹤軌跡，可以看出，3次試驗(yàn)中移動(dòng)機(jī)器人的實(shí)時(shí)軌跡有先靠近中心線，再遠(yuǎn)離中心線，最后又靠近中心線的趨勢(shì)，這主要與樹行跟蹤控制器中的航向負(fù)反饋有關(guān)。圖8b為RTK輸出的橫向偏差，可以看出，3次試驗(yàn)跟蹤偏差都有先減小、后增大、再減小的調(diào)節(jié)趨勢(shì)，符合圖8a所記錄的實(shí)時(shí)軌跡的變化趨勢(shì)，以第2次試驗(yàn)（圖中綠色曲線所示）為例進(jìn)行說明。

移動(dòng)機(jī)器人從0 s（對(duì)應(yīng)橫向偏差為-21.4 cm）出發(fā)向中心線靠近，橫向偏差開始正向減小，至第4 s（對(duì)應(yīng)橫向偏差-12.73 cm）后，CF輸出的橫向偏差小于給定閾值dth（0.08 m），航向調(diào)節(jié)被引入，且航向調(diào)節(jié)作用大于橫向調(diào)節(jié)，于是移動(dòng)機(jī)器人開始背離中心線，橫向偏差反向增大，至第10 s（對(duì)應(yīng)橫向偏差為-17.4 cm），CF輸出的橫向偏差超出給定閾值dth（0.08 m），橫向調(diào)節(jié)作用大于航向調(diào)節(jié)作用，于是移動(dòng)機(jī)器人又開始向中心線靠近，橫向偏差逐漸減小，直至第15 s，橫向偏差變?yōu)?7.1 cm，如果測(cè)試果園足夠長(zhǎng)，移動(dòng)機(jī)器人將按照該規(guī)律反復(fù)調(diào)節(jié)，直至橫向偏差與航向偏差均消失。跟蹤試驗(yàn)證明本文所提出的導(dǎo)航線的擬合方法在籬壁式仿真果園中滿足導(dǎo)航所需的定位精度，改進(jìn)的純跟蹤控制算法能夠控制移動(dòng)機(jī)器人進(jìn)行樹行跟蹤，且具有一定的航向調(diào)節(jié)作用，能夠有效減少跟蹤過程中的超調(diào)次數(shù)。

表3為RTK輸出的絕對(duì)橫向偏差統(tǒng)計(jì)結(jié)果，可以看出，3次跟蹤試驗(yàn)的絕對(duì)最大橫向偏差均值為0.150 m，絕對(duì)最小橫向偏差均值為0.045 m，絕對(duì)偏差均值為0.098 m，絕對(duì)標(biāo)準(zhǔn)差均值為0.025 m。

表3 RTK輸出的絕對(duì)橫向偏差統(tǒng)計(jì)結(jié)果 Table 3 Absolute lateral deviation statistical results of RTK output m

3.2.2 梨園跟蹤

為驗(yàn)證系統(tǒng)對(duì)真實(shí)復(fù)雜果園的適應(yīng)性，在中國(guó)農(nóng)業(yè)大學(xué)上莊試驗(yàn)站的梨園（N40°08′38.7276″，E116°11′30.4584″）進(jìn)行了樹行跟蹤試驗(yàn)，試驗(yàn)場(chǎng)景如圖9a所示，可以看出樹行中間長(zhǎng)有雜草，且路面不平整。圖9a為真實(shí)梨園試驗(yàn)場(chǎng)景，該梨園行距為4 m，株距為1.5 m。隨機(jī)選擇一棵梨樹進(jìn)行物理尺寸測(cè)量，圖9b為梨樹尺寸標(biāo)注，其主干為0.43 m，冠層高度為3.07 m，冠層下層厚度為2.7 m，冠層中層厚度為0.82 m。試驗(yàn)時(shí)隨機(jī)選擇梨園的一段，分別進(jìn)行兩個(gè)線速度的跟蹤試驗(yàn)，每個(gè)試驗(yàn)進(jìn)行3次重復(fù)，每次試驗(yàn)都從同一位置附近出發(fā)，并最終停在同一位置附近，單次試驗(yàn)所行走的路程約為27 m。在第一個(gè)線速度跟蹤試驗(yàn)中，設(shè)定式（43）中K1=20，K2=-1，取v=0.68 m/s，e=1.05 m，dth=0.08 m；在第二個(gè)線速度跟蹤試驗(yàn)中僅改變移動(dòng)機(jī)器人的線速度，且取v=1.35 m/s，其他參數(shù)均不變。圖9c為樹干提取結(jié)果，可以看出相連成片的梨樹墻被成功打斷成若干立方塊，且其體心在xoy平面上的坐標(biāo)被等效為對(duì)應(yīng)果樹在{V}下的位置，有效克服了傳統(tǒng)平面激光掃描儀只能獲取樹干信息實(shí)現(xiàn)果樹定位的缺點(diǎn)。圖9d為等效后的樹干的實(shí)時(shí)擬合結(jié)果，其中紅色直線為目標(biāo)導(dǎo)航線。

圖10a為梨園跟蹤時(shí)RTK記錄的實(shí)時(shí)軌跡，可以看出，在2個(gè)線速度的跟蹤試驗(yàn)中，系統(tǒng)均能控制移動(dòng)機(jī)器人在樹行中心線附近跟隨。圖10b為RTK輸出的橫向偏差，當(dāng)v=0.68 m/s時(shí)，3次重復(fù)試驗(yàn)中系統(tǒng)的橫向跟蹤偏差在27.8 cm以內(nèi)，v=1.35 m/s時(shí)，3次重復(fù)試驗(yàn)中系統(tǒng)的橫向跟蹤偏差在26.4 cm以內(nèi)。

表4為2個(gè)線速度下RTK輸出的絕對(duì)橫向偏差統(tǒng)計(jì)結(jié)果，可以看出，線速度為0.68 m/s時(shí)，系統(tǒng)的最大絕對(duì)橫向偏差為21.3 cm，最小絕對(duì)橫向偏差為0.1 cm，均值為7.9 cm，標(biāo)準(zhǔn)差為5.4 cm；線速度為1.35 m/s時(shí)，系統(tǒng)的最大絕對(duì)橫向偏差為22.1 cm，最小絕對(duì)橫向偏差為0.3 cm，均值為10.9 cm，標(biāo)準(zhǔn)差為5.0 cm。

表4 真實(shí)梨園樹行跟蹤絕對(duì)橫向偏差統(tǒng)計(jì)結(jié)果 Table 4 Absolute lateral deviation statistical results of real Pear orchard’s tracking experiment m

2個(gè)不同線速度的樹行跟蹤，結(jié)果相差不大，但v=0.68 m/s的結(jié)果仍然略優(yōu)于v=1.35 m/s，結(jié)合式（43）分析可知，在較低的線速度下，跟蹤的前視距離更小，系統(tǒng)調(diào)節(jié)的次數(shù)更多，但過多的調(diào)節(jié)次數(shù)會(huì)降低系統(tǒng)的穩(wěn)定性，這也正是v=1.35 m/s的標(biāo)準(zhǔn)差略優(yōu)于v=0.68 m/s的原因，也進(jìn)一步表明改進(jìn)后的純跟蹤算法對(duì)不同線速度的跟蹤具有一定的適應(yīng)能力，系統(tǒng)的實(shí)時(shí)性能基本滿足果園低速作業(yè)要求。該結(jié)果比籬壁式仿真果園跟蹤試驗(yàn)中的結(jié)果略差，主要是因?yàn)檎鎸?shí)梨園路面長(zhǎng)有雜草且不平整、移動(dòng)機(jī)器人重量較輕，容易產(chǎn)生滑移，同時(shí)由于梨樹樹冠并不呈理想的軸對(duì)稱分布，導(dǎo)致系統(tǒng)的定位精度受到影響，從而造成移動(dòng)機(jī)器人偏離中心線較大的距離?？傮w上，系統(tǒng)能夠在梨園下實(shí)現(xiàn)較為精準(zhǔn)的定位，并且能夠控制移動(dòng)機(jī)器人實(shí)現(xiàn)對(duì)梨園樹行的跟蹤。

4 結(jié) 論

本文針對(duì)平面激光掃描儀在果園導(dǎo)航中獲取息量少，導(dǎo)致無法有效應(yīng)對(duì)樹冠茂密、樹干被遮擋、樹枝相連成片的復(fù)雜三維果園場(chǎng)景，提出一種基于3D LiDAR的果園行間導(dǎo)航方法，并分別在籬壁式仿真果園與梨園中進(jìn)行了相關(guān)試驗(yàn)，主要結(jié)論如下：

1）以3D LiDAR為感知設(shè)備，通過直通濾波算法縮小導(dǎo)航所需ROI，使用歐式聚類方法找到ROI中的果樹，并使用每棵果樹的體心等效樹干位置，基于平行度實(shí)現(xiàn)對(duì)RANSAC與LSM所擬合的左右樹行的互補(bǔ)融合，并求其中心線得到目標(biāo)導(dǎo)航線，考慮移動(dòng)機(jī)器人的線速度與航向偏差的影響，對(duì)純跟蹤控制算法進(jìn)行改進(jìn)。

2）基于差速運(yùn)動(dòng)機(jī)器人驗(yàn)證了本導(dǎo)航系統(tǒng)的定位與跟蹤性能。系統(tǒng)在籬壁式仿真果園中的航向定位偏差在1.65°以內(nèi)，橫向定位偏差在6.1 cm以內(nèi)；絕對(duì)橫向跟蹤偏差在15 cm以內(nèi)。

3）系統(tǒng)在梨園中以0.68 m/s的速度跟蹤樹行的最大絕對(duì)橫向偏差不超過21.3 cm，以1.35 m/s的速度跟蹤樹行的最大絕對(duì)橫向偏差不超過22.1 cm。表明本系統(tǒng)具有足夠的定位精度與跟蹤精度，可廣泛用于標(biāo)準(zhǔn)果園與復(fù)雜三維果園機(jī)械的自主導(dǎo)航，具有可靠的穩(wěn)定性。