CN106354102B - 一种初加速度不为零的s型曲线速度控制方法 - Google Patents

Abstract

一种初始加速度不为零的S型曲线速度控制方法，该方法分解为三个层面，其中速度规划层根据给定参数进行变速规划，得到变速过程的加加速截止时间、匀加速截止时间、减加速截止时间以及当前的加加速度；步进规划层根据速度规划层计算结果，计算任意时刻的位置、速度和加速度；位置规划层根据速度规划层和步进规划层的计算结果，计算当前起始点的下一步运动位置、速度和加速度，并保证最终运动到末点时速度不大于末点速度；当当前段链规划长度不够时，将当前段链链接下一条段链，保证段链长度足够规划。由于在每步速度规划过程中都按照运行到最大速度预测一步，并为保证达到末点速度而实时规划，能够快速实现算法，降低硬件要求和成本。

Description

一种初加速度不为零的S型曲线速度控制方法

技术领域

本发明涉及数控系统技术领域，具体是涉及一种初加速度不为零的S型曲线速度控制方法。

背景技术

目前，用于数控系统、工业机器人操作作业的运动速度曲线有很多种，如梯形图曲线、S型速度曲线和正弦速度曲线。速度曲线的作用是保证在启动、停止、调速或者两段曲线之间过渡时不产生震荡、冲击，从而提高运动控制的精度。大多数的速度规划都能够实现在给定首末速度、位置、最大速度、最大加速度和最大加加速度的情况下规划出相应的速度曲线，使得被控制的曲线轨迹的加速度连续。

目前大多数的速度规划算法要求起点加速度为零，这就使得一旦规划完成，运动过程中不能随意调整给定最大速度和给定末点速度，从而使得算法难以实时实现调速、暂停等功能，并为实现自动运行，调速、暂停之间切换，需要重新开发一系列函数，并保存程序运行过程中的大量现场数据和状态。这就要付出大量时间来测试，并且当某个现场数据或状态更新出错时也不易发现和修正。

目前S型曲线规划在计算最大速度时由于公式过于复杂，一般需要采用数值法来求解，使得计算量大增，需要使用更加快速的芯片来完成，增加硬件成本。

发明内容

有鉴于此，本发明提出了一种初始加速度不为零的S型曲线速度控制方法，该方法在每步运行过程中都根据最新的给定最大速度，给定末点速度重新规划，从而能够通过修改最大速度和给定末点速度快速实时实现调速、暂停以及调速、暂停、重启等过程之间的切换。由于在每步速度规划过程中都按照运行到最大速度预测一步，并为保证达到末点速度而实时规划，因而无需耗费大量时间来计算最大速度，故能够快速实现算法，降低硬件要求和成本。

本发明的目的是以下述方式实现的：

一种初加速度不为零的S型曲线速度控制方法，所述方法分解为三个层面，即速度规划层、步进规划层和位置规划层；

其中速度规划层根据给定的起始速度v₀、末点速度v_e、起始加速度a₀、最大加加速度j_max和最大加速度a_max，进行变速规划，得到变速过程的加加速截止时间t1、匀加速截止时间t2、减加速截止时间t3以及当前的加加速度j；

步进规划层根据速度规划层计算结果，计算任意时刻t的位置x_r、速度v_rx和加速度a_rx；

位置规划层根据速度规划层和步进规划层的计算结果、起始点位置x₀、末点位置x_e以及最大速度v_max，计算当前起始点的下一步运动位置、速度和加速度，并保证最终运动到末点时速度不大于v_e；当当前段链规划长度不够时，将当前段链链接下一条段链，并根据链接后段链总长度重新计算，保证段链长度足够规划。

优选地，设向量y₀＝[x₀ v₀ a₀]表示起始点位置、速度、加速度，设向量y_e＝[x_e v_e0]表示末点位置、速度、加速度，设向量y₁＝[x₁ v₁ a₁]表示下一步运动位置、速度、加速度，设向量y＝[x v a]为预测的运动位置、速度、加速度，设给定的最大速度为v_max，所述位置规划层包括以下步骤：

步骤1：针对一条段链，如果末点位置x_e与起始点位置x₀之差ΔL小于一常量Ⅰδ₁，即ΔL＝x_e-x₀＜δ₁，且末点速度v_e与起始点速度v₀之差的绝对值Δv小于一常量Ⅱδ₂，即|Δv|＝|v_e-v₀|＜δ₂，则设置下一步运动向量y₁为末点向量y_e，即y₁＝y_e，返回参数y₁；否则进入步骤2；

