抗差自適應分步濾波算法在PPP/INS組合導航中的應用

張且且，趙 龍，周建華,2

(1.北京航空航天大學自動化科學與電氣工程學院，北京 100191；2.北京衛星導航中心，北京 100094)

0 引言

由于衛星導航和慣性導航組合可以實現優勢互補，提高導航系統的定位精度和可靠性，因此全球導航衛星系統(Global Navigation Satellite System，GNSS)/慣性導航系統 (Inertial Navigation System，INS)組合導航一直是國內外學者的研究重點。近年來，隨著精密單點定位 (Precise Point Positioning，PPP)技術的發展和成熟，將PPP與INS組合，即能克服差分全球導航衛星系統(Differential GNSS，DGNSS)/INS受作業距離限制的缺點，又能獲取高精度的位置、速度和姿態信息，因此很多學者對PPP/INS組合進行了大量的研究工作[1]。

在PPP/INS組合導航定位中，為獲取狀態參數的最優估計，通常采用Kalman濾波。由于濾波器的性能受隨機模型和函數模型精度的影響，因此只有二者都可靠的情況下，才能獲得狀態參數的最優估值。然而，在實際應用中，受各種因素的影響(例如，狀態方程和觀測方程存在誤差，隨機模型所描述的觀測噪聲和過程噪聲通常由經驗值或經驗模型獲得，并不能準確地反映當前系統的真實噪聲水平，觀測量中可能存在非正態分布的異常觀測等)，導致濾波器的性能有時并不理想。為提高濾波器的性能，眾多科研工作者對自適應濾波技術進行了廣泛而深入的研究。為解決根據先驗信息確定的隨機模型不能真實反映系統噪聲水平的問題，提出了對系統噪聲協方差矩陣進行在線估計的方法[2-3]，其中Sage-Husa濾波法[2]應用最為廣泛。該方法利用固定窗口內的新息或殘差估計當前歷元時刻的觀測噪聲協方差矩陣，利用預測狀態殘差估計當前歷元時刻的過程噪聲協方差矩陣[4]。雖然對噪聲協方差矩陣的實時估計可以使預測殘差與相應的統計量保持良好的一致性，但要獲得較好的濾波效果，還需要有準確的函數模型為前提。這是因為系統噪聲的估計與預測的狀態參數存在耦合，當函數模型存在誤差的情況下，會影響噪聲協方矩陣的估計精度，導致濾波器的性能進一步下降。針對該問題，嚴恭敏等[5]利用Allen方差分析實現了對噪聲協方差矩陣估計和濾波狀態參數估計的解耦。但該方法僅適用于緩變系統，對于變化劇烈的系統，該方法的估計精度會明顯變差。周啟帆等[6]針對具有冗余測量的導航系統，提出了基于冗余測量的噪聲方差估計法，可以實現噪聲協方差矩陣的估計和狀態估計誤差的解耦，從而使濾波器結果更加穩定。李正等[7]在此基礎上，進一步對其進行完善，并將其應用到GNSS/SINS組合導航中，提高了導航系統的精度和穩定性。針對狀態模型存在較大誤差的情況，研究人員提出了漸消濾波[8-9]和自適應濾波[10]，它們分別通過漸消因子和自適應因子來調整預測狀態的協方差矩陣，從而控制狀態模型誤差對濾波結果的影響。由于自適應濾波的關鍵是構建自適應因子，因此研究人員主要在如何獲得最佳自適應因子方面做了大量的研究，并提出了多種構建自適應因子的判斷統計量，包括狀態不符統計量[4]、方差分量比統計量[11]和預測殘差統計量[12]。針對觀測量中存在異常觀測的情況，中國科學院院士楊元喜教授基于抗差M估計的原理提出了抗差Kalman濾波算法[13]，通過對觀測量構造等價權矩陣，從而降低異常觀測對濾波結果的影響。為了實現對模型誤差和觀測誤差的自適應處理，將抗差濾波和自適應濾波有機融合，提出了抗差自適應濾波算法[14]。由于抗差自適應濾波能夠同時對模型誤差和觀測誤差進行控制，從而降低它們對濾波結果的影響，其在動態導航定位中得到了廣泛的應用和研究[15-18]。在此基礎上，發展出了各種抗差自適應濾波的改進版本，例如針對具有不同可靠性的狀態參數模型，發展出了分類因子[19]和多因子抗差自適應濾波[20]。盡管如此，抗差自適應濾波需要在某歷元時刻觀測量和動力學模型只出現一個異常的情況下，才會有較好的效果；當觀測量和動力學模型同時出現異常時，會出現抗差自適應濾波同時增大和減小增益矩陣的矛盾。針對該問題，文獻[21]提出了分步抗差自適應濾波方法，基于抗差有偏估計方法首先獲得狀態參數的估值并對觀測量進行修正，然后再進行自適應濾波從而消除動力學模型誤差對定位結果的影響。分類抗差自適應濾波在精密度要求不高的動態導航定位中具有較好的效果，但由于抗差有偏估計的精度有限，并不適用于精密度較高的PPP/INS組合導航中。

