[發明專利]一種多冗余度機械臂系統的協同控制方法與裝置

有效

• 申請號: 201710084019.2
• 申請日: 2017-02-16
• 公開（公告）號: CN106826828B
• 公開（公告）日: 2019-06-14
• 發明（設計）人: 金龍;李帥
• 申請（專利權）人: 香港理工大學深圳研究院
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 深圳中一專利商標事務所 44237
• 代理人: 陽開亮

1.一種多冗余度機械臂系統的協同控制方法，其特征在于，所述多冗余度機械臂系統包括兩個以上的冗余度機械臂，所述兩個以上的冗余度機械臂在通信拓撲圖中連通；

所述控制方法包括：

確定所述兩個以上的冗余度機械臂中至少一個冗余度機械臂為目標冗余度機械臂；

獲取所述目標冗余度機械臂的相鄰冗余度機械臂的位置和速度信息，所述相鄰冗余度機械臂與所述目標冗余度機械臂在通信拓撲圖中相鄰連接；

獲取所述目標冗余度機械臂的參考點軌跡信息，所述參考點軌跡信息由預設的目標參考點和所述目標參考點的期望軌跡確定；

根據所述位置和速度信息，以及所述參考點軌跡信息按照預設的規則構建所述目標冗余度機械臂對應的雅可比矩陣等式；

在所述雅可比矩陣等式、關節角度極限、關節速度極限和關節加速度極限的約束下，根據二次型優化和標準二次規劃方法確定所述目標冗余度機械臂的控制信號；

根據所述控制信號控制所述目標冗余度機械臂，使得所述目標冗余度機械臂實現重復運動。

2.根據權利要求1所述的協同控制方法，其特征在于，所述根據所述位置和速度信息，以及所述參考點軌跡信息按照預設的規則構建所述目標冗余度機械臂對應的雅可比矩陣等式具體包括：

構建所述目標冗余度機械臂對應的權重矩陣，所述權重矩陣中的各個元素為所述目標冗余度機械臂與所述兩個以上的冗余度機械臂中的各個機械臂之間分別對應的第一連通權值；

根據所述參考點軌跡信息的獲取狀態確定所述目標冗余度機械臂的第二連通權值；

根據所述位置和速度信息、所述參考點軌跡信息、所述權重矩陣和所述第二連通權值構建所述目標冗余度機械臂對應的雅可比矩陣等式。

3.根據權利要求2所述的協同控制方法，其特征在于，所述第一連通權值通過以下步驟確定：

將所述目標冗余度機械臂與所述目標冗余度機械臂自身之間的第一連通權值設置為1；

將所述目標冗余度機械臂與相鄰冗余度機械臂之間的第一連通權值設置為1；

將所述目標冗余度機械臂與所述兩個以上的冗余度機械臂中的非自身且非相鄰冗余度機械臂之間的第一連通權值設置為0；

所述根據所述參考點軌跡信息的獲取狀態確定所述目標冗余度機械臂的第二連通權值具體包括：

判斷所述目標冗余度機械臂是否獲取到所述參考點軌跡信息；

若所述目標冗余度機械臂獲取到所述參考點軌跡信息，則將所述第二連通權值設置為1；

若所述目標冗余度機械臂未獲取到所述參考點軌跡信息，則將所述第二連通權值設置為0；

所述雅可比矩陣等式的通用表達式為：

其中，J_i表示目標冗余度機械臂i的雅可比矩陣，為J_i的時間導數；表示目標冗余度機械臂i的關節速度向量，為的時間導數；表示與目標冗余度機械臂i之間的第一連通權值為1的機械臂的編號；A_ij表示目標冗余度機械臂i的權重矩陣，所述權重矩陣中的各個元素為目標冗余度機械臂i與冗余度機械臂j之間的第一連通權值；ρ_i表示目標冗余度機械臂i的第二連通權值；δ_i＝r_i-r_irp為所述位置信息，r_i為目標冗余度機械臂i的末端執行器位置，r_irp為預設的目標冗余度機械臂i的末端執行器相對于所述目標參考點的距離向量；為所述速度信息，為目標冗余度機械臂i的末端執行器速度，r_d為所述目標參考點的期望位置，為所述目標參考點的期望速度；c₀＞0為控制算法收斂速度的參數，其越大則代表收斂速度越快。