摘要：為實(shí)現(xiàn)丘陵山地柑橘果園機(jī)器人的自主導(dǎo)航， 設(shè)計(jì)了一種基于激光雷達(dá)和慣性傳感器的果園自主導(dǎo)航系統(tǒng)． 使用激光SLAM融合多傳感器所獲數(shù)據(jù)， 并利用誤差狀態(tài)卡爾曼濾波器進(jìn)行位姿優(yōu)化， 從而構(gòu)建果園的三維全局點(diǎn)云地圖． 對(duì)全局地圖進(jìn)行分析和處理， 確定機(jī)器人的安全行駛區(qū)域， 并應(yīng)用基于機(jī)器人運(yùn)動(dòng)學(xué)多約束條件的三次非均勻B樣條曲線(xiàn)軌跡優(yōu)化算法實(shí)現(xiàn)路徑規(guī)劃， 使用NDT-ICP算法實(shí)現(xiàn)機(jī)器人在全局地圖中的定位． 為適應(yīng)丘陵山地果園地形復(fù)雜性， 提出了一種基于預(yù)瞄跟蹤的自校正增量PID控制策略， 利用遞歸最小二乘法實(shí)時(shí)調(diào)整PID參數(shù)， 并在機(jī)器人履帶式行走機(jī)構(gòu)上進(jìn)行了系統(tǒng)整合測(cè)試． 試驗(yàn)結(jié)果表明， 誤差狀態(tài)卡爾曼濾波器優(yōu)化后的地圖精度與優(yōu)化前相比有明顯提升． 機(jī)器人以1.2 m/s速度行駛時(shí)， 導(dǎo)航控制系統(tǒng)的直線(xiàn)行駛平均位置偏差和航向角偏差分別為0.18 m和4.2°， 轉(zhuǎn)彎行駛平均位置偏差和航向角偏差分別為0.38 m和16.7°， 可滿(mǎn)足丘陵山地柑橘果園智能農(nóng)機(jī)自主導(dǎo)航作業(yè)需求．

關(guān) 鍵 詞：柑橘果園機(jī)器人； 激光SLAM； 誤差狀態(tài)卡爾曼濾波器； NDT-ICP算法； 自校正增量PID

中圖分類(lèi)號(hào)：TN964； S24

文獻(xiàn)標(biāo)志碼：A

文章編號(hào)：16739868（2024）08018414

Design and Test of Autonomous Navigation and

Precise Control System for Robots Used in

Citrus Orchards in Hilly and Mountainous Area

LIU Jie， FU Xinglan， LI Xu， LI Chuanhong， LI Guanglin

College of Engineering and Technology， Southwest University， Chongqing 400715， China

Abstract： To achieve autonomous navigation of robots in hilly and mountainous citrus orchards， a citrus orchard autonomous navigation system based on laser radar and inertial sensors was designed． The system utilized laser SLAM to fuse data from multiple sensors and an error state Kalman filter for pose optimization， thus constructing a three-dimensional global point cloud map of the orchard． The global map was then analyzed and processed to determine the robot’s safe driving area． A trajectory optimization algorithm based on three-dimensional non-uniform B-spline curves and constraints of robot kinematics was applied to achieve path planning． The NDT-ICP algorithm was used for localization of robot in the global map． To adapt to the complexity of terrain in hilly and mountainous citrus orchards， a self-correction incremental PID control strategy based on preview tracking was proposed． The PID parameters were adjusted in real-time using recursive least squares method， and integration testing was conducted on the robot’s track walking mechanism． The experimental results showed that the map accuracy was significantly improved after optimizing with the error state Kalman filter． When the robot traveled at a speed of 1.2 m/s， the average position error and heading angle error of the navigation control system in straight line driving were 0.18 m and 4.2°， respectively， while in turning driving， the average position error and heading angle error were 0.38 m and 16.7°， respectively． These results meet the requirements for autonomous navigation in smart agricultural machinery for hilly and mountainous citrus orchards．

Key words： citrus orchard robot； laser SLAM； error state Kalman filter； NDT-ICP algorithm； self-correcting incremental PID

柑橘是我國(guó)南方地區(qū)栽培面積最廣、 經(jīng)濟(jì)價(jià)值最顯著的水果之一， 也是全球最重要的果樹(shù)作物之一［1］． 南方柑橘果園主要分布于丘陵和山地地區(qū)， 果園地形復(fù)雜， 坡度較大， 且機(jī)械化程度較低． 為了提高果園生產(chǎn)效率和增加種植收益， 推動(dòng)果園作業(yè)機(jī)械向自動(dòng)化和智能化發(fā)展已勢(shì)在必行［2］． 因此， 果園作業(yè)機(jī)械的自主導(dǎo)航技術(shù)成為農(nóng)業(yè)移動(dòng)機(jī)器人研究的一個(gè)重要組成部分［3］．

機(jī)器人自主導(dǎo)航能夠根據(jù)周?chē)h(huán)境信息規(guī)劃最優(yōu)路徑， 并控制自身行走機(jī)構(gòu)沿著最優(yōu)路徑行駛． 目前廣泛使用的導(dǎo)航傳感器有全球?qū)Ш叫l(wèi)星系統(tǒng)（ Global Navigation Satellite System， GNSS） 、 慣性測(cè)量單元 （ Inertial Measurement Unit， IMU） 、 視覺(jué)相機(jī)、 激光雷達(dá)等［4］． 魏爽等［5］使用GNSS系統(tǒng)定位， 根據(jù)用戶(hù)設(shè)置生成預(yù)設(shè)路徑， 建立基于預(yù)瞄點(diǎn)搜索的純追蹤模型， 并將該模型用于農(nóng)機(jī)轉(zhuǎn)向控制， 從而實(shí)現(xiàn)農(nóng)機(jī)的自主導(dǎo)航． GNSS系統(tǒng)能在空曠的環(huán)境中展現(xiàn)出其優(yōu)越性和巨大的潛力， 但在果園環(huán)境中， 果樹(shù)的樹(shù)冠和枝杈較密， GNSS系統(tǒng)由于遮擋無(wú)法穩(wěn)定接收衛(wèi)星信號(hào)， 容易造成機(jī)器人丟失行進(jìn)目標(biāo)和自身定位信息， 難以單獨(dú)應(yīng)用于果園環(huán)境．

隨著機(jī)器視覺(jué)技術(shù)的發(fā)展， 圖像處理方法變得更加豐富， 處理難度降低， 同時(shí)相較激光雷達(dá)和GNSS系統(tǒng)， 攝像頭成本更低， 所以近年來(lái)國(guó)內(nèi)外有許多學(xué)者利用機(jī)器視覺(jué)技術(shù)來(lái)進(jìn)行導(dǎo)航的研發(fā)與設(shè)計(jì)． 賴(lài)漢榮等［6］使用單目相機(jī)獲取玉米苗帶圖像， 經(jīng)過(guò)圖像處理后， 利用最小二乘法擬合出玉米苗帶， 從而根據(jù)機(jī)器人航向與玉米苗帶偏差實(shí)時(shí)調(diào)整導(dǎo)航線(xiàn)， 實(shí)現(xiàn)玉米除草機(jī)器人的自主作業(yè)． 張振乾等［7］利用雙目相機(jī)獲取機(jī)器人前方場(chǎng)景點(diǎn)云， 對(duì)點(diǎn)云處理后進(jìn)行二維投影， 然后采用改進(jìn)K-means算法分離兩側(cè)香蕉樹(shù)， 從而進(jìn)行導(dǎo)航線(xiàn)的提?。?Ball等［8］設(shè)計(jì)了一款名為AgBot的機(jī)器人， 通過(guò)雙目相機(jī)和GNSS系統(tǒng)感知周?chē)h(huán)境及自身定位， 并利用ROS系統(tǒng)的move_base功能包實(shí)現(xiàn)大面積開(kāi)闊農(nóng)田環(huán)境下農(nóng)業(yè)果園機(jī)器人的自主作業(yè)．

