基于ROS的移动机器人导航系统研究与仿真

王 迪，黎 冠，于 腾，黎仁仕

(1. 华北科技学院 安全工程学院，北京 东燕郊 065201; 2. 华北科技学院 电子信息工程学院, 北京 东燕郊 065201)

0 引言

随着近年来机器人行业的较快发展，自主移动机器人在交通、物流、社会服务及应急救援领域的应用也越来越多[1]。自主移动机器人导航功能的实现，主要在于解决其在未知环境中自主定位、建立环境地图及路径规划等问题。同步定位与地图构建技术(Simultaneous Localization And Mapping, SLAM)的基本思想是利用机器人搭载的深度相机、雷达等传感器，使其在移动的过程中，实时获取周围环境的深度数据和点云数据等，以实现对未知环境的构建。目前，较为常用的激光SLAM方法，根据后端优化的不同进行分类，主要有基于滤波方法的EKF-SLAM、Gmapping等[2,3]，以及基于以图优化为代表的非线性优化方法的KartoSlam、Cartographer等[4,5]。

路径规划方面常用的算法有快速随机搜索树(RRT)算法[6]、蚁群算法[7]、A*算法[8]以及Dijkstra算法[9]等。近年来，在这些路径规划算法的改进方面，国内外学者做了大量的研究。潘成浩等[10]提出一种松弛Dijkstra算法，并证实在障碍物密集的环境中，机器人进行路径规划的效率比采用堆排序实现的Dijkstra算法和A*算法具有明显提高。XUE, Y.等[11]提出多目标进化算法，通过求解路径长度、安全性以及平滑度来获得精确有效的规划结果。QIAN, K.等[12]则提出一种基于PSO-MFB混合优化算法的自主移动机器人多目标路径规划方法，解决现有机器人路径规划探索效率较低，以及最优求解成本较高的问题。

综合已有的研究成果，本文利用机器人操作系统(Robot Operating System,ROS)，提出了一种多目标点导航算法。其算法基本原理包括：首先，根据建立完成的栅格地图，指定机器人需到达的多个目标区域，然后利用多目标点导航算法，通过枚举的方式，枚举出符合条件的最短路径。在各个区域之间，则利用基于ROS的Dijkstra算法和TEB轨迹优化算法进行全局、局部路径规划。利用该方式，可以实现机器人在执行巡检、搜救等任务时，通过输入一次指令，便可完成对多个目标区域的巡检、搜救等。

1 机器人模型及仿真环境设计

1.1 配置机器人模型

GAZEBO中配置机器人模型主要包括建立仿真模型和加载控制器。本文使用Xacro文件对机器人进行建模，如图1所示为建立的机器人模型，该机器人模型使用阿克曼转向机构，其中主要包括机器人主体平台、驱动轮、电池以及雷达、相机、IMU(惯性测量单元)等传感器。

图1 GAZEBO中机器人模型

在控制方面，ROS提供Ros_control功能包来实现和GAZEBO之间的通讯与控制，Ros_control提供有多种接口类型的控制器，并利用控制器管理器进行统一管理，控制较多控制器时，可利用spawner命令加载和启动多个控制器。

1.2 创建仿真环境

为开展后续的测试工作，本文利用GAZEBO软件建立了仿真环境地图，如图2所示。地图长、宽分别为25 m、20 m。通过走廊将地图划分为六块区域，其中，中央横向走廊宽度为2 m，其余走廊宽度为1.5 m。地图中定义了十二个目标区域，以用于后续在该仿真环境中进行路径规划和自主导航测试。

图2 仿真环境地图

2 建图方案设计

2.1 定位方案分析

在机器人的定位方面，通常可以利用轮式里程计或者视觉里程计等解决机器人在建图时的定位问题。由于本文利用GAZEBO仿真环境进行仿真研究，无法使用轮式里程计，而使用视觉里程计计算量较大。因此，本文选用激光里程计的定位方案。利用RF2O(Range Flow-based 2D Odometry)获取机器人的里程计信息，其数据处理过程主要包括：首先，将激光雷达采集到的数据处理成sensor_msgs/LaserScan消息类型的/sacn话题发布出去。其次则发布laser与base_link的tf/tfMessage消息类型的/tf_static话题。RF2O订阅发布的话题之后，经过处理，则将会发布nav_msgs/Odometry消息类型的/odom话题与tf/tfMessage消息类型的/tf话题。

