CN109313429B - S型速度规划方法、装置、系统、机器人以及数控机床 - Google Patents

Abstract

一种S型速度规划方法、装置、系统、机器人以及数控机床。其中，所述方法包括：利用目标路线的最大速度、最大加速度以及最大加加速度计算得到第一次卷积窗和第二次卷积窗(S11)；根据目标路线的位移以及所需满足的卷积的时间要求，计算得到在所述目标路线实际可达的最大速度(S12)；比较所述实际可达的最大速度与所述最大速度之间的大小，重新调整所述目标路线的最大速度(S13)；利用第一次卷积窗和第二次卷积窗对由调整后的目标路线的最大速度形成的原始信号进行二次卷积，得到所述目标路线的S型速度曲线数据(S14)。通过上述方式，能够减少S型速度曲线规划的运算量，并且能够兼容不同路线位移的情况。

Description

S型速度规划方法、装置、系统、机器人以及数控机床

技术领域

本发明涉及速度规划技术领域，特别是涉及S型速度规划方法、装置、系统、机器人以及数控机床。

背景技术

数控系统和工业机器人中，为了保证机床或工业机器人在启动停止或者两条曲线段之间过渡时不产生冲击、失步、超程或者震荡，需要进行曲线加减速控制，即采用专门的加减速控制规划算法，使得电机的输入按照一定的规律进行变化，在各种加工情况下都能快速、准确的定位。

目前，常用的速度曲线规划为S型速度曲线规划。鉴于S型速度曲线规划复杂情况，采用传统的纯公式推导的方式，规划流程变得相当复杂，计算量大。而且，现有的S型速度曲线规划方案中，实际要走的位移必须大于补偿位移，否则，无法达到最大进给速度。此时，则无法得到正确的速度规划。

发明内容

本发明主要解决的技术问题是提供S型速度规划方法、装置、系统、机器人以及数控机床，能够减少S型速度曲线规划的运算量，并且能够兼容不同路线位移的情况。

为解决上述技术问题，本发明采用的一个技术方案是：提供一种S型速度规划方法，包括以下步骤：利用目标路线的最大速度V_max、最大加速度A以及最大加加速度Jerk计算得到第一次卷积窗h₁和第二次卷积窗h₂；根据目标路线的位移S以及所需满足的卷积的时间要求，计算得到在所述目标路线实际可达的最大速度V_max1；比较所述实际可达的最大速度V_max1与所述最大速度V_max之间的大小，重新调整所述目标路线的最大速度V_max；利用第一次卷积窗h₁和第二次卷积窗h₂对由调整后的目标路线的最大速度V_max形成的原始信号进行二次卷积，得到所述目标路线的S型速度曲线数据。

为解决上述技术问题，本发明采用的另一技术方案是，提供一种S型速度规划装置，包括：卷积窗计算模块，用于利用目标路线的最大速度V_max、最大加速度A以及最大加加速度Jerk计算得到第一次卷积窗h₁和第二次卷积窗h₂；速度获得模块，用于根据目标路线的位移S以及所需满足的卷积的时间要求，计算得到在所述目标路线实际可达的最大速度V_max1；速度调整模块，用于比较所述实际可达的最大速度V_max1与所述最大速度V_max之间的大小，重新调整所述目标路线的最大速度V_max；卷积模块，用于利用第一次卷积窗h₁和第二次卷积窗h₂对由调整后的目标路线的最大速度V_max形成的原始信号进行二次卷积，得到所述目标路线的S型速度曲线数据。

为解决上述技术问题，本发明采用的再一技术方案是，提供一种S型速度规划系统，包括执行元件、驱动电机、处理器和存储器；

所述存储器用于存储计算机指令以及所述处理器处理的数据；

所述处理器执行所述计算机指令，用于：利用目标路线的最大速度V_max、最大加速度A以及最大加加速度Jerk计算得到第一次卷积窗h₁和第二次卷积窗h₂；根据目标路线的位移S以及所需满足的卷积的时间要求，计算得到在所述目标路线实际可达的最大速度V_max1；比较所述实际可达的最大速度V_max1与所述最大速度V_max之间的大小，重新调整所述目标路线的最大速度V_max；利用第一次卷积窗h₁和第二次卷积窗h₂对由调整后的目标路线的最大速度V_max形成的原始信号进行二次卷积，得到所述目标路线的S型速度曲线数据；根据所述目标路线的S型速度曲线数据向所述驱动电机发送控制指令；

所述执行元件用于在所述驱动电机的驱动下在目标路线上按照所述S型速度曲线进行运作。

为解决上述技术问题，本发明采用的又再一技术方案是，提供一种应用S型速度规划方法的机器人，所述机器人包括控制系统和机器臂，所述控制系统根据上述的S型速度规划方法得到目标路线的S型速度曲线数据，并根据所述S型速度曲线数据控制所述机器臂运行。

为解决上述技术问题，本发明采用的另再一技术方案是，提供一种应用S型速度规划方法的数控机床，所述数控机床包括数控系统和机床，所述数控系统根据上述的S型速度规划方法得到目标路线的S型速度曲线数据，并根据所述S型速度曲线数据控制所述机床进行加工。

以上方案，采用二次卷积实现S型速度曲线规划，相对于传统的S型速度规划减少了计算量，而且在二次卷积前根据目标路线的唯一和所需满足的卷积时间要求重新计算目标路线可达到的最大速度，进而对目标路线的最大速度进行调整，以利用调整后的最大速度进行二次卷积，避免了由于目标路线由于位移小无法达到其设定的最大速度的情况，故能够兼容不同路线位移的情况。

附图说明

图1是本发明S型速度规划方法一实施例的流程图；

图2是在第一应用场景得到的S型速度曲线示意图；

图3是在第二应用场景得到的S型速度曲线示意图；

图4是在第三应用场景得到的S型速度曲线示意图；

图5是在第四应用场景得到的S型速度曲线示意图；

图6是经图1所示的步骤S14进行二次卷积过程的速度示意图；

图7是本发明S型速度规划方法另一实施例的流程图；

图8是本发明S型速度规划方法再一实施例的部分流程图；

