CN103324141B - 一种高精度变插补周期的多轴联动运动控制方法 - Google Patents

Abstract

一种高精度变插补周期的多轴联动运动控制方法，包括以下步骤：控制系统中采用了指数规律加减速算法建立加速度表和速度表，再结合电机参数建立步长表，继而根据本系统的速度规划得到的起始和结束速度求出加速、匀速、减速阶段各自所需的长度；首先确定固定插补时间T，计算X轴的第i个插补周期中的脉冲数n_xi，再计算Y轴的第i个插补周期中的脉冲数n_yi。本发明提出了一种基于DSP的高精度变插补周期的多轴联动运动控制方法，同步性良好、精确性较高。

Description

一种高精度变插补周期的多轴联动运动控制方法

技术领域

本发明属于运动控制技术领域，尤其是一种多轴联动的运动控制方法。

背景技术

在机械人技术领域中，多轴联动运动控制器是目前得到广泛实际应用的自动化机械装置，在工业制造、医学治疗、娱乐服务、军事、半导体制造等领域都能见到它的身影。尽管它们的形态各有不同，但它们都有一个共同的特点，精确地定位到某一点进行相应的工作。而国内对多轴联动运动控制器的相关需求也在逐渐变大。

发明内容

为了克服现有的多轴联动的运动控制方式的同步性较差、精确性较低的不足，本发明提出了一种变插补周期的多轴联动运动控制方法，主要解决脉冲发送及计算误差、定时器误差，同步性良好、精确性较高。

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

一种高精度变插补周期的多轴联动运动控制方法，所述方法包括以下步骤：

1）设定采用指数规律加减速算法建立加速度表α₀,α₂,...,α_L,及其对应的速度表V₀,V₂,...,V_L，利用指数规律加减速算法与电机参数相结合建立步长表l₀,l₂,...,l_L；当X轴为长轴时，当前线段L足够长，结合X轴的速度规划得到的起始速度v₀和结束速度v₁，计算得到X轴加速、减速、匀速阶段所需长度为N_xi0、N_xi2、N_xi1；Y轴为长轴时每一个阶段运动的长度计算方法与X轴相同；

2）首先确定固定插补时间T，计算X轴的第i个插补周期中的脉冲数n_xi：

n_xi=T*V_xe                           （1）

其中，V_xe是当前X轴的速度的值，将n_xi圆整后得到n'_xi，然后反过来计算X轴的第i个插补周期T_xi：

T_xi=n'_xi/V_xe                          （2）

然后将本次插补周期放入X轴插补周期队列中，脉冲数n'_xi放入X轴脉冲队列中；

再计算Y轴的第i个插补周期中的脉冲数n_yi：

n_yi=T_yi*V_ye                           （3）

其中，V_ye是当前Y轴的速度的值，将n_yi圆整后得到n'_yi，T_yi为X轴当前的插补周期T_xi与上一次Y轴圆整时舍去时间ΔT_yi-1的和，将圆整过程中舍去的小数部分对应的时间ΔT_yi保存，将其加在下一个插补周期计算Y轴的脉冲数的T_xi+1中作为下一次的插补周期，然后将本次插补周期放入Y轴插补周期队列中，脉冲数n'_yi放入Y轴脉冲队列中。

进一步，所述步骤2）中，公式（1）中V_xe的确定方法如下：

在步骤1）中加速阶段的步数是N_xi0，将从第一个插补周期计算得到的脉冲数n'_x0加上之后每一次插补计算得到的脉冲数保存在N_Lx中，直到N_Lx>N_xi0，进入匀速阶段，在加速过程中，V_xe的值将发生变化，当第i个插补周期计算结束，速度V_xe就应该被更新为：

V_xe=V_xe+α_m*n'_xi                         （4）

其中α_m的下标m是通过V_xe与速度表中的速度的比较得到的最接近V_xe的V_m下标，如果V_xe大于V_m+1，则下标改为m+1。

Y轴的速度V_ye确定方法于此相同。

更进一步，在步骤1）中匀速阶段的N_xi1需要重新计算：

N_xi1=N_xi1-(N_Lx-N_xi0)                      （5）

在匀速阶段V_xe＝V_L恒定不变，其大小为V_L，同时加速度下标始终为L。当每一次插补计算结束后都会将得到的脉冲数加入到N_Lx中，直到N_Lx>N_xi1，此时将当前一次的插补计算结果舍去，以便进入减速阶段时剩余的步长数大于等于N_xi2。

