模型輔助和洋流估計下的UUV導航定位方法

嚴浙平, 宋金雪

?

模型輔助和洋流估計下的UUV導航定位方法

嚴浙平, 宋金雪

(哈爾濱工程大學自動化學院, 黑龍江哈爾濱, 150001)

針對深海航行中無人水下航行器(UUV)受多普勒測速儀(DVL)測距的限制, 無法進行對底跟蹤的問題, 提出了洋流估計下的UUV動力學模型導航定位方法。利用基于擴展卡爾曼濾波(EKF)的洋流估計方法, 解算出UUV作業海域洋流信息, 將洋流信息加入到UUV系統模型中, 對UUV動力學模型無法感知外界環境產生的誤差進行了補償, 從而提高了導航定位的精度。仿真結果證明了該算法的可行性以及有效性, 對未來UUV深海探索具有很好的實用性。

無人水下航行器(UUV); 動力學模型; 擴展卡爾曼濾波; 洋流估計; 導航定位

0 引言

《深海關鍵技術與裝備》重點專項是我國“十三五”科技計劃改革實施以來首批部署實施的重點研發計劃。圍繞進入深海-認知深海-探查深海-保障安全這一主線, 重點突破制約我國在深海領域發展能力的核心共性關鍵技術, 形成我國深海運載、探測裝備譜系化和配套能力, 作業支持能力以及在深海油氣和礦產資源開發方面的應用能力, 帶動深海技術和裝備產業的發展。全海深(最大工作水深11 000 m)無人水下航行器(unmanned undersea vehicle, UUV)研制、定位及探測技術是重點研究方向之一。

在海中執行任務時, UUV只有具備高精度的導航定位系統, 才能保證其安全作業并安全返回[1-2]。UUV常用的傳統導航定位系統是利用多普勒測速儀(Doppler velocity log, DVL)、姿態傳感器和壓力傳感器測量得到的UUV的速度信息、姿態角信息和深度信息, 從UUV已知的初始位置進行位置推算, 獲得UUV的地理位置[3-4]。受到DVL測量距離的影響, 并不是所有情況下UUV都能進行對底跟蹤。以Seatronics公司生產的導航DVL-DI Workhourse為例, 其最大的測量距離為200 m[4]。如圖1所示, 當UUV深海作業時, 無法進行對底跟蹤, 根據測流原理DVL測得UUV相對海水的速度。此時, 若不考慮洋流對UUV的作用, 采用UUV相對洋流速度進行航位推算, 將產生較大的定位誤差。因此通常情況下采用超短基線(ultra short base line, USBL)定位系統實時定位UUV[5], 由于水面船和UUV的實時通信, 大大增加了UUV的能量消耗, 縮短了UUV作業時間。

圖1 多普勒測速儀測距及深海無人水下航行器定位

針對上述問題, 文中將USBL定位系統與UUV動力學模型相結合, 并利用擴展卡爾曼濾波(extended Kalman filter, EKF)算法實現洋流信息的估計, 修正UUV系統, 得到洋流干擾下的UUV動力學模型, 提高了模型輔助下UUV導航定位精度。得到精準的UUV動力學模型后, 不再需要USBL實時通信, 從而減小UUV能耗, 縮短了UUV的工作時間, 仿真結果證明了此方法解算洋流信息的有效性, 明顯降低了UUV導航定位誤差。

1 UUV系統模型

1.1 運動學模型

研究UUV在任意空間中六自由度運動時, 通常采用2種正交坐標系: 大地坐標系000, 運動坐標系, 考慮到對洋流的分析, 建立流體坐標系′′′[6]。大地坐標系000采用右手坐標系原則, 定義0軸指向正北,0軸指向正東及0軸垂直于00面, 指向地心。

文中假設洋流是無旋的, 意味著每一個無窮小的流體角速度為零, 因此流體坐標系相對于大地坐標系不發生旋轉[7], 其采用右手坐標系原則, 定義′,′,′ 軸分別指向正北、正東以及地心。運動坐標系的原點通常取在船體的舯縱剖面(一般位于UUV重心),,和軸分別為指向船艏的水平線、指向右舷的橫切線及指向船底的縱切線。

