基于主從式水下自主航行器移動組網(wǎng)的合作目標(biāo)定位方法

趙婧旭 趙 晨 周 鋒*

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

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

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

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

1 引言

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

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

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

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

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

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

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

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

圖2 AUV移動示意圖

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

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

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

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

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

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

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

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

4 仿真分析

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

圖5 AUV編隊理論路徑

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

仿真結(jié)果如圖6、圖7所示。圖6為兩種定位系統(tǒng)的AUV編隊自身位置誤差對比圖，圖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編隊融合定位系統(tǒng)的目標(biāo)定位精度，高于AUV定位系統(tǒng)。AUV編隊融合定位系統(tǒng)的誤差分布圖中藍(lán)色區(qū)域面積大于AUV定位系統(tǒng)。

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

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

進(jìn)一步研究本文方法相對于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)。可以看出，本文算法僅需一個高精度AUV，對目標(biāo)定位精度與雙領(lǐng)航的AUV定位系統(tǒng)相似。

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

4.2 仿真實驗2 不同量測誤差與定位誤差關(guān)系實驗

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

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

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

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

5 結(jié)束語

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