如何采用DSP技術(shù)實(shí)現(xiàn)運(yùn)動(dòng)控制器的設(shè)計(jì)？

運(yùn)動(dòng)控制技術(shù)是數(shù)控機(jī)床的關(guān)鍵技術(shù)，其技術(shù)水平的高低將直接影響一個(gè)國(guó)家裝備制造業(yè)的發(fā)展水平。目前，多軸伺服控制器越來(lái)越多地運(yùn)用在運(yùn)動(dòng)控制系統(tǒng)中，具有較高的集成度和靈活性，可實(shí)時(shí)完成運(yùn)動(dòng)控制過(guò)程中復(fù)雜的邏輯處理和控制算法，能實(shí)現(xiàn)多軸高速高精度的伺服控制。

實(shí)時(shí)實(shí)施復(fù)雜控制算法的關(guān)鍵這些運(yùn)動(dòng)控制系統(tǒng)已經(jīng)成為功能強(qiáng)大的數(shù)字信號(hào)處理器(DSP)的出現(xiàn)。即使在要求較低但成本敏感的應(yīng)用中，例如家用冰箱壓縮機(jī)驅(qū)動(dòng)器，也可以利用DSP的功能來(lái)實(shí)現(xiàn)無(wú)傳感器控制算法，可降低系統(tǒng)成本并提高驅(qū)動(dòng)器的整體穩(wěn)健性。在高性能伺服驅(qū)動(dòng)器中，DSP的強(qiáng)大計(jì)算能力允許通過(guò)矢量控制，紋波轉(zhuǎn)矩降低，預(yù)測(cè)控制結(jié)構(gòu)以及對(duì)非理想系統(tǒng)行為的補(bǔ)償進(jìn)行更精確的控制。

運(yùn)動(dòng)控制器是運(yùn)動(dòng)控制系統(tǒng)的核心部件。國(guó)內(nèi)的運(yùn)動(dòng)控制器大致可以分為3類(lèi)：

第1類(lèi)是以單片機(jī)等微處理器作為控制核心的運(yùn)動(dòng)控制器。這類(lèi)運(yùn)動(dòng)控制器速度較慢、精度不高、成本相對(duì)較低，只能在一些低速運(yùn)行和對(duì)軌跡要求不高的輪廓運(yùn)動(dòng)控制場(chǎng)合應(yīng)用。

第2類(lèi)是以專(zhuān)用芯片(ASIC)作為核心處理器的運(yùn)動(dòng)控制器，這類(lèi)運(yùn)動(dòng)控制器結(jié)構(gòu)比較簡(jiǎn)單，大多只能輸出脈沖信號(hào)，工作于開(kāi)環(huán)控制方式。由于這類(lèi)控制器不能提供連續(xù)插補(bǔ)功能，也沒(méi)有前饋功能，特別是對(duì)于大量的小線段連續(xù)運(yùn)動(dòng)的場(chǎng)合不能使用這類(lèi)控制器。

第3類(lèi)是基于PC總線的以DSP或FPGA作為核心處理器的開(kāi)放式運(yùn)動(dòng)控制器。這類(lèi)開(kāi)放式運(yùn)動(dòng)控制器以DSP芯片作為運(yùn)動(dòng)控制器的核心處理器，以PC機(jī)作為信息處理平臺(tái)，運(yùn)動(dòng)控制器以插件形式嵌入PC機(jī)，即“PC+運(yùn)動(dòng)控制器”的模式。這樣的運(yùn)動(dòng)控制器具有信息處理能力強(qiáng)，開(kāi)放程度高，運(yùn)動(dòng)軌跡控制準(zhǔn)確，通用性好的特點(diǎn)。但是這種方式存在以下缺點(diǎn)：運(yùn)動(dòng)控制卡需要插入計(jì)算機(jī)主板的PCI或者ISA插槽，因此每個(gè)具體應(yīng)用都必須配置一臺(tái)PC機(jī)作為上位機(jī)。這無(wú)疑對(duì)設(shè)備的體積、成本和運(yùn)行環(huán)境都有一定的限制，難以獨(dú)立運(yùn)行和小型化。

