基于改进粒子群算法的工业机器人轨迹规划*

韩顺杰 单新超 于爱君 符金鑫

(①长春工业大学电气与电子工程学院，吉林 长春 130000；②北京首钢国际工程技术有限公司，北京 100043)

随着智能化制造在社会发展中的地位越来越重要，机器人成为智能化发展过程中不可或缺的关键因素，如何让机器人更加智能、安全、可靠地运行显得尤为重要[1-2]。轨迹规划是机器人技术中的重要组成部分，也是一直以来机器人运动学中研究的热点问题。轨迹优化能进一步改善规划轨迹的性能，满足实际需求，缩短机械臂运动时间，减少能量消耗，同时能够避免不必要的抖动及冲击，增加平稳性[3-4]。

国内外学者对轨迹规划的相关研究论文已有很多，文献[5-6]采用三次多项式函数进行轨迹规划，结构简单，易于实现，但其二阶导数不连续导致机械手运行不稳定；文献[7]提出了5-7-5次分段多项式与复合型法相结合的方式，提高了效率，降低了能耗，但运算相对复杂，高阶次更容易引起“龙格”效应的产生；文献[8]以抑制机械振动为目标，将遗传算法与多项式轨迹规划相结合；文献[9]采用权重系数法构成目标函数，并采用粒子群算法对五自由度机械臂进行运动规划。以上研究虽然解决了传统算法难以优化高次多项式的问题，但并未考虑智能算法的收敛速率以及陷入局部最优的问题。

本文以三菱RV-4FL-D机械手为研究对象，以机器人运动空间的轨迹和分拣作业的实际工况提出了4-3-4多项式函数对机械手进行运动轨迹规划，使其计算过程简单，各关节的位置、速度、加速度连续可控。标准的粒子群算法存在前期收敛速度慢，后期容易陷入局部最优等问题，本文采用改进的粒子群算法，将标准粒子群中的学习因子由定值改变为可以根据环境动态调节的变量，改进后的粒子群算法前期可以快速收敛，后期可以准确收敛到最优解。实验证明，将4-3-4多项式函数与改进的粒子群算法相结合的轨迹规划方法有效提高了求解的速度与精度，并且保证了效率与稳定性的综合最优。

1 多项插值函数的构造

为了保证机械手在轨迹规划中的稳定性，其位移、速度、加速度随时间的变化曲线都应是连续可控的，常用的高次多项式函数为3次、5次、7次多项式，则分别需要对运动位置施加至少4个、6个或8个约束条件[10]。3 次多项式插值是关节空间中最基本的插补方法，3次多项式插值能够保证位移和速度连续，但未使用加速度约束，所以不能保证加速度连续，可能会导致关节加速度突变产生冲击。5次多项式能使加速度连续光滑，但加加速度存在明显突变。7次多项式虽然可以保证加加速度连续，但高阶次会导致凸包性过差，并且计算量过大。

为了使机械手在作业中保证运动轨迹整体连续并增加效率，根据实际工况需要，将机械手的工作区间分为三段，采用4-3-4多项式插值函数，其中在第一、第三阶段采用4次多项式进行插值，第二阶段采用3次多项式插值。

4-3-4多项式插值位移函数为：

(1)

式中：i=1,2,3，…,n,n为关节数，q1i、q2i、q3i为第i个关节对应的第一、第二、第三区间的位置。其约束条件分别为初始位置、速度、加速度，路径点1、2的位置，路径点的位置、速度和加速度保持连续，终点的位置、速度和加速度共14个。式(1)中所有的系数可以由上述的约束求出，如式(2)～(4)所示：

(2)

q=[q10 0q2q20 0q3

q30 0q40 0]T

(3)

(4)

式中：

2 改进的PSO算法

在关节空间中采用4-3-4多项式函数进行机械手运动轨迹规划，可以得到机械手在各段端点处的位置、速度、加速度的运动轨迹连续并且可控。但由于在构造分段多项式函数的各工作区间的时间间隔ti1、ti2和ti3是提前预设的，没有对任意时刻的运动学参数进行约束，所以不能保证机械手整体的运动平稳、减小冲击。传统的优化方法对于高次多项式而言并不适用，需要采用智能算法对适应函数进行优化。本文在使优化目标满足运动学约束的基础上，采用改进的优化算法，在完成指定任务的基础上增加机械手的效率和平稳性。

2.1 运动学约束与适应度函数

(5)

本文优化的目的是使机械手在完成任务的基础上增加效率和稳定性，所以将运行时间最短作为适应度函数，即：

f(t)=min(ti1+ti2+ti3)

(6)

2.2 改进的PSO优化算法

在标准的PSO优化算法中，粒子的特征用位置、速度和适应度值来表示，速度代表了粒子在不断迭代中发生的位移，适应度值判定解的质量。在粒子向更优的特征不断迭代中，个体极值和全局极值也在不断更新，更新公式如下：

(7)

(8)