本文分析了常規抗差自適應濾波算法在以PPP為核心的動態精密導航定位應用中存在的問題，并基于分類因子自適應濾波原理提出了一種抗差自適應分步濾波算法。該算法將抗差自適應濾波分為2個濾波階段執行，可以準確地識別和分離觀測粗差和運動異常擾動對定位結果的影響。為驗證算法的性能，采用實際的動態測試數據進行模擬測試。測試結果表明，抗差自適應分步濾波算法可以進一步增強濾波算法抵抗觀測粗差和運動異常擾動對定位結果的影響，提高PPP/INS組合導航系統的穩定性和可靠性。

1 PPP/INS緊組合數學模型

對于PPP/INS緊組合濾波模型，狀態參數向量x取為

x=(δr,δv,δφ,ba,bg,bclk,dclk,Zr,BIF)T

(1)

1.1 緊組合狀態方程

將加速度計常值零偏和陀螺儀常值漂移建模為一階馬爾可夫過程，同時將鐘漂、對流層延遲和載波相位偏差建模為高斯白噪聲過程，各參數的狀態模型為

(2)

根據式(1)描述的各參數的狀態模型，可構建狀態方程的一般形式為

(3)

式中，F為狀態轉移矩陣；G為過程噪聲矩陣；Q為過程噪聲協方差矩陣。

1.2 緊組合觀測方程

對于PPP/INS緊組合導航，觀測量由INS推算的偽距、相位和多普勒與GNSS測量得到的偽距、相位和多普勒分別作差得到，其觀測模型分別為

(4)

根據式(3)所述的觀測模型，當觀測衛星數量為n顆時，可構造觀測方程，寫成一般形式為

y=Hx+ε,ε～N(0,R)

(5)

式中，R為觀測噪聲協方差矩陣；觀測向量y和觀測矩陣H分別為

(6)

(7)

2 抗差自適應濾波算法

2.1 常規抗差自適應濾波

觀測粗差和運動模型異常是影響動態導航定位性能的主要因素，為控制觀測粗差和狀態預測信息異常對狀態參數估值的影響，通常采用抗差自適應濾波對參數進行估計。狀態參數的估值可表示為

(8)

(9)

對于自適應因子的構造，文獻[4,11,12]給出了三種構造自適應因子的判別統計量，包括狀態不符值統計量、方差分量比統計量和預測殘差統計量。為避免觀測信息不可靠時對自適應因子構造的影響，通常采用狀態不符值統計量

(10)

(11)

當觀測量中含有粗差時，可通過式(12)構建等價權矩陣來降低含有粗差觀測量的權重，即

(12)

(13)

從式(9)可以看出，通過調節等價權矩陣和自適應因子ak都能改變增益矩陣，從而實現對觀測粗差和預測狀態異常的控制。然而，增大等價權矩陣或減小自適應因子ak都會引起增益矩陣的增大，因此當觀測量和預測狀態信息均含有異常時，如果不能正確地分離觀測異常和預測狀態信息異常對濾波結果的影響，可能會導致對異常信息的誤判，從而得到不真實的等價權矩陣和自適應因子ak，最終導致參數估值向量無法收斂到正確的極值點。此外，采用狀態不符值統計量對運動模型異常進行診斷時，為獲得可靠的診斷結果，由式(11)確定的狀態參數最小二乘抗差解應盡可能準確。然而，在PPP中，最小二乘抗差解忽略了模糊度參數穩定的特性，使基于單歷元觀測信息求得的狀態參數解精度較差，容易對運動模型異常產生誤判。此外，通過式(13)構建觀測量的等價權矩陣時，由于標準化殘差統計量耦合了預測狀態誤差，當運動模型存在異常時，會發生對觀測異常的誤判。綜上分析，抗差自適應濾波算法在PPP/INS組合導航系統的實際應用中，需要解決以下2個問題。

1)采用最小二乘抗差解構造的狀態不符統計量由于精度有限，會對運動模型異常產生誤判。當運動模型出現異常時，由于標準化殘差檢驗統計量耦合了預測狀態誤差，會將運動模型異常誤判為觀測異常。因此，需要對觀測異常和運動模型異常進行準確的識別。

2)當觀測異常和運動模型異常同時存在時，為消除觀測異常需要減小增益矩陣，為消除運動模型異常需要增大增益矩陣，導致二者是矛盾的。因此，需要正確分離觀測異常和運動模型異常對定位結果的影響。

2.2 抗差自適應分步濾波

