番茄溫室機器人的多傳感器融合建圖與導航研究

摘要：針對番茄溫室中的長廊環境及植被分布問題，對番茄機器人的多傳感器融合的SLAM和導航算法進行優化。基于Cartographer算法，進行激光雷達數據去畸變處理，再通過無跡卡爾曼濾波（UKF）融合激光雷達、里程計及IMU信息進行機器人位姿估計優化，并使用優化后的算法對溫室進行高精度地圖構建。將建立的高精度柵格地圖使用改進的A*與DWA融合算法進行驗證，通過動態調整權重和優化搜索邏輯，提高A*算法的搜索效率和路徑安全性，使得機器人在溫室環境下能夠更加智能地尋找最優且安全的路徑，在ROS的gazebo平臺和實地的溫室環境中驗證算法的有效性。結果表明，在機器人運行速度≤0.6 m/s時，平均位置偏差為10.3 cm，滿足番茄溫室的作業要求。

關鍵詞：番茄；溫室機器人；多傳感器融合；激光SLAM；路徑規劃；導航

中圖分類號：S625； TP24" " " 文獻標識碼：A" " " 文章編號：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 引言

設施農業發展是農業現代化的重要標志和建設任務。溫室機器人作為設施農業種植與生產過程中提高生產效率、保障農作物品質的重要措施，已成為當前研究的熱點之一[1]。截至2023年末，我國溫室大棚面積達到1 855.3 khm2，且保持穩定的發展趨勢[2]。將人工智能、互聯網、區塊鏈和機器人等技術與農業相結合，可以輔助或代替人工操作，大幅提高農業資源利用率和生產效率[3， 4]。

SLAM技術是通過傳感器采集信息，實現機器人對自身運動狀態估計并對周圍環境進行地圖構建的過程。在溫室長廊環境下，基于濾波的Gmapping算法在傳感器觀測時產生的誤差會累計并且無法進行回環檢測，基于圖優化的Cartographer算法在傳感器精度不足時，機器人位姿估計陷入局部極小，定位產生漂移，將無法獲得全局一致的運動軌跡估計與地圖構建，從而難以滿足導航、環境重建等應用需要。針對Cartographer算法中存在的點云特征丟失和低幀率激光雷達導致的運動畸變問題，徐淑萍等[5]利用AUKF優化傳感器數據改進了Cartographer算法，二次篩選體素濾波和點云，提高建圖精度，但算法耗時較長。黃禹翔等[6]采用k鄰域搜索鄰近點的體素濾波方法提高算法運算速率，并通過嵌入輪式里程計輔助模塊去除運動畸變，但未解決算法在復雜場景下位姿估計精度不足的問題。Dwijotomo等[7]通過設定不同的數據采集速度，從而優化激光雷達在不同幀之間的采集效果，以達到消除位姿擾動的目的，但沒考慮運動軌跡估計。章弘凱[8]、賈浩[9]等均對Cartographer算法進行了回環檢測的優化，提高算法后端優化的效率，但未對初始的傳感器進行優化。這些研究雖然都從不同方面提高了建圖精度，但未在實際應用場景中驗證地圖的有效性。通過路徑規劃算法對建立的地圖進行驗證，以實現機器人在已知環境下的自主移動。龐永旭等[10]融合傳統A*與DWA實現動態實時避障，但未在真實環境下測試。王新彥等[11]提出一種遍歷路徑規劃算法，但在障礙物較多的情況下效率較低。Mahmud等[12]根據真實溫室環境構建虛擬環境用于導航測試，通過多目標的路徑規劃算法降低機器人運動成本，但該算法容易陷入局部最小值導致無法向下一個目標移動。

綜上所述，本文對位姿圖優化的Cartographer算法進行多傳感器融合優化，并通過路徑規劃算法在建立的地圖中進行驗證試驗。通過里程計對激光雷達進行去畸變試驗，將優化后的雷達數據通過UKF融合多傳感器信息進行位姿估計，并在仿真平臺進行位姿估計和建圖效果的對比。在真實的溫室環境中，采取優化的建圖和路徑規劃算法驗證不同速度下溫室機器人巡檢功能的有效性。

1 激光雷達數據預處理

1.1 激光雷達觀測模型

目前，激光測距原理有三角測距法和飛行時間法。由于溫室環境影響，本研究采用受光線反射影響更小且短距離內精度更高的三角測距法。其主要原理是激光雷達內部的激光器發射激光束，光束照射到障礙物后被反射，隨后光線被攝像機收集成像在平面上形成點p。通過幾何關系，點p與成像平面交于點p'，如圖1所示。

根據相似原理，可求得障礙物到激光發射器與攝像機的法向距離q及激光雷達與障礙物間的距離d，如式（1）所示。

