抗差自適應(yīng)無跡Kalman濾波在GPS/INS組合導(dǎo)航中的應(yīng)用

張 群 洪志強

(1. 廣東廣量測繪信息技術(shù)有限公司, 廣東 東莞 523000; 2. 南昌工學(xué)院 人工智能學(xué)院, 江西 南昌 330108)

0 引言

全球衛(wèi)星系統(tǒng)(Global Navigation Satellite System,GNSS)已成為許多測量和精密導(dǎo)航應(yīng)用中常用的導(dǎo)航和定位技術(shù)[1]。然而,在動態(tài)環(huán)境中,GNSS信號可能會由于外界環(huán)境干擾而在被部分測距信號中缺失,導(dǎo)致定位性能降低。與GNSS相比,慣性導(dǎo)航系統(tǒng)(Inertial Navigation System,INS)是一個完全獨立自主的導(dǎo)航系統(tǒng),從這個意義上講,它不依賴于任何外部來源。但是,INS很難維持長時間的導(dǎo)航定位性能。因此,將GNSS與INS進行組合,可實現(xiàn)兩者的優(yōu)勢互補。

實現(xiàn)GNSS/INS組合的一種重要方法便是標準Kalman濾波方法[2]。然而,顧及GNSS/INS組合模型中動態(tài)模型的非線性特點(以GNSS/INS松組合為例[3]),其動態(tài)模型本身是非線性的,采用標準Kalman濾波方法需要對動態(tài)模型進行一階求導(dǎo)。針對上面的問題,現(xiàn)有文獻采用無跡變換的思路處理動態(tài)預(yù)測模型[4-5],可避免對動態(tài)方程的求導(dǎo)工作,也有相關(guān)文獻研究了采用該種模式對導(dǎo)航定位精度的提升效果[6];但是采用無跡變化的思想存在計算量隨著狀態(tài)量維度的增加而增大的問題。另一方面,在實際導(dǎo)航應(yīng)用中,信號部分遮擋、多路徑等因素導(dǎo)致衛(wèi)星觀測值可能出現(xiàn)較大觀測粗差以及慣導(dǎo)器件參數(shù)發(fā)生等,導(dǎo)致系統(tǒng)預(yù)測異常,無論采用標準Kalman濾波方法還是無跡Kalman濾波方法均無法有效處理這些異常信息。常用的針對觀測值異常的抗差Kalman濾波方法[7-8]與僅針對動態(tài)異常的自適應(yīng)Kalman濾波方法[9]僅能夠處理單一異常出現(xiàn)時的情況,能夠同時兼顧兩者的方法不多見。因此,針對上述兩方面的問題,本文根據(jù)導(dǎo)航系統(tǒng)動態(tài)模型自身的特點,即可表達為與部分狀態(tài)量非線性相關(guān)(姿態(tài)四元數(shù))而與部分狀態(tài)量(速度和位置)線性相關(guān);因此,結(jié)合邊際無跡變換的思想(Marginalised Unscented Transformation,MUT)[10],在采用無跡變換時,僅需對呈現(xiàn)非線性相關(guān)的狀態(tài)量進行sigma的計算與傳播即可,大大降低了計算量;針對存在觀測粗差與系統(tǒng)異常預(yù)測的情況,本文采用文獻[11]的思想,自動更新新息向量的協(xié)方差矩陣，達到兼顧抗差性與自適應(yīng)性的目的。

1 GPS/INS松組合模型

1.1 動力學(xué)模型

動力學(xué)模型中，姿態(tài)更新可采用四元數(shù)微分方程[10]

(1)

(2)

(3)

(4)

離散時間形式下的速度更新方程可寫為

(5)

n系下,慣導(dǎo)的位置更新可表示為如下離散方程的形式

(6)

其中,rk和rk-1分別為k和k-1歷元慣導(dǎo)中心在n系下的位置矢量。

1.2 觀測模型

(7)

2 一種兼顧抗差與自適應(yīng)性無跡Kalman濾波方法

2.1 結(jié)合邊際無跡轉(zhuǎn)換與GPS/INS松組合模型

式(1)、(4)和(6)可寫為矩陣形式

(8)

可見式(8)中等號右邊項對于姿態(tài)四元數(shù)、角增量及其誤差向量是非線性相關(guān)的,而相對于速度和位置是線性相關(guān)的。因此,可通過條件線性變換將上式改寫為

(9)

(10)

(11)

2.2 抗差與自適應(yīng)因子的引入與確定

采用無跡Kalman濾波方法處理組合系統(tǒng)的步驟如下:

通過式(9)預(yù)測下一歷元狀態(tài)預(yù)測值(預(yù)測量用“-”表示,驗后量用“+”表示)

(12)

并計算新息向量

(13)

(14)

計算增益矩陣Kk:

(15)

則驗后狀態(tài)估值及其方差陣為

(16)

(17)

文獻[11]指出,當(dāng)系統(tǒng)存在異常觀測值或狀態(tài)方程異常預(yù)測時,可依據(jù)Mahalanobis距離的平方滿足卡方分布來檢驗,即須判斷式(17)、(18):

(17)

(18)

(19)

因此,放大因子的求解可分別通過牛頓法求解

(20)

(21)

2.3 方法實現(xiàn)

