CN112643668A

CN112643668A - 一种适用于密集环境下的机械臂推抓协同方法

Info

Publication number: CN112643668A
Application number: CN202011386635.1A
Authority: CN
Inventors: 禹鑫燚; 樊越海; 邹超; 胡加南; 欧林林
Original assignee: Zhejiang University of Technology ZJUT
Current assignee: Zhejiang University of Technology ZJUT
Priority date: 2020-12-01
Filing date: 2020-12-01
Publication date: 2021-04-13
Anticipated expiration: 2040-12-01
Also published as: CN112643668B

Abstract

一种基于深度强化学习的端到端机械臂推抓系统方法，分为仿真训练与实际抓取两个部分。首先在仿真环境中搭建与真实抓取相似的场景，构建FCN网络(推网络)φ_p和抓网络φ_g，由动作后的状态给出评价，以此更新φ_p和φ_g网络参数，得到训练后的网络参数文件。在实际抓取部分，搭建实际抓取环境，导入仿真训练后的深度网络参数，根据网络输出的Q值图控制机械臂动作。本发明可以通过机械臂自主决策动作(抓或推)改变抓取环境，尤其适用于物体密集环境中。

Description

一种适用于密集环境下的机械臂推抓协同方法

技术领域

本发明设计一种适用于密集环境下的机械臂推抓协同方法。特别地，针对具有密集型环境的抓取任务，利用推抓动作的结合，能够有效的实现端到端的目标物体的抓取。

背景技术

随着电子商务的飞速发展，机器人代替人类拾取物体成为在仓储管理，无人商店，工业生产流水线等其他工业，商业领域中必不可少的环节。因此，如何让机器人高效的拾取物体成为了工业机器人主要研究内容之一。然而就目前已存的大多数抓取方法而言，它们仅适用于物体间隔空隙较大的场景，在面对密集物体排布时，机械夹爪容易卡在刚性物体连接处上方，无法快速地正确地抓取单个物体。许多研究者在考虑密集物体场景时，会选择用夹爪吸盘来代替机械夹爪，利用深度彩色相机拍摄当前物品堆放场景的彩色深度图，对图像进行物体分割和计算机械臂吸盘吸取位置。但是对于目前使用同样广泛的装配机械夹爪的机械臂还是很难轻松抓取排列紧密的刚性物体，这使得该种机械臂抓取目标物体的泛化性变差。

M.R.Dogar等研究人员在2012年发表了A planning framework fornonprehensile manipulation under clutter and uncertainty，通过手工调制的方法设计以了一个扫抓框架，能从复杂密集环境中抓取物体，提出可以通过抓扫结合的方式解决密集环境抓取困难问题。但是通过手工调制的方法只能针对一种环境，鲁棒性差，方法不可迁移。

随着人工智能的发展，深度强化学习方法在智能体面对复杂环境决策博弈取得了显著的突破。深度强化学习通过智能体与环境不断交互中，做出决策，并在决策后得到一系列奖励。智能体通过此奖励不断地优化自己的决策过程，目标是使累积奖励的期望最大化，最终得到最优策略。

A.Boularias等研究人员在2015年AAAI会议上发表了Learning to manipulateunknown objects in clutter by reinforcement，基于模型的强化学习，对感知和控制策略建模预测被推动的运动对象，但是此工作建模过程较为复杂，且是两段式控制。I.Popov等研究人员在2017年发表了Data-efficient deep reinforcement learning fordexterous manipulation，基于强化学习实现了端到端的控制，从图像像素级到关节扭矩，但是此工作具有极高的样本复杂度，使得系统训练时间很长，并且系统代价昂贵。因此如何无模型进行控制与降低样本复杂度成为值得思考的问题。

发明内容

本发明要克服现有技术的上述不足，提出了一种适用于密集环境下的机械臂推抓协同方法。本发明通过以下技术方案实现：在仿真阶段，首先对彩色深度图像做预处理，输入到提取特征网络中，得到特征图像；然后把特征图像分别输入到推网络与抓网络中，得到相应的Q值图；然后根据Q值图以及一定的策略选择动作，执行动作；然后进行神经网络参数的更新，并重复上述过程直至推抓取成功率到达一定阈值；仿真结束，将训练得到神经网络参数用于实际系统，使机械臂成功进行推抓物体。

