一種基于雙模型的低成本多AUV 協(xié)同定位方法

徐博，李盛新，張奐

哈爾濱工程大學(xué)自動(dòng)化學(xué)院，黑龍江哈爾濱150001

0 引 言

自主水下航行器（autonomous underwater vehicles，AUV）在水下監(jiān)視、海洋調(diào)查、生物監(jiān)測(cè)等諸多軍事、民事水下任務(wù)中得到了廣泛的應(yīng)用［1］。由于射頻和全球定位系統(tǒng)（GPS）信號(hào)在水下快速衰減的特性，導(dǎo)航定位問題成為發(fā)展AUV 面臨的重要挑戰(zhàn)。高精度陀螺儀和加速度計(jì)組成的慣性導(dǎo)航系統(tǒng)（INS）是目前最常用的水下定位方法。然而，即使是高精度的慣性傳感器也會(huì)隨著時(shí)間的推移積累定位誤差，導(dǎo)致定位誤差發(fā)散［2-5］。1996年日本學(xué)者Kurazume Ryo 在機(jī)器人定位領(lǐng)域首次提出了協(xié)同定位的概念［6］。近年來，利用聲吶技術(shù)測(cè)量AUV 的相對(duì)位置關(guān)系估計(jì)多AUV 協(xié)同定位，已成為AUV 定位領(lǐng)域的重點(diǎn)研究問題。相比于傳統(tǒng)的長(zhǎng)基線（long baseline，LBL）、短基線（short baseline，SBL）和超短基線（ultra-short baseline，USBL）等定位方法［7］，多AUV 協(xié)同定位方法無需事先布放海底基陣和水面船只等輔助設(shè)備，能夠有效降低AUV 定位系統(tǒng)的硬件復(fù)雜度，適用于AUV 在大范圍海域執(zhí)行各種任務(wù)的需求。

協(xié)同定位是目前水下中間層區(qū)域多AUV 系統(tǒng)最有效的定位方法之一，具有廣闊的應(yīng)用前景。通常情況下，多AUV 協(xié)同定位具有兩種形式：1）并行式，即系統(tǒng)中每個(gè)航行器的功能和結(jié)構(gòu)相同，使用各自的導(dǎo)航系統(tǒng)進(jìn)行定位，通過水聲通信，獲取其他航行器的位置信息；2）主從式，也稱為領(lǐng)航式，即系統(tǒng)中少量領(lǐng)航AUV 裝備高精度導(dǎo)航設(shè)備，大量跟隨AUV 裝備低精度導(dǎo)航設(shè)備，跟隨AUV 通過水下聲學(xué)調(diào)制解調(diào)器獲得與領(lǐng)航AUV 的位置關(guān)系以提高自身定位精度。并行式的協(xié)同定位結(jié)構(gòu)簡(jiǎn)單，但每個(gè)AUV 都裝備高精度導(dǎo)航設(shè)備，成本將增加很多倍，而主從式兼顧了定位精度和成本，成為多AUV 協(xié)同定位研究的主要方向［8］。主從式協(xié)同定位系統(tǒng)中，領(lǐng)航AUV 裝備高精度INS，多普勒速度計(jì)程儀（DVL），GPS 和水聲通信設(shè)備等，導(dǎo)航定位系統(tǒng)以高精度慣導(dǎo)設(shè)備為主，初始位置通過GPS 獲得，以DVL 測(cè)量的絕對(duì)速度作為慣導(dǎo)外部輸入，進(jìn)一步提高了協(xié)同定位的精度，跟隨AUV 裝備低精度INS，DVL 和水聲通信設(shè)備等?；诟?、低精度導(dǎo)航設(shè)備的傳感網(wǎng)絡(luò)，通過AUV 間的相對(duì)測(cè)量與信息共享，進(jìn)而實(shí)現(xiàn)整個(gè)編隊(duì)的導(dǎo)航定位性能提升［9］。

