CN108932216B - 一种基于粒子群优化算法的机器人逆运动学求解方法 - Google Patents

Abstract

本发明涉及一种基于粒子群优化算法的机器人逆运动学求解方法，包括以下步骤：1)根据机器人的关节参数建立运动学模型，得到运动学正解公式，设定目标点的位置和姿态矩阵，确定适应度函数；2)初始化粒子种群，计算初始种群的个体适应度值，获得个体最优值和全局最优值；3)使用随迭代次数而改变的动态惯性权重，更新粒子种群的位置和速度，计算新的适应度值，并判断是否更新；4)对粒子种群进行遗传变异操作，计算适应度值；5)根据适应度值和迭代次数判断是否满足终止条件。与现有技术相比，本发明解决了传统PSO算法易陷入局部最优值的缺点，可通过减少迭代次数增强局部搜索能力提升收敛的速度和精度。

Description

一种基于粒子群优化算法的机器人逆运动学求解方法

技术领域

本发明涉及一种机器人逆运动学求解方法，尤其是涉及一种基于粒子群优化 算法的机器人逆运动学求解方法。

背景技术

串联机器人的逆运动学求解是机器人轨迹规划和控制等方面研究的基础。只有通过运动学逆解把空间位姿转换为关节变量，才能实现对机器人末端执行器按空间 位姿进行编程控制。

机器人运动学逆解有数值法、几何法和代数法等，在满足pieper准则(即机器 人的三个相邻关节轴交于一点或三轴线平行)后，利用六个转换矩阵相互关系求得 每个轴角度θ。一个常用的数值方法是将6R串联操作臂各关节的D-H参数中的径 向参数a_i、ɑ_i和轴向参数s_i、θ_i分离，运用双四元数方法或李代数方法将6R串联 操作臂运动学正解矩阵构造成两个独立的齐次变换线性方程组，通过将两个方程组 联立逐次迭代或消元而得到关于各关节转角的16组运动学逆解。如QIAO等运用 双四元数理论得到了一般6R串联操作臂运动学逆解的数值解。通常情况下，数值 法求解逆运动学，会在奇异点处出现无解的结果。

除了使用数值法进行运动学逆解之外，还有使用粒子群优化算法进行求解，粒 子群优化(PSO)求解优化问题时，问题的解对应于搜索空间中某一粒子的位置，称 为“粒子”(particle)或“主体”(agent)。每个粒子都有自己的位置和速度，还有一 个由被优化函数决定的适应值。各个粒子记忆、追随当前的最优粒子，在解空间中 搜索。每次迭代的过程不是完全随机的，如果找到较好解，将会以此为依据来寻找 下一个解。首先令PSO初始化为一群随机粒子(随机解)，在每一次迭代中，粒子通 过跟踪两个“极值”来更新自己，通过不断迭代选择出最优解，根据其适应度值确 定其逆解，但现有的粒子群优化算法易陷入局部最优解。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于粒子群 优化算法的机器人逆运动学求解方法，避免计算陷入局部最优解，准确而快速求得 运动学逆解。

本发明的目的可以通过以下技术方案来实现：

一种基于粒子群优化算法的机器人逆运动学求解方法，包括以下步骤：

1)根据机器人的关节参数(关节类型、尺寸、运动范围)建立运动学模型， 得到运动学正解公式，设定目标点的位置和姿态矩阵，确定适应度函数；

2)初始化粒子种群，计算初始种群的个体适应度值，获得个体最优值和全局 最优值；

3)使用随迭代次数而改变的动态惯性权重，更新粒子种群的位置和速度，计 算新的适应度值，并与个体最优值和全局最优值进行比较，判断是否更新，若是， 则返回步骤3)，若否，则进入步骤4)；

4)对粒子种群进行遗传变异操作，计算适应度值，并与步骤3)中适应度值 比较，判断是否接受该适应度值，若接受，则以该适应度值为当前迭代的适应度； 若不接受，则以步骤3)中未遗传变异之前的适应度值为当前迭代适应度；

5)根据适应度值和迭代次数判断是否满足终止条件，若是，则结束，得到机 器人在该姿态下六轴最优角度，若否，则返回步骤3)。

所述的步骤1)中，适应度函数为搜索到的角度正解与目标笛卡尔坐标矩阵相 减后的范数。

所述的步骤3)中，动态惯性权重计算式如下：

W＝w^((i/Tmax)^a)

其中，W为动态惯性权重，一般是0到1的值，w为预设最小惯性权重，i为 当前迭代次数，Tmax为总迭代次数，ɑ为随i增大而减小的调节量，ɑ>1时W为 凸曲线，ɑ<1时W为凹曲线，更改ɑ的大小以适应各种情况，根据迭代效果选择 最佳的ɑ值，惯性权重前期较大时有利于全局搜索，后期较小有利于更精细的搜索， 误差更小。

