CN105975795A - 一种高精度的多关节串联机械臂运动学反解解法 - Google Patents

Abstract

本发明涉及一种高精度的多关节串联机械臂运动学反解解法。本发明使用万有引力和粒子群相结合的算法构架(PSOGSA)，引入非线性权重分配系数s＝0.65·e^{((‑15·k)/T)}公式，使算法设计前期偏向万有引力算法，中、后期偏重于粒子群算法，以提高搜索效率。并采用了改进点“动态狭小边界”、“非线性时变权重与陷局部最优自校正结合”和“超界带弱方向性返回扩散”三种改进策略。采用以上算法和改进策略可以在较少的迭代次数内获得唯一的反解，并且误差一直稳定在10^‑8级，理论计算时间可低至2.58ms/次。针对后三轴交于一点的特殊结构机械臂，使用位置和姿态分离求反解的策略，可以进一步提高求解性能，误差可降低到10^‑14级，理论计算时间可降低到1.597ms/次。

Description

一种高精度的多关节串联机械臂运动学反解解法

技术领域

本发明涉及多关节串联机械臂的运动学求解技术，尤其是涉及一种不带反三角计算的高精度串联机械臂运动学反解的解法。该解法基于改进的万有引力粒子群算法(PSOGSA)。

背景技术

多关节串联机械臂是一个复杂的机电对象，它不仅涉及到机械方面的机械设计、加工、装配，也涉及到电气方面电机的选型、驱动，还涉及控制方面的以运动学、动力学为基础的多关节的配合控制算法。而其中运动学是任何控制的基础，因为它是用于解算机械臂腕部末端位姿与各个关节角度关系的手段。其中，由关节角度得到机器人末端位姿叫做运动学正解，通过DH坐标系法可以很容易得到表达式。而由机械臂末端位姿求解各关节的角度则是叫做运动学反解，反解的获得就要困难的许多。本发明正是针对运动学反解提出的一种高精度解法。

逆运动学求解的方法有很多，但所有的方法根据其求得的解的性质可分为两大类：解析法和数值法。解析解是由基本函数组成显示解析式，因为给出任意的自变量就可以求出其因变量,也就是问题的解，所以解析解也被称为闭式解或封闭解(closed-formsolution)。如代数法(反变换法、四元数消元法)、图解法(几何法)得到的解都属于解析解。而数值解是采用某种数值计算方法以某种方式不断地与准确解数值逼近，得到满足一定精度要求的近似解。如基于搜索迭代机制算法、模拟输入输出关系的神经网络。

反变换法是最常规普遍的方法，但其求解的表达式会含有反三角，实 际中很多芯片编程时只能用查表的方法，这样就会影响求解精度，且在机械臂结构不符合Pieper准则时，该方法无解；图解法也叫几何法，适用于关节比较少、结构简单的机械臂，同样需要计算反三角；神经网络可以逼近逆运动学的非线性关系，但由于其结构对最终结果的不确定性及需要大量已知样本进行训练，在应用中受到了很大的限制；搜索迭代的方法，本发明方法属于此类方法，该方法从正向运动学出发，不断地把候选解带入到正运动方程，求与目标位姿的差距作为适应度函数，按照各自算法所模拟的自然规律来选下一次迭代的候选解，不断地使适应度值变小，当经过一定的代数后，候选解便成为了一定精度的近似解，这样近似解就从解域的海量的解中筛选了出来。这类方法可以适用于任何结构的串联机械臂(包括不符合piper准则的)，求解时也不会涉及反三角计算。但这种方法通常存在稳定性不高、精度低、求解速度慢和求解不唯一问题。

发明内容

本发明主要是为了解决现有技术存在的稳定性不高、精度低、求解速度慢和求解不唯一的技术问题；提供了一种可以稳定、快速、准确、唯一的解出机械臂的运动学反解的解法。

本发明使用万有引力和粒子群相结合的算法构架，算法设计中前期偏向万有引力算法，中、后期偏重于粒子群算法，以提高搜索效率。这里引入权重分配系数s改变算法的偏向性，s表达式为：s＝0.65·e^((-15·k)/T)，s随着迭代次数非线性变小。优化过程中采用了改进点1“动态狭小边界”、改进点2“非线性时变权重与陷局部最优自校正结合”和改进点3“超界带弱方向性返回扩散”三种改进策略。

采用以上算法和改进策略可以在较少的迭代次数内获得唯一的反解，并且误差稳定在10^-8级，理论计算时间可低至2.58ms/次。针对后三轴交于一点的特殊结构机械臂，使用位置和姿态分离求反解的策略可以进一步提 高求解性能，误差可降低到10^-14级，理论计算时间可降低到1.597ms/次。

本发明的上述技术问题主要是通过下述技术方案得以解决的：

一种高精度的多关节串联机械臂运动姿态求解方法，其特征在于，

步骤1：根据机械臂的外形结构尺寸，建立DH参数表，并按照DH法建立正运动学方程：

⁰T_n＝⁰T₁·¹T₂…n^-1T_n (1)

其中，n为机械臂的关节总数；得到

其中，n＝[n_x,n_y,n_z]^T，o＝[o_x,o_y,o_z]^T，a＝[a_x,a_y,a_z]^T，p＝[p_x,p_y,p_z]^T；n,o,a为姿态向量，p为位置向量；n_x,n_y,n_z,o_x,o_y,o_z,a_x,a_y,a_z，p_x,p_y,p_z的具体表达式中只包含了θ₁,θ₂…θ_n这n个关节角变量；所述ⁱ-¹T_i中，i＝1,2,…n；与i号关节相连的i-1号连杆上固连的DH坐标系作为基坐标，与i号关节相连的i号连杆上固连的DH坐标系相对于基坐标系的位姿转换矩阵为^n-1T_n；

步骤2：规划出所有要求反解的位姿矩阵⁰T_n(1),⁰T_n(2)…⁰T_n(m-1),⁰T_n(end)

读取机械臂各关节编码器记录的角度初值{θ₁,θ₂…θ_n}，带入包含了θ₁,θ₂…θ_n的位姿向量表达式，得到起始位姿⁰T_n(start)。结合机械臂的要到 达的终点位姿⁰T_n(end)，在起始位姿和终点位姿之间任意使用一种轨迹规划进行插值，获得{⁰T_n(start),⁰T_n(1),⁰T_n(2)…⁰T_n(m-1),⁰T_n(end)}，若用均匀插值则其插

值点的公式为：

⁰T_n(i)代表的是机器人末端的dh坐标系{n}相对于机器人基坐标系{0}的位姿变换矩阵，i的具体数值代表第几个插值点。当i＝m时，⁰T_n(i)为⁰T_n(end)。 ⁰T_n(1),⁰T_n(2)…⁰T_n(m-1),⁰T_n(end)为都需要解出对应的反解的m个位姿。

