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

孫海濤，晏正新，王 玥，張 皓

(1.中國船舶重工集團(tuán)公司第七一〇研究所，湖北宜昌 443003;

2.江南工業(yè)集團(tuán)有限公司，長沙 410025)

AUV(Autonomous Underwater Vehicle)水下自主對接技術(shù)為其完成水下作業(yè)能源供給、任務(wù)使命下載和數(shù)據(jù)回傳等任務(wù)提供了重要技術(shù)保障。

在AUV與水下平臺自主對接中，成功對接的關(guān)鍵在于近端對接時AUV姿態(tài)的數(shù)據(jù)實時準(zhǔn)確獲取和對AUV的精準(zhǔn)控制，傳感器數(shù)據(jù)的精準(zhǔn)程度直接決定了完成自主對接的效果，一般采用精度較高的視覺傳感器和短基線(Short Baseline，SBL)定位系統(tǒng)進(jìn)行數(shù)據(jù)融合［1］。AUV與傳感器間的距離決定了這兩種傳感器的能力，當(dāng)距離較遠(yuǎn)時，SBL發(fā)揮主要作用，其獲取的姿態(tài)數(shù)據(jù)足以使AUV朝大體的方向接近對接平臺;當(dāng)距離適中時，兩種傳感器同時工作，數(shù)據(jù)融合系統(tǒng)開始發(fā)揮作用［2］。

由于視覺傳感器和SBL定位系統(tǒng)在不同作用范圍下的精度和可靠性不同，為了使AUV能在兩套定位系統(tǒng)同時工作時實現(xiàn)精準(zhǔn)的自主對接，本文重點研究了兩套定位系統(tǒng)同時工作時的定位數(shù)據(jù)融合方法，并通過水池試驗驗證了方法的有效性。

1 融合數(shù)據(jù)的預(yù)處理

本文在傳感器數(shù)據(jù)融合之前，對傳感器采集過來的數(shù)據(jù)采用了一種基于軟閾值小波方法進(jìn)行數(shù)據(jù)濾波［3－4］。

在AUV與平臺對接過程中，AUV攜帶了多種傳感器，用到的視覺傳感器和SBL每次開機(jī)時間并不是同步的，并且視覺傳感器的采樣頻率為0.5 Hz，SBL傳感器的頻率為2 Hz，由于傳感器測量數(shù)據(jù)的時間并不嚴(yán)格均勻，數(shù)據(jù)并不是都為均勻采樣，存在著時間基準(zhǔn)點和采樣基準(zhǔn)點的不統(tǒng)一。另外各個傳感器安裝的位置不同，所獲取的空間位置必須進(jìn)行相應(yīng)的空間坐標(biāo)變換，統(tǒng)一到同一參考坐標(biāo)系下來。在進(jìn)行數(shù)據(jù)融合之前，傳感器的數(shù)據(jù)必須要進(jìn)行時間和空間的統(tǒng)一配準(zhǔn)。

通常為簡化算法，選擇一個傳感器做為融合中心時基，其他的傳感器都隨此融合中心進(jìn)行配準(zhǔn)，作為中心時基的傳感器一般要求測量頻率較高。在視覺傳感器和SBL數(shù)據(jù)融合系統(tǒng)中，以SBL傳感器的時間基作為融合中心基，將視覺傳感器的數(shù)據(jù)匹配到SBL時間基上，視覺傳感器的采樣大多為0.5 Hz，但存在一定比例的數(shù)據(jù)采樣頻率并不嚴(yán)格穩(wěn)定，其采樣依然看作是不均勻采樣，對采樣時刻不均勻數(shù)據(jù)一般采用曲線擬合法，在具體的時間配準(zhǔn)過程應(yīng)用如下步驟:

1)各個傳感器的開機(jī)時間不一樣，數(shù)據(jù)融合的起點設(shè)定在最晚開機(jī)的傳感器之后;

2)對于視覺采樣的數(shù)據(jù)，選取N個采樣點先進(jìn)行數(shù)據(jù)擬合，得到基于這N個點的光滑曲線函數(shù)y=f(x);

