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

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

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

2.江南工業集團有限公司，長沙 410025)

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

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

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

1 融合數據的預處理

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

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

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

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

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

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

傳感器的空間配準主要是對傳感器在各個坐標系下進行空間坐標變化，將各個傳感器的坐標配準到同一坐標系統下，一般都配準到傳感器精度較高的傳感器系統坐標下［5］。對于視覺傳感器和SBL數據融合系統，視覺傳感器在其有效的距離內傳感器精度要遠高于SBL，選擇將SBL的空間配準到視覺傳感器坐標系統下。

試驗采用高度計在初始位置對AUV和對接平臺靜止數據進行初始校準。

2 系統模型描述

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

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

SBL的測量模型如式(3)

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

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

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

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

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

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

首先得到如下估計方程

最后，根據標準Bar-Shalom公式，將兩個相互聯系的估計做最優結合，即可得到狀態的全局估計如式(15)所示

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

3 模糊自適應EKF融合算法

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

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

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

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

定義殘差的實際協方差和理論協方差的比值

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，調整系數t(k)的FIS規則如下:

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 實驗結果

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

圖1 X軸向數據融合

圖2 Y軸向數據融合

圖3 Z軸向數據融合

5 結束語

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

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

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

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

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

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

［6］ 程崢.多傳感器目標跟蹤中的數據融合理論與算法［D］.西安:西北工業大學，2007:9 －15.

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

［8］ 宋振華，戰興群，張炎華.基于多傳感器融合的水下機器人導航系統［J］.機器人技術，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］嚴濤，王躍剛，楊波，等.模糊自適應卡爾曼濾波算法在航位推算系統中的應用［J］.計算機測量與控制，2012，20(3):774－796.

(責任編輯楊繼森)