基于ROS的应急监测机器人导航系统设计与实现

李贻文，邹树梁，张 德，陈宏斌

（南华大学 机械工程学院，衡阳421000）

核与辐射等相关突发事件具有爆发突然、危害巨大且迅速、影响深远的特点，在核与辐射等相关环境下，救援先遣队伍面临先期的辐射、侦检、环境侦察、指挥决策等一系列关键问题。为了让救援先遣队员减少辐射的伤害，在救援前能够构建好地图就显得尤为重要。随着机器人技术的不断发展，移动机器人在安防、医疗、服务、军事方面的应用越来越广泛。机器人代替人侦检具有辐射的场所并得到精准的环境地图成为可能。其中，同步定位与地图构建SLAM（simultaneous localization and mapping）技术作为机器人自主运动的关键[1-3]之一，具有重要的理论研究价值和实际应用意义。

本文在研究和分析SLAM 算法的基础上，将设计实现室内定位建图以及导航的应急监测机器人，基于ROS 平台利用激光雷达进行周围环境感知，利用Cartographer SLAM 算法，实现在环境中的自主定位并构建出增量式环境地图的方法，实时性好，为移动机器人的自主导航提供基础，同时应用Move_base 导航包完成已知地图的路径规划，设计并实现室内定位和导航系统。

1 基于ROS 的室内建图和导航系统实现

基于ROS的应急监测机器人系统定位和导航系统实物照片如图1所示，可作为室内应急监测机器人SLAM、导航以及ROS 框架的基础实验平台，主要硬件模块有STM32、ROBOSENSE-LIDAR-16 激光雷达、惯性计量单元（IMU）、电源模块，各模块的硬件连接关系如图2所示，加入视觉传感器或机械臂后，可实现更多的功能。

图1 应急监测机器人实物图Fig.1 Physical map of emergency monitoring robot

图2 应急监测机器人系统总体框图Fig.2 Overall block diagram of emergency monitoring robot system

2 系统软硬件设计

2.1 系统硬件设计

2.1.1 机械结构设计

应急监测机器人采用履带式底盘车，如图3所示。机器人的主体采用不锈钢和合金板，不锈钢和合金板具有良好的稳定性和抗撞击能力。采用四轮组平衡悬挂，可适应重载避震，全车安装6 枚避震器结台独特的撞击吸能设计，可有效减少地形冲击，特有张紧履带连接，可适用于较为复杂的环境，在机器人内部有留有足够的空间可以作为搭载平台。

图3 应急监测机器人三维模型Fig.3 Three dimensional model of emergency monitoring robot

2.1.2 控制处理器

由于一般的工业机和普通的笔记本电脑的质量和体积都较大，不适合在空间不大的应急机器人上使用，因此本文选用NVIDIA 公司生产的Jetson TX2 的计算机模块板作为控制处理器，可为嵌入式计算设备提供出色的速度与能效。这款超级计算机模块配有NVIDIA PascalTM GPU、高达8 GB 内存、59.7 GB/s 的显存带宽。其尺寸为50 mm×87 mm，是由双核Denver 2 64 位CPU 和四核ARM A57 Complex，具备一个USB 接口，一个HDMI 接口。因此NVIDIA Jetson TX2 可以满足SLAM 算法的实时运算要求，也能满足在应急机器人平台上体积小的要求，实物如图4所示。

图4 Jetson TX2Fig.4 Jetson TX2

2.1.3 激光雷达

本文采用的激光雷达是深圳市速腾聚创科技有限公司生产的RS-LIDAR-16 国产16 线激光雷达，如图5所示。RS-LiDAR-16 通过16 个激光头同时发射高频率激光束对外界环境进行持续性的扫描，出点数高达640000 点/s，测量距离达到150 m，达到目前世界顶级的测量精度2 cm 以内。

图5 激光雷达Fig.5 Lidar

2.2 系统软件设计

2.2.1 ROS 系统

