基于未知聲速的水下三維單信標(biāo)定位模型

朱仲本，田瑞菊，許培龍

(哈爾濱工程大學(xué) 船舶工程學(xué)院，黑龍江 哈爾濱 150001)

0 引 言

海洋因其重要的戰(zhàn)略地位和豐富的礦物資源成為研究的熱點(diǎn)，水下無人航行器越來越受到重視。水下無人航行器可搭載多種傳感器、專用設(shè)備或武器模塊，在軍事領(lǐng)域廣泛應(yīng)用于水下監(jiān)視偵察，收集情報(bào)，探雷、獵雷和滅雷，跟蹤打擊等諸多領(lǐng)域，在將來海洋戰(zhàn)爭中具有重要地位。另外，無人航行器還可用于海底打撈、海底資源勘探和開發(fā)等，具有非常重要的民用價(jià)值。水下無人航行器的精準(zhǔn)定位對于確保這些應(yīng)用所收集數(shù)據(jù)的準(zhǔn)確性至關(guān)重要，水下定位技術(shù)可以保證航行器獲得實(shí)時(shí)位置信息，是水下航行器進(jìn)行水下科研或生產(chǎn)活動的關(guān)鍵技術(shù)，是其順利完成既定工程任務(wù)的保障。

水下定位技術(shù)主要包括慣性導(dǎo)航定位技術(shù)[1-2]和聲學(xué)導(dǎo)航技術(shù)。聲學(xué)導(dǎo)航技術(shù)包括長基線(Long Base Line，LBL)定位系統(tǒng)[3]、短基線(Short Base Line，SBL)定位系統(tǒng)[4]、超短基線(Ultra Short Base Line，USBL)定位系統(tǒng)[5]及水下單信標(biāo)定位系統(tǒng)[6]等。現(xiàn)有的水下定位技術(shù)能夠滿足各種定位要求,但也存在缺點(diǎn)。例如：將LBL定位系統(tǒng)應(yīng)答器設(shè)置海底是一個(gè)耗時(shí)過程；USBL定位系統(tǒng)典型的定位精度為深度的1%～2%；慣性導(dǎo)航定位系統(tǒng)成本較高，并且在每次部署之前需進(jìn)行對準(zhǔn)工作。相比于其他定位技術(shù)，水下單信標(biāo)定位具有安裝使用簡單、成本低、便于遠(yuǎn)距離拓展等優(yōu)點(diǎn)，在水下航行器定位上具有廣闊的應(yīng)用前景。水下單信標(biāo)定位技術(shù)主要通過測量傳播時(shí)間獲得已知位置的水聲信標(biāo)與航行器或待定位目標(biāo)之間的地理斜距，利用距離信息和航行器本身的慣性傳感器信息經(jīng)過一定濾波算法獲得航行器水下位置信息。

傳統(tǒng)的水下單信標(biāo)定位模型通常考慮航行器在二維運(yùn)動狀態(tài)下建立動力學(xué)模型，但實(shí)際過程中航行器在三維狀態(tài)下運(yùn)動，因此考慮在三維狀態(tài)下基于卡爾曼濾波的水下單信標(biāo)定位五狀態(tài)變量模型(State Variables Model,SVM)[7](簡稱“5-SVM”)，通過引入水深構(gòu)建一種新型水下單信標(biāo)定位模型，主要包括其運(yùn)動學(xué)模型、量測模型及過程噪聲模型。同時(shí)通過仿真數(shù)據(jù)，利用卡爾曼濾波輸出對比新型水下單信標(biāo)定位模型與5-SVM，評估2種模型的位置估計(jì)精度。

1 定位模型構(gòu)建

1.1 水下單信標(biāo)定位5-SVM

考慮海洋航行器二維運(yùn)動情況，建立笛卡爾坐標(biāo)系，如圖1所示。以水聲信標(biāo)所在位置為坐標(biāo)原點(diǎn)，建立水平面內(nèi)坐標(biāo)系xOy，航行器位于(x，y)點(diǎn)時(shí)，其相對于水的運(yùn)動速度為vw，航向角為ψ，海流速度為vc，有效聲速(Effective Sound Velocity,ESV)為ve。

圖1 二維笛卡爾坐標(biāo)系

以水的運(yùn)動速度vw、航向角ψ為控制變量，航行器位置坐標(biāo)(x,y)、海流速度vc及ESVve為狀態(tài)變量，建立基于卡爾曼濾波的傳統(tǒng)水下單信標(biāo)定位模型，其運(yùn)動方程為

