基于改进PSO算法的时间最优机械臂轨迹规划

李 虎，刘泓滨

(昆明理工大学机电工程学院，昆明 650500)

0 引言

随着社会现代化和工业自动化的发展，6自由度机械臂凭借着在工作空间中快速、稳定和高效的工作特点，被大量地使用在工业自动化生产过程中，例如焊接、搬运、喷漆、码垛和机械加工等方面[1-2]。实际应用表明，为了提高工业生产和制造效率，对机械臂的运行轨迹进行合理规划是至关重要的。目前,已有大批的国内外专家和学者在围绕机械臂轨迹规划中的时间最优、能量最优、冲击最优以及综合最优等方面展开了大量的深入研究与探讨，而其中6自由度机械臂时间最优轨迹规划问题则是深入研究与探讨中的热门话题。

时间最优轨迹是指6自由度机械臂在关节空间下利用最短时间完成相应路径的轨迹。对6自由度机械臂轨迹规划的智能算法有很多种，如PSO算法、遗传算法、模拟退火算法和萤火虫算法等其他智能算法，但是由于粒子群算法具有结构简单、参数少和快速搜索能力等优点被广大学者采用在机械臂的轨迹规划中[3]。冯斌等[4]采用标准的粒子群算法和高次多项式插值的方式对机械臂轨迹规划，保证了机械臂的各个关节可以平稳运行，但是标准粒子群优化机械臂轨迹容易陷入局部最优使其精度不佳。肖仁等[5]利用量子粒子群算法和多项式插值构造待优化函数，有效地解决了插值轨迹阶次高和凸包性等问题。韩顺杰等[6]提出一种正弦函数动态调整学习因子的改进粒子群算法并结合3-4-3分段多项式对工业机器人的运行轨迹进行优化，避免了粒子群初期容易陷入局部极值。郭彤颖等[7]采用粒子群算法和B样条曲线对机械臂进行轨迹规划，有效地提高了机械臂的工作效率。陈波等[8]提出一种双种群粒子群算法对机械臂轨迹进行时间最优规划，每个路径点之间通过3次多项式进行连接，该方法在机械臂时间最优轨迹规划中起到了重要作用。田恒等[9]对传统的粒子群算法的学习因子进行了改进，提出综合考虑学习因子的影响并且动态分配学习因子的大小，该方法具有更快的收敛速度。黄超等[10]提出改进粒子群算法中惯性因子，有效地增强了粒子群算法的搜索精度。以上研究在机械臂轨迹规划中取得了大量的成果，但是大多存在阶次高、计算复杂和优化算法易早熟等问题。

综上所述，本文以UR106自由度机械臂为研究对象，提出了一种以时间最优为目标，采用改进PSO算法对6自由度机械臂轨迹进行优化的方法。首先，在关节空间下利用机械臂正逆运动学原理获取其轨迹插值点；然后，为了使机械臂能够快速平稳地到达目标位置，采用3-5-3多项式对其轨迹进行插值；最后，使用改进PSO算法对多项式插值构造的轨迹进行优化，实现6自由度机械臂时间最优的轨迹规划并在MATLAB软件验证该方法的有效性。

1 UR10机械臂的运动学模型与仿真

1.1 机械臂的正运动学分析

UR10协作机械臂是一种6自由度串联机械臂，机械臂的各个关节均为转动关节，该机械臂的额定载量为10 kg，最大工作半径范围为1300 mm。该机械臂的3维模型如图1所示，连杆坐标系如图2所示。

图1 UR106自由度机械臂 图2 UR10机械臂连杆坐标系

坐标系{o}为机械臂基座参考系，连接基座底面中心，坐标系{oi}为第i个连杆的坐标系。由连杆坐标系创建D-H表，如表1所示。

表1 UR10机械臂D-H表

其中，ai-1为沿着xi-1轴，从zi-1移动到zi的距离；αi-1为绕着xi-1轴，从zi-1旋转到zi的扭角；di为沿着zi轴，从xi-1移动到xi的位移；θ为绕着zi轴，从xi-1旋转到xi的转角。

根据D-H表中的参数建立UR10的正运动学方程，2个相邻关节之间坐标系的转换通式为：

(1)

将UR10机械臂D-H表中的参数带入式(1)中可得各关节坐标系的变换矩阵：

(2)

(3)

(4)

(5)

(6)

(7)

在已知各关节的位姿和姿态时，联立式(2)～式(7)可得该机械臂末端关节相对于基坐标系{o}的位姿和姿态：

(8)

式中，n、o和q分别为机械臂位姿矩阵的分量；p为机械臂的位置分量。

1.2 UR10机械臂逆运动学分析与仿真

逆运动学就是通过求解机械臂末端关节相对于基坐标系的位姿和姿态，从而求得各个关节的转角。本文使用MATLAB软件中的Robotics Toolbox工具箱进行机械臂的正逆运动学求解。

在机械臂的工作空间内，选取机械臂各关节角为q=[-π/2,0,π/2,0,0,0]代入式(8)中得到机械臂末端关节相对于基座标系的齐次变换矩阵为：

