基于學生t分布的變分貝葉斯UKF算法在無人船對準中的應用

黃 臻吳 峻

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

初始對準是捷聯(lián)慣導系統(tǒng)導航精度的關鍵之一。對于低精度慣導系統(tǒng)或系泊狀態(tài)下無人船的粗對準誤差較大，會出現(xiàn)大失準角的情況，SINS誤差模型具有較強的非線性[1]。并且無人船無人的特性使其相比傳統(tǒng)艦船更適合在環(huán)境更加復雜和惡劣的場景下完成作業(yè)，因此無人船在惡劣海況下的精對準往往是大失準角的對準。在惡劣海況下無人船會產生大幅搖擺，同時還會受到浪涌、陣風等復雜未知干擾，噪聲往往具有時變特性，且由于傳感器本身的誤差和干擾噪聲等的影響，量測輸出往往含有部分野值，量測噪聲不滿足高斯分布而具有“厚尾”特性。針對這一問題，學者們在傳統(tǒng)算法的基礎上研究提出了很多改進算法。受到廣泛應用的一類算法是以Sage-Husa為代表的對噪聲統(tǒng)計特性實時自適應估計的算法，可以有效解決噪聲特性不確定下的對準問題[2-3]，但是這類算法缺乏魯棒性，野值會導致噪聲的估計存在偏差；強跟蹤算法是另一類具有代表性的自適應算法，文獻[4]針對對準時出現(xiàn)模型失配和未知干擾時魯棒性差的問題，引入多漸消因子，通過選取漸消因子強迫殘差序列正交，從而實時調整增益矩陣，抑制濾波發(fā)散，提高算法的魯棒性；在此基礎上，郭士犖等[5]基于假設檢驗作為漸消因子的引入條件，進一步提升強跟蹤算法的自適應性。但是強跟蹤算法依賴于模型的準確性，模型的偏差會影響漸消因子的求取。因此，近年來學者們逐漸對魯棒自適應算法展開了研究，文獻[6]提出了一種基于滑動窗口的魯棒自適應算法，通過投影統(tǒng)計算法先對滑動窗口內的異常新息進行降權處理，再進行自適應的估計；文獻[7]進一步將遺傳算法與投影統(tǒng)計算法相結合，實現(xiàn)在較強外部干擾時無人船較高精度的大失準角對準。另外，也有學者將抗差估計理論與自適應算法相結合[8]，通過協(xié)方差匹配等檢測手段判別野值，對異常的新息進行降權處理，從而提高自適應算法的魯棒性。

變分貝葉斯是近幾年來新出現(xiàn)的近似計算貝葉斯方法中參數(shù)后驗的方法，不同于以馬爾可夫鏈·蒙特卡洛算法為代表的隨機抽樣算法需要通過大量重復抽樣來逼近參數(shù)的真實后驗，變分貝葉斯由于在具有較高估計精度的同時還有較快的計算速度，近些年來受到了廣泛的關注。文獻[9]在海上SINS對準中使用變分貝葉斯方法對量測噪聲進行自適應估計；為了提高變分貝葉斯方法的魯棒性，文獻[10]將變分貝葉斯方法與強跟蹤算法相結合，文獻[11]將抗差估計與變分貝葉斯結合，抑制野值的影響。但是這些方法都沒有將實際噪聲的厚尾特性進行建模，無法真實反映量測噪聲的統(tǒng)計特性。文獻[12]提出在變分貝葉斯方法中使用學生t分布對厚尾噪聲進行建模，但該方法假設量測噪聲固定不變，具有一定的局限性。受此啟發(fā)，針對惡劣海況下無人船對準時量測噪聲時變且存在野值而表現(xiàn)出“厚尾”特性的問題，提出了一種自適應魯棒的基于學生t分布的變分貝葉斯UKF(Student’s t Distribution Based Variational Bayesian UKF，St-VB UKF)算法，通過將量測噪聲建模成學生t分布來描述存在野值時量測噪聲的厚尾特性，并使用變分貝葉斯方法對其統(tǒng)計特性進行實時的估計。

1 SINS大失準角對準模型

