基于改進自適應濾波的SINS/DVL組合導航算法研究

魏延輝，劉 靜，郝晟功

（哈爾濱工程大學 自動化學院，哈爾濱150001）

采用捷聯慣性導航系統SINS（strapdown inertial navigation system）與多普勒計程儀DVL（doppler velocity log）組合能夠實現完全自主導航、短時定位定向精度高、抗干擾能力強，是水下導航的最廣泛的方式[1]。但是，復雜的水下環境會導致DVL 測速失準，甚至會出現信號短期中斷現象，影響SINS/DVL組合導航系統的精度。

針對DVL 出現的測速失準現象，文中提出了一種改進的自適應濾波算法，引入χ2故障檢測，當觀測量存在異常時，通過設置自適應調節因子矩陣對先驗狀態均方誤差矩陣進行自適應調節從而抑制誤差。最后，通過仿真試驗驗證了所提算法能有效抑制DVL 測速失準時產生的誤差， 并通過Kalman濾波、Sage-Husa 自適應濾波和改進的自適應濾波對比仿真試驗，驗證該方法的有效性。

1 SINS/DVL 組合導航數學模型

SINS/DVL 組合導航系統的誤差模型分為SINS誤差模型和DVL 誤差模型2 部分[2]。由于SINS 的垂直通道不穩定且DVL 的垂直速度測量誤差較大，故文中僅考慮水平通道問題。

根據SINS 長時間工作的誤差特性， 選擇狀態變 量為：經度、緯度誤差（δL，δλ），速度誤差（δvx，δvy），平臺失準角（α，β，γ），陀螺漂移（εx，εy，εz）。其誤差微分方程為

式中：R 為地球半徑；Ω 為地球自轉角速度；λ 為當地緯度；Ax，Ay分別為加速度計測量值在當地地理坐標軸上的分量；βg為陀螺漂移反相關時間常數；wg為白噪聲。

令DVL 速度偏移誤差和偏流角誤差用一階馬爾科夫過程來表示，刻度系數誤差為常值[3]，則對DVL 速度偏移誤差δvd， 刻度系數誤差δc 及偏流角誤差δΔ進行建模，有

式中：wd和wΔ分別為二者的激勵白噪聲。因此建立的系統狀態方程為

其中

式中：（Fins（t））7×7為慣性導航系統狀態轉移矩陣；（Fε（t））3×3為陀螺漂移反相關時間矩陣；（Fd（t））3×3為DVL 誤差反相關矩陣。

SINS/DVL 采用速度組合方式[4]。在導航坐標系下， 將SINS 解算出的速度與DVL 測量得到的速度作差，將此差值作為Kalman 濾波器的量測值，因此得到系統的量測方程，即

其中

式中：Vsx，Vsy為慣導的測量速度在平臺坐標系中的投影；Kd為考慮偏流角的航跡向[5]。

將式（4）和式（5）進行離散化，得到SINS/DVL組合導航系統的Kalman 濾波器為

式中：Φk，k-1為系統從k-1 時刻到k 時刻的n×n 維狀態轉移矩陣；Hk為系統k 時刻的m×n 維觀測矩陣；Wk-1為模型噪聲，Vk為觀測噪聲， 二者為相互獨立的零均值高斯白噪聲。其方差分別為Qk和Rk，滿足以下統計特性：

其中

式中：qk，qj分別為Wk和Wj的數學期望；rk為Vk的數學期望；Qk和Rk為系統噪聲和觀測噪聲方差矩陣。

2 改進的Sage-Husa 自適應濾波算法

經典Kalman 濾波算法在系統的結構參數和噪聲統計特性都準確已知的情況下才能獲得狀態的最優估計； 而利用Sage-Husa 自適應濾波對SINS/DVL 組合導航系統進行信息融合，能不斷地利用量測數據估計和校正噪聲統計特性與模型參數，從而保證濾波始終處于正常狀態。

在Sage-Husa 自適應濾波算法中， 每次濾波都要估計系統噪聲特性和觀測噪聲特性，若系統階數較高則會導致計算量大幅增加。然而，在實際系統中，如果濾波不出現異常，則無需對噪聲特性進行調整，因此引入濾波收斂判據：

