宰德廣, 周 崗, 陳永冰, 李文魁

(海軍工程大學(xué) 導(dǎo)航工程系,湖北 武漢 430033)

0 引 言

目前微機(jī)電系統(tǒng)(micro-electro-mechanical system,MEMS)陀螺儀的應(yīng)用越來越普遍,關(guān)于如何提高陀螺儀的測量精度也越來越引起人們的關(guān)注。通常是對(duì)陀螺儀的隨機(jī)噪聲進(jìn)行建模分析,比如利用時(shí)間序列分析的方法對(duì)噪聲建立ARMA模型,獲得相應(yīng)的參數(shù)并設(shè)計(jì)Kalman濾波器來濾除噪聲[1～3]。但對(duì)于低成本MEMS陀螺儀來說,造成其誤差的因素復(fù)雜,隨機(jī)性較大,很難建立準(zhǔn)確的、具有普遍適用性的數(shù)學(xué)模型[4,5]。實(shí)驗(yàn)室現(xiàn)有一船舶回轉(zhuǎn)運(yùn)動(dòng)模擬測量系統(tǒng),其工作原理是通過驅(qū)動(dòng)馬達(dá)轉(zhuǎn)動(dòng)來模擬船舶回轉(zhuǎn)運(yùn)動(dòng),同時(shí)采用MEMS角速率陀螺儀敏感轉(zhuǎn)臺(tái)轉(zhuǎn)動(dòng)測得角速率(即實(shí)驗(yàn)室條件下模擬出的船舶回轉(zhuǎn)率),但測量結(jié)果不穩(wěn)定,數(shù)據(jù)波動(dòng)較大,需要對(duì)測量數(shù)據(jù)進(jìn)行濾波處理,以獲得較為穩(wěn)定的輸出,實(shí)現(xiàn)進(jìn)一步的船舶操縱控制。

本文從實(shí)際應(yīng)用考慮,對(duì)測量數(shù)據(jù)直接進(jìn)行濾波處理。

首先通過平均濾波和校正零偏的方法對(duì)原始采集信號(hào)進(jìn)行預(yù)處理,之后設(shè)計(jì)了Kalman濾波器對(duì)信號(hào)進(jìn)一步濾波,并借助船舶回轉(zhuǎn)運(yùn)動(dòng)模擬測量系統(tǒng),分別在靜態(tài)、穩(wěn)定回轉(zhuǎn)率和回轉(zhuǎn)率發(fā)生改變的情況下驗(yàn)證Kalman濾波器的效果,針對(duì)其動(dòng)態(tài)響應(yīng)較差的問題,在原有濾波器的基礎(chǔ)上改進(jìn)并設(shè)計(jì)了量測噪聲方差自適應(yīng)的Kalman 濾波器[6],同樣情況下對(duì)其驗(yàn)證,以檢驗(yàn)所設(shè)計(jì)的濾波器是否有效可行。

1 數(shù)據(jù)預(yù)處理

本文采用ADXRS系列的MEMS角速率陀螺儀,通過改變船舶回轉(zhuǎn)運(yùn)動(dòng)模擬測量系統(tǒng)中自動(dòng)操舵儀的舵角,即可改變回轉(zhuǎn)率,使MEMS陀螺儀工作在不同狀態(tài)。為保證測量準(zhǔn)確性,每次測量前先通電工作一段時(shí)間,待穩(wěn)定后再采集所需數(shù)據(jù),而采集到的原始數(shù)據(jù)需要先經(jīng)過平均濾波和校正零偏等預(yù)處理,以便于更好地進(jìn)行Kalman濾波處理。

1.1 平均濾波去除野值

實(shí)驗(yàn)發(fā)現(xiàn)采集到的信號(hào)中往往存在一些數(shù)值大小異常的野值,在進(jìn)行Kalman 濾波前需要對(duì)原始的采集信號(hào)預(yù)處理,剔除野值。

其中求均值公式如下

(1)

1.2 零偏校正

觀察還發(fā)現(xiàn),MEMS陀螺儀測量數(shù)據(jù)存在常值漂移的現(xiàn)象,可通過校正零偏的方法補(bǔ)償陀螺儀漂移。將系統(tǒng)回轉(zhuǎn)率設(shè)定為零,使MEMS陀螺儀工作在零輸入狀態(tài)。通過采集多組靜態(tài)數(shù)據(jù),并通過求均值的方法獲得偏移值,此即陀螺儀的零偏,在之后的數(shù)據(jù)采集和處理中,都需要減去此零偏值以完成零偏校正。

