[發明專利]一種超冗余機械臂的路徑規劃方法、裝置及控制系統

有效

• 申請號: 201910772183.1
• 申請日: 2019-08-21
• 公開（公告）號: CN110561426B
• 公開（公告）日: 2021-01-12
• 發明（設計）人: 徐文福;胡忠華;劉天亮;梁斌;郭笑霖
• 申請（專利權）人: 哈爾濱工業大學（深圳）
• 主分類號: B25J9/16
• 分類號: B25J9/16;B25J13/00
• 代理公司: 廣州嘉權專利商標事務所有限公司 44205
• 代理人: 洪銘福

1.一種超冗余機械臂的路徑規劃方法，所述超冗余機械臂包括多個臂段，多個所述臂段均包括多個小節，多個所述小節間依次通過關節連接，其特征在于，包括步驟：

S10、獲取所述臂段的臂形描述參數并規劃所述超冗余機械臂的期望路徑，在所述期望路徑上取多個期望節點，且分別設有一個所述期望節點處于所述期望路徑的首端和末端，并以第一個所述期望節點為目標期望節點；

S20、根據所述臂形描述參數獲得所述機械臂的當前等效臂形參數；

S30、獲取所述目標期望節點的目標期望位置信息和目標期望指向信息，并根據所述當前等效臂形參數遞推獲得所述超冗余機械臂的等效臂形的末端當前位置信息、末端當前指向信息和雅可比矩陣；

S40、根據所述末端當前位置信息、所述末端當前指向信息、所述雅可比矩陣、所述目標期望位置信息和所述目標期望指向信息獲得末端期望線速度和末端期望角速度；

S50、基于所述雅可比矩陣不斷迭代所述末端期望線速度和所述末端期望角速度獲得各個所述臂段的期望臂形參數；

S60、根據所述期望臂形參數獲得關節驅動參數，并根據所述關節驅動參數將所述超冗余機械臂向所述目標期望節點驅動；

S70、以下一個所述期望節點為所述目標期望節點，重復所述步驟S20至所述步驟S60直至遍歷完所有所述期望節點；

所述步驟S50之后所述步驟S60之前還包括步驟：

S510、根據所述期望臂形參數獲得末端期望位置信息和末端期望指向信息；

S511、設定誤差范圍要求，當所述末端期望位置信息與所述目標期望位置信息的誤差滿足所述誤差范圍要求，且當所述末端期望指向信息與所述目標期望指向信息的誤差滿足所述誤差范圍要求時，執行所述步驟S60，否則返回執行所述步驟S20。

2.根據權利要求1所述的超冗余機械臂的路徑規劃方法，其特征在于，所述步驟S40具體包括子步驟：

S401、根據所述目標期望位置信息、所述目標期望指向信息、所述末端當前位置信息和所述末端當前指向信息獲得位置信息差和指向信息差；

S402、根據所述位置信息差、所述指向信息差及所述雅可比矩陣計算得出所述末端期望線速度和所述末端期望角速度。

3.根據權利要求1或2任一項所述的超冗余機械臂的路徑規劃方法，其特征在于，分別對多個所述臂段內的多個所述關節按順序進行標號，且將所述標號為奇數的所述關節設置為Pitch-Yaw型，所述標號為偶數的所述關節設置為Yaw-Pitch型。

4.一種超冗余機械臂的路徑規劃裝置，所述超冗余機械臂包括多個臂段，多個所述臂段均包括多個小節，多個所述小節兩兩之間通過關節連接，多個所述臂段內的所述關節按順序進行標號，且將所述標號為奇數的所述關節設置為Yaw-Pitch型，所述標號為偶數的所述關節設置為Pitch-Yaw型，其特征在于，包括：

臂形描述參數獲取及期望路徑規劃模塊，用于獲取所述臂段的臂形描述參數并規劃所述超冗余機械臂的期望路徑，在所述期望路徑上取多個期望節點，且分別設有一個所述期望節點處于所述期望路徑的首端和末端，并以第一個所述期望節點為目標期望節點；

當前等效臂形參數獲取模塊，用于根據所述臂形描述參數獲得所述機械臂的當前等效臂形參數；

參數信息計算獲取模塊，用于獲取所述目標期望節點的目標期望位置信息和目標期望指向信息，并根據所述前等效臂形參數推獲得所述超冗余機械臂的等效臂形的末端當前位置信息、末端當前指向信息和雅可比矩陣；

末端期望速度獲取模塊，用于根據所述末端當前位置信息、所述末端當前指向信息、所述雅可比矩陣、所述目標期望位置信息和所述目標期望指向信息獲得末端期望線速度和末端期望角速度；

期望臂形參數獲取模塊，用于基于所述雅可比矩陣不斷迭代所述末端期望線速度速度和所述末端期望角速度獲得各個所述臂段的期望臂形參數；

驅動控制模塊，根據所述期望臂形參數獲得關節驅動參數，并根據所述關節驅動參數將所述超冗余機械臂向所述目標期望節點驅動；

期望節點遍歷模塊，用于以下一個所述期望節點為所述目標期望節點，重復所述當前等效臂形參數獲取模塊、所述參數信息計算獲取模塊、末端期望速度獲取模塊、所述期望臂形參數獲取模塊和所述驅動控制模塊的工作直至遍歷完所有所述期望節點。