工程设计学报, 2023, 30(6): 678-686 doi: 10.3785/j.issn.1006-754X.2024.03.161

机器人与机构设计

基于优化Hector-SLAM算法的机器人自主导航系统设计

汪建华,,, 黄磊,,, 石雨婷, 张晓倩, 祁良剑

南京林业大学 机械电子工程学院,江苏 南京 210037

Design of robot autonomous navigation system based on optimized Hector-SLAM algorithm

WANG Jianhua,,, HUANG Lei,,, SHI Yuting, ZHANG Xiaoqian, QI Liangjian

School of Mechanical and Electronic Engineering, Nanjing Forestry University, Nanjing 210037, China

通讯作者: 黄 磊(1975—),男,安徽合肥人,副教授,博士,从事三维点云处理算法、激光SLAM、惯性导航与组合导航算法以及嵌入式系统开发等研究,E-mail: huanglei@njfu.edu.cn,https://orcid.org/0000-0001-7307-6311

收稿日期: 2023-05-05   修回日期: 2023-06-25  

基金资助: 国家自然科学基金资助项目.  31901239
南京林业大学2023年大学生创新训练计划项目.  2023NFUSPITP0066

Received: 2023-05-05   Revised: 2023-06-25  

作者简介 About authors

汪建华(1996—),男,河南商丘人,硕士生,从事机器人自主导航和激光SLAM技术研究,E-mail:18816212079@163.com,https://orcid.org/0009-0005-7071-9285 , E-mail:18816212079@163.com

摘要

针对机器人在复杂地形环境下定位不准确、建图精度不高和误差堆积等问题,设计了一种基于优化Hector-SLAM(simultaneous localization and mapping,实时定位与建图)算法的自主导航系统,以实现机器人在及时躲避障碍物的同时准确到达目标点。首先,采用双三次插值法代替原有插值法,以解决Hector-SLAM算法在使用低精度激光雷达数据建图时易出现地图不清晰等问题,从而提升扫描匹配的精准度。其次,采用扩展卡尔曼滤波算法对里程计和惯性测量单元的数据进行融合,以提高定位的准确性。再次,针对激光点数据无法瞬时获得而机器人持续运动所导致的运动畸变问题,结合里程计辅助法与PL-ICP(point to line iterative closest points,点到线迭代最近点)配准法,以实现运动畸变的校正;同时,设置倾斜角阈值以消除地图重影,并利用A*算法和动态窗口法规划最优路径。最后,以AGV(automated guided vehicle,自主导引车)为例,在实际场景中开展建图实验和自主导航实验。结果表明,优化后自主导航系统的平均建图相对误差约为0.44%,最小建图误差为0.236 m,较优化前减小了0.041 m,有效地解决了因误差堆积和运动畸变而导致的建图不清晰问题,增强了AGV在复杂地形环境中的适应能力,实现了高精度定位。研究结果对提高移动机器人在室内多障碍物环境下的自主导航能力具有一定的理论和工程意义。

关键词: Hector-SLAM(实时定位与建图) ; 双三次插值法 ; 运动畸变 ; 消除重影 ; 自主导航

Abstract

Aiming at the problems of inaccurate positioning, low mapping accuracy and error accumulation of robots in complex terrain environments, an autonomous navigation system based on optimized Hector-SLAM (simultaneous localization and mapping) algorithm is designed, which ensures that the robot reaches the target point accurately while avoiding obstacles promptly. Firstly, the bicubic interpolation method was used to replace the original interpolation method to solve the problem of map blurring when the Hector-SLAM algorithm was used to build maps with low-precision lidar data, so as to improve the accuracy of scan matching. Secondly, the extended Kalman filter algorithm was used to fuse the data of odometer and inertial measurement unit, which could improve the positioning accuracy. Thirdly, in view of the problem of motion distortion caused by the inability to obtain instantaneous laser point data and the continuous movement of robot, the odometer auxiliary method and PL-ICP (point to line iterative closest points) registration method were combined to realize the correction of motion distortion. At the same time, the tilt angle threshold was set to eliminate the map ghosting, and the optimal path was planned by using A* algorithm and dynamic window approach. Finally, taking AGV (automated guided vehicle) as an example, the mapping experiments and autonomous navigation experiments were carried out in actual scenarios. The results showed that the average mapping relative error of the optimized autonomous navigation system was about 0.44%, and the minimum mapping error was 0.236 m, which was 0.041 m less than that before optimization. It effectively solved the problem of unclear mapping caused by error accumulation and motion distortion, and enhanced the adaptability of AGV in complex terrain environment, so as to achieve high-precision positioning. The research results have certain theoretical and engineering significance for improving the autonomous navigation ability of mobile robots in indoor multi-obstacle environment.

