專利名稱：實(shí)時(shí)運(yùn)動(dòng)控制算法仿真調(diào)試方法
技術(shù)領(lǐng)域：
本發(fā)明涉及一種計(jì)算機(jī)對(duì)運(yùn)動(dòng)部件的控制方法，特別是一種對(duì)運(yùn)動(dòng)控制算法進(jìn)行高效、實(shí)時(shí)仿真調(diào)試的方法。
背景技術(shù)：
現(xiàn)代的高速精密機(jī)電設(shè)備(如半導(dǎo)體生產(chǎn)設(shè)備和數(shù)控機(jī)床等)中普遍采用主-從式計(jì)算機(jī)控制的模式，即由主機(jī)處理用戶輸入和協(xié)調(diào)各方向動(dòng)作而由從機(jī)完成組成各動(dòng)作的基本運(yùn)動(dòng)。從機(jī)一般由數(shù)字信號(hào)處理器(DSP)和外圍輸入輸出接口組成并通過(guò)總線或網(wǎng)絡(luò)與主機(jī)通訊。隨著離散時(shí)間控制(數(shù)字控制)在硬件和軟件兩方面的充分發(fā)展，目前的運(yùn)動(dòng)控制都采用在DSP上運(yùn)行控制算法的數(shù)字方式實(shí)現(xiàn)。從機(jī)的DSP上以中斷服務(wù)例程(ISR)的形式運(yùn)行著運(yùn)動(dòng)控制算法，每個(gè)時(shí)鐘中斷發(fā)生時(shí)，算法依次通過(guò)輸入接口(編碼器接口等)讀取系統(tǒng)當(dāng)前位置，經(jīng)過(guò)計(jì)算后通過(guò)輸出接口(數(shù)模轉(zhuǎn)換器D/A等)更新驅(qū)動(dòng)電機(jī)的電流信號(hào)而改變系統(tǒng)的輸入，同時(shí)接受主機(jī)的運(yùn)動(dòng)命令并將執(zhí)行結(jié)果和自身狀態(tài)反饋給主機(jī)。隨著時(shí)鐘中斷的不斷發(fā)生，運(yùn)動(dòng)控制算法持續(xù)運(yùn)行，算法在時(shí)鐘的精確觸發(fā)下按準(zhǔn)確時(shí)間間隔循環(huán)往復(fù)運(yùn)行是數(shù)字運(yùn)動(dòng)控制算法有效性的根本保證。由于從機(jī)是以嵌入式的方式包含在設(shè)備當(dāng)中，不象主機(jī)那樣具有完備的操作系統(tǒng)和輸入/輸出設(shè)備，無(wú)法方便高效地監(jiān)測(cè)從機(jī)上運(yùn)行程序的實(shí)時(shí)狀態(tài)。另外，現(xiàn)代運(yùn)動(dòng)運(yùn)動(dòng)控制算法呈現(xiàn)出代碼龐大、參數(shù)眾多、關(guān)系繁雜的顯著趨勢(shì)，如何進(jìn)行高效的調(diào)試以確保最短時(shí)間內(nèi)獲得運(yùn)行正確的代碼是一個(gè)極大的挑戰(zhàn)。一種既能準(zhǔn)確反映運(yùn)動(dòng)控制代碼實(shí)時(shí)運(yùn)行狀況又可以方便高效地對(duì)代碼進(jìn)行各種常規(guī)調(diào)試的方法成為必需。
現(xiàn)有的仿真調(diào)試工具都無(wú)法全面滿足前面提出的實(shí)時(shí)運(yùn)動(dòng)控制算法代碼調(diào)試的需要。Matlab和Simulink等軟件可以方便地建立控制對(duì)象的動(dòng)態(tài)模型并與所用控制算法相連，進(jìn)行仿真分析，但因?yàn)橄到y(tǒng)模型中的控制算法與DSP上運(yùn)行的算法代碼沒(méi)有直接聯(lián)系，結(jié)果并不反映實(shí)時(shí)運(yùn)動(dòng)控制代碼執(zhí)行的真實(shí)結(jié)果；而且這類仿真工具也無(wú)法有效體現(xiàn)運(yùn)動(dòng)控制代碼中狀態(tài)監(jiān)測(cè)和邏輯流控制的部分。針對(duì)DSP的仿真調(diào)試環(huán)境，雖然可以對(duì)DSP上的代碼進(jìn)行單步運(yùn)行等操作，但因?yàn)楦咚賹?shí)時(shí)運(yùn)行(每秒數(shù)千次)條件下執(zhí)行元件(電機(jī)和運(yùn)動(dòng)部件)的真實(shí)響應(yīng)無(wú)法隨代碼的運(yùn)行定格在某一時(shí)刻而不具備真實(shí)運(yùn)行條件下的調(diào)試能力；同時(shí)也缺乏必要的與DSP運(yùn)行相協(xié)調(diào)的和主機(jī)間的數(shù)據(jù)交換機(jī)制。目前出現(xiàn)的硬件在回路(Hardware InLoop)仿真通過(guò)將模擬控制對(duì)象(執(zhí)行元件)實(shí)時(shí)響應(yīng)的可編程硬件仿真器接入控制回路的方法，一定程度上實(shí)現(xiàn)了控制算法脫離真實(shí)控制對(duì)象的實(shí)時(shí)仿真調(diào)試，但仍存在著硬件仿真器成本高昂、編程和設(shè)置復(fù)雜的問(wèn)題，也無(wú)法達(dá)到完全自由靈活的代碼實(shí)時(shí)單步、斷點(diǎn)調(diào)試。主機(jī)代碼的開(kāi)發(fā)環(huán)境具有完善的調(diào)試機(jī)制，但無(wú)法和DSP的實(shí)時(shí)代碼協(xié)調(diào)。