式中：s 為調節因子，反映判據的嚴格程度。式（7）成立，則說明實際新息的方差大于理論值的s 倍，即濾波出現異常，量測噪聲變化較大，需要對其進行調整。新息方差理論值為

則濾波收斂條件為

由于水下環境復雜，SINS/DVL 組合導航系統的量測噪聲統計特性受實際作業環境影響可能發生改變， 初始的先驗值可能無法反映實際的噪聲情況，針對此情況，在Sage-Husa 自適應濾波的基礎上進行了改進，利用量測信息實時估計量測噪聲的統計特性，從而精確估計系統的狀態變量。

結合濾波異常判斷條件， 用Sage-Husa 自適應濾波估計量測噪聲協方差陣，則改進后的算法為

其中

改進的算法在新息計算后，增加了對濾波異常的判斷：當濾波正常時，無需對濾波器做調整；當濾波異常時，即量測噪聲突變，則利用新息對Rk進行調整。濾波初始時刻的dk為最大值，此時是估計的Rk對當前信息的依賴程度較高， 之后dk逐漸減小，新息對Rk的調整幅度逐漸減小，這樣增加了對量測噪聲變化的跟蹤能力。改進后的算法只在噪聲統計特性發生變化時對Rk進行調整，減輕了原算法中對Rk頻繁調整引起的計算負擔。

3 改進的自適應Kalman 濾波

相對于經典Kalman 濾波算法， 自適應Kalman濾波重新估計觀測噪聲協方差Rk，在一定程度上增加了計算量。在DVL 測量速度發生突變而導致觀測量失準時， 自適應Kalman 濾波的抑制效果并不理想且會累積誤差。文中在自適應Kalman 濾波算法的基礎上進行了改進：在觀測量正常情況下，使用經典Kalman 濾波算法：在觀測量失準情況下，通過對先驗均方誤差矩陣Pk，k-1進行實時地自適應調節從而使得新息協方差與實際新息方陣相符合。

SINS/DVL 組合導航系統的系統模型采用閉環反饋校正，系統模型誤差較小，可以忽略不計。因此，可以利用新息的統計特性來判斷觀測值是否異常。當DVL 正常工作時，測量新息是零均值白噪聲序列；當DVL 突然失準時，測量新息不再是零均值的白噪聲。引入χ2故障檢測，將上述條件轉變為假設檢驗問題，即

式中：H0為原假設， 表示DVL 正常工作情況；H1為備擇假設，表示DVL 失準情況；Pvk為測量信息的協方差矩陣。

令假設檢驗統計量為

其中Tk服從自由度為t 的χ2分布，即

式中λk為非中心化參數。取顯著性水平為α，則DVL 正常工作與否的邊界條件為

即觀測量是否正常的判別準則為

當Tk＞TD時，觀測量存在異常，為了抑制誤差，需在此時采取措施進行調節，使得測量新息與實際相符，即滿足diag（vkvkT）=diag（Pvk）。

由于新息中攜帶觀測量信息，直接對其進行調節會導致濾波失真，在此通過對先驗狀態均方誤差矩陣Pk，k-1進行調節使測量新息協方差與新息方陣相符。采用的方法是，通過增加系數矩陣λm實現對Pk，k-1的調節控制，具體步驟如下：

首先，設定一個調節因子λm，則最優的先驗均方誤差矩陣為

令diag（vkvkT）=diag（Pvk）

若λm＜1，即調節因子元素≯1 時，則調節因子失效，此時對先驗狀態均方誤差矩陣Pk，k-1的調節是無效的，Pk，k-1保持不變。

綜上所述，調節因子λm的最終公式為

文中對自適應濾波算法的改進僅僅是調節了偏離實際的先驗狀態均方誤差矩陣Pk，k-1，在本質上它與經典Kalman 濾波算法是一致的，都采用間接法中的閉環反饋機制。并且，改進的自適應濾波算法根據當前時刻的新息調節Pk，k-1，相對于傳統Kalman濾波只增加了少量計算量，明顯提高了計算精度。

