蔡保杰，邵 雷，李佳偉，李正杰

（1.空軍工程大學(xué) 防空反導(dǎo)學(xué)院，西安 710051；2.西安電子科技大學(xué) 電子工程學(xué)院，西安 710071）

0 引言

導(dǎo)航系統(tǒng)在航空航天和日常生活中應(yīng)用廣泛。導(dǎo)航的方法有很多種，慣性導(dǎo)航和衛(wèi)星導(dǎo)航的組合導(dǎo)航的方法是最為常用的1 種[1]。在運(yùn)載體運(yùn)動(dòng)過(guò)程中，由于所處環(huán)境錯(cuò)綜復(fù)雜，衛(wèi)星導(dǎo)航的觀測(cè)值會(huì)出現(xiàn)損壞的情況，嚴(yán)重影響導(dǎo)航的精度。

對(duì)于觀測(cè)值故障問(wèn)題[2-3]，常用的是傳統(tǒng)抗差卡爾曼（Kalman）濾波方法[4]，文獻(xiàn)[5]中構(gòu)造了1 個(gè)故障檢驗(yàn)公式，與設(shè)定的卡方檢驗(yàn)閾值相比較，對(duì)檢驗(yàn)值進(jìn)行分段處理。文獻(xiàn)[6]先用假設(shè)檢驗(yàn)的方法檢測(cè)出故障觀測(cè)值的具體位置，用設(shè)定的門(mén)限值代替觀測(cè)噪聲值，可以較大程度地減小粗差的影響。文獻(xiàn)[7]在一步預(yù)測(cè)均方誤差陣上乘1 個(gè)漸消因子，進(jìn)行自適應(yīng)濾波。文獻(xiàn)[8]通過(guò)降低濾波增益值來(lái)達(dá)到修正預(yù)測(cè)值的效果。文獻(xiàn)[9]提出錯(cuò)誤預(yù)警率和故障探測(cè)率的概念，對(duì)故障的定位更加精確。文獻(xiàn)[10]通過(guò)等價(jià)權(quán)函數(shù)法對(duì)觀測(cè)噪 聲方差陣進(jìn)行替換，可以實(shí)時(shí)有效地濾除粗差。

上述方法都能一定程度地減小粗差帶來(lái)的影響，但也存在不足之處?？ǚ綑z驗(yàn)并不能保證百分之百的正確率，適當(dāng)?shù)剡x取閾值尤為重要。當(dāng)檢測(cè)系統(tǒng)出現(xiàn)誤檢情況時(shí)，用門(mén)限值代替正確的觀測(cè)值也會(huì)造成觀測(cè)值誤差相對(duì)增大。針對(duì)這種情況，本文提出1 種基于卡方檢驗(yàn)和牛頓插值的抗差濾波方法，利用前幾個(gè)時(shí)刻的觀測(cè)值組成的牛頓插值多項(xiàng)式進(jìn)行外延，對(duì)當(dāng)前時(shí)刻的故障觀測(cè)值進(jìn)行替換，以使組合導(dǎo)航系統(tǒng)穩(wěn)定性更高，抗差能力更強(qiáng)。

1 組合導(dǎo)航系統(tǒng)建模與常用的抗差濾波方法概述

1.1 組合導(dǎo)航系統(tǒng)建模

導(dǎo)航系統(tǒng)能夠測(cè)量并解算出運(yùn)載體的瞬時(shí)運(yùn)動(dòng)狀態(tài)和位置，提供給駕駛員或自動(dòng)駕駛儀，以實(shí)現(xiàn)對(duì)運(yùn)載體的正確操縱或控制。隨著科學(xué)技術(shù)的發(fā)展，可供導(dǎo)航系統(tǒng)利用的信息源越來(lái)越多，導(dǎo)航系統(tǒng)種類也越來(lái)越多。捷聯(lián)慣導(dǎo)(strapdown inertial navigation system， SINS)和全球衛(wèi)星導(dǎo)航系統(tǒng)(global navigation satellite system， GNSS)所構(gòu)成的組合導(dǎo)航是近年來(lái)導(dǎo)航領(lǐng)域最常使用的1 種方法，SINS 作為主導(dǎo)航系統(tǒng)，利用慣性器件實(shí)時(shí)解算出運(yùn)載體的位置、速度等信息，而GNSS 也可以對(duì)SINS 不斷地進(jìn)行修正，避免了捷聯(lián)慣導(dǎo)系統(tǒng)誤差隨時(shí)間增大的問(wèn)題。