Keywords: Hector-SLAM (simultaneous localization and mapping) ; bicubic interpolation method ; motion distortion ; ghosting elimination ; autonomous navigation

PDF (2511KB) 元数据 多维度评价 相关文章 导出 EndNote| Ris| Bibtex  收藏本文

本文引用格式

汪建华, 黄磊, 石雨婷, 张晓倩, 祁良剑. 基于优化Hector-SLAM算法的机器人自主导航系统设计. 工程设计学报[J], 2023, 30(6): 678-686 doi:10.3785/j.issn.1006-754X.2024.03.161

WANG Jianhua, HUANG Lei, SHI Yuting, ZHANG Xiaoqian, QI Liangjian. Design of robot autonomous navigation system based on optimized Hector-SLAM algorithm. Chinese Journal of Engineering Design[J], 2023, 30(6): 678-686 doi:10.3785/j.issn.1006-754X.2024.03.161

自主移动机器人必须具备自主导航和路径规划能力,而构建环境地图是实现路径规划的前提[1-5]。SLAM(simultaneous localization and mapping,实时定位与建图)是机器人自主导航中的一项关键技术[6-7],其是指机器人在未知环境中移动时,利用传感器定位自身位姿,并基于位姿增量式构建地图。近年来,随着激光雷达和中央处理器的性能不断提升,学者们通过不断改进和优化SLAM算法来满足这些设备的性能需求。支奕琛等[8]提出了一种基于ZBar算法和融合Cartographer-SLAM与Hector-SLAM算法的方法,以实现对二维码目标点的识别和标记。该方法先采用ZBar算法识别机器人的二维码信息,再采用Cartographer算法生成Geotiff地图,最后采用Hector-SLAM算法在地图上绘制目标点。经实验验证,该方法能识别远距离目标点的信息且精度较高,但对目标点的预估定位存在误差。Guan等[9]提出了一种集成Gmapping建议分布与Kullback-Leibler散度的粒子数适应算法。经实验验证,该算法能够采用较少的粒子实现较高的定位精度,但是其对单个粒子的计算负载较大,所耗费的计算时间较长。苏易衡等[10]提出了一种适用于低端激光雷达的优化Hector-SLAM算法:先采用插值法获取高精度的栅格地图,再通过图像相减与矩阵变换来减少系统对激光雷达刷新率的依赖,最后对SLAM中的关键帧判定机制进行优化,以消除旋转时所获取数据的干扰。经实验验证,该算法在搭载低精度激光雷达条件下也能构建清晰的地图。周燕[11]提出了一种利用卡尔曼滤波算法将Hector-SLAM与人工地标视觉定位结果相融合的组合定位算法。该算法可抑制Hector-SLAM所导致的累积误差,削弱人工地标视觉定位引发的跳变异常,提高了定位精度。

综上所述,二维SLAM算法已被广泛应用于实际场景。目前,常用的激光SLAM算法包括Gmapping、Karto-SLAM、Cartographer和Hector-SLAM等。其中,Hector-SLAM是一种基于高斯-牛顿(Gauss-Newton)扫描匹配的算法[12],该算法在构建地图时不需要里程计数据,更适用于无人机以及行驶在不平坦地面上的车辆的定位和建图,但其对激光雷达的精度和刷新率的要求较高。例如:美国Innovusion公司研制的猎鹰(Falcon)激光雷达,其探测距离高达500 m,测距精度为±2 cm,角分辨率为0.05°×0.05°,每秒发出的激光点云数高达100万个,但成本高昂;上海思岚科技有限公司面向低端消费电子产品推出了非接触式激光扫描测距雷达(型号为RPLIDAR-A1),其探测距离为0.15~12 m,最高测距精度为±3 cm,在5~12 m时的测距精度为实际距离的2.5%,角分辨率为1°,每秒发出的激光点云数仅为8 000个。显然,RPLIDAR-A1激光扫描测距雷达的精度远远不及猎鹰激光雷达,但胜在价格低廉。一般情况下,激光雷达的精度越高,价格越昂贵,在实际应用中会受到限制。但Hector-SLAM算法在使用低精度激光雷达数据建图时会产生大量累积误差,在导航定位时易引发故障。针对上述问题,笔者拟作如下改进:1)优化Hector-SLAM算法中的插值函数,即采用双三次插值法构建高精度的栅格地图,以提高扫描匹配的精准度;2)采用扩展卡尔曼滤波算法对里程计和惯性测量单元(inertial measurement unit, IMU)的数据进行融合,通过组合定位方法来降低漂移效应引起的累积误差,以提高定位的准确性;3)提出里程计辅助法与PL-ICP(point to line iterative closest points,点到线迭代最近点)配准法相融合的方法,以实现运动估计过程的快速收敛,从而达到校正运动畸变的目的;4)设置IMU的倾斜角阈值,以消除机器人旋转时出现的地图重影现象。在此基础上,利用优化的Hector-SLAM算法设计一种低成本、高性能的自主导航系统,并以AGV(automated guided vehicle,自动导引车)为例开展建图实验和自主导航实验,以验证系统的可行性。

