果園行間3D LiDAR導航方法

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

（1. 中國農業大學藥械與施藥技術研究中心，北京 100193；2. 中國農業大學工學院，北京 100083；3. 中國農業大學理學院，北京 100193；4. 中國農業大學農業無人機系統研究院，北京 100193；5. 山西農業大學農業工程學院，太谷 030801）

0 引言

中國的水果種植面積與產量均居世界第一，然而落后的果園機械化水平無法滿足當下的水果產業需求。為了促進水果產業的綠色無公害、標準化與產業化的進程，提高農業勞動的生產效率和農產品商品率，保持水果生產質量的穩定性，提高我國水果生產的機械化與自動化成為亟待解決的問題[1-2]。果園機械自主導航技術是實現果園作業機械化、自動化、管理精細化的關鍵，也是實現果園精準施藥作業中人機分離的基礎[3]，相比于傳統的人工駕駛，該方法能夠有效解決水果生產中人力缺乏、勞動強度大等問題[4-8]。

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

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

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

1 導航系統

1.1 硬件組成

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

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

移動機器人為四輪驅動差速運動底盤，可直接通過CAN總線接收來自上位機的目標線速度v與目標角速度ω，實現任意轉彎半徑的軌跡跟蹤。定義滿足右手定則的機體坐標系{V}如下：以3D LiDAR中心為坐標原點o，x軸指向機器人的正前方，y軸平行于機器人的輪軸且指向左方，z軸垂直指向正上方，圖1為系統的硬件平臺與機體坐標系{V}。

1.2 軟件組成

系統的軟件模塊框圖如圖2所示。

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

2 導航方法

2.1 果樹定位

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

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

為降低后續處理的計算量，使用PCL中的“VoxelGrid filter”對點云集Pk″進行降采樣。設置體素尺寸為（0.08, 0.08, 0.08），即將邊長為0.08 m的立方體內的點等效成一個點，將濾波結果存放在Qk中。

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

2.2 導航線擬合

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

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

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

2.3 樹行跟蹤

由于移動機器人為線控底盤，且滿足差速運動學模型，故可直接接收來自上位機發送的線速度（v）與角速度（ω）指令，實現任意半徑的軌跡跟蹤。

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

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

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

將目標線速度v與目標角速度ω發送給線控底盤，即可實現對目標軌跡的跟蹤。

3 試驗結果與分析

3.1 定位試驗

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

由于移動機器人沿著果園的中心線行走，故可將航向定位偏差的真值當作0，CF輸出的結果就是真實的航向偏差。表1為CF輸出的航向定位偏差統計結果，規定以導航線為基準線，機體坐標系的x軸與基準線逆時針所成夾角為正，順時針所成夾角為負，可以看出，3次航向定位偏差正向最大均值為1.24°，負向最大均值為1.65°，平均偏差為0.84°，標準差均值為0.42°，基本滿足果園作業航向定位要求。

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

表2為橫向定位偏差統計結果，規定以導航線為基準線，移動機器人在基準線左側的橫向偏差記為負，移動機器人在基準線右側的橫向偏差記為正，可以看出，3次試驗中，RANSAC與LSM輸出的正向最大橫向定位偏差均值均為0.052 m，負向最大橫向定位偏差均值均為0.050 m，偏差均值均為0.028 m，標準差均值均為0.016 m，這主要是因為果園較為標準，RANSAC與LSM算法性能相當，導致擬合結果幾乎沒有什么差別；CF與RTK輸出的正向最大橫向定位偏差均值分別為0.034、0.036 m，負向最大橫向定位偏差均值分別為0.044、0.039 m，偏差均值均為-0.028 m，標準差均值均為0.016 m，CF的輸出結果與RTK輸出的真值幾乎相同，且CF的輸出較RANSAC與LSM有所提高，說明互補融合算法具有一定的抗干擾能力。

表2 橫向定位偏差統計結果 Table 2 Statistical results of lateral positioning deviation m

3.2 樹行跟蹤試驗

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

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

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

表3為RTK輸出的絕對橫向偏差統計結果，可以看出，3次跟蹤試驗的絕對最大橫向偏差均值為0.150 m，絕對最小橫向偏差均值為0.045 m，絕對偏差均值為0.098 m，絕對標準差均值為0.025 m。

表3 RTK輸出的絕對橫向偏差統計結果 Table 3 Absolute lateral deviation statistical results of RTK output m

3.2.2 梨園跟蹤

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

圖10a為梨園跟蹤時RTK記錄的實時軌跡，可以看出，在2個線速度的跟蹤試驗中，系統均能控制移動機器人在樹行中心線附近跟隨。圖10b為RTK輸出的橫向偏差，當v=0.68 m/s時，3次重復試驗中系統的橫向跟蹤偏差在27.8 cm以內，v=1.35 m/s時，3次重復試驗中系統的橫向跟蹤偏差在26.4 cm以內。

表4為2個線速度下RTK輸出的絕對橫向偏差統計結果，可以看出，線速度為0.68 m/s時，系統的最大絕對橫向偏差為21.3 cm，最小絕對橫向偏差為0.1 cm，均值為7.9 cm，標準差為5.4 cm；線速度為1.35 m/s時，系統的最大絕對橫向偏差為22.1 cm，最小絕對橫向偏差為0.3 cm，均值為10.9 cm，標準差為5.0 cm。

表4 真實梨園樹行跟蹤絕對橫向偏差統計結果 Table 4 Absolute lateral deviation statistical results of real Pear orchard’s tracking experiment m

2個不同線速度的樹行跟蹤，結果相差不大，但v=0.68 m/s的結果仍然略優于v=1.35 m/s，結合式（43）分析可知，在較低的線速度下，跟蹤的前視距離更小，系統調節的次數更多，但過多的調節次數會降低系統的穩定性，這也正是v=1.35 m/s的標準差略優于v=0.68 m/s的原因，也進一步表明改進后的純跟蹤算法對不同線速度的跟蹤具有一定的適應能力，系統的實時性能基本滿足果園低速作業要求。該結果比籬壁式仿真果園跟蹤試驗中的結果略差，主要是因為真實梨園路面長有雜草且不平整、移動機器人重量較輕，容易產生滑移，同時由于梨樹樹冠并不呈理想的軸對稱分布，導致系統的定位精度受到影響，從而造成移動機器人偏離中心線較大的距離。總體上，系統能夠在梨園下實現較為精準的定位，并且能夠控制移動機器人實現對梨園樹行的跟蹤。

4 結 論

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

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

2）基于差速運動機器人驗證了本導航系統的定位與跟蹤性能。系統在籬壁式仿真果園中的航向定位偏差在1.65°以內，橫向定位偏差在6.1 cm以內；絕對橫向跟蹤偏差在15 cm以內。

3）系統在梨園中以0.68 m/s的速度跟蹤樹行的最大絕對橫向偏差不超過21.3 cm，以1.35 m/s的速度跟蹤樹行的最大絕對橫向偏差不超過22.1 cm。表明本系統具有足夠的定位精度與跟蹤精度，可廣泛用于標準果園與復雜三維果園機械的自主導航，具有可靠的穩定性。