融合卡爾曼改進小波的姿態(tài)傳感器濾波方法

王靜靜，秦 剛，李騰

（西安工業(yè)大學 電子信息工程學院，西安 710021）

XGZT-XXX 型姿態(tài)傳感器為西安工業(yè)大學自主研發(fā)，由MEMS 加速度計、MEMS 陀螺儀與MEMS傾角傳感器組成，該傳感器具有體積小、成本低、功耗低等優(yōu)點。但由于MEMS 陀螺儀自身輸出噪聲大，且在實驗過程中受不可避免的各種復雜的振動、干擾激勵（如實驗過程中人員的走動、安放不穩(wěn)引起抖動等），為了提高系統(tǒng)測量精度，需要對其進行降噪處理。

對慣性器件濾波處理，有經(jīng)典濾波器和現(xiàn)代濾波器，慣性器件的噪聲與信號通常是相互重疊，不適合使用經(jīng)典濾波器（IIR、FIR），可根據(jù)信號和噪聲的統(tǒng)計特性，利用數(shù)理統(tǒng)計方法進行估計，即現(xiàn)代濾波器[1]。現(xiàn)代濾波器中常用的方法有卡爾曼濾波、經(jīng)驗模態(tài)分解（empirical mode decomposition，EMD）、小波閾值去噪等[2]。文獻[3]建立了時間序列模型引入卡爾曼進行濾波處理，但缺乏自適應性，長時間會導致濾波發(fā)散。文獻[4]中將卡爾曼與小波進行了融合，并且對卡爾曼進行了抗野值的優(yōu)化，但是計算量較大，過于復雜，對主控芯片有一定的要求。文獻[5]提出新的Garrote 型閾值函數(shù)，克服了軟硬閾值函數(shù)的不足，但是函數(shù)需調(diào)節(jié)的參數(shù)較多，難以掌控。文獻[6]提出將卡爾曼與小波模糊閾值進行了融合，但卻忽略了陀螺儀的低頻噪聲。針對以上問題，本文采用卡爾曼融合改進小波去噪方法，首先對原始數(shù)據(jù)進行預處理，建立MEMS 陀螺儀的時間序列模型，輸入卡爾曼濾波器進行首次濾波，然后使用改進的小波閾值去噪方法進行二次濾波，從而完成對陀螺儀信號的去噪處理。

1 基于ARMA 的卡爾曼濾波

1.1 數(shù)據(jù)預處理

為滿足時間序列模型平穩(wěn)、正態(tài)、零均值的建模要求，要對陀螺儀的原始數(shù)據(jù)進行預處理[7]。采用拉伊達準則法和最小二乘法進行異常值處理與去趨勢項處理。陀螺儀原始數(shù)據(jù)預處理后如圖1 所示。

圖1 預處理前后數(shù)據(jù)對比圖Fig.1 Data comparison before and after preprocessing

1.2 平穩(wěn)性及正態(tài)性檢驗

（1）平穩(wěn)性檢驗

利用采用ADF（augmented dickey-fuller test）準則檢驗序列的平穩(wěn)性[8]。使用Matlab 中的adftest 函數(shù)返回值為1，結(jié)果顯示該序列具有平穩(wěn)性。

（2）正態(tài)性檢驗

本文采用偏態(tài)系數(shù)ξ 和峰態(tài)v 系數(shù)來檢測樣本數(shù)據(jù)的正態(tài)性，公式如下：

當ξ≈0、v≈3 時，可以認為x（t）為正態(tài)序列，求得ξ=2.8281，v=0.0018，故該序列滿足建模要求。

1.3 ARMA 模型定階和參數(shù)估計

MEMS 陀螺儀的ARMA 模型階數(shù)一般不超過3階，采用赤池信息準則（akaike information criterion，AIC）和貝葉斯信息準則（bayesian information criterion，BIC）確定具體的階數(shù)[9]。利用自回歸逼近方法求得AIC 準則與BIC 準則檢驗結(jié)果如表1 所示。通過對比發(fā)現(xiàn)ARMA（1，1）的AIC 和BIC 值小于其他模型，故采用ARMA（1，1），其表達式為

表1 AIC 準則與BIC 準則檢驗結(jié)果Tab.1 Test results of AIC criteria and BIC criteria

采用多項式擬合得陀螺儀X 軸的輸出模型：

