CN112605983B - 一种适用于密集环境下的机械臂推抓系统 - Google Patents

Abstract

一种适用于密集环境下的机械臂推抓系统，包括PC，彩色深度相机，机械臂，智能夹持器；软件端包括CoppliaSim仿真平台；彩色深度相机通过USB与计算机相连接，机械臂通过局域网与PC连接；彩色深度相机获取彩色RGB图和深度图；计算机执行机械臂控制程序；机械臂执行动作；智能夹持器抓取或推物块。CoppliaSim仿真平台包括仿真模块、标定模块、图像预处理模块、特征提取模块、决策网络模块、动作策略模块、机器人I/O模块、机器人模块。本发明利用仿真和实际相结合，减少因训练而造成的机器人损坏，并加快抓取速度；利用深度强化学习的DQN，依据当前物体环境实时动态规划动作语义，实现推和抓的结合，同时规划出最适合抓取的方向，大大增加了机械臂抓取复杂环境的成功率。

Description

一种适用于密集环境下的机械臂推抓系统

技术领域

本发明涉及一种适用于密集环境下的机械臂推抓系统

背景技术

随着经济的发展和人口老龄化进程的加速，社会对实际可用行抓取机器人的需求日益增长。然而不同工业机器人在结构化环境下对工件的抓取，服务机器人将面临多种情况，其中就包括如何在密集型环境中高效抓取所需目标。然而就目前已存的大多数抓取系统而言，大多数仅适用于物体间隔空隙较大的场景，在面对密集物体排布时，由于物体周围没有足够的空间留给机械夹爪夹取物体，规划抓取变得十分困难。M.R.Dogar等研究人员在2012年发表了A planning framework for nonprehensile manipulation under clutterand uncertainty，提出可以通过抓扫结合的方式解决密集环境抓取困难问题。但是通过手工调制的方法只能针对一种环境，鲁棒性差，方法不可迁移。

近年来，随着计算机性能的提高和大数据的发展，深度学习突破了以往存在的限制。在大数据和深层次网络的前提下，基于深度神经网络的系统表现出了良好的性能。S.Levine等研究人员在2016年发表了End-to-end training of deep visuomotorpolicies，设计了一种基于深度神经网络的端到端机械臂推抓控制系统，该系统与M.R.Dogar等研究人员所提出的系统相比具有相对较好的泛化性。但是该系统由于将关节扭矩作为输出，造成了极高的样本复杂度，导致训练时间长，系统代价昂贵问题。

发明内容

本发明对克服现有技术存在的上述问题，提供一种适用于密集环境下的机械臂推抓系统。

本发明主要采用仿真与实际相结合的方式。首先对模型进行仿真训练，得到训练完成的神经网络参数并输入给实际网络；然后对机械臂进行标定；由深度照相机获取彩色图与深度图并做一定的预处理分别得到一个彩色高度图集和深度高度图集；将彩色高度图和深度高度图输入到特征提取网络得到相应的特征张量；将特征张量输入到推抓网络得到相应的Q值图；根据Q值图得到动作和坐标点输入给机械臂；最后机械臂根据指令执行动作。

本发明为解决现有技术问题所用的技术方案是：

一种适用于密集环境下的机械臂推抓系统，其特征在于：包括PC、彩色深度相机、机械臂，机械臂上装有智能夹持器；PC上安装的软件端包括CoppliaSim仿真平台；彩色深度相机通过USB与计算机相连接，机械臂通过局域网与PC连接；彩色深度相机获取彩色RGB图和深度图；计算机执行机械臂控制程序；机械臂执行动作，智能夹持器抓取或推物块；

