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

空軍工程大學 航空工程學院， 西安 710038

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

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

1 非線性快速傳遞對準一體化模型

1.1 坐標系定義

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

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

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

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

目前大多數(shù)研究認為Markov過程可以較好地描述撓曲變形運動[6-9,15,17]，此時甲板的撓曲變形角可以看做是由一個白噪聲激勵的二階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為θ在另兩個軸向上引起的桿臂誤差。

表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)

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

(9)

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

δr=L0θ

(10)

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

(11)

(12)

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

(13)

式中：

(14)

(15)

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

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

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

(16)

(17)

(18)

(19)

(20)

(21)

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

(22)

(23)

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

(24)

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

(25)

(26)

(27)

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

(28)

(29)

(30)

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

(31)

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

(32)

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

(33)

(34)

式中：

(35)

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

(36)

1.4 主、子慣導速度誤差模型

主慣導的速度微分方程為

(37)

基于計算載體坐標系的速度微分方程為

(38)

(39)

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

(40)

式中:

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

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

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

(41)

(42)

(43)

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

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

(44)

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

(45)

2.2 簡化高階CKF(RHCKF)算法

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

1) 濾波器初始化

(46)

2) 時間更新過程

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

奇異值分解(SVD)相比于傳統(tǒng)的Cholesky分解可以更好地解決協(xié)方差矩陣病態(tài)條件的問題，該分解方法沒有要求協(xié)方差矩陣滿足對稱性和正定性的條件，使得整個算法具有更高的數(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為求積分點集，當采用5階容積準則時，共有2n2+1個求積分點，其具體表達式為[7-9]

(49)

(50)

(51)

(52)

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

ωi=

(53)

④ 計算k時刻的預測誤差協(xié)方差陣Pk/k-1。

(54)

3) 簡化量測更新過程

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

(55)

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

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

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

(56)

(57)

⑧ 計算k時刻的量測誤差協(xié)方差陣Pzz,k|k-1和預測互相關(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算法只需在時間更新過程中進行一次容積變換，量測更新過程則不需要。所以相比于傳統(tǒng)HCKF算法，基于SVD分解的SHCKF具有更少的計算量和更強的數(shù)值穩(wěn)定性。

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

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

(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)

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

(70)

(71)

此時隨機變量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ī)則計算均值和方差，此時E(y)和Py可以表示為

(74)

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

(75)

3 仿真實驗

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

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

表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°);標稱桿臂長度r0=[10 m10 m20 m]T，實際失準角φa=[0.5°0.6°10°]T，2階Markov相關(guān)時間τi=60 s(i=x,y,z)。

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

圖4 M-RHCKF和RHCKF對失準角的濾波估計Fig.4 Filter estimation of misalignment angle using M-RHCKF and RHCKF

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

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

圖5 M-RHCKF和RHCKF對實際失準角的濾波估計Fig.5 Filter estimation of true misalignment angle using M-RHCKF and RHCKF

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

Table3Estimatederrorstatisticsoftwofilteralgorithms(100-120s)

估計誤差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對撓曲變形角的濾波估計Fig.6 Filter estimation of flexural deformation angle using M-RHCKF and RHCKF

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

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

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

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

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

4 結(jié) 論

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

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

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