(1)

式(1)可進(jìn)一步簡寫為

(2)

式中：

式(2)為靜態(tài)線性時(shí)不變方程，其通解[8]為

x(t)=exp[F(t-t0)]x(t0)+

(3)

將式(3)進(jìn)行離散處理，令Δt=t-t0，得：

xk=Ak-1xk-1+Bk-1uk-1+wk-1

(4)

式中：xk為系統(tǒng)狀態(tài)變量；Ak-1為狀態(tài)轉(zhuǎn)移矩陣；Bk-1為系統(tǒng)控制項(xiàng)系數(shù)矩陣；uk-1為系統(tǒng)控制項(xiàng)矩陣；wk-1為系統(tǒng)的隨機(jī)動態(tài)噪聲。其中：

式(4)中在tk時(shí)刻的離散狀態(tài)變量xk記為

(5)

系統(tǒng)相應(yīng)的過程噪聲項(xiàng)wk記為

wk=[wx,k-1wy,k-1wcx,k-1wcy,k-1we,k-1]T

(6)

采用水聲信號傳播時(shí)間為系統(tǒng)量測變量，即

mk=Tk,t+vt,k

(7)

式中：mk為水聲信號傳播時(shí)間的量測值；Tk,t為水聲傳播時(shí)間；vt,k為該水聲信號傳播時(shí)間的測量噪聲。

式(7)可表達(dá)為系統(tǒng)狀態(tài)變量x中xk、yk、zk的函數(shù)，記為hk，其表達(dá)式為

(8)

式中：(xb,yb,zb)為單信標(biāo)的位置坐標(biāo)；ve,k為k時(shí)刻的有效聲速。此處航行器的深度zk由深度傳感器測量得到，視為已知量。

(9)

5-SVM水下單信標(biāo)定位模型噪聲項(xiàng)如式(6)所示，將5個(gè)噪聲變量分為3組：(1)wx,k-1和wy,k-1為在x和y方向的位置變化率噪聲；(2)wcx,k-1和wcy,k-1為在x和y方向的海流變化率噪聲；(3)we,k-1為ESV變化率噪聲。假設(shè)這3組噪聲變量相互獨(dú)立，則噪聲向量wk對應(yīng)的離散方差矩陣Qk為

(10)

式中：Qp,k為wx,k和wy,k的協(xié)方差矩陣(wx,k和wy,k相關(guān))；Qc,k為wcx,k和wcy,k的協(xié)方差矩陣(假設(shè)wcx,k和wcy,k不相關(guān))；Qe,k為we,k的方差。

將系統(tǒng)狀態(tài)變量x中x和y進(jìn)行離散，得：

(11)

進(jìn)而可得wx,k和wy,k的協(xié)方差矩陣Qk，將Qp,k、Qc,k和Qe,k代入式(10)，得：

(12)

海流速度量測值表示為

(13)

式中：vcx,k和vcy,k分別為k時(shí)刻海流速度在x軸和y軸的分量；υcx,k和υcy,k分別為海流量測噪聲在x軸和y軸的分量。

海流速度相應(yīng)量測矩陣為

(14)

1.2 新型水下單信標(biāo)定位模型

將深度作為新的狀態(tài)變量，在5-SVM基礎(chǔ)上進(jìn)行拓展得出6-SVM。

考慮海洋航行器三維運(yùn)動情況，建立三維坐標(biāo)系Oxyz，以水的運(yùn)動速度vw、航向角ψ為控制變量，航行器位置坐標(biāo)(x,y,z)、水流速度vcx、vcy及ESVve為狀態(tài)變量，建立基于卡爾曼濾波的水下單信標(biāo)定位6-SVM，其運(yùn)動方程為

(15)

式中：z為航行器深度。

將方程的解進(jìn)行離散，得到與式(4)相同的遞推關(guān)系式，狀態(tài)轉(zhuǎn)移矩陣、控制項(xiàng)系數(shù)矩陣和控制項(xiàng)矩陣分別為

在tk時(shí)刻的離散狀態(tài)變量記為

(16)

系統(tǒng)的過程噪聲項(xiàng)記為

wk=[wx,k-1wy,k-1wz,k-1wcx,k-1wcy,k-1we,k-1]T

(17)

忽略載體自身的漂角，載體和水的相對速度計(jì)算公式為

vw=Rb,nvb

(18)

