張 群 洪志強(qiáng)

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

0 引言

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

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

1 GPS/INS松組合模型

1.1 動(dòng)力學(xué)模型

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

(1)

(2)

(3)

(4)

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

(5)

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

(6)

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

1.2 觀測(cè)模型

(7)

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

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

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

(8)

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

(9)

(10)

(11)

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

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

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

(12)

并計(jì)算新息向量

(13)

(14)

計(jì)算增益矩陣Kk:

(15)

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

(16)

(17)

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

(17)

(18)

(19)

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

(20)

(21)

2.3 方法實(shí)現(xiàn)

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

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

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

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

3 算例與分析

圖1 車載實(shí)驗(yàn)軌跡

為了驗(yàn)證本文采用方法的有效性,采用如下三種方案進(jìn)行試驗(yàn):

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

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

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

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

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

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

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

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

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

圖4給出了同時(shí)存在觀測(cè)異常和動(dòng)態(tài)異常預(yù)測(cè)情況下各方法的定位結(jié)果。從圖4不難發(fā)現(xiàn),UKF方法、RUKF方法和AUKF方法均不能夠?yàn)檎麄€(gè)實(shí)驗(yàn)過程提供有效且連續(xù)的定位結(jié)果;而采用本文的ARUKF方法能夠?yàn)槿范翁峁┯行У亩ㄎ环?wù),能夠有效地對(duì)異常粗差進(jìn)行歸類,從而有針對(duì)性地對(duì)異常向量對(duì)應(yīng)的協(xié)方差進(jìn)行放大,從而減小其對(duì)最終定位結(jié)果的影響。

4 結(jié)束語

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