雙領(lǐng)航多自主水下航行器移動(dòng)長(zhǎng)基線定位最優(yōu)隊(duì)形研究

房新鵬，嚴(yán)衛(wèi)生

(西北工業(yè)大學(xué) 航海學(xué)院，陜西 西安710072)

0 引言

在多自主水下航行器(AUV)協(xié)作系統(tǒng)中，有效的任務(wù)規(guī)劃和分配、避障處理和行為控制等群決策的實(shí)現(xiàn)都建立在高精度定位的前提上。因此需具備遠(yuǎn)距離、長(zhǎng)時(shí)間、高精度的定位能力，以實(shí)現(xiàn)高度自主化和智能化，完成各項(xiàng)復(fù)雜任務(wù)。目前定位技術(shù)主要是聲學(xué)定位系統(tǒng)，通常分為長(zhǎng)基線定位系統(tǒng)、短基線定位系統(tǒng)與超短基線定位系統(tǒng)。長(zhǎng)基線定位系統(tǒng)的精度最高，但AUV 的移動(dòng)范圍受限于應(yīng)答器的布放區(qū)域，且布設(shè)、校準(zhǔn)和回收作業(yè)的過(guò)程十分復(fù)雜［1］。針對(duì)這一問(wèn)題，目前主要有3 種解決途徑:1)挪威Kongsberg 公司利用水下應(yīng)答器輔助AUV定位，并進(jìn)行了DGPS、USBL、UTP、DVL 和INS 的綜合定位導(dǎo)航技術(shù)的開(kāi)發(fā)，由于無(wú)需母船支持，有效地拓展了AUV 作業(yè)范圍，但尚未實(shí)際應(yīng)用［2］;2)法國(guó)ACSA 公司開(kāi)發(fā)了基于GPS 智能浮標(biāo)的長(zhǎng)基線水聲定位技術(shù)，避免了海底應(yīng)答器復(fù)雜的布放和校正過(guò)程，但僅能提供固定區(qū)域的導(dǎo)航定位服務(wù)［3］;3)美國(guó)MIT 提出了移動(dòng)長(zhǎng)基線(MLBL)定位新概念，利用時(shí)間同步信號(hào)和水聲傳播延遲進(jìn)行相對(duì)位置測(cè)量［4］。

MLBL 定位最早由Vaganay 提出［4］，部分AUV裝備高精度導(dǎo)航設(shè)備，其余AUV 裝備低精度導(dǎo)航設(shè)備，利用聲學(xué)距離量測(cè)進(jìn)行位置更新。文獻(xiàn)［5］介紹了WHOI 公司的水聲調(diào)制解調(diào)器，可以實(shí)現(xiàn)AUV間時(shí)鐘同步，精確測(cè)量水聲信號(hào)傳播時(shí)間，為MLBL定位系統(tǒng)的實(shí)現(xiàn)提供了技術(shù)支持。文獻(xiàn)［6］介紹了基于極大似然法研究MLBL 定位系統(tǒng)。文獻(xiàn)［7］利用無(wú)人水面船(USV)輔助AUV 進(jìn)行了一系列MLBL 定位試驗(yàn)。為了降低定位系統(tǒng)復(fù)雜度，學(xué)者提出了單領(lǐng)航者定位系統(tǒng)。文獻(xiàn)［8 -9］基于相對(duì)位移和航位推算信息研究了單領(lǐng)航者定位系統(tǒng)，文獻(xiàn)［9］進(jìn)一步研究了量測(cè)數(shù)據(jù)的選取和定位系統(tǒng)的觀測(cè)性。文獻(xiàn)［10］利用線性時(shí)變系統(tǒng)觀測(cè)性理論分別研究了無(wú)洋流、已知洋流及未知洋流條件下單信標(biāo)測(cè)距定位系統(tǒng)的觀測(cè)性。文獻(xiàn)［11］基于轉(zhuǎn)移損耗函數(shù)極小化法研究了單領(lǐng)航者定位系統(tǒng)，并設(shè)計(jì)單USV 輔助AUV 定位的試驗(yàn)，取得了較好的定位效果。

