李晨陽，彭 程，張振乾，苗艷龍，張 漫，李 寒

融合里程計信息的農(nóng)業(yè)機器人定位與地圖構(gòu)建方法

李晨陽1，彭 程2，張振乾1，苗艷龍1，張 漫1，李 寒2※

（1. 中國農(nóng)業(yè)大學現(xiàn)代精細農(nóng)業(yè)系統(tǒng)集成研究教育部重點實驗室，北京 100083；2. 中國農(nóng)業(yè)大學農(nóng)業(yè)農(nóng)村部農(nóng)業(yè)信息獲取技術(shù)重點實驗室，北京 100083）

目前主流的農(nóng)業(yè)機器人以低成本、低幀率的激光雷達作為即時定位與地圖構(gòu)建（Simultaneous Localization and Mapping, SLAM）的主要傳感器，存在運動畸變和誤匹配的問題。該研究針對這一問題提出了融合里程計信息的Gmapping建圖算法，利用高頻率里程計信息為每一個激光束匹配到近似的機器人位姿，獲取機器人當前位姿下更為精確的激光數(shù)據(jù)，以減少激光雷達運動畸變對地圖構(gòu)建產(chǎn)生的影響。利用機器人搭載掃描頻率為5 Hz的RPLIDAR A1激光雷達在玉米田及香蕉園中進行了SLAM建圖精度測試試驗。試驗結(jié)果表明，在長度為12 m左右的玉米田區(qū)域，Gmapping建圖算法的平均絕對誤差為0.06 m，該研究算法建圖平均絕對誤差為0.01 m，相比于Gmapping建圖算法降低了0.05 m，建圖精度為99.5%；在長度為24.43 m的香蕉園區(qū)域，Gmapping建圖算法的平均絕對誤差為0.46 m，該研究算法建圖平均絕對誤差為0.07 m，相比于Gmapping建圖算法降低了0.39 m，建圖精度為99.1%。該研究算法有效地降低了低幀率激光雷達運動畸變對Gmapping建圖的影響，可以基本滿足農(nóng)業(yè)環(huán)境下的高精度環(huán)境建圖需求。

機器人；傳感器；建圖；運動估計；信息融合

0 引 言

近年來，機器人技術(shù)快速發(fā)展。即時定位與地圖構(gòu)建（Simultaneous Localization and Mapping, SLAM）是機器人導航技術(shù)的關(guān)鍵技術(shù)之一，也是機器人自動導航的重要基礎[1]。傳感器技術(shù)的發(fā)展使得即時定位與地圖構(gòu)建技術(shù)在建圖精度及魯棒性上有了一定程度的提升。根據(jù)傳感器的類型可以將SLAM分為視覺SLAM和激光SLAM兩大類[2]，其中視覺SLAM借助視覺傳感器獲取環(huán)境信息，成本低、結(jié)構(gòu)簡單，但運算量大，易受光照影響，不適用于農(nóng)田等光照變化明顯的環(huán)境[3-4]；激光SLAM技術(shù)相對成熟，測距準確，受光照影響較小，且激光雷達相比于相機、超聲波、紅外傳感器等具有抗干擾能力強、精度高、測量范圍廣等優(yōu)勢，更適用于農(nóng)業(yè)環(huán)境[5-7]。目前主流的農(nóng)業(yè)機器人搭載的激光雷達以低成本、低幀率的激光雷達為主，然而低幀率激光雷達不可避免地存在運動畸變及誤匹配等問題[8]。針對運動畸變?nèi)コ龁栴}，孟蕾等[9]通過點對線迭代最近點（Point-to-Line Iterative Closest Point，PL-ICP）算法對激光雷達相鄰2幀數(shù)據(jù)進行匹配以獲得機器人的相對位姿變換，改進粒子濾波器的建議分布函數(shù)；Jun等[10]提出了一種異常值剔除方案，在使用最近點迭代算法（Iterative Closest Point，ICP）對點云數(shù)據(jù)進行匹配時剔除不匹配的部分，提高運算效率及系統(tǒng)魯棒性；Shi等[11]提出一種基于正態(tài)分布變換的迭代最近配準算法，解決了傳統(tǒng)ICP算法在點云規(guī)模較大時配準速度慢的問題。以上研究均利用ICP算法對點云進行配準，但由于ICP算法需要對2幀點云數(shù)據(jù)進行匹配，所以機器人位姿變化造成的點云數(shù)據(jù)誤差會對激光點云匹配造成較為嚴重的影響。針對ICP算法的這一問題，王雅儀等[12]提出全球衛(wèi)星導航系統(tǒng)（Global Navigation Satellite System, GNSS）數(shù)據(jù)融合激光SLAM的方案，使用GNSS提供機器人位姿信息輔助構(gòu)建點云地圖，但在農(nóng)業(yè)環(huán)境中衛(wèi)星信號受農(nóng)作物遮擋嚴重，無法使用GNSS進行輔助；Hong等[13]提出了速度更新迭代最近點VICP（Velocity Updating Iterative Closest Point）算法，將機器人的運動考慮在內(nèi)，認為獲取激光數(shù)據(jù)的過程中機器人在做勻速運動，從而解決機器人位姿估計問題，但勻速運動的假設過于理想，不能真實的反應機器人運動的情況；Yan等[14]提出一種分層點云全局優(yōu)化算法，通過ICP算法對重疊點云進行匹配，構(gòu)造機器人位姿圖對軌跡進行優(yōu)化，最后通過優(yōu)化后的軌跡對點云進行細化消除采集點云的不一致性，但機器人位姿圖的獲取較為復雜，計算量較大。

