基于改進遞歸最小二乘估計的潛器軌跡預(yù)測

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

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

0 引 言

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

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

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

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

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

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

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

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

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

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

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

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

在爬游潛器單一完整潛次中，母船“深潛號”動力定位開啟，左舷擋風(fēng)擋流，盡量降低爬游潛器海試過程中的定位受環(huán)境和試驗條件的影響。爬游潛器順利坐底后，在甲板單元未主動下達運動指令時，其在海底的絕對位置不發(fā)生變化，通過分析此間140 s 時間內(nèi)母船與爬游潛器的相對定位數(shù)據(jù)，其相對位置均值及方差如表2 所示，爬游潛器x向、y向、z向位置隨時間的分布如圖2 所示。即使母船有先進的動力定位，但是浪和涌引起母船微弱的升沉和搖擺影響了USBL 基陣和應(yīng)答器的方位和距離。同時耦合了水聲測量的誤差，進而影響了定位的準確性和一致性，產(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)答器之間存在通訊錐角，不同廠商和型號均不同，一般錐角在60°～180°，錐角與基陣陣列的布置有關(guān)。當(dāng)潛航器與母船的相對位置超出USBL 通信錐角時，母船的USBL 基陣和爬游潛器的應(yīng)答器之間無法建立水聲通信，因此母船無法確切得到潛器的位置與出水點，存在潛航器與船體碰撞或者潛航器在母船底部或附近出水，被吸入母船螺旋槳的安全風(fēng)險。

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

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

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

3.1 算法概述

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

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

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

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

4）分段按照各最小二乘法的最優(yōu)估計值進行多項式展開，對周期間和周期外的定位軌跡進行預(yù)測。

3.2 算法具體實施

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

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

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

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

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

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

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

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

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

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

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

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

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

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

圖5 爬游潛器下潛過程最小二乘法估計及分段曲線展開Fig.5 RLS result of CSV diving data

5 結(jié) 語

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