當(dāng)前，國(guó)內(nèi)外關(guān)于AUV 水下定位系統(tǒng)的研究多集中在定位算法設(shè)計(jì)以及試驗(yàn)平臺(tái)開(kāi)發(fā)上，關(guān)于AUV 隊(duì)形結(jié)構(gòu)與定位性能關(guān)系的文獻(xiàn)較少，文獻(xiàn)［12］研究了隊(duì)形結(jié)構(gòu)對(duì)異構(gòu)機(jī)器人定位精度的影響，設(shè)計(jì)了最優(yōu)編隊(duì)經(jīng)驗(yàn)規(guī)則。針對(duì)這一問(wèn)題，本文基于距離量測(cè)提出多AUV 移動(dòng)長(zhǎng)基線定位系統(tǒng)，利用Cramer-Rao 下界(CRLB)和Fisher 信息矩陣(FIM)理論建立與AUV 隊(duì)形結(jié)構(gòu)參數(shù)有關(guān)的定位性能評(píng)價(jià)函數(shù)，通過(guò)評(píng)價(jià)函數(shù)極大化實(shí)現(xiàn)定位性能最優(yōu)化，在不提高傳感器配置前提下進(jìn)一步優(yōu)化定位系統(tǒng)的定位性能。重點(diǎn)研究了雙領(lǐng)航者定位系統(tǒng)的最優(yōu)隊(duì)形，并基于EKF 設(shè)計(jì)對(duì)比實(shí)驗(yàn)驗(yàn)證AUV間的距離值與分離角對(duì)定位性能的影響。

1 多AUV 移動(dòng)長(zhǎng)基線定位系統(tǒng)

1.1 移動(dòng)長(zhǎng)基線定位

移動(dòng)長(zhǎng)基線定位是近年來(lái)隨著水聲通信技術(shù)的進(jìn)步而發(fā)展起來(lái)的一種定位技術(shù)。通過(guò)將應(yīng)答器布設(shè)在USV 或者大型AUV 上，充當(dāng)可移動(dòng)的長(zhǎng)基線定位基陣，利用AUV 之間的探測(cè)或通信確定被定位AUV 的位置，極大地降低了安裝復(fù)雜性，拓展了應(yīng)用范圍。多AUV 移動(dòng)長(zhǎng)基線定位系統(tǒng)的原理如圖1所示。

圖1 多AUV 移動(dòng)長(zhǎng)基線定位系統(tǒng)原理圖Fig.1 Cooperative localization based on MLBL of multi-AUVs

其中，領(lǐng)航AUV 可以利用自身攜帶的高精度慣性導(dǎo)航設(shè)備精確定位，而從AUV 僅裝備低精度航位推算導(dǎo)航設(shè)備，需要通過(guò)協(xié)同方式進(jìn)行定位，同時(shí)，二者外部均裝備水聲通信裝置測(cè)量相對(duì)位置關(guān)系。從AUV 利用領(lǐng)航AUV 聲信號(hào)脈沖傳播時(shí)間解算二者間的相對(duì)距離，融合領(lǐng)航AUV 位置廣播信息實(shí)現(xiàn)位置解算。

1.2 系統(tǒng)模型

航行過(guò)程中，AUV 的深度值可以由壓力傳感器直接測(cè)得，而且穩(wěn)態(tài)運(yùn)動(dòng)時(shí)AUV 的俯仰角θ 較小，視作cosθ≈1.因此，可不考慮深度項(xiàng)，減小遞推狀態(tài)的維數(shù)，將AUV 在水下的三維定位問(wèn)題簡(jiǎn)化為二維平面內(nèi)的定位問(wèn)題。本文假設(shè)領(lǐng)航AUV 與從AUV 均在同一深度平面內(nèi)運(yùn)動(dòng)。

