基于主從式水下自主航行器移動組網的合作目標定位方法

趙婧旭 趙 晨 周 鋒*

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

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

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

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

1 引言

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

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

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

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

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

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

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

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

圖2 AUV移動示意圖

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

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

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

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

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

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

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

圖3 目標向AUV所在平面投影圖

4 仿真分析

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

圖5 AUV編隊理論路徑

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

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

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

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

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

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

4.2 仿真實驗2 不同量測誤差與定位誤差關系實驗

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

圖9 定位誤差與量測誤差關系圖

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

圖10 隨機位置靜止目標定位結果圖

5 結束語

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