基于ROS的垃圾督导机器人导航系统设计与实现

何海涛　陈劲杰

关键词：机器人；导航；ROS

中图分类号：TP311 文献标识码：A

1 引言（Introduction）

2019年7月1日起《上海市生活垃圾管理条例》正式实施，上海成为全国第一个立法推进垃圾分类的省级行政区[1]，同时上海各高校也纷纷实行垃圾分类措施，定时定点进行垃圾投放，但是部分同学仍然在规定时间外随意投放垃圾，该行为为学校后勤工作带来了不便。基于学校后勤需求，有必要设计一款垃圾督导机器人对同学随意丢弃垃圾的行为进行文明、友好的监督。然而，机器人系统的复杂性为垃圾督导机器人的开发带来很大的困难。机器人操作系统（ROS）的出现，大大降低机器人的开发门槛[2]，功能也越来越成熟[3]。ROS使用分布式架构，有大量开源用于实现特定功能的机器人应用程序，称为功能包，功能包单独设计，在运行时松散耦合，提高了代码复用率[4]。因此，开发者不需要重复造轮子，可以像搭建积木一样快速搭建各种功能模块，专注于某一领域的研究，快速完成开发[5]。

导航功能是该机器人核心和基础功能，其功能的好坏和开发周期关系到机器人的鲁棒性和生产成本。因此，本文基于ROS提供的功能包设计并开发了垃圾督导机器人导航系统。

2 系统整体设计（Overall system design）

垃圾督导机器人主要由底层驱动模块和顶层控制模块构成，其整体系统设计如图1所示。

底层驱动模块是垃圾督导机器人控制机器人运动的核心。从控制器使用的是ST公司基于Co t e x-M3内核的STM32RCT6，其板载资源能够满足整个控制系统的使用。此外，在该开发板上移植了FreeRTOS（Free Real-TimeOperating System， 自由实时操作系统），使用默认的抢占式调度，能够保证系统的可靠性和实时性[6]，使得不同任务解耦，方便对后续的任务进行扩展。

从控制器通过获取编码器信息，计算得出机器人的线速度与角速度，并通过PWM（Pulse Width Modulation，脉冲宽度调制）驱动电机控制板，进而控制电机的速度；通过模拟I2C（Inter-Integrated Circuit，串行双向总线）获取MPU6050（整合性6轴运动处理组件）的陀螺仪和加速度信息，然后将机器人的速度、陀螺儀和加速度等信息通过串口上传到主控制器，此外将MPU6050放置于机器人的中心，以便获取准确的信息。

顶层控制模块是垃圾督导机器人导航系统的主控制中心，主要对整个系统进行决策。主控制器使用的是JetsonNano，是一款搭载低配GPU（Graphics Processing Unit，显卡）运算平台的小型计算机[7]。该主控制器控制2个激光雷达获取二维平面点云信息，并且通过串口向从控制器以比特流的方式发送线速度和角速度来控制电机的速度，从而控制机器人的移动。此外，主控制器通过获取从控制器发过来的速度信息计算得出机器人的里程计，并获取从控制器发送的陀螺仪和加速度信息，结合从雷达获取的二维点云数据，完成机器人的定位建图和导航功能，并基于上述功能，使用状态机完成巡逻任务。

3 系统硬件设计（System hardware design）

该机器人以四轮差速移动机器人为原型，在进行结构设计时，对部分空间进行分层，放置不同功能的硬件设备，其硬件结构如图2所示。

激光雷达为“思岚”A1激光雷达，可以获取雷达平面的二维点云数据，由于该机器人是在平地上运行，因此使用二维雷达可以满足机器人的运行要求。为了让机器人能够识别低处的障碍物，不宜将雷达放置在高点，应放置于底盘位置，并且在前后位置各放1个，以加快机器人对地图的建立速度并提升定位的准确性。

4 系统软件设计（System software design）

软件部分主要由底层驱动模块和顶层控制模块构成。

4.1 底层驱动模块软件设计

从控制器使用STM32F103RCT6，其系统软件结构如图3所示，移植了FreeRTOS，并且总共设置了4个任务，并且使用中断接收主控制器发送过来的速度控制信息，控制机器人运动，通过查看LED是否闪烁，判断系统是否正常运行。

4.1.1 当前运动速度获取

