果園二維激光雷達自主導航控制研究

中圖分類號：S24 文獻標識碼：A 文章編號：2095-5553（2025）08-0090-07

Abstract：In modern agriculture，orchard plantprotection operations often face chalenges due to complex and unstructuredenvironments，necesitating thedevelopmentofautonomous navigationcapabilities fororchard vehicles.To address this need，anautonomous navigation system based on 2DLiDAR technology was developed.This system was designed to meetagronomicrequirementsof orchard planting，whileaccounting forkey environmental variables such as plantspacing，row spacing，and trunk diameter.Toenhance theaccuracyof navigation，noiseinthe2DLiDAR point cloud datawasefectivelyreduced bycustomized filtering thresholds.Feature extractionoftreetrunks was accomplished usingacombinationoftheEuclideanclusteringalgorithmandthethree-pointcolineargeometrytheoremtoaccurately identify centralfeature points fromthefiltered pointclouddata.Inadition，anavigation path was then generated using the least squares fiting methodtoensure high precisionpath navigation.Toenablereal-timeadaptability，a fuzzycontrol algorithmwasintegrated intothenavigation system，thereby alowing dynamicadjustmentsforautomatic drivingand path tracking.Experimentalresultsdemonstrated thesystem'seffectiveness，withtheaverageerorbetweenthefltered laser point cloud feature points and the actual tree positions measured at 8.96cm .During autonomous driving at a speed of 0.5m/s ，the vehicle maintained high navigation accuracy，with a maximum lateral deviation of 8.2cm and an average deviation of 6.81cm .These findings confirmed the system's robustness and practicality，showing that it significantly improved navigationprecision in thecomplex orchard environment and successfully enabled autonomous vehicle operatio Keywords：orchard;2DLiDAR;autonomous navigation;laser pointcloud data procesing;filtering and noisereduction

0 引言

近年來，隨著工業化進程的加快和人口老齡化，農業勞動力不足的問題越來越嚴重。水果是人類營養的重要來源，也是種植業的支柱產業[1]。相比于傳統農業，果園植保技術與機械化發展滯后。傳統植保農業作業一般由人或人操作的農業機械來進行，勞動重復且單調，操作人員容易疲憊，長時間作業會造成操作人員的操作精度下降，危險性上升。近年來，以農業機器人為首的現代化農業裝備逐漸發展，在很大程度上可解決上述問題[2]。