SINS/GNSS 的組合方式有松組合[11-13]、緊組合、深組合等。松組合的狀態(tài)量為

式中： δL 為經(jīng)度誤差； δλ 為緯度誤差； δ h為高度誤 差；δ v = [ δ vx， δ vy， δvz]為 速度誤差；φ =[φx，φy，φz]為 姿 態(tài) 角 誤 差； ε=[εx， εy，εz]為 陀 螺 儀 漂 移 誤 差?=[?x，?y，?z]為 加 速 度 計(jì) 零 偏 誤 差。其 狀 態(tài) 方 程離散化后的形式為

式中： Ak,k-1為狀態(tài)轉(zhuǎn)移矩陣；W 為系統(tǒng)噪聲。當(dāng)狀態(tài)向量為15 維時(shí)， Ak,k-1是1 個(gè)15 維的方陣，其非零項(xiàng)為： 其中：ωie為地球自轉(zhuǎn)角速度；fx、fy、fz為加速度計(jì)測(cè)得的比力信息；R 為地球半徑。SINS/GNSS 的觀測(cè)模型取GNSS 和SINS 輸出的位置和速度之差作為 觀 測(cè) 值，觀 測(cè) 向 量 為 Zk= [rGNSS- rSINS，vGNSS-vSINS]T。其中： r =[ L， λ， h]； v =[ vx， vy， vz]。

量測(cè)方程為

式中： Hk為量測(cè)矩陣；Vk為量測(cè)噪聲。 Hk為1 個(gè)6 行15 列的矩陣，其非零元素為： H1，2=R cos L ;H2，1=R; H4，4=1; H5，5=1; H6，6=1。

1.2 2 種常用的抗差濾波方法

1.2.1 等價(jià)權(quán)函數(shù)法

等價(jià)權(quán)函數(shù)法[9,14]為調(diào)整觀測(cè)噪聲協(xié)方差矩陣 kR 的大小，抗差卡爾曼濾波在第k 時(shí)刻的估計(jì)值可以表示為

而式（4）中濾波增益為

式中： Pk，k-1為一步預(yù)測(cè)均方誤差陣；Rk為 Rk的等價(jià)權(quán)矩陣，而Rk可以通過(guò)丹麥法、胡貝爾（Huber）函數(shù)或通過(guò)中國(guó)科學(xué)院測(cè)量與地球物理研究所(Institute of Geodesy & Geophysics，Chinese Academy of Sciences， IGG)方案獲得，即

式中：M、N 為常值； ，1kkZ-? 為標(biāo)準(zhǔn)化殘差。此方法可以直接利用觀測(cè)值作為評(píng)判標(biāo)準(zhǔn)，計(jì)算量小，易于實(shí)現(xiàn)，但若系統(tǒng)未能檢測(cè)到小周跳，仍會(huì)有較大誤差出現(xiàn)。

1.2.2 卡方檢驗(yàn)法

卡爾曼濾波是1 種線性最小方差估計(jì)，是1 個(gè)遞推的過(guò)程，而在每1 步遞推的過(guò)程中，殘差是1 個(gè)很重要的信息來(lái)源，它包含有1 步預(yù)測(cè)誤差信息。當(dāng)導(dǎo)航系統(tǒng)發(fā)生故障時(shí)，殘差序列會(huì)有異常變化。利用殘差序列的這個(gè)特性，可以對(duì)故障觀測(cè)值進(jìn)行檢測(cè)。此方法分2 步進(jìn)行，即先對(duì)故障進(jìn)行檢測(cè)，再對(duì)有故障的觀測(cè)值進(jìn)行處理?？ǚ綑z驗(yàn)的方法在計(jì)算上較為復(fù)雜，但其檢測(cè)精度較高，故本文 采用卡方檢驗(yàn)的方法。

