徐 博,郝 芮,王 超,張 勛

（哈爾濱工程大學(xué) 自動(dòng)化學(xué)院，黑龍江 哈爾濱 150001）

基于倒置聲學(xué)基陣的INSUSBL組合導(dǎo)航算法研究

徐 博,郝 芮,王 超,張 勛*

（哈爾濱工程大學(xué) 自動(dòng)化學(xué)院，黑龍江 哈爾濱 150001）

文中關(guān)注水下潛航器導(dǎo)航定位時(shí)，電磁波傳播途徑中能量衰減、慣導(dǎo)系統(tǒng)長(zhǎng)航時(shí)積累誤差以及提高水下潛航器定位精度等問(wèn)題?；诘怪玫某袒€聲學(xué)基陣，分析聲波往返傳播時(shí)間（RTT）、平面波近似方法和USBL導(dǎo)航解算方法及其坐標(biāo)轉(zhuǎn)換過(guò)程，結(jié)合INS誤差方程，建立INS/USBL松組合模型。為進(jìn)一步提高系統(tǒng)精度、動(dòng)態(tài)性能和抗干擾性，考察USBL的原始斜距、斜距差以及聲學(xué)基陣的空間分布信息，提出基于USBL原始輸出信息的INS/USBL緊耦合組合導(dǎo)航方法。通過(guò)MATLAB仿真對(duì)導(dǎo)航算法進(jìn)行驗(yàn)證，結(jié)果表明兩種算法能充分抑制純慣導(dǎo)誤差隨時(shí)間積累問(wèn)題，且有效地估計(jì)出姿態(tài)、速度和位置誤差角；其中緊耦合方法狀態(tài)估計(jì)誤差最小，導(dǎo)航參數(shù)精度相對(duì)松組合提高30%以上，對(duì)于提高水下載體導(dǎo)航定位精度、海洋探測(cè)具有重大意義。

組合導(dǎo)航算法；慣性導(dǎo)航系統(tǒng)；超短基線定位系統(tǒng)；緊組合技術(shù)

海洋覆蓋地球大部分表面積，其中的能源和礦產(chǎn)資源豐富，因此海洋探測(cè)與開發(fā)、海洋儀器設(shè)計(jì)與制造具有極高的經(jīng)濟(jì)價(jià)值和戰(zhàn)略意義，其中不容忽視的重要問(wèn)題就是水下潛航器的定位問(wèn)題[1]。慣導(dǎo)系統(tǒng)（inertial navigation system,INS）給載體提供完備的姿態(tài)、速度和位置等導(dǎo)航信息，短時(shí)間精度高，隱蔽性好，常用做主導(dǎo)航設(shè)備[2-4]。為解決長(zhǎng)航時(shí)其誤差隨時(shí)間積累問(wèn)題，常將慣導(dǎo)與其他導(dǎo)航方式組合。

聲波在水下傳播時(shí)要比光波和電磁波能量衰減都小，傳播距離更遠(yuǎn)。常用的水聲基線定位方式有長(zhǎng)基線、短基線、超短基線等，其中超短基線以其基陣尺寸小、使用靈活和安裝方便等優(yōu)點(diǎn)而廣泛應(yīng)用，更契合水下定位對(duì)導(dǎo)航系統(tǒng)的嚴(yán)苛需求。常用的超短基線定位系統(tǒng)主要由發(fā)射換能器、應(yīng)答器、聲學(xué)基陣組成[5-6]。船底的換能器發(fā)射詢問(wèn)信號(hào)至水下已知位置的應(yīng)答器，然后應(yīng)答器發(fā)射應(yīng)答信號(hào)被聲學(xué)基陣的各個(gè)水聽(tīng)器接收，從而得到目標(biāo)的定位信息。

