張 濤,王 帥，劉興華

（1.東南大學(xué)儀器科學(xué)與工程學(xué)院,南京 210096;2.微慣性儀表與先進導(dǎo)航技術(shù)教育部重點實驗室,南京 210096）

現(xiàn)代艦船都裝備有各種高精度導(dǎo)航設(shè)備，為了能使這些設(shè)備正常協(xié)調(diào)工作，必須有一個統(tǒng)一的空間坐標基準系統(tǒng)。但是，由于艦船并不是一個絕對剛體以及船體自身重量和溫度、海浪沖擊等外部環(huán)境的影響，使得船體和甲板產(chǎn)生微小變形。根據(jù)俄羅斯學(xué)者A.V.Mochalov所述，艦船甲板變形角最高可達1°～1.5°，動態(tài)撓曲變形也可達角分量級[1]。由于船體變形的影響，使艦船設(shè)備獲得的姿態(tài)信息不再準確，而船載設(shè)備一般都是高精度設(shè)備，微小的變形也會使設(shè)備性能受到嚴重影響。因此，對船體變形角進行實時測量和補償，以保證船載設(shè)備在統(tǒng)一的基準坐標系下完成高精度工作成為近年來國內(nèi)外研究的一個熱點問題[2]。

目前，國內(nèi)外對船體變形角采用的有效測量手段有光學(xué)測量法、慣性測量法、GPS測量法等。其中光學(xué)測量法使用光學(xué)設(shè)備對船體甲板變形角進行實時監(jiān)測，測量精度高，理論精度能夠達到5角秒以內(nèi)，測量結(jié)果可以近似為船體的實際變形[3]。但是光學(xué)測量法成本高昂，結(jié)構(gòu)復(fù)雜且實施困難，不能大規(guī)模使用。由于慣性系統(tǒng)體積小、精度高、隱蔽性好等優(yōu)點，使慣性測量匹配法成為目前最具有發(fā)展?jié)摿Φ难芯糠较?，其思想是通過在中心主慣導(dǎo)系統(tǒng)附近和局部戰(zhàn)位點附近分別安裝一套慣性系統(tǒng)，兩個慣導(dǎo)之間輸出的信息差能夠反映二者失準角的大小，通過二者輸出的信息進行動態(tài)匹配實現(xiàn)船體變形的測量。

基于姿態(tài)匹配的船體變形測量方法最早由國防科技大學(xué)提出[4]，之后對激光陀螺的誤差進行補償[5][6]以及對模型參數(shù)的辨識展開了研究[7]。但是其采用的動態(tài)變形角模型是非時變二階馬爾科夫模型，沒有考慮模型偏差帶來的誤差，而實際船體動態(tài)變形模型復(fù)雜多變且慣性系統(tǒng)受到的噪聲不單一。針對變形角模型不確定的問題，哈爾濱工程大學(xué)提出一種交互式多模型濾波的船體變形測量方法[8]，但是由于先驗信息的不準確使得狀態(tài)轉(zhuǎn)移概率很難定義，而且多次更新卡爾曼濾波方程需要大量的計算時間。文獻[9]提出一種基于姿態(tài)四元數(shù)匹配的神經(jīng)網(wǎng)絡(luò)船體變形測量方法，并對卡爾曼濾波中的時間延遲進行了補償[10]，避免了歷史變形數(shù)據(jù)對實時變形影響導(dǎo)致的誤差。但是計算神經(jīng)網(wǎng)絡(luò)系統(tǒng)最優(yōu)參數(shù)需要大量的訓(xùn)練數(shù)據(jù)并且可能會導(dǎo)致過度學(xué)習(xí)。

