基于邊緣采樣的簡化高階CKF在非線性快速傳遞對準(zhǔn)中的應(yīng)用

盧航，郝順義，彭志穎，黃國榮

空軍工程大學(xué) 航空工程學(xué)院， 西安 710038

傳遞對準(zhǔn)是目前一種常用的動基座對準(zhǔn)方法[1-2]。在艦載環(huán)境下，利用艦船高精度的主慣導(dǎo)系統(tǒng)對艦載機(jī)低精度的子慣導(dǎo)系統(tǒng)進(jìn)行傳遞對準(zhǔn)可以有效提高對準(zhǔn)精度和縮短對準(zhǔn)時(shí)間[3-5]。傳遞對準(zhǔn)分為粗對準(zhǔn)和精對準(zhǔn)兩個(gè)過程[1]，粗對準(zhǔn)過程中，主慣導(dǎo)對子慣導(dǎo)提供姿態(tài)、速度、位置等導(dǎo)航信息完成一次裝訂，隨后子慣導(dǎo)以此裝訂值為基礎(chǔ)開始姿態(tài)解算和導(dǎo)航解算。建立傳遞對準(zhǔn)誤差模型并采用卡爾曼濾波器進(jìn)行估計(jì)的過程為精對準(zhǔn)過程，在實(shí)際主、子慣導(dǎo)系統(tǒng)傳遞對準(zhǔn)工作過程中，桿臂效應(yīng)誤差、艦船夾板撓曲運(yùn)動誤差是兩個(gè)較為主要的誤差因素。為了提高傳遞對準(zhǔn)工作精度，通常在濾波器觀測量中對桿臂效應(yīng)帶來的速度誤差進(jìn)行補(bǔ)償，以減弱其所帶來的影響。艦船實(shí)際航行過程中，在日曬、陣風(fēng)、海浪沖擊等外界因素的作用下，其船體結(jié)構(gòu)通常會產(chǎn)生一定的變化，桿臂的長度會因?yàn)閾锨\(yùn)動的存在也會發(fā)生變化，此時(shí)存在動態(tài)桿臂[6-14]。文獻(xiàn)[6-8,15]建立了載艦撓曲變形和桿臂效應(yīng)的一體化傳遞對準(zhǔn)誤差模型，但是均對3個(gè)失準(zhǔn)角進(jìn)行了小角度假設(shè)，實(shí)際上，艦載機(jī)可能?？吭谂灤装迳系娜我馕恢茫灤鲬T導(dǎo)系統(tǒng)卻安裝在甲板下方的導(dǎo)航室，此時(shí)主、子慣導(dǎo)系統(tǒng)之間的方位失準(zhǔn)角可能很大，那么基于小角度假設(shè)的傳遞對準(zhǔn)一體化誤差模型已經(jīng)不適用于這種情況，需要對大失準(zhǔn)角情況下的非線性傳遞對準(zhǔn)一體化誤差模型進(jìn)行進(jìn)一步研究。文獻(xiàn)[9]提出了適用于大方位失準(zhǔn)角的非線性傳遞對準(zhǔn)模型，但是該模型并沒有對實(shí)際失準(zhǔn)角進(jìn)行建模，同時(shí)量測方程是復(fù)雜的非線性方程，這無疑會造成濾波算法精度和數(shù)值穩(wěn)定性的下降。文獻(xiàn)[10]通過對姿態(tài)量測量進(jìn)行安裝誤差角的補(bǔ)償，提出一種適用于安裝誤差角為大失準(zhǔn)角情況下的姿態(tài)匹配方法，但是該方案需要提前知道安裝誤差角，具有一定的局限性。需要指出的是，文獻(xiàn)[6-10]所提出的傳遞對準(zhǔn)模型均是建立在計(jì)算導(dǎo)航坐標(biāo)系內(nèi)，Kain和Cloutier于1989年提出了基于量測失準(zhǔn)角的傳遞對準(zhǔn)誤差模型，并引入計(jì)算載體系，建立了一種新的“速度加姿態(tài)匹配”快速傳遞對準(zhǔn)誤差模型[11]。文獻(xiàn)[12-14,16]在此基礎(chǔ)上建立了非線性快速傳遞對準(zhǔn)模型，但是并沒有進(jìn)一步考慮撓曲變形和桿臂效應(yīng)二者帶來的綜合影響。