Jalving B利用KF(Kalman Filter)技術(shù)把USBL（Ultra Short Base Line）和慣導(dǎo)系統(tǒng)的輸出信息進(jìn)行數(shù)據(jù)融合和濾波處理得到導(dǎo)航信息[7-8]。M Morgado采用直接法反饋校正的擴(kuò)展卡爾曼，補(bǔ)償系統(tǒng)的隨機(jī)漂移誤差，提高了導(dǎo)航精度[9-10]。國(guó)內(nèi)學(xué)者李守軍基于Kalman Filter的INS/USBL水下導(dǎo)航技術(shù)，只是簡(jiǎn)單的進(jìn)行系統(tǒng)模型的研究、誤差分析等，沒(méi)有對(duì)緊組合進(jìn)行深入研究[11]。張亞文提出的三應(yīng)答器SINS/USBL緊耦合導(dǎo)航算法[12]針對(duì)INS到USBL的測(cè)角誤差及安裝角誤差對(duì)導(dǎo)航精度的影響。其三應(yīng)答器緊耦合方法的導(dǎo)航誤差仿真結(jié)果相對(duì)于單應(yīng)答器精度并沒(méi)有明顯提高，只是相對(duì)平滑，穩(wěn)定性提高，對(duì)于應(yīng)答器數(shù)目還有硬性要求。因此要達(dá)到實(shí)際應(yīng)用精度還需要研究一種新的INS/UBSL耦合技術(shù)及導(dǎo)航算法。

本文針對(duì)針對(duì)自主性很高的水下潛航器，介紹超短基線定位原理，推導(dǎo)基于位置量測(cè)坐標(biāo)轉(zhuǎn)換的松組合INS/USBL狀態(tài)、量測(cè)方程。通過(guò)測(cè)量聲學(xué)信號(hào)從應(yīng)答器到達(dá)接收聲學(xué)基陣陣元之間的傳播時(shí)間和相位差來(lái)解算目標(biāo)的斜距和方位角[10]。考察USBL的原始斜距、斜距差以及聲學(xué)基陣的空間分布信息，結(jié)合慣導(dǎo)系統(tǒng)誤差方程，推導(dǎo)INS/UBSL緊組合量測(cè)方程。該緊耦合方法可以大大減少位置和方位角的濾波解算，且輸出信息的誤差形式較為簡(jiǎn)單且易于剔除。計(jì)算機(jī)仿真結(jié)果驗(yàn)證了該算法的可行性，對(duì)于工程實(shí)際中提高導(dǎo)航系統(tǒng)精度、動(dòng)態(tài)性能和抗干擾性具有重要意義。

1 超短基線定位系統(tǒng)的定位方法和誤差分析

1.1 USBL基于相位差的定位原理

建立基陣坐標(biāo)系oxyz，設(shè)定基陣陣元的間距為d，4個(gè)水聽(tīng)器分別位于兩個(gè)相互垂直的基線上（即x軸和y軸），其中心位于坐標(biāo)原點(diǎn)，其坐標(biāo)為(±d/2,±d/2)，目標(biāo)位于 T 處，其坐標(biāo)為（xa，ya，，za即為目標(biāo)的深度。USBL的定位原理圖如圖1所示。θx,θy為位置矢量與x軸和y軸的夾角，位置矢量的模||為斜距。目標(biāo)的位置矢量為→R，它與目標(biāo)的坐標(biāo)(xa，ya，za)的關(guān)系為：

在oxy平面內(nèi)，位置矢量的投影為水平距離r，與x軸的夾角為目標(biāo)的水平方位角：

聲學(xué)基陣尺寸較小，考慮到遠(yuǎn)場(chǎng)接收目標(biāo)的聲學(xué)信號(hào)，根據(jù)平面波近似視入射到聲學(xué)基陣中各基元的聲波平行。假設(shè)已知聲波的波長(zhǎng)為λ，φx,φy為

圖1 超短基線定位系統(tǒng)的定位原理圖

位于x軸和y軸上水聽(tīng)器接收信號(hào)的相位差，有：

將式（3）代入式（1）解得：

式中：R=c·Tri/2；c 為水中聲波傳播速度；Tri為換能器發(fā)射詢問(wèn)信號(hào)到接收到目標(biāo)應(yīng)答信號(hào)的時(shí)間。

