基于ROS的机器人定位与导航系统的仿真设计

熊 安 ，卞春江 ，周 海 ，刘 成

（1.中国科学院国家空间科学中心北京100190；2.中国科学院大学北京100094）

实现机器人像人一样能够在真实环境中行走自如一直是人类的梦想，也是机器人研究领域中最重要的话题。解决该问题的主要关键点在于如何实现机器人的自主定位和自主路径规划。现实生活中往往存在GPS信号不稳定，定位误差大等缺点，尤其对于室内环境存在GPS信号不可用的情况，此时传统的定位方法不在适用。同时定位与地图构建（Simultaneous Localization and Mapping，SLAM）技术是解决在机器人在无GPS等外部定位信息时进行自主定位的最佳手段。该问题是指移动机器人在陌生环境中利用自身传感器数据，实现自身定位且利用定位信息在定位过程中逐步构建环境地图[1]。SLAM技术是移动机器人自主化，智能化的重要手段，因此近十几年以来，该问题始终在机器人研究领占据着重要地位。

ROS（robot operating system）是一个开源的机器人操作系统。它提供了很多关于机器人的硬件抽象、以及常用功能的实现，使得机器人的开发更加便捷迅速，很快在机器人研究领域成为研究热潮。ROS中有很多封装好的代码，用于实现一些常用功能，可以使人们在开发机器人中把研究重点放在核心算法的研究和改进上，极大地提高机器人开发效率[2-4]。基于A*的路径规划算法和基于DWA的局部避障算法[5-7]在ROS中被封装为Navigation的软件包，该软件包能够实现在已知地图和未知地图的环境中机器人的路径规划和自主避障功能。文中基于ROS系统，设计实现了基于ROS的机器人导航和建图的软件系统，并利用Gazebo3-D仿真验证系统的实用性。

1 地图构建与定位算法概述

SLAM技术经过几十年的发展，已经初步形成了基本的理论框架。其按照传感器分类可主要分为基于测距的SLAM和基于视觉的SLAM[8]。基于测距的SLAM技术已经形成成熟的理论框架[9]。文献[10]对基于视觉的SLAM技术的发展做了分析和总结。

基于图优化的SLAM是近几年发展起来的新方法，主要以视觉传感器为主，是一种在线的全SLAM解决方法。它在视觉前端和优化后端中加入回环检测环节，使得构建地图时能能够识别之前到达过的场景，很好地解决了移动中的累计误差，但是计算量较大[11]。其中典型的代表有ORB-SLAM、LSDSLAM、SVO以及RTAB-MAP[12-16]。这些算法大多是基于视觉的SLAM技术，并且采用并行化的思想。由于算力的提升，使得以摄像头为主要传感器的视觉定位成为了如今的主流。但是在小型的嵌入式设备上，以图优化的SLAM的实用性大大降低。

基于贝叶斯滤波的SLAM方法是传统的SLAM解决方案。其中最早的SLAM解决方案是以卡尔曼滤波器为主要手段。扩展卡尔曼滤波虽然在一定程度上弥补了卡尔曼滤波器的不足，但是对于难以建模的复杂场景仍无能为力，且由于维护需要维护每一步计算的雅克比矩阵，计算量巨大。粒子滤波是利用样本的分布来表示系统的状态分布，因此对于复杂环境的建模往往比较容易实现，且估计精度和计算效率较高[17]。文献[18]提出的基于Rao-Blackwellized粒子滤波的RBPF-SLAM是最近几年出现的最为实用的一种SLAM算法。该方法将地图和机器人位姿的联合后验分布分解为对于机器人路径的后验估计和已知位姿时对于地图的估计，在提高定位与建图精度的同时也兼顾了算法的效率。该算法在ROS中被制作成名为Gmapping的软件包。它利用里程计信息和激光雷达测距信息可以在室内建立高精度的二维栅格地图，能够实现机器人自主定位和环境地图构建的功能。

2 ROS系统简介

ROS设计的初衷是提高机器人的开发效率，使得人们对于机器人的开发主要集中在关键问题的研究上，能够快速开发出用于实验验证的机器人。它是一种分布式的处理框架。程序在运行时，所有的进程以及他们所进行处理的数据都会通过一种点对点的网络表现出来，这一级主要包括：节点（Node）、消息（message）、主题（topic）、服务（Service）。节点就是执行计算任务的进程。节点之间的通信通过消息传递。消息以一种订阅和发布的方式进行，每一种消息都是一种严格固定的数据结构[19]。ROS节点之间的通信方式模型如图1所示。

