UUV水下回收中的視覺和短基線定位融合

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

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

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

1 UUV水下回收定位系統及其原理

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

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

1.1 短基線定位系統及其原理

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

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

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

圖3 短基線定位系統坐標系及原理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)，并設信標到各個換能器的直線距離為 R0、R1、R2、R3，可以通過聲吶特性求得信標到基陣的距離.設換能器T0發射聲信號到信標F并接收到應答信號所需時間為t0，若聲速為 v，則有 R0=vt0/2，同理可得 R1、R2、R3.

由幾何關系，能得到短基線定位方程為:

消去z得:

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

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

1.2 視覺定位系統及其原理

圖4是視覺定位系統的簡圖.

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

在UUV的腹部位置安裝3個攝像頭，這樣配置可根據實際對接需要，配合塢艙上的導引，燈選擇單目視覺定位導引或雙目視覺定位導引方案.本文以單目視覺定位導引的原理為例進行介紹.在利用視覺定位導引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)平移定位.

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

2 定位數據的自適應野值剔除方法

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

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

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

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

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

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

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

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

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

圖7 數據的野值剔除Fig.7 Abnormal value eliminating of data

3 定位數據的軟閾值小波去噪方法

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

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

硬閾值的設置方式為

軟閾值的設置方式為

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

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

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

圖9 數據的去噪Fig.9 De-noising of data

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

4.1 系統模型描述

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

同樣建立SBL的測量模型為

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

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

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

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

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

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

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

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

預測方程定義如下:

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

最后，根據標準Bar-Shalom公式，將2個相互聯系的估計做最優結合，可得到狀態的全局估計.

4.2 自適應卡爾曼融合方法

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

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

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

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

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

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

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

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

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

因此，R通過如下方式調整:

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

4.3 試驗結果

圖10～12給出了水池試驗數據融合結果.

圖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

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

5 結束語

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

［1］燕奎臣，吳利紅.AUV水下對接關鍵技術研究［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］吳永亭，周興華，楊龍.水下聲學定位系統及其應用［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數據處理中剔除野值的算法［J］.中國慣性技術學報，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］.北京:機械工業出版社，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］李洪志.信息融合技術［M］.北京:國防工業出版社，1996:1-5.

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

［10］宋振華，戰興群.基于多傳感器融合的水下機器人導航系統［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.