基于變分貝葉斯的容積H∞濾波在無人船載低成本MIMU對準的應用

常興國, 吳峻

(東南大學, 微慣性儀表與先進導航技術教育部重點實驗室, 南京 210096)

對于現代捷聯慣導系統而言,初始對準是影響其導航精度的關鍵因素之一。出于成本、有效載荷等方面的考慮,多數情況下,無人作業設備選用成本低、體積和功耗小的微機械慣性測量單元(micro inertial measurement unit, MIMU);此外,通常情況下無人船由母船牽引至海面后隨機投放,這種投放方法的不確定性會使得無人船載MIMU初始對準出現大失準角情況;同時,考慮到無人船即使在系泊狀態下仍受海浪搖擺、浪涌等干擾,進一步影響無人船載MIMU初始對準的精度。因此,研究無人船載低成本MIMU大失準角抗干擾初始對準對保證無人船高質量作業具有重要意義。

為了解決大失準角初始對準問題,有大量研究團隊對非線性濾波算法展開了研究。擴展卡爾曼濾波(extended Kalman filter, EKF)是最早出現并廣泛應用的算法,之后由Julier等[1]提出的無跡卡爾曼濾波(unscented Kalman filter, UKF)因其具有比EKF算法更高的精度和更小的計算量而逐漸獲得重視。然而,在處理高維非線性系統時,UKF算法的參數選取不當可能會使部分采樣點權值為負而造成濾波不穩定的情況。針對這一問題,相繼有學者提出了不同的采樣點和參數選取策略。Arasaratnam等[2]提出基于球面-相徑準則的容積卡爾曼濾波(cubature Kalman filter, CKF),CKF與UKF算法具有相似的近似計算過程,但其采樣點的選取經過嚴格的數學推導,具有更好的穩定性。

然而,一方面,相較于傳統艦船,無人船的工作環境更加復雜和惡劣,容易受到大幅搖擺、浪涌等影響;另一方面,受工藝等因素的限制,MIMU的輸出具有較大的噪聲,且各項誤差的穩定性較差。上述因素會使系統噪聲和量測噪聲呈現“時變”“厚尾”特性,但不論是何種非線性卡爾曼濾波算法,均依賴于噪聲統計特性的先驗知識,而具有“時變”“厚尾”特性的系統噪聲和量測噪聲使得先驗知識失去其意義,從而限制了無人船初始對準的精度。

對于量測噪聲未知或時變的問題,國內外研究團隊在傳統算法的基礎上提出了多種改進方案,其中最為廣泛應用的是以Sage-Husa自適應濾波為代表的量測噪聲自適應估計算法,可以有效解決時變噪聲下的對準問題[3-4]。變分貝葉斯(variational Bayesian, VB)方法是Sakkra等[5]提出的確定性近似自適應濾波算法,由于其出色的性能,VB-UKF、VB-CKF等VB方法與非線性濾波算法相結合的自適應算法被相繼提出[6-7]。然而,一方面,國內外對于噪聲統計同時具有時變特性和厚尾特性的狀態估計問題還未形成較完善和統一的研究和解決方案;另一方面,當系統噪聲和量測噪聲同時不確定或具有“時變”“厚尾”特性時,無論何種自適應濾波算法均無法同時實現準確估計。

與卡爾曼濾波相比,H∞濾波對信號的頻譜特性不做任何假設,從而在噪聲或狀態方程不確定的情況下具有更好的魯棒性[8];而基于對噪聲情況實時估計的自適應濾波手段能夠降低具有“時變”“厚尾”特性的噪聲對精度帶來的負面影響,進一步提高對準精度。受此啟發,現從惡劣海況下無人船載MIMU大失準角抗干擾初始對準這一具體場景出發,提出基于變分貝葉斯方法的容積H∞濾波算法(variational Bayesian cubature H∞filter, VBCH)。為考察算法性能,本文設計對比仿真實驗進行驗證。

1 模型建立

1.1 傳感器輸出模型

根據形成機理的不同,MIMU輸出噪聲可以分為零偏、標度因數誤差、正交誤差等確定性誤差和由工作環境變化導致的零偏不穩定、隨機游走以及信號采集引入的量化噪聲等隨機誤差?？紤]到低成本MIMU各噪聲項穩定性較差,將MIMU輸出噪聲描述為隨機常值零偏、高斯白噪聲和一階Markov噪聲的組合形式[9],表達式為

(1)

(2)

式(2)中:τg和τa分別為陀螺儀和加速度計相關時間常數;wg和wa分別為陀螺儀和加速度計相關零偏。