定義k 時(shí)刻從AUV 的平面運(yùn)動(dòng)狀態(tài)為Xk=(xkykψk)T，其中，xk和yk表示k 時(shí)刻從AUV 在笛卡爾坐標(biāo)系中的慣性坐標(biāo)，ψk表示k 時(shí)刻從AUV的偏航角。建立采樣周期為T(mén) 的從AUV 二維離散運(yùn)動(dòng)學(xué)模型為

式中vk和ωk分別為k 時(shí)刻從AUV 的前向速度和偏航角速度，均受到高斯白噪聲干擾，且噪聲協(xié)方差

從而，AUV 的二維運(yùn)動(dòng)學(xué)方程可簡(jiǎn)寫(xiě)為

式中uk=(vkωk)T為k 時(shí)刻系統(tǒng)的控制輸入。

建立從AUV 相對(duì)于第i(i =1…n)個(gè)領(lǐng)航AUV的距離量測(cè)方程:

式中:rk，i為k 時(shí)刻從AUV 與第i 個(gè)領(lǐng)航AUV 之間的真實(shí)距離值;(xk，i，yk，i)T為第i(i =1…n)個(gè)領(lǐng)航AUV 在笛卡爾坐標(biāo)系中的慣性坐標(biāo);wk，i為k 時(shí)刻的量測(cè)噪聲。

試驗(yàn)證明，距離測(cè)量值誤差僅僅弱相關(guān)于距離值，因此，假定k 時(shí)刻wk，i(i =1…n)為獨(dú)立不相關(guān)的距離量測(cè)高斯白噪聲，且其協(xié)方差為一個(gè)固定值［13］，即

式中wk=［wk，1… wk，n］T.

此時(shí)，距離量測(cè)方程(3)式的雅可比矩陣為

式中φk，i為k 時(shí)刻第i 個(gè)領(lǐng)航AUV 相對(duì)于從AUV的方位角，量測(cè)時(shí)選取順時(shí)針?lè)较驗(yàn)檎?，如圖2所示。圖中γij為第i、j 個(gè)領(lǐng)航AUV 與從AUV 間的分離角，γij=γji∈［0，π］.

圖2 主從AUV 間相對(duì)結(jié)構(gòu)示意圖Fig.2 Relative geometry between leader AUVs and the follower

此處，從AUV 的系統(tǒng)狀態(tài)只包括位置坐標(biāo)(xk，yk)T，而沒(méi)有考慮其偏航角ψk，這是因?yàn)樵诹繙y(cè)方程(3)式下偏航角參數(shù)不影響其位置誤差，大大簡(jiǎn)化了系統(tǒng)模型。

2 雙領(lǐng)航者定位系統(tǒng)最優(yōu)隊(duì)形

2.1 定位性能評(píng)價(jià)函數(shù)

定位過(guò)程實(shí)際上是一個(gè)狀態(tài)預(yù)測(cè)過(guò)程，常用預(yù)測(cè)誤差協(xié)方差來(lái)評(píng)價(jià)定位性能的優(yōu)劣?？颂m姆-羅不等式下界(CRLB)給出了未知參數(shù)無(wú)偏預(yù)測(cè)的協(xié)方差矩陣?yán)碚撓陆?。因此，可以用CRLB 表征AUV隊(duì)形對(duì)定位性能的影響。假設(shè)從AUV 位置狀態(tài)xk的無(wú)偏估計(jì)為，則預(yù)測(cè)過(guò)程的CRLB 為

式中Jk為k 時(shí)刻的FIM，F(xiàn)IM 描述了預(yù)測(cè)過(guò)程中測(cè)量值所包含的狀態(tài)變量信息量的大小。

預(yù)測(cè)過(guò)程中CRLB 值增大等同于FIM 值減小，也就意味著提高了預(yù)測(cè)性能，降低了預(yù)測(cè)不確定性。

k+1 時(shí)刻系統(tǒng)(2)式的FIM 可以寫(xiě)為如下遞歸計(jì)算形式［14］:

式中:Φk=xk-1f(Xk-1，uk-1);Rk，i和Hk，i的定義分別如(4)式～(5)式所示。

