一种基于FPGA的高速高精插补系统以及直线插补算法

摘要

本发明公开了一种基于FPGA的高速高精插补系统以及直线插补算法，属于工业控制领域；所述系统包括数据输入模块，以及与之相连的时钟计时模块和数据处理模块，时钟计时模块同时连接数据处理模块。所述直线插补算法首先数据输入模块计算各个坐标轴的起终点需要移动的位移量和方向，然后数据处理模块计算总插补次数及每个周期的插补速度。时钟计时模块在每个插补周期计算一次插补速度，并输出速度和方向脉冲。每产生一个脉冲信号，计时器内部的计数器自加一，直至达到插补次数结束插补计算。本发明系统采用并行结构，便于调试和修改，自主进行插补运算并输出，计算速度快；算法使用余数补偿的方法，整个插补过程达到无误差。

说明书

技术领域

本发明属于工业控制领域，具体是一种基于FPGA的高速高精插补系统以及直线插补算法。

背景技术

随着数控技术、机器人技术和工业控制领域的发展，人们对运动的精度和速度要求越来越高。虽然软件插补易修改，但软件插补自身也存在一些固有缺点：如串行的计算方法相比于硬件并行的计算来讲速度慢，精度低，且插补周期长。

对于传统的硬件插补器来说由于体积庞大、灵活性差等缺点很早就被工程技术人员淘汰。所以，用可编程硬件芯片来处理运动控制的插补算法可以兼顾易修改、精度高、响应快等优点。

发明内容

本发明针对高速多轴电机的控制领域中，现有的插补算法占用过多系统时间导致处理器无法处理其他任务的问题，提出了一种基于FPGA的高速高精插补系统以及直线插补算法，能保证插补的精度和速度。

所述的基于FPGA的高速高精插补系统包括：数据输入模块，时钟计时模块和数据处理模块；

数据输入模块用于接收并处理坐标信号和控制信号；输入端口有：系统时钟信号输入clk端口，32位数据输入D端口，4位地址信号A端口，写使能信号nWE端口，复位信号nRESET端口；输出端口有：系统忙输出信号BUSY端口，插补完成输出信号OVER端口。

每个坐标信号分别存储在一个坐标寄存器中，坐标信号包括：8位三个坐标轴输出分速度输出信号fx、fy、fz和三个坐标轴方向输出信号x_dr、y_dr、z_dr，分别存储在6个对应的坐标寄存器中。

数据输入模块的输出OVER端口通过线型变量分别连接时钟计时模块和数据处理模块的输入完成信号RDY端口，将数据输入的完成信息输送到时钟计时模块和数据处理模块，当接收到输入完成信息之后，时钟计时模块和数据处理模块开始工作。

时钟计时模块的功能是在每个插补周期产生一个脉冲信号，并判断是否到达插补终点；输入端口有：数据输入完成信号RDY端口和系统时钟信号输入clk端口；

时钟计时模块同时连接数据处理模块，数据处理模块提供插补次数信号n给时钟计时模块；同时，时钟计时模块的输出端口：开始插补信号cal端口连接数据处理模块。

数据处理模块的功能是在每个插补周期计算一次各坐标轴速度，并输出；输入端口有：数据输入完成信号RDY端口，系统时钟信号clk端口，开始插补信号cal端口和三个坐标轴输出分速度信号fx、fy、fz端口。

所述的直线插补算法数据处理模块包括四个乘法器、四个除法器和一个开方器；其他曲线的插补算法的数据处理模块由这三种计算模块经过组合得到。

每个乘法器的输入端口均有：系统时钟信号输入clk端口，两个33位数据输入a、b端口和使能信号start端口；输出端口有：完成信号over端口，66位乘法结果输出result端口。

开方器的输入端口有：系统时钟信号输入clk端口，64位被开方数输入data端口，使能信号start端口；输出端口有：完成信号over端口，64位乘法结果输出result端口，64位余数输出remain端口。

每个除法器的输入端口有：系统时钟信号输入clk端口，32位被除数输入divider端口，32位除数输入divisor端口，使能信号start端口；输出端口有：完成信号over端口，8位乘法结果输出out端口以及32位余数输出remain端口。

