基于PSO算法的水处理罐搬运机器人时间最优轨迹规划

罗云云，田会方，吴迎峰

(武汉理工大学 机电工程学院，湖北 武汉 430070)

近年来，传统制造方式不断朝着智能制造转型，工业机器人的应用领域不断拓展[1]。由于工业机器人自身结构的复杂性以及应用场景的限制，如何获得高精度的运动轨迹成为机器人开发中的一个普遍难题[2-3]。

国内外众多学者对于机器人的轨迹优化都做了很多研究，文献[4]采用三次多项式函数对轨迹进行规划，计算简单，过程易于实现。文献[5] 对比了三次多项式和五次多项式插值方法对于机器人关节轨迹的规划效果，得出了五次多项式插值法更能够得到机器人在笛卡尔空间的平稳轨迹的结论。但是单纯的多项式插值方式并不涉及轨迹的优化，因此近年来引进智能算法对机器人轨迹进行优化成为趋势，其中对最优时间条件下的轨迹规划算法的研究最为广泛。文献[6]采用了改进萤火虫算法、文献[7]采用了遗传算法对机器人轨迹进行规划，但在对插值点进行编码时可能会造成计算量上的增加。而文献[8-9]与本文采用了相同粒子群算法进行轨迹优化，其中文献[8]采用了3-5-3分段多项式插值法，文献[9]则采用了4-3-4类型的分段方式。而在算法改进时，两篇文章均只对粒子群算法中的学习因子做了改进。虽解决了高次多项式函数难以优化的问题，并得到了运行时间更短的位置、速度、加速度曲线，但未对算法中的另一参数“惯性权重”加以研究。而惯性权重同样会对算法的迭代的速度和参数精度产生影响。

笔者以水处理罐搬运机器人为研究对象，依据机器人运动空间和搬运工件的具体情况，采用3-5-3分段多项式对其运动轨迹进行规划。针对标准粒子群算法所存在的前期局部收敛性差，后期收敛速度慢等问题，采用了改进的粒子群算法将标准粒子群算法中的学习因子和惯性权重由固定值改为随着迭代次数自适应变化的动态值[10-11]，有效弥补了标准粒子群算法的缺点，并保证了效率与稳定性的综合要求。

1 水处理罐搬运机器人运动学模型建立与分析

所研究的水处理罐缠绕生产线上的搬运机器人属于传统的六轴串联臂式机器人，具体结构如图1所示。前3个关节用于确定机器人末端执行器的位置，后3个关节用于确定其姿态。为了便于对机器人进行运动学分析，有必要建立机器人的运动学方程。采用改进D-H法建立连杆坐标系，并通过平移、旋转等方式得到关节坐标系间的转换矩阵。连杆坐标系如图1所示，D-H参数如表1所示。

图1 水处理罐缠绕生产线上的搬运机器人

表1 机器人D-H参数表

表1中αi-1为绕xi-1轴，从zi-1旋转到zi的转角；ai-1为沿xi-1轴，从zi-1移动到zi的距离；di为沿着zi轴，从xi-1移动到xi的距离；θi为绕着zi轴，从xi-1轴旋转到xi的转角(i=1,2，…，6)。

由D-H参数表可计算各轴间变换矩阵：

(1)

式中：i-1Ti为坐标系i到坐标系i-1的变换矩阵；cθi、sθi分别为cosθi、sinθi(i=1,2,…，6)。

将上述变换矩阵依次相乘，得到缠绕生产线机器人的综合变换矩阵0T6:

0T6=0T11T22T33T44T55T6

(2)

2 多项式插值函数构造

关节空间常用的轨迹规划方法包括多项式插值、样条插值和抛物线过渡的线性插值等。在工业机器人使用场景中，次数越高的多项式，加速过程越慢，整个运动过程中的平均速度越小，影响效率。单纯的三次多项式会导致加速度出现跳变的情况。而加速度曲线过于拟合又会导致机器人运行中出现速度抖动的龙格现象，相比较于一味地追求高平滑度而提高多项式阶次或复杂度的方法，笔者采用分段插值的方式来拟合机械臂的运动轨迹，满足所需的平滑插值效果，且插值过程易于理解，计算量相对较小。

假设机器人的点对点轨迹由多个段连接，中间路径点具有连续的速度和加速度。根据实际工况以及各个工位之间的位置约束将整段轨迹分成3段(时间间隔t1,t2,t3)，分别进行多项式插值拟合轨迹。笔者采用3-5-3多项式插值函数，即在第一和第三阶段使用3次多项式进行插值，第二阶段使用五次多项式插值。

