高精度數(shù)字式MEMS陀螺儀驅(qū)動(dòng)閉環(huán)控制系統(tǒng)設(shè)計(jì)

童紫平,張歡陽,喬 偉,賀克軍,張紫乾,龍善麗

(華東光電集成器件研究所,蘇州 215163)

?

高精度數(shù)字式MEMS陀螺儀驅(qū)動(dòng)閉環(huán)控制系統(tǒng)設(shè)計(jì)

童紫平*,張歡陽,喬 偉,賀克軍,張紫乾,龍善麗

(華東光電集成器件研究所,蘇州 215163)

為提高微機(jī)械陀螺檢測(cè)靈敏度,設(shè)計(jì)了一種數(shù)字式微機(jī)械陀螺驅(qū)動(dòng)閉環(huán)控制系統(tǒng),該系統(tǒng)利用數(shù)字鎖相環(huán)來實(shí)現(xiàn)陀螺驅(qū)動(dòng)諧振頻率和相位的跟蹤,同時(shí)利用數(shù)字自動(dòng)增益控制模塊實(shí)現(xiàn)驅(qū)動(dòng)幅值的穩(wěn)定控制。該控制系統(tǒng)先是在MATLAB/Simulink平臺(tái)上進(jìn)行仿真驗(yàn)證,之后在基于FPGA的驗(yàn)證平臺(tái)上進(jìn)行驗(yàn)證。實(shí)驗(yàn)結(jié)果表明在該數(shù)字系統(tǒng)的控制下陀螺驅(qū)動(dòng)起振時(shí)間大約為0.6 s,驅(qū)動(dòng)幅值相對(duì)穩(wěn)定性小于10×106,陀螺零偏穩(wěn)定性達(dá)到10.448 °/hr。

微機(jī)械陀螺;數(shù)字閉環(huán)驅(qū)動(dòng);FPGA;鎖相環(huán);自動(dòng)增益;MATLAB/Simulink

隨著微機(jī)械加工工藝的發(fā)展,MEMS硅微機(jī)械陀螺儀在工業(yè)自動(dòng)化、慣性導(dǎo)航和汽車電子等領(lǐng)域得到了廣泛的應(yīng)用[1-2]。相比于光纖陀螺、激光陀螺,MEMS硅微機(jī)械陀螺儀具有體積小、可靠性高、成本低、可批量生產(chǎn)等優(yōu)點(diǎn)[3-4]。但是目前MEMS硅微機(jī)械陀螺的精度仍然處于較低的水平,如何提高M(jìn)EMS陀螺儀的精度是MEMS傳感器研究領(lǐng)域的主要方向。

MEMS陀螺儀是基于科氏力原理工作的,根據(jù)科氏力公式可知驅(qū)動(dòng)振蕩速率直接影響陀螺的刻度因子和穩(wěn)定性。因此保持陀螺驅(qū)動(dòng)模態(tài)的穩(wěn)定是提高陀螺精度的關(guān)鍵所在[5-6]。文獻(xiàn)[4]采用周期平均法對(duì)陀螺閉環(huán)自激驅(qū)動(dòng)進(jìn)行了理論分析和數(shù)值仿真,但未設(shè)計(jì)電路驗(yàn)證。文獻(xiàn)[5]介紹了一種基于AGC的陀螺閉環(huán)驅(qū)動(dòng)接口電路,但該電路使用模擬電路實(shí)現(xiàn)。文獻(xiàn)[7]在PCB板上設(shè)計(jì)實(shí)現(xiàn)了陀螺雙閉環(huán)測(cè)控電路。無論是采用PCB板電路還是采用模擬電路都容易受封裝、外界環(huán)境和寄生效應(yīng)的干擾,進(jìn)而影響陀螺的測(cè)量精度[8]。為了克服這些缺點(diǎn),提高陀螺測(cè)量精度,本文提出了一種數(shù)字式閉環(huán)驅(qū)動(dòng)控制方案。本文利用MATLAB/Simulink軟件進(jìn)行陀螺驅(qū)動(dòng)數(shù)字算法的設(shè)計(jì)以及驅(qū)動(dòng)閉環(huán)系統(tǒng)的仿真驗(yàn)證[9-10]。最后利用FPGA硬件驗(yàn)證平臺(tái)實(shí)驗(yàn)驗(yàn)證了整個(gè)閉環(huán)系統(tǒng)。

