基于PSO-SA的机器人关节空间轨迹规划

谢能斌 辛绍杰

(上海电机学院机械学院 上海 201306)

0 引 言

随着工业4.0的到来，智能制造设备在工业生产和日常生活中正起着越来越重要的作用。机器人作为智能制造设备中的重要组成，广泛地应用于医疗、汽车制造、焊接、曲面抛光等领域，缓解了劳动力不足的压力[1]，这要求工业机器人有很高的工作效率和运行稳定性。

对机器人关节空间的轨迹进行合理的规划，能够提高机器人的工作效率，减小关节抖动，提高机器人运行的平稳性[2]。

采用传统的数学插补函数来描述关节位置与时间的函数关系，关节的位置、速度、加速度的连续性可以得到保证，避免了各关节的抖动，但是却无法优化关节运动时间，造成了效率的低下[3]。于是智能控制算法被运用到传统的多项式插补函数中优化运行时间，相关专家学者也进行了大量的研究与改进。国内学者采用遗传算法[4-5]和粒子群算法[6]以及对二者的改进对3-5-3轨迹多项式系数进行优化。文献[7-8]将差分进化算法和改进的CMA-ES算法应用到了机器人轨迹规划当中。同时，人工鱼群算法[9]和人工蜂群算法[10]也成功地运用到机器人轨迹规划当中，并且取得了不错的效果。

粒子群算法很适合求解带有约束的寻优问题，但算法存在容易早熟无法找到全局最优解的弊端[11]。本文使用模拟退火算法(SA)对粒子群算法(PSO)进行改进，将模拟退火机制引入到PSO中，同时对模拟退火算法中的Metropolis准则进行改进，增强算法的全局搜索能力。采用惯性权重非线性递减策略对PSO的全局搜索和局部搜索能力进行平衡。引入了动态学习因子，在前期加快算法全局搜索速度，在后期加快局部搜索速度。使用PSO-SA对5-7-5轨迹多项式的系数进行优化，在满足PUMA_560机器人运动学约束的条件下提高了机器人工作的效率。

1 关节空间轨迹曲线生成

1.1 中间点的生成

工业中，使用机器人将货物从起始点运送到目标位置是关节空间点到点轨迹规划问题。本文以6自由度的PUMA_560机器人作为研究对象，对机器人点到点轨迹规划算法进行验证。

已知起止点的位置坐标p1和p4分别为(856,-150,-94)和(350,-614,-502)，单位为毫米。使用逆运动学将起止点位置坐标转换成关节空间下的关节角度。通过以下程序生成两个中间点所对应的各个关节角度：

mdl_puma560

T1=transl(p1)

T4=transl(p4)

q1=p560.ikine6s(T1)

q4=p560.ikine6s(T4)

q=jtraj(q1,q4,4)

语句mdl_puma560打开MATLAB机器人工具箱Robotic_Toolbox中自带的PUMA_560机器人模型。语句T1=transl(p1)将位置坐标p1转换成位姿矩阵T1，语句T4=transl(p4)将位置坐标p4转换成位姿矩阵T4，其中T1和T4都是4×4的矩阵。语句q1=p560.ikine6s(T1)和q4=p560.ikine6s(T4)通过逆运动学求解起止点在关节空间下所对应的关节角度矩阵q1和q4，其中q1和q4都是1行6列的矩阵。使用q=jtraj(q1,q4,4)语句规划出两个中间点在关节空间下所对应的关节角度矩阵，其中q=[q1;q2;q3;q4]是4行6列的矩阵，q2和q3代表关节空间下两个中间点所对应的关节角度矩阵。

关节空间的路径点如表1所示，θij表示关节i在第j个路径点的位置，其中i表示关节的序号，j表示路径点的序号。

表1 关节空间路径点表

1.2 插补多项式的构造

机器人的每个关节在关节空间下有4个路径点，从而形成三段轨迹。当关节在每个路径点的关节角度已知时，采用5-7-5多项式插补函数对关节的三段轨迹曲线进行规划。相比于3-5-3多项式插补函数，5-7-5多项式插补函数不仅使轨迹曲线的位置、速度、加速度连续，而且保证了多项式轨迹加加速度的连续性，减小了关节运动过程中的冲击，提高了机械臂运动时的稳定性。5-7-5三段轨迹曲线表达式如下：

hi1(t)=ai15t5+ai14t4+ai13t3+ai12t2+

ai11t+ai10

(1)

hi2(t)=ai27t7+ai26t6+ai25t5+ai24t4+

ai23t3+ai22t2+ai21t+ai20

(2)

hi3(t)=ai35t5+ai34t4+ai33t3+ai32t2+

ai31t+ai30

(3)

式(1)-式(3)中的hi1(t)、hi2(t)、hi3(t)分别表示关节i的三段轨迹曲线表达式。