本文软件平台ROS 操作系统是基于LINUX 系统上的一个次级操作系统。ROS 软件模块已经可以使用任何已开发了客户端库的语言来写，包括C++、Pthon 等[4]。ROS 集成很多开源软件包供开发者修改并使用，可用于建图、导航、路径规划等功能的实现。且提供了RViz、Gazebo 等仿真软件，通过仿真可以验证算法的有效性等，有助于快速完成机器人系统的开发[5]。ROS 是分布式进程，可执行进程的最小单位（节点，Node）的形式进行编程，每个进程独立运行，并有机地收发数据。由于它以功能包的形式管理着多个具有相同目的的进程，所以开发和使用起来很容易，并且很容易共享、修改和重新发布[4]。

2.2.2 ROS 分布式主从通信部署

在ROS 中，每一个进程都以一个节点的形式进行，可以分布于多个不同的主机。节点间的通信消息通过一个带有发布和订阅功能的RPC 传输系统，从发布节点传送到接收节点。这种点对点的设计可以分散定位、导航等功能带来的实时计算压力。得益于这种点对点的设计，本文可以将即时定位与地图构建以及导航算法运算需要处理的大量实时传感器数据传输到PC 端，将实时生成地图数据显示在PC 端的屏幕上，具体的分布式主从通信部署如下：

（1）PC 端作为从机，控制器Jetson TX2 作为主机，在同一个WIFI 下分别设置固定IP，分别在主从机的终端输入ifconfig 与hostname 得到各自的IP地址和计算机名，在到自己的系统的/etc/hosts 文件中添加对方的IP 地址与名字。

（2）设置ROS_MASTER_URI，ROS_MASTER 的位置可以用环境变量ROS_MASTER_URI 进行定义，在主机和从机中的.bashr 文件的最后添加。

主机：

export ROS_HOSTNAME=jetson-0421518035271

export ROS_MASTER_URI=http：//jetson-042151 8035271：11311

从机：

export ROS_HOSTNAME=LAPTOP

export ROS_MASTER_URI=http：//jetson-042151 8035271：11311

（3）测试。在主机启动turtlesim_node 节点，从机中启动turtlesim turtle_teleop_key，看是否能在从机上控制主机上的节点。

3 室内建图与导航功能实现

3.1 SLAM 算法

SLAM 模型如图6所示，已知机器人的控制数组c1：t=｛c1，c2，…ct｝和观察数组e1：t=｛e1，e2，…et｝，求环境地图mt=｛m1，m2，…mt｝和机器人的实际位姿x0：t=｛x0，x1，…xt｝，其中xt表示t时刻给机器人的位姿；mt表示环境特征；ct表示t时刻给机器人的控制量；et表示t时刻传感器的预估值，机器人从未知环境的未知地点出发，在运动过程中通过反复观测到的环境特征，定位自身位姿，再根据自身位姿增量式地构建地图，从而达到同时定位和地图构建的目的。

图6 SLAM 模型Fig.6 SLAM model

主流的SLAM 算法中，包括传感器获取数据、前端配准、后端优化、地图构建4 个模块，而针对后端优化板块可以分为两类：一类是基于粒子滤波算法，另一类是基于图优化的算法。

（1）基于滤波器的Gmapping 算法

现在主流的基于滤波器的SLAM 算法是Gmapping 算法，其基本原理是RBPF 粒子滤波算法[6]，即将定位和建图过程分离，先进行定位再进行建图。Gmapping 算法可以实时构建室内地图，且在较小的环境地图中构建出来的地图精度与Cartographer 算法构建出来的精度并无太多差别，计算量少于Cartographer，但随着场景增大所需要的粒子数也随之增多，而每一个粒子都相当于一副地图，在构建大场景地图时，所需要的计算内存和计算量骤增。

（2）基于图优化的Cartographer 算法