基于多重漸消因子的強跟蹤卡爾曼濾波（Strong Tracking Kalman Filter，STKF）采用使殘差序列正交的方法確定最佳的多個漸消因子,在模型失配的情況下仍能很好地跟蹤狀態(tài)量的變化，該算法因有較強的魯棒性并且得到廣泛應(yīng)用[11]。在實際的船體變形測量中，由于環(huán)境的復(fù)雜性，慣性設(shè)備的誤差通常是各種噪聲的疊加，它們極少服從正態(tài)分布，有的幅值較大甚至還有奇異點。而傳統(tǒng)的卡爾曼濾波以最小均方誤差（MMSE）為最優(yōu)準則，不能反映真實的噪聲情況，會產(chǎn)生較大誤差。最大互相關(guān)熵卡爾曼濾波以最大相關(guān)熵準則（Maximum Correntropy Criterion，MCC）為最優(yōu)準則，不僅能捕獲通常的二階統(tǒng)計信息，而且還可以獲得更高階的統(tǒng)計信息，因此能夠獲得更好的估計效果[12]。本文設(shè)計了一種強跟蹤最大互相關(guān)熵卡爾曼濾波（Strong Tracking Maximum Correntropy Kalman Filter，STMCKF）算法，與傳統(tǒng)卡爾曼濾波相比，當模型不確定或存在未知干擾時，能夠更有效的估計船體變形角。

1 船體變形測量模型

船體變形測量系統(tǒng)由兩套慣性系統(tǒng)（Inertial navigation system，INS）組成，如圖1所示。在中心慣導(dǎo)附近安裝一個慣性系統(tǒng)（SINS1），局部戰(zhàn)位點安裝另一個慣性系統(tǒng)（SINS2），定義坐標系分別為ox1y1z1和ox2y2z2，x軸沿著船體的右舷，y軸指向船艏方向，z軸與船體甲板平面垂直并指向上方。xyz構(gòu)成右手直角坐標系，即為右前上坐標系。

圖1 船體變形測量系統(tǒng)構(gòu)成Fig.1 Composition of hull deformation measurement system

令SINS1的載體坐標系為b1，并選擇初始時刻t0的載體坐標系b1作為SINS1的慣性坐標系i1。同樣，令SINS2的載體坐標系為b2，選擇初始時刻t0的載體坐標系b2作為SINS2的慣性坐標系i2。則表示i2系到i1系的坐標變換矩陣，反映的是t0時刻的變形角。為系到b1系的坐標變換矩陣，反映t時刻的變形角。由于陀螺偏移，導(dǎo)致解算出的慣性系與真實的慣性系i1，i2之間存在偏差，把解算得到的慣性系稱為計算慣性系，簡記為i1′，i2′。則根據(jù)坐標系的定義，旋轉(zhuǎn)矩陣之間的關(guān)系為：

記t時刻的變形為φ=[φx,φy,φz]T，當φ較小時，近似為：

記t0時刻的失準角為φ0=[φ0x,φ0y,φ0z]T，其包含有初始對準誤差和初始變形角，當φ0較小時，近似有：

記對應(yīng)的歐拉角分別為θi1和θi2，當θi1和θi2較小時，近似有：

將式（2）（3）（4）代入（1），即得：

記和分別為：

將式（6）代入式（5），根據(jù)矩陣兩邊的元素(1,2)、(3,1)、(3,2)相等分別建立等式，化簡并忽略二階小量得到：

式中，矩陣Z和A由SINS1、SINS2的陀螺輸出和決定并隨其改變，具體表示為：

,θi2表示由陀螺漂移引起i′系與i系的偏差，慣性空間失準角與陀螺漂移之間的關(guān)系為：

式中ε1、ε2分別為SINS1和SINS2的陀螺漂移。

根據(jù)船體變形頻譜分布的不同將其分為靜態(tài)變形和動態(tài)變形[8]，其中靜態(tài)變形在短時間內(nèi)可以看作常值，動態(tài)變形角滿足二階Markov過程，其模型為：

式中，βi是撓曲變形的模型參數(shù)，其大小與相關(guān)時間有關(guān)，具體表述為：

