水下復(fù)雜環(huán)境下基于SINS/USBL/DVL 多源信息融合的組合導(dǎo)航算法

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

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

0 引言

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

因此，為了實(shí)現(xiàn)水下航行器在復(fù)雜水下聲場(chǎng)環(huán)境下高精度、長(zhǎng)航時(shí)的導(dǎo)航與定位，本文對(duì)基于SINS/USBL/DVL多源信息融合的組合導(dǎo)航算法展開(kāi)研究，旨在提高水下組合導(dǎo)航的精度、魯棒性和可靠性。

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

1.1 SINS/USBL相對(duì)量測(cè)信息組合的濾波模型

區(qū)別于傳統(tǒng)位置匹配的SINS/USBL組合定位技術(shù)，本節(jié)提出了基于相對(duì)量測(cè)信息的SINS/USBL組合導(dǎo)航策略。SINS/USBL組合導(dǎo)航系統(tǒng)的狀態(tài)變量，在SINS的15維狀態(tài)量的基礎(chǔ)上加入了USBL狀態(tài)誤差量，相應(yīng)SINS狀態(tài)變量選擇為

=[δδδδδ

(1)

SINS的狀態(tài)方程為

(2)

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

=[δδδδδδ]

(3)

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

(4)

其中，=，則SINS/USBL一體化組合導(dǎo)航系統(tǒng)誤差狀態(tài)變量為

=[]

(5)

SINS/USBL組合系統(tǒng)的狀態(tài)誤差方程為

(6)

定義USBL直接輸出的量測(cè)信息為[

(7)

對(duì)式(7)求偏微分可得

(8)

其中

(9)

(10)

(11)

(12)

(13)

(14)

(15)

(16)

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

(17)

其中，根據(jù)反對(duì)稱矩陣具有交換性，則

(18)

根據(jù)式(16)，則

(19)

(20)

(21)

=

(22)

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

(23)

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

(24)

(25)

USBL觀測(cè)到的方位角和斜距可表示為

(26)

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

(27)

(28)

=0+Δ

(29)

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

(30)

則觀測(cè)方程表示為

=δ+δδ-Δ

(31)

SINS/USBL一體化組合系統(tǒng)濾波的量測(cè)方程可表示為

(32)

觀測(cè)矩陣為

(33)

觀測(cè)噪聲陣為

(34)

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

根據(jù)文獻(xiàn)[5]的推導(dǎo)，本節(jié)給出了SINS/DVL基于載體坐標(biāo)系下速度組合的濾波模型。SINS/DVL組合導(dǎo)航系統(tǒng)誤差狀態(tài)變量為

=[]

(35)

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

=+

(36)

觀測(cè)矩陣為

(37)

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

綜上所述，進(jìn)一步結(jié)合集中濾波的特點(diǎn)，SINS/USBL/DVL組合導(dǎo)航集中濾波的狀態(tài)方程為

(38)

其中，狀態(tài)變量為

(39)

系統(tǒng)矩陣可以表示為

(40)

其中，和為濾波器相關(guān)噪聲矩陣項(xiàng)，SINS/USBL/DVL組合導(dǎo)航集中濾波的觀測(cè)方程為

=+

(41)

2 量測(cè)不確定下SINS/USBL/DVL多源融合的魯棒自適應(yīng)濾波算法

在實(shí)際復(fù)雜海洋環(huán)境中，水聲量測(cè)信息呈現(xiàn)出未知或時(shí)變的統(tǒng)計(jì)特性。同時(shí)，由于受外部環(huán)境的干擾和多路徑效應(yīng)等因素的影響，基于寬帶擴(kuò)頻的相干信號(hào)會(huì)發(fā)生干擾，估計(jì)的相關(guān)峰會(huì)產(chǎn)生錯(cuò)亂，導(dǎo)致水聲定位系統(tǒng)測(cè)量信息出現(xiàn)沒(méi)有規(guī)律性的野值。針對(duì)復(fù)雜水下環(huán)境中未知或時(shí)變的量測(cè)噪聲特性，采用變分貝葉斯的方法對(duì)未知參數(shù)進(jìn)行自適應(yīng)估計(jì)。針對(duì)復(fù)雜水下環(huán)境中量測(cè)野值引起的量測(cè)噪聲的厚尾特性，采用基于學(xué)生t分布的新型魯棒處理的方法，下文將給出推導(dǎo)過(guò)程。

2.1 基于學(xué)生t分布的分層狀態(tài)空間模型

對(duì)于一個(gè)厚尾量測(cè)噪聲的線性狀態(tài)空間模型，學(xué)生t后驗(yàn)概率密度函數(shù)(Probability Density Func-tion, PDF)可以視為無(wú)窮多個(gè)高斯PDF的混合，則似然PDF(|)可以表示為

(|)=St(;,,)

(42)

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

(43)

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

(44)

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

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

(45)

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

(46)

由于量測(cè)噪聲的協(xié)方差特性是緩慢變化的，可通過(guò)來(lái)傳遞上一時(shí)刻的后驗(yàn)PDF，則先驗(yàn)參數(shù)和可分別表示為

(47)

(48)

2.2 基于學(xué)生t分布的新型魯棒自適應(yīng)濾波器

本節(jié)的目的是計(jì)算聯(lián)合后驗(yàn)PDF(,,,|1∶)。然而，基于構(gòu)建的分層高斯?fàn)顟B(tài)空間模型，無(wú)法獲取后驗(yàn)PDF的一個(gè)封閉形式的解析解。因此，為了得到其近似解，本文利用變分貝葉斯(Variational Bayes，VB)方法來(lái)聯(lián)合推斷狀態(tài)向量、尺度矩陣、輔助隨機(jī)變量和自由度參數(shù)，即?{,,,}。將(|1∶)表示為一個(gè)具有自由分解形式的近似后驗(yàn)PDF形式為

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

(49)

其中，(·)表示近似的后驗(yàn)PDF。上述表達(dá)式的最優(yōu)解可以通過(guò)最小化真實(shí)后驗(yàn)(,,|1∶)和近似后驗(yàn)()()()PDF的KL散度(Kullback-Leibler Divergence，KLD)來(lái)實(shí)現(xiàn)，則最優(yōu)解滿足以下的方程

(50)

基于構(gòu)建的分層高斯?fàn)顟B(tài)空間模型的條件獨(dú)立性，狀態(tài)向量、量測(cè)噪聲的協(xié)方差陣、輔助隨機(jī)變量和自由度參數(shù)分別更新為高斯分布、逆Wishart分布、伽馬分布及其伽馬分布，可以得到

