一种机器人关节空间轨迹规划方法与流程

本发明涉及机器人智能控制技术领域，具体涉及一种以时间最优为目标的机器人关节空间轨迹规划方法。

背景技术：

随着工业4.0的到来，智能制造设备在工业生产和日常生活中正起着越来越重要的作用。机器人作为智能制造设备中的重要组成，广泛地应用于医疗、汽车制造、焊接、曲面抛光等领域，缓解了劳动力不足的压力，这要求工业机器人有很高的工作效率和运行稳定性。对机器人关节空间的轨迹进行合理的规划，能够提高机器人的工作效率，减小关节抖动，改善机器人运行的平稳性。现有的很多机器人关节空间轨迹规划方法不能保证关节运动过程中的平稳性，同时路径点间时间间隔还需要进一步的优化，机械臂的运动效率和平稳性亟待提高。

在现有技术中，有的轨迹规划方法虽然满足了关节空间轨迹曲线位置、速度、加速度的连续性，但是加加速度的连续性并没有得到保证，所以加速度轨迹曲线的光顺性不够，造成关节运动过程的抖动没有得到有效地控制，且没有对两路径点之间的时间间隔进行优化，造成了机械臂运动效率的低下。有的轨迹规划方法使用粒子群算法对相邻路径点的时间间隔进行优化，使机械臂的运动时间大大减少，但是由于粒子群算法本身存在容易陷入局部最优解的缺陷，所以轨迹的时间间隔有待进一步的优化。因此，现有的机器人关节空间轨迹规划方法普遍存在轨迹加加速度曲线不连续的问题，关节运动过程当中抖动问题未能得到很好地解决。同时智能控制算法用于优化轨迹时间间隔时算法的全局搜索能力仍有待提高，需要新的机器人关节空间轨迹规划方案。

基于上述现有技术中的问题，如何对机器人关节空间轨迹进行合理的规划，如何提高机器人的工作效率，减小关节抖动，改善机器人运行的平稳性，并且如何解决智能控制算法在优化轨迹时间间隔时全局搜索能力不足是目前与有待解决的问题。

技术实现要素：

本发明要解决的技术问题是如何对机器人关节空间轨迹进行合理的规划，如何提高机器人的工作效率，减小关节抖动，改善机器人运行的平稳性，并且如何解决智能控制算法在优化轨迹时间间隔时全局搜索能力不足，提供一种机器人关节空间轨迹规划方法。

本发明是通过下述技术方案来解决上述技术问题：

一种机器人关节空间轨迹规划方法，所述空间轨迹规划方法包括：

将机器人关节在笛卡尔空间起止点坐标转换为关节空间起止点关节角度，并对所述关节空间起止点之间插值得到所述关节空间中间点关节角度；

使用多项式插值函数对所述关节空间中的轨迹曲线进行插值；

构造时间最优适应度函数，并设置运动学约束；

在满足所述运动学约束下，使用pso-sa算法对所述轨迹曲线的时间间隔进行优化并输出所述时间间隔；

将所述时间间隔作为所述多项式插值函数的时间系数，并计算所述多项式插值函数的系数进而得到优化后关节轨迹曲线。

较佳地，使用matlab机器人工具箱对所述关节空间起止点之间进行插值。

较佳地，所述多项式插值函数的约束条件为：所述关节空间起止点和所述关节空间中间点的位置已知，所述关节空间起止点的速度，加速度，加加速度为零，所述关节空间中间点的位置，速度，加速度，加加速度及所述轨迹曲线的四阶导皆连续。

较佳地，当所述中间点个数为两个时，所述适应度函数为所述ti1，ti2，ti3分别为所述机器人关节的三段所述轨迹曲线对应的时间间隔。

更优地，所述运动学约束包括关节速度约束和关节加速度约束，分别为：

其中，为关节最大转速，为关节最大加速度。

较佳地，所述使用pso-sa算法对所述轨迹曲线的时间间隔进行优化并输出所述时间间隔包括：

