基于最大相關熵的水下應答器位置標定方法

李佩娟 ,楊書濤 ,李 睿 ,杜俊峰 ,劉義亭

(1.南京工程學院 工業中心/創新創業學院,江蘇 南京,211167;2.南京工程學院 自動化學院,江蘇 南京,211167)

0 引言

自主水下航行器(autonomous undersea vehicle,AUV)在海洋資源開發、目標探測以及水下作戰等方面有著至關重要的作用[1-2],而高精度的導航定位是解決水下作業的前提。由于聲波在水下的傳輸距離遠、信號衰減損失小,因而水聲導航被廣泛應用于AUV 的導航與定位中。超短基線(ultrashort base line,USBL)定位系統是一種常見的聲學導航方式,由于其體積小、便攜性好等優點,已被廣泛應用于AUV 的水下高精度導航與定位[3]。

由USBL 的工作原理可知[4],應用USBL 系統進行導航定位的前提是需要已知精確的應答器位置。因此,水下應答器的位置標定成為應用USBL進行導航定位的關鍵[5]。傳統的聲學應答器標定依靠長基線定位技術[6],多采用消元法或牛頓迭代法[7-8]。文獻[9]提出了一種基于遞推最小二乘方法的在線標定方法,考慮等效聲速的影響,具有可在線標定、精度高的優勢。然而通常水下環境較為復雜,聲學測量值容易受到野值的干擾[10]。文獻[11]考慮聲學野值的影響,提出了一種基于高斯牛頓迭代的應答器位置標定方法,提高了標定方法的魯棒性。然而這些方法都是以USBL 輸出的斜距信息為觀測量,未能充分利用USBL 的其他測量信息,當斜距誤差較大時,標定精度也將隨之降低。文獻[12]提出了一種基于斜距與方位角觀測模型的聲學應答器位置標定方法,經驗證其優于傳統方法。針對量測方程的非線性,可采用無跡卡爾曼濾波(unscented Kalman filter,UKF)的方式進行狀態估計。但UKF 計算量大,因此線性化地標定模型將更方便應用于實際場合。

因此,針對現有聲學應答器標定中存在的問題,文中采用一種斜距與方位角觀測的標定模型,提高觀測量的冗余性,并推導了線性化的量測模型,采用線性卡爾曼濾波技術,更加易于工程實踐。針對聲學野值對標定精度的影響,M 估計是一種有效的抗差估計方法[12-13]。同時,由于基于Huber 的核函數對異常誤差的處理能力有限,意味著在測量信息不可用的情況下,基于Huber 的算法仍然會提供一定的權重,這將降低濾波精度[14]。而最大相關熵卡爾曼濾波器(maximum correntropy Kalman filter,MCKF)可以用來處理非高斯噪聲以及野值干擾的問題,尤其對于較大的野值,往往具有比M 估計更好的抗差性能[15]。因此,文中提出一種結合最大相關熵的水下聲學應答器位置標定方法,在面對水下復雜環境干擾時,該標定方法具有高精度以及強魯棒性的優勢。

1 模型描述

在介紹聲學應答器標定模型之前,首先定義文中的坐標系。

載體坐標系(obxbybzb)以運動載體的中心為原點,xb軸指向載體右方,yb軸指向載體縱軸向前,zb軸指向載體上方。b系與載體固連。

導航坐標系(onxnynzn)是慣性導航在解算導航參數時采用的參考坐標系。文中采用“東-北-天”的地理坐標系作為導航坐標系。

USBL 聲學基陣坐標系(ouxuyuzu)以基陣中心為原點,xu指向基陣右側,yu指向基陣前方,zu指向天向并與xu、yu構成右手坐標系。

地球坐標系(oexeyeze) 以地心為原點,xe軸與ye軸在地球赤道平面內,其中xe軸指向本初子午線,ze軸為地球自轉軸,指向北極。地球坐標系與地球固連,因此也可稱為地心地固坐標系。

1.1 USBL 定位模型

USBL 定位系統由布放在水下的應答器以及1 個裝在AUV 上的集成聲頭組成,該聲頭由1 個換能器以及多個水聽器組成。換能器負責向外發射聲信號,應答器接收聲信號并返回至水聽器接收,通過測量聲信號在換能器與應答器之間的往返時間,在已知聲速的情況下,即可得到斜距值。在聲信號到達水聽器的時候,通過對波達方向的估計,即可得到應答器在聲學基陣坐標系下的方位角。如圖1 所示。

圖1 超短基線系統定位示意圖Fig.1 Positioning of USBL system