本发明具有机械臂能智能改变密集物体的特点，能将密集推散，避免了机械臂因物体聚集而抓取失败，从而提高了在物体复杂密集中的抓取成功率和效率。

本发明提出了一种适用于密集环境下的机械臂抓推协同方法，包含以下步骤：

1)利用虚拟机器人仿真实验平台CoppeliaSim搭建机械臂抓推系统的环境；

具体步骤如下：

1-1)在CoppeliaSim中导入任意机械臂模型并初始位置P_xyz和任意两指或两指以上的机械夹爪模型；

1-2)在CoppeliaSim中导入深度彩色相机，设定CoppeliaSim中的工作空间；

1-3)步骤1-2)中设定的工作空间中导入m个物体模型作为仿真训练物品；

2)构建深度学习网络和加载深度学习模型，并在步骤1)中搭建的仿真环境训练深度学习网络，并得到训练后的网络，具体步骤如下：

2-1)构建全卷积神经网络FCN

其中

包括两个并行网络分别是PushNet推网络φ_p和GraspNet抓网络φ_g，两个网络的结构相同，包括一个共用的特征提取网络和各自的深度Q学习网络。其中特征提取网络可以是任意CNN网络模型，深度Q学习网络是由两层卷积层和一层反卷积层组成，卷积层中卷积核大小为1，步长为1，激活函数为Relu函数，归一化函数为BatchNorm2d函数，初始化为Kaiming_normal函数；

2-2)令初始时刻为t＝1；

2-3)利用步骤1)中初始化仿真环境；

2-4)利用步骤1-2)中的彩色深度相机获得大小均为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，预处理具体步骤如下：

2-4-1)将RGB高度图I′_tc和深度高度图I′_td用最邻近插值的方法缩放至像素大小为448×448的彩色高度图I″_tc和像素大小为448×448的深度高度图I″_td；

2-4-2)均分360°为16等份，得到旋转角度集合θ_r＝{22.5°,45°,…,360°}将步骤2-4-1)中的I_tc和I″_td通过仿射变换旋转θ°，其中θ°∈θ_r，分别得到彩色图像集合G_tc＝{I″′_{tc 0},I″′_{tc 1}…I″′_{tc 15}}和深度图集合G_td＝{I″′_{td 0},I″′_{td 1}…I″′_{td 15}}；

2-5)将步骤2-4-2)中得到的G_tc和G_td分别输入两个独立的网络φ_g和φ_p，输出一组t时刻时每个动作对应的预测Q值图，Q_pt和Q_gt，具体步骤如下：

2-5-1)从步骤2-4-2)中得到的G_tc和G_td取出16组相同角度的I″′_tci和I″′_tdi，依次输入步骤2-1)的CNN网络得到彩色特征图I_ci，深度特征图I_di，共计32张特征图。并将所有旋转同一角度的彩色特征图I_ci和深度特征图I_di拼接成推张量I_i，共计16维，其中i∈{0,1,…,15}；

2-5-2)将步骤2-5-1)中的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}；

2-6)随机生成随机数λ，选择动作像素坐标和旋转角度；

2-6-1)若λ>ε，其中ε是0至1的一个随机常数。械臂根据当前的预测q值，选择最大值q值对应的动作a′_t：

(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值所对应的动作的像素坐标。

判断步骤2-5-2)中的推预测Q值图集合Q_pt和抓取预测Q值图集合Q_gt的最大值q_pi和q_gi，其中i∈{0,1,…,15}。得到最大值q值的以及(x_p,y_p)像素处深度值zp和机械臂末端的旋转角度α，其中α＝360/i；

比较q_pi和q_gi，决定执行的动作。若q_gi>q_pi，则去执行步骤2-7-1)中的抓取动作；若q_gi<q_pi，则去执行步骤2-7-2)中的推动作；

2-6-2)若λ≤ε，选择随机探索动作和其像素坐标(x_p,y_p,z_p)，(x_p,y_p,z_p)在工作空间内；

2-7)将像素坐标(x_p,y_p,z_p)变换至世界坐标系坐标(x_w,y_w,z_w)，机械臂根据选择的动作a_t′＝((x_w,y_w,z_w),α),做出相应的动作，改变拾取环境,其中α表示推或者抓取动作以及下爪角度；

