CN104597848A - 基于状态机原理的插补控制方法 - Google Patents

Abstract

本发明提供了一种基于状态机原理的插补控制方法，其能够完成四轴三坐标联动功能，在整个插补过程中，插补精度高，插补误差小于一个脉冲当量，并且每一个插补周期至少有一个插补轴有脉冲输出，保证了插补的快速连续。在50M时钟工作频率下，最高插补运算速度可以达到5M次/秒。此外，该装置还集成了速度控制模块，能够在设定的速度参数下，自动的根据插补轨迹所需的脉冲数，进行直线加减速控制或者S曲线加减速控制。最终分别输出四个轴的进给脉冲和方向驱动信号，控制四个电机驱动轴运动。

Description

基于状态机原理的插补控制方法

技术领域

本发明涉及运动控制领域，尤其涉及一种基于状态机原理的插补控制方法。

背景技术

随着计算机技术、电子技术、自动控制技术、传感测量技术、机械制造技术的发展和大量运用，运动控制系统装置的实现主要经历了三种实现方案的形成：传统的硬件插补方案、计算机软件插补方案和采用软硬件配合的两级插补方案。

(1)传统的硬件插补方案

之所以称为传统的硬件插补方案，是因为该方案主要应用在早期的数控机床系统中。如1952年世界上第一台数控机床的控制系统全部采用电子管元器件实现；后来，在1959年，由于晶体管的出现，数控系统中开始广泛采用晶体管和印刷电路板；从1965年开始，数控系统中又逐步采用一些小规模的集成电路。

此方案下的数控系统全部采用专用的硬件线路完成包括插补在内的各种控制任务，由于其采用的都是各种分立元件和小规模集成电路，整个系统体积庞大、可靠性差、设计线路复杂，非常不利于设备的维护和升级。

(2)计算机软件插补方案

由于计算机技术的不断发展，市场上出现了各种小型计算机、微型计算机，数控系统生产厂家认识到，采用计算机软件插补方案替代原来专用控制线路，不仅经济上更合算，而且还能大幅度的提高系统性能。直至目前，工厂使用的数控机床，大多仍采用这种完全由计算机软件实现插补计算。

(3)软硬件配合的两级插补方案

由于插补驱动对实时性要求极高，必须在有限的时间内完成插补计算，输出进给轴的驱动信息。但随着数控系统的控制功能越来越复杂，对速度的要求越来越高，采用单一的软件插补方案已显得力不从心，因此，出现了采用软硬件相配合的两级插补方案。

此方案下，为了减轻计算机的插补时间负荷，将插补任务分为两部分，即由计算机软件和附加的插补器硬件功能承担。以加工弓箭轮廓为例，首先，计算机把弓箭轮廓按一定的周期分割为若干段，称为粗插补，硬件插补器在此基础上，完成各段内的“数据密化”，并形成脉冲输出，称为细插补。

在第(2)种和第(3)种方案下设计实现的系统，在结构和性能上都存在很大的局限性：

①实时性和可靠性差

采用通用的操作系统，与数控无关的任务可能占用了更多的开销，并且还影响着系统的实时性和稳定性。尤其是在加入加减速控制过程后，数控系统的实时性更是受到极大限制。

②成本高

能满足数控加工要求的工控机配上运动控制卡，使得系统成本很难降低。

③便携性不足

由于必须配备相应的工控机，使系统体积庞大且笨重，便携性方面存在缺陷。

发明内容

本发明要解决的技术问题是如何克服传统的硬件插补方案体积庞大、可靠性差、设计线路复杂，非常不利于设备的维护和升级的缺点。

为了解决这一技术问题，本发明提供了一种基于状态机原理的插补控制方法，通过基于IP核上插补控制模块来实现，包括如下步骤：

S1：初始化被积函数寄存器和余数寄存器；

S2：依据选定的进给轴，得到插补的输出脉冲数以及左移规格化处理所需移位次数；

S3：对被积函数寄存器中接收到的数据依据步骤S2中得到的所需移位次数进行左移规格化处理，对余数寄存器进行半加载初始化赋值；

S4：依据插补的输出脉冲数对被积函数寄存器和余数寄存器进行累加操作；

S5：判断余数寄存器的溢出条件是否满足；

