基于ROS的移动机器人自动导航系统设计

秦国威，陈孟元

(1.蚌埠学院 电子与电气工程学院，安徽 蚌埠 233030；2.安徽工程大学 电气工程学院，安徽 芜湖 241000)

随着社会地突飞猛进，在众多学者的孜孜努力下，机器人在20世纪终于得以成功问世。机器人的诞生对我国的工农业、制造业、服务业等领域都产生了非常深远的影响[1]。移动机器人作为其中最典型的代表之一，被广泛地应用在医疗、餐饮等服务行业，为使移动机器人能够提供更加高质量的服务，对移动机器人开展一系列的智能化、自主化研究就显得具有非常远大的意义和价值[2-3]。

自主导航是移动机器人需完成的一项最基本的工作。简而言之，即移动机器人在复杂或陌生的环境中建立环境地图，自主规划出一条安全高效无碰撞的最优路径，并且沿着该路径行进到目标点。归纳起来，移动机器人自主导航主要包含以下三方面工作：第一，地图构建问题；第二，定位问题；第三，路径规划问题。三方面之间的关系如图1所示。地图构建问题即SLAM[4]，作为移动机器人自主导航的首要任务，是实现自主导航的坚实基础。目前，ROS下SLAM建图方法主要有以下三种。(1)基于图优化的cartographer算法[5]，此算法是Goole公司于2016年10月推出的图优化SLAM算法，故又被称为Graph-based SLAM，其需要里程计、惯导(IMU)等传感器。图优化的思想就是在机器人运行过程中，将不同时刻的位姿抽象为若干个点(pose)，在不同位置上观测所产生的约束抽象为点与点之间的边。构图完成后，从多方面调整移动机器人的位姿去满足这些边构成的约束，从全图的角度进行优化。由于cartographer算法没有集成到ROS中，因此，ROS环境下若要使用此算法，还需要采用源码的方式进行安装，过程较为繁琐。(2)基于粒子算法的Gmapping算法，此算法是一种基于粒子滤波的SLAM方法，众多的加权粒子表示路径的后验概率，每个粒子都代表一个重要性因子。机器人在建图过程中，由于粒子基数比较大，而且还不断地被重采样，导致计算量和内存消耗的持续增加，因此此算法建图的效率偏低，此外，由于该算法需要使用多种传感器获取信息，导致所建地图的准确性和鲁棒性也备受影响。(3)基于高斯牛顿法的Hector-SLAM：此方法仅利用激光雷达获取场景的地图信息，完全摆脱里程计数据，仅采用图优化的方法进行建图，此算法非常有利于在特定的非结构化的复杂环境下进行地图构建，而且灵活性高、扩展性强。本文综合考虑，采用此算法建图。本文在环境建图建成后，接着就需要在地图中确定移动机器人的位置和姿态，即定位。在ROS环境下移动机器人定位大部分都采用蒙特卡洛(MCL)算法[6]，但此算法存在计算量大、实时性差等问题，由此本文采用一种基于KLD重采样的自适应AMCL算法[7]，能够实时在线自适应调整粒子数，减少算法的计算量。定位完成后，下一步就需要规划出一条安全且无碰撞的路径，即路径规划[8]。在众多的路径规划算法中，A*算法是一种基于栅格地图的算法[9]，相比于D*算法，对于已知的环境场景，能够规划出更加有效平滑地路径；相比于Dijkstra算法，A*算法采用了启发式搜索模式，大大降低了搜索节点的数量，从而提高了搜索节点的效率。由此可看出A*算法在路径规划方面具有非常大的优势。

图1 自主导航各模块关系图

综上，本文在ROS环境环境下所设计实现的自动导航系统，首先利用Hector-SLAM完成环境地图构建，其次使用基于KLD重采样的自适应AMCL算法确定移动机器人自身位置，最后利用改进型A*算法和DWA[10]动态窗口法完成机器人路径规划和运行控制。本文研究内容对于移动机器人导航领域研究具有非常远大的意义和实用价值[11]。

1 环境地图构建

移动机器人要在陌生环境中实现完全自主导航，必须首先获取周围的环境信息，明确自身所处的位置，进而建立环境地图模型。当下，机器人均是通过自身携带的传感器进行在线测量和位置估计，在陌生且复杂的环境中利用相关的SLAM算法来完成地图构建。基于此，本文采用一种Hector-SLAM图优化算法完成环境地图的建模。