初始化pso算法和sa算法的基本参数；

使用随机方式初始化m个粒子的速度和位置；

判断所述粒子的位置是否满足所述运动学约束，计算每个所述粒子的适应度值；

对比每个所述粒子的所述适应度值，求解所述粒子的个体最佳位置pib和种群历史最佳位置gb；

更新惯性权重值w和动态学习因子c1和c2；

更新所述m个粒子的速度和位置，并判断是否满足所述运动学约束；

根据改进的metropolis准则判断所述粒子是否接受新解；

判断所述粒子的所述个体最佳位置pib和所述种群历史最佳位置gb；

更新模拟退火算法的温度，使温度下降；

判断是否达到最大迭代，若小于最大迭代次数则返回所述更新惯性权重值w和动态学习因子c1和c2；否则退出迭代并输出所述时间间隔。

较佳地，当所述中间点个数为两个时，所述多项式插值函数的各多项式最高次数分别为5，7，5。

在符合本领域常识的基础上，上述各优选条件，可任意组合，即得本发明各较佳实例。

本发明的积极进步效果在于：保证了关节轨迹曲线位置、速度、加速度、加加速度的连续性，减少了关节的抖动；对轨迹的时间间隔进行优化，在满足运动学约束的前提下缩短了轨迹时间，在减少关节抖动的前提下提高了机械臂的运动效率。

附图说明

图1为本发明一种机器人关节空间轨迹规划方法一实施例中的轨迹规划流程图；

图2为本发明一种机器人关节空间轨迹规划方法一实施例中的使用pso算法和sa算法优化关节轨迹曲线时间间隔流程图。

具体实施方式

为了便于理解本申请，下面将参照相关附图对本申请进行更全面的描述。附图中给出了本申请的首选实施例。但是，本申请可以以许多不同的形式来实现，并不限于本文所描述的实施例。相反地，提供这些实施例的目的是使对本申请的公开内容更加透彻全面。

需要说明的是，当一个元件被认为是“连接”另一个元件，它可以是直接连接到另一个元件并与之结合为一体，或者可能同时存在居中元件。本文所使用的术语“安装”、“一端”、“另一端”以及类似的表述只是为了说明的目的。

除非另有定义，本文所使用的所有的技术和科学术语与属于本申请的技术领域的技术人员通常理解的含义相同。本文中在本申请的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本申请。本文所使用的术语“及/或”包括一个或多个相关的所列项目的任意的和所有的组合。

如图1所示为本发明的轨迹规划流程图：

步骤s01：将机器人关节在笛卡尔空间起止点坐标转换为关节空间起止点关节角度，并对所述关节空间起止点之间插值得到所述关节空间中间点关节角度；

在一个示例中，使用matlab机器人工具箱robotic_toolbox对所述关节空间的起始点和终止点进行插补，得到各关节两个中间点的关节角度。具体编程语句如下：

在matlab中使用mdl_puma560语句打开puma_560机器人在机器人工具箱robotic_toolbox中已经建立好的模型，所述puma_560机器人是六自由度机器人。使用q1＝p560.ikine6s(t1)语句将所述笛卡尔空间中起始点位置坐标t1通过逆运动学转换到关节空间下各关节的关节角度q1，所述q1是1行6列的矩阵。使用q4＝p560.ikine6s(t4)语句将所述笛卡尔空间中终止点位置坐标t4通过逆运动学转换到关节空间下各关节的关节角度q4，所述q4是1行6列的矩阵。使用q＝jtraj(q1,q4,4)语句在关节空间中对所述起止点进行插补，生成所述关节空间包括起止点和两个中间在内的4个路径点各自所对应的6个关节的关节角度，所述q是4行6列矩阵。

步骤s02：使用多项式插值函数对所述关节空间中的轨迹曲线进行插值；