再进一步，在步骤1）中减速阶段的所需运动的距离是N_xi2=L-N_Lx，当每一次插补计算结束后都会将得到的脉冲数加入到N_Lx中，直到N_Lx≥L；在减速过程中，V_xe的值将发生变化，当第i个插补周期计算结束，速度V_xe就应该被更新为：

V_xe=V_xe-α_m*n'_xi                         （6）

其中，α_m的下标m是通过V_xe与速度表中的速度的比较得到的最接近V_xe的V_m下标，然后如果V_xe小于V_m，则下标改为m-1，直到当前的速度变为最小值V₀，然后按照最小速度运行的该线段的结束。

优选的，所述运动控制方法还包括以下步骤：

3）使用回溯周期分配法更新步骤2）中Y轴的插补周期T_yi=T_yi+ΔT/n，n为总的插补次数，ΔT是回溯周期误差；

当DSP要在每个插补周期中发送脉冲时，需要计算对应的脉冲频率为：

X轴的频率是：P_xi=n'_xi/T_xi                   (7)

Y轴的频率是：P_yi=n'_yi/T_yi                  (8)；

从而设置X轴和Y轴对应定时器的周期。

所述回溯周期分配法的处理过程为：一条线段的插补计算结束，将此过程中的X轴的所有插补周期相加得到总时间T_xall，将Y轴的所有插补周期相加得到总时间T_yall，由于每一次插补周期的计算都存在误差，所以T_xall必定不等于T_yall，所以计算得到误差时间ΔT=T_xall-T_yall。然后将ΔT平均分配到每个Y轴的插补周期中，更新之前计算得到的Y轴的插补周期T_yi=T_yi+ΔT/n，n为总的插补次数。

本发明的优点在于：变插补周期方法解决了两轴联动问题，很好的实现了两个轴之间的同步；回溯周期分配法进一步优化了多轴的同步；本发明中的插补发计算量小，主要解决脉冲发送及计算误差、定时器误差，同步性良好、精确性较高，适用于DSP控制器。

附图说明

图1为插补周期分割图；

图2分别为圆整前各轴的脉冲时序图；

图3分别为圆整后各轴的脉冲时序图。

本发明的实施方式结合附图作进一步描述。

下面结合附图对本发明做进一步说明。

参照图1～图3，一种高精度变插补周期的多轴联动运动控制方法，包括以下步骤：

1）本发明的控制系统中采用了指数规律加减速算法建立加速度表α₀,α₂,...,α_L,及其对应的速度表V₀,V₂,...,V_L，利用指数规律加减速算法与电机参数相结合建立步长表l₀,l₂,...,l_L。设X轴为长轴，结合本系统的速度规划得到的起始速度v₀和结束速度v₁，分别求得：

X轴加速阶段所需长度：N_xi0=α_i*l_i+α_i+1*l_i+1+...+α_L*l_L           （1）

X轴减速阶段所需长度：N_xi2=α_j+1*l_j+1+α_j+2*l_j+2+...+α_L*lL         （2）

X轴匀速阶段所需长度：N_xi1=L-(N_i0+N_i2)          （3）

其中，L是当前线段的长度，且L足够长，加速阶段加速度α_i的下标i是通过v₀与速度表中的速度的比较得到的最接近v₀的V_i下标，减速阶段加速度α_j的下标j是通过v₁与速度表中的速度的比较得到的最接近v₁的V_j下标。其运动过程是先从速度v₀加速到V_L，再按速度V_L匀速运动N_i1，再开始减速到v₁。Y轴在每一个阶段运动的长度计算方法与之相同；

2）如图1，首先确定固定插补时间T，该时间的确定是由系统的插补精度要求以及系统硬件的相关参数决定的。同时系统确定X、Y轴中哪个是长轴，确定方式是通过比较当前线段的长度及其方向映射到两轴的长度L_x、L_y，这里假设X轴是长轴。计算X轴的第i个插补周期中的脉冲数n_xi：

n_xi=T*V_xe          （4）

其中V_xe是当前X轴的速度的值。将n_xi圆整后得到n'_xi，然后反过来计算X轴的第i个插补周期T_xi：

T_xi=n'_xi/V_xe           （5）

然后将本次插补周期放入X轴插补周期队列中，脉冲数n'_xi放入X轴脉冲队列中，这样就把当前X轴第i个插补周期确定完成；