在開(kāi)闊、 平整的農(nóng)田中， 以上研究在準(zhǔn)確性、 魯棒性、 適應(yīng)性方面均展現(xiàn)出良好效果， 能夠滿(mǎn)足農(nóng)田作業(yè)需求． 但是在丘陵山地， GNSS系統(tǒng)信號(hào)丟失嚴(yán)重， 無(wú)法使用衛(wèi)星定位． 果園地形高低起伏， 導(dǎo)致地圖構(gòu)建難度增加， 并且地形復(fù)雜， 坡度變化較大， 容易出現(xiàn)打滑及側(cè)傾現(xiàn)象， 增加了機(jī)器人自身控制的難度． 為應(yīng)對(duì)該挑戰(zhàn)， 本文提出了一種基于激光雷達(dá)（Light Detection and Ranging， LiDAR）和慣性傳感器（Inertial Measurement Unit， IMU）的丘陵山地柑橘果園機(jī)器人自主導(dǎo)航系統(tǒng)， 該系統(tǒng)能感知周?chē)h(huán)境的起伏變化和機(jī)器人自身姿態(tài)的改變， 以自校正PID控制器精確控制機(jī)器人在果園復(fù)雜環(huán)境下的運(yùn)動(dòng)． 首先利用SLAM（Simultaneous Localization and Mapping）算法融合激光雷達(dá)和慣性傳感器信息， 構(gòu)建果園三維點(diǎn)云全局地圖； 然后通過(guò)去除地面點(diǎn)云， 獲取安全行駛區(qū)域， 使用多約束規(guī)劃算法選取從起始點(diǎn)到目標(biāo)點(diǎn)的最優(yōu)路徑； 最后應(yīng)用NDT-ICP算法進(jìn)行局部點(diǎn)云和全局點(diǎn)云地圖匹配， 獲取當(dāng)前時(shí)刻機(jī)器人在全局地圖中的位姿， 從而實(shí)現(xiàn)機(jī)器人在果園中的路徑規(guī)劃與導(dǎo)航．

1 柑橘果園自主導(dǎo)航系統(tǒng)總體設(shè)計(jì)

1.1 丘陵柑橘果園環(huán)境分析

西南地區(qū)機(jī)械化柑橘果園主要分布在丘陵山地， 柑橘果樹(shù)株間距約為2.5～3 m， 果樹(shù)高度約為2 m， 冠層直徑約為1.5～2 m， 柑橘果樹(shù)行間距一般選用3.5～4.5 m， 果園內(nèi)作業(yè)道路寬度約為2 m［9］．

本研究機(jī)器人自主導(dǎo)航作業(yè)果園位于四川省廣安市武勝縣（30.35369411°N， 106.19301414°E）， 果園種植面積約為6.67 hm2， 果樹(shù)株間距約為3 m， 行間距約為4 m， 果園最大坡度約為25°． 柑橘果園樹(shù)行實(shí)物如圖1所示．

1.2 作業(yè)硬件平臺(tái)

圖2為柑橘果園機(jī)器人硬件組成， 該機(jī)器人選用16線(xiàn)激光雷達(dá)Velodyne VLP-16， 其最大測(cè)距為100 m， 精度為±3 cm， 垂直視野為30°（±15°）， 水平視野為360°， 垂直角分辨率為2°， 水平角分辨率根據(jù)旋轉(zhuǎn)速率不同從0.1°到0.4°不等． 在試驗(yàn)中， 激光雷達(dá)的掃描速率設(shè)定為10 Hz， 水平角分辨率為0.2°， 通過(guò)100 Mbps以太網(wǎng)與主控單元通信． 主控單元為搭載Intel Core i7-12700H的筆記本電腦， 預(yù)裝Ubuntu 18.04系統(tǒng)． 底盤(pán)采用履帶式底盤(pán)， 履帶由2個(gè)1.5 kW的直流無(wú)刷電機(jī)驅(qū)動(dòng)． 底盤(pán)控制芯片為STM32F103ZET6， 通過(guò)RS-485接口與電機(jī)驅(qū)動(dòng)器進(jìn)行通信．

1.3 工作原理

圖3展示了果園機(jī)器人的整體工作流程． 通過(guò)IMU感知自身姿態(tài)變換， 利用激光雷達(dá)獲取周?chē)h(huán)境點(diǎn)云信息， 對(duì)點(diǎn)云執(zhí)行預(yù)處理操作： 坐標(biāo)系變換、 降采樣、 去除離群點(diǎn)、 濾除地面點(diǎn)云． 利用誤差狀態(tài)卡爾曼濾波器（Error State Kalman Filter， ESKF）融合激光雷達(dá)和IMU的數(shù)據(jù)優(yōu)化機(jī)器人姿態(tài)． 獲取柑橘果園全局點(diǎn)云地圖后進(jìn)行歐式聚類(lèi)處理， 得到柑橘果樹(shù)和障礙物的聚類(lèi)邊界， 則邊界框之外的區(qū)域?yàn)闄C(jī)器人的安全通行區(qū)域， 從而可以規(guī)劃?rùn)C(jī)器人行駛至目標(biāo)點(diǎn)的最優(yōu)路線(xiàn)．

規(guī)劃路線(xiàn)后以ROS系統(tǒng)為導(dǎo)航基礎(chǔ)框架， 通過(guò) NDT-ICP點(diǎn)云匹配獲取機(jī)器人當(dāng)前位姿， 并輸出行駛方向的指令， 通過(guò)串口發(fā)送至STM32單片機(jī)， 然后STM32單片機(jī)通過(guò)RS-485接口和底盤(pán)電機(jī)驅(qū)動(dòng)器通信， 應(yīng)用自校正增量PID控制策略實(shí)現(xiàn)機(jī)器人的精確控制． 利用上述數(shù)據(jù)采集和路徑規(guī)劃算法， 結(jié)合機(jī)器人的實(shí)時(shí)坐標(biāo)和姿態(tài)， 成功地實(shí)現(xiàn)柑橘園機(jī)器人的自主導(dǎo)航功能．

2 路徑規(guī)劃過(guò)程

2.1 全局點(diǎn)云地圖構(gòu)建

地圖構(gòu)建過(guò)程中， 機(jī)器人坐標(biāo)系采用三維笛卡爾右手坐標(biāo)系， 其中x方向表示機(jī)器人橫向， y方向表示機(jī)器人航向， z方向表示垂直地面方向， 以機(jī)器人起始點(diǎn)為世界坐標(biāo)系原點(diǎn)． 激光雷達(dá)坐標(biāo)系和機(jī)器人坐標(biāo)系重合， IMU的坐標(biāo)系與機(jī)器人坐標(biāo)系方向一致， 僅存在平移轉(zhuǎn)換關(guān)系．