Hector-SLAM建图分为两步，第一步利用激光雷达获取外界的环境信息，第二步则是与先验激光数据信息进行匹配，进而完成概率栅格地图的构建。整个过程不需要里程计的参与，仅依赖于高频率高精度的激光，在第二步中利用Gauss-Newton法解决扫描匹配问题。本文主要针对2D SLAM子系统，开展Hector-SLAM算法研究，Hector-SLAM原理框架如图2所示。

图2 Hector-SLAM原理图框架

1.1 地图获取

利用Hector-SLAM算法进行地图构建，即为依赖激光雷达进行环境扫描，将连续的环境信息进行采样离散化。同时为了克服离散性对精度的不利影响，采用双线性插值法进行栅格占有概率估计。如图3为双线性插值滤波示意图。

图3 双线性插值滤波示意图

对于给定的地图坐标Pm，M(Pm)表示该栅格的占有值，为了方便计算，其周围的四个点坐标简单表示为(P00,P01,P10,P11)，由此沿横轴和纵轴进行双线性插值，如式(1)所示。

(1)

将Pm在x方向、y方向求偏导如式(2)所示。

(2)

将(1)、(2)两式联合，连续栅格地图坐标Pm点梯度值如式(3)所示。

(3)

1.2 扫描匹配

在环境栅格地图构建完成后，下一步需要将当前采集的激光扫描帧与已建立的栅格地图进行匹配，进而完成移动机器人的位姿估计。在整个过程中，依据激光雷达采集的第一帧数据直接进行建图，之后采集的数据需要与已建地图逐一对齐匹配。利用高斯-牛顿梯度法进行最优解的求解，如式(4)所示。

(4)

式中：η=(Px，Py，β)为栅格地图旋转矩阵，即机器人的最佳位姿。Sj(η)表征激光雷达的端点坐标Sj=(Sj,x，Sj,y)T在世界坐标系下的位置，如式(5)所示。

(5)

式中：M(Sj(η))表征坐标为Sj(η)的环境栅格地图值，依据高斯-牛顿梯度法的高级思想，设定一个初始化数值η，根据下式对增量Δη进行估计，如式(6)所示。

(6)

对式(6)中M(Sj(η+Δη))进行一阶泰勒展开，且对Δη求偏导如式(7)所示。

(7)

由此，Δη问题可转化为函数极值问题进行处理。令上式为0可得Δη的解如式(8)所示。

(8)

式中：H为Hession矩阵如式(9)所示。

(9)

式中：▽M(Sj(η))表征的地图梯度值已在地图获取部分进行阐述，如式(3)所示。

上述的Hector-SLAM算法采用高精度激光传感器，完全不依赖里程计的数据信息，适用的范围非常广泛，尤其在移动机器人SLAM领域，广泛用于解决环境地图建模问题。

2 基于KLD重采样的AMCL定位

移动机器人在获取地图信息后，接着就需要在复杂的环境中确定自身所处的位置。在本文研究中，机器人在利用Hector-SLAM算法获取的先验环境地图信息、上一时刻的位姿估计以及激光雷达的数据信息后，进而经过信息的变换、处理，最终依赖当前时刻机器人所处的姿态来最终完成自主定位。本文采用基于KLD重采样的AMCL(自适应蒙特卡洛定位)算法完成整个定位过程。

2.1 MCL算法

原始的蒙特卡洛(MCL)定位算法广泛应用于估计移动机器人在已知环境地图信息下的位姿状态。移动机器人的运动学方程和观测方程如式(10)所示。

xt=ft(xt-1,ut-1)

zt=ht(xt,vt)

(10)

式中：xt∈Rnx，表征系统在t时刻的运动状态；zt∈Rnz分别表征系统在t时刻的观测状态；ft、ht分别表征系统的状态的转移函数和观测函数；ut、vt分别表征系统的控制噪声和观测噪声。

MCL定位是概率确定位置的结果，可将其理解在环境地图m和系统初始化状态x0给定的条件下，利用一组有限的离子集St来估计机器人位姿状态xt的后验信度，后验概率密度的分布如式(11)所示。