τi是船體變形的相關(guān)時間，ηi是白噪聲，其方差Qηi的大小為：

σi是動態(tài)變形角θi的方差。

影響船體變形測量精度的一個主要因素是陀螺偏移，根據(jù)陀螺漂移的特性，一般將其分為隨機常值誤差和隨機游走誤差，分別建立模型為：

式中，w(t)～(0,σ2)，T為相關(guān)時間，由于相關(guān)時間很大，隨機游走誤差可以簡化為：

選取系統(tǒng)狀態(tài)向量為18維，即：

式中：Φ為靜態(tài)變形角，θ為動態(tài)變形角，為動態(tài)變形角速度，φ0為t0時刻的失準角，ε1、ε2分別為SINS1和SINS2的陀螺漂移。

將式（7）中的Z作為觀測量，則系統(tǒng)的觀測方程可表示為：

考慮到φ=Φ+θ及式(8)，觀測矩陣可表示為：

2 強跟蹤最大熵卡爾曼濾波算法

2.1 模型誤差對估計結(jié)果的影響

實際船體變形模型中，由于導(dǎo)致船體變形的因素復(fù)雜多變，沒有一個固定的形式，采用一個時不變的動態(tài)變形模型必然會產(chǎn)生誤差。在先驗估計中引入偏差項推導(dǎo)模型偏差對動態(tài)變形角的估計誤差[13]，具體表述為：

式中：ΔXk代表先驗估計和真實狀態(tài)Xk的動態(tài)模型偏差，代入卡爾曼濾波標準公式：

得到：

與式（18）相比，動態(tài)模型偏差ΔXk導(dǎo)致的后驗估計偏差為：

從式（20）可以看出，由ΔXk導(dǎo)致的估計誤差與增益矩陣K和觀測矩陣H是緊密相連的，如果在時變觀測系統(tǒng)中不考慮ΔXk導(dǎo)致的誤差，可能會對測量精度帶來較大誤差，甚至在有些特殊情況會導(dǎo)致系統(tǒng)不可觀測。因此，在進行觀測更新之前，需要對模型偏差進行修正。

2.2 多漸消因子強跟蹤濾波算法

傳統(tǒng)卡爾曼濾波具有記憶性，綜合利用了所有的量測值Zk，過去的量測數(shù)據(jù)會直接影響現(xiàn)在時刻狀態(tài)的估計精度，由于模型存在偏差，會使方差陣Pk不能正確反映狀態(tài)的估計精度，造成較大誤差。多漸消因子強跟蹤濾波通過對各數(shù)據(jù)通道以不同的速率進行漸消，更準確地調(diào)整增益矩陣，使系統(tǒng)具有較強的跟蹤能力。

當模型有偏差時，會使式（21）中等號兩邊不相等，導(dǎo)致新息方差矩陣失衡。為使式（21）等號兩邊相等，將Pk,k-1重寫為：

式中Λk=diag(λ1λ2…λk)表示狀態(tài)變量相應(yīng)的漸消因子組成的對角陣。

則式（21）可以重新表述為：

式中為k時刻新息序列γk的協(xié)方差矩陣估計值，可以根據(jù)式(24)進行估算：

式中ρ為遺忘因子，0 ＜ρ≤1，通常ρ= 0.95。

進一步，令

故可得

若Hk滿秩，即m=rank(Hk)，且Hk有j1,j2,…,jm共m列元素不全為0，則Mk可以簡化為：

式中，S為Hk中不全為0的m列元素組成的矩陣，Jm為Jk中第j1,j2,…,jm行和第j1,j2,…,jm列元素組成的矩陣。

將式（27）代入式（26）并整理可得：

令βk=S-1Nk(ST)-1，因此多重漸消因子求法為：

式中βk(i,i)和Jk(ji,ji)分別為矩陣βk和Jk的對角線元素值。

2.3 最大相關(guān)熵卡爾曼濾波

