陳允鋒,陶健龍

(1.海軍裝備部駐上海地區(qū)第九軍事代表室,上海 20206;2.上海交通大學(xué),上海 200240)

0 引 言

近年來,自主水下航行器(AUV)作為海洋開發(fā)中應(yīng)用最廣泛的自動化裝備,能夠代替人類在環(huán)境惡劣、危險系數(shù)高的深海中完成多樣化水下作業(yè)任務(wù),如海洋資源勘測、水下捕撈和目標跟蹤等[1],已經(jīng)成為人類認識海洋、了解海洋不可或缺的裝備之一。而獲取準確的位置信息是AUV完成預(yù)定任務(wù)的必要條件,適用于陸地、水面和空中的北斗、GPS等定位系統(tǒng),主要借助電磁波信號進行定位。但電磁波信號在海水高導(dǎo)電介質(zhì)中傳播會快速衰減,該類方法應(yīng)用于水下環(huán)境時無法提供可靠的定位信息。因此,AUV的精準定位具有挑戰(zhàn)性。

主流的AUV導(dǎo)航定位方式包括慣性導(dǎo)航、地球物理導(dǎo)航和聲學(xué)導(dǎo)航[2]。慣性導(dǎo)航無需發(fā)送任何信號,常用于隱蔽性要求高的AUV,但長航程由于累積誤差,定位偏移可達數(shù)十米。地球物理導(dǎo)航根據(jù)攜帶的聲學(xué)或光學(xué)傳感器重建周邊地理信息,是一種能夠?qū)崿F(xiàn)全球水下自主導(dǎo)航的方式。聲學(xué)導(dǎo)航利用聲波在水下良好的傳輸特性以及信標、應(yīng)答器等設(shè)備實現(xiàn)高精度的定位。

單信標導(dǎo)航作為聲學(xué)導(dǎo)航的一種方式,于1995年被Scherbatyuk首次提出[3]。相比于其他聲學(xué)導(dǎo)航方式,具有經(jīng)濟性高、易于布置等優(yōu)勢,在水下導(dǎo)航中擁有很大的應(yīng)用潛力。Yu等[4]提出基于變分貝葉斯近似的魯棒自適應(yīng)單信標導(dǎo)航方法,克服現(xiàn)有單信標導(dǎo)航方法在伴隨有效聲速誤差、測量異常值和未知噪聲統(tǒng)計參數(shù)時定位精度差的問題。Zhu等[5]針對現(xiàn)有單信標定位模型通常為二維狀態(tài),引入水深狀態(tài)量,建立新型三維單信標定位模型。Liu等[6]引入時鐘平衡因子,緩解時鐘不同步對于水下滑翔機單信標定位的影響。Ssac等[7]針對幾何構(gòu)型不當引起單信標異步導(dǎo)航精度低問題,提出一種基于相位差的水下機器人導(dǎo)航方法。Koshaev[8]提出基于多模型的AUV單信標導(dǎo)航算法,采用1組擴展卡爾曼濾波算法,無需AUV的初始狀態(tài)信息。Zhao等[9]為抑制標準擴展卡爾曼濾波線性化帶來的誤差,將融合自適應(yīng)網(wǎng)絡(luò)和模糊推理系統(tǒng)改進的擴展卡爾曼濾波算法用于單信標定位。

為提高AUV單信標導(dǎo)航的定位精度,本文在標準擴展卡爾曼濾波算法基礎(chǔ)上,融合多新息和遺忘因子,提出基于改進擴展卡爾曼濾波算法的AUV單信標導(dǎo)航定位方法。充分利用不同時刻的新息,降低線性誤差帶來的不確定性,但過多地使用歷史數(shù)據(jù)會造成誤差累積。因此,在狀態(tài)更新時引入遺忘因子區(qū)分新舊數(shù)據(jù),削弱數(shù)據(jù)飽和。最后通過仿真實驗驗證本文提出方法的有效性和準確性。

1 單信標定位狀態(tài)空間描述建立

1.1 運動學(xué)方程

AUV航行過程中的深度信息可以通過深度傳感器準確獲得,因此,能夠?qū)⑷S定位問題簡化為二維問題。為便于研究,以單信標所在位置為慣性坐標系原點,建立二維平面坐標系,如圖 1所示。AUV位置表示為(ξ,η),其相對周圍流體的運動速度表示為v,艏向角表示為ψ。

圖1 AUV位置坐標系

以AUV在東向、北向的位移分量ξ(t)、η(t)和水流速度在東向、北向分量vE、vN為狀態(tài)變量,系統(tǒng)在控制輸入u=(v,ψ)作用下AUV單信標導(dǎo)航系統(tǒng)的運動學(xué)方程可表示為:

(1)

將式(1)使用如下狀態(tài)空間描述,標準形式為:

(2)

式中:vξ(t)=v(t)cosψ(t),vη(t)=v(t)sinψ(t),分別代表AUV速度在東向和北向的分量。