所述的步骤4)中，遗传及变异概率随迭代次数增多而减小在迭代前期时，应 保持较高的变异概率，保持种群多样性。在迭代后期(适应度值较小时)减小变异 概率，此时的粒子群搜索不拍陷入局部最优，应更注重精度搜索，不需要变异概率。 若在迭代过程中，适应度小于一定值时直接减小变异概率，并以此为当前最优解迭 代，适应度值达到某一点时(本发明中使用适应度值越小，所搜索的目标点越适合)， 取消变异几率以增加局部搜索精度。

所述的步骤5)中，终止条件为：达到最大迭代次数或全局最优粒子适应度值 连续n次小于给定收敛阈值，n为设定的次数。

与现有技术相比，本发明使用优化动态惯性权重的结合遗传算法的粒子群优化方法，该方法随迭代结果改变惯性权重及变异概率，将其应用于机器人的逆运动学 逆解中解决了传统PSO算法易陷入局部最优值的缺点，并可以通过减少迭代次数 增强局部搜索能力提升收敛的速度和精度。

附图说明

图1是本实施例六轴机器人的逆运动学仿真算法流程图；

图2是未陷入局部最优值时的迭代寻优结果图；

图3是陷入局部最优值时的迭代寻优结果图；

图4是使用GA-PSO算法的迭代寻优结果图。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范 围不限于下述的实施例。

实施例

机器人的运动学正解就是根据机器人六个关节角度求得机器手末端位姿状态，每个关节轴之间的存在一个转换矩阵，每个矩阵中存在杆长、偏转角、横距、转角 四个属性，将六个转换矩阵相乘即可得到六轴机器人由基座到机器手末端的坐标位 姿转换矩阵。如式1所示：

其中每个转换矩阵依据后置法确定关节之间坐标系的转换模式，后置法即为将固连坐标系建立在下关节处，其坐标轴的转换为首先进行绕x轴旋转α角度，再进 行x轴z轴的平移，最后进行绕z轴旋转的θ角度，如下式2所示：

其中

的运算结果为

针对ePSOnC3机器手的六关节转换矩阵分别为

机器人运动学逆解是已知机器手位姿矩阵求得六个轴的角度。本发明中通过将粒子的6个角度代入上式所得

公式中，以此根据目标矩阵求范数作为适应度函 数。

粒子群优化算法(PSO)中，每个优化问题的潜在解都是搜索空间中的一个“粒子”。每个粒子具有位置和速度两个特征，粒子的位置坐标对应的目标函数值即可 作为该粒子的适应度，算法通过适应度来衡量粒子的优劣。在每一次迭代中，粒子 通过跟踪两个“极值”来更新自己。第一个极值就是粒子本身所找到的最优解，叫 做个体极值pBest；另一个极值是整个种群目前找到的最优解，这个极值是全局极 值gBest。在找到这两个最优值时，粒子根据如下公式更新自己的速度和位置：

v(t+1)＝W·v(t)+C₁·rand()·[pBest(t)-x(t)]+C₂rand()·[gBest(t)-x(t)](4)

x(t+1)＝x(t)+v(t+1) (5)

v(t+1)是粒子下一时刻的速度；x(t+1)是粒子下一时刻的位置；C₁和C₂是学习 因子，通常C₁＝C₂＝2；W为惯性权重，一般取惯性权重∈[0,1]，pBest(t)为当前最 佳个体极值，gBest(t)为当前最佳全局极值。

PSO算法中适应度函数的定义非常重要，本发明中为给定目标点Ptarget与第 t次搜索到的关节变量解所对应的点P_t之间距离的范数，以此为例，搜索的目标是 使该范数最小的转换矩阵以及此矩阵的六轴的角度，取norm(P_target-P_t)最小值即 为搜索到的最适合矩阵，其中P_target与P_t中的姿态矩阵(左上3*3矩阵)取反正弦 函数以增加姿态矩阵的权重，以P_target＝[0 0 90 357；0-90 0 0；90 0 0 357；0 0 0 1]为例， 其中pos(t)为由六轴的角度组成的粒子。

仅使用PSO算法的迭代时易出现陷入局部最优的迭代结果，若未陷入局部最 优值，则可以搜索到与目标姿态相似结果，如图2。如图3中，迭代结果中适应度 值约为105，搜索到的Gbest_position与目标位置差距较大，在调试中仅使用PSO 算法会出现陷入局部最优，这是因为粒子群在迭代过程中会失去粒子多样性，粒子 的六个维数(代表六个轴的角度)的变化范围非常小，即粒子群多样性小。本发明结 合遗传算法优化PSO算法的迭代，使该算法通过增加粒子多样性避免陷入局部最 优值。

