基于ROS 的目标搜寻系统设计∗

薛志凌 孟令军 王佳军 李晓宇

（中北大学仪器与电子学院 太原 030000）

1 引言

自主机器人是一种智能系统，它利用所安装的多种传感器以感知周围环境［1］。同时处理这些信息，并进行决策，做出相应的动作。机器人系统通常需要使用诸多传感器，而各种传感器有着不同的通信方式和数据格式，同时处理各种不同的传感器数据是一件非常烦琐的任务，机器人项目的落地常常因为此原因而困难重重。ROS 操作系统为解决这个机器人项目开发的痛点而研发。ROS（Robot Operation System）为机器人开发提供一个种非常便利的消息通信机制，极大地提高了开发中的软件利用率和复用率［2］。ROS 是一套开源的系统［3］，部署和使用非常的便利。可以看到ROS 系统将成为未来机器人开发的主流。

利用移动机器人对环境进行探索和对特定目标的搜寻是当前机器人领域的研究热点。人类感知周围环境的绝大多数信息来源于视觉［4］，同时视觉信息的获取比较容易。这也就表明了视觉相比于其他传感器，可以获得相对准确丰富的信息，所以视觉传感已经成为大多数机器人机器的标配。本文基于ROS 系统，设计一套智能移动机器人系统，利用激光雷达信息完成对未知环境的探索和导航，再利用视觉信息对特定目标进行搜寻。通过WIFI 在PC 电脑端实时监控机器人的运行状态和查看机器人采回的数据，同时还可以向机器人发送指令来控制机器人。

2 系统设计

2.1 系统总体设计

整个系统的框架如图1 所示。自主导航算法和视觉伺服算法在树莓派上完成。STM32 将采集到的IMU（惯性测量单元）数据和轮速计数据，通过串口传递给树莓派，为SLAM（同步定位与建图）系统提供里程计信息。显示屏显示机器人的电池电压数据和IMU 传感器数据。蓝牙单元可以使机器人被移动设备所控制。机器人通过WIFI（无线宽带）和PC 端通信。PC 端可以监控机器人的运行情况，显示机器人采集的视觉信息，机器人所建地图和机器人的位置信息。同时PC端可以向机器人发布控制指令，将机器人的控制模式从自动控制切换到手动控制。

图1 系统框架

整个系统主要由：目标识别，建图和导航，路径规划和机器人运动控制五个部分组成。

2.2 目标识别

目标识别的关键在于对识别目标特征的提取。目前主流的提取图像特征的方法有，卷积神经网络和特征点提取［5～6］。前者是应用了深度学习的方法，其需要输入目标样本训练识别模型然后建立分类器。这种方法虽然在识别率上取得了显著的成功，但其在针对特定场景时，部署就需要消耗大量的时间和算力。而后者虽然识别准确率不如前者，但其占用更少的计算资源，可以在通用场景中使用。此次设计要求只输入目标的图像信息就可以完成对目标的搜寻。针对此要求，设计采用基于特征点提取点的方式来完成目标的识别。

SIFT（尺度不变特征变换）是一种不需要使用深度学习来提取图像特征的成熟算法［7～8］。其提取图像特征的大致过程有以下几步。首先构造图像的尺度空间，这一步的目的是为了获取图像相对于摄像头在不同距离下图像的特征。一般的做法是该图像乘以高斯函数，如式（1）。

其中，G为高斯函数，I为图像的灰度值：

本质上，它是给图像的每一个像素乘以不同的比例因子，这些比例因子是高斯分布的。σ是高斯函数的方差，其决定了图像的平滑程度，σ值越大，对应的图像就越模糊，也就代表着图像离我们越远。相机采集的图像在远处不光变得模糊，同时会随着距离的增加，图形的像素会逐渐变少，即图像逐渐变小。为此我们构建图像金字塔，给经过高斯函数处理的图像乘以不同的缩放因子，σ值越大，图像的缩放比例越大。然后对不同尺度的图层做差，得到差分高斯金字塔（Difference of Gaussian，DOG）图层。DOG算子定义为

这样做的目的是将差值最大的点作为图像的备选特征点。其次进行关键点的提取。完成以上操作后，找到备选特征点中的极值点，作为图像的特征点。这些点不具备方向信息，为此要确定每个特征点的方向。对于每一个点，只要确定每个点L(x,y)的梯度的模m(x,y)以及其方向θ(x,y)就可以得到每个点的位置，尺度和方向信息。