綜上，使用純估計方法去除激光雷達運動畸變存在無法準確反映機器人運動情況、計算量較大等問題。通常移動機器人會通過編碼器的脈沖數(shù)計算估計機器人的位置和姿態(tài)，即里程計信息，里程計具有較高的更新頻率，可以較為準確地反應機器人的運動情況，且具有較高精度的局部位姿估計[15]。為此，本研究擬采用里程計輔助的方法，在傳統(tǒng)Gmapping算法的基礎上融合里程計信息，為每一幀激光數(shù)據(jù)匹配一個較為準確的機器人位姿，以去除激光雷達運動畸變。

1 材料與方法

1.1 研究平臺

本研究移動機器人平臺選擇jetson nano開發(fā)板作為控制系統(tǒng)平臺，搭載Ubuntu 18.04系統(tǒng)，并運行機器人操作系統(tǒng)（Robot Operating System, ROS），實現(xiàn)對傳感器信息的處理及導航功能的實現(xiàn)；STM32F103RCT6 主控芯片是底盤驅(qū)動的核心，接收來自ROS系統(tǒng)的速度指令并經(jīng)過轉(zhuǎn)化實現(xiàn)對電機驅(qū)動控制，本研究采用GM37-520直流霍爾編碼減速電機；激光雷達與jetson nano開發(fā)板直接連接；12 V 鋰電池同時為激光雷達、電機、STM32、jetson nano kit完成供電。移動機器人平臺如圖1所示。

圖 1 移動機器人平臺

其中，本文機器人搭載的激光雷達為思嵐科技有限公司研發(fā)的RPLIDAR A1。相比于其他類型的激光雷達，RPLIDAR A1價格低廉、使用時間長且具有較好的穩(wěn)定性[16]。RPLIDAR A1的掃描頻率最大為10 Hz，測距范圍最大可達12 m，根據(jù)雷達性能參數(shù)，本文設置其掃描頻率為5 Hz。本研究基于此平臺進行SLAM方法的研究并進行分析。

1.2 研究方法

1.2.1 SLAM問題概率描述

其中1:t代表機器人啟動到時刻傳感器測量數(shù)據(jù)，本文中主要指激光雷達數(shù)據(jù)；1:t指機器人啟動到時刻一系列的控制數(shù)據(jù)，即里程計信息；在獲取上述兩種數(shù)據(jù)的情況下同時對地圖和機器人位姿1:t進行估計。

根據(jù)條件聯(lián)合概率分布可進一步簡化為

式中(1:t|1:t,1:t)表示機器人的位姿估計問題；(|1:t,1:t)表示在已知機器人位姿和傳感器觀測數(shù)據(jù)的情況下進行地圖構(gòu)建。

