趙婧旭 趙 晨 周 鋒*

①(哈爾濱工程大學(xué)水聲技術(shù)重點(diǎn)實(shí)驗(yàn)室 哈爾濱 150001)

②(海洋信息獲取與安全工信部重點(diǎn)實(shí)驗(yàn)室(哈爾濱工程大學(xué))工業(yè)和信息化部 哈爾濱 150001)

③(哈爾濱工程大學(xué)水聲工程學(xué)院 哈爾濱 150001)

④(北京遙感設(shè)備研究所 北京 100854)

1 引言

海洋有著豐富的礦產(chǎn)資源，長(zhǎng)期以來一直是世界關(guān)注的焦點(diǎn)。認(rèn)識(shí)海洋以及海洋資源的研究離不開水下高技術(shù)手段。人力無(wú)法進(jìn)行對(duì)深海大規(guī)模、復(fù)雜的開發(fā)和調(diào)查工作[1-4]，這就需要強(qiáng)而有效的水下裝備以及水下目標(biāo)定位系統(tǒng)的支持。近年來，根據(jù)海洋研究和開發(fā)的需求，水下目標(biāo)定位系統(tǒng)需要具備長(zhǎng)時(shí)間、大范圍和遠(yuǎn)距離的特點(diǎn)。傳統(tǒng)水下目標(biāo)定位系統(tǒng)包括長(zhǎng)基線系統(tǒng)、短基線系統(tǒng)、超短基線系統(tǒng)及綜合定位系統(tǒng)，是以“基線”為基礎(chǔ)對(duì)目標(biāo)與系統(tǒng)之間的聲信號(hào)傳播特征進(jìn)行采集、處理的方法。傳統(tǒng)水下目標(biāo)定位系統(tǒng)需要通過布置于海底的應(yīng)答器、目標(biāo)與船體之間的通信完成定位過程，實(shí)現(xiàn)較為復(fù)雜，且海底應(yīng)答器布放易受到環(huán)境影響，不適用于未知區(qū)域中的目標(biāo)定位。

水聲通信技術(shù)的發(fā)展使得水下自主航行器(Autonomous Underwater Vehicle, AUV)集群作業(yè)在海洋環(huán)境監(jiān)測(cè)、海洋資源開發(fā)利用以及海洋國(guó)防安全等領(lǐng)域呈現(xiàn)出重要的應(yīng)用價(jià)值。所以在水下探索、勘探等作業(yè)場(chǎng)景中，能夠?qū)崿F(xiàn)高精度位置協(xié)同的AUV編隊(duì)對(duì)水下目標(biāo)定位有重大意義。水下AUV協(xié)同定位分為并行式和主從式兩種方法。并行式AUV協(xié)同定位系統(tǒng)中，AUV編隊(duì)由相同結(jié)構(gòu)和功能的AUV組成，通過相鄰AUV間的信息交流獲取位置觀測(cè)值并進(jìn)行定位校準(zhǔn)。主從式AUV協(xié)同定位系統(tǒng)中，AUV編隊(duì)由多個(gè)裝配有高精度和低精度慣性導(dǎo)航系統(tǒng)的AUV組成，通過領(lǐng)航AUV向跟隨AUV傳遞相對(duì)距離信息的方式進(jìn)行定位。相比于并行式AUV協(xié)同定位系統(tǒng)，主從式AUV協(xié)同定位系統(tǒng)能夠兼顧導(dǎo)航精度與設(shè)備成本，廣泛應(yīng)用于實(shí)際場(chǎng)景中[5-9]。

多AUV編隊(duì)系統(tǒng)的精度與對(duì)目標(biāo)的定位精度直接相關(guān)。一些提高AUV集群協(xié)同定位精度的方法也逐漸被提出，文獻(xiàn)[10]提出一種基于迭代擴(kuò)展卡爾曼濾波的建議分布和線性優(yōu)化重采樣的快速同步定位與構(gòu)圖(Simultaneous Localization And Mapping, SLAM)方法，提高了AUV及路標(biāo)的位置估計(jì)精度。文獻(xiàn)[11]應(yīng)用了一種能自動(dòng)學(xué)習(xí)卡爾曼濾波噪聲協(xié)方差參數(shù)的方法，通過仿真分析，進(jìn)一步提高了單領(lǐng)航者AUV協(xié)同導(dǎo)航系統(tǒng)的導(dǎo)航精度。文獻(xiàn)[6]提出了一種基于增廣擴(kuò)展卡爾曼濾波的定位算法，以解決由于測(cè)量時(shí)間延遲而導(dǎo)致的定位失效問題。文獻(xiàn)[12]將AUV按照導(dǎo)航精度高低分層，高精度層的AUV向低精度層的AUV傳遞導(dǎo)航信息，建立AUV的數(shù)學(xué)模型并采用擴(kuò)展卡爾曼濾波(Extended Kalman Filter, EKF)融合內(nèi)外部導(dǎo)航信息，對(duì)低精度層AUV的位置狀態(tài)估計(jì)更新。

