信標(biāo)數(shù)量不固定的聲學(xué)定位組合導(dǎo)航方法及仿真

海軍潛艇學(xué)院航海觀通系 高大遠(yuǎn)

中圖分類(lèi)號(hào):TN967.2文獻(xiàn)標(biāo)識(shí)碼:A文章編號(hào):1007-9416(2023)02-0008-04

DOI:10.19695/j.cnki.cn12-1369.2023.02.02

收稿日期：2022-11-01

作者簡(jiǎn)介：高大遠(yuǎn)（1978—），男，甘肅民勤人，工學(xué)博士，副教授，研究方向：水下導(dǎo)航與組合導(dǎo)航。

針對(duì)水下航行器利用聲學(xué)定位時(shí)可能出現(xiàn)無(wú)法收到足夠數(shù)量信標(biāo)測(cè)距信息的問(wèn)題，提出一種基于單聲信標(biāo)測(cè)距和推算定位的組合導(dǎo)航方法。以航行器航速、航向信息為輸入，位置為狀態(tài)，信標(biāo)測(cè)距信息為輸出，建立組合導(dǎo)航濾波模型，采用擴(kuò)展卡爾曼濾波器進(jìn)行組合導(dǎo)航信息濾波。通過(guò)仿真對(duì)本方法應(yīng)用特性進(jìn)行驗(yàn)證分析。研究表明，本方法可以有效應(yīng)對(duì)信標(biāo)數(shù)量較少或航行器有效接收信標(biāo)應(yīng)答信號(hào)數(shù)量不固定的情況，具有良好的水下定位效果。

水下平臺(tái)航行或工作常采用水聲定位方法獲取位置信息。由于水聲定位方法其具有可控性好、精度高等特點(diǎn)，在海洋工程和科學(xué)考察活動(dòng)中得到廣泛應(yīng)用，目前已有較多貨架產(chǎn)品。其中長(zhǎng)基線定位系統(tǒng)（Long Base Location， LBL）具有定位范圍大、精度高的特點(diǎn)[1，2]。LBL定位原理與衛(wèi)星定位原理類(lèi)似，在海底布設(shè)至少3個(gè)聲信標(biāo)（一般4-6個(gè)），精確測(cè)量其位置作為空間基準(zhǔn)。在定位時(shí)，航行器安裝的聲學(xué)問(wèn)答機(jī)發(fā)射信號(hào)，海底聲信標(biāo)發(fā)射應(yīng)答信號(hào)，航行器根據(jù)信號(hào)往返的延時(shí)，算出距離信息，根據(jù)航行器到多個(gè)聲信標(biāo)的距離，即可解算其位置，實(shí)現(xiàn)導(dǎo)航定位。

但由于海底環(huán)境的制約，有時(shí)難以布設(shè)足夠數(shù)量的信標(biāo)；海洋環(huán)境的復(fù)雜性使得水下航行器也難以接收到所有信標(biāo)的應(yīng)答信號(hào)。在實(shí)際定位時(shí)，航行器與各信標(biāo)距離不同，也無(wú)法同時(shí)接收到所有應(yīng)答信號(hào)，考慮到航行器的動(dòng)態(tài)航行特性，此時(shí)利用應(yīng)答信號(hào)進(jìn)行解算帶來(lái)定位誤差。

針對(duì)上述情況，需要一種利用較少數(shù)量信標(biāo)的水聲定位方法，同時(shí)此方法還應(yīng)能適應(yīng)多個(gè)信標(biāo)以及信標(biāo)應(yīng)答信號(hào)不同時(shí)到達(dá)的情況。

對(duì)于此類(lèi)問(wèn)題，已有利用較少數(shù)量聲信標(biāo)（1或2個(gè)）進(jìn)行定位方法的研究[3，4]，基本思想是航行器通過(guò)在聲信標(biāo)周?chē)叫校@取航跡上多個(gè)點(diǎn)上聲信標(biāo)的距離，利用幾何方法解算航跡相對(duì)聲信標(biāo)的相對(duì)關(guān)系，從而計(jì)算航行器位置。這一方法，類(lèi)似匹配定位，是一種非實(shí)時(shí)的方法，對(duì)航跡約束較多，目前并未廣泛使用。

本文研究一種利用航行器推算定位信息與單聲信標(biāo)測(cè)距信息的組合導(dǎo)航定位方法。采用此方法，航行器在只接收到一個(gè)聲信標(biāo)應(yīng)答信號(hào)的情況下仍然可以進(jìn)行定位，在多個(gè)信標(biāo)組成的聲學(xué)定位基陣內(nèi)，可以根據(jù)所接接收的應(yīng)答信號(hào)分別進(jìn)行定位計(jì)算，不再要求接收到所有信號(hào)后才進(jìn)行定位計(jì)算，放松了對(duì)聲信標(biāo)數(shù)量的要求。這一方法對(duì)工程應(yīng)用具有一定的實(shí)用價(jià)值。