(11)

(12)

在不同的时间间隔内，MCL算法基于重要性采集理论，首先采用权重粒子集St，接着对权重粒子集进行重采样方式处理，处理的新粒子用于姿态更新，由此得出当前时刻系统状态后验信度分布bel(xt)的离散估计，最后，系统状态的最小方差用粒子集替代估计。式(13)为运动状态xt的后验概率密度估计。

(13)

式中：δ表示样本估计概率。

从MCL(蒙特卡洛)算法中可以明确得知，系统的样本容量是一层不变的，同时缺少系统的观测信息zt，忽略了移动机器人所处场景的观测信息对状态估计的修正作用，进而使得采样粒子集分布在观测似然的尾部，导致粒子集退化问题，最终使得移动机器人定位存在偏差。

2.2 基于KLD重采样的AMCL算法

考虑到原始的MCL算法在定位方面存在的问题，本文采用基于KLD重采样的AMCL算法完成移动机器人定位，致力于服务机器人更好的完成自主导航工作。

2.2.1 KLD采样

粒子滤波效率和定位的精度由以下两方面的因素决定。一是后验信度分布与信度函数之间的似然性，二是信度函数的准确分布情况。本文所采用的KLD重采样对采样部分进行调整改进，是自适应定位的关键技术。KLD采样的核心思想如下，在粒子滤波的每一次迭代中，以概率1-δ使真实后验概率和基于样本的估计信度之间的误差是否小于ε，由此来确定重采样样本数目，该误差是通过计算最大似然估计与真实的后验分布之间的差值，即K-L距离来确定的。K-L距离用来表示两个概率分布p和q之间的逼近误差，如式(14)所示。

(14)

式中：k表示K-L距离，且当p，q两个似然分布保持相同且全部为0时，是概率分布之间差异的衡量标准。

q的似然概率估计值λn在粒子数n趋向于正无穷大时服从差分分布，如式(15)所示。

(15)

p=(K(p,q)≤ε)=1-δ

(16)

但是，计算n的值非常困难，由概率论与梳理统计分析所学知识可知：当n的数量满足一定值时，就一定可以保证真实概率密度与估计概率密度之间的K-L距离小于阈值ε，从而可以进一步保证两者之间的逼近误差最小，再接着利用wilson-hilferty变换近似可以得出n的近似值如式(17)所示。

(17)

式中：z1-δ服从标准正态分布，上界为1-δ。

在重采样过程中，对权重大的粒子数进行复制，反之，进行忽略抛弃。在粒子的复制过程中采用KLD采样的方法来确定下一次重采样的粒子数。由此可以看出，采用重采样的粒子处理方式可以实现粒子的实时调整，计算量得以大大减小。

2.2.2 基于KLD的自适应AMCL算法

基于KLD的自适应AMCL算法流程如下。

第二步：将粒子集权重作归一化处理。

第四步：以粒子集的数学期望值作为当前t时刻下移动机器人位置和方向即位姿的输出，位姿与数学期望关系式如式(18)所示。

(18)

式中：E[ ]为数学期望。

3 路径规划

移动机器人自主导航在完成环境地图建模，自身定位两部分工作之后，还剩下最后一个环节，即机器人路径规划。本文采用改进型A*算法，使机器人在所处陌生场景中完成初始点到目标点的全局路径规划，采用DWA动态窗口法完成局部路径规划、避障及运动控制等工作。

3.1 传统的A*算法

A*算法作为一种启发式搜索方法，是非常常用的移动机器人路径规划算法，其关键评价函数定义如式(19)所示。

f(n)=s(n)+h(n)

(19)

式中：n表征当前位置的节点；f(n)表征初始节点经过节点n到目标节点的总估计代价函数；s(n)表征初始点到目标点的实际代价函数；h(n)表征初始节点到目标节点的估计代价。h(n)是启发代价函数中最重要的部分，其取值即可决定算法的性能，正确地选择h(n)能够大大提高A*算法的精准度。A*算法利用曼哈顿举例作为h(n)的代价函数，公示如式(20)所示。

h(n)=|x1-x0|+|y1-y0|

(20)

式中：(x0,y0)表征初始节点所在栅格中心的坐标，(x1,y1)表征目标节点所在栅格中心的坐标。