似然場模型在計算激光雷達測量點的權重時，充分考慮激光束周圍的環境信息并將其轉化為概率分布函數，更好地反映周圍環境的真實情況。準確的激光雷達觀測數據為機器人提供更為準確的定位精度，進而優化后續SLAM過程中位姿估計和地圖更新。

1.2 激光雷達去畸變

運動畸變指由于激光雷達在獲取一幀掃描數據時，機器人本身存在運動，導致掃描數據中不同時刻獲取的激光束測量位置之間存在偏差，從而使得掃描結果在空間中產生誤差和偏移[13， 14]。

通過輪式里程計輔助激光雷達進行運動畸變校正，輪式里程計是一種用于機器人定位和導航的傳感器，通過計算每個輪子的旋轉量和機器人運動學模型，推導出機器人在空間中的位姿。輪式里程計具有較高的局部位姿精度和更新頻率（一般為100～200 Hz），且跟狀態估計完全解耦。為了能將IMU信息與準確的激光雷達和里程計信息進行融合，對激光雷達進行去畸變試驗。

已知當前幀激光初始時間[ts]和結束時間[te]，時間差為[Δt]，截取時間戳小于[ts]且大于[te]的里程計數據進行校正，步驟如下。

2 溫室機器人多傳感器融合算法

2.1 無跡卡爾曼濾波融合算法

無跡卡爾曼濾波（UKF）是一種基于貝葉斯推斷的遞歸濾波器，可用于對非線性系統進行狀態估計。UKF結合無跡變換（UT）和標準卡爾曼濾波（KF），利用UT實現對非線性系統的線性化。通過采用無跡卡爾曼濾波，可將機器人采集的數據進行融合，以過濾傳感器誤差較大的信息。其主要步驟包括狀態預測、觀測預測、狀態更新和協方差更新4個部分[11]。

在機器人位姿估計應用中，假設機器人的狀態為xk，運動模型如式（15）所示。

UKF避免了對非線性函數進行泰勒展開的步驟，而是通過使用一組特殊選擇的點（稱為sigma點）來近似隨機變量的統計特性，從而更有效地處理非線性系統[15]。

2.2 基于無跡卡爾曼濾波融合位姿估計的Cartographer算法

在圖優化SLAM中，SLAM問題被抽象為一個圖，其中圖的節點表示機器人不同時刻的位姿，邊表示這些位姿之間的約束關系[16]。圖優化SLAM由前端匹配和后端優化兩個部分構成，前端部分處理傳感器數據生成圖的頂點和邊，后端則根據前端提供的信息進行機器人的位姿優化，以盡量滿足圖中的約束并構建全局地圖。

為解決Cartographer中使用單一里程計進行位姿估計不準確的問題，采用多傳感器融合位姿估計的方法，除進行里程計標定和激光雷達運動畸變校正之外，利用IMU對角度感知的敏銳性，將IMU傳感器的信息添加到激光里程計模型中，以修正輪式里程計的角度信息。通過使用無跡卡爾曼濾波算法實現多傳感器數據的融合，以提高SLAM算法的性能。

位姿融合過程中，使用輪式里程計信息、經過加速度計和陀螺儀進行互補濾波后的IMU信息以及激光掃描匹配的位姿信息作為輸入，通過無跡卡爾曼濾波的迭代方式進行狀態預測，得到機器人的當前位姿估計。通過激光掃描匹配器修正系統狀態變量和協方差，進一步提高位姿估計的準確性。由于里程計和IMU的采樣頻率比激光雷達高，利用它們進行位姿估計時，存在某些時間段內沒有激光掃描數據進行匹配估計的情況，并且長時間的誤差累積會對里程計和IMU的位姿估計產生較大的影響。因此，當激光掃描匹配器進行估計時，通過鄰近激光匹配的位姿進行位姿更新，以消除累積誤差的影響[17]。

1） 通過激光掃描匹配器對激光數據進行處理，估計機器人的位姿，同時計算出機器人的線速度和角速度。這些估計值作為狀態預測的初始值。

2） 根據IMU獲取的角速度進行積分，得到機器人轉動的角度，估計旋轉，如式（25）所示。

3 溫室機器人仿真分析

3.1 機器人位姿估計精度測驗

為驗證基于拓展卡爾曼濾波（EKF）和無跡卡爾曼濾波（UKF）算法的機器人位姿估計的效果，采用MATLAB進行仿真并對比，假設機器人在做勻速圓周運動，采樣時間dt=80 s，仿真結果如圖4所示。

由圖4可知，基于無跡卡爾曼濾波（UKF）的融合算法得到的估計軌跡比基于EKF得到的估計軌跡更加接近真實情況。

