基于IMM 算法的多AUV 協同定位水聲傳播延遲處理方法

陳世杰 ,劉錫祥 *,黃永江 ,章彩霞 ,陶育杰 ,童金武

(1.東南大學 儀器科學與工程學院,江蘇 無錫,214026;2.微慣性儀表與先進導航技術教育部重點實驗室,江蘇 南京,210096;3.南京工程學院 工業中心創新創業學院,江蘇 南京,211167)

0 引言

協同定位概念在20 世紀90 年代由Kurazume等[1]首次提出,之后各國學者將其運用到自主水下航行器(autonomous undersea vehicle,AUV)系統中,并開展了一系列有關海洋資源探索與水下目標搜尋領域的研究[2-5]。近年來,AUV憑借作業范圍廣、自主性強、安全性高等優勢被廣泛應用。在執行既定行動任務時,協同定位成為AUV 協同系統能否成功完成任務的關鍵技術。

對于水下工作的AUV,常用的陸上導航方式如衛星導航、地形輔助等受環境影響不能充分發揮作用甚至失效,而具有自主導航能力的慣性導航以及航位推算等方法因其不依靠外界信息的特點,被廣泛應用于水下導航系統。但航位推算方法的導航誤差會隨著時間的推移不斷累積,無法滿足長航時的定位需求。多AUV 協同系統即是通過相互觀測與通信實現各AUV 間信息共享,運用基于幾何學或概率學的方法進行數據融合,提高AUV 系統整體定位精度[6-8]。

多AUV 系統協同定位的精度依賴于AUV 間通信與探測兩部分信息的傳遞,探測信號獲得AUV彼此間的觀測距離,通信信號實現AUV 間位置信息的共享。復雜的水下環境使得電磁波等通信手段無法有效工作,水聲通信是當前最常用的通信方法,但是數據的處理以及信號的傳輸都需要一定時間,信息存在滯后性,直接影響其利用效果[9-11]。

基于此,眾多學者提出了一系列改進方案。文獻[12]對基于飛行時間(time of flight,TOF)技術的主從式協同導航定位進行了整周期濾波的算法研究,揭示出協同定位精度與AUV 間的相對位置密切相關;文獻[13]考慮了水聲延遲問題,分析了AUV 間相對定位信息延遲對濾波狀態估計的影響,提出了一種基于狀態估計均方差最小的誤差修正方法;文獻[14]采用了一種非等間隔實時更新(incoordinate interval real-time update,IIRU)的協同定位算法,對主AUV 的航跡預測與時間更新過程進行轉停判斷,兼顧了主從AUV 間狀態相關性并有效降低了水聲延遲引起的系統定位誤差,但當主AUV 運動狀態改變時,定位結果會受到影響。交互多模型(interacting multiple model,IMM)算法具有自適應的特點,尤其在對機動目標進行追蹤時,能夠實現目標各運動狀態模型之間的轉換。在跟蹤機動目標時不可能只靠單一模型來描述運動狀態,在整個運動階段時間內,采用多個模型才能更好地描述目標的運動。文獻[15]分別比較了IMM 算法對3 種運動模型計算的誤差性能,與單一濾波器比較,觀察組合后算法在目標機動方面的性能有所提高。文獻[16]提出一種混合交互多模型(Hybrid-IMM,HIMM)算法,自適應地選擇合適的模型來描述變化的環境,提高復雜水下環境導航系統的性能。

文中針對主從式多AUV 協同系統中存在的水聲通信延遲問題進行研究,在從AUV 擴展卡爾曼濾波(extended Kalman filter,EKF)器中接收到延遲水聲信號時,回溯到發送時間節點進行重新濾波,并采用IMM 方法,實現從AUV 濾波器對主AUV不同運動狀態的跟蹤,降低其狀態估計誤差。設計主AUV 運動狀態變換的航行軌跡,對該方法進行了分析討論與仿真驗證,證明使用IMM 方法精確跟蹤主AUV 狀態時,協同系統整體定位精度得到提高。

1 AUV 協同系統水聲延遲

AUV 協同定位方式主要有主從式和并行式2 種。其中,主從式AUV 協同定位方式兼顧精度和成本,是當前研究的主要內容,該定位方式將多AUV 分為主、從AUV,主AUV 配備高精度慣導設備,由水聲設備向從AUV 發送聲學信號;從AUV裝備低精度航位推算和水聲通信設備等,并通過水聲通信獲取主AUV 的位姿信息和主從AUV 間的相對距離。主從式AUV 協同定位方式如圖1所示。由于單領航主從式多AUV 系統的可觀測性較弱,文中選用雙領航式以增強系統的可觀測性[17-18]。

