冗余机械臂逆运动学的多子群粒子群求解

王 诚，候 力

（1.上海民航职业技术学院，上海200232；2.四川大学，四川 成都 610065）

1 引言

机械臂运动学逆解是机械臂关节空间轨迹规划、机械臂轨迹跟踪控制的基础，运动学逆解的求解精度直接关系到机械臂的控制精度[1]，因此研究机械臂运动学逆解问题具有重要意义。

传统的逆运动学求解方法包括封闭求解法和数值求解法，封闭求解法[2]计算速度较快，但是只适用于特定构型的机械臂，通用性较差；代数数值求解法包括梯度投影法、最小范数法等，此类方法主要通过迭代计算求解[3]，但是计算过程中存在迭代积累误差。随着人工智能的兴起，机械臂逆运动学的智能算法求解得到充分关注和广泛应用，核心思想是将逆运动学求解问题转化为目标优化问题，包括粒子群算法、遗传算法等。文献[4]比较了封闭求解法和数值求解法，得出了数值求解法通用性好、封闭求解法求解快速的结论；文献[5]使用3层RBF神经网络求解逆运动学方程，并提出多种群遗传算法优化神经网络结构和参数，提高了逆解计算准确率，但是此方法计算量较大，算法较为复杂；文献[6]使用粒子群算法，以目标点位姿误差最小为优化目标求得了逆解最优解，但是位姿权重固定，两者之间的相对重要性无法自适应变化。

针对冗余机械臂逆运动学求解问题，提出了多子群粒子群算法的求解方法。构造了由多个子群组成的粒子群算法，不同子群的粒子对应不同的进化方法，从而提高算法的搜索性能和稳定性能，最终提高了机械臂运动学逆解的精度。

2 逆运动学问题描述与建模

2.1 逆运动学问题描述

以Comau NJ-220型机械臂为研究对象，如图1所示。

图1 Comau NJ-220型机械臂模型Fig.1 Comau NJ-220 Manipulator Model

在此机械臂基座和各转动关节处各建立坐标系，基座坐标系记为Ox0y0z0，关节（1～6）坐标系分别记为Ox1y1z1～Ox6y6z6，图中只给出了各坐标系的X轴和Z轴，Y轴可根据右手定则进行确定。图1所示机械臂的参数，如表1所示。

表1 Comau NJ-220型机械臂参数Tab.1 Parameters of Comau NJ-220 Manipulator

表1中θi为关节i的关节角，表中第2列给出的数据为关节i的转动范围；αi为扭角，表示沿X i-1轴从Z i-1转动到Z i的角度；a i为连杆长度，d i为偏置量。

使用D-H法［7］，经过旋转和平移，得到从坐标系i-1到坐标系i的齐次变换矩阵为：

式中：R—旋转算子；D—平移算子。

由末端执行器坐标系到机械臂基坐标系的变换矩阵为：

式中：n、o、a—末端执行器在三个方向上的单位矢量；R e=[noa]—末端执行器相对于基坐标系的姿态矩阵；P e—末端执行器坐标系原点在基坐标系的位置矢量。

2.2 逆运动学求解目标函数

机械臂逆运动学问题求解思路为：根据给定的末端执行器位姿[R e，P e]，解算出或搜索出一组关节角序列，将此关节角序列代入到式（2）中，得到此关节角序列对应的末端执行器位姿[R r，P r]，求解目标为末端执行器的位置误差和姿态误差矩阵和最小。姿态误差矩阵中包含9个变量，且互相之间非完全独立，若直接根据姿态误差矩阵建立目标函数必然会增加优化目标变量。为了减少优化变量，将姿态误差表示为四元素法［8］，即，式中表示旋转变量，则姿态误差为E0=α。位置误差为，式中‖‖表示欧几里得距离。

根据以上分析，构造目标函数为：

式中：w p、w o—位置误差权值和姿态误差权值。将位置误差权值固定为1，姿态误差权值随机械臂结构自适应变化，即：

式中：λ—所有连杆长度和偏距的和；P d—目标位置与基坐标系原点间距离，k—系数，取（3.5～5.5）。

3 多子群粒子群算法求解

经过第2节的分析，建立了以θ1～θ6为优化对象，以式（3）为优化目标，以表1第二列关节角运动范围为优化空间的优化问题，为了对此问题进行求解，提出了多子群粒子群算法用于搜索逆运动学最优解。

3.1 粒子群算法

在粒子群算法中，粒子速度在自身惯性、自身历史最优和种群最优位置的牵引下进行更新[9]，即：

式中：v id(t)、x id(t)—粒子i迭代t次时在d维上的速度和位置；p id—粒子i迭代至当前时刻在d维上的最优位置；p gd—种群迭代至当前时刻在d维上的最优位置；c1、c2—自身学习因子和种群学习因子；rand()—（0，1）间的随机数。

3.2 多子群协同与多策略进化

不同搜索策略或不同进化策略的粒子具有不同的搜索特点，就可以应对不同特点的搜索寻优问题，由不同搜索策略粒子子群组成的种群相比于单一种群在面对不同寻优问题时具有更好的搜索性能，因此本文提出了多子群粒子群算法。

3.2.1 多策略进化粒子

根据惯性权重对粒子探索能力的影响，设置三种进化策略，分别为大范围搜索策略、精细搜索策略和速度自适应更新策略。

在粒子寻优过程中，惯性权重对粒子的探索能力和开发能力影响较大[10]，惯性权重较大时粒子的探索能力较强，能够大范围移动，而探索之前未到达区域，利于全局搜索；惯性权重较小时粒子的开发能力较强，能够在某一区域精细搜索，利于局部搜索和算法收敛。

对于同一搜索策略下的粒子，希望粒子的惯性权重随着算法的迭代而不断调整，从而动态调整算法的探索能力和开发能力，使用线性递减方法自适应调整惯性权重，即