若满足，则产生并输出溢出脉冲，进入步骤S6；

若不满足，则重复步骤S4，进行下一次累加；

S6：更新动点坐标和被积函数寄存器值，判断插补的终点条件是否满足；

若满足，则所述插补控制模块进入空闲状态；

若不满足，则重复步骤S4，进行下一次累加。

可选的，用于螺旋线插补。

可选的，用于直线插补。

可选的，用于圆弧插补。

用于圆弧插补时，在所述步骤S6中，重复步骤S4时：

若x轴方向到达终点，y轴方向未到达终点，则针对y轴方向对应的被积函数寄存器和余数寄存器进行累加操作；针对x轴方向对应的被积函数寄存器和余数寄存器清零；然后进入步骤S5；

若y轴方向到达终点，x轴方向未到达终点，则针对x轴方向对应的被积函数寄存器和余数寄存器进行累加操作；针对y轴方向对应的被积函数寄存器和余数寄存器清零；然后进入步骤S5；

若x、y轴方向均为到达终点，则针对x、y轴方向对应的被积函数寄存器和余数寄存器均进行累加操作；然后进入步骤S5。

在所述步骤S5中，在满足溢出条件的情况下，还进行当前动点所在象限的判断；

然后根据当前动点所在象限和动点运动趋势，在余数寄存器溢出的情况下，选择对应的正方向驱动脉冲或负方向驱动脉冲进行输出。

在进行半加载初始化赋值的过程中，所述余数寄存器的最高有效位设置为1，其余各位均设置为0。

在进行左规格化处理时，将被积函数寄存器中的数同时左移，并记下左移位数，直到最左一位成为规格化数为止。

本发明能够完成四轴三坐标联动功能，在整个插补过程中，插补精度高，插补误差小于一个脉冲当量，并且每一个插补周期至少有一个插补轴有脉冲输出，保证了插补的快速连续。在50M时钟工作频率下，最高插补运算速度可以达到5M次/秒。此外，该装置还集成了速度控制模块，能够在设定的速度参数下，自动的根据插补轨迹所需的脉冲数，进行直线加减速控制或者S曲线加减速控制。最终分别输出四个轴的进给脉冲和方向驱动信号，控制四个电机驱动轴运动。

本发明的最大优势在于，通过硬件的方式实现插补算法和加减速控制算法，极大的满足了数控系统的实时性需求，在控制了系统成本的前提下，使整个系统可靠性高，结构简单，使用方便，更重要的是性能得到了提升。

附图说明

图1是本发明一实施例中提供的基于IP核的运动控制装置的结构示意图；

图2是本发明一实施例中基于状态机原理的插补控制模块的状态转移图；

图3是本发明一实施例中提供的基于IP核的运动控制方法的流程示意图；

图4是本发明一实施例中左移规格化处理的示意图；

图5是本发明一实施例中曲线加减速的示意图。

具体实施方式

以下将结合图1至图5对本发明提供的基于IP核的基于状态机原理的插补控制方法进行详细的描述，其为本发明一可选的实施例，可以认为，本领域的技术人员在不改变本发明精神和内容的范围内，能够对其进行修改和润色。

请参考图3，本实施例提供了一种基于状态机原理的插补控制方法，通过基于IP核上插补控制模块来实现，包括如下步骤：

S1：初始化被积函数寄存器和余数寄存器；

S2：依据选定的进给轴，得到插补的输出脉冲数以及左移规格化处理所需移位次数；插补可以是直线插补、可以是圆弧插补，也可以是螺旋线插补；

S3：对被积函数寄存器中接收到的数据依据步骤S2中得到的所需移位次数进行左移规格化处理，对余数寄存器进行半加载初始化赋值；

对于左移规格化处理，请参考图4，以直线插补为例，若寄存器中的数其最高位为“1”时，该数称为规格化数；反之，若最高位数为“0”，则该数为非规格化数。显然，规格化数经过两次累加后必有一次溢出；而非规格化数必须作两次以上的累加后才会有溢出。直线插补的左移规格化方法是：将被积函数寄存器J_Vx、J_Vy中的数同时左移(最低有效位输入零)，并记下左移位数，直到J_Vx或J_Vy中的一个数是规格化数为止。直线插补经过左移规格化处理后，x、y两方向脉冲分配速度扩大同样倍数(即左移位数)，而两者数值之比不变，所以被插补直线的斜率也不变。因为规格化后，每累加运算两次必有一次溢出，溢出速度不受被积函数的大小影响，较均匀，所以加工的效率和质量都大为提高；