式中：xid为粒子位置，i代表的是粒子群中的一个任意粒子，d代表维度；vid代表上述粒子在相应位置处的速度；pid表示的是上述粒子在该条件下个体的最优位置；pgd是当前条件下全局最优位置。ω为惯性权重；非负常数c1、c2为学习因子,r1和r2为[0,1]上均匀分布的随机数。

标准PSO算法运算简单、高效且搜索速率高，但易陷入局部最优[11]。根据式(7)可知，当c1=0时，剩余为社会认知部分，收敛速度较快，但容易陷入局部最优；当c2=0时，剩余为个体认知部分，收敛速度较慢。为了更高效地获得全局最优解，本文采用一种动态的方式给c1和c2赋值，在搜索初期，个体信息的意义大于全局信息，所以c1的取值大于c2，搜索后期为了让更加快速准确地收敛于全局最优，c2的取值大于c1的取值，构造的函数如下：

(9)

式中：t为当前迭代次数，Tmax是最大迭代次数。

改进的粒子群算法对机械手关节i进行轨迹优化的步骤如下：

(1)将由m个粒子组成的种群初始化，以随机的方式得到粒子的速度和位置。

(2)将m个粒子的参数代入式(2)～(4)，求出14个参数aij代入式(1)并对其求导，将得到的当前速度与约束进行比较，如满足约束，则根据适应度值判断粒子的优劣；如不满足约束，在选择最优粒子时会将该粒子排除，并且未满足约束的粒子在不断迭代过程中会逐渐向最优值前进，直到满足约束。

(3)找出每一个粒子自身到目前为止搜寻到的最优位置作为Pd;找出所有粒子当前的最优位置，作为Gd。

(4)根据式(7)、(8)更新粒子的速度和位置，重新构成具有新的位置和速度的m个粒子组成的种群。

(5)如满足迭代终止条件，则计算结束，得出最优适应度值，否则返回步骤(2)。

设定初始粒子数M=50，初始位置为0～1 s随机位置，飞行速度为[-2，2]，惯性权重ω=0.6，r1、r2为[0,1]上的随机数，迭代次数为50。

3 建模仿真实验

本文以三菱RV-4FL-D机器人为研究对象，实验平台如图1所示。采用标准D-H法建立运动学坐标系模型：以机械手各关节旋转轴轴线方向为坐标zi向，轴zi与zi-1的公垂线方向为xi向，yi通过右手定则确定[12]。依据该方法其具体坐标系如图2，得到的D-H参数如表1。

表1中αi表示扭曲度，ai表示杆件长度，di表示杆件偏移量，其中L1=170 mm，L2=280 mm，L3=60 mm，L4=350 mm。应用MATLAB仿真软件内Robotics Toolbox工具箱对其建模[13-14]，如图3所示。

表1 RV-4FL-D机器人D-H参数

在笛卡尔坐标系下给定机械手的初始位置、路径点1的位置、路径点2的位置、终点位置，见表2。

表2 笛卡尔空间路径

通过逆运动学，将初始点、路径点、终点由笛卡尔空间表示方式转化为关节空间表示方式，如表3。

表3 各关节的关节空间差值点

基本PSO算法的最优粒子适应度曲线与改进的PSO的适应度曲线对比如图4所示。

由图4可看出，改进后的PSO算法在前期的收敛速度更快，且适应度值小于基本PSO算法约20%，由此可见改进后的算法效果更优。

采用改进的PSO算法对4-3-4分段多项式函数进行以时间为目标的优化，关节1的最优粒子位置迭代过程如下：

对于其他关节也同样采用上述方法进行寻优，从而得到各个关节的运行最优时间，如表4。

表4 各关节优化结果

为了保证机械手的各个关节以相同的时间到达目标位置，因此选用所有关节的各段插值时间的最大值，机械手运行时间为T=1.997 4 s。关节1～6的位置、速度和加速度运动轨迹变化曲线分别如图6～8所示。

由以上仿真结果分析可得，采用改进后的轨迹规划方法能够缩短整个机械臂的运行时间，并且保证规划出来的位移、速度、加速度曲线在整个运动过程中的连续性在一个较理想的范围内，从而使得在缩短时间的前提下还能继续保证整个机械臂的稳定运行。

4 结语

本文提出的轨迹优化方法综合考虑了机械手的效率和平稳性指标，提出了4-3-4次多项式插值进行拟合，大幅减少计算的同时，还保证了机械手运行中的位置、速度、加速度连续可控，解决了文献[5-6]加速度不连续带来的冲击和文献[7]计算复杂，易产生“龙格”现象的问题。相较于采用遗传算法[8]和基本粒子群算法[9]，改进的PSO算法有效地解决了高次多项式难以用传统方法优化的问题，动态调节的学习因子可以快速准确地收敛到最优解。结果表明，采用4-3-4分段多项式进行轨迹规划结合改进的PSO算法对机械手运动轨迹进行优化，轨迹运行时间较只进行多项式轨迹规划缩短了约40%，较基本PSO轨迹优化缩短了约20%，各关节运行过程平稳，达到了预期的优化目标。