2 基于牛頓插值的抗差卡爾曼濾波算法

2.1 采用卡方檢驗(yàn)的故障檢測(cè)方法

2.1.1 卡方檢驗(yàn)方法概述

由1 步預(yù)測(cè)值 Xk，k-1代替真實(shí)值 Xk引起的誤差為： X?k，k-1= Xk-X?k，k-1。1 步預(yù)測(cè)與觀測(cè)值相比較的差值為

式中kS 為殘差序列的協(xié)方差矩陣，即

設(shè) H0為原假設(shè)， H1為備選假設(shè)， H0、 H1分別定義為

式中：n 為殘差序列的維數(shù)；γ 為非中心化參數(shù)。取顯著性水平為α，α 的大小直接決定了誤差的判定條件的寬松程度。設(shè)檢測(cè)量容許的最大值為T(mén) ( n )， T ( n) =( n)，一般將 T ( n )稱為閾值，當(dāng)檢驗(yàn)量的值超過(guò)閾值 T ( n )時(shí)，認(rèn)為發(fā)生故障[15-16]。

載體運(yùn)動(dòng)的過(guò)程中，引起觀測(cè)值損壞的因素有很多，當(dāng)出現(xiàn)鏈路中斷等情況時(shí)，會(huì)造成當(dāng)前時(shí)刻的觀測(cè)值全部損壞；但有時(shí)可能只是因?yàn)閭€(gè)別儀器儀表的故障，并不會(huì)使觀測(cè)值全部失效，這時(shí)若將當(dāng)前時(shí)刻全部觀測(cè)值當(dāng)作故障值顯然不妥，對(duì)導(dǎo)航精度也會(huì)造成一定影響。所以本文采用單個(gè)觀測(cè)值的故障檢測(cè)系統(tǒng)。設(shè)k 時(shí)刻第i 個(gè)觀測(cè)值為 Dk( i )，這時(shí)的原假設(shè)和備選假設(shè)可以定義為

令 T (1)= χ2(1 ) ，當(dāng)統(tǒng)計(jì)量 Dk( i ) ＜χ2(1 ) 時(shí)，認(rèn)為系統(tǒng)無(wú)故障；當(dāng) Dk( i ) ＞ χ2(1 ) 時(shí)，認(rèn)為導(dǎo)航系統(tǒng)觀測(cè)值存在故障。

2.1.2 閾值的選取

在實(shí)際的運(yùn)動(dòng)過(guò)程中，載體所處的環(huán)境是比較復(fù)雜的，對(duì)觀測(cè)值故障的容忍程度也會(huì)變大，所以選取的閾值也相對(duì)較大。當(dāng)閾值選取過(guò)小時(shí)，會(huì)把一些正確的觀測(cè)值當(dāng)作故障觀測(cè)值來(lái)處理，隨著時(shí)間的推移會(huì)出現(xiàn)累積誤差；當(dāng)閾值選取過(guò)大時(shí)，被認(rèn)定為出現(xiàn)故障的觀測(cè)值就會(huì)變得很少，系統(tǒng)幾乎檢測(cè)不到故障，使抗差濾波之前和之后的曲線接近重合。

表1 為顯著性水平α 和 ( )χ21 的分布表。

表1 顯著性水平和閾值的關(guān)系

由表1 可知，顯著性水平α 越小， χ2(1 ) 的值越大，故障的判定條件就越寬松。本文通過(guò)大量的實(shí)驗(yàn)分析，發(fā)現(xiàn)當(dāng)顯著性水平α 為0.25～0.01、閾值T (1)為5.024～6.635 時(shí)可以取得較好的效果，如圖1所示?？ǚ綑z驗(yàn)統(tǒng)計(jì)量分布在6.635 以下的概率為 96.8%，以閾值為6.635 作為判定條件可以較為精確地檢測(cè)到故障，誤檢的概率較小，所以本文選取6.635 作為閾值進(jìn)行仿真驗(yàn)證。

