基于改進擴展卡爾曼濾波的AUV單信標定位研究

陳允鋒,陶健龍

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

0 引 言

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

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

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

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

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

1.1 運動學方程

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

圖1 AUV位置坐標系

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

(1)

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

(2)

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

定義采樣時間間隔Δt=t-t0,假設采樣間隔內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é)方差矩陣。結合式(6)和式(7)得到觀測方程為:

Zk=h(Xk)+Vk

(8)

(9)

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

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

(10)

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

(11)

(12)

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

(1)參數初始化

(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)時的迭代流程可知,僅使用前一時刻的標量新息,隱藏在歷史數據中的有效信息無法充分利用,當系統(tǒng)非線性較強時,可能導致濾波器無法收斂。為充分利用當前時刻及歷史量測數據,降低線性化誤差,增強AUV定位精度,將單新息標量拓展為多新息向量形式,表示為:

(20)

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

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

(21)

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

(22)

(23)

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

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

3 仿真分析

為驗證融合遺忘因子的多新息擴展卡爾曼濾波(改進MIEKF)在AUV單信標定位應用中的有效性和準確性,本文借助MATLAB軟件設計了仿真實驗,對比算法包括標準擴展卡爾曼濾波(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),不同算法的定位結果如圖 3和表 1所示。

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

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

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

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

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

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

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

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

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

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

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

圖5 不同新息長度定位結果對比

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

(4)遺忘因子大小對改進算法性能的影響滑動窗口內新、舊歷史數據權重與遺忘因子大小相關。由遺忘因子定義形式可知,當遺忘因子設置過小,遺失了(k-p+1,k-2)時刻數據中有利的信息,多新息作用減弱;設置過大,則會使k-1時刻數據權重變小,修正項中無法充分利用前一時刻數據,AUV定位誤差增大。采用控制變量法,AUV實際初始位置為(500 m,500 m),新息長度p= 6,選取遺忘因子分別為α=0.1、0.3、0.5、0.7。定位結果如圖 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 結束語

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