激光雷達(dá)掃描的原始點(diǎn)云數(shù)據(jù)經(jīng)過(guò)IMU里程計(jì)校正后， 可以消除機(jī)器人運(yùn)動(dòng)時(shí)所引發(fā)的點(diǎn)云畸變， 從而獲得周?chē)h(huán)境的點(diǎn)云信息［10］． 對(duì)原始點(diǎn)云進(jìn)行地面點(diǎn)的濾除， 然后從中提取特征點(diǎn)， 用于后續(xù)的運(yùn)動(dòng)估計(jì)和地圖構(gòu)建． 使用最近5幀的數(shù)據(jù)構(gòu)建局部地圖， 將當(dāng)前幀特征點(diǎn)與局部地圖進(jìn)行匹配， 估計(jì)機(jī)器人的位姿變換． 為提高位姿估計(jì)的精度， 利用ESKF耦合IMU和激光雷達(dá)數(shù)據(jù)． 將當(dāng)前幀的點(diǎn)云數(shù)據(jù)融入到全局地圖中， 即可獲得柑橘果園的全局三維點(diǎn)云地圖［11］．

2.2 ESKF優(yōu)化位姿

在丘陵山地， 柑橘果園主要分布在具有一定坡度的山丘上． 與平原地區(qū)的果園相比， 這些果園的地形變化更加顯著， 地勢(shì)高低起伏較大． 機(jī)器人在這樣的地形中運(yùn)行時(shí)， 經(jīng)常面臨著頻繁的俯仰角、 翻滾角及高度方向坐標(biāo)的變化， 導(dǎo)致系統(tǒng)對(duì)機(jī)器人自身姿態(tài)的準(zhǔn)確估計(jì)變得更加困難， 其精度也會(huì)受到影響． 地面的不平整性以及高低起伏的變化使得機(jī)器人需要適應(yīng)頻繁的姿態(tài)調(diào)整． 地圖構(gòu)建是機(jī)器人自主作業(yè)系統(tǒng)運(yùn)行的基礎(chǔ)和前提， 地圖的精度直接取決于機(jī)器人對(duì)當(dāng)前時(shí)刻自身姿態(tài)的準(zhǔn)確感知． 只有基于可靠的自身姿態(tài)估計(jì)， 機(jī)器人才能更好地理解和適應(yīng)地形的高低變化， 確保地圖構(gòu)建的準(zhǔn)確性和一致性．

因此， 解決丘陵山地柑橘果園的機(jī)器人運(yùn)行問(wèn)題需要綜合考慮感知系統(tǒng)的穩(wěn)定性、 對(duì)復(fù)雜地形的適應(yīng)能力以及對(duì)地圖構(gòu)建的高精度要求． 通過(guò)優(yōu)化姿態(tài)感知算法并結(jié)合多傳感器融合技術(shù)， 可以提高機(jī)器人在這種復(fù)雜環(huán)境中的自主運(yùn)行能力， 確保果園作業(yè)系統(tǒng)的高效運(yùn)行． 在單獨(dú)使用激光雷達(dá)構(gòu)建地圖時(shí)， 由于地形起伏， 高度方向的誤差較大， 導(dǎo)致地圖精度較低， 因此本研究采用ESKF耦合激光雷達(dá)對(duì)機(jī)器人的估計(jì)位姿和IMU對(duì)機(jī)器人的感知位姿［12］， 以實(shí)現(xiàn)對(duì)機(jī)器人位姿準(zhǔn)確且穩(wěn)定的估計(jì)． 相比于傳統(tǒng)擴(kuò)展卡爾曼濾波， ESKF在旋轉(zhuǎn)變換時(shí)， 狀態(tài)變量可采用最小化參數(shù)表達(dá)， 用三維變量表達(dá)旋轉(zhuǎn)增量， 從而減少計(jì)算量并提高濾波器效率． 同時(shí)它對(duì)初始狀態(tài)的選取和初始協(xié)方差矩陣的設(shè)置要求較低， 能夠更好地適應(yīng)不同的初始條件． 通過(guò)使用狀態(tài)更新公式的雅可比矩陣改善濾波器對(duì)非線(xiàn)性系統(tǒng)的收斂性， 同時(shí)引入擴(kuò)展?fàn)顟B(tài)來(lái)描述系統(tǒng)的非線(xiàn)性特征， 可降低對(duì)線(xiàn)性化假設(shè)的依賴(lài)性， 減小狀態(tài)估計(jì)誤差［13］．

在ESKF中， 狀態(tài)變量分為真值狀態(tài)變量、 名義狀態(tài)變量和誤差狀態(tài)變量． 在激光雷達(dá)和IMU數(shù)據(jù)耦合過(guò)程中， 將激光雷達(dá)的初始掃描結(jié)果設(shè)定為機(jī)器人的初始位置和方向． 設(shè)ESKF的真值狀態(tài)變量xt為：

xt=［pt， vt， Rt， bat， bωt， gt］T（1）

名義狀態(tài)變量x為：

x=［p， v， R， ba， bω， g］T（2）

誤差狀態(tài)變量δx為：

δx=［δp， δv， δR， δba， δbω， δg］T（3）

其中： p為當(dāng)前位置； v為速度； R為機(jī)器人姿態(tài)； ba為加速度計(jì)偏置； bω為陀螺儀偏置； g為重力加速度．

3種狀態(tài)變量之間的關(guān)系為：

pt=p+δp

vt=v+δv

Rt=R+δR

bωt=bω+δbω

bat=ba+δba

gt=g+δg

（4）

在連續(xù)時(shí)間模型中， 誤差變量的運(yùn)動(dòng)學(xué)方程如下：

δp·=δv

δv·=-R（a-ba）δθ-Rδba-ηa+δg

δθ·=-（ω-bω）δθ-δbω-ηω

δb·ω=ηbω

δb·a=ηba

δg·=0

（5）

式中： θ為三軸旋轉(zhuǎn)角度； a為直線(xiàn)加速度； ω為角速度； ηa， ηω為IMU測(cè)量噪聲； ηbω， ηba為IMU偏置模型的高斯噪聲．

在離散時(shí)間模型中， 誤差變量的運(yùn)動(dòng)學(xué)方程如下：

δp（t+Δt）=δp+δvΔt

δv（t+Δt）=δv+［-R（a-ba）δθ-Rδba+δg］Δt+ηv

δθ（t+Δt）=exp［-（w-bg）Δt］δθ-δbgΔt+ηθ

δbw（t+Δt）=δbw+ηg

δba（t+Δt）=δba+ηa

δg（t+Δt）=δg

（6）

在離散時(shí)間模型中， 誤差狀態(tài)變量δx的運(yùn)動(dòng)方程為：

δx=f（δx）+ω， ω～N（0， Q）（7）

式中： ω為過(guò)程高斯噪聲； Q為該噪聲的協(xié)方差矩陣：

