線性加速度計(jì)在壓電陀螺卡爾曼濾波技術(shù)中的應(yīng)用

林旻序，汪永陽(yáng)，戴 明，喬彥峰

(1.中國(guó)科學(xué)院長(zhǎng)春光學(xué)精密機(jī)械與物理研究所，吉林長(zhǎng)春130033;2.中國(guó)科學(xué)院 研究生院，北京100039)

1 引言

光電穩(wěn)定平臺(tái)作為偵察、測(cè)量、校射和打擊效果評(píng)估的重要技術(shù)支撐，其視軸穩(wěn)定精度決定了整個(gè)系統(tǒng)性能的好壞。而提高視軸穩(wěn)定精度，必須對(duì)慣性速率反饋元件陀螺的噪聲加以約束。目前小型光電穩(wěn)定平臺(tái)常采用壓電陀螺作為慣性傳感器，由于壓電陀螺隨機(jī)游走噪聲較大，通常采用二階或高階巴特沃思低通濾波對(duì)其進(jìn)行濾波，雖然這種方法能夠?qū)Ω哳l噪聲起到很好的抑制作用，但同時(shí)會(huì)產(chǎn)生陀螺低頻信號(hào)的相位延遲，從而影響陀螺信號(hào)帶寬［1］。

本文在設(shè)定壓電陀螺速率狀態(tài)變量的基礎(chǔ)上，運(yùn)用卡爾曼濾波理論建立狀態(tài)遞推、狀態(tài)估計(jì)以及濾波增益等方程，從而完成對(duì)壓電陀螺信號(hào)的濾波。由于在狀態(tài)遞推的過程中采用了壓電陀螺的角加速度信號(hào)，從而引入系統(tǒng)噪聲，因此濾波后陀螺信號(hào)的噪聲水平和帶寬在很大程度上依賴于加速度信號(hào)。

目前慣性角加速率的測(cè)量方法較少，一般采用CJ4、CJ6兩款角加速度陀螺，因?yàn)槠洳捎脡弘娡勇萁撬俾什罘衷?，從而帶寬上限只能達(dá)到50 Hz，無法保證卡爾曼濾波后陀螺的相位延遲。本文通過在光電平臺(tái)內(nèi)框架跨軸安裝3個(gè)線性加速度計(jì)間接測(cè)量平臺(tái)的慣性角加速度，通過對(duì)線性加速度計(jì)的選型，很好地控制了卡爾曼濾波后陀螺信號(hào)的噪聲和帶寬。

2 壓電陀螺噪聲特性

為了分析壓電陀螺測(cè)量信號(hào)的噪聲特性，取平臺(tái)靜止不動(dòng)時(shí)的測(cè)量信號(hào)(零位測(cè)量信號(hào))序列數(shù)據(jù)為研究對(duì)象，圖1為壓電陀螺零位測(cè)量信號(hào)曲線。陀螺噪聲隨機(jī)過程是各態(tài)歷經(jīng)的隨機(jī)過程，因此對(duì)噪聲特性的分析，必須對(duì)其進(jìn)行傅里葉變換得到其功率譜，隨后根據(jù)噪聲頻率域的分布情況對(duì)其進(jìn)行濾波。

圖1 壓電陀螺零位測(cè)量信號(hào)曲線Fig.1 Zero-position test signal curve of piezoelectric gyro

圖2 壓電陀螺噪聲功率譜Fig.2 Noise power spectrum of piezoelectric gyro

圖3 經(jīng)50 Hz巴特沃思低通濾波后的壓電陀螺零位信號(hào)曲線Fig.3 Zero-position test signal curve of piezoelecric gyro filted with 50 Hz Butterworth

壓電陀螺噪聲功率譜如圖2所示，可以看出噪聲功率譜密度值在低頻段較為突出，若采用巴特沃思低通濾波或均值濾波，只能消除噪聲的高頻段信息，對(duì)壓電陀螺噪聲不能起到明顯的抑制作用，而采用二階或高階低通濾波器，極易引起壓電陀螺信號(hào)的相位延遲，從而影響整個(gè)伺服系統(tǒng)的控制帶寬，圖3為采用50 Hz二階巴特沃思低通濾波器濾波后的壓電陀螺零位測(cè)量信號(hào)［2-3］。

3 卡爾曼濾波器的應(yīng)用

以離散系統(tǒng)為例，設(shè)tk時(shí)刻的被估計(jì)狀態(tài)Xk受系統(tǒng)驅(qū)動(dòng)噪聲序列Wk-1驅(qū)動(dòng)，驅(qū)動(dòng)機(jī)理由下述狀態(tài)方程描述［4］：

對(duì)Xk的量測(cè)滿足線性關(guān)系，量測(cè)方程為：