式(1)中所描述的各狀態參數，由于對流層延遲參數、不發生周跳時的模糊度參數、加速度計和陀螺儀的常值零偏通常是比較穩定的，采用隨機游走過程可以較為準確地描述其動態模型；而對于位置、速度、姿態角、鐘差和鐘飄狀態參數，由于受各種因素的影響難以構建準確地動態模型。因此，根據各狀態參數動態模型的準確性可以將狀態參數分為兩類，即

(14)

式中，x1為第一類動態模型不準確的狀態參數，包括位置、速度、姿態、鐘差和發生周跳時的模糊度參數；x2為第二類動態模型準確已知的狀態參數，包括對流層延遲、不發生周跳的模糊度參數、陀螺儀常值漂移和加速度計常值零偏。

對上述兩類狀態參數分別構建自適應因子

(15)

式中，ax1和ax2分別為第一類和第二類狀態參數的自適應因子。根據分類因子自適應濾波原理[19]，狀態參數的自適應濾波解可表示為

(16)

(17)

根據分類因子自適應濾波[19]的原理可知，對某一類狀態參數的自適應因子進行調節，則僅改變該類狀態的預測信息對狀態參數估值的貢獻程度；當自適應因子設置為ax1=0時，表示該類狀態參數的預測信息將不對狀態參數的估值產生影響；當自適應因子設置為ax1=1時，則表示不改變狀態預測信息對狀態參數估值的影響。在PPP/INS組合導航定位中，由于第二類狀態參數的狀態模型是準確已知的，因此不需要對其進行調節，并將其自適應因子取為ax2=0，而僅對第一類狀態參數的自適應因子ax1進行調節?；谏鲜龇治?，本文提出了一種定位抗差自適應分步濾波算法，其流程如圖1所示。

圖1 PPP/INS抗差自適應分步濾波算法流程Fig.1 Flow chart of two-step robust adaptive filtering algorithm

從圖1中可知，PPP/INS抗差自適應分步濾波算法可概括為以下3個步驟。

步驟1：基于預測殘差ξ對觀測異常和運動異常進行整體檢驗，檢驗統計量構造為

(18)

式中，χ2表示卡方檢驗；m為觀測量個數；Qξ為預測殘差協方差矩陣。給定顯著性檢驗水平a，如果Tξ>χ2(1-a,m)，則執行抗差自適應分步濾波算法，對觀測異常和運動異常分步進行檢測和處理。

(19)

根據預測殘差與觀測殘差和預測狀態誤差之間的關系，式(19)可改寫為

(20)

(21)

(22)

(23)

并基于三段權函數式(10)獲取第一類狀態參數的自適應因子ax1。根據式(17)重新計算預測狀態向量的等價權矩陣，并根據式(21)獲取第2階段濾波的狀態參數估值作為最終輸出。

綜上所示，抗差自適應分步濾波算法在執行第1階段濾波時，將模型不準確的第一類狀態參數對應的自適應因子設置為ax1=0，從而降低了運動模型的預測信息對定位結果的貢獻，此時基于新息的卡方檢驗統計量將主要反映觀測異常；在執行第2階段濾波時，將自適應因子重置為ax1=1，由于第1階段濾波已經對觀測異常進行了處理，此時基于新息的檢驗統計量將主要反映運動異常。因此，本文提出的抗差自適應分步濾波算法可實現對觀測異常和狀態預測信息異常的有效識別和分離。

3 實驗測試與分析

為測試抗差自適應分步濾波算法在PPP/INS組合導航系統中實際應用的性能，采用NovAtel公司的SPAN GNSS/INS組合導航設備于2018年6月30日在北京航空航天大學體育場采集了一組時長約20min的小推車動態測試數據，該測試數據衛星觀測質量較好。實驗設備及安裝示意圖如圖2所示，該組合導航設備所搭載的慣性測量單元(Iner-tial Measurement Unit，IMU)傳感器精度為戰術級，采樣頻率為125Hz，其性能指標如表1所示。

圖2 實驗設備及安裝示意圖Fig.2 Experimental equipment and installation diagram

表1 SPAN PwrPak7-E1設備IMU (G320N) 性能參數

對采集的測試數據作如下處理：在300～310歷元時刻，在偽距和相位觀測量中分別隨機加入1m和20m的粗差模擬觀測異常；在700～710歷元時刻，在慣性傳感器采集的加速度數據中加入0.5m/s2的加速度誤差模擬運動異常擾動；在1000～1010歷元時刻，同時引入觀測粗差和運動異常擾動。采用以下三種濾波方案分別進行定位解算：

方案1：擴展Kalman濾波(Extended Kalman Filter, EKF)；

方案2：常規抗差自適應濾波(Robust Adaptive Kalman Filter, RAKF)；