最后，生成其特征描述子，带有每个像素点方向信息的特征向量。这样我们完成了图像特征的提取。通过RANSACS算法对目标图像和检测图像匹配［9］，即可找到目标图像。检测流程如图2。

图2 目标检测流程

2.3 建图与导航原理

在陌生环境中探索，需要机器人在记录周围环境的同时知道自己的相对位置。SLAM［10］技术可以根据传感器采集回的数据，生成自身周围的环境，即建图。同时根据里程计信息确定自己的位置，在地图和位置信息的基础上完成导航。一般的SLAM 运行过程如图3。传感器数据的输入一般是激光雷达和摄像头。

图3 SLAM运行流程

在ROS中，机器人的导航流程如图4。

图4 ROS导航流程

其中AMCL 是自适应蒙特卡洛定位［11］的简称，它是在粒子滤波的基础上改进而来。通过AMCL可以有效减小传感器测量中的干扰，得出比较准确的机器人位置信息。在ROS 中，AMCL 的运行流程如图5。

图5 ROS中AMCL运行流程

导航框架是整个控制任务的核心，主要是由全局路径规划和本地实时规划这两个部分组成。全局路径规划为找出全局最优路径，这里采用DIJKSTRA［12］全局规划算法。本地实时规划是为了应对地图的实时变化，既完成动态场景的实时路径规划，这也意味着机器人可以完成实时的避障。本地实时规划采用动态窗口法（DWA）［13］。系统的导航框架如图6。

图6 导航框架

代价地图主要描述环境中的障碍物信息。机器人避障过程如图7 所示，图中黑色部分为障碍物。当机器人检测到障碍物时，如以障碍物区域的几何中心画圆，让半径最小的圆包裹中障碍物，图7 中的实线圆，这个圆的半径称为膨胀内径。增加膨胀内径，以膨胀内径圆为圆心，画出膨胀外径圆，膨胀内径与膨胀外径圆之间的区域为膨胀区。机器人绝对不能触碰膨胀内径圆，同时避免进入膨胀区。机器人会沿着膨胀外径圆的外切线规划路径。当机器人运行发生错误时，机器人会自动重置当前状态，让之正常运行。在局部规划中可以完成机器人的实时动态避障行为。导航节点给出的基本的控制信息经过转换发送给机器人控制板，完成对机器人的控制。

图7 避障示意图

2.4 视觉伺服系统

在机器人没有检测到目标物体的时候，机器人在自主导航模式下运行。当机器人检测到目标物体时，机器人的切换到视觉伺服系统控制，即通过视觉反馈完成对机器人的运动控制。在任务中，机器人需要停在被识别目标前，同时机器人头部应正对被识别目标。机器人与识别目标的距离通过图像的深度信息判断。在伺服系统中，用PID 控制器，控制小车接近目标物体的线速度，将机器人与识别目标的距离作为PID 控制器的输出，机器人的线速度作为输出。同时控制机器人，使被识别物体的图像中心处于相机呈像的几何中心附近。

3 系统的ROS框架设计

基于ROS系统，可以很容易地搭建起整个系统的框架。把每一个功能块封装到一个ROS节点，通过ROS 的消息收发机制，实现整个系统的运行，本次设计的ROS 框架如图8。建图导航节点采用ROS 的GMAPPING 功能包来实现基本的SIAM 算法，其需要输入二维点云数据和机器人的里程计数据。使用ROS 的MOVE_BASE 导航框架作为路径规划节点。视觉伺服节点有二个作用，一是为机器人提供视觉伺服控制。二是当机器人没有识别到目标物时，机器人将会由路径规划节点指引，移动到指定目标点。视觉伺服节点和路径规划节点都可以向机器人发布速度指令。目标点设置节点负责设置机器人移动目标点和记录机器人的初始位置。机器人在未知环境下，采用生成随机目标点的方式探索地图并搜寻目标物。机器人在已知地图的条件下，采用固定点搜寻，以此来提高地图的搜索效率。同时记录了机器人的初始位置，在机器人完成任务后，使机器人可以回到出发点。

图8 系统的ROS框架

4 仿真实验

ROS支持GAZEBO 仿真器和RVIZ可视化显示工具。GAZEBO 是一种3D 仿真器［14］，其内置游戏引擎，可以准确地仿真机器人在复杂环境中的运动，同时其支持多种传感器的仿真，如相机，激光雷达等。所以仿真实验选择在GAZEBO 中完成。由于ROS 中发布的机器人速度指令格式为线速度加角速度，所以仿真中的机器人采用圆形差速机器人，同时搭载RGBD相机和激光雷达。