2-7-1)机械臂执行抓取动作，则经由Inverse Kinematics计算后，机械臂末端旋转

下爪至(x_w,y_w,z_w)，关闭机械夹爪，记录夹爪量程l，返回初始位置P_xyz；

2-7-2)机械臂执行推动作，则经由Inverse Kinematics计算后，机械臂末端旋转

下爪至(x_w,y_w,z_w)，关闭机械夹爪，朝α方向推，返回初始位置P_xyz；

2-8)待机械臂完成动作a′_t后，重复步骤2-4)，得到I_t+1,c和I_t+1,d，同时得到G_t+1,c和G_t+1,d作为下一状态s_t+1。同时观察工作空间内是否还剩余物体，若未剩余，则设置此回合结束标志F＝1，否则设置F＝0；

2-9)根据当前状态s_t+1计算机械臂做出动作后的奖励值R_t，奖励计算规则如下：

2-9-1)利用步骤2-8)中I_t+1,c和I_t+1,d与步骤2-4)中的I_tc和I_td做差值对比，若超过阈值τ，

则判断机械臂推抓取。式(3)中n为深度特征图中像素点的个数，τ为一

个常数阈值。设置奖励为R_t(s_t,s_t+1)＝0.5；

2-9-2)利用步骤2-7-1)中夹爪；

2-9-3)量程l，若机械夹爪未能完全闭合，即l＜k，则视为抓取成功R_t(s_t,s_t+1)＝1；

2-9-4)其余情况则视为推失败和抓取失败，设置奖励为R_t(s_t,s_t+1)＝0。记录抓取动作次数j和抓取成功次数k；

2-10)记录一条状态转移变量元组(s_t,a_t,R_g(s_t,s_t+1),s_t+1,F)，并将其存至经验池D。若经验池存储的元组数量大于训练采样数量b，则执行步骤2-11)训练φ_g和φ_p，若经验池存储元组数量小于采样数量，则重复步骤2-4)至步骤2-10)；

2-11)根据步骤2-9)中计算得到的R_g和步骤2-8)中的s_t+1，重复步骤2-5)得到下一状态预测最大q值，计算目标值函数y_t

y_t＝R_at(s_t,s_t+1)+γQ(s_t+1,argmax(Q(s_s+1,a′_t))) (4)

误差为：

δ_t＝|Q_π(s_t,a_t)-y_t| (5)

2-12)根据Huber函数定义φ_g和φ_p的损失函数：

优化目标函数为：

式(7)中b表示训练采样数量；

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

2-13)计算当前成功率

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

2-14)令当前时间为t+1，确认回合结束标志F，若F＝1，则确认为当前回合结束，重新返回步骤1-3)中，；若F＝0，则确认为当前回合未结束，则重新返回步骤2-4)中获得t+1时刻的状态；

3)利用步骤2)中训练后的FCN网络参数进行实际抓取物体，具体步骤如下：

3-1)搭建真实抓取系统，该系统包括一台机械臂，一台深度彩色相机，一台机械夹爪，一台计算机和待拾取物块；

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

式(8)中

是空间机械夹爪坐标系，

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

3-3)取当前时刻为t，使用深度彩色相机获得当前工作空间内彩色图像I_tc和深度图像I_td作为当前状态s_t，经由步2-4-2)步骤中得到G_tc和G_td，并将G_tc和G_td输入步骤2-13)中训练后的FCN网络计算得到预测q值，由公式(1)得到最大动作对应动坐标(x_w,y_w,z_w)和世界坐标系下的旋转角度α；

3-4)若机械臂执行推动作，则经由Inverse Kinematics计算后，机械臂末端旋转

下爪至(x_w,y_w,z_w)，关闭机械夹爪，朝α方向推，返回初始位置P_xyz。若机械臂执行抓取动作，则经由Inverse Kinematics计算后，机械臂末端旋转

3-5)令时间为t+1，利用相机获取得到当前工作空间RGB图像I_t+1,c和深度图像I_t+1,d，生成G_t+1,c和G_t+1,d作为S_t+1。若公式(3)成立，则判断机械臂推抓取，否则，则判断失败。同时利用深度信息I_t+1,d判断工作空间内是否有物体剩余，若未剩余，则进入步骤3-3)，若有剩余，则继续步骤3-3)。