DSP 在電機(jī)控制方面的應(yīng)用是一個(gè)新領(lǐng)域。DSP 是應(yīng)用高性能的處理器提高對(duì)電機(jī)控制精度的一種芯片。高速的DSP 主要用在電機(jī)無(wú)傳感器控制和磁場(chǎng)定向控制中,因?yàn)樵跓o(wú)傳感器控制中需要用已知的電流和電壓實(shí)時(shí)計(jì)算速度和位置,而在電機(jī)磁場(chǎng)定向控制中,需要把所有的變量以矢量形式轉(zhuǎn)化到與定子旋轉(zhuǎn)磁場(chǎng)同步的坐標(biāo)系中,這些都需要進(jìn)行大量的運(yùn)算,高速的DSP可以實(shí)時(shí)完成這些工作。在價(jià)格上DSP已經(jīng)從最初期的幾百美元降到了幾美元。16位的DSP的性能也從5Mips(百萬(wàn)次每秒)提高到了2000Mips。包括大容量片內(nèi)存儲(chǔ)器,還在片上集成多種外圍設(shè)備。

DSP的數(shù)據(jù)運(yùn)算處理功能強(qiáng)大,即使在很復(fù)雜的控制中,采樣周期也可以取得很小,控制效果更接近于連續(xù)系統(tǒng)。把DSP與PC的各自優(yōu)勢(shì)結(jié)合將是高性能數(shù)控系統(tǒng)的發(fā)展趨勢(shì)。本運(yùn)動(dòng)控制器采用TI公司的高性能浮點(diǎn)DSP作為主控芯片,通過(guò)ISA接口與PC協(xié)調(diào)并進(jìn)行數(shù)據(jù)交換,以PC計(jì)算機(jī)作為基本平臺(tái),以DSP高速運(yùn)動(dòng)控制卡作細(xì)插補(bǔ)、伺服控制的核心,對(duì)直線電機(jī)的運(yùn)動(dòng)進(jìn)行控制,取得了良好的實(shí)際應(yīng)用效果。

基于“ PC+ 運(yùn)動(dòng)控制器”結(jié)構(gòu)的開(kāi)放式機(jī)器人運(yùn)動(dòng)控制系統(tǒng)能夠充分利用 PC 開(kāi)放程度高、通用性好、處理能力強(qiáng)等特點(diǎn)以及運(yùn)動(dòng)控制器運(yùn)算速度快、實(shí)時(shí)性能好、控制能力強(qiáng)等特點(diǎn)，因此得到較快發(fā)展，成為目前的研究熱點(diǎn)。但目前采用此種結(jié)構(gòu)的開(kāi)放式機(jī)器人運(yùn)動(dòng)控制系統(tǒng)中，不管是控制器供應(yīng)商所提供的運(yùn)動(dòng)控制器或者是科研人員自主設(shè)計(jì)的運(yùn)動(dòng)控制器，在通用性、軟硬件可重構(gòu)方面都存在一些問(wèn)題，影響著機(jī)器人運(yùn)動(dòng)控制系統(tǒng)的開(kāi)放性。因此，本文通過(guò)研究開(kāi)放式機(jī)器人運(yùn)動(dòng)控制器的結(jié)構(gòu)特點(diǎn)，制定了基于 DSP+FPGA的開(kāi)放式機(jī)器人運(yùn)動(dòng)控制器的總體設(shè)計(jì)方案。根據(jù)所制定的設(shè)計(jì)方案，設(shè)計(jì)并實(shí)現(xiàn)了基于 DSP+FPGA 的開(kāi)放式機(jī)器人運(yùn)動(dòng)控制器，并研究了基于開(kāi)放式機(jī)器人運(yùn)動(dòng)控制器的