1.4 卡爾曼濾波器設(shè)計

離散型線性隨機系統(tǒng)的方程為

式中：Xk為系統(tǒng)狀態(tài)向量；Zk為測量序列；Φk，k-1為系統(tǒng)轉(zhuǎn)移矩陣；Γk，k-1為輸入矩陣；Hk為觀測矩陣；Wk和Vk為系統(tǒng)過程噪聲序列和觀測噪聲序列，為均值為零且相互獨立的高斯白噪聲，它們的協(xié)方差矩陣分別為Qk和Rk，滿足以下特性：

式中：δkj為克羅內(nèi)克爾（kronecker-δ）函數(shù)，其特性是：

則離散型卡爾曼濾波方程為

狀態(tài)一步預測方程：

狀態(tài)估計方程：

濾波增益矩陣：

預測誤差方差陣：

估計誤差方差陣：

通過以上公式可知，給定X0、P0，根據(jù)觀測值Zk即可隨時間循環(huán)更新。

2 小波閾值去噪

小波閾值去噪方法具有快速、簡單、可操作性高等優(yōu)良特性[10]?；驹恚哼x擇合適的小波基函數(shù)對原始信號進行分解得到一系列小波系數(shù)，通過閾值和閾值函數(shù)，將高于該閾值的小波系數(shù)進行保留或適當收縮后保留，低于該閾值的小波系數(shù)歸零，最后選擇不為零的小波系數(shù)重構(gòu)得到去噪后的信號。

2.1 小波閾值函數(shù)

（1）硬閾值

式中：λ 為閾值；ωj，k為去噪前小波系數(shù)；為去噪后小波系數(shù)。

（2）軟閾值

式中：sgn（）為符號函數(shù)。

硬閾值函數(shù)存在斷連、不連續(xù)等問題，導致濾波后信號出現(xiàn)震蕩。而軟閾值函數(shù)雖然連續(xù)，卻存在固定的偏差，會造成濾波不徹底的問題。

2.2 小波閾值

目前，常用的閾值估計方法有固定閾值、極大極小準則閾值、Stein 無偏似然閾值等。

（1）固定閾值

固定閾值λ 由Donoho 和Johnstone 提出[11]，表達式為

式中：σ 為噪聲標準差；N 為信號長度。

（2）Stein 無偏似然閾值

該方法原理是首先求得給定閾值的似然估計，然后將非似然的值進行最小化，對應的值就是所需要的閾值。

（3）極大極小準則閾值。

極大極小準則閾值是一種固定閾值，原理是產(chǎn)生一個最小均方誤差[11]。表達式如下：

固定閾值較為絕對，缺乏靈活性，存在“過扼殺”的問題；極大極小準則閾值過于保守，存在“過保留”的問題；無偏似然閾值對于低信噪比信號不適合[12]。

2.3 小波閾值去噪的改進方案

2.3.1 基于模糊閾值函數(shù)改進的小波閾值去噪

基于以上對常用的閾值函數(shù)和閾值的不足，本文提出通過在2 種經(jīng)典閾值和模糊隸屬度函數(shù)的基礎(chǔ)上設(shè)計閾值函數(shù)，對小于極大極小準則閾值的小波系數(shù)進行去除，大于固定值閾值的小波系數(shù)進行保留，在極大極小準則閾值與固定閾值之間的小波系數(shù)采用模糊隸屬度函數(shù)處理。本文采用升半嶺型模糊隸屬函數(shù)對信號進行去噪[13-14]，函數(shù)表達式為

式中：ωj，k為去噪前小波系數(shù)；為去噪后小波系數(shù)；λa為極大極小準則閾值；λb為固定閾值；μA（ωj，k）為模糊隸屬度函數(shù)。

2.3.2 基于自適應小波模糊閾值去噪

由于不同頻帶的噪聲分量的不同，故本文在模糊閾值函數(shù)的基礎(chǔ)上引入動態(tài)調(diào)節(jié)系數(shù)r，提出了一種改進的閾值計算公式：

式中：λa為極大極小準則閾值；λb為固定閾值。

在小波分析中，信號相關(guān)度較大的小波系數(shù)相對比較大，跟噪聲相關(guān)度較小的小波系數(shù)較小[15]。故根據(jù)不同的分解尺度，每一層都選擇相應的閾值，實現(xiàn)分層閾值動態(tài)變化。

