[發明專利]一種基于RRT算法的雙機械臂解耦運動規劃方法及系統

在審

• 申請號: 202110430722.0
• 申請日: 2021-04-21
• 公開（公告）號: CN113276109A
• 公開（公告）日: 2021-08-20
• 發明（設計）人: 潘愛強;劉哲;魏本剛;江安烽;杜習周;李文鶴;許唐云;白成超;張家維
• 申請（專利權）人: 國網上海市電力公司;國網上海能源互聯網研究院有限公司;哈爾濱工業大學
• 主分類號: B25J9/16
• 分類號: B25J9/16;B25J9/00
• 代理公司: 北京工信聯合知識產權代理有限公司 11266
• 代理人: 夏德政

1.一種基于RRT算法的雙機械臂解耦運動規劃方法，其特征在于，所述方法包括：

根據每個機械臂對應的起始構型信息和目標構型信息，利用雙向快速搜索隨機樹RRT算法分別對每個機械臂進行針對環境靜態障礙物的無碰撞路徑規劃，以確定每個機械臂的第一運行路徑節點序列和所述第一運行路徑節點序列中每個節點對應的時間信息；

將每個機械臂的第一運行路徑節點序列根據每個節點對應的時間信息進行一一對應，并根據對應的結果進行碰撞檢測，獲取碰撞檢測結果；

當所述碰撞檢測結果指示發生碰撞時，將作為主臂的第一機械臂的第一運行路徑節點序列作為動態障礙物，利用單向RRT算法和雙向RRT算法重新對作為從臂的第二機械臂進行針對動態障礙物的無碰撞路徑規劃，以確定所述第二機械臂的最終運行路徑節點序列和所述最終運行路徑節點序列中每個節點對應的時間信息；

根據所述第一機械臂的第一運行路徑節點序列、所述第一機械臂的第一運行路徑節點序列中每個節點對應的時間信息、所述第二機械臂的最終運行路徑節點序列和所述最終運行路徑節點序列中每個節點對應的時間信息確定雙機械臂解耦運動規劃方案。

2.根據權利要求1所述的方法，其特征在于，所述方法還包括：

在利用單向RRT算法和雙向RRT算法重新對作為從臂的第二機械臂進行針對動態障礙物的無碰撞路徑規劃時，利用動態擴展隨機樹的思想，在當單向RRT算法遇到瓶頸時，采用雙向RRT算法進行加速運算，通過迭代的方式計算第二機械臂的第二路徑節點序列，直到第二機械臂的第二路徑節點序列不與第一機械臂的第一路徑節點序列發生碰撞時，確定當前的第二路徑節點序列為所述第二機械臂的最終運行路徑節點序列。

3.根據權利要求2所述的方法，其特征在于，所述方法利用動態擴展隨機樹的思想，在當單向RRT算法遇到瓶頸時，采用雙向RRT算法進行加速運算，通過迭代的方式計算第二機械臂的第二路徑節點序列，直到第二機械臂的第二路徑節點序列不與第一機械臂的第一路徑節點序列發生碰撞時，確定當前的第二路徑節點序列為所述第二機械臂的最終運行路徑節點序列，包括：

當利用單向RRT算法規劃的所述第二機械臂的第一隨機樹的節點數小于等于預設節點數閾值時采用單向目標偏置RRT算法進行規劃，以獲取第一子節點序列；

當所述第一隨機樹的節點數大于預設節點數閾值時，采用雙向RRT算法從所述第二機械臂對應的目標構型信息對應的目標節點開始進行第二棵隨機樹的擴展，以獲取第二子節點序列；

根據所述第一子節點序列和第二子節點序列確定當前的第二運行路徑節點序列，為當前的第二運行路徑節點序列中的每個節點賦予時間信息，根據當前的第二運行路徑節點序列、當前的第二運行路徑節點序列中每個節點的時間信息、所述第一機械臂的第一運行路徑節點序列和所述第一機械臂的第一運行路徑節點序列中每個節點的節點信息進行碰撞檢測，確定第一次發生碰撞的碰撞節點，將當前的第二運行路徑節點序列中的在所述碰撞節點之后的節點去除，保留沒有發生碰撞的節點；

將所述第一機械臂的第一運行路徑節點序列中從所述碰撞節點開始的運行路徑節點序列作為動態障礙物，重新進行路徑規劃，直至所述第二機械臂第二路徑節點序列各節點與所述第一機械臂第一路徑節點序列對應時刻的節點不再發生碰撞時停止，直接將當前的第二運行路徑節點序列和當前的第二運行路徑節點序列中每個節點的時間信息作為所述第二機械臂的最終運行路徑節點序列和所述最終運行路徑節點序列中每個節點對應的時間信息。