一种基于无迹卡尔曼滤波的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-), 男, 教授, 博导，主要研究方向水下导航与控制.

(责任编辑: 杨力军)