由于(7)式中沒(méi)有從AUV 位置坐標(biāo)的先驗(yàn)分布信息，Jk-1→∞，即→0.因此，可以將定位系統(tǒng)的FIM 簡(jiǎn)寫(xiě)為

將Hk，i和Rk，i表達(dá)式代入(8)式，得到多AUV移動(dòng)長(zhǎng)基線定位系統(tǒng)FIM 為

由于Jk是一個(gè)矩陣，很難通過(guò)最大化Jk解算最優(yōu)隊(duì)形，因此，考慮用一個(gè)基于(9)式的實(shí)值標(biāo)量函數(shù)替代作為評(píng)價(jià)函數(shù)。綜合計(jì)算量和評(píng)價(jià)效果等因素，選擇FIM 的行列式(又稱為D-最優(yōu)準(zhǔn)則)作為評(píng)價(jià)標(biāo)準(zhǔn)［15］。

計(jì)算(9)式的行列式，得到多AUV 移動(dòng)長(zhǎng)基線定位系統(tǒng)定位性能評(píng)價(jià)函數(shù)為

2.2 定位系統(tǒng)最優(yōu)隊(duì)形

由(10)式知，當(dāng)n=1 時(shí)，det (Jk)≡0，因此，僅利用單一時(shí)刻AUV 間的距離量測(cè)無(wú)法實(shí)現(xiàn)從AUV位置狀態(tài)的無(wú)偏估計(jì)。一般地，設(shè)計(jì)僅基于距離量測(cè)的多AUV 移動(dòng)長(zhǎng)基線定位系統(tǒng)時(shí)，至少需要n≥2 個(gè)領(lǐng)航AUV.文中多AUV 移動(dòng)長(zhǎng)基線定位系統(tǒng)選用兩個(gè)領(lǐng)航AUV，即n=2.

當(dāng)n=2 時(shí)，

兩個(gè)領(lǐng)航AUV 分別靜止位于(-0.5 m，0)和(0.5 m，0)處，從AUV 在xk∈(-1.5 m，1.5 m)，yk∈(-1.5 m，1.5 m)區(qū)域運(yùn)動(dòng)，令σ2r=1，作雙領(lǐng)航者移動(dòng)長(zhǎng)基線定位系統(tǒng)定位性能評(píng)價(jià)函數(shù)的三維網(wǎng)格圖，如圖3所示，圖4為相應(yīng)的等高線圖。

由圖3、圖4知，從AUV 位置坐標(biāo)在以(-0.5 m，0)和(0.5 m，0)為直徑端點(diǎn)的圓的圓周上時(shí)，評(píng)價(jià)函數(shù)取得最大值，此時(shí)定位性能最優(yōu)。

圖3 評(píng)價(jià)函數(shù)三維網(wǎng)格圖Fig.3 Three-dimensional mesh graph of evaluation function

圖4 評(píng)價(jià)函數(shù)等高線圖Fig.4 The contour graph of evaluation function

綜上所述，雙領(lǐng)航者移動(dòng)長(zhǎng)基線定位系統(tǒng)最優(yōu)隊(duì)形為，兩個(gè)領(lǐng)航AUV 與從AUV 保持90°分離角相對(duì)運(yùn)動(dòng)，且此最優(yōu)隊(duì)形與AUV 間的距離無(wú)關(guān)(實(shí)際操作中，為了避免AUV 之間發(fā)生碰撞等危險(xiǎn)，需要設(shè)定AUV 間的最小允許安全距離)。

3 仿真分析

基于EKF 算法對(duì)基于雙領(lǐng)航AUV 的移動(dòng)長(zhǎng)基線定位系統(tǒng)進(jìn)行仿真實(shí)驗(yàn)，驗(yàn)證最優(yōu)隊(duì)形與分離角和間距之間的關(guān)系。實(shí)驗(yàn)設(shè)計(jì)中，假設(shè)AUV 之間最小安全距離為80 m，從AUV 距離量測(cè)方程高斯白噪聲協(xié)方差σ2r=(10 m)2.采用蒙特卡洛法計(jì)算定位系統(tǒng)的定位誤差，重復(fù)仿真計(jì)算100 次，取其統(tǒng)計(jì)平均值。

