一種基于無跡卡爾曼濾波的UUV協同定位方法

劉明雍, 沈俊元，張加全, 胡俊偉

?

一種基于無跡卡爾曼濾波的UUV協同定位方法

劉明雍, 沈俊元，張加全, 胡俊偉

(西北工業大學 航海學院, 陜西 西安, 710072)

多水下無人航行器(multi-UUV)協同定位技術是解決海洋中間層水下導航定位問題的重要途徑, 針對以往multi-UUV僅靠距離量測的協同導航定位精度低的問題, 給出了一種基于無跡卡爾曼濾波 (UKF) 方法的UUV協同定位方法。利用從UUV的運動學方程和基于距離的量測方程建立了從UUV的導航模型, 針對該非線性導航模型,采用UKF設計了導航濾波算法, 避免了對非線性方程的線性化處理, 實現了遞推導航濾波算法, 并與傳統的航位推算方法進行了仿真對比。仿真結果表明, 從UUV能夠利用該導航濾波算法進行實時定位, 比傳統的航位推算方法具有更高的定位精度。

無人水下航行器; 協同定位; 無跡卡爾曼濾波; 遞推導航濾波算法; 航位推算方法

0 引言

隨著海洋大范圍作業任務需求的增加, 比如大范圍的海底地圖繪制、海底資源探測及協同作戰等, 無人水下航行器(unmanned underwater vehicle, UUV)協同導航定位方法凸顯出水下導航定位的優勢, 該方法不僅可以降低UUV制造成本, 而且執行任務時避免了所有UUV上浮到海面進行全球定位系統(global positioning system, GPS)位置校準的過程, 增加了UUV導航的隱蔽性, 減少了能量消耗, 從而使得多UUV的協同導航定位方法成為一個研究熱點。

早期的研究主要針對固定單信標水聲導航方法[1]。主要研究有Willumsen等人提出的基于距離、方位和多普勒頻移測速等3種信息的輔助導航方法[2]; Pan-Mook, Larsen等人利用單信標測距, 結合慣性導航系統(inertial navigation system, INS)和多普勒測速儀(doppler velocity log, DVL)，簡稱INS/DVL, 采用無跡卡爾曼濾波(unscented Kalman filter, UKF)方法, 通過Monte Carlo仿真及試驗, 證明該方法可提高導航精度[3-4]; Baccou等人提出結合UUV自身的機動來解決單信標測距求解條件不充分的問題[5-6]; Stilwell小組的一系列工作, 從可觀測性分析的角度, 針對已知洋流、未知定常洋流對固定信標測距導航精度的影響, 做了卓有成效的研究[7]。

為更簡化系統, 提高UUV導航定位的靈活性, 研究人員嘗試采用單移動領航者。該方法尚處于初級研究階段, 有關研究人員做了大量工作, 如麻省理工學院的Alexander和John等人提出的一種基于K-L 散度距離的UUV協同導航定位方法[8], 給出了通信受限條件下協同導航濾波算法, 并做了大量實物試驗; 德國的Robert等人引入航位推算信息, 給出一種基于三邊測量技術的UKF協同導航方法[9], 并進行了仿真驗證; 文獻[10-11]對基于水聲傳播延遲的主從式多UUV協同導航定位方法進行了研究; Singh等人提出了單向法時間同步(one-way travel time, OWTT)測距技術, 解決了單距導航(single range navi- gation，SNR)中的測距與通信問題, 克服了傳統雙向法時間同步(two way time transfer, TWTT)方式中通信率與UUV數量成反比的瓶頸, 為協同導航定位提供了更可靠的技術支持[12]。

針對基于單移動領航者僅靠距離量測的UUV協同定位問題, 本論文在文獻[9-11]的基礎上給出一種基于UKF的主從式UUV協同定位方法。利用UKF設計導航濾波算法, 并與常規的航位推算方法進行對比仿真，旨在獲得從UUV更高的定位精度。

1 協同定位模型

基于UKF的主從式UUV協同定位方法原理如圖1, 其中主UUV為單領航者, 裝備高精度的導航設備; 從UUV為跟隨者, 裝備低精度的航位推算系統。主、從UUV間利用水聲Modem進行測距和通信。

圖1 基于UKF的主從式UUV協同定位示意圖

式(1)可簡寫為

從UUV的量測量為-1,時刻主、從UUV間的距離r-1,r, 同時利用水聲通信獲取該時刻主UUV的坐標位置, 并且主、從UUV的位置坐標滿足如下關系

式(3)便是從UUV協同定位的量測方程, 簡寫為

2 導航算法

考慮到系統狀態方程和量測方程的非線性, 采用UKF進行導航濾波算法設計。

UKF 方法是采用一組確定的采樣點來模擬狀態參量的分布特征,因此又稱為Sigma點卡爾曼濾波(sigma point Kalman filter, SPKF)。UKF與傳統的擴展卡爾曼濾波器(extended Kalman filter, EKF)算法相比, 不需要對非線性系統進行線性化處理, 避免計算Jacobian矩陣, 并對任何非線性系統都可以精確到泰勒級數展開的2階精度[13]。

針對以上非線性模型

協同定位算法具體如下。

1) 初始化增廣狀態向量及估計誤差方差

2) 計算Sigma點和相應的加權因子

3) 時間更新

4) 量測更新

3 仿真與分析