拓展卡爾曼濾波（EKF）與無跡卡爾曼濾波（UKF）算法的具體誤差對比如圖5所示。可以看出，在X和Y方向上，無跡卡爾曼濾波（UKF）算法的誤差均低于拓展卡爾曼濾波（EKF）算法。經計算，EKF的平均誤差為1.968 7 m，計算時間為0.007 8 s，而UKF的平均誤差為1.351 5 m，計算時間為0.057 8 s。通過上述比較，基于無跡卡爾曼濾波（UKF）的融合算法在機器人位姿估計上優于拓展卡爾曼濾波（EKF）的融合算法。

3.2 改進的SLAM算法仿真建圖試驗

通過gazebo搭建一個長8 m，寬8 m的壟道溫室進行仿真試驗，設置4個作物行壟，每個作物行壟由10株種植在花盆中的綠植組成，每個花盆直徑為20 cm，每個花盆間距10 cm，壟道寬度為80 cm。搭建的溫室環境如圖6所示。控制搭建的移動機器人以0.2 m/s的速度運動，分別運用原Cartographer和基于無跡卡爾曼濾波融合位姿估計的Cartographer算法進行建圖，如圖7所示。

仿真溫室環境建圖算法對比圖7表明，通過UKF添加IMU信息后的Cartographer建圖效果比原算法建圖效果更好，說明通過UKF將數據融合到一起，可以準確地描述機器人在環境中的運動狀態，保證SLAM過程中能夠準確進行掃描匹配得到更加完整的地圖，驗證加入IMU信息的Cartographer進行多傳感器融合構建溫室環境地圖的可行性。為客觀對比原算法與改進后算法的建圖精度，將圖測值與真實值對比，計算誤差，如表1所示。

4 實際溫室建圖及導航試驗

4.1 溫室真實環境地圖構建

為驗證多傳感器融合的建圖算法和改進的A*與DWA的融合算法可行性，在番茄溫室大棚環境下進行建圖和導航的實地測驗。

在該溫室中，每個番茄的株距為0.1 m，壟道長度約為22 m，可行走的壟道間距為1 m，控制機器人以0.2 m/s的速度沿壟道中心線行駛，按照上述方法進行溫室壟道的雙行建圖，試驗結果如圖8所示。可以看出，當位姿估計不準確時，構建的地圖會出現整體偏移，且有局部建圖不完整的情況。通過UKF融合多傳感器進行位姿估計能減小由于溫室雜葉、路面不平整等情況導致車輪打滑等現象帶來的位姿估計偏差，并較為準確地構建出溫室地圖。經圖測值與真實值對比，改進后的Cartographer算法橫向誤差約為0.2 m，滿足溫室機器人導航需求。

在試驗過程中，通過ROSBag記錄傳感器數據和算法，使用MATLAB對采集的數據進行可視化。將由單一里程計得到的位姿估計與通過UKF融合的機器人位姿估計進行對比，并添加理想的溫室壟道中心線作為準確位姿的參考，如圖9所示。單一里程計在位姿估計上與實際行走軌跡偏差較大，特別是當路面不平整，機器人偏離原軌道，通過旋轉回到原定路線時，由于里程計旋轉估計不準確導致較大的位姿偏差。通過UKF融合的位姿估計，依賴IMU信息提供的旋轉估計得到更準確的位姿估計。

圖10分別提供里程計和UKF融合的機器人位姿估計在X軸和Y軸的對比，可以看出，單一里程計與UKF得到的位姿估計與在X軸上不會產生較大變化，但是Y軸有明顯差別。里程計估計的位姿在兩個壟道處Y軸與壟道中心最大偏移分別為0.328 m、0.680 m；UKF優化后最大偏移縮小為0.131 m、0.541 m。

4.2 溫室機器人自主導航試驗

采用A*與動態窗口法（DWA）的融合算法進行溫室實地導航測驗，為提高溫室機器人路徑規劃搜索速率對A*算法從起始點到目標點的評價函數[F（n）]進行優化，如式（27）所示。

在原A*算法中實際代價和估計代價的權重分配是1∶1，當權重系數[W（n）]較大的時候，搜索路徑會快速向目標節點搜索，導致錯過最優路徑。采用動態加權的方式可以避免因追求搜索速度而導致規劃的路徑不是最優的情況。考慮使用動態加權的方式，將原本的啟發函數H（n）為判斷依據，引入距離判斷D（n），如式（28）所示。

基于改進的A*算法規劃的全局路徑的基礎上，選出關鍵參考點，使用DWA進行分段局部路徑規劃。計算軌跡終點與改進的A*算法的關鍵點的距離，距離越短，越靠近全局路徑。將式（26）作為新的評價函數，如式（30）所示。

