GPS/SⅠNS超緊組合導航系統自適應混合濾波算法

周衛東，蔡佳楠，孫 龍

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

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

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

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

1 GPS/SINS超緊組合導航系統誤差模型

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

在GPS接收機中，接收到的衛星信號經變頻和解調后進入到Ⅰ-Q通道，Ⅰ-Q信號的期望值與載體的速度誤差及位置誤差滿足如下關系式:

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

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

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

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

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

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

圖1 GPS/SⅠNS超緊組合導航系統

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

由此推導得到量測方程是關于Re和ve的非線性方程［14］．為此將Re和ve重新定義為

并且

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

觀測矩陣為

其中:

其中i=1，2，…，n為跟蹤衛星的數目．

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

其協方差為

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

拉格朗日方程滿足

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

則

當且僅當

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

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

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

其中

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

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

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

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

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

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

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

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

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

圖2 混合校正濾波器

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

為使該混合校正濾波器發揮最優性能，在估計誤差較大的濾波初始階段只進行輸出校正，濾波器穩定后，再進行混合校正.輸出校正的周期與濾波器周期相同，反饋校正的周期可調，是濾波周期的整數倍.

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.

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

圖3 平臺失準角

圖4 速度誤差

圖5 位置誤差

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

對于GPS接收機，自適應混合濾波器與傳統的混合濾波器仿真結果對比如圖6所示.

圖6 Ⅰ-Q信號

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

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

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

5 結語

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

［1］秦永元.慣性導航［M］.北京:科學出版社，2006:355-361.

［2］陳帥.精確制導炸彈低成本慣導/衛星組合導航方法研究［D］.南京:南京理工大學，2008:32-54.

［3］林敏敏，房建成，高國江.GPS/SINS組合導航系統混合校正卡爾曼濾波方法［J］.中國慣性技術學報，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自適應反饋校正濾波新方法［J］.航空學報，2008，29(2):430-436.

［7］馬艷紅，胡軍.基于SVD理論的可觀測度分析方法的幾個反例［J］.中國慣性技術學報，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超緊組合架構下的信號處理和信息融合技術研究［D］.哈爾濱:哈爾濱工程大學，2012:3-9.

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

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

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