在运算过程中需要定义已算的反解个数标志，这里定义为tt，初值为1。

步骤3：初始化粒子的速度和位置：定义需要求解的关节角度为x＝[x₁,x₂…x_n]，对应于算法工具中粒子的n维空间位置坐标值，其表达式为：

其中，x_max和x_min为当前的边界向量；当t＝1时，x_last为关节角度初值{θ₁,θ₂…θ_n}，当t≠1时，x_last为上一次的反解；以x_max和x_min为粒子位置边界，以x_max/2和x_min/2为粒子的速度边界，随机生成a个粒子。

为了标志后续步骤中算法循环的代数，引入t为循环代数，初值为1。

步骤4：计算粒子群算法中每个粒子适应度值Q_j(t)、个体历史最优P_(p-best)i、群体历史最优P_g-best。

计算每一个粒子的适应度值Q_j(t)，基于以下公式：

其中为⁰T_n(tt)位姿向量，n,o,a,p为第t代第j个粒子的n维空间位置坐标式(2)得到。 n＝[n_x,n_y,n_z]^T，o＝[o_x,o_y,o_z]^T，a＝[a_x,a_y,a_z]^T，p＝[p_x,p_y,p_z]^T。

当代最优适应度算式如下：

当代最差适应度算式如下：

求每个粒子个体的历史最优适应度值Q_(p-best)j与位置X_(p-best)j：

若Q_j(t)<Q_(p-best)j，则Q_(p-best)j＝Q_j(t)，X_(p-best)j＝x_j(t)； (9)

若Q_j(t)>＝Q_(p-best)j时，Q_(p-best)j、X_(p-best)j均保持不变；

求整个群体历史最优适应度Q_g-best与位置X_g-best：

若Q_(p-best)j<Q_g-best，则Q_g-best＝Q_(p-best)j，X_g-best＝X_(p-best)j； (10)

若Q_(p-best)j>＝Q_g-best时，Q_g-best、X_g-best均保持不变；

步骤5：计算万有引力算法中每个粒子个体的加速度计算每一个粒子质量，基于以下公式：

每个个体n维空间位置坐标可表示为其中a为种群规模，x_d ⁱ表示第i个个体在第d维空间中的位置，n为问题空间维数；

在第t代时，计算个体i和个体j之间的万有引力为：

其中，ε是一个很小的常数；R_ij(t)表示第t代第i个粒子和第j个粒子 的欧几里德距离R_ij(t)＝||(x_i(t),x_j(t))||；G(t)是第t代的万有引力常系数，公式为：

则第i个粒子个体在第d维的合力表示为

第i个粒子个体在第d维空间产生的加速度为:

步骤6：更新粒子个体的速度v和位置x，超界则回置。

更新第i个粒个体第d维空间的速度和位置公式如下：

其中，s＝0.65·e^((-15·k)/T)为用来调节粒子群算法和万有引力算法的影响权重的参数，它从大迅速变小，结合式(15)知前期以万有引力算法为主导，运行到中、后期则以粒子群算法为主导；c₁和c₂为学习因子；r₁,r₂和r₃为[0,1]区间内的伪随机数；v和x分别为粒子速度和位置，k为代数，i为粒子编号，d为空间维数，P_(p-best)i和P_g-best分别为个体和群体的最优点；ω是惯性因子。为提高求解准确性，使用“非线性时变权重与陷局部最优自校正结合”的改进策略，表达式为：

其中，t为迭代次数，T为最大截止代数；

选择①时：连续10代的适应度值Q的变化量没有超过10^-5，且适应度值Q还没小于10^-6；

选择②时：除选择①以外的其他所有情况。

当第i个粒子超过当前边界时，使用“超界带弱方向性返回扩散”的改进策略，表达式为：

这里，i＝1,2…a；x为粒子的位置向量；x_max和x_min为当前的边界向量。

步骤7：检查迭代代数t＝t+1有没有到达循环代数T，若t没有到达T则返回执行步骤4；若到达T则此时的P_g-best便是⁰T_n(tt)对应的反解，并执行步骤8；

步骤8：检查已算的反解个数标志，判断tt＝tt+1是否等于插值点数m，不等则返回执行步骤3，相等则结束计算。

因此，本发明具有如下优点：1、本发明的反解方法可以适用于n关节任意结构(包括不符合piper准则的结构)的运动学反解的计算，具有一般性和通用性。2、本发明针对使用普通PSOGSA算法的缺点，加入了“动态狭小边界”、“非线性时变权重与陷局部最优自校正结合”和“超界带弱方向性返回扩散”等措施，使得求解具有很高的稳定性、速度、精度、准确性、唯一性。3、本发明主框架算法为并行的，容易实现求解的实时性。

附图说明

图1为本发明针对串联机械臂普遍性适用的方法的流程示意图。(注：具有通用性，精度稍低，最大位姿综合误差仅为10^-8数量级)

图2为本发明针对后3关节轴交于一点的6关节串联机械臂专用方法的流程示意图。(注：工业上主流6关节串联机械臂多属后3关节轴交于一点的结构，该方法精度高最大位姿综合误差仅为10^-14数量级)

图3a为本发明通用方法在puma机械臂上的连续轨迹中1000个位姿的反解结果示意图(关节一)。

图3b为本发明通用方法在puma机械臂上的连续轨迹中1000个位姿的 反解结果示意图(关节二)。

图3c为本发明通用方法在puma机械臂上的连续轨迹中1000个位姿的反解结果示意图(关节三)。

图3d为本发明通用方法在puma机械臂上的连续轨迹中1000个位姿的反解结果示意图(关节四)。

图3e为本发明通用方法在puma机械臂上的连续轨迹中1000个位姿的反解结果示意图(关节五)。

图3f为本发明通用方法在puma机械臂上的连续轨迹中1000个位姿的反解结果示意图(关节六)。

图4a是唯一性测试中没有使用“动态狭小边界”的求解结果示意图。

图4b是唯一性测试中使用“动态狭小边界”求解结果示意图。

图5a是准确性测试中方案①对某一位姿进行5次反解的适应度值收敛趋势示意图(本发明通用方法不采用改进措施)。

图5b是准确性测试中方案②对某一位姿进行5次反解的适应度值收敛趋势示意图(本发明通用方法)。

图5c是准确性测试中方案③对某一位置进行5次反解的适应度值收敛趋势示意图(为本发明专用方法)。

图5d是准确性测试中方案③对某一姿态进行5次反解的适应度值收敛趋势示意图(为本发明专用方法)。

图6a是方案①连续轨迹逆解的求解结果示意图(关节一)。

图6b是方案①连续轨迹逆解的求解结果示意图(关节二)。