1 MEMS陀螺驅(qū)動(dòng)系統(tǒng)介紹

圖1是陀螺驅(qū)動(dòng)閉環(huán)控制原理圖,正常工作時(shí)陀螺驅(qū)動(dòng)質(zhì)量塊被激勵(lì)到諧振頻率上并維持恒定的振動(dòng)。為實(shí)現(xiàn)這一功能,系統(tǒng)在數(shù)字域設(shè)計(jì)了鎖相環(huán)(PLL)、自動(dòng)增益控制(AGC)和二階帶通ΣΔ-DAC。C/V電路用于檢測(cè)陀螺振動(dòng)引起的電容變化量,為了方便在數(shù)字域中處理信號(hào)電路中采用了一個(gè)高精度模數(shù)轉(zhuǎn)換器(ADC)。

圖1 陀螺驅(qū)動(dòng)閉環(huán)控制原理圖

圖2 角振動(dòng)陀螺結(jié)構(gòu)及動(dòng)力學(xué)模型示意圖

2 MEMS陀螺結(jié)構(gòu)特性分析

圖2所示是角振動(dòng)式微機(jī)械陀螺結(jié)構(gòu)及等效動(dòng)力學(xué)模型示意圖。與線振動(dòng)式陀螺一樣,當(dāng)不考慮正交誤差時(shí),角振動(dòng)式陀螺的動(dòng)力學(xué)模型也可用一個(gè)二階微分方程表示為:

(1)

式中:θd為陀螺驅(qū)動(dòng)模態(tài)振動(dòng)角度,Jd為角振動(dòng)式陀螺驅(qū)動(dòng)質(zhì)量塊關(guān)于Z軸的等效轉(zhuǎn)動(dòng)慣量,cd為陀螺驅(qū)動(dòng)模態(tài)等效阻尼系數(shù),kd為陀螺驅(qū)動(dòng)模態(tài)等效剛度,Mtotal-d為陀螺由外加靜電驅(qū)動(dòng)力產(chǎn)生的總扭矩。

我們假設(shè)外加驅(qū)動(dòng)力產(chǎn)生的總扭矩等于Mtotal-d0×sin(ωd-in·t),那么求解微分方程(1)可得:

θd(t)=θd0sin(ωd-in·t+φd0)

(2)

式中:轉(zhuǎn)動(dòng)角度幅值和相位可分別表示為:

(3)

(4)

(5)

(6)

式中:ωd是陀螺驅(qū)動(dòng)模態(tài)諧振頻率,Qd是陀螺驅(qū)動(dòng)模態(tài)品質(zhì)因子。

從以上分析可知,當(dāng)外加驅(qū)動(dòng)扭矩的頻率ωd-in等于陀螺驅(qū)動(dòng)固有諧振頻率ωd時(shí),陀螺結(jié)構(gòu)能夠獲得最大的振幅,此時(shí)相位φd0=-90°。因此為了使陀螺工作穩(wěn)定且有效,外加驅(qū)動(dòng)電壓的頻率就必須跟蹤陀螺驅(qū)動(dòng)模態(tài)固有諧振頻率。

3 數(shù)字鎖相環(huán)