式中：r 為動態(tài)調(diào)節(jié)系數(shù)；j 為分解層數(shù)。

2.3.3 基于LS 改進的小波閾值去噪

對于傳統(tǒng)小波閾值去噪過程中只處理高頻分量部分，從而忽略了陀螺儀的低頻噪聲的不足。本文將小波分解后的低頻部分采用最小二乘對陀螺儀低頻部分進行平滑濾波，保留原有的信號趨勢。

3 實驗驗證與結(jié)果分析

3.1 仿真實驗

在仿真實驗中，利用Matlab 軟件生成原始的測試正弦信號：x（t）＝2sin（2π f1t+cos（2π f2t））；加入不同的白噪聲序列n（t）[16]，得到不同信噪比的含噪信號：y（t）=x（t）+n（t）。選取4 dB 小波經(jīng)過3 尺度分解，不同的濾波方法對其進行處理，引入信噪比和均方差來反映濾波效果。對于含噪的信號，信噪比越大、均方根越小證明去噪效果越好。

如表2 所示為不同方案的信噪比與均方根，圖2 所示為信噪比為4 dB 的信號濾波結(jié)果，從實驗結(jié)果可以看出，在不同信噪比下，本文方案的信噪比均大于其他方法，均方誤差均小于其他方法。在信噪比為4 dB 下，改進的小波模糊閾值比小波模糊閾值去噪方法信噪比提高了5.143 dB，均方根減少了0.142。本文方案濾波后的信號更加還原原始信號。

表2 不同方案的信噪比與均方根Tab.2 SNR and RMSE of different schemes

圖2 信噪比為4 dB 的信號濾波結(jié)果Fig.2 Signal filtering results with 4 dB SNR

3.2 實測數(shù)據(jù)

將MEMS 陀螺儀靜置于經(jīng)緯儀控制實驗臺上保持水平，預熱30 min 后，進行數(shù)據(jù)采集，采樣頻率為200 Hz，選取連續(xù)的2000 個采樣點作為樣本數(shù)據(jù)，對其進行不同去噪方法實驗驗證。實驗采集平臺如圖3 所示。

圖3 經(jīng)緯儀實驗采集設(shè)備Fig.3 Theodolite experimental acquisition equipment

不同濾波方法的前后對比圖如圖4 所示，可知本方案處理后的信號減小了毛刺，振蕩更小，更加平滑。查閱文獻可知，Allan 方差雙對數(shù)圖可以在時間頻域上根據(jù)不同噪聲在雙對數(shù)Allan 方差曲線上的斜率不同來將各個噪聲進行區(qū)分，因此常用來識別陀螺儀的隨機噪聲分量[15]。故引入Allan 方差對濾波前后MEMS 陀螺儀噪聲進行辨識，結(jié)果如表3所示。

表3 Allan 方差辨識濾波前后隨機噪聲系數(shù)Tab.3 Allan variance identification random noise figure before and after filtering

圖4 不同濾波方法的前后對比圖Fig.4 Before and after comparison of different filtering methods

實驗結(jié)果表明，利用本方案濾波后的量化噪聲、角度隨機游走、零偏不穩(wěn)定性、角速率隨機游走及速率斜坡分別減少了63.17%、81.57%、87.59%、81.64%、77.80%，相比于卡爾曼濾波融合小波模糊閾值方法提高了3.82%、6.42%、22.14%、16.39%、10.59%。本文方案的濾波效果明顯減少陀螺儀的隨機噪聲，相比于傳統(tǒng)方法效果更佳。

4 結(jié)語

針對XGZT-XXX 型姿態(tài)傳感器輸出噪聲大、易受復雜環(huán)境干擾的問題，本文采用卡爾曼聯(lián)合改進的小波閾值方法進行去噪處理，引入Allan 方差、信噪比、均方根對陀螺儀的噪聲系數(shù)進行估計，通過仿真實驗與實測數(shù)據(jù)實驗結(jié)果表明，本文所改進的方法可以有效減小陀螺儀的噪聲，濾波效果優(yōu)于卡爾曼濾波和小波閾值方法，對提高XGZT-XXX 型姿態(tài)傳感器的穩(wěn)定性和抗干擾能力具有實踐意義。