1 單信標(biāo)測(cè)距/推算定位組合導(dǎo)航建模

一般水聲定位方法利用航行器到多個(gè)信標(biāo)的測(cè)距信息，通過(guò)幾何解算計(jì)算航行器位置。當(dāng)信標(biāo)數(shù)量較少、測(cè)距信息存在誤差時(shí)，位置解算無(wú)法進(jìn)行或存在較大誤差。考慮到水下航行器一般帶有羅經(jīng)和計(jì)程儀等測(cè)向、測(cè)速設(shè)備，可進(jìn)行推算定位，因此通過(guò)建模可將二者結(jié)合實(shí)現(xiàn)定位。

1.1 推算定位模型

航行器水下航行，深度固定且已知，測(cè)量航速V和航向C，則可建立一般推算定位模型如式（1）、式（2）所示：

其中，λ、φ為航行器經(jīng)緯度，R為地球半徑，vN，vE分別為載體的北向速度和東向速度，如式（3）所示：

水聲定位時(shí)，問(wèn)答/應(yīng)答信號(hào)作用距離一般不超過(guò)十幾公里，在此局部范圍內(nèi)，可以建立平面直角坐標(biāo)系，航向定義為航行器與坐標(biāo)系y軸之間的夾角，則模型轉(zhuǎn)化如式（4）、式（5）所示：：

在考慮航速和航向測(cè)量誤差，并對(duì)方程離散化后，可得推算定位方程如下：

其中，ωv和ωc表示航行器速度和航向測(cè)量過(guò)程中的測(cè)量誤差，狀態(tài)(xk+1，yk+1)表示航行器當(dāng)前在直角坐標(biāo)系中的位置，(xk，yk)表示上一時(shí)刻航行器位置，ΔT為推算時(shí)間步長(zhǎng)。由于航速和航向誤差的存在，推算定位的誤差不斷增大，需要利用水聲定位信息校正。

1.2 水聲測(cè)距與定位模型

以4個(gè)信標(biāo)的水聲定位為例，仍然考慮平面直角坐標(biāo)系。假設(shè)海底信標(biāo)的位置坐標(biāo)為（xbk，ybk，zbk），k=1，2，3，4，航行器的位置坐標(biāo)為（x，y，z），測(cè)量得到的航行器到信標(biāo)距離為L(zhǎng)k，則可建立距離方程如式（8）所示：

進(jìn)行水聲定位時(shí)，利用多個(gè)信標(biāo)的測(cè)距信息，即可計(jì)算航行器位置[2]，進(jìn)而對(duì)推算定位信息進(jìn)行校正。

當(dāng)聲學(xué)定位系統(tǒng)信標(biāo)數(shù)量較多時(shí)，公式（8）中的方程數(shù)量增多，可充分利用冗余信息，得到精度較高的定位解；但當(dāng)可測(cè)距信標(biāo)數(shù)量較少時(shí)，方程的個(gè)數(shù)少于待求變量個(gè)數(shù)，則無(wú)法計(jì)算得到定位信息。此時(shí)需將推算定位模型公式（6）和公式（7）與水聲測(cè)距模型公式（8）結(jié)合，利用卡爾曼濾波等方法進(jìn)行組合導(dǎo)航對(duì)位置進(jìn)行最優(yōu)估計(jì)。

2 單信標(biāo)測(cè)距/推算定位組合導(dǎo)航濾波方法

如圖1所示，航行器利用航速、航向信息對(duì)位置進(jìn)行推算，由于存在測(cè)量誤差（常值誤差和隨機(jī)干擾），其定位誤差隨時(shí)間累積而不斷增加，導(dǎo)致位置信息不可信。當(dāng)其接近信標(biāo)，能夠測(cè)得信標(biāo)的距離時(shí)，若只能接收一個(gè)或較少數(shù)量的信標(biāo)信息，則無(wú)法單獨(dú)完成定位計(jì)算。此時(shí)，考慮推算定位已經(jīng)給出了部分位置信息，將其充分利用，則可實(shí)現(xiàn)利用單信標(biāo)測(cè)距信息進(jìn)行定位。

圖1 單信標(biāo)測(cè)距/推算定位組合導(dǎo)航示意圖Fig.1 Combined navigation method based on single beacon/dead reckon