對于兩個服從聯(lián)合概率密度為FX,Y(x,y)的隨機變量X,Y∈R，其互相關(guān)熵定義為：

其中，E(·)表示求期望，κ(·)表示高斯核函數(shù)，即

式中，e=x-y，σ＞ 0為核函數(shù)寬度。

將系統(tǒng)狀態(tài)方程和量測方程重新表述為：

式中：

分別將Pk,k-1和Rk進行Cholesky分解得到Bp,(k,k-1)和Br,k，則可得：

在式（32）左右兩邊同時乘，并重寫為：

式中：

定義基于信息最大熵準則下的代價函數(shù)為：

式中，n+m是Dk的維數(shù)，di,k和wi,k分別是Dk與Wk的第i個元素，Xk的最優(yōu)解可以通過使代價函數(shù)最大求得[14]，即

因此，可以通過式(38)得出Xk的最優(yōu)解：

求解式(28)可得

式中xi,k是Xk的第i個元素。

進一步，定義：

將式（38）寫為矩陣形式為：

從式（40）可以看出，基于信息最大熵準則的濾波方法利用Ck對誤差協(xié)方差重新加權(quán)并重新構(gòu)建量測信息，具體表述為：

3 仿真實驗與結(jié)果分析

3.1 仿真條件

（1）艦船以10 m/s的速度勻速直線航行，以正弦規(guī)律繞橫搖軸、縱搖軸、航向軸作三軸搖擺，相應(yīng)的擺動幅值分別為3 °、4 °、5 °，擺動周期分別為10 s、8 s、6 s，隨機選取初始航向角為北偏東60 °，采樣頻率為100 Hz。仿真軌跡如圖2所示。

（2）認為船體靜態(tài)變形角是常值，分別設(shè)置為0.2 °、0.1 °、0.05 °。船體動態(tài)變形角為二階馬爾科夫隨機過程，變形角方差σ為5 ″、6 ″、7 ″，相關(guān)時間τ為1 s、1.5 s、1.8 s。

（3）SINS1、SINS2的陀螺常值漂移都取為0.05°/h,隨機游走誤差都取為

圖2 仿真軌跡圖Fig.2 Simulation trajectory

3.1 仿真結(jié)果與分析

設(shè)仿真時間為900 s，為驗證本文提出算法的有效性，在三種環(huán)境下進行仿真驗證，環(huán)境設(shè)置如表1所示。

表1 三種環(huán)境設(shè)置表Tab.1 Three environment settings

在第一種情況中，假設(shè)在300～600 s的時間段內(nèi)，系統(tǒng)的真實噪聲變?yōu)?5Q，強跟蹤最大熵卡爾曼濾波和傳統(tǒng)卡爾曼濾波的變形角估計誤差如圖3～5所示。在第二種情況中，假設(shè)在300～400 s的時間段內(nèi)，系統(tǒng)的動態(tài)變形角模型發(fā)生變化，將動態(tài)變形角方差σ改為2σ，對應(yīng)的變形角估計誤差如圖6～8所示。在第三種情況中，假設(shè)慣性系統(tǒng)噪聲不是高斯白噪聲，而是重尾的混合高斯分布，SINS1、SINS2的隨機噪聲設(shè)置為：

對應(yīng)的仿真結(jié)果如圖9～11所示。三種環(huán)境下的均方根誤差如表2所示（20 s之后穩(wěn)定求得）。