1.2 大失準角對準模型

記地心慣性坐標系和地球坐標系為i系和e系,規定MIMU導航坐標系(n系)為“東北天”(E-N-U)地理坐標系,慣性平臺計算坐標系為n′系,載體坐標系(b系)為“右前上”坐標系。假定導航坐標系依次經過三次轉動可以得到慣性平臺計算坐標系,記矢量φ=[φEφNφU],考慮大失準角的情況,非線性狀態模型,即

(3)

記12維狀態向量X、系統噪聲向量W、觀測向量Z和輔助觀測向量表達式為

(4)

記(3)式中與狀態向量相關的部分為f(X),與系統噪聲向量相關的部分為g(X),由此,建立大失準角對準模型表達式為

(5)

式(5)中:H、L分別為狀態向量X到觀測向量Z和狀態向量X到輔助觀測向量Y的轉換矩陣;V為量測噪聲向量。

2 容積H∞濾波

2.1 容積采樣規則

容積采樣規則是根據貝葉斯理論和Spherical-Radial規則,求解得到一組容積點逼近非線性系統狀態均值和協方差,從而實現計算簡單且高精度的非線性濾波。

在n維笛卡爾坐標系下,非線性濾波問題統一描述為

(6)

令x=rs,其中s為方向矢量,r為n維球體半徑,對式(6)進行Spherical-Radial變換,有

(7)

式(7)中:Un為n維單位球面;σ(·)為單位球面上的元素。

三階容積采樣規則利用2n個容積點將式(7)近似計算,得

(8)

聯立式(6)、式(8),得到三階容積采樣規則為

(9)

式(9)中:ξi為容積點;[1]i為對n維單位向量全排列生成的點集的第i列,ωi為對應容積點的權值。

2.2 次優H∞濾波問題

H∞控制理論的核心思想就是在系統模型和外界干擾存在不確定性時將H∞范數最小化。最優H∞濾波一般很難得到,因此通常求解其次優解,即

(10)

式(10)中:γ為H∞濾波魯棒因子;P0為狀態向量后驗協方差矩陣的初始值,Qk、Rk分別為k時刻的系統噪聲向量方差陣和量測噪聲向量方差陣,三者均為對稱正定矩陣;Wk和Vk分別為k時刻的系統噪聲和量測噪聲,滿足約束。

(11)

式(11)中:δjk為Kronecker-δ函數。

由式(10)可得Riccati不等式

(12)

式(12)中:Pk為后驗協方差矩陣,在量測方程為線性的情況下其遞推式為

(13)

2.3 基于新息的魯棒因子自適應估計

由式(12)不難看出,魯棒因子γ對于容積H∞濾波精度和魯棒性具有重要的影響:當魯棒因子γk的值比較小時,容積H∞濾波魯棒性較強,但精度隨之減小;當魯棒因子γk的值比較小時,容積H∞濾波精度增大,但魯棒性隨之減弱。因此,當系統存在較大外界干擾時,需要對魯棒因子進行動態調整,從而提高精度。

濾波新息反映外部不確定干擾的程度,其定義式為

(14)

根據Hermite矩陣的性質,式(12)可以等價轉換為

(15)

式(15)中:maxeig(A)表示矩陣A的最大特征值,C>0為系統常數,可通過實驗確定。

2.4 基于容積采樣規則的H∞濾波

根據前2.1～2.3節的敘述,當系統噪聲和量測噪聲固定不變時,基于容積采樣規則的H∞濾波算法如下:

(1)根據(9)式計算容積采樣點。

(2)進行濾波時間更新,表達式為

(16)

(17)

(3)根據式(13)進行量測更新。

(4)根據式(15)對魯棒因子進行自適應估計。

3 基于變分貝葉斯的自適應濾波

3.1 變分貝葉斯方法原理

變分貝葉斯方法基本原理就是通過將傳統貝葉斯方法中復雜且難以數值計算的后驗概率分布用形式更加簡單的變分分布進行替代,從而將問題轉化為相對簡單的變分分布各參數的優化問題。

假設z是觀測數據,其概率密度函數為變量θ的函數p(z;θ),引入隱變量y輔助計算,公式為

=?p(z|y,θ)p(y|θ)p(θ)dydθ

(18)

由于式(18)可能仍是高維數值積分,且潛在變量的存在導致需要被邊緣化額外的維度,從而造成求解難度極大。引入待估計參數集Θ={θ,y},設x為參數集Θ中任一參數,ψ為參數集中除x外的所有變量,通過Jensen不等式,將式(18)轉化為

