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

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

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

自主移动机器人必须具备自主导航和路径规划能力，而构建环境地图是实现路径规划的前提[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算法包括Gmap‐ping、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)，其中x、y为整数部分，i、j为小数部分。

图1 双三次插值模型示意Fig.1 Schematic of bicubic interpolation model

双三次插值的基函数S(x)可表示为：

式中：a为待定系数。

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

其中：

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

将A、B、C代入式(3)中的第1个方程，化简可得：

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

1.2 扫描匹配

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

其中：

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

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

将式(6)在Δξ处泰勒展开并求偏导，可得：

其中：

式中：H为Hessian矩阵；∇M(Sq(ξ))为地图梯度。

1.3 运动畸变校正

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

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

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

式中：vxt和vyt、axt和ayt分别为t时刻机器人沿X、Y方向的速度和加速度。

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

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

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

式中：xt、yt为t时刻机器人沿X、Y方向的位移；θt为t时刻机器人的旋转角度。

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

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

式中：W0为机器人的状态噪声，服从高斯分布，即W0～N(0,Q0)，其中Q0为预测状态下的协方差矩阵。

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

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

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

其中：

式中：Ft为雅克比矩阵，Kt为卡尔曼增益，h(Xt)为机器人状态测量模型，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

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

图2 AGV底盘实物图Fig.2 Physical diagram of AGV chassis

图3 AGV自主导航系统搭载的激光雷达Fig.3 Lidar equipped on AGV autonomous navigation system

2.1 建图实验

在ROS（robot operating system，机器人操作系统）中订阅“/scan”激光雷达数据命令话题，由此发布AGV的位姿和栅格地图信息。随后，启动launch脚本功能，以构建相应的栅格地图。优化前后Hector-SLAM算法的建图效果对比如图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 中的多个节点；接着，调用“ro‐slaunch lidar_slam.launch”命令，启动激光雷达；最后，利用键盘控制命令来控制AGV的移动。由于AGV速度过快时，其驱动轮会出现打滑现象，影响建图效果，因此设置AGV的最大线速度为0.5 m/s，角速度为0.3 rad/s，以保证建图精度。将构建好的地图保存到固态硬盘中，等待系统调用。

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

图5 百度地图中的南林教九楼二楼走廊Fig.5 Second floor corridor in Ninth Teaching Build‐ing of NJFU in Baidu map

图6 南林教九楼二楼走廊建图结果Fig.6 Mapping results of second floor corridor in Ninth Teaching Building of NJFU

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

图7 百度地图中的南林逸夫楼三楼走廊Fig.7 Third floor corridor in Yifu Building of NJFU in Baidu map

图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 南林教九楼二楼走廊示意图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

2.2 自主导航实验

本文分别使用A*算法和DWA（dynamic win‐dow approach，动态窗口法）来实现AGV的全局路径规划[24]和局部路径规划[25]。根据2.1 节构建的地图，利用Rviz平台开展自主导航实验。由于教九楼的走廊比逸夫楼的长，且教室和透明玻璃更多，更能考验AGV在复杂环境下的自主导航性能，因此在教九楼中开展自主导航实验。在导航实验开始前，将AGV放置在教九楼二楼走廊的地面上，并将此时AGV的位姿作为初始位姿；同时，根据AGV在真实环境中的位姿，调整Rviz 平台中AGV的初始位姿，使其与实际位姿一致，如图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 Rviz平台中AGV的自主导航过程Fig.11 Autonomous navigation process of AGV in Rviz plat‐form

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

图12 实际场景中AGV的避障过程Fig.12 Obstacle avoidance process of AGV in actual scenario

图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%，建图精度大大提升。但是，当建图环境较大时，所设计的自主导航系统的建图与定位效果仍存在不足，在后续研究中可考虑使用高精度的激光雷达来进一步提升系统的有效性。