图9是经图8所示步骤得到的目标路线的原始信号的补偿示意图；

图10是本发明S型速度规划方法又再一实施例的部分流程图；

图11是图10所示的步骤S103中计算段点可达最大速度的原理示意图；

图12是图10所示的步骤S104进行校验时的速度调整示意图；

图13是本发明S型速度规划装置一实施例的结构示意图；

图14是本发明S型速度规划装置另一实施例的结构示意图；

图15是本发明S型速度规划系统一实施例的结构示意图；

图16是本发明机器人一实施例的结构示意图；

图17是本发明数控机床一实施例的结构示意图。

具体实施方式

以下描述中，为了说明而不是为了限定，提出了诸如特定系统结构、接口、技术之类的具体细节，以便透彻理解本申请。然而，本领域的技术人员应当清楚，在没有这些具体细节的其它实施方式中也可以实现本申请。在其它情况中，省略对众所周知的装置、电路以及方法的详细说明，以免不必要的细节妨碍本申请的描述。

下述S型速度规划方法及其相关装置可应用于数控系统和工业机器人等需要对其部件进行速度规划的系统，例如，对数控系统的机床或工业机器人的手臂执行的一段或多段路线进行该速度规划，以使机床或工业机器人能够从该路线的起点开始从初始速度加速到最大速度，并运行到该路线的终点时从最大速度降速至结束速度。该路线可以为直线或曲线。

请参阅图1，图1是本发明S型速度规划方法一实施例的流程图。本实施例中，该方法由S型速度规划装置执行，用于对一对象的运行速度进行该S型速度规划，具体如该规划装置为数控系统或工业机器人的处理器等，该对象为数控系统的机床或工业机器人的手臂等。本实施例以对象为数控系统的机床为例，该方法包括以下步骤：

S11：利用目标路线的最大速度V_max、最大加速度A以及最大加加速度Jerk计算得到第一次卷积窗h₁和第二次卷积窗h₂。

其中，上述目标路线为速度规划对象即将需要运行的路线，该路线的该最大速度V_max、最大加速度A以及最大加加速度Jerk、起始速度V_i和结束速度V_f为在执行该方法时已设定的，具体如根据驱动该对象的驱动能力或该对象在该路线上所执行的具体任务而设定的；或者为用户输入的。

本实施例采用二次卷积来实现S型速度规划，且通过控制卷积窗的大小，可控制卷积上升时间和下降时间，也就是控制加速度和加加速度的大小，故可通过该目标路线的设定的最大加速度和最大加加速度可计算得到对应的卷积窗的大小。

为了保证能够得到起止速度不为0的S型速度规划曲线，该第一次卷积窗的时间考虑起止速度，即利用目标路线的最大速度V_max、起始速度V_i和结束速度V_f的最小值、最大加速度A以及最大加加速度Jerk计算得到第一次卷积窗h₁和第二次卷积窗h₂。具体地，利用下述公式1计算得到第一次卷积窗的时间t₁和第二次卷积窗的时间t₂，并利用下述公式2得到第一次卷积窗h₁和第二次卷积窗h₂。

其中，所述ΔV为目标路线的最大速度V_max与V_min间的差，所述V_min＝min(V_i,V_f)，V_i为该目标路线的起始速度，V_f为该目标路线的结束速度，即，该t₁为在卷积前从初始速度V_i上升到最大速度V_max的时间与从最大速度V_max下降到结束速度V_f的时间之间的最大值；T_s为插补周期。本实施例中，由于机床是根据插补周期运行的，即每隔插补周期走一步，故该第一次卷积窗h₁和第二次卷积窗h₂为离散型函数，故其大小(也即其长度)分别为其对应时间与插补周期间的比值并取整，该第一次卷积窗h₁和第二次卷积窗h₂的高度则可固定取1。经上述方式得到的第一次卷积窗的大小可保证加速度不超限，经上述方式得到的第二次卷积窗的大小可保证加加速度不超限。

上述公式1中由于第一次卷积窗的时间t₁利用上述速度差ΔV求得，故可得到起止速度不为0的速度规划曲线。

S12：根据目标路线的位移S以及所需满足的卷积的时间要求，计算得到在所述目标路线实际可达的最大速度V_max1。

由于当目标路线的位移太小时存在达不到最大速度V_max的情况，在此情况下，S型速度曲线中没有匀速段，只有速度上升和速度下降段，故基于此条件，根据目标路线实际的位移S以及所需满足的卷积的时间要求，利用下述公式3重新计算出能够达到一次卷积的最大速度V_max1；

其中，V_min＝min(V_i,V_f)，所述t₂为所述第二次卷积窗的时间，所述S_c是补偿位移。该补偿位移S_c即为二次卷积后的信号面积(表示目标路线的位移)与原始信号面积之间的差，该补偿位移S_c可以由下述公式4得到。

S_c＝0.5*(V_i+V_f)*(t₁+t₂) (4)；

上述公式3表示所述目标路线所需满足的卷积的时间要求，其等号左边表示目标路线的原始信号的时间长度t₀，其等号右边表示一次卷积和二次卷积时间长度之和t₁+t₂，这样经过二次卷积后，刚好达到一个速度值，整个S型速度规划中没有匀速段。

结合上述公式1、4对上述公式3解二次方程得到下述公式5：

由上述公式5计算得到重解后的V_max1为在所述目标路线实际可达的最大速度V_max1，则在后续S型卷积规划按该最大值V_max1进行。

S13：比较所述实际可达的最大速度V_max1与所述最大速度V_max之间的大小，重新调整所述目标路线的最大速度V_max。

具体地，比较所述实际可达的最大速度V_max1与所述最大速度V_max之间的大小，并根据以下两种比较结果调整该目标路线的最大速度V_max。

A、当V_max1≥V_max，则维持所述目标路线的最大速度V_max不变。

当V_max1大于等于V_max时，S型卷积规划到达的速度则可按V_max来计算，故目标路线的最大速度V_max不需调整，维持不变。经下述步骤S14进行二次卷积后得到的S型速度曲线如图2。

B、当V_max1<V_max，则将所述目标路线的最大速度V_max调整为V_max1。

