自主水下機器人回收導(dǎo)航多傳感器融合研究

包靈卉，曾慶軍，朱志宇，戴曉強

(江蘇科技大學(xué)電子信息學(xué)院，江蘇鎮(zhèn)江212003)

1 引言

自主水下機器人(Autonomous Underwater Vehicle，AUV)作為探索和開發(fā)海洋的新型手段，近年來成為當今研究的重要領(lǐng)域。隨著應(yīng)用市場的日益復(fù)雜，AUV需要更長的工作時間、更強的自主性及更強的通訊能力[1]。由于AUV自身攜帶能源有限且水下通信速率低，所以需要及時導(dǎo)航定位、布放回收，以用于能源補充、數(shù)據(jù)上傳或者接受新的任務(wù)[2]。

由于目前常用的水下導(dǎo)航設(shè)備有捷聯(lián)慣性導(dǎo)航系統(tǒng)(Strapdown Inertial Navigation Systems，SINS)、多普勒里程儀(Doppler Velocity Log，DVL)及超短基線定位系統(tǒng)(Ultra-Short Baseline，USBL)等。以上幾種傳感器可以通過先進的濾波融合技術(shù)進行融合，達到AUV對接回收中導(dǎo)航的精度要求[3-4]。USBL定位系統(tǒng)配備的基陣一般尺寸小且安裝簡易，可以根據(jù)實際情況設(shè)計其布陣方式，來增強系統(tǒng)的定位精度。挪威的Kongsberg Simrad公司研發(fā)了一款名為HiPAP超短基線定位系統(tǒng)，定位精度可以達到0.02m，其中HiPAP102型超短基線工作水深達到了10000m。國內(nèi)對USBL的技術(shù)研究較晚，主要研究機構(gòu)有哈爾濱工程大學(xué)、浙江大學(xué)、中科院沈陽自動化研究所等[5-6]。最具代表性的是哈爾濱工程大學(xué)研究的USBL定位系統(tǒng)，2014年在“蛟龍?zhí)枴鄙狭料啵瑸橹袊暮Ｑ笫聵I(yè)做出巨大貢獻；浙江大學(xué)流體動力與機電系統(tǒng)國家重點實驗室針對在AUV入塢過程中喇叭狀的回收塢對海豚-ⅡAUV的影響作用，進行了一種光學(xué)引導(dǎo)回收對接的研究；沈陽自動化研究所設(shè)計研究了回收中的對接塢，其主要對于AUV進入對接塢時的各種水動力問題展開了研究；但目前國內(nèi)在AUV回收組合導(dǎo)航多傳感器融合這方面的研究開展得較少。

文獻[4]提出一種基于人工蜂群算法(ABC)改進的徑向基函數(shù)(RBF)神經(jīng)網(wǎng)絡(luò)增強自適應(yīng)無跡卡爾曼濾波算法(AUKF)，但是其公式復(fù)雜，在實際情況下很難實現(xiàn)。文獻[5]采用聯(lián)邦濾波技術(shù)設(shè)計了SINSUSBLDVL組合導(dǎo)航系統(tǒng)方案，提出了一種改進的自適應(yīng)UKF算法，改善了濾波精度和發(fā)散問題，但是在建模中采用松組合的方式，USBL系統(tǒng)需經(jīng)過一系列的相位差和方位角解算方可進入濾波器中進行濾波，在濾波之前就易產(chǎn)生不小的誤差。本文為了提高AUV在回收過程中的導(dǎo)航定位精度，在兩方面做了改進和創(chuàng)新。在建模方面，采用改進的基于斜距的SINSUSBLDVL多傳感器融合系統(tǒng)方案，用基于斜距的建模方式避免了USBL系統(tǒng)中的基陣相位差計算的誤差；在濾波算法方面，提出了自適應(yīng)無跡卡爾曼濾波算法(Adaptive Unscented kalman filtering，AUKF)，以無跡卡爾曼濾波為原型構(gòu)建優(yōu)化問題，消除濾波的奇異值和發(fā)散問題。仿真證得基于斜距的SINSUSBLDVL多傳感器融合系統(tǒng)和AUKF算法能夠有效地降低AUV導(dǎo)航系統(tǒng)的定位誤差，可以達到AUV安全回收的要求。

