[发明专利]一种两关节绳驱连续型机械臂逆运动学求解方法

权利要求书

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)的值小于设定的阈值，退出循环，得到最优解