基于改进粒子群算法的机械臂轨迹规划研究

隋 涛，姜 昊，孔刘君，蒋 强

(沈阳理工大学 自动化与电气工程学院，沈阳 110159)

轨迹规划是机器人学中最重要的问题之一[1]，也是机器人运动学研究的热点问题。目前轨迹优化常采用最短执行时间、最小能量和最小冲击等标准[2]，其中时间最优轨迹是当前学者们研究最多的方向。

为解决机械臂时间最优轨迹规划的问题，文献[3]通过五次非均匀B样条构造轨迹函数，采用改进的自适应遗传算法对运行时间进行优化，实现时间最优；文献[4]在满足关节角速度约束条件下，构造4-3-4多项式轨迹，利用改进粒子群算法实现时间最优；文献[5]在采用三次B样条插值构造轨迹规划的基础上，结合改进的粒子群算法达到时间优化的目的；文献[6]采用五次多项式插值法，在满足最大速度、最大加速度和最大冲击速度条件下，结合改进的模拟退火算法得到时间最优轨迹；文献[7]使用粒子群算法，以速度为约束条件，对构造的3-5-3多项式轨迹进行优化，得到最优时间；文献[8]采用五次多项式构造轨迹插值函数，结合改进的粒子群算法实现时间最优。

标准粒子群算法具有规则简单、易于实现等优点，但容易陷入局部优化的困境。为此，本文利用3-5-3多项式插值法构造机械臂的运动轨迹，以时间最优为准则，设定速度、加速度为约束条件，提出一种改进的粒子群算法，通过非线性函数改变学习因子，避免在应用标准粒子群算法时陷入局部优化的问题。

1 机械臂关节空间轨迹规划

本文采用3-5-3多项式插值法进行轨迹规划，在机械臂的工作空间中，选取4个可到达的路径点，包括起始点、终止点、路径点1和路径点2，构成4个关键点、3段轨迹，两点之间分别用3次、5次和3次多项式进行插值。设机械臂关节编号为i(i＝1，2，3，4，5，6)，4个路径点处的关节角度为θi0、θi1、θi2、θi3，构造某一关节的3-5-3插值多项式为

式中:h函数项的下角标中1、2、3表示三段轨迹；t是插值时间；b表示函数中的待求系数，其下角标对应为关节、轨迹和多项式系数序号。

约束条件分别为起始点和终止点处的关节角度、速度和加速度及路径点1、2的关节角度，路径点的关节角度、速度和加速度保持连续，根据满足的约束条件分别求得3-5-3多项式轨迹的14个系数，具体求解过程参见文献[9]。

2 时间最优轨迹规划问题描述

2.1 目标函数的描述

在机械臂的工作空间中选取4个可到达的路径点构成3段轨迹，每段轨迹各自的插值时间为ti1、ti2、ti3，总的运行时间用T表示，以时间最短为优化目标，即优化问题的目标函数为

2.2 约束条件的描述

对于时间最优问题，以三段轨迹的时间总和为目标函数，约束条件为速度、加速度。

(1)速度约束条件

式中:｜｜表示关节运动时的实时速度绝对值；为关节允许的最大关节速度。

(2)加速度约束条件

式中:｜｜表示关节运动时的实时加速度绝对值；为关节允许的最大关节加速度。

3 改进的粒子群算法

粒子群算法是一种基于种群的搜索算法。将随机生成粒子作为初始种群，每个粒子代表一个由速度和位置组成的潜在解，每个粒子根据目前获得的记忆更新其速度[10]。

假设在D维搜索空间中，每个粒子代表一个候选解，设第j个粒子位置为xj，xj＝(xj1，xj2，…，xjD)，速度为vj，vj＝(vj1，vj2，…，vjD)，粒子从初始到迭代结束搜索产生的个体最优解为Pj，Pj＝(pj1，pj2，…，pjD)。整个种群的最优解为Pg，随机初始化j个粒子，随机初始化粒子的位置和速度，更新粒子j在d维分量(1≤d≤D)中的位置和速度，迭代公式为

式中:k为当前的迭代次数；w为惯性权重；r1、r2取值为[0，1]的随机数；c1为局部学习因子；c2为全局学习因子；表示粒子在当前迭代次数下的速度和位置。

标准的粒子群算法容易陷入局部最优、迭代不收敛问题。为此，引入线性递减的惯性权重w为

式中:wmax为惯性权重最大值；wmin为惯性权重最小值；Nmax为允许的最大迭代次数。

本文将迭代公式中的学习因子c1、c2由固定值改为非线性变化的动态函数，分别为

式中cmax、cmin为学习因子的最大值和最小值。

在算法的早期阶段，粒子需要有较强的个体学习能力，所以局部学习因子c1较大，以加快粒子更新速度，更快找到最优位置；在算法迭代后期，需要较强的群体学习能力，全局学习因子c2增大，局部学习因子c1减小，以提高算法精度[11]，故c1为递减函数，c2为递增函数。