2 AUV回收系統(tǒng)原理

根據(jù)此AUV運動系統(tǒng)的特點對AUV對接過程分為四個階段，如圖1所示，在整個對接回收過程中，主要在前兩個階段即直線歸位階段和直線跟蹤階段采用多傳感器融合的方式進行導(dǎo)航，在USBL作用范圍，為了使AUV在耗能較少的情況下航行至中軸線上，需要時刻調(diào)整AUV與回收塢的相對位置，這就需要利用基于USBL的多傳感器融合提供的相對位置/姿態(tài)信息，當AUV即將進入直線對接階段時(距離對接口3到5米)，需要改變控制策略，即通過視覺導(dǎo)航提供的相對位置/姿態(tài)信息來調(diào)整 AUV 與回收塢的位置，保證AUV沿中軸線航行，且艏向角指向?qū)涌赱5]。

圖1 AUV回收對接流程框圖

如圖2所示，USBL發(fā)射基陣6安裝在回收塢2上，聲單元之間的相互位置已精確測定且構(gòu)成聲基陣坐標系4，應(yīng)答器5安裝在AUV自主航行器3上；該系統(tǒng)通過測定水聽器的相位差來計算AUV到回收塢的水平和垂直角度，在通過測定聲波的傳播時間來確定AUV到回收塢的相對距離1，最終得到AUV相對于換能器的位置信息。從而確定水下載體的相對位置。

圖2 超短基線水下定位原理圖

圖3為USBL發(fā)射基陣排列圖，發(fā)射基陣是由四個水聽器組成的，基陣中心安裝一個換能器用于發(fā)射信號。

圖3 USBL發(fā)射基陣排列圖

本文以江蘇科技大學(xué)自主研發(fā)的“探海I型”AUV為研究對象，如圖4所示。光纖慣導(dǎo)SINS選用中科探海(蘇州)海洋科技有限責任公司定制的GIF6536A型光纖慣導(dǎo)，計程儀DVL選用LinkQuest公司的NavQuest 600 Micro DVL，深度計采用MICROSENSOR公司的MPM3808型I2C數(shù)字壓力傳感器。USBL選用的是Blueprint公司生產(chǎn)的SeaTrac X150 USBL超短基線定位系統(tǒng)。

圖4 “探海I型”AUV

3 多傳感器融合系統(tǒng)結(jié)構(gòu)建模

3.1 三種傳感器融合系統(tǒng)狀態(tài)方程設(shè)計

在SINSUSBLDVL多傳感器融合系統(tǒng)中，雖然SINS具有完全的自主導(dǎo)航能力，但由于陀螺存在隨機漂移，容易出現(xiàn)發(fā)散的問題。多普勒計程儀可以從載體發(fā)射水聲信號的回波中解算出載體的對地速度。利用DVL精確的速度信息，可以有效地抑制SINS導(dǎo)航誤差的發(fā)散。在此系統(tǒng)中DVL的速度信息可以與SINS解算的速度信息做差作為量測值，再通過濾波器進行最優(yōu)估計，從而修正導(dǎo)航定位信息。USBL系統(tǒng)一般通過直接解算時延差和斜距得到的AUV的絕對位置信息來輔助SINS系統(tǒng)，但USBL系統(tǒng)的基陣極易偏移，產(chǎn)生的誤差將影響AUV的定位精度和穩(wěn)定性[6]。本文設(shè)計了一種新的計算方法，以到達距離差和斜距之差，作為濾波器的量測值。USBL系統(tǒng)的發(fā)射端由多個水聽器組成，通過回波到達每個水聽器之間的時延差計算而得的距離差，再測得回波到達中心水聽器的時間得到斜距；這是USBL定位系統(tǒng)得到的到達距離差和斜距。至于SINS定位系統(tǒng)，通過AUV的位置信息的坐標轉(zhuǎn)換可得到AUV上各水聽器的位置，從而可得到各水聽器與回收塢上的到達距離差和斜距，分別將兩系統(tǒng)計算的到達距離差和斜距做差，作為濾波器的量測值進行濾波融合，再由精確的到達距離差和斜距通過物理計算得到相對位置信息。綜上所述設(shè)計出基于到達距離差和斜距的SINSUSBLDVL多傳感器融合子系統(tǒng)原理框圖，如圖5所示。

