[发明专利]一种适用于密集环境下的机械臂推抓系统

权利要求书

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)；执行动作，执行完成，关闭机械夹爪，返回初始位置。