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

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

水下載體SINS/USBL組合導(dǎo)航濾波方法研究

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

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

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

SINS;USBL;組合導(dǎo)航;Sage-Husa自適應(yīng)濾波

0 引言

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

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

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

1 系統(tǒng)組成與工作原理

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

圖1 系統(tǒng)組成原理圖Fig.1 Diagram of system construction

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

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

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

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

Pae=Pte+CanPta

(1)

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

2.2USBL定位誤差模型

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

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

(2)

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

(3)

(4)

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

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

2.3USBL絕對定位誤差模型

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

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

(5)

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

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

3.1 量測噪聲特性分析

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

ΔPae=ΔCanPta+CanΔPta

(6)

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

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

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

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

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

3.2 濾波模型

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

(7)

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

觀測量Zk=Pins-Pae=ΔPins-ΔPae，Pins為慣導(dǎo)解算水平位置,ΔPins為慣導(dǎo)解算位置誤差?？梢娏繙y噪聲Vk的特性完全取決于USBL的絕對定位誤差。

3.3 濾波算法

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

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

(8)

(9)

(10)

(11)

(12)

Rk= (1-dk)Rk-1+

(13)

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

(14)

其中,b為遺忘因子，0

3.4 仿真驗證

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

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

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

最終的仿真結(jié)果對比如表1所示。

表1 仿真結(jié)果對比

3.5 實驗驗證

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

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

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

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

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

表2給出了普通Kalman濾波組合算法和自適應(yīng)Kalman濾波組合算法的實驗比對結(jié)果，從系統(tǒng)定位誤差來看，自適應(yīng)Kalman濾波組合方案下系統(tǒng)定位誤差要優(yōu)于普通Kalman濾波組合方案。

表2 不同組合濾波算法結(jié)果對比

4 結(jié)論

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

[1] 佟盛. AUV導(dǎo)航系統(tǒng)及技術(shù)發(fā)展[C]. 中國造船工程學(xué)會2006年船舶通訊導(dǎo)航學(xué)術(shù)會議, 2006: 241-244.

[2] 李俊,徐德民,宋保維,等. 自主式水下潛器導(dǎo)航技術(shù)發(fā)展現(xiàn)狀與展望[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] 錢洪寶,孫大軍. 水聲定位系統(tǒng)現(xiàn)狀[J]. 聲學(xué)技術(shù),2011, 30(3): 389-391.

[5] 喻敏. 長程超短基線定位系統(tǒng)研制[D]. 哈爾濱： 哈爾濱工程大學(xué), 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] 田坦. 水下定位與導(dǎo)航技術(shù)[M]. 北京: 國防工業(yè)出版社, 2007: 27-44.

[9] 秦永元. 慣性導(dǎo)航[M]. 北京: 科學(xué)出版社, 2006: 355-361.

[10] 付夢印,鄧志紅，閆莉萍. Kalman濾波理論及其在導(dǎo)航系統(tǒng)中的應(yīng)用[M]. 北京: 科學(xué)出版社, 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。

裝備預(yù)研船舶重工聯(lián)合基金(6141B04050301-04)

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

U666.12

A

2095-8110(2017)01-0020-05