所述的PC上安装有仿真模块、图像预处理模块、特征提取模块、决策网络模块、动作策略模块，机械臂上安装有标定模块、机器人I/O模块、机器人模块；仿真模块，采用机器人仿真软件搭建仿真环境，然后对仿真模型进行训练，并将其训练得到神经网络参数分别输出至特征提取模块和决策网络模块；标定模块，输入为深度相机图像中机械夹爪的相机坐标位置，计算并输出旋转矩阵R和平移矩阵M至图像预处理模块与机器人I/O模块；图像预处理模块，输入为深度相机直接得到的深度图像和彩色RGB图像以及标定模块得到的旋转矩阵R和平移矩阵M，在对图像进行一定处理之后输出彩色RGB高度图和深度高度图至特征提取模块；特征提取模块，输入为彩色RGB高度图和深度高度图以及仿真模块中得到的部分神经网络参数，经过神经网络，得到并输出一组彩色特征向量和一组深度特征向量至决策网络模块；决策网络模块，输入为特征向量以及仿真模块中得到的部分神经网络参数，经过神经网络，得到并输出Q值图至动作策略模块；动作策略模块，输入为Q值图，根据Q值图中Q值的高低选择动作及像素坐标点并输出至机器人I/O模块；机器I/O模块，输入为动作和像素坐标点以及标定模块中的旋转矩阵R和平移矩阵M，经过逆运动学得到关节位姿并输出至机器人模块；机器人模块，输入为各个关节的位姿并执行动作；

仿真模块包括：构建全卷积神经网络

其中

包括两个并行网络分别是PushNet推网络φ_p和GraspNet抓网络φ_g；利用机器人仿真软件构建仿真环境；从彩色深度相机获得大小均为224×224RGB图像I_tc和深度图像I_td；将I_td和I_tc进行坐标转换，去噪操作后得到RGB高度图I′_tc和深度高度图I′_td；并将RGB高度图I′_tc和深度高度图I′_td预处理后得到彩色图像图集G_tc.和深度图像图集G_td，作为状态S_t；将G_tc和G_td分别输入两个独立的网络φ_g和φ_p，输出一组每个动作对应的预测Q值图，Q_pt和Q_gt；根据一定策略以及式(1)和式(2)得到动作a_t′以及像素坐标(x_p，y_p，z_p)，z_p为(x_p，y_p)像素处深度值；

(x_p，y_p)＝argmax_(x，y)(Q_gt，Q_pt)) (2)

式(1)中φ_p(s_t)，φ_g(s_t)表示当前状态为S_t时，推网络和抓网络输出的Q值图。式(2)中x_p，y_p表示最大q值所对应的动作的像素坐标。

将像素坐标(x_p，y_p，z_p)变换至世界坐标系坐标(x_w，y_w，z_w)，机械臂根据选择的动作a_t′＝((x_w，y_w，z_w)，α)，做出相应的动作，改变拾取环境，其中α表示推或者抓取动作以及下爪角度；待机械臂完成动作a′_t后，深度相机得到下一时刻的彩色图像I_t+1，c和深度图像I_t+1，d，同时得到G_t+1，c和G_t+1，d作为下一状态s_t+1；根据当前状态s_t和s_t+1计算机械臂做出动作后的奖励值R_t；记录一条状态转移变量元组(s_t，a_t，R_g(s_t，s_t+1)，s_t+1，F)，并将其存至经验池

使用随机梯度下降的方法更新式(6)的优化目标值Jt，只通过计算估计执行动作的单像素p和对应执行动作的估计网络φ_ψ梯度并反向传播，其他像素点的梯度以0损失反向传播，加快网络更新速度；

目标值函数：

y_t＝R_g(s_t，s_t+1)+γQ(s_t+1，argmax(Q(s_s+1，a′_t))) (3)

误差为：

δ_t＝|Q_π(s_t，a_t)-y_t| (4)损失函数：

优化目标函数为：

重复操作直至成功率达到阈值ρ，并在此之后维持此成功率一段时间，则结束训练，保存FCN网络参数文件。

标定模块包括：用棋盘格标定法获得相机外参E_cam，旋转矩阵R和平移矩阵M，设机械臂末端初始位置为P_xyz，并将待拾取物体随机置于工作空间中；

式(7)中

是空间机械夹爪坐标系，

是像素坐标系，R是旋转矩阵，M是平移矩阵。

图像预处理模块包括：从彩色深度相机获得大小均为224×224RGB彩色图像I_tc和深度图像I_td；利用标定模块中的旋转矩阵R和平移矩阵M将I_td和I_tc从像素坐标转换至机器人坐标，利用阈值法进行去噪后得到RGB高度图I′_tc和深度高度图I′_td；将RGB高度图I′_tc和深度高度图I′_td用最邻近插值的方法缩放至像素大小为448×448的彩色高度图I″_tc和像素大小为448×448的深度高度图I″_td。