在一个示例中，使用5-7-5多项式插补函数对各关节的三段轨迹曲线进行插值。关节i的三段轨迹多项式hi1(t)、hi2(t)、hi3(t)为：

hi1(t)＝ai15t⁵+ai14t⁴+ai13t³+ai12t²+ai11t+ai10

hi2(t)＝ai27(t-ti1)⁷+ai26(t-ti1)⁶+ai25(t-ti1)⁵+ai24(t-ti1)⁴+ai23(t-ti1)³+ai22(t-ti1)²+ai21(t-ti1)+ai20

hi3(t)＝ai35(t-ti1-ti2)⁵+ai34(t-ti1-ti2)⁴+ai33(t-ti1-ti2)³+ai32(t-ti1-ti2)²+ai31(t-ti1-ti2)+ai30

所述轨迹多项式存在如下约束条件：所述关节空间起止点和两个中间点的位置已知，所述关节空间起止点速度、加速度、加加速度为0，所述两个中间点位置、速度、加速度、加加速度、轨迹曲线的四阶导皆连续。通过所述约束条件求出5-7-5轨迹多项式的系数，从而确定所述5-7-5轨迹多项式。

所述轨迹多项式系数求法如下：

所述起始点位置为θi1：ai10＝θi1

所述起始点速度为0：

所述起始点加速度为0：

所述起始点加加速度为0：

所述终止点位置为θi4：

所述终止点速度为0：

所述终止点加速度为0：

所述终止点加加速度为0：

所述第一个中间点的位置为θi2：

ai20＝θi2

所述第二个中间点的位置为θi3：

ai30＝θi3

所述第一个中间点处速度连续：

所述第一个中间点处加速度连续：

所述第一个中间点处加加速度连续：

所述第一个中间点处轨迹的四阶导数连续：120ai15ti1+24ai14＝24ai24

所述第二个中间点处速度连续：

所述第二个中间点处加速度连续：

所述第二个中间点处加加速度连续：

所述第二个中间点处轨迹的四阶导数连续：

所述θi1、θi2、θi3、θi4分别表示关节i在四个路径点的关节角度。根据所述约束条件得到多项式系数矩阵a：

所述a矩阵为时间系数矩阵。

所述关节i的位置参数矩阵b为：

b＝[θ1,0,0,0,θ4,0,0,0,θ2,θ2,θ3,θ3,0,0,0,0,0,0,0,0]^t

通过式a＝a^-1·b求得所述关节轨迹多项式系数a矩阵，根据所述关节轨迹多项式系数a矩阵求得所述5-7-5轨迹多项式。

步骤s03：构造时间最优适应度函数，并设置运动学约束；

在一个示例中，所述适应度函数为所述适应度函数中ti1、ti2、ti3分别为关节i三段轨迹对应的时间间隔。所述步骤三中关节速度和加速度约束为：

所述分别表示关节的最大转速和最大加速度约束。

步骤s04：在满足所述运动学约束下，使用pso(particleswarmoptimization，粒子群优化)算法和sa(annealingsimulated，退火模拟)算法对所述轨迹曲线的时间间隔进行优化并输出所述时间间隔；

在一个示例中，如图2所示为本发明使用pso算法和sa算法对所述轨迹曲线的时间间隔进行优化并输出所述时间间隔的步骤：

初始化pso算法和sa算法的基本参数；

在一个示例中，初始化粒子群算法的最大迭代次数、种群中粒子的个数m,粒子的位置和速度边界、惯性权重的最大最小值等参数，初始化模拟退火算法的初始温度和温度衰减系数。

使用随机方式初始化m个粒子的速度和位置；

在一个示例中，初始化m个粒子的速度和位置。在所述粒子的位置和速度边界内随机产生粒子的初始位置和速度。分别对所述关节i的三段轨迹多项式hi1(t)、hi2(t)、hi3(t)求一阶导和二阶导，得到关节速度和加速度多项式。如果有所述任何一段关节实时速度或者加速度不满足所述运动学约束,则解不满足约束。

