摘要:針對(duì)番茄溫室中的長(zhǎng)廊環(huán)境及植被分布問(wèn)題,對(duì)番茄機(jī)器人的多傳感器融合的SLAM和導(dǎo)航算法進(jìn)行優(yōu)化?;贑artographer算法,進(jìn)行激光雷達(dá)數(shù)據(jù)去畸變處理,再通過(guò)無(wú)跡卡爾曼濾波(UKF)融合激光雷達(dá)、里程計(jì)及IMU信息進(jìn)行機(jī)器人位姿估計(jì)優(yōu)化,并使用優(yōu)化后的算法對(duì)溫室進(jìn)行高精度地圖構(gòu)建。將建立的高精度柵格地圖使用改進(jìn)的A*與DWA融合算法進(jìn)行驗(yàn)證,通過(guò)動(dòng)態(tài)調(diào)整權(quán)重和優(yōu)化搜索邏輯,提高A*算法的搜索效率和路徑安全性,使得機(jī)器人在溫室環(huán)境下能夠更加智能地尋找最優(yōu)且安全的路徑,在ROS的gazebo平臺(tái)和實(shí)地的溫室環(huán)境中驗(yàn)證算法的有效性。結(jié)果表明,在機(jī)器人運(yùn)行速度≤0.6 m/s時(shí),平均位置偏差為10.3 cm,滿(mǎn)足番茄溫室的作業(yè)要求。
關(guān)鍵詞:番茄;溫室機(jī)器人;多傳感器融合;激光SLAM;路徑規(guī)劃;導(dǎo)航
中圖分類(lèi)號(hào):S625; TP24" " " 文獻(xiàn)標(biāo)識(shí)碼:A" " " 文章編號(hào):2095?5553 (2025) 04?0171?09
Multi?sensor fusion mapping and navigation research for tomato greenhouse robot
Fu Honglong Hu Yubing Xie Limin Cai Yun Fang Bing
(1. College of Mechanical and Electrical Engineering, Fujian Agriculture and Forestry University, Fuzhou, 350002, China; 2. Fujian Key Laboratory of Agricultural Information Sensing Technology, Fuzhou, 350002, China)
Abstract: According to the corridor environment and vegetation distribution in tomato greenhouse, SLAM and navigation algorithm of robot multi?sensor fusion are optimized. Firstly, based on the Cartographer algorithm, the dedistortion processing of LiDAR data was carried out, and then the unscented Kalman filter (UKF) was used to fuse the information of LiDAR, odometer and IMU to optimize the robot pose estimation, and the optimized algorithm was used to construct a high?precision map of the greenhouse. The established high?precision raster map is verified by the improved A* and DWA fusion algorithm, and the search efficiency and path safety of the A* algorithm are improved by dynamically adjusting the weights and optimizing the search logic, so that the robot can find the optimal and safe path more intelligently in the greenhouse environment, and the effectiveness of the algorithm is verified in the gazebo platform of ROS and the greenhouse environment in the field. The experimental results indicated that the average position deviation was 10.3 cm when the running speed of the robot was not more than 0.6 m/s, which met the operation requirements of the tomato greenhouse.
Keywords: tomato; greenhouse robot; multi?sensor fusion; laser SLAM; path planning; navigation
0 引言
設(shè)施農(nóng)業(yè)發(fā)展是農(nóng)業(yè)現(xiàn)代化的重要標(biāo)志和建設(shè)任務(wù)。溫室機(jī)器人作為設(shè)施農(nóng)業(yè)種植與生產(chǎn)過(guò)程中提高生產(chǎn)效率、保障農(nóng)作物品質(zhì)的重要措施,已成為當(dāng)前研究的熱點(diǎn)之一[1]。截至2023年末,我國(guó)溫室大棚面積達(dá)到1 855.3 khm2,且保持穩(wěn)定的發(fā)展趨勢(shì)[2]。將人工智能、互聯(lián)網(wǎng)、區(qū)塊鏈和機(jī)器人等技術(shù)與農(nóng)業(yè)相結(jié)合,可以輔助或代替人工操作,大幅提高農(nóng)業(yè)資源利用率和生產(chǎn)效率[3, 4]。
SLAM技術(shù)是通過(guò)傳感器采集信息,實(shí)現(xiàn)機(jī)器人對(duì)自身運(yùn)動(dòng)狀態(tài)估計(jì)并對(duì)周?chē)h(huán)境進(jìn)行地圖構(gòu)建的過(guò)程。在溫室長(zhǎng)廊環(huán)境下,基于濾波的Gmapping算法在傳感器觀測(cè)時(shí)產(chǎn)生的誤差會(huì)累計(jì)并且無(wú)法進(jìn)行回環(huán)檢測(cè),基于圖優(yōu)化的Cartographer算法在傳感器精度不足時(shí),機(jī)器人位姿估計(jì)陷入局部極小,定位產(chǎn)生漂移,將無(wú)法獲得全局一致的運(yùn)動(dòng)軌跡估計(jì)與地圖構(gòu)建,從而難以滿(mǎn)足導(dǎo)航、環(huán)境重建等應(yīng)用需要。針對(duì)Cartographer算法中存在的點(diǎn)云特征丟失和低幀率激光雷達(dá)導(dǎo)致的運(yùn)動(dòng)畸變問(wèn)題,徐淑萍等[5]利用AUKF優(yōu)化傳感器數(shù)據(jù)改進(jìn)了Cartographer算法,二次篩選體素濾波和點(diǎn)云,提高建圖精度,但算法耗時(shí)較長(zhǎng)。黃禹翔等[6]采用k鄰域搜索鄰近點(diǎn)的體素濾波方法提高算法運(yùn)算速率,并通過(guò)嵌入輪式里程計(jì)輔助模塊去除運(yùn)動(dòng)畸變,但未解決算法在復(fù)雜場(chǎng)景下位姿估計(jì)精度不足的問(wèn)題。Dwijotomo等[7]通過(guò)設(shè)定不同的數(shù)據(jù)采集速度,從而優(yōu)化激光雷達(dá)在不同幀之間的采集效果,以達(dá)到消除位姿擾動(dòng)的目的,但沒(méi)考慮運(yùn)動(dòng)軌跡估計(jì)。章弘凱[8]、賈浩[9]等均對(duì)Cartographer算法進(jìn)行了回環(huán)檢測(cè)的優(yōu)化,提高算法后端優(yōu)化的效率,但未對(duì)初始的傳感器進(jìn)行優(yōu)化。這些研究雖然都從不同方面提高了建圖精度,但未在實(shí)際應(yīng)用場(chǎng)景中驗(yàn)證地圖的有效性。通過(guò)路徑規(guī)劃算法對(duì)建立的地圖進(jìn)行驗(yàn)證,以實(shí)現(xiàn)機(jī)器人在已知環(huán)境下的自主移動(dòng)。龐永旭等[10]融合傳統(tǒng)A*與DWA實(shí)現(xiàn)動(dòng)態(tài)實(shí)時(shí)避障,但未在真實(shí)環(huán)境下測(cè)試。王新彥等[11]提出一種遍歷路徑規(guī)劃算法,但在障礙物較多的情況下效率較低。Mahmud等[12]根據(jù)真實(shí)溫室環(huán)境構(gòu)建虛擬環(huán)境用于導(dǎo)航測(cè)試,通過(guò)多目標(biāo)的路徑規(guī)劃算法降低機(jī)器人運(yùn)動(dòng)成本,但該算法容易陷入局部最小值導(dǎo)致無(wú)法向下一個(gè)目標(biāo)移動(dòng)。
綜上所述,本文對(duì)位姿圖優(yōu)化的Cartographer算法進(jìn)行多傳感器融合優(yōu)化,并通過(guò)路徑規(guī)劃算法在建立的地圖中進(jìn)行驗(yàn)證試驗(yàn)。通過(guò)里程計(jì)對(duì)激光雷達(dá)進(jìn)行去畸變?cè)囼?yàn),將優(yōu)化后的雷達(dá)數(shù)據(jù)通過(guò)UKF融合多傳感器信息進(jìn)行位姿估計(jì),并在仿真平臺(tái)進(jìn)行位姿估計(jì)和建圖效果的對(duì)比。在真實(shí)的溫室環(huán)境中,采取優(yōu)化的建圖和路徑規(guī)劃算法驗(yàn)證不同速度下溫室機(jī)器人巡檢功能的有效性。
1 激光雷達(dá)數(shù)據(jù)預(yù)處理
1.1 激光雷達(dá)觀測(cè)模型
目前,激光測(cè)距原理有三角測(cè)距法和飛行時(shí)間法。由于溫室環(huán)境影響,本研究采用受光線(xiàn)反射影響更小且短距離內(nèi)精度更高的三角測(cè)距法。其主要原理是激光雷達(dá)內(nèi)部的激光器發(fā)射激光束,光束照射到障礙物后被反射,隨后光線(xiàn)被攝像機(jī)收集成像在平面上形成點(diǎn)p。通過(guò)幾何關(guān)系,點(diǎn)p與成像平面交于點(diǎn)p',如圖1所示。
根據(jù)相似原理,可求得障礙物到激光發(fā)射器與攝像機(jī)的法向距離q及激光雷達(dá)與障礙物間的距離d,如式(1)所示。
似然場(chǎng)模型在計(jì)算激光雷達(dá)測(cè)量點(diǎn)的權(quán)重時(shí),充分考慮激光束周?chē)沫h(huán)境信息并將其轉(zhuǎn)化為概率分布函數(shù),更好地反映周?chē)h(huán)境的真實(shí)情況。準(zhǔn)確的激光雷達(dá)觀測(cè)數(shù)據(jù)為機(jī)器人提供更為準(zhǔn)確的定位精度,進(jìn)而優(yōu)化后續(xù)SLAM過(guò)程中位姿估計(jì)和地圖更新。
1.2 激光雷達(dá)去畸變
運(yùn)動(dòng)畸變指由于激光雷達(dá)在獲取一幀掃描數(shù)據(jù)時(shí),機(jī)器人本身存在運(yùn)動(dòng),導(dǎo)致掃描數(shù)據(jù)中不同時(shí)刻獲取的激光束測(cè)量位置之間存在偏差,從而使得掃描結(jié)果在空間中產(chǎn)生誤差和偏移[13, 14]。
通過(guò)輪式里程計(jì)輔助激光雷達(dá)進(jìn)行運(yùn)動(dòng)畸變校正,輪式里程計(jì)是一種用于機(jī)器人定位和導(dǎo)航的傳感器,通過(guò)計(jì)算每個(gè)輪子的旋轉(zhuǎn)量和機(jī)器人運(yùn)動(dòng)學(xué)模型,推導(dǎo)出機(jī)器人在空間中的位姿。輪式里程計(jì)具有較高的局部位姿精度和更新頻率(一般為100~200 Hz),且跟狀態(tài)估計(jì)完全解耦。為了能將IMU信息與準(zhǔn)確的激光雷達(dá)和里程計(jì)信息進(jìn)行融合,對(duì)激光雷達(dá)進(jìn)行去畸變?cè)囼?yàn)。
已知當(dāng)前幀激光初始時(shí)間[ts]和結(jié)束時(shí)間[te],時(shí)間差為[Δt],截取時(shí)間戳小于[ts]且大于[te]的里程計(jì)數(shù)據(jù)進(jìn)行校正,步驟如下。
2 溫室機(jī)器人多傳感器融合算法
2.1 無(wú)跡卡爾曼濾波融合算法
無(wú)跡卡爾曼濾波(UKF)是一種基于貝葉斯推斷的遞歸濾波器,可用于對(duì)非線(xiàn)性系統(tǒng)進(jìn)行狀態(tài)估計(jì)。UKF結(jié)合無(wú)跡變換(UT)和標(biāo)準(zhǔn)卡爾曼濾波(KF),利用UT實(shí)現(xiàn)對(duì)非線(xiàn)性系統(tǒng)的線(xiàn)性化。通過(guò)采用無(wú)跡卡爾曼濾波,可將機(jī)器人采集的數(shù)據(jù)進(jìn)行融合,以過(guò)濾傳感器誤差較大的信息。其主要步驟包括狀態(tài)預(yù)測(cè)、觀測(cè)預(yù)測(cè)、狀態(tài)更新和協(xié)方差更新4個(gè)部分[11]。
在機(jī)器人位姿估計(jì)應(yīng)用中,假設(shè)機(jī)器人的狀態(tài)為xk,運(yùn)動(dòng)模型如式(15)所示。
UKF避免了對(duì)非線(xiàn)性函數(shù)進(jìn)行泰勒展開(kāi)的步驟,而是通過(guò)使用一組特殊選擇的點(diǎn)(稱(chēng)為sigma點(diǎn))來(lái)近似隨機(jī)變量的統(tǒng)計(jì)特性,從而更有效地處理非線(xiàn)性系統(tǒng)[15]。
2.2 基于無(wú)跡卡爾曼濾波融合位姿估計(jì)的Cartographer算法
在圖優(yōu)化SLAM中,SLAM問(wèn)題被抽象為一個(gè)圖,其中圖的節(jié)點(diǎn)表示機(jī)器人不同時(shí)刻的位姿,邊表示這些位姿之間的約束關(guān)系[16]。圖優(yōu)化SLAM由前端匹配和后端優(yōu)化兩個(gè)部分構(gòu)成,前端部分處理傳感器數(shù)據(jù)生成圖的頂點(diǎn)和邊,后端則根據(jù)前端提供的信息進(jìn)行機(jī)器人的位姿優(yōu)化,以盡量滿(mǎn)足圖中的約束并構(gòu)建全局地圖。
為解決Cartographer中使用單一里程計(jì)進(jìn)行位姿估計(jì)不準(zhǔn)確的問(wèn)題,采用多傳感器融合位姿估計(jì)的方法,除進(jìn)行里程計(jì)標(biāo)定和激光雷達(dá)運(yùn)動(dòng)畸變校正之外,利用IMU對(duì)角度感知的敏銳性,將IMU傳感器的信息添加到激光里程計(jì)模型中,以修正輪式里程計(jì)的角度信息。通過(guò)使用無(wú)跡卡爾曼濾波算法實(shí)現(xiàn)多傳感器數(shù)據(jù)的融合,以提高SLAM算法的性能。
位姿融合過(guò)程中,使用輪式里程計(jì)信息、經(jīng)過(guò)加速度計(jì)和陀螺儀進(jìn)行互補(bǔ)濾波后的IMU信息以及激光掃描匹配的位姿信息作為輸入,通過(guò)無(wú)跡卡爾曼濾波的迭代方式進(jìn)行狀態(tài)預(yù)測(cè),得到機(jī)器人的當(dāng)前位姿估計(jì)。通過(guò)激光掃描匹配器修正系統(tǒng)狀態(tài)變量和協(xié)方差,進(jìn)一步提高位姿估計(jì)的準(zhǔn)確性。由于里程計(jì)和IMU的采樣頻率比激光雷達(dá)高,利用它們進(jìn)行位姿估計(jì)時(shí),存在某些時(shí)間段內(nèi)沒(méi)有激光掃描數(shù)據(jù)進(jìn)行匹配估計(jì)的情況,并且長(zhǎng)時(shí)間的誤差累積會(huì)對(duì)里程計(jì)和IMU的位姿估計(jì)產(chǎn)生較大的影響。因此,當(dāng)激光掃描匹配器進(jìn)行估計(jì)時(shí),通過(guò)鄰近激光匹配的位姿進(jìn)行位姿更新,以消除累積誤差的影響[17]。
1) 通過(guò)激光掃描匹配器對(duì)激光數(shù)據(jù)進(jìn)行處理,估計(jì)機(jī)器人的位姿,同時(shí)計(jì)算出機(jī)器人的線(xiàn)速度和角速度。這些估計(jì)值作為狀態(tài)預(yù)測(cè)的初始值。
2) 根據(jù)IMU獲取的角速度進(jìn)行積分,得到機(jī)器人轉(zhuǎn)動(dòng)的角度,估計(jì)旋轉(zhuǎn),如式(25)所示。
3 溫室機(jī)器人仿真分析
3.1 機(jī)器人位姿估計(jì)精度測(cè)驗(yàn)
為驗(yàn)證基于拓展卡爾曼濾波(EKF)和無(wú)跡卡爾曼濾波(UKF)算法的機(jī)器人位姿估計(jì)的效果,采用MATLAB進(jìn)行仿真并對(duì)比,假設(shè)機(jī)器人在做勻速圓周運(yùn)動(dòng),采樣時(shí)間dt=80 s,仿真結(jié)果如圖4所示。
由圖4可知,基于無(wú)跡卡爾曼濾波(UKF)的融合算法得到的估計(jì)軌跡比基于EKF得到的估計(jì)軌跡更加接近真實(shí)情況。
拓展卡爾曼濾波(EKF)與無(wú)跡卡爾曼濾波(UKF)算法的具體誤差對(duì)比如圖5所示。可以看出,在X和Y方向上,無(wú)跡卡爾曼濾波(UKF)算法的誤差均低于拓展卡爾曼濾波(EKF)算法。經(jīng)計(jì)算,EKF的平均誤差為1.968 7 m,計(jì)算時(shí)間為0.007 8 s,而UKF的平均誤差為1.351 5 m,計(jì)算時(shí)間為0.057 8 s。通過(guò)上述比較,基于無(wú)跡卡爾曼濾波(UKF)的融合算法在機(jī)器人位姿估計(jì)上優(yōu)于拓展卡爾曼濾波(EKF)的融合算法。
3.2 改進(jìn)的SLAM算法仿真建圖試驗(yàn)
通過(guò)gazebo搭建一個(gè)長(zhǎng)8 m,寬8 m的壟道溫室進(jìn)行仿真試驗(yàn),設(shè)置4個(gè)作物行壟,每個(gè)作物行壟由10株種植在花盆中的綠植組成,每個(gè)花盆直徑為20 cm,每個(gè)花盆間距10 cm,壟道寬度為80 cm。搭建的溫室環(huán)境如圖6所示??刂拼罱ǖ囊苿?dòng)機(jī)器人以0.2 m/s的速度運(yùn)動(dòng),分別運(yùn)用原Cartographer和基于無(wú)跡卡爾曼濾波融合位姿估計(jì)的Cartographer算法進(jìn)行建圖,如圖7所示。
仿真溫室環(huán)境建圖算法對(duì)比圖7表明,通過(guò)UKF添加IMU信息后的Cartographer建圖效果比原算法建圖效果更好,說(shuō)明通過(guò)UKF將數(shù)據(jù)融合到一起,可以準(zhǔn)確地描述機(jī)器人在環(huán)境中的運(yùn)動(dòng)狀態(tài),保證SLAM過(guò)程中能夠準(zhǔn)確進(jìn)行掃描匹配得到更加完整的地圖,驗(yàn)證加入IMU信息的Cartographer進(jìn)行多傳感器融合構(gòu)建溫室環(huán)境地圖的可行性。為客觀對(duì)比原算法與改進(jìn)后算法的建圖精度,將圖測(cè)值與真實(shí)值對(duì)比,計(jì)算誤差,如表1所示。
4 實(shí)際溫室建圖及導(dǎo)航試驗(yàn)
4.1 溫室真實(shí)環(huán)境地圖構(gòu)建
為驗(yàn)證多傳感器融合的建圖算法和改進(jìn)的A*與DWA的融合算法可行性,在番茄溫室大棚環(huán)境下進(jìn)行建圖和導(dǎo)航的實(shí)地測(cè)驗(yàn)。
在該溫室中,每個(gè)番茄的株距為0.1 m,壟道長(zhǎng)度約為22 m,可行走的壟道間距為1 m,控制機(jī)器人以0.2 m/s的速度沿壟道中心線(xiàn)行駛,按照上述方法進(jìn)行溫室壟道的雙行建圖,試驗(yàn)結(jié)果如圖8所示。可以看出,當(dāng)位姿估計(jì)不準(zhǔn)確時(shí),構(gòu)建的地圖會(huì)出現(xiàn)整體偏移,且有局部建圖不完整的情況。通過(guò)UKF融合多傳感器進(jìn)行位姿估計(jì)能減小由于溫室雜葉、路面不平整等情況導(dǎo)致車(chē)輪打滑等現(xiàn)象帶來(lái)的位姿估計(jì)偏差,并較為準(zhǔn)確地構(gòu)建出溫室地圖。經(jīng)圖測(cè)值與真實(shí)值對(duì)比,改進(jìn)后的Cartographer算法橫向誤差約為0.2 m,滿(mǎn)足溫室機(jī)器人導(dǎo)航需求。
在試驗(yàn)過(guò)程中,通過(guò)ROSBag記錄傳感器數(shù)據(jù)和算法,使用MATLAB對(duì)采集的數(shù)據(jù)進(jìn)行可視化。將由單一里程計(jì)得到的位姿估計(jì)與通過(guò)UKF融合的機(jī)器人位姿估計(jì)進(jìn)行對(duì)比,并添加理想的溫室壟道中心線(xiàn)作為準(zhǔn)確位姿的參考,如圖9所示。單一里程計(jì)在位姿估計(jì)上與實(shí)際行走軌跡偏差較大,特別是當(dāng)路面不平整,機(jī)器人偏離原軌道,通過(guò)旋轉(zhuǎn)回到原定路線(xiàn)時(shí),由于里程計(jì)旋轉(zhuǎn)估計(jì)不準(zhǔn)確導(dǎo)致較大的位姿偏差。通過(guò)UKF融合的位姿估計(jì),依賴(lài)IMU信息提供的旋轉(zhuǎn)估計(jì)得到更準(zhǔn)確的位姿估計(jì)。
圖10分別提供里程計(jì)和UKF融合的機(jī)器人位姿估計(jì)在X軸和Y軸的對(duì)比,可以看出,單一里程計(jì)與UKF得到的位姿估計(jì)與在X軸上不會(huì)產(chǎn)生較大變化,但是Y軸有明顯差別。里程計(jì)估計(jì)的位姿在兩個(gè)壟道處Y軸與壟道中心最大偏移分別為0.328 m、0.680 m;UKF優(yōu)化后最大偏移縮小為0.131 m、0.541 m。
4.2 溫室機(jī)器人自主導(dǎo)航試驗(yàn)
采用A*與動(dòng)態(tài)窗口法(DWA)的融合算法進(jìn)行溫室實(shí)地導(dǎo)航測(cè)驗(yàn),為提高溫室機(jī)器人路徑規(guī)劃搜索速率對(duì)A*算法從起始點(diǎn)到目標(biāo)點(diǎn)的評(píng)價(jià)函數(shù)[F(n)]進(jìn)行優(yōu)化,如式(27)所示。
在原A*算法中實(shí)際代價(jià)和估計(jì)代價(jià)的權(quán)重分配是1∶1,當(dāng)權(quán)重系數(shù)[W(n)]較大的時(shí)候,搜索路徑會(huì)快速向目標(biāo)節(jié)點(diǎn)搜索,導(dǎo)致錯(cuò)過(guò)最優(yōu)路徑。采用動(dòng)態(tài)加權(quán)的方式可以避免因追求搜索速度而導(dǎo)致規(guī)劃的路徑不是最優(yōu)的情況。考慮使用動(dòng)態(tài)加權(quán)的方式,將原本的啟發(fā)函數(shù)H(n)為判斷依據(jù),引入距離判斷D(n),如式(28)所示。
基于改進(jìn)的A*算法規(guī)劃的全局路徑的基礎(chǔ)上,選出關(guān)鍵參考點(diǎn),使用DWA進(jìn)行分段局部路徑規(guī)劃。計(jì)算軌跡終點(diǎn)與改進(jìn)的A*算法的關(guān)鍵點(diǎn)的距離,距離越短,越靠近全局路徑。將式(26)作為新的評(píng)價(jià)函數(shù),如式(30)所示。
DWA算法是實(shí)現(xiàn)局部避障的算法,但只把目標(biāo)終點(diǎn)作為參考項(xiàng),容易陷入局部最優(yōu)。將改進(jìn)A*和DWA融合來(lái)解決上述兩個(gè)問(wèn)題,不僅可以確保規(guī)劃的路徑是最優(yōu)的,而且可以擁有避開(kāi)未知障礙物的能力[19]。兩種算法融合的具體流程如圖11所示。
溫室移動(dòng)機(jī)器人以0.2 m/s、0.4 m/s和0.6 m/s作業(yè)速度在真實(shí)的溫室環(huán)境中進(jìn)行導(dǎo)航試驗(yàn),各速度下重復(fù)進(jìn)行2次試驗(yàn),測(cè)量目標(biāo)導(dǎo)航點(diǎn)與溫室移動(dòng)機(jī)器人平臺(tái)的質(zhì)心標(biāo)定點(diǎn)間的導(dǎo)航偏差,取2次試驗(yàn)數(shù)據(jù)平均值作為該速度下的導(dǎo)航偏差。溫室導(dǎo)航精度測(cè)試試驗(yàn)結(jié)果如表2所示。
由表2可知,溫室移動(dòng)機(jī)器人在導(dǎo)航過(guò)程中,隨著速度的增加,其導(dǎo)航精度有所下降。機(jī)器人可以完整地進(jìn)行壟內(nèi)直行,轉(zhuǎn)彎以及避障操作。機(jī)器人在地圖構(gòu)建的運(yùn)動(dòng)范圍內(nèi),運(yùn)行速度≤0.6 m/s時(shí),實(shí)際運(yùn)行軌跡與所規(guī)劃路徑平均位置偏差為10.3 cm,平均相對(duì)誤差為0.46%。實(shí)際溫室環(huán)境與試驗(yàn)室搭建環(huán)境導(dǎo)航結(jié)果相比,位置偏差和航向偏差均明顯增大,主要原因是當(dāng)周?chē)h(huán)境相似時(shí),激光雷達(dá)觀測(cè)出現(xiàn)誤差,導(dǎo)致定位不準(zhǔn)確,其次是機(jī)器人在溫室路面行走時(shí)出現(xiàn)顛簸導(dǎo)致位置偏移。移動(dòng)機(jī)器人平臺(tái)以0.2 m/s的速度移動(dòng),導(dǎo)航精度較高,可以滿(mǎn)足在該溫室環(huán)境下的導(dǎo)航要求。
5 結(jié)論
1) 針對(duì)傳感器精度不足,機(jī)器人運(yùn)動(dòng)位姿估計(jì)錯(cuò)誤情況下難以完成溫室地圖構(gòu)建及導(dǎo)航任務(wù),使用輪式里程計(jì)輔助進(jìn)行激光雷達(dá)運(yùn)動(dòng)畸變校正,機(jī)器人在溫室壟道內(nèi)進(jìn)行運(yùn)動(dòng)過(guò)程中,激光雷達(dá)數(shù)據(jù)不會(huì)發(fā)生跳變。
2) 針對(duì)傳統(tǒng)粒子濾波的建圖方法對(duì)溫室長(zhǎng)廊環(huán)境會(huì)產(chǎn)生累計(jì)誤差,采用優(yōu)化多傳感器融合的Cartographer算法進(jìn)行地圖構(gòu)建試驗(yàn),并在搭建的仿真溫室場(chǎng)景下進(jìn)行導(dǎo)航建圖試驗(yàn)。結(jié)果表明:通過(guò)UKF加入IMU信息后的多傳感器融合建圖在定位精度和建圖效果上優(yōu)于EKF。
3) 在實(shí)際的溫室場(chǎng)景下進(jìn)行SLAM及導(dǎo)航試驗(yàn),在實(shí)際場(chǎng)景試驗(yàn)下,通過(guò)UKF優(yōu)化的多傳感器位姿估計(jì)優(yōu)于單一里程計(jì),優(yōu)化后的建圖算法構(gòu)造的地圖更為準(zhǔn)確。采用改進(jìn)的A*與DWA融合算法對(duì)構(gòu)建的柵格地圖進(jìn)行導(dǎo)航試驗(yàn)時(shí),在機(jī)器人行進(jìn)速度≤0.6 m/s時(shí),平均位置偏差為10.3 cm,驗(yàn)證多傳感器融合建圖導(dǎo)航方案的可行性。
參 考 文 獻(xiàn)
[ 1 ] 邢希君, 宋建成, 吝伶艷, 等. 設(shè)施農(nóng)業(yè)溫室大棚智能控制技術(shù)的現(xiàn)狀與展望[J]. 江蘇農(nóng)業(yè)科學(xué), 2017, 45(21): 10-15.
[ 2 ] 趙春江. 智慧農(nóng)業(yè)的發(fā)展現(xiàn)狀與未來(lái)展望[J]. 華南農(nóng)業(yè)大學(xué)學(xué)報(bào), 2021, 42(6): 1-7.
Zhao Chunjiang. Current situations and prospects of smart agriculture [J]. Journal of South China Agricultural University, 2021, 42(6): 1-7.
[ 3 ] 閆全濤, 李麗霞, 邱權(quán), 等. 小型移動(dòng)式農(nóng)業(yè)機(jī)器人研究現(xiàn)狀及發(fā)展趨勢(shì)[J]. 中國(guó)農(nóng)機(jī)化學(xué)報(bào), 2019, 40(5): 178-186.
Yan Quantao, Li Lixia, Qiu Quan, et al. Research status and development trends of small?mobile agricultural robots [J]. Journal of Chinese Agricultural Mechanization, 2019, 40(5): 178-186.
[ 4 ] 邢欽淞, 丁素明, 薛新宇, 等. 智能田間除草機(jī)器人發(fā)展現(xiàn)狀研究[J]. 中國(guó)農(nóng)機(jī)化學(xué)報(bào), 2022, 43(8): 173-181.
Xing Qinsong, Ding Suming, Xue Xinyu, et al. Research on the development status of intelligent field weeding robot [J]. Journal of Chinese Agricultural Mechanization, 2022, 43(8): 173-181.
[ 5 ] 徐淑萍, 楊定哲, 房嘉翔, 等. 一種改進(jìn)Cartographer算法的建圖方法研究[J]. 激光雜志, 2024, 45(10): 86-93.
Xu Shuping, Yang Dingzhe, Fang Jiaxiang, et al. Research on a method of improving Cartographer algorithm [J]. Laser Journal, 2024, 45(10): 86-93.
[ 6 ] 黃禹翔, 吳國(guó)新, 左云波. 基于改進(jìn)Cartographer的激光SLAM算法[J]. 北京信息科技大學(xué)學(xué)報(bào)(自然科學(xué)版), 2023, 38(2): 47-52.
Huang Yuxiang, Wu Guoxin, Zuo Yunbo. Laser SLAM algorithm based on improved Cartographer [J]. Journal of Beijing Information Science amp; Technology University, 2023, 38(2): 47-52.
[ 7 ] Dwijotomo A, Abdul Rahman M A, Mohammed Ariff M H, et al. Cartographer slam method for optimization with an adaptive multi?distance scan scheduler [J]. Applied Sciences, 2020, 10(1): 347.
[ 8 ] 章弘凱, 陳年生, 代作曉, 等. 一種多層次數(shù)據(jù)融合的SLAM定位算法[J]. 機(jī)器人, 2021, 43(6): 641-652.
Zhang Hongkai, Chen Niansheng, Dai Zuoxiao, et al. A multi?level data fusion localization algorithm for SLAM [J]. Robot, 2021, 43(6): 641-652.
[ 9 ] 賈浩. 基于Cartographer算法的SLAM與導(dǎo)航機(jī)器人設(shè)計(jì)[D]. 濟(jì)南: 山東大學(xué), 2019.
Jia Hao. Design of SLAM and navigation robot based on Cartographer algorithm [D]. Jinan: Shandong University, 2019.
[10] 龐永旭, 袁德成. 融合改進(jìn)A*與DWA算法的移動(dòng)機(jī)器人路徑規(guī)劃[J]. 計(jì)算機(jī)與現(xiàn)代化, 2022(1): 103-107.
Pang Yongxu, Yuan Decheng. Mobile robot path planning based on fusion of improved A* and DWA algorithms [J]. Computer and Modernization, 2022(1): 103-107.
[11] 王新彥, 盛冠杰, 張凱, 等. 基于改進(jìn)A*算法和DFS算法的割草機(jī)器人遍歷路徑規(guī)劃[J]. 中國(guó)農(nóng)機(jī)化學(xué)報(bào), 2023, 44(2): 142-147.
Wang Xinyan, Sheng Guanjie, Zhang Kai, et al. Traversal path planning of lawn mower robot based on improved A* algorithm and DFS algorithm [J]. Journal of Chinese Agricultural Mechanization, 2023, 44(2): 142-147.
[12] Mahmud M S A, Abidin M S Z, Mohamed Z, et al. Multi?objective path planner for an agricultural mobile robot in a virtual greenhouse environment [J]. Computers and Electronics in Agriculture, 2019, 157: 488-499.
[13] 劉佳豪, 羅天放, 王桐. IMU和輪式里程計(jì)聯(lián)合的雷達(dá)畸變校正算法[J]. 應(yīng)用科技, 2023(5): 149-156.
Liu Jiahao, Luo Tianfang, Wang Tong. Radar distortion correction algorithm based on IMU and wheel odometer fusion [J]. Applied Science and Technology, 2023(5): 149-156.
[14] Jia B, Zhu A, Yang S X, et al. Integrated gripper and cutter in a mobile robotic system for harvesting greenhouse products [C]. 2009 IEEE International Conference on Robotics and Biomimetics (ROBIO). IEEE, 2009: 1778-1783.
[15] Xiong K, Zhang H Y, Chan C W. Performance evaluation of UKF?based nonlinear filtering [J]. Automatica, 2006, 42(2): 261-270.
[16] 潘獻(xiàn)飛, 寧治文, 王茂松, 等. 基于因子圖的導(dǎo)航定位技術(shù)應(yīng)用分析與思考[J]. 控制理論與應(yīng)用, 2023, 40(12): 2130-2141.
Pan Xianfei, Ning Zhiwen, Wang Maosong, et al. Analysis and reflection on the navigation and positioning application based on factor graph [J]. Control Theory amp; Applications, 2023, 40(12): 2130-2141.
[17] 姚露, 聶曉根, 黃漢陽(yáng), 等. 無(wú)跡卡爾曼濾波算法對(duì)UWB/IMU組合定位的研究[J/OL]. 機(jī)械科學(xué)與技術(shù), 1-10[2024-11-13]. https://doi.org/10.13433/j.cnki.1003-8728.20230271.
[18] 張振, 張華良, 鄧永勝, 等. 融合改進(jìn)A*算法與DWA算法的機(jī)器人實(shí)時(shí)路徑規(guī)劃[J]. 無(wú)線(xiàn)電工程, 2022, 52(11): 1984-1993.
Zhang Zhen, Zhang Hualiang, Deng Yongsheng, et al. Real time path planning of robot by combing improved A* algorithm and dynamic window approach [J]. Radio Engineering, 2022, 52(11): 1984-1993.
[19] 勞彩蓮, 李鵬, 馮宇. 基于改進(jìn)A*與DWA算法融合的溫室機(jī)器人路徑規(guī)劃[J]. 農(nóng)業(yè)機(jī)械學(xué)報(bào), 2021, 52(1): 14-22.
Lao Cailian, Li Peng, Feng Yu. Path planning of greenhouse robot based on fusion of improved A* algorithm and dynamic window approach [J]. Transactions of the Chinese Society for Agricultural Machinery, 2021, 52(1): 14-22.
中國(guó)農(nóng)機(jī)化學(xué)報(bào)2025年4期