Q=diag（O3×1， Cov（ηv）， Cov（ηθ）， Cov（ηg）， Cov（ηa）， O3×1）（8）

式中： ηv為速度計(jì)算模型的高斯噪聲； ηθ為旋轉(zhuǎn)角度計(jì)算模型的高斯噪聲．

運(yùn)動(dòng)方程的線(xiàn)性化形式為：

δx=Fδx+ω（9）

F=

IIΔt0000

0I-R（a-ba）Δt-RΔt0IΔt

00exp［-（w-bg）Δt］0-IΔt0

000I00

0000I0

00000I

（10）

式中： F為線(xiàn)性化后的雅可比矩陣．

ESKF運(yùn)行過(guò)程如下：

1） 預(yù)測(cè)過(guò)程： 預(yù)測(cè)名義狀態(tài)以及誤差狀態(tài)：

δxkpred=Fδxk-1（11）

Ppred=FPk-1FT+Q（12）

2） 更新過(guò)程： 通過(guò)激光雷達(dá)觀(guān)測(cè)誤差變量， k時(shí)刻的觀(guān)測(cè)方程為：

zk=hk（x）+vk.vk～N（0， V）（13）

式中： zk為觀(guān)測(cè)數(shù)據(jù)； vk為觀(guān)測(cè)過(guò)程高斯噪聲； V為該噪聲的協(xié)方差矩陣．

誤差狀態(tài)的雅可比矩陣Hk為：

Hk

=hk（x）δxk=hk（x）xkxkδxk=

diag（I3×1， I3×1， J-1r， I3×1， I3×1， I3×1）

（14）

卡爾曼增益Kk為：

Kk=PkpredHTk（HkPkpredHTk+V）-1（15）

k時(shí)刻誤差狀態(tài)δxk和協(xié)方差矩陣Pk的最優(yōu)估計(jì)為：

δxk=Kk（zk-h（xt））（16）

Pk=（I-KkHk）Pkpred（17）

3） 將誤差狀態(tài)歸入名義狀態(tài)， 作為k+1時(shí)刻名義狀態(tài)：

pk+1=pk+δpk

vk+1=pk+δpk

Rk+1=Rkexp（δθk）

bw，k+1=bw，k+δbw，k

ba，k+1=ba，k+δba，k

gk+1=gk+δgk

（18）

2.3 點(diǎn)云處理

由于果園整體點(diǎn)云數(shù)據(jù)量較大， 在獲得三維點(diǎn)云地圖后， 需要對(duì)點(diǎn)云進(jìn)行體素濾波下采樣， 即在不損失點(diǎn)云重要信息特征的情況下對(duì)點(diǎn)云進(jìn)行減量， 從而加快后續(xù)處理計(jì)算過(guò)程［14］． 對(duì)點(diǎn)云數(shù)據(jù)進(jìn)行統(tǒng)計(jì)濾波處理， 以消除噪聲點(diǎn)和去除離群點(diǎn)［15］． 經(jīng)過(guò)歐式聚類(lèi)， 成功分離出樹(shù)冠部分， 并對(duì)每個(gè)獨(dú)立的聚類(lèi)計(jì)算其矩形邊界框， 邊界框的外部區(qū)域即為機(jī)器人通行的安全區(qū)域［16］． 為提高機(jī)器人行駛的安全性， 選取樹(shù)行中最大的邊界框， 將其設(shè)定為當(dāng)前樹(shù)行的代表性邊界． 圖4為點(diǎn)云處理流程的結(jié)果， 橙色部分為果樹(shù)聚類(lèi)后的邊界框， 綠色矩形部分為機(jī)器人行駛的安全區(qū)域．

2.4 果園路徑規(guī)劃算法

機(jī)器人導(dǎo)航系統(tǒng)以周?chē)h(huán)境地圖、 機(jī)器人當(dāng)前位置信息， 以及起點(diǎn)和終點(diǎn)坐標(biāo)為輸入， 并利用路徑規(guī)劃算法規(guī)劃出可安全行駛的最優(yōu)路徑． 本研究采用基于多約束條件的路徑規(guī)劃策略． 首先根據(jù)全局地圖利用A*算法［17］進(jìn)行全局路徑搜索并提取特征點(diǎn)； 根據(jù)特征點(diǎn)曲率篩選出分段特征點(diǎn)與轉(zhuǎn)彎特征點(diǎn)， 并進(jìn)行特征點(diǎn)優(yōu)化； 以轉(zhuǎn)彎處首末端點(diǎn)約束、 底盤(pán)轉(zhuǎn)彎半徑約束與轉(zhuǎn)向機(jī)構(gòu)延遲約束構(gòu)建約束條件， 以轉(zhuǎn)彎半徑曲率均值最小為目標(biāo)建立B樣條曲線(xiàn)并進(jìn)行參數(shù)求解； 最后采用三次非均勻B樣條曲線(xiàn)表示全局路徑［18］．

圖5為果園場(chǎng)景的仿真驗(yàn)證結(jié)果， 黑色矩形為樹(shù)行膨脹后的障礙物信息． 相較于A*和特征點(diǎn)優(yōu)化路徑， 多約束條件路徑可以實(shí)現(xiàn)在樹(shù)行中間行駛， 并且在換行處曲線(xiàn)行駛， 更加符合農(nóng)業(yè)機(jī)械作業(yè)要求． 實(shí)際機(jī)械化果園中， 同一樹(shù)行的果樹(shù)存在間距， 但是一般作業(yè)機(jī)器不會(huì)從中穿過(guò)， 所以將樹(shù)行整體視為一個(gè)障礙物． 同時(shí)， 為保證作業(yè)機(jī)械以“U”形運(yùn)動(dòng)軌跡遍歷果園， 分別將障礙物延伸至果園邊界形成“U”形長(zhǎng)廊式結(jié)構(gòu)．

2.5 基于先驗(yàn)地圖的重定位

在果園環(huán)境中樹(shù)木密集， 三維點(diǎn)云地圖數(shù)據(jù)量較大， 所以在定位時(shí)加載全局地圖的時(shí)間代價(jià)較大， 會(huì)導(dǎo)致果園機(jī)器人作業(yè)效率下降． 在實(shí)際定位中， 點(diǎn)云配準(zhǔn)時(shí)并不需要將激光雷達(dá)采集到的當(dāng)前幀點(diǎn)云數(shù)據(jù)與完整的點(diǎn)云地圖進(jìn)行配準(zhǔn)， 只需要與當(dāng)前位置附近的局部點(diǎn)云地圖配準(zhǔn)即可， 所以采用分塊存儲(chǔ)策略． 將全局點(diǎn)云地圖按區(qū)域進(jìn)行劃分存儲(chǔ)， 在機(jī)器人初始定位時(shí)輸入?yún)^(qū)域標(biāo)號(hào)， 將實(shí)時(shí)點(diǎn)云數(shù)據(jù)與分塊局部地圖做匹配， 以減少內(nèi)存消耗， 提高定位效率［19］．