判断所述粒子的位置是否满足所述运动学约束，计算每个所述粒子的适应度值；

在一个示例中，如果所述粒子的位置满足运动学约束，粒子的适应度值使用所述适应度函数计算。如果所述粒子的位置不满足所述运动学约束，所述粒子的适应度值等于一个固定的较大值。

对比每个所述粒子的所述适应度值，求解所述粒子的个体最佳位置pib和种群历史最佳位置gb；

在一个示例中，将初始种群各粒子的位置设为该粒子的个体最佳位置pib，对比每个粒子的适应度函数值，适应度值最优的粒子位置为粒子种群历史最优位置gb。

更新惯性权重值w和动态学习因子c1和c2；

在一个示例中，所述性权重值w更新公式如下:

所述动态学习因子c1和c2更新公式如下:

所述wmax和wmin分别表示权重的最大最小值，分别取0.9和0.4。所述iter和itermax分别表示当前迭代次数和算法的最大迭代次数。

更新所述m个粒子的速度和位置，并判断是否满足所述运动学约束；

在一个示例中，所述粒子更新速度和位置的公式如下：

所述是粒子i第k+1次迭代时的飞行速度，所述是粒子i第k次迭代时在搜索空间中的位置。所述r1和r2是位于0和1之间的随机数。所述粒子的速度更新公式表明粒子会根据自身个体最佳粒子位置和粒子群的历史最佳位置改变飞行的速度，按照位置更新公式飞向更优的位置。

根据改进的metropolis准则判断所述粒子是否接受新解；

在一个示例中，使用模拟退火算法优化所述粒子群算法,在所述粒子群算法中引入模拟退火机制。在算法迭代过程中使用改进的metropolis准则判断每个粒子是否接受产生的新解，所述改进metropolis准则如下：

所述改进metropolis准则中const＝1表示粒子i上一代的解xi不满足关节运动学约束，那么接受产生的新解xi_new。如果上一代的解满足运动学约束条件，那么δf＜0时接受新解，δf≥0时以一定的概率接受新解。

判断所述粒子的所述个体最佳位置pib和所述种群历史最佳位置gb；

在一个示例中，对比当前迭代种群每个粒子的目标函数值和所述该粒子的个体最佳位置pib的目标函数值，所述粒子当前位置的目标函数值更优，则将pib更新为粒子的当前位置。将所述当前迭代种群每个粒子的适应度函数值和所述粒子群的历史最佳位置gb的适应度函数进行对比，如果该粒子的目标函数值更优，则将所述粒子群的历史最佳位置gb更新为该粒子的位置。

更新模拟退火算法的温度，使温度下降；

在一个示例中，在算法的迭代过程中按照公式t＝α×t更新温度，使温度下降。所述t为所述模拟退火温度，所述α为所述模拟退火算法的温度衰减系数。

判断是否达到最大迭代并输出所述时间间隔。

在一个示例中，在算法的迭代过程中不断判断所述pso算法和sa算法的迭代次数iter是否小于最大迭代次数，如果iter<itermax，那么更新迭代次数，继续循环迭代。如果iter≥itermax，那么退出循环迭代。

步骤s05：将所述时间间隔作为所述多项式插值函数的时间系数，并计算所述多项式插值函数的系数进而得到优化后关节轨迹曲线。

在一个示例中，将所述输出的时间间隔作为所述5-7-5轨迹多项式的时间系数，根据所述式a＝a^-1·b计算各关节轨迹多项式系数，得到所述各关节轨迹曲线，完成机器人关节空间时间最优轨迹规划。

虽然以上描述了本发明的具体实施方式，但是本领域的技术人员应当理解，这仅是举例说明，本发明的保护范围是由所附权利要求书限定的。本领域的技术人员在不背离本发明的原理和实质的前提下，可以对这些实施方式做出多种变更或修改，但这些变更和修改均落入本发明的保护范围。