传统A*算法采用的是八领域搜索法，如图4所示。通过当前栅格中心节点向临近8个方向扩散，由此会产生很多转折点，从运动学的角度来说，传统的A*算法在实际场景中的应用效果显然不理想。

图4 八领域搜索示意图

3.2 改进的A*算法

根据传统的A*算法的八领域搜索方向：x方向、y方向、对角线方向，将A*算法启发函数h(n)采用的曼哈顿距离分解成这三个方向的线性组合，经过采用这种方法处理后，即可更准确地计算出栅格地图上任意两点的实际代价，并且还可使得算法搜寻出最优路径。改进后的A*启发函数如式(21)所示。

(21)

式中：dx(n)=|x1-x0|，dy(n)=|x1-x0|。

A*算法h(n)启发函数经过优化后，对于实际移动代价的估算更加精确。

4 ROS仿真实验

在ROS系统下，移动机器人完成自主导航的流程如图5所示。首先要在ubantu系统下将ROS安装完毕，接着调用ROS环境下的三个功能包，共同组成了导航栈的核心。首先，需要调用的时hector-SLAM功能包，根据hector-SLAM算法以及激光传感器获得的数据完成环境地图的建模；其次，需调用amcl定位功能包，根据基于KLD重采样的AMCL算法，评估机器人在地图中的位置，完成定位工作；最后，需调用Move-base包，使用A*算法以及DWA动态窗口法实现初始点到目标点的路径规划以及机器人的自主运行。下面就逐一讨论分析建图、定位、路径规划的的虚拟仿真效果。

图5 导航流程图

4.1 Hector-SLAM建图仿真

Hector-SLAM算法仅需要激光扫描仪采集的数据就可完成地图的构建，由于Hector-SLAM不需要里程计等传感器，使其应用场景非常广泛，如空中无人机、视觉设备等。在模拟化的ROS环境下进行仿真，首先要在ROS中机器人主要订阅“/scan”激光数据和“/syscommand”系统命令话题，由此向外发布机器人的位姿信息及栅格地图信息。其次通过ROS系统启动launch脚本功能，与此同时，使用键盘方向按键控制机器人在环境中的行进，在移动机器人行进的过程中，环境地图被完整构建出来，与真实场景基本吻合，未出现打滑、重影等情况。地图构建的过程如图6所示，栅格地图白色区域为空闲区域，一圈黑边表示障碍物区域，黑边上的红点为激光点云，浅灰色区域为未知区域。

图6 环境地图

4.2 路径规划仿真

在ROS环境下机器人要完成路径规划，首先需要机器人不管在何处都能估算出自身所处的位置，接着进行路径的规划。此路径规划工作可通过调用ROS中的两个功能包来完成。第一，调用AMCL功能包，对底层的蒙特卡洛算法进行改进，实现基于KLD重采样的AMCL算法，以准确评估出机器人所处的位置。第二，调用Move-base功能包，其中全局路径规划采用A*算法来完成，局部路径规划采用DWA动态窗口法，准确评估出最优路径。如图7所示，为移动机器人定位的结果。开始粒子云在初始位姿处随机分布，随着机器人的移动，粒子逐渐收敛到正确位姿。

图7 定位过程图

通过上述使用多种算法后，实现了SLAM环境地图的构建。由此接下来可以通过将地图加载在ROS框架上实现移动机器人路径规划以及自主导航仿真实验。此实验通过Gazebo进行机器人的虚拟导航仿真。在Gazebo中启动导航节点，最终的路径规划导航过程如图8所示，图中的红蓝区域表示障碍物膨胀区域，以此使得机器人更加有效实现避障。机器人的起点在左下角，终点在右上角，行进过程中规划的路径逐渐缩短，最终顺利到达终点。

图8 路径规划及导航过程图

5 结语

本文基于ROS的移动机器人自动导航系统设计研究为目的，着重对机器人环境感知能力、SLAM建图、AMCL定位、路径规划算法等开展了一系列讨论研究。通过在ROS模拟化场景下搭建导航框架，最终实现移动机器人自主导航功能。下一步将继续利用已有的硬件条件，在真实的场景中，基于ROS搭建自主导航系统，完成真实场景下的导航实验。