1 优化的Hector-SLAM算法

传统Hector-SLAM算法的建图过程分为3个阶段[13]:占据栅格地图、扫描匹配和多分辨率地图映射。具体流程如下:首先,利用激光雷达获取环境信息,并采用双线性插值法对离散数据进行处理,以得到连续的栅格地图;然后,将激光雷达获取的实时数据与已知地图进行对齐匹配,构造误差函数,并采用高斯-牛顿法求解最优位姿与偏差;最后,采用多种分辨率不同的栅格地图对激光点云进行映射,以避免匹配结果陷入局部最小值。

1.1 栅格地图占据

栅格地图[14-17]可有效反映环境信息,其将外部环境划分为多个尺寸相同的栅格,每一个栅格具有3种状态:空闲、占用或未知。在占据栅格地图时,通过数值0和1来表示栅格单元被障碍物占据的概率,数值0表示没有障碍物,数值1表示出现障碍物。由于Hector-SLAM算法构建的栅格地图是连续的,且扫描匹配也需要连续的数据进行偏导运算,但激光雷达获取的是离散数据,因此须采用插值法对离散数据进行处理。传统的Hector-SLAM算法采用双线性插值法完成栅格地图的构建,但该插值法仅考虑采样点周围4个直接相邻点的像素值变化对采样点的影响,具有低通滤波器的性质。此外,采用双线性插值法计算后,图像会丢失高频分量,导致边缘不清晰。为弥补上述缺陷,本文采用双三次插值法,将采样点所有邻近点的像素值变化的影响均考虑在内。同时,由于低精度激光雷达的扫描频率较低,其获取的可用数据较少,采用双三次插值法进行处理,可降低数据量不足对建图精度的影响,获得梯度和精度更高的栅格地图,从而达到减少系统噪声、降低边缘误差以及提高扫描匹配准确度的目的。

图1所示,双三次插值法通过对采样点周围16个邻近点的像素值进行加权平均处理来获得采样点的像素值[18]。其中:采样点的坐标为x+i, y+j,其中xy为整数部分,ij为小数部分。

图1

图1   双三次插值模型示意

Fig.1   Schematic of bicubic interpolation model


双三次插值的基函数Sx可表示为:

Sx=a+2x3-a+3x2+1,   x1ax2x-5+8ax-4a,       1<x<20,                                                    x2

式中:a为待定系数。

经多次实验后发现,参数a取-0.5时的插值效果最佳。利用双三次插值法进行计算,可得采样点的像素值fx+i, y+j

fx+i, y+j=ABCT

其中:

A=S1+iSiS1-iS2-i
B=fx-1, y-1fx-1, yfx-1, y+1fx-1, y+2fx, y-1fx, yfx, y+1fx, y+2fx+1, y-1fx+1, yfx+1, y+1fx+1, y+2fx+2, y-1fx+2, yfx+2, y+1fx+2, y+2
C=S1+jSjS1-jS2-j

栅格地图中插值函数的改变会导致高斯-牛顿法扫描匹配所使用的地图梯度发生改变。对式(2)求XY方向的偏导,可得:

fiA'BCΤfjABC'Τ

ABC代入式(3)中的第1个方程,化简可得:

fi=3a1+i2-10a1+i+8a2a+2i2-2a+3i-3i+21-i2+2a+31-i-3a2-i2+102-i-8aΤBCT

同理,将式(4)中的i改为j即可求得Y方向的偏导函数。

1.2 扫描匹配

获得栅格地图后,将激光雷达采集的实时数据与已知的栅格地图进行对齐匹配[19]。扫描匹配的原理是利用最小二乘法求解机器人的最优位姿ξ*

ξ*=argξminq=1n1-MSqξ2

其中:

Sqξ=cosψ-sinψsinψcosψSq, xSq, y+PxPy

式中:ξ=Px  Py  ψΤ为机器人在世界坐标系的二维平面XOY内的初始位姿,其中PxPyψ分别为机器人位置的横、纵坐标和旋转角度;MSqξ为障碍概率;Sqξ为机器人在世界坐标系中的位置;Sq, xSq, y为激光点在激光雷达坐标系中的位置坐标。

机器人在当前时刻的最优位姿ξ*与上一时刻初始位姿ξ存在如下关系:ξ+Δξ=ξ*。若要减小激光扫描当前帧与栅格地图对齐过程中的误差,则要确保所有激光点在栅格地图中的总占用偏差尽可能小,即Δξ应满足以下条件:

q=1n1-MSqξ+Δξ20

式(6)在Δξ处泰勒展开并求偏导,可得:

Δξ=H-1q=1nMSqξSqξξΤ1-MSqξ

其中:

Η=MSqξSqξξTMSqξSqξξ

式中:H为Hessian矩阵;MSqξ为地图梯度。

1.3 运动畸变校正

在跟随机器人运动的过程中,激光雷达每一帧数据中的激光点并不是在同一位姿下瞬时获得的,即不同激光点的坐标不一致。但激光雷达在封装每一帧数据时,默认该帧数据中的所有激光点均是在同一时刻、同一位姿下采集得到的,这会导致运动畸变[20]。当激光雷达的扫描频率较低时,激光束之间的误差较大,点云数据会出现“变形”现象。本文所使用的激光雷达的扫描频率为5.5 Hz,每一帧数据的扫描用时约为180 ms。设置机器人的线速度为0.5 m/s,每采集一帧数据后,机器人移动0.09 m。由此可知,本文所搭载的激光雷达在实际运动过程中的运动畸变不可忽略。

在机器人快速移动的过程中,里程计易发生位置漂移以及无法有效检测滑动,而IMU的输出速率较快,测量误差会随时间累积,无法实现长时间的精准定位[21]。为了减小累积误差,采用扩展卡尔曼滤波算法对里程计和IMU的数据进行融合滤波处理。

假设t时刻机器人通过IMU获得加速度at,由此可得t时刻机器人的速度:

vxt=axttvyt=aytt

式中:vxtvytaxtayt分别为t时刻机器人沿XY方向的速度和加速度。

则在t时刻,由IMU获得的机器人的状态向量 X1t 可表示为:

X1t=vxt  vyt  ωtT

式中:ωt为IMU坐标系下机器人的Z向角速度。

t时刻,由里程计获得的机器人的状态向量 X2t 可表示为:

X2t=xt  yt  θtT

式中:xtytt时刻机器人沿XY方向的位移;θtt时刻机器人的旋转角度。

在利用里程计和IMU获取机器人的状态向量后,将基于里程计数据的机器人轨迹推导过程视作系统状态方程的计算过程,以IMU和里程计的输出结果作为更新方程的输入,在扩展卡尔曼滤波算法的基础上进行观测更新,以实现对里程计与IMU的位姿数据的融合。由此可得,t时刻机器人的状态向量 Xt 可表示为:

Xt=xtytθtvxtvytωtT

鉴于机器人所搭载的里程计为双轮差速里程计,在tt+1时刻,机器人的状态转移矩阵可表示为:

Xt+1=Xt+Δtcosθt-Δtsinθt0ΔtsinθtΔtcosθt000Δtvxtvytωt+W0

式中:W0为机器人的状态噪声,服从高斯分布,即W0~N0, Q0,其中 Q0为预测状态下的协方差矩阵。

则机器人的状态观测模型更新如下:

Zt=HtXt+V0

式中:Zt为机器人状态更新矩阵;Ht为机器人的状态观测矩阵;V0为观测噪声,服从高斯分布,即V0~(0, R0),其中R0为观测噪声的协方差矩阵。

在此基础上,按照扩展卡尔曼滤波融合流程,计算卡尔曼增益并更新机器人状态及对应的协方差矩阵,具体公式如下:

Kt+1/t+1=Pt+1/tHt+1ΤHt+1Pt+1/tHt+1Τ+R0-1Xt+1=Xt+Kt+1/tZt-hXtPt+1/t+1=I-Kt+1Ht+1Pt+1/t

其中:

Pt+1/t=FtPt/tFtΤ+Q0

式中:Ft为雅克比矩阵,Kt为卡尔曼增益,hXt为机器人状态测量模型, Pt/t 为状态向量的协方差矩阵, I 为单位矩阵。

通过扩展卡尔曼滤波算法对里程计和IMU的数据进行融合后,有效减小了误差,解决了机器人在转弯时检测不精准的问题,但这远远无法达到机器人定位与导航精度的要求。因此,须结合里程计辅助法与PL-ICP配准法来对机器人的位姿进行进一步标定,以实现运动畸变的校正。首先,利用里程计中的编码器获取机器人在当前时刻与上一时刻的位姿差,并将该位姿差作为PL-ICP配准法的输入初始值,以获取机器人在当前时刻的对应位姿。然后,基于当前时刻下里程计获取的位姿差和PL-ICP配准法计算得到的位姿,构建位移误差方程。最后,将位移误差均分至下一时刻的机器人位姿,以修正其位姿差,并再次使用误差方程对修正后的位姿差进行多次迭代计算,不断缩小机器人当前时刻与上一时刻的位移误差。当误差函数收敛或达到最大迭代次数时,迭代终止,最终获得里程计标定的位姿真值。

