学习因子决定粒子自我总结和向优良个体学习的能力，[r1]、[r2]为[0，1]之间均匀分布的随机数。
基本粒子群的算法步骤如下：
①随机初始各粒子的位置和速度。
②计算各粒子的适应度，将粒子[i]的位置存入[pbesti]中，将[pbest]的最好位置存储在[gbest]中。
③采用（6）式更新粒子的位置和速度。
④计算各粒子的适应度，若当前粒子[i]的适应度优于[pbesti]，更新[pbesti]。
⑤将[pbest]的最好位置存储在[gbest]中。
⑥若[gbest]满足精度要求或达到最大迭代次数，算法结束，否则，返回③。
实际在更新粒子的位置时，还应考虑是否越界的问题，即[θj]是否超出了第[j]维坐标的范围。若超出范围，则将边界值赋给[θj]。
3.2 改进的粒子群算法
3.2.1 局部搜索
受惯性权重、学习因子等参数的影响粒子移动步长具有随机性。因而，即便某个粒子进入最优解的势力范围内，也未必能够精确地落在最优解的位置，进而影响到最优解的精度。局部搜索用来改善当前粒子的位置，即搜索粒子邻域内的更优解。若存在更优的位置，则用更优的位置代替当前的位置。
局部搜索的对象可以是整个种群，也可以只对当前最优粒子进行局部搜索。在相同的种群规模和迭代次数的条件下，前者求解精度更高，而后者计算量小一些。考虑算法对时间的要求，本文采用后者的方式。
若在粒子邻域内盲目的搜索，势必会影响算法的效率。沿着负梯度方向进行搜索可以快速找到粒子邻域内极值点，确保了算法的效率[6]。
下面讨论目标函数梯度的求解，在D-H坐标中[7]，末端操作器的位姿矩阵由变换矩阵[Ai]连乘得到如式（8）所示，变换矩阵[Ai]表示成如下的形式：
[Ai=cosθi-sinθicosαisinθisinαiaicosθisinθicosθicosαi-cosθisinαiaisinθi0sinαicosαidi0001]（7）
[Wθ=A1A2...An] （8）
在矩阵[Ai]中，[ai]、[αi]、[θi]、[di]分别是关节偏移、关节扭转、旋转关节的关节变量、滑动关节的关节变量，对于具体的机器人来说，[ai]、[αi]是固定不变的。
现只考虑关节变量是旋转变量的情形，矩阵[Ai]对[θi]求偏导，得：
[∂Ai∂θi=-sinθi-cosθicosαicosθisinαi-aisinθicosθi-sinθicosαisinθisinαiaicosθi00000000]
[=0-100100000000000Ai=RiAi] （9）
由式（9）可以推出末端操作器的位姿矩阵[Wθ]对[θi]的偏导数：
[∂Wθ∂θi=∂Nθ∂θi∂Oθ∂θi∂Aθ∂θi∂Pθ∂θi0000=A1A2..RiAi.An] （10）
因而推出[fθ]对[θi]的偏导数：
[∂fθ∂θi=2Pθ-P∗∂Pθ∂θi+2NθN∗+OθO∗+AθA∗-1∂Nθ∂θiN∗+∂Oθ∂θiO∗+∂Aθ∂θiA∗]（11）
用[∇fθ]表示[fθ]的梯度，由式（11）得：
[∇fθ=∂fθ∂θ1∂fθ∂θ2...∂fθ∂θN] （12）
在沿着负梯度方向进行搜索时，还应考虑搜索步长[λ]。若搜索步长设置的过长，虽然搜索速度快，但算法易發散。若搜索步长设置过短，虽然搜索速度慢，但算法收敛性能好。既然是局部搜索，考虑到粒子本身已经接近最优值，因而采取小步长的方式进行搜索是最佳的[8]。
若将关节变量[θ]采用二维向量的方式进行表示，沿着负梯度方向上的搜索如图2所示，图中的同一曲线上的点具有相同的适应度。每步搜索可写成式（13）的形式。
[θ=θ-λ∇fθ] （13）
设最大搜索步数为[MS]，在一代中粒子[i]的适应度最优，对粒子[i]做局部搜索。每一步搜索，倘若其适应度优于[pbesti]则将其值赋给[pbesti]。搜索完[MS]步后，将[pbesti]的值存入到[gbest]中。
3.2.2 逃逸策略
PSO算法迭代若干次后，所有的粒子都会趋于[gbest]。如果当前[gbest]是一个局部极值，那么一旦所有粒子都收敛于这一点之后，这些粒子就很难跳出这个局极值点。为改善粒子生存密度，在PSO 算法的基础上提出粒子逃逸策略，获得更大的生存空间，从而提高算法的收敛速度和精度[9]。当算法运行过程中[gbest]连续[G]代无变化，需采用逃逸策略来产生新一代的粒子群。将原始一半种群保留，另一半采取式（14）的方式进行逃逸[10]。
[θijt+1=lj+ruj-lj] （14）
式中，[r]为[0，1]区间上的随机数，[θj∈lj，uj]。
3.3.3 改进的PSO算法的流程图
基于局部搜索和逃逸策略的PSO算法流程图如图3所示：
4 实验结果及分析
为验证基于PSO算法求解逆运动学问题性能的好坏，以及改进策略是否提升PSO算法的性能，选取如图4所示的开链式连杆结构的机器人进行测试，其位姿矩阵如式（15）所示的。
[W=-0.4698-0.3420-0.8138-0.5383-0.17100.9397-0.29620.01690.86600.0000-0.50002.06600001] （15）
机器人的结构参数如表1所示，表2给出了算法改进前后的实验结果，图5分别显示目标函数在算法改进前后的收敛情况。
在两种算法中，粒子群规模[M=40]，维数[N=5]，学习因子[c1=c2=1.5]，最大迭代次数设为100，最小误差设为0.02，[θi∈-180∼180]（[i=1，2，...，5]）。原始算法中的惯性权重[w=0.7]。在改进的算法中，惯性权重[w=0.9]，搜索步长[λ=0.2]，最大搜索步数[MS=4]，逃逸代数[G=5]。
表2的数据表明，PSO算法可以完成对逆运动学问题的求解。在加入局部搜索环节和逃逸策略后，解的精度得到很大的提升，并且加快了收敛速度。图5展示了两种算法的整个收敛过程。很明显，改进算法的曲线优于原始算法。
5 结论
本文利用PSO算法避开直接求解逆运动学问题，针对PSO算法中的不足提出改进策略。结果表明，改进后的PSO算法能够加快收敛速度改善计算精度，保证机器人逆运动学问题的实时性和准确性。
参考文献：
[1] Manseur R， Doty KL. Structural Kinematics of 6-Revolute-axis Robot Manipulators[J]. Mechanism and Machine Theory， 1996， 31（5）： 6472657.
[2] Eberhart R， Kennedy J. A new optimizer using particle swarm theory[C]. Proc Int Sym Micro Machine and Human Science： Nago2ya， Japan， Oct. 1995， 139- 43.
[3] 劉洪霞. 粒子群算法改进及应用[D]. 南宁： 广西民族大学， 2011：4-5.
[4] 郝武伟， 曾建潮. 基于聚类分析的随机微粒群算法[J].计算机工程与应用， 2010， 46（8）： 40-44.
[5] 龚纯， 王正林. 精通MATLAB最优化计算[M].北京：电子工业出版社， 2009.
[6] 陈宝林. 最优化理论与算法[M].北京： 清华大学出版社， 2005.
[7] （美）Saeed B. Niku. 机器人学导论——分析、系统及应用[D].北京： 电子工业出版社， 2004 .
[8] 任玉杰. 数值分析及其MATLAB实现[M]. 北京： 高等教育出版社， 2007.
[9] 谢铮桂， 钟少丹， 韦玉科. 改进的粒子群算法及收敛性分析[J].计算机工程与应用，2011， 47（1）： 46-49.
[10] 王志. 粒子群优化算法及其改进[D].重庆： 重庆大学， 2011： 9-10.
【通联编辑：唐一东】