步骤2：以v₀为起始速度，a₀为起始加速度，v_max为末点速度调用速度规划层函数，并按照计算结果调用步进规划层函数往前预测一步，得y₁；接着以y₁为起始点向量，y_e为末点向量调用速度规划层函数和步进规划层函数往前预测达到末点速度v_e所需要运行到的位置x，如果x≤x_e，表明下一步运动到y₁可以保证最终运动到末点时速度不大于v_e，因此y₁即为下一步运动目标，返回参数y₁；否则进入步骤3；

步骤3：如果步骤2往前预测一步使用的加加速度j＝j_max，则转到步骤4，否则转到步骤5；

步骤4：以匀加速即j＝0调用步进规划层函数往前预测一步，得y₁；接着以y₁为起始点向量，y_e为末点向量调用速度规划层函数和步进规划层函数往前预测达到末点速度v_e所需要达到的位置x，如果x≤x_e，表明下一步运动到y₁可以保证最终运动到末点时速度不大于v_e，因此y₁即为下一步运动目标，返回参数y₁；否则进入步骤5；

步骤5：以y₀为起始点向量，y_e为末点向量调用速度规划层函数和步进规划层函数往前预测一步，得y₁，并往前预测达到末点速度v_e所需要达到的位置x，如果x≤x_e，表明下一步运动到y₁可以保证最终运动到末点时速度等于v_e，因此y₁即为下一步运动目标，返回参数y₁；否则进入步骤6；

步骤6：当前段链规划长度不够，将当前段链链接下一条段链，并返回步骤2。

优选地，设置常量Ⅰδ₁＝0.5(a_max-a₀)T²，设置常量Ⅱδ₂＝J_maxT²。

优选地，设v₀为起始速度，v_e为S型速度规划结束速度，首末速度差Δv＝v₀-v_e，a₀为起始加速度，j_max为给定最大加加速度，a_max为给定最大加速度，标志速度常量v_x＝Δv×flag，加速度常量a_x＝a₀×flag；所述速度规划层函数包括以下步骤，其中t₁、t₂、t₃和j分别为加加速截止时间、匀加速截止时间、减加速截止时间和当前的加加速度：

步骤A：当a_x＜0，且时，取

步骤B：当a_x≥0或时，

(1)当时，取j＝-j_max，

(2)当时，取j＝-j_max,

步骤C：更新给定加加速度，即取j＝j×flag。

优选地，设t为当前时刻，所述步进规划层函数根据以下公式计算当前时刻t时的位置x_r、速度v_rx和加速度a_rx：

(1)当t≤t₁时

a_rx＝a₀+jt

(2)当t₁＜t≤t₂时

a_rx＝a₀+jt₁

(3)当t₂＜t≤t₃

a_rx＝a₀+jt₁-j(t-t₂)

(4)当t≥t₃时

对初始加速度不为零的速度规划3段时间点t₁,t₂,t₃的计算，具体见速度规划层设计，该部分以速度增量方向为正，按照加速度方向和速度增量大小，分成3个条件独立计算加加速度，匀加速度和减加速度时间结束点。

步进规划层：对初始加速度不为零的任意时刻加速度，速度，位置计算，计算量小，易于实时实现。

位置规划层:采用按照达到最大速度预测一步，再测试当前段链达到末点速度是否够长的策略，保证最终规划速度不会大于最大速度(最大速度大于当前速度)。

相对现有技术，本发明的优点是：

1.只需要简单调整最大速度和末点速度参数就能实时实现S型调速、暂停、步进等功能。

2.本发明计算量少，能够在较低成本的硬件上实现。

3.本发明函数少，代码量少，执行时间短，可以满足数控系统或机器人的运动控制的高实时性。

附图说明

图1为本发明的速度规划层的流程图；

图2、图3分别为本发明不同起始速度、起始加速度、末点速度情况下的速度和加速度规划示意图；

图4为本发明的位置规划层的流程图。

图5、图6和图7分别为本发明的S型曲线速度控制方法的仿真位置、速度和加速度结果示意图。

具体实施方式

本发明采用下述技术方案：

把规划分解为三个层面进行处理，即速度规划层，步进规划层，位置规划层，并分别采用3个函数实现。速度规划层和步进规划层作为底层实现基本的S型速度规划和步进距离求解。其中速度规划层根据给定的首末速度和首加速度进行变速规划，得到变速过程的加加速、匀加速、减加速的截止时间以及加加速度方向；步进规划层根据速度规划层规划结果计算一步或任意时间运行后的位置，速度，加速度。位置规划层作为外层，根据给定位置、最大速度、起始速度、起始加速度、末点速度计算一步运行后的位置，速度，加速度，该函数可由实时运行线程调用，能够保证整个段链在初始加速度不为零时的S型曲线速度规划的自动实现。

1)速度规划层