運(yùn)動(dòng)控制核心算法。本文通過(guò)充分利用 DSP 的高速數(shù)字信號(hào)處理能力和 FPGA 的強(qiáng)大并發(fā)控制能力以及豐富的邏輯資源來(lái)使運(yùn)動(dòng)控制器獲得較高的運(yùn)算與控制能力。通過(guò)充分利用 FPGA 的邏輯編程能力來(lái)代替集成芯片以及實(shí)現(xiàn)邏輯連接，實(shí)現(xiàn)運(yùn)動(dòng)控制器的軟硬件可重構(gòu)，提高機(jī)器人運(yùn)動(dòng)控制器的開(kāi)放性。并通過(guò)研究曲線擬合預(yù)處理技術(shù)以及速度前瞻技術(shù)來(lái)提高運(yùn)動(dòng)控制器的控制效率，實(shí)現(xiàn)高速運(yùn)動(dòng)控制。

本文所設(shè)計(jì)的機(jī)器人運(yùn)動(dòng)控制器應(yīng)用于開(kāi)放式機(jī)器人控制系統(tǒng)中，與工業(yè) PC 、伺服系統(tǒng)、機(jī)器人本體等組成開(kāi)放式機(jī)器人控制系統(tǒng)。控制系統(tǒng)結(jié)構(gòu)形式為 PC+ 運(yùn)動(dòng)控制器，其中 PC 主要實(shí)現(xiàn)人機(jī)界面、參數(shù)配置、路徑規(guī)劃、機(jī)器人運(yùn)動(dòng)學(xué)正變換與逆變換、多機(jī)通信等功能。而基于 DSP+FPGA 的運(yùn)動(dòng)控制器則主要完成實(shí)時(shí)性要求較高的軌跡規(guī)劃、速度規(guī)劃、位置控制、以及狀態(tài)反饋等任務(wù)。 PC 與運(yùn)動(dòng)控制器采用具有開(kāi)放性的 PC104Plus 總線進(jìn)行通信，通過(guò)雙端口 RAM 進(jìn)行數(shù)據(jù)共享與同步。系統(tǒng)總體結(jié)構(gòu)如圖 2.1 所示。其中 I/O 接口卡主要完成光耦隔離和電平轉(zhuǎn)換，保護(hù)運(yùn)動(dòng)控制器不因來(lái)自伺服系統(tǒng)的脈沖電壓而受到損害。伺服系統(tǒng)實(shí)現(xiàn)對(duì)機(jī)器人運(yùn)動(dòng)關(guān)節(jié)的伺服驅(qū)動(dòng)和光電編碼反饋，為運(yùn)動(dòng)控制器提供實(shí)時(shí)的機(jī)器人運(yùn)動(dòng)關(guān)節(jié)速度和位置狀況。

