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

摘要

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

说明书

技术领域

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

背景技术

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

发明内容

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

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

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

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轴对应定时器的周期。