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

本发明涉及数控系统
技术领域：
，具体是涉及一种初加速度不为零的S型曲线速度控制方法。
背景技术：
：目前，用于数控系统、工业机器人操作作业的运动速度曲线有很多种，如梯形图曲线、S型速度曲线和正弦速度曲线。速度曲线的作用是保证在启动、停止、调速或者两段曲线之间过渡时不产生震荡、冲击，从而提高运动控制的精度。大多数的速度规划都能够实现在给定首末速度、位置、最大速度、最大加速度和最大加加速度的情况下规划出相应的速度曲线，使得被控制的曲线轨迹的加速度连续。目前大多数的速度规划算法要求起点加速度为零，这就使得一旦规划完成，运动过程中不能随意调整给定最大速度和给定末点速度，从而使得算法难以实时实现调速、暂停等功能，并为实现自动运行，调速、暂停之间切换，需要重新开发一系列函数，并保存程序运行过程中的大量现场数据和状态。这就要付出大量时间来测试，并且当某个现场数据或状态更新出错时也不易发现和修正。目前S型曲线规划在计算最大速度时由于公式过于复杂，一般需要采用数值法来求解，使得计算量大增，需要使用更加快速的芯片来完成，增加硬件成本。技术实现要素：有鉴于此，本发明提出了一种初始加速度不为零的S型曲线速度控制方法，该方法在每步运行过程中都根据最新的给定最大速度，给定末点速度重新规划，从而能够通过修改最大速度和给定末点速度快速实时实现调速、暂停以及调速、暂停、重启等过程之间的切换。由于在每步速度规划过程中都按照运行到最大速度预测一步，并为保证达到末点速度而实时规划，因而无需耗费大量时间来计算最大速度，故能够快速实现算法，降低硬件要求和成本。本发明的目的是以下述方式实现的：一种初加速度不为零的S型曲线速度控制方法，所述方法分解为三个层面，即速度规划层、步进规划层和位置规划层；其中速度规划层根据给定的起始速度v0、末点速度ve、起始加速度a0、最大加加速度jmax和最大加速度amax，进行变速规划，得到变速过程的加加速截止时间t1、匀加速截止时间t2、减加速截止时间t3以及当前的加加速度j；步进规划层根据速度规划层计算结果，计算任意时刻t的位置xr、速度vrx和加速度arx；位置规划层根据速度规划层和步进规划层的计算结果、起始点位置x0、末点位置xe以及最大速度vmax，计算当前起始点的下一步运动位置、速度和加速度，并保证最终运动到末点时速度不大于ve；当当前段链规划长度不够时，将当前段链链接下一条段链，并根据链接后段链总长度重新计算，保证段链长度足够规划。优选地，设向量y0＝[x0v0a0]表示起始点位置、速度、加速度，设向量ye＝[xeve0]表示末点位置、速度、加速度，设向量y1＝[x1v1a1]表示下一步运动位置、速度、加速度，设向量y＝[xva]为预测的运动位置、速度、加速度，设给定的最大速度为vmax，所述位置规划层包括以下步骤：步骤1：针对一条段链，如果末点位置xe与起始点位置x0之差ΔL小于一常量Ⅰδ1，即ΔL＝xe-x0＜δ1，且末点速度ve与起始点速度v0之差的绝对值Δv小于一常量Ⅱδ2，即|Δv|＝|ve-v0|＜δ2，则设置下一步运动向量y1为末点向量ye，即y1＝ye，返回参数y1；否则进入步骤2；步骤2：以v0为起始速度，a0为起始加速度，vmax为末点速度调用速度规划层函数，并按照计算结果调用步进规划层函数往前预测一步，得y1；接着以y1为起始点向量，ye为末点向量调用速度规划层函数和步进规划层函数往前预测达到末点速度ve所需要运行到的位置x，如果x≤xe，表明下一步运动到y1可以保证最终运动到末点时速度不大于ve，因此y1即为下一步运动目标，返回参数y1；否则进入步骤3；步骤3：如果步骤2往前预测一步使用的加加速度j＝jmax，则转到步骤4，否则转到步骤5；步骤4：以匀加速即j＝0调用步进规划层函数往前预测一步，得y1；接着以y1为起始点向量，ye为末点向量调用速度规划层函数和步进规划层函数往前预测达到末点速度ve所需要达到的位置x，如果x≤xe，表明下一步运动到y1可以保证最终运动到末点时速度不大于ve，因此y1即为下一步运动目标，返回参数y1；否则进入步骤5；步骤5：以y0为起始点向量，ye为末点向量调用速度规划层函数和步进规划层函数往前预测一步，得y1，并往前预测达到末点速度ve所需要达到的位置x，如果x≤xe，表明下一步运动到y1可以保证最终运动到末点时速度等于ve，因此y1即为下一步运动目标，返回参数y1；否则进入步骤6；步骤6：当前段链规划长度不够，将当前段链链接下一条段链，并返回步骤2。优选地，设置常量Ⅰδ1＝0.5(amax-a0)T2，设置常量Ⅱδ2＝JmaxT2。优选地，设v0为起始速度，ve为S型速度规划结束速度，首末速度差Δv＝v0-ve，a0为起始加速度，jmax为给定最大加加速度，amax为给定最大加速度，标志速度常量vx＝Δv×flag，加速度常量ax＝a0×flag；所述速度规划层函数包括以下步骤，其中t1、t2、t3和j分别为加加速截止时间、匀加速截止时间、减加速截止时间和当前的加加速度：步骤A：当ax＜0，且时，取j=jmax,t1=-ax+12ax2-jmaxvxjmax,t2=t1,t3=t1+t2+axj;]]>步骤B：当ax≥0或时，(1)当时，取j＝-jmax，t1=axjmax+12ax2+jmaxvxjmax,t2=t1,t3=t1+t2+axj;]]>(2)当时，取j＝-jmax,t1=ax+amaxjmax,t2=vxamax+12ax2amaxjmax+axjmax,t3=t1+t2+axj;]]>步骤C：更新给定加加速度，即取j＝j×flag。优选地，设t为当前时刻，所述步进规划层函数根据以下公式计算当前时刻t时的位置xr、速度vrx和加速度arx：(1)当t≤t1时arx＝a0+jtvrx=v0+a0t+12jt2]]>xr=x0+v0t+12a0t2+16jt3]]>(2)当t1＜t≤t2时arx＝a0+jt1vrx=v0+a0t-12jt12+jt1t]]>xr=x0+v0t+12a0t2+16jt13-12jt12t+12jt1t2]]>(3)当t2＜t≤t3arx＝a0+jt1-j(t-t2)vrx=v0+a0t+jt1t-12jt12-12j(t-t2)2]]>xr=x0+v0t+12a0t2-16jt3+16j(t13+t23)+12j(t1+t2)t2-12j(t12+t22)t]]>(4)当t≥t3时arx=a0+jt1-j(t3-t2)vrx=v0+a0t3+jt1t3-12jt12-12j(t3-t2)2xr=x0+v0t3+12a0t32-16jt33+16j(t13+t23)+12j(t1+t2)t32-12j(t12+t22)t3.]]>对初始加速度不为零的速度规划3段时间点t1,t2,t3的计算，具体见速度规划层设计，该部分以速度增量方向为正，按照加速度方向和速度增量大小，分成3个条件独立计算加加速度，匀加速度和减加速度时间结束点。步进规划层：对初始加速度不为零的任意时刻加速度，速度，位置计算，计算量小，易于实时实现。位置规划层:采用按照达到最大速度预测一步，再测试当前段链达到末点速度是否够长的策略，保证最终规划速度不会大于最大速度(最大速度大于当前速度)。相对现有技术，本发明的优点是：1.只需要简单调整最大速度和末点速度参数就能实时实现S型调速、暂停、步进等功能。2.本发明计算量少，能够在较低成本的硬件上实现。3.本发明函数少，代码量少，执行时间短，可以满足数控系统或机器人的运动控制的高实时性。附图说明图1为本发明的速度规划层的流程图；图2、图3分别为本发明不同起始速度、起始加速度、末点速度情况下的速度和加速度规划示意图；图4为本发明的位置规划层的流程图。图5、图6和图7分别为本发明的S型曲线速度控制方法的仿真位置、速度和加速度结果示意图。具体实施方式本发明采用下述技术方案：把规划分解为三个层面进行处理，即速度规划层，步进规划层，位置规划层，并分别采用3个函数实现。速度规划层和步进规划层作为底层实现基本的S型速度规划和步进距离求解。其中速度规划层根据给定的首末速度和首加速度进行变速规划，得到变速过程的加加速、匀加速、减加速的截止时间以及加加速度方向；步进规划层根据速度规划层规划结果计算一步或任意时间运行后的位置，速度，加速度。位置规划层作为外层，根据给定位置、最大速度、起始速度、起始加速度、末点速度计算一步运行后的位置，速度，加速度，该函数可由实时运行线程调用，能够保证整个段链在初始加速度不为零时的S型曲线速度规划的自动实现。1)速度规划层设Δv＝v0-ve，v0为起始速度，ve为S型速度规划结束速度，a0为起始加速度，jmax为给定最大加加速度，amax为给定最大加速度，vx＝Δv×flag，ax＝a0×flag。步骤1：当ax＜0，且时，取j=jmax,t1=-ax+12ax2-jmaxvxjmax,t2=t1,t3=t1+t2+axj;]]>步骤2：当ax≥0或时，(3)当时，取j＝-jmax，t1=axjmax+12ax2+jmaxvxjmax,t2=t1,t3=t1+t2+axj;]]>(4)当时，取j＝-jmaxt1=ax+amaxjmax,t2=vxamax+12ax2amaxjmax+axjmax,t3=t1+t2+axj;]]>步骤3：更新给定加加速度，即取j＝j×flag。该规划层以函数形式实现，记为VelocityPlan，其实现流程图如图1所示。2)步进规划层设v0为起始速度，a0为起始加速度，x0为起始位置。设t为当前时刻，所述步进规划层根据以下公式计算当前时刻t的位置xr、速度vrx和加速度arx：(1)当t≤t1时arx＝a0+jtvrx=v0+a0t+12jt2]]>xr=x0+v0t+12a0t2+16jt3]]>(2)当t1＜t≤t2时arx＝a0+jt1vrx=v0+a0t-12jt12+jt1t]]>xr=x0+v0t+12a0t2+16jt13-12jt12t+12jt1t2]]>(3)当t2＜t≤t3arx＝a0+jt1-j(t-t2)vrx=v0+a0t+jt1t-12jt12-12j(t-t2)2]]>xr=x0+v0t+12a0t2-16jt3+16j(t13+t23)+12j(t1+t2)t2-12j(t12+t22)t]]>(4)当t≥t3时arx＝a0+jt1-j(t3-t2)vrx=v0+a0t3+jt1t3-12jt12-12j(t3-t2)2]]>xr=x0+v0t3+12a0t32-16jt33+16j(t13+t23)+12j(t1+t2)t32-12j(t12+t22)t3]]>该规划层以函数形式实现，记为GetStepPVAG，该函数返回yr＝[xrvrxarx]。不同起始速度、起始加速度、末点速度情况下的速度和加速度规划效果如图2和图3所示。3)位置规划层记向量y0＝[x0v0a0]表示当前起始点位置、速度、加速度，ye＝[xeve0]为末点向量，y1＝[x1v1a1]为下一步给定向量，y＝[xva]为预测向量。步骤1：针对一条段链，如果末点给定位置xe与起始点位置x0之差小于一小量δ1，即ΔL＝xe-x0＜δ1，且末点速度ve与起始点速度v0之差的绝对值小于一小量δ2，即|Δv|＝|ve-v0|＜δ2，则取下一步给定向量y1为末点给定向量ye，即y1＝ye，否则进入步骤2。一般可取δ1＝0.5(amax-a0)T2，δ2＝JmaxT2。步骤2：以当前起始速度v0为起始速度，当前加速度a0为起始加速度，最大速度vmax为末点速度，调用速度规划函数VelocityPlan，并按照规划结果调用步进规划函数GetStepPVAG往前预测一步，得y1。接着以y1为起始点向量，ye为末点向量调用函数VelocityPlan和函数GetStepPVAG往前预测达到末点速度ve所需要运行到的位置x，如果x≤xe，表明下一步运动到y1可以保证最终运动到末点时速度不大于ve，因此y1即为下一步运动目标，否则进入步骤3。步骤3：如果步骤2往前预测一步使用的加加速度j＝jmax，则进入步骤4，否则进入步骤5。步骤4：以匀加速预测一步，即j＝0，得y1。接着以y1为起始点向量，ye为末点向量调用函数VelocityPlan和函数GetStepPVAG往前预测达到末点速度ve所需要达到的位置x，如果x≤xe，表明下一步运动到y1可以保证最终运动到末点时速度不大于ve，因此y1即为下一步运动目标，否则进入步骤5。步骤5：以y0为起始点向量，ye为末点向量调用函数VelocityPlan和函数GetStepPVAG往前预测一步，得y1，并往前预测达到末点速度ve所需要达到的位置x，如果x≤xe，表明下一步运动到y1可以保证最终运动到末点时速度等于ve，因此y1即为下一步运动目标，否则进入步骤6。步骤6：当前段链规划长度不够，将当前段链链接下一条段链，并返回步骤2。该规划层以函数形式实现，记为AutoVelocityPlan，具体实现流程图如图4所示。通过改变最大速度实现调速、暂停、重启的S型规划仿真结果如图5至图7所示。在0.25s时，设置vmax＝0；在1.2s时，设置vmax＝100；在1.5s时，设置vmax＝0；在2s时，设置vmax＝100；在0.25至3s内，设置vmax＝25*rand(1)+25，rand(1)为程序产生的随机数。本申请中的线段队列表示按序存放的一定数量的数控代码所代表的线段。段链由多段线段连接在一起组成，是为保证从起点速度运行到末点速度长度足够长而人为链接在一起的多段线段。以上所述的仅是本发明的优选实施方式，应当指出，对于本领域的技术人员来说，在不脱离本发明整体构思前提下，还可以作出若干改变和改进，这些也应该视为本发明的保护范围。当前第1页1 2 3