運(yùn)動(dòng)控制器是數(shù)控機(jī)床、機(jī)器人等一類(lèi)機(jī)電一體化設(shè)備中常用的核心運(yùn)動(dòng)控制部件。現(xiàn)代數(shù)控技術(shù)對(duì)運(yùn)動(dòng)控制系統(tǒng)的開(kāi)放性、實(shí)時(shí)性、加工速度和精確度等性能指標(biāo)提出了越來(lái)越高的要求。隨著集成電路技術(shù)、微電子技術(shù)、計(jì)算機(jī)技術(shù)不斷發(fā)展，運(yùn)動(dòng)控制器已經(jīng)從以單片機(jī)、微處理器和專(zhuān)用芯片作為核心的運(yùn)動(dòng)控制器發(fā)展到基于PC機(jī)平臺(tái)的以數(shù)字信號(hào)處理器(DSP)和復(fù)雜可編程邏輯器件(CPLD)作為核心處理器的協(xié)處理架構(gòu)的開(kāi)放式運(yùn)動(dòng)控制器。這種將P C機(jī)的信息處理能力和開(kāi)放式的特點(diǎn)與運(yùn)動(dòng)控制器很強(qiáng)的運(yùn)動(dòng)軌跡控制能力有機(jī)地結(jié)合在一起，具有信息處理能力強(qiáng)、開(kāi)放程度高、運(yùn)動(dòng)軌跡控制準(zhǔn)確和通用性好的特點(diǎn)。這種模式在一個(gè)統(tǒng)一的人機(jī)對(duì)話平臺(tái)上，通過(guò)DSP算法和CPLD配置進(jìn)行適當(dāng)?shù)恼{(diào)整來(lái)實(shí)現(xiàn)不同的硬件功能模塊，可以方便地和多種類(lèi)型的驅(qū)動(dòng)器進(jìn)行連接。因此現(xiàn)在基于PC的新一代運(yùn)動(dòng)控制器成為控制系統(tǒng)的主流和發(fā)展方向。

本文開(kāi)發(fā)的開(kāi)放式運(yùn)動(dòng)控制器以PC機(jī)為平臺(tái)，完成坐標(biāo)變換、軌跡規(guī)劃、粗插補(bǔ)運(yùn)算等控制指令的設(shè)置和發(fā)送，同時(shí)可以實(shí)時(shí)顯示當(dāng)前伺服系統(tǒng)的運(yùn)動(dòng)位置、速度和電機(jī)狀態(tài)等參數(shù)。運(yùn)動(dòng)控制器以DSP芯片作為核心處理器，完成數(shù)據(jù)處理和控制算法、進(jìn)行保護(hù)中斷的處理，通過(guò)PCI或USB總線與PC機(jī)實(shí)時(shí)通訊。CPLD芯片作為協(xié)處理器完成編碼信號(hào)的采集，鑒相處理，對(duì)脈沖和模擬量進(jìn)行配置和輸出，把DSP處理過(guò)的控制數(shù)據(jù)經(jīng)過(guò)內(nèi)部轉(zhuǎn)換送到外部設(shè)備，并管理DSP和各種外部設(shè)備的接口。該運(yùn)動(dòng)控制器可以完成非勻速比同步運(yùn)動(dòng)控制，支持NURBS插補(bǔ)，并提供了豐富的動(dòng)態(tài)鏈接庫(kù)函數(shù)。

2 運(yùn)動(dòng)控制器工作原理和硬件構(gòu)成

該控制器可實(shí)現(xiàn)四路編碼器反饋和電機(jī)控制，其核心是TI公司的TMS320LF2407數(shù)字信號(hào)處理芯片和ALTERA 公司的MAX 7000S系列CPLD器件EPM7128SLC84?？刂破鞯挠布Y(jié)構(gòu)如圖1所示。DSP完成與上位機(jī)的實(shí)時(shí)通訊，對(duì)伺服電機(jī)速度和位置的精確控制，電機(jī)狀態(tài)監(jiān)測(cè)和外部信號(hào)檢測(cè)等功能。雙口SRAM(DUAL-PORT STATIC RAM)不僅提供控制器與上位機(jī)的通訊接口，而且還為運(yùn)動(dòng)軌跡控制提供了足夠的緩沖區(qū)。由于一片DSP只提供兩組正交編碼脈沖(QEP)電路，對(duì)于多于兩個(gè)電機(jī)的控制器，若用多片DSP不僅會(huì)造成DSP資源的浪費(fèi)，而且還會(huì)增加由于協(xié)調(diào)DSP之間的工作而產(chǎn)生的難度，因此這里我們用CPLD來(lái)提供另外兩組正交編碼脈沖電路，對(duì)速度、位置進(jìn)行采樣，同時(shí)用CPLD提供高速穩(wěn)定的其它邏輯控制電路。

運(yùn)動(dòng)控制器由基板和接口卡組成，集成了DSP核心電路，通信電路，CPLD譯碼、中斷、正交編碼處理電路，編碼器信號(hào)處理電路，電壓轉(zhuǎn)換電路，光電隔離電路等。運(yùn)行過(guò)程中，PC機(jī)把粗插補(bǔ)的數(shù)據(jù)通過(guò)ISP-1581芯片傳遞給DSP進(jìn)行時(shí)間分割精插補(bǔ)。在每一個(gè)伺服周期中，CPLD器件和DSP各處理兩路反饋的正交編碼信號(hào)進(jìn)而獲取實(shí)時(shí)位置和速度，DSP計(jì)算出理論插補(bǔ)位置與實(shí)際位置的偏差，基于速度和加速度前饋進(jìn)行PID調(diào)節(jié)，計(jì)算獲得速度控制量，產(chǎn)生的輸出信號(hào)經(jīng)DAC7625進(jìn)行數(shù)模轉(zhuǎn)換及放大電路放大后將模擬電壓發(fā)送至伺服驅(qū)動(dòng)器以控制電機(jī)。

DSP兩個(gè)事件管理模塊中的正交編碼脈沖(QEP)電路可以實(shí)現(xiàn)對(duì)兩對(duì)正交編碼信號(hào)的四倍頻和方向檢測(cè)，其定時(shí)器工作在增減計(jì)數(shù)模式，依據(jù)電機(jī)運(yùn)行情況進(jìn)行計(jì)數(shù)。根據(jù)不同伺服周期定時(shí)器內(nèi)數(shù)值的變化，可以計(jì)算獲得電機(jī)的實(shí)際位置，由M/T測(cè)速法可求得其運(yùn)行速度。另外兩個(gè)電機(jī)的反饋信號(hào)由EPM7128進(jìn)行處理，DSP與之通訊以獲取電機(jī)運(yùn)行信息。擴(kuò)展4個(gè)32位寄存器用來(lái)存儲(chǔ)電機(jī)當(dāng)前位置，通過(guò)實(shí)時(shí)從計(jì)數(shù)器讀取每個(gè)伺服周期走過(guò)的脈沖數(shù)與之累加實(shí)現(xiàn)。每隔一定的伺服周期，將4個(gè)16位的計(jì)數(shù)器賦初值32000使之重新計(jì)數(shù)。

2.1 CPLD處理電路

CPLD主要包括正交編碼檢測(cè)邏輯電路，DSP地址譯碼電路和計(jì)數(shù)器及中斷邏輯四個(gè)部分，它對(duì)兩個(gè)正交編碼輸入四倍頻后進(jìn)行計(jì)數(shù)，反饋給DSP。正交編碼脈沖電路的方向檢測(cè)邏輯決定了輸入序列中的哪一個(gè)是先導(dǎo)序列，接著就產(chǎn)生方向信號(hào)作為計(jì)數(shù)器的計(jì)數(shù)方向輸入。如果QEP1是先導(dǎo)序列，則所選的定時(shí)器增計(jì)數(shù);如果QEP2是先導(dǎo)序列，則所選的計(jì)數(shù)器減計(jì)數(shù)。正交編碼脈沖電路對(duì)輸入的兩列脈沖的兩個(gè)邊沿都進(jìn)行計(jì)數(shù)，因此，由它所產(chǎn)生的時(shí)鐘頻率是每個(gè)輸入脈沖序列頻率的四倍。中斷邏輯，限位和報(bào)警等信號(hào)產(chǎn)生時(shí)向DSP申請(qǐng)中斷。每個(gè)軸的正負(fù)限位信號(hào)、歸零信號(hào)和報(bào)警信號(hào)，四個(gè)軸共計(jì)16個(gè)信號(hào)，這些信號(hào)相或，只要有一個(gè)信號(hào)產(chǎn)生時(shí)就產(chǎn)生一個(gè)上升沿信號(hào)，此上升沿觸發(fā)DSP中斷，DSP以查詢方式判斷是哪個(gè)信號(hào)產(chǎn)生的中斷。

2.2 控制卡通訊及接口

早期PC機(jī)通過(guò)ISA總線實(shí)現(xiàn)對(duì)電機(jī)的控制，但響應(yīng)速度慢，實(shí)時(shí)性差，后來(lái)的PCI局部總線，解決了ISA的傳輸速度慢等問(wèn)題。近幾年通用串行總線USB(Universal Serial Bus)以其較快的傳輸速率和支持熱插拔等諸多優(yōu)點(diǎn)而受到用戶的廣泛受青睞。得到了許多硬件和軟件廠商的支持。USB 2.0接口的最高傳輸速率由12Mb/s提高到了480Mb/s，能夠更好地支持?jǐn)?shù)據(jù)實(shí)時(shí)傳輸。把USB引入運(yùn)動(dòng)控制器，實(shí)現(xiàn)了運(yùn)動(dòng)控制器的熱插拔，解決了計(jì)算機(jī)接口資源有限等問(wèn)題，滿足了多軸運(yùn)動(dòng)控制卡對(duì)數(shù)據(jù)實(shí)時(shí)通訊的需要。為提高運(yùn)動(dòng)控制器硬件上的開(kāi)放性，開(kāi)發(fā)了通用的接口卡，輔助完成基板與外部的通訊。除了進(jìn)行反饋信號(hào)等的必要傳輸外，還提供了電機(jī)的限位、回零、報(bào)警、伺服使能等信號(hào)和通用I/O各8路。為防止外界信號(hào)干擾，保證控制器的可靠性和安全性，輸入輸出信號(hào)都經(jīng)由TLP521進(jìn)行光電隔離。

