改进粒子群算法在机器人位置逆解上的应用

王淑青， 王亚洲， 许 琛， 潘 健

(1 湖北工业大学电气与电子工程学院，湖北 武汉 430068；2 内蒙古电力(集团)有限责任公司乌兰察布电业局，内蒙古 乌兰察布 012000)

改进粒子群算法在机器人位置逆解上的应用

王淑青1， 王亚洲1， 许 琛2， 潘 健1

(1 湖北工业大学电气与电子工程学院，湖北 武汉 430068；2 内蒙古电力(集团)有限责任公司乌兰察布电业局，内蒙古 乌兰察布 012000)

为提高工业关节机器人位置逆解的快速性、准确性且避免多解问题, 提出了一种改进的粒子群算法。此算法直接从正运动学出发搜索数值解的逆解，并在边界的产生、惯性权重的处理和超界的处理三个方面进行了很好的改进。单点仿真测试和连续轨迹仿真测试表明该改进算法可以有效避免多解问题，同时具有良好的稳定性、一定的实时性和较高的求解精度。此算法可用于实际的离线和在线求解，具有通用性和一定实用价值。

机器人； 逆运动学解； 粒子群算法； MATLAB仿真

工业机器人逆运动学求解的方法根据解的性质可分为两大类：解析法和数值法。反变换法[1]、几何法[2]都属于解析法。而基于搜索机制的各类智能优化算法[3-6]、模拟输入输出关系的神经网络[7-8]、以及其它的迭代算法[9]则都属于数值法。解析法在求解时存在多解，且机械臂结构不符合Pieper准则时，没有解的表达式。其中的反变换法，表达式没有固定的推导方法，而几何法推导容易，但对于复杂结构的机械臂，无法单独求解；数值法在求解时，没有结构限制，但有多解、以及求解速度与精确性互斥等问题。

在数值法中，林明等[3]为避免多解使用了惩罚函数项的方法，但因为惩罚函数项取初始位置才最小，而目标函数其余项取准确解才最小。两方博弈之下，最终的解只能收敛于准确解和初始位置之间，使得求解精度不高；吴振宇等人[4]、金媛媛[5]和芮挺等人[6]分别将遗传与粒子群、模拟退火与遗传相结合。虽然使求解具有了较高的求解精度，但增加了算法复杂性，以致运算量较大，求解速度不足，也没有解决多解问题；DayaB.等人[7]和Jasim WM.[8]试图以神经网络来逼近逆运动学的非线性关系，但由于其结构对最终结果的不确定性及需要大量已知样本进行训练，在应用中受到了很大的限制；王宪等人[9]使用牛顿拉夫逊迭代法算法，虽然具备了一定的求解速度，但92ms/次的求解速度还是无法满足实时。本文采用单独的粒子群算法，其结构简单不需调整大量参数，且因为它是天然的并行算法，使得实现在线实时计算具有天然的优势，但简单的粒子群用于大量非线性、多峰值复杂问题的优化时，容易陷入局部最优。为了完成本文的求解任务，必须对简单的粒子群进行改进：“动态狭小边界”来约束多解，“非线性时变与陷局部最优自校正结合”来增加准确性和快速性，“超界带弱方向性返回扩散与超界不限制结合”来增强快速性。

1 机器人模型

本文以此Puma560机器人为对象。其DH参数见表1。

表1 PUMA560的DH参数

其中a2=431.8，a3=20.32，d2=149.09，d4= 433.07

根据D-H方法，两个相邻坐标系的4×4齐次变换矩阵为

i-1Ti=

(1)

把DH参数，带入式(1)依次写出各相邻连杆的变换矩阵：

其中ci=cosθi，si=sinθi(i=1、2…6)。

最后，可以由式(2)得到机器人末端器的位姿矩阵：

0T6=0T1·1T2·2T3·3T4·4T5·5T6

(2)

2 改进粒子群算法求解

2.1PSO应用于位置逆解的问题描述

由于puma560的末端器的位置只与前三关节轴有关，故完全可以把位置求解分离出来，即仅研究前三关节θ1θ2θ3与p之间的关系。由正运动学式(2)可得，由前三关节变量映射到末端器的空间位置的非线性方程组为

(3)

式中，s1=sinθ1，s23=sin(θ2+θ3)，其余缩写按同样规则。位置逆解求解也就是在已知p后，根据方程(3)求解出θ1θ2θ3。

2.2 传统粒子群的模型描述

粒子群优化算法(PSO)最基本的算法模型是带惯性权重的PSO模型。其模型描述为

(4)

i=x,y,z

(5)

2.3PSO算法的改进

针对智能优化算法解逆解时的最突出的三个方面的问题(快速性、准确性、唯一性)，本文对传统的粒子群优化算法做出了三点改进。

1)为了限制多解，保证唯一性，本文采用“动态狭小边界”法，其表达式为

(6)