傳統(tǒng)的協(xié)同定位方法是使用基于狀態(tài)空間模型的濾波技術(shù)，它能夠?qū)崿F(xiàn)統(tǒng)計(jì)最優(yōu)估計(jì)，補(bǔ)償基于傳感器的航位推算誤差，獲得較高精度的狀態(tài)估計(jì)信息［10］。房新鵬等［11］提出一種基于雙移動(dòng)信標(biāo)的航行器定位方法，雙移動(dòng)信標(biāo)由2 艘無人水面艇來?yè)?dān)任，在2 艘無人水面艇上分別安裝有差分GPS、航向與航向角速度傳感器及聲定位與通信聲吶，AUV 上安裝有航向、航速、航向角速度傳感器及聲定位與通信聲吶。2 艘無人水面艇上的聲定位與通信聲吶周期性地發(fā)出聲定位信號(hào)，同時(shí)將自身的位置發(fā)送給AUV。當(dāng)AUV 上的聲定位與通信聲吶接收到無人水面艇發(fā)送來的聲定位信號(hào)和位置信息后，建立狀態(tài)空間模型并利用擴(kuò)展卡爾曼濾波器（EKF）進(jìn)行水下定位。仿真試驗(yàn)證明了所提方法的有效性。然而，當(dāng)AUV 上裝備的航向、航速、航向角速度傳感器由于某種原因失效或者被敵方破壞，這種僅基于狀態(tài)空間模型的濾波技術(shù)將會(huì)由于無法進(jìn)行濾波預(yù)測(cè)而失效。另外，在需要大量AUV 組成集群協(xié)同工作的情況下，即使跟隨AUV 裝備低精度的導(dǎo)航設(shè)備，其所需的INS 和DVL 也仍是一筆不菲的成本。故在實(shí)際工程應(yīng)用中，如果跟隨AUV 沒有裝備INS 和DVL，還能夠保障協(xié)同定位系統(tǒng)的定位精度在允許的范圍內(nèi)，將具有重大的研究?jī)r(jià)值。

針對(duì)上述問題，本文將在傳統(tǒng)多AUV 協(xié)同定位方法的基礎(chǔ)上，結(jié)合跟隨AUV 與領(lǐng)航AUV 之間的相對(duì)運(yùn)動(dòng)模型和雙領(lǐng)航協(xié)同定位狀態(tài)空間模型，設(shè)計(jì)一種基于雙運(yùn)動(dòng)模型的主從式多AUV 協(xié)同定位方法，利用相對(duì)運(yùn)動(dòng)模型估計(jì)跟隨AUV 的航向信息和速度信息；再通過應(yīng)用雙領(lǐng)航模式的多AUV 協(xié)同定位狀態(tài)空間模型，進(jìn)一步提高協(xié)同定位系統(tǒng)的魯棒性和準(zhǔn)確性。

1 多AUV 協(xié)同定位狀態(tài)空間模型描述

1.1 相對(duì)運(yùn)動(dòng)狀態(tài)空間模型

AUV 在水下進(jìn)行三維空間運(yùn)動(dòng)，3 個(gè)位置狀態(tài)變量(x，y，z)分別表示以導(dǎo)航坐標(biāo)系原點(diǎn)為參考點(diǎn)的AUV 水平位置和深度。由于深度z可以直接由壓力傳感器得到，為了簡(jiǎn)化模型維數(shù)，使不同深度的AUV 投影在同一水平面內(nèi)，將三維運(yùn)動(dòng)學(xué)模型簡(jiǎn)化為二維運(yùn)動(dòng)學(xué)模型，如圖1 所示。圖中，d 為領(lǐng)航艇的投影與跟隨艇之間的距離，l為領(lǐng)航艇與跟隨艇之間的真實(shí)距離。

