水下復雜環境下基于SINS/USBL/DVL 多源信息融合的組合導航算法

王 健，魯金瑞，鄭 棟，李 璇，張 濤

(1. 北京控制與電子技術研究所，北京 100038；2.火箭軍裝備部駐北京地區第七軍事代表室，北京 100039；3. 東南大學儀器科學與工程學院，南京 210096)

0 引言

目前已有的水下定位技術包括捷聯慣性導航系統(Strapdown Inertial Navigation System，SINS)、聲學導航、地球物理場導航及其水下組合導航等。地球物理場導航利用地球的物理場信息進行定位，主要包括地形匹配、地磁匹配和重力匹配等，但需要事先獲得活動區域的海圖作為匹配的數據庫，測繪工作量較大，測試設備昂貴且隨維數增加匹配計算的復雜度增大。聲學導航包括水聲定位系統和多普勒測速儀(Doppler Velocity Log，DVL)兩類。DVL是基于多普勒效應的測速設備，能測得載體相對于海底和水流的速度且誤差不隨時間累積。由于聲波在水下傳輸距離遠、衰減損失小，因而水聲定位技術得到了充分發展，超短基線(Ultra Short Baseline，USBL)成為水聲定位設備發展的熱點。目前，常用的水下組合導航方式有SINS/DVL組合和SINS/USBL組合導航。但是，SINS/DVL組合導航雖然能夠獲得高精度的速度信息，但位置誤差隨時間是發散的；SINS/USBL組合導航雖然能夠抑制位置誤差的發散，但受換能器作用距離的限制，常用于區域內的高精度定位，無法滿足水下航行器長航時、廣區域內導航與定位的需求。其中，北京自動化控制設備研究所張亞文提出了一種基于集中濾波的SINS/DVL/USBL水下組合導航方法，建立了桿臂在線估計的SINS/DVL速度觀測模型和SINS/USBL相對量測信息的觀測模型；哈爾濱工程大學徐博和趙琳等推導了SINS/USBL組合的子濾波器，結合SINS/DVL子濾波器，提出了SINS/USBL/DVL組合的分布式聯邦濾波，具有較高的容錯性和穩定性。但是，目前公開的研究大多數還處于仿真和半物理驗證階段，還需要經過實際長江或海試試驗來充分驗證，在試驗中發現問題并解決問題的工作還未全面展開。同時，海洋空間物理特性復雜導致水聲傳播信道表現為時變、空變和頻變等不確定，海洋環境噪聲，特別是淺海環境噪聲具有噪聲源復雜、統計分布多樣、聲源多變等特征。因此，水下復雜聲場環境所導致的聲學量測信息的不確定性也會影響導航的性能。

因此，為了實現水下航行器在復雜水下聲場環境下高精度、長航時的導航與定位，本文對基于SINS/USBL/DVL多源信息融合的組合導航算法展開研究，旨在提高水下組合導航的精度、魯棒性和可靠性。

1 SINS/USBL/DVL多源融合的濾波模型

1.1 SINS/USBL相對量測信息組合的濾波模型

區別于傳統位置匹配的SINS/USBL組合定位技術，本節提出了基于相對量測信息的SINS/USBL組合導航策略。SINS/USBL組合導航系統的狀態變量，在SINS的15維狀態量的基礎上加入了USBL狀態誤差量，相應SINS狀態變量選擇為

