基于QPSO算法的机械臂多项式插值轨迹规划方法技术

本发明专利技术提供一种基于QPSO算法的机械臂多项式插值轨迹规划方法，利用QPSO算法对多自由度机械臂的3‑5‑3分段插值轨迹进行时间最优轨迹规划，其特征在于，包括以下步骤：步骤S1，构建机械臂多项式插值轨迹规划所需的目标函数；步骤S2，根据QPSO算法，根据粒子个体最优位置和粒子全局最优位置来对粒子群体进行迭代进化,从而选出具有全局最优适应度的粒子位置和其对应的适应度值。本发明专利技术能保证六自由度机械臂末端轨迹满足运行过程尽量平滑,末端位置、角速度和角加速度无突变的原则；其仿真结果的精度要优于双层PSO优化算法,收敛速度要比传统PSO优化算法快,计算过程所耗时间均比双层PSO算法与PSO算法少。

【技术实现步骤摘要】
基于QPSO算法的机械臂多项式插值轨迹规划方法
本专利技术涉及机器人控制
，尤其是一种基于QPSO算法的机械臂多项式插值轨迹规划方法。
技术介绍
轨迹规划是随着时间的推移在两种不同配置之间移动机器人,以便在履行机器人的约束的同时执行特定的任务.一定的配置需要机器人操纵器的一组关节角度和所有可能的关节角度集合称为配置空间,约束包含机器人的物理限制.它们包括几何约束,其可以用机器人关节角度(即关节角度的边界,避免与环境的碰撞)来表示.它们还包括运动学和动力学约束,包括关节角度的高阶时间导数(即关节速度,加速度等)。机械臂在工业操作中必须遵循一个重要的原则,即运行过程应当尽量平滑,应当避免其末端位置、速度和加速度的突变,而分段多项式插值法可以使得机械臂末端轨迹满足这一原则.常用的分段式插值法有较为普遍的3-3-3-3-3多项式插值法和王焱等提到的4-3-4多项式插值法,对于前者,其使用的是三项式插值法,由于阶次过低,易使得轨迹脉动不连续；于后者,其中两段使用了四项式插值法,多项式阶次越高公式转换计算量越大,需牺牲大量的运算时间.相比这两种方法,徐向荣等提出的3-5-3多项式插值法具有阶次高,轨迹无位置、速度和加速度突变的特点,且其前后两段为三项式插值,针对机械臂模型的复杂性,公式转换计算量可以得到大大的减少.然而基于多项式差值的轨迹规划具有阶次较高,没有凸包性等特点,难以利用传统的方法进行优化,而粒子群算法恰好能解决这一问题，李小为等针对六自由度机械臂,提出一种在速度约束下的PSO时间最优轨迹规划,但是PSO(粒子群算法)的目标值收敛时间和计算时间均较长,为了对粒子群算法收敛速度进行改善,周志勇等提出一种自适应双层粒子群优化算法,但运用于六自由度机械臂的时间最优轨迹规划时,其精度不佳,易于早熟从而达不到最优值。而QPSO(量子粒子群算法)较于一般PSO更适用于对工业机械臂的时间最优轨迹规划。
技术实现思路
本专利技术的目的是在于克服现有技术中存在的不足，提供一种一种基于QPSO算法的机械臂多项式插值轨迹规划方法，其解决了多项式差值的轨迹规划具有阶次较高,没有凸包性等特点,难以利用传统的方法进行优化的问题；并能保证六自由度机械臂末端轨迹满足运行过程尽量平滑,末端位置、角速度和角加速度无突变的原则；其仿真结果的精度要优于双层PSO优化算法,收敛速度要比传统PSO优化算法快,计算过程所耗时间均比双层PSO算法与PSO算法少。本专利技术采用的技术方案是：一种基于QPSO算法的机械臂多项式插值轨迹规划方法，利用QPSO算法对多自由度机械臂的3-5-3分段插值轨迹进行时间最优轨迹规划，其特征在于，包括以下步骤：步骤S1，构建机械臂多项式插值轨迹规划所需的目标函数；步骤S2，根据QPSO算法，根据粒子个体最优位置和粒子全局最优位置来对粒子群体进行迭代进化,从而选出具有全局最优适应度的粒子位置和其对应的适应度值。步骤S1具体包括：步骤S1.1，构建多项式插值函数；多自由度机械臂关节i的3-5-3多项式插值通式为：其中,Si1(t)、Si2(t)、Si3(t)分别为多自由度机械臂的关节i在ti1、ti2、ti3这三段差值时间所对应的角度变化轨迹，aij、bij、cij分别为三段多项式插值函数的第j个系数，j为0、1、2、3、4、5；步骤S1.2，已知条件与目标函数的转换；关节i的角度变化轨迹由3-5-3多项式分为三段，为保证三段轨迹相接处无位置、角速度和角加速度的突变，每段终点与下一段起点关节角度、角速度与角加速度相等，第一段的起点与第三段的终点的角速度、角加速度均为0，所以存在下式(2)、(3)、(4)；其中θi0、θi1、θi2和θi3已知，ti1、ti2、ti3分别为三段角度轨迹从各自的起点到终点所用时间；先进行两段三项式的转换，再进行五项式的转换；首先,将式(2)代入式(1)中，经过转换、消减，得到Si1(t)关于ai3的表达式，ti1关于ai3的表达式；同理,将式(4)代入式(1)中,分别得到Si3(t)关于ci3的表达式、ti3关于ci3的表达式；然后,根据现求得的条件,并将式(3)代入式(1),经过转换、消减,得到Si2(t)关于ti2、ai3、ci3与bi5的表达式；最后,根据需达到优化目标为时间最优轨迹规划，则适应度值为三段轨迹时间之和,求得目标函数为：fitness＝f(ai3,bi5,ci3)＝ti1+ti2+ti3(5)步骤S1.3设定约束条件；设定各关节的角速度与角加速度的最高值限制，以及ai3,bi5,ci3的取值范围。步骤S2具体包括：步骤S2.1，QPSO算法的基本思想包括：在一个D维的目标搜索空间中，一定范围(lowd,highd)内,d＝1,2,...,D,令k＝1,2,...,M,随机取M个粒子组成一个粒子群体X(h)＝{X1(h),X2(h),...,XM(h)},在第h次迭代，其中任意位置的粒子k表示为Xk(h)＝{Xk,1(h),Xk,2(h),...Xk,d(h),...Xk,D(h)},在当次迭代中,粒子群体中各粒子的个体最优位置表示为Pk(h)＝{Pk,1(h),Pk,2(h),...Pk,d(h),...Pk,D(h)}；该群体中，所有粒子的全局最优的粒子位置表示为Gk(h)＝{Gk,1(h),Gk,2(h),...Gk,d(h),...Gk,D(h)}，并且G(h)＝Pg(h)，其中g为处于全局最优位置的粒子下标,g∈{1,2,...,M}；在t时刻，粒子k的个体最优位置为：在t时刻,粒子k的全局最优位置为：gd为第d维的全局最优位置的粒子下标，Gd(h)为第d维的第h次迭代全局最优粒子位置；每次粒子更新时，将该粒子当前最优位置对应的目标函数值与全局最优位置对应的目标函数值进行比较，若前者更优，则前者代替后者为全局最优位置，粒子位置的更新公式为：其中，α为收缩扩-张系数；U(0,1)是随机变量；步骤S2.2，基于QPSO算法时间最优轨迹规划的实现；为了得到目标函数(5)的基于QPSO算法时间最优轨迹规划的实现；式(5)视为一个三维的适应度函数，即D＝3，令Xk(h)＝{ai3,k,bi5,k,ci3,k}；每一维各取M个粒子进行初始化，并置个体最优位置为当前粒子位置；每个粒子代入目标函数，计算每个粒子的适应度值，取最优适应度值对应的粒子位置作为初始的全局最优位置；迭代次数累加1，使用公式(11)对粒子位置进行更新，并求各粒子对应的适应度值；使用公式(9)和(10)，根据当前层粒子适应度值，分别更新粒子个体最优位置与粒子全局最优位置；当全局最优适应度值小于预设阈值或迭代次数达到最大预设次数，跳出循环；得到粒子最优位置为G，即全局最优位置，全局最优适应度值为f(G)；最后，将整个过程再重复数次，取全局最优位置和全局最优适应度值各自的平均值，得到与本专利技术的优点：对本专利技术的QPSO优化算法与粒子群算法PSO和双层粒子群算法就对六自由度机械臂的各个关节的三段轨迹时间最优值求解的效果进行了对比，仿真效果显示QPSO的收敛速度优于一般PSO,其精度比双层PSO和一般PSO好,计算耗时最少,故QPSO在解决复杂的寻优问题时其综合效果要优于一般PSO；而工业机械臂的精度、耗时对其作业效本文档来自技高网...

