[發明專利]一種基于虛擬采樣的六軸碼垛機器人位姿適配方法

有效

• 申請號: 201911150209.5
• 申請日: 2019-11-21
• 公開（公告）號: CN110815230B
• 公開（公告）日: 2022-02-22
• 發明（設計）人: 陳學能;樊海平;王君妍
• 申請（專利權）人: 廣東力生智能有限公司
• 主分類號: B25J9/16
• 分類號: B25J9/16;B65G61/00
• 代理公司: 成都正華專利代理事務所(普通合伙) 51229
• 代理人: 李蕊

1.一種基于虛擬采樣的六軸碼垛機器人位姿適配方法，其特征在于，包括以下步驟：

S1、在世界坐標系下，設六軸碼垛機器人末端手抓的坐標位置為p，六軸碼垛機器人末端手抓的初始坐標位置為p0，六軸碼垛機器人末端手抓本次運動的終點坐標位置為p1，待搬運箱子的初始坐標位置為p3，設待搬運箱子需要到達的位置為虛擬箱位，虛擬箱位的中心坐標位置為p4；

S2、將p0的位置坐標值更新為當前p的位置坐標值,將p1的位置坐標值更新為當前p3的位置坐標值；

S3、根據p0和p1計算得到p0到p1的中間點p2；

S4、根據p0、p1和p2，計算出六軸碼垛機器人末端手抓由p0到p2運動過程中的速度Normal(p0-2)，以及六軸碼垛機器人末端手抓由p2到p1運動過程中的速度Normal(p2-1)，將Normal(p0-2)及其對應的時間記錄于速度列表V1中，將Normal(p2-1)及其對應的時間記錄于速度列表V2中；

S5、將速度列表V1和V2中的時間記錄在信息采樣列表SampleList中，以速度列表V1和V2作為輸入，通過IK算法計算出速度列表V1和V2中各時間點對應的p以及六軸碼垛機器人六個轉軸對應的轉動角度th1、th2、th3、th4、th5和th6，并對應記錄在信息采樣列表SampleList中的各時間點下；

S6、最優姿態調整，確定六軸碼垛機器人第4轉軸、第5轉軸和第6轉軸的最優轉動角度RP4min、RP5min和RP6min，使六軸碼垛機器人的第4轉軸、第5轉軸和第6轉軸的轉動角度分別變動到RP4min、RP5min和RP6min，將變動過程中的時間、變動過程中各時間點對應的th1、th2、th3、th4、th5、th6和p記錄在信息采樣列表SampleList中；

S7、若當前p沒有到達p4，則將p0的位置坐標值更新為當前p的位置坐標值,將p1的位置坐標值更新為p4的位置坐標值，之后跳轉至步驟S3，如果當前p到達p4，則繼續執行步驟S8；

S8、若虛擬箱位已經填滿，則繼續執行步驟S9，否則跳轉至步驟S2；

S9、將步驟S6更新后得到的信息采樣列表SampleList中的數據按真實場景進行變換，得到新的信息采樣列表SampleList；

S10、將步驟S9中新的信息采樣列表SampleList中的數據轉換為機器人能夠識別的機器人程序，并將機器人程序發送給六軸碼垛機器人，完成六軸碼垛機器人位姿適配。

2.根據權利要求1所述基于虛擬采樣的六軸碼垛機器人位姿適配方法，其特征在于，所述步驟S3中，獲取p2的方法是：定義由p0指向p1的向量為vecp_0to1，設p0和p1的坐標分別為(x0,y0,p0)和(x1,y1,z1)，則

vecp_0to1＝(x1-x0,y1-y0,z1-z0)＝(vx,vy,vz)

3.根據權利要求1所述基于虛擬采樣的六軸碼垛機器人位姿適配方法，其特征在于，所述步驟S4中，Normal(p0-2)和Normal(p2-1)的計算方法是：

p0＝(x0,y0,z0)

p1＝(x1,y1,z1)

p2＝(x2,y2,z2)

vecp_0-2＝(x2-x0,y2-y0,z2-z0)

vecp_2-1＝(x1-x2,y1-y2,z1-z2)

4.根據權利要求1所述基于虛擬采樣的六軸碼垛機器人位姿適配方法，其特征在于，所述步驟S6具體包括以下步驟：

S61、設六軸碼垛機器人第4轉軸、第5轉軸和第6轉軸的優化轉動角度分別為RP4、RP5、RP6，當前六軸碼垛機器人末端手抓的坐標系正交基為Co1，當前p1的坐標系正交基為Co2,設sigma為Co1和Co2各個對應基坐標的夾角和,RP4、RP5和RP6的活動范圍分別為Range4、Range5和Range6，定義空白列表SL，定義最優條件判定閾值為a；

S62、通過隨機生成法生成3個隨機數，3個隨機數分別屬于Range4、Range5和Range6,將該三個隨機數分別賦值給RP4、RP5和RP6；

S63、使RP4在范圍Range4內移動，得到若干第一Col，并一一對應的求得若干第一sigma，將RP4移動過程中所獲得的值分別添加為SL的索引，將各第一sigma分別對應記錄為SL中各索引所對應的值，找到最小的第一sigma值，將該最小的第一sigma值所對應的索引確定為所述RP4min，之后清空SL，使SL恢復為空白列表；

S64、設RP4＝RP4min，使RP5在范圍Range5內移動，得到若干第二Col，并一一對應的求得若干第二sigma，將RP5移動過程中所獲得的值分別添加為SL的索引，將各第二sigma分別對應記錄為SL中各索引所對應的值，找到最小的第二sigma值，將該最小的第二sigma值所對應的索引確定為所述RP5min，之后清空SL，使SL恢復為空白列表；

S65、設RP4＝RP4min且RP5＝RP5min，使RP6在范圍Range6內移動，得到若干第三Col，并一一對應的求得若干第三sigma，將RP6移動過程中所獲得的值分別添加為SL的索引，將各第三sigma分別對應記錄為SL中各索引所對應的值，找到最小的第三sigma值，將該最小的第三sigma值所對應的索引確定為所述RP6min，之后清空SL，使SL恢復為空白列表；

S66、若RP6min≤a，則繼續執行步驟S67，否則跳轉至步驟S62；

S67、通過插值算法，使六軸碼垛機器人的第4轉軸、第5轉軸和第6轉軸的轉動角度分別變動到RP4min、RP5min和RP6min，將變動過程中的時間、變動過程中各時間點對應的th1、th2、th3、th4、th5、th6和p記錄在信息采樣列表SampleList中。