[發明專利]一種基于TSDF三維重建的機械臂六自由度視覺閉環抓取方法

有效

• 申請號: 202210551360.5
• 申請日: 2022-05-18
• 公開（公告）號: CN114851201B
• 公開（公告）日: 2023-09-05
• 發明（設計）人: 歐林林;徐靖;禹鑫燚;周利波;魏巖
• 申請（專利權）人: 浙江工業大學
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 杭州天正專利事務所有限公司 33201
• 代理人: 舒良

1.一種基于TSDF三維重建的機械臂六自由度視覺閉環抓取方法，其特征在于：包括如下步驟：

步驟1：通過張正友相機標定法和ArUco?Markers標定法標定機械臂底座坐標系和相機坐標系；

步驟2：利用TSDF函數對獲取的圖像信息進行三維場景重建，以減少物體與物體之間的環境噪聲點；具體步驟如下：

2.1)：采用單一傳感器獲取視覺信息，通過OpenCV獲取雙目相機對抓取操作臺捕獲的信息，所捕獲的信息包括彩色信息和深度信息；

2.2)：由于光線、物體之間相互遮擋的環境因素導致傳感器獲取的深度圖像中存在較大噪聲點，從而影響網絡模型對圖像的抓取預測；首先，在每次抓取前對抓取操作臺進行球型點采樣，通過多次采樣局部圖像降低單一圖像中深度誤差；其次，利用TSDF函數將采樣點姿態和采樣圖像信息融合成三維重建體素網格；對于單獨一個體素而言，不僅包含直觀的x,y,z坐標，還用SDF和CAM兩個值來表示體素到最表面的距離，其中SDF為符號距離函數用于深度圖的融合，它用0表示表面，正值表示模型外部點，負值表示模型內部點，數值正比于點到最近表面的距離；表面容易提取為函數的零交叉點；如果第i幀深度圖像D(u，v)處深度值D(u，v)不為0，則將D(u，v)與體素相機坐標V(x,y,z)中的z相比較，如果D(u，v)大于z，說明此體素距離相機更近，用于重建場景外表面；如果D(u，v)小于z，說明此體素距離相機更遠，為重建場景內表面；體素點位于表面之外更靠近相機一側，則SDF值為正值；體素在表面之內，則SDF值為負值；

2.3)：定義上述體素柵格所在的坐標系為世界坐標系，體素坐標表示為相機的位姿為相機的內參矩陣為K；

所以根據ICP配準得到的變換矩陣將世界坐標系下的體素柵格投影到相機坐標系，再根據相機內參矩陣K轉換到圖像坐標系I_ij，其中i∈(0，1，...，n)，j∈(0，1，...，m)；

將體素柵格投影至圖像坐標系后，首先需要計算每個體素初始化SDF值，如公式(6)所示

式中表示第j個體素在世界坐標系下的位置信息，表示第i個相機位姿在世界坐標系下的位置信息，dep(I_ij)表示第j個體素在第i個相機深度圖像坐標系下的深度值；

根據公式(7)截斷每個體素的sdf值；

式中tsdf_i為體素截斷距離值，trunc表示人為設定的截斷距離，可理解為相機深度信息的誤差值，如果誤差越大，則trunc設置大一些，否則可能造成深度相機獲取的很多信息丟掉，將trunc設定為1；

將每個體素的sdf值截斷后，根據公式(8)重新計算每個體素的tsdf值：

其中ω_j(x，y，z)為當前幀全局體素柵格中體素的權重，ω_j-1(x，y，z)為上一幀體素柵格中體素的權重，tsdf_j為當前幀全局數據立方體中體素到物體表面的距離，tsdf_j-1為上一幀全局數據立方體中體素到物體表面的距離，V_.z表示體素在相機坐標系下的z軸坐標，D(u，v)表示當前幀深度圖像(u，v)處的深度值；

公式(9)計算每個體素的權重值；

w_j＝w_j-1+w_j??????(9)

從上至下渲染出抓取操作臺的環境信息并以此作為網絡模型的輸入；

步驟3：建立強化學習網絡模型；

步驟4：將預測的末端執行器抓取姿態反向投影至三維重建場景中，判斷本次預測抓取質量；具體步驟如下：

將預測的末端執行器抓取姿態反向投影至三維重建場景中，判斷本次預測抓取質量：將網絡輸出的6自由度抓取姿態反向投影至抓取前建立的三維體素場景中，通過視角錐體投影截取預測姿態下深度圖像；通過比較渲染圖像中夾持器兩端深度信息，推理平行夾爪與物體間的距離位置，從而判斷預測抓取姿態的抓取質量，形成機械臂6自由度抓取系統閉環反饋；

步驟5：通過機器人正逆運動學完成機械臂抓取移動；具體步驟如下：

當渲染出的深度圖像中夾持器兩端深度小于等于夾持器指尖深度，即夾持器預測姿態位于物體兩側，判斷本次預測姿態可以抓取；然后，控制機械臂根據預測姿態執行抓取計劃：通過機器人逆運動學求解出當前狀態下機械臂的6個關節角度數；然后將所述步驟3中強化學習網絡模型輸出預測的機械臂末端6維抓取姿態輸入機器人正運動學，求得機械臂從當前姿態變換至預測姿態點的末端執行器移動軌跡；當機械臂末端執行器運動至預測姿態后，機器人發出關閉夾持器信號，嘗試進行抓取動作；夾持器關閉后，末端執行器垂直上移15cm，從雙目相機獲取末端執行器上移后深度圖像，通過計算夾持器兩端的深度判斷實際抓取是否成功；當抓取成功時，強化學習模型獎勵賦值為1；抓取失敗時，強化學習網絡獎勵賦值為0；

步驟6：進行強化學習模型訓練，使得機械臂完成抓取動作。