(9)

由式(9)可知机械臂末端关节的位姿和姿态，在MATLAB机器人工具箱内调用qi=ikine(r,T)函数进行逆运动学仿真，可以得UR106自由度机械臂的关节角的矢量为qi=[-1.5708,0.0000,1.5708,0.0000,0.0000,0.0000]，由仿真结果可知将qi转换为角度制，即q=qi。因此验证了在工具箱中建立的机械臂模型是正确的。

2 UR10机械臂轨迹规划

2.1 分段多项式插值函数的构造

为了确保机械臂在工业生产中运行的平稳性和准确性，其位置、速度和加速度都是连续可控的，通常使用插值的方式对机械臂的运行轨迹进行插值，常用的插值多项式函数有3次和5次多项式。使用3次多项式对轨迹进行插值是一种基本的插值方法，但是采用该多项式函数插值只能保证机械臂各关节的位置和速度连续，无法使各关节的加速度连续可控。5次多项式插值是机械臂轨迹规划中最常用的插值方法，它可以保证机械臂各关节的位置、速度和加速度连续，但是每一段轨迹都使用5次多项式插值其计算量大，且阶次过高会导致凸包性过差等问题。因此本文采用3-5-3分段多项式对机械臂的运行轨迹进行插值，在一定的约束条件下，该多项式既可以保证各关节位置、速度和加速度的连续，也可以减小计算量和避免凸包性过差等问题。

3-5-3分段多项式函数的表达式为：

(10)

式中，qij为机械臂第i个关节第j段的多项式轨迹；aijk为机械臂第i关节第j段插值多项式函数的第k个未知系数；t1、t2、t3分别为机械臂每个关节第1段、第2段和第3段的运动时间。

机械臂各个关节通过起始点β1，路径点β2、β3和终止点β4时，β1和β4处的速度和加速度均为0，其他插值点重合处的速度和加速度应相等。因此利用上述的约束条件可以求得3-5-3分段多项式的各个系数[11]。

(11)

b=[0 0 0 0 0 0β40 0β10 0β3β2]

(12)

a=A-1b=[A1A2A3]T

(13)

式中，

2.2 基于改进粒子群算法的轨迹优化

以时间最少为目标，利用改进PSO算法对6自由度机械臂的运动轨迹进行规划，选取每个关节完成每段轨迹所用的时间t1，t2，t3构成一个三维的粒子群，其适应度函数表达式为：

F=t1+t2+t3

(14)

根据机械臂的实际工作环境以及机械臂自身运动学和动力的物理极限，可以确定机械臂各关节的物理约束条件为：

(15)

式中，Qimax、Vimax和Aimax分别为机械臂各关节运动过程中允许的最大位置、速度和加速度。

PSO算法是一种模拟鸟群捕食活动的智能算法，它凭借着参数少、收敛迅速、结构简单和易控制等特点，在处理优化问题中更加快速便捷[12]，但是传统的PSO算法容易受到待优化函数的影响提前收敛到局部最优。在每次迭代过程中，粒子群中的所有粒子都必须从当前的位置根据其更新的速度向新的位置移动。

其粒子的速度和位置更新公式为[13]：

(16)

(17)

标准粒子群算法中的参数是固定不变的。ω描述的是粒子的惯性，在进化前期ω应该更大一些，保证每个粒子能够独立飞行充分搜索空间，后期应该小一点，多向其他粒子学习。c1和c2分别描述的是个体和全局的最大飞行步长，前期c1尽可能的大一些，后期c2应尽可能的大一些，这样就可以平衡粒子的全局搜索能力和局部搜索能力。3个参数共同影响了粒子的飞行方向，假设其他粒子找到了更好的位置，但是当前粒子的惯性太大不能很快地飞向更优的位置，这就有可能导致算法易早熟或者是算法后期收敛缓慢等问题。

针对PSO易早熟且后期收敛速度慢等问题，本文对粒子群算法进行以下几点改进：

(1)采用混沌序列初始化代替PSO中的随机初始化粒子的种群。PSO中随机初始化易导致种群初始化分布不均从而影响算法的优化。混沌序列初始化种群时具有遍历性、随机性和均匀性的特点，是在规定的范围内根据本身规律不重叠地遍历整个搜索空间。因此，混沌序列初始化的种群可以有效地提高PSO算法的求解的精度和收敛速度。本文采用典型的混沌序列非线性映射[14]，其表达式为：

(18)

(2)提出一种自适应调整惯性权重的方法。常用的线性递减的惯性权重对POS跳出局部最优有一定作用，但是无论粒子的适应度是否变得更好，惯性权重都会根据迭代次数以线性速度递减。所以依靠线性递减惯性因子的粒子群算法有时也会陷入局部最优。自适应调整惯性权重综合考虑了粒子的迭代次数和粒子的目标值，初期使用较大的ω增强粒子的全局搜索能力，当有粒子飞行至最优点附近时，则减小ω增加粒子的局部搜索能力，这样对PSO优化的结果有较大提升。自适应调整惯性权重的表达式为：

