基于改进粒子群算法的机器人时间最优轨迹规划
时间：2023-03-23 12:20:05 来源：千叶帆 本文已影响人
邹 慧，周 虎，张骐薇，马玉环，钱盈锟
（1.东华大学 机械工程学院，上海 201620；
2.上海航天设备制造总厂有限公司，上海 201100）
轨迹优化[1]是机器人领域的热点问题，其任务是根据给定的目标轨迹点规划出通过指定点且满足速度、加速度等约束条件的最优运动轨迹。目前的轨迹优化方式主要包括时间优化和能量优化两种[2,3]，其中时间优化应用最广。
机器人时间优化问题中，粒子群算法（PSO）[4]以其结构简单、参数可调的特点在机械臂时间优化问题上得到广泛应用。文献[5]提出自适应反函数PSO算法对机器人双NURBS轨迹优化，但收敛速度较慢且粒子后期易在最优解周边振荡。文献[6]基于粒子群算法进行轨迹规划，通过时间、速度的双适应度函数之间切换控制，在满足运动学约束的同时优化了时间，但算法效率不太理想。文献[7]采用PSO、DE算法得到平滑运动轨迹的同时提高了机器人的工作效率，但无法保证全局收敛。
针对粒子群算法局部搜索能力差、易陷入局部最优解的不足，本文通过引入模拟退火算法（SA）的扰动机制及免疫算法（IA）的浓度选择机制，在选择粒子群最优位置时以一定突跳概率接受劣解以跳出局部极值，并在进行一定次数迭代后，若发现群体中最优的个体无明显变优时对不同浓度粒子给予抑制或促进。为验证算法的有效性，以PUMA560机器人为对象仿真并与常规粒子群算法进行对比，结果表明，混合免疫粒子群算法（HIPSO）能有效提高机器人工作效率，实现机器人时间最优轨迹规划。
为了实现机器人末端的平滑运动，采用3-5-3多项式插值函数规划轨迹。在起始点和终止点插入2个路径点，进而将其运动路径划分为三段进行插值，通过逆运动学方程逆解算可求解得到各关节在4个插值点对应的关节矢量值。将关节角之间的变换看作是关于时间的函数，3-5-3多项式插值函数的通式为：
式(1)～式(3)中lj1(t)、lj2(t)、lj3(t)为分别代表多项式的角位移函数，ajmn为第j个关节的第m段插值的第n个系数，t为插值的时间变量。
3-5-3多项式插值规划的前提是插值时间已知，根据各关节在起始点和结束点速度和加速度为零，且中间两过渡点的位移、速度、加速度相等的边界约束条件可推导出表示关节插值点与系数的关系矩阵A[8]，因为约束条件和约束边界只与时间t有关，通过对插值时间寻优，当最优时间确定后代入关系式可求解出多项式插值系数a，从而生成各关节时间最优运动轨迹。
本文对机器人末端轨迹优化的目标是在运动学约束的条件下，对每个关节进行时间寻优，使得总运动时间最短。
第i个关节适应度函数为：
分别为第个关节各段的插值时间。
对各关节的速度、加速度予以约束，使其不得超过最大值。
多项式插值轨迹规划的前提是插值时间已知，但传统方法难以对多项式插值进行优化，因此，本文在分段多项式插值的基础上，在速度和加速度约束下基于改进粒子群算法对关节空间的插值时间进行寻优搜索。
3.1 改进粒子群算法
粒子群优化算法中，把机器人关节运动时间看作搜索空间的粒子，每个粒子都有一个位置属性xi和速度属性vi，并由适应度函数f(t)来追寻最优时间。
3.1.1 模拟退火算法扰动机制
粒子群算法通过位置速度更新公式不断更新粒子的位置与速度：
其中，pij为单个粒子目前最优位置，pgj为当前粒子群最优位置，ω为惯性权重，c1、c2为学习因子，r1、r2为0-1之间的随机数。
粒子种群根据个体最优和全局最优位置调整下一步进化方向，但当pgj为局部最优时，所有粒子会受其影响而朝着局部最优运动，导致粒子群快速趋同，从而出现局部极值或停滞现象。
因此，在PSO算法的速度更新过程中，引入模拟退火算法的扰动机制[9]对当前次优粒子赋予一定突跳概率，适应度越接近当前最优粒子则突跳概率越大，于是替换原最优粒子，从而避免陷入局部最优。
随着迭代次数的增加退火温度逐渐降低，突跳概率逐渐降低，粒子趋向全局最优。根据Metropolis准则退火温度T时·pi相对pg的突跳概率为：
于是改进速度位置公式：
3.1.2 免疫算法浓度免疫机制
粒子群算法随着迭代次数的增加，收敛速度和精度逐渐降低，因此引入免疫算法浓度机制对群体中无明显变优的最优个体进行免疫，其核心思想[10,11]：用抗体粒子的浓度代表粒子群中相似抗体粒子的程度，用抗体粒子的亲和度代表在免疫系统中抗体粒子与抗原的相似程度，剔除高浓度低亲和力粒子，保留低浓度高亲和力粒子，从而改善粒子群寻优过程的收敛慢、精度低的问题。
通过设置精度和迭代次数，当到达指定迭代次数后或在一定精度范围内无明显变优时，则进行免疫：由Logistic映射新生成Q个抗体粒子，计算个体抗体粒子的浓度和被保留概率，将S个高亲和力低浓度的抗体粒子保留下来进入下一次迭代。
第i个抗体粒子的浓度公式为：
第i个抗体粒子的浓度概率选择公式：
式中f(xi)为第i个粒子的适应度值，可以看出抗体浓度越高，被保留的可能性越低，确保了粒子群的多样性。
3.2 混合免疫粒子群算法对关节时间优化
混合免疫粒子群算法结合免疫算法和模拟退火粒子群算法的优势，改善了粒子群算法易陷入局部最优的缺陷、加快了收敛速度和精度。将三段插值时间作为粒子的寻优位置，算法求解机器人各关节时间最优解步骤如下：
Step1：初始化参数：设置粒子数量为S=20，粒子维数D=3、学习因子c1=c2=2，惯性权重ω=0.5，初始温度T=100，降温系数k=0.90，最大迭代次数Max=100，免疫间隔迭代次数DS=8。
Step2：在各关节可达范围内由Logistic回归分析生成S个寻优粒子并初始化位置及速度，形成初始抗体粒子种群N0。
Step3：速度、加速度约束：通过初始化粒子的位置得到插值多项式并验证速度、加速度是否满足其约束条件，若满足则获取当前粒子群体N0中的粒子的适应度值，否则将其适应度值置为无穷大。
Step4：更新粒子位置和速度：采用Metropolis准则依照概率F(pi)接受劣解替换原最优粒子，更新粒子的速度和位置并进行退火操作。
Step5：判断若达到最高迭代次数，则输出最优解，终止迭代。
Step6：保持粒子多样性：每迭代指定免疫次数后检查最优粒子，若无明显变优则由Logistic映射产生Q个新抗体粒子开始进行免疫：计算生成S+Q个新抗体粒子的概率，并依照抗体概率浓度选择公式选择个抗体粒子形成抗体种群Ni并转入Step3。
混合免疫粒子群优化算法流程如图1所示。
图1 混合免疫粒子群算法流程
4.1 实验设计
本文以典型的关节空间机器人PUMA560为研究对象验证算法的可行性。其D-H参数如表1所示。
表1 PUMA560的D-H参数
为避免在求解过程中出现奇异位形及保证机械臂的运动可达性，采用正向运动学求解，在已知起点和终止点的基础上插入两个路径点，得到机器人运动轨迹的四个插值点如表2所示。
表2 插值点坐标
根据逆运动学解得各关节的起始点、路径点和结束点的角度值，其中前三个关节角度值如表3所示。
表3 关节序号
根据混合免疫粒子群算法，在速度的约束下对机器人的前三个关节进行优化，求解最优时间。
4.2 实验验证
通过MATLAB对各关节的收敛时间进行仿真，以机械臂的前三个关节在速度为90°/s、加速度为70°/s2时为例，使用常规粒子群算法及混合免疫粒子群算法分别对运动时间优化，生成在速度和加速度约束下的最短插值时间迭代图，获得混合免疫粒子群算法优化关节位置、速度、加速度曲线以实现对机器人时间最优轨迹规划。
由图2～图4所示混合免疫粒子群算法在30次迭代后开始收敛，与常规粒子群算法相比插值时间的收敛速度明显提升，且插值时间更短。100次迭代后得到插值时间分别为t1=1.5672s、t2=1.0436s、t3=1.8327s，取优化后的每段插值时间的最大值t3作为各段插值时间。
图2 关节1的时间收敛图
图3 关节2的时间收敛图
图4 关节3的时间收敛图
通过3-5-3多项式对关节角进行插值，得关节位置、速度、加速度曲线如图5-7所示，可知各关节角变化平滑，且关节速度、加速度满足最大约束无突变。
图5 关节位置曲线
图6 关节速度曲线
图7 关节加速度曲线
表4为两种算法对关节1进行多次仿真优化后的轨迹运动时间对比，HIPSO下的总时间由未优化的12s减少到优化后的4.5s左右，相较未优化前时间减少将近60%，相较常规粒子群算法时间减少近15%，较大提高了机器人的工作效率。
表4 算法优化结果
将改进算法运用到图9所示实验室的箱体打磨六自由度机器人的控制器中，其能够在速度约束下完成预期的运动轨迹并花费的时间更短，验证了算法能够有效减少六自由度机器人运行时间，提高加工效率。
图8 箱体打磨机器人
图9 打磨机器人控制柜
本文针对六自由度工业机器人空间关节时间优化问题进行了研究，提出混合免疫粒子群算法的3-5-3多项式插值轨迹优化方法。通过仿真实验进行了验证，结果表明优化后的运动轨迹不仅符合运动约束，且各曲线在路径范围内平滑过渡，使机器人在保证运行平稳的前提下提高了关节运动的效率，有助于提升机器人的性能。
猜你喜欢 插值轨迹粒子 碘-125粒子调控微小RNA-193b-5p抑制胃癌的增殖和侵袭昆明医科大学学报(2022年1期)2022-02-28轨迹读友·少年文学(清雅版)(2020年4期)2020-08-24基于膜计算粒子群优化的FastSLAM算法改进新疆大学学报(自然科学版)（中英文）(2020年2期)2020-07-25轨迹读友·少年文学(清雅版)(2020年3期)2020-07-24Conduit necrosis following esophagectomy:An up-to-date literature reviewWorld Journal of Gastrointestinal Surgery(2019年3期)2019-04-24基于Sinc插值与相关谱的纵横波速度比扫描方法西南石油大学学报(自然科学版)(2019年1期)2019-01-28轨迹现代装饰(2018年5期)2018-05-26基于粒子群优化极点配置的空燃比输出反馈控制浙江工业大学学报(2017年5期)2018-01-22混合重叠网格插值方法的改进及应用北京航空航天大学学报(2017年2期)2017-11-24进化的轨迹（一）——进化，无尽的适应中国三峡(2017年2期)2017-06-09 相关热词搜索：粒子，最优，机器人，
