一種基于集中濾波的SINS/DVL/USBL水下組合導航算法

張亞文,莫明崗,馬小艷,江 薇

(北京自動化控制設備研究所, 北京 100074)

一種基于集中濾波的SINS/DVL/USBL水下組合導航算法

張亞文,莫明崗,馬小艷,江 薇

(北京自動化控制設備研究所, 北京 100074)

針對水下運載體的長時間高精度導航問題,提出一種基于集中濾波的捷聯慣性導航系統(SINS)/聲學多普勒測速儀(DVL)/超短基線(USBL)水下組合導航方法,建立了桿臂在線估計的SINS/DVL速度觀測模型和SINS/USBL相對量測信息觀測模型,并利用參考信息判定結果實時更新量測方程及維數,既能保證精度,還能一定程度上減少運算量。理論仿真和湖面試驗表明,本算法能夠實現長時間高精度的水下導航定位,具有可行性。

組合導航;集中Kalman濾波;SINS;DVL;USBL

0 引言

相對陸空導航,水下導航因具有環境復雜、信息源少、隱蔽性要求高等特點而格外困難[1]。目前國際常用的水下導航定位技術手段有慣性導航技術、DVL航位推算、水聲定位技術以及組合導航技術等[2]。單一的導航系統由于自身存在的不足,已無法滿足現代導航系統高精度高可靠性的要求,例如,捷聯慣導系統能夠為水下運載體提供完備導航信息,但其誤差隨時間積累;多普勒測速儀雖能夠測得高精度的速度信息,但位置誤差仍隨航程積累;超短基線(USBL)水聲定位系統雖能夠測量應答器相對于基陣的相對位置信息[3],但需要結合慣導的航向姿態信息才能完成導航[4-5]。

組合導航技術利用兩種或兩種以上的非相似導航系統對同一導航信息做測量并解算以形成量測量,從這些量測量中計算出各導航系統的誤差并校正[6],能夠提高水下航行器的提導航精度。常用的組合導航方式有SINS/DVL組合導航和SINS/USBL組合導航。然而SINS/DVL組合導航是一種速度組合的模式,能夠獲得水下航行器實時高精度的速度信息,但位置誤差仍隨時間增長;SINS/USBL組合導航能夠有效抑制導航系統位置誤差的發散,但又受水聲換能器作用距離的限制,常用于區域范圍內的高精度定位;SINS/DVL/USBL組合導航系統能夠兼具SINS/DVL組合和SINS/USBL組合導航的優點,達到互補,是實現水下航行器長航時高精度導航的最優技術手段。本文提出了一種基于集中濾波融合思路的SINS/DVL/USBL組合導航算法,建立了相應的組合導航誤差模型,并優化了量測噪聲算法,通過理論仿真和湖面試驗證明了算法的可行性。

1 SINS/DVL/USBL組合導航模型

相對于聯邦濾波分散地處理各子導航系統獲得導航誤差的全局次優估計,集中式Kalman濾波則利用一個Kalman濾波器來集中地處理所有導航子系統的信息,能夠獲得導航誤差狀態的全局最優估計。實現工程應用中,集中濾波存在以下主要缺點:1)狀態維數高,計算負擔重,不利于實時計算;2)容錯性能相對較差。然而,隨著DSP技術的飛速發展,嵌入式導航計算的計算速度和存儲容量都得到了極大的提高,已經能夠很好地解決集中濾波狀態維數高和運算量大等問題;此外,Kalman濾波計算過程中,觀測量是Kalman濾波器與外圍的唯一橋梁,濾波器是否被污染,主要取決于量測信息是否正確輸入,即在保證嚴格的參考信息檢測的情況下,集中式Kalman濾波器的精度優勢還是非常顯著的。

本文采取集中濾波的信息融合方法實現SINS/DVL/USBL組合導航,詳細濾波框架如圖 1所示。以SINS/DVL速度量測量和SINS/USBL相對位置信息量測量共同作為集中Kalman濾波量測量。

圖1 SINS/DVL/USBL組合導航濾波原理Fig.1 The scheme of SINS/DVL/USBL integrated navigation theory

1.1 SINS/DVL/USBL組合導航狀態模型

1.1.1 SINS子系統狀態誤差模型

選取慣導載體坐標系b系為前上右坐標系,導航坐標系n系為北天東坐標系。選取傳統15維慣導系統誤差狀態量作為SINS子系統誤差狀態量[7]為