無人船采用GNSS/SINS組合對準的方式。記地心慣性坐標系為i系，導航坐標系為“東北天”記作n系，實際的計算平臺坐標系為n′系，載體坐標系為“右前上”記作b系。假設n系經三次轉動可得n′系，三次轉動角為兩個坐標系間的歐拉平臺誤差角，記作φ＝[φxφyφz]，考慮陀螺主要誤差為常值漂移誤差εb和零均值高斯白噪聲，加速度計主要誤差為常值偏置誤差?b和零均值高斯白噪聲，狀態(tài)方程可由以下六個矢量方程表示:

式中:H＝[I4×404×8]，f(X)和g(X)可由式(1)得到。

2 變分貝葉斯下的參數(shù)估計

2.1 變分貝葉斯原理

變分貝葉斯(Variational Bayesian，VB)方法的主要思想是通過使用形式更加簡單的變分分布，將貝葉斯方法中難以直接求解的參數(shù)后驗分布轉換為變分分布中各個參數(shù)的優(yōu)化問題。

在變分貝葉斯方法中，首先需要給待估的參數(shù)在共軛指數(shù)分布族中選取合適的先驗分布。共軛性可以保證參數(shù)的先驗和后驗分布在形式上保持一致，從而變分貝葉斯方法在迭代更新時只需要估計各個分布中的參數(shù)而不需要直接求解后驗分布就可以近似參數(shù)的后驗信息。同時，在參數(shù)估計中往往需要求解多個參數(shù)的聯(lián)合后驗分布，為了簡化計算，變分貝葉斯方法根據(jù)平均場理論(Mean Field Theory)的思想，假設所有待求取后驗分布的參數(shù)都相互獨立，則參數(shù)的聯(lián)合后驗分布可以近似成如下形式的變分分布[13]:

式中:Θ表示所有待求取后驗分布的參數(shù)集合，Q i(θi)是每個參數(shù)在共軛指數(shù)分布族中選取的分布。

變分貝葉斯方法通過最小化兩個分布間的KL散度(Kullback-Leibler Divergence)使構造的變分分布逼近真實的聯(lián)合后驗分布[14]:

式中:z1:k表示1到k時刻的量測信息，q*(Θ)表示真實后驗分布，L表示共軛指數(shù)分布族。

KL散度是一種常用的評價概率分布之間差異的度量。利用變分法來計算式(4)，則待估參數(shù)集中任意參數(shù)的后驗估計可以表示為如下形式，其詳細推導過程見文獻[14]:

式中:φ表示參數(shù)集合Θ中的任意元素，Θ(-φ)表示除去φ的其他元素，constφ表示和φ無關的一個常量。

式(5)是變分貝葉斯方法近似計算參數(shù)后驗的核心，在給待估參數(shù)集選取合適的先驗分布后，通過式(5)就可以推導變分分布中各個參數(shù)的更新公式，然后利用參數(shù)的后驗近似作為下一濾波時刻的先驗信息，循環(huán)迭代地進行更新，從而實現(xiàn)自適應估計參數(shù)統(tǒng)計特性的目的。

2.2 厚尾量測噪聲建模及參數(shù)先驗分布的選取

考慮無人船在惡劣海況下對準會受到不確定的外界干擾，量測噪聲具有時變特性，并且在量測中會存在部分野值，實際噪聲與高斯分布相比具有厚尾特性。因此將量測噪聲建模成學生t分布:

式中:St(v k；0，R k，ν)表示均值為0，尺度矩陣為R k，自由度為ν的學生t分布的概率密度函數(shù)。圖1是高斯分布與學生t分布概率密度函數(shù)的對比，學生t分布在均值附近的概率更小，概率分布表現(xiàn)出較厚的尾部，自由度用來描述噪聲的厚尾程度，自由度越小分布的厚尾程度越重。當ν→∞時，分布等價于高斯分布N(0，R k)。

圖1 高斯分布與學生t分布概率密度函數(shù)對比

由于學生t分布可以看作是高斯分布的無限混合，可以將學生t分布的概率密度寫成如下形式[12]:

式中:λk是引入的輔助隨機變量，學生t分布建模下量測噪聲協(xié)方差陣實際為R k/λk；G(·；α，β)表示Gamma分布的概率密度函數(shù)，兩個參數(shù)α，β分別表示形狀參數(shù)和尺度參數(shù)，其概率密度函數(shù)形式為:

Γ(·)是Gamma函數(shù)。

為了自適應估計量測噪聲的統(tǒng)計特性，需要給不確定或時變的參數(shù)選取先驗分布，然后根據(jù)變分貝葉斯方法來計算參數(shù)的后驗信息。因此，根據(jù)量測噪聲的建模，從共軛指數(shù)分布族中選取Gamma分布作為輔助隨機變量λk、自由度參數(shù)ν的先驗分布，選取inverse Gamma分布作為尺度矩陣R k的先驗分布:

式中:InvG表示inverse Gamma分布，m表示量測向量的維數(shù)，表示量測噪聲協(xié)方差陣對應對角元素的值。

同時，根據(jù)模型定義和量測噪聲先驗分布的選取，利用卡爾曼濾波公式可以推導出狀態(tài)向量服從正態(tài)分布量測向量服從學生t分布St(z k；Hx k，R k，ν)。則根據(jù)先驗分布的選取，狀態(tài)變量x k、尺度矩陣R k、輔助隨機變量λk、自由度參數(shù)ν和量測向量z k的聯(lián)合概率密度函數(shù)為:

式中:待估參數(shù)集Θ＝{x k，R k，λk，ν}。

理論上貝葉斯方法的目標是根據(jù)參數(shù)先驗假設和量測信息計算參數(shù)的聯(lián)合后驗分布，而變分貝葉斯方法將其簡化成了對變分分布中參數(shù)αk，βk，a k，b k，ck，i，d k，i的迭代更新，參數(shù)的聯(lián)合后驗分布的計算被近似成了如下變分分布的形式:

在下一節(jié)將具體推導基于變分貝葉斯方法的參數(shù)更新以及后驗近似。

2.3 基于變分貝葉斯的參數(shù)后驗近似

首先，針對對準過程中噪聲可能時變的情況，在計算參數(shù)后驗近似之前，在先驗分布的參數(shù)中引入遺忘因子ρ來加強當前時刻量測信息的權重:

遺忘因子ρ可取(0，1]，根據(jù)文獻[15]選取1-e-4。

各個參數(shù)的更新公式都是通過最小化變分分布和真實后驗之間的KL散度推導得到的。將式(10)代入式(5)，取φ＝λk，可以得到:

式中:上標(i＋1)表示第i＋1輪循環(huán)變分貝葉斯方法的估計，E(i)[·]表示對應參數(shù)在第i輪變分貝葉斯迭代的數(shù)學期望，tr(·)表示矩陣的跡，由下式給出:

由于λk先驗的共軛性，變分貝葉斯估計下的后驗也服從Gamma分布，參數(shù)λk的后驗可以表示為:

將(15)代入式(13)，可以計算參數(shù)αk，βk的更新公式為:

同理，令φ＝ν代入式(5)，得到:

為了簡化計算式(18)，基于Stirling近似，logΓ可以近似為[16]:

則式(18)可以簡化成如下形式:

因此，自由度ν的后驗可由以下公式進行更新:

式中:根據(jù)Gamma分布的性質，有:

ψ(·)為digamma函數(shù)。

同理，令φ＝R k代入式(5)得到:

因此尺度矩陣R k的后驗近似為:

其數(shù)學期望為:

由于量測噪聲建模成了學生t分布，根據(jù)式(7)，量測噪聲協(xié)方差陣為尺度矩陣R k和輔助隨機變量λk之商:

通過上述的公式，變分貝葉斯方法可以完成在一個濾波周期各個參數(shù)的更新，得到與先驗保持一致的近似后驗分布，并應用到下一濾波周期的參數(shù)估計和更新中，從而不斷迭代，達到參數(shù)實時自適應估計和修正的目的。

3 基于學生t分布的變分貝葉斯UKF算法