1.4 地图重影优化

由于机器人的质量较小,其重心在运动过程中会因地形变化而上移,使得运动姿态不稳定,这会对激光雷达的扫描产生干扰,造成地图重影。另外,由于本文采用的是二维激光雷达,在构建地图时将实际环境转换到二维平面上时可能会出现多平面重叠在同一位置处的情况,同样会造成地图重影。与此同时,常用的特征匹配算法的鲁棒性也会影响单应性矩阵的估计误差,导致图像变换不准确,使得图像拼接时在重叠区域出现明显的对齐误差,进而造成地图模糊[22]。除此之外,当机器人作旋转运动时,由于数据更新较慢,扫描匹配时会存在时间误差,也会造成地图重影。针对上述问题,可通过以下方式来消除地图重影:

1)增大机器人的质量,以减少地形对机器人运动姿态的干扰。

2)融合里程计和IMU的数据,以减小因机器人打滑而导致的里程计测量误差,同时减小机器人运动过程中产生的漂移误差,从而提升定位精度。

3)在IMU中设置倾斜角阈值,本文设倾斜角阈值为45°,当倾斜角大于45°时,系统自动忽略激光雷达采集的数据,进而过滤在机器人倾斜角过大情况下所采集的数据。

2 AGV验证实验

基于优化的Hector-SLAM算法,设计自主导航系统,并以AGV为例,配合Rviz三维可视化平台对系统的可行性进行验证[23]。本次实验中所使用的AGV的软硬件设备如表1所示。

表1   AGV的软硬件设备

Table 1  Hardware and software equipment of AGV

软硬件设备型号
树莓派英伟达Jetson Xavier
激光雷达RPLIDAR-A1
AGV底盘松灵TRACER
IMUGY-85
操作系统Ubuntu 18.04 ROS melodic desktop full

新窗口打开| 下载CSV


AGV底盘及其自主导航系统搭载的激光雷达的实物分别如图2图3所示。

图2

图2   AGV底盘实物图

Fig.2   Physical diagram of AGV chassis


图3

图3   AGV自主导航系统搭载的激光雷达

Fig.3   Lidar equipped on AGV autonomous navigation system


2.1 建图实验

在ROS(robot operating system,机器人操作系统)中订阅“/scan”激光雷达数据命令话题,由此发布AGV的位姿和栅格地图信息。随后,启动launch脚本功能,以构建相应的栅格地图。优化前后Hector-SLAM算法的建图效果对比如图4所示。

图4

图4   Hector-SLAM算法优化前后的建图效果对比

Fig.4   Comparison of mapping effects before and after Hector-SLAM algorithm optimization


由于激光雷达的精度不高,且里程计和IMU在实际应用时会产生漂移误差,因此激光雷达的各扫描帧之间偏移量会不断增大。观察图4(a)可知,采用优化前Hector-SLAM算法建图时,地图左上方出现了重影,同时存在大量噪声,导致边缘处出现毛刺现象。观察图4(b)可知,Hector-SLAM算法优化后,地图左上方的重影和边缘的毛刺现象得到了明显改善,地图边缘更加清晰。由此说明,优化后的Hector-SLAM算法有效地提高了建图精度。

基于优化后的Hector-SLAM算法,选取南京林业大学教九楼(简称南林教九楼)二楼和逸夫科技实验楼(简称南林逸夫楼)三楼的走廊为实验场地,开展建图实验。具体流程如下:首先,将AGV放置在走廊地面的某一位置处,以放置位置作为AGV的运行起点;然后,调用“roslaunch bringup.launch”命令,运行ROS中的多个节点;接着,调用“roslaunch lidar_slam.launch”命令,启动激光雷达;最后,利用键盘控制命令来控制AGV的移动。由于AGV速度过快时,其驱动轮会出现打滑现象,影响建图效果,因此设置AGV的最大线速度为0.5 m/s,角速度为0.3 rad/s,以保证建图精度。将构建好的地图保存到固态硬盘中,等待系统调用。

南林教九楼二楼走廊为封闭的“日”字形区域,其在百度地图中的外围轮廓如图5所示,对应的自主导航系统建图结果如图6所示。

图5

图5   百度地图中的南林教九楼二楼走廊

Fig.5   Second floor corridor in Ninth Teaching Building of NJFU in Baidu map


图6

图6   南林教九楼二楼走廊建图结果

Fig.6   Mapping results of second floor corridor in Ninth Teaching Building of NJFU


南林逸夫楼三楼走廊呈扇形,其在百度地图中的外围轮廓如图7所示,对应的自主导航系统建图结果如图8所示。

图7

图7   百度地图中的南林逸夫楼三楼走廊

