黎南，湛鑫，陳濤，嚴浙平

(1.海軍駐大連地區(qū)軍代表室，遼寧 大連 116021;2.中國船舶重工集團公司 第703研究所，黑龍江 哈爾濱 150036;3.哈爾濱工程大學自動化學院，黑龍江 哈爾濱 150001)

有效地布放回收是UUV(unmanned underwater vehicle)能否廣泛應用的一個關(guān)鍵因素.相對于布放來說，UUV的回收難度很大.因此，世界上UUV領(lǐng)域的研究人員也發(fā)展出了各式各樣的回收方式，而且均以實現(xiàn)UUV的自動回收為最終目標［1］.本文研究了一種利用水下塢艙自動回收UUV的方式.在這種方式中，UUV需要以一定的定位導引方法識別UUV與塢艙的相對方向和相對位置，從而控制自己精確地進入塢艙之中.對于塢艙水下回收UUV中的定位導引系統(tǒng)，采用了適用遠距離定位的短基線定位系統(tǒng)和適用于近端精確定位的視覺導引定位系統(tǒng).由于2套定位系統(tǒng)在不同作用范圍下的精度和可靠性不同，為了UUV能簡單、高效、安全地實現(xiàn)與塢艙對接回收，本文重點研究了短基線和視覺在UUV水下回收中的定位融合方法，并通過水池試驗驗證了所提方法的有效性.

1 UUV水下回收定位系統(tǒng)及其原理

UUV的回收過程是一種空間立體運動，主要需要控制4個自由度:縱向、橫向、垂向和艏向.當UUV開始進入水下對接回收任務(wù)后，其回收方式類似于直升機降落的坐落塢艙式回收，如圖1所示.UUV從對接起始位置水平直航到回收塢艙的正上方，粗略調(diào)整姿態(tài)和艏向，然后垂直降落到距塢艙較近高度的正上方，接下來精確調(diào)整UUV的姿態(tài)和艏向，緩慢降落至回收塢艙上.UUV距離塢艙較遠時視覺定位精度差，SBL定位精度高;距離較近時，SBL聲定位將會出現(xiàn)盲區(qū)，而視覺定位的精度高［2］;距離中等時，2種定位信號均有效.

圖1 UUV水下回收方式Fig.1 UUV underwater docking mode

1.1 短基線定位系統(tǒng)及其原理

圖2是短基線定位系統(tǒng)的簡圖.聲吶基陣由安裝在 UUV 底部的4個換能器 T0、T1、T2、T3構(gòu)成，信標F置于塢艙上.

圖2 UUV水下回收中的短基線定位系統(tǒng)Fig.2 SBL position system for UUV underwater docking

聲吶基陣的4個換能器安裝在一個平面上，基陣的幾何中心為UUV的幾何中心，定義為短基線坐標系的原點，同時也是隨船坐標系的原點.x軸指向為UUV的艏向、y軸為UUV右舷并在基陣平面上、z軸垂直xoy平面指向水底，如圖3所示.

圖3 短基線定位系統(tǒng)坐標系及原理Fig.3 SBL position system coordinate and principle

圖3中換能器T0、T3間距為 a，T0、T1間距為 b.則在隨船坐標系下各換能器的坐標為T0(a/2，b/2，0)，T1(a/2，b/2，0)，T2(－ a/2，b/2，0)，T3(－ a/2，－b/2，0).

定義信標的坐標F(x，y，z)，并設(shè)信標到各個換能器的直線距離為 R0、R1、R2、R3，可以通過聲吶特性求得信標到基陣的距離.設(shè)換能器T0發(fā)射聲信號到信標F并接收到應答信號所需時間為t0，若聲速為 v，則有 R0=vt0/2，同理可得 R1、R2、R3.

由幾何關(guān)系，能得到短基線定位方程為:

消去z得:

由式(5)很容易求解出x、y.

根據(jù)式(1)～(4)可解得4個深度值z，把求解到的4個z值進行平均，得到z的平均值:

1.2 視覺定位系統(tǒng)及其原理

圖4是視覺定位系統(tǒng)的簡圖.

圖4 UUV水下回收中的視覺定位系統(tǒng)Fig.4 Vision position system for UUV underwater docking

在UUV的腹部位置安裝3個攝像頭，這樣配置可根據(jù)實際對接需要，配合塢艙上的導引，燈選擇單目視覺定位導引或雙目視覺定位導引方案.本文以單目視覺定位導引的原理為例進行介紹.在利用視覺定位導引UUV水下回收的過程中，主要需要獲取的是UUV相對于塢艙的艏向和位置信息.