3 速度、加速度前饋PID控制算法

本控制器采用典型的三環(huán)調(diào)節(jié)，其中速度調(diào)節(jié)器和電流調(diào)節(jié)器的功能由伺服電機(jī)驅(qū)動(dòng)器完成，電流環(huán)用來(lái)提高系統(tǒng)的動(dòng)態(tài)響應(yīng)指標(biāo)，增強(qiáng)系統(tǒng)抗干擾能力;速度環(huán)用于調(diào)節(jié)伺服電機(jī)的轉(zhuǎn)速。位置閉環(huán)調(diào)節(jié)原理框圖如圖2所示，它包括位置PID調(diào)節(jié)和速度、加速度前饋，由運(yùn)動(dòng)控制器底層程序完成，用于實(shí)現(xiàn)精確定位、回零等，輸出飽和控制可保證輸出電壓不會(huì)超過(guò)設(shè)定范圍。

4 Matlab仿真及結(jié)果分析

基于KLD-200二維數(shù)控平臺(tái)進(jìn)行仿真。平臺(tái)由兩個(gè)Panasonic公司的MSMA012A 1E伺服電機(jī)及配套的MSDA013A1A驅(qū)動(dòng)器進(jìn)行控制。電機(jī)最高轉(zhuǎn)速為3000RPM，功率1 00W，增量式編碼器，2500P/r，絲杠導(dǎo)程為4mm/r。取Kp=100、Ki=12、Kd=2進(jìn)行仿真。Y軸電機(jī)速度曲線與余弦曲線類(lèi)似，跟隨誤差曲線如圖3所示。無(wú)前饋情況下電機(jī)跟隨誤差從開(kāi)始的63個(gè)脈沖在0.137秒后上升到114個(gè)脈沖，隨后作類(lèi)似余弦曲線的變化。引入前饋后，跟隨誤差從開(kāi)始的63個(gè)脈沖迅速上升到109個(gè)脈沖，然后逐漸下降，在大約0.2秒后穩(wěn)定在±2個(gè)脈沖之間?？梢?jiàn)，速度和加速度前饋大大減小了系統(tǒng)的跟隨誤差。利用自己開(kāi)發(fā)的運(yùn)動(dòng)控制器對(duì)數(shù)控平臺(tái)反復(fù)進(jìn)行控制實(shí)驗(yàn)，效果良好。梯形曲線控制實(shí)驗(yàn)中，設(shè)定加速度為10rev/s2，目標(biāo)速度300RPM，位移120mm。到達(dá)目標(biāo)速度后，驅(qū)動(dòng)器顯示的電機(jī)速度波動(dòng)范圍在±2RPM之間。利用VC的OnTimer()函數(shù)實(shí)時(shí)獲取位置信息并進(jìn)行顯示，可以看出，到位后的最大超調(diào)量約為5～10個(gè)脈沖，穩(wěn)態(tài)誤差在±2個(gè)脈沖之內(nèi)，小于1mm。