將公式（6）、公式（7）、公式（8）進(jìn)行結(jié)合，構(gòu)成組合導(dǎo)航模型，利用擴(kuò)展卡爾曼濾波方法可實(shí)現(xiàn)組合導(dǎo)航定位，方法如下。

首先將模型進(jìn)行重寫(xiě)，對(duì)某一時(shí)刻k，令狀態(tài)和輸入如式（9）所示：

輸出如式（10）所示：

其中（xb，yb，zb）是單信標(biāo)的位置坐標(biāo)。

則過(guò)程模型如式（11）、式（12）所示：

對(duì)于式（11）、式（12）來(lái)說(shuō)，需在實(shí)際航跡附近線性化，計(jì)算雅可比（Jacob）矩陣。

對(duì)于輸入，雅可比矩陣計(jì)算公式如式（13）所示：

對(duì)于測(cè)量方程，其雅可比矩陣可寫(xiě)為如式（14）所示：

對(duì)于上述模型，利用擴(kuò)展卡爾曼濾波進(jìn)行信息處理。

假設(shè)在第k時(shí)刻，已得到對(duì)當(dāng)前狀態(tài)的估計(jì)值，估計(jì)方差陣為Pk，當(dāng)測(cè)量到航向、航速后，則利用過(guò)程模型公式（11）進(jìn)行狀態(tài)的一步預(yù)測(cè)，得到如式（15）所示：

方差的一步預(yù)測(cè)如式（16）所示：

其中，Qk是隨機(jī)干擾[ωVkωCk]的方差。

然后，基于狀態(tài)的一步預(yù)測(cè)，利用公式（10）對(duì)時(shí)延測(cè)量信息進(jìn)行預(yù)測(cè)，記為Zk+1。

當(dāng)水聲定位系統(tǒng)檢測(cè)到應(yīng)答信號(hào)，給出實(shí)測(cè)距離信息，則可進(jìn)行信息修正，對(duì)本時(shí)刻狀態(tài)和方差進(jìn)行估計(jì)，公式如式（17）、式（18）所示：

其中，Kk+1是增益矩陣，如式（19）所示：

其中Rk+1是距離測(cè)量誤差的方差。

這樣公式（15）-公式（19）即單聲信標(biāo)的組合導(dǎo)航卡爾曼濾波處理過(guò)程，實(shí)現(xiàn)航行位置及其誤差方差的估計(jì)。

當(dāng)有多個(gè)信標(biāo)時(shí)，既可以將公式（12）和公式（14）改造為多維表達(dá)式（維數(shù)等于信標(biāo)個(gè)數(shù)），同樣采用卡爾曼濾波計(jì)算；也可以仍然采用單信標(biāo)組合導(dǎo)航方法，在每一次測(cè)量得到某一信標(biāo)的距離信息時(shí)，即與實(shí)時(shí)推算定位信息進(jìn)行組合導(dǎo)航計(jì)算，而不用等到所有信標(biāo)距離信息均獲得以后再進(jìn)行計(jì)算，這樣放松了水聲定位對(duì)信標(biāo)數(shù)量的要求，實(shí)現(xiàn)了可用信標(biāo)數(shù)量不固定情況下的水聲定位。

從此過(guò)程可以看出，通過(guò)將推算定位模型與單信標(biāo)測(cè)距模型相結(jié)合，采用卡爾曼濾波的預(yù)測(cè)-校正體制信息處理方法，實(shí)現(xiàn)了推算定位信息與單信標(biāo)測(cè)距信息的耦合，達(dá)到了僅使用單信標(biāo)距離測(cè)量信息也可實(shí)現(xiàn)水聲定位的功能。

3 仿真分析

對(duì)上述過(guò)程進(jìn)行仿真分析，考慮如下兩種情況。

（1）單信標(biāo)情況，仿真條件如下：信標(biāo)位置為（-2000，-3000，-1000），單位為米（m,下同），稱(chēng)為信標(biāo)1。航行器初始位置為（-3000，-1000，-300），航行器水平航行，航速2m/s（約3.9節(jié)），航向120度。航行推算定位，初始位置誤差設(shè)置為x，y方向具為方差為500m的高斯白噪聲，航向測(cè)量誤差包括1度的常值誤差和方差為0.5度的高斯白噪聲；航速測(cè)量誤差包括0.1m/s的常值誤差和方差為0.1m/s的高斯白噪聲。信標(biāo)距離測(cè)量誤差為方差5m的白噪聲。仿真結(jié)果如圖2、圖3所示。

圖2 航行器定位軌跡(單信標(biāo)情況)Fig.2 Positioning trajectory of UUV（single beacon）