当V_max1小于V_max时，S型卷积规划到达的速度则不可按V_max来计算，故目标路线的最大速度V_max需调整为V_max1<V_max。具体该比较结果可进一步细分为以下：

B1、当max(V_i,V_f)<V_max1<V_max时，经下述步骤S14进行二次卷积后得到的S型速度曲线如图3；

B2、当max(V_i,V_f)＝V_max1时，经下述步骤S14进行二次卷积后得到的S型速度曲线如图4；

B3、当max(V_i,V_f)>V_max1时，经下述步骤S14进行二次卷积后得到的S型速度曲线如图5。此时，单段S型速度曲线难以满足要求，故通过2段S型速度曲线平滑的过渡，完成该S型速度规划。

其中，上述max(V_i,V_f)表示目标路线的初始速度V_i和结束速度V_f中的最大值。

S14：利用第一次卷积窗h₁和第二次卷积窗h₂对由调整后的目标路线的最大速度V_max形成的原始信号进行二次卷积，得到所述目标路线的S型速度曲线数据。

具体，该目标路线的原始信号y₀(k)表示以时间为自变量、所述目标路线的最大速度V_max为函数值。将该原始信号y₀(k)与该第一次卷积窗h₁(k)进行第一次卷积，并将其第一次卷积结果y₁(k)与第二次卷积窗h₂(k)进行第二次卷积，输出得到该目标路线的S型速度曲线数据y₂(k)，如图6所示。本实施例由于规划对象是根据插补周期T_s运行，故，该原始信号y₀(k)的大小为

t₀为其时间长度，该原始信号y₀(k)的高度为V_max；该第一次卷积窗h₁(k)的大小为

即t₁为第一次卷积窗时间；该第二次卷积窗h₂(k)的大小为

即t₂为第二次卷积窗时间。经该二次卷积得到的S型速度曲线信号中的离散速度序列y₂(k)。

在另一实施例中，可采用迭代递推的方式得到该S型速度输出曲线，以进一步减少计算量。具体如，利用以下公式6和7对所述目标路线的原始信号y₀(k)进行二次卷积得到S型速度曲线信号y₂(k)。

其中，k为maxh+1到maxh+L的整数，所述maxh为在卷积时原始信号补偿0的个数，所述L为所述二次卷积后的总时间长度t₀+t₁+t₂与插补周期T_s的比值，h₁为第一次卷积窗，h₂为第二次卷积窗，y₁(k)为第一次卷积输出的信号，y₂(k)为第二次卷积输出的表示S型速度曲线的信号。

这种迭代递推的方式相对直接二次卷积运算进一步减小了计算量，直接利用第一次卷积的结果计算第二次的卷积值，这种策略用一次循环便实现二次卷积，可以得到输出S型速度曲线的信号y₂(k)。

可以理解的是，本发明方法的步骤的描述顺序并不表示其执行的时序顺序，步骤S11(步骤a)虽限于步骤S12(步骤b)和步骤S13(步骤c)描述，但是实际上，步骤S11(步骤a)可在步骤S12(步骤b)和步骤S13(步骤c)之前、之间或之后执行，本发明对该步骤执行顺序不作限定。

本实施例中，采用二次卷积实现S型速度曲线规划，相对于传统的S型速度规划减少了计算量，而且在二次卷积前根据目标路线的唯一和所需满足的卷积时间要求重新计算目标路线可达到的最大速度，进而对目标路线的最大速度进行调整，以利用调整后的最大速度进行二次卷积，避免了目标路线由于位移小无法达到其设定的最大速度的情况，故能够兼容不同路线位移的情况。

请参阅图7，图7是本发明S型速度规划方法另一实施例的流程图。相比于上一实施例，本实施例方法还包括以下步骤：

S71：判断所述目标路线是否可进行S型速度规划。若可以，则执行上述S11，否则结束流程。

具体，可通过判断下述不等式8是否成立，若下述不等式8成立，则判断可进行S型速度规划，否则不可进行S型速度规划。

其中，公式8中的符号表示的意思如上述实施例描述。

在执行上述S11-S13之后，执行下述S72。

S72：根据插补周期Ts对第一次卷积窗h₁和第二次卷积窗h₂进行圆整。

由于规划对象按照插补周期运行，故上述第一次卷积窗和第二次卷积窗的时间t₁、t₂可能不是插补周期Ts的整数倍，此时会存在圆整误差，故本实施例执行下述S72-S74以实现消除该圆整误差。

具体地，该圆整策略为根据插补周期对第一次卷积窗和第二次卷积窗进行向上取整。即，利用下述公式9对第一次卷积窗h₁和第二次卷积窗h₂进行圆整。

S73：利用圆整后的第一次卷积窗h₁和第二次卷积窗h₂重新计算补偿位移S_c。

具体地，利用以下公式10和11对重新计算得到经圆整后的补偿位移S_c。

T₁＝h₁*Ts；T₂＝h₂*Ts (10)；

S_c＝0.5*(V_i+V_f)*(T₁+T₂) (11)；

其中，所述T₁+T₂为二次卷积后从初始速度V_i加速到最大速度V_max或从最大速度V_max下降到结束速度V_f的时间。其中，当t₁该为在卷积前从初始速度V_i加速到最大速度V_max的时间，即初始速度V_i小于结束速度V_f时，该T₁+T₂为二次卷积后从初始速度V_i加速到最大速度V_max的时间；当该t₂为在卷积前从最大速度V_max下降到结束速度V_f的时间，即初始速度V_i大于结束速度V_f时，该T₁+T₂为二次卷积后从最大速度V_max下降到结束速度V_f的时间。

S74：利用所述补偿位移S_c、位移误差ΔS与最大速度V’_max之间的关系，计算当位移误差为零时的最大速度V’_max以作为所述目标路线的最大速度V_max。

具体地，匀速运动段最大速度为V’_max，故匀速段位移确定，相应的插补总时间也确定。对插补周期的整数倍取整后，会有相应的位移误差ΔS。其中，所述补偿位移S_c、位移误差ΔS与最大速度V’_max之间的关系如下：

ΔS＝(S-S_c)-h₀*Ts*V’_max (12)