針對(duì)現(xiàn)有的主從式多AUV編隊(duì)目標(biāo)定位系統(tǒng)存在的領(lǐng)航AUV無(wú)法對(duì)多個(gè)跟隨AUV定位、跟隨AUV位置計(jì)算傳遞信息過多的問題，本文提出一種基于主從式AUV移動(dòng)組網(wǎng)協(xié)同的定位方案。AUV集群由一個(gè)高精度AUV與若干低精度AUV組成。高精度AUV對(duì)部分低精度AUV進(jìn)行基于EKF的位置修正并將其轉(zhuǎn)化為參考AUV，其余低精度AUV為待測(cè)AUV。參考AUV負(fù)責(zé)修正待測(cè)AUV的位置信息。在一個(gè)定位周期中，通過在不同時(shí)刻選取不同觀測(cè)值的方法來對(duì)低精度AUV進(jìn)行位置修正，一周期結(jié)束后即可開啟目標(biāo)定位。本方法可以實(shí)時(shí)繪制水下多AUV編隊(duì)的位置網(wǎng)絡(luò)，同時(shí)不需要AUV集群保持一定的相對(duì)運(yùn)動(dòng)，增加了AUV集群的靈活性，可以進(jìn)行水下目標(biāo)的定位。

2 主從式多AUV編隊(duì)目標(biāo)定位系統(tǒng)

主從式多AUV編隊(duì)目標(biāo)定位系統(tǒng)(以下簡(jiǎn)稱AUV定位系統(tǒng))是指利用水下主從式AUV集群對(duì)水中目標(biāo)進(jìn)行定位的系統(tǒng)。水下AUV集群首先修正自身位置信息，再運(yùn)用基于信號(hào)傳播時(shí)延測(cè)量(Time-Of-Arrival, TOA)的多邊交匯法對(duì)給定范圍的海域中目標(biāo)進(jìn)行掃描定位。圖1為AUV定位系統(tǒng)示意圖。

圖1 AUV水下定位系統(tǒng)示意圖

由于水下作業(yè)過程中，AUV可以通過壓力傳感器得到高精度的深度數(shù)據(jù)。在AUV集群運(yùn)動(dòng)深度保持一致的情況下，可以將3維運(yùn)動(dòng)模型簡(jiǎn)化為2維運(yùn)動(dòng)模型。圖2為AUV的2維運(yùn)動(dòng)模型。

圖2 AUV移動(dòng)示意圖

3 基于EKF的多AUV編隊(duì)融合觀測(cè)協(xié)同目標(biāo)定位方法

針對(duì)AUV定位系統(tǒng)中，AUV編隊(duì)自身位置修正中，領(lǐng)航AUV與跟隨AUV傳遞信息量過大、無(wú)法對(duì)多個(gè)跟隨AUV進(jìn)行定位問題，本文提出一種基于擴(kuò)展卡爾曼濾波的融合觀測(cè)協(xié)同定位法，可以實(shí)現(xiàn)多AUV之間的協(xié)同定位以及對(duì)目標(biāo)定位。本方法在原有AUV定位系統(tǒng)進(jìn)行改進(jìn)，以定位周期為單位，對(duì)AUV編隊(duì)的運(yùn)動(dòng)進(jìn)行觀測(cè)以及修正，并在一個(gè)周期結(jié)束后利用球面多邊交匯定位方法對(duì)水下目標(biāo)進(jìn)行定位。AUV編隊(duì)構(gòu)成情況為：由1個(gè)裝備有高精度慣性導(dǎo)航系統(tǒng)(Inertial Navigation System, INS)的領(lǐng)航AUV以及多個(gè)裝配有低精度INS的跟隨AUV。采用兩種基于不同觀測(cè)方法的擴(kuò)展卡爾曼濾波器對(duì)低精度AUV進(jìn)行位置修正，分別為速度觀測(cè)法和速度距離雙觀測(cè)法。在1個(gè)定位周期中，根據(jù)上一定位周期結(jié)束時(shí)各低精度AUV與高精度AUV距離選定參考AUV，其余為待測(cè)AUV。AUV優(yōu)先級(jí)為高精度AUV、參考AUV、待測(cè)AUV，即參考AUV接收來自高精度AUV的位置信息，待測(cè)AUV接收來自參考AUV的位置信息。全部低精度AUV在沒有接收來自上一級(jí)AUV的測(cè)距信號(hào)時(shí)，均采用速度觀測(cè)法進(jìn)行定位。到達(dá)測(cè)距時(shí)間以及接受到來自上一級(jí)AUV的測(cè)距信號(hào)后，開啟基于速度距離雙觀測(cè)法進(jìn)行定位。圖4為AUV集群一周期內(nèi)觀測(cè)流程示意圖。