式中：w—自适应权重；wmax、wmin—惯性权重最大和最小值；tmax—最大迭代次数。

通过设置不同的惯性权重最值，实现不同搜索策略，即大范围搜索策略的惯性权重变化为：

细致搜索策略的惯性权重变化为：

当本次搜索的粒子优于上一代搜索粒子时，按照基本粒子群算法中速度更新方法依然要改变搜索方向，这种不加区分的粒子搜索方法显然不合理，因此本文设置速度自适应更新策略，基本思想为：当本次搜索的粒子优于上代粒子时，依然沿此方向继续搜索；当本次搜索粒子次于上代粒子时，则在历史最优和种群最优牵引下运动。速度自适应更新策略为：

式中：f(x)—适应度函数。

3.2.2 多种群协同

式（5）给出了大范围搜索策略，此类粒子具有强探索能力；式（6）给出了细致搜索策略，此类粒子具有强开发能力；式（7）给出了速度自适应更新策略，此类粒子具有快速搜索能力。若单独使用细致搜索策略，算法能够在小范围内精细搜索，但是难以从一个区域运动至另一区域进行大范围搜索；同样的，单独使用大范围搜索策略，粒子大范围浏览区域，而没有进行细致搜索，难以搜索到最优解。因此本文提出了由以上3类粒子子群组成的粒子群算法，称为多子群粒子群算法。3个子群的粒子按照自身搜索策略各自进行搜索，每完成一次迭代通过共享子群最优位置实现信息交流，如此循环往复直至算法结束。

3.3 多子群粒子群算法流程

根据粒子群算法原理、多子群粒子群算法构造方法，制定多子群粒子群算法流程为：

（1）初始化算法参数，包括子群规模、粒子维度、搜索区域等；

（2）将3个子群的粒子进行随机初始化；

（3）各子群按各自搜索方式搜索最优位置，每完成一次迭代，共享子群最优粒子位置；

（4）比较子群最优粒子，挑选出种群最优粒子，使用种群最优粒子替换各子群最优粒子；

（5）是否达到最大迭代次数，若是则结束，输出种群最优粒子；若否则转至（3）。

4 运动学逆解仿真分析

同时使用粒子群算法和多子群粒子群算法对机械臂逆运动学问题进行求解，对比两种算法的收敛性能和稳定性能，其在收敛性能包括收敛速度和收敛精度两个方面。

4.1 收敛性能分析

仿真验证在Matlab环境下进行，机械臂构型及参数、搜索空间、搜索目标在前文中已经给出，在此明确粒子编码方式：需要优化的参数为θ1～θ6，因此粒子维度设置为6，采用十进制编码方式。

多子群粒子群算法参数设置为：子群规模设置为40，即种群规模为120，算法最大迭代次数tmax=50，学习因子c1=1.8、c2=2.2。给定精度要求为：位置误差不大于0.1mm，姿态误差不大于10-4rad。当搜索精度达到此要求时，对应的关节角序列才满足使用要求。

分别使用粒子群算法和多子群粒子群算法对机械臂逆运动学进行求解，每种算法各独立运行30次，选择两种算法各自的最优解，目标函数、位置误差及姿态误差随迭代次数的变化过程，如图2所示。

图2 优化结果迭代曲线Fig.2 Iteration Curve of Optimizing Result

由图2（a）可以看出，使用多子群粒子群算法搜索最优解时，目标函数值随迭代次数快速下降，收敛速度明显快于粒子群算法；迭代至250次时，多子群粒子群算法搜索的目标函数值在10-4以下，而粒子群算法搜索的目标函数值数量级为1，说明了两种算法的收敛精度具有极大差距。

由图2（b）和图2（c）可以看出，使用多子群粒子群算法搜索的最优粒子对应的位置误差在10-4mm以下，姿态误差在10-8rad以下，能够满足事先设定的精度要求。而使用粒子群算法搜索的最优粒子对应的位置误差为0.32mm，姿态误差为2.2×10-3rad，没有达到实现设定的精度要求。综上所述，多子群粒子群算法的搜索精度和速度远远优于粒子群算法，这是因为在多子群粒子群算法中，使用多个子群组成种群，而不同子群的粒子搜索策略不同，因此可以应对不同特点的寻优问题，达到了提高算法收敛精度和收敛速度的效果。多子群粒子群算法最终搜索到的最优粒子为（-0.092，0.306，-0.743，-2.17，-0.001，-0.002）rad。

4.2 稳定性能分析

前文是针对工作空间中某一随机选取的点进行逆运动学求解，为了分析多子群粒子群算法的稳定性和鲁棒性，从工作空间中随机选取25个点，分别使用多子群粒子群算法和粒子群算法进行求解，两种算法设置相同的迭代次数，25个点的求解结果，如图3所示。

图3 算法稳定性分析Fig.3 Stability Analysis of Algorithms

从图中可以看出，两类算法的稳定性均较好，但是粒子群算法的收敛精度不高，而多种群粒子群算法的收敛精度极高。另外，多子群粒子群算法的稳定性略好于粒子群算法，这是因为多子群粒子群算法的种群由多类型粒子组成，可以应对不同特点的寻优问题，所以具有更好的稳定性和更强的鲁棒性。

5 结论

研究了机械臂逆运动学求解问题，提出了多子群粒子群算法的求解方法，经仿真验证，得出了以下结论：（1）多子群粒子群算法求得的最优粒子对应的姿态误差和位置误差能够满足设定的精度要求，可以应用于实际；（2）多子群粒子群算法的收敛速度和收敛精度均优于粒子群算法；（3）多子群粒子群算法的稳定性好于粒子群算法，鲁棒性也强于粒子群算法。