水下載體SINS/USBL組合導航濾波方法研究

于 浩，王彥國，胡小毛，王桂如

(天津航海儀器研究所，天津 300131)

水下載體SINS/USBL組合導航濾波方法研究

于 浩，王彥國，胡小毛，王桂如

(天津航海儀器研究所，天津 300131)

捷聯慣性導航系統(SINS)具有短期定位精度高的優點，但其定位誤差隨時間發散，超短基線(USBL)在其有效工作范圍內定位誤差不發散，且由于受作用距離及復雜水下環境的影響,其定位數據不連續，離散度大，野值多。水下載體導航采用SINS/USBL組合導航，可充分發揮各自的優勢。根據USBL量測信息的特性，給出了一種基于改進的帶觀測噪聲時變估計器的Sage-Husa自適應Kalman濾波方案，經實物數據仿真驗證了此濾波方案的有效性。

SINS;USBL;組合導航;Sage-Husa自適應濾波

0 引言

近年來在全世界范圍內，水下運載體(AUV、ROV)在環境監測和監督，河口、海港和管線的水下檢查，地質和生物勘察等方面的應用越來越廣泛。這些應用場合均要求使用低成本、緊湊、高性能的導航系統，能夠精確測量運載體的位置和姿態[1-2]。

SINS具有體積小、質量小、可靠性高的優點，隨著SINS成本的不斷降低，越來越多地裝備到了各類AUV和ROV中，用以替代原來的低精度羅經，為水下載體提供全天候、完全自主、不受外界干擾的全導航參數。SINS具有優異的短期精度，但其誤差會隨時間發散，通常使用其他量測信息來輔助SINS，構成組合導航系統，抑制誤差發散。最典型的就是SINS/GNSS組合導航系統，但GNSS信號無法到達水下，為了獲取GNSS校正信息需要水下載體定期上浮，消耗了過多的時間、動力及成本。使用聲學定位技術輔助，例如長基線(LBL)系統，需要繁瑣而耗時的布放、校準及回收；使用安裝于船體的短基線(SBL)系統，必須將其緊密安裝在船身并且受到船體自然彎曲的影響；使用超短基線(USBL)系統，具有適宜的精度，且使用方便，成為輔助SINS構成組合導航系統的理想選擇[3-4]。

USBL提供的絕對位置是經視在位置坐標變換而來，這樣導致SINS/USBL組合導航系統的量測信息誤差特性疊加了USBL自身的聲速誤差、時延誤差、測相誤差以及航姿誤差等[5]，不再滿足傳統Kalman濾波量測噪聲為零均值高斯白噪聲的假設條件。文獻[6]將USBL誤差建模為有色噪聲和白噪聲的和，并考慮到了水聲數據延遲特性的影響,提出了幾種解決方案，但并沒有給出具體的組合濾波算法[6]。本文通過對SINS/USBL組合量測信息誤差特性分析，使用了觀測噪聲時變估計器對其量測噪聲方差進行實時估計，通過實物數據仿真驗證了算法的正確性。

1 系統組成與工作原理

組合導航系統的組成如圖1所示。SINS中安裝有3個陀螺和3個加速度計，并運行捷聯解算程序。USBL采用傳統的測距及測相原理對目標進行定位，位置固定的應答器提前標定好，并由USBL實時測量其與USBL基陣間的時延及相位差信息，經過位置變換獲得USBL基陣處的絕對位置。SINS利用自身解算位置與USBL位置之差作為觀測量進行Kalman濾波，補償SINS隨時間增長的定位、速度、姿態等誤差。

圖1 系統組成原理圖Fig.1 Diagram of system construction

2 USBL位置變換算法及定位誤差模型

2.1 USBL視在位置到絕對位置變換算法

基陣坐標系定義為：坐標原點位于陣元中心，橫軸X，縱軸Y且與垂向軸Z構成右手坐標系。載體系定義為：坐標原點位于載體搖擺中心，X軸直線右舷，Y軸指向艦艏，Z軸與X、Y軸垂直且構成右手坐標系。導航坐標系為東北天地理坐標系。

USBL直接測量得到的是信標與基陣之間的相對位置信息，將此相對位置(視在位置)變換為基陣處的絕對位置需要經過相應的變換。設已標定完成的應答器位置為Pte，USBL測量得到的應答器在基陣坐標系中的相對位置為Pta，基陣的位置為Pae，則[7]

Pae=Pte+CanPta

(1)

式中,Can為基陣坐標系向地理坐標系轉換的方向余弦矩陣，由載體的航向角、橫搖角和縱搖角構成。

2.2USBL定位誤差模型

使用ΔPta=[σaxσay]表示USBL測量得到的相對基陣系x和y方向的定位誤差為

ΔPta=[σaxσay]=R[δaxδay]

(2)

其中，R為USBL基陣與被測目標間的斜距。忽略陣元間距誤差的影響，則超短基線定位系統的聲學相關的定位誤差定義如下[8]：

(3)

(4)