式中：Φk/k-1為tk-1時(shí)刻至tk時(shí)刻的一步轉(zhuǎn)移矩陣，Γk-1為系統(tǒng)噪聲驅(qū)動(dòng)矩陣，Hk為量測(cè)矩陣，Vk為量測(cè)噪聲序列，Wk為系統(tǒng)激勵(lì)噪聲序列。同時(shí)，Wk和Vk滿足［5］：

式中：Qk為系統(tǒng)噪聲序列的方差陣，假設(shè)為非負(fù)定矩陣;Rk為系統(tǒng)測(cè)量序列的方差陣，假設(shè)為正定矩陣。

狀態(tài)一步預(yù)測(cè)［6］：

狀態(tài)估計(jì)：

濾波增益：

一步預(yù)測(cè)均方誤差：

估計(jì)均方誤差：

卡爾曼濾波過程如圖4所示。

圖4 卡爾曼濾波的兩個(gè)計(jì)算回路和兩個(gè)更新回路Fig.4 Two calculating and updating loops in Kalman filtering method

圖中狀態(tài)變量Xk為k時(shí)刻所估計(jì)的壓電陀螺速度;Φ=1;Uk為采樣周期T;Zk為壓電陀螺儀在k時(shí)刻速度的測(cè)量量;Hk=1;Rk為陀螺自身的噪聲方差;Qk為加速度計(jì)自身的噪聲方差。

采用卡爾曼濾波器濾除壓電陀螺噪聲，必須先獲得平臺(tái)的角加速度a以及角加速度測(cè)量噪聲方差Wk。

4 線性加速度計(jì)測(cè)量角加速度原理

圖5為采用一對(duì)線加速度計(jì)測(cè)量平臺(tái)角加速度的原理示意圖，平臺(tái)的轉(zhuǎn)軸垂直于紙面，一對(duì)線加速度計(jì)A1和A2同向平行地安裝于平臺(tái)上，敏感軸間距為L(zhǎng)。

設(shè)線加速度計(jì)測(cè)得的線加速度為a1(t)和a2(t)，則平臺(tái)的慣性角加速度a可由下式表示：

從圖5可以看出，不論平臺(tái)的轉(zhuǎn)軸在兩個(gè)加速度計(jì)的連線上，還是在兩個(gè)加速度計(jì)連線的延長(zhǎng)線上，或是根本不在兩個(gè)加速度計(jì)的延長(zhǎng)線上，式(11)均成立。

圖5 線加速度計(jì)測(cè)量角加速度示意圖Fig.5 Schematic of angle acceleration measured with linear accelerometer

式(12)說明加速度計(jì)的安裝位置不影響測(cè)量結(jié)果，所以實(shí)際系統(tǒng)中可以根據(jù)安裝空間的位置任意安裝加速度計(jì)。根據(jù)這一原理，測(cè)量互相垂直的兩個(gè)軸向的角加速度，只需要3個(gè)線加速度計(jì)，如圖6所示。

圖6 3個(gè)線加速度計(jì)測(cè)量?jī)奢S角加速度示意圖Fig.6 Schematic of two-axis angle acceleration measured with three linear accelerometers

3個(gè)線加速度計(jì)呈三角形位置，設(shè)加速度計(jì)A1，A2，A3的輸出分別為a1(t)，a2(t)和a3(t)，則兩個(gè)方向的角加速度為［4］：

利用線性加速度計(jì)得到平臺(tái)的角加速度后，可以從角加速序列中統(tǒng)計(jì)得到Qk。

圖7(a)為壓電陀螺的原始噪聲曲線，圖7(b)為卡爾曼濾波后的壓電陀螺噪聲曲線，圖中縱坐標(biāo)為0.003 05(°)/s。實(shí)際計(jì)算時(shí)。壓電陀螺噪聲方差Rk=35，加速度計(jì)噪聲方差Qk=0.3;T=0.001 25 s。

圖7 壓電陀螺噪聲曲線Fig.7 Noise curves of piezoelectric gyro

可以看出若加速度計(jì)的噪聲方差Qk=0.3，那么其噪聲的標(biāo)準(zhǔn)差 σk= ■0.3≈055，則由于加速度計(jì)所引入的噪聲水平最大值為3×σk×T≈

(°)/s，從而 0.002(°)/s作為卡爾曼濾波過程中的系統(tǒng)噪聲，最大程度地影響著卡爾曼濾波的效果，實(shí)驗(yàn)中采用CF-1型線加速度計(jì)，標(biāo)稱帶寬為100 Hz，噪聲均方根值為300 μg(考慮 AD采樣的影響)。