圖1 主從式AUV 協同定位方式示意圖Fig.1 Diagram of master-slave AUV cooperative positioning method

1.1 協同定位無延遲模型

AUV 于水下三維空間航行,其深度信息可由深度傳感器等設備直接獲取,實際航行過程中俯仰角不發生較大的變化,在一般算法研究中,將運動模型簡化,深度信息不計入狀態,運動學模型由三維空間簡化到二維平面下[19]

式中:x(k),y(k),θ(k)分別表示AUV 的經度、緯度和航向角,狀態值X(k)=[x(k),y(k),θ(k)]T;V(k),ω(k)分別為AUV 速度和角速度,輸入量為u(k)=[V(k),ω(k)]T。二者受互不相關的高斯白噪聲干擾,噪聲誤差協方差陣如下

AUV 航位推算運動方程為

遠程方程包含非線性項,由于EKF 針對非線性系統有較好的表現,并且計算量小,易于工程實現,適宜處理水下AUV 定位,經EKF 線性化可得

式中,Φ(k+1,k)為系統狀態轉移矩陣,G(k)為系統噪聲驅動矩陣,二者均為依據EKF 泰勒展開的雅可比矩陣。

從AUV 航位推算位姿信息狀態可表示為Xs(k+1,k)=[xs(k+1),ys(k+1),θs(k+1)]T,主AUV的位姿信息為Xm(k+1,k)=[xm(k+1),ym(k+1)]T,計算主從AUV 間相對位置距離信息

量測方程為

式中:h(·)為關于狀態量的非線性函數;ωZ為不相關的零均值高斯白噪聲。

方差矩陣

量測方程的雅可比矩陣[20]

1.2 水聲延遲問題描述

水聲信號分為聲學探測信號和聲學通信信號2 種,使用不同設備發出,為避免相互干擾,二者在不同時刻發送,分別形成距離探測信號和狀態傳遞信號。距離探測生成原理為[21]

聲學信號在不同AUV 之間傳播,受水下傳播速度c影響發出與到達時間存在一定時間間隔即為延遲時間(tk-tl),該延遲時間對定位精度的影響通常不能忽略。水聲信號的時延情況如圖2 所示,tl時刻主AUV 發送的聲學信號,經過一段時間后在tk時刻到達從AUV,此時間段內各AUV 仍保持航行狀態,當前主AUV 位置信息已經發生改變,探測距離Zm,s不再是式(5)所示同一時刻主從AUV 間的距離信息,傳遞的真實測距信息意義為

圖2 水聲傳遞信號時間示意圖Fig.2 Diagram of underwater acoustic transmission signal time

另一時刻tcom,主AUV 發送自身位置信息,經過一段傳播時間后,被從AUV 接收并使用。

根據式(11)的延遲時間函數進行建模,依據圖2 中的聲學信號傳遞過程選擇降低2 種信號的發送頻率,預先確定信號每次發送的時間節點,一定程度上避免了信號出序到達以及從AUV 同時接收到多個水聲信號時處理優先級的問題。

1.3 水聲信號延遲處理

由相對距離信息引起的非線性濾波問題可以使用EKF 進行解決,在此基礎上建立水聲信號的延遲處理方法如下。

上述協同環境中低頻的聲學信號可能減少一段時間內濾波可用的量測信息,針對不同時間節點發送的聲學探測與通信信號,將二者均作為濾波更新中的量測值,實現量測信息的增加。從AUV在接收到距離探測信號時,可獲取主從AUV 間距離,但EKF 量測更新步驟中新息的建立需要同時獲得量測距離與估計距離,而此時主AUV 位置信息尚未到達,估計距離無法計算。因此,在協同過程中可在從AUV 濾波器中建立主AUV 的航跡預測模型,使用對主AUV 的狀態預測值計算估計距離,完成距離量測更新。從AUV 接收到主AUV狀態位置信息時,只需對主AUV 的航跡預測模型進行修正即可。

1)無水聲信號到達

在無水聲信號到達時,從AUV 的EKF 濾波器只進行航位推算時間更新,同時對主AUV 航跡進行預測,聯立主從AUV 狀態進行狀態擴增,系統預先規定2 種信號的發送時間節點,當同步時間戳運行到每個確定的時間節點,保存對主AUV 的狀態預測值、輸入信息及狀態誤差協方差陣。

