池傳國，黃國勇，孫 磊

(1．昆明理工大學(xué) 信息工程與自動(dòng)化學(xué)院，云南 昆明 650500；2．云南省礦物管道輸送工程技術(shù)研究中心，云南 昆明 650500)

組合導(dǎo)航系統(tǒng)中，觀測數(shù)據(jù)異常是導(dǎo)致濾波系統(tǒng)發(fā)散的原因之一。衛(wèi)星導(dǎo)航系統(tǒng)中系統(tǒng)輸出數(shù)據(jù)不可避免會(huì)出現(xiàn)野值數(shù)據(jù)或粗差，與慣性導(dǎo)航系統(tǒng)數(shù)據(jù)融合濾波后仍不可避免異常數(shù)據(jù)對(duì)系統(tǒng)輸出的影響。衛(wèi)星導(dǎo)航數(shù)據(jù)異常通常分為3類：單粗差、多粗差與緩慢增長誤差。衛(wèi)星導(dǎo)航系統(tǒng)中，緩慢增長誤差常見于接收機(jī)鐘差問題，在松組合方式的組合導(dǎo)航系統(tǒng)中無法消除。衛(wèi)星導(dǎo)航數(shù)據(jù)的單粗差和多粗差異常會(huì)導(dǎo)致組合導(dǎo)航濾波系統(tǒng)魯棒性降低[1]。

強(qiáng)跟蹤SVD-UKF算法中，強(qiáng)跟蹤濾波框架建立在新息正交的基礎(chǔ)上，利用正常新息數(shù)據(jù)實(shí)現(xiàn)對(duì)實(shí)際狀態(tài)的強(qiáng)跟蹤，其并不能判斷帶有單粗差或多粗差的新息序列的健康程度。一旦異常數(shù)據(jù)污染了新息序列，強(qiáng)跟蹤濾波框架強(qiáng)迫新息殘差序列正交后導(dǎo)致多重漸消因子變化，引起估計(jì)結(jié)果的偏差，降低估計(jì)精度并可能導(dǎo)致濾波系統(tǒng)的發(fā)散?？共罟烙?jì)是常用的解決衛(wèi)星信號(hào)野值數(shù)據(jù)問題的手段之一[2]。當(dāng)濾波系統(tǒng)中粗差的影響不可避免時(shí)，通過適當(dāng)?shù)氖侄蝸肀苊獯植顚?duì)狀態(tài)估計(jì)值的影響?？共罟烙?jì)是通過等價(jià)權(quán)來影響新息序列，以降低新息序列在出現(xiàn)野值時(shí)造成對(duì)狀態(tài)估計(jì)值的影響。將抗差估計(jì)引入強(qiáng)跟蹤SVD-UKF算法，在算法進(jìn)行量測方程更新前對(duì)新息序列預(yù)處理，剔除新息序列中存在的衛(wèi)星野值，保證濾波系統(tǒng)的可靠性和魯棒性[3]。

強(qiáng)跟蹤SVD-UKF算法是基于新息序列來判斷估計(jì)狀態(tài)是否偏離，故新息序列的信息完整性是強(qiáng)跟蹤濾波算法對(duì)系統(tǒng)建模及濾波精度保證的要求之一[4]。M估計(jì)是抗差估計(jì)的一種，其本質(zhì)是通過等價(jià)權(quán)函數(shù)對(duì)新息序列進(jìn)行加權(quán)，根據(jù)殘差序列來確定新息序列權(quán)重。當(dāng)回歸殘差較大時(shí)，降低新息序列中異常數(shù)據(jù)對(duì)狀態(tài)估計(jì)值影響；當(dāng)回歸殘差較小時(shí)，增大權(quán)重保證新息序列信息完整性。M估計(jì)的等價(jià)權(quán)函數(shù)方式有效提高新息數(shù)據(jù)的可利用率。因此，選擇M估計(jì)方法用于抑制衛(wèi)星信號(hào)野值問題，能夠最大程度的提出衛(wèi)星信號(hào)中的異常數(shù)據(jù)，并最低程度的影響新息數(shù)據(jù)的信息完整性，保證強(qiáng)跟蹤SVD-UKF算法在新息正交時(shí)不受影響[5]。