2 Kalman濾波器設(shè)計(jì)

因?yàn)镸EMS陀螺儀測得的是轉(zhuǎn)臺(tái)轉(zhuǎn)動(dòng)的角速率,故以角速率為狀態(tài)量,以實(shí)際測量值為觀測量,且系統(tǒng)噪聲W和量測噪聲V均為零均值高斯白噪聲,系統(tǒng)噪聲、量測噪聲和被估計(jì)的角速率狀態(tài)量三者互不相關(guān),則有如下離散Kalman濾波狀態(tài)方程和量測方程

(2)

式中X(k)為被估計(jì)的角速率狀態(tài)量,Z(k)為角速率量測量,W(k)為系統(tǒng)噪聲,其協(xié)方差矩陣為Q,V(k)為量測噪聲,其協(xié)方差矩陣為R,A，B，H分別為狀態(tài)轉(zhuǎn)移矩陣、系統(tǒng)噪聲驅(qū)動(dòng)陣和量測陣。

Kalman濾波方程實(shí)質(zhì)為遞推公式,可分為2個(gè)預(yù)測方程和3個(gè)更新方程,如式(3)所示

(3)

設(shè)系統(tǒng)為一維狀態(tài)系統(tǒng),被估計(jì)狀態(tài)量只有角速率,A,B,H分別取為單位陣,實(shí)驗(yàn)中只要設(shè)定濾波估計(jì)初值XKF(0)和協(xié)方差陣初值P(0),并給出系統(tǒng)噪聲陣Q和量測噪聲陣R,即可進(jìn)行方程的遞推和更新,實(shí)現(xiàn)Kalman濾波。

3 濾波器性能檢驗(yàn)

對(duì)于固定的測量系統(tǒng)其系統(tǒng)噪聲較為穩(wěn)定,由經(jīng)驗(yàn)值設(shè)定系統(tǒng)噪聲陣Q為0.01。實(shí)際測得的角速率即為量測量Z(k),其協(xié)方差作為量測噪聲陣R,其均方差的10倍設(shè)為協(xié)方差陣初值P(0)。設(shè)定好濾波器參數(shù)后,即可通過船舶回轉(zhuǎn)運(yùn)動(dòng)模擬測量系統(tǒng)設(shè)定靜止、恒定回轉(zhuǎn)率和回轉(zhuǎn)率變化三種狀態(tài),對(duì)設(shè)計(jì)的Kalman 濾波器進(jìn)行性能檢驗(yàn)。

3.1 靜態(tài)測試

由上所述可知,MEMS陀螺儀需通電工作一段時(shí)間再進(jìn)行數(shù)據(jù)的采集?，F(xiàn)設(shè)定船舶回轉(zhuǎn)運(yùn)動(dòng)模擬測量系統(tǒng)的回轉(zhuǎn)率為零,使MEMS陀螺儀處于靜止?fàn)顟B(tài),采集10 min原始數(shù)據(jù),并對(duì)其進(jìn)行數(shù)據(jù)預(yù)處理和Kalman濾波處理,結(jié)果如圖1(a)～(c)所示。

圖1 靜態(tài)數(shù)據(jù)濾波處理前后對(duì)比

從圖1的對(duì)比圖可看出,原始測量值存在個(gè)別野值,且整體數(shù)據(jù)出現(xiàn)偏移,而經(jīng)過預(yù)處理之后的數(shù)據(jù)零偏現(xiàn)象明顯改善,野值信號(hào)被濾除,再次經(jīng)過Kalman濾波處理后,信號(hào)降噪效果明顯,數(shù)據(jù)波動(dòng)很小。計(jì)算每次數(shù)據(jù)處理后的誤差均值和誤差標(biāo)準(zhǔn)差,結(jié)果如表1所示。

表1 靜態(tài)測試時(shí)數(shù)據(jù)統(tǒng)計(jì)特性

由表1可看出,經(jīng)過數(shù)據(jù)預(yù)處理后的信號(hào)其誤差均值和標(biāo)準(zhǔn)差都有所降低,再次經(jīng)過Kalman濾波處理后更是降低一個(gè)量級(jí)之多,實(shí)驗(yàn)表明Kalman濾波器在靜態(tài)測試時(shí)濾波效果顯著。