在实际测试过程中，对于特征点较少的区域，里程计的测量结果容易出现错误，为提高定位的准确性，本文采用IMU配合激光里程计进行定位。IMU可用于检测机器人的运动状态和空间位姿。并利用滤波算法来融合IMU与激光里程计数据。ROS中的robot_localization功能包，可以通过扩展卡尔曼滤波的方式，将IMU和激光里程计的数据进行融合。并发布机器人的位姿、角速度及加速度等多种信息。因此，利用其进行传感器的数据融合。

2.2 建图算法分析

移动机器人SLAM可以理解为，在机器人的运行阶段，利用里程计获取到的自身运动数据(u1:t=u1,u2,…,ut)以及传感器获取到的环境数据(z1:t=z1,z2,…,zt)，对机器人的轨迹(x1:t=x1,x2,…,xt)进行估算，以及构建增量式环境地图mt的过程。

本文采用Gmapping的建图方案。其RBPF-SLAM算法的思想是：将机器人的轨迹x1:t和地图m的后验概率p(x1:t,m|z1:t,u1:t-1)分解为机器人的轨迹估计与地图估计两个后验概率的乘积，具体如(1)式所示。

p(x1:t,m|z1:t,u1:t-1)

=p(x1:t|z1:t,u1:t-1)·p(m|x1:t,z1:t)

(1)

其中，p(x1:t|z1:t,u1:t-1)为机器人轨迹x1:t的后验概率，由z1:t与u1:t得出。p(m|x1:t,z1:t)为地图m的后验概率，则由x1:t与z1:t得出。

利用RBPF-SLAM算法，在已知传感器的观测数据和里程计数据的情况下，构建环境的增量式地图。随着粒子的不断迭代，高权重粒子被逐渐保留，而低权重粒子则被舍弃。重采样阶段，若有效粒子数Neff小于设定阈值Nth时，则进行重采样。如(2)式所示，其中，w(i)表示归一化权重。

(2)

最后，利用每个粒子的轨迹x1:t以及观测信息z1:t计算相应的p(m|x1:t,z1:t)，以实现对地图的更新。

3 导航系统设计

机器人的导航系统设计总体共分为两个部分，首先利用多目标点导航算法，根据指令要求，计算起点至终点之间，依次经过目标区域的最短路径。然后在各个区域之间，利用基于Dijkstra算法的全局路径规划器，在区域之间规划出一条无碰撞路径。当机器人开始运行时，则根据规划出的全局路径以及雷达实时探测的情况，利用基于TEB算法的局部路径规划器进行实时调整，对于障碍物做到实时避障。基于ROS的导航方案设计中，局部路径规划器多数针对差速轮设计的，而基于TEB算法的局部路径规划器所提供的CAR-LIKE动力学模型则较符合阿克曼转向结构的特点。

3.1 导航系统框架

机器人的自主导航系统主要包括定位模块、坐标变换模块、里程计模块、路径规划模块以及传感器模块等。其整体结构如图3所示。

图3 机器人导航系统框架图

导航系统中，各模块的主要功能如下：

(1) 定位模块用于在构建完成的栅格地图基础上进行定位，根据机器人自身的里程数据以及地图特征，利用基于粒子滤波的蒙特卡罗算法，修正机器人在已知地图中的位置。

(2) 坐标变换模块用于确定机器人在地图中的位置以及建立传感器数据与静态地图之间的联系。

(3) 里程计模块用于实时发布nav_msgs/Odometry消息类型的里程计消息，包含有机器人的速度等信息。

(4) 地图服务模块用于获取构建完成的栅格地图信息。以实现在路径规划模块中，利用costmap_2d软件包，生成全局、局部代价地图。

(5) 传感器模块用于实时发布激光扫描信息等，以避开周围环境中的障碍物。