本文針對艦載機(jī)慣導(dǎo)系統(tǒng)非線性傳遞對準(zhǔn)誤差模型不完善的問題，同時(shí)考慮載艦撓曲變形和桿臂效應(yīng)兩種主要誤差因素，建立撓曲變形和桿臂效應(yīng)加速度一體化誤差模型。采用“速度+姿態(tài)”組合匹配算法，設(shè)計(jì)了一種適用于該模型的基于邊緣條樣的簡化高階容積科爾曼濾波(M-RHCKF)算法，最后進(jìn)行仿真實(shí)驗(yàn)，結(jié)果證明了所建立模型的正確性和濾波算法的有效性。

1 非線性快速傳遞對準(zhǔn)一體化模型

1.1 坐標(biāo)系定義

模型的推導(dǎo)過程主要涉及到以下幾種坐標(biāo)系，以各坐標(biāo)系的x軸為例，它們之間的關(guān)系如圖1所示。

導(dǎo)航坐標(biāo)系n取當(dāng)?shù)氐乩碜鴺?biāo)系；艦船坐標(biāo)系m的坐標(biāo)原點(diǎn)位于艦船中心位置，它與標(biāo)稱機(jī)體坐標(biāo)系s0之間的失準(zhǔn)角定義為實(shí)際失準(zhǔn)角φa；標(biāo)稱機(jī)體坐標(biāo)系s0與實(shí)際機(jī)體坐標(biāo)系s之間相差撓曲變形角θ；艦船坐標(biāo)系m與計(jì)算機(jī)體坐標(biāo)系s′之間的失準(zhǔn)角定義為量測失準(zhǔn)角φm。

圖1 各坐標(biāo)系關(guān)系圖Fig.1 Diagram of each coordinate system

1.2 撓曲變形和動態(tài)桿臂一體化建模

目前大多數(shù)研究認(rèn)為Markov過程可以較好地描述撓曲變形運(yùn)動[6-9,15,17]，此時(shí)甲板的撓曲變形角可以看做是由一個(gè)白噪聲激勵的二階Markov過程，可以表示為

(1)

(2)

(3)

(4)

(5)

圖2 θz引起的動態(tài)桿臂示意圖Fig.2 Diagram of dynamic lever arm caused by θz

(6)

(7)

同理可以得到由于y軸方向上的撓曲變形在x、z軸上的影響，z軸方向上的撓曲變形在x、y軸上的影響，表1為θ在另兩個(gè)軸向上引起的桿臂誤差。

表1撓曲變形角在y軸和z軸上引起的桿臂誤差

Table1Lever-armerroronyaxisandzaxiscausedbyflexuraldeformationangle

撓曲變形角誤差y軸z軸θxy01-sinθxcosθxθx()y0sin2θxθxθyz01-sinθycosθyθy()θzx0sin2θzθz

在動態(tài)桿臂δr影響下的桿臂長度為

(8)

當(dāng)撓曲變形角滿足小角度假設(shè)時(shí)，θ′滿足sinθ′≈θ′，cosθ′≈1，那么式(8)可以簡化為

(9)

動態(tài)桿臂δr可以表示為

δr=L0θ

(10)

對式(10)左右兩邊求1階導(dǎo)、2階導(dǎo)可得

(11)

(12)

式(12)結(jié)合式(1)后可以進(jìn)一步表示為

(13)

式中：

(14)

(15)