各坐標系關系示意圖如圖2所示。

圖2 各坐標系關系示意

根據各坐標系之間的關系, 推導出UUV各速度向量之間關系表達式

1.2 洋流干擾下的動力學模型

深海航行時, 由于洋流對UUV產生沖擊力, 航行軌跡會發生偏移, 因此建立UUV動力學模型時需將洋流考慮在內。將UUV視為剛體, 根據動量定理和動量矩定理, 建立UUV六自由度動力學模型[8-9]

根據方程(2)和(3)可得

2 考慮洋流的導航定位算法

2.1 洋流估計

2.1.1 定深航行的UUV模型

在很多實際情況下, 非線性運動方程在一個操作點可線性化, 且基于操作點的擾動量進行線性化, 是對于式(6), 操作點及擾動量定義為

動力學模型(式(6))在操作點(V,η)泰勒展開式

定義:

線性UUV動力學模型

式中: 系統向量(t)[u v r x y z Ψ u v];(t-1)為1時刻系統噪聲。

正定矩陣(t)表示系統噪聲方差, 且

記

其中

式中:為UUV自身質量;(·),(·),(·)和(·)為黏性水動力系數;I為UUV相對于運動坐標系G的轉動慣量;0,0分別為UUV初始橫向速度和艏向角;(·)執行機構的增益;為螺旋槳轉數;δ表示舵角。

2.1.2 USBL數學模型

UUV定深航行洋流估計時, 采用USBL定位系統實現UUV實時定位, 在UUV安裝的收發換能器向水面船發送詢問信號, 水面船上安裝的聲學應答器接收到信號后并發送應答信號, 根據水面船地理位置、傳遞的時間間隔以及收發換能器中多個基元交匯測量, 可以確定UUV與水面船之間距離以及UUV相對于水面船的方向角、和, 如圖3所示。

圖3 超短基線數學模型

水面船通過安裝全球定位系統(global positioning system, GPS)獲取位置, 得到系統的量測方程

根據EKF算法公式得到洋流估計算法, 濾波具體過程如下。

1) 一步狀態和方差預測方程

2) 卡爾曼增益和狀態更新方程

3) 估計方差方程

2.2 深海環境下UUV導航定位系統

深海環境下, 在海洋中層定深作業過程中, UUV導航定位算法主要由洋流估計模塊和導航定位模塊組成。洋流估計模塊如上節所述; 導航定位模塊中, 將洋流信息與UUV動力學模型信息融合計算得到的UUV對底速度、姿態傳感器測取的UUV姿態角以及深度計測取的UUV深度信息結合, 經過綜合預處理以及卡爾曼濾波剔除數據野值處理后, 作為航位推算算法輸入信息, 推算出UUV的空間位置。在洋流估計過程中, UUV位置信息以USBL定位信息為準; 當估計得到洋流真實信息后, UUV位置信息通過航位推算算法獲得, 此過程中, USBL不再需要實時通信, 只需當定位誤差較大時進行位置校正, 修正定位誤差。圖4即為深海環境下UUV導航定位系統。

圖4 深海環境下UUV導航定位系統

航位推算算法

3 仿真驗證

3.1 試驗設計

正如上文所述, UUV深海航行時, 洋流是主要環境干擾因素, 洋流參數初始化不正確會嚴重影響UUV動力學模型的性能。

試驗中, 令UUV舵偏轉角為常數, 螺旋槳在水平方向上對UUV產生定常推進力, UUV從初始位置(100 m, 100 m, 6 000 m)定深航行。如圖5所示, UUV將做回轉運動, 仿真中令UUV航行時間為600 s?？梢钥闯? 無洋流干擾時, UUV航行軌跡為一個幾乎完美的圓(見圖5中實線), 但在洋流干擾下(假設洋流大小u=0.5 m/s,v=0.4), UUV航行軌跡發生漂移, 產生較大的位置偏差(如圖5中虛線所示)。

