CN109814496A - 一种s型加减速轨迹规划中多轴时间同步方法 - Google Patents

Abstract

本发明公开了一种S型加减速速度曲线插补的在线轨迹规划方法，即可以针对运动约束条件不同的初始状态和加速度等于零的结束状态可以对多轴进行时间同步轨迹规划。时间同步轨迹规划方法并不唯一，本发明以所有轴在运行过程中尽量保证中间段速度平稳为原则，进行多关节时间同步。

Description

一种S型加减速轨迹规划中多轴时间同步方法

技术领域

本发明涉及工业机器人运动控制及伺服电机速度同步控制领域，特别是涉及速度规划方法与机器人运动规划方面的一种S型加减速轨迹规划中多轴时间同步方法。

背景技术

在机器人运动过程中，一条运动指令往往有多轴同时参与运动，而多轴运动插补的最短时间取决于单轴运动规划时间最长的那个轴。多关节的同起同停会使机器人运动轨迹更加自然，如果将时间同步方法用于笛卡尔空间的路径插补，则中间段轨迹匀速在实际中广泛应用，例如机器人涂胶，焊接等。

传统的多轴时间同步轨迹规划方法往往只考虑所有轴初始运动状态互相成比例关系，末端运动状态和运动约束条件也是这种比例关系的情况。而本方法则拓展了传统的时间同步方法，考虑了任意初始运动状态、约束条件和加速度等于零的末端运动条件，应用场合更加广泛。本发明适用于在线实时生成轨迹，能够使机器人在运动状态下，针对未知事件能够及时做出反应，具有计算速度快精度高的特点，为以后机器人加入传感器并自主规划轨迹具有重要意义。

发明内容

针对现有技术存在的技术问题，本发明提供一种加减速轨迹规划中多轴时间同步方法，该方法适用于在线实时生成轨迹，能够使机器人在运动状态下，针对未知事件能够及时做出反应，具有计算速度快精度高的特点，为以后机器人加入传感器并自主规划轨迹具有重要意义。

为了克服传统多轴时间同步轨迹规划方法的不足，本发明解决其技术问题采用的方案如下：

一种S型加减速轨迹规划中多轴时间同步方法，包括如下步骤：

Step1，通过在关节同步时间t_sync内，对所有轴分别构造一种加速度分界曲线其中t∈[0，t_sync]；根据构造曲线的位移与实际每个关节的位移进行对比，进而生成初始各关节加加速度

Step2，不考虑加速度极限值并计算轨迹参数

Step3，考虑加速度极限值计算加速曲线类型Type_a；

Step4，判断加速度曲线类型Type_a是否等于TriZeroTri，如果满足条件，进入下一步；否则进入对粗略轨迹参数进行修正过程；

Step5，根据Step4的修正参数和插补周期Δt生成轨迹P_i(t)。

所述Step4步骤中对粗略轨迹参数进行修正过程：

2.1、经判断若加速度曲线类型Type_a等于TriZeroTri，对加速度曲线参数可以直接计算；

2.2、若加速度曲线类型Type_a不等于TriZeroTri，则判断加速度曲线类型Type_a是否等于TrapZeroTrap，如果等于则加速度曲线参数可以直接计算；

2.3、若加速度曲线类型Type_a不等于TrapZeroTrap，则需要对参数进行修正计算得到Type_a；经过参数修正后需要再次判断加速度曲线类型Type_a＝TrapZeroTrap？如果加速度曲线类型Type_a为TrapZeroTrap，则需要再次计算参数否则经过上面修正计算之后得到的参数即为最终参数，可以直接输入Step5。

所述Step5步骤中P_i(t)轨迹生成的过程：

首先根据Step3和Step4计算的轨迹参数Type_a可以直接计算加速度轨迹的关键参数然后由上面计算的参数可以直接得到各关节轨迹P_i(t)。具体如下：

第一段

第二段其中分别为第i关节第一段末端的位移，速度，加速度

第三段其中分别为第i关节第二段末端的位移，速度，加速度

第四段其中分别为第i关节第三段末端的位移，速度，加速度

第五段其中分别为第i关节第四段末端的位移，速度，加速度

第六段其中分别为第i关节第五段末端的位移，速度，加速度

第七段其中分别为第i关节第六段末端的位移，速度，加速度

与现有技术相比，本发明具有的优点：

本发明可以针对运动约束条件不同的初始状态和加速度等于零的结束状态可以对多轴进行时间同步轨迹规划；时间同步轨迹规划方法并不唯一，本发明以所有轴在运行过程中尽量保证中间段速度平稳为原则，进行多关节时间同步，本发明具有操作步骤简单计算速度快的特点，适合用于在线实时规划。

