基于自適應(yīng)KF 動態(tài)虛擬陀螺數(shù)據(jù)融合算法的研究

熊泰然，陳雯雯，陳宏宇，高海云，吉言超

（1.中國科學(xué)院 微小衛(wèi)星創(chuàng)新研究院，上海 201210；2.中國科學(xué)院大學(xué) 電子電氣與通信工程學(xué)院，北京 100049；3.上海科技大學(xué) 信息科學(xué)與技術(shù)學(xué)院，上海 201210）

0 引言

在慣性導(dǎo)航系統(tǒng)中，陀螺儀是測量移動物體角速率的關(guān)鍵傳感器，光纖陀螺被廣泛使用在傳統(tǒng)衛(wèi)星中。而衛(wèi)星的小型化逐漸成為一種發(fā)展趨勢，由于微機電系統(tǒng)（Micro-Electro-Mechanical System，MEMS）技術(shù)的快速發(fā)展，微小衛(wèi)星的技術(shù)升級出現(xiàn)了更多可能。MEMS 器件具備很多優(yōu)點，比如體積小、功耗低、成本低，更適合微小衛(wèi)星集成化、模塊化、研制周期短的設(shè)計要求。但是MEMS 傳感器的精度一般較低，無法滿足特定條件下的精度需求。

為了提高MEMS 陀螺儀的精度，目前有一些針對單個陀螺儀的濾波算法研究，比如使用神經(jīng)網(wǎng)絡(luò)等算法，算法普遍較為復(fù)雜，對于單個陀螺的處理也難以大幅提升精度。同時也有一些新型陀螺儀進行在軌驗證，比如半球諧振陀螺。BAYARD 等在2003 年最先提出了一種虛擬陀螺技術(shù)，該技術(shù)使用卡爾曼濾波（Kalman Filter，KF）算法，融合了4 個相同的MEMS 陀螺儀的數(shù)據(jù)，通過實驗證明了方法的可行性，但精度仍然有提升空間。國內(nèi)外也有一些在此基礎(chǔ)上進行的改進研究，比如改進濾波算法，增加陀螺數(shù)量，或者改用異方差模型建模等方法，算法由于模型參數(shù)的固定性，僅在靜態(tài)條件下或者僅在動態(tài)條件下能夠顯著提升精度，難以保證在靜態(tài)和動態(tài)條件下都有良好的性能。尤其是在角速率變化較快的情況下，卡爾曼濾波器的參數(shù)對濾波效果有較大影響。為了解決這個問題，采用多模型濾波算法動態(tài)調(diào)整是有效的解決方法，但是與KF 相比，仍然存在算法計算量太大的問題。

本文針對陀螺儀的靜態(tài)和動態(tài)特性，提出一種自適應(yīng)的KF 算法，在不同條件下自動調(diào)整模型參數(shù)，尤其在角速率變化較快的動態(tài)條件下可以保證濾波效果，通過搭建6 個陀螺儀組成的硬件平臺進行轉(zhuǎn)臺實驗，驗證算法的有效性

1 陀螺儀模型分析

本章選取單個陀螺儀的輸出進行噪聲分析，并對陀螺儀進行建模。MEMS 陀螺儀中隨機誤差的主要來源之一是陀螺儀輸出的白噪聲，通過對角速率積分，該測量噪聲的影響被累積，角速率測量白噪聲表現(xiàn)為角度隨機游走（Angular Random Walk，ARW）。大多數(shù)情況下，精確測量角度比測量角速度更為重要，這使得該測量噪聲的影響更加明顯。MEMS 陀螺儀誤差的另一個主要來源是由于陀螺儀偏置的微小變化，隨時間的變化隨機游動。這種誤差為速率隨機游走（Rate Random Walk，RRW）。MEMS 陀螺儀的一般模型表示如下：

式中：

z

為陀螺儀輸出；

ω

為真實角速率；

b

為白噪聲

w

驅(qū)動的緩慢變化的偏置誤差；

v

為測量白噪聲。

Allan 方差分析法是被國際普遍接受的一種表征速率傳感器隨機噪聲過程的分析方法。Allan方差分析的主要結(jié)果是Root Allan 方差圖，通過該圖可以識別捕獲數(shù)據(jù)中存在的隨機噪聲過程的類型和大小。本文使用6 個MPU6050 陀螺儀進行實驗，對單個陀螺儀采集4 h 靜態(tài)數(shù)據(jù)的Allan 方差分析，如圖1 所示。

