UUV協同導航下MEMS陀螺儀協同標定

王秋瀅,尹 娟

(哈爾濱工程大學 信息與通信工程學院,黑龍江 哈爾濱,150001)

0 引言

協同導航是指通過集群航行器之間導航信息的共享,使單航行器的導航誤差得到校正的導航方式。在協同導航中,只需要給少量航行器配備高精度傳感器,即可達到較好的導航精度。因此,協同導航不僅可以提高系統整體的導航性能,并且大大節約了成本[1],已廣泛應用于無人水下航行器(unmanned undersea vehicle,UUV)集群中。

由于在水下無法有效獲取高精度的全球定位系統(global positioning system,GPS)衛星信號進行導航定位,慣性導航、多普勒測速儀(Doppler velocity log,DVL)、長基線導航與航位推算被廣泛使用于UUV的導航[2]。目前,航位推算是最常用的低成本導航解決方案,是指在已知初始位置的情況下,利用姿態傳感器提供的航向信息以及DVL提供的速度信息對UUV的位置信息進行遞推計算[2-6]。三軸地磁傳感器與兩軸微機電(microelectromechanical system,MEMS)陀螺儀組合姿態傳感器具有功耗低、成本少、自主性強等優點,已經越來越多地應用于從UUV導航中。

由于航位推算的誤差受導航傳感器精度影響較大,當導航傳感器精度不高時,往往需要對航位推算的誤差及時校正。協同導航是最常使用的UUV航位推算誤差校正方法。目前最主要的UUV協同導航方式為單領航艇主從式協同導航,在此方式中存在單個配備高精度導航系統的主UUV,多個配備低精度導航系統的從 UUV。從UUV利用主UUV發送的自身高精度導航信息以及主從UUV的相對距離對從UUV自身導航誤差進行校正。

單領航艇主從式 UUV協同導航主要依賴UUV間的水聲通信,如果水聲通信中斷,協同導航將不可用。由于MEMS陀螺儀精度較差,陀螺漂移大[7],嚴重制約了從UUV自身導航精度,此時若從 UUV上浮接受 GPS信號校正自身誤差,則增加了從UUV能量消耗,減少了從UUV工作時間。因此,必須對MEMS陀螺儀的陀螺漂移進行辨識補償,進而提高從UUV的導航精度,以達到在水聲通信短時失效時,從 UUV導航精度仍能滿足自身需求的目的。

目前,對于協同標定方面研究較少,文獻[8]對協同導航下的 MEMS陀螺儀漂移標定進行了研究,但文獻中僅對單軸陀螺進行標定,并且沒有對協同標定路徑設計進行分析。

針對上述問題,文中利用基于擴展卡爾曼濾波(extended Kalman filter,EKF)的協同導航結果作為標定觀測量,在協同導航正常運行時在線解算出兩軸 MEMS陀螺的陀螺漂移,提高從 UUV自身導航精度,以達到在協同導航短時失效時,從 UUV導航精度能夠滿足自身需求的目的。通過可觀測性分析,確定協同標定的路徑設計原則,并根據該原則設計 3組協同標定路徑,通過濾波算法在線標定 MEMS陀螺儀的陀螺漂移并補償。仿真結果驗證了該方法可以快速有效地解算MEMS陀螺漂移,從而提高從UUV自身導航精度。

1 UUV集群協同標定模型及算法

1.1 UUV集群協同導航模型及算法

1.1.1 協同導航模型

UUV的運動可分為相對獨立的水平面運動和垂直面運動,因為深度可由壓力傳感器直接測得,因此 UUV導航定位問題的研究可由三維空間轉化至二維平面內。最常解決二維平面內導航定位問題的導航方式為航位推算[9]。在已知初始位置的情況下,利用三軸地磁傳感器與兩軸微機電 MEMS陀螺儀組合姿態傳感器提供的載體航向信息以及 DVL等速度傳感器提供的載體速度信息對載體的位置信息進行遞推計算,進而實現水下航行器的導航定位。