測(cè)量USBL的φx,φy,c,Tri等參數(shù)值后，可以解算出目標(biāo)相對(duì)于基陣坐標(biāo)系的水平位置(xa，ya)、聲波入射角θx，θy、水平方位角θ和斜距R等。由式(1)和式(2)可得目標(biāo)在垂直方向的坐標(biāo)（即目標(biāo)的深度）為：

1.2 USBL導(dǎo)航解算方程

1.2.1 聲波往返傳播時(shí)間（RTT） 聲波往返傳播時(shí)間是由安裝在載體上的換能器發(fā)射詢問(wèn)信號(hào)至預(yù)先放置在已知位置的應(yīng)答器，然后應(yīng)答器發(fā)射應(yīng)答信號(hào)并被聲學(xué)基陣的各個(gè)水聽(tīng)器所接收這一過(guò)程確定的；考慮聲學(xué)系統(tǒng)的采樣與量化。

聲學(xué)基陣中各個(gè)水聽(tīng)器測(cè)量的聲波往返傳播時(shí)間為：

式中：tˉp為換能器至應(yīng)答器的標(biāo)稱傳播時(shí)間；tˉri為應(yīng)答器至第i個(gè)水聽(tīng)器的標(biāo)稱傳播時(shí)間；[·]Ts表示聲學(xué)系統(tǒng)的采樣量化公式；Ts為聲學(xué)系統(tǒng)的采樣周期。誤差項(xiàng)εt,εc分別表示詢問(wèn)信號(hào)和應(yīng)答信號(hào)的測(cè)量噪聲，εdi為水聽(tīng)器捕獲應(yīng)答信號(hào)時(shí)額外的微分誤差，它遠(yuǎn)小于時(shí)間序列中的共模誤差，應(yīng)答延遲時(shí)間td視為已知，可方便地從總測(cè)量時(shí)間中剔除。

Miller考慮載體在靜止或低速運(yùn)動(dòng)狀態(tài)下，忽略由于換能器和水聽(tīng)器與應(yīng)答器之間的相對(duì)運(yùn)動(dòng)所引起的詢問(wèn)信號(hào)和應(yīng)答信號(hào)傳播時(shí)間不同，簡(jiǎn)化后第i個(gè)水聽(tīng)器與應(yīng)答器之間的聲波傳播時(shí)間為：

式中：{tRTT}為測(cè)量的所有聲波RTT的平均值；它們之間的斜距測(cè)量值為：

式中：vp為水下聲波的傳播速度；ηc表示斜距的測(cè)量噪聲。

1.2.2 平面波近似法 超短基線定位系統(tǒng)求解應(yīng)答器的方向時(shí)，利用平面波分析法將水聲信號(hào)近似為平面波，如圖2所示，將三維波形轉(zhuǎn)化到二維平面上。

取聲學(xué)基陣的幾何中心O，第i個(gè)水聽(tīng)器距離幾何中心的位置矢量為bi∈R3，那么所有水聽(tīng)器的位置矢量滿足Σbi=0。

圖2 水聲信號(hào)平面波近似化

利用平面波近似法，任意兩個(gè)水聽(tīng)器之間的斜距差均可由位置矢量差和方向矢量d確定，即有：

依據(jù)整體估計(jì)誤差最小原則，使上式滿足：

式中：W為權(quán)值矩陣，利用加權(quán)最小二乘法求解方向矢量d為：

式中：B+為矩陣B的加權(quán)廣義逆矩陣，定義為B+(BTWB)-1BTW

對(duì)于某一應(yīng)答器和聲學(xué)基陣，確定一組線性無(wú)關(guān)的斜距差Δρ就能夠?qū)?yīng)計(jì)算出唯一的方向矢量。以聲學(xué)基陣的幾何中心為原點(diǎn)，結(jié)合式（9）計(jì)算的應(yīng)答器到聲學(xué)基陣的斜距，可得應(yīng)答器相對(duì)于超短基線定位系統(tǒng)的位置為：

1.3 USBL導(dǎo)航解算方程的坐標(biāo)轉(zhuǎn)換

