劉頂峰，張 康，劉 智，游 卓，陳 虹，丁希侖

(1.武漢第二船舶研究設(shè)計(jì)所, 湖北 武漢 430064；2.北京航空航天大學(xué) 機(jī)械工程及自動(dòng)化學(xué)院, 北京 100191)

0 引 言

隨著人類社會(huì)的進(jìn)步與發(fā)展，陸地的資源和礦產(chǎn)不斷被開(kāi)發(fā)與消耗，各國(guó)逐漸將發(fā)展的目光投向占地球表面積70%的海洋。深海潛水器是實(shí)現(xiàn)海洋開(kāi)發(fā)利用的重要海洋工程裝備[1]。根據(jù)作業(yè)使命任務(wù)的不同，各國(guó)先后研制出了不同類型的深海潛水器，主要包括載人潛水器（HOV）、纜控?zé)o人潛水器（ROV）、無(wú)纜自治式無(wú)人潛水器（AUV）[2]，除螺旋槳推進(jìn)方式外，近幾年又出現(xiàn)了一種新的無(wú)人無(wú)纜多腿爬行式深海潛水器[3]。

深海爬游混合型無(wú)人潛水器（以下簡(jiǎn)稱爬游潛器）是十三五重點(diǎn)研發(fā)計(jì)劃項(xiàng)目，是一種既可在深海巡游，又可海底爬行的爬游混合型無(wú)人無(wú)纜潛水器。甲板操控單元通過(guò)USBL 通信系統(tǒng)，向深海作業(yè)的爬游潛器發(fā)送控制指令并接收其狀態(tài)信息，以此實(shí)現(xiàn)對(duì)其在水底和水中的作業(yè)控制，并掌握其狀態(tài)和軌跡信息。為實(shí)現(xiàn)對(duì)爬游潛器有效安全的操控，首先必須獲取穩(wěn)定可靠的定位信息，對(duì)水下軌跡進(jìn)行預(yù)測(cè)。

1 爬游潛器水下定位海試方案

水下無(wú)人潛航器（UUV）在海中和海底采用的常用定位手段有慣性原理、重力場(chǎng)匹配、地磁輔助、聲覺(jué)、視覺(jué)、地形輔助等[4-6]。爬游潛器水下定位采用水聲定位方式，配備水聲通信系統(tǒng)是Evologics 的USBL，標(biāo)稱垂向通信錐角為120°，適用于深海垂向大深度的水聲通信，因此契合深海爬游混合的應(yīng)用場(chǎng)景。通過(guò)超短基線系統(tǒng)實(shí)現(xiàn)對(duì)爬游潛器定向和定位。超短基線定位系統(tǒng)確定水下目標(biāo)位置是通過(guò)測(cè)量信號(hào)的到達(dá)方位和距離實(shí)現(xiàn)，測(cè)向任務(wù)是通過(guò)測(cè)量信號(hào)到達(dá)接收基陣基元之間的相位差實(shí)現(xiàn)的，它是超短基線定位系統(tǒng)的關(guān)鍵。一般來(lái)說(shuō)，最少需要3 個(gè)接收基元構(gòu)成平面接收陣才能夠作為短基線陣進(jìn)行目標(biāo)的三維定位[7]。

爬游潛器搭載在“深潛號(hào)”上，開(kāi)展演示驗(yàn)證試驗(yàn)。 超短基線應(yīng)答器部署在爬游潛器背部，而超短基線基陣通過(guò)剛性連接，部署在“深潛號(hào)”右舷，離開(kāi)母船螺旋槳一定距離，且低于船底最低處2～3 m，降低環(huán)境影響，獲取更優(yōu)的水聲通信環(huán)境。圖1 為爬游潛器深海海試過(guò)程中USBL 系統(tǒng)的實(shí)際安裝配置方式。

圖1 爬游潛器海試定位方案Fig.1 Locating scheme of craw-swimming vehicle (CSV) sea trial