地磁傳感器三軸分別指向東北天,MEMS陀螺儀兩軸分別指向載體的ox、oz軸。地磁傳感器無法獨立解算3個姿態角,需要已知1個姿態角才能解算另外2個姿態角[10]。因此,文中由兩軸陀螺儀解算航向角提供給三軸地磁傳感器解算橫滾角和俯仰角,地磁傳感器解算的橫滾角與俯仰角再提供給陀螺儀解算下一時刻的航向角。

將 UUV的運動視為點運動體,即其運動軌跡可在東北向坐標系下進行描述。若已知 UUV初始時刻的位置 (px,1,py,1),則k時刻 UUV的位置可通過航位推算模型表示為

式中:Δt為采樣時間間隔;Vk-1為 DVL測量的k-1時刻的UUV速度信息;φk,φk-1分別為k,k- 1時刻 UUV 的航向角信息;γk-1為k-1時刻UUV 的橫滾角信息;θk-1為k-1時刻 UUV 的俯仰角信息;為z軸MEMS陀螺儀測量的k-1時刻航向角角速度信息;為z軸MEMS陀螺儀測量的k-1時刻航向角角速度信息。

航位推算模型可簡單描述成

系統噪聲協方差矩陣

雖然航位推算應用簡單、成本低廉,但是航位推算誤差會隨時間累積,導航效果取決于傳感器的量測精度。配備低精度傳感器的從 UUV由于傳感器精度有限,導致航位推算精度較低,航位推算誤差的時間累積性比較明顯[11-12]。為抑制航位推算誤差的不斷積累,考慮以主、從UUV相對距離為觀測量,通過協同導航算法,修正從UUV的位置誤差和航向角誤差,再將修正后的航向角提供給地磁傳感器解算橫滾角和俯仰角。

根據k時刻接收到的主 UUV參考位置信息以及從 UUV自身位置狀態得到相應的距離觀測方程。

1.1.2 協同導航算法

由于協同導航系統的非線性特性,采用 EKF進行協同導航算法設計。根據上一節的協同導航模型,可得系統的濾波方程

從UUV在k時刻的狀態估計值

系統一步預測協方差為

式中,Φk-1為狀態轉移矩陣,且

濾波增益為

狀態更新為

協方差更新為

因此,只需給出從 UUV的初始狀態量和初始化濾波方差,就可根據式(6)～式(12)遞推估計從UUV每一時刻的位姿信息[13]。

1.2 在線標定模型及算法

1.2.1 在線標定模型

標定是一種針對 MEMS陀螺儀的有效誤差補償方法。傳統的離線標定工作量太大并且需要巨大的成本投入,因此在線標定是減小MEMS陀螺儀陀螺漂移的首要選擇。文中借助協同導航的結果,以協同導航校正后的航向角與未經校正的航向角差值以及地磁傳感器輸出的俯仰角與陀螺解算的俯仰角差值為觀測量,通過濾波算法對兩軸MEMS陀螺儀的陀螺漂移進行在線解算。

陀螺漂移是 MEMS陀螺儀最主要的確定性誤差,其是慣性傳感器輸入為零時的輸出。MEMS陀螺的陀螺漂移也稱作零位漂移,單位常用(°)/h表示,陀螺漂移常記作ε。在水聲通信短時失效時,陀螺漂移可認為是常值誤差。

通過兩軸 MEMS陀螺儀可解算航向角與俯仰角

MEMS陀螺儀在線標定狀態模型為

在線標定模型可簡單描述成

在線標定觀測方程為

式中,vk為觀測噪聲矩陣。

1.2.2 在線標定算法

從UUV的MEMS陀螺儀在線標定模型為線性模型,文中選擇卡爾曼濾波(Kalman filter,KF)算法進行解算。根據上一節的在線標定模型,可以得到系統的濾波方程為

狀態一步估計為

一步預測協方差陣為

濾波增益矩陣為

狀態更新為

協方差矩陣更新為

與EKF相同,只需給出初始狀態量和初始化濾波方差,就能夠遞推估計每一時刻的狀態量。