【技术保护点】
1.一种基于QPSO算法的机械臂多项式插值轨迹规划方法，利用QPSO算法对多自由度机械臂的3‑5‑3分段插值轨迹进行时间最优轨迹规划，其特征在于，包括以下步骤：步骤S1，构建机械臂多项式插值轨迹规划所需的目标函数；步骤S2，根据QPSO算法，根据粒子个体最优位置和粒子全局最优位置来对粒子群体进行迭代进化,从而选出具有全局最优适应度的粒子位置和其对应的适应度值。

【技术特征摘要】
1.一种基于QPSO算法的机械臂多项式插值轨迹规划方法，利用QPSO算法对多自由度机械臂的3-5-3分段插值轨迹进行时间最优轨迹规划，其特征在于，包括以下步骤：步骤S1，构建机械臂多项式插值轨迹规划所需的目标函数；步骤S2，根据QPSO算法，根据粒子个体最优位置和粒子全局最优位置来对粒子群体进行迭代进化,从而选出具有全局最优适应度的粒子位置和其对应的适应度值。2.如权利要求1所述的基于QPSO算法的机械臂多项式插值轨迹规划方法，其特征在于，步骤S1具体包括：步骤S1.1，构建多项式插值函数；多自由度机械臂关节i的3-5-3多项式插值通式为：其中,Si1(t)、Si2(t)、Si3(t)分别为多自由度机械臂的关节i在ti1、ti2、ti3这三段差值时间所对应的角度变化轨迹，aij、bij、cij分别为三段多项式插值函数的第j个系数，j为0、1、2、3、4、5；步骤S1.2，已知条件与目标函数的转换；关节i的角度变化轨迹由3-5-3多项式分为三段，为保证三段轨迹相接处无位置、角速度和角加速度的突变，每段终点与下一段起点关节角度、角速度与角加速度相等，第一段的起点与第三段的终点的角速度、角加速度均为0，所以存在下式(2)、(3)、(4)；其中θi0、θi1、θi2和θi3已知，ti1、ti2、ti3分别为三段角度轨迹从各自的起点到终点所用时间；先进行两段三项式的转换，再进行五项式的转换；首先,将式(2)代入式(1)中，经过转换、消减，得到Si1(t)关于ai3的表达式，ti1关于ai3的表达式；同理,将式(4)代入式(1)中,分别得到Si3(t)关于ci3的表达式、ti3关于ci3的表达式；然后,根据现求得的条件,并将式(3)代入式(1),经过转换、消减,得到Si2(t)关于ti2、ai3、ci3与bi5的表达式；最后,根据需达到优化目标为时间最优轨迹规划，则适应度值为三段轨迹时间之和,求得目标函数为：fitness＝f(ai3,bi5,ci3)＝ti1+ti2+ti3(5)步骤S1.3设定约束条件；设定各关节的角速度与角加速度的最高值限制，以及ai3,bi5,ci3的取值范围。3.如权利要求2所述的基于QPSO算法的机械臂多项式插值轨迹规划方法，其特征在于...

【专利技术属性】
技术研发人员：吴定会，肖仁，纪志成，韩欣宏，黄海波，张桐瑞，陶凯，方钦，
申请(专利权)人：江南大学，
类型：发明
国别省市：江苏,32