4 仿真試驗與結果分析

4.1 仿真試驗

為驗證該算法的有效性， 在MatLab 環境下進行SINS/DVL 組合導航系統仿真試驗， 仿真試驗參數設定見表1。

表1 仿真參數的設定Tab.1 Simulation parameter setting

4.2 仿真結果分析

為了更真實地模擬DVL 測速突然失準的狀況，在5000～6000 s，12000～13000 s，23000～24000 s，30000～31000 s，將DVL 的噪聲增大為原來的10 倍，然后恢復正常。在此仿真條件下，采用經典卡爾曼濾波、改進的Sage-Husa 自適應濾波和改進的自適應卡爾曼濾波算法分別對SINS/DVL 組合導航系統進行仿真。速度誤差、位置誤差的仿真結果如圖1～圖4所示。

由圖1和圖2可見，增加10 倍噪聲后，速度和位置誤差都存在明顯跳變， 且曲線呈現發散趨勢，即DVL 速度信息失準時，會導致組合導航的精度下降，甚至會造成組合導航失效。

圖1 正常情況下的速度誤差和位置誤差Fig.1 Velocity error and position error in normal conditions

圖2 十倍噪聲情況下的速度誤差和位置誤差Fig.2 Velocity error and position error in case of ten times noise

通過對比不同算法仿真結果， 可以發現，DVL噪聲突然增大為原來的10 倍，量測噪聲突變，采用3 種不同的濾波算法得到的速度和位置誤差均發生突變，組合導航的精度受到影響。

為了直觀地比較濾波性能，表2列出了3 種濾波算法各個導航參數的誤差絕對值的最終穩定值、絕對值最大值。由表可知，經典Kalman 濾波誤差較大，經緯度誤差分別為89.334 m 及95.415 m；改進的Sage-Husa 自適應濾波精度次之， 經緯度誤差分別為72.045 m 及73.125 m；改進的自適應濾波算法精度最高，經緯度誤差分別為33.482 m 及43.385 m。

圖3 十倍噪聲情況下改進Sage-Husa 濾波算法的速度誤差和位置誤差Fig.3 Velocity error and position error of improved Sage-Husa filtering algorithm in case of ten times noise

圖4 十倍噪聲情況下改進自適應濾波算法的速度誤差和位置誤差Fig.4 Velocity error and position error of improved adaptive filtering algorithm in case of ten times noise

表2 三種濾波算法的效果對比Tab.2 Comparison of three filtering algorithms

由圖3，圖4及表2可以看出，在仿真試驗中由于DVL 噪聲突變，導致Kalman 濾波的精度降低，組合導航精度迅速降低，且有發散趨勢。相對于經典Kalman 濾波算法，改進的Sage-Husa 算法精度有所提高，但累積了異常時候產生的誤差。因此，采用改進的Sage-Husa 算法，能夠稍微抑制DVL 測速失準產生的誤差，但是效果有限。

而在此所提出的改進的自適應濾波算法，通過χ2故障檢測可以判斷出DVL 噪聲突變情況，并根據當前時刻的新息調節先驗狀態均方誤差矩陣Pk，k-1，能夠有效抑制誤差并且不積累DVL 噪聲異常時產生的誤差，既保證了精度，又能在一定程度上減少運算量，抑制誤差效果比較明顯，且誤差波動范圍更小，系統表現的更加穩定。

5 結語

文中所提出的改進的自適應濾波算法，解決了SINS/DVL 組合導航系統由于受到復雜海況影響出現的測速突然失準問題。該算法通過引入χ2故障檢測判斷觀測量異常的情況，并設置自適應調節因子矩陣對先驗狀態均方誤差矩陣進行自適應調節從而抑制誤差。由仿真結果可見，改進的自適應濾波算法精度是最高的，改進的Sage-Husa 自適應濾波算法次之，經典Kalman 濾波算法精度較低。改進的自適應濾波算法能夠有效提高系統組合導航定位精度， 具有一定的抗干擾能力和較好的工程適用性，從而算法的實際應用提供很好的解決方案和參考。