通過對(duì)圖7(a)和圖7(b)分別求功率譜變換曲線，可以得到圖7(a)壓電陀螺隨機(jī)游走噪聲水平為0.005(°)·s-1/ ■Hz，圖7(b)卡爾曼濾波后壓電陀螺隨機(jī)游走噪聲水平為0.001 25(°)·s-1/ ■Hz，圖3中經(jīng)50 Hz二階巴特沃思低通濾波器濾波后的隨機(jī)游走噪聲水平為0.002(°)·s-1/■Hz。

雖然采用線性加速度計(jì)進(jìn)行卡爾曼濾波后壓電陀螺的噪聲水平約為采用巴特沃思濾波器的2/3，但采用基于線性加速度計(jì)的卡爾曼濾波器在相位延時(shí)上卻明顯優(yōu)于巴特沃思低通濾波器。圖8和圖9分別給出了開環(huán)2 Hz和15 Hz的情況下，采用卡爾曼濾波和巴特沃思低通濾波器的壓電陀螺反饋曲線，其中實(shí)線為采用卡爾曼濾波器后的反饋曲線，虛線為采用50 Hz二階巴特沃思濾波器濾波后的反饋曲線。

圖8 2 Hz壓電陀螺開環(huán)曲線圖Fig.8 Curves of piezoelectric gyro in open loop of 2 Hz

圖9 15 Hz壓電陀螺開環(huán)曲線圖Fig.9 Curves of piezoelectric gyro in open loop of 15 Hz

通過圖8(b)兩條曲線峰值的延時(shí)可以看出，2 Hz時(shí)卡爾曼濾波器的相位超前二階巴特沃思濾波器3 ms，等效于(3/500)×360=2.16°。通過圖9(b)兩條曲線峰值的延時(shí)可以看出，15 Hz時(shí)卡爾曼濾波器的相位超前二階巴特沃思濾波器8 ms，等效于(8/66.67) ×360=43°。因此隨著頻率的升高，巴特沃思濾波所帶來的延遲也會(huì)逐漸加大，而卡爾曼濾波則保持很好的相位特性。

圖10給出了原始噪聲、采用CF-1加速度計(jì)濾波后的噪聲以及采用更高精度線性加速度計(jì)濾波后的噪聲對(duì)穩(wěn)定精度的影響。

圖10 壓電陀螺噪聲對(duì)穩(wěn)定精度的影響Fig.10 Influence of piezoelectric gyro noise on stabilization accuracy

5 結(jié)論

針對(duì)目前小型光電穩(wěn)定平臺(tái)上廣泛應(yīng)用的壓電陀螺隨機(jī)游走噪聲大的問題，提出采用基于線性加速度計(jì)的卡爾曼濾波技術(shù)對(duì)其進(jìn)行信號(hào)濾波。結(jié)果表明：采用卡爾曼濾波能夠在保持壓電陀螺帶寬(最小相位延遲)的前提下，依靠高精度線性加速度計(jì)，充分消除原始陀螺信號(hào)中的白噪聲。

［1］ MYUNG H，LEE H K，CHOI K，et al..Constrained Kalman filter for mobile robot localization with gyroscope［C］.Intelligent Robots and Systems，2006 IEEE/RSJ International Conference on，Beijing，China，9-15 Oct.，2006：442-447.

［2］ 畢永利.多框架光電平臺(tái)控制系統(tǒng)研究［D］.北京：中國(guó)科學(xué)院研究生院，2003.BI Y L.Study on control system of multi-frame photoelectric platform［D］.Beijing：Graduate University of Chinese Academy of Sciences，2003.(in Chinese)

［3］ ALMAGBILE A，WANG J L，DING W D.Evaluating the performances of adaptive Kalman filter methods in GPS/INS integration［J］.J．Global Positioning Systems，2010(9)：33-40.

［4］ 王江安，莊奕琪，周淇軍，等.利用改進(jìn)卡爾曼濾波算法抑制GPS接收相位噪聲［J］.數(shù)據(jù)采集與處理，2010(5)：611-614.WANG J A，ZHUANG Y Q，ZHOU Q J，et al..The application of Kalman filter in the suppression of GPS receiving phase noise［J］.Data Acquisition and Processing，2010(5)：611-614.(in Chinese)

［5］ 潘博文.卡爾曼濾波算法在相對(duì)導(dǎo)航中的應(yīng)用［J］.現(xiàn)代導(dǎo)航，2010(4)：15-18.PAN B W.The application of Kalman filter in the relative navigation［J］.Modern Navigation，2010(4)：15-18.(in Chinese)

［6］ 趙延.機(jī)動(dòng)目標(biāo)自適應(yīng)卡爾曼濾波算法研究［D］.西安：電子科技大學(xué)，2004.ZHAO Y.Study on adaptive Kalman filter for the maneuvering target［D］.Xi'an：University of Electronic Science and Technology of China，2004.(in Chinese)