夏 楠，曾慶軍，包靈卉，孫嘯天，許赫威

（1.江蘇科技大學(xué)電子信息學(xué)院，江蘇 鎮(zhèn)江212028; 2.中船海洋探測(cè)技術(shù)研究院有限公司，江蘇 無(wú)錫214142）

0 引 言

海洋資源豐富但環(huán)境復(fù)雜，AUV是探索和開(kāi)發(fā)海洋的新型手段。隨著AUV的系統(tǒng)不斷升級(jí)，性能也在逐漸加強(qiáng)。目前AUV正在向著高自主、高性能、長(zhǎng)時(shí)間、遠(yuǎn)距離通信[1]等更智能方向發(fā)展。但是經(jīng)過(guò)長(zhǎng)時(shí)間工作，AUV需要精準(zhǔn)的導(dǎo)航定位系統(tǒng)來(lái)引導(dǎo)AUV進(jìn)行回收對(duì)接工作，方便布置新的使命[2]，和解決能源系統(tǒng)耗竭與提取內(nèi)部數(shù)據(jù)的問(wèn)題。

可靠的導(dǎo)航系統(tǒng)是AUV回收對(duì)接成功的重要保障。目前針對(duì)多傳感器在組合導(dǎo)航系統(tǒng)中采樣率不同的問(wèn)題，幾乎都是建立在各傳感器數(shù)據(jù)傳輸都同步的理想狀態(tài)模型下，在單一尺度下對(duì)問(wèn)題進(jìn)行分析。但在實(shí)際情況中，組合導(dǎo)航系統(tǒng)中各傳感器的性能指標(biāo)和采樣頻率各不相同，這就導(dǎo)致了多傳感器組合導(dǎo)航系統(tǒng)量測(cè)信息滯后的問(wèn)題，即產(chǎn)生異步采樣問(wèn)題[3]。因此對(duì)聯(lián)邦濾波結(jié)構(gòu)中異步融合算法的研究，對(duì)AUV回收對(duì)接工作具有重要意義。

針對(duì)組合導(dǎo)航系統(tǒng)中多傳感器異步采樣問(wèn)題的研究，文獻(xiàn)[4] 在組合導(dǎo)航系統(tǒng)中采用數(shù)據(jù)塊分析技術(shù)，并提出了一種異步貫序?yàn)V波算法，融合各尺度經(jīng)KF濾波優(yōu)化的局部狀態(tài)估計(jì)，最終得到全局最優(yōu)狀態(tài)估計(jì)值。算法使用要求各傳感器的采樣率是2的整數(shù)倍，實(shí)際應(yīng)用比較受限。文獻(xiàn)[5]提出多尺度數(shù)據(jù)分塊的組合導(dǎo)航算法，主要在最細(xì)尺度下的系統(tǒng)模型中利用數(shù)據(jù)分塊，將量測(cè)信息分解到各尺度進(jìn)行濾波，得到全局最優(yōu)狀態(tài)估計(jì)值。此方法雖然提高了導(dǎo)航精度，但是增加了系統(tǒng)的計(jì)算量。

本文為了提高由于采樣頻率不同而致導(dǎo)AUV在回收對(duì)接過(guò)程中的導(dǎo)航定位精度較低的問(wèn)題，提出了一種基于UKF融合的多尺度異步融合算法。本文算法依據(jù)采樣率劃分多尺度信息，建立基于聯(lián)邦濾波的多尺度系統(tǒng)誤差模型，并針對(duì)不同尺度信息采用UKF算法進(jìn)行非等間隔異步融合，從而得到全局的最優(yōu)狀態(tài)估計(jì)。仿真表明，基于UKF的多傳感器異步融合算法對(duì)AUV回收對(duì)接導(dǎo)航系統(tǒng)中異步采樣信息具有更高的融合精度，提高了導(dǎo)航系統(tǒng)的精度和可靠性。

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

本文以江蘇科技大學(xué)自主研發(fā)的“探海I型”自主水下機(jī)器人（AUV）的回收對(duì)接系統(tǒng)為研究對(duì)象。根據(jù)AUV的運(yùn)動(dòng)特性，可以把整個(gè)回收對(duì)接過(guò)程分為直線(xiàn)歸位、直線(xiàn)跟蹤、直線(xiàn)對(duì)接與慣性對(duì)接4個(gè)階段。前面兩個(gè)運(yùn)動(dòng)對(duì)接階段的導(dǎo)航系統(tǒng)，為本文主要研究?jī)?nèi)容，此階段AUV采用組合導(dǎo)航的方式。未進(jìn)入U(xiǎn)SBL的作用范圍時(shí)，主要由SINS的航位推算提供AUV的相對(duì)位置。當(dāng)進(jìn)入U(xiǎn)SBL的作用范圍時(shí)，用多傳感器融合的方式，提供AUV的相對(duì)精確位置，不斷地調(diào)整AUV的位置與姿態(tài)，使其運(yùn)動(dòng)軌跡在對(duì)接塢在中軸線(xiàn)附近。而后在距離較近的直線(xiàn)對(duì)接階段，采用雙目視覺(jué)導(dǎo)航，使AUV機(jī)身對(duì)在對(duì)接塢的中軸線(xiàn)上，最后進(jìn)入慣性對(duì)接階段，依靠AUV運(yùn)動(dòng)慣性緩緩駛?cè)雽?duì)接塢。圖1為回收對(duì)接流程圖。

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