α為目標與基陣系x軸的夾角，β為目標與基陣系y軸的夾角，c為水中聲速，Δc為聲速誤差，T為時延，ΔT為時延測量誤差，φx、φy為對應軸接收到目標信號的相位差，Δφx、Δφy為對應軸相位差的測量誤差。

由式(2)、式(3)、式(4)可以看出，USBL聲學定位精度與斜距、目標與基陣系坐標軸夾角、時延及相位測量精度相關。

2.3USBL絕對定位誤差模型

對式(1)求全微分，得基陣的位置誤差為

ΔPae=ΔPte+ΔCanPta+CanΔPta

(5)

其中，ΔPae為基陣的位置誤差，ΔPte為應答器的標定位置誤差，ΔCan為姿態陣誤差，ΔPta為USBL聲學定位誤差，由式(2)、式(3)、式(4)確定。

3 SINS/USBL組合濾波模型及仿真

3.1 量測噪聲特性分析

采用高精度的位置基準(例如RTKGNSS)和標定算法，可將信標的標定誤差控制在亞米級，故可將式(5)中的ΔPte項忽略不計，則量測噪聲變為

ΔPae=ΔCanPta+CanΔPta

(6)

由式(6)可以看出，使用USBL變換后的位置作為觀測信息時，觀測誤差與姿態角誤差、基陣與信標間的相對位置及聲學定位誤差有關。

假定信標深度為50m，USBL定位數據頻率為1Hz。SINS陀螺漂移為0.01(°)/h，加速度計零偏為5×10-5g，聲速相對常值誤差為0.001c(c為聲速真實值)，測時常值誤差為0.0005s，測相常值相對誤差為1%，隨機誤差方差與常值誤差相同。載體在距離信標垂直距離為50m的水平面上運動，分兩種情況對USBL水聲定位誤差進行仿真，一是載體繞信標環形，半徑為100m；二是載體沿通過信標在水平面投影點的直線航行，載體起點在信標的投影正上方，速度為2kn。仿真結果如圖2、圖3所示。

圖2 載體環形USBL絕對定位誤差Fig.2 USBL absolute positioning error in ring motion

圖3 載體直線航行USBL絕對定位誤差Fig.3 USBL absolute positioning error in straight line

未完全補償的聲速誤差、時延誤差及測向誤差等系統常值誤差的存在，使得USBL定位誤差呈現非零均值特性。由圖2、圖3可以看出，當載體繞信標環形時，因USBL測量的斜距R與2個相位角α、β均為常值，故其定位誤差會載體航向不同呈被正弦曲線調制的噪聲形式；當載體直航時，由于斜距及2個相位角均發生改變，定位誤差隨斜距的增加誤差不斷增大。兩種航跡的誤差均具有非平穩噪聲特性。

3.2 濾波模型

采用東北天坐標系作為導航坐標系，取INS的2個水平位置誤差、2個水平速度誤差、3個姿態角誤差、3個加速度計零偏、3個陀螺儀常值漂移作為狀態變量，并將INS解算位置與USBL量測位置之差作為觀測變量，系統模型為[9]：

(7)

其中，F為系統矩陣，H為量測矩陣，W為系統過程噪聲，包括3個加速度計零偏、3個陀螺儀常值漂移，W～(0,Q)，V為量測噪聲。

觀測量Zk=Pins-Pae=ΔPins-ΔPae，Pins為慣導解算水平位置,ΔPins為慣導解算位置誤差。可見量測噪聲Vk的特性完全取決于USBL的絕對定位誤差。

3.3 濾波算法

根據3.1節的分析可知，采用USBL量測位置作為觀測量時，觀測噪聲具有時變的均值和方差，傳統的Kalman濾波方法在量測噪聲不滿足高斯白噪聲的條件時無法獲得最優解，甚至會引起濾波器的發散。Sage-Husa自適應Kalman濾波方法具有原理簡單、實時性好，以及可同時估計出過程噪聲和觀測噪聲一、二階矩等特點，但直接使用該算法估計過程噪聲和觀測噪聲很容易引起濾波發散，同時算法對初值敏感及噪聲統計特性估計有偏[10]。考慮到系統過程噪聲較為穩定且方差誤差較小，采用改進的Sage-Husa自適應濾波算法，只對觀測噪聲的統計特性進行估計，會提高算法的實用性。

帶觀測噪聲時變估計器的簡化Sage-Husa自適應濾波算法如下：

(8)

(9)

(10)

(11)

(12)

Rk= (1-dk)Rk-1+

(13)

dk=(1-b)/(1-bk+1)

(14)

其中,b為遺忘因子，0

3.4 仿真驗證

根據以上的模型及濾波算法，針對3.1節的仿真數據進行仿真計算，仿真結果如圖4、圖5所示。

圖4 載體環線航行濾波誤差對比Fig.4 Comparison of filtering error in ring motion

圖5 載體直線航行濾波誤差對比Fig.5 Comparison of filtering error in straight line