定義采樣時間間隔Δt=t-t0,假設(shè)采樣間隔內(nèi)u(τ)=u,單信標定位狀態(tài)空間描述離散化后為:

Xk=AXk-1+Buk-1+Wk-1

(3)

(4)

(5)

1.2 觀測方程

觀測量包括AUV與信標之間的水平距離和水流速度,第k時刻的水平距離可表示為:

(6)

通過多普勒測速儀測得AUV對地速度va與計程儀測得相對速度v之差,可表示為:

(7)

為符合真實環(huán)境,加入均值為0的傳感器測量噪聲Vk,其符合高斯噪聲分布,即Vk～(0,Rk),Rk為測量噪聲的協(xié)方差矩陣。結(jié)合式(6)和式(7)得到觀測方程為:

Zk=h(Xk)+Vk

(8)

(9)

2 改進擴展卡爾曼濾波算法

2.1 標準擴展卡爾曼濾波算法

(10)

Xk=AXk-1+Buk-1+Wk-1

(11)

(12)

基于標準擴展卡爾曼濾波的AUV單信標導(dǎo)航定位算法迭代流程如下:

(1)參數(shù)初始化

(2)時間更新

(13)

(14)

(3)新息更新

(15)

(16)

(4)濾波增益矩陣更新

(17)

(5)狀態(tài)更新

計算當前時刻濾波輸出:

(18)

(6)協(xié)方差陣更新

Pk|k=(I-KkCk)Pk|k-1

(19)

(7)令k=k+1,再次執(zhí)行(2)。

2.2 融合多新息理論和遺忘因子改進算法

新息是指能夠提升狀態(tài)估計精度的有效信息[10]。從標準擴展卡爾曼濾波算法估計AUV 第k時刻狀態(tài)時的迭代流程可知,僅使用前一時刻的標量新息,隱藏在歷史數(shù)據(jù)中的有效信息無法充分利用,當系統(tǒng)非線性較強時,可能導(dǎo)致濾波器無法收斂。為充分利用當前時刻及歷史量測數(shù)據(jù),降低線性化誤差,增強AUV定位精度,將單新息標量拓展為多新息向量形式,表示為:

(20)

式中:p表示新息長度,p≥1,使用滑動窗口的方式進行數(shù)據(jù)更新,將新息長度作為窗口大小。

為保證矩陣計算時維數(shù)相容,將卡爾曼增益矩陣擴展為如下形式:

(21)

但考慮到當前時刻的數(shù)據(jù)在AUV定位中利于精度提升,而過多引入歷史數(shù)據(jù)可能會產(chǎn)生誤差累積,導(dǎo)致數(shù)據(jù)飽和現(xiàn)象。因此,在多新息的基礎(chǔ)上加入遺忘因子,削弱歷史數(shù)據(jù)中誤差累積的影響。改進后的狀態(tài)更新方程可表示為:

(22)

(23)

式中:Γ(p,k)=ΛK(p,k),Λ=diag{λ1,λ2,…,λp},為遺忘因子矩陣,其中元素λi為數(shù)據(jù)權(quán)重;α代表遺忘因子。

基于改進擴展卡爾曼濾波的AUV單信標定位算法遞推流程如圖 2所示。相比于標準擴展卡爾曼濾波算法,不僅在時間維度上增加了相鄰時刻數(shù)據(jù)使用量,而且考慮了新舊數(shù)據(jù)的重要程度,減小了誤差累積作用,提高了擴展卡爾曼濾波算法在單信標導(dǎo)航定位中的準確性和穩(wěn)定性。

3 仿真分析

為驗證融合遺忘因子的多新息擴展卡爾曼濾波(改進MIEKF)在AUV單信標定位應(yīng)用中的有效性和準確性,本文借助MATLAB軟件設(shè)計了仿真實驗,對比算法包括標準擴展卡爾曼濾波(EKF)和多新息擴展卡爾曼濾波(MIEKF)。航跡起始點為(-500 m,500 m),在600～1 000 s,1 100～1 300 s時AUV前進航速為4 m/s,其余時刻航速為2 m/s,東向、南向水流速度分別為0.3 m/s、0.4 m/s,采樣間隔時間為1 s。采用AUV估計位置與真實位置之間的歐氏距離作為定位誤差,表示為:

(24)

(1)無先驗位置偏差下的定位準確性分析

探究改進MIEKF在正確初始狀態(tài)時的定位準確性,與EKF和MIEKF算法進行仿真對比分析。AUV初始位置為(-500 m,500 m),不同算法的定位結(jié)果如圖 3和表 1所示。

圖3 不同算法定位準確性對比

表1 不同算法定位誤差統(tǒng)計