在起始点与终点的3段轨迹中，关节j(j=1,2,…,6)的运动位置与时间t的关系式为：

当0≤t≤t1时

qj,1(t)=aj10+aj11t+aj12t2+aj13t3

(3)

当t1

qj,2(t)=aj20+aj21(Δt2)+aj22(Δt2)2+

aj23(Δt2)3+aj24(Δt2)4+aj25(Δt2)5

(4)

当t1+t2

qj,3(t)=aj30+aj31(Δt3)+aj32(Δt3)2

+aj33(Δt3)3

(5)

式中：待求解系数aj1i、aj2i、aj3i(j=1，2，…，6)分别为第j个关节在第1, 2, 3段插值函数的第i个系数；qj1(t)、qj2(t)、qj3(t)分别为第j个关节第1，2，3段的多项式轨迹函数，Δt2、Δt3分别表示第二段第三段的时间变量。

用Xji表示第j个关节插值的位置，i=0， 1，2，3为插值点序号。给定轨迹规划的约束条件：①各个关节在每一分段的起始点角度值Xj0、中间路径点角度值Xj1，Xj2和终点角度值Xj3；②起始点和终点的速度、加速度；③各个路径点的速度和加速度连续。

根据上述约束条件，可推导各段插值函数中未知系数与插值点间的关系，并使用矩阵形式加以表示。

(6)

(7)

a=A-1·b=[aj10aj11aj12aj13aj20aj21aj22aj23aj24aj25aj30aj31aj32aj33]T

(8)

式中:A为关系矩阵；b为常量矩阵；a为系数矩阵。

3 基于粒子群算法的时间最优路径规划

所要优化的目标是满足水处理罐搬运机器人运动学约束的所有关节的最短运行时间。其函数表达式为：

(9)

式中：f(t)为优化目标函数同时也为算法的适应度函数；tj1,tj2,tj3为关节j的各段路径用时。

为了使所规划的最优轨迹是合理的，必须考虑机器人每个关节的运动学约束，包括最大角度、角速度和角加速度，因此，约束条件如下：

(1)角度约束

|qji(t)|≤qjmax

(10)

式中:qji(t)为第j(j=1,2,…,6)个关节在第i段路径下的实时角度；qjmax为第j个关节的最大角度。

(2)速度约束

(11)

(3)加速度约束

(12)

3.1 改进PSO算法

对于传统的多项式规划算法，机器人运动时间只作为已知条件参与运算，没有优化空间。对关节轨迹进行时间最优规划多采用智能算法。粒子群算法(particle swarm optimization, PSO)源自鸟类捕食问题的研究，相较于其他的智能优化算法，粒子群算法具有结构简单、易于实现、参数少、求解全局最优解效率高等优点。鉴于此，许多学者已将其应用于机器人的轨迹规划中。

PSO算法的基本思想是：在可行解空间初始化一定数量的粒子，每个粒子代表优化问题的一个可能最优解，每个粒子根据自身的当前最佳位置pbest和群在空间中的当前最佳位置gbest确定其接近最佳区域的飞行路线。算法使用位置、速度和适应度值3项指标来表示各个粒子的特征。粒子在可行解空间中，通过跟踪个体极值和群体极值更新个体位置。粒子每更新一次位置，适应度值随之被计算和更新。然后比较适应度值的大小，从而更新个体极值和群体极值的位置。

在每次迭代中，每个粒子的位置和速度按式(13)和式(14)更新：

(13)

(14)

为了克服基础粒子群算法存在的问题，平衡全局和局部搜索能力，得到良好的收敛效果，引入自适应惯性权重和自适应加速常数，对粒子群算法进行改进。

(1)自适应惯性权重。惯性权重ω过大或过小都会影响迭代的速度和参数精度。自适应变权在迭代早期使用较大的权重值来提高粒子的移动速度和全局搜索能力，在迭代后期使用较小的ω来降低粒子的移动速度同时专注于局部搜索，从而提高粒子的精度。本文使惯性权重呈非线性递减，即随着迭代次数k的增加，ω非线性减小。

(15)

式中：ωmin和ωmax为预设的最小与最大惯性系数，一般ωmin取0.4，ωmax取0.9；系数α、β>0；k为当前迭代次数；K为总迭代次数。

(2)自适应学习因子。c1、c2作为学习因子，使粒子具有自我总结和向优秀个体学习的能力，从而使得粒子向着领域中的最优点靠近。c1、c2分别调节粒子向个体最优和群体最优靠近的最大步长，决定微粒个体经验和群体经验对微粒自身运行轨迹的影响。学习因子较小时，可能使微粒在远离目标区域内徘徊；学习因子较大时，可使微粒迅速向目标区域移动。将算法中的学习因子改为自适应变化参数，使得粒子在优化早期尽可能在整个搜索空间中移动，并在优化后期提高接近最优解的收敛速度。在基本算法的基础上改进c1、c2，使c1、c2随着迭代次数的增加非线性变化，其变换公式如下：

