1. 引言
机械臂自主抓取任务 [1] ，包括目标物的检测定位 [2] ，路径规划 [3] 和自动控制等众多理论，在其中目标物的检测定位和路径规划会直接影响目标检测的准确率，因此，本篇文章提出使用YOLOv4-tiny算法和RRT-connect算法方法进行机械臂自主抓取仿真。
山东师范大学研究学者 [4] 提出了一种基于掩模区域卷积神经网络的采摘机器人视觉检测器模型。对该模型进行了改进，使其更适用于重叠苹果的识别与分割。准确率达到97.31%，召回率达到95.70%。识别速度较快，能够满足苹果采摘机器人视觉系统的要求。日本的研究学者 [5] 提出一种利用机械手臂侦测果实并自动采收的方法，首先使用目标检测算法完成水果的检测，然后利用立体摄影机侦测果实的三维位置。利用逆运动学计算出检测位置处各关节的角度后，将机械臂移动到目标水果的位置。机器能在16秒内收获一个水果，并且水果的检出率达到了90%以上。澳大利亚的研究学者 [6] 提取一种估计甜椒作物的位姿计算方法，使用Kinect融合算法来稳健地融合来自手持摄像机的RGB-D数据，并结合颜色分割和聚类步骤来提取作物的准确表示。然后，通过将超椭球拟合到分割的甜椒，经由非线性最小二乘优化来估计甜椒的姿态，改善机械臂抓取的性能。
机械臂的自主抓取一直是机器人领域的研究热点，计算机视觉检测技术和路径规划问题一直是机械臂自主抓取中重点要解决的问题，本文中提出使用深度神经网络YOLOv4-tiny算法和RRT-connect路径规划方法进行机械臂自主抓取仿真，提高目标检测的成功率和路径规划的成功率，最后提高机械臂抓取的成功率。
2. 方法介绍
2.1. YOLOv4-Tiny目标算法
目标检测一直是计算机视觉中重要的研究方向，近年来由于深度神经网络的快速发展，目标检测的精度有了明显的提高，YOLOv4-tiny是基于YOLOv4网络 [7] 改进而来，它是一种轻量级的深度神经网络，它主要可以分成主干网络和特征融合两个部分，它的主干网络是CSPDarknet53-tiny，主要包含3个卷积层和3个CSPNet结构。在特征融合部分，它使用了特征金字塔结构，特征金字塔融合了浅层到深层的特征图，从而充分利用各个层次的特征，并保留了两个分支进行图片识别，能够更好的检测出尺寸大小不同的目标物。YOLOv4-tiny具体的网络结构如图1所示。
Figure 1. Diagram of YOLOv4-tiny network structure
图1. YOLOv4-tiny网络结构图
2.2. 机械臂路径规划算法
在机械臂自主抓取任务中，机械在已知目标位置以后，需要规划一条最优的路径从而控制机械臂各个关节的运动，RRT算法 [8] 一种基于采样的路径规划算法，并且还发展出了RRT-connect算法 [9] 。
随机快速搜索树(RRT)的生长过程如图2所示，Xstar是随机搜索树的起点，Xgoal是随机搜索树的重点，路径规划的目标就是躲避障碍物生成一条从Xstar到Xgoal的路径，在空间中随机生成不触碰到障碍物的随机点Xrand，然后找到离Xrand最近的随机搜索树节点Xnear，连接Xrand和Xnear，判断其是否触碰到障碍物，如果没有，则保留Xrand节点，并将Xrand作为随机搜索的节点，重复上述过程，直到连接到Xgoal。
RRT-connect是基于RRT算法上的改进算法，它和RRT算法最大的不同就是生成了两个搜索树，分别以Xstar和Xgoal为起点生成一颗搜索树。搜索树的生长过程如图3所示。在每次迭代中，只有一颗搜索树进行扩展生成新的节点，下次迭代换成另一个搜索树进行扩展生成新的节点，并重复这个过程，最后连接两条搜索树。
Figure 3. Diagram of the RRT-connect algorithm
图3. RRT-connect算法
3. 算法实验结果
3.1. 目标检测算法实验结果
在机械臂自主抓取的仿真环境中，实验中使用草莓作为检测的目标，在仿真环境中采集草莓的图片，并使用labelimg软件对草莓进行标记，制作模型训练的数据集，一共采集了50张图片，其中40张作为训练集，10张作为测试集，检测模型的准确率。通过Pytorch框架搭建YOLOv4-tiny的网络，迭代次数为1000，批量大小16，学习率le-4，训练使用的GPU为RTX3060，GPU可以加速模型的训练。NVIDIA提供的CUDA和CUDNN版本为11.1和8.0.4。
在进行模型的训练后，将测试集传入模型中进行预测，判断模型预测的精度，这里使用的是AP值作为精度指标，AP能够充分反映模型的预测精度，它是结合了正样本和负样本的检测精度，不仅能够反映未被检测的目标物，还反映了被检测出来的负样本。目标检测结果如图4所示，它的精度高达93.5%，速度为每秒135帧。
Figure 4. Object detection result diagram of YOLOv4-tiny
图4. YOLOv4-tiny的目标检测结果图
3.2. 机械臂路径规划算法实验
为了选择更好的机械臂路径规划算法，本文在三维障碍地图上进行路径规划实验，通过对比最终生成路径的时间，路径的节点数和成功率这三个方面的指标来判断最优的路径规划算法，三维障碍地图的设计如下，地图大小为100 × 100 × 100的空间，在空间中随机产生30个大小不一的灰色长方体障碍物，路径规划的起点坐标是(0,0,0)，路径规划的终点是(100,100,100)。将随机搜索树的两个节点之间的距离设置为2，寻找到的最优路径用红色标记出来。并将起点设置成紫色，终点设置成黑色，在双向RRT算法中，从起点开始的随机搜索树用蓝色进行标记，而以终点开始的随机搜索树用绿色进行标记。RRT算法和RRT-connect算法在三维空间的路径规划路线如图5所示。
(a) RRT算法 (b) RRT-connect算法
Figure 5. Diagram of Path planning on 3D plane
图5. 三维平面上路径规划图
在三维障碍空间中，进行100次重复实验，并统计每次生成的路径节点数，在搜索过程中产生的节点数K，路径生成花费的时间和路径规划的成功率，以这四个指标来评价这两种路径规划算法，在机械臂自主抓取仿真中选择最优的路径规划方法。实验得到的数据如表1所示，两者生成的路径的节点数相差不大，RRT算法要略高于RRT-connect算法，RRT算法在搜索过程中大概要产生115个节点，但是RRT-connect在搜索过程中大概只要产生101个节点，规划时间RRT-connect也要低于RRT。在成功率上，100次试验中，有6次没有成功生成路径，在RRT-connect在100次试验中均成功生成了路径，综合上述四个指标，RRT-connect是优于RRT算法。
Table 1. Experimental results of path planning algorithm
表1. 路径规划算法的实验结果
4. 机械臂自主抓取仿真实验
本章将上面提出的YOLOv4-tiny算法和RRT-connect算法应用在机械臂自主抓取仿真系统中，并进行实验，通过成功检测个数、路径规划成功次数和抓取成功次数来判断算法的可靠性和有效性。
4.1. 机械臂仿真环境搭建
在ROS系统 [10] 中，使用Gazebo [11] 软件搭建机械臂仿真环境，使用Moveit软件进行机械臂的控制和路径规划，本文搭建的六自由度机械臂仿真抓取草莓的场景，如图6所示，在机械的前方仿真了一个桌子，并加上了绿色的背景，目标物草莓通过Blender软件制作，导入进Gazebo软件中，每次实验会随机生成草莓，在桌子的上方放置一个RGBD相机，机械臂的右方是一个绿色的箱子用来放置抓取的草莓。
Figure 6. Diagram of Robot arm simulation environment
图6. 机械臂仿真环境图
机械臂自主抓取的仿真实验过程是1) 在Gazebo软件中搭建仿真环境。2) 调用Moveit软件对机械臂进行控制。3) 在桌面上随机生成草莓。4) RGBD相机拍摄图片并传到YOLOv4-tiny的模型中，进行目标的检测，获得目标的X轴和Y轴坐标。5) RGBD相机获得的深度信息，获得目标位置(X, Y, Z)。6) 通过RRT-connect算法，规划路径，并通过机械臂逆解，获得机械臂的各个节点的运动。7) 如果成功抓取草莓，将草莓转移到箱子里，如果未成功抓取，则复位，重新抓取。机械臂抓取流程如图7所示。
Figure 7. Robot arm grab simulation flow chart
图7. 机械臂抓取仿真流程图
4.2. 机械臂仿真中相机的参数
在机械臂仿真实验中，通过相机拍摄图像，经过目标检测算法YOLOv4-tiny可以获得草莓在图像中的位置，但是需要将这个位置转换成草莓在三维空间的实际位置，这就涉及了相机的内外参数和坐标系转换问题。相机的内参是图像坐标系与像素坐标系，与相机坐标系之间的关系，如式(1)所示
(1)
其中：f为相机焦距；(X, Y, Z)是相机坐标系下的三维坐标；(x, y)是图像坐标系下的二维坐标。
在本文的机械臂仿真环境中，相机在机械臂的外部，它与相机相对独立，可以直接计算相机坐标系和机械臂基底坐标系之间的坐标转换关系，这个关系就是相机的外参矩阵，如式(2)所示
(2)
其中：R是矩阵3 × 3的旋转齐次矩阵，T是大小为3 × 1的平移向量，(X, Y, Z)是相机坐标系下的三维坐标，(XW, YW, ZW)是机械臂基底坐标系下的坐标。
在本次的实验的仿真环境中，相机的参数结果如表2所示。
4.3. 机械臂自主抓取仿真
通过上面的仿真流程，本节开始进行机械臂仿真抓取的实验，在实验中，将提前设计好的草莓模型导入Gazebo中，统计实验中机械臂抓取的次数和抓取的成功率来验证该方法的可靠性。
在六自由度机械臂抓取草莓的过程中，机械臂的状态如图8所示。
在本次的实验中，一共进行了两种场景的实验，分别是环境中只有一个草莓和环境中有多个草莓，每次实验，草莓会随机生成位置，然后进行抓取实验，统计草莓抓取次数、抓取成功次数和抓取失败的次数，计算抓取的成功率，其结果如表3所示。
在单个草莓的抓取环境中，进行了50次单个草莓抓取实验，其中48个草莓被成功检测出来，48次路径规划成功，但在抓取过程中有三次未被成功抓取，最终在50次实验中，45次成功，最终取得了90% 的抓取成功率。而在多个草莓的抓取场景中，分10次，每次放5个草莓，一共50个草莓抓取实验，其中45个草莓被成功检测出来，45次路径规划成功，抓取过程中失败了3次，最终在50个草莓中，成功抓取了42个草莓。综合下来，在50个草莓实验中，平均获得87%的抓取成功率，综上所述，无论是在单个草莓的场景中还是多个草莓的场景，六自由度机械臂 [12] 抓取草莓的成功率都取得了较好的结果。
5. 总结与展望
本文使用YOLOv4-tiny和RRT-connect的方法进行机械臂自主抓取实验，证实了这是一种有效的机械臂自主抓取方案，为以后的机械臂自主抓取系统提供一些借鉴意义。
1) 提出了使用YOLOv4-tiny算法作为机械臂自主抓取仿真系统的检测定位算法，证实这是一种高效的检测算法，获得了93.5%的准确率。
2) 提出了使用RRT-connect算法作为机械臂的路径规划算法，通过在二维障碍平面和三维障碍平面的实验，RRT-connect的路径的节点数P，随机搜索树的节点数K均低于RRT算法，相比于RRT算法，它是一种效率更高，成功率更高的算法。
3) 在ROS系统中搭建了机械臂自主抓取的仿真系统，将YOLOv4-tiny作为目标的检测算法，RRT-connect作为路径规划算法，进行机械臂抓取仿真实验，机械臂在仿真环境中成功抓取单个草莓的概率是90%，成功抓取多个草莓的概率是84%，获得了很高的抓取成功率。
基金项目
本研究由国家自然科学基金资助，批准号：61905219。