根據(jù)第一節(jié)大失準角非線性對準模型以及第二節(jié)變分貝葉斯更新的推導，將變分貝葉斯方法與UKF非線性濾波算法相結合，設計了一種自適應魯棒的基于學生t分布的變分貝葉斯UKF算法(簡稱St-VB UKF算法)，通過將量測噪聲建模成學生t分布，來描述對準中存在野值時量測噪聲的厚尾特性，提升算法的魯棒性，并結合變分貝葉斯方法來實時自適應估計量測噪聲的統(tǒng)計特性。本文提出的算法步驟概況如下:

步驟1 UKF時間更新

選取一組Sigma點來近似系統(tǒng)的狀態(tài)變量分布，通過UT變化計算一步預測狀態(tài)向量和協(xié)方差

步驟2 基于變分貝葉斯的量測更新

基于變分貝葉斯方法的量測更新是通過參數(shù)的先驗信息，利用量測信息來迭代更新先驗分布中的各個參數(shù)，從而近似計算參數(shù)的后驗，并作為下一濾波周期的先驗進行循環(huán)迭代的更新。

首先根據(jù)式(12)在變分分布參數(shù)中引入衰減因子來提高當前時刻的權重，量測更新的迭代過程可以分為UKF量測更新和變分參數(shù)更新兩個部分，為了提高噪聲估計的準確程度，一次量測更新中可以進行N次變分貝葉斯迭代，一般進行2到3次即可。

①UKF量測更新

利用上一輪迭代的變分參數(shù)更新量測噪聲協(xié)方差陣，并以此計算新一輪的卡爾曼濾波量測更新:

②變分參數(shù)更新

在卡爾曼濾波量測更新得到狀態(tài)估計和協(xié)方差統(tǒng)計特性后，利用殘差對變分分布中各個參數(shù)進行更新，更新的公式已經在第二章中進行了詳細的推導。利用式(16)、(17)更新利用式(22)、(23)更新，利用式(29)、(30)更新；并根據(jù)其分布的特性更新輔助隨機變量、自由度ν(i＋1)和尺度矩陣，進行下一輪的變分貝葉斯迭代。

N次迭代完成后，令各個參數(shù)的估計為最后一次迭代得到的結果，然后進入到下一濾波周期。整個算法的流程如圖2所示。

圖2 St-VB UKF算法流程圖

4 仿真分析

模擬無人船在惡劣海況下的航行條件進行對準，在風浪影響下無人船受到大幅的搖擺，其航向角H、俯仰角P、橫滾角R作周期變化，搖擺基座的定義如表1所示。

表1 惡劣海況下對準搖擺基座設置

其他參數(shù)設置如下:對準時的初始大失準角設置為φ＝[10° 10° 30°]，IMU參數(shù)設置為:陀螺常值漂移為0.02°/h，隨機漂移為0.01°/h；加速度計常值偏置為50μg，隨機偏置為10μg。仿真時間1 200 s。

同時，在實際應用中無人船在對準時可能會受到強風、浪涌等其他未知干擾的影響，且量測中會存在部分野值而使噪聲表現(xiàn)出厚尾特性，會對對準精度和速度造成很大的影響。為了驗證本文提出算法的有效性，除了表1條件下的搖擺基座外，模擬無人船惡劣海況下可能的干擾環(huán)境，在量測噪聲中加入干擾噪聲和野值。

仿真實驗分為兩個部分:①強干擾環(huán)境下的對準；②強干擾環(huán)境存在量測野值的對準。提出的算法分別與傳統(tǒng)的UKF算法和Sage-Husa自適應UKF算法進行對比。

①強干擾環(huán)境下的對準

考慮無人船在海上對準時可能遇到強風、浪涌、沖擊等未知的復雜干擾，干擾主要體現(xiàn)為正弦波形式的短時強干擾和瞬時的隨機強干擾，因此在速度觀測量中增加如表2干擾噪聲。

表2 干擾環(huán)境設置

三種算法在強干擾下的失準角曲線如圖3所示。取最后100 s的數(shù)據(jù)計算三個失準角的均值和標準差，結果見表3。

圖3 強干擾下三種算法的失準角曲線

表3 強干擾下三種算法的對準結果

