[发明专利]一种基于最大互相关熵无迹粒子滤波的AUV协同导航方法

权利要求书

1.一种基于最大互相关熵无迹粒子滤波的AUV协同导航方法，其特征在于：包括以下几个步骤：

步骤一：建立描述AUV协同导航系统的状态方程和量测方程；

步骤二：初始化，设置所需要的参数；

步骤三：将AUV协同导航的系统量测方程和状态方程重构成非线性递归方程的形式进行处理，然后进行重要性采样，根据步骤二得到的初始粒子，应用MCUFK获得下一时刻的后验状态估计和对应的后验状态估计方差，基于此生成一个高斯分布，从此高斯分布中随机抽取N个新的粒子，根据新抽取的粒子计算每个粒子对应的权值并将其归一化；

步骤四：KLD重采样，对上一个步骤得到的N个粒子进行筛选，得到最终的粒子集；

步骤五：状态滤波更新，应用步骤四获得的粒子，完成状态的滤波更新，得到协同定位参数的估计状态和估计误差协方差矩阵完成协同导航的任务，其中代表第k时刻重采样后的第i个粒子，代表第k时刻重采样后第i个粒子相对应的权值，代表第k时刻的状态估计值。

2.根据权利要求1所述的一种基于最大互相关熵无迹粒子滤波的AUV协同导航方法，其特征在于：所述的AUV协同导航系统的状态方程和量测方程具体如下：采用的是主从式AUV协同定位模型，此处领航者是通信导航设备(Communication and Navigation Aid，CNA)，基于协同定位系统声学测距的状态空间模型具体为：

其中，式(1)为状态方程，式(2)为观测方程为，x_k和y_k分别为AUV第k时刻的东向和北向位置，Δt是采样时间，和分别为第k时刻由DVL提供的右舷速度和前向速度，是由罗盘测量出的航向角，x_k＝[x_k,y_k]^T表示AUV的位置向量，w_k＝[w_x,k,w_y,k]^T表示过程噪声向量，z_k是CAN和AUV之间的相对方位，并且它是使用水声调制解调器并通过到达时间的方法来测量的，是CAN在第k时刻的位置通过水声调制解调器阶段性地提供，δ_k代表量测噪声，基于式(1)和式(2)，协同定位系统广义的离散时间状态空间模型(状态方程和量测方程)能被建立如下：

这里状态转移矩阵为F＝I₂，其中I₂表示2维单位矩阵，控制输入表达式为

量测函数表达式具体为

3.根据权利要求1所述的一种基于最大互相关熵无迹粒子滤波的AUV协同导航方法，其特征在于：参数设置过程如下：通过理论分析与实际运行经验选择合适的MCUKF的核宽度σ并从已知的先验分布中随机抽取N个初始粒子，并设置它们的权值为1/N，具体为，从已知的先验分布中抽取N个粒子并满足

其中，是N个初始时刻粒子的均值；E是是对变量求均值；P₀是N个初始时刻粒子的协方差矩阵。