1 M估計(jì)理論

1.1 等價(jià)權(quán)原理

選取狀態(tài)向量X=[x1,x2,x3,…,xm]T，引入一組新息序列Z=[z1,z2,z3,…,zn]T，則相應(yīng)的殘差方程為

(1)

在最小二乘估計(jì)中，最優(yōu)線性無偏估計(jì)的準(zhǔn)則是殘差平方和最小，即

(2)

最小二乘估計(jì)在新息異常時(shí)，新息值會(huì)直接影響估計(jì)結(jié)果的精度。M估計(jì)是基于最小二乘的穩(wěn)健估計(jì)，通過加權(quán)新息序列來降低異常數(shù)據(jù)對(duì)估計(jì)過程的影響。如何選擇加權(quán)因子直接影響M估計(jì)的估計(jì)精度。

構(gòu)造觀測加權(quán)矩陣B=diag[b1,b2,b3,…,bn]T，使得

(3)

則加權(quán)最小二乘估計(jì)為

(4)

針對(duì)M估計(jì)建立如式(5)所示的準(zhǔn)則函數(shù)

(5)

式中f為適當(dāng)選擇的函數(shù)，對(duì)準(zhǔn)則函數(shù)以X(k)求偏導(dǎo)得

(6)

令φ[e(k)i]/e(k)i=ωi，則有

(7)

將結(jié)果帶入式中，可以得到基于M估計(jì)的最小二乘估計(jì)

(8)

當(dāng)?shù)葍r(jià)權(quán)矩陣為I時(shí)，基于M估計(jì)的最小二乘估計(jì)降階到標(biāo)準(zhǔn)最小二乘，故實(shí)際上最小二乘估計(jì)是M估計(jì)的一種特殊形式。等價(jià)權(quán)矩陣的選擇直接影響到M估計(jì)的精度，選擇不同的等價(jià)權(quán)函數(shù)代替等價(jià)權(quán)矩陣，能夠自適應(yīng)處理新息數(shù)據(jù)，以更有效的利用新息數(shù)據(jù)。

1.2 等價(jià)權(quán)函數(shù)選擇

等價(jià)權(quán)函數(shù)的選擇是M估計(jì)理論的關(guān)鍵。等價(jià)權(quán)函數(shù)的選擇需要結(jié)合應(yīng)用對(duì)象來進(jìn)行選擇，以保證M估計(jì)的精度[6]。結(jié)合衛(wèi)星導(dǎo)航數(shù)據(jù)的特點(diǎn)，等價(jià)權(quán)函數(shù)的選取目標(biāo)是：

(1)新息數(shù)據(jù)在先驗(yàn)?zāi)Ｐ偷恼Ｔ肼暦秶鷥?nèi)，經(jīng)過M估計(jì)后仍然保證其有效性；

(2)新息數(shù)據(jù)超出先驗(yàn)?zāi)Ｐ偷恼Ｔ肼暦秶潭容^小，經(jīng)過M估計(jì)后，新息數(shù)據(jù)包含的有效信息不被受到破壞性影響；

(3)新息數(shù)據(jù)超出先驗(yàn)?zāi)Ｐ偷恼Ｔ肼暦秶潭容^大，經(jīng)過M估計(jì)后，新息數(shù)據(jù)不會(huì)較大程度影響濾波系統(tǒng)的狀態(tài)估計(jì)值。

根據(jù)等價(jià)權(quán)函數(shù)的選取目標(biāo)，可以將等價(jià)權(quán)分為3類：(1)正常區(qū)，不影響新息序列；(2)調(diào)整區(qū)，通過權(quán)因子調(diào)整新息序列；(3)拒絕區(qū)，剔除新息數(shù)據(jù)。