对于半加载初始化赋值，余数寄存器预置数，是在DDA迭代之前，余数寄存器J_Rx、J_Ry的初值不置为0，而是预置某一数值。通常采用余数寄存器半加载。所谓半加载，就是在DDA插补前，给余数寄存器J_Rx、J_Ry的最高有效位置“1”，其余各位均置“0”，即N位余数寄存器容量的一半值2N-1。这样只要再累加2N-1，就可以产生第一个溢出脉冲，改善了溢出脉冲的时间分布，减少插补误差。在进行半加载初始化赋值的过程中，所述余数寄存器的最高有效位设置为1，其余各位均设置为0。

S4：依据插补的输出脉冲数对被积函数寄存器和余数寄存器进行累加操作；

S5：判断余数寄存器的溢出条件是否满足；

若满足，则产生溢出脉冲，进入步骤S6；

若不满足，则重复步骤S4，进行下一次累加；

若是圆弧插补，在所述步骤S5中，在满足溢出条件的情况下，还进行当前动点所在象限的判断；

然后根据当前动点所在象限和动点运动趋势，在余数寄存器溢出的情况下，选择对应的正方向驱动脉冲或负方向驱动脉冲进行输出。

S6：更新动点坐标和被积函数寄存器值，判断插补的终点条件是否满足；

若满足，则所述插补控制模块进入空闲状态；

若不满足，则重复步骤S4，进行下一次累加；

进一步地，针对圆弧插补，在所述步骤S7中，重复步骤S4时：

若x轴方向到达终点，y轴方向未到达终点，则针对y轴方向对应的被积函数寄存器和余数寄存器进行累加操作；针对x轴方向对应的被积函数寄存器和余数寄存器清零；然后进入步骤S5；

若y轴方向到达终点，x轴方向未到达终点，则针对x轴方向对应的被积函数寄存器和余数寄存器进行累加操作；针对y轴方向对应的被积函数寄存器和余数寄存器清零；然后进入步骤S5；

若x、y轴方向均为到达终点，则针对x、y轴方向对应的被积函数寄存器和余数寄存器均进行累加操作；然后进入步骤S5。

在所述步骤S6之前还包括：利用一个速度控制模块以特定变化模式的速度将产生的溢出脉冲输出；

溢出脉冲输出速度的变化包括匀加速变化过程、匀速过程和匀减速变化过程。

请参考图5，溢出脉冲输出速度的变化包括加加速变化过程、匀加速变化过程、匀速过程、加减速变化过程、匀减速变化过程以及减减速变化过程；S曲线加减速控制可实现加减速过程中加减速度连续变化，有效减小冲击和震荡。如图所示为S曲线加减速过程，整个过程由加加速、匀加速、减加速、匀速、加减速、匀减速及减减速7段组成；

图5中，

