CN109540154B - 一种基于粒子滤波算法的水声导航定位方法 - Google Patents

Abstract

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

Description

一种基于粒子滤波算法的水声导航定位方法

技术领域

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

背景技术

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

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

发明内容

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

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

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中，具体包括：根据航行器接收到的脉冲定位信号和声学导航模型，计算加权算法中的权值，权值的计算公式为：

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

上述任一项技术方案中，进一步地，还包括：步骤3，根据当前时刻的脉冲定位信号和预设修订公式，修订下一时刻接收到的脉冲定位信号，其中，修订公式为：

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

上述任一项技术方案中，进一步地，还包括：步骤4，根据第一位置坐标和预设坐标，计算航行器的位置偏差；步骤5，当判定位置偏差大于或等于第一预设阈值时，生成最近导航信息。

上述任一项技术方案中，进一步地，还包括：步骤6，当判定位置偏差大于或等于第二预设阈值时，生成报警信息，其中，第二预设阈值大于第一预设阈值。

上述任一项技术方案中，进一步地，还包括：步骤7，根据航行器接收到的多条传播途径的脉冲定位信号，构建信号时延模型；步骤8，根据信号时延模型和脉冲定位信号，计算航行器接收到脉冲定位信号的接收时间序列；步骤9，根据等效声速、脉冲定位信号、接收时间序列和坐标计算模型，计算航行器的第三位置坐标；步骤10，根据第一位置坐标和第三位置坐标，采用加权算法，计算并发送航行器的第四位置坐标。

上述任一项技术方案中，进一步地，基于粒子滤波算法的水声导航定位方法被应用于航行器的水下导航，航行器根据第一位置坐标和预设航线，调整航行器的航行方向和航行速度，或者，航行器根据第四位置坐标和预设航线，调整航行器的航行方向和航行速度。

本申请的有益效果是：通过将等效声速、等效声速的改变量作为未知量，构建声学导航模型，并采用加权算法，根据脉冲定位信号计算航行器的位置坐标和等效声速，提高了水下等效声速估计值的准确性，避免了声音信号的衰减显现对水声定位的影响，提高了航行器定位的准确性，降低了水下航行器定位失效的可能性，有利于实现远距离精确水声定位导航。

附图说明

本申请的上述和/或附加方面的优点在结合下面附图对实施例的描述中将变得明显和容易理解，其中：

图1是根据本申请的一个实施例的基于粒子滤波算法的水声导航定位方法的示意流程图；

图2是根据本申请的一个实施例的航行器和信标位置示意图；

图3是根据本申请的一个实施例的等效声速真值示意图；

图4是根据本申请的一个实施例的航行器真实速度和真实航行角度示意图；

图5是根据本申请的一个实施例的导航轨迹仿真示意图；

图6是根据本申请的一个实施例的导航误差仿真示意图。

具体实施方式

为了能够更清楚地理解本申请的上述目的、特征和优点，下面结合附图和具体实施方式对本申请进行进一步的详细描述。需要说明的是，在不冲突的情况下，本申请的实施例及实施例中的特征可以相互结合。

在下面的描述中，阐述了很多具体细节以便于充分理解本申请，但是，本申请还可以采用其他不同于在此描述的其他方式来实施，因此，本申请的保护范围并不受下面公开的具体实施例的限制。

实施例一：

以下结合图1至图6对本申请的实施例一进行说明。

如图1所示，本实施例提供了一种基于粒子滤波算法的水声导航定位方法，包括：步骤1，根据信标的第二位置坐标和航行器与信标之间的脉冲定位信号，构建声学导航模型，其中，脉冲定位信号由信标向航行器发送；

具体地，如图2所示，设定Y轴方向为0°，三个信标的坐标分别为 B1(0,0)，B2(r,0)和航行器的初始位置为A(3r/4,r/5)，航行器沿航迹线201做匀速直线运动，速度为2.5m/s，航向为顺时针75°。在实际应用中，航行器即使做匀速直线运动也会受到水流影响，在x和y 方向都存在扰动，假设其速度扰动满足高斯分布，方差均为0.005m/s。k时刻与k+1时刻时间间隔为120s，时间取样次数为100次。信标信号传播时间的误差参数服从零均值高斯分布方差为0.001m/s。由于航行器位置逐渐变化，等效声速缓慢变化，设定其变化量服从高斯分布，方差为0.01m/s。假设航行器初始位置未知，设定为距离航行器真实位置较远的坐标(r,2.5r)，位置方差均取20000m，设置等效声速初始估计值分别为c₁＝1502m/s,c₂＝1521m/s,c₃＝1496m/s，等效声速方差取40m/s，在航行器位置完全未知以及等效声速误差较大的情况下进行水声导航定位。

该步骤1中，具体包括：

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

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个信标之间脉冲定位信号传播的等效声速改变量；

