基于S形曲线机器人过渡轨迹规划的拐角加速度优化方法及装置与流程

本发明涉及机器人的路径规划技术。

背景技术：

轨迹规划是保证机器人实现稳定运动的重要技术。机器人运动轨迹通常有点到点、直线，圆弧等轨迹，在实际应用中单条轨迹往往不能满足任务需求，而必须依靠多条轨迹衔接而成。在机器人的过渡轨迹规划中，基于向量的合成方法可以满足过渡轨迹的速度约束，但难以满足其加速度约束，从而会导致机器人在运行过程中产生机械振动。

技术实现要素：

本发明所要解决的技术问题在于提供一种基于s形曲线机器人过渡轨迹规划的拐角加速度优化方法及其装置，其能使基于s形曲线的机器人过渡轨迹规划中通过矢量合成法确定的过渡轨迹的加速度满足加速度约束的要求，并使轨迹过渡时的降速小。

本发明实施例提供了一种基于s形曲线机器人过渡轨迹规划的拐角加速度优化方法，该基于s形曲线机器人过渡轨迹规划通过矢量合成法确定轨迹p0p1与轨迹p1p2之间的过渡轨迹pspe的切向加速度，其中，基于s形曲线机器人过渡轨迹规划的拐角加速度优化方法包括以下步骤：

判断过渡轨迹pspe的最大的切向加速度是否大于dmax1和amax2中较小的那一者；dmax1为工程约束条件规定的轨迹p0p1的最大减速度，amax2为工程约束条件规定的轨迹p1p2的最大加速度；

若过渡轨迹pspe的最大的切向加速度大于dmax1和amax2中较小的那一者，则通过遗传算法计算轨迹p0p1的最优减速度a1、以及轨迹p1p2的最优加速度a2，以使过渡轨迹pspe的最大切向加速度的值在小于等于dmax1和amax2中较小的那一者的情况下达到最大。

本发明还提供了一种基于s形曲线机器人过渡轨迹规划的拐角加速度优化装置，包括：存储器，用于存储可由处理器执行的指令；处理器，用于执行所述指令以实现上述的方法。

本发明通过遗传算法计算轨迹p0p1的最优减速度a1、以及轨迹p1p2的最优加速度a2，使得轨迹p0p1与轨迹p1p2之间的过渡轨迹pspe的最大切向加速度的值在小于等于dmax1和amax2中较小的那一者的情况下达到最大化，从而可以使机器人在两段轨迹之间进行过渡时速度降速最小，且不超过加速度限制，对机器人高速运动具有重要意义。

附图说明

图1示出了基于s形曲线机器人过渡轨迹规划的过渡轨迹的示例图。

图2示出了s形速度规划曲线的示意图。

图3示出了根据轨迹p0p1和轨迹p1p2的s形速度曲线对圆滑参数进行调整的原理示意图。

图4示出了用矢量合成法确定过渡轨迹的速度、加速度的示意图。

图5示出了采用根据本发明实施例的拐角加速度优化方法的机器人过渡轨迹规划的流程示意图。

具体实施方式

根据本发明一实施例的一种基于s形曲线机器人过渡轨迹规划的拐角加速度优化方法，其中，基于s形曲线机器人过渡轨迹规划通过矢量合成法确定轨迹p0p1与轨迹p1p2之间的过渡轨迹pspe的切向加速度，该机器人过渡轨迹规划的拐角加速度优化方法包括以下步骤：

判断过渡轨迹pspe的最大的切向加速度是否大于dmax1和amax2中较小的那一者；dmax1为工程约束条件规定的轨迹p0p1的最大减速度，amax2为工程约束条件规定的轨迹p1p2的最大加速度；

若过渡轨迹pspe的最大的切向加速度大于dmax1和amax2中较小的那一者，则通过遗传算法计算轨迹p0p1的最优减速度a1、以及轨迹p1p2的最优加速度a2，以使过渡轨迹pspe的最大切向加速度的值在小于等于dmax1和amax2中较小的那一者的情况下达到最大。

图1示出了基于s型曲线机器人过渡轨迹规划的过渡轨迹的示例图。在图1的示例中，轨迹p0p1与轨迹p1p2均为直线，二者圆滑过渡。在进行基于s形曲线的机器人过渡轨迹规划时，需要根据给定的运动参数以及工程约束条件，确定轨迹p0p1与轨迹p1p2之间的过渡轨迹的起点位置和终点位置。

运动参数包括轨迹p0p1的起始点p0的位置、终点p1的位置、轨迹p1p2的终点p2的位置、以及圆滑参数q。工程约束条件包括轨迹p0p1的最大加速度amax1、最大减速度dmax1、以及轨迹p1p2的最大加速度amax2、最大减速度dmax2。将轨迹p0p1与轨迹p1p2按照s形曲线加减速(如图2以及公式(1)～(4)所示)分别进行速度规划，轨迹p0p1的速度达到最大速度vmax1乘以圆滑参数q时为进入圆滑的条件，ps为过渡轨迹的起点，此时轨迹p1p2开始运动，过渡轨迹当成轨迹p0p1与轨迹p1p2的矢量合成，当轨迹p0p1运动完成时，过渡轨迹完成，即图1中的pe点，pe点为过渡路径的结束点。