其中，xmax和xmin是当前的边界向量，xlast是上一次的逆解向量。即每次求解空间都是以上次的解为中心上下扩张2°为边界的狭小空间，且随着求解的进行该空间在动态移动。由于在狭小的求解空间中不会存在大跳变的多解，所以在连续轨迹中求得的解的曲线不会因多解问题出现大跳变，而是较光滑的曲线。同时由于需要搜索的范围变得很小，快速性也可以得到一定增强。

本方法可在真实机器人中应用的现实依据：在做机器人轨迹规划时，连续的CP控制都是用高速插值的方式来完成的。密集的笛卡尔空间插值，换算到关节空间对应的关节变量的变化值是很小的(关节变量的变化的最大值可通过各机器人铭牌标示的位置误差和最大臂展值计算得到)，不会超过±1°，否则在空间轨迹上就会呈现出很大的误差。所以±2°的范围完全满足求解需要。

2)为提高求解准确性，本文提出“非线性时变权重与陷局部最优自校正结合”的方法，表达式为

(7)

其中，i为迭代的次数，T为最大截止代数。条件①连续10代的适应度值Q的变化量没有超过10-5，条件②适应度值Q还没小于10-6(条件①确定粒子群已趋于收敛，条件②确定此时收敛的值还绝对不是全局最优解)。

式0.5×exp(-i×((T-i+100)/T)/T)使ω随迭代进行从0.5衰减到0.22的处理，与其他的普通变惯性权重改进算法的目的一样，都是为了前期有较好的探索能力，后期有较好的开发能力。另外，本文在普通的变惯性权重改进算法的基础上增加了中期再次提升ω至0.6的处理。这样做可以让粒子群已经趋于收敛局部最优时，再次让其小幅发散[10]，以此跳出局部最优，增加求解准确性。

3)为提高求解快速性，本文提出“超界带弱方向性返回扩散”的方法，表达式为

(8)

式(8)中，rand为区间在[0,1]的随机数，x为粒子的位置向量，xmax和xmin是当前的边界向量。在惯性权重较大时，粒子惯性较大，超界不加限制会出界较远，回到界内可能要很久，这样很多粒子的搜寻时间都浪费在无效解上。且界外也存在全局最优解，粒子还很可能收敛在界限外。所以每当有粒子超界，则重新随机扩散到先前偏向的1/2区域，该处理方式既使得粒子重新随机扩散到解域，又保留了一定的方向性(现在分散为先前偏向的区域)。从理论上分析这样既增强了粒子群的探索能力[10]，又尽可能不浪费先前寻找成果来增加收敛速度；而惯性权重较小时，超界的粒子不多且不会过界太远，可以在短时间内自行回来，所以惯性权重较小时不设界。

总结三个改进的针对点，见表2。

表2 改进粒子群总结

3 MATLAB仿真实验结果分析

3.1 避免多解测试

“动态狭小边界”可以有效避免多解，保证唯一性。在笛卡尔空间中以任意一点为目标，分别用本文改进粒子群算法和仅去其改进点1的算法进行多次逆解求解。图1就是取笛卡尔空间点[0.7601;0.1491;0]为目标点得到的结果，图1a为没有使用“动态狭小边界”的解的情况，可以看到解出了4组解，图1b是使用了“动态狭小边界”的解的情况，始终只有一组解。“动态狭小边界”确实可以有效避免多解。

图 1 位置逆解的分布图

3.2 单点测试

普通改进的例子群算法：c1=2,c2=2,T=120,N=60，机械可行范围做边界，ω=0.5×exp(-i×((T-i+100)/T)/T)为非线性时变，ω>0.41位置超界按本文处理，否则不限。

本文的改进的粒子群算法：c1=2,c2=2,T=120,N=60，边界采用改进点1，ω采用改进点2，超界采用改进点3。

图 2 单点测试20次的适应度值收敛趋势

现将两种算法分别在普通pc机上运行20次，记录每次适应度值随着迭代次数的变化趋势见图2。从图2a和b可以看出本文算法稳定性、准确性、收敛速度确实明显优于普通改进粒子群算法：普通改进粒子群算法收敛速度相对较慢，差些的需要60代才收敛到10-2以下，且容易陷入局部最优解，目标函数在0.1附近就是局部最优解；而本文的改进算法收敛迅速，40代以内就收敛到10-3以下，每次收敛轨迹都很相似(稳定性好)，且成功避免了普通改进算法的局部最优解。

3.3 连续轨迹测试

先做如下轨迹规划：在可达空间任取起始位置pstart和终止位置pend，并已知起始位置对应的关节角θstart。三个初始条件具体数据如下：

pstart=[0.1446,0.4527, -0.406]

θstart=[0.9425,-0.063,0.063 ]

pend=[0.361, -0.507, -0.224]

起点和终点之间的插值由robotics工具箱的函数产生，用ctraj()函数生成的1000个插值点，在笛卡尔空间中的形成轨迹见图3。

图 3 规划出的空间轨迹

图4a与b分别是Matlab工具箱自带逆解函数与本文的改进粒子群算法逆解出的各插值点对应的前三关节的关节角度。横坐标是时间(4s内有1000个插值点)，纵坐标是关节角度值。对比两种方法的结果可以佐证本算法运算结果的正确性。改进点①可以让解出来的逆解始终都与上一组解是最近的，可以保证唯一性，从而使得连续轨迹求解的值是连续光滑的，从图4b中可见连续轨迹逆解的曲线是光滑的。

