基于模糊自適應(yīng)Kalman濾波的MEMS慣導(dǎo)系統(tǒng)在線修正研究*

贠衛(wèi)國，畢 凱，田 易，孟 真

0 引 言

MEMS慣性導(dǎo)航系統(tǒng)由于成本的限制，選用的傳感器精度較低，導(dǎo)致慣性導(dǎo)航系統(tǒng)的解算誤差會(huì)隨著時(shí)間的增加而增大，因此慣性導(dǎo)航系統(tǒng)工作前須采用標(biāo)定的方法來提高M(jìn)EMS慣性導(dǎo)航系統(tǒng)的解算精度[1].確定性零偏、標(biāo)度因數(shù)以及安裝誤差可以通過轉(zhuǎn)臺(tái)標(biāo)定的方法來消除，但隨機(jī)零偏每次上電啟動(dòng)都存在不確定性的問題，無法消除.因此，本文提出了一種基于自適應(yīng)Kalman濾波估計(jì)隨機(jī)零偏的方法來修正MEMS慣導(dǎo)系統(tǒng).

MEMS慣導(dǎo)系統(tǒng)誤差在線修正中通常采用基于Kalman的濾波方法[2,3]對(duì)陀螺和加速度計(jì)的誤差進(jìn)行在線估計(jì)，這種方法在外部環(huán)境影響較小的情況下效果是比較好的，但是在實(shí)際在線標(biāo)定中，慣性導(dǎo)航系統(tǒng)很容易受到多種外部環(huán)境的干擾而導(dǎo)致觀測(cè)噪聲發(fā)生變化，進(jìn)而影響濾波器性能，導(dǎo)致精度下降.因此Kalman濾波在噪聲時(shí)變的情況下難以起到好的估計(jì)效果.針對(duì)噪聲時(shí)變這一問題，不少學(xué)者對(duì)自適應(yīng)濾波技術(shù)進(jìn)行了研究，如 ‘漸消因子自適應(yīng)Kalman濾波’可通過漸消因子對(duì)預(yù)測(cè)誤差協(xié)方差進(jìn)行調(diào)整來抑制濾波發(fā)散，但它在變量較多的情況下跟蹤能力比較差[4].‘Sage-Husa濾波 ’的基本原理是基于歷史觀測(cè)數(shù)據(jù)的統(tǒng)計(jì)特性在線計(jì)算調(diào)節(jié)因子，計(jì)算量比較大，實(shí)時(shí)跟蹤能力差[5].‘模糊自適應(yīng)Kalman濾波’是一個(gè)實(shí)時(shí)調(diào)整噪聲統(tǒng)計(jì)特性從而實(shí)時(shí)調(diào)整并估計(jì)的濾波模型，模型可以對(duì)時(shí)變的觀測(cè)噪聲進(jìn)行準(zhǔn)確估計(jì)，能夠快速的逼近真實(shí)值，并且計(jì)算簡(jiǎn)單，得到了廣泛的應(yīng)用[6].

基于模糊自適應(yīng)控制的Kalman濾波已經(jīng)成功應(yīng)用于導(dǎo)航系統(tǒng)，如Tseng Chien-Hao提出了一種基于組合卡爾曼濾波器(CKF)和模糊邏輯自適應(yīng)系統(tǒng)(FLAS)的組合導(dǎo)航系統(tǒng)的傳感器融合方法[7]，成功的抑制了濾波發(fā)散.Li, Jing等在慣性導(dǎo)航初始對(duì)準(zhǔn)中提出了無跡Kalman結(jié)合模糊自適應(yīng)策略，提高了對(duì)準(zhǔn)的穩(wěn)定性[8].然而在線標(biāo)定中，基于模糊控制的自適應(yīng)Kalman卻很少用到.因此作者提出了基于模糊自適應(yīng)控制的Kalman濾波方法來實(shí)現(xiàn)陀螺和加速度計(jì)的隨機(jī)誤差的在線估計(jì).

因此，本文針對(duì)觀測(cè)噪聲狀態(tài)不準(zhǔn)確時(shí)容易導(dǎo)致濾波發(fā)散的情況，該方法通過制定模糊規(guī)則，將觀測(cè)變量殘差的理論方差與實(shí)測(cè)方差跡的比值模糊化作為模糊推理的輸入，姿態(tài)、速度和位置量測(cè)方差的修正因子作為輸出，實(shí)時(shí)、精確的估計(jì)系統(tǒng)的觀測(cè)噪聲，提高了系統(tǒng)的穩(wěn)定性以及修正效果.

