基于S型速度规划的时间最优在线轨迹规划方法与流程

本发明涉及机器人运动规划领域，特别涉及一种基于s型速度规划的时间最优在线轨迹规划方法。

背景技术：

机器人控制器在执行一指定任务时，会给出运动目标位置点的位置、速度信息，即目标运动状态；同时，控制器在开始任务时，会收集机器人当前所在的位置、速度、加速度信息，即初始运动状态。而在执行任务的过程中，考虑不同应用不同工艺要求，通常还需要明确一定的运动约束条件，例如此运动允许的最大速度等。

目前，按照一定初始运动状态和目标运动状态以及约束条件进行机器人轨迹规划的方式主要有两种：

1)给定首末运动约束，用多项式方法规划。多项式规划通常需要运动提前确定运动时间t，而不需要运动过程中的最大速度，加速度等信息。但是多项式规划难以限制过程中的运动状态是否超过约束条件，且难以保证路径的形状。

2)给定首末运动约束，用s型速度规划方法规划。该方法将一个运动过程分为：加加速运动、匀加速运动、减加速运动、匀速运动、加减速运动、匀减速运动、减减速运动共七个阶段。这种规划方法保证了加速度的连续性，使机器人在开始运动、达到最高速和结束运动时，都能够实现速度的平稳过渡。但现有的s规划方法通常都针对初始加速度为0的情况。而在运动已经开始后，如果运动约束发生变化，例如需要更快的速度，或者运动目标发生变化，例如目标点位置变化，此时需要实时的重新规划，才能确保完成任务，但运动已经发生后，就可能会存在初始加速度值，这时便无法用传统的s规划方法来实现。

技术实现要素：

针对现有技术的不足，本发明提供一种基于s型速度规划的时间最优在线轨迹规划方法，能够实现机器人在运动过程中的实时轨迹规划。

对s型规划而言，需要首先确定加速度曲线的类型，只需要将加速度的形状确定，即可把整个轨迹的类型确定，然后即可根据曲线中加速度的变化过程进一步求解轨迹参数。

在实时轨迹规划时，初始加速度a0往往并不为0，而规划的目标加速度期望为0。在这一前提下，按照s型速度规划的基本思路，加速度曲线共有16种可能的情况。考虑到a0>0和a0<0的情况对称，本发明只考虑a0>0时的情况，a0<0的情况可以通过将轨迹参数取反得到求解，因而可能的加速度曲线共有8种类型，如附图1所示，每种类型概括描述为：

类型1：存在匀加速、匀减速过程、但没有匀速过程；

类型2：存在匀加速过程、但没有匀速、匀减速过程；

类型3，存在匀减速过程、但没有匀速、匀加速过程；

类型4，不存在匀速、匀加速、匀减速过程；

类型5，存在匀加速、匀速、匀减速过程；

类型6，存在匀加速、匀速、但没有匀减速过程；

类型7，存在匀速、匀减速、但没有匀加速过程；

类型8，存在匀速、但没有匀加速、匀减速过程。

在时间最优的前提下，机器人按最大加减速能力运动，通过比较以最大加加速度jm运动到最大加速度am时的速度vmax、运动约束条件中的最大速度vmax等，可以判断出可能的加速度曲线类型。根据该曲线中加速度的变化趋势，可以得到相应的一组关于时间、加速度、速度、位移等的运动方程，即轨迹方程，求解该轨迹方程中的未知数，即可得到完整的轨迹规划。

由此，本公开提供一种基于s型速度规划的时间最优在线轨迹规划方法，其特征在于，包括以下步骤：

s1：输入机器人当前的初始运动状态和目标运动状态，包括初始位置p0、初始速度v0、初始加速度a0、目标位置pt、目标速度vt、目标加速度at，及运动约束条件，包括最大速度vm、最大加速度am、最大加加速度jm；

s2：计算临界值，包括：在轨迹足够长的情况下，从所述初始运动状态以加加速度jm使加速度增加到am时的速度vmax，从所述初始运动状态以加加速度jm使加速度降为0时的速度vzer，以及加速度从0以加加速度jm减小到-am再增加到0的过程中速度的变化量δv(δv<0)；

s3：假设轨迹足够长，比较vm和vmax、以及vm+δv和vt的大小，判断本段轨迹的加速度曲线类型；

s4：按照所述步骤s3得到的加速度曲线类型，得到相应的轨迹方程组，求解其中的轨迹参数并输出；

s5：如果所述步骤s4中的轨迹方程无解，则假设轨迹足够短，即一定不存在匀速段，根据vt与vzer及vzer+δv的大小来判断本段轨迹的加速度曲线类型，根据得到的加速度曲线类型，得到相应的轨迹方程组，求解其中的轨迹参数并输出，如果依然无解，则确定在所述输入条件下的时间最优轨迹规划无解。