式（14）是基于超短基線定位系統(tǒng)解算的，轉(zhuǎn)換到載體坐標(biāo)系時(shí)需要坐標(biāo)變換。取聲學(xué)基陣中水聽(tīng)器的幾何中心為超短基線定位系統(tǒng)坐標(biāo)系（u系）的原點(diǎn)，取oxtytzt為東北天坐標(biāo)系，u系的x,y,z三坐標(biāo)軸分別與載體坐標(biāo)系（b系）的坐標(biāo)軸平行（聲學(xué)基陣的偏移和安裝誤差角等誤差項(xiàng)為零時(shí)，u系與b系完全重合）。組合導(dǎo)航定位時(shí)，u系（b系）、t系和e系三者之間的關(guān)系如圖3所示，則應(yīng)答器的位置矢量為：

考慮到Ou位于聲學(xué)基陣的幾何中心，則在USBL坐標(biāo)系（u系）中水聽(tīng)器的位置矢量Ubi∈R3滿足：

圖3 USBL系統(tǒng)坐標(biāo)框圖

n為聲學(xué)基陣中水聽(tīng)器的數(shù)量，一般為4個(gè)或者8個(gè)。考察u系與b系之間的關(guān)系，假設(shè)Ux為一矢量x在u系中的表達(dá)式，為u系到b系的轉(zhuǎn)換矩陣，Bp0為u系的坐標(biāo)原點(diǎn)到b系坐標(biāo)原點(diǎn)的偏移量，則該矢量在b系中的表達(dá)式為：

公式變換為：

暫不考慮機(jī)動(dòng)狀態(tài)下，u系與b系未完全重合帶來(lái)的誤差因素，如受載體變形等影響，超短基線定位系統(tǒng)中第i個(gè)水聽(tīng)器對(duì)應(yīng)答器的定位斜距為：

Ur∈R3表示應(yīng)答器在u系中的位置矢量。

將式(18)代入到式(19)中解得：

bi∈R3表示第i個(gè)水聽(tīng)器在b系中的位置矢量。將式(15)代入式(20)解得：

2 INS/USBL組合導(dǎo)航系統(tǒng)的數(shù)學(xué)模型

2.1 INS/USBL松組合導(dǎo)航系統(tǒng)

（1）松組合導(dǎo)航系統(tǒng)的狀態(tài)方程

組合導(dǎo)航系統(tǒng)的狀態(tài)方程X˙=FX+W。

選取位置誤差 δL，δλ，δh、東北天方向上的速度誤差 δVE，δVN，δVU、東北天方向上的平臺(tái)誤差角 φE，φN，φU、沿載體坐標(biāo)系x,y,z軸上的陀螺儀的隨機(jī)漂移 εx,εy,εz和加速度計(jì)的零偏作為狀態(tài)量，F(xiàn)，W分別為INS系統(tǒng)矩陣、系統(tǒng)噪聲。

（2）松組合導(dǎo)航系統(tǒng)的量測(cè)方程

地球坐標(biāo)系（e系）中，載體位置信息表示為p=[L λ h]T，其與地球直角坐標(biāo)(x,y,z)關(guān)系為：

上式求解全微分得：

取INS與USBL的位置之差作為外部觀測(cè)量，由式（3）得超短基線定位系統(tǒng)估計(jì)應(yīng)答器的解算位置在載體坐標(biāo)系中表示為：

nr為USBL位置測(cè)量噪聲，向量形式如下：

在導(dǎo)航解算過(guò)程中，INS能夠估算出載體在地球坐標(biāo)系中的位置和旋轉(zhuǎn)矩陣，滿足：

INS估算出應(yīng)答器的位置信息在載體坐標(biāo)系中表示為：

其向量形式為：

由慣性導(dǎo)航系統(tǒng)的誤差方程得δp和R^RT，其中[φ×]為姿態(tài)誤差角三個(gè)分量構(gòu)成的反對(duì)稱矩陣：

將上式代入到式（28）忽略二階小量[φ×]δp得：

由式（24）、式（28）和式（32）可得 USBL與慣性導(dǎo)航系統(tǒng)解算的位置信息差值為：將地球坐標(biāo)系中的直角坐標(biāo)轉(zhuǎn)換為經(jīng)緯度和