=?q(x)q(ψ)lnp(x,ψ,z)dxdψ-

=F[Q(Θ)]

(19)

式(19)中:q(·)為后驗概率密度函數p(·|z)的一個以只以(·)為自變量的近似;Q(Θ)為參數集Θ的后驗概率分布函數P(Θ|z) 的近似;cx為與x無關的常量;F(·)為負自由能函數。

定義Kullback-Leibler散度(KL散度)為

KL(Q‖P)=lnp(z)-F[Q(Θ)]

(20)

顯然KL散度是非負的。

(21)

(22)

式(22)中:E(·)為數學期望;Θ(-θ)為待估計參數集除去任意參數θ后剩余所有元素;cθ為與θ無關的常量。

3.2 基于變分貝葉斯的噪聲自適應估計

為了能夠對量測噪聲的統計特性進行自適應估計,需要對量測噪聲選取合適的先驗分布。文獻[10]指出,可以選取inverse Gamma分布作為Rk的先驗分布,即

(23)

式(23)中:InvG(σ2|α,β)表示樣本方差σ的形狀為α,尺度為β的inverse Gamma分布概率密度函數。

根據變分貝葉斯方法原理計算Rk的后驗近似,得到式(23)中超參數αk,i、βk,i的迭代公式為

(24)

式(24)中:i=1,2,…,2n(A)m,n表示矩陣A的第m行第n列的值;ρ為遺忘因子,滿足ρ∈(0,1];diag(·)表示由m維序列(·)生成m維對角陣的變換。

為使參數的后驗近似更加準確,通常在一個濾波周期內會根據式(22)進行多次變分貝葉斯迭代,再進入下一濾波時刻。

3.3 基于變分貝葉斯的容積H∞濾波(VBCH)

根據前三小節的敘述,基于變分貝葉斯的容積H∞濾波算法如下。

算法 基于變分貝葉斯的容積H∞濾波算法Input:X∧k-1,Pk-1,R∧k-1,αk-1,βk-1,γk-1時間更新:for i=1,2,…,2nξi=2n2[1]iXi,k|k-1=X∧k-1+Pk-1ξiωi=12nX*i,k|k-1=f(Xi,k|k-1)X∧k|k-1=∑2ni=1ωiX*i,k|k-1Pk|k-1=∑2ni=1ωi(X*i,k|k-1-X∧k|k-1)× (X*i,k|k-1-X∧k|k-1)T +Qk-1end量測更新:for j=1,2,…,NK(j)k=Pk|k-1HTk(HkPk|k-1HTk+R(j-1)k)-1X∧(j)k=X∧k|k-1+K(j)k(Zk-HkX∧k|k-1)Ξ(j)k=(I-K(j)kHk)Pk|k-1LTk[-γ(j-1)k2I+LkPk|k-1LTk]-1LkP(j)k=(I-Ξ(j)k)(I-K(j)kHk)Pk|k-1α(j)k=ραk-1+0.5β(j)k=ρβk-1+0.5(Zk-HkX∧(j)k)2+0.5(HkP(j)kHTk)R(j)k=diagβk,1αk,1,…,βk,iαk,i,…,βk,2nαk,2n (i=1,2,…,2n)I(j)k=Zk-HkX∧(j)kγ(j)k=1+CI(j)kTI(j)k maxeig[LTkLk(P(j)k-1+HTkHk)-1]end

4 仿真結果與分析

為考察算法性能,本文進行如下仿真實驗:強干擾下,使用基于變分貝葉斯的容積Kalman濾波算法(variational Bayesian cubature Kalman filter, VBCKF)和本文所述算法對無人船載低、中、高精度IMU的大失準初始對準比較。

考慮到受工藝等因素限制,MIMU的輸出具有較大的噪聲。因此,為了降低噪聲對測量、對準和后續導航解算精度的影響,需要對MIMU輸出數據進行預處理。由于采取濾波可能會對有用信號產生影響,因此需要在保證增量不變的前提下對MIMU輸出數據進行重采樣,將采樣頻率增大到原始頻率的10倍。同時,由于數據預處理會造成時延,在處理之后需要對其進行時延補償。

根據式(1)對MIMU輸出的建?？芍?MIMU噪聲具有弱非線性和弱非平穩性,同時考慮到海洋風浪的搖擺周期均在1 s以上,可以認為MIMU輸出信號中的低頻部分為運動信息,而高頻部分是需要去除的噪聲。小波閾值去噪技術利用小波變換,使信號中的高頻和低頻部分具有不同的“時間-尺度”特征,并通過調整各層的閾值達到噪聲補償的效果。本文中選用小波閾值去噪技術,對MIMU輸出噪聲進行抑制,從而提高MIMU的使用精度。綜上所述,數據預處理流程如圖1所示。