进一步，如图2、图3所示，计算Y轴的第i个插补周期中的脉冲数n_yi：

n_yi=T_yi*V_ye             （6）

其中V_ye是当前Y轴的速度的值，T_yi为X轴当前的插补周期T_xi与上一次Y轴圆整时舍去时间ΔT_yi-1的和。n_yi圆整后得到n'_yi，将圆整过程中舍去的小数部分对应的时间ΔT_yi保存，将其加在下一个插补周期计算Y轴的脉冲数的T_xi+1中作为下一次的插补周期。然后将本次插补周期放入Y轴插补周期队列中，脉冲数n'_yi放入Y轴脉冲队列中，这样就把当前Y轴第i个插补周期确定完成；

更进一步，公式（4）中V_xe的确定方法如下（Y轴的速度V_ye确定方法于此相同）：

在步骤1）中加速阶段的步数是N_xi0，将从第一个插补周期计算得到的脉冲数n'_x0加上之后每一次插补计算得到的脉冲数保存在N_Lx中，直到N_Lx>N_xi0，进入匀速阶段。在加速过程中，V_xe的值将发生变化，其变化规律如下：当第i个插补周期计算结束，速度V_xe就应该被更新为：

V_xe=V_xe+α_m*n'_xi           （7）

其中α_m的下标m是通过V_xe与速度表中的速度的比较得到的最接近V_xe的V_m下标。然后如果V_xe大于V_m+1，则下标改为m+1；

进一步，在步骤1）中匀速阶段的N_xi1需要重新计算：

N_xi1=N_xi1-(N_Lx-N_xi0)           （8）

在匀速阶段V_xe＝V_L恒定不变，其大小为V_L，同时加速度下标始终为L。当每一次插补计算结束后都会将得到的脉冲数加入到N_Lx中，直到N_Lx>N_xi1，进入减速阶段；

在步骤1）中减速阶段的所需运动的距离是N_xi2=L-N_Lx，当每一次插补计算结束后都会将得到的脉冲数加入到N_Lx中，直到N_Lx≥L。在减速过程中，V_xe的值将发生变化，其变化规律如下：当第i个插补周期计算结束，速度V_xe就应该被更新为：

V_xe=V_xe-α_m*n'_xi            （9）

其中α_m的下标m是通过V_xe与速度表中的速度的比较得到的最接近V_xe的V_m下标。然后如果V_xe小于V_m，则下标改为m-1，直到当前的速度变为最小值V₀，然后按照最小速度运行的该线段的结束。

优选的，所述运动控制方法还包括以下步骤：

3）按照步骤2）的处理过程，一条线段的插补计算结束，将此过程中的X轴的所有插补周期相加得到总时间T_xall，然后将Y轴的所有插补周期相加得到总时间T_yall，由于每一次插补周期的计算都存在误差，所以T_xall必定不等于T_yall，所以计算得到误差时间ΔT=T_xall-T_yall。然后将其平均分配到每个Y轴的插补周期中，更新之前计算得到的Y轴的插补周期T_yi=T_yi+ΔT/n，n为总的插补次数。这就是前面提到的回溯周期分配法；

当DSP要在每个插补周期中发送脉冲时，提取步骤2）中得到了X轴的插补周期以及脉冲数和步骤4）用回溯法得到了Y轴的插补周期以及脉冲数，需要计算对应的脉冲频率为：

X轴的频率是：P_xi=n'_xi/T_xi           (10)

Y轴的频率是：P_yi=n'_yi/T_yi           (11)

从而设置X轴和Y轴对应定时器的周期。

Claims (6)

1.一种高精度变插补周期的多轴联动运动控制方法，其特征在于：所述方法包括以下步骤：

1)设定采用指数规律加减速算法建立加速度表α₀,α₂,...,α_L,及其对应的速度表V₀,V₂,...,V_L，利用指数规律加减速算法与电机参数相结合建立步长表l₀,l₂,...,l_L；当X轴为长轴时，当前线段L足够长，结合X轴的速度规划得到的起始速度v₀和结束速度v₁，计算得到X轴加速、减速、匀速阶段所需长度为N_xi0、N_xi2、N_xi1；Y轴为长轴时每一个阶段运动的长度计算方法与X轴相同；

2)首先确定固定插补时间T，计算X轴的第i个插补周期中的脉冲数n_xi：