Cartographer 算法采用基于图优化的方法，在前端部分接收激光雷达的数据后进行匹配，匹配到一个雷达位姿以后，将当前帧的激光插入到子图当中，当子图中的位姿没有变化或者变化较缓慢，就会将其去除，将变化的激光位姿不断更新从而得到子图。后端部分采用分支定界法对地图搜索进行加速，构建一个回环检测，来消除子图的匹配误差[7-9]。

相比较Hector 和Gmapping 两种算法，其鲁棒性好，尤其在没有IMU 的辅助下，应对旋转速度高的转向，没有出现建图错误，且采用了回环检测，可以消除累计误差，建图效果比较稳定[10]。本文采取的是Cartographer 算法作为SLAM 的算法。

3.2 导航功能实现

应急监测机器人的导航功能实现使用ROS 中的move_base 软件包，move_base 包可分为全局路径规划，局部路径规划，里程计信息以及传感器信息4个模块。Move_base 软件包的总体框架图如图7所示。

图7 Move_base 框架图Fig.7 Move_base frame diagram

全局路径规划：通过全局代价地图进行全局的路径规划，计算出机器人到目标位姿的路线。这些功能通过Dijkstra 最短路径的算法实现，或者是A*算法实现。Djikstar 和A*都能实现功能，但A*算法较Djikstar 消耗时间少，可以让机器人在导航过程中连续的运动，本文采用的是A* 算法作为全局路径规划的算法。

局部路径规划：局部路径规划通过Local_planner 中的cost function 与全局路径规划中的参数连接，局部规划的路径会靠近全局规划的路径，并结合实际情况，对局部路径进行调整。局部路径规划的算法按速度采样空间的不同，可以分为Dynamic Windows Approach（DWA）算法和T/rajectory Rollout 算法，两者之间有细微的差别，但差别不大。

为了提高机器人的定位精度，本系统使用了AMCL 功能包。该功能包采用了蒙特卡洛定位方法，来帮助机器人进行二维的定位，针对已有地图使用粒子滤波器跟踪一个机器人的姿态，AMCL 通过传感器采集到的信息在地图上的一个位姿估算，框架如图8所示。

图8 AMCL 框架图Fig.8 AMCL frame diagram

4 仿真与实验

4.1 仿真

为提高实验的开发效率，本文使用ROS 中Gazebo工具进行仿真实验，为了使仿真结果更加接近真实情况，在Gazebo 搭建物理仿真环境，首先配置机器人建立应急机器人URDF 描述文件，为描述文件添加惯性参数，碰撞属性，颜色，摩檫力等参数，同时添加传动装置以及Gazebo 的控制器插件；其次创建仿真环境，利用Gazebo 中的Build editor 工具以及自带的模型库生仿真模型。最后启动之前配置好的Cartographer 算法launch 文件。通过上述的构建地图过程，可以得到仿真环境的先验地图，配置move_base包德个节点关系，先指定机器人的起始位姿，再指定目标点的位姿，最后启动机器人的导航。算法自动规划路径，并实现局部避障。其导航避障过程与结果如图9所示。

图9 仿真测试过程Fig.9 Simulation test process

4.2 实际环境

基于ROS 和激光雷达搭建应急机器人原型系统，在真实环境下进行定位与建图测试，实际测试环境如图10所示，机器人使用Cartographer 算法进行地图构建和定位，move_base 功能包进行导航，其结果如图11所示，在实验过程中没有与障碍物发生碰撞。

图10 实际测试环境Fig.10 Actual test environment

图11 测试结果Fig.11 Test result

5 结语

本文以NVIDIA 公司的Jetson TX2 计算机模块版作为控制器，通过Robosence 公司的RS-LiDAR-16 雷达以及IMU 采集环境信息，在ROS 分布式系统以及Cartographer 算法构建了应急检测履带式机器人的SLAM 系统。对其进行了仿真实验以及实际环境测试结果表明，该应急监测机器人具有良好的建图精度，具有模块性化，拓展方便等优点，可实现在室内环境的自主导航的功能。可作为应急监测机器人SLAM、导航及ROS 框架的实验基础平台。