3)根據(jù)SBL的采樣頻率，選取下一拍即x(k+1)時間點得到擬合的視覺采樣數(shù)據(jù)，如果視覺傳感器在這的拍點上未進(jìn)行采樣，則將擬合的數(shù)據(jù)作為次拍的采樣數(shù)據(jù)，反之，則用視覺傳感器實測數(shù)據(jù)作為采樣數(shù)據(jù)，送入融合中心。

傳感器的空間配準(zhǔn)主要是對傳感器在各個坐標(biāo)系下進(jìn)行空間坐標(biāo)變化，將各個傳感器的坐標(biāo)配準(zhǔn)到同一坐標(biāo)系統(tǒng)下，一般都配準(zhǔn)到傳感器精度較高的傳感器系統(tǒng)坐標(biāo)下［5］。對于視覺傳感器和SBL數(shù)據(jù)融合系統(tǒng)，視覺傳感器在其有效的距離內(nèi)傳感器精度要遠(yuǎn)高于SBL，選擇將SBL的空間配準(zhǔn)到視覺傳感器坐標(biāo)系統(tǒng)下。

試驗采用高度計在初始位置對AUV和對接平臺靜止數(shù)據(jù)進(jìn)行初始校準(zhǔn)。

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

目標(biāo)測量向量位于直角標(biāo)系下，并且視覺的測量模型如式(1)

其中，Zv(k)=［xvyvzv］T，xv、yv和 zv為視覺傳感器測得AUV與對接中心點兩個軸向距離和高度距離。

SBL的測量模型如式(3)

其中，Zs(k)=［xsyszs］T，xs、ys和 zs為 SBL 傳感器測的AUV與對接中心點兩個軸向距離和高度距離。

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

離散非線性動態(tài)過程模型如式(5)

其中，X(k)∈Rn，X(k)為第k時刻系統(tǒng)的狀態(tài)向量，ω(k)∈Rn，ω(k)為一組零均值高斯白噪聲序列，假設(shè)其協(xié)方差矩陣為 Q(k)，Q(k)=E［ω(k)ω(k)T］。F(k+1，k)∈Rn×n，F(xiàn)(k+1，k)，為已知的狀態(tài)轉(zhuǎn)移矩陣。

式(6)中，T為采樣周期。Z(k)∈Rl，Z(k)為傳感器測量向量，v(k)∈Rl，v(k)為零均值高斯觀測噪聲，且其協(xié)方差為R(k)=E［v(k)v(k)T］。

將式(3)進(jìn)行線性化得到如下方程

首先得到如下估計方程

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

P(k|k)為全局誤差協(xié)方差;Pvs( k|k)和Psv(k|k)為兩傳感器的交互協(xié)方差，并且為了方便，將其表示為Pvs和Psv;Pv( k|k)和Ps( k|k)為兩傳感器的局部誤差協(xié)方差，表示為Pv和Ps。

3 模糊自適應(yīng)EKF融合算法

傳統(tǒng)的卡爾曼濾波算法要求先得到系統(tǒng)過程噪聲和測量噪聲的先驗統(tǒng)計信息，即矩陣Q(k)和R(k)。在實際應(yīng)用中，這些數(shù)據(jù)信息的初始值均為估計得到，是根據(jù)系統(tǒng)的先前經(jīng)驗而得到的，并且不能做到與系統(tǒng)狀態(tài)時刻進(jìn)行的實時更新，這種先驗的估計的準(zhǔn)確程度的決定了算法的的精確度［6］。也就是說EKF估計算法的實際應(yīng)用效果與這些先驗的統(tǒng)計信息有著直接的關(guān)系［7］。事實證明，噪聲信息的估計誤差會嚴(yán)重降低EKF算法的準(zhǔn)確性，甚至?xí)?dǎo)致濾波器的發(fā)散［8，9］。

因此，本文提出了一種基于模糊邏輯的在線自適應(yīng)卡爾曼算法。通過獲取的實時測量數(shù)據(jù)，自適應(yīng)調(diào)整噪聲的協(xié)方差矩陣，這種方法的基本思想是使系統(tǒng)殘差的協(xié)方差實際值與理論值相一致［10］。殘差 R(k)如式(16)所示的理論協(xié)方差