采用5-7-5多项式插补函数进行轨迹规划时有如下约束条件：已知关节i起止点和两个中间点的关节位置。保证起止点关节速度、加速度以及加加速度皆为零。两个中间点处关节的位置、速度、加速度、加加速度、轨迹的四阶导数皆连续。这样在已知关节运动时间的前提下，可以推导出各关节轨迹曲线的系数和各路径点关节角度的关系。

由式(4)-式(7)中的矩阵组成式(8)的时间系数矩阵A，式(9)中的b是已知的关节位置参数矩阵。通过智能控制算法优化关节运动时间，得到时间系数矩阵A。通过式(10)求得轨迹曲线的系数矩阵a，进而确定5-7-5轨迹多项式。

关节轨迹多项式的生成也为接下来判断智能控制算法优化的时间间隔是否满足运动学约束打下了基础。

(4)

(5)

(6)

(7)

(8)

b=[θ1,0,0,0,θ4,0,0,0,θ2,θ2,θ3,θ3,

0,0,0,0,0,0,0,0]T

(9)

a=A-1·b

(10)

2 基于PSO-SA的时间最优轨迹规划

2.1 适应度函数

本文主要研究在满足PUMA_560机器人运动学约束条件下，对关节运动时间间隔进行优化，适应度函数如式(11)所示。

(11)

(12)

2.2 粒子群算法速度及位置更新

粒子群算法源于对自然界生物种群觅食行为的模仿，是一种生物智能算法[12]。对于粒子群优化算法，待优化问题在N维空间的任意一个解被称为一个粒子。一个种群由m个粒子组成，在N维空间中粒子i的位置记为xi=(xi1,xi2,…,xiN)，粒子i的速度记为vi=(vi1,vi2,…,viN)。粒子与粒子之间通过相互协作寻找待优化问题的全局最优解。在PSO中通过目标函数值来判断粒子位置的好坏。粒子i的个体最佳位置为pib，粒子群的历史最佳位置为gb。粒子在N维搜索空间中向更优的位置飞行，粒子i的速度和位置更新如式(13)-式(14)所示。

(13)

(14)

2.3 惯性权重的设置

式(13)中w代表惯性权重的大小，在算法迭代过程中调整w值能平衡PSO的全局与局部搜索能力。采用惯性权重线性递减的方法使得粒子权重值随着迭代次数的增加线性递减，在算法的前期加强全局搜索，在算法后期加强局部搜索。

对于一些复杂的或者非线性的寻优问题，惯性权重线性递减的策略往往无法很好地平衡PSO的全局搜索和局部搜索能力。本文采用一种惯性权重非线性递减策略对全局搜索和局部搜索进行平衡。惯性权重非线性递减策略如式(15)所示，其中iter和itermax分别代表算法当前迭代次数和最大迭代次数。wmax和wmin分别是最大最小权重值，通过以自然常数为底数的幂函数使惯性权重从最大值非线性递减至最小值。

(15)

2.4 引入动态学习因子

式(13)中的c1和c2分别表示粒子的全局学习因子和局部学习因子，动态学习因子由式(16)和式(17)来更新。动态学习因子的引入使算法早期加快全局搜索，同时使算法后期加快局部搜索。

(16)

(17)

2.5 模拟退火算法基本原理

模拟退火算法在原解xi的邻域通过扰动随机产生新解xi_new，算法通过 Metropolis准则判断是否接受产生的新解[13]。Metropolis准则如式(19)所示，其中T0为模拟退火算法的初始温度，α为温度衰减系数，iter为算法的当前迭代次数，p表示接受新解的概率。根据Metropolis准则，在接受使Δf<0的新解的同时，以一定的概率接受使Δf≥0的新解，接受的概率会随着温度的降低而减小。

Δf=f(xi_new)-f(xi)

(18)

(19)

2.6 改进Metropolis准则

针对本文关于关节运动时间的优化问题，产生的新解需要满足机器人运动学约束，需要对Metropolis准则进行改进，增强算法的寻优能力。

改进的Metropolis准则如式(20)所示。

(20)

式中：const=1表示上一代的解xi不满足关节运动学约束，那么接受产生的新解。如果上一代的解满足运动学约束条件，那么Δf<0时接受新解，Δf≥0时以一定的概率接受新解。

2.7 PSO-SA的步骤

粒子群模拟退火算法具体步骤如下：

步骤1初始化算法的参数：粒子群算法的最大迭代次数itermax为500，种群规模的大小为20，粒子的飞行速度范围为-2到2，粒子的位置边界为0.2到4。将惯性权重w的最大最小值wmax和wmin分别设置为0.9和0.4。令当前迭代次数iter为1。模拟退火的初始温度T0设置为100 ℃，温度衰减系数α为0.8。

步骤2初始化粒子的速度和位置，粒子的位置是关节轨迹三段运行时间(ti1,ti2,ti3)。