圖5 SINSUSBLDVL多傳感器融合系統(tǒng)原理圖

選擇SINS的速度誤差、姿態(tài)誤差、位置誤差、加速度計零偏和陀螺漂移作為狀態(tài)變量，則SINS系統(tǒng)狀態(tài)變量為

XSINS=[δVEδVNδVUφEφNφUδLδλ

(1)

(2)

WSINS為過程噪聲矩陣。GSINS為過程噪聲驅(qū)動矩陣。

FSINS可由SINS誤差方程確定。

根據(jù)改進的基于斜距的USBL的誤差模型，選取到達距離差和斜距誤差作為狀態(tài)量，USBL系統(tǒng)狀態(tài)變量為

(3)

式中，δRUSBLX為超短基線系統(tǒng)的斜距誤差，δRUSBL13、δRUSBL24為接收器到兩個水聽器之間的距離差的量測誤差。

建立USBL系統(tǒng)狀態(tài)方程為

(4)

其中，狀態(tài)轉(zhuǎn)移矩陣為

(5)

式中，τδR、τδΔR13、τδΔR24分別為一階馬爾科夫過程的相關(guān)時間，即接收器到中心換能器的時間、接收器到1、3水聽器的時間差和接收器到2、4水聽器的時間差。

根據(jù)DVL的誤差模型，選取東北天向速度誤差作為狀態(tài)量，則DVL狀態(tài)向量為

(6)

其中，δVEDVL，δVNDVL，δVUDVL是DVL的東向，北向，天向測速誤差，均假設(shè)為白噪聲。

建立DVL系統(tǒng)狀態(tài)方程為

(7)

其中，狀態(tài)轉(zhuǎn)移矩陣為

(8)

綜上所述可得SINSUSBLDVL系統(tǒng)狀態(tài)方程為

(9)

3.2 三種傳感器融合系統(tǒng)量測方程設(shè)計

在SINSUSBLDVL多傳感器融合系統(tǒng)中，將DVL的速度信息及USBL的位置信息分別與SINS的相應(yīng)信息相減得到量測量。量測量ZSINS/USBL/DVL可進一步表示為

(10)

但為了避免系統(tǒng)由坐標轉(zhuǎn)換和基陣偏移帶來的誤差，本文設(shè)計了新的組合模型，即將SINS和USBL的到達距離差和斜距之差作為其中量測量，由改進的基于斜距的多傳感器融合的原理，量測量ZSINS/USBL/DVL可進一步表示為

(11)

由此可以得出SINSUSBLDVL系統(tǒng)的量測方程為

(12)

其中，量測矩陣為

(13)

(14)

其中，δVED、δVND、δVUD為DVL的東北天向測速誤差，δR、δR13、δR24為USBL的斜距和到達距離差的誤差。

3.3 多傳感器融合中AUKF算法

一般來說，Kalman濾波是狀態(tài)估計中相對簡單實用的方法，但必須在隨機動態(tài)系統(tǒng)的結(jié)構(gòu)參數(shù)和噪聲統(tǒng)計特性參數(shù)都已知的情況下才能發(fā)揮作用。自適應(yīng)濾波算法可以在進行狀態(tài)估計的同時，通過量測輸出在線實時地估計系統(tǒng)的噪聲參數(shù)。雖然自適應(yīng)濾波算法解決了隨機系統(tǒng)的模型誤差的影響，但是由于水下環(huán)境復(fù)雜，且USBL系統(tǒng)易收到水下其余雜質(zhì)聲波的影響，易產(chǎn)生奇異值，針對此問題，提出了一種AUKF算法，步驟如下

1)初始化