由此可見，精確的機器人即時定位與地圖構(gòu)建依賴于高精度的激光雷達數(shù)據(jù)。本研究基于經(jīng)典的激光SLAM算法——Gmapping算法展開進一步分析。

1.2.2 激光雷達運動畸變分析

激光SLAM建圖的過程為控制機器人移動的同時激光雷達對環(huán)境持續(xù)掃描，進而生成二維柵格地圖[19]。激光雷達數(shù)據(jù)的獲取伴隨著機器人的運動，而一般激光雷達進行數(shù)據(jù)封裝時默認一幀激光雷達數(shù)據(jù)是在機器人同一位姿下采集的，這導致了機器人建圖過程中的激光數(shù)據(jù)存在畸變。圖2所示為機器人獲取一幀激光數(shù)據(jù)的運動過程，在機器人頂端中心位置搭載掃描頻率為5 Hz的激光雷達，即獲取一幀激光數(shù)據(jù)首尾時間差為 200 ms。

圖2 激光雷達運動畸變示意圖

假設機器人以0.5 m/s的速度由A點運動至到B點，與此同時，激光雷達沿逆時針方向旋轉(zhuǎn)獲取一幀激光數(shù)據(jù)，即在A點獲取第0束激光數(shù)據(jù)，在B點獲取第359束激光數(shù)據(jù)，該幀激光數(shù)據(jù)首尾時間差為200 ms。經(jīng)計算可得出A點至B點之間的距離為 10 cm，即獲取該幀激光數(shù)據(jù)的過程中機器人位置發(fā)生了10 cm 的移動，并非在同一位姿下獲取到了完整的一幀激光數(shù)據(jù)。所以當使用低幀率的激光雷達獲取環(huán)境信息時，激光雷達運動畸變是一個不能忽略的問題。

1.2.3 去除激光雷達運動畸變

為了解決低幀率激光雷達存在的運動畸變問題，本研究使用較高更新頻率里程計信息反映激光數(shù)據(jù)采集過程中機器人的運動狀況。設置激光雷達掃描頻率為5 Hz，里程計信息更新頻率大于激光雷達掃描頻率，為20 Hz，確?？梢詾槊恳粋€激光束都匹配到較為精確的機器人位姿，基于傳統(tǒng)Gmapping算法，使用輪式里程計輔助解決激光雷達運動畸變的問題。在激光雷達掃描到的一幀數(shù)據(jù)中，為每一個激光束的原點都匹配一個近似的里程計位姿，并認為該激光束是在該里程計位姿上采集到的，再進行進一步的坐標變換，把所有激光點數(shù)據(jù)都變換到第一束激光原點的坐標系基準坐標系下，也即基準坐標系，最后重新進行數(shù)據(jù)封裝，從而減小運動畸變，具體流程如圖3所示。

具體步驟如下：

1）激光雷達與里程計數(shù)據(jù)時間同步

在CPU獲取激光雷達數(shù)據(jù)的同時，單片機上傳里程計數(shù)據(jù)，對激光雷達數(shù)據(jù)和里程計數(shù)據(jù)進行時間同步及數(shù)據(jù)對齊。定義當前幀激光數(shù)據(jù)的起始和結(jié)束時間分別為t、t，為了確?？梢詾樵搸す鈹?shù)據(jù)的每個激光點都匹配到里程計信息，將里程計數(shù)據(jù)按照時間順序存儲在一個隊列中，且該隊列中最早的里程計數(shù)據(jù)時間戳小于t，最晚的里程計數(shù)據(jù)時間戳大于t。

2）線性插值

為了獲得每個激光點對應的里程計位姿，對機器人運動的時間段進行線性近似與線性插值。首先求解t時刻的里程計信息p：

式中LinarInterp()表示線性插值方法；t至t時間段為線性近似得到的分段，且t<t<t；p、p分別為機器人在t、t時刻的位姿。同理可求得p。再對p至p的里程計隊列中進行線性插值，得到個位姿{p,p+1, …,p+k–2,p}，在個位姿之間進行線性插值，得到每一個激光點時間戳對應的里程計信息，記為{1,2, …,p–1,p}[20]。