其中，所述运动约束条件为假设约束值都对称的情况，实际上也可以不对称。另外，还包含如下前提：

(1)目标运动加速度at＝0；

(2)初始运动状态v0<＝abs(vm)，a0<＝abs(am)；

(3)以初始速度v0、初始加速度a0的状态下，加速度以加加速度jm降为0时的速度vzer，vzer<＝abs(vm)。

优选地，所述步骤s2中临界值vmax、vzer及δv的计算方法为：

优选地，所述步骤s3中判断加速度曲线类型的具体方法为：

如果vmax<vm且vt<vm+δv，判定加速度曲线存在匀加速、匀速、匀减速过程，即类型5；

如果vmax<vm且vt>vm+δv，判定加速度曲线存在匀加速、匀速、但没有匀减速过程，即类型6；

如果vmax>vm且vt<vm+δv，判定加速度曲线存在匀速、匀减速、但没有匀加速过程，即类型7；

如果vmax>vm且vt>vm+δv，判定加速度曲线存在匀速、但没有匀加速、匀减速过程，即类型8。

优选地，所述步骤s5中判断加速度曲线类型的具体方法为：

如果vt<vzer且vt>vzer+δv，判定加速度曲线不存在匀速、匀加速、匀减速过程，即类型4；

如果vt<vzer且vt<vzer+δv，判定加速度曲线存在匀减速过程、但没有匀速、匀加速过程，即类型3；

如果vt>vzer且vt>vzer+δv，判定加速度曲线存在匀加速过程、但没有匀速、匀减速过程，即类型2；

如果vt>vzer且vt<vzer+δv，判定加速度曲线存在匀加速、匀减速过程、但没有匀速过程，即类型1。

优选地，所述步骤s4和s5中求解轨迹参数的方法为：

根据得到的加速度曲线类型i，得到相应的位移误差函数fi(x)及其变量x的取值范围[xi1，xi2](i＝1…，8)，其中，所述位移误差函数fi(x)为根据轨迹方程组得到的、以其中一个未知的轨迹参数x为变量表示的完成本段轨迹所需的位移值δp，其中，轨迹参数x的取值范围应能根据加速度曲线类型和所述步骤s1的输入条件确定；

然后用计算机数值求解的方法，查找在取值范围[xi1，xi2]内，能否找到一个值x0满足fi(x0)＝pt-p0，如果能，则轨迹参数x取值为x0，进一步根据参数x与其它未知参数的关系，得到其它轨迹参数的值。

本发明中，位移误差函数的变量x应根据每种加速度曲线的特点确定，同时，考虑每种加速度曲线对应的轨迹方程也不同，因而每种类型的轨迹，位移误差函数的表达式也不同。

通常，在判断出可行的加速度曲线类型后，即可列出以时间、加速度、速度、位移为变量的一组轨迹方程，方程的个数与未知数的个数相同，可以采用传统的矩阵法等其他多种方法求解。但这种方法在方程数量较多的情况下，效率较低，且受数值精度影响，会进一步降低求解效率。因此，作为优选方案，可以采用效率更高的通过位移误差函数求解的方法。

本发明中基于s型速度规划的时间最优在线轨迹规划方法，按照时间最优原则，通过计算在以最大加减速能力运动情况下能够得到的运动状态，并与运动约束条件对比，能够分析判断任意初始加速度情况下的加速度曲线类型，进而求解轨迹参数；而位移误差函数的引入，进一步提高了轨迹参数的求解效率，从而使得传统的s型速度规划法可以扩展应用于初始加速度不为0情况下的在线轨迹规划。与现有技术相比，本公开的有益效果是：保证在约束条件内，一定是时间最优的；可用于在线实时调整轨迹，可任意输入初始加速度；用过程参数来表示位移误差函数，通过对误差函数的分析来求解线性方程组，稳定高效。

附图说明

通过结合附图对本公开示例性实施例进行更详细的描述，本公开的上述以及其它目的、特征和优势将变得更加明显，其中，在本公开示例性实施例方式中，相同的参考标号通常代表相同部件。

图1显示初始加速度a0>0时的各种s规划加速度曲线类型；

图2显示示例性实施例的基于s型速度规划的在线轨迹规划方法流程图；

图3显示示例性实施例中加速度曲线为类型8时相应的速度和位置变化趋势图，其中a为加速度，v为速度，p为位置；

图4显示示例性实施例中得到的规划轨迹。

具体实施方式

下面将参照附图更详细地描述本公开的优选实施例。虽然附图中显示了本公开的优选实施例，然而应该理解，可以以各种形式实现本公开而不应被这里阐述的实施例所限制。相反，提供这些实施例是为了使本公开更加透彻和完整，并且能够将本公开的范围完整地传达给本领域的技术人员。

