[發(fā)明專利]一種基于FPGA的高速高精插補(bǔ)系統(tǒng)以及直線插補(bǔ)算法

有效

• 申請?zhí)枺?/td>: 201711470440.3
• 申請日: 2017-12-29
• 公開（公告）號: CN108268013B
• 公開（公告）日: 2020-04-14
• 發(fā)明（設(shè)計）人: 陳友東;胡嘉航
• 申請（專利權(quán)）人: 北京航空航天大學(xué)
• 主分類號: G05B19/4103
• 分類號: G05B19/4103
• 代理公司: 北京永創(chuàng)新實專利事務(wù)所 11121
• 代理人: 冀學(xué)軍

1.一種基于FPGA的高速高精插補(bǔ)系統(tǒng)，其特征在于，包括數(shù)據(jù)輸入模塊，時鐘計時模塊和數(shù)據(jù)處理模塊；

數(shù)據(jù)輸入模塊用于接收并處理坐標(biāo)信號和控制信號；輸入端口有：系統(tǒng)時鐘信號輸入clk端口，32位數(shù)據(jù)輸入D端口，4位地址信號A端口，寫使能信號nWE端口，復(fù)位信號nRESET端口；輸出端口有：系統(tǒng)忙輸出信號BUSY端口，插補(bǔ)完成輸出信號OVER端口；

數(shù)據(jù)輸入模塊的輸出OVER端口通過線型變量分別連接時鐘計時模塊和數(shù)據(jù)處理模塊的輸入完成信號RDY端口，將數(shù)據(jù)輸入的完成信息輸送到時鐘計時模塊和數(shù)據(jù)處理模塊，當(dāng)接收到輸入完成信息之后，時鐘計時模塊和數(shù)據(jù)處理模塊開始工作；

時鐘計時模塊的功能是在每個插補(bǔ)周期產(chǎn)生一個脈沖信號，并判斷是否到達(dá)插補(bǔ)終點；輸入端口有：數(shù)據(jù)輸入完成信號RDY端口和系統(tǒng)時鐘信號輸入clk端口；

時鐘計時模塊同時連接數(shù)據(jù)處理模塊，將數(shù)據(jù)處理模塊提供插補(bǔ)次數(shù)信號n給時鐘計時模塊；同時，時鐘計時模塊的輸出端口：開始插補(bǔ)信號cal端口連接數(shù)據(jù)處理模塊；

數(shù)據(jù)處理模塊的功能是在每個插補(bǔ)周期計算一次各坐標(biāo)軸速度，并輸出；輸入端口有：數(shù)據(jù)輸入完成信號RDY端口，系統(tǒng)時鐘信號clk端口，開始插補(bǔ)信號cal端口和三個坐標(biāo)軸輸出分速度信號fx、fy、fz端口；

經(jīng)過數(shù)據(jù)處理模塊得到每個坐標(biāo)軸的插補(bǔ)速度，并輸出該速度脈沖；

所述的坐標(biāo)信號包括：8位三個坐標(biāo)軸輸出分速度輸出信號fx、fy、fz和三個坐標(biāo)軸方向輸出信號x_dr、y_dr、z_dr，分別存儲在6個對應(yīng)的坐標(biāo)寄存器中。

2.如權(quán)利要求1所述的一種基于FPGA的高速高精插補(bǔ)系統(tǒng)，其特征在于，所述的數(shù)據(jù)處理模塊包括四個乘法器、四個除法器和一個開方器；

每個乘法器的輸入端口均有：系統(tǒng)時鐘信號輸入clk端口，兩個33位數(shù)據(jù)輸入a、b端口和使能信號start端口；輸出端口有：完成信號over端口，66位乘法結(jié)果輸出result端口；

開方器的輸入端口有：系統(tǒng)時鐘信號輸入clk端口，64位被開方數(shù)輸入data端口，使能信號start端口；輸出端口有：完成信號over端口，64位乘法結(jié)果輸出result端口，64位余數(shù)輸出remain端口；

每個除法器的輸入端口有：系統(tǒng)時鐘信號輸入clk端口，32位被除數(shù)輸入divider端口，32位除數(shù)輸入divisor端口，使能信號start端口；輸出端口有：完成信號over端口，8位乘法結(jié)果輸出out端口以及32位余數(shù)輸出remain端口。

3.基于權(quán)利要求1所述的一種基于FPGA的高速高精插補(bǔ)系統(tǒng)的直線插補(bǔ)算法，其特征在于，包括以下步驟：