根據(jù)等價(jià)權(quán)區(qū)劃分，選擇合適的等價(jià)權(quán)函數(shù)，保證新息數(shù)據(jù)在正常區(qū)能夠不影響后續(xù)狀態(tài)估計(jì)，在調(diào)整區(qū)提高新息數(shù)據(jù)的可利用率，在拒絕區(qū)實(shí)現(xiàn)對(duì)污染數(shù)據(jù)的抗差[7]。由此來選擇等價(jià)權(quán)函數(shù)，解決衛(wèi)星信號(hào)野值問題。

常用的等價(jià)權(quán)函數(shù)有IGG法、丹麥法、Huber法等。根據(jù)上面等價(jià)權(quán)函數(shù)的選取原則以及衛(wèi)星信號(hào)數(shù)據(jù)的特點(diǎn)，選擇IGG法作為本文M估計(jì)的等價(jià)權(quán)函數(shù)。IGG法相對(duì)等價(jià)權(quán)區(qū)將權(quán)函數(shù)分為3段，在正常區(qū)不對(duì)新息序列產(chǎn)生任何影響，即等價(jià)權(quán)矩陣為I，不影響后續(xù)估計(jì)；在調(diào)整區(qū)，對(duì)新息序列進(jìn)行降權(quán)處理，保證新息數(shù)據(jù)信息的部分可用；在拒絕區(qū)，拋棄不可用新息數(shù)據(jù)避免估計(jì)受到粗差影響[8]。

(9)

其中，c0由系統(tǒng)新息數(shù)據(jù)統(tǒng)計(jì)確定。

1.3 基于M估計(jì)的強(qiáng)跟蹤SVD-UKF算法

強(qiáng)跟蹤SVD-UKF算法由于其新息正交性原理在新息數(shù)據(jù)誤差較大的情況易導(dǎo)致濾波發(fā)散。當(dāng)衛(wèi)星信號(hào)出現(xiàn)野值時(shí)，強(qiáng)跟蹤濾波框架不可避免的會(huì)受到野值的影響。將M估計(jì)應(yīng)用到強(qiáng)跟蹤SVD-UKF算法中，在強(qiáng)跟蹤SVD-UKF算法迭代前對(duì)新息數(shù)據(jù)進(jìn)行M估計(jì)，通過選擇恰當(dāng)?shù)牡葍r(jià)權(quán)函數(shù)，判斷新息數(shù)據(jù)是否為野值并處理，有效解決了衛(wèi)星信號(hào)野值對(duì)強(qiáng)跟蹤SVD-UKF算法干擾。等價(jià)權(quán)函數(shù)的選擇充分考慮衛(wèi)星信號(hào)數(shù)據(jù)的基本模型以及強(qiáng)跟蹤SVD-UKF算法的內(nèi)部結(jié)構(gòu)，選擇IGG法等價(jià)權(quán)函數(shù)在數(shù)據(jù)異常時(shí)降低數(shù)據(jù)對(duì)強(qiáng)跟蹤SVD-UKF算法的干擾，在數(shù)據(jù)正常時(shí)，保證新息數(shù)據(jù)對(duì)于強(qiáng)跟蹤SVD-UKF算法的可利用率[9]。

基于M估計(jì)的強(qiáng)跟蹤SVD-UKF算法在強(qiáng)跟蹤SVD-UKF算法框架下，利用M估計(jì)對(duì)新息數(shù)據(jù)預(yù)處理，數(shù)據(jù)處理后送入強(qiáng)跟蹤SVD-UKF算法得到結(jié)果[10]。

將基于M估計(jì)的強(qiáng)跟蹤SVD-UKF算法應(yīng)用到組合導(dǎo)航系統(tǒng)中，選擇系統(tǒng)狀態(tài)方程狀態(tài)向量的選擇如下

X(t)=[L,λ,h,VE,VN,VU,φE,φN,φU,εbx,εby,εbz]

(10)

式中,L,λ,h為位置；φE,φN,φU為INS輸出的姿態(tài)角；VE,VN,VU為速度；εbx,εby,εbz為陀螺零偏。系統(tǒng)的量測向量選擇如下

Z=[LB,λB,hB,VEB,VNB,VUB]

(11)

基于M估計(jì)的強(qiáng)跟蹤SVD-UKF算法的基本流程如下：