设Δv＝v₀-v_e，v₀为起始速度，v_e为S型速度规划结束速度，a₀为起始加速度，j_max为给定最大加加速度，a_max为给定最大加速度，v_x＝Δv×flag，a_x＝a₀×flag。

步骤1：当a_x＜0，且时，取

步骤2：当a_x≥0或时，

(3)当时，取j＝-j_max，

(4)当时，取j＝-j_max

步骤3：更新给定加加速度，即取j＝j×flag。

该规划层以函数形式实现，记为VelocityPlan，其实现流程图如图1所示。

2)步进规划层

设v₀为起始速度，a₀为起始加速度，x₀为起始位置。设t为当前时刻，所述步进规划层根据以下公式计算当前时刻t的位置x_r、速度v_rx和加速度a_rx：

(1)当t≤t₁时

a_rx＝a₀+jt

(2)当t₁＜t≤t₂时

a_rx＝a₀+jt₁

(3)当t₂＜t≤t₃

a_rx＝a₀+jt₁-j(t-t₂)

(4)当t≥t₃时

a_rx＝a₀+jt₁-j(t₃-t₂)

该规划层以函数形式实现，记为GetStepPVAG，该函数返回y_r＝[x_r v_rx a_rx]。

不同起始速度、起始加速度、末点速度情况下的速度和加速度规划效果如图2和图3所示。

3)位置规划层

记向量y₀＝[x₀ v₀ a₀]表示当前起始点位置、速度、加速度，y_e＝[x_e v_e 0]为末点向量，y₁＝[x₁ v₁ a₁]为下一步给定向量，y＝[x v a]为预测向量。

步骤1：针对一条段链，如果末点给定位置x_e与起始点位置x₀之差小于一小量δ₁，即ΔL＝x_e-x₀＜δ₁，且末点速度v_e与起始点速度v₀之差的绝对值小于一小量δ₂，即|Δv|＝|v_e-v₀|＜δ₂，则取下一步给定向量y₁为末点给定向量y_e，即y₁＝y_e，否则进入步骤2。

一般可取δ₁＝0.5(a_max-a₀)T²，δ₂＝J_maxT²。

步骤2：以当前起始速度v₀为起始速度，当前加速度a₀为起始加速度，最大速度v_max为末点速度，调用速度规划函数VelocityPlan，并按照规划结果调用步进规划函数GetStepPVAG往前预测一步，得y₁。接着以y₁为起始点向量，y_e为末点向量调用函数VelocityPlan和函数GetStepPVAG往前预测达到末点速度v_e所需要运行到的位置x，如果x≤x_e，表明下一步运动到y₁可以保证最终运动到末点时速度不大于v_e，因此y₁即为下一步运动目标，否则进入步骤3。

步骤3：如果步骤2往前预测一步使用的加加速度j＝j_max，则进入步骤4，否则进入步骤5。

步骤4：以匀加速预测一步，即j＝0，得y₁。接着以y₁为起始点向量，y_e为末点向量调用函数VelocityPlan和函数GetStepPVAG往前预测达到末点速度v_e所需要达到的位置x，如果x≤x_e，表明下一步运动到y₁可以保证最终运动到末点时速度不大于v_e，因此y₁即为下一步运动目标，否则进入步骤5。

步骤5：以y₀为起始点向量，y_e为末点向量调用函数VelocityPlan和函数GetStepPVAG往前预测一步，得y₁，并往前预测达到末点速度v_e所需要达到的位置x，如果x≤x_e，表明下一步运动到y₁可以保证最终运动到末点时速度等于v_e，因此y₁即为下一步运动目标，否则进入步骤6。

步骤6：当前段链规划长度不够，将当前段链链接下一条段链，并返回步骤2。该规划层以函数形式实现，记为AutoVelocityPlan，具体实现流程图如图4所示。

通过改变最大速度实现调速、暂停、重启的S型规划仿真结果如图5至图7所示。在0.25s时，设置v_max＝0；在1.2s时，设置v_max＝100；在1.5s时，设置v_max＝0；在2s时，设置v_max＝100；在0.25至3s内，设置v_max＝25*rand(1)+25，rand(1)为程序产生的随机数。

本申请中的线段队列表示按序存放的一定数量的数控代码所代表的线段。段链由多段线段连接在一起组成，是为保证从起点速度运行到末点速度长度足够长而人为链接在一起的多段线段。

以上所述的仅是本发明的优选实施方式，应当指出，对于本领域的技术人员来说，在不脱离本发明整体构思前提下，还可以作出若干改变和改进，这些也应该视为本发明的保护范围。

Claims (3)

1.一种初加速度不为零的S型曲线速度控制方法，其特征在于：所述方法分解为三个层面，即速度规划层、步进规划层和位置规划层；