步驟一、將數(shù)據(jù)輸入模塊，時鐘計時模塊和數(shù)據(jù)處理模塊依次連接起來；

步驟二、將高速多軸電機(jī)的各個坐標(biāo)軸的起終點數(shù)據(jù)，輸入到數(shù)據(jù)輸入模塊；

將高速多軸電機(jī)的各個坐標(biāo)軸的起終點數(shù)據(jù)輸入到數(shù)據(jù)輸入模塊的具體過程為：當(dāng)數(shù)據(jù)輸入模塊的寫使能信號nWE端口為低電平時，寫入數(shù)據(jù)：當(dāng)4位地址信號A端口的地址信號為0時，對應(yīng)寫入數(shù)字信號x軸的起點；1對應(yīng)寫入數(shù)字信號x軸的終點；2對應(yīng)寫入數(shù)字信號y軸的起點；3對應(yīng)寫入數(shù)字信號y軸的終點；4對應(yīng)寫入數(shù)字信號z軸的起點；5對應(yīng)寫入數(shù)字信號z軸的終點；當(dāng)寫使能信號nWE端口置為高電平時，完成數(shù)據(jù)寫入；

步驟三、當(dāng)輸入完成后，數(shù)據(jù)輸入完成信號RDY由低電平跳變?yōu)楦唠娖剑瑫r數(shù)據(jù)處理模塊開始計算各個坐標(biāo)軸需要移動的位移量和方向；

三個坐標(biāo)軸的位移量計算如下：

Δx＝|x_e-x_s|

Δy＝|y_e-y_s|

Δz＝|z_e-z_s|

其中，Δx為x軸上的位移量，x_e為x軸的終點坐標(biāo)，x_s為x軸的起點坐標(biāo)；Δy為y軸上的位移量，y_e為y軸的終點坐標(biāo)，y_s為y軸的起點坐標(biāo)；Δz為z軸上的位移量，z_e為z軸的終點坐標(biāo)，z_s為z軸的起點坐標(biāo)；

三個坐標(biāo)軸上的位移量Δx，Δy和Δz分別存儲在delta_x、delta_y、delta_z三個寄存器中；

各個坐標(biāo)軸需要移動的方向判斷如下：

當(dāng)各坐標(biāo)軸的終點坐標(biāo)減起點坐標(biāo)大于零時，該坐標(biāo)軸的方向為正；反之為負(fù)；給三個坐標(biāo)軸方向輸出信號x_dr、y_dr、z_dr的端口賦值，正向為1；反向為0；

步驟四、當(dāng)各個坐標(biāo)軸需要移動的位移量和方向計算完后，數(shù)據(jù)輸入完成信號RDY和系統(tǒng)忙輸出信號BUSY由低電平跳變?yōu)楦唠娖?，?shù)據(jù)處理模塊開始計算總插補(bǔ)次數(shù)及每個周期的插補(bǔ)速度；

具體步驟如下：

步驟401、將三個位移量寄存器delta_x、delta_y和delta_z，分別連接到第一乘法器，第二乘法器和第三乘法器的數(shù)據(jù)輸入端，RDY信號變?yōu)楦唠娖剑_始進(jìn)行計算，得到Δx²，Δy²和Δz²；

步驟402、計算完成后，將平方后的Δx²，Δy²和Δz²三個結(jié)果相加，賦給開方器的數(shù)據(jù)輸入端；

步驟403、三個乘法模塊的over端均變?yōu)楦唠娖胶?，開方器開始工作，得到插補(bǔ)總長度L；

插補(bǔ)總長度：

[]表示取整運算；

步驟404、利用第一除法器將插補(bǔ)總長度除以每次插補(bǔ)移動的距離，得到總插補(bǔ)次數(shù)；

插補(bǔ)次數(shù)：

其中，F(xiàn)為插補(bǔ)速度，即每個插補(bǔ)周期移動的距離，t為插補(bǔ)周期；

步驟五、得到插補(bǔ)次數(shù)n后，時鐘計時模塊在每個插補(bǔ)周期計算一次插補(bǔ)速度，并輸出速度和方向的脈沖；

同時，每產(chǎn)生一個脈沖信號，計時器內(nèi)部的計數(shù)器自加一，直至達(dá)到插補(bǔ)次數(shù)結(jié)束插補(bǔ)計算；

時鐘計時模塊計算插補(bǔ)速度的具體過程為：時鐘計時模塊每4ms產(chǎn)生一個脈沖信號，控制第二除法器，第三除法器和第四除法器工作，計算三個軸的插補(bǔ)速度。