2 USBL 數(shù)據(jù)直接用于定位存在的問(wèn)題分析

2.1 對(duì)爬游潛器下潛過(guò)程數(shù)據(jù)進(jìn)行分析

爬游潛器從“深潛號(hào)”右舷吊入水中，脫扣后開(kāi)始下潛，到達(dá)一定深度后進(jìn)入U(xiǎn)SBL 通信錐角內(nèi)，建立水聲通信連接。通信連接一旦建立，USBL 基陣依據(jù)通信建立過(guò)程中，聲波發(fā)送和接收的間隔計(jì)算爬游潛器的定位信息，并發(fā)送給爬游潛器的甲板操控單元進(jìn)行顯示和存儲(chǔ)。下潛定位數(shù)據(jù)（截取部分）如表1 所示，通過(guò)分析，可得下述結(jié)論：1）異常值在定位過(guò)程中隨機(jī)出現(xiàn)，如表第2、5、8 行所示；2）基陣建立定位，向甲板操控單元上發(fā)定位數(shù)據(jù)時(shí)間間隔不一致，如時(shí)間列所示。

表1 爬游潛器下潛過(guò)程USBL 定位數(shù)據(jù)（截取部分）Tab.1 CSV USBL locating data of diving (fragmented)

異常值的隨機(jī)出現(xiàn)使無(wú)法直接采用USBL 數(shù)據(jù)進(jìn)行爬游潛器的定位顯示，時(shí)間間隔不一致的問(wèn)題會(huì)導(dǎo)致無(wú)法直接應(yīng)用常用濾波算法。

2.2 對(duì)爬游潛器在海底駐留過(guò)程數(shù)據(jù)分析

在爬游潛器單一完整潛次中，母船“深潛號(hào)”動(dòng)力定位開(kāi)啟，左舷擋風(fēng)擋流，盡量降低爬游潛器海試過(guò)程中的定位受環(huán)境和試驗(yàn)條件的影響。爬游潛器順利坐底后，在甲板單元未主動(dòng)下達(dá)運(yùn)動(dòng)指令時(shí)，其在海底的絕對(duì)位置不發(fā)生變化，通過(guò)分析此間140 s 時(shí)間內(nèi)母船與爬游潛器的相對(duì)定位數(shù)據(jù)，其相對(duì)位置均值及方差如表2 所示，爬游潛器x向、y向、z向位置隨時(shí)間的分布如圖2 所示。即使母船有先進(jìn)的動(dòng)力定位，但是浪和涌引起母船微弱的升沉和搖擺影響了USBL 基陣和應(yīng)答器的方位和距離。同時(shí)耦合了水聲測(cè)量的誤差，進(jìn)而影響了定位的準(zhǔn)確性和一致性，產(chǎn)生系統(tǒng)誤差。

表2 爬游潛器坐底USBL 定位數(shù)據(jù)數(shù)值特征Tab.2 CSV USBL statistic value of locating data motionless at bottom

圖2 爬游潛器坐底USBL 定位數(shù)據(jù)Fig.2 CSV USBL locating data of motionless at bottom

2.3 USBL 通訊錐角造成通信中斷

USBL 系統(tǒng)基陣與應(yīng)答器之間存在通訊錐角，不同廠商和型號(hào)均不同，一般錐角在60°～180°，錐角與基陣陣列的布置有關(guān)。當(dāng)潛航器與母船的相對(duì)位置超出USBL 通信錐角時(shí)，母船的USBL 基陣和爬游潛器的應(yīng)答器之間無(wú)法建立水聲通信，因此母船無(wú)法確切得到潛器的位置與出水點(diǎn)，存在潛航器與船體碰撞或者潛航器在母船底部或附近出水，被吸入母船螺旋槳的安全風(fēng)險(xiǎn)。

2.4 USBL 定位數(shù)據(jù)直接使用存在的問(wèn)題總結(jié)

