一种基于指定速度的参数自适应S形速度规划插补方法与流程

本发明属于机器人运动控制领域，尤其涉及一种运动控制速度规划插补方法，具体涉及一种基于指定速度的参数自适应S形速度规划插补方法。

背景技术：

目前，依靠运动控制器(卡)与伺服驱动器通讯来实现机器人或伺服电机的运动，已经成为目前各大主流机器人、运动控制器厂商的选择，其原理是运动控制器器通过不断插补运算得到机器人/伺服电机的位置、速度、加速度数据，传送给伺服驱动器来实现电机运转，机器人运动。其中，对机器人的速度进行规划及插补，既实现能机器人平稳起停、又能节省加减速时间，一直是一个难以解决的问题。

目前，各大厂商利用的主要方法为基于梯形速度规划的插补算法，主要是插补得到的位置连续可导、速度连续、但其主要缺点是加速度不连续，存在冲击，尤其不适用于机器人高速运行环境，另一种主要方法为不可自适应或局部自适应的S形速度规划方法，可以实现平稳起停、但是其初始参数不可动态调整，导致如果设定的参数不合理，比如设定位移L较短，或者设定运行速度较大时，机器人或电机就停止运行，报错，不能够自动调整初始参数，使得作业停止，用户需要重新设置，效率和易用性相对较差。

技术实现要素：

为了解决上述技术问题，本发明提出了一种可靠的运动控制机器人速度规划插补方法。该方法一共包括6大类情形，细分为23种插补类型，规划算法容易编程实现，可靠快速，运动起停平稳，无冲击，具备参数自调整功能；且留有最大加速度和加加速度调整接口，用户可以根据运动系统结构的强度和刚度调整最大加速度Accmax和加加速度Jerk的大小，在结构强度刚度允许的前提下，提高最大加速度和加加速度的值，达到节省加减速时间的效果。

本发明采用的技术方案如下：

第一步，根据输入的速度规划参数(Vs—初始速度,V—运行速度，Ve—终点速度，L—指定的位移量，Accmax—最大加速度，Jerk—加加速度)；判断是否只存在匀速插补，如果是的话，则进入情形1，如果不存在匀速插补，则转到第二步。

第二步，计算位移L1和L2(L1—从Vs变速到V的位移量加上从V变速到Ve的位移量；L2—不经过速度V从Vs直接变速到Ve的位移量)。

第三步，判断是否存在匀速插补，即是否满足L1<L,如果满足，则存在匀速段，进入情形2，否则，不存在匀速段，进入第四步；

第四步，然后判断L1>＝L且L2>L是否同时满足，如果是，则需要对最初设定的末速度Ve重新规划，得到新的终点速度Venew,进入情形3，如果否，则进入第五步。

第五步，需要对指定的运行速度V进行重新规划，得到新的运行速度Vnew，如果最初设定的运行速度V大于起始速度Vs和末速度Ve，则加速过程中能够达到的最大速度Vmax小于最初设定的运行速度V，进入情形4；否则进入第六步；

第六步，如果最初设定运行速度V小于初始速度Vs和末速度Ve，则以[V,min(Vs,Ve)]为取值区间，迭代求解得到更新的运行速度Vnew，运动过程可分为前变速段(Vs减速到Vnew)和后变速段(从Vnew增速到Ve)，进入情形5；如果运行速度V介于Vs和Ve之间，则进入情形6。

其特征在于：情形内容如下：

情形1：属于插补插补类型1(全程只有匀速插补)。

情形2：判断是否存在匀加速段，如果存在匀加速段，则进入步骤2.1，否则，则进入步骤2.2。

步骤2.1：如果存在匀加速段，则接着判断是否存在存在匀减速段，如果存在匀减速段，则属于插补类型2(按照插补顺序依次为①-加加速段，②--匀加速段，③--减加速段，④--匀速段，⑤--加减速段，⑥--匀减速段，⑦--减减速段)；如果不存在匀减速段，则属于插补类型3(按照插补顺序依次为①-加加速段，②--匀加速段，③--减加速段，④--匀速段，⑤--加减速段，⑥--减减速段)；