可以看到，在強干擾環(huán)境下，傳統(tǒng)UKF算法在受到干擾噪聲污染時失準角產生嚴重震蕩，并導致發(fā)散。而Sage-Husa自適應UKF算法與St-VB UKF算法都可以自適應的估計和修正量測噪聲的統(tǒng)計特性，可以抵抗干擾的影響。但是Sage-Husa算法在初期受到瞬時強干擾時出現(xiàn)較大誤差，導致收斂速度和精度都受到影響，方位在700 s左右收斂，收斂精度水平在9′和4′左右，方位角在12′左右；St-VB UKF算法具有更快的收斂速度和魯棒性，方位失準角在100 s左右收斂，水平方向對準精度達到1′和4′，方位對準精度在5′左右。

②強干擾環(huán)境存在量測野值的對準

實際無人船在惡劣海況下進行對準，往往由于傳感器本身的誤差和干擾噪聲等的影響，量測輸出中會存在部分野值，量測噪聲具有“厚尾”特性。被野值污染的量測噪聲可表示為如下伯努利分布的形式[17]:

在仿真(1)的干擾環(huán)境以及式(35)描述的被野值污染的量測噪聲條件下進行20次Monte Carlo仿真，三種算法的仿真結果如圖4所示。

圖4 強干擾環(huán)境下且存在野值時三種算法的失準角曲線

取20次仿真最后100 s的數(shù)據(jù)計算三個失準角的均值和標準差并取平均值，計算結果見表4。在強干擾且存在量測野值的情況下，傳統(tǒng)UKF算法嚴重發(fā)散；Sage-Husa自適應UKF算法雖然可以自適應地修正量測噪聲，但由于缺乏魯棒性，量測在受到野值污染時對準精度下降，20次仿真平均的方位對準精度在5°左右。而本文提出的算法在自適應跟蹤噪聲的同時具有魯棒性，可以抑制野值的影響，方位失準角收斂速度略慢于沒有野值的情況，可以在400 s左右達到收斂，并且水平和方位的收斂精度均可達到5′以內，精度與沒有野值的情況基本保持一致，具有較好的魯棒性。

表4 強干擾下且存在野值時三種算法的對準結果

以上兩組仿真結果表明，本文提出的基于學生t分布的變分貝葉斯UKF算法在短時強干擾和瞬時干擾環(huán)境下可以得到較好的對準精度，且對準速度和精度都略優(yōu)于傳統(tǒng)的Sage-Husa自適應算法；同時，當量測存在野值的條件下，提出的算法可以有效地抑制野值的影響，對準精度與無野值的情況基本保持一致，與傳統(tǒng)算法相比具有更好的魯棒性。

5 總結

無人船在惡劣海況下進行對準，不僅會受到大幅的搖擺，還會受到強風、浪涌等未知的復雜干擾，且由于傳感器本身誤差等影響，量測輸出中會存在部分野值，量測噪聲不符合高斯分布而具有“厚尾”特性，此時傳統(tǒng)的UKF算法完全失效；而自適應算法雖然可以通過實時修正量測噪聲的統(tǒng)計特性來抑制干擾噪聲對對準的影響，但自適應算法缺乏魯棒性，在野值的污染下會影響對量測噪聲的估計，對準精度和速度都會受到大幅影響。

針對這一問題提出了一種自適應魯棒的基于學生t分布的變分貝葉斯UKF算法，通過將量測噪聲建模成學生t分布來抑制野值的影響，并根據(jù)變分貝葉斯方法對量測噪聲統(tǒng)計特性和學生t分布的自由度參數(shù)進行實時的估計和修正。通過仿真實驗表明，本文提出的算法在短時正弦波強干擾和瞬時隨機干擾環(huán)境下對準精度略優(yōu)于Sage-Husa自適應UKF算法，水平對準精度和方位對準精度都可以控制在5′以內，且收斂速度優(yōu)于Sage-Husa自適應UKF算法；當存在干擾且量測噪聲同時具有厚尾特性的情況下，Sage-Husa算法由于缺乏魯棒性對準精度大幅下降，而本文提出的算法可以保證基本一致的對準精度和速度，魯棒性更好。