集合卡尔曼滤波－粒子滤波相结合的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之间的非线性关系；计算背景集合的均值和协方差，并计算相应的卡尔曼增益：${\hat{x}}_k^{\left(i\right),b}=\frac{1}{n}\underset{j=1}{\overset{n}{\Sigma }}{x}_{k,j}^{\left(i\right),b}---\left(12\right)$${\hat{P}}_{\mathrm{xh}}^{\left(i\right),k}=\frac{1}{n-1}\underset{j=1}{\overset{n}{\Sigma }}(x_{k,j}^{\left(i\right),b}-{\hat{x}}_k^{\left(i\right),b}){(h\left(x_{k,j}^{\left(i\right),b}\right)-h\left({\hat{x}}_k^{\left(i\right),b}\right))}^T---\left(13\right)$${\hat{P}}_{\mathrm{hh}}^{\left(i\right)k}=\frac{1}{n-1}\underset{j=1}{\overset{n}{\Sigma }}(h\left(x_{k,j}^{\left(i\right),b}\right)-h\left({\hat{x}}_k^{\left(i\right),b}\right)){(h\left(x_{k,j}^{\left(i\right),b}\right)-h\left({\hat{x}}_k^{\left(i\right),b}\right))}^T---\left(14\right)$$K_k^{\left(i\right)}={\hat{P}}_{\mathrm{xh}}^{\left(i\right),k}{({\hat{P}}_{\mathrm{hh}}^{\left(i\right),k}+v_k)}^{-1}---\left(15\right)$其中为第i个粒子在k时刻背景集合的均值，为第i个粒子在k时刻背景集合第j个样本点的值，为第i个粒子在k时刻背景集合的x与h(·)的协方差矩阵，为第i个粒子在k时刻背景集合的h(·)与h(·)的协方差矩阵，为第i个粒子在k时刻的卡尔曼增益，上标T表示转置运算；3)利用式(7)更新分析集合，$x_{k,i}^a=x_{k,i}^b+K_k(y_k-h\left(x_{k,i}^b\right)),i=\mathrm{1,2},...,n---\left(7\right)$其中y_k，i是以y_k为均值，v_k为方差的高斯分布的采样，为第i个粒子在k时刻的分析集合，为第i个粒子在k时刻的背景集合；并利用式(8)和(9)计算分析集合的均值和协方差：${\hat{x}}_k^a=\frac{1}{n}\underset{i=1}{\overset{n}{\Sigma }}{x}_{k,i}^a---\left(8\right)$${\hat{P}}_k^a=\frac{1}{n-1}\underset{i=1}{\overset{n}{\Sigma }}(x_{k,i}^a-{\hat{x}}_k^a){(x_{k,i}^a-{\hat{x}}_k^a)}^T---\left(9\right)$其中为k时刻的分析集合的均值，为k时刻的分析集合第i个样本点的值，为k时刻的分析集合的协方差；4)利用分析集合构造建议分布函数，采样得到新的粒子，并更新权系数：${\hat{x}}_k^{\left(i\right)}~q\left(x_k^{\left(i\right)}\right|x_{k-1}^{\left(i\right)},z_k)=N({\hat{x}}_k^{\left(i\right),a},{\hat{P}}_k^{\left(i\right)},a)---\left(16\right)$$w_k^{\left(i\right)}=w_{k-1}^{\left(i\right)}\frac{p\left(z_k\right|{\hat{x}}_k^{\left(i\right)})p\left({\hat{x}}_k^{\left(i\right)}\right|x_{k-1}^{\left(i\right)})}{q\left({\hat{x}}_k^{\left(i\right)}\right|x_{0:k-1}^{\left(i\right)},z_{1:k})}---\left(17\right)$5)判断是否存在退化现象，如果不存在退化现象则进行下一次循环，如果存在退化现象则先进行重采样，再进行下一次循环，直到滤波结束；滤波结束后估计得到AUV每一时刻的位置及姿态变化信息，实现对AUV的全局定位。