王 旺, 趙海軍, 姜 暖

(海軍潛艇學(xué)院, 山東 青島 266042)

濾波算法在導(dǎo)航信息處理中有廣泛應(yīng)用,隨著導(dǎo)航技術(shù)的不斷進(jìn)步,國內(nèi)外學(xué)者提出了許多適合不同導(dǎo)航信息的濾波算法。穩(wěn)定性是濾波算法得以正常工作的前提,是其性能的重要指標(biāo)之一。評(píng)估濾波算法穩(wěn)定性,有助于判斷其優(yōu)劣,以便為算法的使用和改進(jìn)提供理論參考。目前,濾波算法穩(wěn)定性評(píng)估主要是對(duì)其收斂性進(jìn)行定性判斷。文獻(xiàn)[1]給出了濾波穩(wěn)定性定理,即對(duì)于一致完全可控和一致完全可觀測(cè)的系統(tǒng),卡爾曼濾波算法是穩(wěn)定的,但實(shí)際中往往會(huì)產(chǎn)生濾波發(fā)散現(xiàn)象。文獻(xiàn)[1-3]提出了卡爾曼濾波過程的穩(wěn)定性概念,給出了濾波發(fā)散的判定定理。文獻(xiàn)[3]針對(duì)組合導(dǎo)航系統(tǒng)濾波算法,給出了性能指標(biāo)和評(píng)估過程。文獻(xiàn)[4-6]從定義出發(fā)研究了算法的穩(wěn)定性,但未給出用于評(píng)估濾波算法穩(wěn)定性的指標(biāo)。

綜合現(xiàn)有文獻(xiàn),有關(guān)濾波算法穩(wěn)定性評(píng)估問題僅局限于穩(wěn)定性概念及收斂判斷條件,并沒有給出評(píng)價(jià)穩(wěn)定性的技術(shù)指標(biāo)及精度要求,尚無完整的用于評(píng)價(jià)濾波算法穩(wěn)定性的指標(biāo)體系,本文在分析濾波算法穩(wěn)定性影響因素的基礎(chǔ)上,提出濾波算法穩(wěn)定性評(píng)估指標(biāo),并給出了具體的數(shù)學(xué)表現(xiàn)形式,構(gòu)建其指標(biāo)體系,結(jié)合卡爾曼濾波在INS/GPS初始對(duì)準(zhǔn)中的應(yīng)用實(shí)例,對(duì)卡爾曼濾波的穩(wěn)定性進(jìn)行了評(píng)估,通過計(jì)算機(jī)仿真驗(yàn)證了所提指標(biāo)的有效性。

1 濾波算法穩(wěn)定性評(píng)估建模

穩(wěn)定性是系統(tǒng)受到某一擾動(dòng)后恢復(fù)原有運(yùn)動(dòng)狀態(tài)的能力,即如果系統(tǒng)受到有界擾動(dòng),不論擾動(dòng)引起的初始偏差有多大,在擾動(dòng)撤出后,系統(tǒng)都能以足夠的準(zhǔn)確度恢復(fù)到原來的平衡狀態(tài),則稱這種系統(tǒng)是穩(wěn)定的[1]。

濾波算法的穩(wěn)定性是其正常工作的前提,影響導(dǎo)航信息處理中濾波算法穩(wěn)定性的主要方面有:收斂性、發(fā)散性、容錯(cuò)性及初值敏感性。

1.1 收斂性

收斂性是濾波穩(wěn)定性評(píng)估最重要的方面,導(dǎo)航信息處理中濾波算法的目的是導(dǎo)航信息能夠穩(wěn)定收斂。

濾波收斂的判定為:若誤差函數(shù)序列小于等于給定的發(fā)散檢驗(yàn)閾值Δ,則稱此次濾波收斂。

定義在N次濾波算法中,濾波收斂次數(shù)M與濾波總次數(shù)N的比值M/N為收斂度,記為ηa(0≤ηa≤1),即:

ηa=M/N

(1)

收斂度表征濾波算法收斂能力的強(qiáng)弱。其值越大,收斂能力越強(qiáng);反之,越弱。收斂度與濾波算法的發(fā)散檢驗(yàn)閾值Δ、濾波總次數(shù)N有關(guān)。

1.2 發(fā)散性

濾波發(fā)散的判定為:若誤差函數(shù)序列大于給定的發(fā)散檢驗(yàn)閾值Δ,則稱此次濾波發(fā)散。

定義在N次濾波算法中,濾波發(fā)散次數(shù)L與濾波總次數(shù)N的比值L/N為發(fā)散度,記為ηb(0≤ηb≤1),即

ηb=L/N

(2)