图1 ROS节点的通讯方式

图1中椭圆表示计算的节点（node），矩形表示话题。一个节点既可以发布话题，也可以订阅相应的话题来接受消息。在以上基础上，需要有一个进程控制所有的节点有条不紊的执行，该进程就是ROS Master，其模型如图2所示。

图2 ROS进程间通信模型

该进程控制所有进程的话题和消息，节点之间的通信通过ROS Master进行。节点与节点之间的建立是直接的，ROS Master负责提供了查询信息，就像一个DNS服务器。节点订阅一个主题将会要求建立一个与发布该主题的节点进行连接，并且会在同意连接协议的基础上建立连接。所有的服务由ROS控制器[20]ROS Master控制。

3 基于ROS的系统设计

ROS中提供给了大量的功能级软件包，包括用于移动机器人在室内环境下定位与建图的Gmapping软件包、用于导航和避障的Navigation包、处理激光雷达点云的软件包等。这些软件包被封装成一个一个节点和功能包集以便开发调用。如第二节所示，ROS的通信方式为我们利用这些算法提供便利。在实现机器人的定位与建图时需要利用软件包Gmapping，它是一套使用机器人传感器和里程计功能的算法。该软件包基于RBPF算法，提供基于激光雷达的SLAM功能。节点名为slam_gampping。该算法能够输出机器人的精确定位和栅格化的二维环境地图。ROS的导航功能包集是一个名叫move_base的软件包。该软件包能够利用里程计信息的激光测距信息实现精准的定位与导航功能。其系统组成如图3所示。

图3 ROS导航功能的配置项

图3中，move_base_simple/gaol是一个ROS消息，人为给定的，该消息就是机器人导航的目标地点；amcl是蒙特卡洛定位，其利用粒子滤波算法来确定机器人在构建的grid map地图中的位置，在无地图时可以不用配置；sensor transform是用来实现机器人内部各传感器之间复杂的坐标变换，如编码器到机器人中心的坐标变换，激光雷达到机器人中心的坐标变换等；odometry为里程计发布信息，该信息是用来获取机器人的运动轨迹的预测；sensor传感器模块是检查机器人运动中的障碍物的，本文中为激光测距仪；全局路径规划global_planner是默认情况下是基于Dijkstra算法，本文设置为更高效的A*导航算法；局部规划local_palnner是基于动态窗口的DWA导航算法；map_serve节点是用来在导航中获取地图数据的。recovery_behaviors节点是用来配置机器人在导航失败后的执行动作。其中规划所用的参数配置分别用全局代价地图global_costmap和局部代价local_cosmap文件来进行设置

3.1 导航系统的配置

机器人通过两种导航算法在地图中移动，分别是全局导航和局部导航。全局导航用于从起点到终点的路径规划，局部导航用来躲避临时障碍物。这些信息通过代价地图来处理，他们有着基本的通用参数[21]，这些参数会保存在共享文件中。调用这两个算法需要配置3个文件。

3.1.1 通用文件配置

该文件主要用于配置基本参数，这些参数会用于局部规划器和全局规划器，其中配置如下：

其中obstacle_range和raytrace_rang分别表示传感器探测的最大距离和在代价地图中引入的探测的障碍物距离信息。参数footprint用于将机器人的几何参数告知导航功能包，由于在导航时，导航路径的规划是以质点的形式进行的，并没有考虑机器人的几何信息，该参数可使得机器人在导航时与障碍物能够保持安全距离。inflation_radius为机器人的半径。其他两个为导航时的传感器信息描述，本文用的是激光雷达。

3.1.2 全局代价地图配置

全局代价地图配置是用于全局路径规划的相应参数，其配置如下：

其中gloab_frame和robot_baseframe是用于定义地图和机器人之间的坐标转换。update_frequency是用来配置全局规划的频率的，由于在移动中会遇到障碍物等信息，局部规划会改变原有路径，因此需要实时更新全局路径来实现动态的避障效果。static_map表示使用的是静态地图数据，即地图是预先创建的。

3.1.3 局部代价地图的配置

局部代价地图是在机器人进行局部避障时需要设置的参数。由于ROS中的局部导航是基于动态窗口的DWA算法。因此需要告诉规划器机器人自身性能相关参数。其配置如下。

