專利名稱：一種fsm遠(yuǎn)程實(shí)時(shí)控制時(shí)間補(bǔ)償系統(tǒng)及方法
技術(shù)領(lǐng)域：
本發(fā)明屬于光電控制測量技術(shù)領(lǐng)域。具體涉及一種FSM遠(yuǎn)程實(shí)時(shí)控制時(shí)間補(bǔ)償系統(tǒng)及方法。
背景技術(shù)：
隨著技術(shù)的進(jìn)步，信息技術(shù)與計(jì)算機(jī)技術(shù)得到了快速發(fā)展，快速傾斜反射鏡，SPFSM,簡稱快反鏡，其控制精度越來越高，對控制信息提出了更高的實(shí)時(shí)性要求。目前廣泛應(yīng)用的光電經(jīng)緯儀的粗精跟蹤方式很難適應(yīng)人工手動控制高精度的實(shí)時(shí)性要求，特別是遠(yuǎn)程控制操作，對于時(shí)間要求越來越嚴(yán)格的FSM控制，遠(yuǎn)程控制FSM不具有時(shí)效性。中國專利文獻(xiàn)庫公布了一項(xiàng)名稱為《基于目標(biāo)反射信號的光束瞄準(zhǔn)系統(tǒng)中三角掃描偏差校準(zhǔn)方法》(專利申請?zhí)?01110100694.2)的發(fā)明專利申請技術(shù)，該發(fā)明專利申請技術(shù)公開了一種基于目標(biāo)反射信號的光束貓準(zhǔn)系統(tǒng)中三角掃描偏差校準(zhǔn)方法:(I)光束貓準(zhǔn)時(shí)，目標(biāo)相對于光束統(tǒng)計(jì)中心存在偏差b，采集目標(biāo)反射信號，估計(jì)偏差大小，記為bl ; (2)瞄準(zhǔn)系統(tǒng)驅(qū)動快反鏡使光束沿X正軸偏轉(zhuǎn)bl ; (3)采集信號，估計(jì)偏差，記為b2 ; (4)如果已瞄準(zhǔn)，執(zhí)行第I步；如果b2大于bl的1.9倍，執(zhí)行第5步；否則執(zhí)行第6步；(5)控制光束沿x軸負(fù)軸偏轉(zhuǎn)b2 ；
(6)根據(jù)b、bl、b2，求出偏差b2的方向；(7)控制光束沿估計(jì)出的方向角偏轉(zhuǎn)b2 ; (8)采集信號，估計(jì)偏差，記為b3 ;(9)如果已瞄準(zhǔn)，則執(zhí)行第I步；否則執(zhí)行第10步；(10)控制光束沿I軸負(fù)向偏轉(zhuǎn)b3，執(zhí)行第I步。發(fā)明申請?jiān)谶M(jìn)行跟蹤控制的時(shí)候時(shí)效性不高，不適合用于遠(yuǎn)程實(shí)時(shí)控制，當(dāng)控制數(shù)據(jù)到達(dá)執(zhí)行器的時(shí)候，該數(shù)據(jù)對應(yīng)的時(shí)刻已經(jīng)過去了，不能實(shí)現(xiàn)對快反鏡的有效遠(yuǎn)程控制。

