基于激光雷達的牧場巡檢機器人定位與建圖算法設計

摘要：

針對牧場巡檢機器人定位精度和魯棒性低、建圖精度和穩定性差的問題，提出一種基于激光雷達測距和測繪技術與改進LOAM-SLAM算法的LOM-SLAM算法。LOM-SLAM算法在LOAM-SLAM算法的基礎上將SLAM分解為運動估計和地圖構建兩個過程，利用激光雷達的高精度測距和測繪技術，實現同時進行機器人的定位和地圖構建，從而提高定位與建圖的精度，提高魯棒性和穩定性。將LOM-SLAM搭載在麥輪結構的巡檢機器人上進行試驗驗證。結果表明：在位姿估計試驗中，LOM-SLAM算法的絕對軌跡誤差（ATE）和相對位姿誤差（RPE）的RMSE值分別僅為7.28m和2.23m，均低于對比算法。在定位與建圖試驗中，當巡檢機器人分別以0.2 m/s、0.5 m/s、1 m/s的速度運動時，LOM-SLAM的定位誤差分別為0.12 m、1 m、1.2 m，具有更好的定位精度和穩健性。

關鍵詞：巡檢機器人；激光雷達；改進SLAM；牧場環境；定位與建圖

中圖分類號：S24

文獻標識碼：A

文章編號：2095-5553 （2024） 04-0222-09

收稿日期：2023年9月24日" 修回日期：2023年11月30日

基金項目：內蒙古自治區級大學生創新創業訓練計劃項目（S202310128023）；內蒙古自治區高等學校科學研究項目（NJZY21311）

第一作者：高金喆，男，2002年生，湖北襄陽人，碩士研究生；研究方向為智能巡檢。E-mail： 2575949685@qq.com

通訊作者：寇志偉，男，1984年生，內蒙古卓資人，博士，副教授；研究方向為微機電傳感器技術、計算機測控技術等。E-mail： kouzhiwei@imut.edu.cn

Design of location and mapping algorithm of pasture inspection robot based on LiDAR

Gao Jinzhe1， Kou Zhiwei1， 2， 3， Kong Zhe1， Jing Gaole1， Ma Jiayin1， Xu Hanqi1

（1. College of Electric Power， Inner Mongolia University of Technology， Hohhot， 010051， China;

2. Inner Mongolia Key Laboratory of Electromechanical Control， Hohhot， 010051， China；

3. Engineering Research Center of Large Energy Storage Technology， Ministry of Education， Hohhot， 010051， China）

Abstract：

Aiming at the problems" of low positioning accuracy and robustness， as well as poor precision and stability in mapping for pasture inspection robots， a novel LOM-SLAM algorithm based on LiDAR ranging and mapping technology and improved LOM-SLAM algorithm is proposed. This algorithm is derived from an enhanced LOAM-SLAM algorithm， which integrates laser range finding and surveying technology. LOM-SLAM decomposes SLAM into two separate processes such as motion estimation and map construction. By leveraging the high precision of laser range finding and surveying technology， LOM-SLAM achieves simultaneous robot localization and map building， thereby enhancing the accuracy， robustness， and stability of both positioning and mapping. LOM-SLAM was installed on a designed inspection robot with Mecanum wheel structure for test verification. The results showed that in pose estimation tests， LOM-SLAM significantly outperformed other methods in terms of relative pose error （RPE） and absolute trajectory error （ATE）， with RMSE values of just 7.28m and 2.23m， respectively， which were lower than the comparative algorithms. In the positioning and mapping tests， with the inspection robot moving at speeds of 0.2 m/s， 0.5 m/s， and 1 m/s， the positioning errors of LOM-SLAM were only 0.12 m， 1 m， and 1.2 m， respectively， demonstrating better positioning accuracy and robustness compared to the comparative algorithms.

Keywords：

inspection robots; LiDAR; improved SLAM; pastoral environment; positioning and mapping

0 引言