附图2中给出了基于s型速度规划的在线轨迹规划方法示例性实施例的流程图，包括以下步骤：

s1：输入机器人当前的初始运动状态和目标运动状态。包括初始位置p0、初始速度v0、初始加速度a0、目标位置pt、目标速度vt、目标加速度at，及运动约束条件，包括最大速度vm、最大加速度am、最大加加速度jm；

s2：计算临界值。包括：在轨迹足够长的情况下，从所述初始运动状态以加加速度jm使加速度增加到am时的速度vmax，从所述初始运动状态以加加速度jm使加速度降为0时的速度vzer，以及加速度从0以加加速度jm减小到-am再增加到0的过程中速度的变化量δv(δv<0)；

s3：假设轨迹足够长，判断本段轨迹的加速度曲线类型；

s4：按照得到的加速度曲线类型，得到相应的位移误差函数，求解各轨迹参数并输出。

s5：如果步骤s4中的位移误差函数无解，则假设轨迹足够短，判断本段轨迹的加速度曲线类型。

s6：根据s5得到的加速度曲线类型，得到相应的位移误差函数，求解各轨迹参数并输出，如果依然无解，则确定在所述输入条件下的时间最优轨迹规划无解。

其中，临界值vmax、vzer及δv的计算方法为：

作为优选方案，步骤s3中判断加速度曲线类型的具体方法为：

如果vmax<vm且vt<vm+δv，判定为类型5；

如果vmax<vm且vt>vm+δv，判定为类型6；

如果vmax>vm且vt<vm+δv，判定为类型7；

如果vmax>vm且vt>vm+δv，判定为类型8。

作为优选方案，步骤s5中判断加速度曲线类型的具体方法为：

如果vt<vzer且vt>vzer+δv，判定为类型4；

如果vt<vzer且vt<vzer+δv，判定为类型3；

如果vt>vzer且vt>vzer+δv，判定为类型2；

如果vt>vzer且vt<vzer+δv，判定为类型1。

作为优选方案，本实施例中采用了效率更高的通过位移误差函数求解的方法。以判定的加速度曲线为类型8为例进行进一步说明。

附图3给出了加速度曲线为类型8时的加速度、速度、位置随时间变化的趋势示意图。

根据图中的运动关系，可以列出轨迹方程如下：

上述方程中未知数为t1,t2,t3,t4,a1,a2,v1,v2,v3,p1,p2,p3。p4的期望值为机器人的目标运动位置pt。

根据上述方程，可以得到用加速段达到的最大速度a1表示的完成本段轨迹规划所需的最短位移

然后用计算机数值求解的方法，如果a1在[a0,am]间能找到满足f8(a1)＝pt-p0的值，则说明该规划成立，进而根据a1与其它未知数的关系，即可解出其它轨迹参数的值。此规划满足运动约束，且保证时间最优。

根据示例性实施例的基于s型速度规划的时间最优在线轨迹规划方法，按照时间最优原则，通过将最大加减速能力运动情况下的临界值与运动约束条件对比，能够分析判断任意初始加速度情况下的加速度曲线类型，然后通过位移误差函数，进一步提高了轨迹参数的求解效率，从而使得传统的s型速度规划法可以扩展应用于初始加速度不为0情况下的在线轨迹规划。与现有技术相比，本公开的有益效果是：可以在机器人运动过程中根据约束条件或目标运动状态的变化而实时更新已有的轨迹规划，并保证时间最优。

应用示例

输入机器人初始状态p0＝600，v0＝330，a0＝400，目标状态pt＝850，vt＝200，约束条件vm＝900，am＝1000，jm＝3000；

计算临界值vzer＝357，vmax＝637，δv＝-333；

假设轨迹足够长，由于vmax<vm，δv>vt-vm，故轨迹应该是类型5；

计算该类型对应的位移误差函数，其变量为加速段达到的最大加速度，取值范围为[400，1000]，发现位移误差函数在[400,1000]区间内全部大于(pt-p0)，说明计算得到的位移要大于期望位移，故而假设不成立，该轨迹一定不存在匀速段；

假设轨迹足够短，根据vzer>vt，δv<vt-vzer，轨迹应该是类型4，进一步计算位移误差函数，发现当a1＝559.1时，f(a1)＝pt-p0，由此可以得到整个规划。规划轨迹以及位移误差函数取值范围如附图4。

上述技术方案只是本发明的示例性实施例，对于本领域内的技术人员而言，在本发明公开了应用方法和原理的基础上，很容易做出各种类型的改进或变形，而不仅限于本发明上述具体实施例所描述的方法，因此前面描述的方式只是优选的，而并不具有限制性的意义。