令上述公式12的位移误差ΔS为零，可得到下述公式13。利用公式13计算得到当位移误差为零时的最大速度V’_max。

其中，t₀＝(S-S_c)/V_max，

将所述最大速度V’_max作为所述目标路线的最大速度V_max。即，适当降低达到的最大速度至V’_max，使位移无偏差。

利用上述调整后的目标路线的最大速度V_max执行S14，以实现S型速度规划的圆整，并在执行完S14后，执行S75。

S75：根据所述离散速度序列计算得到对应的若干个插值位置，并根据所述插值位置控制对象在所述目标路线上进行操作。

本实施例中，在执行S14后得到S型速度曲线上的离散速度序列，计算出该序列中每个离散速度值对应的插补位置，具体可通过利用每个速度值计算出相邻插补位置间的插补距离，并累计该插补距离得到该插补位置。在得到插补位置后，控制机床或机器人手臂以插补周期在目标路线中的插补位置上运行。

可以理解的是，在其他实施例中，步骤S71和步骤S75中的任一个或两个可以被省略，也即不执行。

请参阅图8，图8是本发明S型速度规划方法再一实施例的部分流程图。由于图7所示的圆整方式通过调整速度上升或下降时间T₁+T₂和最大速度V_max来实现消除误差，而最大速度降低会影响加工效率。故，区别于图7所示的实施例，本实施例方法不执行上述S72-S74所述的圆整方式，而执行下面步骤S81-S84所述的位移补偿方式，以消除该圆整误差。

S81：根据插补周期Ts对所述第一次卷积窗h₁和第二次卷积窗h₂进行圆整。

S82：利用圆整后的所述第一次卷积窗h₁和第二次卷积窗h₂重新计算补偿位移S_c。

S83：利用所述补偿位移S_c与位移误差ΔS间的关系，计算得到位移误差ΔS。

该S81-82具体如上述S72-S73所描述，该S83具体可利用上述公式12计算得到位移误差ΔS。此时，该目标路线的最大速度即为上述S13所得的调整后的最大速度V_max，该公式12中的最大速度也为S13得到的调整后的最大速度V_max。

S84：在所述目标路线的原始信号前补充一个

的离散速度值，以作为新的得到目标路线的原始信号。

通过在原始信号前补充一个

的离散速度值，使卷积后的总位移为额定值。如图9所示，将原来的目标路线的原始信号y₀(k)的在对插补周期取整后的剩余部分的位移补偿到新的目标路线的原始信号y’₀(k)的在插补周期取整部分之前，故保证其总位移不变。

结合图9进行具体分析，由于原始信号y₀(k)存在不足一个补偿周期的阴影部分，无法进行离散卷积运算。本实施例将不足一个补偿周期的值，根据等效的面积(图中阴影部分面积)，补成一个补偿周期等面积的值，然后补偿到原始信号y₀(k)起始的位置得到新的原始信号y’₀(k)。经过上面实施例的卷积方法，可把补偿的面积平滑。这种方法，等效于利用卷积窗对于剩余的面积做平均，然后把平均的速度值放在速度上升开始时的区间内，二次卷积的平滑后，能够有效的降低速度波动，把波动控制在有效的范围内，故可得到S型速度曲线比较平滑，二次卷积后的加速度波动会显著地减小。其中，相对的卷积窗越大，平均效果越好，波动越小。

请参阅图10，图10是本发明S型速度规划方法又再一实施例的部分流程图。本实施例中，还包括以下步骤：

S101：获取输入的N个段点坐标。

其中，每相邻两个段点组成一段路线。

在实际操作中，数控系统的机床通常需要在若干段路线进行作业。由于位移限制，一段路线进行S型速度规划时，其临界的速度限制主要在只有一段速度上升或下降的阶段。故，本实施例对每段路线进行前瞻规划，以对每段路线的速度进行限制调整。在调整后，再对该每段路线进行上述实施所述的S型速度规划。

S102：根据所述N个段点的每相邻段点之间的位移、所述N个段点所在总路线的初始速度和结束速度计算得到每个段点的速度V(n)。

例如，N个段点坐标的每相邻段点Q(n)、Q(n+1)组成的路线为L(n)，其中，该n为1到N-1。该L(1)、L(2)、…、L(N-1)构成该N个段点组成的总路线L。该总路线L为以初始速度V(1)为0加速到最大速度，并从最大速度降速到结束速度V(N)为0的路线。利用N个段点坐标计算得到每段L(n)的位移，从初始速度为0开始，利用每段路线L(n)的位移，可分别计算得到每个段点Q(n)对应的速度V(n)，以使其加速到最大速度后降速到最后段点的结束速度为0。

S103：计算得到每段路线可达到的速度Vf_up。

具体地，假设每段路线L(n)的初始速度为V_i，终点速度为V_f。在给定的加速度A、加加速度Jerk情况下，相比较于梯形加减速，S型加减速比梯形加减速有延迟，延迟时间为A/Jerk。如图11所示的虚线与坐标轴围成的三角形面积Sn，即S型速度规划所走的路线L(n)的距离。

当V_f>V_i时，

当V_f<V_i时，

相应的在给定Sn(即路线L(n)的位移)，V_i，A，Jerk情况下，V_f有一定的限定。

(a)针对V_f>V_i情况，解关于V_f的方程，由公式14得到：

其中，

由于，

则V_f一定有实数解。

由于V_f>V_i，取正值。则

此时，V_f最大不能超过此值才能做S型速度规划。

(b)针对V_f<V_i情况，解关于V_f的方程，由公式15得到：

当

时，无实数解。此时，在0到V_i的区间内的速度都满足可达条件。

由于V_f<V_i，则

该Vf_down为所能降低的最小末速度值，使之能够完成一个S型速度规划。

(c)则根据前面公式推导，根据V_i，S，A，Jerk已知条件，能够完成一个S型速度规划，V_f的限制条件如下公式：

故，由上述公式17计算得到每段段点可达到的速度Vf_up。

S104：将n从1到N-1依次进行下述前向检验，并将n从N-1到2依次进行下述后向检验。

该前向检验如下：

