黃 臻吳 峻

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

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

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

2.1 變分貝葉斯原理

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Γ(·)是Gamma函數。

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

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

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

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

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

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

2.3 基于變分貝葉斯的參數后驗近似

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

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

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

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

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

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

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

為了簡化計算式(18)，基于Stirling近似，log?？梢越茷閇16]:

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

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

式中:根據Gamma分布的性質，有:

ψ(·)為digamma函數。

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

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

其數學期望為:

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

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

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

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

步驟1 UKF時間更新

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

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

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

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

①UKF量測更新

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

②變分參數更新

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

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

圖2 St-VB UKF算法流程圖

4 仿真分析

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

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

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

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

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

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

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

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

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

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

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

可以看到，在強干擾環(huán)境下，傳統(tǒng)UKF算法在受到干擾噪聲污染時失準角產生嚴重震蕩，并導致發(fā)散。而Sage-Husa自適應UKF算法與St-VB UKF算法都可以自適應的估計和修正量測噪聲的統(tǒng)計特性，可以抵抗干擾的影響。但是Sage-Husa算法在初期受到瞬時強干擾時出現較大誤差，導致收斂速度和精度都受到影響，方位在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的數據計算三個失準角的均值和標準差并取平均值，計算結果見表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分布來抑制野值的影響，并根據變分貝葉斯方法對量測噪聲統(tǒng)計特性和學生t分布的自由度參數進行實時的估計和修正。通過仿真實驗表明，本文提出的算法在短時正弦波強干擾和瞬時隨機干擾環(huán)境下對準精度略優(yōu)于Sage-Husa自適應UKF算法，水平對準精度和方位對準精度都可以控制在5′以內，且收斂速度優(yōu)于Sage-Husa自適應UKF算法；當存在干擾且量測噪聲同時具有厚尾特性的情況下，Sage-Husa算法由于缺乏魯棒性對準精度大幅下降，而本文提出的算法可以保證基本一致的對準精度和速度，魯棒性更好。