(19)

式中，ωmax、ωmin为惯性因子的最大值和最小值；Tmax为最大迭代次数；f为当前粒子的目标值；favg为平均目标值。

(3)将局部和全局的学习因子c1和c2联合起来。当增大c1时，会增强粒子的个体认知能力，但是收敛速度缓慢；当增大c2时，会增强粒子的社会认知能力，收敛速度迅速，但是极易使算法早熟。为了保证PSO算法的搜索能力和协调粒子的个体认知和社会认知能力，采用动态调整学习因子的方法，将学习因子的取值与粒子当前的迭代次数联系起来。在搜索初期粒子个体的搜索意义大于全局的搜索意义，所以c1>c2；在搜索后期为了更加快速地获得全局最优解，应增加粒子与种群之间的交流，所以c1

(20)

式中，c1max、c1min、c2max和c2min为c1、c2的最大值和最小值。

改进PSO算法对6自由度机械臂轨迹优化的具体步骤如下：

步骤1：机械臂每个关节3段轨迹的运动时间(t1、t2、t3)构成一个三维的搜索空间，采用混沌初始化产生N个粒子(共有3N个粒子)构成种群，并初始化种群中各粒子的位置和速度。

步骤2：将初始化产生的N个三维时间组合带入式(11)～式(13)，求得各分段函数的系数。

步骤3：判断机械臂各关节3段运动轨迹的速度和加速度是否满足最大约束条件，若3段都满足约束条件，则将3段轨迹的运行时间带入式(14)计算其适应度值，若3段中任意1段的速度或加速度不满足约束，则赋予一个很大的值作为该粒子的适应度值，以便在下一次迭代中淘汰该粒子。

步骤4：经过k次迭代后，通过比较个体极值和全局极值的适应度值，选出适应度最佳的粒子，即为最优极值。

步骤5：当机械臂所有关节的轨迹都完成优化时，每段时间取各关节完成该段轨迹所需要的最长时间，进而确保机械臂的所有关节都能完成轨迹的运行。

6自由度机械臂轨迹优化的具体流程如图3所示。

3 仿真实验与结果分析

为了验证上述方法的正确性和有效性，本文以UR106自由度机械臂为实验对象，在MATLAB软件中采用3-5-3分段多项式构造关节轨迹，再使用改进PSO算法以时间最优为目的对关节轨迹进行优化。机械臂各关节插值点的位置信息如表2所示。

表2 各关节位置信息

在仿真实验中，根据改进PSO算法对6自由度机械臂轨迹进行优化，对所需要的参数进行设定，初始粒子数N为50；初始位置为0.1～4 s之间的随机数；总迭代次数Tmax为60；c1，c2的最大值和最小值分别为2和0.5；惯性权重ω的最大值和最小值为0.9和0.4；控制映射参数μ为3；混沌序列的初值x0为0.6。优化后可得各关节适应度值变化曲线如图4所示。

图4 改进PSO算法的适应度变化曲线

为了确保6自由度机械臂各关节能够快速平稳地运动到指定的位置，在起始位置和终止位置之间插值两个路径位置，并使用3-5-3分段多项式连接每段轨迹且起始点和终止点的速度和加速度均为0，每段轨迹重合处的速度、加速度相等。由仿真实验结果可知，机械臂各关节的位置、速度和加速度变化曲线如图5～图10所示，优化后机械臂各关节的时间分量如表3所示。

图5 优化前各关节位置曲线 图6 优化前各关节速度曲线

图7 优化前各关节加速度曲线 图8 优化后各关节位置曲线

图9 优化后各关节速度曲线 图10 优化后各关节加速度曲线

表3 优化后时间分量

采用改进PSO算法优化6自由度机械臂的轨迹后，机械臂各关节轨迹有了明显的改变。由图5～图10可知：与优化前机械臂各关节轨迹相比，机械臂各关节的位置轨迹和加速度更加平滑，速度更加快速，完成运动轨迹所用的时间也有了明显的减少。由表3机械臂各关节优化后的时间分量可知：为了确保机械臂各关节都有充足的时间完成相应的轨迹，取每个关节完成相同段轨迹所用的时间中的最大值为该段机械臂的运行时间，机械臂完成3段轨迹所用的时间从3 s减小到2.1 s，有效地提高了机械臂的运行速度和工作效率。6自由度机械臂在3维空间内运动轨迹如图11所示。

图11 机械臂运动轨迹

4 结论

本文提出一种改进PSO算法对6自由度机械臂轨迹进行优化。为了解决机械臂各关节在运行过程不稳定，位置、速度和加速度出现突变等问题，采用改进PSO算法对3-5-3分段多项式插值的每段轨迹进行优化。在满足机械臂运动学约束的前提下，机械臂完成整段轨迹所用的时间从3 s减少到2.1 s，整体时间相对于PSO算法优化前缩短了近30%，不仅缩短了机械臂的运行时间还提高了机械臂的工作效率。因此验证了基于改进PSO算法的时间最优机械臂轨迹规划的方法是有效的。