基于改进QPSO-NN的冗余机械臂逆运动学算法*

张云峰，马振书，孙华刚，陆继山

（1.军械工程学院，石家庄050003；2.军械技术研究所，石家庄050003）



基于改进QPSO-NN的冗余机械臂逆运动学算法*

张云峰1，马振书2，孙华刚2，陆继山1

（1.军械工程学院，石家庄050003；2.军械技术研究所，石家庄050003）

摘要：针对某种冗余机械臂逆运动学求解的问题，提出了一种基于改进量子粒子群神经网络的求解算法。以冗余机械臂末端位姿为输入，经神经网络求得其逆解；针对神经网络输出结果误差较大的问题，把神经网络求初值加入初始化的粒子群中，通过基于Metropolis准则改进量子粒子群算法，避免了量子粒子群算法的早熟现象；以关节坐标经正向运动学求得的末端位姿和期望位姿的误差为适应度函数，对机械臂关节坐标迭代寻优。仿真结果表明该方法结合了神经网络算法的快速性和改进量子粒子群算法的精确性，满足求冗余机械臂逆运动学问题的速度和精度要求。

关键词：冗余机械臂，逆运动学，神经网络，量子粒子群，Metropolis算法

0　引言

机械臂运动学建立了机械臂关节坐标和笛卡尔坐标间的映射关系，已知机械臂各个关节变量值来求机械臂末端的位姿称为机械臂前向运动学，而已知机械臂末端位姿求机械臂各个关节变量的值称为机械臂逆运动学。运动学问题对于机器人控制十分重要，它与运动分析、离线编程、轨迹规划密切相关，是机械臂设计的基础［1］。机械臂前向运动学具有唯一解，机械臂逆向运动学问题则复杂得多。解决机械臂逆运动学的传统方法有代数法［2］、迭代法［3］和几何法［4］，通常使用代数法来得到简单结构的解析解，复杂结构往往难以求到解析解，需要进行迭代，迭代法运算量很大、耗时较长、解的精度无法保证，几何法的实质是对代数法、迭代法求解过程利用几何语言进行描述。

对于机械臂的实时控制来说，逆运动学的快速算法十分重要［5］，越来越多的研究关注用神经网络解决机械臂逆运动学问题［6］。Bekir Karlik［7］等证明了双隐层BP神经网络结构在解决该问题时具有优势。Zhang［1］等应用RBF网络解决机械臂逆运动学问题，并与Bekir Karlik提出的网络结构进行比较，得到了理想的效果。Rasit Koker［6］使用3个平行的Elman网络解决机械臂逆运动学问题，对Elman网络在该问题上的应用进行了探索。应用神经网络法解机械臂逆运动学问题精度不高，需要对网络输出结果迭代寻优。Rasit Koker［8-9］分别使用遗传算法，模拟退火算法优化3个并联Elman网络的输出结果的小数点后十位小数部分，减小了网络输出误差，得到了较好效果。

作为一种群体智能优化算法，粒子群算法（PSO）已经在很多领域得到成功应用。量子粒子群（QPSO）通过赋予种群粒子量子行为提高了粒子群全局收敛能力，但仍存在早熟问题。逄珊［10］等把分群机制引入量子粒子群，较好地解决了其局部收敛的问题，并应用在了航空发动机性能仿真上。龙海侠［11］等基于锦标赛法和轮盘赌法改进吸引子的生成方式，较好地解决了量子粒子群早熟收敛的问题。

本文以某型冗余机械臂为平台，首先针对其求运动学逆解的问题，使用不同的神经网络结构进行对比，找到最佳神经网络结构。再依据Metropolis准则改进量子粒子群吸引子的选择方式。最后应用改进量子粒子群算法以神经网络输出作为初值迭代寻优，找到该机械臂逆运动学的近似最优解。

1机械臂逆运动学问题描述