3）坐標變換

在得到每一個激光點時間戳對應的位姿信息后，將該幀激光數(shù)據(jù)中每個激光點的坐標進行坐標轉(zhuǎn)換：

式中z為第個激光點去除運動畸變前的坐標；p為對里程計信息進行線性插值得到的第個激光點對應的機器人位姿；′為經(jīng)過坐標變換后去除運動畸變的第個激光點坐標，記為

（4）

4）數(shù)據(jù)封裝

在得到去除運動畸變后的激光點坐標后，對激光數(shù)據(jù)重新封裝后進行發(fā)布：

式中 (e,e)為去除運動畸變后第個激光點的坐標；為第個激光點距離該激光束原點的距離，即該激光束的長度，m；為該激光束的角度，rad。

至此，完成第個激光點的運動畸變?nèi)コ?，同理可去除一幀激光?shù)據(jù)中所有激光點的運動畸變。

2 試驗設計

為了分析該研究運動畸變?nèi)コЧ?，基?.1節(jié)中介紹的機器人平臺，運用Gmapping算法及去除運動畸變后的Gmapping算法進行SLAM建圖試驗，通過測量建圖精度分析運動畸變?nèi)コЧ?。試驗?021年3月在中國農(nóng)業(yè)大學上莊實驗站玉米田及廣西亞熱帶農(nóng)科新城香蕉園中進行，試驗場景如圖4所示。

為了對比去除運動畸變前后的建圖效果，將兩種算法的參數(shù)設置保持一致，包括設置地圖更新頻率為5 Hz，傳感器探測最大范圍為 5 m，最終生成分辨率為0.025 dpi的二維柵格地圖[21-23]。建圖具體操作流程如下：

1）啟動ROS節(jié)點管理器；

2）啟動底盤驅(qū)動節(jié)點、里程計節(jié)點、激光雷達節(jié)點、遙控節(jié)點；

3）啟動建圖程序的可執(zhí)行文件，訂閱傳感器數(shù)據(jù)話題開始構(gòu)建地圖；

4）使用遙控器控制機器人移動，通過可視化工具Rviz實時監(jiān)測機器人建圖情況；

5）地圖構(gòu)建完成后進行保存。

2.1 玉米田試驗方案

建圖區(qū)域為5行玉米作物行，總長約12 m，總寬約6 m，遙控機器人在玉米作物行之間進行移動完成整個作物行區(qū)域的環(huán)境建圖。試驗過程中，在農(nóng)田中放置若干個標記物，首先對標記物之間的距離進行人工測距，獲取距離真值，將地圖測量的距離值與距離真值進行比較以檢驗建圖精度。為了降低人工測量誤差對試驗結(jié)果的影響，本次試驗選取多個采樣點，將長度相似的采樣點作為一組，選擇8個0.6 m左右的距離作為第1組采樣點，10個1.5 m左右的距離作為第2組采樣點，5個12 m左右的距離作為第3組采樣點。分別使用兩種算法進行10次建圖，獲得地圖后對每組采樣點進行測距，取平均值后與距離真值進行比較。

2.2 香蕉園試驗方案

香蕉園中選擇的建圖區(qū)域長約25 m，寬約4 m。選擇3個不同長度的距離作為采樣點，長度分別為4.23、8.42和24.43 m。使用兩種算法在香蕉園中進行10次建圖，獲取地圖后對3個采樣點進行測距并與人工測量真值進行對比。

3 結(jié)果與分析

3.1 玉米田試驗結(jié)果與分析

玉米田部分建圖效果如圖5所示，由于人工種植的玉米田為非結(jié)構(gòu)化環(huán)境，所以去除運動畸變前后的Gmapping算法在玉米田的直觀建圖效果上并無較大差異。本研究通過3組采樣點的測量結(jié)果分析去除運動畸變前后的Gmapping算法的精度，測量結(jié)果如圖6所示。