步骤2.2：判断后续是否存在匀减速段，如果是，则属于插补类型4(按照插补顺序依次为①-加加速段，②--减加速段，③--匀速段，④--加减速段，⑤--匀减速段，⑥--减减速段)；如果否，则属于插补类型5(按照插补顺序依次为①-加加速段，②--减加速段，③--匀速段，④--加减速段，⑤--减减速段)。

情形3：判断Vs<Venew是否满足，如果满足，进入步骤3.1，如果不满足则进入步骤3.2。

步骤3.1：判断是否存在匀加速段，如果是，则属于插补类型6(按照插补顺序依次为①-加加速段，②--匀加速段，③--减加速段)；如果否，则属于插补类型7(按照插补顺序依次为①-加加速段，②--减加速段)。

步骤3.2：判断是否存在匀减速段，如果是，则属于插补类型8(按照插补顺序依次为，①--加减速段，②--匀减速段，③--减减速段)；如果否，则属于插补类型9(按照插补顺序依次为①--加减速段，②--减减速段)。

情形4：判断Ve>Vs是否满足，如果满足，则进入步骤4.1，否则进入步骤4.2。

步骤4.1：判断是否存在匀减速段，如果是，则属于插补类型10(按照插补顺序依次为①-加加速段，②--匀加速段，③--减加速段，④--加减速段，⑤--匀减速段，⑥--减减速段)；如果否，则继续判断是否存在匀加速段，如果存在匀加速段，则属于插补类型11(按照插补顺序依次为①-加加速段，②--匀加速段，③--减加速段，④--加减速段，⑤--减减速段)；如果不存在匀加速段，则属于插补类型12(按照插补顺序依次为①-加加速段，②--减加速段，③--加减速段，④--减减速段)。

步骤4.2：判断是否存在匀加速段，如果是，则属于插补类型13(按照插补顺序依次为①-加加速段，②--匀加速段，③--减加速段，④--加减速段，⑤--匀减速段，⑥--减减速段)；如果否，则继续判断是否存在匀减速段，如果是，则属于插补类型14(按照插补顺序依次为①-加加速段，②--减加速段，③--加减速段，④--匀减速段，⑤--减减速段)，如果否则属于插补类型15(按照插补顺序依次为①-加加速段，②--减加速段，③--加减速段，④--减减速段)。

情形5：判断前变速段是否存在匀减速段，如果是，则进入步骤5.1，如果否，则进入步骤5.2。

步骤5.1：判断后变速段是否存在匀加速段，如果是，则属于插补类型16(①-加减速段，②--匀减速段，③--减减速段，④--加加速段，⑤--匀加速段，⑥--减加速段)；如果否，则属于插补类型17(①-加减速段，②--匀减速段，③--减减速段，④--加加速段，⑤--减加速段)

步骤5.2:判断后变速段是否存在匀加速段，如果是，则属于插补类型18(①-加减速段，②--减减速段，③--加加速段，④--匀加速段，⑤--减加速段)；如果否，则属于插补类型19(①-加减速段，②--减减速段，③--加加速段，④--减加速段)。

情形6：判断是否满足Vs<Ve,如果是，进入步骤6.1；如果否，进入步骤6.2.

步骤6.1：为了节省运动时间，将匀速段以速度为Ve运行，插入到变速段之后，然后判断是否存在匀加速段，如果是，则属于插补类型20(①-加加速段，②--匀加速段，③--减加速段，④--匀速段)；如果否，则属于插补类型21(①-加加速段，②--减加速段，③--匀速段)。

步骤6.2：为了节省运动时间，将匀速段以速度为Vs运行，插入到匀速段之间，然后判断是否存在匀减速段，如果是，则属于插补类型22(①-匀速段，②--加减速段，③--匀减速段，④--减减速段)；如果否，则属于插补类型23(①-匀速段，②--加减速段，③--减减速段)。

【附图说明】

此处所说明的附图是用来提供对本发明的进一步理解，构成本申请的一部分，但并不构成对本发明的不当限定，在附图中：