定義跟隨AUV 在tk時(shí)刻的位置向量為(xsk，ysk) ，領(lǐng) 航AUV 在tk時(shí) 刻 的 位 置 向 量 為(xmk，ymk) ，跟 隨AUV 在tk時(shí) 刻 的 狀 態(tài) 向 量 為

圖6 跟隨AUV 的北向速度比較圖Fig.6 A comparison of the northward velocity of follower AUV

圖7 跟隨AUV 的航跡向比較圖Fig.7 A comparison of the track direction of follower AUV

從試驗(yàn)結(jié)果可以看出，本文提出的雙模型協(xié)同定位方法可以實(shí)現(xiàn)在跟隨AUV 沒有裝備INS 和DVL 的情況下實(shí)時(shí)估計(jì)跟隨AUV 的位置，并且保障協(xié)同定位系統(tǒng)的定位精度在允許范圍之內(nèi)。

圖8 跟隨AUV 的航行軌跡比較圖Fig.8 A comparison of the sailing track of follower AUV

圖9 跟 誤差比較圖Fig.9 A comparisong error of follower AUV

4 結(jié) 語(yǔ)

在主從式多AUV 協(xié)同定位系統(tǒng)中，跟隨AUV裝備低精度INS，DVL 和水聲通信設(shè)備等傳感器。然而在跟隨AUV 數(shù)量較多的情況下，就需要很多的INS 和DVL，系統(tǒng)配置復(fù)雜，相應(yīng)成本也隨之提高。針對(duì)上述問題，本文以縮減多AUV 協(xié)同定位系統(tǒng)成本為目標(biāo)，在傳統(tǒng)多AUV 協(xié)同定位方法的基礎(chǔ)上，設(shè)計(jì)一種基于雙模型的主從式多AUV 協(xié)同定位方法，利用EKF 進(jìn)行狀態(tài)估計(jì)，估計(jì)跟隨AUV 的實(shí)時(shí)航向信息和速度信息，再應(yīng)用雙領(lǐng)航模式的多AUV 協(xié)同定位狀態(tài)空間模型，進(jìn)一步保障了多AUV 系統(tǒng)協(xié)同定位性能。最后，通過海面試驗(yàn)數(shù)據(jù)進(jìn)行了仿真試驗(yàn)驗(yàn)證。結(jié)果表明，本文提出的基于雙模型主從式多AUV 協(xié)同定位方法能夠?qū)崿F(xiàn)在跟隨AUV 沒有裝備INS 和DVL情況下，實(shí)時(shí)、準(zhǔn)確地估計(jì)跟隨AUV的位置。

本文提出的系統(tǒng)定位方法的主要優(yōu)點(diǎn)體現(xiàn)在：僅基于跟隨AUV 和領(lǐng)航AUV 間的相對(duì)測(cè)量距離及領(lǐng)航AUV 廣播的自身位置和速度信息，即可計(jì)算得到跟隨AUV 的位置、速度和航向信息，節(jié)約了大量INS 和DVL，降低了多AUV 協(xié)同定位的成本，從而降低AUV 系統(tǒng)配置的復(fù)雜性，且節(jié)約了跟隨AUV 的內(nèi)部空間并減輕重量；將AUV 相對(duì)運(yùn)動(dòng)狀態(tài)空間模型與雙領(lǐng)航模式的多AUV 協(xié)同定位狀態(tài)空間模型相結(jié)合，保障了多AUV 協(xié)同定位系統(tǒng)的協(xié)同定位性能。同時(shí)，本文所提出的方法也可作為一種提高多AUV 協(xié)同定位系統(tǒng)容錯(cuò)性的措施，當(dāng)跟隨AUV 上裝備的INS 和DVL 由于某種原因失效或者遭到破壞時(shí)，該方法可作為后補(bǔ)算法實(shí)時(shí)估計(jì)AUV 的位置信息，為進(jìn)一步研究多AUV 協(xié)同定位系統(tǒng)、提高系統(tǒng)協(xié)同定位性能提供了可行性思路和方法。