USBL 根據測得的時間以及波達方向,可以得到斜距r,以及與x軸和y軸的方位角 α和 β。根據USBL 的輸出,可以計算出應答器在基陣坐標系下的相對位置為

式中:L和h分別為緯度和高度;RM和RN分別為地球子午圈和卯酉圈曲率半徑。

1.2 傳統應答器位置標定模型

在應答器標定過程中,測量船在水面圍繞應答器航行,同時記錄下由全球定位系統(global positioning system,GPS)測得的連續多組測量船的位置。以第1 個位置為參考原點,其余所有位置轉化到以初始位置為原點的直角坐標系下。因此,坐標轉化后船的位置為

可以根據球面交匯原理,建立應答器位置標定的數學模型為

基于式(5)進行1 階的泰勒展開,在得到多組斜距觀測值的情況下,即可根據遞推最小二乘法或者牛頓高斯迭代法對應答器的位置進行估計。

然而傳統方法只考慮了斜距,沒有充分利用方位角的觀測量。從式(1)可看出,方位角與應答器位置之間存在高度的非線性。因此,給出基于方位角和斜距觀測量的線性化量測方程推導模型。

2 基于MCKF 的應答器位置標定方法

以斜距和方位角為觀測量推導線性化的標定量測方程,同時考慮到聲學野值的影響,給出一種基于MCKF 的應答器位置標定方法。

2.1 狀態空間模型

將安裝誤差角以及應答器位置作為系統狀態量,建立基于斜距與方位角觀測的卡爾曼濾波器模型。狀態量為

由于安裝誤差和應答器位置都是常值,因此,其狀態微分方程為

系統狀態方程可以表示為

式中,FUSBL(t)=06×6表示6×6的零矩陣。

2.2 基于斜距與方位角的量測模型

式(2)可以改寫為

姿態精度越高,標定的效果就越好。因此,文中標定算法的姿態輸出采用慣性導航系統(inertial navigation system,INS)/GPS 組合導航系統的姿態輸出結果,以保證姿態精度盡可能地高。

傳統的標定方法優勢在于標定過程中不需要考慮安裝誤差,然而其缺點也很明顯,即標定精度嚴重依賴于斜距誤差。考慮了方位角模型后,如式(9) 所示,勢必會引入安裝誤差,導致系統待估計參數增加。其優點在于,位置標定的精度將由方位角和斜距誤差共同決定,在斜距誤差很大時,只要方位角誤差足夠小,依然能夠使位置標定保持較高精度。

下面將推導出聯合斜距與方位角的量測模型。改寫式(9)可以得到

其中

根據式(10),應答器在u系下的位置可以表示為

由于誤差的存在,估計的安裝誤差角、應答器位置和真值之間會有差異,真實的角度誤差、位置誤差與估計值之間的關系有

引入誤差量,將式(15)代入(14),并忽略高階誤差量可以得到

其次,式(1)顯示了斜距與方位角信息和基陣坐標系下應答器位置之間關系,改寫式 (1) 可得

式(18)的偏微分方程為

其中

結合式(17)和(19),可以得到觀測量與狀態量的關系為

因此,系統的觀測方程為

式中: αINS,βINS,rINS為由慣性導航計算的斜距與方位角信息;αUSBL,βUSBL,rUSBL為USBL 的輸出信息;V為測量噪聲。

2.3 MCKF 設計

為減小異常值的影響,采用基于最大相關熵準則的殘差處理方法。相關熵表示2 個隨機變量X和Y之間的一種廣義相似度量,可以表示為[16]

式中:E[·]表示求期望;κ (X,Y) 表示核函數;F(X,Y)表示隨機變量X和Y的聯合概率密度函數。

選取高斯函數作為核函數,表達式為

因此,相關熵的估計值可以表示為

對于一個線性模型,有

式中:Fk-1為狀態轉移矩陣;vk為量測噪聲。

則狀態量的最大熵代價函數為

用高斯核函數代入代價函數的量測殘差部分可以得到

對式(28)在xk處求偏導,可以得到

代入核函數的表達式,可以整理得到

式中,ψk=diag[Gσ(eki)]。

因此,經過最大相關熵準則修正后的新的量測方差矩陣可以表示為

基于上述方法,結合2.1 和2.2 節的標定量測方程和狀態空間方程,應答器標定濾波算法可以總結如圖2 所示。

圖2 基于MCKF 的水下應答器位置標定方法Fig.2 Calibration method of underwater transponder position based on MCKF

圖中,Fk-1表示k-1 時刻的狀態轉移矩陣,由FUSBL(t)離散化得到。

3 試驗驗證

為驗證所提的基于最大相關熵濾波的應答器位置標定方法,在長江進行了試驗。