(1)初始化。狀態(tài)向量X以及協(xié)方差矩陣P的初值為

(12)

(2)構(gòu)造Sigma點(diǎn)集

(13)

(3)時(shí)間更新

(14)

(15)

(16)

Yi=f(Xi);i=0,1,…,2n

(17)

(4)新息序列M估計(jì)處理

(18)

(19)

(20)

(5)多重漸消因子更新

(21)

Ck,i=tr[Nk]/tr[Mii,k]

(22)

(23)

(24)

(25)

(6)量測更新

(26)

(27)

(28)

由上可知，利用M估計(jì)理論，對(duì)異常新息數(shù)據(jù)進(jìn)行“篩選”，保留有用新息，剔除有害新息，提供可靠有效的新息數(shù)據(jù)輸入強(qiáng)跟蹤SVD-UKF算法。經(jīng)過M估計(jì)處理后的新息數(shù)據(jù)對(duì)于強(qiáng)跟蹤SVD-UKF算法的意義在于濾除衛(wèi)星信號(hào)野值，保留新息有效信息，避免強(qiáng)跟蹤SVD-UKF算法因模型不精確導(dǎo)致估計(jì)精度的降低[11]。

2 仿真驗(yàn)證

為了驗(yàn)證本文提出算法的有效性，以下通過仿真實(shí)驗(yàn)對(duì)基于M估計(jì)的強(qiáng)跟蹤SVD-UKF算法進(jìn)行驗(yàn)證，并與強(qiáng)跟蹤SVD-UKF算法進(jìn)行對(duì)比驗(yàn)證。

假設(shè)飛機(jī)做機(jī)動(dòng)飛行，飛行軌跡中含有爬升、變速、平飛和轉(zhuǎn)彎等各種飛行狀態(tài)。飛機(jī)的初始位置為東經(jīng)107.002°，北緯29.498°，高度300 m；初始速度為50 m/s，方向正北。其中，陀螺的常值漂移為0.1 deg/h，加速度計(jì)常值誤差1.0g～4g，陀螺一階馬爾可夫過程相關(guān)時(shí)間為3 600 s，加速度一階馬爾可夫過程相關(guān)時(shí)間為1 800 s。BDS水平位置誤差均方根10 m，高度誤差均方根20 m，速度誤差均方根0.1 m/s。INS初始水平位置誤差50 m，高度誤差為100 m；初始速度誤差0.5 m/s。BDS采樣周期1 s，INS采樣周期0.02 s，濾波周期1 s，仿真時(shí)間1 000 s。

兩組算法系統(tǒng)狀態(tài)向量與量測向量的選擇與強(qiáng)跟蹤SVD-UKF算法仿真實(shí)驗(yàn)相同，在以上仿真模型中，衛(wèi)星導(dǎo)航位置數(shù)據(jù)加入異常粗差后進(jìn)行對(duì)比驗(yàn)證算法效果。

(1)衛(wèi)星單粗差數(shù)據(jù)異常對(duì)比。在仿真模型中加入5組水平位置異常數(shù)據(jù)，加入時(shí)間分別為第200 s、第400 s、第600 s、第800 s、第1 000 s。5組數(shù)據(jù)分別為：-1 000 m、-1 000 m、-1 000 m；-800 m、-800 m、-800 m；1 000 m、1 000 m、1 000 m；800 m、800 m、800 m；500 m、500 m、500 m。仿真結(jié)果如圖1和圖2所示。

圖1 兩種算法位置誤差比較

圖2 兩種算法速度誤差比較