為確定機(jī)器人在果園環(huán)境中的具體位置， 需要將當(dāng)前時(shí)刻激光雷達(dá)掃描得到的點(diǎn)云和先驗(yàn)點(diǎn)云地圖進(jìn)行配準(zhǔn)， 點(diǎn)云配準(zhǔn)的輸出是點(diǎn)云之間的變換矩陣． 傳統(tǒng)的點(diǎn)云配準(zhǔn)方法為迭代最近點(diǎn)法（Iterative Closest Point， ICP）［20］， 通過(guò)最小化2個(gè)點(diǎn)云中最近點(diǎn)對(duì)的誤差之和來(lái)確定最優(yōu)變換矩陣． 目標(biāo)函數(shù)可以表示為：

C（R∧， T∧）=argmin∑Ni=1（Rpi+T）-qi2（19）

式中： N表示掃描的點(diǎn)數(shù)； R和T分別表示旋轉(zhuǎn)矩陣和平移矩陣； 目標(biāo)函數(shù)C（R∧， T∧）表示變換誤差； pi為當(dāng)前激光雷達(dá)掃描點(diǎn)云中點(diǎn)的坐標(biāo)； qi為先驗(yàn)點(diǎn)云地圖中點(diǎn)的坐標(biāo)．

ICP點(diǎn)云配準(zhǔn)在周?chē)h(huán)境基本不變的情況下表現(xiàn)良好， 但是實(shí)際生產(chǎn)中基本不變的環(huán)境很少， 在允許周?chē)h(huán)境小部分差異的情況下， 可應(yīng)用正態(tài)分布變換（Normal Distribution Transform， NDT）進(jìn)行處理． NDT是一種基于高斯分布對(duì)點(diǎn)進(jìn)行建模的方法［21］， 將參考點(diǎn)云（即先驗(yàn)地圖）劃分為多個(gè)單元， 每個(gè)單元都連續(xù)地按高斯分布建模， 將點(diǎn)云數(shù)據(jù)用多維變量的分布形式來(lái)表示， 此時(shí)2個(gè)點(diǎn)云之間的配準(zhǔn)問(wèn)題就轉(zhuǎn)換成了求解某個(gè)變換參數(shù)使得參與變換的點(diǎn)云在參考點(diǎn)云中概率密度之和最大的問(wèn)題， 采用優(yōu)化的方法求出這個(gè)變換參數(shù)， 即可得到2幅激光點(diǎn)云數(shù)據(jù)的最優(yōu)匹配．

局部點(diǎn)云—全局點(diǎn)云匹配確定機(jī)器人在先驗(yàn)全局點(diǎn)云地圖中的位姿時(shí)， 初始值的選取對(duì)ICP算法結(jié)果的影響較大， 當(dāng)初始值與真實(shí)值誤差較小時(shí)， 算法收斂較好． 相反， NDT算法受初始誤差的影響較ICP算法略低， 因此本文結(jié)合ICP和NDT算法進(jìn)行點(diǎn)云配準(zhǔn)定位， 點(diǎn)云匹配流程如圖6所示．

3 控制過(guò)程

3.1 航向角控制

在履帶式果園機(jī)器人的路徑跟蹤控制過(guò)程中， 定義二維全局地圖坐標(biāo)系XOY， 其中履帶式車(chē)輛的前進(jìn)方向與Y軸之間的角度為航向角［22］， 如圖7所示． 將整段導(dǎo)航路線(xiàn)分為多段， 選取階段性導(dǎo)航目標(biāo)點(diǎn)． 履帶車(chē)在導(dǎo)航目標(biāo)點(diǎn)處的航向角θ為：

θ=arctanXP-XOYP-YO（20）

式中： XP和YP為目標(biāo)導(dǎo)航點(diǎn)的坐標(biāo)； X0和YO為機(jī)器人當(dāng)前位置坐標(biāo)．

機(jī)器人當(dāng)前位置航向角與導(dǎo)航目標(biāo)點(diǎn)處航向角之差θ′為：

θ′=θ+δ（21）

式中： θ為機(jī)器人在導(dǎo)航目標(biāo)點(diǎn)處的航向角； δ為機(jī)器人當(dāng)前位置航向角．

3.2 自校正增量PID控制

在現(xiàn)代機(jī)電系統(tǒng)中， 位置和轉(zhuǎn)角的精確控制是至關(guān)重要的， 這不僅關(guān)乎系統(tǒng)的穩(wěn)定性， 還直接影響其實(shí)際應(yīng)用效果和安全性． 傳統(tǒng)的位置PID控制可能會(huì)在某些情況下遇到以下問(wèn)題： 系統(tǒng)受到干擾時(shí)， 控制器可能會(huì)過(guò)度反應(yīng)． 為了解決這個(gè)問(wèn)題， 引入增量PID控制［23］． 這種方法不是簡(jiǎn)單地考慮當(dāng)前誤差， 而是考慮誤差的增量， 所以能更好地平衡系統(tǒng)的響應(yīng)速度和穩(wěn)定性， 使得系統(tǒng)在受到擾動(dòng)或參數(shù)變化時(shí)更具魯棒性．

如圖8所示， 在丘陵山地果園中， 地面不平整會(huì)導(dǎo)致機(jī)器人在運(yùn)動(dòng)過(guò)程中經(jīng)常出現(xiàn)傾斜［24］． 這種傾斜狀態(tài)會(huì)導(dǎo)致兩側(cè)履帶受力不同， 機(jī)器人會(huì)面臨擺動(dòng)、 方向偏移和不均勻負(fù)載的情況， 從而使得傳統(tǒng)的PID控制器在維持穩(wěn)定性和高控制精度方面面臨挑戰(zhàn)． 為了應(yīng)對(duì)這些變化， 并保持系統(tǒng)的穩(wěn)定性和最優(yōu)控制性能， 采用遞歸最小二乘法（Recursive Least Squares， RLS）， 實(shí)時(shí)估計(jì)系統(tǒng)參數(shù)， 再利用Ziegler-Nichols整定方法計(jì)算PID參數(shù)值［25］．