表2 變形角均方根誤差Tab.2 Deformation Angle root mean square error(")

圖3 情況一縱搖角變形估計誤差Fig.3 pitching angle deformation estimation error in case-1

圖4 情況一橫搖角變形估計誤差Fig.4 rolling angle deformation estimation error in case-1

圖5 情況一航向角變形估計誤差Fig.5 yawing angle deformation estimation error in case-1

從圖3～5可以看出，在情況一中，當系統(tǒng)噪聲突然變大時，變形角估計誤差變大，其中航向角誤差變化最大，強跟蹤最大熵卡爾曼濾波比傳統(tǒng)卡爾曼濾波的估計精度有明顯提高，尤其航向角最為明顯。結(jié)合表2數(shù)據(jù)，縱搖角、橫搖角和航向角的估計精度分別提高8.42%、22.41%和30.29%。由此可知，強跟蹤最大熵卡爾曼濾波可以更有效地跟蹤系統(tǒng)噪聲突變狀態(tài)，同時，由于系統(tǒng)噪聲突變和濾波器慣性等原因，導(dǎo)致600 s后傳統(tǒng)卡爾曼濾波航向角估計誤差出現(xiàn)一個偏置，但強跟蹤最大熵卡爾曼濾波的收斂誤差可以減少30.41″，使偏置減小。

圖6 情況二縱搖角變形估計誤差Fig.6 pitching angle deformation estimation error in case-2

圖7 情況二橫搖角變形估計誤差Fig.7 rolling angle deformation estimation error in case-2

圖8 情況二航向角變形估計誤差Fig.8 yawing angle deformation estimation error in case-2

從圖6～8可以看出，在情況二中，變形角估計誤差在300s時有突變的現(xiàn)象，這是因為在300～400s的時間內(nèi)變形角模型突變使數(shù)據(jù)不連續(xù)導(dǎo)致的。結(jié)合表2數(shù)據(jù)，縱搖角、橫搖角和航向角的估計精度分別提高11.78%、24.14%和12.82%，由此可知，強跟蹤最大熵卡爾曼濾波比傳統(tǒng)卡爾曼濾波的收斂精度提高較大，說明強跟蹤最大熵卡爾曼濾波提高了系統(tǒng)的魯棒性及環(huán)境適應(yīng)性。

圖9 情況三縱搖角變形估計誤差Fig.9 pitching angle deformation estimation error in case-3

圖10 情況三橫搖角變形估計誤差Fig.10 rolling angle deformation estimation error in case-3

圖11 情況三航向角變形估計誤差Fig.11 yawing angle deformation estimation error in case-3

在情況三中，仿真試驗設(shè)置的σ為0.4和0.5，從圖9～11可以看出，當σ為0.5時，其效果比σ為0.4時更接近傳統(tǒng)卡爾曼濾波，這是因為σ越大，二階誤差矩越占主導(dǎo)地位。結(jié)合表2數(shù)據(jù)，縱搖角、橫搖角和航向角的估計精度平均提高了14.17%、11.48%和9.25%，由此可知，當慣性系統(tǒng)的噪聲為重尾的混合高斯分布時，通過調(diào)節(jié)核寬度σ，強跟蹤最大熵卡爾曼濾波比傳統(tǒng)卡爾曼濾波的估計精度明顯提高。

4 結(jié) 論

本文針對船體變形測量過程中模型不確定以及受未知噪聲干擾導(dǎo)致的誤差問題，討論了模型偏差對濾波估計的影響，設(shè)計了一種基于姿態(tài)匹配的強跟蹤最大熵卡爾曼濾波算法。利用慣性系統(tǒng)輸出的姿態(tài)信息建立船體變形測量模型，以最大互相關(guān)熵為最優(yōu)準則，不僅可以獲得二階誤差矩，還可以獲得更高階誤差矩。通過自適應(yīng)地調(diào)整多個漸消因子，即使在濾波達到穩(wěn)態(tài)時，仍然可以調(diào)整濾波增益。本文在三種環(huán)境下對該算法進行仿真驗證，仿真結(jié)果表明，相比較傳統(tǒng)卡爾曼濾波，強跟蹤最大熵卡爾曼濾波在模型不確定以及未知噪聲情況下能夠取得更好的效果，使其對復(fù)雜環(huán)境具有更好的適應(yīng)性，提高了系統(tǒng)的估計精度和魯棒性。