[發明專利]一種基于三維任務空間約束的機械臂避障方法及裝置

有效

• 申請號: 202011132708.4
• 申請日: 2020-10-21
• 公開（公告）號: CN112223291B
• 公開（公告）日: 2022-02-11
• 發明（設計）人: 劉鵬飛;徐妍;郭龍;楊健
• 申請（專利權）人: 哈工大機器人（合肥）國際創新研究院
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 合肥市浩智運專利代理事務所(普通合伙) 34124
• 代理人: 丁瑞瑞

1.一種基于三維任務空間約束的機械臂避障方法，其特征在于，所述方法包括：

步驟一：獲取關節空間中機械臂運動路徑的起始點qs和目標點qd；

步驟二：雙向RRT搜索樹包括搜索樹RRTree1和搜索樹RRTree2，初始化搜索樹RRTree1的樹根節點為起始點qs，搜索樹RRTree2的樹根節點為目標點qd，對搜索樹RRTree1進行擴展得到當前擴展關節點；

步驟三：依據設定的三維任務空間約束對當前擴展關節點new_point_temp進行約束更新得到更新后的擴展關節點；具體過程為：

利用給定約束矩陣獲取給定的三維任務空間約束范圍；

根據公式獲取機械臂末端工具相對于約束坐標系下的位姿矩陣，其中，表示機械臂末端工具坐標系的齊次變換矩陣，表示三維任務空間約束范圍的參考變換；

將機械臂末端工具相對于約束坐標系下的位姿矩陣通過轉換為一個6維的位移矢量，其中，表示機械臂末端工具相對于約束坐標系的位移，后三行表示末端工具相對于約束坐標系的姿態，考慮給給定約束矩陣C的邊界，利用公式

得到該約束下的笛卡爾空間位移Δx，C_max表示約束矩陣C的上邊界，C_min表示約束矩陣C的下邊界；

通過公式Δq＝J^T(JJ^T)^-1Δx將笛卡爾空間位移Δx投影到關節空間得到關節空間位移Δq，其中，J表示雅克比矩陣；

利用公式new_point_temp’＝new_point_temp-Δq對當前擴展關節點new_point_temp進行更新得到更新后的擴展關節點new_point_temp’；

步驟四：得到更新后的擴展關節點后，進行環境障礙物及自碰撞檢測并且對雙向RRT搜索樹進行擴展，提取出一條從起始點qs到達目標點qd的無碰撞原始路徑；

步驟五：對原始路徑進行優化得到優化的路徑；具體過程為：

步驟501：在原始路徑path中隨機提取樹節點i和樹節點j，原始路徑path中從樹節點i到樹節點j的路徑為子路徑path_ij；

步驟502：從樹節點i開始，依照給定的三維任務空間約束利用公式new_path_ij＝path_ij+Δq更新子路徑path_jj得到更新后的子路徑new_path_ij；

步驟503：判斷子路徑path_ij的長度是否小于更新后的子路徑new_path_ij，若是，則將更新后的子路徑new_path_ij替換子路徑path_ij得到更新后的路徑new_path，將更新后的路徑new_path作為原始路徑返回執行步驟501至步驟503，若否，則返回執行步驟501至步驟503，達到預設的迭代次數以后輸出最終優化的路徑；

步驟六：根據優化后的路徑，移動六軸機械臂從起始點qs無碰撞地運動到目標點qd。

2.根據權利要求1所述的一種基于三維任務空間約束的機械臂避障方法，其特征在于，所述步驟二包括：

搜索時，先對搜索樹RRTree1進行擴展，在關節空間范圍內通過公式獲取隨機點，其中，rand是一個0～1的隨機值，α是給定的0～1之間的值，random(χ)表示生成一個小于常數χ的隨機數；

在搜索樹RRTree1的樹節點中找到與隨機點最近的樹節點closestNode_1，從與隨機點最近的樹節點closestNode_1出發通過公式向著隨機點擴展一個步長得到當前擴展關節點new_point_temp，其中，segmentLength表示搜索步長。