DWA算法是實現局部避障的算法，但只把目標終點作為參考項，容易陷入局部最優。將改進A*和DWA融合來解決上述兩個問題，不僅可以確保規劃的路徑是最優的，而且可以擁有避開未知障礙物的能力[19]。兩種算法融合的具體流程如圖11所示。

溫室移動機器人以0.2 m/s、0.4 m/s和0.6 m/s作業速度在真實的溫室環境中進行導航試驗，各速度下重復進行2次試驗，測量目標導航點與溫室移動機器人平臺的質心標定點間的導航偏差，取2次試驗數據平均值作為該速度下的導航偏差。溫室導航精度測試試驗結果如表2所示。

由表2可知，溫室移動機器人在導航過程中，隨著速度的增加，其導航精度有所下降。機器人可以完整地進行壟內直行，轉彎以及避障操作。機器人在地圖構建的運動范圍內，運行速度≤0.6 m/s時，實際運行軌跡與所規劃路徑平均位置偏差為10.3 cm，平均相對誤差為0.46%。實際溫室環境與試驗室搭建環境導航結果相比，位置偏差和航向偏差均明顯增大，主要原因是當周圍環境相似時，激光雷達觀測出現誤差，導致定位不準確，其次是機器人在溫室路面行走時出現顛簸導致位置偏移。移動機器人平臺以0.2 m/s的速度移動，導航精度較高，可以滿足在該溫室環境下的導航要求。

5 結論

1） 針對傳感器精度不足，機器人運動位姿估計錯誤情況下難以完成溫室地圖構建及導航任務，使用輪式里程計輔助進行激光雷達運動畸變校正，機器人在溫室壟道內進行運動過程中，激光雷達數據不會發生跳變。

2） 針對傳統粒子濾波的建圖方法對溫室長廊環境會產生累計誤差，采用優化多傳感器融合的Cartographer算法進行地圖構建試驗，并在搭建的仿真溫室場景下進行導航建圖試驗。結果表明：通過UKF加入IMU信息后的多傳感器融合建圖在定位精度和建圖效果上優于EKF。

3） 在實際的溫室場景下進行SLAM及導航試驗，在實際場景試驗下，通過UKF優化的多傳感器位姿估計優于單一里程計，優化后的建圖算法構造的地圖更為準確。采用改進的A*與DWA融合算法對構建的柵格地圖進行導航試驗時，在機器人行進速度≤0.6 m/s時，平均位置偏差為10.3 cm，驗證多傳感器融合建圖導航方案的可行性。

參 考 文 獻

[ 1 ] 邢希君， 宋建成， 吝伶艷， 等. 設施農業溫室大棚智能控制技術的現狀與展望[J]. 江蘇農業科學， 2017， 45（21）： 10-15.

[ 2 ] 趙春江. 智慧農業的發展現狀與未來展望[J]. 華南農業大學學報， 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 ] 閆全濤， 李麗霞， 邱權， 等. 小型移動式農業機器人研究現狀及發展趨勢[J]. 中國農機化學報， 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]. 中國農機化學報， 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 ] 徐淑萍， 楊定哲， 房嘉翔， 等. 一種改進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 ] 黃禹翔， 吳國新， 左云波. 基于改進Cartographer的激光SLAM算法[J]. 北京信息科技大學學報（自然科學版）， 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 ] 章弘凱， 陳年生， 代作曉， 等. 一種多層次數據融合的SLAM定位算法[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]. 濟南： 山東大學， 2019.

Jia Hao. Design of SLAM and navigation robot based on Cartographer algorithm [D]. Jinan： Shandong University， 2019.

[10] 龐永旭， 袁德成. 融合改進A*與DWA算法的移動機器人路徑規劃[J]. 計算機與現代化， 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] 王新彥， 盛冠杰， 張凱， 等. 基于改進A*算法和DFS算法的割草機器人遍歷路徑規劃[J]. 中國農機化學報， 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]. 應用科技， 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] 潘獻飛， 寧治文， 王茂松， 等. 基于因子圖的導航定位技術應用分析與思考[J]. 控制理論與應用， 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] 姚露， 聶曉根， 黃漢陽， 等. 無跡卡爾曼濾波算法對UWB/IMU組合定位的研究[J/OL]. 機械科學與技術， 1-10[2024-11-13]. https：//doi.org/10.13433/j.cnki.1003-8728.20230271.

[18] 張振， 張華良， 鄧永勝， 等. 融合改進A*算法與DWA算法的機器人實時路徑規劃[J]. 無線電工程， 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] 勞彩蓮， 李鵬， 馮宇. 基于改進A*與DWA算法融合的溫室機器人路徑規劃[J]. 農業機械學報， 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.