在丘陵山地果園中， 機(jī)器人行進(jìn)過(guò)程中往往伴隨著俯仰角度和翻滾角度的變換， 并且左右履帶的高低不同， 會(huì)導(dǎo)致電機(jī)發(fā)生轉(zhuǎn)向和打滑， 僅依靠電機(jī)編碼器數(shù)據(jù)的反饋會(huì)造成較大的誤差． 利用卡爾曼濾波器融合電機(jī)編碼器數(shù)據(jù)和SLAM系統(tǒng)輸出的位姿變換信息， 可以提高自校正PID控制器的精度， 降低因?yàn)槁膸Т蚧斐傻恼`差．

自校正調(diào)節(jié)（Self-Tuning Regutator， STR）的設(shè)計(jì)思想， 是把未知被控對(duì)象參數(shù)的估計(jì)和控制器的設(shè)計(jì)分開(kāi)進(jìn)行． 自校正增量PID調(diào)節(jié)器由3部分構(gòu)成： 最小二乘遞推參數(shù)估計(jì)， 控制器的參數(shù)設(shè)計(jì)和增量PID調(diào)節(jié)器， 其控制系統(tǒng)結(jié)構(gòu)如圖9所示［26］．

3.2.1 增量式PID控制器

在離散時(shí)間下， 對(duì)于直線(xiàn)位移的增量式PID控制的公式可以表示為以下形式：

uek

=Kep［ek-ek-1+TeSTeIek+TeDTeS（ek-2ek-1+ek-2）］+uek-1

（22）

式中： uk為當(dāng)前時(shí)刻的控制量； Kep， TeI， TeD， TeS分別為比例常數(shù)、 積分時(shí)間常數(shù)、 微分常數(shù)和采樣周期； ek為k時(shí)刻誤差， rk為k時(shí)刻實(shí)際值， yk為k時(shí)刻預(yù)設(shè)值， ek=rk-yk．

在離散時(shí)間下， 對(duì)于差速轉(zhuǎn)角的增量式PID控制的公式可以表示為如下形式：

uθk

=Kθp［ek-ek-1+TθSTθIek+TθDTθS（ek-2ek-1+ek-2）］+uθk-1

（23）

式中： uθk為當(dāng)前時(shí)刻控制量； Kθp， TθI， TθD， TθS分別為比例常數(shù)、 積分時(shí)間常數(shù)、 微分常數(shù)和采樣周期．

3.2.2 實(shí)時(shí)參數(shù)估計(jì)算法

系統(tǒng)實(shí)時(shí)參數(shù)估計(jì)過(guò)程中會(huì)出現(xiàn)數(shù)據(jù)飽和問(wèn)題， 即當(dāng)數(shù)據(jù)累積到一定數(shù)量時(shí)， 參數(shù)估計(jì)的準(zhǔn)確度反而下降． 所以采用遺忘因子遞推最小二乘法， 可以對(duì)歷史數(shù)據(jù)進(jìn)行加權(quán)， 動(dòng)態(tài)地調(diào)整歷史數(shù)據(jù)對(duì)系統(tǒng)的影響［27］．

設(shè)系統(tǒng)模型方程為：

y（t）=φT（t）θ+e（t）（24）

式中： y（t）為系統(tǒng)模型輸出； φT（t）為系統(tǒng)采樣得到的數(shù)據(jù)信息向量； e（t）為系統(tǒng)噪聲； θ為參數(shù)向量．

遞推最小二乘法是利用系統(tǒng)的輸入輸出數(shù)據(jù){u（t）， y（t）}， 使準(zhǔn)則函數(shù)J（θ）極小， 從而得到參數(shù)θ的估計(jì)：

J（θ）=∑ti=1［y（i）-φT（i）θ］2（25）

為避免對(duì)采樣值的微小波動(dòng)過(guò)于靈敏而造成控制器參數(shù)的不斷變化， 引入開(kāi)關(guān)變量σ， 遞推最小二乘法公式如下：

θ∧（t）=θ∧（t-1）+K（t）［y（t）-φT（t）θ∧（t-1）］

K（t）=σP（t-1）φ（t）［λ+φT（t）P（t-1）φ（t）］-1

P（t）=1λ［I-K（t）φT（t）］P（t-1）

（26）

式中： θ∧為參數(shù)最優(yōu)估計(jì)向量； K為卡爾曼增益； P為估計(jì)誤差協(xié)方差矩陣； λ為遺忘因子． λ越小， 算法的跟蹤能力越強(qiáng)， 但參數(shù)的波動(dòng)越大， 遺忘因子一般選擇為0.95＜λ＜1．

4 系統(tǒng)試驗(yàn)驗(yàn)證

果園機(jī)器人的可靠性取決于機(jī)器人導(dǎo)航系統(tǒng)的精度， 高精度的導(dǎo)航系統(tǒng)是果園機(jī)器人能否可靠穩(wěn)定地完成各類(lèi)作業(yè)的前提． 果園機(jī)器人導(dǎo)航性能試驗(yàn)地點(diǎn)位于四川省廣安市武勝縣扶手村， 試驗(yàn)區(qū)縱向尺寸為115.3 m， 橫向尺寸為15.4 m， 以試驗(yàn)區(qū)地圖坐標(biāo)系的原點(diǎn)為機(jī)器人的起點(diǎn)． 機(jī)器人在實(shí)際柑橘果園中進(jìn)行試驗(yàn)的場(chǎng)景如圖10所示．

果園機(jī)器人性能的評(píng)價(jià)指標(biāo)包括建圖準(zhǔn)確度和導(dǎo)航精度． 建圖準(zhǔn)確度的衡量標(biāo)準(zhǔn)為所建三維地圖與實(shí)際環(huán)境之間的縱向與橫向幾何偏差， 以及在建圖過(guò)程中機(jī)器人位姿與預(yù)定位姿的位置和航向角偏差； 導(dǎo)航精度的衡量標(biāo)準(zhǔn)為測(cè)量機(jī)器人預(yù)規(guī)劃導(dǎo)航路徑與機(jī)器人實(shí)際運(yùn)動(dòng)路徑的偏差， 研究該導(dǎo)航系統(tǒng)精度是否能滿(mǎn)足機(jī)器人日常巡檢作業(yè)的需求［28］．

4.1 試驗(yàn)平臺(tái)

試驗(yàn)中使用的果園履帶機(jī)器人如圖11所示， 試驗(yàn)過(guò)程中設(shè)置激光雷達(dá)與地面的距離為1 000 mm， 慣性傳感器與激光雷達(dá)布置在同一平面， 二者之間的轉(zhuǎn)換只存在平移轉(zhuǎn)換， 平移向量為T(mén)=［0， 0.15， 0］． 試驗(yàn)使用RViz軟件實(shí)現(xiàn)環(huán)境信息可視化， 驗(yàn)證改進(jìn)算法的可行性．

4.2 試驗(yàn)設(shè)計(jì)與結(jié)果分析

4.2.1 算法優(yōu)化試驗(yàn)

為了驗(yàn)證ESKF是否能夠提高位姿估計(jì)的準(zhǔn)確性和魯棒性， 設(shè)定機(jī)器人起點(diǎn)為試驗(yàn)區(qū)地圖坐標(biāo)系的原點(diǎn)， 控制機(jī)器人在初始位置偏差和航向角偏差為零的情況下， 以0.8 m/s的速度進(jìn)行算法優(yōu)化試驗(yàn)．

試驗(yàn)步驟如下：

1） 如圖12所示， 在果園環(huán)境中選擇8個(gè)檢測(cè)點(diǎn)A～H， 所有檢測(cè)處均有位移輸入和轉(zhuǎn)向輸入．

2） 沿著關(guān)鍵點(diǎn)A～H進(jìn)行地圖的構(gòu)建， 在關(guān)鍵點(diǎn)設(shè)定預(yù)定位姿， 并記錄關(guān)鍵點(diǎn)的位置和機(jī)器人姿態(tài)．

3） 在地圖構(gòu)建過(guò)程中， 為保證盡可能小的測(cè)量誤差， 同時(shí)開(kāi)啟2個(gè)算法框架（使用ESKF和不使用ESKF）進(jìn)行地圖構(gòu)建．

4） 重復(fù)進(jìn)行多次試驗(yàn)， 并將試驗(yàn)數(shù)據(jù)進(jìn)行滑動(dòng)均值濾波， 消除異常點(diǎn)的干擾， 分析ESKF優(yōu)化特性．

地圖尺寸信息試驗(yàn)結(jié)果如表1所示， 未進(jìn)行優(yōu)化的地圖系統(tǒng)測(cè)量累積誤差為6.61 m， 經(jīng)過(guò)ESKF優(yōu)化后的地圖測(cè)量累積誤差為1.96 m． 在建圖過(guò)程中， 圖13展示了系統(tǒng)感知的機(jī)器人位姿與機(jī)器人實(shí)際位姿的偏差， 未優(yōu)化時(shí)機(jī)器人在關(guān)鍵點(diǎn)的平均位置偏差為13.57 cm， 平均航向角偏差為6.43°． 而優(yōu)化后的平均位置偏差為6.54 cm， 平均航向角偏差為2.53°． 通過(guò)對(duì)比可以明顯觀(guān)察到， ESKF優(yōu)化使得系統(tǒng)對(duì)機(jī)器人位姿的感知更為準(zhǔn)確， 地圖精度顯著提高， 可以為后續(xù)導(dǎo)航環(huán)節(jié)提供有力的支持．

4.2.2 點(diǎn)云匹配定位試驗(yàn)

為驗(yàn)證NDT-ICP算法在果園環(huán)境下進(jìn)行點(diǎn)云配準(zhǔn)定位的可靠性， 以試驗(yàn)區(qū)地圖坐標(biāo)系的原點(diǎn)為機(jī)器人出發(fā)的起點(diǎn)進(jìn)行試驗(yàn)分析． 分別利用NDT、 ICP和NDT-ICP對(duì)相同的163幀點(diǎn)云數(shù)據(jù)和全局點(diǎn)云進(jìn)行配準(zhǔn)， 多次循環(huán)， 并記錄點(diǎn)云配準(zhǔn)的效率和誤差． 點(diǎn)云匹配效率試驗(yàn)時(shí)， 設(shè)定點(diǎn)云配準(zhǔn)的誤差閾值為2 cm， 記錄配準(zhǔn)每幀點(diǎn)云的消耗時(shí)間． 點(diǎn)云精度試驗(yàn)時(shí)， 設(shè)定點(diǎn)云配準(zhǔn)的最大循環(huán)次數(shù)為1 000次， 點(diǎn)云配準(zhǔn)的誤差閾值為0.01 cm， 記錄配準(zhǔn)每幀點(diǎn)云的誤差．

由表2對(duì)3種算法點(diǎn)云配準(zhǔn)定位效率的對(duì)比分析可知， NDT-ICP算法的配準(zhǔn)效率介于NDT和ICP算法的配準(zhǔn)效率之間， 平均每幀耗時(shí)為0.53 s．

由圖14對(duì)點(diǎn)云配準(zhǔn)誤差對(duì)比分析可知， NDT-ICP算法的誤差也介于ICP算法和NDT算法的配準(zhǔn)誤差之間， 平均配準(zhǔn)誤差為4.72 cm． 由以上分析可知， NDT-ICP點(diǎn)云配準(zhǔn)算法兼具高效率和高準(zhǔn)確性， 可以滿(mǎn)足柑橘果園機(jī)器人的定位需求．

4.2.3 路徑跟蹤試驗(yàn)

為了驗(yàn)證航向角的控制策略及RLS自適應(yīng)參數(shù)PID轉(zhuǎn)向角增量控制的有效性， 機(jī)器人以試驗(yàn)區(qū)地圖坐標(biāo)系的原點(diǎn)為起點(diǎn)， 獲取預(yù)定的規(guī)劃路徑和階段性導(dǎo)航目標(biāo)點(diǎn)． 隨后機(jī)器人以0.5 m/s， 0.8 m/s， 1.2 m/s的行駛速度分別進(jìn)行5次試驗(yàn)， 分別測(cè)量轉(zhuǎn)彎目標(biāo)點(diǎn)與直線(xiàn)目標(biāo)點(diǎn)處的軌跡偏差， 以此對(duì)導(dǎo)航系統(tǒng)的性能進(jìn)行系統(tǒng)性的評(píng)估和驗(yàn)證． 試驗(yàn)結(jié)果如表3所示， 隨著速度的增大， 目標(biāo)點(diǎn)平均位置偏差和航向角偏差均增大． 行駛速度為1.2 m/s時(shí)， 直線(xiàn)行駛平均位置偏差為0.18 m， 最大位置偏差為0.23 m， 平均航向角偏差為4.2°， 曲線(xiàn)轉(zhuǎn)彎處平均位置偏差為0.38 m， 最大位置偏差為0.57 m， 平均航向角偏差為16.7°， 果園平均行間距約為4 m， 所以在1.2 m/s的速度下仍能滿(mǎn)足自主導(dǎo)航作業(yè)要求．

5 結(jié)論

1） 利用激光雷達(dá)和IMU傳感器實(shí)現(xiàn)了果園自主導(dǎo)航系統(tǒng)的構(gòu)建， 并采用了ESKF來(lái)進(jìn)一步優(yōu)化建圖過(guò)程中機(jī)器人的位姿． 對(duì)點(diǎn)云進(jìn)行處理， 利用NDT-ICP算法實(shí)現(xiàn)了機(jī)器人基于先驗(yàn)全局點(diǎn)云地圖的定位． 試驗(yàn)結(jié)果表明， 通過(guò)該優(yōu)化策略， 機(jī)器人對(duì)自身姿態(tài)感知的平均位置和角度偏差分別為6.43 cm和2.53°， 說(shuō)明該優(yōu)化算法可以提供較為精準(zhǔn)的自身位姿信息和周?chē)h(huán)境地圖， 為后續(xù)導(dǎo)航提供支持．

2） 在地圖構(gòu)建與自身定位算法的基礎(chǔ)上， 設(shè)計(jì)了基于預(yù)瞄點(diǎn)的航向角控制系統(tǒng)， 為了應(yīng)對(duì)果園地形的變化， 采用了實(shí)時(shí)調(diào)整增量式PID參數(shù)的路徑跟蹤控制策略． 為了驗(yàn)證其效果， 在履帶式底盤(pán)上進(jìn)行了不同速度下的控制試驗(yàn)． 在速度最高時(shí)控制系統(tǒng)的平均位置偏差和角度偏差均可滿(mǎn)足要求， 說(shuō)明該控制策略在3種不同的行駛速度下均能實(shí)現(xiàn)對(duì)預(yù)定路徑的跟蹤， 滿(mǎn)足果園機(jī)械自主導(dǎo)航作業(yè)需求．

丘陵山地柑橘果園機(jī)器人導(dǎo)航控制系統(tǒng)實(shí)現(xiàn)了果園環(huán)境地圖構(gòu)建、 全局路徑規(guī)劃和路徑跟蹤， 為柑橘果園作業(yè)機(jī)械的自主導(dǎo)航提供了技術(shù)支持． 下一步將開(kāi)展自主作業(yè)過(guò)程中面對(duì)不同障礙物的實(shí)時(shí)避障與繞行問(wèn)題研究， 從而進(jìn)一步拓寬果園機(jī)器人的使用場(chǎng)景．

參考文獻(xiàn)：

［1］

楊蕾， 李勛蘭， 楊海健， 等． 重慶市柑橘產(chǎn)業(yè)發(fā)展成就、 存在問(wèn)題與建議 ［J］． 南方園藝， 2020， 31（3）： 32-35．

［2］ 程良威． 山地果園機(jī)械化開(kāi)發(fā)要點(diǎn)及利弊分析 ［J］． 時(shí)代農(nóng)機(jī)， 2019， 46（2）： 24-25．

［3］ 劉成良， 貢亮， 苑進(jìn)， 等． 農(nóng)業(yè)機(jī)器人關(guān)鍵技術(shù)研究現(xiàn)狀與發(fā)展趨勢(shì) ［J］． 農(nóng)業(yè)機(jī)械學(xué)報(bào)， 2022， 53（7）： 1-22， 55．

［4］ 趙春江． 智慧農(nóng)業(yè)的發(fā)展現(xiàn)狀與未來(lái)展望 ［J］． 華南農(nóng)業(yè)大學(xué)學(xué)報(bào)， 2021， 42（6）： 1-7．

［5］ 魏爽， 李世超， 張漫， 等． 基于GNSS的農(nóng)機(jī)自動(dòng)導(dǎo)航路徑搜索及轉(zhuǎn)向控制 ［J］． 農(nóng)業(yè)工程學(xué)報(bào)， 2017， 33（S1）： 70-77．

［6］ 賴(lài)漢榮， 張亞偉， 張賓， 等． 玉米除草機(jī)器人視覺(jué)導(dǎo)航系統(tǒng)設(shè)計(jì)與試驗(yàn) ［J］． 農(nóng)業(yè)工程學(xué)報(bào)， 2023， 39（1）： 18-27．

［7］ 張振乾， 李世超， 李晨陽(yáng)， 等． 基于雙目視覺(jué)的香蕉園巡檢機(jī)器人導(dǎo)航路徑提取方法 ［J］． 農(nóng)業(yè)工程學(xué)報(bào)， 2021， 37（21）： 9-15．

［8］ BALL D， ROSS P， ENGLISH A， et al． Farm Workers of the Future： Vision-Based Robotics for Broad-Acre Agriculture ［J］． IEEE Robotics amp; Automation Magazine， 2017， 24（3）： 97-107．

［9］ 敖方源， 文澤富． 丘陵地區(qū)機(jī)械化柑橘果園建設(shè)實(shí)踐與分析 ［J］． 農(nóng)機(jī)市場(chǎng)， 2018（9）： 30-32．

［10］李梟凱， 李廣云， 索世恒， 等． 激光SLAM技術(shù)進(jìn)展 ［J］． 導(dǎo)航定位學(xué)報(bào)， 2023， 11（4）： 8-17．

［11］馮涵， 張浩， 王梓， 等． 基于激光點(diǎn)云的三維虛擬果園構(gòu)建方法 ［J］． 智慧農(nóng)業(yè)（中英文）， 2022， 4（3）： 12-23．

［12］華智， 宋吉來(lái)， 杜振軍， 等． 基于濾波與圖優(yōu)化的定位與建圖系統(tǒng) ［J］． 電子測(cè)量技術(shù)， 2022， 45（4）： 99-106．

［13］崔文， 薛棋文， 李慶玲， 等． 基于三維點(diǎn)云地圖和ESKF的無(wú)人車(chē)融合定位方法 ［J］． 工礦自動(dòng)化， 2022， 48（9）： 116-122．

［14］楊必勝， 梁福遜， 黃榮剛． 三維激光掃描點(diǎn)云數(shù)據(jù)處理研究進(jìn)展、 挑戰(zhàn)與趨勢(shì) ［J］． 測(cè)繪學(xué)報(bào)， 2017， 46（10）： 1509-1516．

［15］盧凌雯， 梁棟棟． 點(diǎn)云數(shù)據(jù)多種濾波方式組合優(yōu)化研究 ［J］． 安徽師范大學(xué)學(xué)報(bào)（自然科學(xué)版）， 2018， 41（1）： 50-54．

［16］劉洋， 冀杰， 趙立軍， 等． 基于激光雷達(dá)與相機(jī)融合的樹(shù)干檢測(cè)方法 ［J］． 西南大學(xué)學(xué)報(bào)（自然科學(xué)版）， 2024， 46（2）： 183-196．

［17］沈躍， 劉子涵， 劉慧， 等． 基于多約束條件的果園噴霧機(jī)器人路徑規(guī)劃方法 ［J］． 農(nóng)業(yè)機(jī)械學(xué)報(bào)， 2023， 54（7）： 56-67．

［18］趙輝， 郝夢(mèng)雅， 王紅君， 等． 基于改進(jìn)A*算法與天牛須搜索算法的農(nóng)業(yè)機(jī)器人路徑規(guī)劃方法 ［J］． 科學(xué)技術(shù)與工程， 2019， 19（31）： 185-190．

［19］王瑜． 基于激光SLAM的無(wú)人車(chē)定位與建圖方法研究 ［D］． 天津： 中國(guó)民航大學(xué)， 2021．

［20］SHARP G C， LEE S W， WEHE D K． ICP Registration Using Invariant Features ［J］． IEEE Transactions on Pattern Analysis and Machine Intelligence， 2002， 24（1）： 90-102．

［21］張曉， 張愛(ài)武， 王致華． 基于改進(jìn)正態(tài)分布變換算法的點(diǎn)云配準(zhǔn) ［J］． 激光與光電子學(xué)進(jìn)展， 2014， 51（4）： 100-109．

［22］何杰， 滿(mǎn)忠賢， 胡煉， 等． 履帶式花生聯(lián)合收獲機(jī)路徑跟蹤控制方法與試驗(yàn) ［J］． 農(nóng)業(yè)工程學(xué)報(bào)， 2023， 39（1）： 9-17．

［23］張哲， 吳劍， 何誠(chéng)， 等． 基于卡爾曼濾波和增量式PID控制的智能車(chē)載系統(tǒng) ［J］． 現(xiàn)代信息科技， 2019， 3（10）： 29-32， 35．

［24］朱晨輝， 王淼森， 陳博， 等． 丘陵區(qū)履帶式煙葉采收機(jī)液壓行駛系統(tǒng)的創(chuàng)新設(shè)計(jì)與試驗(yàn) ［J］． 河南農(nóng)業(yè)大學(xué)學(xué)報(bào)， 2022， 56（4）： 532-542．

［25］汪平平， 張歆， 劉深． 基于線(xiàn)性最小方差和遞歸最小二乘的融合算法 ［J］． 探測(cè)與控制學(xué)報(bào)， 2013， 35（2）： 33-36．

［26］閆秀英， 任慶昌， 孟慶龍． 一種自校正PID控制器設(shè)計(jì)與仿真研究 ［J］． 系統(tǒng)仿真學(xué)報(bào)， 2006， 18（S2）： 753-756．

［27］魏丹丹， 周翊， 趙宇． 基于最大似然估計(jì)的自適應(yīng)回聲消除算法 ［J］． 西南大學(xué)學(xué)報(bào)（自然科學(xué)版）， 2023， 45（4）： 210-218．

［28］宋懷波， 段援朝， 李嶸， 等． 基于激光SLAM的牛場(chǎng)智能推翻草機(jī)器人自主導(dǎo)航系統(tǒng) ［J］． 農(nóng)業(yè)機(jī)械學(xué)報(bào)， 2023， 54（2）： 293-301．

責(zé)任編輯 柳劍