由圖3可知，艦船在航行時(shí)會產(chǎn)生船體搖擺，通常認(rèn)為主慣性組件的安裝位置為船體的搖擺中心，由于子慣性組件的安裝位置和艦船搖擺中心的位置不一致，導(dǎo)致子慣導(dǎo)的加速度計(jì)中包含干擾加速度，從而主、子慣導(dǎo)導(dǎo)航計(jì)算機(jī)解算的速度信息不同的現(xiàn)象稱為桿臂效應(yīng)誤差，主、子慣性組件敏感到的比力信息之間的差異稱為桿臂加速度誤差。

圖3 桿臂效應(yīng)示意圖Fig.3 Schematic diagram of lever-arm effect

Oixiyizi坐標(biāo)為慣性坐標(biāo)系，Omxmymzm為主慣導(dǎo)載體坐標(biāo)系，Osxsyszs為子慣導(dǎo)載體坐標(biāo)系，根據(jù)圖3中矢量關(guān)系可得

(16)

(17)

(18)

(19)

(20)

(21)

1.3 主、子慣導(dǎo)姿態(tài)誤差模型

(22)

(23)

考慮到撓曲變形角可以近似為小角度，式(23)可以改寫為

(24)

由姿態(tài)矩陣微分方程容易得到[11-14,16]

(25)

(26)

(27)

式(27)兩邊同時(shí)求微分得到

(28)

(29)

(30)

將式(30)代入式(29)得到

(31)

由反對稱矩陣的性質(zhì)可以得到

(32)

將式(24)代入式(32)得到

(33)

(34)

式中：

(35)

將式(33)代入式(34)，得到非線性傳遞對準(zhǔn)姿態(tài)誤差方程為

(36)

1.4 主、子慣導(dǎo)速度誤差模型

主慣導(dǎo)的速度微分方程為

(37)

基于計(jì)算載體坐標(biāo)系的速度微分方程為

(38)

(39)

通常情況下，固定桿臂引起的桿臂速度誤差可以在速度誤差量測量中進(jìn)行補(bǔ)償，但是無法補(bǔ)償動態(tài)桿臂加速度引起的速度誤差量，將式(21)代入式(39)即可得到非線性一體化速度誤差方程為

(40)

式中:

2 基于M-RHCKF算法設(shè)計(jì)

2.1 傳遞對準(zhǔn)的狀態(tài)方程與觀測方程

將式(10)兩邊同時(shí)微分可以得到動態(tài)桿臂δr的微分方程為

(41)

(42)

(43)

由式(1)、式(36)、式(40)～式(43)可以構(gòu)成非線性一體化傳遞對準(zhǔn)狀態(tài)方程。

采用“速度加姿態(tài)”匹配模式，速度觀測量可以通過求取主、子慣導(dǎo)速度差值并對標(biāo)稱桿臂r0進(jìn)行桿臂速度補(bǔ)償后得到。姿態(tài)觀測量可以由主、子慣導(dǎo)的姿態(tài)陣相乘得到，其表達(dá)式為

(44)

不難發(fā)現(xiàn)量測失準(zhǔn)角φm即做狀態(tài)變量也做觀測量，此時(shí)的量測方程為簡單的線性方程，觀測矩陣H為

(45)

2.2 簡化高階CKF(RHCKF)算法

HCKF算法的濾波精度可以達(dá)到5階[18-19]，相比于傳統(tǒng)的CKF或UKF算法的3階估計(jì)精度具有明顯優(yōu)勢，但是其每次在時(shí)間、量測更新過程中的容積變換需要采樣(2n2+1)個(gè)容積點(diǎn)，所以，較大的計(jì)算量也是限制其應(yīng)用的一個(gè)原因。為了保留濾波算法的高精度優(yōu)勢，由式(45)可知，本文提出的快速傳遞對準(zhǔn)模型中的量測方程為線性，故可采用簡化量測更新過程(具體證明過程見附錄A)代替?zhèn)鹘y(tǒng)的HCKF量測更新過程，只需在時(shí)間更新過程中進(jìn)行一次容積變化即可完成全部濾波算法，具體算法流程如下。

