基于改进自适应粒子群算法的机器人逆解研究*

武明虎，周喜悦，庆毅辉，胡 胜，刘 聪

(1．湖北工业大学电气与电子工程学院，武汉 430068；2．湖北太阳能高效利用协同创新中心，武汉 430068)

0 引言

机器人逆解求解指的是已知机器人末端的位置和姿态求出每个关节的角度，是机器人领域的基础问题，是实现对机器人精准实时控制的关键部分。目前机器人逆运动学求解方法主要分为两大类：解析法和数值法。但解析法[1]在求解结构复杂、自由度较高的机器人逆运动学时具有较大的局限性，因此数值法已成为当下获得机器人逆解的主要研究方法。

目前研究较多的数值方法主要是神经网络和群体智能方法。李文博等[2]利用BP神经网络求解机械臂逆运动学，网络设计过程简单但存在局部极小、收敛速度慢等缺点。李进等[3]提出一种基于RBF神经网络间接求取运动学逆解的方法，相对于BP神经网络，在局部极小值上有所改善，但求解精度完全取决于训练集质量的好坏且不同结构的机器人无法共用同一训练集。而常用的群体智能算法，如遗传算法[4]、粒子群算法[5]等在机器人逆运动学求解的通用性上更具有优势。王淑青等[6]提出的改进粒子群算法求逆解分别从“边界限定、惯性权重、超界返回”三个方面来限制多解、提高求解的准确性与快速性，但求解精度仍未达到最优。因此本文在文献[6]所提算法的基础上，引入种群曼哈顿距离实时确定种群的进化状态，提升算法的性能；将进化状态分为勘探状态与收敛状态，进而根据状态的不同选择自适应学习因子，平衡局部搜索与全局搜索的能力，结果验证了IAPSO算法在机器人逆运动学求解中的有效性。

1 运动学分析

1.1 运动学模型

为满足工业自动化的灵活度需求，建立的机器人模型具有6个旋转关节，等效关节分布如图1所示。

图1 机器人等效关节分布图

采用改进D-H参数法[7]对上述机器人进行坐标系的建立，如图2所示，Xi和Zi分别表示各个关节坐标系{i}的X方向和Z方向。

图2 机器人连杆坐标系

根据连杆参数在连杆坐标系中的表示方法，得到如表1所示的D-H参数。表中：ai-1为连杆长度，αi-1为连杆扭转角，di为连杆偏距，θi为关节角。

表1 机器人的D-H参数

表1中，d3=200 mm，a4=100 mm，a5=100 mm，d6=70 mm。

1.2 运动学正解

改进D-H参数法建系得到的两个相邻坐标系变换矩阵的一般表达式[8]为：

(1)

根据表1中的D-H参数和公式(1)得到坐标系{6}相对于坐标系{0}的变换矩阵：

(2)

(3)

其中，c45=cos(θ4+θ5)，s45=sin(θ4+θ5)，si=sinθi，ci=cosθi。

结合表1与式(3)在MATLAB中使用机器人工具箱建立机器人模型如图3所示。令关节变量角q=[0，π/2，π/3，0，-π/3，π/2，0]，使用函数得到的运动学正解与式(3)得到的运动学正解相同，验证了所建机器人模型的正确性。

图3 本文所建机器人模型

2 改进自适应粒子群算法求逆解

2.1 已有PSO求逆解模型描述

已有PSO应用于运动学逆解的算法流程如下：

步骤1：初始化粒子的位置和速度；

步骤2：适应度值计算，得到个体最优pb和全局最优gb；

步骤3：为限制多解，采用“狭小动态边界”策略确定搜索范围；

步骤4：迭代循环，w采用“非线性时变权重与陷局部最优自校正结合”方法确定；

步骤5：粒子的位置和速度按照公式(4)更新；

步骤6：若粒子超界，采用“超界带弱方向性返回扩散”方法处理；

步骤7：计算粒子的适应度函数值，个体最优和全局最优更新；

步骤8：未达到终止条件则执行步骤2，满足终止条件则结束循环。

速度和位置更新迭代公式[9]：

其中，w为惯性权重，k为当前迭代次数，pbi是粒子i的历史最佳位置，gb是整个群体的历史最佳位置，c1和c2为学习因子，r1和r2为均匀分布在[0，1]上的随机数。

上述PSO算法实现了利用“动态狭小边界”策略来限制多解，确保解的唯一性；对w采用“非线性时变与陷局部最优自校正结合”方法提高准确性；提出“超界带弱方向性返回扩散”策略来增强算法的快速性。

2.2 本文改进的算法模型

针对上述PSO算法寻优精度不足的缺陷，本文算法在文献[6]的基础上，做了两点改进：分析种群进化状态[10]、自适应学习因子策略[11]。

(1)种群进化状态