图6c是方案①连续轨迹逆解的求解结果示意图(关节三)。

图6d是方案①连续轨迹逆解的求解结果示意图(关节四)。

图6e是方案①连续轨迹逆解的求解结果示意图(关节五)。

图6f是方案①连续轨迹逆解的求解结果示意图(关节六)。

图7a是方案②或③连续轨迹逆解的求解结果示意图(关节一)。

图7b是方案②或③连续轨迹逆解的求解结果示意图(关节二)。

图7c是方案②或③连续轨迹逆解的求解结果示意图(关节三)。

图7d是方案②或③连续轨迹逆解的求解结果示意图(关节五)。

图7e是方案②或③连续轨迹逆解的求解结果示意图(关节五)。

图7f是方案②或③连续轨迹逆解的求解结果示意图(关节六)。

具体实施方式

下面通过实施例，并结合附图，对本发明的技术方案作进一步具体的说明。

实施例1

本发明提供一种高精度的多关节串联机械臂运动姿态求解方法，包括：

步骤1：根据机械臂的外形结构尺寸，建立DH参数表，并按照DH法建立正运动学方程：

⁰T_n＝⁰T₁·¹T₂…^n-1T_n (18)

其中，n为机械臂的关节总数；得到

其中，n＝[n_x,n_y,n_z]^T，o＝[o_x,o_y,o_z]^T，a＝[a_x,a_y,a_z]^T，p＝[p_x,p_y,p_z]^T；n,o,a为姿态向量，p为位置向量；n_x,n_y,n_z,o_x,o_y,o_z,a_x,a_y,a_z，p_x,p_y,p_z的具体表达式中只包含了θ₁,θ₂…θ_n这n个关节角变量；所述^i-1T_i中，i＝1,2,…n；与i号关节相连的i-1号连杆上固连的DH坐标系作为基坐标，与i号关节相连的i号连杆上固连的DH坐标系相对于基坐标系的位姿转换矩阵；

步骤2：规划出所有要求反解的位姿矩阵⁰T_n(1),⁰T_n(2)…⁰T_n(m-1),⁰T_n(end)，具体如下：读取机械臂各关节编码器记录的角度初值{θ₁,θ₂…θ_n}，带入包含了θ₁,θ₂…θ_n的位姿向量表达式，得到起始位姿⁰T_n(start)；结合机械臂的要到达的终点位姿⁰T_n(end)；在起始位姿和终点位姿之间任意使用一种轨迹规划进行插值，获得{⁰T_n(start),⁰T_n(1),⁰T_n(2)…⁰T_n(m-1),⁰T_n(end)}，若用均匀插值则其插值点的公式为：

⁰T_n(i)代表的是机器人末端的dh坐标系{n}相对于机器人基坐标系{0}的位姿变换矩阵，i的具体数值代表第几个插值点。当i＝m时，⁰T_n(i)为⁰T_n(end)。 ⁰T_n(1),⁰T_n(2)…⁰T_n(m-1),⁰T_n(end)为都需要解出对应的反解的m个位姿。

在运算过程中需要定义已算的反解个数标志，这里定义为tt，初值为1。

步骤3：初始化粒子的速度和位置：定义需要要求解的关节角度x＝[x₁,x₂…x_n]，它对应于算法工具中粒子的它对应于算法工具中粒子的n维空间位置坐标；表达式为：

其中，x_max和x_min是当前的边界向量，当t＝1时，x_last为关节角度初值{θ₁,θ₂…θ_n}，当t≠1时，x_last为上一次的反解；以x_max和x_min为粒子位置边界，以x_max/2和x_min/2为粒子的速度边界，随机生成a个粒子；

为了标志后续步骤中算法循环的代数，设当前代数标志t＝1；

步骤4：计算粒子群算法中每个粒子适应度值Q_j(t)、个体历史最优P_(p-best)i、群体历史最优P_g-best；

计算每一个粒子的适应度值Q_j(t)，基于以下公式：

其中为⁰T_n(tt)位姿向量，n,o,a,p是由第t代第j个粒子的3维空间位置坐标式(19)中得到， n＝[n_x,n_y,n_z]^T，o＝[o_x,o_y,o_z]^T，a＝[a_x,a_y,a_z]^T，p＝[p_x,p_y,p_z]^T。

当代最优适应度算式如下：

当代最差适应度算式如下：

求每个粒子个体的历史最优适应度值Q_(p-best)j与位置X_(p-best)j：

若Q_j(t)<Q_(p-best)j，则Q_(p-best)j＝Q_j(t)，X_(p-best)j＝x_j(t)； (26)

若Q_j(t)>＝Q_(p-best)j时，Q_(p-best)j、X_(p-best)j均保持不变；

求整个群体历史最好适应度Q_g-best与位置X_g-best：

若Q_(p-best)j<Q_g-best,则Q_g-best＝Q_(p-best)j，X_g-best＝X_(p-best)j；

若Q_(p-best)j>＝Q_g-best时，Q_g-best、X_g-best均保持不变； (27)

步骤5：计算万有引力算法中每个粒子个体的加速度计算每一个粒子质量，基于以下公式：

每个个体n维空间位置坐标可表示其中a为种群规模，x_d ⁱ表示第i个体在第d维空间中的位置，n为问题空间维数；

在第t代时，计算个体i和个体j之间的万有引力为：

其中，ε是一个很小的常数；R_ij(t)表示第t代第i个粒子和第j个粒子的欧几里德距离R_ij(t)＝||(x_i(t),x_j(t))||；G(t)是t代的万有引力常系数

则第i个粒子个体在第d维的合力表示为：

第i个粒子个体在第d维空间产生的加速度为:

步骤6：更新粒子个体的速度v和位置x，超界则回置：更新第i个粒个体第d维空间的速度和位置公式如下：

其中，s＝0.65·e^((-15·k)/T)是用来调节粒子群算法和万有引力算法的影响权重的参数，它从大迅速变小，结合式(15)知前期以万有引力算法为主导，运行到中、后期则以粒子群算法为主导；c₁和c₂为学习因子；r₁,r₂和r₃为[0,1]区间内的伪随机数；v和x分别为粒子速度和位置，k为代数，i为粒子编号，d为空间维数，P_(p-best)i和P_g-best分别为个体和群体的最优点；ω是惯性因子； 为提高求解准确性，使用“非线性时变权重与陷局部最优自校正结合”的改进策略，表达式为：

其中，t为迭代的次数，T为最大截止代数；

选择①：连续10代的适应度值Q的变化量没有超过10^-5，且适应度值Q还没小于10^-6；

选择②：除选择①以外的其他所有情况；

当第i个粒子超过当前边界时，使用“超界带弱方向性返回扩散”的改进策略，表达式为：

i＝1,2…a；x为粒子的位置向量；x_max和x_min是当前的边界向量；