實際協(xié)方差C(k)通過大小為N的移動估計窗口內(nèi)的平均值估計得到，如式(17)所示

其中:i0=k－N+1是估計窗口的第1個樣本。根據(jù)經(jīng)驗選擇窗口尺寸的大小，以進(jìn)行統(tǒng)計濾波。在算法運行過程中，對實際協(xié)方差和理論協(xié)方差進(jìn)行實時比較，如果R(k)的實際協(xié)方差和理論協(xié)方差有差異，那么調(diào)整R(k)修正差異。

定義殘差的實際協(xié)方差和理論協(xié)方差的比值

Tr()表示對矩陣求跡。從式(18)可知，如果量測噪聲增大，則(k)增大，繼而Ra(k)增大，此時需要增大R(k)使Ra(k)回到1附近;反之，如果量測噪聲減少，就需要減小Ra(k)。Ra(k)的變化由t(k)來控制，即t(k)=FIS(Ra(k))。

定義模糊子集equal1表示在1附近，more1表示基本大于1，less1表示基本小于1，調(diào)整系數(shù)t(k)的FIS規(guī)則如下:

1)if Ra(k)is equal1，then t(k)is equal1

2)if Ra(k)is lessl1，then t(k)is less1

3)if Ra(k)is more1，then t(k)is more1

4 實驗結(jié)果

圖1～圖3為AUV對視覺數(shù)據(jù)和SBL數(shù)據(jù)進(jìn)行自適應(yīng)融合后的水池試驗結(jié)果，選取系統(tǒng)開機(jī)后350 s到550 s短基線定位系統(tǒng)和視覺定位系統(tǒng)獲取AUV三個軸向上的位置數(shù)據(jù)，經(jīng)過模糊自適應(yīng)算法進(jìn)行數(shù)據(jù)融合后，其結(jié)果表明融合后的位置數(shù)據(jù)好于單個任意傳感器系統(tǒng)的數(shù)據(jù)，能為AUV水下對接控制系統(tǒng)提供更為可靠的定位數(shù)據(jù)。

圖1 X軸向數(shù)據(jù)融合

圖2 Y軸向數(shù)據(jù)融合

圖3 Z軸向數(shù)據(jù)融合

5 結(jié)束語

本文研究了基于視覺與短基線定位系統(tǒng)的AUV數(shù)據(jù)融合定位方法，為提高水下定位精度，提出了一種基于模糊自適應(yīng)思想的卡爾曼濾波數(shù)據(jù)融合方法。水池試驗結(jié)果表明:融合后的定位數(shù)據(jù)優(yōu)于單傳感器定位系統(tǒng)，為AUV水下自主對接提供了更可靠的定位數(shù)據(jù)。

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

［2］ 晏正新.UUV回收對接的數(shù)據(jù)融合及預(yù)報方法研究［D］.哈爾濱，哈爾濱工程大學(xué)，2012:38 －41.

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

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

［5］ 鄧自立.信息融合濾波理論及其應(yīng)用［M］.哈爾濱:哈爾濱工業(yè)大學(xué)出版社，2007:78－156;272－404.

［6］ 程崢.多傳感器目標(biāo)跟蹤中的數(shù)據(jù)融合理論與算法［D］.西安:西北工業(yè)大學(xué)，2007:9 －15.

［7］ 王曉娟.基于視覺的AUV水下回收導(dǎo)引定位技術(shù)研究［D］.哈爾濱:哈爾濱工程大學(xué)，2011:68 －69.

［8］ 宋振華，戰(zhàn)興群，張炎華.基于多傳感器融合的水下機(jī)器人導(dǎo)航系統(tǒng)［J］.機(jī)器人技術(shù)，2008，24(1－2):240－241.

［9］ GAN Q，HARRIS C.J.Comparison of two measurement fusion methods for kalman-filter based multisensor data fusion［J］.IEEE Trans.on Aerospace and Electronic Systems，2001，37(1):273 －279.

［10］嚴(yán)濤，王躍剛，楊波，等.模糊自適應(yīng)卡爾曼濾波算法在航位推算系統(tǒng)中的應(yīng)用［J］.計算機(jī)測量與控制，2012，20(3):774－796.

(責(zé)任編輯楊繼森)