n_xi＝T*V_xe                         (1)

其中，V_xe是当前X轴的速度的值，将n_xi圆整后得到n'_xi，然后反过来计算X轴的第i个插补周期T_xi：

T_xi＝n'_xi/V_xe                         (2)

然后将本次插补周期放入X轴插补周期队列中，脉冲数n'_xi放入X轴脉冲队列中；

再计算Y轴的第i个插补周期中的脉冲数n_yi：

n_yi＝T_yi*V_ye                         (3)

其中，V_ye是当前Y轴的速度的值，将n_yi圆整后得到n'_yi，T_yi为X轴当前的插补周期T_xi与上一次Y轴圆整时舍去时间ΔT_yi-1的和，将圆整过程中舍去的小数部分对应的时间ΔT_yi保存，将其加在下一个插补周期计算Y轴的脉冲数的T_xi+1中作为下一次的插补周期，然后将本次插补周期放入Y轴插补周期队列中，脉冲数n'_yi放入Y轴脉冲队列中。

2.如权利要求1所述的一种高精度变插补周期的多轴联动运动控制方法，其特征在于：所述步骤2)中，公式(1)中V_xe的确定方法如下：

在步骤1)中加速阶段的步数是N_xi0，将从第一个插补周期计算得到的脉冲数n'_x0加上之后每一次插补计算得到的脉冲数保存在N_Lx中，直到N_Lx>N_xi0，进入匀速阶段，在加速过程中，V_xe的值将发生变化，当第i个插补周期计算结束，速度V_xe就应该被更新为：

V_xe＝V_xe+α_m*n'_xi                         (4)

其中α_m的下标m是通过更新前V_xe与速度表中的速度的比较得到的最接近V_xe的V_m下标，如果V_xe大于V_m+1，则下标改为m+1。

3.如权利要求1或2所述的一种高精度变插补周期的多轴联动运动控制方法，其特征在于：在步骤1)中匀速阶段的N_xi1需要重新计算：

N_xi1＝N_xi1-(N_Lx-N_xi0)                         (5)

在匀速阶段V_xe＝V_L恒定不变，其大小为V_L，同时加速度下标始终为L，当每一次插补计算结束后都会将得到的脉冲数加入到N_Lx中，直到N_Lx>N_xi1，此时将当前一次的插补计算结果舍去，以便进入减速阶段时剩余的步长数大于等于N_xi2。

4.如权利要求1或2所述的一种高精度变插补周期的多轴联动运动控制方法，其特征在于：在步骤1)中减速阶段的所需运动的距离是N_xi2＝L-N_Lx，当每一次插补计算结束后都会将得到的脉冲数加入到N_Lx中，直到N_Lx≥L；在减速过程中，V_xe的值将发生变化，当第i个插补周期计算结束，速度V_xe就应该被更新为：

V_xe＝V_xe-α_m*n'_xi                         (6)

其中，α_m的下标m是通过更新前的V_xe与速度表中的速度的比较得到的最接近V_xe的V_m下标，然后如果V_xe小于V_m，则下标改为m-1，直到当前的速度变为最小值V₀，然后按照最小速度运行的该线段的结束。

5.如权利要求1或2所述的一种高精度变插补周期的多轴联动运动控制方法，其特征在于：所述运动控制方法还包括以下步骤：

3)使用回溯周期分配法更新步骤2)中Y轴的插补周期T_yi＝T_yi+ΔT/n，n为总的插补次数，ΔT是回溯周期误差；

当DSP要在每个插补周期中发送脉冲时，需要计算对应的脉冲频率为：

X轴的频率是：P_xi＝n'_xi/T_xi           (7)

Y轴的频率是：P_yi＝n'_yi/T_yi          (8)；

从而设置X轴和Y轴对应定时器的周期。

6.如权利要求5所述的一种高精度变插补周期的多轴联动运动控制方法，其特征在于：所述回溯周期分配法的处理过程为：一条线段的插补计算结束，将此过程中的X轴的所有插补周期相加得到总时间T_xall，将Y轴的所有插补周期相加得到总时间T_yall，由于每一次插补周期的计算都存在误差，所以T_xall必定不等于T_yall，所以计算得到误差时间ΔT＝T_xall-T_yall，然后将ΔT平均分配到每个Y轴的插补周期中，更新之前计算得到的Y轴的插补周期T_yi＝T_yi+ΔT/n，n为总的插补次数。