步骤7：检查迭代代数t＝t+1有没有到达循环代数T，

若还没有到达则返回执行步骤4；

若到达则此时的P_g-best便是⁰T_n(tt)对应反解，并执行步骤8；

步骤8：检查已算的反解个数标志tt＝tt+1是否等于插值点数m，不等则返回执行步骤3，相等则结束计算。

实施例2

本发明提供一种高精度的多关节串联机械臂运动姿态求解方法，具体涉及满足piper准则的6关节结构的机械臂。

假设机械臂为满足piper准则的6关节结构，后三关节轴交于1点，正运动学的位置方程不会含有后三关节角，则可以使用位置和姿态分步求反解，从而进一步提高反解的精度，求反解的方法按照以下步骤：

步骤1：根据机械臂的外形结构尺寸，建立DH参数表，并按照DH法建立正运动学方程

⁰T₆＝⁰T₁·¹T₂·²T₃·³T₄·⁴T₅·⁵T₆ (35)

可以计算出：

n为机械臂的关节总数。n,o,a为姿态向量，为位置向量。在这里p只与前三关节θ₁θ₂θ₃有关，先求出位置反解θ₁θ₂θ₃后，把θ₁θ₂θ₃作为已知，再姿态逆解出θ₄θ₅θ₆。

步骤2：规划出所有要求反解的位姿矩阵⁰T_n(1),⁰T_n(2)…⁰T_n(m-1),⁰T_n(end)

读取机械臂各关节编码器记录的角度初值{θ₁,θ₂,θ₃,θ₄,θ₅,θ₆}，带入式(19)可以得到起始位姿⁰T_n(start)。结合机械臂的要到达的终点位姿⁰T_n(end)。在起始位姿和终点位姿之间任意使用一种轨迹规划进行插值，获得{⁰T_n(start),⁰T_n(1),⁰T_n(2)…⁰T_n(m-1),⁰T_n(end)}，若用均匀插值则其插值点的公式为：

当i＝m是时⁰T_n(i)就是⁰T_n(end)。由于⁰T_n(1),⁰T_n(2)…⁰T_n(m-1),⁰T_n(end)这m个位姿都要解出对应的反解。tt作为后续步骤反解到第几个点的标志，在这里赋初值为1。

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

设需要求解的前3关节角度x＝[x₁,x₂,x₃]，它对应于算法工具中粒子的3维空间位置坐标。为了限制多解，保证唯一性，使用“动态狭小边界”，其表达式为：

其中，x_max(1～3)和x_min(1～3)是当前的边界向量，x_last(1～3)是上一次的反解。当t＝1时没有上一次的反解，此时x_last(1～3)等于关节角度初值{θ₁,θ₂,θ₃}。以x_max(1～3)和x_min(1～3)为粒子位置边界，以x_max(1～3)/2和x_min(1～3)/2为粒子的速度边界，随机生成a个粒子。

为了标志后续步骤中算法循环的代数，引入t为循环代数，初值为1。

步骤4：计算粒子群算法中每个粒子适应度值Q_j(t)、个体历史最优P_(p-best)i、群体历史最优P_g-best。

计算每一个粒子的适应度值Q_j(t)，计算公式如下：

其中为⁰T_n(tt)的姿态向量，p为第t代第j个粒子的3维位置坐标带入计算的位置向量，p＝[p_x,p_y,p_z]^T。

求当代最好适应度：

求当代最差适应度：

求每个粒子个体的历史最好适应度值Q_(p-best)j与位置X_(p-best)j：

若Q_j(t)<Q_(p-best)j，则Q_(p-best)j＝Q_j(t)，X_(p-best)j＝x_j(t)。 (43)

求整个群体历史好适应度Q_g-best与位置X_g-best：

若Q_(p-best)j<Q_g-best,则Q_g-best＝Q_(p-best)j，X_g-best＝X_(p-best)j。 (44)

步骤5：计算万有引力算法中每个粒子个体的加速度

计算每一个粒子质量，计算公式如下

每个个体3维空间位置为a为粒子个体的总数， 表示第i个体在第d维空间中的位置。

在t代，计算个体i和个体j之间的万有引力为：

其中，ε是一个很小的常数；R_ij(t)表示第t代第i个粒子和第j个粒子的欧几里德距离R_ij(t)＝||(x_i(t),x_j(t))||；G(t)是t代的万有引力常系数则个体i在d维的合力可以表示为

个体i在第d维空间产生的加速度为:

步骤6：更新粒子个体的速度v和位置x，超界则回置

更新第i个粒个体第d维空间的速度和位置公式如下：

其中，s＝0.65·e^-15·k/T是用来调节粒子群算法和万有引力算法的影响权重 的参数，它从大迅速变小，说明开前期万有引力算法为主导，后期粒子群算法为主导；c₁和c₂为学习因子；r₁,r₂和r₃为[0,1]区间内的伪随机数。v和x分别为粒子速度和位置，k为代数，i为粒子编号，d为空间维数，P_(p-best)i和P_g-best分别为个体和群体的最优点。ω是惯性因子；为提高求解准确性，使用“非线性时变权重与陷局部最优自校正结合”，表达式为：

其中，t为迭代代数，T为最大截止代数。条件①为连续10代的适应度值Q的变化量没有超过10^-5，且适应度值Q还没小于10^-6。条件②为除条件①以外的其他情况。

为提高求解快速性，使用“超界带弱方向性返回扩散”的方法，其表达式为：

其中，i＝1,2…a；x为粒子的位置向量；x_max(1～3)和x_min(1～3)是边界向量。

步骤7：检查迭代代数t＝t+1有没有到达循环代数T。若还没有到达，则跳回第4步。若到达，则此时的P_g-best便是⁰T_n(tt)的位置向量p对应反解[x₁,x₂,x₃]，也就是前三个关节角的值，带着已知的前3个关节值[x₁,x₂,x₃]、t重新置为1，跳到第8步。

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

设需要求解的后3关节角度x＝[x₄,x₅,x₆]，它对应于算法工具中粒子的3维空间位置坐标。为了限制多解，保证唯一性，使用“动态狭小边界”，其表达式为：

其中，x_max(4～6)和x_min(4～6)是当前的边界向量，x_last是上一次的反解。当t＝1时没有上一次的反解，此时x_last(4～6)等于关节角度初值{θ₄,θ₅,θ₆}。以x_max和x_min为粒子位置边界，以x_max/2和x_min/2为粒子的速度边界，随机生成a个粒子。

为了标志后续步骤中算法循环的代数，设当前代数标志t＝1。

步骤9：计算粒子群算法中每个粒子适应度值Q_j(t)、个体历史最优P_(p-best)i、群体历史最优P_g-best。

