[发明专利]一种单关节锁定失效下的空间机械臂逆运动学方法

权利要求书

1.一种单关节锁定失效下的空间机械臂逆运动学方法，其特征在于步骤如下：

步骤1、建立SSRMS机械臂的正运动学模型：

[n_r,o_r,a_r,p_r]＝f_r(θ_r)        (2)

建立将SSRMS机械臂去掉一个偏置量后的改进机械臂的正运动学模型和逆运动学模型：

[n_m,o_m,a_m,p_m]＝f_m(θ_m)    (4)

式中θ_r表示实际SSRMS机械臂的7个关节角，[n_r,o_r,a_r,p_r]表示实际SSRMS机械臂的末端位姿，θ_m表示改进机械臂的7个关节角，[n_m,o_m,a_m,p_m]表示改进机械臂的末端位姿；

将待求解目标末端位姿[n_r,o_r,a_r,p_r]带入等效机械臂的逆运动学函数式5中，得到8组逆运动学结果；

步骤2、确定逆运动学的优化目标函数：

机械臂末端的期望位置为p_r，机械臂的位置误差函数定义为：

机械臂的各关节角为θ＝[θ₁ θ₂ θ₃ θ₄ θ₅ θ₆ θ₇]，初始关节角为将机械臂各个关节角的改变量定义为：

将逆运动学的求解转换成下面的最小约束优化问题来解决：

ΔP-ΔP_max≤0        (9)

式中ΔP_max为最大误差；

利用罚函数将上述约束优化问题转化成无约束优化问题进行处理，转化后得到下面的优化目标函数：

式中f(θ)即为优化目标函数，为罚函数，α为惩罚系数；

步骤3、设置相关参数：

设粒子群优化算法的种群规模为m，7自由度的机械臂锁定失效一个关节，需求解的粒子维数即机械臂剩余关节的个数为6；将种群中8个粒子的初值分别设为步骤1改进机械臂所对应的8组逆运动学结果，其余的m-8个粒子的初值按下面的式子进行混沌初始化

θ_ij＝l_θj+rand₁(h_θj-l_θj)     (12)

v_ij＝l_vj+rand₂(h_vj-l_vj)        (13)

式中θ_ij表示第i个粒子的第j维位置分量，v_ij表示第i个粒子的第j维速度分量，h_θj、l_θj分别表示粒子第j维位置分量的最大值和最小值，h_vj、l_vj分别表示粒子第j维速度分量的最大值和最小值，rand₁、rand₂分别为0～1之间的随机数；

步骤4：采用K均值聚类的方法将种群分成N＝8个子群，将改进机械臂8组逆运动学结果作为初始聚类中心；

步骤5：选取每个子群的前％Top个评价值最小的粒子，即按四舍五入取整数，即为Z_k个，构成一个新的精英群，原来的普通子群中仍然包括那些精英粒子；

步骤6：8个普通子群和精英群分别独立进行粒子群算法进行更新，每个子群的学习对象为该子群的最优粒子即局部最优值：

v_ij(t+1)＝ωv_ij(t)+c₁r₁(p_ij(t)-θ_ij(t))+c₂r₂(g_kj(t)-θ_ij(t))  (14)

θ_ij(t+1)＝θ_ij(t)+v_ij(t+1)    (15)

式中t为当前进化代数，t_max为算法最大迭代次数，g_kj(t)表示第k个子群的局部最优值(k＝1～N)，p_ij(t)为粒子的历史最优值，ω为惯性权重，ω∈[ω_min,ω_max]，c₁、c₂分别为自身学习因子和社会学习因子，r₁、r₂、r₃分别为0～1之间的随机数；

粒子位置和速度更新完成后判断当前代数t是否已经达到最大迭代次数t_max，若t＝t_max，则退出循环，得到机械臂的逆运动学结果；

否则，执行步骤7；

步骤7：对每个子群和精英群分别独立进行差分变异进化操作

θ_i′(t)＝g_k(t)+F(g_k(t)-p_i(t))     (17)

式中，θ_i′(t)表示粒子i对应的变异向量，F为0～1之间的缩放因子，按照式18对粒子历史最优值和子群局部最优值进行更新；如果差分变异后的粒子越界，则从改进机械臂8组逆解中任选一组，将其设置成该粒子的位置；

步骤8、判断精英群是否进入停滞状态：f_max、f_min分别表示精英群中粒子的最大评价值和最小评价值，det为给定的精度误差，若f_max＞det且f_max-f_min＜det，则认为子群进入停滞状态，此时按下式改变精英群中的每个粒子：

θ_ij＝g_kj-L_j+rand₃(2L_j)    (19)

式中rand₃为0～1之间的随机数，β为比例因子；

否则，执行步骤9；

步骤9：每个普通子群从精英群中选择Z_k个粒子替换掉自己子群中评价值最小的Z_k个粒子；

步骤10、判断是否达到子群合并的条件：计算8个普通子群局部最优值的方差D(g_kj)，若D(g_kj(t))≤ε、D(g_kj(t+1))≤ε、D(g_kj(t+2))≤ε，即连续三代的方差均小于给定误差ε，则将所有普通子群进行合并，迭代计数器t＝t+1，返回步骤5继续进行更新计算；

若不满足子群合并的条件，则迭代计数器t＝t+1，返回步骤4，进行下一代的分群操作；

一旦达到子群合并条件将子群合并后，后续的每一代便只剩下一个普通群和一个精英群不断进行更新进化，直到达到算法的最大迭代次数后，输出机械臂的逆运动学结果。

2.根据权利要求1所述单关节锁定失效下的空间机械臂逆运动学方法，其特征在于：所述β＝0.1。