試驗中,將所提的MCKF 方法與高斯牛頓(Gauss-Newton,GN)迭代法以及基于Huber-M 估計的卡爾曼濾波(Huber-M Kalman filter,HKF)方法進行對比。包含的傳感器有USBL、慣性測量單元(inertial measurement unit,IMU)、GPS 以及GPS/PHINS 組合導航系統,如圖3 所示。由于GPS/PHINS系統的位置精度達到厘米級,因此可以用來作為系統的位置真值。

圖3 試驗設備示意圖Fig.3 Experimental equipment

各個傳感器的性能參數如表1 所示。

表1 傳感器參數Table 1 Parameters of sensors

其中,USBL 定位誤差中的r表示USBL 斜距,即定位誤差與USBL 的作用距離成正比。

試驗中,測量船的航行軌跡如圖4 所示。

圖4 試驗軌跡Fig.4 Trajectory in the field test

在上述試驗軌跡下,USBL 系統輸出的原始斜距與方位角信息量測如圖5 所示。

圖5 超短基線原始量測信息Fig.5 Raw measurement information of USBL system

由于水下環境復雜多變,聲波在水下傳播會產生多徑效應,使得聲波接收點容易發生干涉,導致收到的聲波信號異常產生畸變,引起量測異常通常表現為量測信號中的野值。如圖5 中的紅色圓圈所示,表示USBL 系統量測輸出的異常野值信息,抑制野值對標定精度的影響是要解決的關鍵問題之一。

下面將基于上述試驗軌跡,對野值影響下的標定效果進行對比。由于應答器位置與安裝誤差沒有真值,因此無法給出其估計的誤差,即無法通過比較應答器位置估計誤差以及安裝角估計誤差的形式直接比較標定效果。文中將直接給出應答器絕對位置(經緯度)與安裝誤差(角度)的估計結果對比曲線(如圖6 和圖7 所示),并通過進一步補償應答器位置與安裝誤差后的定位誤差(如圖7 所示),間接比較分析標定的效果。

圖6 應答器位置標定結果Fig.6 Calibration result of transponder position

圖7 安裝誤差角標定結果Fig.7 Calibration results of installation error angle

此外,由于狀態量中含有安裝誤差,其估計結果如圖7 所示。

由于GN 迭代法是一種聯立方程組求解的形式,無法得到隨時間變化的估計結果。因此在圖6和圖7 中只給出了HKF 和MCKF 的對比曲線。GN 迭代法的標定效果將通過補償應答器位置后的定位誤差來比較。從圖6 和圖7 中可以看出,文中方法估計的曲線更加平滑,收斂速度更快。

由于沒有應答器位置和安裝誤差角的真值,無法直接比較3 種方法的效果。通常情況下,應答器位置越精確,根據其位置計算的船的位置精度越高。因此可以通過不同標定方法得到的應答器位置計算USBL 對測量船的定位誤差,間接比較標定的效果。將GN、HKF 以及文中方法標定的應答器位置代入到式(2)中,結合USBL 的輸出值計算船的位置,并與真值比較,間接比較標定效果。

采用均方根誤差(root mean square error,RMSE)作為性能評價指標,對采用不同應答器位置計算的船的位置誤差進行統計,其在東北天3 個方向上的RMSE 直方圖如圖8 所示。

圖8 位置均方根誤差比較Fig.8 RMSE comparison of positioning results

從圖中可以看出,采用文中方法得到的應答器計算的位置誤差精度最高,相比于傳統的GN 迭代法,東北天方向上的定位精度分別提高了48.3%、48.2%和40.4%,間接體現了文中方法標定的效果最好。因此,標定方法具有實用性,得到的應答器位置可以用于USBL/INS 的組合導航中。

4 結束語

精確的應答器位置是應用USBL 定位系統進行導航定位的關鍵和前提。針對傳統基于斜距觀測方法存在精度不高、魯棒性差的問題,文中推導了一種線性化的基于斜距和方位角的應答器標定量測模型。此外,考慮到復雜水下聲場環境引起的量測野值的干擾問題,提出一種基于最大相關熵濾波器的標定方法,對于量測粗差具有較好地抑制作用。試驗結果也證明了該方法標定精度高、抗噪性能強。標定后的應答器提高了USBL的定位精度,可滿足AUV 提供長航時、高精度的導航定位需求。

文中研究雖較好地解決了復雜環境下地應答器位置標定問題,但其斜距計算是基于常值聲速的假設,在深海等環境,聲線彎曲、聲速不均勻分布等因素對斜距誤差的影響較大。未來將進一步考慮水下聲速變化影響下的標定以及定位問題,以提高系統的定位精度。