2 協同標定可觀測性分析及路徑設計

卡爾曼最早提出可觀測性的概念:在一個線性系統中,若系統的觀測信息唯一地確定系統所有狀態量,則稱該系統是可觀測的。系統的可觀測性反映了觀測信息對于系統狀態信息的解算能力。對于UUV協同標定來說,觀測信息有限、觀測能力較弱,因此,在協同標定之前有必要對系統的可觀測性進行分析,明確系統的可觀測條件,設計協同標定路徑。

以如下所示的線性時變離散系統為例,說明系統可觀測條件。

式中:x(k)為n維狀態向量;y(k)為m維輸出向量。

對于式(23)所示的線性時變離散系統完全可觀測的充分必要條件為

式中,N為系統的可觀測性矩陣。

協同導航觀測信息是一維距離量測量,因此至少需要進行3次觀測才能獲得系統的三維狀態量唯一解。當進行3次距離觀測時的系統可觀測矩陣為

已知k、k+1、k+2時刻主UUV的位置為和以及主從UUV之間的距離觀測量和則系統可觀測矩陣進一步表示為

由于第 3列的第 1個元素為0,故從可觀測矩陣可得:當第 1列或第 2列為 0時,即相鄰 3個量測時刻主從 UUV的x軸或者y軸坐標是相同的,系統不可觀測;或當第 1列或第 2列線性相關時,矩陣不滿秩,即

式中,θk為k時刻距離觀測方位角。

從UUV的MEMS陀螺儀在線標定模型可以看出,當UUV的俯仰角與橫滾角均為小量時,航向角誤差可近似看做z軸MEMS陀螺儀漂移的累積,俯仰角誤差可近似看做x軸MEMS陀螺儀漂移的累積,即

此時的MEMS陀螺儀在線標定可觀測矩陣

由于 UUV正常定深航行下,其俯仰角與橫滾角僅有小幅度擺動,因此最簡單有效的標定路徑為從UUV定深定向航行。

綜上所述,協同標定路徑設計需要遵循以下原則:1)相鄰3個量測時間的主從UUV距離觀測方位角不同且從UUV定深定向航行。

3 仿真驗證與分析

為了驗證協同標定的有效性,利用MATLAB進行仿真驗證。設定兩軸MEMS陀螺儀陀螺漂移為10°/h;角度隨機游走為 1°/h;從 UUV在載體系三軸上有搖擺的角速度,k時刻分別為

為了驗證協同標定路徑設計原則的有效性,設計3種路徑,對兩軸MEMES陀螺儀陀螺漂移進行解算,路徑設計如下。

路徑 1:主 UUV 初始位置(0,125),以速度2m/s沿固定航向角0°定深航行,從UUV初始位置(0,0),以速度3 m/s沿固定航向角10°定深航行;

路徑 2:主 UUV 初始位置(0,125),以速度1m/s沿固定航向角0°定深航行,從UUV初始位置(0,0),以速度2 m/s沿固定航向角10°定深航行;

路徑3:主UUV初始位置為(0,90),以速度1 m/s沿固定航向角0°方向定深航行;從UUV初始位置(0,0),以速度2 m/s沿固定航向角-5°定深航行。

3.1 路徑1仿真

協同導航EKF濾波器初始設置如下[14-15]

陀螺零偏標定KF濾波器初始設置如下

主、從UUV軌跡如圖1所示。協同標定仿真結果如圖 2所示。由圖2可以看出,在路徑 1下,1 min內濾波估計值趨于收斂,兩軸MEMS陀螺漂移標定值為10°/h。

圖1 路徑1下主、從UUV軌跡Fig.1 Tracks of master and slave UUVs in path 1 test

圖2 路徑1下微機電陀螺儀陀螺漂移估計結果Fig.2 Estimation results of gyro drift of micro-electro mechanical system(MEMS)gyroscope in path 1 test