步骤3将m组解代入式(10)，求出系数矩阵a。将系数矩阵a代入式(1)-式(3),求出关节轨迹多项式。对关节5-7-5轨迹多项式求导得到关节速度多项式，对轨迹多项式求二阶导得到加速度多项式，根据式(12)判断解是否满足运动学约束。

步骤4计算每个粒子的适应度值，如果粒子位置不满足式(12)的约束，那么粒子的适应度值设为12。如果粒子满足式(12)的约束，那么按照式(11)计算粒子的适应度值。

步骤5将m个粒子的初始位置设为每个粒子的个体最佳位置pib。比较m个粒子的目标函数值的大小，目标函数值最小的粒子位置设为粒子群的历史最佳位置gb。

步骤6根据式(15)更新惯性权重值w，根据式(16)和式(17)更新全局学习因子c1和局部学习因子c2。

步骤7根据式(13)和式(14)分别更新m个粒子的速度和位置，并判断每个粒子当前位置是否满足运动学约束。

步骤8按照式(20)改进的Metropolis准则判断每个粒子是否接受新产生的位置。

步骤9对比当前代种群每个粒子的目标函数值和该粒子的个体最佳位置pib的目标函数值，粒子当前位置的目标函数值更优，则将pib更新为粒子的当前位置。以步骤5的方式更新粒子群的历史最佳位置gb。记录每个粒子的当前位置。

步骤10按照公式T=α×T更新模拟退火温度，使温度下降。

步骤11判断迭代次数iter是否小于最大迭代次数itermax，如果iter

3 仿真实验和结果分析

3.1 5-7-5轨迹多项式的生成

本文以PUMA_560机器人作为研究对象，对机器人前3个关节进行仿真实验。机器人关节空间下起止点和两个中间点的关节角度如表1所示。

初始化关节运动时间间隔，令t1、t2、t3都等于3 s。根据式(8)到式(10)编写程序，得到5-7-5轨迹多项式。关节1到关节3的原始位置、速度、加速度轨迹曲线如图1至图3所示。

图1 关节1原始轨迹

图2 关节2原始轨迹

图3 关节3原始轨迹

由图1至图3可知，当t1、t2、t3取初始时间值时，关节的运动满足速度和加速度的约束，但是还没有达到关节的最大运行速度，关节运行效率低下。

3.2 基于PSO-SA的时间优化

使用PSO-SA对关节的运动时间进行优化，根据PSO-SA的执行步骤编写程序，得到优化后各关节的运动时间间隔如表2所示。从表2可知，经过优化后，关节1到关节3运行时间分别从9 s减少到了4.883 1 s、2.374 8 s、1.176 9 s，大大地提高了关节的运行效率。

表2 优化后时间对比 单位：s

图4 关节1优化轨迹

图5 关节2优化轨迹

图6 关节3优化轨迹

由图4-图6所示的关节轨迹多项式曲线可知，关节的位置、速度、加速度皆连续，而且加速度曲线有很好的光顺性，有利于关节的平稳运行，减小机器人关节的磨损。

图7为机械臂末端在笛卡尔空间的轨迹示意图，轨迹通过了起止点和两个中间点，同时轨迹曲线具有很好的光顺性，验证了使用5-7-5插补多项式规划机器人轨迹的正确性和有效性。

图7 机械臂末端笛卡尔空间轨迹示意图

3.3 传统PSO和PSO-SA优化时间对比

以关节1作为研究对象，分别使用传统PSO和PSO-SA对关节运动时间进行优化。由图8可知PSO-SA比传统PSO收敛速度更快，能在搜索空间搜寻到更短的轨迹时间，说明PSO-SA具有更强的全局搜索能力。

图8 PSO和PSO-SA优化时间对比

分别使用PSO-SA和PSO对关节1运动时间进行5次寻优，由表3可知PSO-SA的优化时间明显短于PSO。同时，PSO的5次优化时间不断变化，而PSO-SA的优化时间非常稳定，具有很强的稳定性。

表3 传统PSO和PSO-SA在优化时间上的对比 单位：s

4 结 语

本文针对工业机器人点到点轨迹规划问题，提出了一种基于PSO-SA的时间最优机器人关节空间轨迹规划方法。使用模拟退火算法优化粒子群算法，增强了PSO的全局搜索能力。使用惯性权重非线性递减策略和动态学习因子平衡了算法的全局搜索和局部搜索能力。实验表明PSO-SA相比于传统PSO能够得到更短的轨迹时间，具有更强的全局搜索能力，同时算法的稳定性更好。PSO-SA在满足运动学约束的前提下优化了关节的运动时间，提高了机械臂运行效率。5-7-5多项式插补函数规划关节轨迹曲线，保证了轨迹曲线位置、速度、加速度的连续性，增强了关节运动的稳定性，避免了抖动，延长了机器臂的使用寿命。