机械臂是由一系列由关节连接的连杆构成的，当所有的关节变量已知时，可用正运动学来确定机器人末端手的位姿。如果要使机械臂末端放在特定的点上并且具有特定的姿态，可用逆运动学计算出每一关节变量的值。本文使用一个六自由度机械臂结构作为研究对象，它由5个旋转关节和1个滑动关节（5R1T）组成，即腰回转关节、肩关节、肘关节、小臂伸缩关节、腕部仰俯关节和腕部回转关节。θ1、θ2、θ3、d4、θ5、θ6分别表示关节1至关节6的关节变量，其中，θ为旋转关节变量，d为滑动关节变量。用Denavit-Hartenberg法为机械臂每个关节指定一个参考坐标系，并用齐次变换矩阵来表示坐标系间的相对位置和姿态［12］。机械臂的连杆坐标系如图1所示，该姿态下D-H参数如表1所示，a表示两个关节参考坐标系z轴公垂线的长度即连杆长度，α表示关节参考系间的旋转角度，d表示z轴上两条相邻公垂线之间的距离（或称关节偏移）。定义连杆坐标系和D-H参数后，就可以用线性变换矩阵表示连杆坐标系之间的关系，坐标系{i}相对于坐标系{i-1}的坐标变换可以看作坐标系{i}先绕xi-1轴转αi-1角，再沿xi-1轴移动ai-1，再绕zi轴转角θi，最后沿zi轴移动di。所以，两连杆间相对变换的通式为：

其中，i-1iT表示连杆坐标系{i}相对于{i-1}的变换关系，cθi表示cos θi，sθi表示sin θi。06T=01T12T23T34T45T56T表示机械臂末端相对于基座标系的位姿变换矩阵。用一个4×4的矩阵（n o a p）来表示机械臂末端位姿，其中，n表示机械臂末端参考坐标系的x、y、z轴相对基坐标系x轴的姿态，o表示机械臂末端参考坐标系的x、y、z轴相对基坐标系y轴的姿态，a表示机械臂末端参考坐标系的x、y、z轴相对基坐标系z轴的姿态，p表示机械臂末端参考系相对基坐标系x、y、z轴的位移，因此，（n o a）表示机械臂末端姿态，（p）表示机械臂末端位置。FK（θ1θ2θ3d4θ5

θ6）=（n o a p）即为前向运动学问题，IK（n o a p）=（θ1θ2θ3d4θ5θ6）即为逆向运动学问题，由于2、3两个转动关节平行并且与滑动关节4相邻，因此，该机械臂决定末端位置的关节数是冗余的，其逆运动学问题没有解析解。

图1　冗余机械臂模型

表1　D-H参数

2神经网络结构

针对没有解析解的冗余机械臂求运动学逆解的问题，提出了3种神经网络结构求解，它们分别为Bekir Karlik的双隐层BP神经网络（图2）、Zhang 的RBF网络结构（图3）和双隐层Elman神经网络（图4）。它们都是12输入1输出结构，隐层有12个神经元，Bekir Karlik已经证明了该种结构的双隐层BP神经网络优于12输入6输出结构的双隐层BP神经网络［6］。其内部原因应为前一结构所要学习非线性映射关系的复杂程度小于后一结构，因此，该结论也适用于其他类型神经网络。

样本质量对神经网络性能产生很大影响，本文采用让神经网络学习到所有工作空间位姿的思想准备训练样本，如果采取均分每个关节变量范围再计算末端位姿的方法，比如把每个关节变量范围分为20份，则样本矩阵就有206=64 000 000行，这会占用很大的计算内存并且无法代表全部输入模式。采用蒙特卡罗法来准备样本［13］，在每个关节活动范围随机的选取若干个值，利用机械臂正向运动学计算末端位姿，取其中5 000组作为训练样本，1 000组作为验证样本，机械臂末端位姿（n o a p）作为神经网络的输入，1个关节变量作为网络的输出，共需6个并联神经网络。

分别对3种结构神经网络训练，结果如表2所示，对于训练样本和测试样本，BP神经网络的预测结果均优于RBF神经网络和Elman神经网络。BP神经网络拥有很强的泛化能力，理论上能以任何精度逼近复杂的非线性映射关系；而RBF网络基于聚类原理，RBF层神经元过多会造成分类过多，使对测试样本的输出精度降低，RBF层神经元过少对于该问题则精度明显不足；递归原理的Elman网络不适合这类有充足样本的问题，无论从理论上还是仿真结果，双隐层BP网络结构更适用于对该冗余机械臂逆运动学求解。

图2　BP网络结构

图4　Elman网络结构

表2　网络预测结果

3基于Metropolis准则的改进量子粒子群算法

神经网络的应用初步解决了冗余机械臂运动学求逆的问题，但其结果不够精确，在神经网络求得初值的基础上，使用改进量子粒子群算法迭代寻优，可以减小迭代次数并找到近似最优解。

3.1量子粒子群算法简介

量子粒子群算法（Quantum-behaved Particle Swarm Optimization，QPSO）是Sun［14］等人基于量子力学原理，提出的新的粒子群优化算法，认为粒子在搜索空间移动时，存在一个以Pbest为中心的δ势阱，因此，量子粒子群具有收敛性。由于量子的运动轨迹具有不确定性，可在全局范围内搜索最优解，因此，QPSO的全局搜索能力优于PSO。

