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

说明书

本申请公开了一种基于粒子滤波算法的水声导航定位方法，该方法包括：步骤1，根据信标的第二位置坐标和航行器与信标之间的脉冲定位信号，构建声学导航模型，其中，脉冲定位信号由信标向航行器发送，步骤1具体包括：步骤11，根据航行器的航行速度以及脉冲定位信号传播的等效声速改变量，构建第一矩阵方程；步骤12，根据第二位置坐标、脉冲定位信号的传播时间以及传播时间的误差参数，构建第二矩阵方程；步骤13，将第一矩阵方程和第二矩阵方程，记作声学导航模型；步骤2，采用加权算法，根据脉冲定位信号和声学导航模型，计算航行器的第一位置坐标和等效声速。通过本申请中的技术方案，提高水下等效声速估计值的准确性，降低了水下航行器定位失效的可能性。

技术领域

本申请涉及水下导航的技术领域，具体而言，涉及一种基于粒子滤波算法的水声导航定位方法。

背景技术

水下航行器是一种可以凭借自身动力系统和导航系统完成水下工作任务的集成系统，声学导航利用声音信号在水下传播衰减较小、传播距离较远的特点，将水面或水下已知位置的信标作为参考位置，通过测量信标发射的脉冲定位信号到达水下航行器的时间，计算信标到水下航行器的距离，从而得到水下航行器的位置信息。声学导航主要包括超短基线导航、短基线导航和长基线导航等方式。

而现有技术中，常规的长基线水声导航定位利用信标发送的脉冲定位信号到水下航行器的传播时间作为观测量信息，将估计或测量得到的等效声速作为已知的参数，将二者相乘计算出各个信标与水下航行器的距离，在已知信标位置的前提下，利用距离的几何关系，计算出水下航行器的位置信息。但无论是工程实测还是理论计算，由于水声信道的复杂性，将得到的等效声速作为已知参量存在误差，而等效声速的误差，将导致水下航行器的定位误差随着导航距离的增加而变大，进而导致水下航行器定位失效。

发明内容

本申请的目的在于：提高水下等效声速估计值的准确性，降低了水下航行器定位失效的可能性。

本申请的技术方案是：提供了一种基于粒子滤波算法的水声导航定位方法，该方法包括：步骤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为信标的数量，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，