如圖2所示為“探海Ⅰ型”AUV實(shí)物圖，其裝配的傳感器與相關(guān)參數(shù)如表1所示。

表1 傳感器及相關(guān)參數(shù)

圖2 “探海Ⅰ型”AUV

2 多傳感器組合導(dǎo)航系統(tǒng)建模

圖3 聯(lián)邦濾波結(jié)構(gòu)圖

2.1 多傳感器組合導(dǎo)航系統(tǒng)狀態(tài)方程

選取導(dǎo)航系統(tǒng)的誤差作為狀態(tài)量，主要誤差包括速度、姿態(tài)、位置、加速度計(jì)零偏和陀螺漂移[7]，USBL/DVL/深度計(jì)分別提供位置、速度、深度信息作為輔助則系統(tǒng)狀態(tài)變量為：

式中：F（k）——系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣；

W（k）——過(guò)程噪聲矩陣；

G（k）——過(guò)程噪聲驅(qū)動(dòng)矩陣。

其中子系統(tǒng)的狀態(tài)量X（k）為：

式中：δVE,δVN,δVU——東、北、天三個(gè)軸向的速度誤差；

φE,φN,φU——東、北、天三個(gè)軸向的失準(zhǔn)角；

δL,δλ,δh——緯度、經(jīng)度、深度誤差；

?bx,?by,?bz——加速度計(jì)三個(gè)軸向的隨機(jī)偏置誤差[8]；

εbx,εby,εbz——陀螺的三個(gè)軸向隨機(jī)漂移值。

由已知的系統(tǒng)誤差模型，可建立子系統(tǒng)的非線(xiàn)性系統(tǒng)狀態(tài)方程[9]為：

2.2 多傳感器組合導(dǎo)航系統(tǒng)量測(cè)方程

本文設(shè)計(jì)的多傳感器組合導(dǎo)航系統(tǒng)中，有3個(gè)子濾波器，每個(gè)子濾波器的量測(cè)方程：

1）取SINS位置數(shù)據(jù)與USBL轉(zhuǎn)換后輸出的位置數(shù)據(jù)作差為量測(cè)值，得SINS/USBL子濾波器量測(cè)方程：

式中：LSINS，λSINS，hSINS——SINS的緯度、經(jīng)度和深度信息；

LUSBL，λUSBL，hUSBL——USBL的緯度、經(jīng)度和深度信息；

DE，DN，DU——USBL沿東、北、天方向的位置測(cè)量誤差；

RN——地球子午圈半徑；

RM——卯酉圈的曲率半徑[10-11]。

2）取SINS與DVL輸出的位置數(shù)據(jù)作差為量測(cè)值，得SINS/DVL子濾波器量測(cè)方程：

3）SINS與深度計(jì)輸出的位置數(shù)據(jù)作差為量測(cè)值，得SINS/深度計(jì)子濾波器量測(cè)方程：

3 多傳感器異步融合算法

3.1 多傳感器多尺度系統(tǒng)模型

假設(shè)用i=1,2,···,N來(lái)表示N個(gè)傳感器，每個(gè)傳感器對(duì)應(yīng)的采樣率為T(mén)i，假設(shè)N為最細(xì)尺度，1為最粗尺度，ni∈N+，設(shè)傳感器i和j之間的采樣頻率之比為A，則A為任意正有理數(shù)，即

由多尺度模型分析可知，i表示傳感器所在尺度，同時(shí)也表示其位數(shù)。本文設(shè)計(jì)的多傳感器組合導(dǎo)航系統(tǒng)中，已知輔助參考系統(tǒng)在最高采樣頻率下的狀態(tài)方程和觀測(cè)方程[12]。第i=1個(gè)為低頻采樣傳感器在最粗尺度上，第i=N個(gè)為高頻采樣傳感器在最細(xì)尺度上。每個(gè)子傳感器所在尺度各不相同，建立系統(tǒng)模型為：

可得，尺度i上k時(shí)刻的采樣值與尺度N上nik時(shí)刻的采樣值相對(duì)應(yīng)，即可表述為：

可建立尺度i上的非線(xiàn)性系統(tǒng)狀態(tài)方程和觀測(cè)方程：

圖4 多尺度異步采樣示意圖

3.2 多傳感器異步融合算法建立

式中：D（N,nik）——nik時(shí)刻SINS的數(shù)據(jù)信息；

D（i,k）——尺度i上導(dǎo)航傳感器在k時(shí)刻的量測(cè)值。