(16)

(17)

式中：τ为控制系数；cmax、cmin为学习因子的最大值和最小值，按照经验分别设置为4和0；系数α、β1、β2>0。

用改进的惯性权重和学习因子代替基础粒子群算法中的原有项参与算法运算。

在本文的优化问题中，未知数分为两部分：①3-5-3多项式的未知数，共14个；②每段多项式的插值时间，共3个。若将3-5-3中的未知数作为自变量，时间作为因变量，则粒子群算法的解算维度为14，计算量过于庞杂，不利于工程实现。为降低运算量，直接将待优化的对象时间作为自变量，将粒子群解算维度降为3维。未知数t1、t2、t3根据生产节拍设置其搜索空间大小。优化粒子群算法求解最优时间的迭代步骤如下：

步骤1初始化粒子群，在ti取值范围 [0,1.8]内，随机产生M(M=50)个粒子(每个粒子均由t1、t2、t3组成)，构成初始种群，并初始化粒子的位置和速度。

步骤2将M个粒子群分别带入式(6)、式(7)求解出各段多项式的系数，确定各段的运动方程。

步骤3根据求得的3段运动学方程以及粒子的参数计算适应度，并且判断位置、速度和加速度是否满足约束条件，若每段中的速度或加速度都满足则按照式(13)、式(14)更新位置与速度，否则将粒子适应度值设为无穷大，以便后续去除。

步骤4更新当前粒子和相应位置的历史最佳适应度，以及群体的最佳适应度。

步骤5经过n次迭代后，适应度最小的粒子即为最优解。

在完成所有关节的时间优化后，取各关节在每段运动中的最大时长作为最优解，以确保机器人总体都能满足运动学约束条件。

3.2 优化结果分析

取水处理罐缠绕生产线机器人在生产线中将水处理罐芯模从水平旋转台搬运至缠绕机的一段工作轨迹进行仿真验证。

关节1～6的初始位置、中间路径点和终点的笛卡尔坐标转换到关节空间的角度值如表2所示。表2中，Xi(i=0,1,2,3)为插值点的编号。

表2 关节空间的角度插值点

在Matlab中，结合3-5-3分段多项式轨迹插值方法和改进粒子群算法编写时间最优轨迹规划程序，经过100次迭代后得到优化结果。图2为群体适应度的优化过程，图3～图5分别为第一个关节优化前后的位置、速度和加速度曲线。

图2 群体适应度优化曲线

图3 关节1优化前后位置曲线

图4 关节1优化前后速度曲线

图5 关节1优化前后加速度曲线

从图2可知，随着迭代的增加，各关节达到目标位置的时间明显减少并最终趋于稳定。从图3～图5可知，算法很好地缩短了运行时间并且保证了运行的平稳性。将优化后的各关节运行时间加以记录，结果如表3所示。

表3 各关节最优时间表

为了确保机器人各关节同时到达目标点和中间路径点，选择各个关节中插值时间的最大值,即t1=0.629 3、t2=1.248 7、t3=1.123 5。由此，机器人在优化所得的时间内各关节位置、速度和加速度曲线分别如图6～图8所示。

图6 优化后的位置变化曲线

图7 优化后的速度变化曲线

图8 优化后的加速度变化曲线

由以上仿真实验可知，改进的轨迹规划方法可以有效缩短机械手的运行时间，同时保证规划的位移、速度和加速度曲线的连续性，从而保证机械手的稳定运行。

4 结论

为提高水处理罐缠绕生产线机器人的工作效率进而提高整体生产效率，笔者提出了基于改进粒子群算法的时间最优轨迹规划方法。该方法综合考虑了机械手的运行效率和运动的稳定性，将3-5-3分段多项式插值拟合方法与改进的粒子群优化算法结合。在保证了机械手位置、速度和加速度连续，避免了不连续加速度引起的碰撞问题，以及高阶插值拟合方式可能出现的速度“龙格现象”的同时，采用粒子群算法解决了传统方法难以优化高阶多项式的问题，动态地调整学习因子和惯性因子能够快速准确的收敛到最优解。仿真结果表明，采用3-5-3分段多项式结合改进粒子群算法对机械手运动轨迹进行优化，轨迹运行时间缩短了45%左右。并且各关节运行过程稳定，达到了预期的优化目的。