优选地，步骤2-4)中总共旋转32次，彩色高度图旋转16次，深度高度图旋转16次。

优选地，步骤2-5)中得到1605632个Q值和1605632个可能执行的动作。

优选地，步骤2-7-2)和3-4)中机械臂朝方向推5cm。

本发明的优点在于：

本发明提出了一种适用密集环境下的机械臂推抓协同方法，训练全程由机械臂自主完成；并且依次经过密集连接卷积网络和强化学习决策网络的连接，上采样，做到了端到端的输出动作语义,输入深度彩色图像，输出机械臂的目标位置，从而提高了处理效率。本发明利用深度强化学习DQN的方法，能够依据当前物体环境实时动态规划动作语义，在实现推抓结合的同的时，规划最合适抓取的方向，大大增加了机械臂在复杂环境下抓取物块的成功率。

附图说明

图1为本发明的方法流程图。

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

图3为本发明的FCN网络架构。

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

图5为本发明的搭建的实际抓取环境。

图6为本发明的仿真训练系统结构图。

图7为本发明的实际推抓系统结构图。

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

具体实施实例

本发明提出了一种适用于密集环境下的机械臂推抓协同方法，下面结合附图和具体实施例对本发明进一步详细说明如下：

本发明提出一种适用于密集环境下的机械臂推抓协同方法，整体流程图如图1所示，具体包含以下步骤：

1)构建机器人仿真平台，本实例采用的是CoppeliaSim仿真软件，具体步骤如下：

1-1)在CoppeliaSim仿真软件中导入一台可编程控制的机械臂，在本实施实例中采用的是Universal Robots UR5机械臂；导入任意一款机械夹爪，本实施实例选取BY-P80智能夹持器，设机械臂初始位置为P_xyz；

1-2)在CoppeliaSim软件中导入深度彩色相机，本实施实例中选取用IntelRealsenseD415相机，置于工作空间上方50cm处，并斜下方45°朝下；

1-3)划定工作空间，导入待抓取物体。本实施实例中设定为长70cm，宽70cm的正方形工作空间，在工作空间中导入待抓取的物体，实施实例选取为八个物体，分别是两个个边长为50*50mm，高为30mm，半径为25mm的拱形和两个个半径为25mm，高30mm的半圆体。一个30mm*30mm*30mm的正方体，一个40mm*20mm*40mm的长方体，一个42mm*20mm*40mm的三角体，以及一个半径为25mm，高30mm的的圆柱体。其中任意两块拱形和两个半圆体组合成一个长方体，其余物体以任意角度随意放置在工作空间内；

2)构建FCN

网络模型，并在步骤1)中搭建的仿真环境训练深度学习网络，并得到训练后的神经网络参数，具体步骤如下：

2-1)构建PushNetφ_p推网络和GraspNetφ_g抓网络。以PushNetφ_p推网络为例，PushNet推网络包含特征提取网络和Q值估计网络，网络的顺序依次如下：

在ImageNet预训练后导入特征提取网络模型Densenet-121，输入为224×224×3图像，输出为1024×1的矩阵特征。定义两层Conv2d层为卷积核大小为1，步长为1，激活函数为Relu函数，归一化函数为BatchNorm2d函数，初始化为Kaiming_normal函数。输入尺寸为1024×1，输出尺寸为14×14×1。定义上采样层为放大系数为16倍，采用双线性插值方法；

2-2)选取当前时刻为t；

2-3)利用步骤1)中初始化仿真环境；

2-4)利用realsenseD415彩色深度相机得到当前工作空间大小均为224×224×3的RGB图像I_tc和深度图像I_td；将I_td和I_tc从像素坐标转换至机器人坐标，并采用阈值法进行去噪操作后得到RGB高度图I′_tc和深度高度图I′_td；并将RGB高度图I′_tc和深度高度图I′_td预处理后得到彩色图像图集G_tc.和深度图像图集G_td，作为状态S_t，预处理具体步骤如下：

2-4-2)将彩色高度图I″_tc和深度高度图I″_td进行仿射变化等角度旋转成大小均为224×224×3的16张彩色RGB高度图，并组成彩色图像图集G_tc＝{I″′_{tc 0},I″′_{tc 1}…I″′_{tc 15}}和大小均为224×224×3的16张深度高度图，并组成的深度图像图集G_td＝{I″′_{td 0},I″′_{td 1}…I″′_{td 15}}。；最后将两者作为网络输入状态S_t；