G(;-1,-1)

(51)

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

log(,1∶)=

(52)

令分別等于、、、，將方程(50)代入方程(52)，其具體的推導(dǎo)過(guò)程類似于文獻(xiàn)[12]和文獻(xiàn)[13], 基于學(xué)生t分布的魯棒自適應(yīng)濾波算法，其執(zhí)行的偽代碼可以總結(jié)如表1所示。

表1 基于學(xué)生t分布的魯棒自適應(yīng)濾波算法

3 試驗(yàn)驗(yàn)證

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

圖1 試驗(yàn)系統(tǒng)設(shè)備的連接圖Fig.1 Connection diagram of the test system equipments

表2 IMU參數(shù)

表3 試驗(yàn)設(shè)備的主要參數(shù)表

在實(shí)際海試試驗(yàn)中，由于水下復(fù)雜環(huán)境的干擾和多路徑效應(yīng)等因素的影響，USBL和DVL實(shí)測(cè)的測(cè)量信息在很多時(shí)候都會(huì)有野值的干擾，有些是明顯的野值，有些不明顯。圖2和圖4所示為這次海試試驗(yàn)在一段時(shí)間內(nèi)實(shí)測(cè)的水聲數(shù)據(jù)。圖3和圖5所示為USBL和DVL有無(wú)野值時(shí)的概率密度對(duì)比圖。

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

圖2 USBL實(shí)測(cè)數(shù)據(jù)Fig.2 Data measured by USBL in real time

圖3 概率密度對(duì)比Fig.3 Probability density comparison

圖4 DVL實(shí)測(cè)數(shù)據(jù)Fig.4 Data measured by DVL in real time

圖5 概率密度對(duì)比Fig.5 Probability density comparison

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

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

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

表4 定位統(tǒng)計(jì)的結(jié)果

圖8 濾波器參數(shù)分析Fig.8 Filter parameter analysis

結(jié)合圖6、圖7和表4統(tǒng)計(jì)結(jié)果，將提出的Rst-SINS/USBL/DVL與Rst-SINS/USBL、Rst-SINS/DVL做比較可以看出，Rst-SINS/USBL/DVL具有更好的定位精度。這是因?yàn)殡m然USBL能夠?qū)INS累積的位置誤差起調(diào)制和校正作用，但是仍受限于USBL定位精度；同時(shí)，Rst-SINS/USBL組合的速度誤差仍然很大，且Rst-SINS/USBL組合為區(qū)域內(nèi)導(dǎo)航。因此，可以通過(guò)融合SINS/USBL/DVL的多源信息，利用DVL進(jìn)一步抑制速度誤差進(jìn)而提高組合定位精度，實(shí)現(xiàn)長(zhǎng)航時(shí)和廣區(qū)域內(nèi)導(dǎo)航。Rst-SINS/DVL組合導(dǎo)航雖然可以抑制速度誤差，但是其位置誤差隨時(shí)間是發(fā)散的，且與初始的位置誤差有關(guān)。因此，通過(guò)海試試驗(yàn)證明了本文提出的基于學(xué)生t分布的SINS/USBL/DVL多源組合導(dǎo)航算法的有效性和優(yōu)越性，提高了組合定位的精度、魯棒性和可靠性。

4 結(jié)論

針對(duì)復(fù)雜水下聲場(chǎng)環(huán)境下水聲量測(cè)信息的不確定性所引起的厚尾特性，本文推導(dǎo)了基于學(xué)生t分布的新型魯棒自適應(yīng)濾波器，通過(guò)提出的SINS/USBL/DVL多源信息融合的組合導(dǎo)航策略，提高了水下航行器導(dǎo)航與定位的精度和可靠性。

1)針對(duì)復(fù)雜水下環(huán)境下量測(cè)噪聲未知和量測(cè)野值等不確定量測(cè)對(duì)定位精度的影響，將狀態(tài)向量、量測(cè)噪聲的協(xié)方差陣、輔助隨機(jī)變量和自由度參數(shù)分別更新為高斯分布、逆Wishart分布、伽馬分布及其伽馬分布，基于學(xué)生t分布推導(dǎo)了新型的魯棒自適應(yīng)濾波算法，實(shí)現(xiàn)了對(duì)未知量測(cè)噪聲方差的自適應(yīng)估計(jì)和對(duì)量測(cè)野值的魯棒處理。

2)推導(dǎo)了SINS/USBL/DVL多源信息融合的濾波模型，基于設(shè)計(jì)的集中卡爾曼濾波器，提高了組合定位的精度，能夠滿足水下航行器在復(fù)雜水下聲場(chǎng)環(huán)境下高精度、長(zhǎng)航時(shí)導(dǎo)航與定位的需求。