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

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

A Solution Method for Inverse Kinematics of Robots Based on Particle Swarm Optimization

The invention relates to a method for solving inverse kinematics of robots based on particle swarm optimization, which includes the following steps: 1) establishing kinematics model according to joint parameters of robots, obtaining forward kinematics formula, setting position and attitude matrix of target points, determining fitness function; 2) initializing particle swarm and calculating initial kinematics. Individual fitness value of the population is used to obtain individual optimum value and global optimum value. 3) The position and speed of the particle population are updated by using dynamic inertia weight which changes with the number of iterations, and the new fitness value is calculated, and whether it is updated or not is judged. 4) The genetic mutation operation of the particle population is carried out to calculate the fitness value; 5) The fitness value is calculated according to the fitness. The degree value and iteration number are used to determine whether the termination condition is satisfied. Compared with the prior art, the present invention solves the disadvantage that the traditional PSO algorithm is easy to fall into the local optimal value, and improves the convergence speed and precision by reducing the number of iterations and enhancing the local search ability.

【技术实现步骤摘要】
一种基于粒子群优化算法的机器人逆运动学求解方法
本专利技术涉及一种机器人逆运动学求解方法，尤其是涉及一种基于粒子群优化算法的机器人逆运动学求解方法。
技术介绍
串联机器人的逆运动学求解是机器人轨迹规划和控制等方面研究的基础。只有通过运动学逆解把空间位姿转换为关节变量，才能实现对机器人末端执行器按空间位姿进行编程控制。机器人运动学逆解有数值法、几何法和代数法等，在满足pieper准则(即机器人的三个相邻关节轴交于一点或三轴线平行)后，利用六个转换矩阵相互关系求得每个轴角度θ。一个常用的数值方法是将6R串联操作臂各关节的D-H参数中的径向参数ai、ɑi和轴向参数si、θ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)+C1·rand()·[pBest(t)-x(t)]+C2rand()·[gBest(t)-x(t)](4)x(t+1)＝x(t)+v(t+1)(5)v(t+1)是粒子下一时刻的速度；x(t+1)是粒子下一时刻的位置；C1和C2是学习因子，通常C1＝C2＝2；W为惯性权重，一般取惯性权重∈[0,1]，pBest(t)为当前最佳个体极值，gBest(t)为当前最佳全局极值。PSO算法中适应度函数的定义非常重要，本专利技术中为给定目标点Ptarget与第t次搜索到的关节变量解所对应的点Pt之间距离的范数，以此为例，搜索的目标是使该范数最小的转换矩阵以及此矩阵的六轴的角度，取norm(Ptarget-Pt)最小值即为搜索到的最适合矩阵，其中Ptarget与Pt中的姿态矩阵(左上3*3矩阵)取反正弦函数以增加姿态矩阵的权重，以Ptarget＝[0090357；0-9000；9000357；0001]为例，其中pos(t)为由六轴的角度组成的粒子。仅使用PSO算法的迭代时本文档来自技高网...

【技术保护点】
1.一种基于粒子群优化算法的机器人逆运动学求解方法，其特征在于，包括以下步骤：1)根据机器人的关节参数建立运动学模型，得到运动学正解公式，设定目标点的位置和姿态矩阵，确定适应度函数；2)初始化粒子种群，计算初始种群的个体适应度值，获得个体最优值和全局最优值；3)使用随迭代次数而改变的动态惯性权重，更新粒子种群的位置和速度，计算新的适应度值，并与个体最优值和全局最优值进行比较，判断是否更新，若是，则返回步骤3)，若否，则进入步骤4)；4)对粒子种群进行遗传变异操作，计算适应度值，并与步骤3)中适应度值比较，判断是否接受该适应度值，若接受，则以该适应度值为当前迭代的适应度值；若不接受，则以步骤3)中未遗传变异之前的适应度值为当前迭代适应度值；5)根据适应度值和迭代次数判断是否满足终止条件，若是，则结束，得到机器人在该姿态下六轴最优角度，若否，则返回步骤3)。

【技术特征摘要】
1.一种基于粒子群优化算法的机器人逆运动学求解方法，其特征在于，包括以下步骤：1)根据机器人的关节参数建立运动学模型，得到运动学正解公式，设定目标点的位置和姿态矩阵，确定适应度函数；2)初始化粒子种群，计算初始种群的个体适应度值，获得个体最优值和全局最优值；3)使用随迭代次数而改变的动态惯性权重，更新粒子种群的位置和速度，计算新的适应度值，并与个体最优值和全局最优值进行比较，判断是否更新，若是，则返回步骤3)，若否，则进入步骤4)；4)对粒子种群进行遗传变异操作，计算适应度值，并与步骤3)中适应度值比较，判断是否接受该适应度值，若接受，则以该适应度值为当前迭代的适应度值；若不接受，则以步骤3)中未遗传变异之前的适应度值为当前迭代适应度值；5)根据适应度值和迭代次数判断是否满足终止条件，若是，则结束，得到机器人在该姿态下六轴最优角度，若否，则返回步骤3)。2.根据权利要求1所述的一种基于粒子群优化算法的机器人逆运动学求解方法，其特征在于，所述的步骤1)中，适应度函...

【专利技术属性】
技术研发人员：吕亚辉，么娆，严雨灵，
申请(专利权)人：上海工程技术大学，
类型：发明
国别省市：上海,31