从n＝1开始，检验V(n+1)是否大于V(n)。

当小于或等于时，直接将n加1并继续上述检验V(n+1)是否大于V(n)，直至该n为N-1，则结束该前向检验。

当大于时，判断当前路线是否可进行S型速度规划，具体可如上述S71所述进行判断。若不可进行S型速度规划，则将降低V(n+1)到上述S103得到的第n+1个段点可达到的速度Vf_up,如图12所示,并将n加1且继续上述检验V(n+1)是否大于V(n)，直至该n为N-1，则结束该前向检验；若可进行S型速度规划，则直接将n加1并继续上述检验V(n+1)是否大于V(n)，直至该n为N-1，则结束该前向检验。

其中，所述当前路线为第n段点与第n+1段点组成的路线。

该后向检验如下：

从n＝N-1开始，检验V(n-1)是否大于V(n)。

当小于或等于时，直接将n减1并继续上述检验V(n-1)是否大于V(n)，直至该n为2，则结束该后向检验。

当大于时，判断当前路线是否可进行S型速度规划，具体可如上述S71所述进行判断。若不可进行S型速度规划，则将降低V(n-1)到上述S103得到的第n-1个段点可达到的速度Vf_up,如图12所示，并将n减1并继续上述检验V(n-1)是否大于V(n)，直至该n为2，则结束该后向检验；若可进行S型速度规划，则直接将n减1并继续上述检验V(n-1)是否大于V(n)，直至该n为2，则结束该后向检验。

其中，所述当前路线为第n段点与第n-1段点组成的路线。

S105：分别将每相邻两个所述段点组成路线作为目标路线。

具体如，将上述路线L(1)、L(2)、…、L(N-1)分别作为上述实施例中的目标路线，并执行上述实施例步骤，以对该目标路线进行S型速度规划。

可以理解的是的，由于在上述前瞻方式的检验中已对是否可S型速度规划进行判断并相应调整，故后续可不执行上述S71重复判断是否可S型速度规划。

请参阅图13，图13是本发明S型速度规划装置一实施例的结构示意图。本实施例中，该装置130包括卷积窗计算模块131、速度获得模块132、速度调整模块133和卷积模块134。

卷积窗计算模块131用于利用目标路线的最大速度V_max、最大加速度A以及最大加加速度Jerk计算得到第一次卷积窗h₁和第二次卷积窗h₂；

速度获得模块132用于根据目标路线的位移S以及所需满足的卷积的时间要求，计算得到在所述目标路线实际可达的最大速度V_max1；

速度调整模块133用于比较所述实际可达的最大速度V_max1与所述最大速度V_max之间的大小，重新调整所述目标路线的最大速度V_max；

卷积模块134用于利用第一次卷积窗h₁和第二次卷积窗h₂对由调整后的目标路线的最大速度V_max形成的原始信号进行二次卷积，得到所述目标路线的S型速度曲线数据。

可选地，速度调整模块133具体用于比较所述实际可达的最大速度V_max1与所述最大速度V_max之间的大小；当V_max1≥V_max，则维持所述目标路线的最大速度V_max不变；当V_max1<V_max，则将所述目标路线的最大速度V_max调整为V_max1。

可选地，卷积模块134具体用于利用上述公式6和7对所述目标路线的原始信号y₀(k)进行二次卷积得到S型速度曲线信号y₂(k)。

可选地，速度获得模块132具体用于利用上述公式3计算得到在所述目标路线实际可达的最大速度V_max1。

可选地，卷积窗计算模块131具体用于利用上述公式1和2计算得到第一次卷积窗h₁和第二次卷积窗h₂。

请结合参阅图14，图14是本发明S型速度规划装置另一实施例的结构示意图。本实施例中，该装置140除包括图13所示的模块外，还包括判断模块141、圆整模块142、插值位置模块143、前瞻模块144。

判断模块141用于判断所述目标路线是否可进行S型速度规划，具体可利用上述公式8进行判断。该卷积窗计算模块131具体用于在目标路线可进行S型速度规划是，在执行上一实施例所述功能。

圆整模块142用于根据插补周期Ts对所述第一次卷积窗h₁和第二次卷积窗h₂进行圆整；利用圆整后的所述第一次卷积窗h₁和第二次卷积窗h₂重新计算补偿位移S_c；利用所述补偿位移S_c、位移误差ΔS与最大速度V’_max之间的关系，计算当位移误差为零时的最大速度V’_max以作为所述目标路线的最大速度V_max。

在另一实施例中，圆整模块142还可不执行上述功能，而用于：根据插补周期Ts对所述第一次卷积窗h₁和第二次卷积窗h₂进行圆整；利用圆整后的所述第一次卷积窗h₁和第二次卷积窗h₂重新计算补偿位移S_c；利用所述补偿位移S_c与位移误差ΔS间的关系，计算得到位移误差ΔS；在所述目标路线的原始信号前补充一个

的离散速度值，以作为新的得到目标路线的原始信号。

插值位置模块143用于根据所述离散速度序列计算得到对应的若干个插值位置，并根据所述插值位置控制对象在所述目标路线上进行操作。

前瞻模块144用于获取输入的N个段点坐标；根据所述N个段点的每相邻段点之间的位移、所述N个段点坐标的初始速度和结束速度计算得到每个段点的速度V(n)；将n从1到N-1依次进行上述前向检验；将n从N到2依次进行下上述后向检验。

上述装置的各个模块可分别执行上述方法实施例中对应步骤，故在此不对各模块进行赘述，详细请参阅以上对应步骤的说明。

请参阅图15，图15是本发明S型速度规划系统一实施例的结构示意图。该S型速度规划系统可以执行上述方法的步骤。相关内容请参见上述方法中的详细说明，在此不再赘叙。

该S型速度规划系统可具体为数控系统、工业机器人等。本实施例中，该S型速度规划系统150包括：处理器151，与处理器151连接的存储器152、驱动电机153，与驱动电机153连接的执行元件154。

该执行元件154也即上述实施例中的规划对象。例如为数控系统的机床、工业机器人的手臂等，该驱动电机153为驱动该机床或机器人的伺服电机等。

