李晨陽(yáng)，彭 程，張振乾，苗艷龍，張 漫，李 寒

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

李晨陽(yáng)1，彭 程2，張振乾1，苗艷龍1，張 漫1，李 寒2※

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

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

機(jī)器人；傳感器；建圖；運(yùn)動(dòng)估計(jì)；信息融合

0 引 言

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

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

1 材料與方法

1.1 研究平臺(tái)

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

圖 1 移動(dòng)機(jī)器人平臺(tái)

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

1.2 研究方法

1.2.1 SLAM問(wèn)題概率描述

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

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

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

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

1.2.2 激光雷達(dá)運(yùn)動(dòng)畸變分析

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

圖2 激光雷達(dá)運(yùn)動(dòng)畸變示意圖

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

1.2.3 去除激光雷達(dá)運(yùn)動(dòng)畸變

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

具體步驟如下：

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

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

2）線(xiàn)性插值

為了獲得每個(gè)激光點(diǎn)對(duì)應(yīng)的里程計(jì)位姿，對(duì)機(jī)器人運(yùn)動(dòng)的時(shí)間段進(jìn)行線(xiàn)性近似與線(xiàn)性插值。首先求解t時(shí)刻的里程計(jì)信息p：

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

3）坐標(biāo)變換

在得到每一個(gè)激光點(diǎn)時(shí)間戳對(duì)應(yīng)的位姿信息后，將該幀激光數(shù)據(jù)中每個(gè)激光點(diǎn)的坐標(biāo)進(jìn)行坐標(biāo)轉(zhuǎn)換：

式中z為第個(gè)激光點(diǎn)去除運(yùn)動(dòng)畸變前的坐標(biāo)；p為對(duì)里程計(jì)信息進(jìn)行線(xiàn)性插值得到的第個(gè)激光點(diǎn)對(duì)應(yīng)的機(jī)器人位姿；′為經(jīng)過(guò)坐標(biāo)變換后去除運(yùn)動(dòng)畸變的第個(gè)激光點(diǎn)坐標(biāo)，記為

（4）

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

在得到去除運(yùn)動(dòng)畸變后的激光點(diǎn)坐標(biāo)后，對(duì)激光數(shù)據(jù)重新封裝后進(jìn)行發(fā)布：

式中 (e,e)為去除運(yùn)動(dòng)畸變后第個(gè)激光點(diǎn)的坐標(biāo)；為第個(gè)激光點(diǎn)距離該激光束原點(diǎn)的距離，即該激光束的長(zhǎng)度，m；為該激光束的角度，rad。

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

2 試驗(yàn)設(shè)計(jì)

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

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

1）啟動(dòng)ROS節(jié)點(diǎn)管理器；

2）啟動(dòng)底盤(pán)驅(qū)動(dòng)節(jié)點(diǎn)、里程計(jì)節(jié)點(diǎn)、激光雷達(dá)節(jié)點(diǎn)、遙控節(jié)點(diǎn)；

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

4）使用遙控器控制機(jī)器人移動(dòng)，通過(guò)可視化工具Rviz實(shí)時(shí)監(jiān)測(cè)機(jī)器人建圖情況；

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

2.1 玉米田試驗(yàn)方案

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

2.2 香蕉園試驗(yàn)方案

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

3 結(jié)果與分析

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

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

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

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

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

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

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

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

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

4 討 論

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

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

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

5 結(jié) 論

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

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

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

[1] 季宇寒，李寒，張漫，等. 基于激光雷達(dá)的巡檢機(jī)器人導(dǎo)航系統(tǒng)研究[J]. 農(nóng)業(yè)機(jī)械學(xué)報(bào)，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è)機(jī)器人關(guān)鍵技術(shù)研究進(jìn)展[J]. 農(nóng)業(yè)工程學(xué)報(bào)，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] 陳?ài)?，郭宇翔，高天嘯，等. 移動(dòng)機(jī)器人RGB-D視覺(jué)SLAM算法[J]. 農(nóng)業(yè)機(jī)械學(xué)報(bào)，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] 李晨曦，張軍，靳欣宇，等. 激光雷達(dá)SLAM技術(shù)及其在無(wú)人車(chē)中的應(yīng)用研究進(jìn)展[J]. 北京聯(lián)合大學(xué)學(xué)報(bào)，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] 孫宗濤，朱永強(qiáng). 2D激光slam算法在室內(nèi)建圖對(duì)比研究[J]. 內(nèi)燃機(jī)與配件，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] 黃瑞，張軼. 高適應(yīng)性激光雷達(dá)SLAM[J]. 電子科技大學(xué)學(xué)報(bào)，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] 孟蕾，王臣臣，李康. 基于蛇形機(jī)器人的SLAM技術(shù)研究[J]. 制造業(yè)自動(dòng)化，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] 王雅儀，余萌，朱鋒. 線(xiàn)面特征結(jié)合的三維激光SLAM算法及性能評(píng)估[J]. 導(dǎo)航定位學(xué)報(bào)，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] 趙新洋. 基于激光雷達(dá)的同時(shí)定位與室內(nèi)地圖構(gòu)建算法研究[D]. 哈爾濱：哈爾濱工業(yè)大學(xué)，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] 李昊. 基于激光雷達(dá)的二維即時(shí)定位與制圖技術(shù)研究[D]. 南京：東南大學(xué)，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)東，張飛．移動(dòng)機(jī)器人的同步自定位與地圖創(chuàng)建研究進(jìn)展[J]. 控制理論與應(yīng)用，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] 羅元，余佳航，汪龍峰，等. 改進(jìn)RBPF的移動(dòng)機(jī)器人同步定位與地圖構(gòu)建[J]. 智能系統(tǒng)學(xué)報(bào)，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] 李輝. 基于激光雷達(dá)的2D-SLAM的研究[D]. 杭州：浙江工業(yè)大學(xué)，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)研究與實(shí)現(xiàn)[D]. 福州：福州大學(xué)，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

李晨陽(yáng)，彭程，張振乾，等. 融合里程計(jì)信息的農(nóng)業(yè)機(jī)器人定位與地圖構(gòu)建方法[J]. 農(nóng)業(yè)工程學(xué)報(bào)，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

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

李晨陽(yáng)，研究方向?yàn)檗r(nóng)業(yè)機(jī)器人導(dǎo)航技術(shù)。Email：txlichenyang@163.com

李寒，博士，副教授，博士生導(dǎo)師，研究方向?yàn)檗r(nóng)業(yè)電氣化與自動(dòng)化。Email：cau_lihan@cau.edu.cn