發(fā)散度表征濾波算法發(fā)散能力的強(qiáng)弱。其值越大,越容易發(fā)散;反之,越不易發(fā)散。發(fā)散度也與濾波算法的發(fā)散檢驗(yàn)閾值Δ、濾波總次數(shù)N有關(guān)。

根據(jù)上述定義,收斂度和發(fā)散度有如下關(guān)系:

ηa+ηb=1

(3)

1.3 容錯(cuò)性

當(dāng)濾波算法的輸入存在較大的波動(dòng)或偏差(離群值)時(shí),算法仍能滿足一定導(dǎo)航精度要求的能力,稱為容錯(cuò)能力或抗離群值能力。當(dāng)離群值作為量測(cè)量進(jìn)入濾波時(shí),會(huì)對(duì)系統(tǒng)的濾波精度產(chǎn)生較大的影響,其影響可看作濾波算法的脈沖響應(yīng)。描述濾波算法容錯(cuò)能力的指標(biāo)主要有二次收斂時(shí)間和容錯(cuò)濾波精度。

當(dāng)離群值作為量測(cè)量進(jìn)入濾波算法時(shí),會(huì)影響后續(xù)濾波算法的收斂,濾波算法在離群值出現(xiàn)后至再次穩(wěn)定收斂所經(jīng)歷的時(shí)間,稱為二次收斂時(shí)間t2。二次收斂時(shí)間是一時(shí)間區(qū)間,區(qū)間的大小反映了濾波算法在離群值存在的條件下再次穩(wěn)定收斂的快慢。t2越大,濾波算法再次穩(wěn)定收斂的速度越慢;反之,則越快。

t2=tw-tl

(4)

其中，tw為濾波值再次穩(wěn)定收斂時(shí)刻;tl為離群值出現(xiàn)時(shí)刻。

容錯(cuò)濾波精度定義如下:

(5)

容錯(cuò)濾波精度為t2時(shí)間段內(nèi)誤差函數(shù)序列與相對(duì)真值比值的均值,反映了在離群值存在的條件下濾波值相對(duì)于相對(duì)真值的平均偏離程度,是一種相對(duì)精度。R值越大,濾波算法的抗離群值能力就越強(qiáng);反之,則越弱。

1.4 初值敏感性

濾波算法的初值影響其精度及收斂時(shí)間。當(dāng)濾波選取不同的初值,算法仍能穩(wěn)定收斂至規(guī)定精度的能力,稱為抗初值能力。其模型如下:

(6)

其中:ΔXw為穩(wěn)態(tài)誤差;Δ為發(fā)散檢驗(yàn)閾值。

q值越大,濾波算法對(duì)初值越敏感;反之,則越不敏感。

1.5 濾波算法穩(wěn)定性評(píng)估指標(biāo)體系

基于上述分析,得到導(dǎo)航信息處理中濾波算法穩(wěn)定性評(píng)估的指標(biāo)體系，如圖1所示。

圖1 濾波算法穩(wěn)定性評(píng)估指標(biāo)體系

2 仿真及結(jié)果

本文將卡爾曼濾波算法應(yīng)用于INS/GPS組合導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)中,并根據(jù)上述建立的濾波算法穩(wěn)定性評(píng)估指標(biāo)體系對(duì)卡爾曼濾波算法的穩(wěn)定性進(jìn)行評(píng)估。

2.1 動(dòng)基座下初始對(duì)準(zhǔn)動(dòng)態(tài)方程的建立

在位置/速度組合模式中,將INS/GPS組合導(dǎo)航系統(tǒng)的誤差量(位置、速度、平臺(tái)誤差角)作為濾波器的估計(jì)對(duì)象,動(dòng)基座無阻尼慣導(dǎo)的誤差方程[7]即可作為狀態(tài)方程的一部分。為便于對(duì)慣性器件的誤差源進(jìn)行估計(jì),陀螺漂移εx、εy、εz和加速度計(jì)誤差ΔAx、ΔAy也應(yīng)表示為系統(tǒng)的狀態(tài)。假定3個(gè)陀螺漂移誤差模型相同,均為一階馬爾科夫過程和白噪聲之和,加速度計(jì)誤差模型為零位偏置和白噪聲之和,則濾波器狀態(tài)方程如下:

(7)

式中,選取狀態(tài)變量X=[δVx,δVx,δφ,φλ,φx,φy,φz,εx,εy,εz,ΔAx,ΔAy];F(t)為狀態(tài)轉(zhuǎn)移矩陣,由慣導(dǎo)誤差方程[7]列出;w(t)為噪聲向量,w(t)=[wAx,wAy,0,0,wgx,wgy,wgz,wmx,wmy,wmz,0,0]。wAx,wAy為加速度計(jì)誤差的白噪聲,wgx,wgy,wgz為陀螺漂移白噪聲,wmx,wmy,wmz為激勵(lì)白噪聲。