在MEMS陀螺驅(qū)動(dòng)系統(tǒng)中驅(qū)動(dòng)電壓信號(hào)和驅(qū)動(dòng)檢測(cè)信號(hào)通常是正交的,因此在一個(gè)陀螺自激驅(qū)動(dòng)閉環(huán)系統(tǒng)中不僅需要持續(xù)跟蹤陀螺振動(dòng)信號(hào)的頻率,還需要對(duì)它的相位進(jìn)行跟蹤和遷移。在本電路中我們?cè)O(shè)計(jì)了一種適用于FPGA驗(yàn)證的數(shù)字鎖相環(huán)(也稱軟件鎖相環(huán)SPLL)來實(shí)現(xiàn)這一功能。圖3所示是軟件鎖相環(huán)SPLL的Simulink仿真圖。它是一個(gè)閉環(huán)控制網(wǎng)絡(luò),主要由鑒相器(PD)、環(huán)路濾波器(lf)以及數(shù)控振蕩器(NCO)組成。鑒相器(PD)檢測(cè)輸入?yún)⒖夹盘?hào)和NCO的輸出信號(hào)之間的相位差;環(huán)路濾波器是一個(gè)低通濾波器,用于濾除鑒相器輸出信號(hào)中的高頻無用成分;數(shù)控振蕩器NCO根據(jù)輸入信號(hào)產(chǎn)生一定頻率的正弦波和余弦波。

鑒相器的種類有很多種,本設(shè)計(jì)中采用了乘法鑒相器,當(dāng)環(huán)路穩(wěn)定時(shí)它可以近似于線性系統(tǒng)。假設(shè)輸入?yún)⒖夹盘?hào)u1=U1sin(ω1t+θ1),NCO的輸出信號(hào)u2=U2cos(ω2t+θ2),那么鑒相器輸出信號(hào)等于:

upd(t)=u1(t)·u2(t)=U1U2sin(ω1t+θ1)×

cos(ω2t+θ2)=[(U1U2)/2](sin[(ω1+ω2)t+

θ1+θ2]+sin[(ω1-ω2)t+θ1-θ2]}

(7)

式中:第1項(xiàng)為高頻分量,會(huì)被環(huán)路濾波器濾除;第2項(xiàng)為有用的低頻分量。當(dāng)鎖相環(huán)鎖定時(shí)ω1=ω2,令Kd=(U1U2)/2,忽略高頻分量,鑒相器的輸出近似有:

upd(t)≈Kdsin(θe)

(8)

式中:θe=θ1-θ2為相位誤差信號(hào)。如果相位誤差較小,正弦函數(shù)可以用其自變量替換得到:

upd(t)≈Kdθe

(9)

式(9)表示了乘法鑒相器的近似線性模型。

圖3 軟件鎖相環(huán)SPLL的Simulink仿真圖

在軟件鎖相環(huán)(SPLL)的數(shù)控振蕩器(NCO)設(shè)計(jì)中通?？刹捎肈DS或者CORDIC算法實(shí)現(xiàn)。由于CORDIC算法簡單而且非常適合FPGA實(shí)現(xiàn),本系統(tǒng)采用了CORDIC算法來實(shí)現(xiàn)NCO的功能。下面簡單介紹下CORDIC算法的工作原理。

圖4 圓坐標(biāo)系旋轉(zhuǎn)示意圖

圖4是質(zhì)點(diǎn)在圓坐標(biāo)系中旋轉(zhuǎn)示意圖,在xy坐標(biāo)平面內(nèi)將點(diǎn)(x1,y1)旋轉(zhuǎn)θ角度到點(diǎn)(x2,y2)。其關(guān)系用下式表示:

x2=x1cosθ-y1sinθ

y2=x1sinθ+y1cosθ

(10)

通過提出因數(shù)cosθ,式(10)可寫成如下形式:

x2=cosθ(x1-y1tanθ)

y2=cosθ(y1+x1tanθ)

(11)

如果去掉cosθ項(xiàng),我們得到偽旋轉(zhuǎn)方程:

(12)