3.2 恒定回轉(zhuǎn)率測試

設(shè)定船舶回轉(zhuǎn)運(yùn)動(dòng)模擬測量系統(tǒng)的回轉(zhuǎn)率為30°/min,采集10 min穩(wěn)定數(shù)據(jù)進(jìn)行Kalman 濾波處理,結(jié)果如圖2所示,計(jì)算可知,經(jīng)Kalman濾波后數(shù)據(jù)誤差均值由-3.776°/min降至0.243°/min,而誤差標(biāo)準(zhǔn)差由2.344°/min降至0.259°/min,可以看出,在回轉(zhuǎn)率恒定情況下,設(shè)計(jì)的Kalman濾波器依然具有很好的濾波效果。

圖2 恒定回轉(zhuǎn)率時(shí)Kalman濾波前后對(duì)比

3.3 回轉(zhuǎn)率變化測試

設(shè)定船舶回轉(zhuǎn)運(yùn)動(dòng)模擬測量系統(tǒng)開始時(shí)處于零回轉(zhuǎn)率狀態(tài),在100 s時(shí)刻改變舵角,使得回轉(zhuǎn)率發(fā)生變化,采集這一時(shí)間段內(nèi)的測量數(shù)據(jù),進(jìn)行Kalman 濾波處理得到結(jié)果如圖3所示。為便于對(duì)比突出Kalman 動(dòng)態(tài)濾波效果,只給出了經(jīng)過預(yù)處理之后的數(shù)據(jù)曲線和Kalman濾波曲線與真值的對(duì)比圖。

圖3 回轉(zhuǎn)率變化時(shí)的Kalman濾波前后對(duì)比

從圖中可看出當(dāng)船舶回轉(zhuǎn)率發(fā)生改變時(shí),Kalman濾波動(dòng)態(tài)性能變差,出現(xiàn)滯后現(xiàn)象,所設(shè)計(jì)的Kalman濾波器不再適用,必須對(duì)此濾波器進(jìn)行改進(jìn),提高其動(dòng)態(tài)響應(yīng)性能。

4 改進(jìn)后的自適應(yīng)Kalman濾波器

關(guān)于如何提高Kalman濾波器的動(dòng)態(tài)性能,很多學(xué)者提出了各種自適應(yīng)Kalman濾波算法,比如過程噪聲自適應(yīng)、最小二乘法估計(jì)、設(shè)定指數(shù)漸消因子、加權(quán)Sage-Husa自適應(yīng)濾波等[7～10]?？芍狵alman濾波器中系統(tǒng)噪聲陣Q和量測噪聲陣R與濾波性能緊密相關(guān),一般Q越大R越小系統(tǒng)動(dòng)態(tài)性能越好,相反則穩(wěn)態(tài)濾波效果越好。對(duì)于確定的測量系統(tǒng)其系統(tǒng)噪聲較為穩(wěn)定,而角速率測量值很大程度上受量測噪聲影響,故可采用固定Q陣,改變R值的方法,實(shí)現(xiàn)自適應(yīng)濾波。參考文獻(xiàn)[6],可針對(duì)本實(shí)驗(yàn)設(shè)計(jì)量測噪聲陣R自適應(yīng)調(diào)節(jié)的Kalman濾波方法,具體算法流程如圖4所示。

圖4 自適應(yīng)Kalman濾波算法流程