1.2 .1 UUV相對塢艙艏向的確定

塢艙艏向依靠導引燈的安裝位置來確定.如圖5所示，在塢艙底部縱中線上等距離安裝了大小和形狀不同的8個導引燈，其中，正中心大燈的箭頭形狀能標示塢艙的艏向.當采用單目識別時，單攝像頭至少要能獲取到2個或以上的導引燈圖像，將幾個導引光源連線后就能判斷塢艙艏向角.

圖5 導引光源布置示意Fig.5 Guided lamp collocation sketch map

圖6 艏向和垂向視覺定位原理Fig.6 Heading and vertical position principle

1.2 .2 UUV相對塢艙位置的確定

1)垂向定位.

2)平移定位.

依據(jù)此幾何關(guān)系能求解出攝像頭光心和塢艙的相對位置.假設(shè)攝像頭坐標系到UUV隨船坐標系的變換矩陣為aHc=［△x 0 △z］T，其中△x、△z為攝像頭相對UUV中心的橫向和垂向距離，最終可以求解出UUV與塢艙的相對位置為

2 定位數(shù)據(jù)的自適應野值剔除方法

視覺和短基線定位傳感器均會產(chǎn)生野值.而利用帶野值的數(shù)據(jù)進行數(shù)據(jù)融合將會導致融合結(jié)果的嚴重錯誤.常用的剔除野值的方法有萊以特準則(3σ 準則)和格羅布斯(Grubbs)準則［3-4］.這2 種方法的基礎(chǔ)是統(tǒng)計決策，都需要預先設(shè)定門限，這就使它們失去了實時性.本文采取一種在線的自適應野值剔除算法，其野值剔除步驟如下:

1)首先把采集的數(shù)據(jù)每M個采樣時刻分為一組，去掉最大的和最小的，第1個數(shù)據(jù)(中間時刻的數(shù)據(jù))就取剩余的M－2個數(shù)的平均值;

2)同理得到了第2個數(shù)據(jù)(第2組數(shù)據(jù)中間時刻的數(shù)據(jù))，以及第N組數(shù)據(jù)中間時刻的數(shù)據(jù).依次這樣的步驟算n次，得到一組采樣值，先進行最小二乘法擬合數(shù)據(jù)N個采樣時刻的數(shù)值;

3)接著對上述處理的數(shù)據(jù)做殘差變化率計算，得到每一時刻的數(shù)據(jù)變化率;

4)去掉數(shù)據(jù)變化率中的最大值和最小值，將剩余的平均值作為平均數(shù)據(jù)變化率的初值;

5)實時計算k大于N×M以后的數(shù)據(jù)變化率:

6)當數(shù)據(jù)滿足以下2個條件時，就認為該數(shù)據(jù)是野值，要將其剔除:

對于野值，根據(jù)與變化率閾值的關(guān)系進行相應的替換.若data(k)大于thra(k)的變化率，將此數(shù)據(jù)用data(k－1)代替;若變化率介于 thra(k)和thrb(k)之間，將此數(shù)據(jù)用數(shù)據(jù) 2data(k－1)－data(k－2)代替.

圖7給出了在進行UUV水下回收水池試驗時，利用上述野值剔除方法對一段時間的SBL定位X軸向的數(shù)據(jù)進行在線野值剔除的結(jié)果.從圖7可以看到絕大部分的野值均被在線剔除掉了.

圖7 數(shù)據(jù)的野值剔除Fig.7 Abnormal value eliminating of data

3 定位數(shù)據(jù)的軟閾值小波去噪方法

短基線和視覺定位數(shù)據(jù)不可避免地會包含噪聲，這些噪聲包括信號干擾、水下聲信息傳輸時反射聲信號噪聲、攝像機在水下背景光變化等.

本文在進行定位數(shù)據(jù)融合前，采用一種軟閾值小波方法進行數(shù)據(jù)去噪.小波去噪的原理是:數(shù)據(jù)經(jīng)過小波正交變換后，實際信號的小波系數(shù)要大于噪聲信號的小波系數(shù)［5］.根據(jù)這一原理設(shè)定某個閾值，將低于閾值的小波系數(shù)進行衰減處理，一般直接設(shè)置成零，而高于閾值的小波系數(shù)則進行保留或者適當?shù)貕嚎s處理［6］.根據(jù)對小波閾值系數(shù)的處理方式不同，可以分為硬閾值濾波方法和軟閾值濾波方法，圖8給出了2種閾值的選取方法.