经过数据处理模块得到每个坐标轴的插补速度，并输出该速度脉冲。

一种基于FPGA的直线插补算法，包括以下步骤：

步骤一、将数据输入模块，时钟计时模块和数据处理模块依次连接起来；

步骤二、将高速多轴电机的各个坐标轴的起终点数据，输入到数据输入模块；

具体为：当数据输入模块的写使能信号nWE端口为低电平时，写入数据：当4位地址信号A端口的地址信号为0时，对应写入数字信号x轴的起点；1对应写入数字信号x轴的终点；2对应写入数字信号y轴的起点；3对应写入数字信号y轴的终点；4对应写入数字信号z轴的起点；5对应写入数字信号z轴的终点；当写使能信号nWE端口置为高电平时，完成数据写入。

步骤三、当输入完成后，数据输入完成信号RDY由低电平跳变为高电平，同时数据处理模块开始计算各个坐标轴需要移动的位移量和方向。

三个坐标轴的位移量计算如下：

Δx＝|x_e-x_s|

Δy＝|y_e-y_s|

Δz＝|z_e-z_s|

其中，Δx为x轴上的位移量，x_e为x轴的终点坐标，x_s为x轴的起点坐标；Δy为y轴上的位移量，y_e为y轴的终点坐标，y_s为y轴的起点坐标；Δz为z轴上的位移量，z_e为z轴的终点坐标，z_s为z轴的起点坐标。

三个坐标轴上的位移量Δx，Δy和Δz分别存储在delta_x、delta_y、delta_z三个寄存器中。

各个坐标轴需要移动的方向判断如下：

当各坐标轴的终点坐标减起点坐标大于零时，该坐标轴的方向为正；反之为负。给三个坐标轴方向输出信号x_dr、y_dr、z_dr的端口赋值，正向为1；反向为0。

步骤四、当各个坐标轴需要移动的位移量和方向计算完后，数据输入完成信号RDY和系统忙输出信号BUSY由低电平跳变为高电平，数据处理模块开始计算总插补次数及每个周期的插补速度。

具体步骤如下：

步骤401、将三个位移量寄存器delta_x、delta_y和delta_z，分别连接到第一乘法器，第二乘法器和第三乘法器的数据输入端，RDY信号变为高电平，开始进行计算，得到Δx²，Δy²和Δz²；

步骤402、计算完成后，将平方后的Δx²，Δy²和Δz²三个结果相加，赋给开方器的数据输入端。

步骤403、三个乘法模块的over端均变为高电平后，开方器开始工作，得到插补总长度L；

插补总长度：

[]表示取整运算；

步骤404、利用第一除法器将插补总长度除以每次插补移动的距离，得到总插补次数。

插补次数：

其中，F为插补速度(每个插补周期移动的距离)，t为插补周期。

步骤五、得到插补次数n后，时钟计时模块在每个插补周期计算一次插补速度，并输出速度和方向的脉冲。

具体为：时钟计时模块每4ms产生一个脉冲信号，控制第二除法器，第三除法器和第四除法器工作，计算三个轴的插补速度。

同时，每产生一个脉冲信号，计时器内部的计数器自加一，直至达到插补次数结束插补计算。

本发明的优点及其带来的有益效果在于：

1)一种基于FPGA的数据采样法直线插补系统，采用FPGA芯片进行插补，不占用CPU过多的资源，可以自主进行插补运算并输出。

2)一种基于FPGA的数据采样法直线插补系统，结构模块化，便于调试和修改。

3)一种基于FPGA的数据采样法直线插补方法，采用并行结构，计算速度快；

4)一种基于FPGA的数据采样法直线插补方法，使用余数补偿的方法，整个插补过程达到无误差。

附图说明

图1为本发明基于FPGA的高速高精插补系统的示意图；

图2为本发明基于FPGA的高速高精插补系统直线插补算法中数据处理模块的示意图；

图3为本发明基于FPGA的直线插补算法流程图；

图4为本发明基于FPGA的高速高精直线插补算法的整体仿真波形；

