基于2D建图算法的导航方法

李 娜，贺海育，贾 伟，雍龙泉

(1.陕西理工大学 数学与计算机科学学院，陕西 汉中723000；2.西安航空学院 电子工程学院，陕西 西安 710017)

0 引言

近年来，在智能控制方面，对于即时定位与地图构建(Simultaneous Localization and Mapping，SLAM)算法的研究是一大热点[1-8]。

在2D-SLAM建图与路径规划的应用方面研究较多，文献[9-11]中机器人的里程计信息进行坐标变换后，利用Gmapping建图算法可以实现在室内小范围地图的建立;文献[12]提出了一种基于多传感器融合的室内建图和定位算法；文献[13]中采用基于高斯牛顿法(Guass-Newton)的Hector建图算法，不需要里程计的情况下可以建立出高精度的地图；文献[14]提出了一种实时定位和建图前端优化的解决方法；文献[15-19]基于机器人操作系统(Robot Operating System,ROS)平台,对导航机器人的整体架构和路径规划进行研究。以上研究大多需要大量的传感器，造成在实际应用中成本过高，且常用建图算法较多而鲜有对比。在实际应用中，最佳方案应根据不同的环境选取不同的建图算法，实现精确定位与导航。

因此，本文使用ROS管理控制2D激光雷达和树莓派来采集环境信息，对信息处理后分别利用Gmapping与Hector两种算法建立二维栅格地图,然后进行路径规划，实现小车对未知环境的自由探索。实验证明，该系统不仅成本较低、结构简单，而且扩展性以及适用性较强。

1 小车硬件平台设计

首先需建立一辆智能监测系统小车，设计小车的实时建图与路径规划系统的结构框架如图1所示。

图1 系统结构

当小车处于未知环境时，首先在雷达节点中，计算机通过串口读取到雷达所传回的环境深度信息，然后在建图节点中实时建立二维栅格地图，并利用实时更新地图节点不断保存更新所建地图。接着，将栅格地图信息传送到ROS系统中内嵌的导航算法包(Navigation)进行实时路径规划，再将所计算出的里程计信息给到底层控制节点(STM32)，进行小车运动控制，同时读取底层里程计信息，构成闭环控制。最后，在PC端利用远程登陆到树莓派并在rviz可视化环境下进行人机交互，可以在PC端设置目标点，观察和记录小车以及外部环境。

本文所用的小车平台由一个可解读里程计信息的底盘、一个激光雷达和一台Linux嵌入式计算机构成，如图2所示。激光雷达所使用的是杉川Delta-2A，其量程为8 m，扫面频率6.2 Hz。所使用的小车主控制机器为Raspberry Pi 4B+，8 GB内存版本。采用Ubuntu18.04操作系统，并配置了ROS。

图2 小车平台实物

2 基于Hector算法建图

Hector无需里程计，通过扫描匹配的方法进行地图创建，基于高斯牛顿法以求其最优解，大大减少了算法所需的计算量。

扫描匹配用当前雷达所传回的一帧数据与以往所建地图数据构造误差函数：

(1)

式中，ξ=(Px,Py,ψ)T是运动小车在世界坐标系中xoy平面的位姿，Px和Py是在世界坐标系的二维横纵坐标，ψ是以世界坐标系y轴为参考的旋转角度,ξ为上一时刻运动小车的位姿或初始位姿；ξ*为当前时刻最优位姿。经激光扫描所体现外部深度信息的点云坐标转化到世界坐标系后的坐标为：

(2)

式中，si=(si,x,si,y)T是激光扫描端点在雷达自身坐标系下的坐标，si下标序号代表激光点标签，可由激光雷达采集的原始数据的极坐标转化而来。其中右侧第1个矩阵为旋转矩阵，第3个矩阵为平移矩阵，此时用一组旋转与平移即可完全描述出物体的运动，函数M(Pm)代表坐标点Pm在占用栅格地图中的占用值，式(1)中坐标点Pm为Si(ξ)的占用值，其值越大匹配效果越好，算法核心示意如图3所示。

图3 Hector算法核心示意

在Hector算法中，坐标变换则显得无比重要。根据传感器传回第1帧图像来确定世界坐标系。通过所采样的第n帧图像依次通过雷达运动来推算出每一帧图像的坐标变换，通过特征点检查和叠加建立出完整的地图，如图4所示。