令φt、λt為載體實(shí)際位置,vtX,vtY為載體實(shí)際速度,則慣導(dǎo)系統(tǒng)位置和速度可表示為

(8)

式中,φI、λI為慣導(dǎo)輸出經(jīng)緯度,vtIX、vtIY為慣導(dǎo)輸出速度,δφ、δλ、δvX、δvY為慣導(dǎo)系統(tǒng)誤差。

GPS位置和速度可表示為

(9)

式中,φG、λG為GPS輸出經(jīng)緯度,vtGX、vtGY為GPS輸出速度,δvGX、δvGY為GPS速度誤差,NE、NN分別為GPS在經(jīng)度方向和緯度方向的距離誤差,為簡(jiǎn)便計(jì),認(rèn)為NE、NN為白噪聲,R為地球半徑。

在位置/速度組合模式中,定義量測(cè)向量:

(10)

位置/速度組合模式下INS/GPS組合導(dǎo)航系統(tǒng)的量測(cè)方程為

Y(t)=H(t)X(t)+V(t)

(11)

式中,

對(duì)于線性組合Z(t)=Lx(t),取

2.2 仿真條件

設(shè)載體為艦船,航速10kn,航跡如圖2所示(橫軸為經(jīng)度,縱軸為緯度),包括正東向航行(航行時(shí)間20min)、圓周航行(航行時(shí)間10min)、正北向航行(航行時(shí)間15min)、西南向航行(航行時(shí)間40min)四個(gè)階段。

圖2 仿真時(shí)的艦船“航跡”

取采樣時(shí)間T=1s,地球自轉(zhuǎn)角速率wie=7.29212e-5 rad/s,重力加速度g=9.78。假定選用一低精度慣導(dǎo),其等效陀螺漂移為0.1°/h,隨機(jī)漂移為0.001°/h,陀螺漂移相關(guān)時(shí)間為1h;加速度計(jì)零位偏置為10-4g,隨機(jī)偏差為10-8;慣導(dǎo)平臺(tái)的初始誤差為φx0=5′、φy0=5′、φz0=20′,慣導(dǎo)初始定位誤差北向和東向均為200m,初始速度誤差為1m/s;GPS接收機(jī)輸出的位置誤差北向和東向均為25m,速度誤差為0.1m/s。

2.3 卡爾曼濾波算法穩(wěn)定性評(píng)估

條件1:

卡爾曼濾波算法的初值如下:

X0=[1m/s,1m/s,0,0,5′,5′,20′, 0.001°/h, 0.001°/h, 0.001°/h, 10-4g, 10-4g]

P0=diag{(1m/s)2,(1m/s)2,0,0,(5′)2,(5′)2,(20′)2,(0.001°/h)2,(0.001°/h)2,(0.001°/h)2,(10-4g)2,(10-4g)2}

Q0=diag{(10-8)2,(10-8)2,0,0,(10-8)2,(10-8)2,(10-8)2,(10-8)2, (10-8)2,(10-8)2,0,0}

R0=diag{(0.1m/s)2,(0.1m/s)2, (25m)2,

(25m)2}

狀態(tài)初值X0和估計(jì)誤差方差矩陣的初值P0為無偏的,即X0=E[X0]、P0=Var[X0]。發(fā)散檢驗(yàn)閾值Δ1=Δ2=0.4′,Δ3=1.5′。穩(wěn)定性評(píng)估指標(biāo)數(shù)值如表1所示。誤差估計(jì)結(jié)果如圖3-5所示。

圖3 東向水平失準(zhǔn)角估計(jì)誤差

圖4 北向水平失準(zhǔn)角估計(jì)誤差

圖5 方位失準(zhǔn)角估計(jì)誤差

項(xiàng)目東向水平失準(zhǔn)角北向水平失準(zhǔn)角方位失準(zhǔn)角收斂度0.99510.99980.9686發(fā)散度0.00490.00020.0314容錯(cuò)濾波精度———二次收斂時(shí)間———初值敏感值0.43670.29210.3898

條件2:

保持條件1的初值選取不變,引入離群值:慣導(dǎo)輸出的東向速度在1000s:1010s=-8.0467m/s;發(fā)散檢驗(yàn)閾值Δ1=Δ2=0.4′,Δ3=1.5′。條件2下的穩(wěn)定性評(píng)估指標(biāo)的數(shù)值如表2。

表2 條件2下的穩(wěn)定性評(píng)估指標(biāo)數(shù)值

條件3:

改變?yōu)V波算法的初值X0=[0,0,0,0,0,0,0, 0, 0, 0, 0,0];P0=diag{0,0,0,0,0,0,0, 0, 0, 0, 0,0};Q0和R0保持不變;發(fā)散檢驗(yàn)閾值Δ1=Δ2=0.4′,Δ3=1.5′,此時(shí)不加入野值。評(píng)估數(shù)值如表3所示。

表3 條件3下的穩(wěn)定性評(píng)估指標(biāo)數(shù)值

由表1、表2,得出以下結(jié)論:在相同的發(fā)散檢驗(yàn)閾值Δ1=Δ2=0.4′,Δ3=1.5′下,加入離群值后：

1)水平失準(zhǔn)角的收斂度變小,方位失準(zhǔn)角的收斂度沒有變化;

2)與離群值相關(guān)的北向失準(zhǔn)角的容錯(cuò)濾波精度為0.6215,即濾波值相對(duì)于相對(duì)真值平均偏離了62.15%。二次收斂時(shí)間為68s,再次穩(wěn)定收斂的時(shí)間較長,說明了卡爾曼濾波算法的抗離群值能力較弱。

由表1、表3,得出以下結(jié)論:在相同的發(fā)散檢驗(yàn)閾值Δ1=Δ2=0.4′,Δ3=1.5′下,改變?yōu)V波初值后：

1)三個(gè)失準(zhǔn)角的收斂度均變小,且變化較大,可看出卡爾曼濾波算法的初值對(duì)濾波收斂及發(fā)散有較大影響;

2)由表3,三個(gè)失準(zhǔn)角的初值敏感值均大于1,即穩(wěn)態(tài)收斂誤差大于發(fā)散檢驗(yàn)閾值,說明卡爾曼濾波算法的初值影響其穩(wěn)態(tài)收斂誤差,抗初值能力較差。

綜上,卡爾曼濾波算法在滿足其假設(shè)條件下,穩(wěn)定性好;但其抗離群值能力較差;當(dāng)初值不滿足無偏估計(jì)時(shí),收斂性較差,抗初值能力較差。評(píng)估結(jié)果與理論分析一致。

3 結(jié)束語

穩(wěn)定性是導(dǎo)航信息估計(jì)算法正常運(yùn)行的前提和重要保障。本文以卡爾曼濾波在INS/GPS初始對(duì)準(zhǔn)中的應(yīng)用實(shí)例,得到了與理論分析一致的結(jié)論。但文中仿真條件只涉及了載體的航跡運(yùn)動(dòng),載體艦船的姿態(tài)運(yùn)動(dòng)對(duì)濾波穩(wěn)定性同樣具有一定的影響,這也是需進(jìn)一步分析的因素,有關(guān)這方面的內(nèi)容將在后續(xù)工作中進(jìn)一步深入研究。

[1] 邱愷,黃國榮,陳天如,等.卡爾曼濾波過程的穩(wěn)定性研究[J]. 系統(tǒng)工程與電子技術(shù)，2005,27(1):33-35.

[2] K. You, M. Fu, L. Xie. Mean square stability for Kalman filtering with markovian packet losses[J]. Automatica， 2011,47(12):2647-2657.

[3] 邱愷,黃國榮,陳天如，等.基于濾波過程的卡爾曼濾波發(fā)散判定方法[J].系統(tǒng)工程與電子技術(shù)， 2005,27(2):230-231.

[4] 劉勇志,賈興亮,劉丙杰.一種組合導(dǎo)航濾波性能評(píng)估算法[J].彈箭與制導(dǎo)學(xué)報(bào)，2008,28(5):45-47，50.

[5] 劉國良,張迎春,孫增圻.聯(lián)邦濾波器的濾波穩(wěn)定性研究[J].中國慣性技術(shù)學(xué)報(bào)，2004,12(6):28-31.

[6] 高法欽,談?wù)怪?北斗組合動(dòng)態(tài)定位算法穩(wěn)定性分析[J].電子與信息學(xué)報(bào)，2007,29(10):2388-2391.

[7] 陳永冰,鐘斌.慣性導(dǎo)航原理[M].北京:國防工業(yè)出版社,2007.

[8] 閻紅星,王曉博,王國宏.跟蹤濾波算法性能評(píng)估研究[J].現(xiàn)代雷達(dá)，2008,30(4):33-36.

[9] Boyi Ni, Qinghua Zhang. Stability of the Kalman Filter for Output Error Systems[J]. IFAC,2015，48(28):1106-1111.

[10] A li T A, John E G, Mc Cabe D H. Performance Evalunation of a Asynchronous Multisensor Track Fusion Filter[C]∥Proceedings of SPIE,2003.

[11] Junfeng Wua, Ling Shi b, Lihua Xie.An improved stability condition for Kalman filtering with bounded Markovian packet losses[J]. Automatica, 2015,62(C):32-38.