图5为本发明基于FPGA的高速高精直线插补算法的整体仿真波形局部放大。

具体实施方式

下面结合附图，对本发明的技术方案进行进一步的说明：

本发明一种基于FPGA的高速高精插补系统，如图1所示，主要由三个主模块构成，分别是：数据输入模块，时钟计时模块和数据处理模块；

数据输入模块用于接收并处理坐标信号和控制信号；

时钟计时模块在每个插补周期产生一个脉冲信号，并判断是否到达插补终点；

数据处理模块在每个插补周期计算一次各坐标轴的速度并输出。

数据输入模块的输入端口有：clk(系统时钟)、D(32位数据输入)、A(4位地址信号)、nWE(写使能信号)、nRESET(复位信号)；输出端口有：BUSY(系统忙输出信号)，fx、fy、fz(8位三个坐标轴输出分速度输出信号)，x_dr、y_dr、z_dr(三个坐标轴方向输出信号)和OVER(插补完成输出信号)。

时钟计时模块的输入端口有：RDY(数据输入完成信号)、clk(系统时钟信号)、n(插补次数信号)；输出端口是：cal(开始插补信号)。

数据处理模块的输入端口有：RDY(数据输入完成信号)，clk(系统时钟信号)，cal(开始计算信号)和fx、fy、fz(三个方向输出速度信号)。

其中，针对所述直线插补算法的数据处理模块是由三种小模块组成的，如图2所示，包括四个乘法乘法器、四个除法器和一个开方器。如果要完成其他曲线的插补，只需将这三种小的计算模块按照插补算法进行连接。

乘法器的输入端口有：clk(系统时钟输入)，a、b(两个33位数据输入端口)和start(使能信号)；输出端口有：over(乘法完成信号)、result(66位乘法结果输出)。

开方器的输入端口有：clk(系统时钟输入)、data(64位被开方数输入端口)、start(使能信号)；输出端口有：over(开方完成信号)、result(64位乘法结果输出)、remain(64位余数输出)。

除法器的输入端口有：clk(系统时钟输入)、divider(32位被除数输入端口)、divisor(32位除数输入端口)、start(使能信号)；输出端口有：over(开方完成信号)、out(8位乘法结果输出)、remain(32位余数输出)。

在计算时，将这三种模块按照既定的插补算法依次连接起来，完成插补的计算任务。

一种基于FPGA的高速高精直线插补算法，涉及一种基于FPGA硬件可编程芯片的直线插补算法，是控制多个电机协同运动形成直线轨迹的一种解决方法。如图3所示，包括以下步骤：

步骤一、将数据输入模块，时钟计时模块和数据处理模块依次用线型变量连接起来；

当上一级输出端口的电平变化时，会通过线型变量使下一级输入端口的电平产生改变，达到传递信息的效果。

步骤二、将高速多轴电机的各个坐标轴的起终点数据，输入到数据输入模块；

当数据输入模块的nWE端口为低电平时，可以进行数据的写入；同时地址端的地址信号为0-5时，分别对应此时写入的数据为x、y、z轴的起点和终点。

具体为：当4位地址信号A端口的地址信号为0时，对应写入数字信号x轴的起点；1对应写入数字信号x轴的终点；2对应写入数字信号y轴的起点；3对应写入数字信号y轴的终点；4对应写入数字信号z轴的起点；5对应写入数字信号z轴的终点；

当写使能信号nWE端口置为高电平时，默认为数据写入完成。

步骤三、当输入完成后，数据输入完成信号RDY由低电平跳变为高电平，同时数据处理模块开始计算各个坐标轴需要移动的位移量和方向。

三个坐标轴的位移量计算如下：

Δx＝|x_e-x_s|

Δy＝|y_e-y_s|

Δz＝|z_e-z_s|

其中，Δx为x轴上的位移量，x_e为x轴的终点坐标，x_s为x轴的起点坐标；Δy为y轴上的位移量，y_e为y轴的终点坐标，y_s为y轴的起点坐标；Δz为z轴上的位移量，z_e为z轴的终点坐标，z_s为z轴的起点坐标。