對UKF導航算法和航位推算算法進行了仿真, 繪制了UUV航行軌跡圖, 對誤差進行了比較。由圖2、圖3可以得到，從UUV利用只依賴內部傳感器進行航位推算導航時, 由于偏航角的漂移、速度誤差等影響使得定位曲線出現了明顯的偏離, 定位誤差不斷增大。相對航位推算方法, 該UKF協同定位濾波算法能有效抑制誤差的增長, 定位精度明顯提高。

圖2 主從式UUV協同定位軌跡

圖3 從UUV協同定位誤差曲線

4 結束語

對基于單移動領航者主從式UUV協同定位方法進行了研究, 利用從UUV的運動學方程和量測方程建立了單個從UUV協同定位的數學模型, 設計了基于UKF的導航濾波算法, 并與常規的航位推算方法進行仿真對比。仿真驗證了算法的有效性, 相對航位推算方法, 基于UKF的導航濾波算法能有效抑制定位誤差的增長, 明顯提高了從UUV的導航定位精度。

[1] Vaganay J, Leonard J J, Curcio J A, et al. Experimental Validation of the Moving Long Base-Line Navigation Concept[C]//Autonomous Underwater Vehicles, 2004 IEEE/ OES: 59-65.

[2] Willumsen, Hallingstad A B, Jalving O, et al. Integration of Range, Bearing and Doppler Measurements from Trans- ponders into Underwater Vehicle Navigation Systems[C]// in Oceans 2006, 2006: 1-6.

[3] Lee P, Jun B, Kim K, et al. Simulation of an Inertial Acoustic Navigation System with Range Aiding for an Autonomous Underwater Vehicle[J]. IEEE Journal of Oceanic Engineering, 2007, 32(2): 327-345.

[4] Larsen M B. High Performance Doppler-inertial Navigation Experimental Results[C]//In IEEE Oceans, RI, USA, 2000: 2043-2050.

[5] Baccou P, Jouvencel B. Homing and Navigation Using one Transponder for UUV, Post-processing Comparisons Results with Long Base-line Navigation[C]//Proceedings IEEE International Conference on Robotics and Automation, 2002: 4004-4009.

[6] Baccou P, Jouvencel B. Simulation Results, Post-processing Experimentations and Comparisons Results for Navigation, Homing and Multiple Vehicles Operations with a New Positioning Method Using on Transponder[C]//Intelligent Robots and Systems, 2003 IEEE/RSJ International Conference, 2003: 811- 817.

[7] Gader A, Stilwell D. Toward Underwater Navigation Based on Range Measurements from a Single Localization[C]// Proceedings of IEEE International Conference on Robotics and Automation, New Orleans, 2004: 1-6.

[8] Alexander B, John J L. Cooperative Localization for Autonomous Underwater Vehicles[C]//Springer Tracts in Advanced Robotics, 2008: 387-395.

[9] Engel,Kalwa R. Relative Positioning of Multiple Underwater Vehicles in the GREX project[C]//IEEE：Oceans 2009 Europe. Bremen, 2009:1-7.

[10] Zhang Li-chuan, Liu Ming-yong, Xu De-min, et al. Coope- rative Localization for Underwater Vehicles[C]//ICIEA IEEE, 2009: 2524-2527.

[11] 張立川, 劉明雍, 徐德民. 基于水聲傳播延遲的主從式多無人水下航行器協同導航定位研究[J].兵工學報, 2009, 30(12): 1674-1678.

Zhang Li-chuan, Liu Ming-yong , Xu De-min. Cooperative Localization for Multi-UUVs Based on Time-of-flight of Acoustic Signal[J]. Acta ArmamentarII, 2009, 30(12): 1674- 1678.

[12] Singh S,Grund M,Bingham B, et al. Underwater Acoustic Navigation with the WHOI Micro-Modem[C]//Oceans 2006, Boston, 2006.

[13] Julier J, Uhlmann K. Unscented and Nonlinear Estimation [J]. Proceeding of the IEEE, 2004, 92(3):401-422.

A Cooperative Localization Method of UUV Based on Unscented Kalman Filter

LIU Ming-yong, SHEN Jun-yuan, ZHANG Jia-quan, HU Jun-wei

(College of Marine Engineering, Northwestern Ploytechnical University, Xi′an 710072, China)

The multi-unmanned underwater vehicles (multi-UUV) cooperative localization technology is important for solving the UUV localization problem in middle depth zone of the sea. To increase lower precision of multi-UUV cooperative localization using only range measurement, a cooperative localization method of UUV based on unscented Kalman filter (UKF) is presented in this paper. A follower UUV navigation model is derived from kinematic equation and measurement equation based on range of follower UUV. For the nonlinear navigation model, we designed navigation filtering algorithm using the UKF to avoid linearization of the nonlinear equations, and realized the recursive navigation filtering algorithm. Simulation results show that follower UUV can use the navigation filtering algorithm for real-time positioning. Compared with the traditional dead reckoning method, the new method has higher localization accuracy.

unmanned underwater vehicle(UUV); cooperative localization; unscented Kalman filter(UKF); recursive navigation filtering algorithm; dead reckoning method

TJ630.33; TP242.3

A

1673-1948(2011)03-0205-04

2010-07-13;

2010-08-30.

國家自然基金(50979093), 新世紀優秀人才計劃資助(NCET-06-0877).

劉明雍(1971-), 男, 教授, 博導，主要研究方向水下導航與控制.

(責任編輯: 楊力軍)