圖4 基于EKF的融合觀測(cè)協(xié)同定位法流程圖

3.1 恒定航向角速度和速度幅度模型

3.2 基于EKF的多AUV編隊(duì)融合觀測(cè)協(xié)同目標(biāo)定位算法實(shí)現(xiàn)

基于EKF的多AUV編隊(duì)融合觀測(cè)協(xié)同目標(biāo)定位算法(以下簡(jiǎn)稱AUV編隊(duì)融合定位算法)對(duì)AUV的觀測(cè)有速度觀測(cè)法和速度距離雙觀測(cè)法。如圖4，采取兩種觀測(cè)方法以時(shí)間為節(jié)點(diǎn)交替進(jìn)行。測(cè)速觀測(cè)法是AUV通過加速度計(jì)與陀螺儀測(cè)得自身速度、航向角速度，得到k時(shí)刻觀測(cè)矩陣為

利用式(13)計(jì)算估計(jì)誤差協(xié)方差矩陣，以供下一時(shí)刻使用。待AUV編隊(duì)如圖3所示的定位周期結(jié)束后，利用式(15)中球面多邊交匯定位方法開啟對(duì)水中目標(biāo)的定位。

圖3 目標(biāo)向AUV所在平面投影圖

4 仿真分析

根據(jù)本文提出算法，分別對(duì)AUV定位系統(tǒng)和AUV編隊(duì)融合定位系統(tǒng)做出仿真分析。仿真參數(shù)設(shè)置：靜止目標(biāo)所在平面深度為30 m，聲速c=1500 m/s，AUV的有效通信距離為10 km，深度為20 m。高精度AUV起始點(diǎn)水平坐標(biāo)為 (-1270,1270)，航速2 m/s，航向角45°，航行時(shí)間3600 s。低精度AUV1, AUV2, AUV3的起始點(diǎn)水平坐標(biāo)分別為(1000,-1000), (0,-2000), (0,4000)，航速1.5 m/s，航向角分別為60°, 60°, -60°，航向角速度分別為-2·10-4rad/s, 2·10-4rad/s, 1·10-4rad/s。航速量測(cè)誤差服從均值為0，標(biāo)準(zhǔn)差為0.1 m/s的高斯分布，航向角角速度量測(cè)誤差均值為0，標(biāo)準(zhǔn)差為0.01°的高斯分布，測(cè)時(shí)誤差均值為0，標(biāo)準(zhǔn)差為20 ms的高斯分布。AUV集群的理論航跡如圖5所示。

圖5 AUV編隊(duì)理論路徑

4.1 仿真實(shí)驗(yàn)1 AUV協(xié)同定位精度與目標(biāo)定位精度分析

仿真結(jié)果如圖6、圖7所示。圖6為兩種定位系統(tǒng)的AUV編隊(duì)自身位置誤差對(duì)比圖，圖6(a)的AUV位置的均方誤差最大值為36 m，圖6(b)的AUV位置的均方誤差最大值為14 m。從圖7仿真數(shù)據(jù)結(jié)果得到，圖7(a)的定位均方根誤差最小值為15 m，最大均方誤差值為124 m，圖7(b)的定位均方根誤差最小值為14 m，最大均方誤差值為98 m。可以看出，AUV編隊(duì)融合定位系統(tǒng)的目標(biāo)定位精度，高于AUV定位系統(tǒng)。AUV編隊(duì)融合定位系統(tǒng)的誤差分布圖中藍(lán)色區(qū)域面積大于AUV定位系統(tǒng)。