三个坐标轴上的位移量Δx，Δy和Δz分别存储在delta_x、delta_y、delta_z三个寄存器中。

各个坐标轴需要移动的方向判断如下：

当各坐标轴的终点坐标减起点坐标大于零时，该坐标轴的方向为正；反之为负。给三个坐标轴方向输出信号x_dr、y_dr、z_dr的端口赋值，正向为1；反向为0。

步骤四、当各个坐标轴需要移动的位移量和方向计算完后，数据输入完成信号RDY和系统忙输出信号BUSY由低电平跳变为高电平，下一级模块数据处理模块开始计算总插补次数。

完成插补次数计算的部分是数据处理模块的中的mult1、mult2、mult3、sqrt和divide1这几个运算模块。

具体步骤如下：

步骤401、将三个位移量寄存器delta_x、delta_y和delta_z，分别连接到第一乘法器，第二乘法器和第三乘法器的两个数据输入端，RDY信号变为高电平，开始进行计算，得到Δx²，Δy²和Δz²；

步骤402、计算完成后，将平方后的Δx²，Δy²和Δz²三个结果相加，赋给开方器的数据输入端。

步骤403、三个乘法模块的over端均变为高电平后，开方器开始工作，得到插补总长度L；

插补总长度：

[]表示取整运算；

步骤404、利用第一除法器将插补总长度除以每次插补移动的距离，得到总插补次数。

插补次数：

其中，F为插补速度(每个插补周期移动的距离)，t为插补周期。

取整运算一是为了避免浮点数的运算，二是为了避免出现速度大于安全速度的情况。并且可以证明，以上两步取整运算对插补精度没有影响。

步骤五、得到插补次数n后，时钟计时模块在每个插补周期计算一次插补速度，并输出各个轴在当前插补周期的插补速度。

此步骤在图2中的mult4、divide2、divide3、divide4中进行；具体为：在计算插补次数之后，时钟计时模块开始工作，每4ms产生一个脉冲信号，控制第二除法器，第三除法器和第四除法器工作，计算三个轴的插补速度。

同时，每计算一次插补速度，计时器内部的计数器自加一，直至达到插补次数停止输出开始插补信号。同时，数据输入模块的BUSY端口和OVER端口由高电平跳变为低电平，结束插补。

每个插补周期计算完成一次插补速度之后，将当前除法计算中的余数与位移量寄存器相加，赋给下一次除法计算的被除数。这样余数补偿之后可以达到消除误差的效果。

本发明整体的仿真波形图如图4所示，在该次仿真中，令输入的delta_x＝237213、delta_y＝537437、delta_z＝51517238。然后令数据输入模块中的使能端置nWE为1，插补系统开始计算。在下方的fx_over、fy_over、fz_over是三个轴的插补完成信号，每个插补周期的计算完成后，这三个输出管脚会输出一个高电平脉冲，同时输出一个位移增量。

根据初始值的计算，平均每个插补周期的脉冲位移增量应该为fx＝0.92、fy＝2.08、fz＝199.9。将图中fx、fy、fz三行中其中一个发生交叉的部分局部放大之后如图5(a)，从图中可以看出，在某个插补周期计算完成后，系统输出的三个轴位移脉冲依次为：1、2、200。而另一个交叉部分的放大图，如图5(b)中，另一个插补周期计算完成后，系统输出的三个轴位移脉冲依次为：0、2、200。从输出的三个轴速度上看，速度比等于位移量之比。而且从图中的插补次数计数器上看，正常计数；当插补次数达到计算数值时，停止插补。

再另一次仿真中，令输入的delta_x＝2314、delta_y＝1542、delta_z＝5341，插补周期为4ms。经统计，每个插补周期计算出的速度分别为：X轴：72、72、72、73、72、72、73、73、72、72、73；Y轴：48、48、48、48、48、49、48、48、49；Z轴：166、167、167、167、167、167、166、167、166、166、167、166。经计算：Δx＝4×∑fx＝2314、Δy＝4×∑fy＝1542、Δz＝4×∑fz＝5341。