[发明专利]集合卡尔曼滤波－粒子滤波相结合的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的全局定位。