[发明专利]一种基于解析数值混合法的冗余机械臂逆运动学方法

权利要求书

1.一种基于解析数值混合法的冗余机械臂逆运动学方法，其特征在于，包括以下内容：

步骤1、按照臂型角参数化的方法建立机械臂的逆运动学模型；

步骤2、确定逆运动学问题的代价函数和约束条件；

步骤3、基于粒子群优化算法、寻找满足所述步骤2中约束条件的臂型角

步骤4、更新粒子的速度和位置；

步骤5、根据边界约束条件、按照所述步骤2中的代价函数、对速度和位置超限的粒子进行初始化；

步骤6、计算更新后所有粒子的代价值、历史最优值以及种群的全局最优值；

步骤7、更新迭代次数，t＝t+1，t表示粒子的迭代次数，返回步骤4，不断更新粒子的位置与速度；当t＝t_max达到最大迭代次数后，终止循环，代价值最小的粒子及其所对应的那组逆解即为最终期望末端位姿所对应的机械臂逆运动学结果。

2.如权利要求1所述的一种基于解析数值混合法的冗余机械臂逆运动学方法，其特征在于，所述步骤1中逆运动学模型为：

式中，⁰T₇表示机械臂末端相对于基座的位姿矩阵，表示臂型角，Θ＝[θ₁ θ₂ θ₃ θ₄ θ₅θ₆ θ₇]表示机械臂的7个关节角，每个臂型角可以得到机械臂的8组逆解。

3.如权利要求2所述的一种基于解析数值混合法的冗余机械臂逆运动学方法，其特征在于，所述步骤2中的代价函数的形式如下：

其中，F_D、F_S、分别表示避障、避奇异、避免关节极限和运动连续的代价函数，分别表示机械臂第i个关节角允许的最大值和最小值，θ_i^re表示上一时刻机械臂第i个关节角的值，d表示机械臂臂型面和障碍物之间的距离，d_min表示机械臂臂型面和障碍物之间允许的最小距离，λ_D、λ_S、λ_L、λ_C分别表示相应代价函数的缩放因子，ε为避奇异的阈值范围。

4.如权利要求3所述的一种基于解析数值混合法的冗余机械臂逆运动学方法，其特征在于，所述步骤3的具体内容为：

设置种群规模N，最大迭代次数t_max，计算每组逆运动学结果的代价值，其中的最小值即为粒子的代价值，按照下式对所有粒子进行初始化：

式中v_i、分别表示第i个粒子的速度分量和位置分量，v_max、v_min分别表示粒子速度分量的最大值和最小值，分别表示粒子位置分量的最大值和最小值，r_v、r_x分别为0～1之间的随机数。

5.如权利要求4所述的一种基于解析数值混合法的冗余机械臂逆运动学方法，其特征在于，所述步骤4的具体内容为：

计算每个粒子的历史最优值b_i和种群的全局最优值g_t，按照下式更新粒子的速度和位置；

式中，t表示当前的迭代次数，v_i(t)、v_i(t+1)分别表示第i个粒子当代和下一代的速度分量，分别表示第i个粒子当代和下一代的位置分量，φ₁、φ₂分别表示认知学习率和社会学习率，K表示压缩系数，K_max、K_min表示压缩系数的最大值和最小值。