计算每一个粒子的适应度值Q_j(t)，计算公式如下：

其中为⁰T_n(tt)的姿态向量，n,o,a为第t代第j个粒子的3维位置坐标值带入式(36)的姿态部分。n＝[n_x,n_y,n_z]^T，o＝[o_x,o_y,o_z]^T，a＝[a_x,a_y,a_z]^T。

求当代最好适应度：

求当代最差适应度：

求每个粒子个体的历史最好适应度值Q_(p-best)j与位置X_(p-best)j：

若Q_j(t)<Q_(p-best)j，则Q_(p-best)j＝Q_j(t)，X_(p-best)j＝x_j(t) (57)

求整个群体历史好适应度Q_g-best与位置X_g-best：

若Q_(p-best)j<Q_g-best,则Q_g-best＝Q_(p-best)j，X_g-best＝X_(p-best)j。 (58)

步骤10：计算万有引力算法中每个粒子个体的加速度

计算每一个粒子质量，计算公式如下

每个个体3维空间位置为a为粒子个体的总数， 表示第i个体在第d维空间中的位置。

在t代，计算个体i和个体j之间的万有引力为：

其中，ε是一个很小的常数；R_ij(t)表示第t代第i个粒子和第j个粒子的欧几里德距离R_ij(t)＝||(x_i(t),x_j(t))||；G(t)是t代的万有引力常系数

则个体i在d维的合力可以表示为

个体i在第d维空间产生的加速度为:

步骤11：更新粒子个体的速度v和位置x，超界则回置

更新第i个粒个体第d维空间的速度和位置公式如下：

其中，s＝0.65·e^-15·k/T是用来调节粒子群算法和万有引力算法的影响权重的参数，它从大迅速变小，说明开前期万有引力算法为主导，后期粒子群算法为主导；c₁和c₂为学习因子；r₁,r₂和r₃为[0,1]区间内的伪随机数。v和x分别为粒子速度和位置，k为代数，i为粒子编号，d为空间维数，P_(p-best)i和 P_g-best分别为个体和群体的最优点。ω是惯性因子；为提高求解准确性，使用“非线性时变权重与陷局部最优自校正结合”，表达式为：

其中，t为迭代代数，T为最大截止代数。条件①为连续10代的适应度值Q的变化量没有超过10^-5，且适应度值Q还没小于10^-6。条件②为除条件①以外的其他情况。

为提高求解快速性，使用“超界带弱方向性返回扩散”的方法，其表达式为：

其中，i＝1,2…a；x为粒子的位置向量；x_max(4～6)和x_min(4～6)是步骤3的边界向量x_max和x_min的后3维。

步骤12：检查迭代代数t＝t+1有没有到达循环代数T，若还没有到达则跳回第9步，若到达则此时的P_g-best便是⁰T_n(tt)的姿态向量n,o,a对应反解[x₄,x₅,x₆]，跳到第13步。

步骤13：由第7步和第12步的合起来结果可知⁰T_n(tt)的反解[x₁,x₂,x₃,x₄,x₅,x₆]。检查反解个数标志tt＝tt+1是否等于插值点数m，不等则跳到第三步，相等则结束计算。

下面是以国际上具有代表性的六轴工业机器人puma560为例的具体一个案例。

为了方便对比，以国际上具有代表性的六轴工业机器人puma560为例。其DH参数见表1。

表1 PUMA560的DH参数

其中a₂＝431.8，a₃＝20.32，d₂＝149.09，d₄＝433.07。

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

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

其中c_i＝cosθ_i，s_i＝sinθ_i(i＝1、2…6)。其余均按类似规则缩写。

步骤1：根据机械臂的外形结构尺寸，建立DH参数表，并按照DH法建立正运动学方程：

⁰T₆＝⁰T₁·¹T₂·²T₃·³T₄·⁴T₅·⁵T₆ (67)

带入数据可得

令n＝[n_x,n_y,n_z]^T，o＝[o_x,o_y,o_z]^T，a＝[a_x,a_y,a_z]^T，p＝[p_x,p_y,p_z]^T。n,o,a为姿态向量，p为位置向量。n_x,n_y,n_z,o_x,o_y,o_z,a_x,a_y,a_z，p_x,p_y,p_z的具体表达式中只包含了θ₁,θ₂…θ_n这n个关节角变量(此时n＝6)。