時間更新部分如下

構建一步預測協方差陣的更新如下

初始化時,考慮主從AUV 間的相關性,其中

2)水聲距離探測信號到達

tk+1時刻,從AUV 接收到主AUV 于tl時刻發送的距離探測信號,解析數據獲取主從AUV 間距離信息,Zm,s(l,k+1)表示Xm(l)與Xs(k+1)之間的距離,此時X(k+1,k)=[Xm(k+1,k),Xs(k+1,k)]T為濾波器估計狀態,使用在時間更新階段預先保存的發送時間節點的主AUV 估計狀態值調整濾波器擴增狀態Xl(k+1,k)=[Xm(l),Xs(k+1,k)]T以及協方差矩陣Pl(k+1,k)=diag[Pm(l),Ps(k+1,k)]進行量測更新,即

式中:Z(k+1)表示計算距離與觀測距離建立的新息;R(k+1)表示距離量測值Zm,s(l,k+1)的方差矩陣;Xl(k+1,k)=[Xm(l),Xs(k+1,k)]T表示2 個時間節點的主從狀態,按照式(12)時間更新到X(k+1,k)=[Xm(k+1,k),Xs(k+1,k)]T。

3)水聲通信信號到達

從AUV 于tk+1時刻接收到主AUV 于tl時刻發送的水聲狀態通信信號,該信號包含主AUV 在tl時刻的位置信息Xm,r(l)、狀態誤差協方差陣Pm,r(l)和速度角速度輸入信息u(k)=[V(k),ω(k)]T,將狀態位置信息Xm,r(l)作為量測值,在從AUV 濾波器中對主AUV 的航跡預測狀態值進行量測更新,使用時間更新過程中發送節點預先保存的主AUV 估計狀態值Xm(l,l-1)與Pm(l,l-1)完成量測更新,即

狀態量測更新后,完成u(l)=[V(l),ω(l)]T輸入信息的替換更新,并利用式(12)將時間更新到當前時間節點,X(k+1)=[Xm(k+1),Xs(k+1)]T。

延遲擴展卡爾曼濾波(delay EKF,DEKF)算法分別利用主AUV 發送的探測距離信息與傳遞的狀態信息進行量測更新與輸入量的更新,對于時間延遲的影響,進行回溯形式的對應時間節點狀態信息更新,完成重新濾波。考慮低頻水聲信號,主AUV 作機動性航行時,從AUV 濾波器中對主AUV 狀態的預測無法實現及時更新,例如主AUV作加速運動時,輸入量為速度等信息,在無信號到達的時間段,速度保持在一定值,對主AUV 的運動狀態估計會出現較大偏差。IMM 算法對于運動狀態的追蹤具有一定的優勢,在DEKF 算法中,位置與輸入等狀態信號到達時,進行IMM 算法更新運動模型概率,有望在準確跟蹤主AUV 運動狀態的前提下,提升協同系統整體定位精度。

2 IMM 模型

2.1 IMM 算法

IMM 算法通過馬爾可夫轉移概率及模型概率的更新實現多模型間的切換。在上述主從式多AUV 協同系統中,從AUV 濾波器需要對主AUV進行航跡預測,選擇IMM 算法能夠實現良好的跟蹤效果。一次IMM 跟蹤算法主要包含輸入模型交互、濾波器濾波、概率模型的更新和新的數據融合4 個步驟[16]。IMM 算法流程如圖3 所示。

圖3 IMM 算法流程圖Fig.3 Flow chart of IMM algorithm

首先,在IMM 算法中預先設定多個運動目標,狀態方程為

式中:X為系統狀態向量;Aj為目標狀態轉移矩陣;Wj為均值為零的高斯白噪聲。

任意模型j的觀測方程

式中:Z為量測向量;Hj為模型j的觀測矩陣;Vj為均值為零的高斯白噪聲。

模型轉換矩陣

式中,pij表示從模型i轉移到模型j的概率。

1)輸入交互

根據前一時刻系統狀態估計和協方差估計來推測當前時刻的量測值Z(k),然后對模型進行重新初始化計算,其中新的初始值通過不同模型之間的馬爾科夫運算矩陣獲得,對模型k-1時刻的狀態最優估計值和估計協方差矩陣分別設定為:Xj(k-1|k-1)和Pj(k-1|k-1)。進一步推導交互后模型的初始化條件

其中

2)濾波器濾波

IMM 算法的EKF 算法如下。