1) 濾波器初始化

(46)

2) 時(shí)間更新過程

① 計(jì)算容積點(diǎn)Xi,k-1|k-1(i=0,1,…,2n2)。

奇異值分解(SVD)相比于傳統(tǒng)的Cholesky分解可以更好地解決協(xié)方差矩陣病態(tài)條件的問題，該分解方法沒有要求協(xié)方差矩陣滿足對稱性和正定性的條件，使得整個(gè)算法具有更高的數(shù)值穩(wěn)定性和濾波精度，對協(xié)方差矩陣Pk-1/k-1使用SVD分解，即

(47)

(48)

式中：S=diag(s1,s2,s3,…,sr)，s1≥s2≥s3≥…≥sr≥0，U∈Rm×m，V∈Rn×n，為矩陣Pk-1/k-1的奇異值;ξi為求積分點(diǎn)集，當(dāng)采用5階容積準(zhǔn)則時(shí)，共有2n2+1個(gè)求積分點(diǎn)，其具體表達(dá)式為[7-9]

(49)

(50)

(51)

(52)

式中：ωi為容積點(diǎn)的權(quán)值，如式(53)所示

ωi=

(53)

④ 計(jì)算k時(shí)刻的預(yù)測誤差協(xié)方差陣Pk/k-1。

(54)

3) 簡化量測更新過程

⑤ 計(jì)算更新后的狀態(tài)容積點(diǎn)Xi,k|k-1。

(55)

式中:Pk/k-1=Sk|k-1(Sk|k-1)T。

⑥ 計(jì)算經(jīng)過量測方程傳遞的容積點(diǎn)Zi,k|k-1。

Zi,k|k-1=HkXi,k|k-1

(56)

(57)

⑧ 計(jì)算k時(shí)刻的量測誤差協(xié)方差陣Pzz,k|k-1和預(yù)測互相關(guān)協(xié)方差陣Pxz,k|k-1。

(58)

(59)

4) 濾波更新過程

Kk=Pxz,k|k-1(Pzz,k|k-1)-1

(60)

(61)

Pk|k=Pk|k-1-KkPzz,k|k-1(Kk)T

(62)

顯然，RHCKF算法只需在時(shí)間更新過程中進(jìn)行一次容積變換，量測更新過程則不需要。所以相比于傳統(tǒng)HCKF算法，基于SVD分解的SHCKF具有更少的計(jì)算量和更強(qiáng)的數(shù)值穩(wěn)定性。

2.3 非線性快速傳遞對準(zhǔn)模型的部分線性結(jié)構(gòu)

采用歐幾里德方法對狀態(tài)方程進(jìn)行離散化處理[20-21]，離散間隔為dt，離散形式的非線性快速傳遞對準(zhǔn)模型可以寫為

(63)

式中：

A=

B=

γ(a(k))·b(k)

(64)

式中:ψ(a(k))和γ(a(k))分別表示為

(65)

(66)

2.4 基于邊緣采樣的高階CKF(M-HCKF)算法

(67)

式中:ψ(·)為非線性函數(shù);γ(·)為非線性函數(shù)或線性函數(shù)。假設(shè)狀態(tài)變量x服從高斯分布，它的均值與方差可以表示為

(68)

(69)

可以得到高斯隨機(jī)變量b相對于高斯隨機(jī)變量a的條件均值E(b|a)和條件協(xié)方差Pb|a的表達(dá)式為[20-22]

(70)

(71)

此時(shí)隨機(jī)變量y的均值和方差可以表示為

E(y)=?(ψ(a)+γ(a)b)p(a,b)d(ab)=

(72)

Py=?(y-E(y))(y-E(y))T·p(a,b)d(ab)=

?(ψ(a)+γ(a)b-E(y))·(ψ(a)+

γ(a)b-E(y))T·p(a,b)d(ab)=

γ(a)Pb|aγ(a)T]·p(a)da