2-5-1)从步骤2-4-2)中得到的G_tc和G_td取出16组相同角度的I″′_tci和I″′_tdi依次输入至步骤2-1)中的Densenet-121网络，得到16个大小为1024×1×14×14的彩色图像特征和16个大小为1024×1×14×14的深度图像特征；将旋转相同角度的16个彩色图像特征和16个深度图像特征在第二维度上拼接成1024×2×14×14深度彩色图像特征I_i，其中i∈{0,1,…,15}；

2-5-2)将步骤2-5-1)中的I_i依次输入至PushNetφ_p推网络，同时也依次输入至GraspNetφ_g抓网络中，输出224×224预测抓Q值图集Q_gt＝{Q_gt1,Q_gt2,…,Q_gt15}和224×224预测推Q值图集Q_pt＝{Q_pt1,Q_pt2,…,Q_pt15}，共计得到1605632个q值；

2-6)取ε＝0.1。生成随机数λ，本实施实例中λ>0.1，由公式

(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值所对应的动作的像素坐标。

计算最大q值的像素坐标(x_p,y_p,z_p)和机械臂末端的旋转角度α，设动作为推a_tp；

2-7)将像素坐标(x_p,y_p,z_p)变换至世界坐标系坐标(x_w,y_w,z_w)，机械臂根据选择的动作a_t′＝((x_w,y_w,z_w),α),做出相应的动作，改变拾取环境,其中α表示推或者抓取动作以及下爪角度，具体步骤如下：

下爪至(x_w,y_w,z_w)，关闭机械夹爪，朝α方向推5cm，返回初始位置P_xyz；

2-8)待机械臂回到初始位置P_xyz后，得到当前工作空间RGB图像I_t+1,c和深度图像I_t+1,d，生成G_t+1,c和G_t+1,d作为S_t+1。本实施实例中I_t+1,d≥10，工作空间内还存在物体，F＝0；

2-9)计算机械臂完成动作后的奖励R_t，对步骤2-8)中S_t+1与步骤2-4)中的S_t两者状态中每个像素点做差值对比，本实施实例中：

式(3)中n为特征图中像素的总个数，则判断为机械臂推成功，设置奖励为R_t(s_t,s_t+1)＝0.5。记录抓取动作次数j和抓取成功次数k；

2-10)记录一条状态转移变量元组(s_t,a_t,R_g(s_t,s_t+1),s_t+1,F)，并将其存至经验池D。设经验池存储的元组数量大于训练采样数量256，则执行步骤2-9)训练PushNetφ_p推网络和GraspNetφ_g抓网络；

2-11)根据步骤2-6)中计算得到的R_t和步骤2-7)中的s_t+1，重复步骤2-5)得到下一状态预测最大q值，计算目标值函数y_t：

y_t＝R_at(s_t,s_t+1)+γQ(s_t+1,argmax(Q(s_s+1,a′_tp))) (4)

误差为：

δ_t＝|Q_π(s_t,a′_tp)-y_t| (5)

2-12)计算损失函数：

优化目标函数为：

式(7)中，b表示训练采样数量,使用随机梯度下降的方法更新优化目标值J_t，实例中选择学习率为0.0001，动量因子为0.9，权重衰减因子为0.00002；

2-13)计算当前成功率

若成功率到达阈值80％后，并在此之后维持此成功率一段时间，则结束训练；

2-14)令当前时间为t+1，确认回合结束标志F，若F＝1，则确认为当前回合结束，重新返回步骤中；若F＝0，则确认为当前回合未结束，则重新返回步骤2-2)中获得t+1时刻的状态；

3)搭建真实深度强化学习抓取系统：

