[發明專利]一種適用于密集環境下的機械臂推抓系統

有效

• 申請號: 202011386636.6
• 申請日: 2020-12-01
• 公開（公告）號: CN112605983B
• 公開（公告）日: 2022-04-19
• 發明（設計）人: 禹鑫燚;樊越海;胡加南;鄒超;歐林林
• 申請（專利權）人: 浙江工業大學
• 主分類號: B25J9/08
• 分類號: B25J9/08;B25J9/16;B25J15/08;B25J18/00;B25J19/02;G06N3/04;G06N3/08
• 代理公司: 杭州天正專利事務所有限公司 33201
• 代理人: 王兵

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)；執行動作，執行完成，關閉機械夾爪，返回初始位置。