在每个电机的尾部都装有增量式光电编码器，通过捕获对应的脉冲，可以计算出电机的转速。假设编码器的分辨率为M，设在一个时间周期t（s）的范围内，通过输入捕获得到的脉冲数为N，那么可以计算得出电机的实际转速n（r/min）。

4.1.3 MPU6050信息读取

MPU6050是一款常用的IMU（Inertial MeasurementUnit，惯性测量单元），使用模拟I2C周期性读取MPU6050的陀螺仪和加速度信息，在MPU6050上电之后，需要等待一段时间稳定后再对其进行读数，此外水平面要平稳且放置于机器人的中间，提高其测量稳定性。

4.1.4 串口通信发送数据信息

使用串口发送的数据，进行数据处理的时候，均没有使用浮点类型，而是使用16位短整型，将参数扩大了1 000倍，STM32F103系列没有浮点运算单元，如果计算浮点运算，速度比较慢，会影响其运行速度；并且，由于使用串口发送，没有校验，因此在数据帧中添加了简易的异或校验进行数据帧格式的校验，保证数据传输的正确性。

4.2 顶层控制模块软件设计

主控制器使用Jetson Nano，环境为Ubuntu18.04，安装ROS版本为Melodic。ROS提供了Gazebo（开源机器人仿真器）进行机器人仿真，使用Rviz（ROS Visualization，ROS可视化工具）可视化界面，实时查看机器人的行为动作。

垃圾督导机器人的巡逻任务主要基于定位建图和导航功能完成。定位建图与导航框架如图6所示。

4.2.1 定位建图

由于垃圾督导机器人只是在一小片区域活动，因此建图方法选择Gmapping（Grid-based FastSLAM mapping，基于栅格的快速建图算法） 建图， 是一种基于滤波的SLAM（Simultaneous Localization and Mapping， 同时定位与建图）框架[8-9]，其在使用构建小区域地图时所需的计算量较小且在室外测试具有良好的效果。由于有2个激光雷达，需要对两个激光雷达的信息进行融合，因此使用ROS提供的laser_tools功能包对2个激光雷达的点云数据进行融合。由于安装在前面的雷达后半部分会被机器人自身挡住，后面雷达的前面部分也会被机器人自身遮挡，在实际测试中如果不进行处理，机器人会认为其位置存在障碍，导致其导航功能失效，机器人会不停地在原地摆动，不断进行软件复位，因此可以使用ROS中的功能包laser_filters设置需要过滤的区域，避免对建图导航等产生影响。

此外，机器人在地图中的定位也十分重要，只有让机器人知道身处的确定位置，才能根据路径规划到达下一个目的地，这就需要较高的定位精确度。使用Gmapping建图算法与AMCL（Adaptive Monte Carlo Localization，自适应蒙特卡罗定位）进行定位，非常依赖里程计数据。现实生活中，机器人在运动过程中不可避免地会发生打滑，导致建图与重定位的效果不理想。为改善这种情况，使用EKF（ExtendedKalman Filter， 扩展卡尔曼滤波）算法对轮式里程计与IMU数据进行融合，从而进一步提升里程计数据的准确度[10]，进而提高定位精确度。

4.2.2 导航

全局路径规划使用global_planner功能包，其实际在调用A*算法或者Dijkstra最短路径算法进行全局规划，通过更改参数即可在两者之间进行选择，该系统使用了A*全局路径规划算法，A*算法相比Djkstra算法，在寻找最短路径的问题上效率更高。使用局部路径规划DWA算法（Dynamic WindowApproach， 动态窗口法）进行导航，通过ROS进行参数的配置和调节，即可进行机器人的导航操作。在实际测试中，DWA算法在进行局部路径规划时，不同于TEB（Timed ElasticBand， 时域弹性带）算法，如果使用该方式进行导航，可以使机器人进行原地旋转的操作，满足该任务需求。只需在Rviz界面中指定到达的位置，通过上述算法就可以规划出一条全局路径，然后将速度信息发送给从控制器进行底盘的控制，完成机器人的移动，同时根据周围的环境调整局部路径，使机器人顺利到达指定地点。

4.2.3 巡逻任务