存储器152用于存储处理器151执行的计算机指令以及处理器151在进行下述处理过程中的数据，如目标路线的设定最大速度、最大加速度、最大加加速度等。

处理器151执行所述计算机指令，可用于：

利用目标路线的最大速度V_max、最大加速度A以及最大加加速度Jerk计算得到第一次卷积窗h₁和第二次卷积窗h₂；

根据目标路线的位移S以及所需满足的卷积的时间要求，计算得到在所述目标路线实际可达的最大速度V_max1；

比较所述实际可达的最大速度V_max1与所述最大速度V_max之间的大小，重新调整所述目标路线的最大速度V_max；

利用第一次卷积窗h₁和第二次卷积窗h₂对由调整后的目标路线的最大速度V_max形成的原始信号进行二次卷积，得到所述目标路线的S型速度曲线数据；

根据所述目标路线的S型速度曲线数据向所述驱动电机发送控制指令。

执行元件154用于在所述驱动电机153的驱动下在目标路线上按照所述S型速度曲线进行运作。

可选地，处理器151具体用于：比较所述实际可达的最大速度V_max1与所述最大速度V_max之间的大小；当V_max1≥V_max，则维持所述目标路线的最大速度V_max不变；当V_max1<V_max，则将所述目标路线的最大速度V_max调整为V_max1。

可选地，处理器151具体用于：利用上述公式6和7对所述目标路线的原始信号y₀(k)进行二次卷积得到S型速度曲线信号y₂(k)。

可选地，处理器151具体用于利用上述公式1、3和4计算得到在所述目标路线实际可达的最大速度V_max1。

可选地，处理器151具体利用上述公式1和2计算得到第一次卷积窗h₁和第二次卷积窗h₂。

可选地，处理器151还用于根据插补周期Ts对所述第一次卷积窗h₁和第二次卷积窗h₂进行圆整；利用圆整后的所述第一次卷积窗h₁和第二次卷积窗h₂重新计算补偿位移S_c；利用所述补偿位移S_c、位移误差ΔS与最大速度V’_max之间的关系，计算当位移误差为零时的最大速度V’_max以作为所述目标路线的最大速度V_max。

可选地，处理器151还用于：根据插补周期Ts对所述第一次卷积窗h₁和第二次卷积窗h₂进行圆整；利用圆整后的所述第一次卷积窗h₁和第二次卷积窗h₂重新计算补偿位移S_c；利用所述补偿位移S_c与位移误差ΔS间的关系，计算得到位移误差ΔS；在所述目标路线的原始信号前补充一个

的离散速度值，以作为新的得到目标路线的原始信号。

可选地，处理器151还用于：获取输入的N个段点坐标，其中，每相邻两个段点组成一段路线；根据所述N个段点的每相邻段点之间的位移、所述N个段点坐标的初始速度和结束速度计算得到每个段点的速度V(n)；将n从1到N-1依次进行上述前向检验；将n从N到2依次进行上述后向检验；分别将每相邻两个所述段点组成路线作为目标路线。

其中，处理器151进一步用于根据上述公式17计算得到每个段点可达到的速度Vf_up。

本发明还提供了一种机器人，请参阅图16，所述机器人160包括控制系统161和机器臂162，所述控制系统161根据上述的S型速度规划方法得到目标路线的S型速度曲线数据，并根据所述S型速度曲线数据控制所述机器臂162运行。

本发明还提供了一种数控机床，所述数控机床170包括数控系统171和机床172，所述数控系统171根据上述的S型速度规划方法得到目标路线的S型速度曲线数据，并根据所述S型速度曲线数据控制所述机床172进行加工。

以上方案，采用二次卷积实现S型速度曲线规划，相对于传统的S型速度规划减少了计算量，而且在二次卷积前根据目标路线的唯一和所需满足的卷积时间要求重新计算目标路线可达到的最大速度，进而对目标路线的最大速度进行调整，以利用调整后的最大速度进行二次卷积，避免了由于目标路线由于位移小无法达到其设定的最大速度的情况，故能够兼容不同路线位移的情况。

以上所述仅为本发明的实施方式，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。

Claims (27)

1.一种S型速度规划方法，其特征在于，包括以下步骤：

a、利用目标路线的最大速度V_max、最大加速度A以及最大加加速度Jerk计算得到第一次卷积窗h₁和第二次卷积窗h₂；

b、根据目标路线的位移S以及所需满足的卷积的时间要求，计算得到在所述目标路线实际可达的最大速度V_max1；

c、比较所述实际可达的最大速度V_max1与所述最大速度V_max之间的大小，重新调整所述目标路线的最大速度V_max；

d、利用第一次卷积窗h₁和第二次卷积窗h₂对由调整后的目标路线的最大速度V_max形成的原始信号进行二次卷积，得到所述目标路线的S型速度曲线数据。

2.如权利要求1所述的方法，其特征在于，所述步骤c具体包括：

比较所述实际可达的最大速度V_max1与所述最大速度V_max之间的大小；

当V_max1≥V_max，则维持所述目标路线的最大速度V_max不变；

当V_max1<V_max，则将所述目标路线的最大速度V_max调整为V_max1。

3.如权利要求1所述的方法，其特征在于，所述步骤d具体包括：

利用以下公式对所述目标路线的原始信号y₀(k)进行二次卷积得到S型速度曲线信号y₂(k)；

其中，k为maxh+1到maxh+L的整数，所述maxh为卷积信号补偿0的个数，所述L为所述二次卷积后的总时间长度与插补周期的比值，h₁为第一次卷积窗，h₂为第二次卷积窗，y₀(k)表示以时间为自变量、所述目标路线的最大速度V_max为函数值，y₁(k)为第一次卷积输出的信号，y₂(k)为第二次卷积输出的表示S型速度曲线的信号。

4.如权利要求1所述的方法，其特征在于，所述步骤b具体包括：

利用以下表示所述目标路线所需满足的卷积的时间要求的公式计算得到在所述目标路线实际可达的最大速度V_max1；

其中，V_min＝min(V_i,V_f)，所述V_i为该目标路线的起始速度，所述V_f为该目标路线的结束速度，所述t₂为所述第二次卷积窗的时间，所述S_c是补偿位移。

