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

陳允鋒,陶健龍

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

0 引 言

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

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

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

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

1 單信標定位狀態空間描述建立

1.1 運動學方程

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

圖1 AUV位置坐標系

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

(1)

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

(2)

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

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

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

(3)

(4)

(5)

1.2 觀測方程

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

(6)

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

(7)

為符合真實環境,加入均值為0的傳感器測量噪聲Vk,其符合高斯噪聲分布,即Vk～(0,Rk),Rk為測量噪聲的協方差矩陣。結合式(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)狀態更新

計算當前時刻濾波輸出:

(18)

(6)協方差陣更新

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

(19)

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

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

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

(20)

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

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

(21)

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

(22)

(23)

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

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

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在正確初始狀態時的定位準確性,與EKF和MIEKF算法進行仿真對比分析。AUV初始位置為(-500 m,500 m),不同算法的定位結果如圖 3和表 1所示。

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

表1 不同算法定位誤差統計

圖3中“*”表示單信標位置,可以看出3種算法均能準確地獲得AUV實時位置信息。改進MIEKF算法收斂時間稍遜于EKF算法,MIEKF算法誤差累積逐漸增大,預測位置和實際位置之間存在較大偏差。在調整階段,EKF算法的最大定位偏差為23.5 m,改進MIEKF算法最大定位偏差為21 m,收斂時間稍遜于EKF算法。由于融合了遺忘因子和歷史數據進行修改,改進MIEKF算法整體定位精度和穩定性優于標準卡爾曼濾波算法,最大定位誤差減少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內收斂到預定航跡附近。仿真結果中,最大定位誤差在米量級,平均定位誤差優于4 m,并且濾波器收斂精度與初始狀態偏差相關,初始狀態與實際值越接近,定位精度越高。

表2 不同先驗位置偏差定位誤差統計

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

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

表3 不同新息長度定位誤差統計

圖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倍,且穩定后波動范圍較大,算法性能降低。綜上可知,改進算法的平均定位誤差隨遺忘因子增加呈先降低后增加的趨勢。

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

表4 不同遺忘因子定位誤差統計

4 結束語

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