1 誤差模型

1.1 慣導(dǎo)系統(tǒng)誤差模型

根據(jù)理想平臺(tái)和導(dǎo)航計(jì)算的數(shù)學(xué)平臺(tái)確定了以下的導(dǎo)航系統(tǒng)誤差，系統(tǒng)的姿態(tài)誤差和速度誤差如下[9,10]：

(1)

(2)

系統(tǒng)的位置誤差公式如下：

(3)

式中，δL表示緯度誤差、δλ表示經(jīng)度誤差、δh表示高度誤差、h表示高度、RM表示卯西圈半徑、RN表示子午圈半徑.

航向角誤差和姿態(tài)誤差角之間的關(guān)系式為：

(4)

1.2 慣性器件誤差模型

本文考慮到MEMS慣性器件的隨機(jī)誤差，得到陀螺儀和加速度計(jì)的誤差模型[11]：

(5)

2 Kalman濾波算法

2.1 狀態(tài)方程和觀測(cè)方程

Kalman 濾波是一種線性最小方差估計(jì)[12]，通過迭代運(yùn)算，事先預(yù)測(cè)、校正更新估計(jì)的一組數(shù)學(xué)方程.在濾波算法中，首先是建立濾波的狀態(tài)方程和觀測(cè)方程.

系統(tǒng)的狀態(tài)方程為:

(6)

其中，A為狀態(tài)矩陣，W為狀態(tài)噪聲，狀態(tài)矩陣A為[13]：

其中，狀態(tài)方程A中的每一個(gè)模塊由誤差模型以及對(duì)應(yīng)的狀態(tài)變量得出.

本文以慣性導(dǎo)航系統(tǒng)解算的航向角、速度、位置信息與參考航向角、GPS速度、位置的差值作為觀測(cè)量，設(shè)VS、PS、ψS為慣性導(dǎo)航解算的速度、位置及航向角，VG、PG、ψG為參考的速度、位置及航向角，則觀測(cè)變量為：

則系統(tǒng)觀測(cè)方程為:

Z(t)=HX(t)+V(t)

(7)

其中，H為觀測(cè)矩陣，V為觀測(cè)噪聲.系統(tǒng)觀測(cè)矩陣為:

H=[H1H2]

其中：

H1=[H11H1201×10]