由結(jié)果分析可知，在玉米田構(gòu)建地圖試驗中，去除運動畸變后的Gmapping算法相較于傳統(tǒng)Gmapping算法在0.6和1.5 m左右的距離測量精度上有小幅度的提升，平均絕對誤差均由0.02 m降低至0.01 m，在0.6 m左右的距離平均建圖精度由97.0%提升至99.1%，在1.5 m左右的距離平均建圖精度由98.3%提升至99.2%；在12 m左右的距離地圖測量平均絕對誤差由 0.06 m降低至0.01 m，平均建圖精度由99.1%提升至99.5%。去除運動畸變前后的Gmapping算法3種距離的建圖精度差異不大，但是隨著建圖距離的增加，Gmapping算法的累積誤差不斷增大，而去除運動畸變后的Gmapping算法的累積誤差均為0.01 m。由此可見，去除運動畸變后的Gmapping算法在玉米田中建圖累積誤差較小，建圖精度較高。

3.2 香蕉園試驗結(jié)果與分析

去除運動畸變前后的香蕉園建圖效果如圖7所示，由于香蕉園中障礙物形態(tài)多變且復雜，直觀建圖效果并無較大差異。3個采樣點地圖測距與人工測量真值對比分析結(jié)果如圖8及表1所示。

由試驗結(jié)果可知，去除運動畸變后的Gmapping算法相比于改進前的Gmapping算法，在4.23 m的香蕉園作物行區(qū)域平均絕對誤差由0.08 m降低至0.06 m，平均建圖精度由98.1%提升至99.0%；在8.42 m的區(qū)域平均絕對誤差由0.13 m降低至0.06 m，平均建圖誤差由97.8%提升至98.5%；在24.43 m的區(qū)域內(nèi)平均絕對誤差由0.46 m降低至0.07 m，降低了0.39 m，平均建圖誤差由98.1%提升至99.1%。去除運動畸變后Gmapping算法的建圖誤差相比于Gmapping算法均有小幅度的提升，且Gmapping算法的累積誤差隨著建圖區(qū)域長度的增加不斷增大，而去除運動畸變后的Gmapping算法建圖累積誤差均在0.07 m左右，有較高的建圖精度。

圖7 部分香蕉園建圖結(jié)果

表1 香蕉園建圖總體誤差情況

綜合玉米田間和香蕉園中的建圖試驗結(jié)果可以看出，去除運動畸變Gmapping算法相較于傳統(tǒng)Gmapping算法在較長區(qū)域內(nèi)建圖效果有明顯的提升，累積誤差較小，使用低幀率激光雷達可以得到精度較高的環(huán)境地圖，基本滿足機器人在農(nóng)業(yè)環(huán)境中的環(huán)境地圖構(gòu)建需求。

4 討 論

1）本文利用輪式移動機器人在玉米田進行了SLAM建圖試驗，試驗結(jié)果表明，在玉米田中建圖平均絕對誤差均為0.01 m；在香蕉園中建圖平均絕對誤差在0.07 m左右，均有較高的建圖精度。李輝[24]在機器人上搭載二維激光雷達使用Hector_slam算法在距離小于3 m的室內(nèi)環(huán)境中建圖誤差為0.01 m，在距離30 m以內(nèi)的室內(nèi)環(huán)境中建圖誤差平均為0.05 m；吳成鼎[25]在機器人上搭載二維激光雷達使用Cartorgrapher算法在長為8.54 m、寬為3.97 m的室內(nèi)環(huán)境中建圖誤差為0.07 m。由此可見，本研究算法在農(nóng)業(yè)環(huán)境中的建圖精度不低于Hector_slam及Cartorgrapher算法在室內(nèi)環(huán)境中的建圖精度，但Hector_slam算法需要高幀率的激光雷達完成環(huán)境建圖，Cartorgrapher算法計算量大，對內(nèi)存資源要求較高。相比之下，本研究算法可基于低成本的機器人平臺在農(nóng)田環(huán)境下完成高精度的環(huán)境建圖。

2）本研究里程計信息發(fā)布頻率為20 Hz，激光雷達掃描頻率為5 Hz，基本可以滿足為每一個激光束都匹配到較為精確位姿的需求。里程計信息發(fā)布的頻率決定著激光束匹配位姿的精確程度，進而也會影響SLAM建圖精度，為了驗證不同頻率的里程計信息發(fā)布頻率對去除運動畸變效果的影響，下一步將進行多組對照試驗進行分析。