圖1 單MEMS 陀螺Allan 方差分析Fig.1 Allan variance analysis of the single MEMS gyroscope

根據(jù)Allan 方差分析結(jié)果可知，陀螺的主要誤差來源為角度隨機游走和零偏不穩(wěn)定性，速率隨機游走誤差對陀螺精度影響較小。

對陀螺噪聲進行進一步分析，首先對噪聲的分布擬合，如圖2 所示，根據(jù)擬合結(jié)果，陀螺的噪聲可以近似擬合為正態(tài)分布；然后對陀螺噪聲的相關(guān)性進行分析，如圖3 所示。

圖2 單MEMS 陀螺噪聲分布擬合Fig.2 Noise distribution fitting of the single MEMS gyroscope

圖3 單MEMS 陀螺相關(guān)性分析Fig.3 Correlation analysis of the single MEMS gyroscope

根據(jù)圖3 分析可知，自相關(guān)曲線在起點顯示1個峰值，而后快速地下降到零，顯示沒有相關(guān)性，也沒有周期性。同時，為了降低算法復(fù)雜度，

b

不參與周期性和相關(guān)性分析，陀螺儀的模型可以簡化為

2 自適應(yīng)卡爾曼濾波器設(shè)計

2.1 狀態(tài)空間模型

建立離散卡爾曼濾波器，需要先建立狀態(tài)空間的測量模型和預(yù)測模型，根據(jù)式（2）的陀螺儀誤差模型，并且由于本文實驗使用6 個陀螺儀的組合陣列，可以得到卡爾曼濾波器的測量模型為

式中：

式中：

w

為陀螺儀的真實角速率，6 個陀螺均相同；

Z

為陀螺儀陣列的輸出測量矩陣；

H

為測量轉(zhuǎn)換矩陣；

V

為測量噪聲矢量，且測量噪聲的期望為零。令陀螺噪聲的協(xié)方差矩陣為

R

，由于每個陀螺儀之間如存在工藝或者環(huán)境等方面的一致性，即使是在單個硅晶片上制造的陀螺儀陣列中存在的相關(guān)因子，也無法精確獲得，但是為了分析相關(guān)性對精度提高的影響，假設(shè)陀螺之間存在相關(guān)性。所以

R

矩陣不是對角陣，存在非對角線元素。這里假設(shè)在相同分量的陀螺儀之間存在恒定的互相關(guān)，則矩陣

R

可以表示為

式中：

σ

為陀螺的噪聲方差；

ρ

為恒定的相關(guān)系數(shù)。

在用于表示平穩(wěn)時間序列信號的許多標(biāo)準(zhǔn)模型中，最常用的是AR 模型，AR 模型能夠較好地體現(xiàn)出噪聲的動態(tài)變化。AR 建模過程包括3 個迭代步驟：模型識別、參數(shù)估計和檢查診斷。為了降低整體算法的計算量，本文使用模型如下：

式中：

a

為模型參數(shù)；

τ

為預(yù)測模型的白噪聲。

根據(jù)式（6），可以得到卡爾曼濾波器的狀態(tài)空間預(yù)測模型為

式中：

X

為系統(tǒng)的狀態(tài)矩陣；

A

為預(yù)測模型參數(shù)，且

A

=

a

；

W

為系統(tǒng)噪聲，期望為

q

=0，方差為

Q

。

2.2 卡爾曼濾波器改進

根據(jù)建立的陀螺儀狀態(tài)空間測量模型和預(yù)測模型，可以得到標(biāo)準(zhǔn)的離散卡爾曼濾波器方程，預(yù)測模型狀態(tài)預(yù)測為

預(yù)測方差更新為

求解濾波增益為

狀態(tài)最優(yōu)估計為

最優(yōu)估計方差更新為

設(shè)計卡爾曼濾波器時，選取不同的參數(shù)會改變?yōu)V波器的性能，在靜態(tài)條件下，卡爾曼濾波器可以通過調(diào)整參數(shù)大幅提升陀螺的性能。在濾波器中，

Q

矩陣為預(yù)測模型的方差，

R

矩陣為測量模型的方差，

Q

矩陣小，則認(rèn)為預(yù)測模型較測量值相比更準(zhǔn)確，系統(tǒng)更信任模型的預(yù)測值。但是，在實際使用時，不同的動態(tài)條件下使用固定參數(shù)無法保證濾波效果，特別是角速率變化較快時。在這種條件下，濾波器的跟蹤性能顯著下降，因此可以通過增加反饋來實現(xiàn)自適應(yīng)卡爾曼濾波器，動態(tài)地調(diào)整