H11=-T(1,2)T(3,2)/(T(1,2)2+T(2,2)2

H12=-T(3,2)T(2,2)/(T(1,2)2+T(2,2)2)

2.2 Kalman濾波過程

將2.1節(jié)的狀態(tài)方程和觀測(cè)方程離散化，得到遞推Kalman濾波器如下[14]：

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

(8)

狀態(tài)更新：

(9)

新息：

(10)

濾波增益矩陣：

Kk+1=Pk+1/kHT[HPk+1/kHT+R]-1

(11)

一步預(yù)測(cè)協(xié)方差：

Pk+1/k=APk/kΦT+ΓQΓT

(12)

協(xié)方差更新：

Pk+1/k+1=[In-Kk+1H]Pk+1/k

(13)

3 模糊控制

Kalman濾波器在對(duì)象模型足夠準(zhǔn)確時(shí), 性能較好；但如果系統(tǒng)模型或噪聲的統(tǒng)計(jì)特性不準(zhǔn)確，過去的量測(cè)數(shù)據(jù)會(huì)直接影響現(xiàn)時(shí)狀態(tài)估計(jì)的精度，嚴(yán)重時(shí)甚至導(dǎo)致濾波發(fā)散[15,16].因此，為了抑制Kalman濾波器發(fā)散，本文制定了模糊規(guī)則，通過輸入殘差的實(shí)測(cè)方差與理論方差跡的比值來對(duì)量測(cè)方差進(jìn)行實(shí)時(shí)估計(jì)，并輸出調(diào)整參數(shù)，對(duì)量測(cè)誤差的參數(shù)進(jìn)行估計(jì)和修正，以求獲得更好的估計(jì)效果.

3.1 模糊邏輯

模糊理論不需要建立準(zhǔn)確的數(shù)學(xué)模型，但能較好的反映事物的不確定性，它提供了一種有效的表示不確定性和不精確性的模糊算子和決策規(guī)則.本文主要對(duì)實(shí)測(cè)方差與理論方差跡的比值進(jìn)行模糊化，得到論域上的模糊子集所對(duì)應(yīng)的隸屬度函數(shù)，當(dāng)系統(tǒng)的觀測(cè)噪聲不平穩(wěn)時(shí)，調(diào)整實(shí)測(cè)方差與理論方差跡的比值來使觀測(cè)噪聲的設(shè)定值逐漸逼近于真實(shí)噪聲水平.

本文將觀測(cè)信息分為兩個(gè)模塊，則殘差的量測(cè)方差Ck和理論方差Sk如下：

式中，CψV和SψV分別為姿態(tài)和速度的實(shí)測(cè)方方差與理論方差，CP、SP分別為位置的實(shí)測(cè)方差與理論方差.

本文通過分別監(jiān)測(cè)CψV、SψV和CP、SP來制定模糊規(guī)則，其中對(duì)應(yīng)理論方差與實(shí)測(cè)方差跡的比值G1、G2作為模糊推理的輸入，觀測(cè)噪聲的修正因子qψV，qP作為輸出，當(dāng)觀測(cè)噪聲的統(tǒng)計(jì)特性準(zhǔn)確時(shí)，G1，G2的值應(yīng)該1附近，當(dāng)系統(tǒng)的測(cè)量噪聲的實(shí)際值與給定值不一致時(shí)，G1，G2將偏離1，即當(dāng)G1、G2大于1時(shí)，則系統(tǒng)的理論方差大于實(shí)測(cè)方差，應(yīng)該增大修正因子qψV、qP,使G1、G2接近于1，同理，當(dāng)系統(tǒng)理論方差小于實(shí)測(cè)方差時(shí)，應(yīng)當(dāng)減小qψV、qP.

系統(tǒng)中G1、G2采用相同的輸入G，qψV、qP采用相同的輸出q，建立模糊推理規(guī)則為：

if 1.5

if 0.5

if 0

得到輸出的修正因子為：

if ′∈MA′，q=1+0.5·N

if ′∈M′，q=1+0.3,(1-N)·sign(G-1) if ′∈MI′，q=1-0.5·N

其中N為輸入隸屬度函數(shù)得到的隸屬模糊度.

3.2 模糊自適應(yīng)Kalman濾波

模糊自適應(yīng)Kalman濾波器是根據(jù)3.1節(jié)所得的修正因子來實(shí)時(shí)修正常規(guī)Kalman濾波過程中的觀測(cè)噪聲，式(14)中，qψV修正姿態(tài)和速度的觀測(cè)噪聲，qP修正位置的觀測(cè)噪聲，修正過程如下式：

(14)

上式中qψV和qP是由上一步濾波過程中的實(shí)測(cè)方差和理論方差決定.

實(shí)測(cè)方差和理論方差的求取過程如下.式(10)中，新息ε被稱為殘差向量，它反映了濾波模型的準(zhǔn)確性.殘差的量測(cè)方差為：

Ck=εk·εkT

(15)

由于本文的觀測(cè)噪聲是由簡(jiǎn)單的統(tǒng)計(jì)特性得到的，但觀測(cè)噪聲經(jīng)常受到外部環(huán)境的影響，容易導(dǎo)致濾波發(fā)散，因此需要對(duì)式(15)進(jìn)行數(shù)據(jù)平滑操作，得到平滑新息測(cè)量方差為：

Ck=Ck-1·(1-dk)+dkεkεkT

(16)

dk=(1-b)/(1-bk+1)

(17)

式(19)中，d為遺忘因子，b為遺忘因子中的參數(shù)，通常b的取值為[0.95,0.995].

定義殘差的理論方差為

Sk=Hk(AkPkAkT+Qk)HkT+Rk

(18)

那么，殘差的理論方差與實(shí)測(cè)方差的跡的比值為：

(19)

其中，tr表示矩陣的跡.

圖1為模糊自適應(yīng)Kalman濾波的算法流程圖.

圖1 模糊自適應(yīng)Kalman濾波算法流程圖Fig.1 Flow chart of fuzzy adaptive Kalman filtering algorithm

4 載體路徑的分析及設(shè)計(jì)

為了實(shí)現(xiàn)對(duì)慣性導(dǎo)航系統(tǒng)中慣性器件誤差的有效在線估計(jì)和補(bǔ)償，本文需研究載體不同機(jī)動(dòng)方式對(duì)慣性傳感器誤差帶來的影響與激勵(lì)效果.

4.1 可觀測(cè)性分析

本文利用奇異值分解理論的方法進(jìn)行可觀測(cè)分析，由于在奇異值分解的過程中有引入外測(cè)設(shè)備的信息，一般利用離散型系統(tǒng)的模型對(duì)系統(tǒng)進(jìn)行奇異值分解.首先需要線性時(shí)變系統(tǒng)進(jìn)行分段處理，則將表達(dá)式寫成離散型分段式定常系統(tǒng)形式[18]PWCS(Piece-Wise Constant System).

離散化后的狀態(tài)方程和觀測(cè)方程如下

(20)

式中：Xk∈Rn;Zk∈Rm;Fj∈Rn×n;Hj∈Rm×n;Fj和Hj均為常值矩陣.則線性系統(tǒng)的可觀測(cè)性矩陣可表示為：

其中，

式中，Qj為線性時(shí)變系統(tǒng)中第i時(shí)間段nmxn階的可觀測(cè)性矩陣.同時(shí)，時(shí)變系統(tǒng)的可觀測(cè)性矩陣(SOM，Stripped Observability Matrix)為：

Qs(r)=[Q1Q2…Qr]T

(21)

奇異值的分解如下：

存在矩陣A∈Rm×n,那么存在正交矩陣U1∈Rm×m和U2∈Rn×n，使得A=U1∑U2T,式中

且對(duì)角元素等于S=diag{σ1,…,σr}為對(duì)角陣，σ1≥σ2≥…≥σr≥0順序排列.

4.2 可觀測(cè)性分析仿真

本文在用奇異值分解方法分析時(shí)變系統(tǒng)可觀測(cè)性與狀態(tài)可觀測(cè)度時(shí)，通過奇異值的分解，各狀態(tài)量對(duì)應(yīng)的奇異值的相對(duì)大小就反映了各狀態(tài)量的相對(duì)可觀測(cè)性.

通過對(duì)四種簡(jiǎn)單路徑的可觀測(cè)分析，可以得到與狀態(tài)方程中相對(duì)應(yīng)的15個(gè)狀態(tài)變量的奇異值如表1.

表1 不同狀態(tài)下的奇異值Tab.1 Singular values in different states

表1表示的是15種狀態(tài)變量在不同路徑下的奇異值，通過奇異值的大小可以判斷系統(tǒng)的可觀測(cè)性.為了能更簡(jiǎn)便的區(qū)分可觀測(cè)度的大小，準(zhǔn)確的估計(jì)出誤差參數(shù)，本文設(shè)奇異值小于1的不可觀測(cè)，因此， 由上表可得： 在勻速直線運(yùn)動(dòng)狀態(tài)下，陀螺的z軸零偏和加計(jì)的零偏的奇異值太小，則不可觀測(cè)；在加速直線運(yùn)動(dòng)下，加計(jì)的x,y軸零偏可觀測(cè)度較低；在轉(zhuǎn)彎運(yùn)動(dòng)中，加計(jì)零偏的可觀測(cè)性相對(duì)來說較低；圓周運(yùn)動(dòng)中，陀螺和加計(jì)零偏的可觀測(cè)性相對(duì)來說都比較好.

4.3 路徑設(shè)計(jì)

通過以上可觀測(cè)分析，路徑的設(shè)計(jì)如下圖2，設(shè)載體所在的初始緯度為45°，初始經(jīng)度為118°，仿真路徑開始為指北方向，路徑行駛分為以下三種不同的階段：

①指北方位勻速直線運(yùn)動(dòng)，該軌跡中陀螺的x軸零偏和y軸零偏的可觀測(cè)性比較好.

②拐彎至東北方位加速直線運(yùn)動(dòng),x軸加計(jì)零偏和z軸加計(jì)零偏的可觀測(cè)性比較好.

③圓周運(yùn)動(dòng),可標(biāo)定出陀螺z軸零偏和y軸加計(jì)的零偏.

圖2 載體仿真軌跡Fig.2 Carrier simulation track

5 仿真及結(jié)果分析

通過可觀測(cè)分析可知，在以上設(shè)計(jì)的路徑中，陀螺和加計(jì)的誤差參數(shù)模型的可觀測(cè)性比較好.在這三種路徑軌跡的行駛下，可以標(biāo)定出陀螺和加速度計(jì)三軸的隨機(jī)零偏.

本次在線估計(jì)的仿真時(shí)間是120 s, 初始平臺(tái)誤差角為0.5(°)/s,初始速度誤差為0.1 m/s,初始位置誤差為1.5 m.慣導(dǎo)系統(tǒng)的狀態(tài)噪聲均方差為:

Q=[(0.3(°)/s)2(0.3(°)/s)2(0.3(°)/s)2(0.2m/s2)2(0.2m/s2)2(0.2m/s2)20 0 0 0 0 0 0 0 0].

觀測(cè)噪聲均方差為:

R=[(0.5(°)/s)2(0.2 m/s)2(0.2 m/s)2(0.2 m/s)2(2 m)2(2 m)2(2 m)2].

仿真階段在50 s至70 s給觀測(cè)量加入干擾噪聲，航向角加入的干擾噪聲為1(°)/s的隨機(jī)噪聲,速度加入干擾噪聲為5 m/s的隨機(jī)噪聲，位置加入干擾噪聲為7 m的隨機(jī)噪聲.設(shè)置陀螺的隨機(jī)零偏為0.5(°)/s,設(shè)置加速度計(jì)的隨機(jī)零偏為0.8 m/s2.如下圖為在線估計(jì)隨機(jī)零偏的仿真結(jié)果：

圖3 陀螺隨機(jī)零偏的兩種不同濾波估計(jì)結(jié)果Fig.3 Two different filtering estimation results of gyro random zero offset

圖4 加計(jì)隨機(jī)零偏的兩種不同濾波估計(jì)結(jié)果Fig.4 Adds two different filtering estimates for random zero bias

表2為系統(tǒng)的觀測(cè)噪聲發(fā)生變化時(shí)Kalman濾波與自適應(yīng)Kalman濾波的估計(jì)結(jié)果與設(shè)定的標(biāo)準(zhǔn)值之間的最大誤差對(duì)比.

表2 觀測(cè)噪聲變化兩種不同濾波下陀螺和加計(jì)隨機(jī)零偏估計(jì)結(jié)果誤差Tab.2 Observed noise variation gyro and additive random zero-offset estimation error under two different filtering

由圖3和圖4的結(jié)果可知，Kalman濾波和自適應(yīng)Kalman濾波都能很快收斂到設(shè)定值，但是在觀測(cè)噪聲受到外部環(huán)境影響而變化比較大時(shí)，從仿真結(jié)果和表2來看Kalman濾波標(biāo)定的結(jié)果在受到影響時(shí)跳變比較大，而自適應(yīng)Kalman濾波比常規(guī)Kalman濾波更收斂，幾乎沒有變化，將誤差減小了二十幾倍，可以更穩(wěn)定的估計(jì)出陀螺和加速度計(jì)的隨機(jī)零偏，因此，該方案具備更強(qiáng)的抗干擾能力，可以更有效的估計(jì)MEMS慣導(dǎo)系統(tǒng)的隨機(jī)零偏來提高導(dǎo)航精度.

6 結(jié) 論

由于在MEMS慣性導(dǎo)航系統(tǒng)中傳感器零偏存在隨機(jī)啟動(dòng)的不確定性誤差，以及Kalman濾波估計(jì)中觀測(cè)噪聲的統(tǒng)計(jì)特性不準(zhǔn)確，容易受外部環(huán)境影響導(dǎo)致濾波發(fā)散，因此本文提出了一種自適應(yīng)Kalman濾波在線估計(jì)隨機(jī)零偏的方法來提高M(jìn)EMS慣導(dǎo)系統(tǒng)的精度.本文首先介紹了慣性導(dǎo)航系統(tǒng)的誤差模型和模糊自適應(yīng)Kalman濾波算法，然后建立狀態(tài)方程以及觀測(cè)方程，并進(jìn)行載體仿真路徑的設(shè)計(jì)及分析，然后通過在線仿真得出了兩種不同濾波的標(biāo)定結(jié)果，由圖3、圖4以及表2可以得出，在觀測(cè)噪聲受到外部環(huán)境影響而發(fā)散時(shí)，自適應(yīng)Kalman濾波在估計(jì)的過程中抑制了這種發(fā)散，相比常規(guī)Kalman濾波來說，更穩(wěn)定的估計(jì)出了系統(tǒng)的隨機(jī)零偏，因此，本文提出的方法具有更好的實(shí)用性.