基于ROS的移动机器人自主建图与路径规划*

温淑慧， 问泽藤， 刘 鑫， 温淑焕

(燕山大学 河北省工业计算机控制工程重点实验室， 河北 秦皇岛 066004)

近年来，移动机器人技术发展迅速，已经应用到农业、服务业、物流业等多个领域.导航技术作为移动机器人技术研发的关键技术，主要包括SLAM(实时定位与建图)和路径规划部分.SLAM[1]需要完成机器人的定位与建图任务，而路径规划问题实际上就是依据某个或者某些优化准则(如实时性、安全性、最优性等)，为移动机器人规划出一条成功躲避从初始点到目标点的移动过程中所遇到的动态障碍物的可行路径.

针对移动机器人SLAM问题的解决方法，王元华等[2]通过对局部地图和全局地图的线段特征关系进行扫描匹配，实现机器人的全局定位，完成机器人在未知环境下的SLAM.

针对路径规划问题，传统的路径规划算法有常用作全局路径规划的Dijkstra法、BFS算法及A*算法，以及常用作局部路径规划的动态窗口法(DWA)、人工势场法[3].Dijkstra算法的优点是即使在存在障碍物的环境下，它仍能保证找到一条从初始节点到目标节点的最短路径，但是运行速度较慢[3].传统的人工势场法结构简单，便于控制，但容易发生障碍物附近震荡以及所得路径并非最优路径的情况[4].

为了提高移动机器人导航的自主性能，本文设计了基于ROS的移动机器人自主建图与路径规划系统，通过搭载的2D激光雷达获取周围环境信息，同时利用姿态传感器(IMU)获取机器人的姿态和加速度信息，利用Gmapping算法实现机器人的自主定位与建图.路径规划部分又分为全局路径规划及局部路径规划.由于传统A*算法的时间复杂度与起始点位置、节点数量密切相关，为了提高搜索效率，本文对A*算法进行改进，采用基于头尾双向搜索的A*算法作为全局路径规划方法，局部路径规划方法采用经典的DWA算法[5]来实现避障.

1 自主导航系统设计

1.1 AMCL定位系统

AMCL定位系统是移动机器人在二维环境中的概率定位系统，核心是一种利用粒子表示置信度的自适应蒙特卡罗算法[6]，其主要原理是根据环境地图，利用粒子滤波器来跟踪机器人的位姿，根据采样粒子估计机器人位姿的概率分布，机器人位姿估计的可靠程度由粒子权重决定[7].算法主要由预测位姿和更新位姿两部分组成，首先将随机抽取的位姿粒子均匀分布在整个位姿空间，之后根据传感器信息，为每个粒子分配重要性系数，预测t时刻的机器人位姿，并估计其运动轨迹[8]；然后再次通过传感器的扫描数据，更新采样粒子的重要性系数，重要性系数越大，说明其位姿越接近于真实位姿.同时，对粒子进行重采样，使得定位更加准确，在采样粒子小于阈值时不断重复这一过程.

在启动导航时，AMCL根据所配置的参数情况初始化其粒子滤波器，并在Rviz中使用2D Pose Estimate按钮来初始化位姿.

1.2 基于2D激光雷达的SLAM系统

根据传感器进行分类，SLAM的实现方法主要分为两类：利用摄像头获取外部图像数据来解析环境信息的视觉SLAM和依赖于激光雷达的激光SLAM.其中，视觉SLAM由于图像信息复杂、包含信息量大，很难做到实时运算并占用较大的CPU空间和内存空间.相比较下，本文所使用的激光雷达通过向目标环境发射激光数据，并根据接收反射回来的数据进行对周围环境的感知，进而进行地图和位置的计算.此外，摄像头在室外工作时受环境、光照影响较大，而激光雷达受光照影响较小，即使是在黑暗条件下，也能很好地工作.

本文采用基于RBPF滤波算法的Gmapping算法进行SLAM.由于传统的RBPF算法在实际应用中计算量较大，在构建地图时需要较多的粒子进行重复采样[9]，导致粒子退化、多样性降低.Gmapping算法通过改进粒子采样过程中的提议分布来降低计算位姿所需要的粒子数量；同时提出选择性重采样，为采样粒子数设置一个合适的阈值，只有在满足条件时才进行重采样.

1.3 路径规划

1.3.1 全局路径规划

传统A*算法是一种启发式的路径搜索方法，它结合了BFS和Dijkstra算法的优点，其估价函数的一般形式为

f(n)=g(n)+h(n)

(1)

式中：f(n)为节点n从初始节点到目标节点所消耗代价值；g(n)为初始节点到任意节点n耗费的真实代价值；h(n)为机器人从节点n移动到目标节点所消耗代价值的启发函数[10].整个算法的关键是对启发函数h(n)的选取.

预估移动代价h(n)的计算方法有多种，例如曼哈顿距离、欧几里得距离、对角线距离[11]等，其中，曼哈顿距离表示为

h(n)=|xd-xn|+|yd-yn|

(2)

式中：xd、xn分别为目标点和节点n的横坐标；yd、yn分别为目标点和节点n的纵坐标.

A*算法的时间复杂度与起始点位置以及节点数量密切相关.实验环境1如图1所示，当起始点(浅色方块)位于宽阔地带，目标点(深色方块)位于狭窄地带时，扩展的节点较多(如图1a所示).而当起始点与目标点互换位置时，扩展节点明显减少(如图1b所示).

图1 实验环境1下A*算法仿真图Fig.1 Simulation diagram of A* algorithm under experimental environment 1

为了证明结果的非偶然性，在两种不同环境中再次进行了仿真对比，如图2、3所示.

