[发明专利]一种基于粒子滤波算法的水声导航定位方法

权利要求书

1.一种基于粒子滤波算法的水声导航定位方法，其特征在于，该方法包括：

步骤1，根据信标的第二位置坐标和航行器与所述信标之间的脉冲定位信号，构建声学导航模型，其中，所述脉冲定位信号由所述信标向所述航行器发送，

所述步骤1具体包括：

步骤11，根据所述航行器的航行速度以及所述脉冲定位信号传播的等效声速改变量，构建第一矩阵方程，所述第一矩阵方程的计算公式为：

X_k＝X_k-1+V_k-1，

X_k＝[x_k y_k c_1k … c_sk … c_mk]^T，

式中，m为所述信标的数量，s＝1…m，c_sk为当前时刻k第s个信标与所述航行器之间的等效声速，(x_k,y_k)为所述航行器的第一位置坐标，(v_xk,v_yk)为所述航行器的航行速度，n_k^cs为当前时刻k所述航行器与第s个信标之间所述脉冲定位信号传播的所述等效声速改变量；

步骤12，根据所述第二位置坐标、所述脉冲定位信号的传播时间以及所述传播时间的误差参数，构建第二矩阵方程，所述第二矩阵方程的计算公式为：

Z_k＝H(X_k)+N_k，

Z_k＝[t_1k … t_sk … t_mk]^T，

N_k＝[n_1k … n_sk … n_mk]^T，

式中，(x_os,y_os)为第s个信标的所述第二位置坐标，t_sk为当前时刻k第s个信标的所述脉冲定位信号到达所述航行器的所述传播时间，n_sk为所述误差参数，所述误差参数n_sk满足零均值高斯分布其中，为所述零均值高斯分布中的方差参数；

步骤13，将所述第一矩阵方程和所述第二矩阵方程，记作所述声学导航模型；

步骤2，采用加权算法，根据所述脉冲定位信号和所述声学导航模型，计算所述航行器的所述第一位置坐标和等效声速。

2.如权利要求1所述的基于粒子滤波算法的水声导航定位方法，其特征在于，所述步骤2中，具体包括：根据所述航行器接收到的所述脉冲定位信号和所述声学导航模型，计算所述加权算法中的权值，所述权值的计算公式为：

式中，为所述权值，为所述零均值高斯分布中的方差参数。

3.如权利要求1所述的基于粒子滤波算法的水声导航定位方法，其特征在于，还包括：

步骤3，根据当前时刻的所述脉冲定位信号和预设修订公式，修订下一时刻接收到的所述脉冲定位信号，其中，所述修订公式为：

式中，A为第一运动系数矩阵，U_k+1|k为第二运动系数矩阵，X_k|kⁱ为当前时刻的所述脉冲定位信号，X_k+1|kⁱ为下一时刻接收到的所述脉冲定位信号。

4.如权利要求1所述的基于粒子滤波算法的水声导航定位方法，其特征在于，还包括：

步骤4，根据所述第一位置坐标和预设坐标，计算所述航行器的位置偏差；

步骤5，当判定所述位置偏差大于或等于第一预设阈值时，生成最近导航信息。

5.如权利要求4所述的基于粒子滤波算法的水声导航定位方法，其特征在于，还包括：

步骤6，当判定所述位置偏差大于或等于第二预设阈值时，生成报警信息，其中，所述第二预设阈值大于所述第一预设阈值。