$\left\{\begin{array}{c}{T}_1=T_3=T_5=T_7=\frac{A_{\max }}{J}\\ T_2=T_6=\frac{V_{\max }-V_s}{A_{\max }}-T_1\\ T_4=\frac{L-V_s({2T}_1+T_2)}{V_{\max }}-({2T}_1+T_2)\end{array}\right.$

各个阶段的持续运行时间表示为T_k(k＝1,2,3,4,5,6,7)。

其中，V_max为最大驱动速度，A_max为最大加速度，J为加速度变化率，V_S为初始速度，L为驱动脉冲总数。

请参考图2，其为基于状态机原理的插补过程的状态示意图；展现了插补过程中状态转移过程：

状态idle：功能：空闲。触发条件N1：开始执行指令信号有效。下一状态为St1；

状态St1：功能：对于直线插补，除初始化寄存器之外，还需确定所选驱动轴中的长轴，从而确定直线插补总的输出脉冲数，以及对当前输入数据进行左移规格化所需要的移位次数。同样，对于圆弧插补，也要进行相应的数据处理。触发条件：无。下一状态为St2；

状态St2：功能：针对直线插补、圆弧插补的算法要求，对被积函数寄存器进行左移规格化处理，余数寄存器进行半加载初始化赋值。触发条件：无。下一状态为S5；

状态St3：功能：Y轴进行被积函数寄存器和余数寄存器累加操作，X轴相应寄存器清零，不再累加。触发条件：无。下一状态为St6；

状态St4：功能：功能：X轴进行被积函数寄存器和余数寄存器累加操作，Y轴相应寄存器清零，不再累加。触发条件：无。下一状态为St6；

状态St5：功能：进行被积函数寄存器和余数寄存器累加操作。触发条件：无。下一状态：St6；

状态St6：功能：判断余数寄存器溢出条件是否满足，满足则产生溢出脉冲，此外这一状态还会根据当前的速度信息产生等待条件，用于控制插补定时器。触发条件：定时器被触发。如果当前插补为直线插补，则下一状态为St10；如果当前插补为圆弧插补，则下一状态为St13；

状态St10：功能：对应的溢出轴输出驱动脉冲，更新当前动点坐标值。触发条件：无。下一状态为St11；

状态St11：功能：驱动脉冲信号拉低，判断直线插补或圆弧插补终点判别条件是否满足。如果直线插补或圆弧插补已到达终点，则下一状态为idle；如果是直线插补过程且未到达终点，则下一状态为St5；如果是圆弧插补过程，相应的x轴已到达终点，则下一状态为St3；如果是圆弧插补过程，相应的y轴已到达终点，则下一状态为St4；如果是圆弧插补过程，且x轴和y轴都未到达终点，那么下一状态为St5；

状态St13：功能：圆弧插补当前动点所在象限判断。如果判断结果为逆时针第一象限，则下一状态为St14；如果判断结果为逆时针第二象限，则下一状态为St15；如果判断结果为逆时针第三象限，则下一状态为St16；如果判断结果为逆时针第四象限，则下一状态为St17；如果判断结果为顺时针第一象限，则下一状态为St18；如果判断结果为顺时针第二象限，则下一状态为St19；如果判断结果为顺时针第三象限，则下一状态为St20；如果判断结果为顺时针第一象限，则下一状态为St21；

状态St14：功能：根据当前动点所在象限和动点运动趋势，在余数寄存器溢出的情况下对应轴输出正方向驱动脉冲或负方向驱动脉冲，并更新动点坐标和被积函数寄存器值。触发条件：无。下一状态为St11；

状态St15：功能：根据当前动点所在象限和动点运动趋势，在余数寄存器溢出的情况下对应轴输出正反向驱动脉冲或负方向驱动脉冲，并更新动点坐标和被积函数寄存器值。触发条件：无。下一状态为St11；

状态St16：功能：根据当前动点所在象限和动点运动趋势，在余数寄存器溢出的情况下对应轴输出正反向驱动脉冲或负方向驱动脉冲，并更新动点坐标和被积函数寄存器值。触发条件：无。下一状态为St11；

状态St17：功能：根据当前动点所在象限和动点运动趋势，在余数寄存器溢出的情况下对应轴输出正反向驱动脉冲或负方向驱动脉冲，并更新动点坐标和被积函数寄存器值。触发条件：无。下一状态为St11；

状态St18：功能：根据当前动点所在象限和动点运动趋势，在余数寄存器溢出的情况下对应轴输出正反向驱动脉冲或负方向驱动脉冲，并更新动点坐标和被积函数寄存器值。触发条件：无。下一状态为St11；

状态St19：功能：根据当前动点所在象限和动点运动趋势，在余数寄存器溢出的情况下对应轴输出正反向驱动脉冲或负方向驱动脉冲，并更新动点坐标和被积函数寄存器值。触发条件：无。下一状态为St11；

状态St20：功能：根据当前动点所在象限和动点运动趋势，在余数寄存器溢出的情况下对应轴输出正反向驱动脉冲或负方向驱动脉冲，并更新动点坐标和被积函数寄存器值。触发条件：无。下一状态为St11；

状态St21：功能：根据当前动点所在象限和动点运动趋势，在余数寄存器溢出的情况下对应轴输出正反向驱动脉冲或负方向驱动脉冲，并更新动点坐标和被积函数寄存器值。触发条件：无。下一状态为St11。

为了实现这一插补控制方法，本实施例提供了如图1所示的运动控制IP核，包括了外部接口、寄存器组、基于状态机原理的插补控制模块、速度控制模块、脉冲输出模块、中断处理模块等。其中，基于状态机原理的插补控制模块、速度控制模块是本装置的核心。

外部接口模块，用于实现本装置与专用芯片主控模块的通信连接。主控模块可通过外部接口模块配置、监控本装置。

寄存器组，用于存储该装置的配置信息以及实时状态。

脉冲输出模块，用于将来自插补控制模块输出的溢出脉冲分配给相应的X、Y、Z、U四个进给轴进行脉冲形式变换后，以指定的脉冲形式进行输出。包括：

正原理，符号+脉冲；

正原理，CW+CCW；

正原理，90°相位差2相脉冲；

负原理，符号+脉冲；

负原理，CW+CCW；

负原理，90°相位差2相脉冲。

中断处理模块，用于对各种输入中断如报警、到位、越限、急停、暂停等进行处理以及向上级主控模块发送中断如出错、超时、任务完成等，

速度控制模块，用于在设定的速度参数下，自动的根据插补轨迹所需的脉冲数进行直线加减速控制或者S曲线加减速控制，最终分别输出四个轴的进给脉冲和方向驱动信号，控制四个电机驱动轴运动。

基于状态机原理的插补控制模块，采用适合于硬件方式实现的数字积分插补算法。包括直线插补、圆弧插补、螺旋线插补三种插补算法子模块。

总结一下，本发明包括以下几个关键点：

关键点1：该装置主要由外部接口、寄存器组、基于状态机原理的插补控制模块、速度控制模块、脉冲输出模块、中断处理模块等组成。基于状态机原理的插补控制模块，采用优化后的适用于硬件方式实现的数字积分插补算法，。速度控制模块采用硬件实时计算的方式完成直线加减速或者S曲线加减速的速度控制。

关键点2：基于状态机原理的插补控制模块采用数字积分插补算法。为使每一个插补周期都至少有一个轴有脉冲溢出，使进给轴驱动脉冲更加均匀，在插补计算前，对被积函数寄存器中的数据进行了左移规格化处理。

关键点3：基于状态机原理的插补控制模块采用数字积分插补算法。为提高数字积分插补算法的插补精度，使直线插补的理论误差控制在半个脉冲当量以内，使圆弧插补的理论误差控制在一个脉冲当量以内，对算法中的余数寄存器进行了半加载预置数。

关键点4：速度控制模块包括直线加减速控制和S曲线加减速控制两种功能。其实质是根据设定的参数和相应的算法预测速度曲线的变化。对于每一个插补周期，根据当前的驱动速度，得到当前驱动脉冲与下一个脉冲的时间间隔，再通过定时器定时，进入下一次插补计算。

关键点5：速度控制模块的S曲线加减速控制功能，整个运行过程分为7个阶段：加加速段、匀加速段、减加速段、匀速段、加减速段、匀减速段、减减速段，各个阶段的持续运行时间表示为T_k(k＝1,2,3,4,5,6,7)。

$\left\{\begin{array}{c}{T}_1=T_3=T_5=T_7=\frac{A_{\max }}{J}\\ T_2=T_6=\frac{V_{\max }-V_s}{A_{\max }}-T_1\\ T_4=\frac{L-V_s({2T}_1+T_2)}{V_{\max }}-({2T}_1+T_2)\end{array}\right.$

其中，V_max为最大驱动速度，A_max为最大加速度，J为加速度变化率，V_S为初始速度，L为驱动脉冲总数。

关键点6：基于状态机原理的插补控制模块采用数字积分插补算法。以空间直线插补为例，推导出的迭代公式如下：

$\begin{array}{c}{\mathrm{JRx}}_{(k+1)}=\frac{\mathrm{xe}}{2}\&CenterDot;[(\mathrm{xe}\&GreaterEqual;\mathrm{ye})\&CenterDot;(\mathrm{xe}\&GreaterEqual;\mathrm{ze})]+[(\mathrm{ye}\&GreaterEqual;\mathrm{xe})\&CenterDot;(\mathrm{ye}\&GreaterEqual;\mathrm{ze})]\&CenterDot;\\ \left\{\right[{\mathrm{JRx}}_{\left(k\right)}+\mathrm{xe}-\mathrm{ye}]\&CenterDot;[{\mathrm{JRx}}_{\left(k\right)}+\mathrm{xe}\&GreaterEqual;\mathrm{ye}]+[{\mathrm{JRx}}_{\left(k\right)}+\mathrm{xe}]\&CenterDot;\\ [{\mathrm{JRx}}_{\left(k\right)}+\mathrm{xe}<\mathrm{ye}]\}+[(\mathrm{ze}\&GreaterEqual;\mathrm{xe})\&CenterDot;(\mathrm{ze}\&GreaterEqual;\mathrm{ye})]\&CenterDot;\\ \left\{\right[{\mathrm{JRx}}_{\left(k\right)}+\mathrm{xe}-\mathrm{ze}]\&CenterDot;[{\mathrm{JRx}}_{\left(k\right)}+\mathrm{xe}\&GreaterEqual;\mathrm{ze}]+[{\mathrm{JRx}}_{\left(k\right)}+\mathrm{xe}]\&CenterDot;\\ [{\mathrm{JRx}}_{\left(k\right)}+\mathrm{xe}<\mathrm{ze}]\}\end{array}$

$\begin{array}{c}{\mathrm{JRy}}_{(k+1)}=\frac{\mathrm{ye}}{2}\&CenterDot;[(\mathrm{ye}\&GreaterEqual;\mathrm{xe})\&CenterDot;(\mathrm{ye}\&GreaterEqual;\mathrm{ze})]+[(\mathrm{xe}\&GreaterEqual;\mathrm{ye})\&CenterDot;(\mathrm{xe}\&GreaterEqual;\mathrm{ze})]\&CenterDot;\\ \left\{\right[{\mathrm{JRy}}_{\left(k\right)}+\mathrm{ye}-\mathrm{xe}]\&CenterDot;[{\mathrm{JRy}}_{\left(k\right)}+\mathrm{ye}\&GreaterEqual;\mathrm{xe}]+[{\mathrm{JRy}}_{\left(k\right)}+\mathrm{ye}]\&CenterDot;\\ [{\mathrm{JRy}}_{\left(k\right)}+\mathrm{ye}<\mathrm{xe}]\}+[(\mathrm{ze}\&GreaterEqual;\mathrm{xe})\&CenterDot;(\mathrm{ze}\&GreaterEqual;\mathrm{ye})]\&CenterDot;\\ \left\{\right[{\mathrm{JRy}}_{\left(k\right)}+\mathrm{ye}-\mathrm{ze}]\&CenterDot;[{\mathrm{JRy}}_{\left(k\right)}+\mathrm{ye}\&GreaterEqual;\mathrm{ze}]+[{\mathrm{JRy}}_{\left(k\right)}+\mathrm{ye}]\&CenterDot;\\ [{\mathrm{JRy}}_{\left(k\right)}+\mathrm{ye}<\mathrm{ze}]\}\end{array}$

$\begin{array}{c}{\mathrm{JRz}}_{(k+1)}=\frac{\mathrm{ze}}{2}\&CenterDot;[(\mathrm{ze}\&GreaterEqual;\mathrm{xe})\&CenterDot;(\mathrm{ze}\&GreaterEqual;\mathrm{ye})]+[(\mathrm{xe}\&GreaterEqual;\mathrm{ye})\&CenterDot;(\mathrm{xe}\&GreaterEqual;\mathrm{ze})]\&CenterDot;\\ \left\{\right[{\mathrm{JRz}}_{\left(k\right)}+\mathrm{ze}-\mathrm{xe}]\&CenterDot;[{\mathrm{JRz}}_{\left(k\right)}+\mathrm{ze}\&GreaterEqual;\mathrm{xe}]+[{\mathrm{JRz}}_{\left(k\right)}+\mathrm{ze}]\&CenterDot;\\ [{\mathrm{JRz}}_{\left(k\right)}+\mathrm{ze}<\mathrm{xe}]\}+[(\mathrm{ye}\&GreaterEqual;\mathrm{xe})\&CenterDot;(\mathrm{ye}\&GreaterEqual;\mathrm{ze})]\&CenterDot;\\ \left\{\right[{\mathrm{JRz}}_{\left(k\right)}+\mathrm{ze}-\mathrm{ye}]\&CenterDot;[{\mathrm{JRz}}_{\left(k\right)}+\mathrm{ze}\&GreaterEqual;\mathrm{ye}]+[{\mathrm{JRz}}_{\left(k\right)}+\mathrm{ze}]\&CenterDot;\\ [{\mathrm{JRz}}_{\left(k\right)}+\mathrm{ze}<\mathrm{ye}]\}\end{array}$

其中，JRx为X轴余数寄存器，JRy为Y轴余数寄存器，JRz为Z轴余数寄存器，xe为X轴目标值，ye为Y轴目标值，ze为Z轴目标值。

圆弧插补算法的迭代公式：

JRx_(k+1)＝[JRx_(k)+JVx_(k)-r]·[JRx_(k)+JVx_(k)≥r]

+[JRx_(k)+JVx_(k)]·[JRx_(k)+JVx_(k)<r]

JRy_(k+1)＝[JRy_(k)+JVy_(k)-r]·[JRy_(k)+JVy_(k)≥r]

+[JRy_(k)+JVy_(k)]·[JRy_(k)+JVy_(k)<r]

JVx_(k+1)＝{[JVx_(k)+1]·(NR1+NR3+SR2+SR4)

+[JVx_(k)-1]·(NR2+NR4+SR1+SR3)}

·[JRy_(k)+JVy_(k)≥r]+JVx_(k)·[JRy_(k)+JVy_(k)<r]

JVy_(k+1)＝{[JVy_(k)+1]·(NR2+NR4+SR1+SR3)

+[JVy_(k)-1]·(NR1+NR3+SR2+SR4)}

·[JRx_(k)+JVx_(k)≥r]+JVy_(k)·[JRx_(k)+JVx_(k)<r]

其中，JRx为X轴余数寄存器，JRy为Y轴余数寄存器，JVx为X轴被积函数寄存器，JVy为Y轴被积函数寄存器，r为圆弧半径，NRx(x＝1,2,3,4)表示第x象限逆时针方向，SRx(x＝1,2,3,4)表示第x象限顺时针方向。

螺旋线插补算法的迭代公式：

JRx_(k+1)＝[JRx_(k)+JVx_(k)-L]·[JRx_(k)+JVx_(k)≥L]

+[JRx_(k)+JVx_(k)]·[JRx_(k)+JVx_(k)<L]

J Ry_(k+1)＝[JRy_(k)+JVy_(k)-L]·[JRy_(k)+JVy_(k)≥L]

+[JRy_(k)+JVy_(k)]·[JRy_(k)+JVy_(k)<L]

JRz_(k+1)＝[JRz_(k)+JVz_(k)-L]·[JRz_(k)+JVz_(k)≥L]

+[JRz_(k)+JVz_(k)]·[JRz_(k)+JVz_(k)<L]

JVx_(k+1)＝{[JVx_(k)+1]·(NR1+NR3+SR2+SR4)

+[JVx_(k)-1]·(NR2+NR4+SR1+SR3)}

·[JRy_(k)+JVy_(k)≥L]+JVx_(k)·[JRy_(k)+JVy_(k)<L]

JVy_(k+1)＝{[JVy_(k)+1]·(NR2+NR4+SR1+SR3)

+[JVy_(k)-1]·(NR1+NR3+SR2+SR4)}

·[JRx_(k)+JVx_(k)≥L]+JVy_(k)·[JRx_(k)+JVx_(k)<L]

${\mathrm{JVz}}_{(k+1)}=\frac{h}{2\mathrm{\&pi;}}$

$L=\max (r,\frac{h}{2\mathrm{\&pi;}})$

其中，JRx为X轴余数寄存器，JRy为Y轴余数寄存器，JRz为Z轴余数寄存器，JVx为X轴被积函数寄存器，JVy为Y轴被积函数寄存器，JVz为Z轴被积函数寄存器，r为螺旋线半径，h为螺旋线高度，NRk(k＝1,2,3,4)表示第k象限逆时针方向，SRk(k＝1,2,3,4)表示第k象限顺时针方向。

综上所述，本发明具有以下有益效果：

①该装置的核心部件为运动控制处理器，采用专用芯片IP核的方式实现内部结构。

②其中的基于状态机原理的插补控制模块，采用优化了的适合于硬件方式实现的数字积分插补算法。

③为了提高进给轴驱动脉冲的均匀性，对被积函数寄存器进行了左移规格化处理。

④为了提高插补轨迹的拟合精度，对余数寄存器进行了半加载初始化赋值。

⑤为了进行完整的S曲线加减速控制，给出了7段S曲线加减速控制的各阶段维持时间的处理算法。

本发明能够完成四轴三坐标联动功能，在整个插补过程中，插补精度高，插补误差小于一个脉冲当量，并且每一个插补周期至少有一个插补轴有脉冲输出，保证了插补的快速连续。在50M时钟工作频率下，最高插补运算速度可以达到5M次/秒。此外，该装置还集成了速度控制模块，能够在设定的速度参数下，自动的根据插补轨迹所需的脉冲数，进行直线加减速控制或者S曲线加减速控制。最终分别输出四个轴的进给脉冲和方向驱动信号，控制四个电机驱动轴运动。

本发明的最大优势在于，通过硬件的方式实现插补算法和加减速控制算法，极大的满足了数控系统的实时性需求，在控制了系统成本的前提下，使整个系统可靠性高，结构简单，使用方便，更重要的是性能得到了提升。

Claims (8)

1.一种基于状态机原理的插补控制方法，通过基于IP核上插补控制模块来实现，包括如下步骤：

S1：初始化被积函数寄存器和余数寄存器；

S2：依据选定的进给轴，得到插补的输出脉冲数以及左移规格化处理所需移位次数；

S3：对被积函数寄存器中接收到的数据依据步骤S2中得到的所需移位次数进行左移规格化处理，对余数寄存器进行半加载初始化赋值；

S4：依据插补的输出脉冲数对被积函数寄存器和余数寄存器进行累加操作；

S5：判断余数寄存器的溢出条件是否满足；

若满足，则产生并输出溢出脉冲，进入步骤S6；

若不满足，则重复步骤S4，进行下一次累加；

S6：更新动点坐标和被积函数寄存器值，判断插补的终点条件是否满足；

若满足，则所述插补控制模块进入空闲状态；

若不满足，则重复步骤S4，进行下一次累加。

2.如权利要求1所述的基于状态机原理的插补控制方法，其特征在于：用于直线插补。

3.如权利要求1所述的基于状态机原理的插补控制方法，其特征在于：用于圆弧插补。

4.如权利要求3所述的基于状态机原理的插补控制方法，其特征在于：在所述步骤S6中，重复步骤S4时：

若x轴方向到达终点，y轴方向未到达终点，则针对y轴方向对应的被积函数寄存器和余数寄存器进行累加操作；针对x轴方向对应的被积函数寄存器和余数寄存器清零；然后进入步骤S5；

若y轴方向到达终点，x轴方向未到达终点，则针对x轴方向对应的被积函数寄存器和余数寄存器进行累加操作；针对y轴方向对应的被积函数寄存器和余数寄存器清零；然后进入步骤S5；

若x、y轴方向均为到达终点，则针对x、y轴方向对应的被积函数寄存器和余数寄存器均进行累加操作；然后进入步骤S5。

5.如权利要求3所述的基于状态机原理的插补控制方法，其特征在于：在所述步骤S5中，在满足溢出条件的情况下，还进行当前动点所在象限的判断；

然后根据当前动点所在象限和动点运动趋势，在余数寄存器溢出的情况下，选择对应的正方向驱动脉冲或负方向驱动脉冲进行输出。

6.如权利要求1所述的基于状态机原理的插补控制方法，其特征在于：在进行半加载初始化赋值的过程中，所述余数寄存器的最高有效位设置为1，其余各位均设置为0。

7.如权利要求1所述的基于状态机原理的插补控制方法，其特征在于：在进行左规格化处理时，将被积函数寄存器中的数同时左移，并记下左移位数，直到最左一位成为规格化数为止。

8.如权利要求1所述的基于状态机原理的插补控制方法，其特征在于：用于螺旋线插补。