具体地，基于声波在水中的传播特性，航行器在当前时刻k会分别接收到三个信标的多个脉冲定位信号，在本实施例中，设定在当前时刻k，航行器接收到N个粒子，每个粒子均包含相同维度的状态参量，根据第一矩阵方程的计算公式可知，第i个粒子为其中，(x_k ⁱ,y_k ⁱ)为航行器的第一位置坐标，c_1k ⁱ、c_2k ⁱ和c_3k ⁱ为三个信标与航行器之间的等效声速。

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

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，采用加权算法，根据脉冲定位信号和声学导航模型，计算航行器的第一位置坐标和等效声速。

进一步地，根据航行器接收到的脉冲定位信号和声学导航模型，计算加权算法中的权值，权值的计算公式为：

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

具体地，航行器在当前时刻k接收到三个信标发送的脉冲定位信号之后，根据接收到的脉冲定位信号获取三个信标的第二位置坐标(x_o1,y_o1)、 (x_o2,y_o2)和(x_o3,y_o3)以及对应的传播时间t_1k、t_2k和t_3k，根据构建的声学导航模型，计算对应的权重值

优选地，对权重值进行归一化运算，对应的归一化计算公式为：

进而利用归一化后的权重值对航行器接收到的N个粒子进行加权运算，对应的计算公式为：

X_k＝[x_k y_k c_1k c_2k c_3k]，

提取矩阵X_k中的前两个元素，记作当前时刻k的航行器的第一位置坐标(x_k,y_k)，提取矩阵X_k中的后三个元素，分别记作当前时刻k的航行器与三个信标之间的等效声速。

进一步地，该方法中还包括：步骤3，根据当前时刻的脉冲定位信号和预设修订公式，修订下一时刻接收到的脉冲定位信号，其中，修订公式为：

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

进一步地，该基于粒子滤波算法的水声导航定位方法还包括：步骤4，根据第一位置坐标和预设坐标，计算航行器的位置偏差；步骤5，当判定位置偏差大于或等于第一预设阈值时，生成最近导航信息。

具体地，得出第一位置坐标之后，与当前时刻对应的预设坐标相比较，计算出航行器对应的位置偏差，当判定位置偏差大于或等于第一预设阈值时，表明航行器偏离航线较远，计算航行器与预设航线之间距离最近的航线，生成最近导航信息，使得航行器沿最近导航信息返回预设航线。

进一步地，该基于粒子滤波算法的水声导航定位方法还包括：步骤6，当判定位置偏差大于或等于第二预设阈值时，生成报警信息，其中，第二预设阈值大于第一预设阈值。

具体地，当判定航行器的位置偏差大于或等于第二预设阈值时，表明航行器严重偏离预设航线，生成报警信息，进入手动航行状态，由操作人员根据航行器所处的当前状态，对航行器进行手动导航。

在本实施例中，将圆相交几何方法与本申请中的基于粒子滤波算法的水声导航定位方法进行比较，设定三个信标与航行器之间的等效声速真值如图3所示，三个信标发射的脉冲定位信号到达航行器的等效声速真值依次为曲线301A、曲线302A和曲线303A，通过本申请的基于粒子滤波算法的水声导航定位方法得到的等效声速依次为曲线301B、曲线302B和曲线303B，通过对比可知，三组曲线基本相同。

航行器的真实速度与真实航行角度如图4(A)和图4(B)所示，圆相交几何方法与本申请的基于粒子滤波算法的水声导航定位方法的导航轨迹仿真如图5所示，其中，图5(A)为设定的航行器真实轨迹，图 5(B)为圆相交几何方法的导航轨迹，图5(C)为本申请基于粒子滤波算法的水声导航定位方法的导航轨迹。圆相交几何方法与本申请的基于粒子滤波算法的水声导航定位方法的导航误差仿真如图6所示，分别为仿真曲线601和仿真曲线602。由图5和图6可知，本申请中的基于粒子滤波算法的水声导航定位方法的导航轨迹与航行器的真实轨迹更加吻合、误差较小，且圆相交几何方法在采样时间的末尾时刻，误差大于初始值产生的误差，定位误差由200m增加至400m，而本申请中的基于粒子滤波算法的水声导航定位方法将定位误差控制在100m范围内，远小于初始定位误差，对水下航行器的定位更加准确。

可以看出，本申请中的基于粒子滤波算法的水声导航定位方法，在水下航行器位置完全未知的状况下，不仅减小了圆相交几何方法因等效声速误差产生的定位误差，且随着时间推移精度逐渐提高，更准确地估计其运动状态，显著提高了定位的精度，具备较高应用价值。

进一步地，该基于粒子滤波算法的水声导航定位方法被应用于航行器的水下导航，航行器根据第一位置坐标和预设航线，调整航行器的航行方向和航行速度。

实施例二：

在通过实施例一计算出航行器的第一位置坐标和等效声速之后，还可以构建信号时延模型，利用等效声速，计算航行器的第三位置坐标，进而采用加权算法，根据第一位置坐标和第三位置坐标，计算出航行器的第四位置坐标，具体包括：

步骤7，根据航行器接收到的多条传播途径的脉冲定位信号，构建信号时延模型，其中，脉冲定位信号由信标向航行器发送，传播途径数量由当前水域环境决定，信号时延模型的计算公式为：

