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

盧航，郝順義，彭志穎，黃國(guó)榮

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

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

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

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

1.1 坐標(biāo)系定義

模型的推導(dǎo)過(guò)程主要涉及到以下幾種坐標(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)角定義為量測(cè)失準(zhǔn)角φm。

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

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

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

(1)

(2)

(3)

(4)

(5)

圖2 θz引起的動(dòng)態(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

在動(dòng)態(tài)桿臂δr影響下的桿臂長(zhǎng)度為

(8)

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

(9)

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

δr=L0θ

(10)

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

(11)

(12)

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

(13)

式中：

(14)

(15)

由圖3可知，艦船在航行時(shí)會(huì)產(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)

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

(32)

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

(33)

(34)

式中：

(35)

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

(36)

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

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

(37)

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

(38)

(39)

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

(40)

式中:

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

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

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

(41)

(42)

(43)

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

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

(44)

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

(45)

2.2 簡(jiǎn)化高階CKF(RHCKF)算法

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

1) 濾波器初始化

(46)

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

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

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

(54)

3) 簡(jiǎn)化量測(cè)更新過(guò)程

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

(55)

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

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

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

(56)

(57)

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

(58)

(59)

4) 濾波更新過(guò)程

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í)間更新過(guò)程中進(jìn)行一次容積變換，量測(cè)更新過(guò)程則不需要。所以相比于傳統(tǒng)HCKF算法，基于SVD分解的SHCKF具有更少的計(jì)算量和更強(qiáng)的數(shù)值穩(wěn)定性。

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

采用歐幾里德方法對(duì)狀態(tài)方程進(jìn)行離散化處理[20-21]，離散間隔為dt，離散形式的非線性快速傳遞對(duì)準(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相對(duì)于高斯隨機(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)動(dòng)，仿真總時(shí)長(zhǎng)為120 s。SINS的解算周期為0.01 s，艦載機(jī)子慣導(dǎo)的陀螺和加速度計(jì)的參數(shù)如表2所示，假設(shè)艦船主慣導(dǎo)系統(tǒng)無(wú)誤差。

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

表2 慣性測(cè)量組件主要性能參數(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)稱桿臂長(zhǎng)度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對(duì)姿態(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)閷?duì)于采用“速度+姿態(tài)”匹配的快速傳遞對(duì)準(zhǔn)模型而言，由于觀測(cè)量中含有量測(cè)失準(zhǔn)角的的信息，所以即使艦船不做加速機(jī)動(dòng)，M-RHCKF和RHCKF均可以對(duì)姿態(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對(duì)失準(zhǔn)角的濾波估計(jì)Fig.4 Filter estimation of misalignment angle using M-RHCKF and RHCKF

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

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

圖5 M-RHCKF和RHCKF對(duì)實(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對(duì)撓曲變形角的濾波估計(jì)Fig.6 Filter estimation of flexural deformation angle using M-RHCKF and RHCKF

圖7 M-RHCKF和RHCKF對(duì)動(dòng)態(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處理器，分別對(duì)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é)果證明邊緣采樣算法和簡(jiǎn)化量測(cè)更新過(guò)程可以在保持濾波精度的同時(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) 建立了一種非線性一體化快速傳遞對(duì)準(zhǔn)模型，并驗(yàn)證了其正確性，該模型不對(duì)實(shí)際失準(zhǔn)角和姿態(tài)失準(zhǔn)角進(jìn)行小角度假設(shè)，可以準(zhǔn)確估計(jì)出撓曲變形角和動(dòng)態(tài)桿臂，具有更廣闊的應(yīng)用范圍。

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

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