其中速度规划层包含速度规划层函数，所述速度规划层函数根据给定的起始速度v₀、末点速度v_e、起始加速度a₀、最大加加速度j_max和最大加速度a_max进行变速规划，得到变速过程的加加速截止时间t₁、匀加速截止时间t₂、减加速截止时间t₃以及当前的加加速度j；

所述速度规划层函数得出变速过程的加加速截止时间t₁、匀加速截止时间t₂、减加速截止时间t₃以及当前的加加速度j过程包括以下步骤：

设v₀为起始速度，v_e为S型速度规划结束速度，首末速度差Δv＝v₀-v_e，a₀为起始加速度，j_max为给定最大加加速度，a_max为给定最大加速度，标志速度常量v_x＝Δv×flag，加速度常量a_x＝a₀×flag；

步骤A：当a_x＜0，且时，取

步骤B：当a_x≥0或时，

当时，取j＝-j_max，

当时，取j＝-j_max，

步骤C：更新当前加加速度，即取j＝j×flag；

步进规划层包含步进规划层函数，所述步进规划层函数根据速度规划层计算结果，计算任意时刻t时的位置x_r、速度v_rx和加速度a_rx；

所述步进规划层函数根据以下公式计算任意时刻t的位置x_r、速度v_rx和加速度a_rx：

(1)当t≤t₁时，

a_rx＝a₀+jt

(2)t₁＜t≤t₂时，

a_rx＝a₀+jt₁

(3)当t₂＜t≤t₃时，

a_rx＝a₀+jt₁-j(t-t₂)

(4)t≥t₃时，

a_rx＝a₀+jt₁-j(t₃-t₂)

位置规划层根据速度规划层和步进规划层的计算结果、起始点位置x₀、末点位置x_e以及最大速度v_max，计算当前起始点下一步运动位置、速度和加速度，并保证最终运动到末点时速度不大于v_e；当当前段链规划长度不够时，将当前段链链接下一条段链，并根据链接后段链总长度重新计算。

2.根据权利要求1所述的初加速度不为零的S型曲线速度控制方法，其特征在于：设向量y₀＝[x₀ v₀ a₀]表示起始点位置、速度、加速度，设向量y_e＝[x_e v_e 0]表示末点位置、速度、加速度，设向量y₁＝[x₁ v₁ a₁]表示下一步运动位置、速度、加速度，设向量y＝[x v a]为预测的运动位置、速度、加速度，设给定的最大速度v_max，所述位置规划层包括以下步骤：

步骤1：针对一条段链，如果末点位置x_e与起始点位置x₀之差ΔL小于一常量Iδ₁，即ΔL＝x_e-x₀<δ₁，且末点速度v_e与起始点速度v₀之差的绝对值|Δv|小于一常量IIδ₂，即|Δv|＝|v_e-v₀|＜δ₂，则设置下一步运动向量y₁为末点向量y_e，即y₁＝y_e，返回参数y₁；否则进入步骤2；

步骤2：以v₀为起始速度，a₀为起始加速度，v_max为末点速度调用速度规划层函数，并按照计算结果调用步进规划层函数往前预测一步，得y₁；接着以y₁为起始点向量，y_e为末点向量调用速度规划层函数和步进规划层函数往前预测达到末点速度v_e所需要运行到的位置x，如果x≤x_e，表明下一步运动到y₁可以保证最终运动到末点时速度不大于v_e，因此y₁即为下一步运动目标，返回参数y₁；否则进入步骤3；

步骤3：如果步骤2往前预测一步使用的加加速度j＝j_max，则转到步骤4，否则转到步骤5；

步骤4：以匀加速即j＝0调用步进规划层函数往前预测一步，得y₁；接着以y₁为起始点向量，y_e为末点向量调用速度规划层函数和步进规划层函数往前预测达到末点速度v_e所需要运行到的位置x，如果x≤x_e，表明下一步运动到y₁可以保证最终运动到末点时速度不大于v_e，因此y₁即为下一步运动目标，返回参数y₁；否则进入步骤5；

步骤5：以y₀为起始点向量，y_e为末点向量调用速度规划层函数和步进规划层函数往前预测一步，得y₁，并往前预测达到末点速度v_e所需要达到的位置x，如果x≤x_e，表明下一步运动到y₁可以保证最终运动到末点时速度等于v_e，因此y₁即为下一步运动目标，返回参数y₁；否则进入步骤6；

步骤6：当前段链规划长度不够，将当前段链链接下一条段链，并返回步骤2。

3.根据权利要求2所述的初加速度不为零的S型曲线速度控制方法，其特征在于：设置常量Iδ₁＝0.5(a_max-a₀)T²，设置常量IIδ₂＝J_maxT²。