發(fā)明內(nèi)容
本發(fā)明的目的是提供一種實(shí)時(shí)運(yùn)動(dòng)控制算法仿真調(diào)試方法，針對(duì)實(shí)時(shí)運(yùn)動(dòng)控制算法調(diào)試的特殊需要，解決完整不變的從機(jī)DSP運(yùn)動(dòng)控制代碼和主機(jī)程序在同一計(jì)算機(jī)處理器上以真實(shí)的實(shí)時(shí)狀態(tài)運(yùn)行的問(wèn)題；還解決目前代碼調(diào)試工具并不能實(shí)現(xiàn)實(shí)時(shí)運(yùn)動(dòng)控制代碼執(zhí)行真實(shí)結(jié)果的問(wèn)題，并解決控制算法與真實(shí)控制對(duì)象的實(shí)時(shí)仿真調(diào)試無(wú)法和DSP的實(shí)時(shí)代碼協(xié)調(diào)的問(wèn)題；同時(shí)解決利用現(xiàn)有的主機(jī)代碼開(kāi)發(fā)環(huán)境，對(duì)兩部分程序進(jìn)行簡(jiǎn)便、準(zhǔn)確、有效的單步、斷點(diǎn)等調(diào)試的問(wèn)題。
本發(fā)明的技術(shù)方案該方法把從機(jī)DSP上運(yùn)行的運(yùn)動(dòng)控制代碼完整不變地轉(zhuǎn)移到主機(jī)上運(yùn)行并采用數(shù)值仿真的方法來(lái)準(zhǔn)確模擬控制對(duì)象的真實(shí)響應(yīng)；首先，把從機(jī)DSP上定義的用于與主機(jī)進(jìn)行數(shù)據(jù)交換的內(nèi)存區(qū)間轉(zhuǎn)移定義到主機(jī)的內(nèi)存映射中，并將相應(yīng)的主機(jī)對(duì)DSP內(nèi)存區(qū)間讀寫的函數(shù)修改為新的函數(shù)，新的函數(shù)對(duì)新定義的主機(jī)內(nèi)存映射進(jìn)行操作；其次，將運(yùn)動(dòng)控制代碼中通過(guò)輸出接口更新驅(qū)動(dòng)電機(jī)電流信號(hào)的函數(shù)修改為根據(jù)代碼計(jì)算的輸出量用數(shù)值積分方法來(lái)計(jì)算控制對(duì)象的響應(yīng)，并存儲(chǔ)在相應(yīng)的內(nèi)存單元中；同時(shí)，把代碼中通過(guò)輸入接口讀取系統(tǒng)當(dāng)前位置的函數(shù)修改為新的函數(shù)，新的函數(shù)從存儲(chǔ)控制對(duì)象響應(yīng)的數(shù)值計(jì)算結(jié)果的內(nèi)存單元中讀取系統(tǒng)當(dāng)前位置；最后，在主機(jī)中循環(huán)檢測(cè)從機(jī)狀態(tài)，等待從機(jī)完成運(yùn)動(dòng)命令的函數(shù)中加入對(duì)從機(jī)DSP代碼ISR的調(diào)用，以達(dá)到運(yùn)動(dòng)控制算法的反復(fù)運(yùn)行。
本發(fā)明通過(guò)以上措施實(shí)現(xiàn)了完整不變的從機(jī)DSP運(yùn)動(dòng)控制代碼和主機(jī)程序在同一計(jì)算機(jī)處理器上以真實(shí)的實(shí)時(shí)狀態(tài)運(yùn)行，可以利用現(xiàn)有的主機(jī)代碼開(kāi)發(fā)環(huán)境對(duì)兩部分程序進(jìn)行方便有效的單步、斷點(diǎn)等調(diào)試。該方法適用于各種獨(dú)立運(yùn)行于嵌入式處理器的運(yùn)動(dòng)控制算法的仿真調(diào)試。