(1)獲得初始位置、速度和姿態(tài)的參數(shù)值及其方程陣;

(2)顧及狀態(tài)方程的條件非線性特性,圍繞姿態(tài)參數(shù)的均值和方差采樣sigma點(考慮到姿態(tài)四元素的維度不匹配問題,這里須將姿態(tài)四元素轉(zhuǎn)換成羅德里格參數(shù)進行計算[10]);

(4)不引人放大因子,根據(jù)式(13)～(17)計算無跡Kalman濾波結(jié)果(結(jié)果1);

(7)將(4)～(6)中三種方法確定的驗后參數(shù)估值作為基準,預(yù)測三者下一歷元所對應(yīng)的新息向量,并將三種預(yù)測新息所確定的Mahalanobis距離[式(17)]的最小值對應(yīng)的驗后狀態(tài)估計作為該歷元的最終估計值。

3 算例與分析

圖1 車載實驗軌跡

為了驗證本文采用方法的有效性,采用如下三種方案進行試驗:

方案1(僅含有觀測粗差):在第100歷元給衛(wèi)星解算的位置模擬東北天(20 m,20 m,50 m)的觀測粗差;

方案2(僅含有系統(tǒng)狀態(tài)異常誤差):從第200歷元開始,給后續(xù)動態(tài)預(yù)測的東北天三個方向分量的速度值模擬加入0.01 m/s的誤差及位置分量加入(3 m,3 m,5 m)的誤差;

方案3(同時含有系統(tǒng)狀態(tài)異常誤差和觀測粗差):同時按照方案1和方案2,分別在100歷元模擬觀測粗差，在200歷元模擬動態(tài)模型粗差。

依據(jù)上述三種方案,分別采用標準UKF方法(UKF),抗差UKF方法(RUKF)、自適應(yīng)UKF方法(AUKF)和本文的兼顧自適應(yīng)與抗差性能的UKF濾波(本文ARUKF)進行對比分析。需要說明的是,針對GNSS/INS松組合模型中動態(tài)模型的非線性特點,上述各種UKF方法均可采用MUT思想簡化動態(tài)預(yù)測的計算量,因此,本節(jié)實驗驗證主要探討各方法在處理觀測粗差或且動態(tài)模型粗差時的性能。

圖2給出了在100 s時加入觀測粗差后各方法的定位結(jié)果,從圖2的結(jié)果不難發(fā)現(xiàn),在僅含有觀測誤差時采用RUKF和本文ARUKF方法均能夠有效地處理含有觀測粗差歷元給定位帶來的影響,而UKF方法和AUKF方法在存在粗差時無法有效抵抗其影響,這是因為UKF方法本身沒有針對粗差進行有效處理;而AUKF將觀測粗差當(dāng)成系統(tǒng)動態(tài)異常進行處理,其沒有正確地對粗差進行歸類,從而也沒有得到連續(xù)可靠的定位結(jié)果。綜上,方案1驗證了處理存在觀測粗差情況下本文ARUKF方法的有效性。

圖2 在100 s時加入觀測粗差情況下各方法的定位結(jié)果

圖3為在200 s后系統(tǒng)存在狀態(tài)異常誤差下各方法的定位結(jié)果。在僅含有系統(tǒng)動態(tài)異常情況下,UKF方法和RUKF方法均未得到連續(xù)可靠的定位結(jié)果,其中RUKF的定位結(jié)果存在很大的發(fā)散;不難理解,此時RUKF方法將系統(tǒng)動態(tài)異常引起的新息向量異常歸為由于觀測異常引起的,從而錯誤地放大了觀測向量協(xié)方差矩陣,最終導(dǎo)致了定位結(jié)果的嚴重發(fā)散。從圖3還可看出,采用AUKF方法和本文的ARUKF均能夠獲得該情況下的連續(xù)可靠的定位結(jié)果,驗證了本文ARUKF方法在處理存在系統(tǒng)動態(tài)模型異常預(yù)測時的有效性。

圖3 在200 s后系統(tǒng)存在狀態(tài)異常誤差下各方法的定位結(jié)果

圖4 同時在100 s時存在觀測粗差和200 s后系統(tǒng)存在狀態(tài)異常誤差下各方法的定位結(jié)果

圖4給出了同時存在觀測異常和動態(tài)異常預(yù)測情況下各方法的定位結(jié)果。從圖4不難發(fā)現(xiàn),UKF方法、RUKF方法和AUKF方法均不能夠為整個實驗過程提供有效且連續(xù)的定位結(jié)果;而采用本文的ARUKF方法能夠為全路段提供有效的定位服務(wù),能夠有效地對異常粗差進行歸類,從而有針對性地對異常向量對應(yīng)的協(xié)方差進行放大,從而減小其對最終定位結(jié)果的影響。

4 結(jié)束語

上述分析驗證了本文提出的ARUKF方法在本文實驗條件下是切實可行的,其能夠處理觀測時段非同時存在觀測異常與動態(tài)異常的情形。但采用本文方法無法處理某一特定歷元同時存在觀測異常與動態(tài)異常的情況，這是因為新息向量無法同時對這兩種異常分量進行歸類,這也是目前相關(guān)方法的痛點,進一步研究可采用機器學(xué)習(xí)等方法輔助這兩類異常信息進行分類,以期得到更完善有效的處理策略。