即旋轉(zhuǎn)角度是正確的,但是模值變大了arccosθ倍,雖然我們并不能通過適當(dāng)?shù)臄?shù)學(xué)方法去除cosθ項(xiàng),但是我們發(fā)現(xiàn)去除cosθ項(xiàng)后能夠簡化坐標(biāo)平面旋轉(zhuǎn)的計(jì)算操作。

事先選取合適的角度θ,使得tanθi=2-i,這樣在n次迭代中方程(12)可以寫出更一般的形式

xi+1=xi-di(2-iyi)

yi+1=yi+di(2-ixi)

(13)

這里我們引入第3個(gè)方程,稱為角度累加器,用來在每次迭代過程中追蹤累加的旋轉(zhuǎn)角度:

zi+1=zi-di·arctan(2-i)

(14)

上式偽旋轉(zhuǎn)方程分析我們忽略掉了cosθ項(xiàng),這樣n次迭代后xn、yn被伸縮了Kn倍。其中

(15)

事實(shí)上如果迭代次數(shù)可知,則我們可以預(yù)先計(jì)算伸縮因子Kn。

因此,在旋轉(zhuǎn)模式中經(jīng)過n次迭代后我們得到

(16)

在式(16)中通過設(shè)置x0=1/Kn,y0=0就可以根據(jù)n次迭代的結(jié)果計(jì)算出cosz0和sinz0的數(shù)值。

4 自動(dòng)增益控制

為了保證微機(jī)械陀螺的刻度因子和零位的穩(wěn)定性,必須確保驅(qū)動(dòng)幅值和頻率的穩(wěn)定性。本設(shè)計(jì)中采用雙閉環(huán)控制網(wǎng)絡(luò)來分別實(shí)現(xiàn)MEMS微機(jī)械陀螺驅(qū)動(dòng)幅值和頻率的穩(wěn)定性控制。前面我們已經(jīng)討論了利用鎖相環(huán)進(jìn)行相位閉環(huán)控制;在幅度閉環(huán)控制中我們采用自動(dòng)增益控制(AGC)來實(shí)現(xiàn)。

陀螺驅(qū)動(dòng)閉環(huán)自動(dòng)增益控制原理圖如圖5所示。陀螺振蕩引起的角度變化被檢測(cè)電路檢測(cè)并進(jìn)行放大;經(jīng)過ADC轉(zhuǎn)換后在數(shù)字域提取得到其正弦波幅值amp,并與基準(zhǔn)參考值Ref進(jìn)行比較得到殘差幅值信號(hào)e;最后幅值殘差信號(hào)經(jīng)過PI控制器放大后與PLL輸出進(jìn)行混頻形成反饋驅(qū)動(dòng)信號(hào)。當(dāng)陀螺振動(dòng)引起的電壓變化幅值大于參考值Ref時(shí),殘差信號(hào)變成負(fù)值,PI控制器輸出減小即驅(qū)動(dòng)信號(hào)減小,結(jié)果陀螺振動(dòng)角度位移減小;反之亦然。通過這種反饋控制,最終陀螺振動(dòng)角度位移將穩(wěn)定在參考值Ref等效的位移附近。

圖5 AGC自動(dòng)增益控制原理圖

在閉環(huán)控制系統(tǒng)中最重要的是系統(tǒng)穩(wěn)定性,本系統(tǒng)中采用了經(jīng)典控制理論中的PI(比例積分)控制器來調(diào)節(jié)系統(tǒng)穩(wěn)定性[11-12]。其離散域的傳輸函數(shù)可表示為:

(17)

式中:Kp和Ki分別表示比例和積分常數(shù),它們由陀螺結(jié)構(gòu)參數(shù)和電路增益決定。

5 實(shí)驗(yàn)和仿真結(jié)果