图1是本发明的方法流程图。

图2是本发明情形1的流程示意图。

图3是本发明情形2的流程示意图。

图4是本发明情形3的流程示意图。

图5是本发明情形4的流程示意图。

图6是本发明情形5的流程示意图。

图7是本发明情形6的流程示意图。

图8是本发明插补类型2的速度插补结果图。

【具体实施方式】

下面将结合附图以及具体实施例来详细说明本发明，其中的示意性实施例以及说明仅用来解释本发明，但并不作为对本发明的限定。

首先，本发明将插补分为23种类型，具体描述如下：

插补类型1：全程匀速插补；

插补类型2：按照插补顺序依次为①加加速段，②匀加速段，③减加速段，④匀速段，⑤加减速段，⑥匀减速段，⑦减减速段；

插补类型3：按照插补顺序依次为①加加速段，②匀加速段，③减加速段，④匀速段，⑤加减速段，⑥减减速段；

插补类型4：按照插补顺序依次为①加加速段，②减加速段，③匀速段，④加减速段，⑤匀减速段，⑥减减速段；

插补类型5：按照插补顺序依次为①加加速段，②减加速段，③匀速段，④加减速段，⑤减减速段；

插补类型6：按照插补顺序依次为①加加速段，②匀加速段，③减加速段；

插补类型7：按照插补顺序依次为①加加速段，②减加速段；

插补类型8：按照插补顺序依次为①加减速段，②匀减速段，③减减速段；

插补类型9：按照插补顺序依次为①加减速段，②减减速段；

插补类型10：按照插补顺序依次为①加加速段，②匀加速段，③减加速段，④加减速段，⑤匀减速段，⑥减减速段；

插补类型11：按照插补顺序依次为①加加速段，②匀加速段，③减加速段，④加减速段，⑤减减速段；

插补类型12：按照插补顺序依次为①加加速段，②减加速段，③加减速段，④减减速段；

插补类型13：按照插补顺序依次为①加加速段，②匀加速段，③减加速段，④加减速段，⑤匀减速段，⑥减减速段；

插补类型14：按照插补顺序依次为①加加速段，②减加速段，③加减速段，④匀减速段，⑤减减速段；

插补类型15：按照插补顺序依次为①加加速段，②减加速段，③加减速段，④减减速段；

插补类型16：按照插补顺序依次为①加减速段，②匀减速段，③减减速段，④加加速段，⑤匀加速段，⑥减加速段；

插补类型17：按照插补顺序依次为①加减速段，②匀减速段，③减减速段，④加加速段，⑤减加速段；

插补类型18：按照插补顺序依次为①加减速段，②减减速段，③加加速段，④匀加速段，⑤减加速段；

插补类型19：按照插补顺序依次为①加减速段，②减减速段，③加加速段，④减加速段；

插补类型20：按照插补顺序依次为①加加速段，②匀加速段，③减加速段，④匀速段；

插补类型21：按照插补顺序依次为①加加速段，②减加速段，③匀速段；

插补类型22：按照插补顺序依次为①匀速段，②加减速段，③匀减速段，④减减速段；

插补类型23：按照插补顺序依次为①匀速段，②加减速段，③减减速段。

上述23种插补类型，限定了速度规划时的插补结果，基于这23种插补类型，本发明提出了参数自适应的插补方法，参见附图1，其示出了本发明方法的基本流程图，具体说明如下：

(1)根据输入的速度规划参数，判断是否只存在匀速插补，如果是的话，则进入情形1，如果不是，则转到步骤(2)。

其中，所述速度规划参数包括初始速度Vs、运行速度V、终点速度Ve和指定的位移量L。

(2)计算从Vs变速到V的位移量与从V变速到Ve的位移量之和L1；计算不经过速度V从Vs直接变速到Ve的位移量L2。

(3)判断是否存在匀速插补，即是否满足L1＜L，如果满足，则存在匀速段，进入情形2，如果不满足，则不存在匀速段，进入步骤(4)。

(4)判断L1≥L且L2＞L是否同时满足，如果是，则对最初设定的终点速度V重新规划，得到新的终点速度Venew，进入情形3，如果不是，则进入步骤(5)。