Fig.7   Third floor corridor in Yifu Building of NJFU in Baidu map


图8

图8   南林逸夫楼三楼走廊建图结果

Fig.8   Mapping results of third floor corridor in Yifu Building of NJFU


为了定量分析基于优化Hector-SLAM算法的AGV自主导航系统在真实环境中的映射精度,选取教九楼二楼的东西走廊和南北走廊的长度为例,来验证优化后Hector-SLAM算法的建图精度,并与优化前Hector-SLAM算法进行对比。

本文实验采用成都景瑞特科技有限公司研制的高精度激光测距仪(最大探测距离为150 m,误差为3 mm)对教九楼二楼的走廊长度进行多次测量,并求平均值作为最终的实测值。通过测量和计算可得,教九楼二楼东西走廊的长度约为46 m,南北走廊的长度约为91 m。基于该数据,使用Visio软件等比例绘制教九楼二楼走廊示意图,各走廊分别用编号a、b、c、d表示,如图9所示。

图9

图9   南林教九楼二楼走廊示意图

Fig.9   Schematic of second floor corridor in Ninth Teaching Building of NJFU


使用Rviz平台中的测量函数获取图6所示地图中各走廊的长度(同样是多次测量并求平均值),并与利用激光测距仪获取的走廊长度实测值进行比较,结果如表2所示。从表2中可以看出,Hector-SLAM算法优化前,AGV自主导航系统的平均建图相对误差为0.50%左右;Hector-SLAM算法优化后,系统的平均建图相对误差约为0.44%,最小建图误差仅为0.236 m,相较于优化前减小了0.041 m,说明建图精度明显提升,验证了本文算法的优化是可行的。

表2   优化前后AGV自主导航系统的建图误差对比

Table 2  Comparison of mapping errors of AGV autonomous navigation system before and after optimization

建图算法走廊编号实测值/mSLAM测量值/m误差/m相对误差/%

优化前

Hector-SLAM

a91.11091.4670.3570.392
b90.77091.1220.3520.388
c45.67045.9470.2770.607
d45.93046.2130.2830.616

优化后

Hector-SLAM

a91.11091.4280.3180.349
b90.77091.0820.3120.344
c45.67045.9060.2360.517
d45.93046.1750.2450.533

新窗口打开| 下载CSV


2.2 自主导航实验

本文分别使用A*算法和DWA(dynamic window approach,动态窗口法)来实现AGV的全局路径规划[24]和局部路径规划[25]。根据2.1节构建的地图,利用Rviz平台开展自主导航实验。由于教九楼的走廊比逸夫楼的长,且教室和透明玻璃更多,更能考验AGV在复杂环境下的自主导航性能,因此在教九楼中开展自主导航实验。在导航实验开始前,将AGV放置在教九楼二楼走廊的地面上,并将此时AGV的位姿作为初始位姿;同时,根据AGV在真实环境中的位姿,调整Rviz平台中AGV的初始位姿,使其与实际位姿一致,如图10所示。

图10

图10   Rviz平台中AGV自主导航的起点

Fig.10   Starting point of AGV autonomous navigation in Rviz platform


为了验证所设计的AGV自主导航系统的定位精度和避障准确度,本次实验共设置4个目标点(第4个目标点与起点重合)和多个障碍物,控制AGV从图10所示的起点出发,按照A*算法规划的路径前往不同目标点,AGV沿地图运行一周,最终返回至起点,具体过程如图11所示。由图11可知,AGV能够按照A*算法规划的最优路径准确前往各个目标点。

图11

图11   Rviz平台中AGV的自主导航过程

Fig.11   Autonomous navigation process of AGV in Rviz platform


在实际场景中,AGV自主导航时可能会发生紧急情况,如出现行人或障碍物。为避免AGV因紧急情况出现而影响正常运行,自主导航系统会调用DWA来重新规划路径,以躲避行人和障碍物,保证AGV最终到达目标点。通过2次实验来验证AGV自主导航系统的紧急避障性能,结果分别如图12图13所示。

图12

图12   实际场景中AGV的避障过程

Fig.12   Obstacle avoidance process of AGV in actual scenario


图13

图13   Rviz平台中AGV的避障过程

Fig.13   Obstacle avoidance process of AGV in Rviz platform


图12图13中可以看出,当行人突然出现在AGV的前方时,自主导航系统能及时调用DWA来重新规划AGV的行驶路径,保证其在躲避行人和障碍物后准确到达目标点,实现自主导航功能。

3 总 结

