基于目标检测的移动抓取机器人系统设计与实现

郑若璇

（西南交通大学计算机与人工智能学院，成都 611756）

0 引言

近些年，人工智能的发展使得越来越多的智能化产品投入到人类的生活、生产中替代人工，提升生活水平、生产效能。目前公共场所清洁很大程度上仍然依赖于人工，但是垃圾回收类工作完全可被机器人系统替代，极具发展前景。日常常见的机器人大致分为两类：工业型［1］和服务型［2］。工业型机器人广泛应用于工农业生产制造环节；服务型机器人更为公众熟知，例如送餐机器人、迎宾机器人等。现阶段很多服务型机器人应用了深度学习技术实现机器人的智能化，比如迎宾机器人的对话功能、人脸识别功能等。

基于深度学习的目标检测算法主要分为两种：两阶段和单阶段算法。两阶段算法将目标检测分为候选区域生成和卷积神经网络分类两步，代表算法有RCNN 系列［3-5］。其在目标框的定位精度和目标类别的检测准确度两个性能上表现优异，然而检测速度较慢，很难应用于实时场景。单阶段算法是一种端到端算法，不再生成候选区域，而是直接对图像进行目标的定位和类别检测，以一部分检测精度为代价换取了检测速度的大幅度提升，代表算法有YOLO系列［6-9］、SSD［10］等，而且随着YOLO系列算法的逐渐优化，算法的检测精度也得到提升，因而被普遍应用在实时检测场景。

本文基于ROS［11］系统，将可移动机器人、机械臂控制与目标检测算法相结合，设计并实现了基于目标检测的移动抓取机器人系统，完成机器人定位导航抓取物品并分类回收的功能，并针对此系统的应用场景构造了Grasp 数据集，使用YOLOv5在数据集上训练、测试；最终进行了不同场景下的实景测试，抓取率达到92%，分类回收放置正确率达到100%。

1 系统架构

本系统（图1）基于ROS进行开发，主要分为三个模块，建图导航、目标检测、抓取及分类放置。

建图导航模块采用激光SLAM［12］算法进行全局地图构建，采用Amcl 算法实现导航过程中实时定位，采用Move_base 进行全局路径规划和局部避障，通过ROS 的话题通信向底盘运动控制话题发布运动命令，以实现系统架构的“移动”功能。该模块话题通信发布机器人是否到达指定位置的消息，待目标检测模块订阅。

目标检测模块只有接收到机器人到达指定位置的消息才会运行，包含目标检测和坐标系转换两部分。目标检测部分通过话题通信，订阅REALSENSE D435 深度摄像机的RGB 图像话题和深度图像话题，使用YOLOv5 模型对实时RGB 图像进行目标检测，确定当前位置可抓取目标的分类和在图像像素坐标系中的坐标，再结合深度图像得到目标的深度信息。坐标系转换部分通过整合当前图像所有待抓取目标的图像中定位和深度信息，确定本次抓取目标的深度及其在图像像素坐标系的定位，再经过坐标系转换的矩阵计算，获得本次抓取目标在相机坐标系的定位，最终将该定位维护到机器人系统的坐标系转换树中，待抓取及分类放置模块获取。

抓取及分类放置模块采用MoveIt!［13］控制机械臂，主要包含机械臂抓取和分类回收两个阶段。机械臂抓取阶段，该模块获取本次抓取目标的定位并调用MoveIt!，以该位置为目标点进行机械臂路径规划以及机械臂移动控制以到达目标所在位置抓取目标。目标物抓取完成后进入分类回收阶段，该模块订阅目标检测模块发布的本次抓取目标的分类消息，根据分类结果，控制机械臂到达该类别的回收位置，并松开夹爪以回收目标物。

图1 系统框架

2 建图导航

2.1 建图导航TF坐标系转换

ROS 通过坐标系标定各关节或物体位置，三维世界中任何两个坐标系的关系可以用平移量和旋转量两个量表示。TF（坐标系转换）维护了机器人系统中全部坐标系的关系，形成了一棵遵循“单继承”原则的树，因此TF 树中任两个已存在的非父子坐标系的转换关系可以由树形结构中记录的父子坐标系的转换关系获得。

建图导航中的TF 除了机器人本身各个部件之间的TF 之外，主要维护了地图坐标系、机器人底盘坐标系以及里程计坐标系之间的关系，以便机器人在移动建图和导航过程中确定行进的方位和距离以及在地图中的方位，一般设置如图2所示。

