基于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；而工业机械臂的精度、耗时对其作业效率起到至关重要的作用,qpso较于一般pso更适用于对工业机械臂的时间最优轨迹规划。

附图说明

图1为本发明的算法流程图。

图2为本发明仿真的关节角度变化轨迹示意图。

图3为本发明仿真的关节角速度变化轨迹示意图。

图4为本发明仿真的关节角加速度变化轨迹示意图。

图5为本发明的三种算法迭代进化效果对比图。

具体实施方式

下面结合具体附图和实施例对本发明作进一步说明。

本发明提出一种基于qpso算法的机械臂多项式插值轨迹规划方法，利用qpso算法对六自由度机械臂的3-5-3分段插值轨迹进行时间最优轨迹规划,其在解决了多项式差值的轨迹规划具有阶次较高,没有凸包性等特点,难以利用传统的方法进行优化的问题。

步骤s1，构建目标函数；

qpso算法需根据目标函数确定每个粒子的适应度，然后才能根据每次迭代各个粒子适应度选出适应度最优的粒子位置；

步骤s1.1，构建多项式插值函数；

六自由度机械臂的轨迹规划可以将6个关节的角度轨迹规划于1个关节,即关节i进行考虑,i＝1，2，...,n,n＝6，根据3-5-3多项式插值法，在初始点和终点空间坐标确定情况下,引入两个中间点的空间坐标；基于机械臂模型,利用逆运动学求解,可以得到关节i的初始点、中间点1、中间点2和终点对应的关节角度si1(ti0)、si1(ti1)、si2(ti2)和si3(ti3)；关节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多项式分为三段，因为初始点、中间点1、中间点2和终点的空间坐标是已知的，所以三段轨迹的起点、终点角度也是已知的，为保证三段轨迹相接处无位置、角速度和角加速度的突变，每段终点与下一段起点关节角度、角速度与角加速度相等，第一段的起点与第三段的终点的角速度、角加速度均为0，所以存在下式(2)、(3)、(4)；下式中si1(t)、si2(t)、si3(t)上方一点表示求导(即角速度)，两点表示求导后再求导(即角加速度)；

其中θ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的取值范围；

考虑到运动学约束，六自由度机械臂各关节的角速度与角加速度受到最大速度vmax与最大加速度amax的限制，为保证机械臂作业的正常运行，应使得各关节角速度与角加速度都小于最高值，所以，有：

根据式(7),并考虑到t1、t2、t3均大于0,可以分别得到ai3,bi5,ci3的可取范围,有

步骤s2，qpso算法的路径规划；

qpso算法根据粒子个体最优位置和粒子全局最优位置来对粒子群体进行迭代进化,从而选出具有全局最优适应度的粒子位置和其对应的适应度值；qpso算法一开始的全局搜索能力强,而局部搜索能力相对较弱,随着迭代其全局搜索能力减弱，局部搜索能力增强；

步骤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}，粒子是否为最优，需代入目标函数fitness＝f(xk,1,xk,2,...,xk,d),其值越小越好；

在t时刻，粒子k的个体最优位置为：

在t时刻,粒子k的全局最优位置为：

gd为第d维的全局最优位置的粒子下标，gd(h)为第d维的第h次迭代全局最优粒子位置；

每次粒子更新时，将该粒子当前最优位置对应的目标函数值与全局最优位置对应的目标函数值进行比较，若前者更优，则前者代替后者为全局最优位置，粒子位置的更新公式为：

其中，α为收缩扩-张系数；

u(0,1)是随机变量；

步骤s2.2，基于qpso算法时间最优轨迹规划的实现；

为了得到目标函数(5)的基于qpso算法时间最优轨迹规划的实现，以六自由度机械臂中关节i为例，式(5)可以视为一个三维的适应度函数，即d＝3，令xk(h)＝{ai3,k,bi5,k,ci3,k},根据机械臂的实际情况设定vmax＝2πrad/s,amax＝4πrad/s²,并设定ai3,bi5,ci3的取值范围；如图1所示，图1中h为迭代次数，l为重复次数；

每一维各取m个粒子进行初始化，并置个体最优位置为当前粒子位置；m＝20；

每个粒子代入目标函数，计算每个粒子的适应度值，取最优适应度值对应的粒子位置作为初始的全局最优位置；

迭代次数累加1，使用公式(11)对粒子位置进行更新，并求各粒子对应的适应度值；

使用公式(9)和(10)，根据当前层粒子适应度值，分别更新粒子个体最优位置与粒子全局最优位置；

当全局最优适应度值小于预设阈值或迭代次数达到最大预设次数，跳出循环；得到粒子最优位置为g，即全局最优位置，全局最优适应度值为f(g)；

最后，将整个过程再重复10次，即l＝10，取全局最优位置和全局最优适应度值各自的平均值，得到与

六自由度机械臂建模与仿真；

选取的对象是较为典型的工业机械臂puma560机械手；利用d-h坐标系对puma560进行的运动学建模；利用matlabtoolbox建立机械臂模型,并在matlab中进行相关仿真；

工业机械臂puma560的六个关节角度变化轨迹如图2所示，六个关节的运行过程中，角度轨迹平滑；

通过对式(1)求导与二次求导可以得到六个关节的角速度变化轨迹和角加速度变化轨迹，分别如图3和图4所示，在各插值点，角速度与角加速度变化连续并具有一定的平滑度，故基于qpso算法的时间最优轨迹规划满足机械臂末端位置、角速度与角加速度无突变的原则。

本实施例在利用qpso优化算法对六自由度机械臂的各个关节的三段轨迹时间之和,即式(5)进行最优值求解的同时,也分别利用双层pso算法和pso算法对相同对象进行了同样的时间最优轨迹规划；均对20个粒子进行100次迭代,为了观察各个算法结果的精确度,再将整个过程循环10次,最终取10次结果的平均值；三种算法对6个关节最优时间的迭代进化效果进行对比，关节1的对比结果如图5所示；

本发明利用qpso算法对六自由度机械臂的3-5-3分段插值轨迹进行时间最优轨迹规划，其末端运行过程平滑,末端位置、角速度和角加速度无突变,在最优时间内轨迹效果良好；在利用qpso优化算法对六自由度机械臂的各个关节的三段轨迹时间最优值求解的过程中,与双层pso算法和pso算法进行了最优值求解效果对比，仿真效果显示:对六自由度机械臂的3-5-3分段插值轨迹进行时间最优轨迹规划时，qpso的收敛速度优于一般pso，其精度比双层pso和一般pso好，计算耗时最少；故qpso在解决复杂的寻优问题时其综合效果要优于一般pso；qpso算法能够较好的收敛于全局最优点,不易陷入局部最优点,具有较强的全局搜索能力和局部搜索能力,算法一开始的全局搜索能力强,而局部搜索能力相对较弱,随着迭代其全局搜索能力减弱,局部搜索能力增强，工业机械臂的精度、耗时对其作业效率起到至关重要的作用，qpso较于一般pso更适用于对工业机械臂的时间最优轨迹规划。

最后所应说明的是，以上具体实施方式仅用以说明本发明的技术方案而非限制，尽管参照实例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。