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

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

0 引言

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

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

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

1 陀螺儀模型分析

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

式中：

z

為陀螺儀輸出；

ω

為真實(shí)角速率；

b

為白噪聲

w

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

v

為測(cè)量白噪聲。

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

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

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

對(duì)陀螺噪聲進(jìn)行進(jìn)一步分析，首先對(duì)噪聲的分布擬合，如圖2 所示，根據(jù)擬合結(jié)果，陀螺的噪聲可以近似擬合為正態(tài)分布；然后對(duì)陀螺噪聲的相關(guān)性進(jì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)曲線(xiàn)在起點(diǎn)顯示1個(gè)峰值，而后快速地下降到零，顯示沒(méi)有相關(guān)性，也沒(méi)有周期性。同時(shí)，為了降低算法復(fù)雜度，

b

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

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

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

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

式中：

式中：

w

為陀螺儀的真實(shí)角速率，6 個(gè)陀螺均相同；

Z

為陀螺儀陣列的輸出測(cè)量矩陣；

H

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

V

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

R

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

R

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

R

可以表示為

式中：

σ

為陀螺的噪聲方差；

ρ

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

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

式中：

a

為模型參數(shù)；

τ

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

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

式中：

X

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

A

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

A

=

a

；

W

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

q

=0，方差為

Q

。

2.2 卡爾曼濾波器改進(jìn)

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

預(yù)測(cè)方差更新為

求解濾波增益為

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

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

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

Q

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

R

矩陣為測(cè)量模型的方差，

Q

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

Q

矩陣和

R

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

Q

矩陣和

R

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

Q

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

Q

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

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

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

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

預(yù)測(cè)方差更新為

求解濾波增益為

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

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

反饋計(jì)算為

偏差更新為

方差更新為

式中：

a

、

b

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

f

為反饋因子；

t

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

3 實(shí)驗(yàn)及結(jié)果分析

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

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

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

圖5 轉(zhuǎn)臺(tái)測(cè)試Fig.5 Turntable test

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

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

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

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

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

圖8 Allan 方差對(duì)比Fig.8 Allan variance comparison

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

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

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

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

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

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

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

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

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

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

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

表2 殘差的方差對(duì)比Tab.2 Comparison of residual variance

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

4 結(jié)束語(yǔ)

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