3）在農(nóng)田等地面不平整的環(huán)境中，里程計信息存在一定誤差，對建圖結(jié)果產(chǎn)生影響。在試驗前使用電子羅盤或慣性測量單元對里程計信息進行校正，可以得到更加精確的里程計數(shù)據(jù)及環(huán)境地圖，進而提升環(huán)境建圖的精度。

5 結(jié) 論

1）本文從概率的角度對SLAM問題進行分析，并對使用低幀率激光雷達進行地圖構(gòu)建存在的運動畸變問題進行了討論，分析得出使用高頻率的里程計信息進行輔助可以有效解決低幀率激光雷達存在的運動畸變問題。

2）針對低幀率激光雷達的運動畸變問題，在Gmapping算法的基礎上融合里程計信息，為每一幀激光數(shù)據(jù)匹配到近似的機器人位姿，進而實現(xiàn)更加精確環(huán)境地圖構(gòu)建。

3）試驗結(jié)果表明，在長度為12 m左右的玉米田區(qū)域，本研究算法建圖平均誤差為0.01 m，相比于Gmapping算法降低了0.05 m，建圖精度為99.5%；在長度為24.43 m的香蕉園區(qū)域，本研究算法建圖平均絕對誤差為0.07 m，相比于Gmapping算法降低了0.39 m，建圖精度為99.1%，表明本研究算法較為有效地解決了低幀率激光雷達存在的運動畸變問題，可基本滿足機器人在農(nóng)業(yè)環(huán)境中的環(huán)境建圖需求。

[1] 季宇寒，李寒，張漫，等. 基于激光雷達的巡檢機器人導航系統(tǒng)研究[J]. 農(nóng)業(yè)機械學報，2018，49(2)：14-21.

Ji Yuhan, Li Han, Zhang Man, et al. Navigation system for inspection robot based on lidar[J]. Transactions of the Chinese Society for Agricultural Machinery, 2018, 49(2): 14-21. (in Chinese with English abstract)

[2] 李道亮，包建華. 水產(chǎn)養(yǎng)殖水下作業(yè)機器人關(guān)鍵技術(shù)研究進展[J]. 農(nóng)業(yè)工程學報，2018，34(16)：1-9.