XSINS= [δVNδVUδVEφNφUφEδφδh

(1)

本文使用捷聯式指北方位系統通用誤差模型狀態方程為

(2)

其中，慣性導航系統系統矩陣FSINS、系統噪聲WSINS和系統噪聲分配矩陣GSINS可參考捷聯式指北方位系統通用的15維系統矩陣。

1.1.2 DVL子系統狀態誤差模型

通過試驗數據分析發現,DVL測速誤差以及DVL到SINS的安裝誤差等對組合導航精度影響較大。DVL的測速誤差主要為受聲速引起的刻度系數誤差,直接導致參考速度信息精度下降,引起組合導航精度下降;DVL到SINS的安裝誤差主要包括:DVL到SINS的安裝角誤差會導致慣性/DVL組合導航信息觀測點的空間不一致,引起組合導航精度下降;DVL到SINS的桿臂誤差在水下運載體轉彎或橫搖縱搖時,同樣會引起慣導速度與DVL速度測量點空間不一致問題,引起組合導航精度下降。

因此,本文選取DVL刻度系數誤差、DVL到SINS的航向安裝誤差角誤差和桿臂作為DVL部分的狀態誤差量,即DVL子系統誤差狀態量為

(3)

其中， βd為DVL到SINS的航向安裝誤差角誤差,δKd為DVL刻度系數誤差,R=[Rx Ry Rz]T為DVL到慣導的桿臂誤差。

建立DVL子系統狀態方程為

(4)

其中, FDVL=[05×5]為DVL子系統系統矩陣,GDVL和WDVL為相關噪聲矩陣項。

1.1.3USBL子系統狀態誤差模型

通過實驗數據處理發現,USBL自身測角精度以及USBL基陣坐標系到SINS載體坐標系的安裝角標定精度對定位精度影響很大。從數值角度粗略分析:如果USBL測量的方位角β或者航向安裝誤差角 θy有1°誤差,則會引入約1.7%R的水平位置誤差,而且隨著斜距R變大誤差會進一步增大。USBL的測角誤差在工作范圍內都為小角度,且安裝角誤差通過事先標定后能夠收斂到一個小角度范圍內,故本文將對USBL自身測角誤差和USBL到SINS的安裝角誤差進行建模,并將其設計為誤差狀態量。受不同水文氣象對聲波傳輸速度的影響[8],USBL的測距誤差也一定程度上影響著組合導航的精度,故本文也將USBL測距刻度系數誤差δKd設計為Kalman濾波組合導航的誤差狀態量。

因此,本文選取USBL到慣導的3個安裝誤差角誤差、USBL自身測量誤差為USBL子系統誤差狀態量

(5)

建立USBL子系統的濾波狀態方程為

(6)

其中,FUSBL=[06×6]為 USBL子系統系統矩陣,GUSBL和WUSBL為相關噪聲矩陣項。

1.1.4 SINS/DVL/USBL組合導航系統狀態誤差模型

綜上所述,本文涉及的SINS/DVL/USBL組合導航集中Kalman濾波狀態方程為

(7)

其中,集中Kalman濾波狀態量為

(8)

集中Kalman濾波系統矩陣為

1.2 SINS/DVL/USBL組合導航觀測模型

本文設計的SINS/DVL/USBL組合導航集中Kalman濾波器的觀測模型主要由兩部分組成:SINS /DVL速度觀測模型、SINS/USBL相對量測信息觀測模型。

1.2.1 桿臂在線估計SINS /DVL速度觀測模型

(9)

(10)

1.2.2SINS/USBL相對量測信息觀測模型

綜合分析本研究結果可知，恩施煙區煙葉在海拔540～1 680 m范圍內，感官評吸質量對海拔高度較主要化學成分及其協調性敏感，而感官評吸質量是衡量煙葉質量好壞的決定性指標，因此，在恩施州，適當將植煙區域向高海拔地區調整有利于煙葉品質的提高。

(11)

對式(11)求導可得構造的USBL水聲基陣坐標系中應答器相對于慣導的高度角、方位角和斜距對應誤差項為

(12)

(13)

對式(13)進行誤差分析,可得SINS/USBL相對量測信息誤差觀測方程為

(14)

HS/U= [03×3(Hφ)3×3(HδP)3×306×305×3

(Hθ)3×3(HU)3×3]3×26

(15)

1.2.3 SINS/DVL/USBL組合導航系統觀測模型

綜上所述可得,本技術方案中 SINS/DVL/USBL組合導航集中Kalman濾波觀測方程為

Z=HX+v

(16)

由于DVL和USBL的數據更新頻率不一致,這對集中濾波的實現帶來了一定的不便。針對上述情況,本算法根據DVL和USBL參考信息的有效性實時判斷結果,自適應地改變觀測方程的維數,以期望盡可能地挖掘DVL和USBL每個更新周期提供的參考信息,同時減少運算量。DVL數據更新頻率為1Hz,而USBL數據更新頻率通常為2Hz,甚至更長,但均可配置為DVL數據更新頻率的整數倍,故本文采取的具體方法為:

1)當僅DVL有效時,僅利用SINS/DVL速度量測信息進行Kalman濾波,觀測方程簡化為