在QPSO中粒子没有速度向量，粒子的位置更新由式（2）到式（4）决定：

其中A（i）为吸引子，k、u为（0，1）间的随机数，pbest（i）为粒子i的个体最优位置，gbest为群体最优位置，mbest为个体平均适应度，M为种群规模，为粒子i的位置，α为收缩扩张系数。

3.2改进量子粒子群算法

QPSO有很强的全局搜索能力，但会发生早熟收敛的现象。参考模拟退火过程，本位提出基于Metropolis准则的改进量子粒子群算法。QPSO中粒子会逐渐收敛于gbest附近，在此过程中若gbest与全局最优点距离较远，粒子有可能错过全局最优点，容易早熟收敛［11］。因此，改进吸引子A（i）的产生条件，使得局部吸引子由gbest（i）和Nbest决定式（5），其中Nbest的产生方法如下:

①Do Nbest=gbest；ini（Npop，p）

②if f（Npop）＜f（gbest）

③then Nbest=Npop

④else if p＜exp-（f（Npop）- f（gbest））/T

⑤then Nbest=Npop

其中Npop为随机产生的新粒子，p为（0，1）间的随机数，当Npop的适应度小于gbest的适应度值时，Nbest=Npop，当Npop的适应度大于gbest的适应度值时，Nbest仍有可能等于Npop，其概率取决于exp-（f（Npop）- f（gbest））/T，通过该Metropolis过程，保留了粒子搜索到全局最优解的可能性。T为退火温度，在改进算法中，退火温度T与粒子群迭代次数负相关，高温时即粒子群算法进化前期，使该方法能跳出局部最小值，从而避免早熟；低温时即粒子群进化后期，可使粒子加快收敛。

4仿真对比试验

图5　网络结构

图5为改进量子粒子群神经网络结构，以理想机械臂末端位姿（n o a p）为6个神经网络的输入，每个网络输出一个关节量，以改进QPSO算法优化网络输出关节量值，减小关节量值经正向运动学计算后与机械臂理想末端位姿的误差。QPSO算法的适应度函数见式（6），ko为姿态误差系数，kP为位置误差系数，它们后面的项分别为姿态误差和位置误差，θ为关节角度，Ranθ为关节活动范围。通过调节各项前的系数，可以调整各项在适应度函数中的比重，当ko增大，kP减小时，得到的优化结果姿态误差减小，但位置误差增大，反之，机械臂末端位值误差变大，姿态误差减小。

使用MATLAB软件数值仿真，注意位置和姿态数据的归一化，令ko、kP均为0.5；设置粒子群的数目为50，迭代次数为200。仿真结果如图6所示，实线为改进QPSO的迭代寻优曲线，虚线为QPSO的迭代寻优曲线，点划线为没有神经网络初值的改进QPSO迭代寻优曲线。通过曲线1和曲线2的对比，看到改进QPSO跳出了局部最小值，避免了QPSO的早熟收敛现象；通过曲线1和曲线3的对比得出结论，有神经网络作为输出的QPSO得到的结果更优，说明合理的初值改善了QPSO的优化结果。

图6　迭代寻优曲线

表3　3种方法结果比较

经过10次试验，表3为改进QPSO-NN，QPSO-NN和改进QPSO的对比结果。当最终寻优适应度值小于0.2时，该结果经逆向归一化后求的末端位姿误差可容忍，结果可用。因此，当最终寻优结果小于0.2时，认为该次迭代寻优成功。表中最后一行为若寻优成功，使得适应度小于0.2所需的平均迭代次数。通过对比进一步证明了改进QPSO避免了QPSO早熟收敛的现象，使粒子跳出局部最小值，收敛于近似全局最优解。QPSO-NN法每次求解均能得到可靠结果，并且平均迭代次数较少，满足研究对象逆运动学求解的要求。

5　结论

本文针对冗余机械臂逆运动学求解问题，提出了一种基于改进QPSO-NN的求解算法。首先通过对比，得到Bekir Karlik的双隐层BP神经网络结构在解冗余机械臂逆运动学是更具有优势。之后以神经网络输出为初值，以末端位姿误差为目标函数迭代寻优，通过对比，认为改进QPSO算法的寻优能力强于QPSO算法，前者收敛速度更快，得到极值更优。最后，对比3种算法得到结果，得出了改进QPSO-NN方法结果适应度更优，迭代次数更少，适用于机械臂逆运动学问题的结论。