圖6 兩種定位系統(tǒng)協(xié)同定位誤差

圖7 兩種定位方法定位誤差分布圖

進(jìn)一步研究本文方法相對(duì)于AUV定位系統(tǒng)的優(yōu)越性，增加AUV定位系統(tǒng)的領(lǐng)航AUV數(shù)量，將圖6、圖7仿真參數(shù)中的低精度AUV1作領(lǐng)航AUV進(jìn)行仿真，得到圖8。從仿真數(shù)據(jù)結(jié)果得到，雙領(lǐng)航的AUV定位系統(tǒng)的定位均方根誤差最小值為13 m，最大均方誤差值為97 m，藍(lán)色區(qū)域面積較大于圖7(b)?？梢钥闯?，本文算法僅需一個(gè)高精度AUV，對(duì)目標(biāo)定位精度與雙領(lǐng)航的AUV定位系統(tǒng)相似。

圖8 雙領(lǐng)航主從式多AUV編隊(duì)目標(biāo)定位系統(tǒng)定位誤差分布圖

4.2 仿真實(shí)驗(yàn)2 不同量測(cè)誤差與定位誤差關(guān)系實(shí)驗(yàn)

在實(shí)際應(yīng)用中，AUV在水下運(yùn)動(dòng)過程中，受到海洋環(huán)境的影響，會(huì)使得航速與航向角速度的測(cè)量誤差增大。為了比較航速、航向角速度量測(cè)誤差對(duì)兩種方法的影響，在圖7的基礎(chǔ)上，分別添加不同的航速、航向角速度量測(cè)誤差，計(jì)算兩種方法對(duì)應(yīng)的最大均方誤差變化如圖9?？梢钥闯?，相對(duì)于AUV編隊(duì)融合定位系統(tǒng)，AUV定位系統(tǒng)的定位精度受量測(cè)誤差的影響較大，對(duì)惡劣環(huán)境的適應(yīng)能力較差。故AUV編隊(duì)融合定位系統(tǒng)更適用于未知海域中的目標(biāo)定位。

圖9 定位誤差與量測(cè)誤差關(guān)系圖

為進(jìn)一步比較量測(cè)誤差對(duì)兩種方法的影響，隨機(jī)選取20個(gè)靜止目標(biāo)位置，在航速量測(cè)誤差服從均值為0，標(biāo)準(zhǔn)差為0.3 m/s的高斯分布，航向角角速度量測(cè)誤差均值為0，標(biāo)準(zhǔn)差為0.15°的高斯分布的環(huán)境下，分別采用AUV定位系統(tǒng)和AUV編隊(duì)融合定位系統(tǒng)實(shí)現(xiàn)定位并記錄隨機(jī)靜止目標(biāo)的真實(shí)位置與測(cè)量結(jié)果并進(jìn)行比較，結(jié)果如圖10?？梢钥闯鲈谳^大量測(cè)誤差的情況下，兩種方法均可得到較為相近的定位結(jié)果，本文方法相比于多AUV編隊(duì)目標(biāo)定位系統(tǒng)的定位誤差較小。

圖10 隨機(jī)位置靜止目標(biāo)定位結(jié)果圖

5 結(jié)束語(yǔ)

本文提出一種基于EKF的融合觀測(cè)協(xié)同定位方法。本方法基于擴(kuò)展卡爾曼濾波，通過設(shè)置定位周期和觀測(cè)時(shí)間間隔的方式，將低精度AUV分為參考AUV與待測(cè)AUV，從而設(shè)置3種觀測(cè)模式：高精度AUV觀測(cè)參考AUV、低精度AUV自身觀測(cè)以及參考AUV對(duì)待測(cè)AUV進(jìn)行觀測(cè)，對(duì)不同時(shí)刻的不同AUV采用不同的狀態(tài)觀測(cè)方式。本方法相比于主從式多AUV編隊(duì)目標(biāo)定位系統(tǒng)，獲取得到的AUV位置信息更為準(zhǔn)確且受環(huán)境影響小。多AUV集群僅含一個(gè)高精度AUV，實(shí)現(xiàn)了自身定位與水中目標(biāo)定位，精度與雙領(lǐng)航主從式AUV目標(biāo)定位系統(tǒng)相近，可以實(shí)現(xiàn)低功耗、遠(yuǎn)距離的水中目標(biāo)定位。