[发明专利]集合卡尔曼滤波－粒子滤波相结合的AUV组合导航方法

说明书

技术领域

本发明涉及海洋工程领域，尤其是一种自主水下航行器（Autonomous Underwater Vehicle，AUV）组合导航方法。

背景技术

自主水下航行器是目前海洋工程领域技术发展的热点，在水下环境监测、近海石油工程作业、水下搜索与测绘、以及水雷对抗和实时战区警戒等军事领域获得越来越广泛的应用。导航技术是实现AUV实现自主航行的关键，由于AUV存在工作时间长、环境复杂、信息源少、隐蔽性要求高等工作特点，这给稳定、精确的AUV自主导航带来很大的困难和挑战。

近年来国际上对于AUV导航技术的研究一直非常活跃。水下导航技术通常需要在精度、运动范围和自动化程度等之间进行平衡，概括起来主要有声学定位、地球物理导航及航位推算与惯性导航等多种类型的导航技术。

现有的AUV通常利用惯性导航系统（Inertial Navigation System，INS）采集的姿态角及加速度信息，三维电子罗盘（TCM）采集的姿态角信息以及多普勒计程仪（Doppler Velocity Log，DVL）采集的速度信息进行组合导航，常用的数据融合方法包括卡尔曼滤波（Kalman Filter，KF）扩展卡尔曼滤波（Extended Kalman Filter，EKF）、无味卡尔曼滤波（Unscented Kalman Filter，UKF）、粒子滤波（Particle Filter，PF）和集合卡尔曼滤波（Ensemble Kalman Filter，EnKF）等，其中扩展卡尔曼滤波和无味卡尔曼滤波的共同问题是在非线性/非高斯性较强时收敛性急剧下降甚至发散。而粒子滤波利用序贯蒙特卡洛方法的，对于非线性/非高斯问题具有较好的性能，但是在粒子滤波中如果最新的观测信息位于先验概率分布的尾部或者似然函数相比先验概率是峰化的，则会导致粒子选择的盲目性，降低了估计精度。

发明内容

本发明的目的是提供一种基于集合卡尔曼滤波－粒子滤波相结合的自主水下航行器组合导航方法，以提高AUV导航算法的精度。

本发明解决其技术问题所采用的技术方案是：

一种集合卡尔曼滤波－粒子滤波相结合的AUV组合导航方法，所述组合导航方法包括以下步骤：

1）数据采集：获取AUV在水面时的初始位置信息，采集AUV的速度和姿态角信息；

2）滤波定位：以步骤1）的初始位置信息、速度和姿态角信息定义状态向量、观测向量、状态模型方程和观测模型方程，滤波过程如下：

2.1)根据初始条件生成初始粒子和它相应的权系数；

2.2)定义每个粒子背景集合根据式(1)对背景集合进行预测，

状态模型方程：X_k=f(X_k-1，w_k)                 (1)

其中X_k-1，X_k分别表示k-1和k时4的状态向量，w_k表示状态过程噪声，f(·)表示X_k-1和X_k之间的非线性关系；

计算背景集合的均值和协方差，并计算相应的卡尔曼增益：

x^k(i),b=1nΣj=1nxk,j(i),b---(12)]]>