近年來，智能機器人技術在農牧業領域，特別是在牧場的巡檢與管理中，得到了廣泛應用［1］。傳統的牧場巡檢通常依賴于人工，這不僅耗費大量的人力和時間，還可能因為效率和準確性不高而影響牧場的運營。為了提升效率，牧場開始采用巡檢機器人進行巡檢和牧場管理。在這個過程中，精確的定位和有效的建圖是提高牧場巡檢機器人效率的關鍵［2， 3］。然而，在復雜的牧場環境中，傳統的定位方法（如全局定位系統和慣性導航系統）的精度和魯棒性常受到限制［4， 5］。

為應對復雜環境中的挑戰，陳建等［3］引入基于視覺的同時定位與建圖（SLAM）技術。視覺SLAM的優點在于其較低的成本和獲取高頻率圖像信息的能力。Labbe等［4］通過擴展RTAB地圖，支持結合視覺和激光雷達的SLAM方法，以優化自主導航應用的性能。彭夢等［5］則將SLAM算法與基于深度學習的方法及多種傳感器相結合，證明視覺SLAM在無人機、自動駕駛和增強現實等領域的有效應用。盡管如此，視覺SLAM在牧場環境中的定位精度和建圖穩定性仍有待提高［6-8］。

綜上所述，本文為解決牧場環境下低定位精度和建圖穩定性的問題，提出一種基于激光雷達測距及測繪技術和改進的LOAM-SLAM算法的LOM-SLAM算法。LOM-SLAM在LOAM-SLAM的基礎上將SLAM過程分解為2個主要部分：運動估計和地圖構建。巡檢機器人使用激光雷達的高精度測距和測繪技術，通過插值法得到任意時刻的位姿估計，計算位姿變換的旋轉矩陣及平移向量，從而精確估計運動軌跡，實現自主定位。在世界坐標系中，通過位姿估計匹配算法，得到更高精度的位姿變換，并據此構建出新的地圖，從而提高定位和建圖的精度，增強系統的魯棒性和穩定性。

1 牧場巡檢機器人結構

牧場環境復雜，地面不均勻、土壤濕滑且松軟，并且常位于開闊的室外環境，需要應對多變的氣候條件。因此，牧場巡檢機器人在設計時需具備高機動性、耐候性。此外，巡檢機器人還應盡量避免干擾動物的活動，并能在灰塵環境中保持正常功能，同時確保在廣闊區域內能實現穩定通信和精準導航［9， 10］。

本文采用麥輪（Mecanum wheel）結構的巡檢機器人，確保機器人在牧場環境中活動的靈活性和魯棒性，為巡檢機器人的定位與建圖提供硬件基礎。巡檢機器人的主板選用樹莓派4B，搭載SLAMTEC A2M12型激光雷達和INTERL D455型雙目攝像機，其結構如圖1所示。

3 試驗驗證

3.1 巡檢機器人位姿估計試驗

為評估LOM-SLAM性能，試驗選用的數據集為歐洲機器人挑戰賽數據集（EuRoC）。試驗平臺為本文所設計的巡檢機器人，傳感器子系統負責實時采集環境數據，包括激光雷達生成的點云和雙目攝像機捕捉到的圖像，主控計算機樹莓派4B以定時調用傳感器接口方式，獲取機器人的當前位姿和周圍環境的幾何特征等數據流，通過解析、特征提取和匹配等操作，推導出機器人的實時位姿，經過處理后的結果通過通信通道返回至由STM32芯片構成的機器人底盤控制系統，系統根據這些結果調整運動參數，以確保機器人在自主導航中按照預定路徑行進。通過連續循環執行這一過程，機器人能夠在復雜環境中實現高效自主導航和精準位姿估計，具體操作框架如圖7所示。

本文采用輕型和地面優化的激光雷達測距和測繪法（Lightweight and Ground-Optimized LiDAR Odometry and Mapping， LeGO-LOAM）［23］以及快速激光雷達實時定位與建圖技術（Fast LiDAR Odometry and Mapping on Autonomous Vehicles， Fast-LIO2）［24］與本文所提方法進行結果對比。各個算法的掃描頻率范圍為10～20 Hz，掃描角度為360°，分辨率范圍為0.1°～0.5°。