圖3中“*”表示單信標位置,可以看出3種算法均能準確地獲得AUV實時位置信息。改進MIEKF算法收斂時間稍遜于EKF算法,MIEKF算法誤差累積逐漸增大,預(yù)測位置和實際位置之間存在較大偏差。在調(diào)整階段,EKF算法的最大定位偏差為23.5 m,改進MIEKF算法最大定位偏差為21 m,收斂時間稍遜于EKF算法。由于融合了遺忘因子和歷史數(shù)據(jù)進行修改,改進MIEKF算法整體定位精度和穩(wěn)定性優(yōu)于標準卡爾曼濾波算法,最大定位誤差減少2.352 2 m,定位誤差平均值減少0.856 4 m,驗證了改進算法良好的定位能力。

(2)先驗位置偏差對改進算法性能的影響

AUV實際工作時很難獲得準確的先驗位置信息。因此,進一步探究改進MIEKF算法在不同先驗位置偏差下的收斂性與準確性。設(shè)計3種不同先驗位置進行分析,AUV實際初始位置分別為(505 m,505 m)、(510 m,510 m)和(515 m,515 m),新息長度p= 5,遺忘因子α= 0.3。不同先驗位置偏差仿真結(jié)果如圖 4和表 2所示。

圖4 不同先驗位置偏差定位準確性對比

從圖4和表2中可以看出,本文的改進算法均能在1 100 s內(nèi)收斂到預(yù)定航跡附近。仿真結(jié)果中,最大定位誤差在米量級,平均定位誤差優(yōu)于4 m,并且濾波器收斂精度與初始狀態(tài)偏差相關(guān),初始狀態(tài)與實際值越接近,定位精度越高。

表2 不同先驗位置偏差定位誤差統(tǒng)計

(3)新息長度對改進算法性能的影響

歷史數(shù)據(jù)使用量與新息長度相關(guān),對改進算法定位精度有較大的影響。采用控制變量法,AUV實際初始位置為(500 m,500 m),遺忘因子α= 0.3,新息長度p依次設(shè)置為2、4、6、8和10,同時與EKF算法進行對比。定位結(jié)果如圖 5、圖 6和表 3所示。收斂時間隨新息長度增加而增加,最大收斂時間約為400 s。收斂精度和穩(wěn)定性隨新息長度增加而增加,但當新息長度超過6時,其收斂精度趨于某一固定值。相比于以單新息修正的EKF算法,改進算法平均定位誤差比標準EKF算法至少降低25%。在該工況下,新息長度選取6～8時定位算法性能達到最佳。

表3 不同新息長度定位誤差統(tǒng)計

圖5 不同新息長度定位結(jié)果對比

圖6 不同新息長度定位誤差

(4)遺忘因子大小對改進算法性能的影響滑動窗口內(nèi)新、舊歷史數(shù)據(jù)權(quán)重與遺忘因子大小相關(guān)。由遺忘因子定義形式可知,當遺忘因子設(shè)置過小,遺失了(k-p+1,k-2)時刻數(shù)據(jù)中有利的信息,多新息作用減弱;設(shè)置過大,則會使k-1時刻數(shù)據(jù)權(quán)重變小,修正項中無法充分利用前一時刻數(shù)據(jù),AUV定位誤差增大。采用控制變量法,AUV實際初始位置為(500 m,500 m),新息長度p= 6,選取遺忘因子分別為α=0.1、0.3、0.5、0.7。定位結(jié)果如圖 7、圖 8和表 4所示,當遺忘因子為0.3時,位置估計最大誤差、平均誤差、均方差分別為1.629 9 m、0.659 1 m和0.323 7 m。當遺忘因子為0.7時,各項誤差顯著增加,接近遺忘因子為0.3時的2倍,且穩(wěn)定后波動范圍較大,算法性能降低。綜上可知,改進算法的平均定位誤差隨遺忘因子增加呈先降低后增加的趨勢。

圖8 不同遺忘因子定位誤差對比

表4 不同遺忘因子定位誤差統(tǒng)計

4 結(jié)束語

水下精確定位對于基于自主水下航行器的海洋開發(fā)任務(wù)至關(guān)重要。本文針對EKF算法在AUV單信標導(dǎo)航系統(tǒng)中引入線性化誤差、定位精度不足等問題,融合多新息理論和遺忘因子,提出基于改進MIEKF的AUV單信標導(dǎo)航定位算法。根據(jù)AUV航行特點建立運動模型,基于單信標測距、水流速度建立觀測模型。以多新息取代EKF算法中的單新息修正,進一步引入遺忘因子削弱歷史數(shù)據(jù)的飽和現(xiàn)象。通過MATLAB軟件進行仿真對比實驗,并對初始狀態(tài)、新息長度和遺忘因子大小對改進算法性能的影響進行了討論。結(jié)果表明,相比于標準EKF算法,改進MIEKF算法最大定位誤差減少2.352 2 m,定位誤差平均值減少0.856 4 m,意味著改進算法具有良好的定位能力。在相同環(huán)境下,新息長度p為6,遺忘因子為0.3時,算法性能最優(yōu)。