圖1 卡方檢驗(yàn)統(tǒng)計(jì)量分布

2.2 利用牛頓插值修正觀測(cè)值

插值法[17]是利用已知函數(shù) f ( x )某個(gè)區(qū)間的若干點(diǎn)，構(gòu)造1 個(gè)近似函數(shù) P ( x )來(lái)近似代替 f ( x )，牛頓插值可以根據(jù)計(jì)算精度靈活增加插值節(jié)點(diǎn)進(jìn)行遞推運(yùn)算，公式形式對(duì)稱、結(jié)構(gòu)緊湊，容易編寫(xiě)程序。本文采用的牛頓插值方法是將近似函數(shù)P ( x )進(jìn)行1 個(gè)單位的外延。

經(jīng)典組合導(dǎo)航系統(tǒng)中，每次濾波過(guò)程的間隔時(shí)間一般是固定的，可以使用等距的插值公式，其表達(dá)式[18]為

據(jù)式（13）便可用前n 個(gè)時(shí)刻的函數(shù)值對(duì)當(dāng)前時(shí)刻函數(shù)值進(jìn)行插值。

一般來(lái)說(shuō)，插值節(jié)點(diǎn)取得越多，利用牛頓插值構(gòu)造的曲線與原函數(shù)曲線近似程度越高，誤差越小，適當(dāng)增加節(jié)點(diǎn)數(shù)對(duì)計(jì)算結(jié)果的準(zhǔn)確度是有幫助的，但并非插值多項(xiàng)式的次數(shù)越高越好。當(dāng)插值節(jié)點(diǎn)增多時(shí)，不能保證非節(jié)點(diǎn)處的插值精度得到改善，有時(shí)反而誤差更大。龍格現(xiàn)象表明，大范圍內(nèi)使用高次插值，逼近的效果往往是不理想的。根據(jù)大量實(shí)驗(yàn)驗(yàn)證，牛頓插值在4 階時(shí)精度較高，出現(xiàn)龍格現(xiàn)象的概率也比較小，所以本文將以4 階牛頓插值多項(xiàng)式來(lái)進(jìn)行實(shí)驗(yàn)驗(yàn)證?；诳ǚ綑z驗(yàn)和牛頓插值的抗差濾波算法[13]為：

1）計(jì)算狀態(tài)量的1 步預(yù)測(cè)值

2）計(jì)算1 步預(yù)測(cè)的均方誤差值

3）計(jì)算濾波增益值

4）計(jì)算估計(jì)均方誤差待下1 時(shí)刻使用

仿真步長(zhǎng)為1 s，共用時(shí)500 s，為營(yíng)造與飛機(jī)飛行相近的環(huán)境，人為地給觀測(cè)值加入噪聲，加入噪聲的數(shù)值如表3 所示。

表3 隨機(jī)噪聲值

5）根據(jù)殘差向量計(jì)算當(dāng)前時(shí)刻 kD 值

6）確定觀測(cè)值

式 中 βk=Diag[β1， β2,… ,βn](i =1， 2， … , n)。

7）得到估計(jì)值

3 仿真分析及驗(yàn)證

3.1 仿真軌跡生成

