[发明专利]一种基于指定速度的参数自适应S形速度规划插补方法

权利要求书

1.一种速度规划插补方法，其特征在于，该方法应用于机器人的运动控制，包括以下步骤：

（1）根据输入的速度规划参数，判断是否只存在匀速插补，如果是的话，则进入情形1，如果不是，则转到步骤（2）；其中，所述速度规划参数包括初始速度Vs、运行速度V、终点速度Ve、指定的位移量L、最大加速度Accmax和加加速度Jerk；

（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-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-23设置如下：

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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