圖5 洋流干擾下的UUV航行軌跡

如圖6, 由于洋流的干擾, UUV會產生300 m左右的縱向位移以及大概240 m橫向位移, 較大的位移誤差嚴重影響UUV作業及返航的安全性。因此, UUV執行作業任務時, 能否準確的估計洋流信息對UUV航位推算及定位至關重要。

圖6 UUV橫向及縱向的位移軌跡

3.2 洋流估計下UUV導航定位

EKF濾波算法中, 設舵偏轉角為10°, 水平推進力為1 000 N; UUV的橫向速度、縱向速度、偏航角速度以及艏向角的初始值分別為1 m/s,1 m/s,0°/s0°; UUV航行軌跡的初始位置為(,,)=(100 m, 100 m, 6 000 m); USBL的初始位置為(0, 0, 0)?？紤]到洋流為水平面方向上未知的定向流, 且未知洋流不具有任何先驗信息,洋流的估計初始速度為u=0 m/s,v=0 m/s, 仿真結果如圖7所示。

圖7 擴展卡爾曼濾波洋流速度估計

圖7表示EKF算法下洋流估計結果, 可以看出, 文中提出的EKF算法能較準確且快速的估計洋流信息, UUV航行時間大概為100 s時, 估計出的洋流信息接近真實值。由艏向角隨時間變化曲線(圖8)可知, 艏向角變化周期為60 s, 即UUV完成一次回轉運動的時間。因此, 定深航行的UUV只需做2次回轉運動即可估計出洋流速度信息。

圖8 UUV艏向角變化曲線

圖9表示深海環境下, 洋流估計、多傳感器及模型輔助的導航定位方法的定位誤差, 即在定深水平面上的估計位置與實際位置的位移差。由于洋流的干擾, 0~100 s過程中產生的定位誤差相對較大, 當洋流估計接近真實值, 航行過程中大部分定位誤差小于1 m, 由此可見, 文中所提算法能有效補償由于洋流對UUV作用所產生的偏移, 算法具有較好的精度和收斂性, 可滿足實際情況下UUV在未知海域執行作業任務的需求。

圖9 UUV定位誤差

4 結束語

針對國家重點研發計劃《深海關鍵技術與裝備》重點專項中關于深海UUV定位技術研究的有關精神, 文中提出一種洋流估計和UUV動力學模輔助下的UUV導航定位方法。USBL測得的UUV位置信息, UUV動力學模型輸出的狀態向量, 利用EKF算法估計出作業海域的洋流信息。實現UUV深海不對底作業情況下的導航定位, 執行任務時不再需要USBL實時測量UUV的位置, 從而降低了UUV能量消耗, 提高UUV的工作時間。仿真結果表明, 文中提出的算法較為精準地估計出洋流的信息, 提高了深海環境下UUV導航定位精度。該方法滿足實際需求, 基本可實現UUV深海定位, 對未來定深航行UUV的導航定位方法具有指導性意義。

文中涉及的USBL定位系統測量信息, 未考慮海流、海浪對水面船只的沖擊擾動以及水聲通信的時間延遲問題, 如何解決這些問題是今后研究的重點。

[1] 嚴浙平, 吳迪, 周佳加, 等. 基于UUV動力學模型的導航方法[J]. 華中科技大學學報(自然科學版), 2013, 41 (11): 108-113.Yan Zhe-ping, Wu Di, Zhou Jia-jia, et al. UUV Dynamic Model-based Navigation Method[J]. Journal of Huazhong University of Science and Technology(Natural Science Edition), 2013, 41(11): 108-113.

[2] ?yvind H, Oddvar H, Bj?rn J. A Framework for Obtaining Steady-State Maneuvering Characteristics of Underwater Vehicles Using Sea-Trial Data[C]//Mediterranean Conference on Control and Automation. Mediterranean: IEEE, 2009.

[3] 周佳加. 基于扁平UUV動力學模型的導航定位與路徑跟隨控制方法研究[D]. 哈爾濱: 哈爾濱工程大學, 2012.