在局部代价地图的配置中，以下3个参gloab_frame、static_map、robot_baseframe、update_frequency与全局代价地图的参数含义相同，参数publish_frequency表示局部规划器发布信息的频率，rolling_window参数表示代价地图始终以机器人为中心。参数transform_tolerance表示转换的最大延迟，planner_frequency为规划算法的循环频率[22]，planner_patinete为寻找一条有效路径的等待时间，其余参数为地图的尺寸和分辨率。max_vel_x之后的参数表示机器人的最大速度，最大角速度，最大加速度和角加速度等信息，这些信息用于速度采样对于速度空间的限制。

配置完成以上文件之后，就可以编写仿真的launch文件了，launch文件的编写可参考文献[4]，其中主要是加入导航时的一些节点和以上配置文件的路径和文件名。以上参数主要根据本文设定的机器人几何信息指定，每个参数都是具体化的数据，对于不同机器人，其配置可能略有差异。

3.2 SLAM系统的配置

ROS已经为我们提供了机器人建图和定位的Gmapping算法，开发者只需要根据自己的机器人修改相应的参数即可，这些算法为我们开发机器人提供了极大的便利。将这些软件包配置起来需要以下步骤：

1）让机器人发布所有传感器相对于自身的位置关系，以机器人的中心为坐标原点；

2）机器人提供运动消息cmd_vel，包括线速度和角速度。由里程计（odom）发布；

3）接受激光雷达的信息来实现地图的构建和定位；

机器人在开发时，需要统一机器人本体坐标系和各个传感器之间的坐标系，在坐标系比较多时，人工管理工作量巨大，非常不便，ROS提供了TF软件包可以方便的实现这些坐标的管理。在本文中主要是激光传感器相对于小车中心的位置关系。机器人利用里程计信息估计自己的轨迹和位姿，为了减少测距的误差同时利用激光观测值来修正定位精度，随后采样和更新地图。这是基于RBPF粒子滤波算法的核心，具体原理见文献[18]。

4 实验仿真及结果

经过上节的配置之后，就可以编写launch文件进行仿真测试了。本实验在以下环境中进行，ROS版本ROS-kinetic，3-D环境模拟Gazebo8.0，操作系统版本：Ubuntu16.04。基于以上配置，其中SLAM算法仿真的具体操作如下：

1）启动机器人模型和Gazebo仿真环境，该模型由URDF文件描述；

2）启动Gmapping节点；

3）启动键盘控制节点，控制机器完成环境建图。

机器人的URDF文件可参考文献[4]。Gmapping节点的启动也是以launch文件的形式。其发布话题/map。控制机器人在仿真环境中行走需要启动键盘控制节点，该节点在ROS中是名为teleop_twist_keyboard。调用命令：rosrun teleop_twist_keyboard.py。启动该节点后，就可以利用键盘控制机器人在模拟环境中移动，其中i表示前进，k表示暂停，l表示右转，j表示左转。建图后调用命令rosrun map_server map_saver-f map完成地图保存，其中map为地图的名字，可任意指定。

其中三维的真实环境如所示图4所示。

图4 Gazebo仿真环境

仿真结果如图5所示。

图5 最终建立的栅格化地图

导航算法的仿真步骤主要是启动move_base节点，调用amcl算法实现地图中的自定位，然后在rviz中点击2-D Nav Goal并制定目标，此时机器人会根据创建的环境地图来规划一条可执行的路径。并且根据激探测的障碍物信息进行局部避障。实验结果如图6、图7、图8所示。

图6 全局路径规划

图7 避障重新规划

图8 导航完成

5 结论

文中利用ROS提供的模拟环境Gazebo以及Gmapping等相关软件包，设计实现了用于机器人定位与建图的SLAM系统，同时又利用ROS中提供的导航功能包，设计实现了基于ROS的机器人导航系统。实验仿真结果表明，该仿真设计系统能够在搭载激光雷达传感器的轮式机器人中实现室内环境的定位、建图和导航等功能。对实际机器人的应用具有一定的指导价值。但是本文只是在模拟环境中实现了该系统的仿真，后续将在此系统的基础上修改相关节点驱动，将模拟的激光雷达和机器人等替换成真实的机器人驱动，把本系统设计应用于真实机器人中。