為了驗(yàn)證陀螺驅(qū)動(dòng)閉環(huán)控制系統(tǒng)的正確性,基于FPGA開發(fā)平臺(tái)設(shè)計(jì)了一款陀螺閉環(huán)測(cè)控電路FPGA驗(yàn)證平臺(tái)。該平臺(tái)主要包括MEMS傳感器,前端C/V檢測(cè)電路,一款高精度ADC以及FPGA板(V6)。在FPGA中我們?cè)O(shè)計(jì)了用于陀螺閉環(huán)頻率控制的PLL環(huán)路、用于陀螺閉環(huán)幅度控制的AGC環(huán)路以及用于驅(qū)動(dòng)激勵(lì)信號(hào)輸出的高精度ΣΔ-DAC。

圖6所示是陀螺驅(qū)動(dòng)閉環(huán)系統(tǒng)驅(qū)動(dòng)檢測(cè)電壓的Simulink仿真波形圖,圖7所示是陀螺驅(qū)動(dòng)閉環(huán)系統(tǒng)驅(qū)動(dòng)檢測(cè)電壓的測(cè)試結(jié)果圖。對(duì)比兩個(gè)波形圖可知,陀螺驅(qū)動(dòng)閉環(huán)測(cè)試結(jié)果與仿真結(jié)果基本吻合,陀螺上電啟動(dòng)時(shí)間大約為0.6s。圖8所示是陀螺驅(qū)動(dòng)檢測(cè)電壓幅值采集結(jié)果,數(shù)據(jù)來源于FPGA中的幅值檢測(cè)模塊輸出,并乘以ADC的基準(zhǔn)電壓(大約3V)得到圖中電壓數(shù)據(jù)。分析圖中數(shù)據(jù)可知,陀螺驅(qū)動(dòng)檢測(cè)電壓幅值穩(wěn)定在0.992 7V,幅值信號(hào)變化的標(biāo)準(zhǔn)方差為8.156×10-6V,驅(qū)動(dòng)幅值相對(duì)變化率小于10×106。圖9所示是在190Hz采樣率下陀螺零偏輸出,由圖9可知陀螺零偏穩(wěn)定性達(dá)到10.448 °/hr。

圖6 陀螺驅(qū)動(dòng)檢測(cè)電壓仿真波形圖

圖7 陀螺驅(qū)動(dòng)檢測(cè)電壓測(cè)試波形圖

圖8 陀螺驅(qū)動(dòng)檢測(cè)電壓幅值

圖9 陀螺零偏輸出

6 結(jié)論

為了保證陀螺結(jié)構(gòu)驅(qū)動(dòng)模態(tài)的穩(wěn)定工作,本文利用MATLAB/Simulink軟件設(shè)計(jì)了一種數(shù)字式的陀螺驅(qū)動(dòng)閉環(huán)控制系統(tǒng)。該閉環(huán)控制系統(tǒng)利用數(shù)字鎖相環(huán)PLL來實(shí)現(xiàn)頻率和相位的自動(dòng)跟蹤控制,同時(shí)利用自動(dòng)增益控制AGC來實(shí)現(xiàn)幅度的自動(dòng)調(diào)整控制。最后在基于FPGA的驗(yàn)證平臺(tái)上驗(yàn)證了該數(shù)字驅(qū)動(dòng)閉環(huán)控制系統(tǒng)的有效性。實(shí)驗(yàn)結(jié)果表明在該數(shù)字系統(tǒng)的控制下陀螺驅(qū)動(dòng)起振時(shí)間大約為0.6 s,驅(qū)動(dòng)幅值穩(wěn)定性小于10×106,陀螺零偏穩(wěn)定性達(dá)到10.448 °/hr。

[1] Liu Dachuan,Lu N N,Cui J,et al. Digital Closed-Loop Control Based on Adaptive Filter for Drive Mode of a MEMS Gyroscope[J]. IEEE Sensors,2010,143(2):1722-1726.

