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

于 浩，王彥國(guó)，胡小毛，王桂如

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

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

于 浩，王彥國(guó)，胡小毛，王桂如

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

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

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

0 引言

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

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

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

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

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

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

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

2.1 USBL視在位置到絕對(duì)位置變換算法

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

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

Pae=Pte+CanPta

(1)

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

2.2USBL定位誤差模型

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

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

(2)

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

(3)

(4)

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

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

2.3USBL絕對(duì)定位誤差模型

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

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

(5)

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

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

3.1 量測(cè)噪聲特性分析

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

ΔPae=ΔCanPta+CanΔPta

(6)

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

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

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

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

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

3.2 濾波模型

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

(7)

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

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

3.3 濾波算法

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

帶觀測(cè)噪聲時(shí)變估計(jì)器的簡(jiǎn)化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 仿真驗(yàn)證

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

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

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

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

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

3.5 實(shí)驗(yàn)驗(yàn)證

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

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

采用標(biāo)準(zhǔn)Kalman濾波和帶觀測(cè)噪聲時(shí)變估計(jì)器的簡(jiǎn)化Sage-Husa自適應(yīng)濾波仿真結(jié)果如圖6所示。

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

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

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

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

4 結(jié)論

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

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

[2] 李俊,徐德民,宋保維,等. 自主式水下潛器導(dǎo)航技術(shù)發(fā)展現(xiàn)狀與展望[J]. 中國(guó)造船,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] 喻敏. 長(zhǎng)程超短基線定位系統(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]. 北京: 國(guó)防工業(yè)出版社, 2007: 27-44.

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

[10] 付夢(mèng)印,鄧志紅，閆莉萍. 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