最終的仿真結果對比如表1所示。

表1 仿真結果對比

3.5 實驗驗證

試驗區域選擇千島湖相對寬闊的水域，東西向及南北向長度均為2.5km以上，水深在50m左右。位置基準采用RTK DGPS，精度優于0.1m。試驗選用的慣性設備其標度因數重復性、穩定性，標度因數非線性度及不對稱度均為5×10-6，零偏穩定性和重復性均優于0.01(°)/h。USBL定位系統由基陣、甲板單元及對應的應答器組成，定位精度為1m±1%D(D為基陣與應答器之間的距離)。

試驗過程中首先進行應答器的布放及標定，使用反轉長基線方法標定應答器的位置，同時計算IMU與USBL之間的安裝誤差角并補償進系統。慣導對準完成后進入USBL作用距離范圍內，收到USBL提供的原始定位信息(與應答器之間的斜距、基陣坐標系下應答器的橫縱距離)反向解算出基陣的位置，在補償基陣與IMU之間的桿臂誤差后進行位置組合，輸出組合后的導航信息。

采用標準Kalman濾波和帶觀測噪聲時變估計器的簡化Sage-Husa自適應濾波仿真結果如圖6所示。

圖6 不同組合濾波算法軌跡比對Fig.6 Trajectory comparison of different combination filtering algorithms

圖6的細節圖顯示了兩種不同方案的效果，載體機動過程中，相對于普通Kalman濾波方案來說，自適應濾波方案能很好地跟蹤GPS真實軌跡。

表2給出了普通Kalman濾波組合算法和自適應Kalman濾波組合算法的實驗比對結果，從系統定位誤差來看，自適應Kalman濾波組合方案下系統定位誤差要優于普通Kalman濾波組合方案。

表2 不同組合濾波算法結果對比

4 結論

本文采用SINS/USBL組合導航方案，根據USBL量測信息的特性，給出了一種基于改進的帶觀測噪聲時變估計器的Sage-Husa自適應Kalman濾波方案，經實物數據仿真結果表明自適應Kalman濾波方案相比常規Kalman濾波器更能適應USBL有色噪聲的特性。

[1] 佟盛. AUV導航系統及技術發展[C]. 中國造船工程學會2006年船舶通訊導航學術會議, 2006: 241-244.

[2] 李俊,徐德民,宋保維,等. 自主式水下潛器導航技術發展現狀與展望[J]. 中國造船,2004, 45(3): 70-77.

[3] Bingham B. Navigating autonomous underwater vehicles[D]. U. S. A Franklin W. Olin College of Engineering,2008.

[4] 錢洪寶,孫大軍. 水聲定位系統現狀[J]. 聲學技術,2011, 30(3): 389-391.

[5] 喻敏. 長程超短基線定位系統研制[D]. 哈爾濱： 哈爾濱工程大學, 2006： 19-24.

[6] Mandt M, Gade K, Jalving B. Integrating DGPS-USBL position measurements with inertial navigation in the HUGIN 3000 AUV[C]//Proceedings of the 8thSaint Petersburg International Conference on Integrated Navigation Systems. Saint Petersburg, Russia, 2001: 28-30.

[7] Arkhipov M. A coordinate determination algorithm for USBL systems[C]//Proceedings of the 2thWSEAS International Conference on Circuits, Systems, Signal and Telecommunications. Acapulco, Mexico, 2008: 50-55.

[8] 田坦. 水下定位與導航技術[M]. 北京: 國防工業出版社, 2007: 27-44.

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

[10] 付夢印,鄧志紅，閆莉萍. Kalman濾波理論及其在導航系統中的應用[M]. 北京: 科學出版社, 2010: 108-118.

Research on SINS/USBL Integrated Navigation Filitering Method for Underwater Vehicles

YU Hao,WANG Yan-guo, HU Xiao-mao, WANG Gui-ru

(Tianjin Navigation Instrument Research Institute, Tianjin 300131, China)

Strapdown inertial navigation system (SINS) has the advantage of short-term high precision, butitspositioning error diverges over time. The positioningerror of ultra short baseline(USBL) doesn’t diverge within its effective working range, but itspositioning databecomes discontinuity with greater discreteness and more outliersdue to theimpact of operating range and complicated underwater environment. While SINS/UBSL integrated navigation scheme can help underwater vehiclemake full use of respective advantages.According to the characteristic of the USBL measurement information, a Sage-Husa adaptive Kalman filtering scheme which is based on improved observation time-varying noise estimatoris presented. Simulation verify the effectiveness of filtering scheme.

SINS; USBL; Integrated navigation; Adaptive filtering based on Sage-Husa

10.19306/j.cnki.2095-8110.2017.01.004

2016-10-11；

2016-11-10。

裝備預研船舶重工聯合基金(6141B04050301-04)

于浩(1964-)，男，研究員，主要從事導航制導與控制方面的研究。E-mail：yuhao_707@123.com

U666.12

A

2095-8110(2017)01-0020-05