[2] Wang Xiaolei,Li Hongsheng,Yang Bo. Design and Implementation of Digital Closed-Loop Drive Control System of a MEMS Gyroscope[J]. Journal of Southeast University,2012,28(1):35-40.

[3] 譚曉昀,雷龍剛,王冠石.電容式微機(jī)械陀螺雙環(huán)路閉環(huán)驅(qū)動(dòng)電路研究[J]. 傳感技術(shù)學(xué)報(bào),2010,23(10):1449-1453.

[4] 王展飛,魯文高,李峰,等. MEMS振動(dòng)陀螺閉環(huán)自激驅(qū)動(dòng)的理論分析及數(shù)值仿真[J]. 傳感技術(shù)學(xué)報(bào),2008,21(8):1337-1342.

[5] Fang Ran,Lu Wengao,Wang Guannan,et al. A Low-Noise High-Linearity Interface ASIC for MEMS Gyroscopes[J]. Journal of Semiconductors,2013,34(12):1-6.

[6] Dennis Kim,Robert M’Closkey. Noise Analysis of Closed-Loop Vibratory Rate Gyros[J]. American Control Conference,2012,50(6):92-97.

[7] Chen Fang,Yuan Weizheng,Chang Honglong,et al. Design and Implementation of an Optimized Double Closed-Loop Control System for MEMS Vibratory Gyroscope[J]. IEEE Sensors Journal,2014,14(1):184-196.

[8] Ismail A,George B,Elmallah A,et al. A High Performance Mems Based Digital-Output Gyroscope[J]. IEEE Transducers,2013,16(20):2523-2526.

[9] 孫茜,劉建業(yè),李榮冰. 基于混合控制的閉環(huán)MEMS陀螺建模與仿真[J]. 傳感器與微系統(tǒng),2009,28(1):36-39.

[10] 薛定宇,陳陽泉. 基于MATLAB/Simulink的系統(tǒng)仿真技術(shù)與應(yīng)用[M]. 北京:清華大學(xué)出版社,2002:350-366.

[11] 盧海曦,王壽榮,周百令.微機(jī)械陀螺儀反饋控制器魯棒性分析及閉環(huán)接口檢測(cè)電路設(shè)計(jì)[J]. 東南大學(xué)學(xué)報(bào),2011,41(3):526-532.

[12] 李海濤,蘇中. 鐘形振子式角速率陀螺驅(qū)動(dòng)控制技術(shù)研究[J]. 傳感技術(shù)學(xué)報(bào),2015,28(1):28-33.

童紫平(1985-),男,碩士,現(xiàn)為華東光電集成器件研究所工程師,主要研究方向?yàn)閿?shù)?；旌霞呻娐吩O(shè)計(jì),蘇州市高新區(qū)科技城龍山路89號(hào)中國兵器,215163,tzp117350@163.com。

Design of High Precision Digital Closed-Loop Drive Control System of MEMS Gyroscope

TONGZiping*,ZHANGHuanyang,QIAOWei,HEKejun,ZHANGZiqian,LONGShanli

(East China Institude of Photo-Electron IC,Suzhou 215163,China)

A digital closed-loop drive control system is designed for improving the detection sensitivity of MEMS Gyroscope. The system adopts a digital phase-locked loop(PLL)to track the resonant frequency and phase of the gyroscope. And a digital automatic gain control(AGC)method is utilized to control the amplitude constant. All algorithms are simulated by MATLAB/Simulink and tested on a high performance core Field Programmable Gate Array(FPGA)chip. The experiment results indicate that the startup time of the gyroscope is about 0.6 s,the drive amplitude stability is less than 10×106and the velocity bias stability is 10.448 °/hr.

MEMS Gyroscope;Digital closed-loop drive;FPGA;PLL;AGC;MATLAB/Simulink

2016-08-22 修改日期:2016-11-10

TN453

A

1004-1699(2017)03-0397-05

C:1265;7230M

10.3969/j.issn.1004-1699.2017.03.011