附图说明

图1为本发明一种S型加减速轨迹规划中多轴时间同步方法流程图。

图2为本发明中部分时间同步加速度曲线类型Type_a(横轴时间t，纵轴加速度A)示意图。

图3为本发明中关节时间同步轨迹测试结果。

具体实施方式

下面结合附图对本发明的技术方案作进一步说明。

如图1所示，本发明提供一种S型加减速轨迹规划中多轴时间同步方法，包括如下步骤：Step1，通过S型加减速单轴规划，分别计算规划每个轴的运动时间T_j＝[t_j1，t_j2，t_j3，...t_jn]选取所有轴规划的最大时间max(T_j)作为关节同步时间t_sync；进而生成各关节初始加加速度该步骤是计算边界加速度类型和初始加加速度J_s。

边界曲线构造：首先以最大加加速度将关节i的加速度由初始加速度降到零，然后再保持加速度等于零段。最后在同步时间t_sync约束条件下，再通过在某一时刻加速或减速来达到目标速度，将这样的加速度曲线作为边界加速度类型曲线

将关节i以边界加速度曲线所经过的位移与实际目标位移进行比较。若小于目标位移则初始加加速度等于若等于目标位移则初始加加速度等于边界加速度曲线对应的初始加加速度，且边界加速度曲线就是实际加速度曲线；否则，初始加加速度等于

Step2，不考虑加速度极限值并计算轨迹参数如图2所示，将速度曲线分成三部分，第一段加速部分、第二段匀速部分、第三段加速部分。该图包括了时间同步加速度轨迹的所有情况，其中前4幅图的共同特点是第一段初始加加速度和第三段初始加加速度符号相异。后4幅图表示了第一段初始加加速度和第三段初始加加速度符号相同。图2的8种情况可以分成4大类，1和5一类表示为TriZeroTri、2和6一类表示为TrapZeroTri，3和7一类表示为TriZeroTrap，4和8一类表示为TrapZeroTrap。其中TriZeroTri表示不存在匀加速段的所有情况；TrapZeroTri包含了只有第一段含有匀加速段的情况；TriZeroTrap包含了2段含有匀加速段的情况；TrapZeroTrap包含了第一段和第三段都含有匀加速段的情况。这4大类即可覆盖所有双S型加减速规划的所有情况。

通过位移、速度、加速度之间的微分关系可以建立非线性方程组，使用牛顿法对方程组进行求解，可以得到第一段加速时间第二段匀速时间第三段加速时间匀速段速度

Step3，考虑加速度极限值再次计算轨迹参数并确定加速曲线形状Type_a。为了计算加速度曲线的第一段和第三段加速部分的详细参数，首先假设第一段加速部分不存在匀加速段，通过第一段加速时间计算第一段加速曲线规划过程中达到的最大加速度如果最大加速度大于加速度极限值则存在匀加速度段和两段变加速段，否则不存在匀加速段，则第一段加速曲线只包括两段匀加速段。第三段加速曲线处理方法同第一段，不同的是第三段需要计算第三段开始的加加速度值通过匀速速度与首末速度相比较，若匀速速度大于始末速度中的最大值或小于始末速度的最小值，则等于否则等于

Step4，判断加速度曲线类型Type_a是否等于TripZeroTri，如果满足条件，进入下一步；否则进入对粗略轨迹参数进行修正过程；

1、经判断若加速度曲线类型Type_a等于TriZeroTri，对加速度曲线参数可以直接计算；

2、若加速度曲线类型Type_a不等于TriZeroTri，则判断加速度曲线类型Type_a是否等于TrapZeroTrap，如果等于则加速度曲线参数可以直接计算；

3、若加速度曲线类型Type_a不等于TrapZeroTrap，则需要对参数进行修正计算得到Type_a。经过参数修正后需要再次判断加速度曲线类型Type_a＝TrapZeroTrap？如果加速度曲线类型Type_a为TrapZeroTrap，则需要再次计算参数否则经过上面修正计算之后得到的参数即为最终参数，可以直接输入Step5。

由于Step3计算的轨迹参数不适用于存在匀加速度段Trap的情况，因此对于存在匀加速段的轨迹需要再次计算轨迹参数。如图2所示，其中TriZeroTrap、TriZeroTrap、TrapZeroTrap这3种加速类型曲线需要再次修正计算。

