[发明专利]一种速度规划方法

权利要求书

1.一种速度规划方法，其特征在于，包括：

根据第一约束条件和路径规划参数，获取所述路径上对应点的速度约束，生成速度约束信息；其中，所述第一约束条件包括：机器人关节速度约束、加速度约束、加加速度约束、雅克比矩阵及其导数；

根据第二约束条件和所述速度约束信息，以预设步长对所述路径进行分段速度规划，获取规划样条的控制点；其中，所述第二约束条件包括：运动的总路程、最大加速度、最大加加速度、最大减加速度；

根据所述控制点，以三次B样条曲线为规划曲线，生成第一速度规划结果；

其中，所述根据第二约束条件和所述速度约束信息，以预设步长对所述路径进行分段速度规划，获取规划样条的控制点，具体包括：

选取目标规划点，并获取所述目标规划点对应的初始信息；其中所述初始信息包括：所述目标规划点对应的速度和加速度；

根据所述目标规划点、所述路径和所述预设步长确定下一规划点，并根据所述速度约束信息确定所述下一规划点对应的速度约束值；

根据所述初始信息和所述最大加加速度，确定所述下一规划点的第一速度、第一加速度和第一加加速度；

当所述第一速度小于所述下一规划点对应的速度约束值，将所述目标规划点列为所述规划样条控制点，记录所述下一规划点的第一加速度和第一加加速度，将所述下一规划点设置为目标规划点，循环计算至路径终点；

当所述第一速度大于所述下一规划点对应的速度约束值，则根据所述初始信息和所述最大减加速度的近似加加速度，确定满足所述加加速度约束及加速度约束条件下的所述下一规划点的第二速度、第二加速度和第二加加速度；根据所述第二速度与所述速度约束值的大小比较，确定所述规划样条控制点。

2.根据权利要求1所述的速度规划方法，其特征在于，所述根据第二速度与所述速度约束值的大小比较，确定所述规划样条控制点，具体包括：

当所述第二速度小于所述下一规划点对应的速度约束值，将所述目标规划点列为所述规划样条控制点，并利用所述下一规划点对应的速度约束值更新所述下一规划点的第二速度；记录所述下一规划点的第二加速度和第二加加速度，将所述下一规划点设置为目标规划点，循环计算至路径终点；

当所述第二速度大于所述下一规划点对应的速度约束值，以给定步长向上回溯至新目标规划点进行速度规划；其中，所述新目标规划点对应的加加速度大于最大减加速度，所述给定步长为所述预设步长。

3.根据权利要求1所述的速度规划方法，其特征在于，所述根据目标规划点、所述路径和所述预设步长确定下一规划点，具体包括：

判断目标规划点以所述预设步长确定的规划点是否在所述路径上，获取判断结果；

若所述判断结果为所述规划点在所述路径上，则将所述规划点确认为所述下一规划点；

若所述判断结果为所述规划点未在所述路径上，则根据所述路径的终点的速度信息确定所述下一规划点。

4.根据权利要求3所述的速度规划方法，其特征在于，所述则根据所述路径的终点的速度信息确定所述下一规划点，具体包括:

将所述路径的终点作为末端规划点，获取所述末端规划点的末速度和末加速度，判断得到的末速度和末加速度是否满足系统加加速度的约束，获取判断结果；

当判断结果为满足时，则将所述末端规划点列为所述规划样条控制点，结束速度规划；

当判断结果为不满足时，以给定步长向上回溯至新目标规划点进行速度规划；其中，所述给定步长为路程总长度减去所述目标规划点对应的路程长度。

5.根据权利要求2或4任意一项所述的 速度规划方法，其特征在于，所述以给定步长向上回溯至新目标规划点进行速度规划，具体包括：

以给定步长，从所述目标规划点回溯至上一规划点；

若所述上一规划点已列入控制点，且所述上一规划点对应的加加速度大于所述最大减加速度，将所述上一规划点设置为新目标规划点；

根据所述新目标规划点对应的速度、加速度及减小的加加速度计算并更新下一规划点对应的第一速度及第一加速度；

将所述下一规划点撤出控制点列，并将所述下一规划点设置为目标规划点进行速度规划。