具体实施方式

现结合实施例、附图对本发明作进一步描述：

本发明提出了一种基于粒子群优化算法的逆运动学计算方法，总体流程如图1所示。该算法在标准粒子群算法的基础上采用了K均值动态分群的思想，将粒子群分成几个普通子群，然后在普通子群中选择优秀个体组成精英群。后期子群之间的多样性会减弱，此时再将普通子群进行合并，减少计算资源的消耗。整个种群中评价值最差的粒子的收敛时间将决定最终整个粒子群算法的收敛时间，因此在普通群和精英群之间加入了交换操作，替换评价值较差的粒子，进而加快粒子群的收敛速度，降低了算法的时间消耗。

下面结合附图对本发明作进一步详细描述，具体实施方式如下：

步骤1:根据图2以及表1的D-H参数建立SSRMS型机械臂的正运动学模型。

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

表1实际SSRMS型机械臂的D-H参数

将SSRMS机械臂去掉一个偏置量后的机械臂构型定义为改进机械臂，如图3所示，可以看到改进机械臂的前3个关节轴线相交于一点，满足Pieper准则，因此改进机械臂的逆运动学存在解析解，建立改进机械臂的正运动学模型和逆运动学模型：

[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]，由于改进机械臂的逆运动学是存在解析解的，将待求解目标末端位姿带入等效机械臂的逆运动学函数中，可以得到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个子群，此处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之间的随机数，β为比例因子，此处可选β＝0.1；

步骤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，进行下一代的分群操作。

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

以上可以看出本发明所提出的算法是一种数值求解方法，该方法可以很好的用于逆运动学无法求得解析解的机械臂。空间机械臂大多都是具有偏置构型的，本发明是以SSRMS型机械臂为例。SSRMS型机械臂和其改进机械臂在同一组关节角下对应的末端位置不同，但姿态是相同的，因此将目标末端位姿用等效机械臂的逆运动学计算出的逆解结果是很接近实际SSRMS机械臂的逆解结果的，利用该特征，用其等效机械臂的逆运动学结果来作为算法的迭代初值，会大大减少算法收敛到最优值的时间。该算法在满足任务要求的前提下可以减少计算时间，且该算法也可以类比推广到其它构型的机械臂，而且关节正常情况下机械臂逆运动学的计算也可以参考使用该方法。