圖3為路徑1下從UUV航行10 min時協同標定補償結果圖,可以看出沒有進行標定補償的定位誤差在10 min后達30 m,而補償后的軌跡與真實軌跡幾乎完全重合。由局部放大圖知,從 UUV補償后定位誤差不到1 m,導航精度得到極大提高。

3.2 路徑2仿真

協同導航EKF濾波器初始設置如下

圖3 路徑1下協同標定補償結果Fig.3 Results of collaborative calibration compensation in path 1 test

陀螺零偏標定KF濾波器初始設置如下

主、從UUV軌跡如圖4所示。

圖4 路徑2下主、從UUV軌跡Fig.4 Tracks of master and slave UUVs in path 2 test

協同標定仿真結果如圖5所示。路徑2下的協同標定補償結果如圖6所示。

圖5 路徑2下MEMS陀螺儀陀螺漂移估計結果Fig.5 Estimation results of MEMS gyroscope gyro drift in path 2 test

圖6 路徑2下協同標定補償結果Fig.6 Results of collaborative calibration compensation in path 2 test

由圖4～圖6可以看出,在路徑2下濾波估計值1 min內趨于收斂,MEMS陀螺零偏誤差標定值為10°/h,標定補償后從UUV導航精度得到極大提高。

3.3 路徑3仿真

協同導航EKF濾波器初始設置如下

陀螺零偏標定KF濾波器初始設置如下

主、從UUV軌跡如圖7所示。協同標定結果如圖8所示。路徑3下的協同標定補償結果如圖9所示。

圖7 路徑3下主、從UUV軌跡Fig.7 Tracks of master and slave UUVs in path 3 test

圖8 路徑3下MEMS陀螺陀螺儀漂移估計結果Fig.8 Estimation results of MEMS gyroscope gyro drift in path 3 test

圖9 路徑3下協同標定補償結果Fig.9 Results of collaborative calibration compensation in path 3 test

由圖7～圖9可以看出,在路徑3下濾波估計值1 min內趨于收斂,MEMS陀螺零偏誤差標定值為10°/h,標定補償后,從UUV導航精度得到極大提高。

綜上所述,在 3條標定路徑下,在短時間內完成對兩軸MEMS陀螺儀陀螺漂移的標定,補償后的從 UUV導航誤差明顯減小。仿真結果驗證了協同標定的有效性以及協同標定路徑設計原則的準確性。

4 結束語

針對 UUV協同導航水聲通信短時失效時協同導航不可用,且從 UUV配備的三軸地磁傳感器與兩軸微機電 MEMS陀螺儀組合姿態傳感器中MEMS陀螺的陀螺漂移大,導致UUV航位推算精度差的問題,文中提出了 UUV協同導航下兩軸 MEMS陀螺儀協同標定方法并推導了協同標定路徑設計原則。利用基于EKF的協同導航結果作為標定觀測量,在協同標定路徑下在線解算MEMS陀螺儀漂移并補償,提高了從 UUV自身導航精度。當主、從 UUV通信短時中斷時,從UUV不需要上浮接受GPS信號校正自身導航誤差,降低了從UUV能量消耗,增長了從UUV工作時間。仿真結果表明,文中提出的算法能夠快速有效估計出 MEMS陀螺儀漂移,極大提高從UUV導航精度。考慮時間延遲等誤差的從UUV協同標定將是下一步研究的重點。

相關文章導航

1.鐘宏偉,李國良,宋林樺,等.國外大型無人水下航行器發展綜述[J].水下無人系統學報,2018,26(4).

2.嚴浙平,宋金雪.模型輔助和洋流估計下的UUV導航定位方法[J].水下無人系統學報,2017,25(5).

3.鐘宏偉.國外無人水下航行器裝備與技術現狀及展望[J].水下無人系統學報,2017,25(4).

4.錢東,趙江,楊蕓.軍用 UUV 發展方向與趨勢(上)——美軍用無人系統發展規劃分析解讀[J].水下無人系統學報,2017,25(2).

5.劉明雍,沈俊元,張加全,等.一種基于無跡卡爾曼濾波的UUV協同定位方法[J].魚雷技術,2011,19(3).