三軸數(shù)控系統(tǒng)高速高精度小線段插補(bǔ)協(xié)處理器的制造方法
【專利摘要】本發(fā)明公開了一種三軸數(shù)控系統(tǒng)高速高精度小線段插補(bǔ)協(xié)處理器，其組成及連接方案是：由幾何特征提取單元、規(guī)劃約束單元、規(guī)劃元生成單元、運(yùn)動(dòng)規(guī)劃單元、路徑插補(bǔ)單元連接形成數(shù)據(jù)處理流水線實(shí)現(xiàn)并行處理。協(xié)處理器通過數(shù)據(jù)讀寫接口與數(shù)控系統(tǒng)中央處理器連接進(jìn)行數(shù)據(jù)的交換，構(gòu)成多處理器并行處理系統(tǒng)。三軸數(shù)控小線段加工所需的插補(bǔ)相關(guān)運(yùn)算均在協(xié)處理器內(nèi)高速運(yùn)行。小線段插補(bǔ)協(xié)處理器采用專用硬件邏輯電路實(shí)現(xiàn)，可以明顯加速算法的執(zhí)行。插補(bǔ)協(xié)處理器本身功能專一，因而結(jié)構(gòu)精簡(jiǎn)，可部署在數(shù)控系統(tǒng)已有的可編程FPGA芯片內(nèi)，從而降低了系統(tǒng)的總體成本。
【專利說明】
三軸數(shù)控系統(tǒng)高速高精度小線段插補(bǔ)協(xié)處理器
技術(shù)領(lǐng)域
[0001] 本發(fā)明屬于數(shù)控機(jī)械加工領(lǐng)域，具體涉及到三軸數(shù)控系統(tǒng)加工中小線段插補(bǔ)技術(shù) 處理裝置。
【背景技術(shù)】
[0002] 在高速加工中心、雕銑機(jī)等三軸數(shù)控加工應(yīng)用中，具有復(fù)雜曲面的零件加工程序 常常包含大量的連續(xù)小線段直線移動(dòng)命令。為了提升曲面加工的精度和光滑性，可以增多 小線段的數(shù)量，但這樣會(huì)大幅增加數(shù)控系統(tǒng)的運(yùn)算負(fù)擔(dān)。另外，在高速加工的插補(bǔ)運(yùn)算中， 為了提高加工效率和減少震動(dòng)沖擊等目的，需要采用前瞻預(yù)讀和平滑加減速算法，這樣就 進(jìn)一步增加了實(shí)時(shí)運(yùn)算量。如西門子、FANUC、三菱等國(guó)際上具有高性能小線段加工處理能 力的數(shù)控系統(tǒng)，均采用高性能的中央處理器及高速存儲(chǔ)器，以提高小線段插補(bǔ)運(yùn)算軟件的 處理能力。但是數(shù)控系統(tǒng)是典型的多任務(wù)系統(tǒng)，控制器不但要處理插補(bǔ)運(yùn)算，還需要同時(shí)進(jìn) 行程序指令的譯碼、PLC邏輯運(yùn)算、伺服控制、人機(jī)交互、現(xiàn)場(chǎng)總線通信、程序傳輸與管理等 諸多軟件功能，這將使得分配到插補(bǔ)運(yùn)算的處理資源受到很大約束，限制了高速高精度小 線段加工的處理性能。此外，采用高性能中央處理器的數(shù)控平臺(tái)會(huì)導(dǎo)致生產(chǎn)和使用成本高 昂?；诖耍景l(fā)明的提出可以低成本硬件的基礎(chǔ)上，提高數(shù)控系統(tǒng)小線段加工程序的處理 性能的裝置。

【發(fā)明內(nèi)容】