發(fā)明內(nèi)容
本發(fā)明技術(shù)解決問題:為了克服FSM跟蹤控制過程中時(shí)效性差的缺點(diǎn)，提供一種FSM遠(yuǎn)程實(shí)時(shí)控制時(shí)間補(bǔ)償系統(tǒng)及方法，能夠有效、實(shí)時(shí)遠(yuǎn)程控制FSM，提升FSM跟蹤控制的能力，本發(fā)明技術(shù)解決方案:一種FSM遠(yuǎn)程實(shí)時(shí)控制時(shí)間補(bǔ)償系統(tǒng),包括:FSM1、信號調(diào)理模塊2、數(shù)據(jù)采集模塊3、嵌入式處理平臺4、PC端5、以太網(wǎng)9、模擬驅(qū)動模塊10、控制執(zhí)行器11、電機(jī)12和位置傳感器13 ;所述嵌入式處理平臺4包括ARM7和DSP8，DSP8直接通過總線接口與數(shù)據(jù)采集模塊3的接口相接；位置傳感器13對FSMl的位置信息進(jìn)行探測，將位置信息經(jīng)過信號調(diào)理模塊2調(diào)理，再把調(diào)理后的模擬電信號送給數(shù)據(jù)采集模塊3進(jìn)行采集，數(shù)據(jù)采集模塊3把模擬電信號轉(zhuǎn)換成數(shù)字信號后送給嵌入式處理平臺4，嵌入式處理平臺4中的ARM7通過以太網(wǎng)9把該信息傳給遠(yuǎn)程的PC端5，PC端5獲取到該位置信息后，把這些信息送給內(nèi)部的GPU6，在GPU6中進(jìn)行預(yù)測算法運(yùn)算，利用FSMl每個(gè)新的位置和時(shí)間參數(shù)進(jìn)行預(yù)測，從而產(chǎn)生超前的位置控制參數(shù)，然后通過PC端5與嵌入式處理平臺4的以太網(wǎng)9傳給DSP8，DSP8通過對直接從數(shù)據(jù)采集模塊3送過來的數(shù)據(jù)進(jìn)行分析與分組緩存濾波處理，然后利用PC端5傳過來的預(yù)測控制信息補(bǔ)償該結(jié)果或者直接利用其中的控制信息直接控制FSM1，從而產(chǎn)生相應(yīng)的控制數(shù)據(jù)，并送給模擬驅(qū)動模塊10，模擬驅(qū)動模塊10根據(jù)控制數(shù)據(jù)向控制執(zhí)行器11傳遞控制信息，控制執(zhí)行器11通過所給的控制信息對FSMl進(jìn)行跟蹤控制。所述預(yù)測算法運(yùn)算為卡爾曼濾波運(yùn)算，要結(jié)合FSM控制系統(tǒng)模型，根據(jù)包含測量位置信息與控制數(shù)據(jù)在內(nèi)的信息進(jìn)行計(jì)算，算出預(yù)測的控制信息。所述DSP8將直接從數(shù)據(jù)采集模塊3送過來的數(shù)據(jù)進(jìn)行分組、每組數(shù)據(jù)采用FIFO緩存，然后對數(shù)據(jù)進(jìn)行噪聲濾波。所述信號調(diào)理模塊2中的調(diào)理是對位置傳感器13輸出信號進(jìn)行濾波、縮放。所述數(shù)據(jù)采集模塊3采用分辨率大于14bit的高精度的A/D轉(zhuǎn)換器，同時(shí)采用FPGA或者CPLD獲取數(shù)據(jù)，并設(shè)計(jì)相應(yīng)的接口與嵌入式處理平臺4上的接口相連。所述總線接口為PCI總線或PCIe總線。所述ARM7和DSP8集成于ARM+DSP雙核處理器，或采用ARM+DSP模式設(shè)計(jì)，ARM和DSP通過高速接口進(jìn)行數(shù)據(jù)通信。所述PC端5采用的是X86架構(gòu)的CPU，以實(shí)時(shí)性強(qiáng)的VxWorks操作系統(tǒng)為基礎(chǔ)，同時(shí)采用能夠并行處理的GPU6作為計(jì)算單元，GPU6直接進(jìn)行硬件執(zhí)行運(yùn)算，速度很快，僅有納米級的時(shí)間誤差，時(shí)間確定，預(yù)測計(jì)算時(shí)，這些預(yù)測計(jì)算花費(fèi)的時(shí)間可忽略或在本身計(jì)算時(shí)補(bǔ)償?shù)娇偟臅r(shí)間里，GPU6可以選用NVIDIA公司的GeForce GT640T，也可以選用其他產(chǎn)品，能用在X86架構(gòu)上均可，該X86架構(gòu)的CPU可以選擇intel公司的雙核或者多核處理器，具有相應(yīng)的100M/1000M的MAC接口即可。一種FSM遠(yuǎn)程實(shí)時(shí)控制時(shí)間補(bǔ)償方法，實(shí)現(xiàn)步驟如下:第一步，計(jì)算出FSMl的位置信息從位置傳感器13到嵌入式處理平臺4的傳輸時(shí)間，然后把這個(gè)時(shí)間段記錄在嵌入式處理平臺4中進(jìn)行統(tǒng)一處理，此時(shí)At4為O ;第二步，當(dāng)該位置信息傳遞到了嵌入式處理平臺4后，在每個(gè)數(shù)據(jù)幀之后加時(shí)間戳，編碼后通過以太網(wǎng)9送給PC端5，此處時(shí)間戳表示的是此時(shí)的時(shí)間信息；第三步，遠(yuǎn)程的PC端5接收到數(shù)據(jù)后，立刻記錄當(dāng)前的時(shí)間，計(jì)算出通過以太網(wǎng)9傳遞的時(shí)間延遲，即此時(shí)的時(shí)間減去數(shù)據(jù)幀的時(shí)間戳，得到Λ t2和FSMl位置數(shù)據(jù)幀，At2是以太網(wǎng)9傳遞的時(shí)間延遲；第四步，把Λ t2是以太網(wǎng)9傳遞的時(shí)間延遲Λ t2與FSMl位置數(shù)據(jù)幀一起發(fā)給PC端5中的GPU6，利用GPU6解算出位置數(shù)據(jù)，在GPU6進(jìn)行預(yù)測算法運(yùn)算，利用該算法進(jìn)行FSMl的位置預(yù)測，通過已有的時(shí)間差Λ t2，預(yù)測出從此時(shí)刻tn再把預(yù)測信息傳遞過去的預(yù)估時(shí)間差Λ t5，即tn+Λ t5時(shí)刻的FSMl的預(yù)測控制信息；所述Λ t5為Λ t2和Λ t4的二分之一，即 Δ t5= ( Δ t2+ Δ t4) /2 ；第五步，把tn+At5時(shí)刻的FSMl預(yù)測控制信息加上此時(shí)的時(shí)間戳，編碼組成一個(gè)數(shù)據(jù)幀；第六步，利用以太網(wǎng)9把第五步產(chǎn)生的數(shù)據(jù)幀傳遞給嵌入式處理平臺4，記錄下嵌入式處理平臺4收到第五步產(chǎn)生的數(shù)據(jù)幀的時(shí)刻信息，通過此時(shí)的時(shí)刻減去第五步產(chǎn)生的數(shù)據(jù)幀包含的時(shí)間，得到從PC端5通過以太網(wǎng)9傳給嵌入式處理平臺4的時(shí)間At3 ；第七步，嵌入式處理平臺4中的DSP8通過第五步產(chǎn)生的數(shù)據(jù)幀對從數(shù)據(jù)采集模塊3傳遞過來的數(shù)據(jù)進(jìn)行補(bǔ)償，將補(bǔ)償后的數(shù)據(jù)通過雙環(huán)的閉環(huán)控制PID算法計(jì)算出控制數(shù)據(jù)，實(shí)現(xiàn)實(shí)時(shí)時(shí)間補(bǔ)償控制計(jì)算，從而對FSMl進(jìn)行實(shí)時(shí)控制；同時(shí)在ARM7中計(jì)算出加權(quán)時(shí)間差 At4，即 At4= ( At5+At3) /2 ；第八步，如果需要進(jìn)行FSMl的控制，則返回第二步，否則結(jié)束。所述的控制執(zhí)行器是直接驅(qū)動FSM中的控制電機(jī)的設(shè)備，直接利用控制執(zhí)行器可以控制FSM的光電跟蹤控制。本發(fā)明的FSMl由多軸控制，對每個(gè)位置的新息進(jìn)行預(yù)測，根據(jù)實(shí)際的FSMl系統(tǒng)，獲得系統(tǒng)狀態(tài)，即目前tn時(shí)刻的位置信息X (k-Ι)和控制系數(shù)A, A為支撐FSMl控制軸的參數(shù)矩陣，此時(shí)的X (k-Ι)和控制系數(shù)A已知；設(shè)U (k)是預(yù)測的控制參數(shù)，B是控制系數(shù)，B是矩陣，此時(shí)U (k)和B已知；通過數(shù)據(jù)采集模塊3獲取目前tn時(shí)刻的數(shù)據(jù)Z (k-Ι)也是矩陣，Z (k)是tn+At5時(shí)亥Ij相應(yīng)的數(shù)據(jù)，H是測量系數(shù)，H是矩陣為已知條件。建立系統(tǒng)微分方程如下:X (k) =A X (k-1) +B U (k) (I)Z (k) =H X (k) (2)根據(jù)實(shí)際情況，設(shè)tn+ Δ t5時(shí)刻預(yù)測位置信息為:X (k I k-1) =A X (k-1 |k-l) +B U (k-1) (3)用P (k I k-1)表不 X (k I k-1)對應(yīng)的協(xié)方差，P (k-11 k-1)表不 X (k-11 k-1)對應(yīng)的協(xié)方差，則有:P (k I k-1) =A P (k-11 k-1) A， (4)式(4)中A’表示A的轉(zhuǎn)置矩陣。結(jié)合預(yù)測值和測量值，我們可以得到現(xiàn)在狀態(tài)tn+Λ t5時(shí)刻的最優(yōu)化估算值X(kIk):X (k|k)=X (k I k-1) +Kg (k) (Z (k) -H X (k|k_l)) (5)從該式(5)中得到X (k|k)，就可以利用目標(biāo)位置與最優(yōu)估算位置X (k|k)之差作為控制輸入信號，直接利用多軸控制PID算法進(jìn)行tn+At5時(shí)刻的FSM (I)位置控制。其中Kg為卡爾曼增益(Kalman Gain):Kg (k)=P (k|k-l)H，/ (HP (k|k_l)H，) (6)H’為H矩陣的轉(zhuǎn)置矩陣。根據(jù)上述6個(gè)公式，已經(jīng)得到了 tn+Λ t5時(shí)刻下最優(yōu)的估算值X (k|k)。但是為了要令卡爾曼濾波器不斷的運(yùn)行下去直到系統(tǒng)過程結(jié)束，我們還要更新tn+At5時(shí)刻下X(k|k)的協(xié)方差:P (k|k)= (1-Kg (k)H)P (k|k-l) (7)其中I為I的矩陣。當(dāng)系統(tǒng)進(jìn)入tn+At5時(shí)刻的后一個(gè)時(shí)刻時(shí)即當(dāng)在tn+At5時(shí)刻預(yù)測下一個(gè)時(shí)刻時(shí)，P (k|k)就是式子(4)的P (k-llk-Ι)，算法就可以自回歸的運(yùn)算下去，自動的計(jì)算X(k I k),這也就能一直計(jì)算新的預(yù)測控制值U (k)。根據(jù)預(yù)測出的FSM的位置信息，將該預(yù)測值和控制目標(biāo)值相減代入PID控制算法，得到相應(yīng)的控制信息，該方法能夠極好的在DSP中進(jìn)行PID運(yùn)算，達(dá)到控制的目的。所述的實(shí)時(shí)控制中，可以直接利用在GPU中計(jì)算出來的結(jié)果控制FSM。本發(fā)明與現(xiàn)有技術(shù)相比的優(yōu)點(diǎn)在于:
(I)本發(fā)明充分運(yùn)用了遠(yuǎn)程數(shù)據(jù)傳輸中對總線數(shù)據(jù)的時(shí)間補(bǔ)償，通過一步步的計(jì)算和平均，通過卡爾曼濾波對FSM進(jìn)行位置預(yù)測，隨著控制次數(shù)的增加，對FSM實(shí)時(shí)控制的時(shí)間補(bǔ)償就會越來越準(zhǔn)確。(2)本發(fā)明避免了對FSM遠(yuǎn)程測量控制過程中的時(shí)間丟失，在不增加任何硬件的基礎(chǔ)上實(shí)現(xiàn)了對FSM實(shí)時(shí)控制過程中的時(shí)間補(bǔ)償，提升了控制性能，降低了工程成本，還減輕了設(shè)計(jì)的工作量和難度。(3)本發(fā)明實(shí)現(xiàn)了對FSM實(shí)時(shí)控制過程中的時(shí)間補(bǔ)償，提升了控制性能，降低了成本，還減輕了設(shè)計(jì)的工作量和難度。


