GPS/SⅠNS超緊組合導(dǎo)航系統(tǒng)自適應(yīng)混合濾波算法

周衛(wèi)東，蔡佳楠，孫 龍

(哈爾濱工程大學(xué)自動化學(xué)院，150001哈爾濱)

GPS/SINS超緊組合導(dǎo)航系統(tǒng)在建模時，將導(dǎo)航參數(shù)的誤差作為估計狀態(tài)，稱為間接法濾波.間接法濾波有兩種，分別是輸出校正和反饋校正.理論上，當模型能夠準確反映系統(tǒng)本身時，輸出校正和反饋校正的估計效果是一致的［1］.工程實踐中，輸出校正實現(xiàn)簡單，濾波器故障不影響SINS工作，但它只能改善輸出的準確性，不能對SINS內(nèi)部誤差作修正，長時間工作時，SINS誤差累積，最后使系統(tǒng)模型跟實際模型不匹配，導(dǎo)致濾波精度下降或發(fā)散;反饋校正估計的是經(jīng)過校正的導(dǎo)航參數(shù)誤差，保持為小量，沒有模型誤差，更接近于真實系統(tǒng)，但是反饋校正實現(xiàn)復(fù)雜，濾波器故障或估計精度不高會污染SINS輸出，使系統(tǒng)可靠性降低［2］.文獻［3］結(jié)合這兩種結(jié)構(gòu)的優(yōu)缺點提出了混合校正濾波結(jié)構(gòu)，該結(jié)構(gòu)只有在所有狀態(tài)估計精度都很高時才能得出較好的濾波結(jié)果，而狀態(tài)的估計精度是由其可觀測性決定的［4-5］，因此這種結(jié)構(gòu)只能提高完全可觀系統(tǒng)估計精度，對于GPS/SINS超緊組合導(dǎo)航系統(tǒng)這種不完全可觀測的系統(tǒng)是不適用的.

對此，文獻［6］中給出以奇異值分解(SVD，singular value decomposition)可觀測度分析方法為基礎(chǔ)的自適應(yīng)反饋校正濾波方法，它將歸一化的奇異值作為反饋因子，對SINS進行反饋校正.該方法根據(jù)載體的運動狀態(tài)實時計算系統(tǒng)的可觀測性矩陣，進而獲得反饋因子，而隨著時間的增長，可觀測性矩陣的維數(shù)急劇增加，從而導(dǎo)致計算量的增大，甚至?xí)霈F(xiàn)維數(shù)災(zāi)難，導(dǎo)致系統(tǒng)崩潰;同時該方法在計算可觀測度的過程中進行了無量綱化處理，這種處理并不能保證處理前后可觀測度信息是一致的［7］，因此文獻［6］計算出的反饋因子與系統(tǒng)真實情況不完全相符.

針對以上問題，本文在文獻［8-10］中提出的超緊組合模型基礎(chǔ)上重新推導(dǎo)出線性的量測方程，并給出自適應(yīng)混合校正濾波方法.該方法引用Ham的特征值和特征向量可觀測性分析方法［11］，將濾波過程中計算出的狀態(tài)誤差方差陣進行規(guī)范化，求取其特征值，將歸一化的最小特征值對應(yīng)的特征向量作為反饋因子，確定反饋量.這種混合校正方法相對于文獻［6］來說，不受誤差模型和運動時間的限制，計算量相對較小.

1 GPS/SINS超緊組合導(dǎo)航系統(tǒng)誤差模型

狀態(tài)變量為:位置誤差δR=[δLδλδh]T;速度誤差δv=[δvEδvNδvU]T;平臺失準角φ =[φEφNφU]T;加速度計零位誤差▽=[▽x▽y▽z]T;陀螺常值漂移ε=[εxεyεz]T;接收機鐘差為 δtu，鐘差漂移為 δtru．則系統(tǒng)誤差方程為［10，12］

在GPS接收機中，接收到的衛(wèi)星信號經(jīng)變頻和解調(diào)后進入到Ⅰ-Q通道，Ⅰ-Q信號的期望值與載體的速度誤差及位置誤差滿足如下關(guān)系式:

這里利用Ⅰ和Q的期望值E［Ⅰ］和E［Q］代替Ⅰ和Q本身對系統(tǒng)進行建模．其中，A為衛(wèi)星的信號強度;T為時間常數(shù);ω為載波角頻率;c為光速;ωe和φe滿足如下關(guān)系式:

其中:Ru和分別為接收機到地心距離矢量的測量值和估計值;vu和分別為接收機到地心速度矢量的測量值和估計值;Re=|Ru-|和ve=|vu-v|分別代表位置誤差和速度誤差．

同理，將SINS的測量值與估計值之差代入式(2)～(5)，計算得到SINS對Ⅰ，Q的預(yù)測值為

接收機第i個Ⅰ-Q通道輸出值為

將 ⅠSINS，QSINS與 ⅠGPS，QGPS之差作為測量

則GPS/SINS超緊組合導(dǎo)航系統(tǒng)如圖1所示［13］.δⅠ，δQ為 SINS 對Ⅰ，Q的預(yù)測偏差．ηⅠ，ηQ是接收機Ⅰ-Q 通道噪聲，ηⅠ～ N(0，1)，ηQ～ N(0，1)，E［ηⅠηQ］=0．

圖1 GPS/SⅠNS超緊組合導(dǎo)航系統(tǒng)

在文獻［8-10］中選取Re和ve為狀態(tài)量，則由式(2)和式(3)分別得到

由此推導(dǎo)得到量測方程是關(guān)于Re和ve的非線性方程［14］．為此將Re和ve重新定義為

并且

其中:L為緯度;λ為經(jīng)度;h為高度;R0為地球半徑;vE，vN，vU分別為載體在東、北、天方向的速度分量．綜上所述，量測方程為［15］

觀測矩陣為

其中:

其中i=1，2，…，n為跟蹤衛(wèi)星的數(shù)目．

2 特征值特征向量可觀測性分析方法

其協(xié)方差為

其中P為誤差協(xié)方差陣，pij為其元素．選取約束條件為

拉格朗日方程滿足

其中λ為拉格朗日因子．將式(12)代入式(14)得

則

當且僅當

方程(16)有非零解.因此，式(16)的解為P的特征向量，將式(16)的兩邊分別左乘vT得

將式(12)，式(13)代入式(18)得

通過全等變換將P規(guī)范化為PN+(k)，

其中

全等變換使特征值大小發(fā)生變化，被限定在［0，1］之間(n為系統(tǒng)階數(shù))，但它正特性和矩陣的秩不變．由此可以發(fā)現(xiàn)，PN+(k)矩陣的特征值實際上是對應(yīng)狀態(tài)的方差.那么，特征值越小，其對應(yīng)的狀態(tài)(或狀態(tài)的線性組合)的可觀測性越強［11］.因此，最小的特征值對應(yīng)可觀測性最好的狀態(tài)(或狀態(tài)的線性組合).

對動態(tài)系統(tǒng)進行可觀測性分析時，誤差協(xié)方差陣的特征值和特征向量法相對于分段定常系統(tǒng)(PWCS，piece-wise constant system)可觀測性分析方法和SVD有如下優(yōu)點:

1)PWCS和SVD分析系統(tǒng)可觀測性時，都只是在齊次系統(tǒng)方程的基礎(chǔ)上進行的，沒有考慮到系統(tǒng)過程噪聲和量測噪聲的影響;而特征值和特征向量法利用了包含噪聲影響的誤差協(xié)方差陣，分析系統(tǒng)在有噪聲情況下的可觀測性;

2)SVD在計算可觀測度的過程中進行了無量綱化處理，這種處理并不能保證處理前后可觀測度信息是一致的［7］;而特征值和特征向量法中采用的全等變換不改變特征值的特性，變換前后得到的可觀測性結(jié)論一致;

3)PWCS和SVD某一時刻的可觀測性矩陣，包含當前時刻之前的所有時間段的可觀測性矩陣，可觀測性矩陣維數(shù)隨時間延長而增加，計算量增大;而特征值和特征向量法需要求解每一時刻誤差協(xié)方差陣的特征值和特征向量，而該協(xié)方差陣剛好是系統(tǒng)濾波迭代計算時產(chǎn)生的中間數(shù)據(jù)，且其維數(shù)取決于系統(tǒng)狀態(tài)的個數(shù)，不隨時間的延長而增加，計算量相對較小;