(5)对指定的运行速度V进行重新规划，得到新的运行速度，如果最初设定的运行速度V大于初始速度Vs和终点速度Ve，则加速过程中能够达到的最大速度Vmax小于最初设定的运行速度V，进入情形4；否则进入步骤(6)。

(6)如果最初设定运行速度V小于初始速度Vs和终点速度Ve，则以[V，min(Vs,Ve)]为取值区间，迭代求解得到更新的运行速度Vnew，将运动过程分为前变速段(即Vs减速到Vnew)和后变速段(即从Vnew增速到Ve)，进入情形5；如果运行速度V介于Vs和Ve之间，则进入情形6。

所述情形1-6是基于不同的具体情况，为速度规划设置不同的插补类型，具体说明如下：

情形1(参见图2)：设置速度规划为插补类型1；

情形2(参见图3)：判断是否存在匀加速段，如果存在，则进入步骤2.1，否则进入步骤2.2；

步骤2.1：判断是否存在匀减速段，如果存在匀减速段，则设置速度规划为插补类型2；如果不存在匀减速段，则设置速度规划为插补类型3；

步骤2.2：判断是否存在匀减速段，如果存在，则设置速度规划为插补类型4；否则设置为插补类型5；

情形3(参见图4)：判断是否满足Vs＜Venew，如果满足，进入步骤3.1，如果不满足则进入步骤3.2。

步骤3.1：判断是否存在匀加速段，如果存在，则设置速度规划为插补类型6，否则设置为插补类型7；

步骤3.2：判断是否存在匀减速段，如果存在，则设置速度规划为插补类型8，否则设置为插补类型9。

情形4(参见图5)：判断是否满足Ve＞Vs，如果满足，则进入步骤4.1，否则进入步骤4.2.

步骤4.1：判断是否存在匀减速段，如果存在匀减速段，则设置速度规划为插补类型10；如果不存在匀减速段，则继续判断是否存在匀加速段，如果存在匀加速段，则设置速度规划为插补类型11；如果不存在匀加速段，则设置为插补类型12；

步骤4.2：判断是否存在匀加速段，如果存在匀加速段，则设置速度规划为插补类型13；如果不存在匀加速段，则继续判断是否存在匀减速段，如果存在匀减速段，则设置速度规划为插补类型14；如果不存在匀减速段，则设置为插补类型15。

情形5(参见图6)：判断前变速段是否存在匀减速段，如果存在，则进入步骤5.1，如果不存在，则进入步骤5.2。

步骤5.1：判断后变速段是否存在匀加速段，如果存在，则设置速度规划为插补类型16；否则设置为插补类型17；

步骤5.2：判断后变速段是否存在匀加速段，如果存在，则设置速度规划为插补类型18；否则设置为插补类型19。

情形6(参见图7)：判断是否满足Vs＜Ve，如果满足，则进入步骤6.1，如果不满足，进入步骤6.2。

步骤6.1：为了节省运动时间，设置匀速段以速度Ve运行，并插入到变速段之后，然后判断是否存在匀加速段，如果存在，则设置速度规划为插补类型20；否则设置为插补类型21；

步骤6.2：为了节省运动时间，设置匀速段以速度为Vs运行，插入到变速段之前，然后判断是否存在匀减速段，如果存在，则设置速度规划为插补类型22；否则设置为插补类型23。

参见附图8，其示出了插补类型2，一个标准的七段式插补类型。其中S(t)、v(t)、acc(t)，J(t)分别表示位置、速度、加速度、加加速度关于时间的插补结果曲线，由图8可知，位置、速度连续可调、加加速度连续、运动无冲击。

通过本发明的上述方法，一方面可以让机器人运动平稳无冲击，而且在系统刚度强度允许的情况下，可以通过调大Accmax和Jerk的值，得到较小加减速时间，提高效率。

以上所述仅是本发明的较佳实施方式，故凡依本发明专利申请范围所述的构造、特征及原理所做的等效变化或修饰，均包括于本发明专利申请范围内。