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

摘要

一种集合卡尔曼滤波－粒子滤波相结合的AUV组合导航方法，包括以下步骤：1）数据采集：利用全球定位系统获取AUV在水面时的初始位置信息，利用多普勒计程仪、电子罗盘等导航传感器采集AUV的速度和姿态角等信息；2）滤波定位：利用基于集合卡尔曼滤波－粒子滤波相结合的滤波算法将传感器采集到的导航信息进行融合，估计得到AUV每一时刻的位置及姿态变化信息，实现对AUV的全局定位。本发明提供一种提高精度的集合卡尔曼滤波－粒子滤波相结合的AUV组合导航方法。

主权项

1.一种集合卡尔曼滤波－粒子滤波相结合的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时刻的状态向量，w_k表示状态过程噪声，f(·)表示X_k-1和X_k之间的非线性关系；计算背景集合的均值和协方差，并计算相应的卡尔曼增益：x^k(i),b=1nΣj=1nxk,j(i),b---(12)]]>P^xh(i),k=1n-1Σj=1n(xk,j(i),b-x^k(i),b)(h(xk,j(i),b)-h(x^k(i),b))T---(13)]]>P^hh(i)k=1n-1Σj=1n(h(xk,j(i),b)-h(x^k(i),b))(h(xk,j(i),b)-h(x^k(i),b))T---(14)]]>Kk(i)=P^xh(i),k(P^hh(i),k+vk)-1---(15)]]>其中为第i个粒子在k时刻背景集合的均值，为第i个粒子在k时刻背景集合第j个样本点的值，为第i个粒子在k时刻背景集合的x与h(·)的协方差矩阵，为第i个粒子在k时刻背景集合的h(·)与h(·)的协方差矩阵，为第i个粒子在k时刻的卡尔曼增益，上标T表示转置运算；3)利用式(7)更新分析集合，xk,ia=xk,ib+Kk(yk-h(xk,ib)),i=1,2,...,n---(7)]]>其中y_k，i是以y_k为均值，v_k为方差的高斯分布的采样，为第i个粒子在k时刻的分析集合，为第i个粒子在k时刻的背景集合；并利用式(8)和(9)计算分析集合的均值和协方差：x^ka=1nΣi=1nxk,ia---(8)]]>P^ka=1n-1Σi=1n(xk,ia-x^ka)(xk,ia-x^ka)T---(9)]]>其中为k时刻的分析集合的均值，为k时刻的分析集合第i个样本点的值，为k时刻的分析集合的协方差；4)利用分析集合构造建议分布函数，采样得到新的粒子，并更新权系数：x^k(i)~q(xk(i)|xk-1(i),zk)=N(x^k(i),a,P^k(i),a)---(16)]]>wk(i)=wk-1(i)p(zk|x^k(i))p(x^k(i)|xk-1(i))q(x^k(i)|x0:k-1(i),z1:k)---(17)]]>5)判断是否存在退化现象，如果不存在退化现象则进行下一次循环，如果存在退化现象则先进行重采样，再进行下一次循环，直到滤波结束；滤波结束后估计得到AUV每一时刻的位置及姿态变化信息，实现对AUV的全局定位。