從AUV 從(20 m，20 m)處出發(fā)，以3 節(jié)的速度沿直線運(yùn)動(dòng)，偏航角為45°，兩個(gè)領(lǐng)航AUV 分別從(100 m，20 m)和(20 m，100 m)處出發(fā)，以相同的速度和偏航角沿直線運(yùn)動(dòng)。領(lǐng)航AUV 與從AUV 之間的距離為80 m，分離角為90°，此時(shí)定位系統(tǒng)的運(yùn)動(dòng)隊(duì)形最優(yōu)。

在以上最優(yōu)隊(duì)形基礎(chǔ)上改變兩個(gè)領(lǐng)航AUV 的初始位置，使AUV 隊(duì)形發(fā)生變化，驗(yàn)證分離角和間距的改變對(duì)定位性能的影響。

1)改變兩個(gè)領(lǐng)航AUV 的初始位置分別為(100 m，20 m)和(89.3 m，30 m)，速度與偏航角不變。與改變前的最優(yōu)隊(duì)形相比，間距仍為80 m，只是分離角減小到30°。通過(guò)分析知，此時(shí)AUV 隊(duì)形為非最優(yōu)隊(duì)形;

2)改變兩個(gè)領(lǐng)航AUV 的初始位置分別為(160 m，20 m)和(20 m，160 m)，速度與偏航角不變。與改變前的最優(yōu)隊(duì)形相比，分離角仍為90°，只是間距增大到120 m.通過(guò)分析知，此時(shí)AUV 隊(duì)形仍為最優(yōu)隊(duì)形。

圖5和圖6分別給出了隊(duì)形改變前后定位系統(tǒng)平均定位誤差的對(duì)比。由圖5知，保持領(lǐng)航AUV 與從AUV 的間距為80 m 時(shí)，最優(yōu)隊(duì)形定位系統(tǒng)平均定位誤差為3.037 m，非最優(yōu)隊(duì)形定位系統(tǒng)平均定位誤差為4.312 m，最優(yōu)隊(duì)形提高定位精度約29.6%。由圖6知，保持領(lǐng)航AUV 與從AUV 間的分離角為90°時(shí)，兩種最優(yōu)隊(duì)形定位系統(tǒng)平均定位誤差分別為3.032 m 和2.826 m，二者相差很小，間距對(duì)定位性能的影響并不明顯。

圖5 變分離角定位系統(tǒng)平均定位誤差Fig.5 The average localization errors with different separation angles

圖6 變間距定位系統(tǒng)平均定位誤差Fig.6 The average localization errors with different range

仿真實(shí)驗(yàn)進(jìn)一步驗(yàn)證了最優(yōu)隊(duì)形受分離角的影響較大，而與間距無(wú)關(guān)。

4 結(jié)論

本文從CRLB 和FIM 的角度研究了雙領(lǐng)航AUV 移動(dòng)長(zhǎng)基線定位系統(tǒng)。建立了定位性能評(píng)價(jià)函數(shù)，并通過(guò)評(píng)價(jià)函數(shù)極大化得到雙領(lǐng)航者移動(dòng)長(zhǎng)基線定位系統(tǒng)的最優(yōu)隊(duì)形。仿真實(shí)驗(yàn)表明，保持分離角為90°基礎(chǔ)上改變領(lǐng)航AUV 與從AUV 間的距離，對(duì)定位性能的影響很小，而保持間距為80 m 基礎(chǔ)上改變領(lǐng)航AUV 與從AUV 間的分離角，則能顯著影響定位性能。

References)

［1］ Stutters L，Liu H H，Tiltman C，et al.Navigation technologies for autonomous underwater vehicles［J］.IEEE Transactions on Systems，Man，and Cybernetics，Part C:Applications and Reviews，2008，38(4):581 -589.