3-1)如图7，该系统需要一台装有Ubuntu系统的计算机，本实例中采用的计算机CPU使用Intel Core i7-7800K 3.50Ghz，GPU使用的是Nvidia TITAN Xp，一个BY-P80智能夹持器，一台RealsenseD435深度彩色相机和八个待抓物体，四块块边长为50mm*50mm，高为30mm，半径为25mm的拱形和两个个半径为25mm，高30mm的半圆体。一个30mm*30mm*30mm的正方体，一个40mm*20mm*40mm的长方体，一个42mm*20mm*40mm的三角体，以及一个半径为25mm，高30mm的圆柱体。机械臂基座固定于工作台上，并将机械夹爪安装至机械臂末端。深度彩色相机置于机械臂正对面高25cm处并以俯视角度45°朝下。计算机与机械臂和深度彩色相机进行连接，用以接收深度彩色图像和实时控制机械臂运动。工作空间内任意放置两块拱形和两个半圆体组合成一个长方体，其余物体以任意角度随意放置在工作空间内；

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

式(8)中

是空间机械夹爪坐标系，

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

3-3)取当前时刻为t，使用深度彩色相机获得当前工作空间内彩色图像I_tc和深度图像I_td作为当前状态s_t，经由步骤2-4-1)和2-4-2)步骤中得到G_tc和G_td，并将G_tc和G_td输入步骤2-13)中训练后的FCN网络计算得到预测Q值图，由式(1)得到最大动作对应动坐标(x_w,y_w,z_w)和世界坐标系下的旋转角度α；

3-4)本实施实例中机械臂执行推动作，则经由Inverse Kinematics计算后，机械臂末端旋转

下爪至(x_w,y_w,z_w)，关闭机械夹爪，朝α方向推5cm，返回初始位置P_xyz。

3-5)令当前时间为t+1，利用相机获取得到当前工作空间RGB图像I_t+1,c和深度图像I_t+1,d，生成G_t+1,c和G_t+1,d作为S_t+1；实施实例式(3)中τ＝300,并且机械臂推成功；确认回合结束标志F，实施实例中F＝0，则确认为当前回合未结束，则重新返回步骤2-2)0中获得t+1时刻的状态。

本方法经实验证明，在密集环境下，经过推抓动作的结合实验成功率达80％。

以上所述并非是对本发明的限制，本发明也并不仅限于上述举例，本技术领域的技术人员在本发明的实质范围内所做出的变化，改型，添加或替换，也属于本发明的保护范围。

Claims

1.一种适用于密集环境下的机械臂抓推协同方法，包含以下步骤：

具体步骤如下：

2-1)构建全卷积神经网络

其中

包括两个并行网络分别是PushNet推网络φ_p和GraspNet抓网络φ_g，两个网络的结构相同，包括一个共用的特征提取网络和各自的深度Q学习网络；其中特征提取网络可以是任意CNN网络模型，深度Q学习网络是由两层卷积层和一层反卷积层组成，卷积层中卷积核大小为1，步长为1，激活函数为Relu函数，归一化函数为BatchNorm2d函数，初始化为Kaiming_normal函数；

2-2)令初始时刻为t＝1；

2-3)利用步骤1)中初始化仿真环境；

2-4-2)均分360°为16等份，得到旋转角度集合θ_r＝{22.5°，45°，…，360°}将步骤2-4-1)中的I″_tc和I″_td通过仿射变换旋转θ°，其中θ°∈θ_r，分别得到彩色图像集合G_tc＝{I″′_{tc 0}，I″′_{tc 1}…I″′_{tc 15}}和深度图集合G_td＝{I″′_{td 0}，I″′_{td 1}…I″′_{td 15}}；

2-5-1)从步骤2-4-2)中得到的G_tc和G_td取出16组相同角度的I″′_tci和I″′_tdi，依次输入步骤2-1)的CNN网络得到彩色特征图I_ci，深度特征图I_di，共计32张特征图并将所有旋转同一角度的彩色特征图I_ci和深度特征图I_di拼接成推张量I_i，共计16维，其中i∈{0，1，…，15}；

2-5-2)将步骤2-5-1)中的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}；

2-6)随机生成随机数λ，选择动作像素坐标和旋转角度；

2-6-1)若λ＞ε，其中ε是0至1的一个随机常数；械臂根据当前的预测q值，选择最大值q值对应的动作a′_t：

(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值所对应的动作的像素坐标；

判断步骤2-5-2)中的推预测Q值图集合Q_pt和抓取预测Q值图集合Q_gt的最大值q_pi和q_gi，其中i∈{0，1，…，15}；得到最大值q值的以及(x_p，y_p)像素处深度值z_p和机械臂末端的旋转角度α，其中α＝360/i；