本文選取絕對軌跡誤差（ATE）和相對位姿誤差（RPE）作為評價指標，運用均方根誤差（RMSE）來量化ATE與RPE［21， 22］。試驗流程包括如下步驟：首先，進行試驗設備準備，選擇合適的激光雷達并建立試驗平臺。其次，在牧場環境中執行激光雷達掃描并記錄掃描數據。隨后，對掃描數據進行預處理，包括噪聲去除和濾波等處理，將預處理后的數據輸入LeGO-LOAM、Fast-LIO2以及本文提出的方法，并運行算法以獲取定位和建圖結果。最后，通過計算ATE和RPE，采用RMSE量化誤差，并進行各算法的定位和建圖結果對比，分析其優缺點。

各方法在EuRoC數據集上的RPE結果如圖8所示，橫坐標x代表數據集的圖像序列幀數，中間值與平均值是整個測試圖像序列得到的RPE的中間值與平均值。

由圖8（a）可知，LeGO-LOAM算法的RPE中間值和平均值分別為3.95m、4.22m，RMSE值高達3.15m。由圖8（b）可知，Fast-LIO2算法的RPE中間值和平均值分別為4.13m、4.42m，RMSE值高達2.27m。由圖8（c）可知，LOM-SLAM的RPE中間值和平均值分別為2.02m和1.86m，RMSE值僅為2.23m，其各項指標明顯優于LeGO-LOAM和Fast-LIO2算法。表明本文提出的方法具有更高的精度，能夠提供更準確的相對位姿變換值。

各方法在EuRoC數據集上的ATE結果如圖9所示。從圖9（a）可知，LeGO-LOAM算法的ATE中間值和平均值分別為14.58m、14.02m，RMSE值高達14.54m。由圖9（b）可知，Fast-LIO2算法的ATE中間值和平均值分別為14.96m、11.66m，RMSE值高達15.25m。由圖9（c）可知，LOM-SLAM的ATE中間值和平均值分別為6.51m和5.73m，RMSE值高達7.28m，其各項指標明顯優于LeGO-LOAM和Fast-LIO2算法。說明LOM-SLAM在位姿估計方面具有顯著優勢。

3.2 巡檢機器人定位與建圖試驗

本文進一步使用MATLAB進行LOM-SLAM的可行性試驗驗證，采用LeGO-LOAM以及Fast-LIO2與本文所提方法進行結果對比，試驗對不同定位方法與真實值的誤差進行測量，以驗證本文所提方法的優越性。評價指標為定位方法檢測值與真實測量值的定位誤差值。選取內蒙古工業大學體育場草坪作為試驗環境，巡檢機器人試驗環境如圖10所示，試驗將機器人的轉向角設為-90°～90°，運行速度分別設定為0.2 m/s、0.5 m/s、1 m/s。定位誤差結果如表1所示。

當機器人的運行速度為0.2 m/s時，試驗結果如圖11所示，圖中障礙物用藍色區域表示，信號屏蔽區域則是黃色部分。定位方法與真實值之間的誤差結果如圖12所示。由圖11可知，與LeGO-LOAM以及Fast-LIO2算法相比，LOM-SLAM算法的定位曲線與真實軌跡更吻合，說明LOM-SLAM算法的定位精度更高。從圖12可知，當巡檢機器人的速度為0.2 m/s時，LOM-SLAM的定位誤差僅為0.12 m，相對于LeGO-LOAM和Fast-LIO2，LOM-SLAM的定位誤差分別降低了0.12 m和0.18 m，表明LOM-SLAM在定位精度和魯棒性方面具有顯著的優勢。

機器人運行速度為0.5 m/s時的試驗結果如圖13所示。不同定位方法與真實值之間的誤差結果如圖14所示。從圖13可知，當速度提升至0.5 m/s時，各個方法的定位曲線與真實軌跡的偏差均有所增加。然而，LOM-SLAM的定位曲線與真實軌跡依然保持著更接近的狀態，顯示其在定位性能上明顯優于其他兩種算法。由圖14可知，各個方法的定位誤差值均有所提升。具體來說，LOM-SLAM的定位誤差相比0.2 m/s時的誤差值僅增長了0.8 m。相較之下，LeGO-LOAM方法的誤差值在速度為0.5 m/s時達到0.39 m，相比于0.2 m/s時的誤差值增長0.15 m。Fast-LIO2方法的誤差值則達到0.42 m，相比0.2 m/s時的誤差值提升0.12 m。這表明LOM-SLAM的定位誤差值更小，顯示出更優越的定位性能。