图2 建图导航坐标系转换关系

2.2 SLAM建图

在实际场景中，实现导航功能通常是基于已经获取全局地图的条件，之后每当机器人得到目标地点信息时，可以结合全局地图、自身位置经过一系列算法规划一条全局最优路径。SLAM 是一项二维地图建模技术，用于机器人在陌生环境中通过移动实时获取自身定位及周围环境信息，并构建增量式地图，最终得到静态全局地图。

图3 建图示例

本系统采用基于激光雷达和里程计的SLAM算法。建图节点订阅激光雷达发布的环境信息以及激光雷达、里程计和机器人底盘的TF 信息，在机器人探索建图的过程中，发布机器人姿态、以固定频率更新地图的元数据（长、宽、分辨率等），同时发布地图坐标系和里程计坐标系的转换，将通过里程计记录的移动信息和地图原点信息相结合，从而使机器人在探索未知领域时能够确定当前激光雷达扫描到的信息更新在二维地图中的方位。

2.3 导航

导航模块（图4）采用ROS_Navigation，其核心部分是Move_base节点。

图4 导航模块框架

Move_base 基于静态全局地图，通过在移动过程中实时获取的外界环境信息和自身定位信息等维护了全局代价地图和局部代价地图，用于全局和局部路径规划，其中全局路径规划用于规划完整行驶路线，而局部路径规划用于躲避移动过程中激光雷达扫描到的建图时不存在的障碍物，最终实现控制底盘移动到指定目标点。导航模块会将导航结果发布在设定的相应话题中。

由于里程计只是通过里程计数据实现里程计坐标系和机器人底盘坐标系的坐标系转换，存在累计误差并且打滑等特殊情况会导致定位偏差，不适用于长期定位或长距离定位，所以采用ROS_Navigation 的Amcl 功能包实现机器人导航过程中的定位。Amcl 订阅激光雷达数据、地图数据等估计机器人在地图坐标系中的位姿同时结合里程计信息降低定位偏差。

3 目标检测

3.1 数据集

本文针对机器人抓取系统的应用场景及目标抓取物类别，制作了Grasp 数据集，图像共2200 余张，分为瓶子（金属、塑料、玻璃）、水果及果皮（香蕉、苹果、橘子等）、纸盒3 个类别，全部为人工标注，其数据格式采用VOC 数据集的格式。

3.2 目标检测

目标检测模块以深度摄像机的RGB 图像作为输入，使用YOLOv5s模型进行可抓取物检测，检测结果是图中每个可抓取物的类别及其检测框在二维图像像素坐标系的坐标。结合深度摄像机的深度信息，可以计算得每个目标检测框的平均深度值，以此作为该目标的深度距离d。由于单次机械臂抓取只抓取单个目标，所以设定深度距离最小的为当前抓取目标，同时目标检测模块以自定义的话题消息格式，向指定话题发布本次抓取目标的分类待机械臂抓取模块订阅。

YOLO 系列是典型的端到端目标检测算法。YOLOv5 主要包含四个模块：Input 负责数据输入和数据增强，其创新点在于将初始锚框的计算嵌入到训练中，使模型在不同训练集的训练过程中找到相应的最佳初始锚框；Backbone 主要包含切片操作的Focus 结构、特征提取网络CSP 以及激活函数，用于提取图像特征；Neck由FPN+PAN 结构构成，融合不同特征层参数；Head 利用Neck 传递的特征进行预测，输出目标的分类和定位，而在训练中，网络将预测框和真实框求差，反向更新迭代网络参数。

YOLOv5 官方提供了5 种不同深度和宽度的结构。深度加深和宽度加宽能够提升网络的特征提取、特征融合能力，同时也会增加网络的计算量。由于本系统是部署在移动端，对网络的轻量化有较高的要求，而YOLOv5s 的准确度和轻量化能够满足本系统使用场景的要求，所以系统采用YOLOv5s作为检测模型之一。

3.3 TF坐标系转换

在实际抓取时，机械臂需要获取目标相对机械臂的三维物理坐标，而机械臂坐标系与相机坐标系均已维护在机器人系统TF中，所以目标检测模块首先依据图像像素坐标系（二维）、图像物理坐标系（二维）、相机坐标系（三维）的转换关系（图5）以及深度摄像机的参数矩阵经矩阵运算得到目标在相机坐标系中的三维物理坐标。

图5 坐标系转换示意图