特征提取模块包括：将360°均分为16等份，得到旋转角度集合θ_r＝{22.5°，45°，…，360°}；将彩色高度图I″_tc和深度高度图I″_td通过仿射变换旋转θ°，其中θ°∈θ_r，分别得到彩色图像集合G_tc＝{I″′_tc0，I″′_tc1...I″′_tc15}和深度图集合G_td＝{I″′_td0，I″′_td1...I″′_td15}；导入特征提取神经网络参数；将得到的G_tc和G_ta取出16组相同角度的I″′_tci和I″′_tdi，依次输入特征提取神经网络得到彩色特征图I_ci，深度特征图I_di，共计32张特征图，其中i∈{0，1，…，15}。

决策网络模块包括：将所有旋转同一角度的彩色特征图I_ci和深度特征图I_di拼接成推张量I_i，共计16维，其中i∈{0，1，…，15}；导入决策神经网络参数，决策网络包括PushNet深度Q网络和GraspNet深度Q网络；将I_i依次送入决策网络中的PushNet深度Q网络中得到像素级推Q值图，同时依次送入GraspNet深度Q网络中得到像素级抓Q值图；上采样恢复224×224尺寸大小，得到推预测Q值图集合Q_pt＝{Q_pt1，Q_pt2，…，Q_pt15}和抓预测Q值图集合Q_gt＝{Q_gt1，Q_gt2，…，Q_gt15}。

动作策略模块包括：判断推预测Q值图集合Q_pt和抓取预测Q值图集合Q_gt的最大值q_pk和q_gk；由式(1)和式(2)得到最大值q值的以及(x_p，y_p)像素处深度值z_p和机械臂末端的旋转角度α，其中α＝360/k。

机器I/O模块包括：利用标定模块中的旋转矩阵R和平移矩阵M将将像素坐标(x_p，y_p，z_p)变换至世界坐标系坐标(x_w，y_w，z_w)；则经由InverseKinematics计算，得到机械臂各关节位姿，并输出给机器人。

机器人模块包括：机器人接受指令后，机械臂末端旋转

下爪至(x_w，y_w，z_w)；执行动作，执行完成，关闭机械夹爪，返回初始位置。

优选地，实现了端到端的控制，输入为视觉图像信息，输出为机械臂目标位置。

优选地，所述仿真模块中，以先在仿真环境中进行神经网络参数的训练，继而用于实际系统，提高实际抓取物体的效率。

本发明的优点和积极效果是：

首先本发明利用仿真和实际相结合，减少了实际系统因训练而造成的机器人损坏，并加快了实际系统的抓取速度。

本发明利用了深度强化学习的DQN，依据当前物体环境实时动态规划动作语义，实现了推和抓的结合，同时规划出最适合抓取的方向，大大增加了机械臂抓取复杂环境的成功率。

附图说明

图1为本发明的系统结构框图。

图2为本发明的仿真系统结构框图。

图3为本发明搭建的实际环境。

图4为发明图像预处理框架图。

图5为本发明决策系统总体框架。

图6为本发明的实际抓取流程图。

图7为本发明的仿真训练成功率图。

图8为本发明的FCN网络结构图。

图9为本发明的仿真训练流程图

具体实施方式

以下结合附图对本发明做进一步详述：

一种适用于密集环境下的机械臂推抓系统，其系统结构框架图如图1所示。平台硬件组成主要包括PC，彩色深度相机，机械臂，智能夹持器；软件端主要组成包括CoppliaSim仿真平台。彩色深度相机通过USB与计算机相连接，机械臂通过局域网与PC连接。彩色深度相机负责获取彩色RGB图和深度图；计算机负责执行机械臂控制程序；机械臂负责执行动作；智能夹持器负责抓取或推物块。

本发明的具体实施方式如下：

CoppliaSim仿真软件,Pycharm编程环境安装在Ubuntu环境下；PC使用CPU为Intelcore i7-7800k 3.50hz，GPU为Nvidia TITAN Xp；彩色深度相机使用RealsenseD435；机械臂使用Universal Robot公司生成的UR5机械臂；智能夹持器为BY-P80。

