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

?

一種基于無(wú)跡卡爾曼濾波的UUV協(xié)同定位方法

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

(西北工業(yè)大學(xué) 航海學(xué)院, 陜西 西安, 710072)

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

無(wú)人水下航行器; 協(xié)同定位; 無(wú)跡卡爾曼濾波; 遞推導(dǎo)航濾波算法; 航位推算方法

0 引言

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

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

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

針對(duì)基于單移動(dòng)領(lǐng)航者僅靠距離量測(cè)的UUV協(xié)同定位問(wèn)題, 本論文在文獻(xiàn)[9-11]的基礎(chǔ)上給出一種基于UKF的主從式UUV協(xié)同定位方法。利用UKF設(shè)計(jì)導(dǎo)航濾波算法, 并與常規(guī)的航位推算方法進(jìn)行對(duì)比仿真，旨在獲得從UUV更高的定位精度。

1 協(xié)同定位模型

基于UKF的主從式UUV協(xié)同定位方法原理如圖1, 其中主UUV為單領(lǐng)航者, 裝備高精度的導(dǎo)航設(shè)備; 從UUV為跟隨者, 裝備低精度的航位推算系統(tǒng)。主、從UUV間利用水聲Modem進(jìn)行測(cè)距和通信。

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

式(1)可簡(jiǎn)寫(xiě)為

從UUV的量測(cè)量為-1,時(shí)刻主、從UUV間的距離r-1,r, 同時(shí)利用水聲通信獲取該時(shí)刻主UUV的坐標(biāo)位置, 并且主、從UUV的位置坐標(biāo)滿(mǎn)足如下關(guān)系

式(3)便是從UUV協(xié)同定位的量測(cè)方程, 簡(jiǎn)寫(xiě)為

2 導(dǎo)航算法

考慮到系統(tǒng)狀態(tài)方程和量測(cè)方程的非線(xiàn)性, 采用UKF進(jìn)行導(dǎo)航濾波算法設(shè)計(jì)。

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

針對(duì)以上非線(xiàn)性模型

協(xié)同定位算法具體如下。

1) 初始化增廣狀態(tài)向量及估計(jì)誤差方差

2) 計(jì)算Sigma點(diǎn)和相應(yīng)的加權(quán)因子

3) 時(shí)間更新

4) 量測(cè)更新

3 仿真與分析

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

圖2 主從式UUV協(xié)同定位軌跡

圖3 從UUV協(xié)同定位誤差曲線(xiàn)

4 結(jié)束語(yǔ)

對(duì)基于單移動(dòng)領(lǐng)航者主從式UUV協(xié)同定位方法進(jìn)行了研究, 利用從UUV的運(yùn)動(dòng)學(xué)方程和量測(cè)方程建立了單個(gè)從UUV協(xié)同定位的數(shù)學(xué)模型, 設(shè)計(jì)了基于UKF的導(dǎo)航濾波算法, 并與常規(guī)的航位推算方法進(jìn)行仿真對(duì)比。仿真驗(yàn)證了算法的有效性, 相對(duì)航位推算方法, 基于UKF的導(dǎo)航濾波算法能有效抑制定位誤差的增長(zhǎng), 明顯提高了從UUV的導(dǎo)航定位精度。

[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] 張立川, 劉明雍, 徐德民. 基于水聲傳播延遲的主從式多無(wú)人水下航行器協(xié)同導(dǎo)航定位研究[J].兵工學(xué)報(bào), 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.

國(guó)家自然基金(50979093), 新世紀(jì)優(yōu)秀人才計(jì)劃資助(NCET-06-0877).

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

(責(zé)任編輯: 楊力軍)