4)特征值和特征向量法具有更加明確的物理意義，易于理解.

3 基于可觀測性的混合校正濾波器設(shè)計

由于狀態(tài)的估計精度由其可觀測性決定，可觀測性高的狀態(tài)其估計精度高，可觀測性低的狀態(tài)其估計精度低，為使反饋量更具有可靠性，將估計的誤差量按可觀測性的強弱成比例反饋給SINS，修正SINS和GPS原始數(shù)據(jù).因此，在反饋校正中反饋因子是一個重要的參數(shù)，它的大小應(yīng)該由系統(tǒng)狀態(tài)可觀測性決定，即可觀測性高的狀態(tài)給予較大的反饋因子，可觀測性低的狀態(tài)給予較小的反饋因子，不可觀測的狀態(tài)反饋因子為零，并且反饋因子能夠隨著狀態(tài)可觀測性的變化而變化.基于以上分析，利用濾波過程中的誤差協(xié)方差陣來實時計算反饋因子可以有效滿足上述要求.

文獻［16］指出小的特征值對應(yīng)高可觀測度，相應(yīng)的特征向量給出高可觀測度的“方向”，即最小特征值對應(yīng)的特征向量可以指示出各個狀態(tài)可觀測性強弱.將該特征向量的元素做歸一化處理，得到的即為狀態(tài)(或狀態(tài)的線性組合)的可觀測度，它是衡量狀態(tài)估計精度的數(shù)值指標［4，16］.那么，利用可觀測度與估計精度呈正相關(guān)的關(guān)系，選取最小特征值對應(yīng)的歸一化后的特征向量vmin中的元素vi(i∈{1，2，…，n})為相應(yīng)狀態(tài)的反饋因子，將反饋因子與狀態(tài)估計值的乘積作為反饋量．該反饋量雖然比較保守，但其保留了可觀測性強的狀態(tài)對系統(tǒng)良好的修正能力，同時削弱了可觀測性弱的狀態(tài)對系統(tǒng)盲目修正造成的不良影響．混合校正濾波器的結(jié)構(gòu)如圖2所示．

圖2 混合校正濾波器

將vmin中的元素vi與系統(tǒng)狀態(tài)對應(yīng)后，得到的反饋因子為α =diag{ α1α2…αn}，其中 αi∈{v1v2…vn}．當αm=1，m∈{1，2，…，n}，表示第m個狀態(tài)的可觀測性最高，其估計精度最高，完全反饋;αe=0，e∈ {1，2，…，n}，表示第 e個狀態(tài)的可觀測性最低，其估計精度最低，不反饋;0＜αk＜1，k∈{1，2，…，n}，第k個狀態(tài)的可觀測性介于第m和第e個狀態(tài)之間，反饋量為αkk．

為使該混合校正濾波器發(fā)揮最優(yōu)性能，在估計誤差較大的濾波初始階段只進行輸出校正，濾波器穩(wěn)定后，再進行混合校正.輸出校正的周期與濾波器周期相同，反饋校正的周期可調(diào)，是濾波周期的整數(shù)倍.

4 仿真與分析

仿真條件:初始位置誤差為[2m 2m 2m]，初始速度誤差為[0.6 m/s 0.6 m/s 0.6 m/s]，初始平臺失準角為[300″300″300″]，陀螺常值漂移為10(°)/h，加速度計常值漂移為1 mg.濾波周期1 s，輸出校正周期1 s，從第51 s開始加入反饋校正，反饋校正周期5 s，仿真時間400 s.

實驗分為兩組:一組為本文給出的基于可觀測性的自適應(yīng)混合校正濾波器;另一組為傳統(tǒng)的混合校正濾波器，即反饋因子為αtra=diag{11…1}，其他參數(shù)相同.自適應(yīng)混合濾波器與傳統(tǒng)的混合濾波器仿真結(jié)果對比如圖3～5所示.