Z=(ZS/D)3×26=(HS/D)3×26X26×1+v

(17)

2)當DVL和USBL同時有效時,采用式(15)進行Kalman濾波估計。

由于集中濾波模型建立時統一考慮了兩種觀測模型所需的相關狀態誤差量,故上述兩種濾波狀態可直接進行切換。

2 SINS/DVL/USBL組合導航集中Kalman濾波方程

對式(8)和式(15)進行離散化處理可得SINS/DVL/USBL組合導航集中Kalman濾波模型為:

(18)

其中，Φk+1/k為第k時刻到第k+1時刻的一部狀態轉移矩陣,Γk為系統噪聲驅動陣,Wk和Vk+1均為白噪聲,且方差陣依次為Q、R。

本方法采用標準Kalman濾波基本方程,各變量的定義與標準離散Kalman濾波的變量定義保持一致:

Pk=(I-KkHk)Pk/k-1

(19)

選擇合適的濾波初值(狀態初始值X0,初始估計均方誤差陣P0,系統噪聲初始方差陣Q0以及量測噪聲方差陣R0),按照式(19)進行Kalman濾波計算,待Kalman濾波器穩定后,利用Kalman濾波得到的慣導速度誤差、位置誤差和航向姿態誤差等誤差狀態估計值Xk實時修正慣導速度、位置和航向姿態角,可以得到高精度的SINS/DVL/USBL組合導航結果。

3 理論數據仿真分析

慣導初始地理位置為北緯39.8°、東經116.2°、高度等于0,以4m/s(約6節)的前向速度按圖2所示航跡行駛,其中應答器位置為[39.802026764,-50,116.198185395],理想初始航向姿態角均為0°。慣導采樣周期為5ms,DVL測速更新周期為1s,USBL數據更新周期為2s。各傳感器誤差和初始誤差設置如表1所示。

表1 傳感器誤差和導航初始誤差

按照本文設計的SINS/DVL/USBL組合導航算法進行仿真,并對SINS/DVL 組合導航、SINS/USBL組合導航、聯邦濾波以及集中濾波定位結果進行對比,如表2所示。仿真航跡如圖2所示,定位誤差曲線如圖3～圖5所示。

表2 1σ定位精度對比

從圖3～圖5位置誤差曲線可以看出,本文設計的SINS/DVL/USBL組合導航集中濾波算法能夠獲得較高的定位精度,且精度優于其他三種情況,具有理論可行性。

圖2 仿真航跡Fig.2 The navigation track of simulation

圖3 北向位置誤差Fig.3 North position error

圖4 東向位置誤差Fig.4 East position error

圖5 水平定位誤差Fig.5 Level position error

4 湖面實驗數據處理分析

湖面試驗系統主要包括:光纖捷聯慣導系統(SINS)、DVL測速儀、USBL水聲定位系統和DGPS系統。光纖陀螺漂移優于0.02(°)/h,加表零偏優于30μg(1σ);DVL系統采用RDI公司的WHN300kHZ的多普勒測速儀,數據更新周期為2s,測速精度為0.4%±4cm/s;USBL系統采用NavQuest公司的Track Link 1500HA水下基陣和應答器,其原始輸出為高度角、方位角和斜距,其測角精度優于1°,定位精度為0.5%,數據更新周期為2s。

按照本文設計的基于集中SINS/DVL/USBL組合導航算法對湖面試驗采集的一組導航原始試驗數據進行處理,并以DGPS提供的高精度位置為基準信息考核組合導航算法的定位精度,同時對SINS/DVL 組合導航、SINS/USBL組合導航、聯邦濾波以及集中Kalman濾波的導航結果進行對比,定位精度如表3所示,組合導航初始時刻,先用DGPS對慣導解算的位置速度進行重置,以避免原始積累的誤差對組合導航精度考核產生影響[10]。

表3 湖面試驗定位精度

從表3湖面試驗定位精度可以看出:本文提出的基于集中濾波的SINS/DVL/USBL組合導航定位精度(1σ)優于SINS/DVL、SINS/USBL以及聯邦濾波的組合導航結果。