基于改进的粒子群算法时间最优轨迹规划的步骤如下。

步骤1:参数初始化。初始化种群位置为0.1~4上的随机数值，构成初始种群，并初始化粒子的速度。

步骤2:将初始化后得到的随机粒子带入到式(1)~(3)中，解出各关节轨迹函数的系数，并得到关节的每段轨迹方程。

步骤3:对各关节的每段轨迹方程求导得到各关节每段轨迹的关节角速度和关节角加速度方程，判断每段方程中的速度和加速度是否同时满足速度和加速度约束条件，若有任何一段不满足，则设此粒子的适应度值为无穷大，方便后续剔除该粒子。

步骤4:计算各粒子适应度，挑选出具有个体和全局最佳适应度值以及对应最佳位置的粒子。

步骤5:更新惯性权重与学习因子，利用迭代公式更新粒子的位置和速度，随着粒子位置的不断更新，计算粒子适应度，通过迭代，得到全局最优解，若满足最大迭代次数，则适应度值最小的粒子为最优解，算法结束，否则回到步骤2。

时间最优轨迹规划流程如图1所示。

图1 时间最优轨迹规划程序流程图

4 仿真分析

4.1 机械臂建模

本文以KUKA KR3 R540为研究对象，该机械臂D-H参数如表1所示。

表1 D-H参数表

利用Matlab中的机器人工具箱对该机械臂采用标准D-H参数法进行建模，连杆坐标系建立方式参考文献[12]，本文的基坐标系O0建立在地面上，O1~O6为各关节对应的连杆坐标系。O4、O5坐标系建立在同一原点处，O6坐标系建立在末端法兰盘上。机械臂D-H连杆坐标系如图2所示，始关节角度为(0，-90，0，0，0，0)，机械臂仿真图及路径点选取如图3所示。

图2 机械臂D-H连杆坐标系

图3 机械臂仿真图及路径点选取

在机械臂工作空间中选取4个能到达的路径点作为插值点，将选取的4个插值点从笛卡尔空间变换到关节空间，路径点处关节空间角度如表2所示。

表2 路径点处关节空间角度

4.2 时间最优轨迹规划仿真

对于传统的3-5-3构造插值多项式的方法，需输入指定的预设时间以求取多项式的系数，本文预设的3段插值时间均为4 s。

初始化种群规模为50的粒子群，迭代次数Nmax＝200；wmax＝0.9、wmin＝0.4；参考文献[9]，选取cmax＝2.5、cmin＝0.5，使c1、c2在[0.5，2.5]范围变化。

设定机械臂各关节速度约束条件为̇θmax＝[-100，100]，加速度约束条件设为＝50。

利用粒子群算法求解时间最优，分为3个策略进行比较。策略1:标准粒子群，w＝0.9，c1＝c2＝2；策略2:惯性权重根据式(7)变化，c1＝c2＝2；策略3:惯性权重根据式(7)变化，学习因子根据式(8)、式(9)变化。

以关节1为例，分别在策略1、策略2和策略3下进行适应度仿真，结果对比如图4所示。

图4 3种策略的适应度曲线对比图

根据仿真结果的对比可以明显看出，策略3相比策略1提高了收敛速度与精度，与策略2相比提高了收敛速度，验证了改进算法的有效性。

策略3优化下的各关节运行时间如表3所示。

表3 关节优化结果 s

在执行工作任务时，机械臂的所有关节在同一时间内运动，所以选取的是优化后各关节在每段轨迹中的最大值。最终的插值时间组合为(2.906 6，2.445 0，2.446 8)，总时间为7.798 4 s。

利用3-5-3多项式插值进行规划，优化前6个关节角度变化曲线如图5所示。

图5 优化前6个关节角度变化曲线

按照策略3优化后6个关节角度变化曲线如图6所示。

图6 优化后6个关节角度变化曲线

优化前6个关节角速度变化曲线如图7所示。

图7 优化前6个关节角速度变化曲线

策略3优化下各关节的角速度更快一些，优化后6个关节角速度变化曲线如图8所示。

图8 优化后6个关节角速度变化曲线

优化前6个关节角加速度变化曲线如图9所示。

图9 优化前6个关节角加速度变化曲线

策略3优化下各关节角加速度更快一些，优化后6个关节角加速度变化曲线如图10所示。

图10 优化后6个关节角加速度变化曲线

根据上述优化前后的仿真图对比，优化后的机械臂关节可以得到平滑、连续且时间最优的轨迹曲线，优化后比优化前各关节的速度和加速度峰值明显提高，表明提高了机械臂的工作效率。

5 结论

针对机械臂关节空间时间最优轨迹规划问题，提出一种改进的粒子群算法，通过对学习因子非线性变化及线性惯性权重合理匹配，实现机械臂轨迹时间优化。通过Matlab仿真实验可以看出:改进粒子群算法将机械臂的运行时间从预设12 s缩短至7.798 4 s，并可以使各关节的角度、角速度、角加速度轨迹连续平滑。