當巡檢運行速度為1 m/s時的試驗結果如圖15所示。其定位方法與真實值之間的誤差結果如圖16所示。

由圖15可知，當速度升至1 m/s時，各個方法的定位曲線與真實軌跡的偏差都有所增加。然而，本文提出的方法在這個情況下展現出最小的偏差，這表明LOM-SLAM具備顯著的定位性能。由圖16可知，在運行速度達到1 m/s時，三種方法的定位誤差值都呈現上升趨勢。具體而言，LOM-SLAM的定位誤差最小，僅為0.28 m。相較之下，LeGO-LOAM方法的定位誤差值在速度為1 m/s時達到0.48 m，相比0.2 m/s時的誤差值增加0.24 m。Fast-LIO2方法的定位誤差值高達0.58 m，相比0.2 m/s時的誤差值增加0.28 m。這顯示出LOM-SLAM的定位精度更高，且具備更強的魯棒性。

4 結論

牧場作為畜牧業生產活動中的重要組成部分，使用巡檢機器人提高牧場巡檢和管理的效率具有重要的現實意義。考慮到牧場的復雜環境，本文采用麥克納姆輪結構的巡檢機器人，主板選用樹莓派4B，搭載SLAMTEC A2M12型激光雷達和Intel D455型雙目攝像機，并使用LOM-SLAM算法，從而實現巡檢機器人的定位和建圖。

1）" 在位姿估計試驗中，EuRoC數據集上，LOM-SLAM算法在RPE方面明顯優于其他兩種方法，其RMSE值僅為2.23m，而LeGO-LOAM和Fast-LIO2的RMSE值分別為3.15m和2.27m。此外，LOM-SLAM算法的ATE也明顯優于其他兩種方法，其RMSE值僅為7.28m，相較于其他兩種方法分別降低7.26m和7.97m。說明LOM-SLAM算法在位姿估計方面具有更高的精度，能夠提供更準確的相對位姿變換值。

2）" 在定位與建圖試驗中，當機器人速度為0.2 m/s時，LOM-SLAM的定位誤差僅為0.12 m，相對LeGO-LOAM和Fast-LIO2分別減少0.12 m和0.18 m。當機器人速度分別增加到0.5 m/s和1 m/s時，盡管各種方法的定位偏差都有所增加，但LOM-SLAM的定位曲線仍更接近真實軌跡。說明LOM-SLAM方法具有更優異，更穩定的定位性能，有利于提升建圖的準確性。

3） 試驗表明與其他兩種定位方法相比，LOM-SLAM的定位和建圖精度更高，適用于實際的牧場巡檢。然而，目前的探測方法無法覆蓋所有的環境信息，且在某些復雜的環境中，難以準確識別和理解環境中的各種元素。為改進這一點，后續研究可以考慮引入更先進的機器學習算法以提高環境適應性和定位精度，或者結合其他傳感器以提升地圖構建的準確性。

參 考 文 獻

［1］ 羅亮， 談莉斌， 余曉流， 等. 一種融合二維激光雷達和RGB-D相機的移動機器人建圖方法研究［J］. 制造業自動化， 2023， 45（4）： 137-140， 190.

Luo Liang， Tan Libin， Yu Xiaoliu， et al. Research on a mobile robot mapping method based on the fusion of 2D LiDAR and RGB-D camera ［J］. Manufacturing Automation， 2023， 45（4）： 137-140， 190.

［2］ 胡煉， 王志敏， 汪沛， 等. 基于激光感知的農業機器人定位系統［J］. 農業工程學報， 2023， 39（5）： 1-7.

Hu Lian， Wang Zhimin， Wang Pei， et al. Agricultural robot positioning system based on laser sensing ［J］. Transactions of the Chinese Society of Agricultural Engineering， 2023， 39（5）： 1-7.

［3］ 陳建， 于殿泓， 惠鑫鑫， 等. 基于二維激光雷達多車道車輛檢測與分車算法的研究［J］. 傳感技術學報， 2022， 35（11）： 1513-1520.