(1)仿真系统：其框架图如图2所示。采用CoppliaSim仿真软件导入机械臂，智能夹持器，深度彩色相机和九个形状颜色不同的物块。需改变摩擦系数，使其与真实世界相似；构造FCN神经网络并对其参数进行初始化操作，网络结构图如图8；采用Pycharm与CoppliaSim进行通信；环境配置完成后，开始对仿真环境进行训练；得到训练完成的神经网络参数进行保存,仿真流程图如图9，仿真训练成功率如图7。

(2)标定模块：对彩色深度相机中的内参进行标定；采用SVD分解得到旋转矩阵和平移矩阵；利用Nelder–Mead算法迭代得到最优旋转矩阵和平移矩阵，输出相机内参，最优旋转矩阵和最优平移矩阵。

(3)图像预处理模块：将彩色深度相机置于工作空间上方50cm,斜下方24°朝向处，如图3所示，并获取大小均为224×224×3彩色图像和深度图像；将图像的投影平面由像素坐标系转至机器人坐标系；利用阈值法对图像中的深度信息进行去噪处理；利用去噪后的深度信息对RGB图像进行排序，构造出上至下的垂直视角图像；将排序后的RGB图像像素赋值给RGB投影面像素，排序后的深度信息赋值给深度投影面，继而使原始图像转换至高度图；采用最邻近插值方法将彩色RGB高度图和深度高度图缩放至大小为448×448×3，通过仿射变换将其等角度旋转成16张大小均为224×224×3的彩色RGB高度图和16张大小均为224×224×3的深度高度图。图像预处理框架图见图4。

(4)特征提取网络模块：特征网络由Densenet-121网络构成，并采用Imagenet数据集对Densenet-121网络进行预训练。导入仿真系统训练完成的神经网络参数；将图像预处理以后的图像依次输入特征提取网络，输出16个大小为1024×1×14×14彩色高度特征张量和16个大小为1024×1×14×14深度高度特征张量。

(5)决策网络模块：决策网络由两个神经网络构成，分别是抓取网络和推网络，两个网络结构完全相同，由两个1×1的卷积层与非线性激活函数ReLU和一个空间标准化层构成。将仿真系统训练完毕的网络参数导入；将特征提取网络中输出的特征张量按彩色高度特征张量与深度高度特征张量等角度拼接成一个大小为1024×2×14×14张量，一共形成16个张量，输入决策网络中；在网络的输出中进行双线性上采样，得到与原始图像像素大小一致的Q值图，两个网络一共输出32张大小为224×224×1预测Q值图和1605632个预测Q值。特征提取网络模块与决策网络模块总体框架图如图5。此外，在仿真阶段，决策网络模块会根据动作策略模块输出的实际Q值图与预测Q值图做差值运算，采用Huber损失函数以及随机梯度下降优化算法进行整体FCN网络的训练更新。

(6)动作策略模块：根据推抓，下爪角度，下爪像素点位置生成一个动作空间，一共包括1605632个动作；由决策网络模块的输出作为动作策略模块的输入，根据Q值图最高的Q值选择相应的动作和对应的像素点位置。此外，在仿真阶段，该模块会生成一个随机数，当随机数小于一定阈值时，随机从动作空间中选择一个动作及对应的像素点位置；当随机数大于一定阈值时，根据Q值图中最高的Q值选择相对应的动作以及像素点位置，并生成实际的Q值图，用于网络参数的更新；将像素点从像素坐标转换到机器人坐标；最后将动作信息输出。

(7)机器I/O模块：由动作策略模块的输出作为输入；根据机器人坐标下的像素点位置，以及目标动作和旋转角度利用逆运动学求解出机械臂每个关节位姿；输出给机器人模块。

(8)机器人模块：该模块包括了机械臂和机械夹爪。机械臂根据机器I/O模块中得到的信息进行关节位姿的调整；机械夹爪是单独I/O控制的，根据下达开抓指令和闭爪指令来完成抓取。机器人和机械夹爪在完成相应指令动作后会回传当前状态信息，作为执行下一动作的判断标准和生成实际动作Q值图的必要信息。

