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

黎南，湛鑫，陳濤，嚴(yán)浙平

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

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

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

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

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

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

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

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

聲吶基陣的4個(gè)換能器安裝在一個(gè)平面上，基陣的幾何中心為UUV的幾何中心，定義為短基線坐標(biāo)系的原點(diǎn)，同時(shí)也是隨船坐標(biāo)系的原點(diǎn).x軸指向?yàn)閁UV的艏向、y軸為UUV右舷并在基陣平面上、z軸垂直xoy平面指向水底，如圖3所示.

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

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

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

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

消去z得:

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

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

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

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

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

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

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

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

圖5 導(dǎo)引光源布置示意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è)攝像頭坐標(biāo)系到UUV隨船坐標(biāo)系的變換矩陣為aHc=［△x 0 △z］T，其中△x、△z為攝像頭相對UUV中心的橫向和垂向距離，最終可以求解出UUV與塢艙的相對位置為

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4 短基線與視覺定位自適應(yīng)融合方法

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

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

同樣建立SBL的測量模型為

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

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

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

式中:X(k)∈Rn，為第k時(shí)刻系統(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)進(jìn)行線性化得到如下方程:

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

估計(jì)的不確定性由如下矩陣得到

預(yù)測方程定義如下:

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

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

4.2 自適應(yīng)卡爾曼融合方法

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

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

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

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

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

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

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

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

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

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

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

4.3 試驗(yàn)結(jié)果

圖10～12給出了水池試驗(yàn)數(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

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

5 結(jié)束語

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

［1］燕奎臣，吳利紅.AUV水下對接關(guān)鍵技術(shù)研究［J］.機(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］吳永亭，周興華，楊龍.水下聲學(xué)定位系統(tǒng)及其應(yī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ù)學(xué)報(bào)，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］張帆，盧崢.自適應(yīng)抗野值Kalman濾波［J］.電機(jī)與控制學(xué)報(bào)，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］孫延奎.小波分析及應(yīng)用［M］.北京:機(jī)械工業(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］鄧自立.信息融合濾波理論及其應(yīng)用［M］.哈爾濱:哈爾濱工業(yè)大學(xué)出版社，2007:10-13.

［10］宋振華，戰(zhàn)興群.基于多傳感器融合的水下機(jī)器人導(dǎo)航系統(tǒng)［J］.微計(jì)算機(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.