[發明專利]一種兩關節繩驅連續型機械臂逆運動學求解方法

有效

• 申請號: 202110359452.9
• 申請日: 2021-04-02
• 公開（公告）號: CN113033065B
• 公開（公告）日: 2023-06-09
• 發明（設計）人: 余鑫;張浩;武海雷;楊靖琦;郭毓;郭健
• 申請（專利權）人: 南京理工大學;上海航天控制技術研究所
• 主分類號: G06F30/25
• 分類號: G06F30/25;G06F30/17;G06F119/14
• 代理公司: 南京理工大學專利中心 32203
• 代理人: 王安

1.一種兩關節繩驅連續型機械臂逆運動學求解方法，其特征在于，包括以下步驟：

步驟1、利用分段常曲率假設對兩關節繩驅連續型機械臂進行參數化建模；

步驟2、構建目標函數；

步驟3、根據粒子群算法求目標函數的最優解；

步驟4、確定連續型機械臂兩個關節之間的耦合量，并在驅動空間到關節空間的映射中進行解耦，得到驅動繩長變化量，完成兩關節繩驅連續型機械臂的逆運動學求解，具體為：

步驟4-1：確定第一關節運動對第二關節的耦合量：

其中，r₂是中心桿到第二關節驅動繩之間的距離，n₁是第一關節間隔盤數量；

L₁、L₂表示弧長，θ₁、θ₂表示待確定參數彎曲角，表示彎曲平面角；

步驟4-2：確定第二關節對第一關節的耦合量：

第一關節的彎曲角為彎曲平面角為則第二關節運動對第一關節造成的耦合量為：

其中，r₁為中心桿到第一關節驅動繩之間的距離；

步驟4-3：根據確定的耦合量，確定驅動繩長的變化：

其中，n₂是第二關節間隔盤數量。

2.根據權利要求1所述的兩關節繩驅連續型機械臂逆運動學求解方法，其特征在于，所述步驟2中的構建目標函數具體包括以下步驟：

步驟2-1：機械臂末端的目標位姿為其中，R_d＝[r_d1?r_d2?r_d3]^T為方向矩陣，P_d＝[p_d1?p_d2?p_d3]^T為位置向量；

根據分段常曲率假設，當前的機械臂末端位姿其中，R_c(q)＝[r_c1(q)?r_c2(q)?r_c3(q)]，P_c(q)＝[p_c1(q)?p_c2(q)?p_c3(q)]^T，q為關節角參數，

步驟2.2、位置誤差目標P(q)＝||P_d-P_c(q)||，姿態誤差目標目標函數為E(q)＝P(q)+O(q)。

3.根據權利要求2所述的兩關節繩驅連續型機械臂逆運動學求解方法，其特征在于，所述T_c(q)＝T₁T₂：

4.根據權利要求1所述的兩關節繩驅連續型機械臂逆運動學求解方法，其特征在于，所述步驟3中根據粒子群算法求目標函數的最優解具體為：

設置每個粒子位置為初始化例子種群X＝(x₁,x₂,...,x_n)以及每個粒子的飛行速度V＝(v₁,v₂,...,v_n)，粒子群進化公式為：

其中，k為迭代次數；ω(k)為慣性權重，c₁、c₂為學習因子；z₁、z₂為在[0，1]之間的隨機數；是迭代了k次后，粒子i的最優位置；為當前循環，粒子群的最優位置；

首先初始化種群，設置參數，然后進入算法的循環，每一次循環中，調用目標函數，更新粒子的位置和速度，更新個體粒子的最優位置以及全局最優位置，直到目標函數E(q)的值小于設定的閾值，退出循環，得到最優解