改进QPSO-NN算法结合了神经网络算法的快速性和QPSO算法收敛结果的精确性，对求解没有解析解的冗余机械臂逆运动学问题效果良好。该种算法求得的逆运动学解可以直接作为机器人轨迹规划器的输入，以此为基础进行关节空间轨迹规划；同时该方法具有一定的通用性，可以移植到其他类型机构的逆运动学计算问题。

参考文献：

［1］ZHANG P Y，SHENG T，BOSONG L L.RBF network-based inverse kinematics of 6R manipulator［J/OL］.Int J Adv Manuf Technol，2004.

［2］VASILYWV I A，LYASHIN A M.Analytical solution to inverse kinematic problem for 6-DOF robot-manipulator［J］.Automation and Remote Control，2010，71（10）:2195–2199.

［3］XU J X，WANG W，SUN Y G.Two optimization algriothms for solving robotics inverse kinematics with redundancy［J］.Control Theory Appl，2010，8（2）:166-175.

［4］SHIMIZU M，KAKUYA H，YOON W K，et al.Analytical inverse kinematic computation for 7-DOF redundant manipulators with joint limits and its application to redundancy resolution［J］.IEEE Transactions on Robotics，2008，24（5）:1131-1142.

［5］HILDENBRAND D，ZAMORA J，EDUARDO B C.Inverse kinematics computation in computer graphics and robotics using conformal geometric algebra［J］.Advances in Applied Clifford Algebras，2008（18）:699-713.

［6］KOKER R.Reliability-based approach to the inverse kinematics solution of robots using elman’s networks［J］.Engineering Applications of Artificial Intelligence，2005（18）:685-693.

［7］KARLIK B，AYDIN S.An improved approach to solution of inverse kinematics problem for robot manipulators［J］.Engineering Applications of Artifical Intelligence，2000（13）:159-164.

［8］KOKER R.A neuro-simulated annealing approach to the inverse kinematics solution of redundant robotic manipulators［J］.Engineering with Computers，2013（29）:507–515.

［9］KOKER R.A genetic algorithm approach to a neural-network-based inverse kinematics solution of robotic manipulators based on error minimization［J］.Information Sciences，2013（222）:528–543.

［10］逄珊，杨欣毅，张小峰.航空发动机性能仿真的分群量子粒子群算法［J］.系统仿真学报，2013，25（6）:1176-1182.［11］龙海侠，须文波，王小根，等.基于选择操作的量子粒子群算法［J］.控制与决策，2010，25（10）:1449-1506.

［12］SAEED B.Niku.机器人学导论分析、控制及应用［M］.2 版.北京:电子工业出版社，2013.

［13］卞学良，杜清，马振书，等.危险弹药机器人机械臂运动学仿真分析［J］.现代制造工程，2010（6）:137-140.

［14］SUN J，LAI C H，XU W B，et al.A modified quantum behaved particle swarm optimization［C］//Proc of the 7th Int Conf on Computational Science，Beijing，China:Springer Press，2007.

Inverse Kinematics of Redundant Manipulators Based on Improved QPSO-NN

ZHANG Yun-feng1，MA Zhen-shu2，SUN Hua-gang2，LU Ji-shan1
（1.Ordnance Engineering College，Shijiazhuang 050003，China；2.Institute of Mechanical Technology，Shijiazhuang 050003，China）

Abstract：A technique based on improved QPSO-NN is proposed to solve the inverse kinematics of redundant manipulators.The posture of manipulator’s end -effector is used as input of neural networks and the inverse kinematics results are got from neural networks.To minimize the error of neural networks’output，a type of QPSO is selected.The results of neural network are added to initialized particle swarm and optimized through improved QPSO.QPSO is improved by Metropolis process，which could avoid premature convergence phenomenon.The orientation and position error of end effector calculated from joints’angle by forward kinematics is performed as fitness function.The simulation results show that the algorithm combines velocity of neural network and precision of improved QPSO，can satisfy the application in inverse kinematics of redundant manipulators.

Key words：redundant manipulator，inverse kinematics，neural network，QPSO，Metropolis algorithm

作者简介：张云峰（1990－），男，河北无极人，硕士研究生。研究方向：机器人技术。

*基金项目：国家“863”基金资助项目（2001AA422420）

收稿日期：2015-03-11修回日期：2015-05-05

文章编号：1002-0640（2016）03-0043-05

中图分类号：TP241.3

文献标识码：A