4.1 建图仿真

图9 为GAZEBO 机器人的仿真场景，仿真场景为室内封闭的房间。建图效果如图10。可以看出其具有比较良好的建图效果。

图9 GAZEBO仿真场景

图10 SLAM建图效果图

4.2 导航和避障仿真

导航仿真效果如图11、图12。导航中，机器人的位置信息用四元数表示。在图12 中，箭头的节点为机器人的到达点，箭头的指向设定机器人的姿态。

图11 RVIZ导航显示效果

图12 GAZEBO仿真环境运行效果

避障效果如图13、图14，避障原理参见上文。

图13 RVIZ显示效果

图14 GAZEBO仿真环境运行效果

4.3 目标搜寻任务仿真

在GAZEBO 仿真打开相机模型，在RVIZ 端可以查看相机发布的图像信息。输出目标物体图15，在GAZEBO仿真环境放置此物体。程序将设置随机点，使机器人在场景中进行探索，如图16。

图15 目标物体

图16 机器人探索过程

如果随机设置的目标点在封闭场景外，机器人可以完成整个环境的探索，则一定可以找到目标物体。如何设置的目标点在封闭环境内，且到达目标点后没发现目标物体，则机器人将前往下一个随机目标点，此过程持续到找到目标物体。在找到目标物体后，如图17。程序退出导航程序，视觉伺服节点启动，机器人接近目标物体，将头部朝向目标物体。到达物体前0.5m，完成目标搜寻任务，此时机器人返回起始点，如图18。

图17 识别到目标物体

图18 机器人返回起始点

5 实物测试

本次系统设计的实物平台如图19，机器人的动力部分由直流减速电机和麦克纳姆轮［15～16］（简称麦轮）组成。电机带有光电霍尔编码器，便于发布里程计信息。机器人的剩余主要硬件如表1所示。

表1 机器人剩余的主要硬件

图19 机器人实物

使用麦轮的机器人可进行全方位的移动，但它的控制比一般的差速机器人和带转向舵的机器人控制复杂。试验中，麦轮的机器人的结构如图20，图中W 为轮距，H 为轴距。对机器人进行运动学分析的时候，不考虑轮子打滑的情况。

图20 麦轮机器人的结构模型

若机器人中四个轮子对应的转速分别为VA，VB，VC，VD。Vx为机器人沿X 轴的速度分量，Vy为机器人沿Y 轴的速度分量。ω为机器人沿Z 轴的角速度。令。根据机器人的运动学状态可以解算出机器人的四个轮子的转速方程为

ROS中的速度指令cmd_vel包含三自由度的线速度和三个自由度的角速度。对于地面机器人不存在Z 轴的线速度和X，Y 轴的角速度。实际应用中，将cmd_vel 中X 轴的线速度赋给Vx，Y 轴的线速度赋给Vy，Z 轴的角速度赋给ω，即可完成对机器人的控制。室内运行机器人，建图效果如图21。图中左边为机器人所建栅格地图，右边为机器人实时传回的图像。在图中我们可以看到实验中使用的激光雷达只能检测到激光雷达所在平面的二维障碍物信息。

图21 机器人建图与图传

在机器人开启自主导航模式时，会在所建基本地图信息的周围生成膨胀区，详见2.3 节。实时导航效果如图22。在机器人搜寻的目标物体是，机器人会主动靠近目标物，结果如图23。可以看到系统在复杂环境中有较好的识别效果。

图22 机器人导航避障

图23 目标识别结果

6 结语

本文设计了一套以ROS 为基础的自主式机器人搜寻系统。整个系统框架和相关算法先在GAZEBO 中仿真实现，然后应用于实际之中。在仿真和实际验证中，该系统均拥有良好的性能。系统的核心算法在机器人的机载计算机中完成，通过WIFI 可在PC 端监控机器人的运行状态和采回的传感器数据，同时可向机器人发布控制指令。在设计中，强调仿真的重要性，期望实现真实世界和虚拟世界机器人运动的一致性。相信以后的机器人系统设计会遵循先仿真后实物搭建的顺序来完成。本研究的下一步研究重点是在机器人识别到目标后做出实时的动作，主要应用特殊场合的设备巡视与维护。