Li Daoliang, Bao Jianhua. Research progress on key technologies of underwater robot for aquaculture[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2018, 34(16): 1-9. (in Chinese with English abstract)

[3] 陳劭，郭宇翔，高天嘯，等. 移動機器人RGB-D視覺SLAM算法[J]. 農(nóng)業(yè)機械學報，2018，49(10)：38-45.

Chen Shao, Guo Yuxiang, Gao Tianxiao, et al. RGB-D visual SLAM algorithm for mobile robots[J]. Transactions of the Chinese Society for Agricultural Machinery, 2018, 49(10): 38-45. (in Chinese with English abstract)

[4] Fang B, Mei G, Yuan X, et al. Visual SLAM for robot navigation in healthcare facility[J]. Pattern Recognition, 2021, 113(12): 107822.

[5] 李晨曦，張軍，靳欣宇，等. 激光雷達SLAM技術(shù)及其在無人車中的應用研究進展[J]. 北京聯(lián)合大學學報，2017，31(4)：61-69.

Li Chenxi, Zhang Jun, Jin Xinyu, et al. Research progress of lidar SLAM technology and its application in unmanned vehicles[J]. Journal of Beijing Union University, 2017, 31(4): 61-69. (in Chinese with English abstract)

[6] Shi Y, Wang H, Yang T, et al. Integrated navigation by a greenhouse robot based on an odometer/lidar[J]. Instrumentation Mesure Metrologie, 2020, 19(2): 91-101.

[7] 孫宗濤，朱永強. 2D激光slam算法在室內(nèi)建圖對比研究[J]. 內(nèi)燃機與配件，2020(1)：15-16.

Sun Zongtao, Zhu Yongqiang. Comparison of 2D laser slam algorithms for indoor map construction[J]. Internal Combustion Engine and Accessories, 2020(1): 15-16. (in Chinese with English abstract)

[8] 黃瑞，張軼. 高適應性激光雷達SLAM[J]. 電子科技大學學報，2021，50(1)：52-58.

Huang Rui, Zhang Yi. High adaptive lidar SLAM[J]. Journal of University of Electronic Science and Technology, 2021, 50(1): 52-58. (in Chinese with English abstract)

[9] 孟蕾，王臣臣，李康. 基于蛇形機器人的SLAM技術(shù)研究[J]. 制造業(yè)自動化，2020，42(7)：59-61，73.

Meng Lei, Wang Chenchen, Li Kang. Research on SLAM technology based on snake-like robot[J]. Manufacturing Automation, 2020, 42(7): 59-61, 73. (in Chinese with English abstract)

[10] Jun J H, Moon C B. Development of a practical ICP outlier rejection scheme for graph-based SLAM using a laser range finder[J]. International Journal of Precision Engineering and Manufacturing, 2019, 20(10): 1735-1745.

[11] Shi X, Peng J, Li J, et al. The iterative closest point registration algorithm based on the normal distribution transformation[J]. Procedia Computer Science, 2019, 147: 181-190.

[12] 王雅儀，余萌，朱鋒. 線面特征結(jié)合的三維激光SLAM算法及性能評估[J]. 導航定位學報，2021，9(1)：122-129.

Wang Yayi, Yu Meng, Zhu Feng. Three-dimensional laser SLAM algorithm and performance evaluation based on line and surface features[J]. Journal of Navigation and Positioning, 2021, 9(1): 122-129. (in Chinese with English abstract)

[13] Hong S, Ko H, Kim J. VICP: Velocity updating iterative closest point algorithm[C]. IEEE International Conference on Robotics & Automation. IEEE, 2012.

[14] Yan L, Dai J, Tan J, et al. Global fine registration of point cloud in lidar SLAM based on pose graph[J]. Journal of Geodesy and Geoinformation Science, 2019，48(3): 313-321.

[15] 趙新洋. 基于激光雷達的同時定位與室內(nèi)地圖構(gòu)建算法研究[D]. 哈爾濱：哈爾濱工業(yè)大學，2017.

Zhao Xinyang. Research on Simultaneous Localization and Indoor Map Construction Algorithm Based on Lidar[D]. Harbin: Harbin Institute of Technology, 2017. (in Chinese with English abstract)

[16] 李昊. 基于激光雷達的二維即時定位與制圖技術(shù)研究[D]. 南京：東南大學，2018.

Li Hao. Research on Two-Dimensional Real-time Positioning and Mapping Technology Based on Lidar[D]. Nanjing: Southeast University, 2018. (in Chinese with English abstract)

[17] 陳衛(wèi)東，張飛．移動機器人的同步自定位與地圖創(chuàng)建研究進展[J]. 控制理論與應用，2005，22(3)：455-460.

Chen Weidong, Zhang Fei. Research progress on synchronous self-localization and map creation of mobile robots[J]. Control Theory and Application, 2005, 22(3): 455-460. (in Chinese with English abstract)

[18] Guo L, Wang H, Meng Q, et al. FastSLAM and its data association approaches for mobile robots[C]// IEEE International Conference on Automation & Logistics. IEEE, 2007.

[19] Christopher C, Alexander L. FastSLAM using compressed occupancy grids[J]. Journal of Sensors, 2016 (3): 1-23.

[20] Bezet O, Cherfaoui V. Time error correction for laser range scanner data[C]. International Conference on Information Fusion. IEEE, 2007.

[21] Zhang X, Lai J, Xu D, et al. 2D lidar-based SLAM and path planning for indoor rescue using mobile robots[J]. Journal of Advanced Transportation, 2020(3): 1-14.

[22] 羅元，余佳航，汪龍峰，等. 改進RBPF的移動機器人同步定位與地圖構(gòu)建[J]. 智能系統(tǒng)學報，2015(3)：460-464.

Luo Yuan, Yu Jiahang, Wang Longfeng, et al. Improved RBPF mobile robot synchronous positioning and map construction[J]. Journal of Intelligent System, 2015(3): 460-464. (in Chinese with English abstract)

[23] Peng W, Chen Z, Zhang Q, et al. A loop closure improvement method of Gmapping for low cost and resolution laser scanner[C]. 8th IFAC Conference on Manufacturing Modelling, Management and Control, 2016.

[24] 李輝. 基于激光雷達的2D-SLAM的研究[D]. 杭州：浙江工業(yè)大學，2017.

Li Hui. Research on 2D-SLAM Based on Lidar[D]. Hangzhou: Zhejiang University of Technology, 2017. (in Chinese with English abstract)

[25] 吳成鼎. 基于Cartographer的室內(nèi)SLAM系統(tǒng)研究與實現(xiàn)[D]. 福州：福州大學，2018.

Wu Chengding. Research and Implementation of Indoor SLAM System Based on Cartographer[D]. Fuzhou: Fuzhou University, 2018. (in Chinese with English abstract)

Positioning and map construction for agricultural robots integrating odometer information

Li Chenyang1, Peng Cheng2, Zhang Zhenqian1, Miao Yanlong1, Zhang Man1, Li Han2※

(1.100083;2.100083)

A relatively low-cost and low frame rate lidar can be very popular to serve as the main sensor of Simultaneous Localization and Mapping (SLAM) in the mainstream agricultural robots at present. The lidar can scan the environment, while the pose of the robot can also change in the SLAM process. However, motion distortion and mismatching can often occur for the environment mapping, because one frame of lidar data can be obtained under the same pose of the robot by default. The resulting errors in the SLAM map can directly determine the accuracy of the automatic navigation of the robot. In this study, a commonly-used classical SLAM Gmapping was utilized to integrate the odometer information, to reduce the motion distortion of lidar with a low frame rate. The displacement and angle of the robot were directly measured with high accuracy of local position using an odometer, one type of important pose sensor in robots. An approximate odometer pose was also matched for each laser point in a frame of data that was scanned by lidar, according to the odometer information of high frequency. Among them, the odometer information was considered to be the collected laser points, thereby obtaining their coordinates after removing the motion distortion. Finally, the data was re-encapsulated to reduce the motion distortion of low frame rate lidar data on the map construction in this frame. A SLAM mapping test was also carried out to verify the improved Gmapping in a maize field and banana garden using a robot equipped with RPLIDAR A1 lidar with a scanning frequency of 5 Hz. The experimental results showed that the average absolute error of the improved Gmapping was 0.01 m in the maize field with a length of about 12 m, 0.05 m lower than that of the original one (0.06 m). In the banana garden area with a length of 24.43 m, the average absolute error of the improved Gmapping was 0.07 m, 0.39 m lower than that of the original one (0.46 m). Consequently, the mapping accuracy of the improved Gmapping was higher than before in the agricultural environment, indicating that the motion distortion of lidar with a low frame rate can be removed effectively.

robots; sensors; mapping; motion estimation; data fusion

10.11975/j.issn.1002-6819.2021.21.003

S24

A

1002-6819(2021)-21-0016-08

李晨陽，彭程，張振乾，等. 融合里程計信息的農(nóng)業(yè)機器人定位與地圖構(gòu)建方法[J]. 農(nóng)業(yè)工程學報，2021，37(21)：16-23.doi：10.11975/j.issn.1002-6819.2021.21.003 http://www.tcsae.org

Li Chenyang, Peng Cheng, Zhang Zhenqian, et al. Positioning and map construction for agricultural robots integrating odometer information[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2021, 37(21): 16-23. (in Chinese with English abstract) doi：10.11975/j.issn.1002-6819.2021.21.003 http://www.tcsae.org

2021-07-21

2021-10-23

國家自然科學基金項目（32171893）；中國農(nóng)業(yè)大學建設世界一流大學（學科）和特色發(fā)展引導專項資金（2021AC006）；中國農(nóng)業(yè)大學 2115 人才工程資助項目

李晨陽，研究方向為農(nóng)業(yè)機器人導航技術(shù)。Email：txlichenyang@163.com

李寒，博士，副教授，博士生導師，研究方向為農(nóng)業(yè)電氣化與自動化。Email：cau_lihan@cau.edu.cn