本文对搭载低精度激光雷达的机器人自主导航系统进行了优化,解决了机器人在使用传统Hector-SLAM算法时存在的建图不清晰、定位不精准等问题。首先,利用双三次插值法代替双线性插值法来提高数据的连续性,使高斯-牛顿法扫描匹配地图的精准度得以提升。然后,采用扩展卡尔曼滤波算法对里程计与IMU的数据进行了融合;同时,将里程计辅助法和PL-ICP配准法相结合,以使机器人的位姿标定更加准确,提高了定位的准确性。最后,通过设置IMU倾斜角阈值,消除了机器人旋转时产生的地图重影现象。实验结果表明,Hector-SLAM算法优化后,自主导航系统的最小建图误差相比于优化前减小了0.041 m,其平均建图相对误差低于0.50%,建图精度大大提升。但是,当建图环境较大时,所设计的自主导航系统的建图与定位效果仍存在不足,在后续研究中可考虑使用高精度的激光雷达来进一步提升系统的有效性。

参考文献

于宁波王石荣徐昌.

一种基于RGB-D的移动机器人未知室内环境自主探索与地图构建方法

[J].机器人,2017396):860-871. doi:10.13973/j.cnki.robot.2017.0860

[本文引用: 1]

YU N BWANG S RXU C.

RGB-D based autonomous exploration and mapping of a mobile robot in unknown indoor environment

[J]. Robot, 2017396): 860-871.

DOI:10.13973/j.cnki.robot.2017.0860      [本文引用: 1]

MISIR O.

Dynamic local path planning method based on neutrosophic set theory for a mobile robot

[J]. Journal of the Brazilian Society of Mechanical Sciences and Engineering, 2023453): 127.

崔嵩鹤姜树海耿志斌.

人工林立木整枝作业机械研究进展

[J].世界林业研究,2019325):47-51. doi:10.13348/j.cnki.sjlyyj.2019.0055.y

CUI S HJIANG S HGENG Z B.

Research progress in tree pruning machinery for plantation

[J]. World Forestry Research, 2019325): 47-51.

DOI:10.13348/j.cnki.sjlyyj.2019.0055.y     

郭伟斌陈勇.

基于模糊控制的除草机器人自主导航

[J].机器人,2010322):204-209. doi:10.3724/sp.j.1218.2010.00204

GUO W BCHEN Y.

Fuzzy control based autonomous navigation for a weeding robot

[J]. Robot, 2010322): 204-209.

DOI:10.3724/sp.j.1218.2010.00204     

WU BCHI X NZHAO C Cet al.

Dynamic path planning for forklift AGV based on smoothing A* and improved DWA hybrid algorithm

[J]. Sensors, 20222218): 7079.

[本文引用: 1]

TAHERI HXIA Z.

SLAM; definition and evolution

[J]. Engineering Applications of Artificial Intelligence, 202197104032.

[本文引用: 1]

HUANG LSHI Y TWANG J Het al.

Novel pedestrian navigation system based on zero velocity update procedure technology and improved Sage-Husa adaptive Kalman filter with index fading memory factor

[J]. IET Wireless Sensor System, 2023132): 48-57.

[本文引用: 1]

支奕琛谷玉海龚志力.

机器人二维码目标点识别与SLAM地图标记研究

[J].机床与液压,20225015):20-24. doi:10.3969/j.issn.1001-3881.2022.15.004

[本文引用: 1]

ZHI Y CGU Y HGONG Z Let al.

Research on target recognition of robot quick response code and SLAM map marking

[J]. Machine Tool & Hydraulics, 20225015): 20-24.

DOI:10.3969/j.issn.1001-3881.2022.15.004      [本文引用: 1]

GUAN R PRISTIC BWANG L Pet al.

KLD sampling with Gmapping proposal for Monte Carlo localization of mobile robots

[J]. Information Fusion, 20194979-88.

[本文引用: 1]

苏易衡张奇志周亚丽.

适用于低端激光雷达的优化Hector SLAM算法

[J].实验室研究与探索,2019389):47-51. doi:10.3969/j.issn.1006-7167.2019.09.012

[本文引用: 1]

SU Y HZHANG Q ZZHOU Y L.

Hector SLAM algorithm for low performance lidar

[J]. Research and Exploration in Laboratory, 2019389): 47-51.

DOI:10.3969/j.issn.1006-7167.2019.09.012      [本文引用: 1]

周燕.

地铁隧道内四旋翼自主导航系统设计

[D].南京南京理工大学202135-40. doi:10.1109/icca51439.2020.9264455

[本文引用: 1]

ZHOU Y.

Design of autonomous navigation system for quadrotor in subway tunnel

[D]. NanjingNanjing University of Science and Technology202135-40.

DOI:10.1109/icca51439.2020.9264455      [本文引用: 1]

ALMASRI MELLEITHY KALAJLAN A.

Sensor fusion based model for collision free mobile robot navigation

[J]. Sensors, 2016161): 24.

[本文引用: 1]

ZHAO J WLIU S YLI J Y.