圖3 導(dǎo)航定位誤差曲線(單信標(biāo)情況)Fig.3 Error of position（single beacon）

從仿真圖可以看出，對(duì)于近600m的初始位置誤差，在推算定位中，定位誤差緩慢增加，而組合導(dǎo)航可使位置誤差迅速減小，隨著測(cè)量不斷進(jìn)行，組合導(dǎo)航定位誤差不斷減小，最終定位誤差穩(wěn)定小于5m。

（2）有多個(gè)信標(biāo)的情況，假設(shè)有2個(gè)信標(biāo)（多于2個(gè)信標(biāo)的水聲定位效果比較理想，這里不考慮），另一信標(biāo)位置為（-2500，-1000，-800），稱(chēng)為信標(biāo)2。航行器航向135度，速度2m/s。假設(shè)在航行前600s和后600s，分別只接收信標(biāo)1和信標(biāo)2的信號(hào)，在航行過(guò)程中間600s，受海洋環(huán)境的影響，航行器以等概率隨機(jī)接收信號(hào)1、信號(hào)2或2個(gè)信標(biāo)的信號(hào)，仿真結(jié)果如圖4-圖6所示。

從圖4和圖5可以看出，在可接收信號(hào)的信標(biāo)數(shù)量不固定的情況下，利用單信標(biāo)測(cè)距和推算定位組合導(dǎo)航方法，仍然能夠有效地對(duì)航行器進(jìn)行定位，定位誤差逐漸收斂。

圖4 航行器定位軌跡圖Fig.4 Positioning trajectory of UUV

從圖6可以看出，航行中間階段，航行器接收信標(biāo)信號(hào)的情況具有隨機(jī)性，對(duì)照?qǐng)D5中相應(yīng)時(shí)間段的誤差曲線，可以看出，采用本文方法，在接收信標(biāo)信號(hào)具有隨機(jī)性、可用信標(biāo)數(shù)量不固定的情況下，定位誤差依然能夠較好的收斂。

圖5 導(dǎo)航定位誤差曲線Fig.5 Error of position

圖6 航行器接收信標(biāo)信號(hào)的情況Fig.6 Beacon receive condition of UUV

在多次仿真中還發(fā)現(xiàn)，航行器的航跡對(duì)組合導(dǎo)航定位結(jié)果有一定影響，特別是當(dāng)只能接收一個(gè)信標(biāo)信號(hào)時(shí)。對(duì)照?qǐng)D3和圖5的初始階段，可以看出第二情況仿真時(shí)，由于航跡更靠近信標(biāo)1，導(dǎo)致定位誤差收斂相對(duì)較慢。這一現(xiàn)象可從控制理論對(duì)其進(jìn)行解釋[5]，是因?yàn)樵诖撕桔E情況下，系統(tǒng)的可觀測(cè)性較弱，因此組合導(dǎo)航定位誤差收斂較慢，在進(jìn)入第二階段后，可收到不同信標(biāo)信號(hào)或可收到2個(gè)信標(biāo)的信號(hào)時(shí)，收斂速度加快，效果改善。

4 結(jié)語(yǔ)

本文所提出基于單信標(biāo)測(cè)距信息/推算定位的組合導(dǎo)航方法，對(duì)于單信標(biāo)聲學(xué)定位具有較好效果。在實(shí)際水下聲學(xué)定位的應(yīng)用過(guò)程中，受海水聲波傳輸特性復(fù)雜的影響，聲信號(hào)接收具有較大的不確定性，容易導(dǎo)致可接收信號(hào)的信標(biāo)數(shù)量不夠、信號(hào)接收不同步等情況。本文的方法具有一定的通用性，可以有效應(yīng)對(duì)這些情況，具有較大的應(yīng)用價(jià)值。

引用

[1]付進(jìn).長(zhǎng)基線定位信號(hào)處理若干關(guān)鍵技術(shù)研究[D].哈爾濱:哈爾濱工程大學(xué),2007.

[2]田坦.水下定位與導(dǎo)航技術(shù)[M].北京:國(guó)防工業(yè)出版社,2007.

[3]張福斌,張永清.一種利用單信標(biāo)修正AUV定位誤差的方法[J].魚(yú)雷技術(shù),2012,20(1):38-41.

[4]曹俊.基于單信標(biāo)測(cè)距的水下載體定位研究[D].哈爾濱:哈爾濱工程大學(xué),2017.

[5]嚴(yán)衛(wèi)生,房新鵬,崔榮鑫.單信標(biāo)測(cè)距AUV水下定位系統(tǒng)觀測(cè)性分析[J].中南大學(xué)學(xué)報(bào)(自然科學(xué)版),2012,43(8):3085-3090.