步骤2：规划出所有要求反解的位姿矩阵⁰T_n(1),⁰T_n(2)…⁰T_n(m-₁₎,⁰T_n(end)

读取机械臂各关节编码器记录的角度初值θ₁,θ₂,θ₃,θ₄,θ₅,θ₆：

[θ₁,θ₂,θ₃,θ₄,θ₅,θ₆]＝[0.919989477558371,-0.0678998189131538,0.0920824094486244,0.201312616289960,-0.0278773641141659,-0.196228448576235]

带入式(51)，可以得到起始位姿⁰T_n(start)

⁰T_n(start)＝[0.609802213043344,0.792528340475009,0.00633170682899847,0.148341405331900；0.792547333496747,-0.609809758316640,-0.000884776240474939,0.440899912303689；0.00315992636556806,0.00555771587329567,-0.999979563120985,-0.404138067152012；0,0,0,1]；

若需要到达终点位姿为

⁰T_n(end)＝＝[0.359195566484664,-0.919158272891769,0.161637292703085,0.358231409336435；-0.831941105240732,-0.393843942881297,-0.390846448194868,-0.494560644554584；0.422909614963518,0.00591760342835049,-0.906152547610541,-0.226622549363056；0,0,0,1]；

在起始位位姿和终点位姿之间任意使用一种轨迹规划进行插值，获得{⁰T_n(start),⁰T_n(1),⁰T_n(2)…⁰T_n(m-1),⁰T_n(end)}，若用均匀插值则其插值点的公式为：

当i＝m是时⁰T_n(i)就是⁰T_n(end)。由于⁰T_n(1),⁰T_n(2)…⁰T_n(m-1),0T_n(end)这m个位姿都要解出对应的反解。tt作为后续步骤反解到第几个点的标志，在这里赋值为1。取m＝1000。

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

设需要求解的关节角度x＝[x₁,x₂,x₃,x₄,x₅,x₆]，它对应于算法工具中粒子的它对应于算法工具中粒子的n维空间位置坐标。为了限制多解，保证唯一性，使用“动态狭小边界”，其表达式为：

其中，x_max和x_min是当前的边界向量，x_last是上一次的反解。当t＝1时没有上一次的反解，此时x_last等于关节角度初值{θ₁,θ₂,θ₃,θ₄,θ₅,θ₆}。以x_max和x_min为粒子位置边界，以x_max/2和x_min/2为粒子的速度边界，随机生成a(取a为60)个粒子。

为了标志后续步骤中算法循环的代数，设当前代数标志t＝1。

步骤4：计算粒子群算法中每个粒子适应度值Q_j(t)、个体历史最优P_(p-best)i、群体历史最优P_g-best。

计算每一个粒子的适应度值Q_j(t)，计算公式如下：

其中为⁰T_n(tt)位姿向量，n,o,a,p是由第t代第j个粒子的6维空间位置坐标带入式(68)中得到。 n＝[n_x,n_y,n_z]^T，o＝[o_x,o_y,o_z]^T，a＝[a_x,a_y,a_z]^T，p＝[p_x,p_y,p_z]^T。

求当代最好适应度：

求当代最差适应度：

求每个粒子个体的历史最好适应度值Q_(p-best)j与位置X_(p-best)j：

若Q_j(t)<Q_(p-_best)j，则Q_(p-_best)j＝Q_j(t)，X_(p-_best)j＝x_j(t)。 (75)

求整个群体历史好适应度Q_g-best与位置X_g-best：

若Q_(p-best)j<Q_g-best,则Q_g-best＝Q_(p-best)j，X_g-best＝X_(p-best)j。 (76)

步骤5：计算万有引力算法中每个粒子个体的加速度

计算每一个粒子质量，计算公式如下

每个个体6维空间位置坐标可表示其中a为种群规模，x_d ⁱ表示第i个体在第d维空间中的位置，n为问题空间维数。

在t代，计算个体i和个体j之间的万有引力为：

其中，ε是一个很小的常数；R_ij(t)表示第t代第i个粒子和第j个粒子的欧几里德距离R_ij(t)＝||(x_i(t),x_j(t))||；是t代的万有引力常系数。(取G₀＝50；α＝50)

则粒子个体i在d维的合力可以表示为

粒子个体i在第d维空间产生的加速度为:

步骤6：更新粒子个体的速度v和位置x，超界则回置

更新第i个粒个体第d维空间的速度和位置公式如下：

其中，s＝0.65·e^((-15·t)/T)是用来调节粒子群算法和万有引力算法的影响权重的参数，它从大迅速变小，说明开前期万有引力算法为主导，后期粒子群算法为主导；c₁和c₂为学习因子；r₁,r₂和r₃为[0,1]区间内的伪随机数。v和x分别为粒子速度和位置，k为代数，i为粒子编号，d为空间维数，P_(p-best)i和P_g-best分别为个体和群体的最优点。ω是惯性因子；为提高求解准确性，使用“非线性时变权重与陷局部最优自校正结合”，表达式为：

其中，t为迭代的次数，T为最大截止代数。条件①为连续10代的适应度值Q的变化量没有超过10^-5，且适应度值Q还没小于10^-6。条件②为除条件①以外的其他情况。

为提高求解快速性，使用“超界带弱方向性返回扩散”的方法，其表达式为：

其中，i＝1,2…a；x为粒子的位置向量；x_max和x_min是当前的边界向量。

步骤7：检查迭代代数t＝t+1有没有到达循环代数T(T取150)，若还没有到达则跳回第4步。若到达则此时的P_g-best便是⁰T_n(tt)对应反解，跳到第8步。

步骤8：检查已反解个数标志tt＝tt+1是否等于插值点数m(m取1000)，不等则跳到第三步，相等则结束计算。求解结果见图3a至图3f。

为了验证本发明各种性能，现以如下三种方案来进行一系列测试。(唯一性测试只与改进粒子群算法“动态狭小边界”。

方案①普通的万有引力粒子群：c₁＝2,c₂＝2,T＝150,N＝60，G₀＝50；α＝50机械可行范围做边界；ω＞0.4时位置超界按本文处理，否则不限；ω＝0.65*exp(-i*((T-i+240)/T)/T)为非线性时变。

方案②本发明的改进的万有引力粒子群：c₁＝2,c₂＝2,T＝150,N＝60，G₀＝50；α＝50边界采用改进点1；ω采用改进点2；超界采用改进点3。

方案③本文的改进的万有引力粒子群+分步求解策略：T₁＝70,T₂＝80，其余参数同②。由于位置逆解和位姿逆解分级求解，故该方案是以式(8)为目标函数是的两步串联迭代过程，且为保证两次跌代的总代数与其他算法一致，故缩减各自的迭代次数为70代和80代。

(1)唯一性测试

改进点1即“动态狭小边界”可以有效避免多解，保证唯一性。对同一位姿用方案②和去掉“动态狭小边界”的方案②分别逆解20次来对比佐证算法避免多解的效果。图4a为方案②没有使用“动态狭小边界”的解的情况，可以看到解出了很多组解，图4b是方案②使用了“动态狭小边界”的解的情况，始终只有一组解。“动态狭小边界”确实可以有效避免多解。

(2)准确性测试

用三种方案分别对某点求逆解，且都重复运行5遍，把每遍中适应度值随迭代次数的变化都记录在表2中，具体的适应度值数值则是取5遍的平均值记录表2中。

表2各方案平均适应度值与代数之间的关系

理论上，由于方案③因分步计算策略避免多目标问题，会使得求解精度大幅提高，而方案②由于本文改进求解精度应高于方案①。从图5a可以看到方案①大多在40代左右都陷入了局部最优解，且表2可知适应度值最终平均收敛于0.32，位姿误差过大。而从图5b可见方案②却能够稳定的向零收敛，且由表2知平均收敛于5.9e-8，具有一定的位姿精度。但方案③确实具有更高位姿精度，从表2可知，平均位置误差和姿态误差分别为2.1e-14和2.3e-14，故位姿精度3.11e-14。另外从图5a、图5b和图5cd对比发现方案③的收敛曲线更加紧密一致，故方案③也更加稳定。

(3)快速性测试

用三种方案分别对某点求逆解，且都重复运行1000遍，记录各自的整体运行时间于表3。本次实验的pc机相关配置为：win7、matlab2012b、i5-4440CPU、RAM8GB。

表3各方案完成1000次逆解的耗时

	方案①	方案②	方案③
				耗时(s)	133.631	154.591	95.832

方案③是位置和姿态分步求解，这使得迭代中每代的目标函数要比其他两种算法的要简单，即每代的计算量要另外两种算法小，但总的迭代代数又和其他两种算法相等，故理论上应该是最快的。从表3可知，方案③确 实最快、方案①次之，方案②最慢。方案②比方案①还慢，是方案②因为改进点二在每代都要检测算法是否陷入局部最优而解增加额外计算量。最快的方案③也要平均每95.8ms才能完成一次解算，这样的速度直接用于在线计算显然不够。但粒子群算法本身就是一种天然的并行算法，在每一代各个粒子的相应计算是独立的。本文仿真受到MABLAB的环境框架制约每一代的每个粒子的计算必须挨个进行的，不能同时计算。所以如果把程序改编到FPGA或其他可以并行处理的芯片中，每一代所有的粒子的计算是可以一次完成。那么假设FPGA和普通pc处理速度一样，那么计算速度变快为现在的60倍。即1.597ms一次解算，这样的足以用于实时。

(4)连续轨迹求解仿真

先做如下轨迹规划：在可达空间任取起始位姿T₁和终止位姿T₂，并已知起始位置对应的关节角θ_start。起点和终点之间的插值由robotics工具箱的函数产生，其中，T1＝[0.61,0.79,0.006,0.148；0.79,-0.61,

-0.001,0.441；0.003,0.0055,-0.1,-0.404；0,0,0,1]；％起始位姿

T2＝[0.36,-0.92,0.16,0.358；-0.83,-0.394,-0.39,-0.494；0.423,0.006,-0.906,-0.2266；0,0,0,1]；％终止位姿。

再用三种方案分别逆解所有的插值点，方案①由于没有解决多解问题根本无法完成有效的逆解，其解一直在大幅跳变，如图6a-f所示。方案②和③都能成功解出有效逆解，由于分辨率限制表现在曲线图上都差不多，故仅以图7a-b共示之。算法改进点1可以让解出来的逆解始终都与上一组解是最近的，可以保证唯一性，从而使得连续轨迹求解的值是连续光滑的， 从图7a-b中可见连续轨迹逆解的曲线是光滑的。这个仿真的成功也意味着方案②和③皆已具备用于连续轨迹求解的基础。只不过方案③求解精度更高、速度更快，而方案②更加通用，可用于一般结构机器人求解。

本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。

Claims (1)

1.一种高精度的多关节串联机械臂运动学反解解法，其特征在于，包括：

步骤1：根据机械臂的外形结构尺寸，建立DH参数表，并按照DH法建立正运动学方程：

⁰T_n＝⁰T₁·¹T₂…^n-1T_n (1)

其中，n为机械臂的关节总数；得到

${T_0}_n=\left[\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right]\left\{\begin{array}{c}{n}_x=f_1\left({\mathrm{\&theta;}}_1,{\mathrm{\&theta;}}_2...{\mathrm{\&theta;}}_n\right)\\ n_y=f_2\left({\mathrm{\&theta;}}_1,{\mathrm{\&theta;}}_2...{\mathrm{\&theta;}}_n\right)\\ n_z=f_3\left({\mathrm{\&theta;}}_1,{\mathrm{\&theta;}}_2...{\mathrm{\&theta;}}_n\right)\\ o_x=f_4\left({\mathrm{\&theta;}}_1,{\mathrm{\&theta;}}_2...{\mathrm{\&theta;}}_n\right)\\ o_y=f_5\left({\mathrm{\&theta;}}_1,{\mathrm{\&theta;}}_2...{\mathrm{\&theta;}}_n\right)\\ o_z=f_6\left({\mathrm{\&theta;}}_1,{\mathrm{\&theta;}}_2...{\mathrm{\&theta;}}_n\right)\\ a_x=f_7\left({\mathrm{\&theta;}}_1,{\mathrm{\&theta;}}_2...{\mathrm{\&theta;}}_n\right)\\ a_y=f_8\left({\mathrm{\&theta;}}_1,{\mathrm{\&theta;}}_2...{\mathrm{\&theta;}}_n\right)\\ a_z=f_9\left({\mathrm{\&theta;}}_1,{\mathrm{\&theta;}}_2...{\mathrm{\&theta;}}_n\right)\\ p_x=f_{10}\left({\mathrm{\&theta;}}_1,{\mathrm{\&theta;}}_2...{\mathrm{\&theta;}}_n\right)\\ p_y=f_{11}\left({\mathrm{\&theta;}}_1,{\mathrm{\&theta;}}_2...{\mathrm{\&theta;}}_n\right)\\ p_z=f_{12}\left({\mathrm{\&theta;}}_1,{\mathrm{\&theta;}}_2...{\mathrm{\&theta;}}_n\right)\end{array}\right.---\left(2\right)$

其中，n＝[n_x,n_y,n_z]^T，o＝[o_x,o_y,o_z]^T，a＝[a_x,a_y,a_z]^T，p＝[p_x,p_y,p_z]^T；n,o,a为姿态向量，p为位置向量；n_x,n_y,n_z,o_x,o_y,o_z,a_x,a_y,a_z，p_x,p_y,p_z的具体表达式中只包含了θ₁,θ₂…θ_n这n个关节角变量；所述^i-1T_i中，i＝1,2,…n；与i号关节相连的i-1号连杆上固连的DH坐标系作为基坐标，与i号关节相连的i号连杆上固连的DH坐标系相对于基坐标系的位姿转换矩阵就是^n-1T_n；

步骤2：规划出所有要求反解的位姿矩阵⁰T_n(1),⁰T_n(2)…⁰T_n(m-1),⁰T_n(end)，

读取机械臂各关节编码器记录的角度初值{θ₁,θ₂…θ_n}，带入包含了θ₁,θ₂…θ_n的位姿向量表达式，得到起始位姿⁰T_n(start)；结合机械臂的要到达的终点位姿⁰T_n(end)；在起始位姿和终点位姿之间任意使用一种轨迹规划进行插值，获得{⁰T_n(start),⁰T_n(1),⁰T_n(2)…⁰T_n(m-1),⁰T_n(end)}，若用均匀插值则其插值点的公式为：

$\begin{array}{cc}{T_0}_{n\left(i\right)}={T_0}_{n\left(start\right)}+\frac{i}{m}\&CenterDot;({T_0}_{n\left(end\right)}-{T_0}_{n\left(start\right)})& i=1,2...m\end{array}---\left(3\right)$

⁰T_n(i)代表的是机器人末端的dh坐标系{n}相对于机器人基坐标系{0}的位姿变换矩阵，i的具体数值代表第几个插值点；当i＝m时，⁰T_n(i)为⁰T_n(end)；⁰T_n(1),⁰T_n(2)…⁰T_n(m-1),⁰T_n(end)为都需要解出对应的反解的m个位姿；

在运算过程中需要定义已算的反解个数标志，这里定义为tt，初值为1；

步骤3：初始化粒子的速度和位置：定义需要要求解的关节角度x＝[x₁,x₂…x_n]，它对应于算法工具中粒子的它对应于算法工具中粒子的n维空间位置坐标；表达式为：

$\left\{\begin{array}{c}{x}_{\max }=x_{last}+2/180*\mathrm{\&pi;}\\ x_{\min }=x_{last}-2/180*\mathrm{\&pi;}\end{array}\right.---\left(4\right)$

其中，x_max和x_min是当前的边界向量；当t＝1时，x_last为关节角度初值{θ₁,θ₂…θ_n}，当t≠1时，x_last为上一次的反解；；以x_max和x_min为粒子位置边界，以x_max/2和x_min/2为粒子的速度边界，随机生成a个粒子；

$\left\{\begin{array}{c}{x}_i=\left(x_{\max }-x_{\min }\right)\&CenterDot;rand+x_{\min }\\ v_i=\left(v_{\max }-v_{\min }\right)\&CenterDot;rand+v_{\min }\end{array}\right.---\left(5\right)$

为了标志后续步骤中算法循环的代数，设当前代数标志t＝1；

步骤4：计算粒子群算法中每个粒子适应度值Q_j(t)、个体历史最优P_(p-best)i、群体历史最优P_g-best；

计算每一个粒子的适应度值Q_j(t)，基于以下公式：

$\begin{array}{ccc}{Q}_j\left(t\right)=\sqrt{\mathrm{\&Sigma;}\&lsqb;{(n_i-{\hat{n}}_i)}^2+{(o_i-{\hat{o}}_i)}^2+{(a_i-{\hat{a}}_i)}^2+{(p_i-{\hat{p}}_i)}^2\&rsqb;}& i=x,y,z& j=1,2...a\end{array}---\left(6\right)$

其中为⁰T_n(tt)位姿向量，n,o,a,p由第t代第j个粒子的3维空间位置坐标带入式(2)中得到， n＝[n_x,n_y,n_z]^T，o＝[o_x,o_y,o_z]^T，a＝[a_x,a_y,a_z]^T，p＝[p_x,p_y,p_z]^T；

求当代最优适应度：

求当代最差适应度：

求每个粒子个体的历史最优适应度值Q_(p-best)j与位置X_(p-best)j：

若Q_j(t)<Q_(p-best)j，则Q_(p-best)j＝Q_j(t)，X_(p-best)j＝x_j(t)； (9)

若Q_j(t)>＝Q_(p-best)j时，Q_(p-best)j、X_(p-best)j均保持不变；

求整个群体历史最优适应度Q_g-best与位置X_g-best：

若Q_(p-best)j<Q_g-best,则Q_g-best＝Q_(p-best)j，X_g-best＝X_(p-best)j； (10)

若Q_(p-best)j>＝Q_g-best时，Q_g-best、X_g-best均保持不变；

步骤5：计算万有引力算法中每个粒子个体的加速度：计算每一个粒子质量，基于以下公式：

$m_i\left(t\right)=\frac{Q_i\left(t\right)-Q_{worst}\left(t\right)}{Q_{best}\left(t\right)-Q_{worst}\left(t\right)}$

$\begin{array}{cc}{M}_i\left(t\right)=\frac{m_i\left(t\right)}{{\mathrm{\&Sigma;}}_{j=1}^a{m}_j\left(t\right)}& i=1,2...a\end{array}---\left(11\right)$

每个个体n维空间位置坐标可表示为其中a为种群规模，x_d ⁱ表示第i个体在第d维空间中的位置，n为问题空间维数；

在第t代时，计算个体i和个体j之间的万有引力为：

$F_{ij}^d\left(t\right)=G\left(t\right)\frac{M_i\left(t\right)\&CenterDot;M_j\left(t\right)}{R_{ij}\left(t\right)+\mathrm{\&epsiv;}}\left(x_j^d\right(t)-x_i^d(t\left)\right)---\left(12\right)$

其中，ε是一个很小的常数；R_ij(t)表示第t代第i个粒子和第j个粒子的欧几里德距离R_ij(t)＝||(x_i(t),x_j(t))||；G(t)是t代的万有引力常系数

则第i个粒子个体在第d维的合力表示为

$F_i^d\left(t\right)=\underset{j=1,j\&NotEqual;i}{\overset{a}{\&Sigma;}}rand\&CenterDot;F_{ij}^d\left(t\right)---\left(13\right)$

第i个粒子个体在第d维空间产生的加速度为:

$a_i^d\left(t\right)=\frac{F_i^d\left(t\right)}{M_i\left(t\right)}---\left(14\right)$

步骤6：更新粒子个体的速度v和位置x，超界则回置；

更新第i个粒个体第d维空间的速度和位置公式如下：

$\left\{\begin{array}{c}{v}_i^d\left(t+1\right)=\left(1-s\right)\&CenterDot;\left(\mathrm{\&omega;}\&CenterDot;v_i^d\left(t\right)+c_1\&CenterDot;r_1\&CenterDot;\left({P_{\left(p-best\right)}}_i^d-x_i^d\left(t\right)\right)+c_2\&CenterDot;r_2\&CenterDot;\left({P_{g-best}}^d-x_i^d\left(t\right)\right)\right)\\ +s\&CenterDot;\left(r_3\&CenterDot;v_i^d\left(t\right)+a_i^d\left(t\right)\right)\\ x_i^d\left(t+1\right)=x_i^d\left(t\right)+v_i^d\left(t+1\right)\end{array}\right.---\left(15\right)$

其中，s＝0.65·e^((-15·k)/T)是用来调节粒子群算法和万有引力算法的影响权重的参数，它从大迅速变小，结合式(15)知前期以万有引力算法为主导，运行到中、后期则以粒子群算法为主导；c₁和c₂为学习因子；r₁,r₂和r₃为[0,1]区间内的伪随机数；v和x分别为粒子速度和位置，k为代数，i为粒子编号，d为空间维数，P_(p-best)i和P_g-best分别为个体和群体的最优点；ω是惯性因子；为提高求解准确性，使用“非线性时变权重与陷局部最优自校正结合”，表达式为：

其中，t为迭代的次数，T为最大截止代数；

选择①：连续10代的适应度值Q的变化量没有超过10^-5，且适应度值Q还没小于10^-6；

选择②：除选择①以外的其他所有情况；

当第i个粒子超过当前边界时，使用“超界带弱方向性返回扩散”的改进策略，表达式为：

$x_i=\left\{\begin{array}{cc}0.5\&CenterDot;rand\&CenterDot;\left(x_{\max }-x_{\min }\right)+x_{\min }& x_i<x_{\min },\mathrm{\&omega;}>0.4\\ x_{\max }-0.5\&CenterDot;rand\&CenterDot;\left(x_{\max }-x_{\min }\right)& x_i>x_{\max },\mathrm{\&omega;}>0.4\end{array}\right.---\left(17\right)$

i＝1,2…a；x为粒子的位置向量；x_max和x_min是当前的边界向量；

步骤7：检查迭代代数t＝t+1有没有到达循环代数T

若还没有到达则返回执行步骤4；

若到达则此时的P_g-best便是⁰T_n(tt)对应反解，并执行步骤8；

步骤8：检查已反解个数标志tt＝tt+1是否等于插值点数m，不等则返回执行步骤3，相等则结束计算。