=[δδδδδ

(1)

SINS的狀態方程為

(2)

將SINS/USBL一體化安裝誤差角δ=[δδδ]和測角誤差δ、δ、USBL測距精度誤差δ設置為狀態變量，則USBL的狀態變量選擇為

=[δδδδδδ]

(3)

定義δ=[δδδ]，由于安裝誤差角、測角誤差和 USBL 測距精度誤差等均為小量，可以視為常數，USBL的狀態方程為

(4)

其中，=，則SINS/USBL一體化組合導航系統誤差狀態變量為

=[]

(5)

SINS/USBL組合系統的狀態誤差方程為

(6)

定義USBL直接輸出的量測信息為[

(7)

對式(7)求偏微分可得

(8)

其中

(9)

(10)

(11)

(12)

(13)

(14)

(15)

(16)

忽略式(16)的高階誤差小量，則

(17)

其中，根據反對稱矩陣具有交換性，則

(18)

根據式(16)，則

(19)

(20)

(21)

=

(22)

將式(18)～式(22)代入式(17)，進一步整理為

(23)

將式(23)代入式(8)，解算出應答器相對SINS投影于-系的方位角和斜距為

(24)

(25)

USBL觀測到的方位角和斜距可表示為

(26)

將式(24)與式(26)相減得

(27)

(28)

=0+Δ

(29)

其中，Δ為觀測噪聲，式(28)和式(29)相減得到深度誤差觀測量

(30)

則觀測方程表示為

=δ+δδ-Δ

(31)

SINS/USBL一體化組合系統濾波的量測方程可表示為

(32)

觀測矩陣為

(33)

觀測噪聲陣為

(34)

1.2 SINS/DVL基于載體坐標系下速度組合的濾波模型

根據文獻[5]的推導，本節給出了SINS/DVL基于載體坐標系下速度組合的濾波模型。SINS/DVL組合導航系統誤差狀態變量為

=[]

(35)

其中，=[δ]，δ是DVL的刻度因子誤差，量測方程可表示為

=+

(36)

觀測矩陣為

(37)

1.3 SINS/USBL/DVL多源信息融合的集中濾波模型

綜上所述，進一步結合集中濾波的特點，SINS/USBL/DVL組合導航集中濾波的狀態方程為

(38)

其中，狀態變量為

(39)

系統矩陣可以表示為

(40)

其中，和為濾波器相關噪聲矩陣項，SINS/USBL/DVL組合導航集中濾波的觀測方程為

=+

(41)

2 量測不確定下SINS/USBL/DVL多源融合的魯棒自適應濾波算法

在實際復雜海洋環境中，水聲量測信息呈現出未知或時變的統計特性。同時，由于受外部環境的干擾和多路徑效應等因素的影響，基于寬帶擴頻的相干信號會發生干擾，估計的相關峰會產生錯亂，導致水聲定位系統測量信息出現沒有規律性的野值。針對復雜水下環境中未知或時變的量測噪聲特性，采用變分貝葉斯的方法對未知參數進行自適應估計。針對復雜水下環境中量測野值引起的量測噪聲的厚尾特性，采用基于學生t分布的新型魯棒處理的方法，下文將給出推導過程。

2.1 基于學生t分布的分層狀態空間模型

對于一個厚尾量測噪聲的線性狀態空間模型，學生t后驗概率密度函數(Probability Density Func-tion, PDF)可以視為無窮多個高斯PDF的混合，則似然PDF(|)可以表示為

(|)=St(;,,)

(42)

似然PDF(|)可以表示分層高斯的形式為

(43)

在貝葉斯推斷中，尺度矩陣及其輔助的隨機變量選擇為共軛先驗分布

(44)

其中，為自由度參數，可以更新為伽馬分布

()=G(;-1,-1)

(45)

(-1|1∶-1)更新為逆Wishart PDF，即

(46)

由于量測噪聲的協方差特性是緩慢變化的，可通過來傳遞上一時刻的后驗PDF，則先驗參數和可分別表示為

(47)

(48)

2.2 基于學生t分布的新型魯棒自適應濾波器

本節的目的是計算聯合后驗PDF(,,,|1∶)。然而，基于構建的分層高斯狀態空間模型，無法獲取后驗PDF的一個封閉形式的解析解。因此，為了得到其近似解，本文利用變分貝葉斯(Variational Bayes，VB)方法來聯合推斷狀態向量、尺度矩陣、輔助隨機變量和自由度參數，即?{,,,}。將(|1∶)表示為一個具有自由分解形式的近似后驗PDF形式為

(|1∶)≈()()()()

(49)

其中，(·)表示近似的后驗PDF。上述表達式的最優解可以通過最小化真實后驗(,,|1∶)和近似后驗()()()PDF的KL散度(Kullback-Leibler Divergence，KLD)來實現，則最優解滿足以下的方程

(50)

基于構建的分層高斯狀態空間模型的條件獨立性，狀態向量、量測噪聲的協方差陣、輔助隨機變量和自由度參數分別更新為高斯分布、逆Wishart分布、伽馬分布及其伽馬分布，可以得到

G(;-1,-1)

(51)

根據方程(50)，log(,1∶)可以計算為

log(,1∶)=

(52)

令分別等于、、、，將方程(50)代入方程(52)，其具體的推導過程類似于文獻[12]和文獻[13], 基于學生t分布的魯棒自適應濾波算法，其執行的偽代碼可以總結如表1所示。

表1 基于學生t分布的魯棒自適應濾波算法

3 試驗驗證

為驗證本文提出的基于SINS/USBL/DVL多源組合導航系統在實際水下環境中的導航性能，在廣西省北海市周邊的水域做了海試試驗。試驗開始前，將應答器預先布放在水底，其位置可由長基線的方法精確標定。試驗系統設備的連接和原理圖如圖1所示。整個試驗系統包含GPS、IMU、USBL、DVL這4個傳感器及其他外圍設備。其中，RTK-GPS和SINS組合的結果可作為位置信息的參考基準。

圖1 試驗系統設備的連接圖Fig.1 Connection diagram of the test system equipments

表2 IMU參數

表3 試驗設備的主要參數表

在實際海試試驗中，由于水下復雜環境的干擾和多路徑效應等因素的影響，USBL和DVL實測的測量信息在很多時候都會有野值的干擾，有些是明顯的野值，有些不明顯。圖2和圖4所示為這次海試試驗在一段時間內實測的水聲數據。圖3和圖5所示為USBL和DVL有無野值時的概率密度對比圖。

由圖2和圖4可以看出，USBL和DVL水聲數據中有大量明顯的野值存在，而且也有很多相對小的野值。一方面，假如通過簡單識別并剔除的話會嚴重破壞數據的完整性，且為事后處理。USBL水聲的數據更新頻率本身就不是很高，大約2s才更新一個數據，簡單剔除的話，量測更新的頻率會更低；組合定位的曲線也比較曲折，不但會降低定位的精度，而且會導致對應GPS的時序也得做相應調整。另一方面，野值的存在會導致外部觀測信息概率分布近似為厚尾分布而不服從高斯分布，如圖3和圖5所示。此時，傳統的卡爾曼濾波已不是最優估計。

圖2 USBL實測數據Fig.2 Data measured by USBL in real time

圖3 概率密度對比Fig.3 Probability density comparison

圖4 DVL實測數據Fig.4 Data measured by DVL in real time

圖5 概率密度對比Fig.5 Probability density comparison

因此，本文提出了基于學生t分布的SINS/USBL/DVL多源組合導航算法(Rst-SINS/USBL/DVL)，將其與基于學生t分布的SINS/USBL組合導航算法(Rst-SINS/USBL)、基于學生t分布的SINS/DVL組合導航算法(Rst-SINS/DVL)、SINS/USBL/DVL、SINS/USBL和SINS/DVL組合導航算法做比較，不同算法的定位誤差曲線如圖6和圖7所示。表4給出了不同算法下定位誤差統計的結果(RMSE)。

圖6 東向定位誤差Fig.6 East orientation errors

圖7 北向定位誤差Fig.7 North orientation error

表4 定位統計的結果

圖8 濾波器參數分析Fig.8 Filter parameter analysis

結合圖6、圖7和表4統計結果，將提出的Rst-SINS/USBL/DVL與Rst-SINS/USBL、Rst-SINS/DVL做比較可以看出，Rst-SINS/USBL/DVL具有更好的定位精度。這是因為雖然USBL能夠對SINS累積的位置誤差起調制和校正作用，但是仍受限于USBL定位精度；同時，Rst-SINS/USBL組合的速度誤差仍然很大，且Rst-SINS/USBL組合為區域內導航。因此，可以通過融合SINS/USBL/DVL的多源信息，利用DVL進一步抑制速度誤差進而提高組合定位精度，實現長航時和廣區域內導航。Rst-SINS/DVL組合導航雖然可以抑制速度誤差，但是其位置誤差隨時間是發散的，且與初始的位置誤差有關。因此，通過海試試驗證明了本文提出的基于學生t分布的SINS/USBL/DVL多源組合導航算法的有效性和優越性，提高了組合定位的精度、魯棒性和可靠性。

4 結論

針對復雜水下聲場環境下水聲量測信息的不確定性所引起的厚尾特性，本文推導了基于學生t分布的新型魯棒自適應濾波器，通過提出的SINS/USBL/DVL多源信息融合的組合導航策略，提高了水下航行器導航與定位的精度和可靠性。

1)針對復雜水下環境下量測噪聲未知和量測野值等不確定量測對定位精度的影響，將狀態向量、量測噪聲的協方差陣、輔助隨機變量和自由度參數分別更新為高斯分布、逆Wishart分布、伽馬分布及其伽馬分布，基于學生t分布推導了新型的魯棒自適應濾波算法，實現了對未知量測噪聲方差的自適應估計和對量測野值的魯棒處理。

2)推導了SINS/USBL/DVL多源信息融合的濾波模型，基于設計的集中卡爾曼濾波器，提高了組合定位的精度，能夠滿足水下航行器在復雜水下聲場環境下高精度、長航時導航與定位的需求。