如圖5所示為AUV對(duì)接導(dǎo)航系統(tǒng)中異步融合算法流程圖。

圖5 AUV多傳感器異步融合算法流程圖

4 仿真結(jié)果分析與湖試實(shí)驗(yàn)

4.1 仿真結(jié)果分析

為了驗(yàn)證本文設(shè)計(jì)的多傳感器組合導(dǎo)航異步融算法在AUV對(duì)接導(dǎo)航系統(tǒng)中的有效性，利用Matlab軟件進(jìn)行仿真實(shí)驗(yàn)。仿真系統(tǒng)中對(duì)AUV水下航行的動(dòng)態(tài)軌跡仿真以及利用導(dǎo)航傳感器真實(shí)的誤差數(shù)據(jù)仿真生成動(dòng)態(tài)數(shù)據(jù)。如表2和表3所示，為所用傳感器的誤差參數(shù)和采樣率。

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

表3 各傳感器采樣時(shí)間

表2中，由于AUV使用的深度計(jì)為400 m級(jí)大量程，所以精度最高在米級(jí)。

圖6 AUV運(yùn)動(dòng)軌跡及狀態(tài)估計(jì)

為了突出比較基于UKF的多傳感器異步融合算法的優(yōu)越性，針對(duì)速度、位置在同一運(yùn)動(dòng)軌跡上的誤差，對(duì)三種不同的融合算法結(jié)果進(jìn)行對(duì)比，表4所示為單一尺度狀態(tài)估計(jì)和多尺度兩種融合算法的狀態(tài)估計(jì)標(biāo)準(zhǔn)差對(duì)比，圖7為速度誤差對(duì)比，圖8為位置誤差對(duì)比。

圖7 AUV運(yùn)動(dòng)速度誤差對(duì)比

圖8 AUV運(yùn)動(dòng)位置誤差對(duì)比

表4 各參數(shù)標(biāo)準(zhǔn)差對(duì)比

仿真結(jié)果顯示基于UKF的多傳感器導(dǎo)航異步融合算法相對(duì)于基于KF的單一尺度融合算法和基于KF的多尺度融合算法更加穩(wěn)定，濾波精度更高。本文設(shè)計(jì)的算法，有效的解決了由于采樣頻率不同而導(dǎo)致多傳感器組合導(dǎo)航系統(tǒng)精度較低的問(wèn)題，提高了AUV回收對(duì)接導(dǎo)航系統(tǒng)中異步采樣信息的融合精度。

4.2 湖試實(shí)驗(yàn)

實(shí)驗(yàn)地選擇在水域較為開(kāi)闊的蘇州某湖進(jìn)行，便于試驗(yàn)開(kāi)展，水深5～40 m，水流2節(jié)左右。如圖9，為回收對(duì)接過(guò)程，圖10為水面回收航跡圖，表5為實(shí)驗(yàn)部分?jǐn)?shù)據(jù)。

表5 部分實(shí)驗(yàn)數(shù)據(jù)

圖9 回收對(duì)接實(shí)驗(yàn)

圖10 水面回收航跡圖

實(shí)驗(yàn)將對(duì)接塢固定在岸邊碼頭，通過(guò)上位機(jī)規(guī)劃AUV的目標(biāo)點(diǎn)和深度的使命，到達(dá)目標(biāo)點(diǎn)后開(kāi)始自主對(duì)接實(shí)驗(yàn)。設(shè)置AUV速度0.8 kn（1 kn≈0.514 m/s），目標(biāo)點(diǎn)坐標(biāo)為（22,180），回收點(diǎn)坐標(biāo)為（28.68,180.4），進(jìn)行將近1.5 h的使命回收時(shí)間，對(duì)接實(shí)驗(yàn)成功。實(shí)驗(yàn)表明本文所設(shè)計(jì)的導(dǎo)航系統(tǒng)更加精準(zhǔn)，在實(shí)際應(yīng)用上為AUV的對(duì)接回收提供了安全保障。

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

本文為了提高由于采樣頻率不同而致導(dǎo)AUV在回收對(duì)接過(guò)程中的導(dǎo)航定位精度較低的問(wèn)題，提出了一種基于UKF融合的多尺度異步融合算法。本文算法依據(jù)采樣率劃分多尺度信息，建立基于聯(lián)邦濾波的多尺度系統(tǒng)誤差模型，并針對(duì)不同尺度信息采用UKF算法進(jìn)行非等間隔異步融合，從而得到全局的最優(yōu)狀態(tài)估計(jì)。通過(guò)Matlab軟件針對(duì)實(shí)際系統(tǒng)進(jìn)行仿真實(shí)驗(yàn)，相比于基于KF單一尺度算法與基于KF多尺度算法，本文基于UKF的多尺度異步融合算法具有更高的濾波精度和實(shí)際應(yīng)用價(jià)值。湖試實(shí)驗(yàn)充分驗(yàn)證了AUV對(duì)接導(dǎo)航系統(tǒng)的有效性。