Research and implementation of autonomous navigation for mobile robots based on SLAM algorithm under ROS

[J]. Sensors, 20222211): 4172.

[本文引用: 1]

薛迎港.

基于ROS的仓储机器人自主导航研究和实现

[D].西安西安工业大学202225-26. doi:10.17520/biods.2023167

[本文引用: 1]

XUE Y G.

Research and implementation of automatic navigation of warehouse robot based on ROS

[D]. Xi’anXi’an Technological University202225-26.

DOI:10.17520/biods.2023167      [本文引用: 1]

张洪邱晓天.

基于EtherCAT的ROS全向移动导航系统

[J].工程设计学报,2021282):241-247. doi:10.3785/j.issn.1006-754X.2021.00.021

ZHANG HQIU X T.

Omni-directional mobile navigation system based on ROS and EtherCAT

[J]. Chinese Journal of Engineering Design, 2021282): 241-247.

DOI:10.3785/j.issn.1006-754X.2021.00.021     

陈甲伟刘英於亚斌.

基于节点优简算法的实木板材自动搬运系统

[J].计算机集成制造系统,2020269):2522-2530. doi:10.13196/j.cims.2020.09.021

CHEN J WLIU YYU Y Bet al.

Automatic handling system of solid wood panels based on node simplification algorithm

[J]. Computer Integrated Manufacturing Systems, 2020269): 2522-2530.

DOI:10.13196/j.cims.2020.09.021     

郝奇陈智君郑亮.

基于概率匹配的栅格地图拼接算法

[J].组合机床与自动化加工技术,20225):83-87.

[本文引用: 1]

HAO QCHEN Z JZHENG Let al.

Mosaic algorithm of grid map based on probability matching

[J]. Modular Machine Tool & Automatic Manufacturing Technique, 20225): 83-87.

[本文引用: 1]

EISENSTAT SJACKSON KLEWIS J.

The order of monotone piecewise cubic interpolation

[J]. SIAM Journal on Numerical Analysis, 1985226): 1220-1237.

[本文引用: 1]

岳胜杰王红旗刘群坡.

结合扩展卡尔曼滤波与基于点线的最近点迭代扫描匹配算法的机器人位姿自适应估计

[J].测绘通报,20227):49-53. doi:10.3969/j.issn.0494-0911.2022.7.chtb202207009

[本文引用: 1]

YUE S JWANG H QLIU Q Pet al.

Adaptive pose estimation for robot based on extended Kalman filter and point-line iterative closest point

[J]. Bulletin of Surveying and Mapping, 20227): 49-53.

DOI:10.3969/j.issn.0494-0911.2022.7.chtb202207009      [本文引用: 1]

李荣华王振宇陈凤.

空间失稳目标线阵成像畸变分析与三维重建

[J].宇航学报,2020412):224-233. doi:10.3873/j.issn.1000-1328.2020.02.012

[本文引用: 1]

LI R HWANG Z YCHEN Fet al.

Distortion analysis and 3D reconstruction of space instability target line array imaging

[J]. Journal of Astronautics, 2020412): 224-233.

DOI:10.3873/j.issn.1000-1328.2020.02.012      [本文引用: 1]

孙贵杰.

自主移动机器人控制系统研究

[D].长春吉林大学202122-50.

[本文引用: 1]

SUN G J.

Research on control system of autonomous mobile robot

[D]. ChangchunJilin University202122-50.

[本文引用: 1]

熊求兴.

基于图像梯度稀疏性先验去除重影反射

[D].长沙湖南师范大学202025-39.

[本文引用: 1]

XIONG Q X.

Ghost reflection removal using imagegradient sparsity prior

[D]. ChangshaHunan Normal University202025-39.

[本文引用: 1]

刘宇程姜振华胡俊.

基于ROS平台的六自由度机械臂轨迹规划及仿真研究

[J].制造业自动化,2022441):38-41. doi:10.3969/j.issn.1009-0134.2022.01.009

[本文引用: 1]

LIU Y CJIANG Z HHU J.

Research on trajectory planning and simulation of 6-DOF manipulator based on ROS

[J]. Manufacturing Automation, 2022441): 38-41.

DOI:10.3969/j.issn.1009-0134.2022.01.009      [本文引用: 1]

张浩.

基于A*算法的自动泊车全局路径规划算法研究

[D].长春吉林大学202229-36.

[本文引用: 1]

ZHANG H.

Research on global path planning algorithm of automatic parking based on A* algorithm

[D]. ChangchunJilin University202229-36.

[本文引用: 1]

ZHANG Y XKONG X DCHEN X Z.

Multi robot navigation formation simulation based on ROS

[J]. Journal of Physics: Conference Series, 202222161): 012015.

[本文引用: 1]

/