Chen Jian， Yu Dianhong， Hui Xinxin， et al. Research on multi-lane vehicle detection and separation algorithm based on two-dimensional LiDAR ［J］. Chinese Journal of Sensors and Actuators， 2022， 35（11）： 1513-1520.

［4］ Labbe M， Michaud F. RTAB-Map as an open-source LiDAR and visual simultaneous localization and mapping library for large-scale and long-term online operation ［J］. Journal of Field Robotics， 2019， 36（2）： 416-446.

［5］ 彭夢， 萬琴， 陳白帆， 等. 基于觀測概率有效下界估計的二維激光雷達和攝像機標定方法［J］. 電子與信息學報， 2022， 44（7）： 2478-2487.

Peng Meng， Wan Qin， Chen Baifan， et al.A calibration method of 2D LiDAR and a camera based on effective lower bound estimation of observation probability ［J］. Journal of Electronics amp; Information Technology， 2022， 44（7）： 2478-2487.

［6］ 張浩， 左杭， 劉寶華. 視覺與二維激光雷達的目標檢測方法［J］. 電子測量與儀器學報， 2022， 36（3）： 79-86.

Zhang Hao， Zuo Hang， Liu Baohua. Target detection method for visual and 2D laser radar ［J］. Journal of Electronic Measurement and Instrumentation， 2022， 36（3）： 79-86.

［7］ 王祝， 王智， 張旭， 等. 基于二維激光雷達的自適應閾值聚類分割方法［J］. 中國激光， 2021， 48（16）： 182-189.

Wang Zhu， Wang Zhi， Zhang Xu， et al. Adaptive threshold clustering segmentation method based on two-dimensional LiDAR ［J］. Chinese Journal of Lasers， 2021， 48（16）： 182-189.

［8］ 張宏群， 劉開輝， 鄭峰， 等. 基于二維激光雷達植株高度測量系統的研究［J］. 電子測量技術， 2021， 44（4）： 97-103.

Zhang Hongqun， Liu Kaihui， Zheng Feng， et al. Research on plant height measurement system based on two-dimensional LiDAR ［J］. Electronic Measurement Technology， 2021， 44（4）： 97-103.

［9］ 張磊， 徐孝彬， 何佳， 等. 基于室內結構特征的二維激光雷達與相機標定方法（英文）［J］. 光子學報， 2020， 49（12）： 67-78.

Zhang Lei， Xu Xiaobin， He Jia， et al.Calibration method of 2D LiDAR and camera based on indoor structural features ［J］.Acta Photonica Sinica， 2020， 49（12）： 67-78.

［10］ 黃志清， 蘇毅， 王慶文， 等. 二維激光雷達與可見光相機外參標定方法研究［J］. 儀器儀表學報， 2020， 41（9）： 121-129.

Huang Zhiqing， Su Yi， Wang Qingwen， et al. Research on extrinsic parameter calibration method of 2D laser rangefinder （LRF） and visible light camera ［J］. Chinese Journal of Scientific Instrument， 2020， 41（9）： 121-129.

［11］ 牛秦玉， 暢科劍， 李珍惜. 基于ROS的多AGV在交叉路口下協同控制策略［J］. 現代制造工程， 2020（10）： 33-38.

Niu Qinyu， Chang Kejian， Li Zhenxi. Cooperative control strategy of multi-AGV based on ROS at intersection ［J］. Modern Manufacturing Engineering， 2020（10）： 33-38.

［12］ 田佳， 劉泓濱， 王宇， 等. 一種機器視覺的安防巡邏機器人系統設計［J］. 機械設計與制造， 2020（11）： 254-257.

Tian Jia， Liu Hongbin， Wang Yu， et al. Design of a robot system with machine vision for security and patrol ［J］. Machinery Design amp; Manufacture， 2020（11）： 254-257.

［13］ 蔡澤宇， 金誠謙. 基于二維激光雷達點云的物體輪廓識別［J］. 應用激光， 2020， 40（3）： 513-518.

Cai Zeyu， Jin Chengqian. Object contour recognition based on 2D LIDAR point cloud ［J］. Applied Laser， 2020， 40（3）： 513-518.