［2］ Jalving B，Gade K，Svartveit K，et al.DVL velocity aiding in the HUGIN 1000 intergrated inertial navigation system［J］.Modeling，Identification and Control，2004，24(4):223 -236.

［3］ Thomas H G.GIB buoys:an interface between space and depths of the oceans［C］∥Proceedings of The 1998 Workshop on Autonomous Underwater Vehicles，Cambridge:IEEE，1998:181 -184.

［4］ Leonard J J，Curcio J A，Willcox J S.Experimental validation of the moving long base-line navigation concept［C］∥Proceedings of 2004 IEEE/OES Autonomous Underwater Vehicles.Cambridge:IEEE，2004:59 -65.

［5］ Freitag L，Johnson M，Grund M，et al.Integrated acoustic communication and navigation for multiple UUVs［C］∥Proceedings of MTS/IEEE Oceans 2001 Conference and Exhibition.Honolulu:IEEE，2001:2065 -2070.

［6］ Eustice R M，Whitcomb L L，Singh H，et al.Recent advances in synchronous-clock one-way-travel-time acoustic navigation［C］∥Proceedings of the IEEE/MTS Oceans 2006 Conference and Exhibition.Boston:IEEE，2006:1 -6.

［7］ Curcio J，Leonard J，Vaganay J，et al.Experiments in moving baseline navigation using autonomous surface craft［C］∥Proceedings of the IEEE/MTS Oceans 2005 Conference and Exhibition.Washnigon DC:IEEE，2005:730 -735.

［8］ 張立川，劉明雍，徐德民，等.基于水聲傳播延遲的主從式多無(wú)人水下航行器協(xié)同導(dǎo)航定位研究［J］.兵工學(xué)報(bào)，2009，30(12):1674 -1678.ZHANG Li-chuan，LIU Ming-yong，XU De-min，et al.Cooperative localization for multi-UUVs based on time-of-flight of acoustic signal［J］.Acta Armamentarii，2009，30(12):1674 -1678.(in Chinese)

［9］ 李聞白，劉明雍，張立川，等.單領(lǐng)航者相對(duì)位移測(cè)量的多自主水下航行器協(xié)同導(dǎo)航［J］.兵工學(xué)報(bào)，2011，32(8):1002 -1007.LI Wen-bai，LIU Ming-yong，ZHANG Li-chuan，et al.Cooperative navigation for multiple autonomous underwater vehicles based on relative displacement measurement with a single leader［J］.Acta Armamentarii，2011，32(8):1002 -1007.(in Chinese)

［10］ Gadre A S.Observability analysis in navigation systems with an underwater vehicle application［D］.Virginia:Virginia Polytechnic Institute and State University，2007:39 -42，53 -56.

［11］ Papadopoulos G，F(xiàn)allon M F，Leonard J J，et al.Cooperative localization of marine vehicles using nonlinear state estimation［C］∥Proceedings of 2010 IEEE/RSJ International Conference on Intelligent Robots and Systems.Taipei:IEEE，2010:4874 -4879.

［12］ Yukikazu S H，Anastasios I M，Stergios I R.Optimal formation for cooperative localization of mobile robots［C］∥Proceedings of 2005 IEEE International Conference on Robotics and Automation.Barcelona:IEEE，2005:4137 -4142.

［13］ Bahr A，Leonard J J，F(xiàn)allon M F.Cooperative localization for autonomous underwater vehicles［J］.The International Journal of Robotics Research，2009，28(6):714 -728.

［14］ Hernandez M L.Optimal sensor trajectories in bearings-only tracking［C］∥Proceedings of the Seventh International Conference on Information Fusion.Stockholm:Internation Society of Information Fusion，2004:893 -900.

［15］ Sameera S P，Richard M K，Emilio F.Trajectory optimization for target localization using small unmanned aerial vehicles［C］∥Proceedings of AIAA Conference on Guidance，Navigation，and Control.Chicago:AIAA，2009:1211 -1235.