高度坐標(biāo)，其向量形式為：

將其寫為矩陣等式則有：

建立量測(cè)方程Z(t)=H(t)X(t)+V(t)觀測(cè)矩陣如下:

2.2 INS/USBL緊組合導(dǎo)航系統(tǒng)

USBL緊耦合只需提供斜距、斜距差等原始信息，減少了位置和方位角的濾波解算，輸出信息的誤差模型簡(jiǎn)單且易于剔除，對(duì)組合濾波算法的濾波結(jié)果更有益。

泰勒展開，取前兩項(xiàng)得：

式中：ri，eix，fix如下，eiy，eiz，fiy，fiz同理。

USBL中第i(i≤n)個(gè)水聽(tīng)器距離第j個(gè)應(yīng)答器的斜距為：

斜距測(cè)量值之差為：

設(shè)聲學(xué)基陣中有4個(gè)水聽(tīng)器，則有：

外部量測(cè)量斜距差的量測(cè)方程為：

設(shè)第i(i≤n)個(gè)水聽(tīng)器距離第j個(gè)應(yīng)答器的斜距為 ρi，由式（21），（30）與（31）得：

引入列向量符號(hào)，記為：

對(duì)特定某個(gè)水聽(tīng)器進(jìn)行研究，將式（35）化為等式形式，可得：

量測(cè)噪聲為零均值高斯白噪聲，HIU如下：

考慮聲學(xué)基陣水聽(tīng)器之間的空間分布，第i(i≤n)個(gè)水聽(tīng)器距離第j個(gè)水聽(tīng)器的斜距之差為：

聲學(xué)基陣中有4個(gè)水聽(tīng)器之間的斜距差為：

則外部量測(cè)量水聽(tīng)器之間斜距差的量測(cè)方程為：

量測(cè)噪聲為零均值高斯白噪聲，HIJ如下：

3 數(shù)值仿真與分析

為驗(yàn)證文中算法有效性進(jìn)行如下仿真，導(dǎo)航誤差參數(shù)如表1所示。慣性導(dǎo)航系統(tǒng)的初始水平和方位失準(zhǔn)角分別為0.2°,0.2°和0.5°，考慮到濾波收斂速度，將初始速度誤差和位置誤差設(shè)為0；其中一個(gè)應(yīng)答器的位置坐標(biāo)設(shè)定為(0,200,0)，單位為m；USBL聲學(xué)基陣的空間分布，如圖4所示，u系的原點(diǎn)在b系的位置為Bp0=[0 0.3 0]T，4個(gè)水聽(tīng)器在u系中的位置為：

表1 導(dǎo)航系統(tǒng)的參數(shù)值

圖4 聲學(xué)基陣空間分布

松組合的量測(cè)噪聲矩陣的初值如下所示，緊組合時(shí)只需將其維數(shù)擴(kuò)展到m即可。

設(shè)定濾波解算周期為0.1 s，仿真時(shí)間為480 s，航行軌跡的初始位置為緯度 45.779 6°，東經(jīng)126.670 5°，分別對(duì)松組合和緊組合導(dǎo)航算法進(jìn)行仿真，理論航行軌跡如圖5所示。

圖5 航行軌跡

圖6 松組合導(dǎo)航系統(tǒng)的位置估計(jì)曲線

作為對(duì)比，松組合和緊組合導(dǎo)航系統(tǒng)的位置曲線由圖6～7所示，組合導(dǎo)航系統(tǒng)解算的姿態(tài)角、速度和位置誤差曲線分別如圖8～圖10所示。

圖7 緊組合導(dǎo)航系統(tǒng)的位置估計(jì)曲線

圖8 姿態(tài)誤差曲線

表2 姿態(tài)、速度和位置的誤差值

圖9 速度誤差曲線

圖10 位置誤差曲線

將上述仿真曲線的誤差值（RMS）進(jìn)行統(tǒng)計(jì)整理，如表2所示。

通過(guò)對(duì)以上仿真曲線和表格的分析可知：