圖1是一個(gè)典型運(yùn)動(dòng)控制系統(tǒng)的硬件組成及代碼運(yùn)行關(guān)系示意圖；圖2是針對(duì)圖1中系統(tǒng)的運(yùn)動(dòng)控制算法實(shí)時(shí)仿真調(diào)試方法示意圖。
圖3是本發(fā)明被應(yīng)用在一個(gè)半導(dǎo)體生產(chǎn)設(shè)備工作臺(tái)的示意圖。
圖4是對(duì)圖3的設(shè)備在X方向典型的18毫秒內(nèi)完成2.54mm的運(yùn)動(dòng)的仿真結(jié)果示意圖。
具體實(shí)施例方式
如圖1所示，一個(gè)典型的運(yùn)動(dòng)控制系統(tǒng)由主機(jī)、從機(jī)和控制對(duì)象組成。主機(jī)CPU中運(yùn)行的主程序調(diào)用DSP內(nèi)存寫函數(shù)DSPWrite()向從機(jī)DSP的數(shù)據(jù)交換內(nèi)存寫入運(yùn)動(dòng)命令，之后運(yùn)行運(yùn)動(dòng)完成檢測(cè)函數(shù)MotionDone()來(lái)檢測(cè)從機(jī)是否完成所寫入的運(yùn)動(dòng)命令。MotionDone()調(diào)用函數(shù)DSP內(nèi)存讀函數(shù)DSPRead()自從機(jī)DSP的數(shù)據(jù)交換內(nèi)存中讀出從機(jī)狀態(tài)，判斷運(yùn)動(dòng)命令是否完成，MotionDone()循環(huán)運(yùn)行直至運(yùn)動(dòng)完成為止。從機(jī)DSP中的中斷服務(wù)例程ISR首先調(diào)用編碼器讀函數(shù)ReadEnc()通過(guò)編碼器接口讀取控制對(duì)象運(yùn)動(dòng)部件的當(dāng)前位置，其后經(jīng)過(guò)運(yùn)動(dòng)控制算法的計(jì)算得出新的電流控制輸出量，然后運(yùn)行D/A寫函數(shù)WriteDA()通過(guò)D/A將這一電流信號(hào)傳遞給控制對(duì)象的電機(jī)。ISR運(yùn)行的過(guò)程中通過(guò)DSP的數(shù)據(jù)交換內(nèi)存接受主機(jī)CPU的運(yùn)動(dòng)命令并將包含命令執(zhí)行結(jié)果的自身狀態(tài)反饋給主機(jī)CPU。ISR在時(shí)鐘中斷的精確觸發(fā)下，每隔一個(gè)中斷周期TItr運(yùn)行一次，一直循環(huán)持續(xù)下去?？刂茖?duì)象的電機(jī)以從機(jī)DSP規(guī)定的電流驅(qū)動(dòng)運(yùn)動(dòng)部件到達(dá)當(dāng)前位置。
圖2給出了針對(duì)圖1中系統(tǒng)的運(yùn)動(dòng)控制算法實(shí)時(shí)仿真調(diào)試方案。在原來(lái)主機(jī)CPU中運(yùn)行的主程序基礎(chǔ)之上，把從機(jī)DSP上運(yùn)行的ISR代碼轉(zhuǎn)移到主機(jī)CPU上運(yùn)行，同時(shí)將DSP中的數(shù)據(jù)交換內(nèi)存也轉(zhuǎn)移定義到主機(jī)的內(nèi)存映射中。將主程序中的函數(shù)DSPWrite()和DSPRead()分別修改為對(duì)定義在主機(jī)上的數(shù)據(jù)交換內(nèi)存進(jìn)行寫入和讀出。在主程序的函數(shù)MotionDone()中加入對(duì)ISR()的調(diào)用，以使ISR()隨函數(shù)MotionDone()反復(fù)循環(huán)運(yùn)行直至運(yùn)動(dòng)完成為止。將ISR()中的函數(shù)WriteDA()修改為調(diào)用運(yùn)動(dòng)仿真函數(shù)MotionSim()。函數(shù)MotionSim()根據(jù)運(yùn)動(dòng)控制算法計(jì)算的電流控制輸出量用四階龍格-庫(kù)塔法(Runge-Kutta)計(jì)算控制對(duì)象的響應(yīng)并存儲(chǔ)在位置內(nèi)存中。把ISR()中的函數(shù)ReadEnc()修改為從位置內(nèi)存中讀取系統(tǒng)當(dāng)前位置。ISR()中ReadEnc()與WriteDA()之間的代碼是運(yùn)動(dòng)控制算法的主體，在從DSP到主機(jī)的轉(zhuǎn)移中保持完整不變。仿真調(diào)試方案中內(nèi)存定義和函數(shù)的少量修改可以通過(guò)條件編譯命令實(shí)現(xiàn)，達(dá)到同一源文件用于DSP可執(zhí)行文件編譯和運(yùn)動(dòng)控制算法實(shí)時(shí)仿真調(diào)試。方案中位置內(nèi)存里儲(chǔ)存的控制對(duì)象位置及其導(dǎo)出量(速度、加速度等)可以與各種數(shù)據(jù)可視化手段相結(jié)合，形成完整、直觀、高效的，完全脫離從機(jī)和控制對(duì)象的運(yùn)動(dòng)控制算法虛擬實(shí)時(shí)仿真調(diào)試環(huán)境。
方案中用于計(jì)算控制對(duì)象響應(yīng)的四階龍格-庫(kù)塔法是最常用的準(zhǔn)確數(shù)值積分方法，適用于任何可以用狀態(tài)空間中表達(dá)(即動(dòng)態(tài)特性可以寫成一個(gè)線性一階常微分方程組)的系統(tǒng)。一個(gè)運(yùn)動(dòng)控制系統(tǒng)中典型的單自由度質(zhì)量-彈簧-阻尼運(yùn)動(dòng)部件的動(dòng)力學(xué)方程可以寫為md2xd2t+cdxdt+kx=F---(1)]]>其中x為運(yùn)動(dòng)部件的位置，t為時(shí)間，m為運(yùn)動(dòng)部件的質(zhì)量，c為阻尼系數(shù)，k為彈簧剛度，F(xiàn)為作用于運(yùn)動(dòng)部件的電機(jī)驅(qū)動(dòng)力。方程(1)可以重寫為d2xd2t=Fmcmdxdtkmx---(2)]]>電機(jī)線圈內(nèi)的電流和電機(jī)的驅(qū)動(dòng)力之間存在以下關(guān)系F＝KfI (3)其中F為電機(jī)驅(qū)動(dòng)力，I為運(yùn)動(dòng)控制算法計(jì)算得出的電流控制輸出量，Kf為電機(jī)的力敏感度。將方程(3)代入方程(2)可以得到d2xd2t=KfImcmdxdtkmx---(4)]]>定義變量矢量如下y1y2=xdxdt---(5)]]>定義相應(yīng)的變量導(dǎo)數(shù)矢量如下[dy1ddy2]dt=[dxdtd2xd2t]---(6)]]>將方程(4)代入方程(6)可以得到
方程(5)和方程(7)給出了單自由度二階線性動(dòng)態(tài)系統(tǒng)的狀態(tài)空間表達(dá)。根據(jù)四階龍格-庫(kù)塔法的定義，控制對(duì)象的響應(yīng)通過(guò)以下數(shù)值積分方法計(jì)算b1i＝hdydxi(y1，n，y2，n)b2i=hdydxi(y1,n+b112,y2,n+b122)]]>b3i=hdydxi(y1,n+b212,y2,n+b222)]]>b4i＝hdydxi(y1，n+b31，y2，n+b32)(8)yi,n+1=yi,n+b1i6+b2i3+b3i3+b4i6]]>其中i＝1，2，h為積分的時(shí)間步長(zhǎng)，在ISR中斷周期TItr足夠小的情況下可以把h設(shè)為TItr，y1，n和y2，n分別為方程(5)中變量矢量?jī)蓚€(gè)分量在當(dāng)前時(shí)刻的已知值，yi，n+1為相應(yīng)變量矢量分量在與當(dāng)前時(shí)刻相隔時(shí)間h的下一時(shí)刻的計(jì)算值。
實(shí)施例下面以主機(jī)和從機(jī)代碼中關(guān)鍵函數(shù)的修改來(lái)說(shuō)明該實(shí)時(shí)仿真調(diào)試方法的具體實(shí)施步驟。代碼都采用類C語(yǔ)言的偽代碼的方式表示，與方法實(shí)施無(wú)關(guān)而保持不變的代碼部分用省略號(hào)“…”代表。
實(shí)施方法時(shí)，首先在頭文件中定義兩個(gè)編譯變量#define DSPConfig#define SimulationConfigDSPConfig代表普通DSP可執(zhí)行文件編譯的配置，SimulationConfig代表實(shí)時(shí)仿真調(diào)試的配置，使用時(shí)根據(jù)需要將其中一行注釋掉而保留另一行。
主機(jī)的主程序如下所示Main(){DSPWrite(CommandAddress，MotionCommand)；//向DSP的數(shù)據(jù)交換內(nèi)存寫入運(yùn)動(dòng)命令
MotionDone()；//檢測(cè)從機(jī)是否完成所寫入的運(yùn)動(dòng)命令}其中CommandAddress為從機(jī)DSP數(shù)據(jù)交換內(nèi)存中運(yùn)動(dòng)命令的地址，MotionCommand為運(yùn)動(dòng)命令。
DSP內(nèi)存寫函數(shù)DSPWrite()如下所示DSPWrite(int Address，int Content){*(int*)DSPWriteAddressPort＝Address；//地址寫入寄存器*(int*)DSPWriteContentPort＝Content；//內(nèi)容寫入寄存器}其中Address為寫入地址，Content為寫入內(nèi)容，DSPWriteAddressPort為DSP內(nèi)存寫入地址寄存器的端口地址，DSPWriteContentPort為DSP內(nèi)存寫入內(nèi)容寄存器的端口地址。方法實(shí)施后，DSPWrite()修改為DSPWrite(int Address，int Content){#ifdef DSP_config*(int*)DSPWriteAddressPort＝Address；//地址寫入寄存器*(int*)DSPWriteContentPort＝Content；//內(nèi)容寫入寄存器#endif#ifdef simulation_config*(int*)Address＝Content；//內(nèi)容直接寫入地址指向的主機(jī)內(nèi)存單元#endif}運(yùn)動(dòng)完成檢測(cè)函數(shù)MotionDone()如下所示MotionDone(){ int Doneflag＝0；//將運(yùn)動(dòng)完成標(biāo)志初始化為假while(！Doneflag){DSPRead(StatusAddress，&Doneflag)；//從DSP數(shù)據(jù)交換內(nèi)存讀出從機(jī)狀態(tài)}
}其中StatusAddress為從機(jī)DSP數(shù)據(jù)交換內(nèi)存中狀態(tài)的地址，Doneflag為運(yùn)動(dòng)完成標(biāo)志。方法實(shí)施后，MotionDone()修改為MotionDone(){ int Doneflag＝0；//將運(yùn)動(dòng)完成標(biāo)志初始化為假while(！Doneflag){#ifdef simulation_configISR()；//調(diào)用中斷服務(wù)例程#endifDSPRead(StatusAddress，&Doneflag)；//從DSP數(shù)據(jù)交換內(nèi)存讀出從機(jī)狀態(tài)}}DSP內(nèi)存讀函數(shù)DSPRead()如下所示DSPRead(int Address，int*Destination){*(int*)DSPReadAddressPort＝Address；//地址寫入寄存器*Destination＝*(int*)DSPReadContentPort；//寄存器內(nèi)容讀入目的地址}其中Address為寫入地址，Destination為目的地址，DSPReadAddressPort為DSP內(nèi)存讀出地址寄存器的端口地址，DSPReadContentPort為DSP內(nèi)存讀出內(nèi)容寄存器的端口地址。方法實(shí)施后，DSPRead()修改為DSPRead(int Address，int*Destination){#ifdef DSP_config*(int*)DSPReadAddressPort＝Address；//地址寫入寄存器*Destination＝*(int*)DSPReadContentPort；//寄存器內(nèi)容讀入目的地址#endif#ifdef simulation_config*Destination＝*(int*)Address；//直接將地址指向的主機(jī)內(nèi)存單元的內(nèi)容讀入目的地址
#endif}中斷服務(wù)例程ISR()如下所示ISR(){ float x；x＝ReadEnc()；//通過(guò)編碼器接口讀取控制對(duì)象運(yùn)動(dòng)部件的當(dāng)前位置？運(yùn)動(dòng)控制算法)WriteDA(I)；//通過(guò)D/A將電流信號(hào)傳遞給控制對(duì)象的電機(jī)}其中x為控制對(duì)象運(yùn)動(dòng)部件的當(dāng)前位置，I為運(yùn)動(dòng)控制算法計(jì)算得出的電流控制輸出量，ReadEnc()與WriteDA()之間的代碼是運(yùn)動(dòng)控制算法。
編碼器讀函數(shù)ReadEnc()如下所示float ReadEnc(){ float EncValue；EncValue＝*(float*)EncValuePort；//讀取編碼器讀數(shù)return EncValue；//返回編碼器讀數(shù)}其中EncValuePort為編碼器計(jì)數(shù)值端口地址。方法實(shí)施后，ReadEnc()修改為float ReadEnc(){ float EncValue；#ifdef DSP_configEncValue＝*(float*)EncValuePort；//讀取編碼器讀數(shù)return EncValue；//返回編碼器讀數(shù)#endif#ifdef simulation_configreturn xSim；//直接返回仿真計(jì)算的當(dāng)前位置#endif}
其中xSim為存儲(chǔ)仿真計(jì)算的當(dāng)前位置的全局變量，將會(huì)在后面關(guān)于運(yùn)動(dòng)仿真函數(shù)MotionSim()的描述中詳細(xì)解釋。
D/A寫函數(shù)WriteDA()如下所示W(wǎng)riteDA(float I){*(float*)DAValuePort＝I；//寫入DA值寄存器}其中DAValuePort為DA值寄存器端口地址。方法實(shí)施后，WriteDA()修改為WriteDA(float I){#ifdef DSP_config*(float*)DAValuePort＝I；//寫入DA值寄存器#endif#ifdef simulation_configMotionSim(I)；//調(diào)用運(yùn)動(dòng)仿真函數(shù)計(jì)算當(dāng)前位置#endif}在定義MotionSim()前，首先定義兩個(gè)全局變量float xSim，velSim；其中xSim為仿真計(jì)算的當(dāng)前位置，velSim為仿真計(jì)算的當(dāng)前速度。通常在仿真起始時(shí)xSim和velSim都初始化為0。
根據(jù)方程(5)和方程(7)定義由矢量[y1y2]和電流I計(jì)算導(dǎo)數(shù)矢量[dydt1dydt2]的函數(shù)derivative()如下derivative(float*y，I，float*dydt){dydt
＝y(tǒng)[1]；dydt[1]＝(Kf*I c*y[1]-k*y
)/m；}其中I，m，Kf，c，k的定義均與方程(7)中相同。
根據(jù)方程(8)定義運(yùn)動(dòng)仿真函數(shù)MotionSim()如下
MotionSim(float I){ int i；float y[2]，yt[2]，dydt[2]；float h；float b1[2]，b2[2]，b3[2]，b4[2]；h＝TItr；y
＝xSim；y[1]＝velSim；derivative(y，I，dydt)；for(i＝0；i＜2；i++)b1[i]＝h*dydt[i]；for(i＝0；i＜2；i++)yt[i]＝y(tǒng)[i]+0.5*b1[i]；derivative(yt，I，dydt)；for(i＝0；i＜2；i++)b2[i]＝h*dydt[i]；for(i＝0；i＜2；i++)yt[i]＝y(tǒng)[i]+0.5*b2[i]；derivative(yt，I，dydt)；for(i＝0；i＜2；i++)b3[i]＝h*dydt[i]；for(i＝0；i＜2；i++)yt[i]＝y(tǒng)[i]+b3[i]；derivative(yt，I，dydt)；for(i＝0；i＜2；i++)b4[i]＝h*dydt[i]；for(i＝0；i＜2；i++)y[i]＝y(tǒng)[i]+b1[i]/6+b2[i]/3+b3[i]/3+b4[i]/6；xSim＝y(tǒng)
；velSim＝y(tǒng)[1]；
}MotionSim()計(jì)算并最終更新當(dāng)前仿真位置xSim和當(dāng)前仿真速度velSim。
實(shí)施例參見(jiàn)圖3實(shí)時(shí)仿真調(diào)試方法被應(yīng)用在一個(gè)半導(dǎo)體生產(chǎn)設(shè)備的工作臺(tái)上。圖3給出了工作臺(tái)的結(jié)構(gòu)，它由沿相互垂直的方向運(yùn)動(dòng)的X滑塊1、Y滑塊2和各自的驅(qū)動(dòng)電機(jī)X電機(jī)3和Y電機(jī)4組成。對(duì)于工作臺(tái)X方向的運(yùn)動(dòng)，X滑塊是運(yùn)動(dòng)控制系統(tǒng)中控制對(duì)象的運(yùn)動(dòng)部件，它的特征值如下所示m＝5kg，c＝0，k＝0。
X電機(jī)的特征值為Kf＝16.31N/A；按前面的說(shuō)明對(duì)主機(jī)和DSP代碼進(jìn)行相應(yīng)的修改，并將上列系統(tǒng)常數(shù)代入方程(7)，就可以對(duì)運(yùn)動(dòng)控制系統(tǒng)在主機(jī)上進(jìn)行完全脫離DSP和控制對(duì)象(X滑塊和X電機(jī))的真實(shí)反映運(yùn)動(dòng)控制算法實(shí)時(shí)運(yùn)行狀況的仿真和調(diào)試。
參見(jiàn)圖4，該圖表示了對(duì)設(shè)備在X方向典型的18毫秒內(nèi)完成2.54mm的運(yùn)動(dòng)的仿真結(jié)果。圖中顯示了仿真計(jì)算得到的運(yùn)動(dòng)過(guò)程中的X滑塊的理想和實(shí)際位置以及兩者之間的差值-跟隨誤差，X電機(jī)電流。各信號(hào)的顯示比例是理想/實(shí)際位置1mm/格，跟隨誤差1mm/格，電機(jī)電流5A/格。實(shí)時(shí)仿真調(diào)試方法可以以同樣的方式應(yīng)用在工作臺(tái)Y方向的運(yùn)動(dòng)上。
權(quán)利要求
1.一種實(shí)時(shí)運(yùn)動(dòng)控制算法仿真調(diào)試方法，包括主機(jī)、從機(jī)和控制對(duì)象，其特征在于該方法把從機(jī)DSP上運(yùn)行的運(yùn)動(dòng)控制代碼完整不變地轉(zhuǎn)移到主機(jī)上運(yùn)行，并采用數(shù)值仿真的方法來(lái)準(zhǔn)確模擬控制對(duì)象的真實(shí)響應(yīng)；首先，把從機(jī)DSP上定義的用于與主機(jī)進(jìn)行數(shù)據(jù)交換的內(nèi)存區(qū)間轉(zhuǎn)移定義到主機(jī)的內(nèi)存映射中，并將相應(yīng)的主機(jī)對(duì)DSP內(nèi)存區(qū)間讀寫的函數(shù)修改為新的函數(shù)，新的函數(shù)對(duì)新定義的主機(jī)內(nèi)存映射進(jìn)行操作；其次，將運(yùn)動(dòng)控制代碼中通過(guò)輸出接口更新驅(qū)動(dòng)電機(jī)電流信號(hào)的函數(shù)修改為根據(jù)代碼計(jì)算的輸出量用數(shù)值積分方法來(lái)計(jì)算控制對(duì)象的響應(yīng)，并存儲(chǔ)在相應(yīng)的內(nèi)存單元中；同時(shí)，把代碼中通過(guò)輸入接口讀取系統(tǒng)當(dāng)前位置的函數(shù)修改為新的函數(shù)，新的函數(shù)從存儲(chǔ)控制對(duì)象響應(yīng)的數(shù)值計(jì)算結(jié)果的內(nèi)存單元中讀取系統(tǒng)當(dāng)前位置；最后，在主機(jī)中循環(huán)檢測(cè)從機(jī)狀態(tài)，等待從機(jī)完成運(yùn)動(dòng)命令的函數(shù)中加入對(duì)從機(jī)DSP代碼ISR的調(diào)用，以達(dá)到運(yùn)動(dòng)控制算法的反復(fù)運(yùn)行。
2.一種實(shí)時(shí)運(yùn)動(dòng)控制算法仿真調(diào)試方法，包括主機(jī)、從機(jī)和控制對(duì)象，其特征在于主機(jī)CPU中運(yùn)行的主程序調(diào)用DSP內(nèi)存寫函數(shù)DSPWrite()，向從機(jī)DSP的數(shù)據(jù)交換內(nèi)存寫入運(yùn)動(dòng)命令，之后，運(yùn)行運(yùn)動(dòng)完成檢測(cè)函數(shù)MotionDone()，來(lái)檢測(cè)從機(jī)是否完成所寫入的運(yùn)動(dòng)命令；MotionDone()調(diào)用函數(shù)DSP內(nèi)存讀函數(shù)DSPRead()自從機(jī)DSP的數(shù)據(jù)交換內(nèi)存中讀出從機(jī)狀態(tài)，判斷運(yùn)動(dòng)命令是否完成，MotionDone()循環(huán)運(yùn)行直至運(yùn)動(dòng)完成為止；從機(jī)DSP中的中斷服務(wù)例程ISR首先調(diào)用編碼器讀函數(shù)ReadEnc()通過(guò)編碼器接口讀取控制對(duì)象運(yùn)動(dòng)部件的當(dāng)前位置，其后經(jīng)過(guò)運(yùn)動(dòng)控制算法的計(jì)算得出新的電流控制輸出量，然后運(yùn)行D/A寫函數(shù)WriteDA()通過(guò)D/A將這一電流信號(hào)傳遞給控制對(duì)象的電機(jī)；ISR運(yùn)行的過(guò)程中通過(guò)DSP的數(shù)據(jù)交換內(nèi)存接受主機(jī)CPU的運(yùn)動(dòng)命令并將包含命令執(zhí)行結(jié)果的自身狀態(tài)反饋給主機(jī)CPU；ISR在時(shí)鐘中斷的精確觸發(fā)下，每隔一個(gè)中斷周期TItr運(yùn)行一次，一直循環(huán)持續(xù)下去；控制對(duì)象的電機(jī)以從機(jī)DSP規(guī)定的電流驅(qū)動(dòng)運(yùn)動(dòng)部件到達(dá)當(dāng)前位置。
3.一種實(shí)時(shí)運(yùn)動(dòng)控制算法仿真調(diào)試方法，包括主機(jī)、從機(jī)和控制對(duì)象，其特征在于在主機(jī)CPU中運(yùn)行的主程序基礎(chǔ)之上，把從機(jī)DSP上運(yùn)行的ISR代碼轉(zhuǎn)移到主機(jī)CPU上運(yùn)行，同時(shí)將DSP中的數(shù)據(jù)交換內(nèi)存也轉(zhuǎn)移定義到主機(jī)的內(nèi)存映射中；將主程序中的函數(shù)DSPWrite()和DSPRead()分別修改為對(duì)定義在主機(jī)上的數(shù)據(jù)交換內(nèi)存進(jìn)行寫入和讀出；在主程序的函數(shù)MotionDone()中加入對(duì)ISR()的調(diào)用，以使ISR()隨函數(shù)MotionDone()反復(fù)循環(huán)運(yùn)行直至運(yùn)動(dòng)完成為止；將ISR()中的函數(shù)WriteDA()修改為調(diào)用運(yùn)動(dòng)仿真函數(shù)MotionSim()；函數(shù)MotionSim()根據(jù)運(yùn)動(dòng)控制算法計(jì)算的電流控制輸出量用四階龍格—庫(kù)塔法計(jì)算控制對(duì)象的響應(yīng)并存儲(chǔ)在位置內(nèi)存中；把ISR()中的函數(shù)ReadEnc()修改為從位置內(nèi)存中讀取系統(tǒng)當(dāng)前位置；ISR()中ReadEnc()與WriteDA()之間的代碼是運(yùn)動(dòng)控制算法的主體，在從DSP到主機(jī)的轉(zhuǎn)移中保持完整不變；仿真調(diào)試方案中內(nèi)存定義和函數(shù)的少量修改通過(guò)條件編譯命令實(shí)現(xiàn)，達(dá)到同一源文件用于DSP可執(zhí)行文件編譯和運(yùn)動(dòng)控制算法實(shí)時(shí)仿真調(diào)試；方案中位置內(nèi)存里儲(chǔ)存的控制對(duì)象位置及其導(dǎo)出量與各種數(shù)據(jù)可視化手段相結(jié)合，形成完全脫離從機(jī)和控制對(duì)象的運(yùn)動(dòng)控制算法虛擬實(shí)時(shí)仿真調(diào)試環(huán)境。
4.根據(jù)權(quán)利要求3所述的實(shí)時(shí)運(yùn)動(dòng)控制算法仿真調(diào)試方法，其特征在于上述用于計(jì)算控制對(duì)象響應(yīng)的四階龍格—庫(kù)塔法是準(zhǔn)確數(shù)值積分方法，適用于任何可以用狀態(tài)空間中表達(dá)，即動(dòng)態(tài)特性可以寫成一個(gè)線性一階常微分方程組的系統(tǒng)；一個(gè)運(yùn)動(dòng)控制系統(tǒng)中典型的單自由度質(zhì)量—彈簧—阻尼運(yùn)動(dòng)部件的動(dòng)力學(xué)方程可以寫為md2xd2t+cdxdt+kx=F----(1)]]>其中x為運(yùn)動(dòng)部件的位置，t為時(shí)間，m為運(yùn)動(dòng)部件的質(zhì)量，c為阻尼系數(shù)，k為彈簧剛度，F(xiàn)為作用于運(yùn)動(dòng)部件的電機(jī)驅(qū)動(dòng)力；方程(1)可以重寫為d2xd2t=Fmcmdxdtkmx----(2)]]>電機(jī)線圈內(nèi)的電流和電機(jī)的驅(qū)動(dòng)力之間存在以下關(guān)系F＝KfI(3)其中F為電機(jī)驅(qū)動(dòng)力，I為運(yùn)動(dòng)控制算法計(jì)算得出的電流控制輸出量，Kf為電機(jī)的力敏感度；將方程(3)代入方程(2)可以得到d2xd2t=KfImcmdxdtkmx----(4)]]>定義變量矢量如下y1y2=xdxdt----(5)]]>定義相應(yīng)的變量導(dǎo)數(shù)矢量如下 將方程(4)代入方程(6)可以得到 (7)方程(5)和方程(7)給出了單自由度二階線性動(dòng)態(tài)系統(tǒng)的狀態(tài)空間表達(dá)；根據(jù)四階龍格—庫(kù)塔法的定義，控制對(duì)象的響應(yīng)通過(guò)以下數(shù)值積分方法計(jì)算b1i＝hdydxi(y1，n，y2，n)b2i=hdydxi(y1,n+b112,y2,n+b122)]]>b3i=hdydxi(y1,n+b212,y2,n+b222)]]>b4i＝hdydxi(y1，n+b31，y2，n+b32)yi,n+1=yi,n+b1i6+b2i3+b3i3+b4i6]]>(8)其中i＝1，2，h為積分的時(shí)間步長(zhǎng)，在ISR中斷周期TItr足夠小的情況下可以把h設(shè)為TItr，y1，n和y2，n分別為方程(5)中變量矢量?jī)蓚€(gè)分量在當(dāng)前時(shí)刻的已知值，yi，n+1為相應(yīng)變量矢量分量在與當(dāng)前時(shí)刻相隔時(shí)間h的下一時(shí)刻的計(jì)算值。
5.根據(jù)權(quán)利要求3所述的實(shí)時(shí)運(yùn)動(dòng)控制算法仿真調(diào)試方法，其特征在于上述導(dǎo)出量是速度或加速度。
6.一種根據(jù)權(quán)利要求1-5任一項(xiàng)所述實(shí)時(shí)運(yùn)動(dòng)控制算法仿真調(diào)試方法的應(yīng)用方法，其特征在于上述實(shí)時(shí)仿真調(diào)試方法被應(yīng)用在一個(gè)半導(dǎo)體生產(chǎn)設(shè)備的工作臺(tái)上，工作臺(tái)的由沿相互垂直的方向運(yùn)動(dòng)的X滑塊、Y滑塊和各自的驅(qū)動(dòng)電機(jī)X電機(jī)和Y電機(jī)組成，對(duì)于工作臺(tái)X方向的運(yùn)動(dòng)，X滑塊是運(yùn)動(dòng)控制系統(tǒng)中控制對(duì)象的運(yùn)動(dòng)部件，它的特征值如下所示m＝5kg，c＝0，k＝0；X電機(jī)的特征值為Kf＝16.31N/A；對(duì)主機(jī)和DSP代碼進(jìn)行相應(yīng)的修改，并將上列系統(tǒng)常數(shù)代入方程(7)，就能對(duì)運(yùn)動(dòng)控制系統(tǒng)在主機(jī)上進(jìn)行完全脫離DSP和控制對(duì)象，即X滑塊和X電機(jī)，的真實(shí)反映運(yùn)動(dòng)控制算法實(shí)時(shí)運(yùn)行狀況的仿真和調(diào)試。
全文摘要
一種實(shí)時(shí)運(yùn)動(dòng)控制算法仿真調(diào)試方法，把從機(jī)上運(yùn)行的運(yùn)動(dòng)控制代碼完整不變地轉(zhuǎn)移到主機(jī)上運(yùn)行，并采用數(shù)值仿真的方法來(lái)模擬控制對(duì)象的真實(shí)響應(yīng)；先把從機(jī)定義的內(nèi)存區(qū)間轉(zhuǎn)移定義到主機(jī)的內(nèi)存映射中，并將主機(jī)對(duì)DSP內(nèi)存區(qū)間讀寫的函數(shù)修改為對(duì)新定義的主機(jī)內(nèi)存映射進(jìn)行操作；然后將運(yùn)動(dòng)控制代碼中通過(guò)輸出接口更新驅(qū)動(dòng)電機(jī)電流信號(hào)的函數(shù)修改為根據(jù)代碼計(jì)算的輸出量用數(shù)值積分方法計(jì)算控制對(duì)象的響應(yīng)，并存儲(chǔ)在內(nèi)存單元中；同時(shí)把代碼中通過(guò)輸入接口讀取系統(tǒng)當(dāng)前位置的函數(shù)修改為從存儲(chǔ)控制對(duì)象響應(yīng)的數(shù)值計(jì)算結(jié)果的內(nèi)存單元中讀取系統(tǒng)當(dāng)前位置；最后由主機(jī)循環(huán)檢測(cè)從機(jī)狀態(tài)，等待從機(jī)完成運(yùn)動(dòng)命令的函數(shù)中加入對(duì)從機(jī)DSP代碼ISR的調(diào)用。
文檔編號(hào)G06F9/00GK1770046SQ200510200639
公開(kāi)日2006年5月10日 申請(qǐng)日期2005年10月24日 優(yōu)先權(quán)日2005年10月24日
發(fā)明者何田 申請(qǐng)人:中國(guó)電子科技集團(tuán)公司第四十五研究所