[0003] 本發(fā)明的目的是，提供一種面向三軸數(shù)控加工系統(tǒng)的高速高精度小線段插補(bǔ)協(xié)處 理器，以提高三軸數(shù)控系統(tǒng)高速小線段插補(bǔ)運(yùn)算性能，降低系統(tǒng)成本。
[0004] 為實(shí)現(xiàn)上述目的，本發(fā)明的技術(shù)方案如下：
[0005] 三軸數(shù)控系統(tǒng)高速高精度小線段插補(bǔ)協(xié)處理器，包括:數(shù)據(jù)讀寫接口、指令寄存 器、狀態(tài)寄存器、輸入緩沖區(qū)、配置寄存器、小線段緩沖區(qū)、插補(bǔ)輸出緩沖區(qū)、控制單元、幾何 特征提取單元、規(guī)劃約束單元、規(guī)劃元生成單元、運(yùn)動(dòng)規(guī)劃單元、路徑插補(bǔ)單元、規(guī)劃元緩沖 區(qū)等。
[0006] 其連接組成方案是：指令寄存器、狀態(tài)寄存器、輸入緩沖區(qū)、配置寄存器和輸出緩 沖區(qū)通過數(shù)據(jù)讀寫接口與數(shù)控系統(tǒng)中央處理器連接;幾何特征提取單元、規(guī)劃約束單元、規(guī) 劃元生成單元、運(yùn)動(dòng)規(guī)劃單元、路徑插補(bǔ)單元依次連接形成數(shù)據(jù)處理流水線實(shí)現(xiàn)并行處理； 輸入緩沖區(qū)與幾何特征提取單元連接以讀取輸入的小線段數(shù)據(jù);路徑插補(bǔ)單元與插補(bǔ)輸出 緩沖區(qū)連接以寫入插補(bǔ)結(jié)果;規(guī)劃元生成單元、路徑插補(bǔ)單元和小線段緩沖區(qū)連接進(jìn)行小 線段數(shù)據(jù)的存儲(chǔ)與交換;規(guī)劃元生成單元、運(yùn)動(dòng)規(guī)劃單元、路徑插補(bǔ)單元和規(guī)劃元緩沖區(qū)連 接進(jìn)行規(guī)劃元數(shù)據(jù)的存儲(chǔ)與交換。
[0007] 在小線段緩沖區(qū)內(nèi)部，存儲(chǔ)著小線段隊(duì)列;在規(guī)劃元緩沖區(qū)內(nèi)部，存儲(chǔ)著規(guī)劃元隊(duì) 列。
[0008] 上述三軸數(shù)控系統(tǒng)高速高精度小線段插補(bǔ)協(xié)處理器的運(yùn)行方法，包含以下處理步 驟：
[0009] SI:初始化，控制單元將狀態(tài)寄存器中緩沖區(qū)空閑標(biāo)志位置1，插補(bǔ)完成標(biāo)志位置 1;清空指令寄存器。
[0010] S2:控制單元等待數(shù)控系統(tǒng)中央處理器向指令寄存器寫入指令;若是執(zhí)行插補(bǔ)指 令，則進(jìn)入步驟S3;若是初始化指令，則進(jìn)入步驟S1。
[0011] S3:控制單元將狀態(tài)寄存器(3)中插補(bǔ)完成標(biāo)志位清0。
[0012 ] S4:控制單元同步啟動(dòng)數(shù)據(jù)處理流水線上的幾何特征提取單元、規(guī)劃約束單元、規(guī) 劃元生成單元、運(yùn)動(dòng)規(guī)劃單元、路徑插補(bǔ)單元。
[0013] S5:數(shù)據(jù)處理流水線上各單元執(zhí)行一次計(jì)算與邏輯處理，完成后各單元暫停。
[0014] S6:控制單元更新狀態(tài)寄存器中緩沖區(qū)空閑標(biāo)志位，若輸入緩沖區(qū)、小線段緩沖區(qū) 和規(guī)劃元緩沖區(qū)均未滿，則標(biāo)志位置1;否則，清0。
[0015] S7:控制單元將狀態(tài)寄存器中的插補(bǔ)完成標(biāo)志位置1，清空指令寄存器。
[0016] S8:返回步驟S2。
[0017] 幾何特征提取單元在步驟S5中的一次計(jì)算與邏輯處理進(jìn)一步包括以下步驟：
[0018] S5-9-1:檢測(cè)緩沖區(qū)空閑標(biāo)志位，若該標(biāo)志位為0，則幾何特征提取單元自動(dòng)停止。
[0019] S5-9-2:檢測(cè)輸入緩沖區(qū)中是否有數(shù)控系統(tǒng)中央處理器新寫入的小線段數(shù)據(jù);若 沒有新寫入的小線段數(shù)據(jù)，則幾何特征提取單元自動(dòng)停止。
[0020] S5-9-3:從輸入緩沖區(qū)中讀取一條小線段的數(shù)據(jù)。
[0021] S5-9-4:計(jì)算讀入小線段的幾何特征數(shù)據(jù)。
[0022] S5-9-5:將含有幾何特征的小線段數(shù)據(jù)輸出到規(guī)劃約束單元。
[0023] S5-9-6:幾何特征提取單元自動(dòng)停止。
[0024] 規(guī)劃約束單元在步驟S5中的一次計(jì)算與邏輯處理進(jìn)一步包括以下步驟：
[0025] S5-10-1:檢測(cè)是否從幾何特征提取單元輸入了新的小線段數(shù)據(jù);若沒有新輸入的 小線段數(shù)據(jù)，則規(guī)劃約束單元自動(dòng)停止。
[0026] S5-10-2:讀取新輸入的小線段數(shù)據(jù)。
[0027] S5-10-3:根據(jù)配置寄存器中的規(guī)劃約束參數(shù)計(jì)算小線段的規(guī)劃約束條件。
[0028] S5-10-4:將含有規(guī)劃約束條件的小線段數(shù)據(jù)輸出到規(guī)劃元生成單元。
[0029] S5-10-5 :規(guī)劃約束單元自動(dòng)停止。
[0030] 規(guī)劃元生成單元在步驟S5中的一次計(jì)算與邏輯處理進(jìn)一步包括以下步驟：
[0031] S5-11-1:檢測(cè)是否從規(guī)劃約束單元輸入了新的小線段數(shù)據(jù);若沒有新輸入的小線 段數(shù)據(jù)，則將規(guī)劃元更新標(biāo)志位清〇，進(jìn)入步驟S5-11-5。
[0032] S5-11-2:將規(guī)劃元更新標(biāo)志位置1。
[0033] S5-11-3:判斷是否滿足連續(xù)運(yùn)動(dòng)條件;若小線段隊(duì)列為空，則直接判定不滿足連 續(xù)運(yùn)動(dòng)條件。否則，讀取小線段隊(duì)列尾部小線段數(shù)據(jù)，并與從規(guī)劃約束單元輸入的小線段數(shù) 據(jù)進(jìn)行比較，判斷這兩條相連的小線段是否滿足連續(xù)運(yùn)動(dòng)條件。
[0034] S5-11-4:若滿足連續(xù)運(yùn)動(dòng)條件，則讀取規(guī)劃元隊(duì)列尾部的規(guī)劃元數(shù)據(jù)，將小線段 添加到該規(guī)劃元上，并更新規(guī)劃元隊(duì)列尾部的規(guī)劃元數(shù)據(jù)。若不滿足連續(xù)運(yùn)動(dòng)條件，則生成 一個(gè)含有新輸入的小線段的新規(guī)劃元數(shù)據(jù)，并添加到規(guī)劃元隊(duì)列尾部。
[0035] S5-11-5:將規(guī)劃元更新標(biāo)志位輸出到運(yùn)動(dòng)規(guī)劃單元。
[0036] S5-11-6:規(guī)劃元生成單元自動(dòng)停止。
[0037] 運(yùn)動(dòng)規(guī)劃單元在步驟S5中的一次計(jì)算與邏輯處理進(jìn)一步包括以下步驟：
[0038] S5-12-1:若規(guī)劃元隊(duì)列空，則運(yùn)動(dòng)規(guī)劃單元自動(dòng)停止；
[0039] S5-12-2:檢測(cè)來自規(guī)劃元生成單元（11)的規(guī)劃元更新標(biāo)志位;若該位為1，則根據(jù) 配置寄存器中的規(guī)劃約束參數(shù)對(duì)規(guī)劃元隊(duì)列重新進(jìn)行一次反向規(guī)劃。
[0040] S5-12-3:根據(jù)配置寄存器中的規(guī)劃約束參數(shù)和反向規(guī)劃結(jié)果，對(duì)規(guī)劃元隊(duì)列頭部 的規(guī)劃元進(jìn)行正向規(guī)劃。
[0041] S5-12-4:將正向規(guī)劃結(jié)果輸出到路徑插補(bǔ)單元。
[0042] S5-12-5:運(yùn)動(dòng)規(guī)劃單元自動(dòng)停止。
[0043] 路徑插補(bǔ)單元在步驟S5中的一次計(jì)算與邏輯處理進(jìn)一步包括以下步驟：
[0044] S5-13-1:讀取配置寄存器中插補(bǔ)數(shù)量參數(shù)n和插補(bǔ)周期參數(shù)。
[0045] S5-13-2:檢測(cè)是否從路徑插補(bǔ)單元輸入了正向規(guī)劃結(jié)果;若沒有輸入正向規(guī)劃結(jié) 果，則向插補(bǔ)輸出緩沖區(qū)寫入n個(gè)最后一次插補(bǔ)的坐標(biāo)，路徑插補(bǔ)單元自動(dòng)停止。
[0046] S5-13-3:根據(jù)插補(bǔ)周期參數(shù)、正向規(guī)劃結(jié)果對(duì)小線段隊(duì)列頭部的小線段數(shù)據(jù)進(jìn)行 n次數(shù)據(jù)采樣插補(bǔ)，計(jì)算得到n個(gè)時(shí)刻插補(bǔ)坐標(biāo)。
[0047] S5-13-4:向插補(bǔ)輸出緩沖區(qū)寫入計(jì)算的n個(gè)時(shí)刻插補(bǔ)坐標(biāo)。
[0048] S5-13-5:刪除小線段緩沖區(qū)和規(guī)劃元緩沖區(qū)14中已完成插補(bǔ)的部分。
[0049] S5-13-6:路徑插補(bǔ)單元自動(dòng)停止。
[0050] 本發(fā)明的優(yōu)點(diǎn)以及產(chǎn)生的有益效果在于：
[0051] (1)三軸數(shù)控小線段加工所需的插補(bǔ)相關(guān)運(yùn)算均在協(xié)處理器內(nèi)高速運(yùn)行，不占用 數(shù)控系統(tǒng)中央處理器的資源。協(xié)處理器與中央處理器同時(shí)執(zhí)行不同的任務(wù)，構(gòu)成了多處理 器并行處理系統(tǒng)，提高了數(shù)控系統(tǒng)小線段加工程序的處理性能。
[0052] (2)小線段插補(bǔ)協(xié)處理器采用專用硬件邏輯電路實(shí)現(xiàn)，相比采用軟件的插補(bǔ)方法， 可以明顯加速算法的執(zhí)行。處理器采用流水線方式實(shí)現(xiàn)多個(gè)小線段的并行計(jì)算，能夠成倍 提高計(jì)算性能。
[0053] (3)應(yīng)用小線段插補(bǔ)協(xié)處理器的數(shù)控系統(tǒng)，降低了對(duì)中央處理器性能的需求，可采 用價(jià)格更為低廉的中央處理器。而插補(bǔ)協(xié)處理器本身功能專一，因而結(jié)構(gòu)精簡(jiǎn)，可部署在數(shù) 控系統(tǒng)已有的可編程FPGA芯片內(nèi)，從而降低了系統(tǒng)的總體成本。
【附圖說明】
[0054]圖1是本發(fā)明的小線段插補(bǔ)協(xié)處理器硬件架構(gòu)框圖。
[0055]圖2是本發(fā)明的數(shù)控系統(tǒng)硬件連接原理圖。
[0056]圖3是本發(fā)明的幾何特征提取原理圖。
[0057]圖4是本發(fā)明的規(guī)劃元生成原理圖。
[0058]圖5是本發(fā)明的運(yùn)動(dòng)規(guī)劃曲線示意圖。
[0059]圖6是本發(fā)明的前瞻運(yùn)動(dòng)規(guī)劃原理圖。
【具體實(shí)施方式】
[0060]以下結(jié)合附圖并通過實(shí)施例對(duì)本發(fā)明的組成方案，以及協(xié)處理器的運(yùn)行方法作進(jìn) 一步的說明。
[0061] 需要說明的是下述實(shí)施例是敘述性的，而非限定性的，本發(fā)明所涵蓋的內(nèi)容并不 限于下述實(shí)施例。
[0062] 本實(shí)施例以含有X、Y、Z三個(gè)相互垂直軸的數(shù)控銑床的應(yīng)用為例。
[0063]三軸數(shù)控系統(tǒng)高速高精度小線段插補(bǔ)協(xié)處理器，其組成方案是：指令寄存器2、狀 態(tài)寄存器3、輸入緩沖區(qū)4、配置寄存器5和插補(bǔ)輸出緩沖區(qū)7通過數(shù)據(jù)讀寫接口 1與數(shù)控系統(tǒng) 中央處理器連接;幾何特征提取單元9、規(guī)劃約束單元10、規(guī)劃元生成單元11、運(yùn)動(dòng)規(guī)劃單元 12、路徑插補(bǔ)單元13依次連接形成數(shù)據(jù)處理流水線實(shí)現(xiàn)并行處理;輸入緩沖區(qū)4與幾何特征 提取單元9連接以讀取輸入的小線段數(shù)據(jù);路徑插補(bǔ)單元13與插補(bǔ)輸出緩沖區(qū)7連接以寫入 插補(bǔ)結(jié)果;規(guī)劃元生成單元11、路徑插補(bǔ)單元13和小線段緩沖區(qū)6連接進(jìn)行小線段數(shù)據(jù)的存 儲(chǔ)與交換;規(guī)劃元生成單元11、運(yùn)動(dòng)規(guī)劃單元12、路徑插補(bǔ)單元13和規(guī)劃元緩沖區(qū)14連接進(jìn) 行規(guī)劃元數(shù)據(jù)的存儲(chǔ)與交換(如圖1)。
[0064]在具體實(shí)施例的數(shù)控系統(tǒng)硬件方案中，數(shù)控系統(tǒng)的中央處理器15與可編程FPGA芯 片20連接。FPGA芯片包含數(shù)控系統(tǒng)固有的控制接口邏輯，包含進(jìn)給軸控制接口 17、主軸控制 接口 18和機(jī)床10控制接口 19 APGA芯片還包含了所述的小線段插補(bǔ)協(xié)處理器16 TPGA內(nèi)部 的上述四個(gè)組件均通過總線分別與數(shù)控系統(tǒng)的中央處理器15進(jìn)行連接。芯片中的進(jìn)給軸控 制接口與進(jìn)給伺服電機(jī)21連接;主軸控制接口與主軸電機(jī)22連接;機(jī)床10控制接口與繼電 器、電磁閥、傳感器組件23連接(如圖2)。
[0065] FPGA芯片內(nèi)部各組件均采用硬件描述語言編寫，并轉(zhuǎn)換為FPGA內(nèi)部的硬件邏輯電 路。其中，小線段協(xié)處理器的硬件邏輯也在該FPGA芯片內(nèi)部實(shí)現(xiàn)，無需增加額外芯片。協(xié)處 理器內(nèi)部的輸入緩沖區(qū)4、小線段緩沖區(qū)6、插補(bǔ)輸出緩沖區(qū)7和規(guī)劃元緩沖區(qū)14中的數(shù)據(jù)存 儲(chǔ)區(qū)域由FPGA內(nèi)的RAM塊資源實(shí)現(xiàn)，其他模塊可用通用邏輯資源實(shí)現(xiàn)。
[0066]在小線段緩沖區(qū)6內(nèi)部，存儲(chǔ)著小線段隊(duì)列；在規(guī)劃元緩沖區(qū)14內(nèi)部，存儲(chǔ)著規(guī)劃 元隊(duì)列。小線段隊(duì)列中的元素是每一條小線段的數(shù)據(jù)，包括起始點(diǎn)和終止點(diǎn)的位置坐標(biāo)、編 程速度、幾何特征數(shù)據(jù)、規(guī)劃約束數(shù)據(jù)。規(guī)劃元隊(duì)列中的元素是每一個(gè)規(guī)劃元的數(shù)據(jù)，包括 規(guī)劃元的長(zhǎng)度、規(guī)劃約束數(shù)據(jù)、規(guī)劃元包含的小線段范圍。
[0067]三軸數(shù)控系統(tǒng)高速高精度小線段插補(bǔ)協(xié)處理器的運(yùn)行方法，包含以下處理步驟：
[0068] (1)
[0069] S1:初始化，控制單元8將狀態(tài)寄存器3中緩沖區(qū)空閑標(biāo)志位置1，插補(bǔ)完成標(biāo)志位 置1;清空指令寄存器。
[0070] S2:控制單元等待數(shù)控系統(tǒng)中央處理器向指令寄存器寫入指令;若是執(zhí)行插補(bǔ)指 令，則進(jìn)入步驟S3;若是初始化指令，則進(jìn)入步驟S1。
[0071] S3:控制單元將狀態(tài)寄存器中插補(bǔ)完成標(biāo)志位清0。
[0072] S4:控制單元同步啟動(dòng)數(shù)據(jù)處理流水線上的幾何特征提取單元9、規(guī)劃約束單元 10、規(guī)劃元生成單元11、運(yùn)動(dòng)規(guī)劃單元12、路徑插補(bǔ)單元13。
[0073] S5:數(shù)據(jù)處理流水線上各單元執(zhí)行一次計(jì)算與邏輯處理，完成后各單元暫停。
[0074] S6:控制單元更新狀態(tài)寄存器3中緩沖區(qū)空閑標(biāo)志位，若輸入緩沖區(qū)4、小線段緩沖 區(qū)6和規(guī)劃元緩沖區(qū)14均未滿，則標(biāo)志位置1;否則，清0。
[0075] S7:控制單元將狀態(tài)寄存器3中的插補(bǔ)完成標(biāo)志位置1，清空指令寄存器。
[0076] S8:返回步驟S2。
[0077]如圖1所示，在小線段插補(bǔ)協(xié)處理器內(nèi)部，每條小線段的數(shù)據(jù)將依次通過幾何特征 提取單元9、規(guī)劃約束單元10、規(guī)劃元生成單元11、運(yùn)動(dòng)規(guī)劃單元12、路徑插補(bǔ)單元13組成的 數(shù)據(jù)處理流水線進(jìn)行分步驟的計(jì)算處理，最終生成插補(bǔ)數(shù)據(jù)。前一個(gè)單元的處理結(jié)果成為 下一個(gè)單元的數(shù)據(jù)輸入，各單元并行、獨(dú)立進(jìn)行計(jì)算，使得協(xié)處理器可以同時(shí)處理連續(xù)的多 條小線段。
[0078] (2)幾何特征提取單元9在步驟S5中的一次計(jì)算與邏輯處理進(jìn)一步包括以下步驟：
[0079] S5-9-1:檢測(cè)緩沖區(qū)空閑標(biāo)志位，若該標(biāo)志位為0，則幾何特征提取單元9自動(dòng)停 止。
[0080] S5-9-2:檢測(cè)輸入緩沖區(qū)4中是否有數(shù)控系統(tǒng)中央處理器新寫入的小線段數(shù)據(jù);若 沒有新寫入的小線段數(shù)據(jù)，則幾何特征提取單元自動(dòng)停止。
[0081] S5-9-3:從輸入緩沖區(qū)4中讀取一條小線段的數(shù)據(jù)。
[0082] S5-9-4:計(jì)算讀入小線段的幾何特征數(shù)據(jù)。
[0083] 圖3是本實(shí)施例中小線段的幾何特征數(shù)據(jù)的計(jì)算原理。若輸入的某段小線段起始 點(diǎn)為Pi，終止點(diǎn)為P1+1，計(jì)算出的小線段的幾何特征數(shù)據(jù)包括:長(zhǎng)度豕=|耳^|，單位方向向量
，方向改變量Af = f G和曲率半徑估計(jì)值
[0084] S5-9-5:將含有幾何特征的小線段數(shù)據(jù)輸出到規(guī)劃約束單元10。
[0085] S5-9-6:幾何特征提取單元9自動(dòng)停止。
[0086] (3)規(guī)劃約束單元10在步驟S5中的一次計(jì)算與邏輯處理進(jìn)一步包括以下步驟：
[0087] S5-10-1:檢測(cè)是否從幾何特征提取單元9輸入了新的小線段數(shù)據(jù);若沒有新輸入 的小線段數(shù)據(jù)，則規(guī)劃約束單元10自動(dòng)停止。
[0088] S5-10-2:讀取新輸入的小線段數(shù)據(jù)。
[0089] S5-10-3:根據(jù)配置寄存器中的規(guī)劃約束參數(shù)計(jì)算小線段的規(guī)劃約束條件。
[0090] 本實(shí)施例中，小線段的規(guī)劃約束條件包括小線段目標(biāo)運(yùn)動(dòng)速度Vtl和轉(zhuǎn)角位置速度 限制vci。配置寄存器中規(guī)劃約束參數(shù)中指定了最大向心加速度為A?，X、Y、Z各軸速度允許 的最大階躍量分別為Av mx，Avmy和Avmz。若小線段的編程速度為^，方向改變量為 = ( ，A?^，則小線段的規(guī)劃約束條件的計(jì)算方法為：小線段目標(biāo)運(yùn)動(dòng)速度 K.，，轉(zhuǎn)角位置速度限制
[0091] S5-10-4:將含有規(guī)劃約束條件的小線段數(shù)據(jù)輸出到規(guī)劃元生成單元11。
[0092] S5-10-5 :規(guī)劃約束單元自動(dòng)停止。
[0093] (4)規(guī)劃元生成單元11在步驟S5中的一次計(jì)算與邏輯處理進(jìn)一步包括以下步驟：
[0094] S5-11-1:檢測(cè)是否從規(guī)劃約束單元10輸入了新的小線段數(shù)據(jù);若沒有新輸入的小 線段數(shù)據(jù)，則將規(guī)劃元更新標(biāo)志位清〇，進(jìn)入步驟S5-11-5。
[0095] S5-11-2:將規(guī)劃元更新標(biāo)志位置1。
[0096] S5-11-3:判斷是否滿足連續(xù)運(yùn)動(dòng)條件;若小線段隊(duì)列為空，則直接判定不滿足連 續(xù)運(yùn)動(dòng)條件。否則，讀取小線段隊(duì)列尾部小線段數(shù)據(jù)，并與從規(guī)劃約束單元10輸入的小線段 數(shù)據(jù)進(jìn)行比較，判斷這兩條相連的小線段是否滿足連續(xù)運(yùn)動(dòng)條件。
[0097] 本例中兩條相連的小線段連續(xù)性判斷條件為:兩條相連的小線段的目標(biāo)運(yùn)動(dòng)速度 vtl相等，且該目標(biāo)運(yùn)動(dòng)速度vtd、于兩小線段間的轉(zhuǎn)角位置速度限制vcl。
[0098] S5-11-4:若滿足連續(xù)運(yùn)動(dòng)條件，則讀取規(guī)劃元隊(duì)列尾部的規(guī)劃元數(shù)據(jù)，將小線段 添加到該規(guī)劃元上，并更新規(guī)劃元隊(duì)列尾部的規(guī)劃元數(shù)據(jù)。若不滿足連續(xù)運(yùn)動(dòng)條件，則生成 一個(gè)含有新輸入的小線段的新規(guī)劃元數(shù)據(jù)，并添加到規(guī)劃元隊(duì)列尾部。
[0099] 如圖4所示，規(guī)劃元是運(yùn)動(dòng)速度規(guī)劃的基本單位，某個(gè)規(guī)劃元包含一條或多條連續(xù) 的小線段。圖4中，B卜B13是13條小線段，U1和U2是2個(gè)規(guī)劃元，分別包含M-B8段及B9-B13 段。圖4中，在讀入B1和B9時(shí)，因不滿足連續(xù)性條件，生成了 U1和U2;在讀入其他小線段時(shí)，不 生成新的規(guī)劃元，僅更新規(guī)劃元數(shù)據(jù)。規(guī)劃元的數(shù)據(jù)，包括規(guī)劃元的長(zhǎng)度、規(guī)劃約束數(shù)據(jù)、規(guī) 劃元包含的小線段范圍。
[0100] S5-11-5:將規(guī)劃元更新標(biāo)志位輸出到運(yùn)動(dòng)規(guī)劃單元12。
[0101] S5-11-6:規(guī)劃元生成單元自動(dòng)停止。
[0102] (5)運(yùn)動(dòng)規(guī)劃單元12在步驟S5中的一次計(jì)算與邏輯處理進(jìn)一步包括以下步驟：
[0103] S5-12-1:若規(guī)劃元隊(duì)列空，則運(yùn)動(dòng)規(guī)劃單元12自動(dòng)停止。
[0104] S5-12-2:檢測(cè)來自規(guī)劃元生成單元11的規(guī)劃元更新標(biāo)志位;若該位為1，則根據(jù)配 置寄存器2中的規(guī)劃約束參數(shù)對(duì)規(guī)劃元隊(duì)列重新進(jìn)行一次反向規(guī)劃。
[0105] 如圖5所示，本例中運(yùn)動(dòng)規(guī)劃采用包含7個(gè)運(yùn)動(dòng)階段的S型多項(xiàng)式加減速方式，其運(yùn) 動(dòng)曲線由配置寄存器5中規(guī)劃約束參數(shù)指定的最大加速度、最大加加速度，以及當(dāng)前的起始 速度和各運(yùn)動(dòng)段的時(shí)間參數(shù)T1~T7決定。
[0106] 如圖6所示，運(yùn)動(dòng)規(guī)劃采用前瞻方式進(jìn)行。反向規(guī)劃的目的是，保證在規(guī)劃元緩沖 區(qū)范圍內(nèi)可以平穩(wěn)減速到0,且各規(guī)劃元均能滿足規(guī)劃約束。假設(shè)緩沖區(qū)內(nèi)此時(shí)共有N個(gè)規(guī) 劃元，根據(jù)規(guī)劃約束參數(shù)，從最后一個(gè)規(guī)劃元U N開始進(jìn)行反向規(guī)劃，直至獲得到第一個(gè)規(guī)劃 元Ul的最大結(jié)束速度Vet。
[0107] S5-12-3:根據(jù)配置寄存器2中的規(guī)劃約束參數(shù)和反向規(guī)劃結(jié)果，對(duì)規(guī)劃元隊(duì)列頭 部的規(guī)劃元進(jìn)行正向規(guī)劃。
[0108] 運(yùn)動(dòng)規(guī)劃單元為了后續(xù)插補(bǔ)環(huán)節(jié)做準(zhǔn)備，需要提前計(jì)算出規(guī)劃元的運(yùn)動(dòng)曲線的參 數(shù)。本例中，對(duì)Ui進(jìn)行正向規(guī)劃，根據(jù)反向規(guī)劃計(jì)算的v et和規(guī)劃約束參數(shù)，計(jì)算出T1~T7(見 圖5和圖6)。
[0109] S5-12-4:將正向規(guī)劃結(jié)果輸出到路徑插補(bǔ)單元13。
[0110] S5-12-5:運(yùn)動(dòng)規(guī)劃單元自動(dòng)停止。
[0111 ] (6)路徑插補(bǔ)單元13在步驟S5中的一次計(jì)算與邏輯處理進(jìn)一步包括以下步驟：
[0112] S5-13-1:讀取配置寄存器(2)中插補(bǔ)數(shù)量參數(shù)n和插補(bǔ)周期參數(shù)。
[0113] S5-13-2:檢測(cè)是否從路徑插補(bǔ)單元13輸入了正向規(guī)劃結(jié)果;若沒有輸入正向規(guī)劃 結(jié)果，則向插補(bǔ)輸出緩沖區(qū)7寫入n個(gè)最后一次插補(bǔ)的坐標(biāo)，路徑插補(bǔ)單元自動(dòng)停止。
[0114] S5-13-3:根據(jù)插補(bǔ)周期參數(shù)、正向規(guī)劃結(jié)果對(duì)小線段隊(duì)列頭部的小線段數(shù)據(jù)進(jìn)行 n次數(shù)據(jù)采樣插補(bǔ)，計(jì)算得到n個(gè)時(shí)刻插補(bǔ)坐標(biāo)。
[0115] 本例中，根據(jù)規(guī)劃元的各運(yùn)動(dòng)段的時(shí)間參數(shù)T1~T7和S型加減速的規(guī)劃約束參數(shù) 即可確定運(yùn)動(dòng)曲線，再根據(jù)插補(bǔ)周期參數(shù)，可得到n個(gè)時(shí)刻的插補(bǔ)點(diǎn)在規(guī)劃元上的位移。最 后，根據(jù)規(guī)劃元與小線段的對(duì)應(yīng)關(guān)系，可計(jì)算出n個(gè)時(shí)刻插補(bǔ)點(diǎn)的坐標(biāo)。
[0116] S5-13-4:向插補(bǔ)輸出緩沖區(qū)7寫入計(jì)算的n個(gè)時(shí)刻插補(bǔ)坐標(biāo)。
[0117] S5-13-5:刪除小線段緩沖區(qū)6和規(guī)劃元緩沖區(qū)14中已完成插補(bǔ)的部分。
[0118] S5-13-6:路徑插補(bǔ)單元自動(dòng)停止。
[0119] (7)應(yīng)用三軸數(shù)控系統(tǒng)小線段程序的插補(bǔ)流程為：
[0120] S100:數(shù)控系統(tǒng)的中央處理器15向小線段插補(bǔ)協(xié)處理器16上的配置寄存器5寫入 配置參數(shù)。
[0121] S101:中央處理器向小線段插補(bǔ)協(xié)處理器上的指令寄存器2寫入初始化指令。
[0122] S102:中央處理器檢測(cè)小線段插補(bǔ)協(xié)處理器上的狀態(tài)寄存器3中的緩沖區(qū)空閑標(biāo) 志位，若該位為1，則向輸入緩沖區(qū)4內(nèi)寫入新的小線段數(shù)據(jù)。
[0123] S103:中央處理器向小線段插補(bǔ)協(xié)處理器上的指令寄存器2寫入執(zhí)行插補(bǔ)指令。
[0124] S104:中央處理器等待小線段插補(bǔ)協(xié)處理器上的狀態(tài)寄存器3中的插補(bǔ)完成標(biāo)志 位由〇變?yōu)?，等待期間中央處理器可以執(zhí)行其他任務(wù)。
[0125] S105:中央處理器從小線段插補(bǔ)協(xié)處理器上的插補(bǔ)輸出緩沖區(qū)7讀取插補(bǔ)結(jié)果。
[0126] S106:中央處理器將讀取的插補(bǔ)結(jié)果中各軸坐標(biāo)位置作為伺服電機(jī)指令，按照對(duì) 應(yīng)的插補(bǔ)時(shí)刻輸出到各進(jìn)給軸的伺服電機(jī)驅(qū)動(dòng)器上，驅(qū)動(dòng)各軸實(shí)現(xiàn)小線段運(yùn)動(dòng)。在各插補(bǔ) 時(shí)刻的間隔，中央處理器可以執(zhí)行其他任務(wù)。
[0127] 本例中，中央處理器將插補(bǔ)輸出緩沖區(qū)7的n個(gè)插補(bǔ)坐標(biāo)數(shù)據(jù)讀取到中央處理器的 內(nèi)部存儲(chǔ)器中進(jìn)行暫存，同時(shí)釋放插補(bǔ)輸出緩沖區(qū)，以接受新的插補(bǔ)輸出數(shù)據(jù)。在電機(jī)控制 周期到來時(shí)，中央處理器將內(nèi)部暫存的對(duì)應(yīng)該周期的插補(bǔ)數(shù)據(jù)輸出到進(jìn)給軸控制接口 17， 驅(qū)動(dòng)X、Y、Z軸的進(jìn)給伺服電機(jī)21執(zhí)行運(yùn)動(dòng)。
[0128] S107:若小線段程序的插補(bǔ)未完成，則繼續(xù)返回步驟S102。
[0129] 中央處理器在執(zhí)行小線段加工程序期間循環(huán)執(zhí)行上述步驟S102-S107,可以實(shí)現(xiàn) 連續(xù)的小線段實(shí)時(shí)插補(bǔ)計(jì)算處理。
[0130] 從本實(shí)施例可以看出，由于小線段協(xié)處理器的幾何特征提取單元、規(guī)劃約束單元、 規(guī)劃元生成單元、運(yùn)動(dòng)規(guī)劃單元、路徑插補(bǔ)單元采用數(shù)據(jù)處理流水線方式連接，使得各個(gè)單 元可以同時(shí)對(duì)多個(gè)順序相連的小線段進(jìn)行并行化計(jì)算操作，提升了計(jì)算效率，實(shí)現(xiàn)了小線 段加工程序的高速連續(xù)插補(bǔ)。小線段協(xié)處理器的全部硬件邏輯在數(shù)控系統(tǒng)的FPGA芯片內(nèi)部 實(shí)現(xiàn)，無需增加額外芯片。數(shù)控系統(tǒng)的中央處理器和小線段協(xié)處理器并行工作，小線段協(xié)處 理器分擔(dān)了復(fù)雜的高速高精度小線段插補(bǔ)任務(wù)，提升了三軸數(shù)控系統(tǒng)的整體性能。
[0131] 本實(shí)施例僅對(duì)一種實(shí)現(xiàn)具體實(shí)現(xiàn)方案進(jìn)行了描述。在具體實(shí)施中，各個(gè)單元實(shí)現(xiàn) 的具體計(jì)算方法可在本發(fā)明的小線段協(xié)處理器結(jié)構(gòu)內(nèi)進(jìn)行調(diào)整，滿足不同的小線段加工需 求。
【主權(quán)項(xiàng)】
1. 三軸數(shù)控系統(tǒng)高速高精度小線段插補(bǔ)協(xié)處理器，包括:數(shù)據(jù)讀寫接口、指令寄存器、 狀態(tài)寄存器、輸入緩沖區(qū)、配置寄存器、小線段緩沖區(qū)、插補(bǔ)輸出緩沖區(qū)、控制單元、幾何特 征提取單元、規(guī)劃約束單元、規(guī)劃元生成單元、運(yùn)動(dòng)規(guī)劃單元、路徑插補(bǔ)單元、以及規(guī)劃元緩 沖區(qū)，其特征在于:指令寄存器(2)、狀態(tài)寄存器(3)、輸入緩沖區(qū)(4)、配置寄存器(5)和插補(bǔ) 輸出緩沖區(qū)（7)通過數(shù)據(jù)讀寫接口（1)與數(shù)控系統(tǒng)中央處理器連接;幾何特征提取單元(9)、 規(guī)劃約束單元(10)、規(guī)劃元生成單元(11)、運(yùn)動(dòng)規(guī)劃單元(12)、路徑插補(bǔ)單元(13)依次連接 形成數(shù)據(jù)處理流水線實(shí)現(xiàn)并行處理;輸入緩沖區(qū)（4)與幾何特征提取單元連接以讀取輸入 的小線段數(shù)據(jù)；路徑插補(bǔ)單元與插補(bǔ)輸出緩沖區(qū)（7)連接以寫入插補(bǔ)結(jié)果；規(guī)劃元生成單 元、路徑插補(bǔ)單元和小線段緩沖區(qū)（6)連接進(jìn)行小線段數(shù)據(jù)的存儲(chǔ)與交換；規(guī)劃元生成單 元、運(yùn)動(dòng)規(guī)劃單元、路徑插補(bǔ)單元和規(guī)劃元緩沖區(qū)（14)并行連接進(jìn)行規(guī)劃元數(shù)據(jù)的存儲(chǔ)與 交換;在小線段緩沖區(qū)內(nèi)部，存儲(chǔ)有小線段隊(duì)列;在規(guī)劃元緩沖區(qū)內(nèi)部，存儲(chǔ)有規(guī)劃元隊(duì)列。2. -種按照權(quán)利要求1所述三軸數(shù)控系統(tǒng)高速高精度小線段插補(bǔ)協(xié)處理器的運(yùn)行方 法，其特征是:所述運(yùn)行方法包含以下處理步驟： SI:初始化，所述控制單元(8)將所述狀態(tài)寄存器(3)中緩沖區(qū)空閑標(biāo)志位置1，插補(bǔ)完 成標(biāo)志位置1;清空所述指令寄存器； S2:控制單元等待所述數(shù)控系統(tǒng)中央處理器向指令寄存器寫入指令;若是執(zhí)行插補(bǔ)指 令，則進(jìn)入步驟S3;若是初始化指令，則進(jìn)入步驟Sl; S3:控制單元將狀態(tài)寄存器中插補(bǔ)完成標(biāo)志位清0; S4:控制單元同步啟動(dòng)所述數(shù)據(jù)處理流水線上的幾何特征提取單元、規(guī)劃約束單元、規(guī) 劃元生成單元、運(yùn)動(dòng)規(guī)劃單元、路徑插補(bǔ)單元； S5:數(shù)據(jù)處理流水線上各單元執(zhí)行一次計(jì)算與邏輯處理，完成后各單元暫停； S6:控制單元更新狀態(tài)寄存器中緩沖區(qū)空閑標(biāo)志位，若輸入緩沖區(qū)、小線段緩沖區(qū)和規(guī) 劃元緩沖區(qū)均未滿，則標(biāo)志位置1;否則，清0; S7:控制單元將狀態(tài)寄存器中的插補(bǔ)完成標(biāo)志位置1，清空指令寄存器； S8:返回步驟S2。3. 按照權(quán)利要求2所述的三軸數(shù)控系統(tǒng)高速高精度小線段插補(bǔ)協(xié)處理器的運(yùn)行方法， 其特征在于:所述的幾何特征提取單元(9)在步驟S5中的一次計(jì)算與邏輯處理進(jìn)一步包括 以下步驟： S5-9-1:檢測(cè)緩沖區(qū)空閑標(biāo)志位，若該標(biāo)志位為0，則幾何特征提取單元自動(dòng)停止； S5-9-2:檢測(cè)輸入緩沖區(qū)中是否有數(shù)控系統(tǒng)中央處理器新寫入的小線段數(shù)據(jù);若沒有 新寫入的小線段數(shù)據(jù)，則幾何特征提取單元自動(dòng)停止； S5-9-3:從輸入緩沖區(qū)中讀取一條小線段的數(shù)據(jù)； S5-9-4:計(jì)算讀入小線段的幾何特征數(shù)據(jù)； S5-9-5:將含有幾何特征的小線段數(shù)據(jù)輸出到規(guī)劃約束單元； S5-9-6:幾何特征提取單元自動(dòng)停止。4. 按照權(quán)利要求2所述的三軸數(shù)控系統(tǒng)高速高精度小線段插補(bǔ)協(xié)處理器的運(yùn)行方法， 其特征在于:所述的規(guī)劃約束單元(10)在步驟S5中的一次計(jì)算與邏輯處理進(jìn)一步包括以下 步驟： S5-10-1:檢測(cè)是否從幾何特征提取單元輸入了新的小線段數(shù)據(jù);若沒有新輸入的小線 段數(shù)據(jù)，則規(guī)劃約束單元自動(dòng)停止； S5-10-2:讀取新輸入的小線段數(shù)據(jù)； S5-10-3:根據(jù)配置寄存器中的規(guī)劃約束參數(shù)計(jì)算小線段的規(guī)劃約束條件； S5-10-4:將含有規(guī)劃約束條件的小線段數(shù)據(jù)輸出到規(guī)劃元生成單元； S5-10-5:規(guī)劃約束單元自動(dòng)停止。5. 按照權(quán)利要求2所述的三軸數(shù)控系統(tǒng)高速高精度小線段插補(bǔ)協(xié)處理器的運(yùn)行方法， 其特征在于:所述的規(guī)劃元生成單元(11)在步驟S5中的一次計(jì)算與邏輯處理進(jìn)一步包括以 下步驟： S5-11-1:檢測(cè)是否從規(guī)劃約束單元輸入了新的小線段數(shù)據(jù);若沒有新輸入的小線段數(shù) 據(jù)，則將規(guī)劃元更新標(biāo)志位清〇，進(jìn)入步驟S5-11-5; S5-11-2:將規(guī)劃元更新標(biāo)志位置1; S5-11-3:判斷是否滿足連續(xù)運(yùn)動(dòng)條件;若小線段隊(duì)列為空，則直接判定不滿足連續(xù)運(yùn) 動(dòng)條件;否則，讀取小線段隊(duì)列尾部小線段數(shù)據(jù)，并與從規(guī)劃約束單元輸入的小線段數(shù)據(jù)進(jìn) 行比較，判斷這兩條相連的小線段是否滿足連續(xù)運(yùn)動(dòng)條件； S5-11-4:若滿足連續(xù)運(yùn)動(dòng)條件，則讀取規(guī)劃元隊(duì)列尾部的規(guī)劃元數(shù)據(jù)，將小線段添加 到該規(guī)劃元上，并更新規(guī)劃元隊(duì)列尾部的規(guī)劃元數(shù)據(jù);若不滿足連續(xù)運(yùn)動(dòng)條件，則生成一個(gè) 含有新輸入的小線段的新規(guī)劃元數(shù)據(jù)，并添加到規(guī)劃元隊(duì)列尾部； S5-11-5:將規(guī)劃元更新標(biāo)志位輸出到運(yùn)動(dòng)規(guī)劃單元； S5-11-6:規(guī)劃元生成單元自動(dòng)停止。6. 按照權(quán)利要求2所述的三軸數(shù)控系統(tǒng)高速高精度小線段插補(bǔ)協(xié)處理器的運(yùn)行方法， 其特征在于:所述的運(yùn)動(dòng)規(guī)劃單元(12)在步驟S5中的一次計(jì)算與邏輯處理進(jìn)一步包括以下 步驟： S5-12-1:若規(guī)劃元隊(duì)列空，則運(yùn)動(dòng)規(guī)劃單元自動(dòng)停止； S5-12-2:檢測(cè)來自規(guī)劃元生成單元的規(guī)劃元更新標(biāo)志位;若該位為1，則根據(jù)配置寄存 器中的規(guī)劃約束參數(shù)對(duì)規(guī)劃元隊(duì)列重新進(jìn)行一次反向規(guī)劃； S5-12-3:根據(jù)配置寄存器中的規(guī)劃約束參數(shù)和反向規(guī)劃結(jié)果，對(duì)規(guī)劃元隊(duì)列頭部的規(guī) 劃元進(jìn)行正向規(guī)劃； S5-12-4:將正向規(guī)劃結(jié)果輸出到路徑插補(bǔ)單元； S5-12-5:運(yùn)動(dòng)規(guī)劃單元自動(dòng)停止。7. 按照權(quán)利要求2所述的三軸數(shù)控系統(tǒng)高速高精度小線段插補(bǔ)協(xié)處理器的運(yùn)行方法， 其特征在于:所述的路徑插補(bǔ)單元(13)在步驟S5中的一次計(jì)算與邏輯處理進(jìn)一步包括以下 步驟： S5-13-1:讀取配置寄存器中插補(bǔ)數(shù)量參數(shù)η和插補(bǔ)周期參數(shù)； S5-13-2:檢測(cè)是否從路徑插補(bǔ)單元輸入了正向規(guī)劃結(jié)果;若沒有輸入正向規(guī)劃結(jié)果， 則向插補(bǔ)輸出緩沖區(qū)寫入η個(gè)最后一次插補(bǔ)的坐標(biāo)，路徑插補(bǔ)單元自動(dòng)停止； S5-13-3:根據(jù)插補(bǔ)周期參數(shù)、正向規(guī)劃結(jié)果對(duì)小線段隊(duì)列頭部的小線段數(shù)據(jù)進(jìn)行η次 數(shù)據(jù)采樣插補(bǔ)，計(jì)算得到η個(gè)時(shí)刻插補(bǔ)坐標(biāo)； S5-13-4:向插補(bǔ)輸出緩沖區(qū)寫入計(jì)算的η個(gè)時(shí)刻插補(bǔ)坐標(biāo)； S5-13-5:刪除小線段緩沖區(qū)和規(guī)劃元緩沖區(qū)中已完成插補(bǔ)的部分； S5-13-6:路徑插補(bǔ)單元自動(dòng)停止。
【文檔編號(hào)】G05B19/414GK105929796SQ201610286851
【公開日】2016年9月7日
【申請(qǐng)日】2016年5月3日
【發(fā)明人】董靖川, 王太勇, 張永賓, 申騰躍, 劉長(zhǎng)玲
【申請(qǐng)人】天津大學(xué)