在调试中仅使用PSO算法会出现陷入局部最优，这是因为粒子群在迭代过程 中会失去粒子多样性，如图4，图中粒子的六个维数(代表六个轴的角度)的变化范 围非常小，即粒子群多样性小。

遗传算法(GA)在经过变异和交叉后会使粒子群一直保持较高的粒子群多样 性：

交叉函数：

p₁'＝rand*p₁+(1-rand)p₂

p₂'＝rand*p₂+(1-rand)p₁ (6)

变异函数：

其中p₁和p₂为所选取的两个优秀的粒子，rand为0到1随机数，i为当前迭代 次数，Tmax为总迭代次数，v为与最大(最小)角度边界的距离，p'即为变异后 角度。

交叉函数是选取较优秀的粒子选取该粒子某维数与另一优秀粒子的同维数进 行交换。变异函数中原角度变异后增加或降低的概率相同。在GA算法优化PSO 后可以使其保持较高的种群多样性。

GA-PSO算法不仅解决了陷入局部最优的情况，并且可以缩减迭代次数与运算 时间，使PSO算法具有更优秀的全局搜索能力和更快的搜索效率。

在GA-PSO算法中可以加入以下细节优化可以适用于不同情况：

(1)PSO算法的惯性权重

PSO算法的惯性权重对粒子群搜索具有很大的影响，在搜索前期惯性权重应 有较大的值，此时由上次迭代中的速度(搜索角度的变化速率)对此次迭代速度的 影响程度，采用下式(8)，则有前期较大时有利于全局搜索，后期较小有利于更 精细的搜索，误差更小。而且采用a>1，其递减曲线为凸曲线，此种曲线在前期较 大且减小速度慢，有利于全局搜索，后期迭代次数较高时，减小快，进入精度搜索 快。

W＝w^((i/Tmax)^a) (8)

其中i为当前迭代次数，T为总迭代次数，该式即为1至w的凸曲线递减，该 式中w可根据实际需要进行更改，粒子群优化算法中惯性权重一般是0到1的数， 更改w为迭代末期惯性权重，ɑ决定1至w的曲线，a>1为凸曲线，a<1为凹曲线。

(2)GA算法的变异概率改变

变异的概率是保持种群多样性的重要属性，在迭代前期时，应保持较高(本实 施例中以0.3为例)的变异概率，保持种群多样性。在迭代后期(适应度值较小时) 减小变异概率，此时的粒子群搜索不怕陷入局部最优，应更注重精度搜索，不需要 变异概率。

Claims (4)

1.一种基于粒子群优化算法的机器人逆运动学求解方法，其特征在于，包括以下步骤：

1)根据机器人的关节参数建立运动学模型，得到运动学正解公式，设定目标点的位置和姿态矩阵，确定适应度函数；

适应度函数为搜索到的角度正解与目标笛卡尔坐标矩阵相减后的范数；所述的角度正解具体为根据机器人六个关节角度求得的机器手末端位姿状态；

2)初始化粒子种群，计算初始种群的个体适应度值，获得个体最优值和全局最优值；

3)使用随迭代次数而改变的动态惯性权重，更新粒子种群的位置和速度，计算新的适应度值，并与个体最优值和全局最优值进行比较，判断是否更新，若该粒子当前适应度值小于该粒子以往的适应度值，则将当前粒子作为个体最优解，若该粒子相比其他所有粒子个体最优仍为最小则同时将该粒子更新为全局最优解；

动态惯性权重计算式如下：

W＝w^((i/Tmax)^a)

其中，W为动态惯性权重，w为预设最小惯性权重，i为当前迭代次数，Tmax为总迭代次数，ɑ为随i增大而减小的调节量，ɑ>1；

4)对粒子种群进行遗传变异操作，计算适应度值，并与步骤3)中适应度值比较，判断是否接受该适应度值，若接受，则以该适应度值为当前迭代的适应度值；若不接受，则以步骤3)中未遗传变异之前的适应度值为当前迭代适应度值；

5)根据适应度值和迭代次数判断是否满足终止条件，若是，则结束，得到机器人在该姿态下六轴最优角度，若否，则返回步骤3)。

2.根据权利要求1所述的一种基于粒子群优化算法的机器人逆运动学求解方法，其特征在于，所述的步骤4)中，遗传及变异概率随迭代次数增多而减小。

3.根据权利要求1所述的一种基于粒子群优化算法的机器人逆运动学求解方法，其特征在于，所述的步骤4)中，在迭代后期，适应度值小于设定值时，变异几率为0。

4.根据权利要求1所述的一种基于粒子群优化算法的机器人逆运动学求解方法，其特征在于，所述的步骤5)中，终止条件为：达到最大迭代次数或全局最优粒子适应度值连续n次小于给定收敛阈值，n为设定的次数。

Images