Q

矩陣和

R

矩陣的大小，提升系統(tǒng)的性能。由于

Q

矩陣和

R

矩陣的相對大小就可以影響卡爾曼增益的大小，所以為了減小計算量，選取維度更低的

Q

矩陣進行自適應(yīng)調(diào)節(jié)，對于

Q

矩陣來說，動態(tài)條件下模型誤差增大，通過反饋可以更好地改善動態(tài)性能，為了不影響靜態(tài)性能，在反饋中可以引入Sigmod函數(shù)：

在機器學(xué)習(xí)中，Sigmod 函數(shù)通常作為神經(jīng)網(wǎng)絡(luò)的激活函數(shù)被廣泛使用。通過引入Sigmod 函數(shù)，在變化較快的動態(tài)條件下，發(fā)揮自適應(yīng)調(diào)節(jié)反饋的功能，提升動態(tài)性能，同時不對靜態(tài)條件的濾波造成影響，使不同條件下的參數(shù)變化平滑過渡。改進的離散自適應(yīng)卡爾曼濾波器（S-Kalman Filter，SKF）方程可以表示如下。

生成放縮平移調(diào)整的Sigmod 函數(shù)為

預(yù)測模型狀態(tài)預(yù)測為

預(yù)測方差更新為

求解濾波增益為

狀態(tài)最優(yōu)估計為

最優(yōu)估計方差更新為

反饋計算為

偏差更新為

方差更新為

式中：

a

、

b

為Sigmod 函數(shù)的縮放和平移系數(shù)；

f

為反饋因子；

t

為跟蹤因子，與標(biāo)準(zhǔn)卡爾曼濾波相比，SKF 改進了靜態(tài)和動態(tài)的性能，更適合陀螺儀的濾波算法處理。

3 實驗及結(jié)果分析

搭建陀螺陣列進行實際測試，MEMS 陀螺儀使用6 個MPU6050，主控芯片使用STM32，設(shè)計原理圖并制作PCB 電路板，如圖4 所示。

圖4 陀螺儀陣列PCB 板Fig.4 Gyroscope array PCB board

與衛(wèi)星通信時使用CAN 總線接口，本文中實驗使用STM32 的SPI 接口進行通信，所有實驗均在相同的外部環(huán)境下進行測試，室溫為25 ℃，使用高精度轉(zhuǎn)臺（0.000 01（°）/s）進行實驗測試。由于轉(zhuǎn)臺的精度比MEMS 陀螺儀的精度高出幾個數(shù)量級，因此，轉(zhuǎn)臺的角度率可以認(rèn)為是真實角速率，先在靜態(tài)條件下測試，然后在加入簡諧波的動態(tài)條件下進行測試，最后加入人造信號進行角速率變化較快的動態(tài)測試，驗證算法的可行性。使用的轉(zhuǎn)臺如圖5 所示。

圖5 轉(zhuǎn)臺測試Fig.5 Turntable test

3.1 靜態(tài)濾波結(jié)果

首先在靜態(tài)條件下進行測試，在陀螺儀開始工作后求取一定時間內(nèi)的穩(wěn)態(tài)誤差，并在程序上做處理，加和求平均值；然后減去均值消除穩(wěn)態(tài)誤差的影響，以50 Hz 的采樣頻率采集6 個陀螺儀的數(shù)據(jù)，分別使用平均值算法、標(biāo)準(zhǔn)卡爾曼濾波算法和自適應(yīng)卡爾曼濾波算法進行濾波處理；最后分別對比6個陀螺的輸出數(shù)據(jù)、SKF 算法的濾波數(shù)據(jù)，以及不同算法濾波的效果對比，得出的實驗結(jié)果如圖6 和圖7 所示。

圖6 靜態(tài)條件SKF 算法Fig.6 SKF algorithm under the static condition

圖7 靜態(tài)條件不同算法對比Fig.7 Comparison of different algorithms under the static condition

對使用不同算法得到的虛擬陀螺靜態(tài)數(shù)據(jù)分別計算Allan 方差，與單MEMS 陀螺儀的Allan 方差進行對比，畫出Allan 方差圖，得到的對比結(jié)果如圖8 所示。

圖8 Allan 方差對比Fig.8 Allan variance comparison