图像像素坐标系的原点为图像左上角，单位为像素（pixel），原点为op(0,0)；图像物理坐标系的原点是图像的中心点，在图像像素坐标系中坐标为o(u0,v0)，坐标系单位为毫米（mm）。以目标物检测框的中心点为目标点，设目标点在图像像素坐标系中坐标为(u,v)，在x方向1 pixel =dxmm，在y方向1 pixel =dymm，则目标点在图像物理坐标系中坐标(x,y)满足：

oOc为相机焦距f，依据三角形相似，目标点在相机坐标系中坐标(X,Y,Z)满足：

其中Z即目标点的深度距离d。

联立公式（1）和（2），则目标物的相机坐标系坐标(X,Y,Z)与图像像素坐标系坐标(u,v)满足：

目标检测模块以相机坐标系为父坐标系，目标物坐标系为子坐标系，通过TF 的broadcaster 发布目标物在相机坐标系中的位置，将目标物维护至TF树，从而使机器人TF树中任一坐标系均可与目标物坐标系进行坐标系转换，其中也包括机械臂坐标系。

4 抓取及分类放置

4.1 机械臂控制

MoveIt!是ROS 提供的控制机械臂的平台，其中结合了运动规划、运动学求解、仿真配置、控制等内容。抓取及分类放置模块中机械臂主要包含四个静止状态：归位状态、探出状态、抓取状态、放置状态。模块通过调用MoveIt!的不同接口，完成实时读取机械臂各关节的位姿、规划末端执行器姿态移动方案、发布指定目标姿态、控制机械臂移动等功能。

4.2 抓取及分类放置

机械臂在机器人未到达指定目标地点时，处于归位状态。抓取及分类放置模块首先订阅导航模块发布的导航结果信息，若机器人没有到达指定目标点，则循环监测导航结果；若机器人完成导航到达指定目标，则机械臂移动至探出状态。之后，该模块触发目标检测模块发布的目标检测结果话题的回调函数，获取待抓取目标的分类信息，若分类信息不属于具体类别，表示机器人所处位置不存在可抓取物，机械臂返回归位状态，等待指定下一个地点。若分类信息指向具体某个类别，表示当前位置存在可抓取物，则机械臂订阅TF 树，获取目标检测模块维护的目标物与机械臂的位置关系，并移动到目标物上方，收紧夹爪（即抓取状态）。抓取完毕后，该模块根据分类信息的分类结果，控制机械臂到相应的放置点并松开夹爪（即放置状态），完成一个目标地点的一个可抓取物的抓取。之后，机械臂回到探出状态，抓取当前目标地点的下一个可抓取物，直到当前地点不存在可抓取物，机械臂回到归位状态，完成了一个目标点的抓取及分类放置任务，等待下一个地点。流程如图6所示。

图6 抓取及分类放置流程

5 系统运行效果

5.1 算法评估

5.1.1 实验环境

YOLOv5实验设置最大迭代次数为100 epochs，初始学习率为0.01，学习率变化因子为0.1；实验使用3 块NVIDIA GeForce RTX 2080 Ti，环境为Python3.7、PyTorch1.6.0、CUDA10.2。

5.1.2 结果

在Grasp 数据集的测试集上对训练完成的YOLOv5s模型进行测试，结果如表1所示。

表1 YOLOv5s-Grasp数据集测试结果

5.2 系统评估

本系统的实现基于上海一坤电气的轮式机器人Bulldog、遨博智能机械臂manipulator_i5、Intel 双目摄像头D435、搭载第八代智能Intel 酷睿处理器的INUC 套件NUC8i7BEH 等硬件设备，以移动抓取室内杂物并将抓取目标分为瓶子和其他（非瓶子）两类进行分类回收为测试要求进行测试，测试物品共50 件，测试次数100 次，对正确抓取、漏抓、分类放置正确及放置错误次数进行统计，其中正确抓取92 次，漏抓8 次，抓取准确率92%；分类回收准确92 次，分类回收正确率100%。

测试过程中部分导航轨迹如图7所示，实景效果如图8、图9所示。

图7 建图导航模块展示

图8 目标检测模块展示

图9 抓取及分类放置模块展示

6 结语

本文设计的基于目标检测的移动抓取机器人系统，将深度学习算法与机器人相关技术结合，实现了机器人自主导航至指定目标地点并抓取及分类回收等功能，可应用于日常生活、服务中，具有一定的实践意义。在实验演示中，系统运行效果良好，为移动机器人实际应用提供了新的思路。增加机器人自主循环导航，实现分类多样化等，是本系统进一步优化的方向。