一种定插补周期的速度规划方法与流程

本发明属于工业数控设备自动化技术领域，涉及一种应用于定插补周期的多轴数控设备的速度规划方法。

背景技术：

在当今的工业领域中，随着工业自动化进程的加速，以及日益激烈的市场竞争，使得大量的生产制造商不得不寻求更高效的生产方式，以求得生存。并且伴随着人们物质生活水平的提高，人工劳动的成本也随之上涨，这使得传统的劳动密集型的生产方式难以维持。所以为了在竞争激烈的市场大环境下更好的发展，提高生产效率，降低生产成本，机器换人成为了必然的趋势。随着国内中小企业对中低端运动控制设备的需求越来越大，多轴机械运动控制平台备受青睐，如模板切割机、喷墨机、裁床等。

如今，所述运动控制平台的控制系统也日趋成熟，目前许多运动控制平台采用梯形速度曲线规划方法，但在实际计算中，轨迹插补周期往往不是整数倍，这对控制速度及精度会带来较大的影响，尤其是在定插补周期的前提下，每段轨迹中必须保证是整数插补周期，且不能有较大的速度突变，因此更难进行速度规划。针对上述问题，本发明提出一种定插补周期的梯型曲线速度规划方法，可有效实现机械运动控制平台速度的平稳控制；很好地解决了对于速度规划插补计算中最后一个插补周期内速度突变的问题，实现了速度的平稳过渡，提高了控制精度。

技术实现要素：

为了克服现有在定插补周期的前提下,对梯形速度曲线进行速度规划在最后速度突变问题的不足，本发明提供了一种定插补周期的速度规划方法，在保证整数个插补周期的前提下，对最后两个插补周期速度突变的问题提出了较好的解决方案。

本发明解决其技术问题所采用的技术方案是：

一种定插补周期的速度规划方法，包括如下步骤：

第一步，以中间点作为拐点对轨迹进行分段，进行速度规划；设置速度规划过程中的限定参数：每一个轨迹轮廓的起始速度vstart、运动过程中的最大速度vmax、最大加速度amax以及插补周期t；

第二步，根据第一步设定好的限定参数，根据每个拐点处的前后三个点的坐标pi-1(xi-1,yi-1)、pi(xi,yi)、pi+1(xi+1,yi+1)，起点和终点除外，计算对应拐点处的夹角θi，然后根据拐点处速度与夹角的关系式，确定该拐点的最大允许速度vcormax_i，i＝2、3···n-1。再采用倒推法，从轨迹的最后一点开始计算，将最后一点的速度vcor_n设置成启动速度，即vcor_n＝vstart，然后从最后一点开始向倒数第二点，以最大加速度做匀加速运动，计算运动到倒数第二点时的速度vcor_(n-1)，n为轨迹上的拐点数，将其与该点允许的最大拐点速度vcormax_(n-1)比较，取较小的为该拐点速度；以此类推计算其他拐点的速度vcor_i，i＝1、2···n；

第三步，根据起始点和各拐点处的速度对每段轨迹进行速度规划；

根据第二步计算所得轨迹上各拐点处的速度vcor_i，以及根据每段轨迹的起始点的x、y坐标值，定义其中差值较大的轴作为长轴，则另一个轴就为短轴，以长轴作为参考轴来计算；从起点开始，根据梯形速度曲线对每段轨迹作加减速规划，计算每个插补周期t时间内的脉冲数；再根据每段轨迹的起点对应的长轴坐标值，计算每个插补周期后的末点对应轴的坐标值，结合长短轴之间的关系，计算出相应短轴的坐标值，实现两周同步；再根据实际规划出的每段的末速度vi_n与之前的对应拐点速度vcor_i比较，取较小者为下一段的起始速度；

第四步，根据以上计算，每段轨迹插补周期都是非整数倍；

比较最后两个插补周期的脉冲数，通过将之前的各个插补周期内的脉冲数减小，使最后两个插补周期的差值变小，最后两个插补周期的脉冲数spn-1和spn，n为当前段轨迹的插补周期数。第一次通过使前n-1个插补周期内的脉冲数都减1，从而使spn增大，再比较spn-1和spn，如果spn-1>spn，则再从第2个插补周期到第n-1个插补周期内的脉冲数都减1，以此类推，直到spn-1＝spn。

进一步，所述第二步中，根据每个拐点处的前后三个点的坐标pi-1(xi-1,yi-1)、pi(xi,yi)、pi+1(xi+1,yi+1)，计算出当前拐点夹角θi：

通过反三角函数求得θi的值，再根据拐点处速度公式：

计算出每个拐点的限制速度vcormax_i；根据轨迹上各拐点的坐标值计算出每一段轨迹的长度si：

各拐点速度值vcor_i：

其中，n为轨迹上拐点数，再将所计算得到的拐点速度vcor_i与拐点处的最大限制速度vcormax_i比较，如果vcor_i>vcormax_i，则将vcormax_i赋值给vcor_i，否则就取计算所得到的拐点速度值。