對(duì)比兩組算法仿真結(jié)果可知，基于M估計(jì)的強(qiáng)跟蹤SVD-UKF濾波算法在衛(wèi)星加入異常數(shù)據(jù)后仍能夠不受異常數(shù)據(jù)干擾，濾波系統(tǒng)魯棒性增強(qiáng)[12]。其中，強(qiáng)跟蹤SVD-UKF濾波算法在遇到衛(wèi)星野值后無法克服野值干擾，在加入異常數(shù)據(jù)的時(shí)間點(diǎn)均出現(xiàn)較大誤差。在遇到異常新息數(shù)據(jù)后，強(qiáng)跟蹤SVD-UKF濾波算法無法判斷數(shù)據(jù)是否異常。由于強(qiáng)跟蹤濾波框架的新息正交性，多重漸消因子影響協(xié)方差矩陣使系統(tǒng)后驗(yàn)狀態(tài)估計(jì)值傾向于新息數(shù)據(jù)，使得濾波系統(tǒng)輸出估計(jì)產(chǎn)生較大誤差。由于多重漸消因子利用歷史殘差數(shù)據(jù)統(tǒng)計(jì)，異常數(shù)據(jù)對(duì)多重漸消因子影響一直存在。故仿真結(jié)果中，異常數(shù)據(jù)對(duì)強(qiáng)跟蹤SVD-UKF濾波算法的影響隨著多重漸消因子慢慢遺忘歷史數(shù)據(jù)漸漸消失?；贛估計(jì)的強(qiáng)跟蹤SVD-UKF濾波算法在遇到異常數(shù)據(jù)時(shí)，通過M估計(jì)辨識(shí)出異常后，等價(jià)權(quán)因子降低了異常數(shù)據(jù)的影響。在本次仿真中，5組異常數(shù)據(jù)經(jīng)過M估計(jì)后等價(jià)權(quán)因子均降為0，使得異常數(shù)據(jù)并未對(duì)濾波系統(tǒng)造成干擾，提高了濾波系統(tǒng)魯棒性[13]。

(2)衛(wèi)星多粗差數(shù)據(jù)異常對(duì)比。在仿真模型中第45 s時(shí)加入持續(xù)100 s均值為0、強(qiáng)度為50的白噪聲干擾。仿真結(jié)果如圖3和圖4所示。

圖3 兩種算法位置誤差比較

圖4 兩種算法速度誤差比較

對(duì)比兩組算法仿真結(jié)果可知，基于M估計(jì)的強(qiáng)跟蹤SVD-UKF濾波算法在衛(wèi)星加入持續(xù)異常數(shù)據(jù)仍

能保證濾波系統(tǒng)不受干擾。而強(qiáng)跟蹤SVD-UKF濾波算法在新息數(shù)據(jù)持續(xù)異常的時(shí)間段內(nèi)亦產(chǎn)生較大誤差?；贛估計(jì)的強(qiáng)跟蹤SVD-UKF濾波算法異常數(shù)據(jù)殘差較大時(shí)，等價(jià)權(quán)因子權(quán)值為0，剔除異常干擾；在異常數(shù)據(jù)殘差在調(diào)整區(qū)時(shí)，等價(jià)權(quán)因子權(quán)值根據(jù)先驗(yàn)統(tǒng)計(jì)降低新息權(quán)值，減少異常數(shù)據(jù)對(duì)系統(tǒng)產(chǎn)生的影響[14]?；贛估計(jì)的強(qiáng)跟蹤SVD-UKF濾波算法在多粗差情況下依然提高了系統(tǒng)魯棒性[15]。

3 結(jié)束語

本文提出一種基于M估計(jì)的強(qiáng)跟蹤SVD-UKF算法。該算法利用M估計(jì)理論，采用等價(jià)權(quán)因子對(duì)異常新息數(shù)據(jù)進(jìn)行“篩選”，最大程度保留有效新息，剔除有害新息，避免由衛(wèi)星信號(hào)野值引起的粗差對(duì)強(qiáng)跟蹤SVD-UKF算法的魯棒性影響。將該算法應(yīng)用于組合導(dǎo)航系統(tǒng)仿真實(shí)驗(yàn)，在仿真模型中加入單粗差、多粗差數(shù)據(jù)與強(qiáng)跟蹤SVD-UKF算法進(jìn)行驗(yàn)證，仿真結(jié)果表明，基于M估計(jì)的強(qiáng)跟蹤SVD-UKF算法有效提高了系統(tǒng)在新息數(shù)據(jù)異常時(shí)的魯棒性，證明了本文方法的可行性和有效性。