圖1 數據預處理算法流程圖Fig.1 Flowchart of data preprocessing

無人船受風浪影響而使橫搖角R、縱搖角P、艏搖角H產生周期性的變化,搖擺基座的相關參數如表1[11]所示。

表1 惡劣海況搖擺基座參數[11]Table 1 Parameter of swaying-base under severe sea conditions[11]

考慮惡劣海況下海浪對于無人船的影響多呈現瞬間沖擊,導致量測輸出中會存在部分野值,量測噪聲具有“厚尾”特性。被野值污染的量測噪聲可表示為伯努利分布,即

(25)

其他參數設置如下:對準時的初始大失準角φ為[10° 10° 30°];系統常數C取1.5;遺忘因子ρ取1×10-3,變分貝葉斯迭代次數N取20;仿真時間為300 s。

無人船載低、中、高精度IMU的有關參數如表2所示。

表2 低、中、高精度IMU噪聲系數Table 2 Noise coefficients of low / medium / high accuracy IMU

圖2～圖4分別為使用兩種算法對高精度、中精度和低精度IMU進行初始對準解算東向、北向和天向三個失準角對準曲線。

圖2 VBCKF/VBCH算法對高精度IMU對準失準角曲線Fig.2 Curves of misalignment angles of VBCKF/VBCH algorithm for initial alignment of high accuracy IMU

圖3 VBCKF/VBCH算法對中精度IMU對準失準角曲線Fig.3 Curves of misalignment angles of VBCKF/VBCH algorithm for initial alignment of medium accuracy IMU

圖4 VBCKF/VBCH算法對低精度IMU對準失準角曲線Fig.4 Curves of misalignment angles of VBCKF/VBCH algorithm for initial alignment of low accuracy IMU

取最后100 s數據計算不同精度IMU使用兩種算法進行初始對準解算東向、北向和天向三個失準角的均值和標準差,如表3所示。

表3 VBCKF/VBCH算法對低、中、高精度IMU初始對準均值和標準差Table 3 Mean and standard deviation of misalignment angles of VBCKF/VBCH algorithm for initial alignment of low/medium/high IMU

圖2～圖4和表3表明,當選用高精度IMU時,不論VBCKF算法還是本文所述的VBCH算法,天向對準精度都能夠達到0.02°,東向、北向對準精度都能夠達到5.0×10-4°,這表明當IMU噪聲較小、各項誤差系數穩定性較高時,變分貝葉斯方法的抗干擾特性得到了充分的發揮,兩種算法對外界強干擾均具有魯棒性。

當選用中等精度MIMU時,VBCKF算法的天向對準精度和收斂時間均顯著下降,而本文所述的VBCH算法天向對準精度能達到0.9°,東向、北向對準精度能達到0.01°;當選用低精度MIMU時,VBCKF算法已經發散,而本文所述的VBCH算法天向對準精度仍能達到1.5°,這表明,隨著MIMU噪聲的增大以及各項誤差系數的不穩定性增強,系統噪聲和狀態方程逐漸變得不確定,基于卡爾曼濾波的VBCKF算法逐漸失效,而本文提出基于H∞控制理論VBCH算法則能夠繼續保持一定的對準精度和算法魯棒性。

5 結論

無人船在惡劣海況下對其船載低成本MIMU進行初始對準,其精度不僅會受到海浪搖擺、浪涌等未知復雜干擾的影響,還會受傳感器噪聲的影響。針對這一問題,本文提出基于變分貝葉斯的H∞濾波算法(VBCH),引入魯棒因子γ增強對系統噪聲或狀態方程不確定情況的魯棒性,并使用變分貝葉斯方法對魯棒因子γ和量測噪聲統計特性進行實時估計和修正,從而實現在系統噪聲或狀態方程不確定且系統噪聲和量測噪聲同時具有“時變”“厚尾”特性情況下的自適應魯棒濾波。

仿真實驗表明,在強干擾下,本文所述VBCH算法對無人船載高精度IMU的對準能夠達到與VBCKF及其同類自適應卡爾曼濾波同等程度的精度;對無人船載中等精度MIMU的天向對準精度達到0.9°,東向、北向對準精度能達到0.01°;對無人船載低精度MIMU的天向對準精度達到1.5°,東向、北向對準精度能達到0.1°,滿足了惡劣海況下無人船載低成本MIMU大失準角初始對準的要求,對同類型問題具有一定借鑒意義。