(6) 路径规划模块包括全局、局部路径规划和局部位姿调整等。根据全局、局部代价地图，利用基于ROS的Dijkstra算法与TEB算法，在各区域之间，进行全局和局部路径规划。

(7)运动控制模块用于订阅move_base发布的geometry_msgs/Twist类型的/cmd_vel话题，并将速度信息转换成电机控制指令，进而控制机器人运动。

3.2 全局路径规划算法分析

针对目标区域之间的全局路径规划，本文利用基于ROS的A*算法与Dijkstra算法进行的实验测试，在仅设定终点、未设定中途目标区域的情况下，机器人从1号区域自主导航至12号区域。算法的实际测试结果如图4所示。从图中可以看出，利用两种算法均能得出从起始位置至终点位置的无碰撞路径。但在路径的总体长度、平滑性等方面，Dijkstra算法表现较好。相关数据对比见表1。综合以上因素，选择Dijkstra算法作为机器人的全局路径规划算法。

图4 全局路径规划算法测试结果

表1 导航测试数据对比

4 实验结果分析

4.1 建图结果分析

本实验使用的操作系统版本为Ubuntu16.04，ROS版本为ROS-Kinetic，仿真环境和显示界面使用GAZEBO7.16和RVIZ。机器人起始位置设置在1号区域，通过搭建的键盘控制节点控制机器人进行运动，设计时速为7.2 km/h，通过搭载的Kinect相机，可以查看机器人行进过程中的实时画面。如图5所示为机器人的建图场景。

图5 机器人建图过程图

机器人在建图过程中，通过Gmapping建图算法构建仿真环境的二维栅格地图，最终启动地图服务器保存扫描完成的地图，其结果如图6所示。根据保存的地图结果可以看出，利用Gmapping建图算法建图的效果较好，在长直走廊等特征点较少的区域时，合理控制行进速度，可减少漂移问题的发生。

图6 仿真环境建图结果

4.2 导航结果分析

根据建立完成的仿真环境地图，开展机器人的导航功能测试实验。首先，需在程序中设定地图中各个区域的中心位置坐标，以及各个区域之间的连接关系。其次，设定机器人的起始位置以及朝向。规划路径时，首先利用枚举的方式枚举出从起始位置到终点位置之间，所有满足条件的路径，本实验设置最大步长为10步，达到最大步长仍未枚举出路径时，则定义为任务失败。然后在这些满足条件的路径中，根据步长及计算的距离选取最短路径。

导航测试的起点、终点分别为1、12号区域，中途目标区域为3、7号区域。图7为机器人的导航过程图，图中红色点云表示雷达扫描出的障碍物信息，绿色线表示规划出的全局路径。机器人从起点开始，沿着绿色线依次经过2、3、7、11号区域，最终到达终点，步长为6步，满足最短路径要求。RVIZ中的显示模式为代价地图模式，黑色区域表示障碍物，越靠近障碍物的代价值越大，灰度值越高，表示越危险。机器人在行进过程中，根据激光雷达探测的实时情况，遇到障碍物时，便可结合TEB局部路径规划器进行调整。

图7 机器人导航过程图

根据以上分析表明，机器人在路径规划方面以及导航过程中的表现效果较好。其中，在配置导航功能时，由于参数设置不合理，导致无法规划出前进路线时，机器人便会出现停滞或者倒退的状况。因此，需要将TEB局部路径规划器的参数进行优化，其中一些关键参数见表2。

表2 TEB局部路径规划器相关参数

5 结论

(1) 在定位方面，当无法使用轮式里程计时，利用激光里程计和IMU的多传感器融合的定位方式，可以有效提高机器人在建图时定位的准确性。

(2) 在建图方面，利用Gmapping的建图方案，建图效果准确，误差较小。经过特征点较少的区域时，需控制合理的行进速度。

(3) 在路径规划方面，利用多目标点导航算法，可规划出最短路径，有效解决了机器人针对多目标点的导航问题。并且基于ROS的Dijkstra算法，在规划路径的长度、平滑性等方面的表现优于A*算法。

(4) 本次仿真测试验证了多目标导航算法的可行性，下一步会对该算法进行进一步的优化，并搭建实物，测试机器人在巡检、搜救等方面的实际表现。