對于靜態(tài)條件下的Allan 方差進行分析，3 種算法均能降低陀螺的噪聲，KF 算法和SKF 算法對噪聲的過濾性能更好。最后計算了靜態(tài)條件下濾波后的數(shù)據(jù)殘差的方差，與Allan 方差計算得到的ARW 和零偏不穩(wěn)定性分別進行效果對比，結(jié)果見表1。

表1 整體方差、ARW、零偏不穩(wěn)定性對比Tab.1 Comparison of overall variance，ARW，and zero bias instability

由實驗數(shù)據(jù)可知：靜態(tài)條件下標(biāo)準(zhǔn)KF 實現(xiàn)的虛擬陀螺，數(shù)據(jù)殘差的方差可以降低為單個陀螺的1/62；而SKF 算法實現(xiàn)的虛擬陀螺，數(shù)據(jù)殘差的方差可以降低為單個陀螺的1/94，靜態(tài)條件下小幅優(yōu)于標(biāo)準(zhǔn)卡爾曼濾波，算法可以顯著提升陀螺的精度。

3.2 動態(tài)濾波結(jié)果

對于動態(tài)濾波，首先在加入簡諧波的條件下進行測試，這里為了對比靜態(tài)和動態(tài)下的不同效果，令標(biāo)準(zhǔn)卡爾曼濾波器和自適應(yīng)卡爾曼濾波器與靜態(tài)條件下的參數(shù)保持一致；然后分別求出不同算法濾波后的殘差進行對比。在簡諧波的動態(tài)條件下，得到的實驗結(jié)果如圖9 和圖10 所示。

圖9 動態(tài)條件SKF 算法Fig.9 SKF algorithm under the dynamic condition

圖10 動態(tài)條件不同算法對比Fig.10 Comparison of different algorithms under the dynamic condition

簡諧波的變化比較平緩，由計算可得，簡諧波經(jīng)過KF 算法濾波后，殘差的方差為1.351 9×10，方差為單個陀螺的1/36；SKF 濾波后，殘差的方差為1.049 9×10，方差為單個陀螺的1/47。SKF 算法與標(biāo)準(zhǔn)KF 算法相比，精度更高，但提升的精度不明顯。為了測試變化較快的動態(tài)條件下的效果，加入角速率突變的人工信號，對比測試不同算法的濾波性能，同時計算殘差對比效果，實驗結(jié)果如圖11和圖12 所示。

圖11 特殊動態(tài)條件SKF 算法Fig.11 SKF algorithm under the special dynamic conditions

圖12 快速動態(tài)條件不同算法對比Fig.12 Comparison of different algorithms under the special dynamic condition

在這種動態(tài)條件下，由實驗數(shù)據(jù)可知，在濾波器參數(shù)保持不變時，快速動態(tài)條件下標(biāo)準(zhǔn)KF 算法的濾波效果難以保證，標(biāo)準(zhǔn)KF 算法無法識別快速變化的噪聲，跟蹤性能大幅下降；而自適應(yīng)卡爾曼濾波器通過不同權(quán)重的反饋調(diào)節(jié)，仍然可以大幅提升陀螺的精度，保持良好的跟蹤性能；最后根據(jù)實驗數(shù)據(jù)分別計算不同算法濾波結(jié)果殘差的方差，對比見表2。

表2 殘差的方差對比Tab.2 Comparison of residual variance

根據(jù)表中數(shù)據(jù)可知：在對快速變化的數(shù)據(jù)進行濾波時，KF 算法效果較差，殘差的方差僅為單個陀螺的1/5；而使用SKF 算法，數(shù)據(jù)殘差的方差可以降低為單個陀螺的1/18。

4 結(jié)束語

為了減小MEMS 陀螺儀噪聲的影響，本文研究了多MEMS 陀螺融合的虛擬陀螺技術(shù)，通過Allan方差等方法分析了陀螺的噪聲，通過分析卡爾曼濾波器的參數(shù)對陀螺靜態(tài)動態(tài)性能的改善特點，有針對性地提出了一種自適應(yīng)的卡爾曼濾波器，以優(yōu)化陀螺儀的靜態(tài)和動態(tài)性能。對于快速變化的數(shù)據(jù)，SKF 算法優(yōu)勢明顯。通過搭建的硬件平臺進行實驗測試，驗證了算法的正確性，但仍然存在不足，提出的算法的參數(shù)比標(biāo)準(zhǔn)卡爾曼濾波算法更多，雖然濾波效果更好，但是在實際使用時更依賴參數(shù)的調(diào)節(jié)，增加了不便性，后續(xù)將繼續(xù)優(yōu)化算法的復(fù)雜度。