可知Kalman濾波方程可分為預(yù)測方程與更新方程,實(shí)現(xiàn)自適應(yīng)的目的就是根據(jù)量測噪聲變化趨勢自適應(yīng)改變?yōu)V波增益,以調(diào)節(jié)下一時(shí)刻的預(yù)測值和濾波估計(jì)值,而改變量測噪聲陣即可達(dá)到改變?yōu)V波增益的目的。本算法思想是對(duì)每一時(shí)刻的狀態(tài)預(yù)測值Xpre(k)與上一時(shí)刻的值Xpre(k-1)作差并取絕對(duì)值,根據(jù)差值大小判斷狀態(tài)量是否發(fā)生機(jī)動(dòng)性變化,以作出相應(yīng)地調(diào)整。如圖4所示,d為設(shè)定的基準(zhǔn)值,若兩個(gè)時(shí)刻的預(yù)測值差值大于此基準(zhǔn)值,則說明角速率發(fā)生改變,需要提高濾波的動(dòng)態(tài)性能,R則需要相應(yīng)減小,即Rg=αLR(0<αL<1)；若兩個(gè)時(shí)刻的預(yù)測值差值小于此基準(zhǔn)值,則說明角速率變化穩(wěn)定,需要提高穩(wěn)態(tài)濾波效果,使濾波更加穩(wěn)定,R則需要相應(yīng)增大,即Rg=αHR(αH>1)。量測噪聲Rg更新后,濾波增益Kg也隨之更新,即可實(shí)現(xiàn)Kalman濾波方程自適應(yīng)調(diào)整,原來的濾波方程改進(jìn)如下

(4)

設(shè)置實(shí)驗(yàn)參數(shù)時(shí)將系統(tǒng)噪聲陣Q適當(dāng)減小,量測噪聲陣R適當(dāng)增大,以便之后對(duì)R自適應(yīng)調(diào)整。在實(shí)驗(yàn)過程中基準(zhǔn)值d和調(diào)節(jié)因子αL,αH都需要根據(jù)實(shí)際情況靈活調(diào)整,以適應(yīng)該濾波模型。經(jīng)多次實(shí)驗(yàn)選定d=2,αL=0.05,αH=20。

首先對(duì)自適應(yīng)Kalman濾波器進(jìn)行同樣的靜態(tài)和恒定回轉(zhuǎn)率測試,濾波后數(shù)據(jù)誤差均值和誤差標(biāo)準(zhǔn)差如表2所示,可以看出設(shè)計(jì)的自適應(yīng)Kalman濾波器在靜態(tài)和恒定回轉(zhuǎn)率狀態(tài)下也可以達(dá)到很好的濾波效果。

表2 自適應(yīng)Kalman濾波數(shù)據(jù)統(tǒng)計(jì)特性

同樣地,對(duì)自適應(yīng)Kalman濾波器進(jìn)行動(dòng)態(tài)變化測試,為不失一般性,重新采集了一組數(shù)據(jù),通過自動(dòng)操舵儀在100 s時(shí)刻向右打舵使回轉(zhuǎn)率增大,又在350 s時(shí)刻向左打舵減小回轉(zhuǎn)率,使得回轉(zhuǎn)率的變化更加復(fù)雜,以更好地驗(yàn)證自適應(yīng)Kalman濾波器的動(dòng)態(tài)性能,曲線圖如圖5所示。

圖5 動(dòng)態(tài)變化時(shí)自適應(yīng)Kalman濾波效果

由圖5可以看出,當(dāng)角速率發(fā)生變化時(shí)濾波值能夠隨之變化,雖然在短時(shí)間內(nèi)出現(xiàn)滯后,但是在達(dá)到穩(wěn)態(tài)前很快作出響應(yīng)變化,幾乎與真值同時(shí)達(dá)到穩(wěn)態(tài),在船舶操縱控制系統(tǒng)中是可行的,而且在達(dá)到穩(wěn)態(tài)后的兩個(gè)階段濾波效果也不錯(cuò),計(jì)算誤差標(biāo)準(zhǔn)差可得,150～350 s時(shí)間內(nèi)由1.743°/min降0.266°/min,400～500 s時(shí)間內(nèi)由1.457°/min降至0.218°/min,可知在回轉(zhuǎn)率動(dòng)態(tài)變化的情況下自適應(yīng)Kalman濾波器是適用的。

5 結(jié) 論

實(shí)驗(yàn)分析表明:所設(shè)計(jì)的自適應(yīng)Kalman濾波器在達(dá)到濾波效果的同時(shí)又兼顧了較好的動(dòng)態(tài)性能,是有效可行的。經(jīng)過數(shù)據(jù)預(yù)處理和自適應(yīng)Kalman濾波處理,實(shí)驗(yàn)室中的船舶回轉(zhuǎn)運(yùn)動(dòng)模擬測量系統(tǒng)測得的角速率數(shù)據(jù)波動(dòng)得到了有效抑制,獲得了較穩(wěn)定的測量數(shù)據(jù),為下一步實(shí)現(xiàn)船舶操縱控制奠定了良好的基礎(chǔ)。