(73)

式中:φ(a)=ψ(a)+γ(a)·E(b|a)。想要求得式(72)、式(73)的解析解是比較困難的，可以采用高斯近似求和濾波的思想得到它們的數(shù)值積分解，結(jié)合2.2節(jié)提出的HCKF算法，選擇高階容積采樣規(guī)則計(jì)算均值和方差，此時(shí)E(y)和Py可以表示為

(74)

γ(ξi)Pb|aγ(ξi)T]

(75)

3 仿真實(shí)驗(yàn)

設(shè)置載體的初始位置為東經(jīng)127°，北緯30°，高度0 m，載體初始速度大小為10 m/s，方向正北，艦船做勻速直線運(yùn)動，仿真總時(shí)長為120 s。SINS的解算周期為0.01 s，艦載機(jī)子慣導(dǎo)的陀螺和加速度計(jì)的參數(shù)如表2所示，假設(shè)艦船主慣導(dǎo)系統(tǒng)無誤差。

假設(shè)艦船在海浪的作用下做如下形式的3軸搖擺運(yùn)動：

表2 慣性測量組件主要性能參數(shù)Table 2 Parameters of inertial measurement module

式中：θm、γm和φm為艦船的搖擺幅度(θm=12°，γm=15°，φm=10°);ωi=2π/Ti(i=θ,γ,φ)為搖擺頻率(Tθ=8 s，Tγ=10 s，Tφ=6 s);αi(i=θ,γ,φ)為初始相位角(αθ=0°,αγ=0°,αφ=30°);標(biāo)稱桿臂長度r0=[10 m10 m20 m]T，實(shí)際失準(zhǔn)角φa=[0.5°0.6°10°]T，2階Markov相關(guān)時(shí)間τi=60 s(i=x,y,z)。

圖4和圖5分別為M-RHCKF和RHCKF對姿態(tài)失準(zhǔn)角φ和實(shí)際失準(zhǔn)角φa的估計(jì)曲線。可以看到，姿態(tài)失準(zhǔn)角大約在10 s后收斂到穩(wěn)態(tài)，實(shí)際失準(zhǔn)角φa大約在20 s后收斂到穩(wěn)態(tài)。這是因?yàn)閷τ诓捎谩八俣?姿態(tài)”匹配的快速傳遞對準(zhǔn)模型而言，由于觀測量中含有量測失準(zhǔn)角的的信息，所以即使艦船不做加速機(jī)動，M-RHCKF和RHCKF均可以對姿態(tài)失準(zhǔn)角φ和φa進(jìn)行有效的估計(jì)。表3為M-RHCKF和RHCKF在100 s至120 s時(shí)間段的姿態(tài)失準(zhǔn)角φ和實(shí)際失準(zhǔn)角φa的估計(jì)誤差統(tǒng)計(jì)表。由表3可知M-RHCKF算法在具有更少采樣點(diǎn)的情況下濾波精度與RHCKF相當(dāng)，兩者的估計(jì)結(jié)果驗(yàn)證了邊緣采樣算法的有效性。

圖4 M-RHCKF和RHCKF對失準(zhǔn)角的濾波估計(jì)Fig.4 Filter estimation of misalignment angle using M-RHCKF and RHCKF

圖6為撓曲變形角θ的估計(jì)曲線，圖7為動態(tài)桿臂δr的估計(jì)曲線。從圖6和圖7可以看出大約在20 s后RHCKF和M-RHCKF兩者趨于穩(wěn)定，均可以跟蹤上撓曲變形角和動態(tài)桿臂的變化。由標(biāo)稱桿臂長度r0并結(jié)合式(10)可知，由于r0在z軸的分量最大，產(chǎn)生沿x軸上的動態(tài)桿臂誤差同樣最大，同理沿y軸上的動態(tài)桿臂誤差最小，這與圖7中δr的真實(shí)值曲線是相符合的。