请参阅图3。如图3中的左边一幅图所示，如果根据给定的圆滑参数q完成了轨迹p0p1的减速段的圆滑后，超出了轨迹p1p2的最大速度，那么还需要对圆滑参数q做出调整。此时根据轨迹p1p2的最大速度反推进入圆滑(过渡)时的速度，如图3中的右边一幅图所示，轨迹p0p1进入减速段后，再经过td-t3时间即可进入圆滑区域，最后根据进入圆滑时的速度推导出圆滑参数。td、ta分别为轨迹p0p1的减速段时间和轨迹p1p2的加速段时间，t2为调整圆滑参数q之前的圆滑时间，t3为调整圆滑参数q之后的圆滑时间。

请参阅图4。根据矢量合成法，过渡轨迹pspe在时刻t的速度v是轨迹p0p1在时刻t的速度v1和轨迹p1p2在时刻t的速度v2的矢量合成，过渡轨迹pspe在时刻t的加速度a是轨迹p0p1在时刻t的减速度a1和轨迹p1p2在时刻t的加速度a2的矢量合成。圆滑时第一条轨迹p0p1处于减速度段，第二条轨迹p1p2处于加速段，矢量合成后的加速度可能会超出给定的加速度限制。因此需要计算圆滑后最大的切向加速度，若过渡轨迹pspe的最大的切向加速度小于等于dmax1和amax2中较小的那一者，则无需调整，若大于dmax1和amax2中较小的那一者，则采用遗传算法调整轨迹p0p1的最优减速度a1、以及轨迹p1p2的最优加速度a2，以使过渡轨迹pspe的最大切向加速度的值在小于等于dmax1和amax2中较小的那一者的情况下达到最大化。

过渡轨迹pspe在时刻t的切向加速度可用公式(5)表示：

at(t)＝a1*cosθ+a2*sin(π-α-θ)(5)

其中

θ为v与v1之间的夹角，α为v1与v2之间的夹角，ts为进入过渡轨迹的时刻，te为过渡轨迹结束的时刻。

若要以最快速度通过圆滑区域而加速度又不超出限制，可在区间[ts,te]上求出满足以下公式(6)的a1和a2，使at最大。

amax≥a1*cosθ+a2*sin(π-α-θ)(6)

上述方程为非线性方方程，本实施例中采用遗传算法求得最优的a1和a2。本实施例中，采用遗传算法计算轨迹p0p1的最优减速度a1、以及轨迹p1p2的最优加速度a2包括以下步骤：

以待计算的轨迹p0p1的最优减速度a1、以及轨迹p1p2的最优加速度a2作为种群中的一个个体的基因，根据实际情况确定种群数量，生成初始种群；

计算种群中的每一个个体的适应度；

迭代地进行遗传操作，生成新种群，计算新种群中每个个体的适应度，直至满足预设的迭代终止条件，输出满足迭代终止条件时的最优解作为轨迹p0p1的最优减速度a1、以及轨迹p1p2的最优加速度a2的取值，其中，迭代终止时将进化过程中所得到的具有最大适应度的个体作为最优解输出。

上述的遗传操作包括选择、交叉和变异，生成新种群的过程包括：根据上一代种群中的各个个体的适应度，利用轮盘赌规则从上一代种群中选择进行下一次进化的个体，将选择出的个体作为母体，通过交叉操作得到新的个体，对选择出的个体进行变异操作，得到新个体。

图5示出了采用根据本发明实施例的拐角加速度优化方法的机器人过渡轨迹规划的流程示意图。

具体而言，在本实施例中，通过遗传算法计算轨迹p0p1的最优减速度a1、以及轨迹p1p2的最优加速度a2包括以下步骤：

a、构建求解目标函数f及约束条件；

求解目标函数f为：

f＝max(at(t))

约束条件为：

at(t)表示过渡轨迹pspe在时刻t的切向加速度，ts为进入过渡轨迹的时刻，te为过渡轨迹结束的时刻；

b、初始化种群；

p＝{p¹,p²,p³,...,p^pop_size}

其中：pop_size表示种群大小，pⁱ表示个体，表示轨迹p0p1的最优减速度,在轨迹p0p1的减速度限制范围内随机产生，表示轨迹p1p2的最优加速度,在轨迹p1p2的加速度限制范围内随机产生；

c、计算适应度

对于种群中每一个个体pⁱ，利用以下计算其适应度：

d、选择

根据步骤c的适应度计算结果，采用轮盘赌选择，令种群中第i个个体的适应度为gi(i＝1,2,…,pop_size)，根据轮盘赌算法，第i个个体被选中的概率qi为：

e、交叉

其中，为交叉之后的个体，为随机选择的个体(为经过轮盘赌选择后的种群中选择的个体),α为预设的交叉常数，α的取值为(0,1]；

f、变异

其中为变异后的个体，为变异之前的个体，k为预设的变异常数，k的取值为(0,1]，pmax、pmin分别为预设的个体的上限和下限，rand()为随机函数，r为随机函数产生的随机数；

g、终止条件判断

判断新形成的种群中的个体是否已满足终止条件，若已满足终止条件则将最后得到的具有最大适应度的个体对作为轨迹p0p1的最优减速度a1、以及轨迹p1p2的最优加速度a2的取值；终止条件为求解目标函数f与min(dmax1，amax2)之间的偏差小于预设的终止偏差或者该种群的遗传代数达到预设的终止遗传代数。

本发明还提供了一种基于s形曲线机器人过渡轨迹规划的拐角加速度优化装置，包括：存储器，用于存储可由处理器执行的指令；处理器，用于执行所述指令以实现上述的方法。

显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。