圖6～圖9所示為幾種不同組合導航算法的水平定位誤差對比圖,圖中曲線對應載體的行駛軌跡,黑色箭頭表示起點位置及行駛方向,色彩由紅(5m誤差)到藍(0m)表示誤差遞減。對比圖6和圖9可知,第二圈開始集中Kalman濾波(圖9)精度明顯優于SINS/DVL組合精度(圖6),即表明集中濾波由于參考信息的增加,其長時間導航精度得到進一步提高;由圖7可知,SINS/USBL組合導航由于開始時濾波器未完全收斂,第一圈時位置誤差較大,可達5m左右;圖8中聯邦濾波能利用SINS/DVL濾波器的快速收斂，一定程度上改善圖6第一圈出現的定位誤差較大段(深紅色軌跡段);對比圖7、圖8和圖9可知,集中濾波導航結果不僅能夠更快地估計出各導航誤差,且其精度優勢更為明顯。綜上所述,本文設計的基于集中濾波的SINS/DVL/USBL水下組合導航算法具有較高的精度,即驗證了湖面試驗數據情況下本算法的可行性。

圖6 SINS/DVL組合導航結果Fig.6 The navigation result of SINS/DVL

圖7 SINS/USBL組合導航結果Fig.7 The navigation result of SINS/USBL

圖8 聯邦濾波導航結果Fig.8 The navigation result of SINS/DVL/USBL based on the federal-KF

圖9 集中濾波導航結果Fig.9 The navigation result of SINS/DVL/USBL based on the Concentrated-KF

5 結論

本文利用集中Kalman濾波方法,提出一種基于集中濾波的SINS/DVL/USBL水下組合導航方法,建立了組合導航狀態方程和量測方程,并根據當前參考信息判定結果實時更新量測方程及維數,在減少運算量的同時保證了組合導航精度。理論仿真和湖面試驗表明,本算法能夠實現長時間高精度的水下導航定位,具有可行性。

[1] 馬偉峰, 胡震. AUV 的研究現狀與發展趨勢[J]. 火力與指揮控制, 2008, 33(6): 10-13.

[2] 周保軍, 劉碩. 水下導航技術研究[J].現代導航, 2012, 3(1): 19-23.

[3] Link Quest Inc.TrackLink 1500 USBL Tracking System, User’s Guide Version 1500.7.2.

[4] 蘭華林，孫大軍，張殿倫，等. 單應答器導航深海實驗[J]. 海洋工程，2007, 25(2)：110-113.

[5] 隋海琛，田春和，韓德忠，等. 水下定位系統誤差分析[J].水道港口, 2010, 31(1):69-72.

[6] 秦永元, 張洪鉞, 汪叔華. 卡爾曼濾波與組合導航原理[M].西安：西北工業大學出版社，2004.

[7] 秦永元. 慣性導航[M]. 北京:科學出版社, 2006.

[8] 馮守珍, 吳永亭, 唐秋華. 超短基線聲學定位原理及其應用[J]. 海岸工程, 2002, 21(4): 13-18.

[9] 張亞文, 莫明崗, 馬小艷, 等. 一種基于相對測量信息的SINS/USBL組合導航算法[J]. 導航定位與授時, 2014, 3(2): 7-13.

[10] 唐康華, 江明明, 翁炬. 基于自適應濾波的水下SINS/相控陣DVL組合導航算法設計[J]. 中國慣性技術學報, 2013, 21(1): 66-70.

An Algorithm of Underwater SINS/DVL/USBL Integrated Navigation Based on Concentrated Filtering

ZHANG Ya-wen， MO Ming-gang， MA Xiao-yan， JIANG Wei

(Beijing Institute of Automatic Control Equipment, Beijing 100074, China)

For long-time and high-precision navigation of UUV, an integrated navigation algorithm of SINS,DVL and USBL which is based on the Concentrated-KF is designed, then a SINS/DVL observation model which can compensate the arm error in real time and a SINS/USBL model which observes the relative measurement information are established. Besides, on the basis of the result of reference information judgement, the realtime updates of the Filter observation equation and its dimension are achievedwithout increasing of the computation or losing the navigation precision. Finally, the results of theoretical simulation and data analysis of lake test show the advantage and the practicability of the new integrated navigation algorithm.

Integrated navigation; Concentrated-KF; SINS; DVL; USBL

10.19306/j.cnki.2095-8110.2017.01.005

2016-11-20；

2016-12-07。

海軍“十三五”預先研究項目(3020603030403)

張亞文(1990-),男,碩士,助理工程師,主要從事慣性/水聲組合導航技術研究。

V249

A

2095-8110(2017)01-0025-07