图2 实验环境2下A*算法仿真图Fig.2 Simulation diagram of A* algorithm under experimental environment 2

图3 实验环境3下A*算法仿真图Fig.3 Simulation diagram of A* algorithm under experimental environment 3

多组实验证明，A*算法的搜索效率与始末位置密切相关.不同起始位置下的算法性能分析表如表1所示.由表1可知，起始位置对搜索效率影响很大，当起始位置位于开阔位置时，搜索节点数是位于狭窄位置的一倍，搜索时长增加，因此，传统A*算法存在明显的缺陷.

表1 不同实验环境下A*算法不同起始位置性能分析

Tab.1 Performance analysis of different starting positions in A*algorithm under different experimental environments

实验环境起始位置搜索时间/ms扩展节点数环境1环境2环境3对调前2.345202对调后1.25095对调前1.850143对调后0.89074对调前2.255199对调后1.10086

为了提高路径规划的效率，改善上述缺陷，在传统A*算法搜索策略上提出了改进方法——基于头尾双向搜索的A*算法[11].传统A*算法搜索范围是从初始节点开始的，而改进算法是对初始节点以及目标节点同时进行扩展搜索，直到扩展节点为同一个子节点时，结束搜索.此时遍历所有父节点，可得到最优路径.双向A*算法不仅改变了搜索策略，同时相应调整了启发函数h(n).改进后的A*算法启发函数为

h(n)=|xsn-xen|+|ysn-yen|

(3)

式中：xsn为当前节点横坐标；xen为另一搜索方向上当前节点横坐标；ysn为当前节点纵坐标；yen为另一搜索方向上当前节点纵坐标.

对于每个节点需要进行上、下、左、右等8个方向的遍历，直到某个扩展节点的启发函数为0，结束搜索.

在不同的实验环境下进行仿真实验，对调起始位置和终点位置，得到改进A*算法效果图如图4所示.

图4 不同实验环境下改进A*算法仿真图Fig.4 Simulation diagram of improved A* algorithm under different experimental environments

不同实验环境下改进A*算法性能表现如表2所示，通过与表1对比可知，改进后的A*算法性能较稳定，无论初始位置位于狭窄地带还是宽阔地带，其搜索时间和扩展节点数近似一致.对比传统A*算法，搜索效率有所提高.

表2 不同实验环境下改进A*算法性能分析Tab.2 Performance analysis of improved A* algorithm under different experimental environments

1.3.2 局部路径规划

DWA算法可以实现实时路径规划以及实时避障，其基本原理是：计算移动机器人每个周期内应该行驶的线速度和角速度.先离散采样机器人控制空间，再根据机器人当前的状态预测可能出现的状况，并根据一些度量标准(如障碍物接近目标、接近全局路径和速度等)来评价每个轨迹结果，舍弃不合适的路径(有障碍物碰撞的)，最后挑选得分最高的运动轨迹，并发布相关的速度给移动平台.DWA算法流程图如图5所示.

图5 DWA算法流程Fig.5 Flow chart of DWA algorithm

2 移动机器人真实环境实验

2.1 硬件平台

硬件平台主要以主控电脑为核心，外部搭载激光雷达传感器，用来采集机器人周围所有与物体相关的距离信息；同时也搭载了姿态传感器(IMU)，用来获得机器人整体的姿态信息和加速度信息.使用直流无刷电机来控制其驱动装置；使用舵机用来控制整体的转向，通过单片机作为下位机的控制器，接收PC端传输的控制信号，进一步再对直流无刷电机和舵机进行控制，进而实现对车体速度和方向的控制.由于机器人并不配有屏幕，所以为其搭载了路由器，方便操作人员进行远程控制的工作.硬件系统总体框架如图6所示.

2.2 软件平台

软件主要以ROS机器人操作系统为框架主体，在ROS机器人操作系统上，添加激光雷达和IMU的姿态传感器的驱动程序，这样就可以获得周围环境的信息和机器人整体的姿态信息；通过无刷直流电机和舵机控制机器人的移动来采集整体的环境信息.将这些采集到的数据进行处理，利用Gmapping算法完成对整个环境地图的构建，获得全局静态地图；再通过路径规划和自主导航来控制机器人的位置.在电脑的ROS端上，启用ssh远程访问，用以远程控制整个系统.系统软件框架如图7所示.

图6 系统总体框架Fig.6 General frame of system

图7 系统软件框架Fig.7 Software frame of system

2.3 路径规划实验

本文以实验室走廊为基本环境，在其运动过程中，添加纸盒、锥桶等作为障碍物，模拟室内基本环境.实验环境如图8所示.

通过使用ROS中的Gmapping功能包完成建图工作，得到二维环境地图.使用Navigation功能包完成自主导航工作，并在Rviz可视化界面中观察规划路径情况.图9即为规划出的全局路径，局部代价地图随着机器人的移动不断更新，相应地，局部路径也发生改变，顺利避开障碍物.

3 结 论

本文针对移动机器人导航缺乏自主性问题，设计了基于2D激光雷达的SLAM系统；提出基于头尾双向搜索的A*算法用于路径规划工作，并进行仿真研究.仿真结果表明，与传统A*算法相比，该方法的搜索效率大大提高.在真实环境中进行实验验证，结果表明，采用基于头尾双向搜索的A*算法可以规划出一条最优路径，同时，DWA算法能够完成对动态未知障碍物的检查与躲避任务，最终到达目标位置，从而提高移动机器人的自主导航性能及工作效率.

图8 实验环境Fig.8 Experimental environment

图9 全局路径规划提取Fig.9 Global path planning extraction