通過(guò)上述對(duì)試驗(yàn)數(shù)據(jù)和USBL 自身特性的分析，直接使用USBL 的定位數(shù)據(jù)來(lái)確定爬游潛器的水下軌跡和位置存在誤差。同時(shí)也存在很大的安全風(fēng)險(xiǎn)，因此提出一種基于USBL 定位數(shù)據(jù)的遞歸最小二乘法軌跡預(yù)測(cè)算法[2]。

3 基于USBL 定位數(shù)據(jù)的軌跡預(yù)測(cè)算法

3.1 算法概述

針對(duì)USBL 數(shù)據(jù)直接定位存在問(wèn)題，提出如下爬游潛器的軌跡預(yù)測(cè)算法：

1）數(shù)據(jù)剔除異常值；

2）按水聲通信節(jié)拍，對(duì)定位時(shí)刻進(jìn)行聚類分析，通過(guò)拉格朗日二次多項(xiàng)式插值，將數(shù)據(jù)按周期對(duì)齊；

3）建立量測(cè)模型，通過(guò)遞歸最小二乘法對(duì)狀態(tài)量進(jìn)行最優(yōu)估計(jì)；

4）分段按照各最小二乘法的最優(yōu)估計(jì)值進(jìn)行多項(xiàng)式展開(kāi)，對(duì)周期間和周期外的定位軌跡進(jìn)行預(yù)測(cè)。

3.2 算法具體實(shí)施

1）數(shù)據(jù)去異常值

爬游潛器在水下慣性很大，運(yùn)動(dòng)變化率小，屬于緩慢運(yùn)動(dòng)，因此可以設(shè)置方位變化率閾值進(jìn)行定位數(shù)據(jù)異常值的剔除。

2）按水聲通信節(jié)拍，對(duì)定位數(shù)據(jù)進(jìn)行聚類分析，并進(jìn)行數(shù)據(jù)按周期對(duì)齊

爬游潛器水聲通信的周期是20 s，通信前基陣和應(yīng)答器需建立水聲通訊鏈路，定位信息是在通訊鏈路建立的握手過(guò)程和數(shù)據(jù)通過(guò)水聲通信鏈路收發(fā)過(guò)程中建立。從定位時(shí)刻分布看連續(xù)數(shù)次定位由單次水聲通信過(guò)程產(chǎn)生，如定位時(shí)刻表3 所示。

表3 爬游潛器坐底USBL 定位時(shí)刻表Tab.3 USBL locating instant when CSV motionless at bottom

爬游潛器下潛過(guò)程中的USBL 定位數(shù)據(jù)如圖3 所示（異常值已經(jīng)剔除），在下潛全過(guò)程中，水深較淺時(shí)，水聲通信建立比較差，定位次數(shù)也比較少；水深增大時(shí)，單次水聲通訊都可完成多次水聲并將定位信息上傳至上位機(jī)。但也從圖中發(fā)現(xiàn)，定位信息相對(duì)節(jié)拍點(diǎn)時(shí)刻并不完全對(duì)中，為能進(jìn)行最優(yōu)估計(jì)，定位數(shù)據(jù)需按周期給出。

圖3 爬游潛器下潛過(guò)程USBL 定位數(shù)據(jù)Fig.3 USBL locating data when CSV is diving

因此對(duì)USBL 定位數(shù)據(jù)進(jìn)行實(shí)時(shí)聚類[8]，分析單次水聲通信的定位值，選用拉格朗日插值法，對(duì)數(shù)據(jù)按時(shí)間進(jìn)行周期對(duì)齊，方法如下：取聚類分析后連續(xù)3 個(gè)時(shí)刻，使用拉格朗日二階多項(xiàng)式插值公式（1）進(jìn)行插值運(yùn)算[9]。

式中：(t1,f(t1))、(t2,f(t2))、(t3,f(t3))為聚類分析后的3 個(gè)已知點(diǎn)；l1(t)、l2(t)、l3(t)為二次拉格朗日插值的基函數(shù)；L2(t)為二次拉格朗日插值函數(shù)。