以上是整个系统的控制情况。仿真模块的存在减少了实际训练时出现的问题；标定模块能对机械人进行位姿标定；图像预处理模快降低了图像的噪声，提升了抓取精确度；特征提取网络模块使用Densenet-121实现了特征的重用和效率的提升；特征提取网络模块和决策网络模块的连接实现了像素级的价值估计。动作策略模块在保持有效的寻找最优动作的前提下保证了一定的环境探索度。

本说明书实施例所述的内容仅仅是对发明构思的实现形式的列举，本发明的保护范围不应当被视为仅限于实施例所陈述的具体形式，本发明的保护范围也及于本领域技术人员根据本发明构思所能够想到的等同技术手段。

Claims (3)

1.一种适用于密集环境下的机械臂推抓系统，其特征在于：包括PC、彩色深度相机、机械臂，机械臂上装有智能夹持器；PC上安装的软件端包括CoppliaSim仿真平台；彩色深度相机通过USB与计算机相连接，机械臂通过局域网与PC连接；彩色深度相机获取彩色RGB图和深度图；计算机执行机械臂控制程序；机械臂执行动作，智能夹持器抓取或推物块；

所述的PC上安装有仿真模块、图像预处理模块、特征提取模块、决策网络模块、动作策略模块，机械臂上安装有标定模块、机器人I/O模块、机器人模块；仿真模块，采用机器人仿真软件搭建仿真环境，然后对仿真模型进行训练，并将其训练得到神经网络参数分别输出至特征提取模块和决策网络模块；标定模块，输入为深度相机图像中机械夹爪的相机坐标位置，计算并输出旋转矩阵R和平移矩阵M至图像预处理模块与机器人I/O模块；图像预处理模块，输入为深度相机直接得到的深度图像和彩色RGB图像以及标定模块得到的旋转矩阵R和平移矩阵M，在对图像进行一定处理之后输出彩色RGB高度图和深度高度图至特征提取模块；特征提取模块，输入为彩色RGB高度图和深度高度图以及仿真模块中得到的部分神经网络参数，经过神经网络，得到并输出一组彩色特征向量和一组深度特征向量至决策网络模块；决策网络模块，输入为特征向量以及仿真模块中得到的部分神经网络参数，经过神经网络，得到并输出Q值图至动作策略模块；动作策略模块，输入为Q值图，根据Q值图中Q值的高低选择动作及像素坐标点并输出至机器人I/O模块；机器I/O模块，输入为动作和像素坐标点以及标定模块中的旋转矩阵R和平移矩阵M，经过逆运动学得到关节位姿并输出至机器人模块；机器人模块，输入为各个关节的位姿并执行动作；

仿真模块包括：构建全卷积神经网络

其中

包括两个并行网络分别是PushNet推网络φ_p和GraspNet抓网络φ_g；利用机器人仿真软件构建仿真环境；从彩色深度相机获得大小均为224×224RGB图像I_tc和深度图像I_td；将I_td和I_tc进行坐标转换，去噪操作后得到RGB高度图I′_tc和深度高度图I′_td；并将RGB高度图I′_tc和深度高度图I′_td预处理后得到彩色图像图集G_tc和深度图像图集G_td，作为状态S_t；将G_tc和G_td分别输入两个独立的网络φ_g和φ_p，输出一组每个动作对应的预测Q值图，Q_pt和Q_gt；根据一定策略以及式(1)和式(2)得到动作a_t′以及像素坐标(x_p，y_p，z_p)，z_p为(x_p，y_p)像素处深度值；

(x_p，y_p)＝argmax_(x，y)(Q_gt，Q_pt)) (2)

式(1)中φ_p(s_t)，φ_g(s_t)表示当前状态为S_t时，推网络和抓网络输出的Q值图；式(2)中x_p，y_p表示最大q值所对应的动作的像素坐标；

将像素坐标(x_p，y_p，z_p)变换至世界坐标系坐标(x_w，y_w，z_w)，机械臂根据选择的动作a_t′＝((x_w，y_w，z_w)，α)，做出相应的动作，改变拾取环境，其中α表示推或者抓取动作以及下爪角度；待机械臂完成动作a′_t后，深度相机得到下一时刻的彩色图像I_t+1，c和深度图像I_t+1，d，同时得到G_t+1，c和G_t+1，d作为下一状态s_t+1；根据当前状态s_t和s_t+1计算机械臂做出动作后的奖励值R_t；记录一条状态转移变量元组(s_t，a_t，R_g(s_t，s_t+1)，s_t+1，F)，并将其存至经验池