（1）雖然受到各種噪聲和誤差等因素的影響，將超短基線定位系統(tǒng)的定位信息與INS解算信息融合后，松耦合與緊耦合有效地抑制了純INS速度和位置估計(jì)誤差隨時(shí)間逐漸累積發(fā)散問(wèn)題，且能有效地跟蹤載體位置信息，說(shuō)明該算法的有效性。

（2）系統(tǒng)輸出的姿態(tài)、速度和位置誤差參數(shù)均能得到大幅改善，兩種組合方法均能夠有效地估計(jì)出各個(gè)誤差角，從而提高整個(gè)導(dǎo)航系統(tǒng)的估計(jì)精度。

（3）通過(guò)對(duì)比圖6～圖7可以看出，緊耦合方法的位置估計(jì)相對(duì)實(shí)際航行軌跡波動(dòng)最小，濾波效果更加平穩(wěn)；從圖8～圖10以及表2的姿態(tài)、速度和位置誤差RMS值可以看出，緊組合方法的導(dǎo)航參數(shù)姿態(tài)、速度和位置的估計(jì)誤差更小，導(dǎo)航參數(shù)精度相對(duì)松組合平均提高30%以上，說(shuō)明基于考察USBL原始輸出導(dǎo)航參數(shù)的緊耦合方法對(duì)于進(jìn)一步提高組合導(dǎo)航系統(tǒng)定位精度具有重要意義。

4 結(jié)論

本文針對(duì)水下載體高精度導(dǎo)航定位問(wèn)題，結(jié)合水聲定位技術(shù)，基于倒置的超短基線聲學(xué)基陣，利用平面波近似法推導(dǎo)出應(yīng)答器相對(duì)USBL的定位方法。綜合采用INS/USBL組合導(dǎo)航方法，通過(guò)卡爾曼濾波器對(duì)導(dǎo)航誤差進(jìn)行估計(jì)，有效降低了導(dǎo)航參數(shù)的誤差，抑制了純慣導(dǎo)誤差積累及水下電磁波傳播能量衰減問(wèn)題。考察USBL的原始斜距、斜距差以及聲學(xué)基陣的空間分布信息并合理建立誤差模型，提出一種緊耦合方法以提高系統(tǒng)的動(dòng)態(tài)性能和抗干擾性。仿真結(jié)果表明，該耦合方法有效地估計(jì)出各導(dǎo)航參數(shù)誤差，精簡(jiǎn)了USBL應(yīng)答器裝置數(shù)目，降低硬件成本，且相對(duì)松組合方法導(dǎo)航精度平均至少提高30%以上，對(duì)于進(jìn)一步提高水下潛航器高精度導(dǎo)航定位，該算法具有重要的工程應(yīng)用意義和價(jià)值。針對(duì)考察USBL的斜距、斜距差等原始信息的緊耦合算法，在實(shí)際工程應(yīng)用中，可能會(huì)出現(xiàn)水下信號(hào)傳播時(shí)的多徑效應(yīng)，產(chǎn)生斜距量測(cè)異常值，在下一部分工作中，應(yīng)用基于Huber的M估計(jì)算法構(gòu)造偽觀測(cè)量提高系統(tǒng)魯棒性及穩(wěn)定性是迫切并具有實(shí)際需求意義。

[1]惠俊英,生雪莉.水下聲信道[M].北京:國(guó)防工業(yè)出版社,1992:1-29.

[2]劉帥.GPS/INS組合導(dǎo)航算法研究與實(shí)現(xiàn)[D].解放軍信息工程大學(xué),2012.

[3]陳坡.GNSS/INS深組合導(dǎo)航理論與方法研究[D].解放軍信息工程大學(xué),2013.

[4]李傳軍,彭鐘鋒,李興城.慣性導(dǎo)航系統(tǒng)自適應(yīng)輔助GNSS矢量跟蹤方法[J].探測(cè)與控制學(xué)報(bào),2014,36(5):74-79.