式中，y^*(n)为航行器接收到的信标发射的脉冲定位信号，s(n)为采样后的脉冲定位信号，a^* _m为第m条传播途径的脉冲定位信号采样后的信号幅值，τ^* _m为第m条传播途径的脉冲定位信号采样后的信号时延，M为脉冲定位信号由信标向航行器发送对应的传播途径数量，L为M条传播途径对应的脉冲定位信号的观测时间长度；

步骤8，根据信号时延模型和脉冲定位信号，计算航行器接收到脉冲定位信号的接收时间序列；

该步骤8中，具体包括：

步骤81，根据矩阵化公式，将信号时延模型进行矩阵化，矩阵化后的信号时延模型计算公式为：

Y＝S_L×β×W，

Y＝[y^*(0) y^*(1) … y^*(L-1)]^T，

W＝[w₁ w₂ … w_β]^T，

式中，W为待求解量，对应待估计信道向量，待估计信道向量W中各元素取值分别为脉冲定位信号各传播途径的衰减系数，上角标T为转置运算符，为信号时延τ^* _m中的最大值，β为脉冲定位信号的信道长度，对应的计算公式为：

步骤82，根据矩阵化后的信号时延模型和脉冲定位信号，计算任一个传播途径对应的传输时延；

步骤83，根据任一个传播途径的传输时延，生成接收时间序列。

具体地，将矩阵化后的信号时延模型进行奇异值分解，分别设定两个正交矩阵U_L×L、和一个对角阵Λ，其中，H为酉矩阵，对应的矩阵公式为：

式中，σ₁≥σ₂≥…≥σ_r，计算矩阵的有效秩，令

则有效秩为第一个满足ψ(α)＞θ的α值，通常系数θ的取值接近于1，本申请中，系数θ＝0.95。

将脉冲定位信号带入矩阵化后的信号时延模型计算公式进行求解，其最小二乘解的计算公式为：

式中，u_f和v_f分别为正交矩阵U_L×L和V_β×β的第f个列向量，W_LS-SVD为对应信道(传播途径)的最小二乘解。

本申请的申请人根据大量的数学运算以及水声信道的稀疏特性，推导得出，从矩阵化后的信号时延模型的最小二乘解中，选取模值最大的M 个元素对应的接收时刻，作为脉冲定位信号各传播途径对应的传输时延，即当前稀疏信道估计值，按照传输时延的顺序，生成接收时间序列。

综上所述，在本实施例中，航行器完成构建信号时延模型之后，以信标为单位，根据接收到脉冲定位信号的传播途径的顺序，利用信号时延模型，计算信道长度范围内的最小二乘解，再选取计算出的最小二乘解中模值最大的M个元素对应的接收时刻，作为当前传播途径对应的传输时延。根据传播途径的顺序，生成接收时间序列。

步骤9，根据等效声速、脉冲定位信号、接收时间序列和坐标计算模型，计算航行器的第三位置坐标，其中，坐标计算模型的计算公式为：

式中，(x，y)为航行器的定位坐标，(x_i，y_i)为第i个信标的位置坐标，N为信标的数量，为第i个信标发射脉冲定位信号的发射时间，为航行器通过第k个传输途径接收到第i个信标的脉冲定位信号的接收时间，传输途径的个数为M，C_i为等效声速，r_i为航行器与第i个信标之间的距离。

步骤10，根据第一位置坐标和第三位置坐标，采用加权算法，计算并发送航行器的第四位置坐标。

进一步地，该基于粒子滤波算法的水声导航定位方法被应用于航行器的水下导航，航行器根据第四位置坐标和预设航线，调整航行器的航行方向和航行速度。

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

本申请中的步骤可根据实际需求进行顺序调整、合并和删减。

本申请装置中的单元可根据实际需求进行合并、划分和删减。

尽管参考附图详地公开了本申请，但应理解的是，这些描述仅仅是示例性的，并非用来限制本申请的应用。本申请的保护范围由附加权利要求限定，并可包括在不脱离本申请保护范围和精神的情况下针对发明所作的各种变型、改型及等效方案。

Claims (7)

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，当判定所述位置偏差大于或等于第二预设阈值时，生成报警信息，其中，所述第二预设阈值大于所述第一预设阈值。

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

步骤7，根据航行器接收到的多条传播途径的所述脉冲定位信号，构建信号时延模型；

步骤8，根据所述信号时延模型和所述脉冲定位信号，计算所述航行器接收到所述脉冲定位信号的接收时间序列；

步骤9，根据所述等效声速、所述脉冲定位信号、所述接收时间序列和坐标计算模型，计算所述航行器的第三位置坐标；

步骤10，根据所述第一位置坐标和所述第三位置坐标，采用加权算法，计算并发送所述航行器的第四位置坐标。

7.如权利要求6所述的基于粒子滤波算法的水声导航定位方法，其特征在于，所述基于粒子滤波算法的水声导航定位方法被应用于所述航行器的水下导航，所述航行器根据所述第一位置坐标和预设航线，调整所述航行器的航行方向和航行速度，或者，

所述航行器根据所述第四位置坐标和所述预设航线，调整所述航行器的所述航行方向和航行速度。