在早期的研究中，許多學者專注于以物理軌道為基礎的物理導航系統[3]和超聲波傳感器[4]，但這一系統容易受到外界環境的干擾，并沒有完全解放勞動力。隨著環境感知定位技術的不斷發展，GNSS導航[57逐漸成為熱門。而果樹由于遮擋不能穩定接收衛星信號，在果園環境中，樹冠、樹葉茂密，GNSS系統適應性較差。基于數字圖像處理的機器視覺導航技術具有靈活性大、信息獲取豐富等優點。目前常用的視覺傳感器主要是單目相機和雙目相機。單目相機結構簡單，可用于某些類型的已知環境下的導航路徑提取，由于結構限制，只能獲取二維信息。Matthies等8首次證明了雙目視覺技術用于地面移動車的可行性。周學成等9針對視覺導航系統在果園環境中面臨干擾因素多、圖像背景復雜等問題，提出了一種基于改進DeepLabv 3+ 網絡的火龍果園視覺導航路徑識別方法。彭書博等1°提出一種果園內導航線的檢測方法，增強果樹目標特征。近年來，雙目視覺技術在農業上也取得了廣泛應用。然而，機器視覺的硬件構造非常繁瑣，并且價格昂貴，對于處理器的性能要求較高。而且，圖像信息容易受到環境噪聲的干擾，從而影響導航系統的準確性。

與視覺導航相比，激光導航具有距離精度高、抗干擾能力強等優點。激光導航技術根據結構不同，可分為二維激光和三維激光。三維激光通過結合二維激光或單目視覺多線混合或增加盤式傾斜掃描裝置的固態旋轉在三維空間實現環境測繪。二維激光通常只是用一組激光傳感器在一條線上進行掃描。Thanpattranon等[使用二維激光雷達和Kingwell拖拉機為試驗平臺，設計一種自動曲線導航駕駛系統，試驗表明，路徑跟蹤的平均偏差為 0.275m 。標準差為 0.009mm 。吳城等[12提出一種基于單線激光雷達獲取環境信息的方法，研究機器人在樹行中心線的導航性能以及機器人到達行尾進行自主換行的能力。劉慧等[13]提出一種基于激光雷達三維點云的果園行間高低瀕雙源信息融合實時導航方法。劉理民等[14研制了一種果園自主導航兼自動對靶噴霧機器人，采集果樹信息確定興趣區內點云進行2D化處理得到果樹質心坐標和果樹行線，并確定果樹行中間線（導航線）。毛文菊等[15]設計了一種雙導航模式小型果園運輸機器人，可根據需求選擇行人引領導航或定點導航。胡廣銳等[16]提出了一種基于改進人工勢場法的機器人，利用LSM、Hough變換和RANSAC方法優化行間導航路徑。李秋潔等[提出一種基于激光雷達的行間路徑提取方法，構建多樣化虛擬果園環境仿真行間路徑導航過程，評估路徑提取算法性能。

在激光定位系統中，定位精度和質量取決于激光雷達的激光入射角、掃描物體幾何和物體表面特征等諸多因素。但是，現有的激光點云數據采集與處理研究忽略了雜草、不規則樹干等障礙物的影響。如果不進行必要的濾波，擬合的導航基線會出現較大的偏差。本文針對果園低速作業履帶式車輛，設計一種果園二維激光自主導航系統，綜合考慮履帶式車輛行駛路線上的干擾物對二維激光采集點云數據精度的影響，利用激光雷達距離閾值和統計濾波完成濾波過程；利用聚類算法和三點共線的重要幾何定理提取樹干的中心點特征；通過最小二乘法識別并提取線性導航路徑，并以履帶式車輛為試驗平臺進行試驗，驗證自主導航系統的準確性和魯棒性。

1 材料與方法

1.1 系統組成

基于電動履帶車的果園自主測試平臺主要由二維激光雷達、速度控制系統和驅動輪速度檢測系統組成。試驗臺結構如圖1所示。激光雷達安裝在車輛前端中央，距離地面 0.4m ，數據通過網口傳輸到主控制器工控機。速度控制系統由三相直流電機、電機驅動器和換向繼電器組成。基于Arduinomega256O，通過USB串口與主控制器工控機通信，通過比例積分微分PID控制實現自動轉向。速度控制系統基于歐姆龍E6B2-CWZ3E編碼器，通過AD轉換與Arduino通信。在測試平臺的中軸線上固定一個跟蹤裝置，并使該裝置的出水口盡量靠近地面。跟蹤裝置中放置純凈水，通過水滴軌跡記錄測試平臺的實際行走軌跡路徑。

標軸的夾角，點 （x_i，y_i） 為二維激光掃描的第 i 點的笛卡爾坐標。

圖1測試平臺結構

Fig.1 Test platform structure1.2D激光雷達2.IMU3.電源4.主控制器工控機5.Arduino6.速度控制系統

各硬件具體參數如表1所示，系統原理如圖2所示。

表1硬件性能參數 Tab.1Hardware performance parameters

1.2果樹位置信息確定

在使用二維激光雷達對果園環境進行映射時，可能會遇到樹干等障礙物造成的信號遮擋問題。激光掃描儀發射脈沖激光，并根據激光束與物體接觸后反射回來的時間來測量距離。如果激光束直接擊中樹干，那么從激光發射器到激光接收器的路徑上就存在一個障礙物，這會導致接收器獲取的是被樹干反射回來的脈沖激光信號。基于該原理，通過分析激光掃描數據來確定果樹的位置。激光雷達系統會收集環境中物體的距離信息，然后將這些信息轉換為點云數據。通過處理這些點云數據，可以提取出果樹所在的區域并確定它們的位置。

如圖3所示，建立以激光雷達安裝位置O為原點、車輛前進方向為 y 軸的直角坐標系。其中， ρ_i 為激光掃描器發射點與目標之間的距離， λ_i 是激光束與坐

圖3激光雷達掃描笛卡爾坐標Fig.3LiDAR scans the Cartesian coordinates

通過式（1）完成對果樹位置信息的笛卡爾坐標變換過程。

由于果園環境復雜，二維激光雷達收集的點云數據中包含眾多噪點和異常數據，嚴重影響導航路徑識別的精度。因此，需要先對獲取的點云數據進行濾波處理。如圖4（a）所示，在履帶車的行駛方向與規劃的導航路線存在較大偏差時，激光雷達發出的光束有時會透過果園內樹木之間的空隙，在更遠的距離探測到果樹信息。這種現象可能會引入非目標信息，對所捕獲的關鍵導航數據產生干擾。將第 i 個數據點設為q_i ，其特征向量如式（2）所示，設置距離閾值為 ρ_max 和x_max 。如果 ρ_ilt;ρ_max 且 x_imax ，則 q_i 保留，否則淘汰。

Φ_qi=[Φ_ρi，λ_i，x_i，y_i]^T

圖4激光數據干涉點類型

Fig.4Laser data interference point types

雜草等小型障礙不影響車輛行駛，試驗的激光雷達數據往往包含一些離散點，如圖4（c）所示。平均距離計算如式（3）所示。

式中： d_i ——從點 q_i 到任一點的歐氏距離；D_i 1 ?q_i 到所有 k 個鄰居的平均距離；H_i 計算的標準范圍；μ 距離均值；σ 距離標準差；g 1 標準范圍 H_i 的常系數。

當 D_igt;H_i 時，將該點視為離散點并剔除，否則保留。當獲得完整的果樹坐標時，使用歐式聚類對同一果樹上的所有坐標點進行聚類。 M_i，i+1 為激光數據點q_i 與 q_i+1 之間的歐式距離，計算如式（4）所示。

當 M_i，i+1lt; 聚類搜索半徑 r 時，兩點來自同一棵果樹，聚類為一種；當 M_i，i+1gt;r 時，2個點來自不同果樹。通過試驗發現二維激光雷達得到的點云數據區域是當前激光雷達高度下果樹樹十的外表面。果樹的樹干可以近似為圓柱體，利用三點共線性的幾何定理可以提取樹干的中心特征點。

如圖5所示，將樹干的中心特征點的極坐標設為N（ρ_n，λ_n） ，樹干簇連續的 N 個數據點為 q_n ，選取 ρ 值最小的點作為特征點 （ρ_min，λ_min），R_ave 表示果樹樹干的平均半徑。通過幾何推導得到式（5）。

1.3導航控制參數獲取

為獲取履帶車導航路徑，試驗需要直線擬合所有樹干的中心特征點。將左、右果樹的中心點坐標設置為 （x_?1i，y_?1i） 和 （x_n，y_ri） 。選取履帶車左、右兩側3棵果樹的中心點坐標，組成2組。果樹中心點坐標 （x_c，y_c） 計算如式（6）所示。

設擬合直線為 y=ax+b ，其中 a 表示擬合直線的斜率， b 表示與 y 軸的截距。參數 a，b 的最佳估計值根據式（7）求得。

當完成導航路徑擬合后，根據車輛的當前位置獲取自動導航控制的參數。如圖6所示，導航線為導航路徑，導航線與 y 軸夾角 θ 為航向偏差，激光中心位置與導航線的平行距離為橫向偏差。

圖6導航信息參數提取示意圖

航向偏差 θ 和橫向偏差 d 的計算如式（8）和式（9）所示。

d=b×sinθ

式中： K —導航路徑的斜率。

1.4導航控制器設計

導航控制器主要包括二維激光雷達數據收集處理系統、路徑追蹤系統以及速度控制系統，如圖7所示。

二維激光雷達用于實時收集果樹數據，這些數據被傳輸至主控制器的工控機，工控機對收到的激光雷達數據進行處理。系統識別導航路徑，計算履帶車目標速度并發送給Arduino。底層控制平臺采用PID控制電機旋轉，調整驅動輪速度，實現路徑跟蹤。

經過研究發現，履帶車移動同時受到橫向偏差 d 和航向偏差 θ 的影響[18]。因此， d 和 θ 作為模糊控制器的輸入， Ld 作為輸出。通過模糊控制器對 Ld 進行實時調節。當 d 和 θ 較大時，采用較小的 Ld 使車輛快速接近跟蹤路徑，減少系統調整時間，提高系統響應速度。當 d 和 θ 較小時，采用較大的 Ld 防止系統超調，提高系統穩定性。

2 試驗結果與分析

2.1定位和導航參數獲取精度測試

為測試系統的可靠性，在新疆維吾爾自治區喀什地區葉城縣核桃產業基地進行試驗，先后進行導航路徑擬合、參數采集精度測試、路徑軌跡跟蹤測試3個試驗。試驗果園果樹橫向間距為 8m ，縱向間距為 5m ○如圖8所示，以果樹P為參考，以左 4m 后 5m 為原點O位置，建立絕對坐標系 xOy ，在與二維激光雷達相同高度上，用卷尺測量果樹樹干的周長，得到果樹直徑為 0.27m ○

圖8系統可靠性試驗現場與建圖效果 Fig.8System reliabilitytesting site and mappingperformance

將測試平臺靜態放置在核桃園的中心，以捕獲二維激光點云數據。在測試平臺的行駛區域放有核桃樹枝作為噪聲干擾。以3棵果樹為目標，設定距離閾值ρ_max=15m，x_max=5m ，相鄰點個數 k=5 。距離閾值濾波處理后的激光點云如圖9（a）所示。

在圖9（a）中，歐式聚類后同一果樹上的激光點數量為6、7個點。激光點的密度與樹干到激光的距離和直徑有關。以3個數為1個周期進行激光數據處理。最遠樹干距離為 15m ，在激光測量的范圍內，由于激光角度分辨率是 0.36^° ，所以當定位一個最大距離為15m 、直徑為 27cm 的樹干時，缺少激光數據點的問題很小。因此，可以為主干中心特征點的提取提供足夠的激光點支持。

在圖9（b）中，得到的中心點特征圖中沒有干擾點，與樹干實際位置的誤差分別為 3.7cm.2.6cm 、13.8cm?17.8cm?12.3cm?3.6cm ，平均誤差為8. 96cm 。結果表明，獲取樹干中心點特征圖的方法滿足定位精度要求，該方法提取的導航路徑準確可靠。

將試驗設備放置在果園的任意地方，并進行二維激光點數據的掃描。以人工測量的航向偏差和橫向偏差為實際值，由算法獲取的航向偏差和橫向偏差為測量值。航向偏差測量范圍為 [-15^° ， 15^°. 1，每隔5進行一次采樣。為避免人為操作造成的隨機誤差，對每個位置進行3次測試，得出的平均數作為航向偏差和該角度計算結果的依據，如表2所示。其中， Δd 表示橫向偏差的實際值與測量值之間的誤差。

表2偏差數據Tab.2Biased data

由表2可知，航向角 θ 的最大偏差為 0.9^° ，平均絕對偏差為 0.628^° ，標準差為 0.197^° 。橫向偏差 d 最大誤差為 3.61cm （靜態測量），平均絕對偏差為2.599cm ，標準差為0.767。采用所提方法進行導航路徑識別和導航參數獲取，可滿足果園自主導航作業中試驗平臺的精度要求。

2.2路徑跟蹤精度測試

在研究過程中，測試平臺使用履帶車以 0.5m/s 的速度自動行駛，驗證二維激光雷達的果園車輛自主導航系統的性能。試驗選取 50m 為測試距離，每隔50cm 的位置進行精確的數據測量和記錄，獲取盡可能密集和全面的數據，以便更準確地刻畫和評估導航系統的性能。試驗被分為5組進行，減少產生統計誤差的可能性，并提高測試結果的可靠性和精確性，測試的平均跟蹤偏差如圖10所示。

由圖10可知，行駛至 20m 之前橫向偏差大多集中在上半區也就是右側，原因是履帶車在行駛時右側履帶驅動輪嚙合間隙比左側大，右側履帶裝配誤差，橫向偏差在車輛行駛至 30m 后逐漸減小。行駛至 45m 處，橫向偏差趨于0處振蕩。

圖10路徑跟蹤橫向偏差變化

由表3可知，5次試驗后的最大橫向偏差為8.2cm ，最大平均偏差為 6.81cm ，最大平均絕對偏差為 7.68cm ，平均橫向標準差為0.641。

表3路徑規劃偏差數據Tab.3 Path planning deviation data

3結論

1）在復雜的核桃園環境中實現履帶車的自主導航和路徑跟蹤，果樹之間涉及高噪聲和干擾；針對復雜干擾環境下的果園，提出一種基于歐式聚類算法的濾波算法，該算法通過設置不同的閾值，可以有效消除果園中微小擾動產生的噪聲，從而大大提高定位精度。

2）考慮株距、行距和樹干直徑等因素，基于最小二乘法和幾何線性擬合，提取果樹樹木中心特征點，得到的中心點特征圖中沒有干擾點，與樹干實際位置的誤差分別為 3.7cm.2.6cm.13.8cm.17.8cm.12.3cm 和 3.6cm ，平均誤差為 8.96cm 。獲取樹干中心點特征圖的方法滿足定位精度要求。

3）設計基于履帶式農業機器人二維激光雷達自主導航系統，最大橫向偏差為 8.2cm ，最大平均偏差為 6.81cm ，滿足果園車輛自主導航的精度要求，為果園實現自主導航提供指導。

參考文獻

［1］陳蕾，聶瑩，趙繼春．我國設施種植業從機械化到智能化的發展及思考[J]．中國農機化學報，2022，43（10）：78-85.Chen Lei，Nie Ying，Zhao Jichun．Development andthinking of facilityplanting frommechanizationtointellectualization in China[J].Journal of ChineseAgricultural Mechanization，2022，43（10）：78-85.

［2］胡東宇，高健，黃力平，等．南疆四地州核桃產業現狀與發展思路[J]．北方園藝，2021（13）：148—154.Hu Dongyu，Gao Jian，Huang Liping，et al. Currentsituation and development idea of walnut industry in fourprefecturesofSouthernXinjiang[J]. NorthernHorticulture，2021（13）：148-154.

[ 3]Keicher R， Seufert H. Automatic guidance for Agriculturalvehicles in Europe [J].Computers and Electronics inAgriculture，2000，25（1—2）：169－194.

[4]YayanU，YucelH，YaziciA.Alow cost ultrasonic basedpositioning system for the indoor navigation of mobilerobots[J].Journal of Intelligent amp;.Robotic Systems，2015，78：541—552.

［5］李雪峰，李濤，邱權，等．果園移動機器人自主導航研究進展[J]．中國農機化學報，2022，43（5）：156－164.Li Xuefeng，Li Tao，Qiu Quan，et al.Reviewonautonomous navigation for orchard mobile robots [J].Journal of Chinese Agricultural Mechanization， 2022，43（5）：156—164.

［6］姜寬舒，于泓，高菊玲，等．基于MissionPlanner的多功能水產養殖作業船自主導航控制系統設計與實現[J].中國農機化學報，2020，41（8）：148—155.JiangKuanshu，Yu Hong，Gao Juling，et al.Design andimplementation of autonomous navigation and controlsystem for multi-functional aquaculture operation shipbased on Mission Planner [J].Journal of ChineseAgricultural Mechanization，2020，41（8）：148-155.

［7］韓長杰，韓鴻飛，尤佳，等．自動移栽機工況信息監測系統設計與試驗[J]．中國農機化學報，2022，43（4）：60-65.Han Changjie，Han Hongfei，You Jia，etal.Research anddesign of operation information monitoring system forautomatic transplanter [J]. Journal of Chinese AgriculturalMechanization，2022，43（4）：60-65.

[8]Matthies L，Kelly A，Litwin T，et al.Obstacle detectionfor unmanned ground vehicles： A progress report [C].Proceedings of the Intelligent Vehicles '95 . Symposium.IEEE，1995：66—71.

[9］周學成，肖明瑋，梁英凱，等．基于改進DeepLabv3十的火龍果園視覺導航路徑識別方法[J].農業機械學報，2023，54（9）：35—43.Zhou Xuecheng，Xiao Mingwei，Liang Yingkai，et al.Navigation path recognition between dragon orchard usingimproved DeepLabv3+network[J].Transactionsof theChinese Society for Agricultural Machinery，2O23，54（9）：35-43.

[10］彭書博，陳兵旗，李景彬，等．基于改進YOLOv7的果園行間導航線檢測［J].農業工程學報，2023，39（16）：131—138.Peng Shubo，Chen Bingqi，Li Jingbin，et al.Detection ofthe navigation line between lines in orchard using improvedYOLOv7[J]. Transactions of the Chinese Society ofAgriculturalEngineering，2023，39（16）：131-138.

[11]ThanpattranonP，Ahamed T，Takigawa T.Navigation ofautonomous tractor for orchards and plantations using alaserrange finder：Automatic control of trailer positionwith tractor [J]．Biosystems Engineering，2O16，147：90—103.

[12]吳城，李曉娟．基于單線激光的果園機器人自主導航方法［J]．江蘇大學學報（自然科學版），2023，44（5）：530-539.Wu Cheng，Li Xiaojuan. Autonomous navigation methodof orchard robot based on single laser [J].Journal ofJiangsu University （Natural Science Edition），2023，44（5）：530-539.

[13］劉慧，段云鵬，沈躍．果園移動機器人激光雷達雙源信息融合實時導航方法[J].農業機械學報，2023，54（8）：249-258.Liu Hui，Duan Yunpeng，Shen Yue.Real-time navigationmethod of orchard mobile robot based on laser radar dual source information fusion[J].Transactions of the ChineseSociety for Agricultural Machinery，2023，54（8）：249-258.

[14］劉理民，何雄奎，劉偉洪，等．果園自主導航兼自動對靶噴霧機器人[J].智慧農業（中英文），2022，4（3）：63-74.Liu Limin，He Xiongkui，Liu Weihong，etal.Autonomous navigation and automatic target sprayingrobot for orchards[J]． Smart Agriculture，2O22，4（3）：63-74.

[15］毛文菊，劉恒，王小樂，等．雙導航模式果園運輸機器人設計與試驗[J].農業機械學報，2022，53（3）：27-39，49.Mao Wenju，Liu Heng，Wang Xiaole，et al. Design andexperiment of dual navigation mode orchard transportrobot [J]. Transactions of the Chinese Society forAgriculturalMachinery，2022，53（3）：27—39，49.

[16］胡廣銳，孔微雨，齊闖，等．果園環境下移動采摘機器人導航路徑優化[J].農業工程學報，2021，37（9）：175—184.Hu Guangrui， KongWeiyu，Qi Chuang， etal.Optimization of the navigation path for a mobile harvestingrobot in orchard environment [J]. Transactions of theChinese Society ofAgricultural Engineering， 2021，37（9）：175—184.

[17］李秋潔，丁旭東，鄧賢．基于激光雷達的果園行間路徑提取與導航[J]．農業機械學報，2020，51（S2）：344—350.Li Qiujie，Ding Xudong，Deng Xian. Intra-row pathextractionandnavigationfororchards basedonLiDAR [J]. Transactions of the Chinese Society forAgricultural Machinery，2020，51（S2）：344—350.

[18］郭成洋．果園智能車輛自動導航系統關鍵技術研究[D].楊陵：西北農林科技大學，2021.