比较q_pi和q_gi，决定执行的动作；若q_gi＞q_pi，则去执行步骤2-7-1)中的抓取动作；若q_gi＜q_pi，则去执行步骤2-7-2)中的推动作；

2-6-2)若λ≤ε，选择随机探索动作和其像素坐标(x_p，y_p，z_p)，(x_p，y_p，z_p)在工作空间内；

2-7)将像素坐标(x_p，y_p，z_p)变换至世界坐标系坐标(x_w，y_w，z_w)，机械臂根据选择的动作a_t′＝((x_w，y_w，z_w)，α)，做出相应的动作，改变拾取环境，其中α表示推或者抓取动作以及下爪角度；

下爪至(x_w，y_w，z_w)，关闭机械夹爪，记录夹爪量程l，返回初始位置P_xyz；

下爪至(x_w，y_w，z_w)，关闭机械夹爪，朝α方向推，返回初始位置P_xyz；

2-8)待机械臂完成动作a′_t后，重复步骤2-4)，得到I_t+1，c和I_t+1，d，同时得到G_t+1，c和G_t+1，d作为下一状态s_t+1；同时观察工作空间内是否还剩余物体，若未剩余，则设置此回合结束标志F＝1，否则设置F＝0；

2-9-1)利用步骤2-8)中I_t+1，c和I_t+1，d与步骤2-4)中的I_tc和I_td做差值对比，若超过阈值τ，

则判断机械臂推抓取，式(3)中n为深度特征图中像素点的个数，设置奖励为R_t(s_t，s_t+1)＝0.5；

2-9-2)利用步骤2-7-1)中夹爪；

2-9-3)量程l，若机械夹爪未能完全闭合，即l＜k，则视为抓取成功R_t(s_t，s_t+1)＝1；

2-9-4)其余情况则视为推失败和抓取失败，设置奖励为R_t(s_t，s_t+1)＝0；记录抓取动作次数j和抓取成功次数k；

2-10)记录一条状态转移变量元组(s_t，a_t，R_g(s_t，s_t+1)，s_t+1，F)，并将其存至经验池D；若经验池存储的元组数量大于训练采样数量b，则执行步骤2-11)训练φ_g和φ_p，若经验池存储元组数量小于采样数量，则重复步骤2-4)至步骤2-10)；

y_t＝R_at(s_t，s_t+1)+γQ(s_t+1，argmax(Q(s_s+1，a′_t))) (4)

误差为：

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

2-12)根据Huber函数定义φ_g和φ_p的损失函数：

优化目标函数为：

式(7)中b表示训练采样数量；

2-13)计算当前成功率

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

式(8)中

是空间机械夹爪坐标系，

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

3-3)取当前时刻为t，使用深度彩色相机获得当前工作空间内彩色图像I_tc和深度图像I_td作为当前状态s_t，经由步骤2-4-1)和2-4-2)步骤中得到G_tc和G_td，并将G_tc和G_td输入步骤2-13)中训练后的FCN网络计算得到预测Q值图，由公式(1)得到最大动作对应动坐标(x_w，y_w，z_w)和世界坐标系下的旋转角度α；

下爪至(x_w，y_w，z_w)，关闭机械夹爪，朝α方向推，返回初始位置P_xyz；若机械臂执行抓取动作，则经由Inverse Kinematics计算后，机械臂末端旋转

3-5)令时间为t+1，利用相机获取得到当前工作空间RGB图像I_t+1，c和深度图像I_t+1，d，生成G_t+1，c和G_t+1，d作为S_t+1；若公式(3)成立，则判断机械臂推抓取成功，否则，则判断失败；同时利用深度信息I_t+1，d判断工作空间内是否有物体剩余，若未剩余，则进入步骤3-3)，若有剩余，则继续步骤3-3)。

2.如权利要求1所述的一种适用于密集环境下的机械臂抓推协同方法，其特征在于：步骤2-4)中总共旋转32次，彩色高度图旋转16次，深度高度图旋转16次。

3.权利要求1所述的一种适用于密集环境下的机械臂抓推协同方法，其特征在于：步骤2-5)中得到1605632个Q值和1605632个可能执行的动作。

4.如权利要求1所述的一种适用于密集环境下的机械臂抓推协同方法，其特征在于：步骤2-7-2)和3-4)中机械臂朝α方向推5cm。