5.如权利要求1所述的方法，其特征在于，所述步骤a具体包括：

利用以下公式计算得到第一次卷积窗的时间t₁和第二次卷积窗的时间t₂；

其中，所述ΔV为最大速度V_max与V_min间的差，所述V_min＝min(V_i,V_f)，所述V_i为该目标路线的起始速度，所述V_f为该目标路线的结束速度；

并利用下述公式计算得到第一次卷积窗h₁和第二次卷积窗h₂；

其中，所述T_s为插补周期。

6.如权利要求1所述的方法，其特征在于，在所述步骤c之后，还包括：

根据插补周期Ts对所述第一次卷积窗h₁和第二次卷积窗h₂进行圆整；

利用圆整后的所述第一次卷积窗h₁和第二次卷积窗h₂重新计算补偿位移S_c；

利用所述补偿位移S_c、位移误差ΔS与最大速度V'_max之间的关系，计算当位移误差为零时的最大速度V'_max以作为所述目标路线的最大速度V_max。

7.如权利要求6所述的方法，其特征在于，所述根据插补周期Ts对所述第一次卷积窗h₁和第二次卷积窗h₂进行圆整，包括：

利用以下公式对所述第一次卷积窗h₁和第二次卷积窗h₂进行圆整；

其中，t₁为第一次卷积窗的时间，t₂为第二次卷积窗的时间，T_s为插补周期；

所述利用圆整后的所述第一次卷积窗h₁和第二次卷积窗h₂重新计算补偿位移S_c，包括：

利用以下公式对重新计算得到补偿位移S_c；

T₁＝h₁*Ts；T₂＝h₂*Ts；

S_c＝0.5*(V_i+V_f)*(T₁+T₂)；

其中，所述T₁+T₂为二次卷积后从初始速度加速到最大速度或从最大速度下降到结束速度的时间；

所述利用所述补偿位移S_c、位移误差ΔS与最大速度V'_max之间的关系，计算当位移误差为零时的最大速度V'_max以作为所述目标路线的最大速度V_max，包括：

利用以下公式计算得到当位移误差为零时的最大速度V’_max；

其中，t₀＝(S-S_c)/V_max，

将所述最大速度V'_max作为所述目标路线的最大速度V_max。

8.如权利要求1所述的方法，其特征在于，在所述步骤c之后，还包括：

根据插补周期Ts对所述第一次卷积窗h₁和第二次卷积窗h₂进行圆整；

利用圆整后的所述第一次卷积窗h₁和第二次卷积窗h₂重新计算补偿位移S_c；

利用所述补偿位移S_c与位移误差ΔS间的关系，计算得到位移误差ΔS；

在所述目标路线的原始信号前补充一个

的离散速度值，以作为新的得到目标路线的原始信号。

9.如权利要求8所述的方法，其特征在于，所述根据插补周期Ts对所述第一次卷积窗h₁和第二次卷积窗h₂进行圆整，包括：

利用以下公式对所述第一次卷积窗h₁和第二次卷积窗h₂进行圆整；

其中，t₁为第一次卷积窗的时间，t₂为第二次卷积窗的时间，T_s为插补周期；

所述利用圆整后的所述第一次卷积窗h₁和第二次卷积窗h₂重新计算补偿位移S_c，包括：

利用以下公式对重新计算得到补偿位移S_c；

T₁＝h₁*Ts；T₂＝h₂*Ts；

S_c＝0.5*(V_i+V_f)*(T₁+T₂)；

其中，所述T₁+T₂为二次卷积后从初始速度加速到最大速度或从最大速度下降到结束速度的时间；

所述利用所述补偿位移S_c与位移误差ΔS间的关系，计算得到位移误差ΔS，包括：

利用以下公式计算得到位移误差ΔS；

ΔS＝(S-S_c)-h₀*Ts*V_max；

其中，t₀＝(S-S_c)/V_max，

10.如权利要求1至9任一项所述的方法，其特征在于，所述步骤d得到的为S型速度曲线上的离散速度序列；

所述步骤d之后，还包括：

根据所述离散速度序列计算得到对应的若干个插值位置，并根据所述插值位置控制对象在所述目标路线上进行操作。

11.如权利要求1至9任一项所述的方法，其特征在于，在步骤a之前，还包括：

判断所述目标路线是否可进行S型速度规划；

若是，则执行所述步骤a，否则结束流程。

12.如权利要求1至9任一项所述的方法，其特征在于，还包括：

获取输入的N个段点坐标，其中，每相邻两个段点组成一段路线；

根据所述N个段点的每相邻段点之间的位移、所述N个段点坐标的初始速度和结束速度计算得到每个段点的速度V(n)；

将n从1到N-1依次进行下述前向检验：检验V(n+1)是否大于V(n)；当大于时，判断当前路线是否可进行S型速度规划；若不可以，则将降低V(n+1)到对应段点可达到的速度；其中，所述当前路线为第n段点与第n+1段点组成的路线；

将n从N到2依次进行下述后向检验：检验V(n-1)是否大于V(n)；当大于时，判断当前路线是否可进行S型速度规划；若不可以，则将降低V(n-1)到对应段点可达到的速度；其中，所述当前路线为第n段点与第n-1段点组成的路线；

分别将每相邻两个所述段点组成路线作为目标路线。

13.如权利要求12所述的方法，其特征在于，还包括：

根据以下公式计算得到每个段点可达到的速度Vf_up；

14.如权利要求12所述的方法，其特征在于，所述判断当前路线是否可进行S型速度规划，包括：

判断以下不等式是否成立，若成立，则可进行S型速度规划，否则不可进行S型速度规划；

15.一种S型速度规划装置，其特征在于，包括：

卷积窗计算模块，用于利用目标路线的最大速度V_max、最大加速度A以及最大加加速度Jerk计算得到第一次卷积窗h₁和第二次卷积窗h₂；

速度获得模块，用于根据目标路线的位移S以及所需满足的卷积的时间要求，计算得到在所述目标路线实际可达的最大速度V_max1；