硬閾值的設(shè)置方式為

軟閾值的設(shè)置方式為

圖8 小波去噪的硬閾值和軟閾值方法Fig.8 Hard and soft limen method of wavelet de-noising

從圖8中可以看出硬閾值方法存在閾值λ處的不連續(xù)性，這將導致濾波后信號出現(xiàn)部分振蕩.而軟閾值方法在閾值λ處是連續(xù)的，間接反映到濾波后的信號光滑性［7］.本文采用軟閾值小波方法進行數(shù)據(jù)的去噪處理.

圖9為對進行野值剔除后的短基線定位數(shù)據(jù)應用軟閾值小波去噪后的結(jié)果，可以看出去噪效果比較理想.

圖9 數(shù)據(jù)的去噪Fig.9 De-noising of data

4 短基線與視覺定位自適應融合方法

4.1 系統(tǒng)模型描述

首先，建立視覺的測量模型如下:

同樣建立SBL的測量模型為

式中:Zs(k)=［xsyszs］T，xs、ys和 zs為 SBL 傳感器測得的UUV與對接中心點的2個軸向距離和高度距離，Hs為SBL定位的觀測轉(zhuǎn)移矩陣.

Vv(k)和Vs(k)分別為零均值的高斯測量噪聲，協(xié)方差為Rv(k)和Rs(k).

考慮一離散非線性動態(tài)過程模型1，如式(6)所示:

式中:X(k)∈Rn，為第k時刻系統(tǒng)狀態(tài)向量:

ω(k)∈Rn，為一組零均值高斯白噪聲序列，假設(shè)其協(xié)方差矩陣為 Q(k)=E［ω(k)ω(k)T］.F(k+1，k)∈Rn×n為狀態(tài)轉(zhuǎn)移矩陣，T 為采樣周期.Z(k)∈Rl，為傳感器測量向量，v(k)∈Rl為零均值高斯觀測噪聲，且其協(xié)方差為

將式(6)進行線性化得到如下方程:

考慮到式(6)所描述的模型，可以通過擴展卡爾曼濾波的方法得到估計值(k|k)和預測值(k+1|k).

估計的不確定性由如下矩陣得到

預測方程定義如下:

式中:用Zv(k)和Zs(k)代替了Zj(k).

最后，根據(jù)標準Bar-Shalom公式，將2個相互聯(lián)系的估計做最優(yōu)結(jié)合，可得到狀態(tài)的全局估計.

4.2 自適應卡爾曼融合方法

卡爾曼濾波方法(Kalman filter，KF)是應用最廣的數(shù)據(jù)融合方法，也是數(shù)據(jù)融合中進行位置估計的有效方法［8］.而傳統(tǒng)的KF算法要求先得到系統(tǒng)過程噪聲和測量噪聲的先驗統(tǒng)計信息，而在實際中，這是非常困難的.噪聲信息的估計誤差會嚴重降低KF算法的準確性，甚至會導致濾波器的發(fā)散［9］.

本文提出了一種基于模糊邏輯的在線自適應KF算法，通過獲取的實時測量數(shù)據(jù)，自適應調(diào)整噪聲的協(xié)方差矩陣.據(jù)此，推導出了基于更新的自適應估計算法來調(diào)節(jié)噪聲協(xié)方差矩陣R(k)，并且采用了協(xié)方差匹配的方法.這種方法的基本思想是使系統(tǒng)殘差的協(xié)方差實際值與理論值相一致.更新序列或殘差的理論協(xié)方差如式(7):

理論協(xié)方差可以通過KF算法實時得到.考慮到更新序列R(k)有效性，它的實際協(xié)方差C(k)通過大小為N的移動估計窗口內(nèi)的平均值估計得到公式如下:

式中:i0=k－N+1是估計窗口的第1個樣本.根據(jù)經(jīng)驗選擇窗口尺寸的大小，以進行統(tǒng)計濾波.在算法過程中，對2個協(xié)方差進行實時比較，如果發(fā)現(xiàn)R(k)的實際協(xié)方差與其理論值有差異，那么調(diào)整R來修正差異.為檢測理論值S(k)與實際值Cr(k)之間的差異，定義了一個新的變量，稱為匹配度(Dom).