圖1為本發(fā)明的FSM遠(yuǎn)程實(shí)時(shí)控制時(shí)間補(bǔ)償系統(tǒng)硬件結(jié)構(gòu)圖；圖2為本發(fā)明的FSM遠(yuǎn)程實(shí)時(shí)控制時(shí)間補(bǔ)償方法實(shí)現(xiàn)流程圖；圖中l(wèi).FSM，2.信號調(diào)理模塊，3.數(shù)據(jù)采集模塊，4.嵌入式處理平臺，5.PC端，
6.GPU, 7.ARM, 8.DSP, 9.以太網(wǎng)，10.模擬驅(qū)動，11.控制執(zhí)行器，12.電機(jī)，13.位置傳感器。
具體實(shí)施例方式如圖1所示，本發(fā)明的FSM遠(yuǎn)程實(shí)時(shí)控制時(shí)間補(bǔ)償方法，由硬件和軟件組成，硬件中的位置傳感器13對FSMl的位置信息進(jìn)行探測，然后將位置信息經(jīng)過信號調(diào)理模塊2調(diào)理，然后把調(diào)理后的模擬電信號送給數(shù)據(jù)采集模塊3進(jìn)行采集，數(shù)據(jù)采集模塊3把模擬電信號轉(zhuǎn)換成數(shù)字信號后送給嵌入式處理平臺4，嵌入式處理平臺4中的ARM7通過以太網(wǎng)9把該信息傳給遠(yuǎn)程的PC端5，PC端5獲取到FSMl的位置信息后，把這些信息送給內(nèi)部的GPU6，在GPU6中進(jìn)行類似于卡爾曼濾波算法的運(yùn)算，利用FSMl每個(gè)新的位置的新息進(jìn)行預(yù)測，從而產(chǎn)生超前的控制參數(shù)，然后通過PC端5與嵌入式處理平臺4的以太網(wǎng)9傳給DSP8，DSP8通過對直接從數(shù)據(jù)采集模塊3送過來的數(shù)據(jù)進(jìn)行分組，然后采用與該數(shù)據(jù)等寬的FIFO進(jìn)行緩存，然后通過FIR低通濾波器濾除噪聲干擾，然后利用PC端5傳過來的預(yù)測控制信息補(bǔ)償該結(jié)果，從而產(chǎn)生相應(yīng)的控制數(shù)據(jù)，并送給模擬驅(qū)動10，模擬驅(qū)動模塊10根據(jù)控制數(shù)據(jù)向控制執(zhí)行器11傳遞控制信息，控制執(zhí)行器11通過所給的控制信息對FSMl進(jìn)行跟蹤控制，采用PID控制算法，實(shí)現(xiàn)雙環(huán)反饋控制，滿足控制信息要求。軟件又包含底層軟件和控制軟件，底層軟件包含PC端5的VxWorks操作系統(tǒng)和嵌入式處理平臺4中的Linux操作系統(tǒng)，以及在各自的操作系統(tǒng)下的驅(qū)動程序；控制軟件是基于底層軟件的，如圖2所示，其處理流程如下:a)在設(shè)計(jì)的時(shí)候計(jì)算出FSMl的位置信息從位置傳感器13到嵌入式處理平臺4的傳輸時(shí)間，因?yàn)檫@段時(shí)間全都是硬件傳輸，可以計(jì)算出確定的時(shí)間，然后把這個(gè)時(shí)間段記錄在嵌入式處理平臺4中進(jìn)行統(tǒng)一處理，此時(shí)At4為O ;b)當(dāng)該位置信息傳遞到了嵌入式處理平臺4后，在送給以太網(wǎng)9之前進(jìn)行編碼，同時(shí)在每幀數(shù)據(jù)之后加上時(shí)間戳，該時(shí)間戳是當(dāng)前的時(shí)間，編碼的時(shí)候把時(shí)間信息編在FSMl位置信息之后；c)當(dāng)遠(yuǎn)程的PC端5接收到數(shù)據(jù)后，立刻記錄當(dāng)前的時(shí)間，計(jì)算出通過以太網(wǎng)9傳遞的延遲，即此時(shí)的時(shí)間減去數(shù)據(jù)幀的時(shí)間戳，得到At2 ；
d)然后把該時(shí)間與傳遞過來的FSMl位置數(shù)據(jù)幀一起發(fā)給PC端5中的GPU6，利用GPU6解算出位置數(shù)據(jù)，利用GPU6建立狀態(tài)觀測矩陣，如果利用卡爾曼濾波算法則要建立狀態(tài)觀測矩陣，利用該算法進(jìn)行FSMl的位置預(yù)測，通過已有的時(shí)間差Λ t2，預(yù)測出從此時(shí)tn再把預(yù)測信息傳遞過去的時(shí)間段Λ t5，即tn+ Δ t5時(shí)刻的FSMl的預(yù)測控制信息，Λ t5=(At2+At4) /2 ；e)PC端5對通過GPU6計(jì)算出來的tn+ Δ t5時(shí)刻的FSMl預(yù)測控制信息進(jìn)行編碼，同時(shí)加上此時(shí)的時(shí)間戳，時(shí)間戳信息放在預(yù)測出來的FSMl預(yù)測控制信息之后組成一個(gè)數(shù)據(jù)幀；f )利用以太網(wǎng)9把該tn+ Δ t5時(shí)刻的FSMl的預(yù)測控制信息數(shù)據(jù)幀傳遞給嵌入式處理平臺4，利用嵌入式中的DSP8進(jìn)行實(shí)時(shí)數(shù)據(jù)補(bǔ)償，同時(shí)記錄下嵌入式處理平臺4收到tn+At5時(shí)刻的FSMl的預(yù)測控制信息數(shù)據(jù)幀的時(shí)刻信息，通過此時(shí)的時(shí)刻減去tn+At5時(shí)刻的FSMl的預(yù)測控制信息數(shù)據(jù)幀中的時(shí)間，得到從PC端5通過以太網(wǎng)9傳給嵌入式處理平臺4的時(shí)間Δ t3 ；g)嵌入式處理平臺4中的DSP8通過PC端5傳遞過來的預(yù)測FSMl的預(yù)測控制信息對從數(shù)據(jù)采集模塊3傳遞過來的數(shù)據(jù)進(jìn)行補(bǔ)償，將補(bǔ)償后的數(shù)據(jù)通雙環(huán)的閉環(huán)控制PID算法計(jì)算出控制數(shù)據(jù)，實(shí)現(xiàn)實(shí)時(shí)時(shí)間補(bǔ)償控制計(jì)算，從而對FSMl進(jìn)行實(shí)時(shí)控制；h)同時(shí)在 ARM7 中計(jì)算出 At4，即 Λ t4= ( At5+At3)/2 ；i)如果需要進(jìn)行FSMl的控制，則返回步驟b)，否則結(jié)束。所述的位置傳感器13采集的是FSMl的三維數(shù)據(jù)信息，描述的是FSMl的方向信息，輸出的是模擬電信號。本發(fā)明的FSM (I)由多軸控制，對每個(gè)位置的新息進(jìn)行預(yù)測，根據(jù)實(shí)際的FSM (I)系統(tǒng)，獲得系統(tǒng)狀態(tài)，即目前tn時(shí)刻的位置信息X (k-Ι)和控制系數(shù)A, A為支撐FSM (I)控制軸的參數(shù)矩陣，此時(shí)的X (k-Ι)和控制系數(shù)A已知；設(shè)U (k)是預(yù)測的控制參數(shù)，B是控制系數(shù)，B是矩陣，此時(shí)U (k)和B已知；通過數(shù)據(jù)采集模塊3獲取目前tn時(shí)刻的數(shù)據(jù)Z(k-Ι)也是矩陣，Z (k)是tn+At5時(shí)亥I湘應(yīng)的數(shù)據(jù)，H是測量系數(shù)，H是矩陣為已知條件。建立系統(tǒng)微分方程如下:X (k) =A X (k-1) +B U (k) (I)Z (k) =H X (k) (2)根據(jù)實(shí)際情況，設(shè)tn+ Δ t5時(shí)刻預(yù)測位置信息為:X (k I k-1) =A X (k-1 |k-l) +B U (k-1) (3)用P (k I k-1)表不 X (k I k-1)對應(yīng)的協(xié)方差，P (k-11 k-1)表不 X (k-11 k-1)對應(yīng)的協(xié)方差，則有:P (k I k-1) =A P (k-11 k-1) A， (4)式(4)中A’表示A的轉(zhuǎn)置矩陣。結(jié)合預(yù)測值和測量值，可以得到現(xiàn)在狀態(tài)tn+ Δ t5時(shí)刻的最優(yōu)化估算值X (k | k):X (k|k)=X (k I k-1) +Kg (k) (Z (k) -H X (k|k_l)) (5)從該式(5)中得到X(k|k)，就可以利用目標(biāo)位置與最優(yōu)估算位置X(k|k)之差作為控制輸入信號，直接利用多軸控制PID算法通過模擬驅(qū)動10傳遞出去進(jìn)行FSMl的tn+At5時(shí)刻位置控制。
其中Kg為卡爾曼增益(Kalman Gain):Kg (k)=P (k|k-l)H，/ (HP (k|k_l)H，) (6)H’為H矩陣的轉(zhuǎn)置矩陣。根據(jù)上述6個(gè)公式，已經(jīng)得到了 tn+ Δ t5時(shí)刻下最優(yōu)的估算值X(k | k)。但是為了要令卡爾曼濾波器不斷的運(yùn)行下去直到系統(tǒng)過程結(jié)束，還要更新tn+ Δ t5時(shí)刻下X (k I k)的協(xié)方差:P (k|k)= (1-Kg (k)H)P (k|k-l) (7)其中I為I的矩陣。當(dāng)系統(tǒng)進(jìn)入tn+At5時(shí)刻的后一個(gè)時(shí)刻時(shí)即當(dāng)在tn+Δ t5時(shí)刻預(yù)測下一個(gè)時(shí)刻時(shí)，P (k|k)就是式子(4)的P (k-llk-Ι)，算法就可以自回歸的運(yùn)算下去，自動的計(jì)算X(k|k)，這也就能一直計(jì)算新的預(yù)測控制值u(k)。該算法已經(jīng)非常成熟，自卡爾曼在其博士論文中提出，在包含航天航空在內(nèi)的各領(lǐng)域中成功的應(yīng)用已有四五十年的歷史。所述的信號調(diào)理主要對位置傳感器13輸出的信號進(jìn)行濾波、縮放到數(shù)據(jù)采集模塊3允許輸入的范圍內(nèi)。本發(fā)明的數(shù)據(jù)采集模塊3采用的是高精度的A/D轉(zhuǎn)換器，同時(shí)采用FPGA或者CPLD進(jìn)行數(shù)據(jù)的獲取，并設(shè)計(jì)相應(yīng)的接口與嵌入式處理平臺4上的接口相連。所述的嵌入式處理平臺4主要包含ARM7和DSP8，DSP8直接通過總線接口與數(shù)據(jù)采集模塊3的接口相接，這些接口可以采用包含PCI總線、PCIe總線在內(nèi)的諸多總線實(shí)現(xiàn)。本發(fā)明的嵌入式處理平臺4中的ARM7和DSP8可以集成于ARM+DSP雙核處理器，也可以采用ARM+DSP模式設(shè)計(jì)，ARM和DSP通過高速接口進(jìn)行數(shù)據(jù)通信，選擇的ARM處理器上需要有100M或者1000M的MAC接口。所述的實(shí)時(shí)控制中，可以直接利用在GPU6中計(jì)算出來的結(jié)果控制FSM1。本發(fā)明的PC端5采用的是X86架構(gòu)的CPU，以高實(shí)時(shí)性的VxWorks操作系統(tǒng)為基礎(chǔ)，同時(shí)采用能夠并行處理的GPU6作為計(jì)算單元，GPU6直接進(jìn)行硬件執(zhí)行計(jì)算，速度很快，僅有納米級的時(shí)間誤差，時(shí)間確定，預(yù)測計(jì)算時(shí)，這些預(yù)測計(jì)算花費(fèi)的時(shí)間可以忽略或在本身計(jì)算時(shí)補(bǔ)償?shù)娇偟臅r(shí)間里，該GPU6可以選用NVIDIA公司的GeForce GT640T，也可以選用其他產(chǎn)品，能用于X86架構(gòu)上GPU均可，該X86架構(gòu)的CPU可以選擇intel公司的雙核或者多核處理器，具有相應(yīng)的100M/1000M的MAC接口即可。所述控制執(zhí)行器11直接驅(qū)動FSMl中的控制電機(jī)12的設(shè)備，直接利用控制執(zhí)行器11可以進(jìn)行FSMl的光電跟蹤控制。
權(quán)利要求
1.一種FSM遠(yuǎn)程實(shí)時(shí)控制時(shí)間補(bǔ)償系統(tǒng)，其特征在于包括:FSM(1)、信號調(diào)理模塊(2)、數(shù)據(jù)采集模塊(3)、嵌入式處理平臺(4)、PC端(5)、以太網(wǎng)(9)、模擬驅(qū)動模塊(10)、控制執(zhí)行器(11)、電機(jī)(12)和位置傳感器(13);所述嵌入式處理平臺(4)包括ARM (7)和DSP (8)，DSP (8)直接通過總線接口與數(shù)據(jù)采集模塊(3)的接口相接；位置傳感器(13)對FSM (I)位置信息進(jìn)行探測，將該信息經(jīng)過信號調(diào)理模塊(2 )調(diào)理，再把調(diào)理后的模擬電信號送給數(shù)據(jù)采集模塊(3)進(jìn)行采集，數(shù)據(jù)采集模塊(3)把模擬電信號轉(zhuǎn)換成數(shù)字信號后送給嵌入式處理平臺(4)，嵌入式處理平臺(4)中的ARM (7)通過以太網(wǎng)(9)把該信息傳給遠(yuǎn)程的PC端(5),PC端(5)獲取到該信息后，把這些信息送給內(nèi)部的GPU (6)，在GPU (6)中進(jìn)行預(yù)測算法運(yùn)算，利用FSM (I)每個(gè)新的位置和時(shí)間參數(shù)進(jìn)行預(yù)測，從而產(chǎn)生超前的針對FSM (I)位置信息的控制參數(shù)，然后通過PC端(5)與嵌入式處理平臺(4)的以太網(wǎng)(9)傳給DSP (8)，DSP (8)通過對直接從數(shù)據(jù)采集模塊(3)送過來的數(shù)據(jù)進(jìn)行分析與分組緩存濾波處理，然后利用PC端(5)傳過來的預(yù)測控制信息補(bǔ)償該結(jié)果或者直接利用其中的控制信息直接控制FSM (1)，從而產(chǎn)生相應(yīng)的控制數(shù)據(jù)，并送給模擬驅(qū)動模塊(10)，模擬驅(qū)動模塊(10)根據(jù)控制數(shù)據(jù)向控制執(zhí)行器(11)傳遞控制信息，控制執(zhí)行器(11)通過所給的控制信息對FSM (I)進(jìn)行控制。
2.根據(jù)權(quán)利要求1所述的FSM遠(yuǎn)程實(shí)時(shí)控制時(shí)間補(bǔ)償系統(tǒng)，其特征在于:所述預(yù)測算法運(yùn)算結(jié)合FSM (I)控制系統(tǒng)模型，根據(jù)包含測量位置信息與控制數(shù)據(jù)在內(nèi)的信息進(jìn)行卡爾曼濾波運(yùn)算，預(yù)測FSM (I)的位置信息，然后根據(jù)該預(yù)測信息計(jì)算出控制信息。
3.根據(jù)權(quán)利要求1所述的FSM遠(yuǎn)程實(shí)時(shí)控制時(shí)間補(bǔ)償系統(tǒng)，其特征在于:所述DSP(S)將直接從數(shù)據(jù)采集模塊(3)送過來的數(shù)據(jù)進(jìn)行分組、每組數(shù)據(jù)采用FIFO緩存，然后對數(shù)據(jù)進(jìn)行噪聲濾波。
4.根據(jù)權(quán)利要求1所述的FSM遠(yuǎn)程實(shí)時(shí)控制時(shí)間補(bǔ)償系統(tǒng)，其特征在于:所述信號調(diào)理模塊(2)中的調(diào)理是對位置傳感器(13)輸出信號進(jìn)行濾波、縮放。
5.根據(jù)權(quán)利要求1所述的FSM遠(yuǎn)程實(shí)時(shí)控制時(shí)間補(bǔ)償系統(tǒng)，其特征在于:所述數(shù)據(jù)采集模塊(3)采用分辨率大于14bit高精度的A/D轉(zhuǎn)換器，同時(shí)采用FPGA或者CPLD獲取數(shù)據(jù)，并設(shè)計(jì)相應(yīng)的接口與嵌入式處理平臺(4)上的接口相連。
6.根據(jù)權(quán)利要求1所述的FSM遠(yuǎn)程實(shí)時(shí)控制時(shí)間補(bǔ)償系統(tǒng)，其特征在于:所述總線接口為PCI總線或PCIe總線。
7.根據(jù)權(quán)利要求1所述的FSM遠(yuǎn)程實(shí)時(shí)控制時(shí)間補(bǔ)償系統(tǒng)，其特征在于:所述ARM(7)和DSP (8)集成于ARM+DSP雙核處理器，或采用ARM+DSP模式設(shè)計(jì)，ARM和DSP通過高速接口進(jìn)行數(shù)據(jù)通信。
8.根據(jù)權(quán)利要求1所述的FSM遠(yuǎn)程實(shí)時(shí)控制時(shí)間補(bǔ)償系統(tǒng)，其特征在于:所述PC端(5)采用的是X86架構(gòu)的CPU，以實(shí)時(shí)性強(qiáng)的VxWorks操作系統(tǒng)為基礎(chǔ)，同時(shí)采用能夠并行處理的GPU (6)作為計(jì)算單元。
9.一種FSM遠(yuǎn)程實(shí)時(shí)控制時(shí)間補(bǔ)償方法，其特征在于實(shí)現(xiàn)步驟如下: 第一步，計(jì)算出FSM (I)的位置信息從位置傳感器(13)到嵌入式處理平臺(4)的傳輸時(shí)間，然后把這個(gè)時(shí)間段記錄在嵌入式處理平臺(4)中進(jìn)行統(tǒng)一處理，此時(shí)At4為O ; 第二步，當(dāng)FSM (I)的位置信息傳遞到了嵌入式處理平臺(4)后，在每個(gè)數(shù)據(jù)幀之后加時(shí)間戳，編碼后通過以太網(wǎng)(9)送給PC端(5)，此處時(shí)間戳表示的是此時(shí)的時(shí)間信息；第三步，遠(yuǎn)程的PC端(5)接收到數(shù)據(jù)后，立刻記錄當(dāng)前的時(shí)間，計(jì)算出通過以太網(wǎng)(9)傳遞的時(shí)間延遲，即此時(shí)的時(shí)間減去數(shù)據(jù)幀的時(shí)間戳，得到Λ t2和位置數(shù)據(jù)幀，其中At2是以太網(wǎng)(9)傳遞的時(shí)間延遲； 第四步，把以太網(wǎng)(9)傳遞的時(shí)間延遲Λ t2與位置數(shù)據(jù)幀一起發(fā)給PC端(5)中的GPU(6)，利用GPU (6)解算出位置數(shù)據(jù)，在GPU (6)進(jìn)行預(yù)測算法運(yùn)算，利用該算法進(jìn)行FSM (I)的位置預(yù)測，通過已有的時(shí)間差A(yù)t2，預(yù)測出從此時(shí)刻tn再把預(yù)測信息傳遞過去的預(yù)估時(shí)間差A(yù)t5，即tn+At5時(shí)刻的FSM (I)的預(yù)測控制信息，此時(shí)Λ t5為Λ t2和Λ t4的二分之一，g卩 At5= ( At2+At4) /2 ； 第五步，把tn+At5時(shí)刻的FSM (I)預(yù)測控制信息加上此時(shí)的時(shí)間戳，編碼組成一個(gè)數(shù)據(jù)幀； 第六步，利用以太網(wǎng)(9)把第五步產(chǎn)生的數(shù)據(jù)幀傳遞給嵌入式處理平臺(4)，記錄下嵌入式處理平臺(4)收到第五步產(chǎn)生的數(shù)據(jù)幀的時(shí)刻信息，通過此時(shí)的時(shí)刻減去第五步產(chǎn)生的數(shù)據(jù)幀包含的時(shí)間，得到從PC端(5)通過以太網(wǎng)(9)傳給嵌入式處理平臺(4)的時(shí)間At3 ； 第七步，嵌入式處理平臺(4)中的DSP (8)通過第五步產(chǎn)生的數(shù)據(jù)幀對從數(shù)據(jù)采集模塊(3)傳遞過來的數(shù)據(jù)進(jìn)行補(bǔ)償，將補(bǔ)償后的數(shù)據(jù)通過雙環(huán)的閉環(huán)控制PID算法計(jì)算出控制數(shù)據(jù)，實(shí)現(xiàn)實(shí)時(shí)時(shí)間補(bǔ)償控制計(jì)算，從而對FSM (I)進(jìn)行實(shí)時(shí)控制；同時(shí)在ARM (7)中計(jì)算出加權(quán)時(shí)間差A(yù)t4，即At4= ( At5+At3)/2 ； 第八步，如果需要進(jìn)行FS M (I)的控制，則返回第二步，否則結(jié)束。
全文摘要
本發(fā)明提供了一種FSM遠(yuǎn)程實(shí)時(shí)控制時(shí)間補(bǔ)償系統(tǒng)及方法，由硬件和軟件組成，硬件中的位置傳感器把獲取的FSM位置信息通過數(shù)據(jù)采集模塊與嵌入式處理平臺最后送給遠(yuǎn)程PC端，同時(shí)記錄下數(shù)據(jù)傳遞時(shí)間戳，通過時(shí)間戳計(jì)算出數(shù)據(jù)傳遞的時(shí)間差，PC端的GPU進(jìn)行卡爾曼濾波計(jì)算出把數(shù)據(jù)傳遞回控制執(zhí)行器時(shí)刻的位置信息并把該信息傳遞給嵌入式處理平臺，在嵌入式處理平臺進(jìn)行數(shù)據(jù)的實(shí)時(shí)補(bǔ)償校正，用計(jì)算控制信息控制FSM，同時(shí)用傳回的時(shí)間戳計(jì)算傳遞的時(shí)間差平均值，并不斷的用新時(shí)間差計(jì)算時(shí)間差平均值進(jìn)行時(shí)間補(bǔ)償；軟件包含底層軟件和控制軟件。本發(fā)明實(shí)現(xiàn)了對FSM實(shí)時(shí)控制過程中的時(shí)間補(bǔ)償，提升了控制性能，降低了成本，還減輕了設(shè)計(jì)的工作量和難度。
文檔編號G05D3/12GK103092219SQ201310013669
公開日2013年5月8日 申請日期2013年1月15日 優(yōu)先權(quán)日2013年1月15日
發(fā)明者蘇艷蕊, 嚴(yán)發(fā)寶, 王強(qiáng), 黃永梅, 杜升平, 柳萬勝 申請人:中國科學(xué)院光電技術(shù)研究所