［14］ 張旭， 王智， 崔粲， 等. 二維激光雷達定位建圖系統設計與實現［J］. 光學技術， 2019， 45（5）： 596-600.

Zhang Xu， Wang Zhi， Cui Can， et al. Design and implementation of 2D LiDAR positioning and mapping system ［J］. Optical Technique， 2019， 45（5）： 596-600.

［15］ 胡麗軍， 吳燕玲， 宋全軍， 等. 基于ST-GCN警用巡邏機器人警情識別系統設計［J］. 傳感器與微系統， 2023， 42（6）： 78-81.

Hu Lijun， Wu Yanling， Song Quanjun， et al. Design of police situation recognition system for police patrol robots based on ST-GCN ［J］. Transducer and Microsystem Technologies， 2023， 42（6）： 78-81.

［16］ Spuler S M， Hayman M， Stillwell R A， et al. MicroPulse DIAL （MPD）-a diode-laser-based LiDAR architecture for quantitative atmospheric profiling ［J］. Atmospheric Measurement Techniques， 2021， 14（6）： 4593-4616.

［17］ 靳荔成， 劉一萱， 白瑞峰， 等. 面向校園多機器人協同巡查的路徑規劃虛擬仿真實驗設計［J］. 實驗技術與管理， 2023， 40（3）： 93-99.

Jin Licheng， Liu Yixuan， Bai Ruifeng， et al. Design of virtual simulation experiment of path planning for multi-robot cooperative patrol in campus ［J］. Experimental Technology and Management， 2023， 40（3）： 93-99.

［18］ 張淑清， 鄧慶彪， 蔡志文. 安防巡邏機器人全覆蓋視覺導航方法［J］. 機械設計與制造， 2022（12）： 264-268.

Zhang Shuqing， Deng Qingbiao， Cai Zhiwen. Visual navigation method for security patrol robot with full coverage ［J］. Machinery Design amp; Manufacture， 2022（12）： 264-268.

［19］ Chen R， Li C， Zhu Y， et al. Extraction of crown information from individual fruit tree by UAV LiDAR ［J］. Nongye Gongcheng Xuebao/Transactions of the Chinese Society of Agricultural Engineering， 2020， 36（22）： 50-59.

［20］ 張輝， 王耀南， 易俊飛， 等. 面向重大疫情應急防控的智能機器人系統研究［J］. 中國科學： 信息科學， 2020， 50（7）： 1069-1090.

Zhang Hui， Wang Yaonan， Yi Junfei， et al. Research on intelligent robot systems for emergency prevention and control of major pandemics ［J］. Scientia Sinica （Informationis）， 2020， 50（7）： 1069-1090.

［21］ 趙云濤， 李宗剛， 杜亞江. 基于最大空閑時間的分布式巡邏機器人數量優化［J］. 模式識別與人工智能， 2020， 33（4）： 375-382.

Zhao Yuntao， Li Zonggang， Du Yajiang. Team size optimization for distributed patrol of multi-robot systems based on maximum idle time ［J］. Pattern Recognition and Artificial Intelligence， 2020， 33（4）： 375-382.

［22］ 徐曉蘇， 李諾， 姚逸卿. 基于快速回環檢測的室外環境下激光雷達SLAM算法［J］. 中國慣性技術學報， 2022， 30（6）： 716-722.

Xu Xiaosu， Li Nuo， Yao Yiqing. LiDAR SLAM algorithm in outdoor environment based on fast loop detection ［J］. Journal of Chinese Inertial Technology， 2022， 30（6）： 716-722.

［23］ 李歡， 余紅英. 基于FPGA的高性能激光雷達測距系統設計［J］. 電子測量技術， 2021， 44（20）： 6-10.

Li Huan， Yu Hongying. Design of high-performance LiDAR ranging system based on FPGA ［J］. Electronic Measurement Technology， 2021， 44（20）： 6-10.

［24］ 李莉. 基于傾斜攝影測量和激光雷達的三維模型快速制作方法研究［J］. 測繪與空間地理信息， 2022， 45（9）： 220-221， 225.

Li Li. Research on rapid construction method of 3D models based on oblique photogrammetry and LiDAR ［J］. Geomatics amp; Spatial Information Technology， 2022， 45（9）： 220-221， 225.