基於兩級dsp的並聯裝備開放式運動控制卡及控制方法
2023-05-08 05:11:41
專利名稱：基於兩級dsp的並聯裝備開放式運動控制卡及控制方法
技術領域：
本發明涉及一種基於兩級DSP的並聯裝備開放式運動控制卡及控制方法，屬於開放式數控系統技術。
背景技術：
並聯機構是一組有兩個或兩個以上的分支機構並聯而成的機構。它的特點是所有分支機構可同時接受驅動器輸入，而最終共同給出輸出，在機構學上屬於多路閉環機械系統。以並聯機構作為傳動進給機構的數控工具機、機器人操作機以及由此組成的製造單元統稱為並聯構型裝備，是近年來發展起來的一種有著廣闊應用前景的新型製造裝備。
並聯構型裝備的最大特點在於其結構簡單而控制複雜，由於在笛卡爾空間中的運動是關節空間各伺服軸運動的非線性映射(又稱虛實映射)，即使是笛卡爾坐標單軸上的勻速直線運動也要由關節空間內幾個伺服軸同時非線性運動共同合成，因此在進行運動控制時，必須先將事先給定的機構末端位姿以及速度信息通過運動學逆解運算轉換為關節空間各伺服軸的控制指令，才能驅動並聯機構實現期望運動。
目前的商用控制卡都是基於笛卡爾空間坐標系設計的，其在操作空間內的運動控制量與伺服電機的控制量之間是簡單直接的線性對應關係，不能直接應用於並聯機構的控制，因此建造並聯機構控制系統普遍採用在「PC機十傳統運動控制卡」的結構基礎上進行二次開發的方法以工控機作為基本平臺，負責運動學模型運算、軌跡規劃以及人機互動、系統管理等實時性要求不高但計算量大的任務，運動控制卡僅完成對伺服單元的實時控制。這類系統雖然結構比較簡單，容易實現，但實踐上難以滿足對並聯機構的運動軌跡高速、高精度控制。主要原因如下1.並聯構型裝備採用的插補策略與傳統串聯機構有重大區別。以「PC+傳統運動控制卡」為硬體平臺的並聯機構控制系統採用的是二次插補策略，即粗插補和細插補分別由PC機和控制卡分開進行。由於運動控制卡本身對插補周期的限制，為了保證並聯機構的控制精度，就必須限制其進給速度，因而難以實現並聯裝備高速高精度的要求；2.由於對並聯機構的運動控制必須由PC機與控制卡共同協調完成，在兩個CPU之間頻繁進行大量數據交換極易形成通訊瓶頸，進而影響整個系統的高速運行；
3.並聯構型裝備具有非線性、強耦合、時變的動力學特性，原則上應針對不同構型特點採用參數可變的智能控制算法以提高其動態性能。然而由於目前的運動控制卡大多只提供系統接口的開放，其核心控制策略仍是封閉的，用戶無法更改其內部控制算法，導致並聯裝備的工作性能難以進一步提高。
由此可見，由於在控制原理上存在較大差異，現有傳統的運動控制卡並不完全適合於並聯機構的控制特點，而且不同構型的並聯裝備結構和尺度參數不盡相同，用戶應能很方便地對運動學模型及控制策略進行修改，這要求控制系統有極高的靈活性和開放性，因此，針對並聯機構的特點研製具有開放式體系結構的新型運動控制卡，對推動並聯構型裝備的實用化、產業化進程具有十分重要的意義。
發明內容
本發明的目的在於提供一種基於兩級DSP的並聯裝備開放式運動控制卡及控制方法。該系統具有更強的實時性和更快的處理速度；實現運動軌跡的直接插補，滿足並聯構型裝備高速高精度的控制需要。
為達到上述目的，本發明是通過下述技術方案加以實現的，一種基於兩級DSP的並聯裝備開放式運動控制卡，其特徵在於控制卡採用多CPU並行處理的兩級結構，將控制卡的任務調度、系統監控和伺服控制功能分級處理控制級由一個高端的數位訊號處理器(DSP)構成中央處理單元，作為指揮、協調整個系統工作的核心；執行級由兩片以上DSP構成並行處理的電機控制單元，每個執行級的DSP分別負責1～2個伺服驅動單元的直接控制，伺服單元由交流伺服驅動器和伺服電機組成；中央處理單元和執行級的電機控制單元通過現場可編程門陣列(FPGA)晶片構造的通訊接口邏輯電路相連接，在先進先出寄存器(FIFO)和同步邏輯控制信號的控制下實現各單元間的高速數據傳輸和協調同步。
採用上述控制卡實現控制方法其過程為1.首先根據插補周期，由中央處理CPU將操作空間中的軌跡段在時間軸上進行細化，並根據系統加速度要求，插入若干速度過渡段，一次性完成插補工作；2.中央處理CPU完成插補運算後，將得到的笛卡爾空間離散點的包括位置、速度、加速度的大量數據以運動控制指令序列的形式寫入FIFO，通過FPGA的同步控制邏輯保證不同的電機控制單元能夠同步讀取控制指令。
3.控制級CPU調用運動學逆解模型解算出關節空間軸電機的位置、速度控制量，與電機實際位置反饋信號進行比較得到位置誤差值，通過系統設定的控制算法處理器和數/模轉換器(DAC)轉換為±10V的模擬電壓速度控制信號，輸出到交流伺服驅動器，實現對電機運動狀態的位置、速度雙閉環控制。
本發明的優越性在於針對並聯機構的運動控制特點，採用了多CPU並行處理的兩級控制結構，該結構的優勢在於通過將任務合理分配，將總體決策管理與具體執行任務分離、非實時性與實時性任務分離，可以最大限度地發揮各子系統的運行效率，提高整個系統的計算速度，而且PC機不再參與實時控制，減輕了與PC間的數據通訊壓力；由於計算性能提高，在軌跡插補時實現了粗精插補合一的的直接插補方法，使系統插補周期大大縮短，從而有效提高並聯機構的進給速度和定位精度；採用了完全開放式的控制結構，支持運動控制策略的用戶定製，允許用戶根據不同機構的特點和控制需要應用各種複雜的智能控制算法，能使系統獲得更好的動態性能，從而滿足並聯構型裝備用於高速高精度控制的需要。
圖1為本發明的運動控制卡整體結構原理框2本發明的運動控制卡DSP間通訊控制接口原理框3為本發明的軌跡控制方法原理框4為本發明的控制卡系統軟體結構框5為本發明的控制卡控制的三自由度並聯機構的結構示意中1為伺服電機，2為連架從動臂，3為主動臂，4為支架，5為從動臂，6為動平臺，7為手爪，8為進給機構。
具體實施例方式
下面結合附圖對本發明加以說明。
由圖1所示為本控制卡的原理框圖，控制卡採用了多CPU並行處理的兩級控制結構，控制級選用TI公司的DSP晶片TMS320VC5402作為中央處理單元。TMS320C5402能夠達到100MIPS的指令處理速度，擁有較多的外圍電路接口和較好的數據處理能力。中央處理CPU實現系統的整體控制，包括任務調度、控制決策、故障處理、誤差補償，以及操作空間運動軌跡的計算，如運動曲線的軌跡插補和速度控制。由於中央處理CPU不參與電機的具體控制，只需完成運算並將結果傳送到電機控制CPU，不需要進行外圍接口控制，故控制卡中採用了TMS320VC5402的最小系統電路作為基本電路結構。
執行級選用兩片以上的DSP晶片TMS320LF2407A作為電機控制單元的核心處理器，其基本電路與當前通用運動控制卡的基本電路結構相同。為了保證對多軸電機處理的快速性和精確性，每個電機控制CPU只負責控制1～2軸電機，保證每個伺服軸具有30μS以下的刷新時間和可控的、複雜的電機加減速控制能力。外圍電路主要包括64K*16bit的外部程序存儲器SRAM(61LV6416)、64K*16Bit的外部數據存儲器SRAM(61LV6416)和仿真口(JTAG)等。
控制卡採用了多CPU並行處理的兩級控制結構，為解決各CPU之間的數據通訊與協調同步問題，中央處理單元與電機控制單元通過FPGA構造的通訊接口邏輯電路相連接。利用FPGA的資源，構造多路結構相同的雙向FIFO，每路與一個電機控制CPU對應。中央處理CPU完成插補運算後，將得到的笛卡爾空間離散點的位置、速度、加速度等大量數據以運動控制指令序列的形式寫入FIFO，由FPGA的控制邏輯保證不同的電機控制單元能夠同步讀取控制指令。電機控制CPU讀取FIFO中的控制指令後，調用運動學逆解模型進行反演，生成關節空間軸電機的位置、速度控制量，與實際位置反饋信號進行比較得到位置誤差值，通過系統設定的控制算法處理器和數/模轉換器(DAC)將信號轉換為±10V的模擬電壓速度控制信號，輸出到交流伺服驅動器實現對電機運動狀態的位置、速度雙閉環控制。
控制卡採集增量式脈衝編碼器作為位置反饋信號，脈衝編碼盤輸出+A、-A、+B、-B、+Z、-Z三組差分形式的方波脈衝，脈衝編碼器產生的信號經控制卡的DS26LS32晶片差分整形後得到整形後的A、A-、B、B-、Z信號。通過A相與B相的相位關係可判別電機的旋轉方向，利用系統中FPGA的部分資源實現對反饋信號的倍頻/鑑相/計數的功能，構成位置信號反饋迴路。控制卡的數/模轉換接口採用D/A轉換晶片DAC7724提供四通道12位並行輸入、電壓型輸出；模/數轉換接口選用四通道12位並行輸出、電壓差分輸入的A/D轉換晶片ADS7864，可用於電機轉矩和轉速等其它模擬信號的反饋輸入。
與PC機組成系統時，PC只需實現用戶操作界面和基本軌跡規劃等實時性要求不高的任務，這樣就大大減少了工控機和運動控制卡之間的通訊數據量和對實時性能力的要求，從而有效避免了PC與控制卡間形成數據通訊瓶頸的問題。考慮到PCI接口具有較高的數據傳輸速度和兼容性，使用PCI總線可充分利用工控機的控制功能和系統資源，本控制卡的PC通訊接口選用了PCI接口，採用當前比較成熟的PCI接口晶片PXI9052設計控制卡的通訊接口，並通過雙埠RAM(DPRAM)實現PC機和控制卡之間的數據通訊。DPRAM選用Integrated DeviceTechnology，Inc.公司的高速8K×16DUAL-PORT STATIC RAM IDT7025。
由於並聯機構的末端執行器在笛卡爾空間的運動是關節空間各伺服軸運動的非線性映射，因此折算到關節空間的等效負載是耦合的，且是位形的函數，因此在運動過程中應動態調節各控制器的PID參數。針對並聯機構這種非線性、強耦合、時變的動力學特徵，系統採用變參數的PID控制算法。為了使系統具有良好的開放性和可擴展性，為用戶保留了在線編程接口，允許用戶對系統的邏輯功能或程序進行動態修改。用戶可根據並聯機構的結構特點和不同應用場合下的控制要求自行定製運動學模型以及控制算法，包括各種複雜的智能控制算法。用戶程序首先須經過彙編、連接生成DSP能夠識別的機器代碼，再通過為用戶保留的在線編程接口下載到閃速存儲器(FLASH)中，在系統復位時由DSP片內掩膜ROM中固化的引導裝載(Boot loader)程序，採用並行加載方法將用戶算法程序從外部引導到高速RAM中，保證其高速運行。選擇適當的控制算法能夠進一步改善整個系統的動態特性。
為實現控制卡系統內核完全開放，在硬體設計中採用了可重構器件FPGA構造邏輯接口和位置反饋電路。FPGA的主要優點就在於具有非常強的靈活性，其內部的具體邏輯功能可以根據需要動態配置，對電路結構的修改和維護很方便。系統採用這種構造方式實現硬體功能軟體化的過程，使運動控制卡內部結構開放，極大增強了系統的開放性。
圖5示出了3自由度並聯機構的三維實體造型。該機構包括機架、動平臺和兩個結構相同的支鏈，各支鏈含兩個平行四邊形。安裝在機架上的伺服電機分別帶動主動臂轉動，進而可實現動平臺的二自由度平動。在與機構運動平面垂直的方向串接一進給機構，以伺服電機+滾珠絲槓傳動，構成3自由度並聯機構。
採用本發明的控制卡，實現對上述機構的控制過程如下(如圖3所示)(1)由中央處理CPU離散控制軌跡根據設定的速度和插補周期，將運動軌跡在「時間軸」上離散成為操作空間中的點序，並根據設定的加速度確定需要插入的過渡段。將計算結果以運動指令序列的形式(離散點位置、速度、加速度)通過FIFO發送到相應的執行級CPU；(2)由執行級CPU進行位置與速度反演執行級CPU分別調用位置、姿態和速度加速度逆解模型，對插補後的離散點序列進行反演，得到關節空間中的相應離散點序，即各伺服軸電機的位置、速度控制量；(3)由執行級CPU進行變參數位置PID控制伺服電機的給定位置與電機實際位置反饋信號進行比較得到位置誤差值，通過變參數位置PID控制器和數/模轉換器(DAC)轉換為±10V的模擬電壓速度控制信號，輸出到交流伺服驅動器，實現對電機運動狀態的位置、速度雙閉環控制。各電機控制單元間通過FPGA的同步控制實現協調同步，驅動伺服電機使機構末端走出希望軌跡。
權利要求
1.一種基於兩級DSP的並聯裝備開放式運動控制卡，其特徵在於，該控制卡採用多CPU並行處理的兩級結構，將控制卡的任務調度、系統監控和伺服控制功能分級處理控制級由一個高端的數位訊號處理器構成中央處理單元，作為指揮、協調整個系統工作的核心；執行級由兩片以上DSP構成並行處理的電機控制單元，每個執行級的DSP分別負責1～2個伺服驅動單元的直接控制，伺服單元由交流伺服驅動器和伺服電機組成；中央處理單元和執行級的電機控制單元通過現場可編程門陣列晶片構造的通訊接口邏輯電路相連接，在先進先出寄存器和同步邏輯控制信號的控制下實現各單元間的高速數據傳輸和協調同步。
2.一種採用按權利要求1所述的運動控制卡基於兩級DSP的控制並聯裝備的方法，其特徵在於包括以下過程1)首先根據插補周期，由中央處理CPU將操作空間中的軌跡段在時間軸上進行細化，並根據系統加速度要求，插入若干速度過渡段，一次性完成插補工作；2)中央處理CPU完成插補運算後，將得到的笛卡爾空間離散點的包括位置、速度、加速度的大量數據以運動控制指令序列的形式寫入先進先出寄存器，通過現場可編程門陣列的同步控制邏輯保證不同的電機控制單元能夠同步讀取控制指令；3)控制級CPU調用運動學逆解模型解算出關節空間軸電機的位置、速度控制量，與電機實際位置反饋信號進行比較得到位置誤差值，通過系統設定的控制算法處理器和數/模轉換器轉換為±10伏的模擬電壓速度控制信號，輸出到交流伺服驅動器，實現對電機運動狀態的位置、速度雙閉環控制。
全文摘要
本發明公開了一種基於兩級DSP的並聯裝備開放式運動控制卡及控制方法。該系統採用了多CPU並行處理的兩級控制結構，將系統的總體控制和電機的具體控制功能分離，由控制級CPU作為控制卡的核心，控制和協調整個系統的操作；採用可編程邏輯器件FPGA構造通訊接口，將控制級的中央處理單元和執行級的電機控制單元與FPGA晶片相連接，通過FPGA控制FIFO和同步狀態信號實現數據的高速傳輸和電機同步控制；控制方法包括中央處理CPU離散控制軌跡，由執行級CPU進行位置與速度反演，然後通過變參數位置PID控制伺服電機。本發明的控制卡具有開放性好、處理速度快、控制能力強、實時性高等特點，適合控制具有非線性、強耦合、時變特點的並聯構型裝備。
文檔編號G05B15/02GK1808319SQ20051012232
公開日2006年7月26日 申請日期2005年12月13日 優先權日2005年12月13日
發明者倪雁冰, 王輝, 李可 申請人:天津大學