种群的进化状态随着粒子的迭代处于不断变化中，根据进化状态动态调整更新迭代公式的参数更符合种群的进化需求，实现算法性能的提升。

定义种群曼哈顿距离PMD[12]表示种群中每个粒子与其他粒子之间最小曼哈顿距离的均值，粒子间的曼哈顿距离MD可以用式(5)计算：

(5)

其中，D表示种群维度，xin表示第i个粒子的n维度位置值。对MD排序获得每个粒子与其他粒子的最小曼哈顿距离MDmin，进一步对最小曼哈顿距离求均值可得到种群曼哈顿距离PMD。算法最大迭代次数为T，则用式(6)表示第t-1代的种群曼哈顿距离PMD(t-1)，前t-2代中最小种群曼哈顿距离可以用式(7)表示：

PMD(t-1)

(6)

min(PMD[1:t-2])，1≤t≤T

(7)

通过比较式(6)与式(7)之间的大小关系，将种群的进化状态分为以下两种状态：

1)种群勘探状态：式(6)>式(7)，此时整个种群的粒子均处于积极搜索最优解的状态，具有很强的搜索能力。

2)种群收敛状态：式(6)≤式(7)，在这种状态下，种群进化处于一种停滞状态，若改变位置和速度更新参数有可能在后面的迭代过程中得到改进。

(2)自适应学习因子

从公式(4)可知，学习因子c1代表“种群个体认知”，可促进粒子获得它历史上最好的位置，学习因子c2代表“全局认知”，它能推进粒子向全局中最好的区域收敛。在种群勘探状态下为尽可能得到更多最优解，增加c1、减小c2有利于种群粒子更新自身的个体最优位置；在种群收敛状态下，为防止过早收敛陷入局部最优解应增加c1，同时也应增加c2的值使其他粒子尽快到达全局最优区域。IAPSO算法中，学习因子c1、c2初始值均为2，在种群勘探与收敛状态的自适应策略分别由公式(8)与公式(9)给出：

(8)

(9)

学习因子的变化需缓慢进行，变化太大对种群的收敛速度与算法精度均会有所影响。因此规定在迭代循环时c1、c2变化范围为[1.5，2.5]，若c1、c2之和大于4，则将两个参数进行归一化处理，归一化公式如下：

(10)

综上所述，本文IAPSO算法流程图如图4所示。

图4 本文IAPSO算法流程图

3 MATLAB仿真结果分析

在IAPSO算法中，确定以“目标点位姿与末端位姿距离误差最小”为求解目标，由于位置和姿态在数量级上的差距，在位置误差项前引入惩罚因子，使得位置误差与姿态误差满足整体误差最小。取粒子个数为60，最大迭代次数为100，惯性权重初始值为0.5，学习因子初始值为2进行仿真测试。

3.1 单点测试

随机选取机器人末端一组已知位姿Te：

(11)

现将已有PSO算法与IAPSO算法在普通PC上运行20次，分别计算运行20次所得到的适应度值的均值，得到改进前后算法的收敛曲线对比如图5所示，随机抽取IAPSO算法得到5组逆解结果如表2所示。

(a) 总适应度值收敛曲线(b) 第20～100次迭代收敛曲线图5 两种算法的适应度值曲线对比图

表2 给定位姿点的逆解验证结果(单位/rad)

从表2可以看出，每次运算求得的关节角度基本恒定在一个值，最大误差也不会超过0.005 rad，由此可见本文算法在机器人逆运动学求解中的准确性。

由图5a可以看出，在迭代前期IAPSO算法的收敛速度明显优于改进前，图5b也显示改进后的收敛精度有所提高，由此可见种群收敛状态和自适应学习因子策略能够增加粒子群的多样性，提升算法的搜索性能。

3.2 连续轨迹测试

任取机器人的起始位置T0和终止位置T1，并已知机器人起始位置所对应的关节角qs=[0，π/2，-π/2，π/2，0，π/2]，T0和T1数据如下：

(12)

在起始位姿和末端位姿间利用机器人工具箱函数进行10次插值，使用IAPSO算法得到连续轨迹逆解结果如图6所示。

图6 连续轨迹逆解验证

从图6中可以看出，连续轨迹测试所得关节角曲线是光滑且唯一的，并且没有大的跳变，表明所得解始终在上一次全局最优解附近产生，充分说明了IAPSO算法的正确性与有效性。

4 结论

本文建立了一种6自由度抓取机械手模型，通过机器人工具箱仿真验证了所建机器人模型的正确性。IAPSO算法是针对已有PSO算法求解精度不足的缺陷进行改进，兼顾了已有PSO算法求逆解的优势，同时仿真结果也表明IAPSO算法收敛速度快，计算精度更高。相比于传统的逆运动学求解方法，本文算法无须复杂的公式推导过程，且求解结果比较理想，能够应用于所有复杂结构机器人逆运动学求解。