需要完成的任务如下：首先使用机器人在固定的垃圾堆旁邊建立地图并保存。然后设置需要进行导航的位置，为了能够观察四周的情况，假设安装相机的可视范围为90°，那么每次需要旋转4次，并且每旋转一次后，需要暂停模拟摄像头捕捉行为，在每个指定地点巡视一周后，到达下一个指定地点，如此反复，可完成巡逻任务。

协调各个任务，也是软件设计中的一个重点，在ROS中，不同的任务运行在不同的节点，那么需要协调各个节点，使其能够有序地进行。为了完成相对复杂的任务，该机器人使用SMACH（State Machine for Advanced Robots，高级机器人状态机）有限状态机，有限状态机又称有限状态自动机，是表示系统中有限个状态以及在这些状态之间的转移和动作等行为的数学模型[11]，该状态机可以明确地描述可能的状态和状态转换，将不同的模块组合成为一个系统，对于复杂的机器人系统可以有效地分解成不同的任务进行组合。其整个工作流程如图7所示。假设进行导航巡逻任务，地图建立完毕后，在地图上找到需要进行多点导航巡逻的点，进行标记，记录需要到达的位置，将需要导航到地点的位置参数按照数组的格式写入文本文档中。当启动导航巡逻任务时，读取文档中的位置地点，并创建相应的任务，在到达每个位置之后，都需要进行四周的巡检，循环进行任务，直到完成所有的任务。

虽然该巡逻任务并不复杂，但是可以基于该状态机对任务进行扩展，比如在安装摄像头后检测到有行人路过，那么状态就可以更新为停止，等待行人经过，当行人走过之后，再返回之前的状态继续执行任务。也就是说，使用状态机具有良好的扩展性。

5 仿真与实地测试（Simulation and experimental test）

5.1 功能仿真

ROS提供了仿真功能，可以使用Gazebo进行仿真，仿真平台模拟机器人运行的真实环境，为机器人有关实物实验提供安全可靠的依据，并大大缩短机器人的开发以及实验周期，具有安全可靠、高效灵活的优点[12]。

为了使仿真环境更接近真实环境，使用SolidWorks（CAD设计绘画软件）进行模型建立，在建模时对仿真模型进行了简化，设置机器人在长宽方向的尺寸一致，高度略有差距，但是不影响对功能的仿真。由于计算机使用虚拟机，资源有限，因此选取比较简单的场景进行仿真，如图8所示。

首先操作机器人进行建图，可以手动控制机器人进行地图的建立，或者使用导航算法进行建图。本次测试通过导航进行建图操作，如图9所示。

使用机器人行走路径形成闭环之后，可以生成最终的地图，进行保存，如图10所示。

最后通过在地图上采集巡逻点，将巡逻位置写入记事本，使用状态机进行巡逻测试，可以达到预期的要求。

5.2 实地测试

由于在室外条件下，没有Wi-Fi连接，因此使用JetsonNano的网卡，开启热点，使用电脑与机器人实现连接，在仿真满足功能需求的前提下，将垃圾督导机器人拿到扔垃圾的地点进行实地的定位建图、导航及巡逻功能的测试，如图11所示。经过测试，垃圾督导机器人定位准确且导航功能可以完成既定的目标，也可以完成在多个地点之间的巡逻操作，符合任务的需要。

6 结论（Conclusion）

本文设计了基于ROS的垃圾督导机器人导航巡逻系统。该机器人能够快速响应控制信号，及时移动或者停止。主控制器使用ROS，实现了建图、定位导航等功能，并且通过其提供的仿真功能，节省了在实体机上调试的时间，使用状态机对复杂的移动巡逻任务进行规划。通过实地实验，证明机器人能够完成巡逻任务，其导航功能能够满足现实需要。同时，该机器人移动导航系统的软、硬件设计为移动机器人的开发具有较强的指导意义和应用价值，对于不同的移动机器人模型，只需要修改对应的运动学模型，即可实现导航功能。

由于该机器人必须通过手动建图、手动设置目标点，才能完成相应的功能，因此自动探索和生成导航路径也是该导航系统需要进一步研究的方向，并且可以逐步融合视觉、语音等方面以增加并完善该机器人的功能，做到真正的商业化落地。

作者简介：

何海涛（1998-），男，硕士生.研究领域：嵌入式，智能控制.

陈劲杰（1965-），男，硕士，副教授.研究领域：人工智能，智能机器人.