使用随机梯度下降的方法更新式(6)的优化目标值J_t，只通过计算估计执行动作的单像素p和对应执行动作的估计网络φ_ψ梯度并反向传播，其他像素点的梯度以0损失反向传播，加快网络更新速度；

目标值函数：

y_t＝R_g(s_t，s_t+1)+γQ(s_t+1，argmax(Q(s_s+1，a′_t))) (3)

误差为：

δ_t＝|Q_π(s_t，a_t)-y_t| (4)

损失函数：

优化目标函数为：

重复操作直至成功率达到阈值ρ，并在此之后维持此成功率一段时间，则结束训练，保存FCN网络参数文件；

标定模块包括：用棋盘格标定法获得相机外参E_cam，旋转矩阵R和平移矩阵M，设机械臂末端初始位置为P_xyz，并将待拾取物体随机置于工作空间中；

式(7)中

是空间机械夹爪坐标系，

是像素坐标系，R是旋转矩阵，M是平移矩阵；

图像预处理模块包括：从彩色深度相机获得大小均为224×224RGB彩色图像I_tc和深度图像I_td；利用标定模块中的旋转矩阵R和平移矩阵M将I_td和I_tc从像素坐标转换至机器人坐标，利用阈值法进行去噪后得到RGB高度图I′_tc和深度高度图I′_td；将RGB高度图I′_tc和深度高度图I′_td用最邻近插值的方法缩放至像素大小为448×448的彩色高度图I″_tc和像素大小为448×448的深度高度图I″_td；

特征提取模块包括：将360°均分为16等份，得到旋转角度集合θ_r＝{22.5°，45°，…，360°}；将彩色高度图I″_tc和深度高度图I″_td通过仿射变换旋转θ°，其中θ°∈θ_r，分别得到彩色图像集合G_tc＝{I″′_tc0，I″′_tc1...I″′_tc15}和深度图集合G_td＝{I″′_td0，I″′_td1...I″′_td15}；导入特征提取神经网络参数；将得到的G_tc和G_ta取出16组相同角度的I″′_tci和I″′_tdi，依次输入特征提取神经网络得到彩色特征图I_ci，深度特征图I_di，共计32张特征图，其中i∈{0，1，…，15}；

决策网络模块包括：将所有旋转同一角度的彩色特征图I_ci和深度特征图I_di拼接成推张量I_i，共计16维，其中i∈{0，1，…，15}；导入决策神经网络参数，决策网络包括PushNet深度Q网络和GraspNet深度Q网络；将I_i依次送入决策网络中的PushNet深度Q网络中得到像素级推Q值图，同时依次送入GraspNet深度Q网络中得到像素级抓Q值图；上采样恢复224×224尺寸大小，得到推预测Q值图集合Q_pt＝{Q_pt1，Q_pt2，…，Q_pt15}和抓预测Q值图集合Q_gt＝{Q_gt1，Q_gt2，…，Q_gt15}；

动作策略模块包括：判断推预测Q值图集合Q_pt和抓取预测Q值图集合Q_gt的最大值q_pk和q_gk；由式(1)和式(2)得到最大值q值的以及(x_p，y_p)像素处深度值z_p和机械臂末端的旋转角度α，其中α＝360/k；

机器I/O模块包括：利用标定模块中的旋转矩阵R和平移矩阵M将将像素坐标(x_p，y_p，z_p)变换至世界坐标系坐标(x_w，y_w，z_w)；则经由InverseKinematics计算，得到机械臂各关节位姿，并输出给机器人；

机器人模块包括：机器人接受指令后，机械臂末端旋转

下爪至(x_w，y_w，z_w)；执行动作，执行完成，关闭机械夹爪，返回初始位置。

2.根据权利要求1所述的一种适用于密集环境下的机械臂推抓系统，其特征在于：实现端到端的控制，输入视觉图像信息，输出机械臂目标位置。

3.根据权利要求1所述的一种适用于密集环境下的机械臂推抓系统，其特征在于：所述仿真模块中，以先在仿真环境中进行神经网络参数的训练，继而用于实际系统，提高实际抓取物体的效率。

Images