聚類和對(duì)齊后數(shù)據(jù)如圖4 所示，聚類后屬于同一次水聲通信產(chǎn)生的多次定位數(shù)據(jù)進(jìn)行算術(shù)平均，融合為一個(gè)點(diǎn)，聚類后數(shù)據(jù)分布不均勻，如圖4(a)所示。采用聚類后的數(shù)據(jù)進(jìn)行拉格朗日二次插值，對(duì)分段插值函數(shù)按周期點(diǎn)取函數(shù)值，結(jié)果如圖4(b)所示。

圖4 聚類后數(shù)據(jù)和對(duì)齊后數(shù)據(jù)Fig.4 Data after clustering and alligning

4 使用遞推最小二乘法進(jìn)行最優(yōu)估算，實(shí)時(shí)分段二次曲線擬合

USBL 定位有效數(shù)據(jù)從水深30 m 開(kāi)始，在這個(gè)水深潛航器受水面波浪影響較小，海流是爬游潛器下潛過(guò)程主要影響因素，它能使水下機(jī)器人運(yùn)動(dòng)阻力增大，并產(chǎn)生漂流運(yùn)動(dòng)，其形式多種多樣。其中定海流與潮流為主要影響類型，其的流向、流速的變化比較小或呈一定周期性，因此在一個(gè)時(shí)間點(diǎn)可近似認(rèn)為爬游潛器下潛過(guò)程的海洋流場(chǎng)為無(wú)限大穩(wěn)恒定流場(chǎng)[10]。因此按照上述假設(shè)建立爬游潛器下潛的狀態(tài)模型。將其下潛運(yùn)動(dòng)分解為X向、Y向和Z向。以X向?yàn)槔?，設(shè)tk為數(shù)據(jù)對(duì)齊時(shí)刻，zk為該時(shí)刻的量測(cè)值，狀態(tài)Xk定義為二次曲線的系數(shù)[akbkck]T，建立量測(cè)方程為：

式中：Pk為協(xié)方差矩陣；Xk為狀態(tài)估計(jì)；Wk為權(quán)重矩陣，取為單位矩陣。初始估計(jì)值X0選用USBL 最開(kāi)始3 點(diǎn)定位數(shù)據(jù)的多項(xiàng)式二次擬合的系數(shù)，P0選用一個(gè)很大的參數(shù)作為初始值。完成各周期點(diǎn)的系數(shù)狀態(tài)最小二乘估計(jì)后，計(jì)算出各個(gè)周期點(diǎn)的狀態(tài)值（即運(yùn)動(dòng)二次模型的曲線系數(shù)），然后在每個(gè)周期內(nèi)，進(jìn)行曲線展開(kāi)。對(duì)于節(jié)拍之間的時(shí)刻和由于USBL 水聲通信信道不穩(wěn)定或由于通信錐角導(dǎo)致的通信丟失時(shí)的時(shí)刻，估計(jì)出爬游潛器的下潛的軌跡如圖5 所示，估計(jì)的軌跡圖曲率變化連續(xù)，相較原始的數(shù)據(jù)更符合爬游潛器在水中緩慢連續(xù)變動(dòng)的運(yùn)動(dòng)特性。

圖5 爬游潛器下潛過(guò)程最小二乘法估計(jì)及分段曲線展開(kāi)Fig.5 RLS result of CSV diving data

5 結(jié) 語(yǔ)

針對(duì)使用USBL 進(jìn)行爬游潛器定位和軌跡預(yù)測(cè)存在的問(wèn)題，本文提出一種改進(jìn)型的遞歸最小二乘估計(jì)的實(shí)時(shí)處理算法，有效地解決了USBL 原始定位數(shù)據(jù)的數(shù)值和時(shí)間的隨機(jī)離散問(wèn)題，為爬游潛器深海作業(yè)的軌跡預(yù)測(cè)提供解決方案，同時(shí)也為其他類型的潛航器的軌跡預(yù)測(cè)提供借鑒。