本文模擬1 條飛機(jī)飛行軌跡來(lái)驗(yàn)證抗差濾波算法的有效性，飛行軌跡如圖2 所示，將模擬的軌跡數(shù)據(jù)送入軌跡發(fā)生器，分別得到慣性導(dǎo)航系統(tǒng)(inertial navigation system， INS）和全球定位系統(tǒng)(global positioning system， GPS）的數(shù)據(jù)，各誤差值如表2 所示。

為驗(yàn)證本抗差算法的有效性，制造量測(cè)不準(zhǔn)確的外部環(huán)境，仿真在每隔10 s 的時(shí)刻加入1 個(gè)噪聲放大系數(shù)m，使此時(shí)刻的噪聲含有m 倍粗差。

3.2 分析與對(duì)比

本文對(duì)比使用上述抗差方法和不使用抗差方法的位置和速度誤差，如圖3、圖4 所示。

圖3 抗差濾波前后位置誤差

圖4 抗差濾波前后速度誤差

考慮到當(dāng)GNSS 信號(hào)受到影響時(shí)，INS 仍然處于工作狀態(tài)，分析對(duì)比了使用本文提出的抗差方法和僅使用INS 的誤差，如圖5、圖6 所示。

圖5 只使用慣導(dǎo)和使用抗差濾波后位置誤差

從圖3、圖4 可以看出，當(dāng)不使用任何抗差方法時(shí)，由于GNSS 信號(hào)誤差非常大，造成觀測(cè)值發(fā)生急劇變化，如表4 所示。在使用本文提出的基于牛頓插值的抗差濾波方法后，對(duì)GNSS 所收到的觀測(cè)值進(jìn)行改良，使觀測(cè)值恢復(fù)到正常范圍，使位置和速度誤差大大減小，如表5 所示。

慣性導(dǎo)航誤差是隨著時(shí)間逐漸增大的，沒(méi)有衛(wèi)星導(dǎo)航的實(shí)時(shí)修正，慣性導(dǎo)航方法無(wú)論是位置誤差還是速度誤差都會(huì)慢慢增大，與此相比，本文提出的基于牛頓插值的抗差濾波方法能較好地使誤差維持在1 個(gè)較小的范圍內(nèi)，使導(dǎo)航精度得到提高，如圖5、圖6 所示。但不可否認(rèn)的是，慣性導(dǎo)航在獨(dú)立工作時(shí)間較短時(shí)，導(dǎo)航精度仍有一定的準(zhǔn)確性。與此相比，由于在導(dǎo)航數(shù)據(jù)已有誤差的前提下又人為加入噪聲，致使使用本文提出的抗差方法在短時(shí)間內(nèi)導(dǎo)航精度比只使用慣性導(dǎo)航精度略低，北向速度誤差和天向速度誤差在個(gè)別點(diǎn)處出現(xiàn)較大波動(dòng)，如圖6(b)、圖6(c)所示。但北向速度誤差值仍在-1.78～0.57 m/s，天向速度誤差仍在-0.62～0.59 m/s 這樣1 個(gè)較小的范圍內(nèi)，且隨著時(shí)間的推移，慣性導(dǎo)航誤差增大的現(xiàn)象慢慢顯現(xiàn)出來(lái)。綜合對(duì)比來(lái)看，基于牛頓插值的抗差卡爾曼濾波新算法仍不失為1 種有效的抗差濾波方法。

表4 抗差濾波前誤差值

表5 抗差濾波后誤差值

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

本文對(duì) INS/GNSS 導(dǎo)航系統(tǒng)在運(yùn)動(dòng)過(guò)程中GNSS 信號(hào)會(huì)出現(xiàn)故障問(wèn)題，提出1 種基于牛頓插值的抗差卡爾曼濾波算法。該算法通過(guò)當(dāng)前時(shí)刻單個(gè)觀測(cè)值的卡方檢驗(yàn)方法，通過(guò)大量實(shí)驗(yàn)選取合適的閾值，對(duì)故障觀測(cè)值進(jìn)行更精確的定位。用過(guò)去幾個(gè)時(shí)刻的觀測(cè)值組成牛頓插值多項(xiàng)式進(jìn)行外延，得到當(dāng)前時(shí)刻觀測(cè)值，給出了基于牛頓插值的抗差卡爾曼濾波方法的具體過(guò)程。最后，通過(guò)大量的仿真分析進(jìn)行驗(yàn)證，該算法使誤差控制在1 個(gè)較小的范圍內(nèi)，具有較好的抗差效果。該算法提高了抗差濾波系統(tǒng)的穩(wěn)定性，計(jì)算簡(jiǎn)單，易于工程實(shí)現(xiàn)。