速度调整模块，用于比较所述实际可达的最大速度V_max1与所述最大速度V_max之间的大小，重新调整所述目标路线的最大速度V_max；

卷积模块，用于利用第一次卷积窗h₁和第二次卷积窗h₂对由调整后的目标路线的最大速度V_max形成的原始信号进行二次卷积，得到所述目标路线的S型速度曲线数据。

16.一种S型速度规划系统，其特征在于，包括执行元件、驱动电机、处理器和存储器；

所述存储器用于存储计算机指令以及所述处理器处理的数据；

所述处理器执行所述计算机指令，用于：

利用目标路线的最大速度V_max、最大加速度A以及最大加加速度Jerk计算得到第一次卷积窗h₁和第二次卷积窗h₂；

根据目标路线的位移S以及所需满足的卷积的时间要求，计算得到在所述目标路线实际可达的最大速度V_max1；

比较所述实际可达的最大速度V_max1与所述最大速度V_max之间的大小，重新调整所述目标路线的最大速度V_max；

利用第一次卷积窗h₁和第二次卷积窗h₂对由调整后的目标路线的最大速度V_max形成的原始信号进行二次卷积，得到所述目标路线的S型速度曲线数据；

根据所述目标路线的S型速度曲线数据向所述驱动电机发送控制指令；

所述执行元件用于在所述驱动电机的驱动下在目标路线上按照所述S型速度曲线进行运作。

17.如权利要求16所述的系统，其特征在于，所述处理器具体用于：

比较所述实际可达的最大速度V_max1与所述最大速度V_max之间的大小；

当V_max1≥V_max，则维持所述目标路线的最大速度V_max不变；

当V_max1<V_max，则将所述目标路线的最大速度V_max调整为V_max1。

18.如权利要求16所述的系统，其特征在于，所述处理器具体用于：

利用以下公式对所述目标路线的原始信号y₀(k)进行二次卷积得到S型速度曲线信号y₂(k)；

其中，k为maxh+1到maxh+L的整数，所述maxh为卷积信号补偿0的个数，所述L为所述二次卷积后的总时间长度与插补周期的比值，h₁为第一次卷积窗，h₂为第二次卷积窗，y₀(k)表示以时间为自变量、所述目标路线的最大速度V_max为函数值，y₁(k)为第一次卷积输出的信号，y₂(k)为第二次卷积输出的表示S型速度曲线的信号。

19.如权利要求16所述的系统，其特征在于，所述处理器具体用于：

利用以下表示所述目标路线所需满足的卷积的时间要求的公式计算得到在所述目标路线实际可达的最大速度V_max1；

其中，V_min＝min(V_i,V_f)，所述V_i为该目标路线的起始速度，所述V_f为该目标路线的结束速度，所述t₂为所述第二次卷积窗的时间，所述S_c是补偿位移。

20.如权利要求16所述的系统，其特征在于，所述处理器具体用于：

利用以下公式计算得到第一次卷积窗的时间t₁和第二次卷积窗的时间t₂；

其中，所述ΔV为最大速度V_max与V_min间的差，所述V_min＝min(V_i,V_f)，所述V_i为该目标路线的起始速度，所述V_f为该目标路线的结束速度；

并利用下述公式计算得到第一次卷积窗h₁和第二次卷积窗h₂；

其中，所述T_s为插补周期。

21.如权利要求16所述的系统，其特征在于，所述处理器还用于：

根据插补周期Ts对所述第一次卷积窗h₁和第二次卷积窗h₂进行圆整；

利用圆整后的所述第一次卷积窗h₁和第二次卷积窗h₂重新计算补偿位移S_c；

利用所述补偿位移S_c、位移误差ΔS与最大速度V_m'_ax之间的关系，计算当位移误差为零时的最大速度V_m'_ax以作为所述目标路线的最大速度V_max。

22.如权利要求16所述的系统，其特征在于，所述处理器还用于：

根据插补周期Ts对所述第一次卷积窗h₁和第二次卷积窗h₂进行圆整；

利用圆整后的所述第一次卷积窗h₁和第二次卷积窗h₂重新计算补偿位移S_c；

利用所述补偿位移S_c与位移误差ΔS间的关系，计算得到位移误差ΔS；

在所述目标路线的原始信号前补充一个

的离散速度值，以作为新的得到目标路线的原始信号。

23.如权利要求16至22任一项所述的系统，其特征在于，所述处理器还用于：

获取输入的N个段点坐标，其中，每相邻两个段点组成一段路线；

根据所述N个段点的每相邻段点之间的位移、所述N个段点坐标的初始速度和结束速度计算得到每个段点的速度V(n)；

将n从1到N-1依次进行下述前向检验：检验V(n+1)是否大于V(n)；当大于时，判断当前路线是否可进行S型速度规划；若不可以，则将降低V(n+1)到对应段点可达到的速度；其中，所述当前路线为第n段点与第n+1段点组成的路线；

将n从N到2依次进行下述后向检验：检验V(n-1)是否大于V(n)；当大于时，判断当前路线是否可进行S型速度规划；若不可以，则将降低V(n-1)到对应段点可达到的速度；其中，所述当前路线为第n段点与第n-1段点组成的路线；

分别将每相邻两个所述段点组成路线作为目标路线。

24.如权利要求23所述的系统，其特征在于，还包括：

根据以下公式计算得到每个段点可达到的速度Vf_up；

25.如权利要求23所述的系统，其特征在于，所述判断所述当前路线是否可进行S型速度规划，包括：

判断以下等式是否成立，若成立，则可进行S型速度规划，否则不可进行S型速度规划；

26.一种应用S型速度规划方法的机器人，其特征在于，所述机器人包括控制系统和机器臂，所述控制系统根据权利要求1至14任一项所述的S型速度规划方法得到目标路线的S型速度曲线数据，并根据所述S型速度曲线数据控制所述机器臂运行。

27.一种应用S型速度规划方法的数控机床，其特征在于，所述数控机床包括数控系统和机床，所述数控系统根据权利要求1至14任一项所述的S型速度规划方法得到目标路线的S型速度曲线数据，并根据所述S型速度曲线数据控制所述机床进行加工。

Images