體坐標(biāo)系相對導(dǎo)航坐標(biāo)系的橫滾角、縱傾角和航向角，通過姿態(tài)航向基準(zhǔn)系統(tǒng)(Attitude Heading Reference System ,AHRS)測得。

采用水聲信號傳播時(shí)間為系統(tǒng)量測變量，擴(kuò)展卡爾曼濾波量測方程如式(7)所示。hk表達(dá)式如式(8)所示。

(19)

所設(shè)計(jì)的新型水下單信標(biāo)定位模型噪聲項(xiàng)如式(17)所示，將6個(gè)噪聲變量分為3組：(1)wx,k-1、wy,k-1、wz,k-1為在x、y和z方向的位置變化率噪聲；(2)wcx,k-1和wcy,k-1為在x和y方向的海流變化率噪聲；(3)we,k-1為ESV變化率噪聲。假設(shè)這3組噪聲變量相互獨(dú)立，則噪聲向量wk對應(yīng)的離散方差矩陣Q′k為

(20)

式中：Q′p,k為wx,k、wy,k、wz,k的協(xié)方差矩陣(wx,k、wy,k、wz,k相關(guān))；Q′c,k為wcx,k和wcy,k的協(xié)方差矩陣(假設(shè)wcx,k和wcy,k不相關(guān))；Q′e,k為we,k的方差。

將系統(tǒng)狀態(tài)變量x中的x、y、z進(jìn)行離散，得：

(21)

海流速度在z軸方向無分量。

(22)

海流速度量測值與式(13)相同。

海流速度相應(yīng)量測矩陣為

(23)

2 仿真分析

為驗(yàn)證所提出的6-SVM，利用該模型基于仿真數(shù)據(jù)進(jìn)行數(shù)值模擬。基于定常的ESV情形，使用數(shù)值仿真數(shù)據(jù)對比5-SVM和6-SVM兩種水下單信標(biāo)定位模型。水聲傳播時(shí)間的量測噪聲標(biāo)準(zhǔn)差設(shè)置為0.000 1 s；海流在x軸和y軸方向的量測噪聲標(biāo)準(zhǔn)差均為0.000 1 m/s；深度計(jì)量測噪聲標(biāo)準(zhǔn)差為0.000 1 m；真實(shí)聲速設(shè)置為1 530 m/s，海流速度在x和y方向的分量均為恒定值0.3 m/s；水聲信標(biāo)每隔10 s發(fā)出1次水聲信號，進(jìn)行1次測距更新。采用擴(kuò)展卡爾曼濾波進(jìn)行航行器位置估計(jì)時(shí)，設(shè)置航行器在x、y、z方向的位置偏差均為10 m，初始ESV設(shè)為1 520 m/s。系統(tǒng)的隨機(jī)動態(tài)噪聲設(shè)置如下：假設(shè)vw在x、y、z方向的過程噪聲標(biāo)準(zhǔn)差σwx、σwy、σwz均設(shè)置為0.1 m/s，vc在x、y方向的過程噪聲標(biāo)準(zhǔn)差σcx和σcy均設(shè)置為0.01 m/s，設(shè)置ESV的過程噪聲σe為0.1 m/s。當(dāng)σe為0.1 m/s時(shí)，需要較長的時(shí)間才可以達(dá)到ESV真實(shí)值，但之后估計(jì)誤差基本趨于0。在ESV設(shè)置為定常值時(shí)，選擇較小的σe，可以得到較好的ESV仿真結(jié)果。σe為0.1 m/s時(shí)采用6-SVM進(jìn)行擴(kuò)展卡爾曼濾波對ESV的估計(jì)結(jié)果(見圖2)，500 s后估計(jì)結(jié)果趨于真實(shí)值且不再發(fā)散。

圖2 ESV估計(jì)

圖3 模型真實(shí)軌跡

圖4 航行器軌跡對比

圖5 定位誤差對比

圖6 6-SVM測距新息和時(shí)延新息

3 結(jié) 語

建立的新型水下單信標(biāo)定位6-SVM將水深作為狀態(tài)變量，以水聲傳播時(shí)間和海流速度作為量測變量，利用擴(kuò)展卡爾曼濾波進(jìn)行航行器位置估算，建立相應(yīng)的過程噪聲模型。同時(shí)，基于仿真數(shù)據(jù)，進(jìn)行6-SVM與5-SVM的數(shù)值仿真，對比2種模型的定位軌跡及定位誤差。數(shù)值仿真顯示新型單信標(biāo)定位模型具有更好的ESV實(shí)時(shí)估計(jì)能力，且具有更高的定位精度。