[5]張濤,徐曉蘇,李瑤.基于慣導(dǎo)及水下聲學(xué)輔助系統(tǒng)的AUV容錯(cuò)導(dǎo)航技術(shù)[J].中國(guó)慣性技術(shù)學(xué)報(bào),2013,21(4):512-516.

[6]張亞文,莫明崗,馬小艷,等.一種基于集中濾波的SINS/DVL/USBL水下組合導(dǎo)航算法[J].導(dǎo)航定位與授時(shí),2017,(1):25-31.

[7]Vickery K.Acoustic Positioning Systems.New Concepts-The Future[C]//Proceedings of the 1998 Workshop on Autonomous Underwater Vehicles,AUV’98,Cambridge,1998:103-110.

[8]Jalving B,Gade K,Hage O,et al.A Toolbox of Aiding Techniques for The HUGIN AUV Integrated Inertial Navigation System[C]//Proceedings ofthe OCEANS’03 MTS/IEEE,San Diego,2003:1146-1153.

[9]Morgado M,Oliveria P,Silvestre C.Tightly Coupled Ultra short Baseline and Inertial Navigation System for Underwater Vehicles:An Experimental Validation[J].Journal ofField Robotics,2013,30(1):142-170.

[10]Morgado M,Oliveria P,Silvestre C.Embedded Vehicle Dynamics Aiding for USBL/INS Underwater Navigation System[J].IEEE Transactions on Control Systems Technology,2014,22(1):322-330.

[11]李守軍,陶春輝,包更生.基于卡爾曼濾波的INS/USBL水下導(dǎo)航系統(tǒng)模型研究[J].海洋技術(shù),2008,27(3):47-50.

[12]張亞文，莫明崗，馬小艷，鄧?yán)^權(quán).三應(yīng)答器SINS/USBL緊耦合導(dǎo)航算法[J].導(dǎo)航定位與授時(shí),2015,2(3):29-33.

Research on INS/USBL Integrated Navigation Algorithm Based on the Inverted Acoustic Array

XU Bo,HAO Rui,WANG Chao,ZHANG Xun
College of Automation,Harbin Engineering University,Harbin 150001,Heilongjiang Province,China

This paper aims to address the problems occurred during underwater vehicle navigation,for example,electromagnetic wave is unable to be used as decay underwater,INS has accumulated mass error,and the precision of positioning of underwater vehicles needs to be improved.Based on the inverted acoustic array of Ultra Short Base Line(USBL),this paper thoroughly studies the return transfer time(RTT)of acoustic wave and the approximate method of plane wave.It also derives the positioning method of the transponder with respect to USBL.Combined with the INS error equation,the mathematical model of INS/USBL is established for integrated navigation system.In order to improve the accuracy,dynamic performance and anti-interference performance of the system,the INS/USBL compact combination technique is studied based on the original slant-range of USBL,slant-range difference and the spatial distribution information of acoustic array.The navigation algorithm is verified by MATLAB simulation,and the results show that two kinds of INS/USBL integrated navigation algorithms are able to inhibit filtering divergence of INS to effectively estimate the error angle.Tight combination method is more obvious for reducing the effect of navigation errors.It has more than 30%improvement in relative navigation parameters to loose integrated navigation.Therefore,it is of great significance for the navigation and positioning of underwater vehicles as well as for marine detection.

integrated navigation algorithm;inertial navigation system;Ultra Short Base Line (USBL);tight combination technique

V249.32+8

A

1003-2029（2017）05-0046-09

10.3969/j.issn.1003-2029.2017.05.008

2017-05-31

國(guó)家自然科學(xué)基金資助項(xiàng)目（61203225，61633008）；黑龍江省自然科學(xué)基金資助項(xiàng)目（QC2014C069）；中央高校專項(xiàng)基金資助項(xiàng)目（HEUCF041702）；海洋工程國(guó)家重點(diǎn)實(shí)驗(yàn)室開放課題（1616）資助

徐博（1982-），男，副教授，主要從事慣性導(dǎo)航、組合導(dǎo)航、初始對(duì)準(zhǔn)、信息融合研究。E-mail:xubocarter@sina.com

張勛，E-mail:zhangxun_hit@sina.com