時間更新

濾波增益

量測更新

3)模型概率更新

采用極大似然函數法實現模型的更新,通過計算當前模型和當前運動目標狀態的相似度來給出當前最適合跟蹤模型所占權重。

在k時刻與j模型最匹配的極大似然函數為

模型j的更新概率為

式中,c為歸一化常數,即

4)數據融合

根據每個模型單獨計算的跟蹤結果和模型匹配的權重給出k時刻交互數據的最終輸出結果,即,

總體估計值

總體協方差

以上為IMM 卡爾曼濾波跟蹤算法的一次遞推過程,每一次將上一次的交互輸出作為下一次的交互輸入值,完成整個濾波跟蹤過程的循環。

2.2 運動模型

鑒于主從式多AUV 協同系統中AUV 運動機動性并不高,選擇幾種常用的運動模型作為IMM算法中的子模型,包括恒定速度(constant velocity,CV)模型、恒定加速度(constant acceleration,CA)模型和恒定轉率和速度(constant turn rate and velocity,CTRV)模型,各子模型狀態轉移方程如下。

1)CV 模型

狀態向量

狀態轉移方程

式中,μa和μω分別為AUV 加速度和角速度的零均值高斯白噪聲。

2)CA 模型

狀態向量

狀態轉移方程

3)CTRV 模型

狀態向量

狀態轉移方程

當偏航角為0 時

在IMM 算法中使用上述3 個子運動模型進行EKF 濾波更新并完成IMM 循環流程,實現濾波器對主AUV 運動狀態的跟蹤。

3 仿真結果與分析

3.1 仿真環境設置

為驗證算法的有效性,進行如下仿真研究。采用雙領航式主從AUV 協同系統,主從AUV 狀態更新周期皆為Δt=1 s,探測信號和狀態通信信號傳播速度均為Vp=1 500 m/s,發送時間間隔均設置為20 s。從AUV 作速度為3 m/s的定深直線航行;主AUV 分別在時間[0 s,1 000 s]、[1 500 s,2 000 s]和[2 500 s,3 000 s]內作勻速直線運動,[1 000 s,1 500 s]內作加速度為0.1 m/s2的勻加速度運動,[2 000 s,2 500 s]內以-0.1 m/s2的加速度作變速運動,初始速度設置為3 m/s,主AUV 高精度導航設備速度和角速度傳感器的測量噪聲分別為σv(k)=0.3 m/s和σω(k)=0.05(°)/s,從AUV 低精度導航設備速度和角速度傳感器的測量噪聲分別為σv(k)=0.5 m/s,σω(k)=0.1(°)/s。IMM-DEKF 算法僅使用3 個運動模型,因此模型轉換矩陣取值為p11=p22=p33=0.98,p12=p21,p13=p31,p23=p32,p12=p13=p23=0.01。

上述環境條件下,驗證文中協同定位算法,并將以下算法結果進行仿真對比。其中:S1 為理想環境下無延遲,水聲信號頻率與航位推算頻率一致的情況;S2 為忽略通信延遲的EKF 協同定位算法;S3 為考慮通信延遲的DEKF 協同定位算法;S4為考慮通信延遲的IMM-DEKF 協同定位算法。

3.2 仿真結果

繪制航跡圖與定位誤差均方根誤差(root mean square error,RMSE)曲線,對定位誤差進行分析比對,誤差曲線是運行100 次蒙特卡洛仿真所得定位誤差的統計平均值。

