包靈卉，曾慶軍，朱志宇，戴曉強(qiáng)

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

1 引言

自主水下機(jī)器人(Autonomous Underwater Vehicle，AUV)作為探索和開(kāi)發(fā)海洋的新型手段，近年來(lái)成為當(dāng)今研究的重要領(lǐng)域。隨著應(yīng)用市場(chǎng)的日益復(fù)雜，AUV需要更長(zhǎng)的工作時(shí)間、更強(qiáng)的自主性及更強(qiáng)的通訊能力[1]。由于AUV自身攜帶能源有限且水下通信速率低，所以需要及時(shí)導(dǎo)航定位、布放回收，以用于能源補(bǔ)充、數(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)等。以上幾種傳感器可以通過(guò)先進(jìn)的濾波融合技術(shù)進(jìn)行融合，達(dá)到AUV對(duì)接回收中導(dǎo)航的精度要求[3-4]。USBL定位系統(tǒng)配備的基陣一般尺寸小且安裝簡(jiǎn)易，可以根據(jù)實(shí)際情況設(shè)計(jì)其布陣方式，來(lái)增強(qiáng)系統(tǒng)的定位精度。挪威的Kongsberg Simrad公司研發(fā)了一款名為HiPAP超短基線定位系統(tǒng)，定位精度可以達(dá)到0.02m，其中HiPAP102型超短基線工作水深達(dá)到了10000m。國(guó)內(nèi)對(duì)USBL的技術(shù)研究較晚，主要研究機(jī)構(gòu)有哈爾濱工程大學(xué)、浙江大學(xué)、中科院沈陽(yáng)自動(dòng)化研究所等[5-6]。最具代表性的是哈爾濱工程大學(xué)研究的USBL定位系統(tǒng)，2014年在“蛟龍?zhí)枴鄙狭料?，為中?guó)的海洋事業(yè)做出巨大貢獻(xiàn)；浙江大學(xué)流體動(dòng)力與機(jī)電系統(tǒng)國(guó)家重點(diǎn)實(shí)驗(yàn)室針對(duì)在AUV入塢過(guò)程中喇叭狀的回收塢對(duì)海豚-ⅡAUV的影響作用，進(jìn)行了一種光學(xué)引導(dǎo)回收對(duì)接的研究；沈陽(yáng)自動(dòng)化研究所設(shè)計(jì)研究了回收中的對(duì)接塢，其主要對(duì)于AUV進(jìn)入對(duì)接塢時(shí)的各種水動(dòng)力問(wèn)題展開(kāi)了研究；但目前國(guó)內(nèi)在AUV回收組合導(dǎo)航多傳感器融合這方面的研究開(kāi)展得較少。

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

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

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

圖1 AUV回收對(duì)接流程框圖

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

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

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

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

本文以江蘇科技大學(xué)自主研發(fā)的“探海I型”AUV為研究對(duì)象，如圖4所示。光纖慣導(dǎo)SINS選用中科探海(蘇州)海洋科技有限責(zé)任公司定制的GIF6536A型光纖慣導(dǎo)，計(jì)程儀DVL選用LinkQuest公司的NavQuest 600 Micro DVL，深度計(jì)采用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è)計(jì)

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

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

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

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

(1)

(2)

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

FSINS可由SINS誤差方程確定。

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

(3)

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

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

(4)

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

(5)

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

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

(6)

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

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

(7)

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

(8)

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

(9)

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

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

(10)

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

(11)

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

(12)

其中，量測(cè)矩陣為

(13)

(14)

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

3.3 多傳感器融合中AUKF算法

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

1)初始化

(15)

2)噪聲估計(jì)

3)時(shí)間更新

(16)

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

(17)

和量測(cè)一步預(yù)測(cè)

(18)

4)量測(cè)更新

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

(19)

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

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

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

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

圖6 AUV軌跡仿真

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

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

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

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

5 結(jié)束語(yǔ)

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

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

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

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