图 4 Robotics toolbox逆解函数与本文算法的求解结果

改进点1压缩了搜索空间，使得搜索困难度降低。改进点2可以有效避免陷入局部最优解，增加准确性。图5为连续轨迹求解1000个插值点的目标函数值，即每个逆解对应的空间位置与实际期望的目标空间位置的欧式距离，也就是误差。从图5中可知误差已达到10-17数量级，而单点测试普通改进粒子群仅达到了10-2，可见本文的改进算法确实具有较高的准确性。

图 5 连续轨迹插值点对应的目标函数值

记录该连续轨迹逆解的整个计算的耗时，除以求解次数1000得到解算平均速度为70.02ms/次，这样的速度直接用于在线计算显然不够。但粒子群算法本身就是一种天然的并行算法，在每一代各个粒子的相应计算独立的。本文仿真受到Matlab的环境框架制约，每一代的每个粒子的计算必须挨个进行，不能同时计算。所以如果把程序改编到FPGA或其他可以并行处理的芯片中，每一代所有的粒子的计算可以一次完成[11]。假设FPGA和普通PC处理速度一样，那么计算速度变快为现在的60倍。即1.167ms一次，足以用于实时。

4 结论

为解决工业关节机器人位置逆运动学问题, 本文提出了一种用粒子群算法直接从正运动学出发搜索数值解的逆解算法。针对快速性、准确性、唯一性进行了三点改进，并经过仿真验证：本文的改进粒子群算法能够有效避免多解，保证解的唯一性；较高的求解精度，位置逆解误差仅在10-17数量级；较快的求解速度，MATLAB仿真70.02ms一次，实际应用时，理论上可以达到1.167ms。

[1] 兰文宝. 基于PMAC的工业机器人控制系统研究与实现[D]. 哈尔滨：哈尔滨工程大学, 2013.

[2] 李宪华，郭永存，张军，等. 模块化六自由度机械臂逆运动学解算与验证[J]. 农业机械学报，2013，04:246-251.

[3] 林明，王冠，林永才. 改进的遗传算法在机器人逆解中的应用[J]. 江苏科技大学学报(自然科学版)，2012，04:370-375.

[4] 吴振宇，姚明江，冯林，等. 基于遗传粒子群的PUMA机器人逆运动求解[J]. 计算机仿真，2011，10:173-176.

[5] 金媛媛. 机器人逆运动学的模拟退火自适应遗传算法研究[D]. 重庆：重庆大学，2007.

[6] 芮挺，朱经纬，蒋新胜，等.PUMA机器人逆运动模拟退火粒子群求解方法[J]. 计算机工程与应用，2010，03:27-29.

[7]DayaB,KhawandiS,AkoumM.ApplyingNeuralNetworkArchitectureforInverseKinematicsProbleminRobotics[J].JournalofSoftwareEngineering&Applications, 2010, 3(3):230-239.

[8]DayaB,KhawandiS,AkoumM.ApplyingNeuralNetworkArchitectureforInverseKinematicsProbleminRobotics[J].JournalofSoftwareEngineering&Applications, 2010, 3(3):230-239.

[9] 王宪，杨国梁，张方生，等. 基于牛顿-拉夫逊迭代法的6自由度机器人逆解算法[J]. 传感器与微系统，2010，29(10):116-118.

[10] 潘峰，李位星，高琪著. 粒子群优化算法与多目标优化[M]. 北京：北京理工大学出版社.2013.

[11] 罗犇，邵之江，徐祖华，等. 一种新的基于PSO的MPC及其硬件实现[J]. 控制工程，2013，20(02):227-230.

[责任编校： 张岩芳]

Robot Position Inverse kinematics based on improved PSO

WANG Shuqing1， WANG Yazhou1， XU Chen2， PAN Jian1

(1SchoolofElectricalandElectronicEngin.,HubeiUniv.ofTech.,Wuhan430068,China; 2UlaachabElectricPowerBureau,InnerMongoliaPower(Grope)CO.,LTD,Ulaanchab012000,China)

To avoid the problem of multiple solutions and enhance the performance of rapidity and accuracy in computing industrial robot position inverse kinematics, this paper presents an improved PSO. This algorithm searches for the numerical inverse solution from the forward kinematics directly and had a very good improvement on boundary generating, inertia weight handling and bound processing. Single-point and continuous path simulation tests show that the improved algorithm can effectively avoid the problem of multiple solutions, while having good stability, a certain degree of real-time and high precision solver. This algorithm can be used in offline and online practical solving and have versatile and practical value.

robot, inverse kinematics solution, PSO, MATLAB simulation

2016-03-07

王淑青(1969-)，女，河北衡水人，理学博士，湖北工业大学教授，研究方向为计算机检测与控制,电厂智能控制,系统分析与集成

1003-4684(2017)01-0046-05

TP242.2

A