圖4 為主從式多AUV 系統各AUV 理想軌跡與實際航行軌跡圖,局部放大圖為[2 000 s,2 500 s]時間段內,2 個主AUV 均作減速運動過程中,從AUV 根據主AUV 通信傳遞的信息完成濾波的軌跡細節圖,可以看出在此時間段,S4 算法下從AUV實際航行軌跡更靠近理想軌跡。圖5 和圖6 表明,隨著主AUV 運動狀態的改變,從AUV 對其的航跡預測會產生較大誤差。由圖7 可知,S2 算法表明延遲時間的存在使得從AUV 協同定位結果產生了極大的誤差;而在整個運動過程中,S3 算法都能夠在一定程度上降低由水聲通信延遲引起的定位誤差,但S3 算法在[1 000 s,1 500 s]和[2 000 s,2 500 s]兩段變速運動時間內,相較于其自身在其他時間段內產生了明顯的誤差增大;S4 算法對此有明顯的改善。下文僅針對S3 與S4 算法即DEKF和IMM-DEKF 這2 種算法進行比較,并給出IMM方法全過程模型概率變化圖。圖8 單獨比較DEKF 與IMM-DEKF 算法定位結果誤差。表1 列出了主AUV 在[1 000 s,1 500 s]時段作變速運動,濾波器對主從AUV 狀態估計的具體誤差數據;表2 列出了主AUV 在[1 500 s,2 000 s]時段作勻速運動,濾波器對主從AUV 的估計狀態誤差,此時間段內,IMM-DEKF 與DEKF算法對主從狀態估計定位誤差相當;最后,表3 列出了在[2 000 s,2 500 s]內主AUV 又作變速運動期間,濾波器對主從AUV 狀態估計的具體誤差數據。由表4 統計的誤差降低百分比可知,主AUV作變速運動時,IMM-DEKF 對主AUV 的狀態預測誤差相較于DEKF 算法降低了40%以上,從AUV定位誤差降低了30%左右。由圖9 的全過程概率分布圖可知,整個協同定位過程中,IMM-DEKF 算法可有效跟蹤主AUV 的運動狀態。圖10 和圖11表示從AUV 濾波器對雙領航主AUV 狀態預測的誤差,在變速運動期間,IMM-DEKF 算法對于主AUV 的航跡預測誤差相較于使用DEKF 有明顯降低,實現了對主AUV 狀態更優的跟蹤結果。

圖4 多場景各AUV 軌跡圖Fig.4 Track diagram under multi-scene of each AUV

圖5 濾波器中主AUV1 RMSE 曲線Fig.5 RMSE curves of master AUV1 in the filter

圖7 多場景下從AUV 定位距離RMSE 曲線Fig.7 RMSE curves of slaver AUV positioning distance in multi-scene

圖8 DEKF 與IMM-DEKF 算法下的從AUV 距離RMSE曲線對比Fig.8 Comparison of RMSE curves from slaver AUV between DEKF and IMM-DEKF algorithms

表1 [1 000 s,1 500 s]時段濾波狀態誤差平均值Table 1 Average values of state errors in [1 000 s,1 500 s]

表2 [1 500 s,2 000 s]時段濾波狀態誤差平均值Table 2 Average values of state errors in [1 500 s,2 000 s]

表3 [2 000 s,2 500 s]時段濾波狀態誤差平均值Table 3 Average values of state errors in [2 000 s,2 500 s]

表4 IMM-DEKF 算法下各AUV 距離誤差下降百分比Table 4 Percentage decrease in distance error of each AUV

圖9 IMM-DEKF 算法下AUV 模型概率更新過程Fig.9 Model probability update processes of AUV in IMM-DEKF algorithm

圖10 DEKF 與IMM-DEKF 算法下主AUV1 航跡預測距離RMSE 曲線Fig.10 RMSE curves of track prediction distance from master AUV1 under DEKF and IMM-DEKF algorithms

圖11 DEKF 與IMM-DEKF 算法下主AUV2 航跡預測距離RMSE 曲線Fig.11 RMSE curves of track prediction distance of master AUV2 under DEKF and IMM-DEKF algorithms

4 結束語

協同定位技術是多AUV 系統的關鍵技術之一。文中針對主從式多AUV 系統水下運行過程中不可避免的水聲通信延遲進行定位算法優化。水聲通信傳播時間延遲依賴于水聲速度與主從AUV 間距,延遲時間隨距離的增加而增加。文中在建立AUV 非線性運動模型的基礎上,分析了基于EKF 的延遲時間處理算法,將探測信號與狀態通信信號分時分地發送,避免信號出序到達,同時增加固定時間段內可用量測信息量,在從AUV 濾波器中對主AUV 進行航跡預測狀態估計,為有效實現對主AUV 運動狀態的跟蹤,使用3 種常見運動模型作為IMM 算法子濾波器模型,通過馬爾可夫轉移概率及模型概率的更新實現多模型間的切換。仿真結果表明,基于IMM 的協同定位水聲傳播延遲處理方法能夠有效跟蹤主AUV 運動狀態,影響從AUV 定位精度,減小由水聲通信延遲引起的定位誤差。

下一步將針對以下幾點開展研究:1)解決短時間間隔通信條件下信號出序到達問題,以及考慮可能存在的通信信號丟失等情況;2)優化IMMDEKF 算法中多模型交互的模型概率更新部分,豐富模型以及加速運動模型匹配,以應對更加復雜的水下環境,并緩解IMM 方法的滯后特點。