[發明專利]一種用于機器人抓取場景的點云碰撞檢測方法

有效

• 申請號: 202010885649.1
• 申請日: 2020-08-28
• 公開（公告）號: CN112060087B
• 公開（公告）日: 2021-08-03
• 發明（設計）人: 汪良紅;王輝;陳新;許藤
• 申請（專利權）人: 佛山隆深機器人有限公司
• 主分類號: B25J9/16
• 分類號: B25J9/16;G06T1/00;G06T7/70;G06T17/00
• 代理公司: 廣州粵高專利商標代理有限公司 44102
• 代理人: 劉俊

1.一種用于機器人抓取場景的點云碰撞檢測方法，其特征在于，包括以下步驟：

S1：構建機器人夾爪的包圍盒模型，并通過機器人相機采集工件區域的點云數據；

S2：建立機器人坐標系、包圍盒模型各個頂點的頂點坐標系和點云中各個點的點坐標系，并獲取各個頂點坐標系和各個點坐標系在機器人坐標系下的齊次變換矩陣；

S3：根據齊次變換矩陣分別獲取包圍盒模型的各個頂點和點云的各個點在機器人坐標系下的坐標；

S4：根據包圍盒模型的各個頂點和點云的各個點在機器人坐標系下的坐標，判斷點云的各個點與包圍盒模型的關系，得到點云在包圍盒模型內的點的數量；

在步驟S4中，點云的點與包圍盒模型的關系通過點云的點與包圍盒模型的三對平行平面的關系判斷得到；

如果點云的點在包圍盒模型的三對平行平面內，則點云的點在包圍盒模型內；否則，點云的點在包圍盒模型外；

點云的點與平行平面的關系通過計算點云的點和包圍盒模型頂點組成的向量與包圍盒模型的平行平面的法線夾角來判斷得到；

如果點云的點和包圍盒模型頂點組成的向量與一對平行平面的法線夾角均為銳角或均為鈍角，則該點在這對平面外；否則該點在這對平面內；

通過計算點云的點和包圍盒模型頂點組成的向量與包圍盒模型的平行平面的法線夾角來判斷點云的點與平行平面的關系的步驟具體如下：

S4.1：在包圍盒模型中選定一個頂點，設為第一頂點，將與該頂點相連的三個頂點分別設為第二頂點、第三頂點、第四頂點；

其中，設包圍盒模型的三對平行平面分別為一對平行的x平面、一對平行的y平面和一對平行z平面，且第一頂點和第二頂點分屬不同的z平面，第一頂點和第三頂點分屬不同的y平面，第一頂點和第四頂點分屬不同的x平面；

S4.2：獲取第一頂點、第二頂點、第三頂點、第四頂點在機器人坐標系下的的坐標

S4.3：計算第二頂點與第一頂點的向量v₁₂：

計算第三頂點與第一頂點的向量v₁₃：

計算第四頂點與第一頂點的向量v₁₄：

S4.4：將v₁₂、v₁₃、v₁₄兩兩相乘分別得到包圍盒模型在機器人坐標系下的x平面的法向量v_x、y平面的法向量v_y、z平面的法向量v_z：

v_x＝v₁₂×v₁₃；

v_y＝v₁₂×v₁₄；

v_z＝v₁₃×v₁₄；

S4.5：設點云中一個點A在機器人坐標系下的坐標為p₁₅，計算點A與第一頂點的向量：

計算點A與第二頂點的向量：

計算點A與第三頂點的向量：

計算點A與第四頂點的向量：

S4.6：計算v_x與v_p1、v_p4的點乘積，

若與同號，則點A在平行的x平面外，若與異號，則點A在平行的x平面內；

計算v_y與v_p1、v_p3的點乘積，

若與同號，則點A在平行的y平面外，若與異號，則點A在平行的y平面內；

計算v_z與v_p1、v_p2的點乘積，

若與同號，則點A在平行的z平面外，若與異號，則點A在平行的z平面內；

S5：將點云在包圍盒模型內的點的數量與預設的閾值進行比較；

若點云在包圍盒模型內的點的數量小于預設的閾值，則夾爪不會與實際物體發生碰撞；否則，夾爪會與實際物體發生碰撞。