[4] 張福斌, 鮑鴻杰, 段小偉, 等. 一種考慮洋流影響的AUV組合導航算法[J]. 計算機測量與控制, 2012, 20(2): 513-515.Zhang Fu-bin, Bao Hong-jie, Duan Xiao-wei, et al. A AUV Integrated Navigation Algorithm Considering the Effect of Ocean Current[J]. Computer Measurement & Control, 2012, 20(2): 513-515.

[5] 金鑫. 超短基線定位系統檢測技術研究[D]. 哈爾濱: 哈爾濱工程大學, 2008.

[6] 嚴浙平, 吳迪, 周佳加, 等. 基于UUV動力學模型的實時海流估計方法研究[J]. 中國造船, 2012, 53(1): 99-106.Yan Zhe-ping, Wu Di, Zhou Jia-jia et al. An In-situ Ocean- current Estimating Method Based on UUV Dynamic Model[J]. Shipbuilding of China, 2012, 53(1): 99-106.

[7] ?yvind H, Oddvar H. Model-aided INS with Sea Current Estimation for Robust Underwater Navi- gation[J]. IEEE Journal of Oceanic Engineering, 2011, 36(2): 316-337.

[8] 王宏建, 陳子印, 賈鶴鳴, 等. 基于濾波反步法的欠驅動AUV三維路徑跟蹤控制[J]. 自動化學報, 2015, 41(3): 631-645.Wang Hong-jian, Chen Zi-yin, Jia He-ming, et al. There- dimensional Path-following Control of Underactuated Au- tonomous Underwater Vehicle with Command Filtered Backstepping[J]. Acta Automatica Sinica, 2015, 41(3): 631-645.

[9] 馬騁, 連璉. 水下運載器操縱控制及模擬仿真技術[M]. 北京: 國防工業出版社, 2009.

[10] Martinez A, Hemandez L, Sahli H. Modelaided Navigat- ion with Sea Current Estimation for an Autonomous Underwater Vehicle[J]. International Journal of Advanced Robotic Systems, 2015, 12(12): 1-15.

[11] 邊信黔, 周佳加, 嚴浙平, 等. 基于EKF的無人潛航器航位推算算法[J]. 華中科技大學學報(自然科學版), 2011, 39(3): 100-104.Bian Xin-qian, Zhou Jia-jia, Yan Zhe-ping, et al. Dead Re- ckoning Method for Autonomous Underwater Vehicles Based on EKF[J]. Journal of Huazhong University of Sci- ence and Technology(Natural Science Edition), 2011, 39 (3): 100-104.

(責任編輯: 楊力軍)

UUV Navigation and Positioning Method Based on Model-Aided and Ocean Current Estimation

YAN Zhe-ping, SONG Jin-xue

(College of Automation Harbin Engineering University, Harbin 150001, China)

For deep sea navigation, unmanned undersea vehicle(UUV) cannot track to the bottom because of the limitation of Doppler velocity log(DVL) measured distance. Therefore, a navigation and positioning method based on the UUV dynamic model is proposed in this paper. The estimation method of ocean current based on the extended Kalman filter is employed to calculate the ocean current information in UUV operating area, then the information is added into the UUV system model to compensate the errors induced by the model due to its unable perception of external environment, hence the accuracy of navigation and positioning is improved. Simulation proves the feasibility and effectiveness of the proposed method. This method is able to be applied to future UUV deep sea exploration.

unmanned undersea vehicle(UUV); dynamics model; extended Kalman filter(EKF); ocean current estimation; navigation and positioning

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

TP6; TP242.6; TN929.3;

A

2096-3920(2017)04-0319-07

10.11993/j.issn.2096-3920.2017.04.003

2017-05-05;

2017-06-07.

國家自然科學基金項目資助(51679057); 2016年黑龍江省杰出青年科學基金項目資助(JC2016007).

嚴浙平(1972-), 男, 博士, 教授, 研究方向為水下無人潛航器的總體設計、智能控制和運動控制.