(a)正常的室内二维地图

利用Hector算法在教室中(矩形无障碍区域)进行建图实验，可以发现，在不使用里程计的情况下，当传感器有大幅度角度运动或行进速度过快时，图像会出现运动畸变且Hector算法没有地图回环，一旦定位丢失，Hector算法无法重新定位，也无法继续建图。

3 基于Gmapping算法建图

Gmapping是基于滤波SLAM框架的常用开源算法。利用粒子滤波来实现定位[3]和建图，在室内或较小环境中的建图效果更好。粒子滤波是基于蒙特卡罗方法的小车定位算法，其缺点是需要很多的样本，算法复杂度很高，所需计算量较大，但却可以适应不同的误差分布方式。

在SALM中要解决的问题就是由控制数据和观测数据来求位姿和地图的联合分布。即先进行定位再进行建图。此算法会将第1帧所得到的数据作为小车的初始点，然后小车进行移动时用所接收到的雷达数据与上一时刻所建的地图进行匹配，这样可以对小车定位，用此时小车的位置可以建立出这一时刻的地图。这样重复下去即可一帧一帧地迭代建立出完整的地图。算法核心流程如下：

步骤1：输入上一时刻的粒子群、最近时刻的里程计和雷达扫描信息，同时初始化当前时刻的粒子群。

步骤2：遍历并获取上一时刻粒子群中粒子所携带的位姿、权重和地图信息。

步骤3：通过里程计进行位姿更新，用极大似然估计法可以求得局部极值。如果没有找到局部极值则提议分布。

步骤4：更新粒子位姿状态，然后使用观测模型对位姿权重更新。若找到局部极值在其附近取k个位姿并认为这k个位姿服从高斯分布。

步骤5：计算这k个位姿的权重与均值，将均值归一化后，求出k位姿的方差并将方差归一化。

步骤6：使用多元正态分布计算出新的位姿，计算该位姿粒子的权重。

步骤7：更新地图，更新粒子群并循环重复计算上一时刻的每一个粒子。

步骤8：计算所有粒子权重的离散程度，并判断是否超过阈值，是否进行重采样。

在PC端中的rviz可视化环境下，可以观察到建立的地图，如图5所示。

(a)里程计不精准时建图效果

在实验中，使用Gmapping算法对教室进行建图。从实际结果可以看出，在有里程计信息的(矩形无障碍区域)情况下，当里程计不精准时建图效果并不理想，在地图的后端与前端发生重影现象，但是可以精确计算里程计时(矩形有障碍区域)是可以建立精准地图的。由此可知，Gmapping算法严重依赖里程计，而无法适应复杂的地面环境，且此算法在没有里程计的情况下是无法建立地图的。

4 导航效果对比

基于上述2种算法在不同的环境中，使用ROS中内嵌的导航算法包(Navigation)进行导航实验，其中导航包要求输入小车定位、地图信息、里程计信息和雷达测量的点云数据以及小车、雷达与世界坐标系的转换关系。有了这些必需信息，导航包即可提供全局与局部的路径规划，从而实现导航。

在配置好导航包后，分别在路面平整与路面不平整的情况下进行建图导航实验，实验效果如图6所示。

(a)自由探索时最佳路径

经实验验证，可以建立出精准地图并成功在图中定位小车位置，且做出最优路径规划(图6中箭头指向的绿线)。当自由探索时，小车可以自动按照当前地图边界向外界扩展，并实时更新路径，当规定目标点时，小车可以进行避障，且在此基础上寻找最优路径。

5 结束语

通过2种算法进行建图导航，所设计的系统不仅成本较低，结构简单，而且扩展性以及适用性较强。2种方法各有优缺点，其中Gmapping算法严重依赖里程计，无法适应无人机在三维情况下飞行时的定位建图及地面不平坦的区域，无回环即无法重新定位。在室外大场景粒子较多的情况下，非常消耗计算资源，很依赖计算力会造成在实际应用中的成本过高，其更加适合于路面情况不复杂的巡检工作。Hector算法基于解最小二乘问题，只用求出函数极值而不需要里程计，而且可以适应空中，手持实时记录地图信息或者地面不平坦的情况，但缺点在于对于雷达扫描频率和帧率要求很高(40 Hz)，并且需要估计三维空间下的6自由度机器人的位姿，这使得算法复杂度大幅增加。