方案3：抗差自適應分步濾波(RA2KF)。

采用上述三種方案對PPP/INS組合導航進行濾波解算，其中方案2和方案3中觀測粗差和運動異常擾動的檢驗閾值均設置為k0=1.3、k1=3.5、c0=1.5和c1=5.0。各方案解算的位置、速度和姿態誤差的時間序列如圖3～圖5所示，其統計結果如表2所示。由方案3算法計算得到的各歷元時刻的自適應因子如圖6所示，其對觀測異常和運動異常的檢測結果如圖7所示，圖中T1和T2分別為觀測異常和運動模型異常檢驗統計量。

從圖3、圖4、圖5和表2中可以看出，在3處觀測量中含有粗差和運動模型存在異常擾動的歷元時刻，方案1獲得的位置、速度和姿態導航參數均出現了較大的偏差，其位置和速度誤差在3個方向(東，北，天)上的統計結果分別為(14.7cm，8.9cm，40.5cm)和(4.7cm/s，7.8cm/s，22.2cm/s)，航向、橫滾和俯仰角誤差分別為18.44°、0.73°和1.399°;方案2獲得的位置和速度誤差在3個方向(東，北，天)上的統計結果分別為(13.7cm，7.4cm，34cm)和(4.2cm/s，5.7cm/s，6.6cm/s)，航向、橫滾和俯仰角誤差分別為10.19°、0.424°和0.735°。方案2比方案1在3個方向(東，北，天)上的定位精度和速度精度分別提高了(1.0cm，1.6cm，6.5cm)和(0.5cm/s，2.1cm/s，15.6cm/s)，航向、橫滾和俯仰角精度分別提高了8.25°、0.30°和0.66°。方案2獲得導航定位參數的精度和穩定性均比方案1有所改善，但在700～710歷元和1000～1010歷元時刻仍出現小幅波動，這是由于常規抗差自適應濾波算法未能準確識別和分離觀測粗差和運動異常擾動。方案3獲得的位置和速度誤差在3個方向(東，北，天)上的統計結果分別為(13.2cm，7.0cm，33.7cm)和(4.0cm/s，4.5cm/s，4.6cm/s)，航向、橫滾和俯仰角誤差分別為9.901°、0.25°和0.674°。方案3比方案2在3個方向(東，北，天)上的定位精度和速度精度分別提高了(0.5cm，0.3cm，0.3cm)和(0.2cm/s，1.2cm/s，2.0cm/s)，航向、橫滾和俯仰角精度分別提高了0.289°、0.174°和0.06°。方案3的解算結果與方案2相比，其精度和穩定性得到進一步改善，這是因為方案3對觀測粗差和運動異常擾動進行了正確識別和分離。從圖6和圖7中可以看出，抗差自適應分步濾波算法準確地檢測出了3處觀測異常和運動異常擾動，在運動模型存在異常擾動的歷元時刻，抗差自適應分步濾波算法通過調節自適應因子來降低運動異常對定位結果的影響。

圖3 三種濾波方案下PPP/INS組合導航位置誤差Fig.3 Position errors of PPP/INS under the three filtering schemes

圖4 三種濾波方案下PPP/INS組合導航速度誤差Fig.4 Velocity errors of PPP/INS under the three filtering schemes

圖5 三種濾波方案下PPP/INS組合導航姿態誤差Fig.5 Attitude errors of PPP/INS under the three filtering schemes

表2 位置、速度和姿態誤差的RMS統計結果

圖6 RA2KF濾波方案各歷元時刻的自適應因子aFig.6 Adaptive factors calculated by RA2KF

圖7 RA2KF濾波方案對觀測異常和運動異常的檢驗結果Fig.7 Detection results of observation gross errors and kinematic abnormality by RA2KF

綜上所述，采用抗差自適應分步濾波算法可以有效增強PPP/INS組合導航系統的性能，提高組合導航系統的魯棒性和穩定性。

4 結論

本文針對常規抗差自適應濾波算法存在難以準確識別和分離觀測粗差及運動異常擾動對定位結果影響的問題，基于對PPP/INS組合導航狀態參數特征的分析，并借鑒分類因子自適應濾波原理，提出了一種抗差自適應分步濾波算法。算法分析與實驗結果表明：

1)抗差自適應濾波將抗差與自適應分為2個濾波階段執行，實現了對觀測粗差和運動異常擾動的準確識別和分離。因此具有比常規抗差自適應濾波算法更優的定位性能，可以顯著地增強濾波算法抵抗觀測粗差和運動異常擾動對定位結果的影響，提高PPP/INS組合導航定位結果的穩定性和可靠性。

2)本文提出的抗差自適應分步濾波算法主要針對狀態參數模型具有不同精度的一些應用對象，而對于狀態模型都不準確的應用對象需要進一步研究。