圖3 平臺失準角

圖4 速度誤差

圖5 位置誤差

在0～50 s，兩個濾波器都沒有進行反饋校正，誤差有一定的累積，從51 s開始進行每5 s一次反饋校正.傳統(tǒng)混合濾波器由于沒有考慮系統(tǒng)狀態(tài)可觀測性影響，將狀態(tài)的估計值無論優(yōu)劣全部反饋給SINS，可觀測性低和不可觀測的狀態(tài)估計盲目對SINS進行不完全正確的修正，容易導(dǎo)致校正過度，使估計誤差出現(xiàn)較大范圍波動.以陀螺儀常值漂移ε為例，在第400 s時，傳統(tǒng)混合校正濾波器針對ε的3個反饋因子分別為1、1、1;自適應(yīng)混合校正濾波器中對ε的3個反饋因子分別為0.064 1、0.300 7、0.043 2.而事實上，只有與外部觀測量直接相關(guān)的δv和δR的可觀測性最高，反饋因子接近于1，ε的可觀測性相對較低，反饋因子小于1;則傳統(tǒng)混合濾波器的反饋因子偏離于實際情況，相對于自適應(yīng)混合校正濾波器來說，對SINS修正效果相對較差，對濾波精度的提高有限.

對于GPS接收機，自適應(yīng)混合濾波器與傳統(tǒng)的混合濾波器仿真結(jié)果對比如圖6所示.

圖6 Ⅰ-Q信號

GPS信號變頻到中頻的頻率為ω=4.309 MHz，反饋量由式(9)和式(10)轉(zhuǎn)換為 δⅠ和δQ，對接收機的Ⅰ、Q進行修正，由于信號頻率較高，僅根據(jù)圖6無法比較出兩種方法對接收機Ⅰ、Q校正效果的優(yōu)劣．則可以通過Ⅰ、Q的兩個重要特征參數(shù)ωe和φe來比較．Ⅰ、Q信號可簡寫為［17］其中a為信號的幅值，D(t)為數(shù)據(jù)碼．Ⅰ、Q的初相位誤差φe和角速度誤差ωe與速度誤差和位置誤差有關(guān)，當Ⅰ、Q的初相位誤差和角速度誤差較小時，測量的速度誤差和位置誤差會變?。诶硐霔l件下，ωe和φe均為零，則Ⅰ=aD(t)，Q=0，即數(shù)據(jù)碼完全被解調(diào)出來，由此測量的速度和位置不存在由于信號解調(diào)而引入的誤差．因此可以通過比較ωe和φe來比較兩種混合校正對接收機的校正效果，即ωe和φe越小，Ⅰ、Q的值就越接近于理想情況，導(dǎo)航定位精度越高．經(jīng)兩種方法校正接收機后，ωe和φe如圖7所示(這里沒有考慮a的大小)，從圖中可以看出，經(jīng)本文方法校正后的ωe和φe明顯小于傳統(tǒng)方法的校正結(jié)果，即相比于傳統(tǒng)方法，自適應(yīng)混合校正使得ωe和φe進一步減小，從而在信號解調(diào)過程中引入的誤差更小，進而提高了定位精度．

圖7 Ⅰ-Q信號的初相位誤差與角速度誤差

除了估計精度外，系統(tǒng)的實時性是另一個至關(guān)重要的因素.若反饋因子計算過于復(fù)雜，則系統(tǒng)的實時性勢必會降低.實時性的強弱可以間接由參與計算矩陣的維數(shù)來反映.如果采用文獻［6］中的方法，要計算系統(tǒng)的反饋因子，需要首先計算系統(tǒng)的可觀測性矩陣，假設(shè)可觀測性矩陣的分段時間間隔為1 s，則可觀測性矩陣維數(shù)為136T ×17，T=1，2，…，400，第400 s時系統(tǒng)可觀測性矩陣的維數(shù)將達到54 400×17，計算量隨著時間的延長而劇增，系統(tǒng)實時性將會遭到嚴重破壞.而本文方法，無論時間多長，做可觀測性分析的誤差協(xié)方差陣維數(shù)始終保持17×17，計算量不隨時間的增加而增加，可以有效保持算法的實時性.