表4為未考慮動態(tài)桿臂的傳遞對準(zhǔn)模型和提出的一體化傳遞對準(zhǔn)模型在濾波收斂后最后40 s的均值和方差統(tǒng)計(jì)表。顯然，對于未考慮動態(tài)桿臂的傳遞對準(zhǔn)模型而言，由于速度誤差模型中沒有包含動態(tài)桿臂速度誤差項(xiàng)，此時(shí)速度誤差模型與實(shí)際情況是失配的，然而一體化傳遞對準(zhǔn)模型在子慣導(dǎo)比力中考慮了動態(tài)桿臂加速度誤差項(xiàng)，建立了動態(tài)桿臂δr的數(shù)學(xué)模型，與艦船航行中的實(shí)際情況更相符，所以相比于前者具有更好的濾波性能。

圖5 M-RHCKF和RHCKF對實(shí)際失準(zhǔn)角的濾波估計(jì)Fig.5 Filter estimation of true misalignment angle using M-RHCKF and RHCKF

表3兩種濾波算法的估計(jì)誤差統(tǒng)計(jì)(100～120s)

Table3Estimatederrorstatisticsoftwofilteralgorithms(100-120s)

估計(jì)誤差M-RHCKFRHCKF?x/(′)1.36721.3792?y/(′)1.70811.7998?z/(′)1.33911.3169?ax/(′)0.58310.5384?ay/(′)0.87520.8329?az/(′)1.19681.1213

圖6 M-RHCKF和RHCKF對撓曲變形角的濾波估計(jì)Fig.6 Filter estimation of flexural deformation angle using M-RHCKF and RHCKF

圖7 M-RHCKF和RHCKF對動態(tài)桿臂變形的濾波估計(jì)Fig.7 Filter estimation of dynamic lever arm deformation using M-RHCKF and RHCKF

為了比較M-RHCKF和RHCKF算法的運(yùn)行時(shí)間，仿真軟件采用MATLAB2014A，仿真處理器為Intel inside core i5處理器，分別對M-RHCKF和RHCKF兩種算法進(jìn)行50次仿真實(shí)驗(yàn)，得到兩種濾波算法的總運(yùn)行時(shí)間和平均單次運(yùn)行時(shí)間如表5所示。

由表5可知M-RHCKF算法所消耗的運(yùn)行時(shí)間較RHCKF算法更少，結(jié)合表3的計(jì)算結(jié)果，結(jié)果證明邊緣采樣算法和簡化量測更新過程可以在保持濾波精度的同時(shí)可有效減小計(jì)算量，具有更強(qiáng)的實(shí)用性。

表4 兩種模型估計(jì)誤差統(tǒng)計(jì)Table 4 Estimated error statistics of two models

表5 兩種濾波算法運(yùn)行時(shí)間統(tǒng)計(jì)Table 5 Statistics of running time for two filtering algorithms

4 結(jié) 論

1) 建立了一種非線性一體化快速傳遞對準(zhǔn)模型，并驗(yàn)證了其正確性，該模型不對實(shí)際失準(zhǔn)角和姿態(tài)失準(zhǔn)角進(jìn)行小角度假設(shè)，可以準(zhǔn)確估計(jì)出撓曲變形角和動態(tài)桿臂，具有更廣闊的應(yīng)用范圍。

2) 分析了所提出的快速傳遞對準(zhǔn)誤差模型的部分線性結(jié)構(gòu)，結(jié)合該特點(diǎn)設(shè)計(jì)了一種基于邊緣采樣的M-RHCKF算法。理論分析證明該算法相比于傳統(tǒng)的HCKF具有更小的計(jì)算量，可以滿足傳遞對準(zhǔn)精度和時(shí)間的要求。

3) 實(shí)際中由于各種外界因素的影響，在無法準(zhǔn)確獲得撓曲變形和動態(tài)桿臂變化特性的條件下，可以通過研究魯棒非線性濾波算法達(dá)到抑制撓曲變形和動態(tài)桿臂影響的目的。