(15)

2)噪聲估計

3)時間更新

(16)

一步預(yù)測誤差協(xié)方差矩陣

(17)

和量測一步預(yù)測

(18)

4)量測更新

計算K時刻系統(tǒng)狀態(tài)估計值k和誤差協(xié)方差矩陣Pk，其中

(19)

4 SINSUSBLDVL多傳感器融合系統(tǒng)仿真驗證

為了驗證所建立的基于斜距的SINSUSBLDVL多傳感器融合系統(tǒng)的誤差模型以及所提出的AUKF算法的性能，在MATLAB環(huán)境下進行仿真，仿真系統(tǒng)采用AUV真實導(dǎo)航傳感器誤差數(shù)據(jù)生成動態(tài)數(shù)據(jù)的方式。首先建立了SINSUSBLDVL多傳感器融合系統(tǒng)(模型1)和基于斜距的SINSUSBLDVL多傳感器融合系統(tǒng)(模型2)的仿真平臺，其次在基于斜距的模型中設(shè)計了自適應(yīng)卡爾曼濾波算法(算法1)和AUKF算法(算法2)，并進行比較。仿真參數(shù)如表1所示。

表1 導(dǎo)航誤差參數(shù)

設(shè)置一條AUV運動的軌跡曲線，如圖6所示，包括平直前行、轉(zhuǎn)彎、上升、下滑等過程。初始位置(120°，31°)，系統(tǒng)仿真時間為600s，仿真結(jié)果如圖7和圖8所示。

圖6 AUV軌跡仿真

圖7 有無基于斜距的多傳感器融合模型濾波誤差輸出結(jié)果對比

圖8 基于斜距的多傳感器融合模型濾波算法誤差對比

圖7為有無基于斜距的SINSUSBLDVL多傳感器融合系統(tǒng)輸出的誤差對比圖，即紅線代表的是基于斜距的SINSUSBLDVL多傳感器融合系統(tǒng)的速度和位置誤差，可以看出采用基于斜距的方法建模，避免了位置解算中基陣偏移所帶來的誤差，速度誤差降低了60%左右，經(jīng)緯度誤差更是從3m降低到0.5m之內(nèi)。

圖8為基于斜距的SINSUSBLDVL多傳感器融合系統(tǒng)在自適應(yīng)卡爾曼濾波算法和AUKF算法上速度與位置的誤差曲線，可以看出，AUKF算法比自適應(yīng)卡爾曼濾波的效果更好，減少了濾波的奇異值與發(fā)散，濾波精度及穩(wěn)定性都得到了提高。其原因主要在于，AUKF算法在進行狀態(tài)估計的同時還可以通過量測輸出在線實時地估計系統(tǒng)的噪聲參數(shù)，更好地降低了模型誤差，抑制了濾波的發(fā)散。

5 結(jié)束語

在AUV的回收過程中，針對多傳感器的組合導(dǎo)航，在建模和融合算法兩方面進行了改進和創(chuàng)新，得出以下結(jié)論：

1)對于USBL系統(tǒng)的基陣偏移的所帶來的誤差不可忽略，采用基于斜距的建模方式可有效地避免這個誤差，使得速度與位置精度提高60%～70%。

2)由于USBL系統(tǒng)易受水下或其他傳感器信號干擾，針對整個回收系統(tǒng)的各傳感器的濾波融合算法，采用AUKF算法實時地估計系統(tǒng)的噪聲參數(shù)，可有效地消除奇異值和濾波發(fā)散的情況。

3)在回收過程中，導(dǎo)航精度是至關(guān)重要的，提高濾波精度，加強濾波的穩(wěn)定性，方可保證實際系統(tǒng)中AUV回收的安全性。