5 結(jié)語

重新推導(dǎo)了GPS/SINS超緊組合系統(tǒng)模型的量測方程，將Ham提出的特征值和特征向量可觀測性分析方法應(yīng)用于該系統(tǒng)可觀測性分析，并利用分析結(jié)果制定反饋因子，設(shè)計了一種基于系統(tǒng)狀態(tài)可觀測性的自適應(yīng)混合校正濾波方法.仿真結(jié)果表明，該方法可以有效校正慣導(dǎo)和接收機的原始數(shù)據(jù)，提高系統(tǒng)長時間運行時的導(dǎo)航精度，并且計算量小、實時性好.

［1］秦永元.慣性導(dǎo)航［M］.北京:科學(xué)出版社，2006:355-361.

［2］陳帥.精確制導(dǎo)炸彈低成本慣導(dǎo)/衛(wèi)星組合導(dǎo)航方法研究［D］.南京:南京理工大學(xué)，2008:32-54.

［3］林敏敏，房建成，高國江.GPS/SINS組合導(dǎo)航系統(tǒng)混合校正卡爾曼濾波方法［J］.中國慣性技術(shù)學(xué)報，2003，11(3):29-33.

［4］GOSHEN-MESKIN D，BAR-ITZHACK I Y.Observability analysis of piece-wise constant systems，part I:theory［J］.IEEE Transactions on Aerospace and Electronic Systems，1992，28(4):1056-1067.

［5］GOSHEN-MESKIN D，BAR-ITZHACK IY.Observability analysis of piece-wise constant systems II:application to inertial navigation in-flight alignment［J］.IEEE Transactions on Aerospace and Electronic Systems，1992，28(4):1068-1075.

［6］劉百奇，房建成.一種基于可觀測度分析的SINS/GPS自適應(yīng)反饋校正濾波新方法［J］.航空學(xué)報，2008，29(2):430-436.

［7］馬艷紅，胡軍.基于SVD理論的可觀測度分析方法的幾個反例［J］.中國慣性技術(shù)學(xué)報，2008，16(4):448-452.

［8］BABU S R，WANG J L，RAO G.Analysis of ultra-tight GPS/INS integrated system for navigation performance［C］//IEEE-International Conference on Signal processing， Communications and Networking.Piscataway:IEEE，2008:234-237.

［9］BABU S R，WANG J L.Ultra-tight GPS/INS/PL Integration:a system concept and performance analysis［J］.GPS Solutions，2008，13(1):75-82.

［10］BABU S R.Ultra-tight integration of GPS/Pseudolites/INS:system design and performance analysis［D］.Australia:The University of New South Wales，2006:67-90.

［11］HAM F M，BROWN R G.Observability，eigenvalues，and Kalman filtering［J］.IEEE Transactions on Aerospace and Electronic Systems，1983，19(2):269-273.

［12］GENG Q B，LI N，LI B K.The application of GPS/SINS integration based on Kalman filter［C］//Proceedings of the 31st Chinese Control Conference.Piscataway:IEEE，2012:4607-4610.

［13］FU L，WANG M.An ultra-tightly MIMU/GPS integrated system and its integrity monitoring［C］//Second International Conference on Instrumentation ＆Measurement， Computer， Communication and Control.Piscataway:IEEE，2012:202-205.

［14］高帥和.分布式GPS/SINS超緊組合架構(gòu)下的信號處理和信息融合技術(shù)研究［D］.哈爾濱:哈爾濱工程大學(xué)，2012:3-9.

［15］周衛(wèi)東，蔡佳楠，孫龍，等.GPS/SINS超緊組合導(dǎo)航系統(tǒng)可觀測性分析［J］.北京航空航天大學(xué)學(xué)報，2013，39(9):1157-1162.

［16］程向紅，萬德鈞，鐘巡.捷聯(lián)慣導(dǎo)的可觀測性和可觀測度研究［J］.東南大學(xué)學(xué)報，1997，27(6):6-11.

［17］謝剛.GPS原理與接收機設(shè)計［M］.北京:電子工業(yè)出版社，2009:281-282.