再进一步，所述第三步中，先根据每段轨迹的起始位置点的x轴、y轴坐标差值，取差值较大的轴为长轴，另一个轴就为短轴；再根据梯形速度曲线的特点，在长轴上做速度规划，每个插补周期t内作匀速运动，每个插补周期段内的起始速度vj

如果处于加速段，则

vj＝vj-1+amax*t

如果处于减速段，则

vj＝vj-1-amax*t

如果处于匀速段，则

vj＝vmax

那么，一个插补周期内电机运动的脉冲数spj为：

spj＝vj*t

再根据当前轨迹的斜率计算出相应短轴的脉冲数，以t时间进行速度规划，得到当前轨迹的末速度vn，将vn与之前该拐点的速度vcor_i进行比较，取较小的速度值作为下一段轨迹的起始速度。

本发明的有益效果主要表现在：在对初始限定参数设定后，每一段最后不足一个插补周期内的速度突变的问题得到了很好的解决，使每一段轨迹之间都平缓的过渡。

附图说明

图1为定插补周期速度规划方法的程序流程图。

图2为某一个拐点夹角示意图。

图3为某一个轮廓轨迹示意图。

图4为轮廓轨迹其中一段的加减速规划及优化对比示意图。

具体实施方案

下面结合附图以及具体实例对本发明作进一步描述。

结合图1～图4，一种定插补周期的速度规划方法，包括以下步骤：

第一步，设置一些规划过程中的限定参数：每一个轨迹轮廓的起始速度vstart、运动过程中的最大速度vmax、最大加速度amax以及插补周期t；

第二步，根据第一步设定好的限定参数，根据第一步设定好的限定参数，根据每个拐点出的前后三个点的坐标pi-1(xi-1,yi-1)、pi(xi,yi)、pi+1(xi+1,yi+1)，起点和终点除外，计算对应拐点处的夹角θi，然后根据拐点处速度与夹角的关系式，确定该拐点的最大允许速度vcormax_i，i＝2、3···n-1。采用倒推法，从轨迹的最后一点开始计算，将最后一点的速度vcor_n设置成启动速度，即vcor_n＝vstart，然后从最后一点开始向倒数第二点，以最大加速度做匀加速运动，计算运动到倒数第二点时的速度vcor_(n-1)，将其与该点允许的最大拐点速度vcormax_(n-1)比较，取较小的为该拐点速度。以此类推计算其他拐点的速度；

如图2、图3所示，根据夹角公式，分别计算出第2、3、4、5点处的夹角θ2、θ3、θ4、θ5，再根据拐点速度公式分别确定各拐点处的最大限制速度：

根据轨迹上各拐点的坐标值计算出每一段轨迹的长度si：

各拐点速度值vcor_i：

vcor_1＝vcor_6＝vstart

再将所计算得到的拐点速度vcor_i与拐点处的最大限制速度vcormax_i比较，如果vcor_i>vcormax_i，则将vcormax_i赋值给vcor_i，否则就取计算所得到的拐点速度值；

第三步，根据起始点和各拐点处的速度对每段轨迹进行速度规划。

如图3、图4所示，根据第二步计算所得轨迹上各拐点处的速度vcor_i(i＝1、2···6)，选取1，2两点为例进行说明，先根据1、2两点的x、y轴坐标差值，取差值较大的x轴为长轴。再根据梯形速度曲线的特点，在长轴上做速度规划，每个插补周期t内作匀速运动，每个插补周期段内的起始速度vj为：

v0＝vstart

v1＝v0+amax*t

v2＝v1+amax*t

···

v8＝v7+amax*t

v9＝v8-amax*t

v10＝v9-amax*t

vend1＝v10-amax*t

如果有匀速段

vj＝vmax

一个插补周期内电机在长轴x轴上运动的脉冲数spj

spj＝vj*t

再根据当前轨迹的斜率计算出相应y轴的脉冲数，实现两轴同步。一直以t时间进行速度规划，可以得到当前轨迹的末速度vend1，将vend1与之前对应拐点的速度vcor_i进行比较，取较小的速度值作为下一段轨迹的起始速度。

第四步，根据以上计算，往往每段轨迹插补周期都是非整数倍，根据实际计算结果，解决每段轨迹的最后一小段不足一个插补周期的速度突变问题。如图4上部所示，假设第一段有10个完整的插补周期，最后剩余0.6t。所以在每一段的最后不足一个插补周期的时间内，速度容易发生较大的突变，使得电机出现抖动。

所以通过减小前面插补周期内的脉冲数，以减小最后两个插补周期的脉冲数的差值，从而防止电机出现抖动。最后两个插补周期的脉冲数sp10和sp11，第一次通过使sp1～sp10的值减1，相当于sp11增加了1，再比较sp10和sp11，如果sp10>sp11，则再从第2个插补周期到第10个插补周期内的脉冲数sp2～sp10的值减1，以此类推，直到sp10＝sp11，如图4下部所示。