采用模糊推理的方法來調(diào)整R.根據(jù)式(7)可以看出，R的增長會導致S的增長，反之亦然.因此，可以根據(jù)Dom的值通過R來改變S的值，最終減少S與之間的偏差.

綜上所述，可以定義自適應算法的以下3條基本原則:

1)如果Dom?0，保持R不變;

2)如果Dom＞0，R減少;

3)如果Dom＜0，R增加.

因此，R通過如下方式調(diào)整:

式中:△R(k)是每個時間采樣點R要增加或減少的值，R(k+1)是模糊推理系統(tǒng)的輸出值，Dom是輸入值.

4.3 試驗結(jié)果

圖10～12給出了水池試驗數(shù)據(jù)融合結(jié)果.

圖10 視覺和SBL的X軸向定位融合Fig.10 Vision and SBL position fusion in X axis

圖11 視覺和SBL的Y軸向定位融合Fig.11 Vision and SBL position fusion in Y axis

圖12 視覺和SBL的Z軸向定位融合Fig.12 Vision and SBL position fusion in Z axis

試驗數(shù)據(jù)選取了UUV在塢艙上方緩慢下降過程中的數(shù)據(jù).該過程中，X和Y軸向的SBL、視覺定位數(shù)據(jù)基本上保持在零位置上做類似周期的擺動，在Z方向上隨著UUV逐漸接近塢艙，SBL的定位精度越來越差.但融合后的定位數(shù)據(jù)是始終有效的，且精度較高，從實際回收對接效果看，能為UUV的回收提供可靠的定位數(shù)據(jù)源.

5 結(jié)束語

本文基于UUV所配置的視覺和短基線定位系統(tǒng)，對其水下回收中的定位進行融合，以提高定位的精度.首先采用自適應野值剔除和軟閾值小波去噪方法對定位數(shù)據(jù)進行預處理，然后利用自適應模糊融合方法進行定位融合.水池試驗表明，定位傳感器的絕大部分野值被剔除且去噪效果明顯，融合后的數(shù)據(jù)好于單個任意定位傳感器的數(shù)據(jù)，能為UUV的水下回收提供更好的定位數(shù)據(jù)源.

［1］燕奎臣，吳利紅.AUV水下對接關(guān)鍵技術(shù)研究［J］.機器人，2007，29(3):267-273.

YAN Kuichen，WU Lihong.A survey on the key technologies for underwater AUV docking［J］.Robot，2007，29(3):267-273.

［2］吳永亭，周興華，楊龍.水下聲學定位系統(tǒng)及其應用［J］.海洋測繪.2003，23(4):18-21.

WU Yongting，ZHOU Xinghua，YANG Long.Underwater acoustic positioning system and its application［J］.Oceanic Topography，2003，23(4):18-21.

［3］朱榮生，施小成.一種用于GPS數(shù)據(jù)處理中剔除野值的算法［J］.中國慣性技術(shù)學報，2006，8(2):27-30.

ZHU Rongsheng，SHI Xiaocheng.Method of rejecting outliers in the data process of GPS［J］.Journal of Chinese Inertial Technology，2006，8(2):27-30.

［4］張帆，盧崢.自適應抗野值Kalman濾波［J］.電機與控制學報，2007，11(2):188-195.

ZHANG Fan，LU Zheng.Robust Kalman filter for outliers suppression［J］.Electric Machines and Control，2007，11(2):188-195.

［5］孫延奎.小波分析及應用［M］.北京:機械工業(yè)出版社，2005:6-8.

［6］DONOHO D L.De-oising by Soft-threhoding［J］.IEEE Trans on Info Theory，1995，5(41):613-627.

［7］DONG C Y，YUAN Q.A combined wavelet analysis-fuzzy adaptive algorithm for rada/infrared data fusion［J］.Expert Systems with Aplications，2010，37(3):2563-2570.

［8］李洪志.信息融合技術(shù)［M］.北京:國防工業(yè)出版社，1996:1-5.

［9］鄧自立.信息融合濾波理論及其應用［M］.哈爾濱:哈爾濱工業(yè)大學出版社，2007:10-13.

［10］宋振華，戰(zhàn)興群.基于多傳感器融合的水下機器人導航系統(tǒng)［J］.微計算機信息，2008，24(2):240-241.SONG Zhenhua，ZHAN Xingqun.Autonomous navigation of underwater vehicle based on multi-sensor fusion technology［J］.Microcomputer Information，2008，24(2):240-241.