[發明專利]一種基于投影和距離判斷的工業機器人碰撞預測方法

有效

• 申請號: 201710321118.8
• 申請日: 2017-05-09
• 公開（公告）號: CN107336230B
• 公開（公告）日: 2020-05-05
• 發明（設計）人: 張立彬;王佳才;陳教料;胥芳;鮑官軍
• 申請（專利權）人: 浙江工業大學
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 杭州斯可睿專利事務所有限公司 33241
• 代理人: 王利強

1.一種基于投影和距離判斷的工業機器人碰撞預測方法，其特征在于：

對于運行過程中可能發的碰撞，采用一種檢測關節投影干涉情況然后判斷關鍵點距離的方法進行碰撞檢測：將機器人關節向速度向量的法平面投影，若關節投影發生干涉則對關節軸線上各關鍵點最短距離進行判斷，若小于運行時的安全距離則認為存在碰撞的風險；所述碰撞預測方法包括以下步驟：

步驟1：機器人關節標準化處理

對于兩個機器人同時作業，按照關節幾何形狀，采用半球端面圓柱體包絡兩個關節從而得到標準化后的關節邊界；

步驟2：求解運動過程中的安全距離

將兩球面端點連線得到關節軸線，計其長度為l_i，i＝1，2；并得到圓柱體半徑為r_i，在各軸線按等間距取個點作為計算關節距離的關鍵點記為根據關節軸線上所取關鍵點可得兩關節碰撞時的臨界距離：

根據d_hit設定碰撞檢測的極限距離d_lim＝k₁*d_hit以及安全距離d_safe＝k₂*d_hit，k₁，k₂為安全系數；

步驟3：碰撞預測

根據關節角位移以及瞬時角速度可由正運動學求解得各關鍵點的笛卡爾坐標以及關節末端速度向量V_F，將投影到V_F向量的法平面得到關鍵點的投影點判斷各投影點之間的最小距離d′若：

則說明兩個關節在速度向量法平面的投影發生干涉，則進一步判斷各關節關鍵點之間最短距離d，若有：

d＝min{||P₁₁-P₂₁||，||P₁₁-P₂₂||，||P₁₁-P₂₃||...||P₁₁-P₂₄||}＜d_safe

則判定為有可能碰撞的危險狀態，否則認為是安全運行。