修正计算的方法为考虑匀速部分速度并不一定等于速度极限值，匀加速度部分的加速度等于加速度极限值，以位移作为约束条件，以速度和加速度的微分几何关系针对上面3种情况可以分别列写非线性方程组。利用牛顿法对方程组进行求解，最终得到修正后的轨迹参数，即第一部分加速段、第二部分匀速段、第三部分加速段的时间第二部分匀速速度第三段开始加加速度的值和加速度曲线类型Type_a。

Step5，根据Step4修正参数和插补周期Δt生成轨迹P_i(t)。

时间同步S型加速曲线主要包括3大部分：加速段、匀速段、加速段。具体可能包括加加速段、匀加速段、减加速段、匀速段、加减速段、匀减速段、减减速段。

轨迹曲线P_i(t)可以通过对加速度曲线A_i(t)进行2次积分得到。速度曲线V_i(t)可以通过对加速度曲线A_i(t)进行1次积分。加加速度曲线J_i(t)是对加速度曲线A_i(t)对时间进行1阶求导。

以轨迹曲线P_i(t)为例，首先根据Step3和Step4计算的轨迹参数Type_a，可以直接计算加速度轨迹的关键参数然后由上面计算的参数带入下面公式可以得到各关节轨迹P_i(t)。

P_i(t)具体如下：

第一段

第二段其中分别为第i关节第一段末端的位移，速度，加速度

第三段其中分别为第i关节第二段末端的位移，速度，加速度

第四段其中分别为第i关节第三段末端的位移，速度，加速度

第五段其中分别为第i关节第四段末端的位移，速度，加速度

第六段其中分别为第i关节第五段末端的位移，速度，加速度

第七段其中分别为第i关节第六段末端的位移，速度，加速度

通过以上5个主要步骤即可实现各关节的时间同步。

测试结果

输入以下参数进行测试，同步时间t_sync是由单轴时间最优规划而得，最终得到的多轴轨迹如图3所示。

J_max＝[2500，2500，2500，2500，2500，2500]

A_max＝[500，500，500，500，500，500]

V_max＝[200，150，180，180，180，180]

A_s＝[50，0，10，50，0，0]

V_s＝[0，20，20，0，0，0]

P_s＝[0，0，0，0，0，0]

V_trgt＝[60，-20，30，0，0，0]

dP＝[50，50，200，20，100，0]

从图3可以看出，本发明的方法规划结果可靠。

应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。

Claims (3)

1.一种S型加减速轨迹规划中多轴时间同步方法，其特征在于，包括如下步骤：

Step1，通过在关节同步时间t_syhc内，对所有轴分别构造一种加速度分界曲线其中t∈[0，t_sync]；根据构造曲线的位移与实际每个关节的位移进行对比，进而生成初始各关节加加速度

Step2，不考虑加速度极限值并计算轨迹参数

Step3，考虑加速度极限值计算加速曲线类型Type_a和轨迹参数

Step4，判断加速度曲线类型Type_a是否等于TriZeroTri，如果满足条件，进入下一步；否则进入对粗略轨迹参数进行修正过程；

Step5，根据Step4的修正参数和插补周期Δt生成轨迹P_i(t)。

2.根据权利要求1所述的一种S型加减速轨迹规划中多轴时间同步方法，其特征在于，所述Step4步骤中对粗略轨迹参数进行修正过程：

2.1、经判断若加速度曲线类型Type_a等于TriZeroTri，对加速度曲线参数可以直接计算；

2.2、若加速度曲线类型Type_a不等于TriZeroTri，则判断加速度曲线类型Type_a是否等于TrapZeroTrap，如果等于则加速度曲线参数可以直接计算；

2.3、若加速度曲线类型Type_a不等于TrapZeroTrap，则需要对参数进行修正计算得到Type_a；经过参数修正后需要再次判断加速度曲线类型Type_a＝TrapZeroTrap？如果加速度曲线类型Type_a为TrapZeroTrap，则需要再次计算参数否则经过上面修正计算之后得到的参数即为最终参数，可以直接输入Step5。

3.根据权利要求1所述的一种S型加减速轨迹规划中多轴时间同步方法，其特征在于，所述Step5步骤中P_i(t)轨迹生成的过程：

首先根据Step3和Step4计算的轨迹参数Type_a可以直接计算加速度轨迹的关键参数

然后由上面计算的参数可以直接得到各关节轨迹P_i(t)，具体如下：

第一段

第二段其中分别为第i关节第一段末端的位移，速度，加速度

第三段其中分别为第i关节第二段末端的位移，速度，加速度

第四段其中分别为第i关节第三段末端的位移，速度，加速度

第五段其中分别为第i关节第四段末端的位移，速度，加速度

第六段其中分别为第i关节第五段末端的位移，速度，加速度

第七段其中分别为第i关节第六段末端的位移，速度，加速度