CN113901383A - 一种基于多层等梯度声速剖面模型的声学定位方法 - Google Patents

Abstract

本发明公开了一种基于多层等梯度声速剖面模型的声学定位方法，包括：建立多层等梯度声速剖面的数学模型；确定若干传感器节点的状态信息，并获取传感器节点之间接收到的FDOA信息或FDOA信息和TDOA信息；根据获取的传感器节点的状态信息结合建立的多层等梯度声速剖面的数学模型建立目标节点和所述若干传感器节点位置信息之间的到达频率模型或到达频率和到达时间模型；利用FDOA或TDOA与FDOA联合估计方法结合步骤2)获取的信息和步骤3)的模型估计得到待定位目标节点的位置、速度信息。本发明提供的声学定位方法，通过多层等梯度声速剖面模型对声线进行有效的跟踪，减轻基于声线直线传播假设带来的误差，从而对移动目标的位置和速度做出更可靠和准确的估计。

Description

一种基于多层等梯度声速剖面模型的声学定位方法

技术领域

本发明属于水声定位导航领域，具体涉及一种基于多层等梯度声速剖面模型的声学定位方法。

背景技术

水下声学传感器网络(UASNs)的出现，极大地提高了海洋环境监测的效率。在大多数水下声学传感器网络应用中，精确的节点定位是最基本和最重要的。节点的位置信息是完成其他水下任务的关键所在。在依赖位置的数据采集任务中，只有当位置信息正确时，数据才是有用的。由于水中声速随水深、盐度和温度的变化而变化，基于声线直线传播假设的定位方法存在固有偏差，因此需要提出能够处理真实声速剖面的更精确的定位方法。

发明内容

针对现有技术中存在的上述技术问题，本发明提出了一种基于多层等梯度声速剖面模型的声学定位方法，设计合理，克服了现有技术的不足，减轻了基于声线直线传播假设带来的误差，从而对移动目标的位置和速度作出更可靠和准确的估计。

为达到上述目的，本发明的技术方案如下：

一种基于多层等梯度声速剖面模型的声学定位方法，包括以下步骤：

步骤1)建立多层等梯度声速剖面的数学模型；所述多层等梯度声速剖面的数学模型每层声速剖面的梯度相同，不同层之间的梯度会有变化，用P层声速剖面来接近真实的声速剖面，第p个声线层中的声速c_p(z)表示为：

式中，z代表水深，范围为第p层的起点深度

到终点深度

a_p是该层的声速梯度，b_p是该层声速的水平轴截距。

其中，根据声线在第p层的终点和第p+1层的起点的掠射角相等，进行声线跟踪。

步骤2)获取若干传感器节点的状态信息，并获取传感器节点之间接收到的FDOA信息或FDOA信息和TDOA信息；其中，TDOA(Time Difference of Arrival)为到达时间差，FDOA(Frequency Difference of Arrival)为到达频率差。

步骤3)根据步骤2)获取的传感器节点的状态信息结合步骤1)建立的多层等梯度声速剖面的数学模型建立目标节点和所述若干传感器节点位置信息之间的到达频率模型或到达频率和到达时间模型；

步骤4)利用FDOA或TDOA与FDOA联合估计方法结合步骤2)获取的FDOA信息或FDOA信息和TDOA信息和步骤3)的模型估计得到待定位目标节点的位置、速度信息。

进一步地，所述步骤1)中，根据声线在第p层的终点和第p+1层的起点的掠射角相等，进行声线跟踪，具体为：不考虑海底和海面的声线反射情况，声线在第p层的终点和第p+1层的起点的掠射角相等：

其中掠射角的计算公式如下：

α_p＝arctan(Y_p)

β_p＝arctan(X_p)

Y_p＝L_p/X_p,当

式中，d_p表示水平方向上起止点的距离，X_p和Y_p是过程中定义的计算中间变量，α_p是实际声线与两节点直线传播路径之间的夹角，β_p是两节点之间直线路径与水平方向的夹角，根据掠射角计算公式以及三角函数和差化积的特性，得到下式：

基于X_p参数，获取各层的

和

进而实现声线跟踪。

进一步地，所述步骤2)中，若干传感器节点的状态信息包括位置和速度信息。

进一步地，所述步骤3)中根据步骤2)获取的传感器节点的状态信息结合步骤1)建立的多层等梯度声速剖面的数学模型建立待定位目标节点和所述若干传感器节点位置信息之间的到达频率模型具体包括：

通过多层等梯度声速剖面模型，从发射端到接收端的FOA的表达式为：

g＝g_s-g_D

其中g_s是考虑发射端运动的发射频率，g_D是接收端运动产生的多普勒频移，具体写作：

其中，f₀是载波频率，c_s和c_r分别是发射端和接收端所在位置深度处的声速，v_⊥s和v_⊥r分别是发射端和接收端的径向速度：

其中

是声线在xoy平面的投影与x轴的夹角，v_s＝[v_sx,v_sy,v_sz]^T和v_r＝[v_rx,v_ry,v_rz]^T分别为发射端和接收端的速度；

分别是发射端和接收端所在位置所处声速剖面模型层数对应的起始点掠射角和终止点掠射角。

根据上述关系求解FOA对目标节点位置和速度的偏导。

进一步地，将目标节点作为发射端，传感器节点作为接收端；假设目标和传感器节点的速度不为零，定义目标位置为μ＝[x,y,z]^T，目标节点在位置μ的速度为v_μ＝[v_x,v_y,v_z]^T＝[v_sx,v_sy,v_sz]^T。则FOA对目标节点位置的偏导为：

其中偏导进一步写成：

FOA对目标节点速度的偏导写作：

同理可以获得FOA对目标节点位置和速度的偏导。

进一步地，所述步骤4)中，利用FDOA估计得到目标节点的位置具体为：

建立传感器节点收到目标节点定位信号的FOA模型为：

f＝g(μ)+n

其中g(·)＝[g₁(·),g₂(·),...,g_N(·)]^T是信号的实际到达频率，N为传感器节点数量，f＝[f₁,f₂,...,f_N]^T是目标与每个传感器节点之间的测量到达频率，n是测量噪声，服从均值为零，方差为Σ＝[σ² _f1,σ² _f2,...,σ² _fN]^T的高斯分布；各个传感器节点的相应测量噪声相互独立；

用传感器参考节点的到达频率减去其他N-1个传感器节点的到达频率，FDOA测量模型为：

Δf＝g'(μ)+ζ

其中g'(·)＝[g₂₁'(·),g₃₁'(·),...,g_i1'(·),...,g_N1'(·)]^T，Δf＝[Δf₂₁,Δf₃₁,...,Δf_i1,...,Δf_N1]^T，ζ＝[ζ₂₁,ζ₃₁,...,ζ_i1,...,ζ_N1]^T，下标1为参考节点编号，更具体可以写作：

g_i1'(·)＝g_i(·)-g₁(·)

Δf_i1＝f_i-f₁

噪声向量的协方差矩阵为:

其中

是一个对角线矩阵，1_N-1是单位矩阵；

假定测量噪声是高斯的，则目标节点位置μ的最大似然估计值为：

通过高斯牛顿法采用泰勒级数近似，通过多次迭代，使最大似然估计值最小获得目标节点位置；其中，第k次迭代可以表示为：

其中

r_f＝g'(μ^(k))-Δf：

其中：

是传感器节点对目标节点位置的偏导。

进一步地，所述步骤4)中，利用TDOA与FDOA联合估计得到目标节点的位置和速度信息具体为：

目标节点的待估计变量建模为Φ＝[x,y,z,v_x,v_y,v_z]^T，使用高斯牛顿法求解Φ的最大似然估计值获得目标节点的位置和速度信息，其中第k次迭代可以表示为：

Φ^(k+1)＝Φ^(k)-(J^TR_ξ ^-1J)^-1J^TR_ξ ^-1r

其中：

为TDOA测量模型的噪声向量的协方差矩阵；

其中，TDOA测量模型表示为：

Δt＝h'(μ)+ρ

其中h'(·)＝[h₂₁'(·),h₃₁'(·),...,h_N1'(·)]^T，Δt＝[Δt₂₁,Δt₃₁,...,Δt_N1]^T，ρ＝[ρ₂₁,ρ₃₁,...,ρ_N1]^T，下标1为参考节点编号，更具体可以写作：

h_i1'(·)＝h_i(·)-h₁(·)

Δt_i1＝t_i-t₁

本发明提供的一种基于多层等梯度声速剖面模型的声学定位方法具有如下有益效果：

1、提出了多层等梯度声速剖面模型，该模型有效地跟踪了声线。

2、与直线传播假设下的定位方法相比，新提出的算法有效地处理了声线的弯曲现象，获得了更准确和可靠的目标定位与速度估计结果。

附图说明

图1为本发明实施例所公开的一种基于多层等梯度声速剖面模型的TDOA和FDOA声学定位方法的流程示意图；

图2为本发明实施例所公开的真实声速剖面示意图；

图3为本发明实施例所公开的直线传播模型和多层等梯度声速剖面模型下，位置估计误差曲面图；

图4为本发明实施例所公开的直线传播模型和多层等梯度声速剖面模型下，速度估计误差曲面图；

图5为本发明实施例所公开的FDOA算法和TDOA算法的定位均方根误差；

图6为本发明实施例所公开的TDOA与FDOA联合估计算法在位置和速度上的均方根误差。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述。

一种基于多层等梯度声速剖面模型的TDOA和FDOA声学定位方法，具体包括如下步骤：

步骤1)建立多层等梯度声速剖面的数学模型：

步骤1-1)多层等梯度声速剖面模型将真实的声速剖面根据深度分成P层：每层声速剖面的梯度相同，不同层之间的梯度会有变化，用P层声速剖面来接近真实的声速剖面，第p个声线层中的声速c_p(z)可以表示为：

式中，z代表水深，范围为第p层的起点深度

到终点深度

a_p是该层的声速梯度，b_p是该层声速的水平轴截距。a_p和b_p都是由真实的声速剖面决定；

步骤1-2)计算声线路径，实现声线跟踪：

在一层等梯度声速剖面中，声线开始节点为：

声线结束节点为：

根据斯内尔定律(Snell's law)和声线的几何关系，声线开始节点和声线结束节点的关系可以用如下一系列式子来表达：

Y_p＝L_p/X_p,当

α_p＝arctan(Y_p) (8)

β_p＝arctan(X_p) (9)

式中，d_p表示水平方向上起止点的距离，L_p是一个由水体声速剖面性质决定的常数，X_p和Y_p是过程中定义的计算中间变量，α_p是实际声线与两节点直线传播路径之间的夹角，β_p是两节点之间直线路径与水平方向的夹角，

和

分别是声线起止点的掠射角,从声线起始点S_p到终止点E_p的传播时间被记作t_p；

不考虑海底和海面的声线反射情况，声线在第p层的终点和第p+1层的起点的掠射角有如下关系：

从式(6)到式(12)，以及三角函数和差化积的特性，可以得到下式：

式(13)是X_p(或d_p)的高阶多项式；

基于X_p参数，获取各层的

和

进而实现声线跟踪；

因此，在多层等梯度声速剖面模型下，声线跟踪等同于确定P-1个多项式的根。

步骤2)确定若干传感器节点的状态信息，并获取TDOA与FDOA信息：

确定所述若干传感器节点位置a_i(i＝1,2,3...N)为：

传感器节点速度v_ai(i＝1,2,3...N)为：

式中，N为传感器节点的个数；

获取TDOA信息Δt_i＝[Δt₂₁,Δt₃₁,...,Δt_N1]^T与FDOA信息Δf＝[Δf₂₁,Δf₃₁,...,Δf_N1]^T，其中1为参考节点编号,i＝2,3...N。

步骤3)计算待定位目标节点和所述若干信标节点位置信息之间的到达频率：

步骤3-1)确定发射端到接收端的到达频率(frequency of arrival，FOA)的表达式：

通过多层等梯度声速剖面模型，从发射端到接收端的FOA的表达式为：

g＝g_s-g_D (16)

其中g_s是考虑发射端运动的发射频率，g_D是接收端运动产生的多普勒频移，具体可以写作：

其中c_s和c_r分别是发射端和接收端所在位置深度处的声速，v_⊥s和v_⊥r分别是发射端和接收端的径向速度；v_⊥s和v_⊥r由以下公式给出：

其中

是声线在xoy平面的投影与x轴的夹角，v_s＝[v_sx,v_sy,v_sz]^T和v_r＝[v_rx,v_ry,v_rz]^T分别为发射端和接收端的速度；

分别是发射端和接收端所在位置所处声速剖面模型层数对应的起始点掠射角和终止点掠射角。

步骤3-2)任意点多普勒频移的梯度计算：即根据上述关系求解FOA对目标节点位置和速度各分量的偏导，本实施例中，将目标节点作为发射端，传感器节点作为接收端；假设目标和传感器节点的速度不为零，定义目标位置为μ＝[x,y,z]^T，目标节点在位置μ的速度为v_μ＝[v_x,v_y,v_z]^T＝[v_sx,v_sy,v_sz]^T，则FOA对目标节点位置分量x的偏导可由式(16)得到，写作：

其中偏导可以进一步写成：

其中

f₀是载波频率。类似的，g关于目标位置分量y和z的偏导也可以如此得到；

FOA对目标节点速度的偏导也可以从式(16)中得到，写作：

类似的，可以得到g关于目标节点速度分量v_y和v_z的偏导。

步骤4)用FDOA信息进行目标定位：

考虑一个三维的水下传感器网络，传感器节点的数量是N，所有传感器节点的位置都是已知的。目标节点的位置是μ，那么传感器节点收到的定位信号的FOA可以被建模为：

f＝g(μ)+n (29)

其中信号的实际到达频率g(·)＝[g₁(·),g₂(·),...,g_N(·)]^T是目标节点位置μ＝[x,y,z]^T的函数，由式(16)表示；f＝[f₁,f₂,...,f_N]^T是目标与单个传感器之间的测量FOA；N维列向量n是测量噪声，它服从均值为零，方差为Σ＝[σ² _f1,σ² _f2,...,σ² _fN]^T的高斯分布；各个传感器节点的相应测量噪声是相互独立的；

以第一个传感器节点为参考节点，用参考节点的到达频率减去其他N-1个节点的到达频率，FDOA测量模型为：

Δf＝g'(μ)+ζ (30)

其中g'(·)＝[g₂₁'(·),g₃₁'(·),...,g_i1'(·),...,g_N1'(·)]^T，Δf＝[Δf₂₁,Δf₃₁,...,Δf_i1,...,Δf_N1]^T，ζ＝[ζ₂₁,ζ₃₁,...,ζ_i1,...,ζ_N1]^T，更具体可以写作：

g_i1'(·)＝g_i(·)-g₁(·) (31)

Δf_i1＝f_i-f₁ (32)

噪声向量的协方差矩阵可由式(34)得到:

其中

是一个对角线矩阵，其对角线值为

1_N-1是单位矩阵；

由于假定测量噪声是高斯的，目标位置μ的最大似然估计值可以写成：

显然，式(36)所代表的解是非线性的，可以用高斯牛顿法来求解；高斯牛顿法采用泰勒级数近似，通过多次迭代，使式(36)最小；其中，第k次迭代可以表示为：

其中

r_f＝g'(μ^(k))-Δf，更进一步可以写作：

其中：

其中：

步骤5)用TDOA信息进行目标定位：

同样考虑一个三维的水下传感器网络，传感器节点的数量是N，所有传感器节点的位置都是已知的。目标节点的位置是μ，那么传感器节点收到的定位信号的TOA可以被建模为：

t＝h(μ)+m (41)

其中t＝[t₁,t₂,...,t_N]^T是目标与单个传感器之间的测量TOA；N维列向量m是测量噪声，它服从均值为零，方差为

的高斯分布；各个传感器节点的相应测量噪声是相互独立的；信号的实际传播时间h(·)＝[h₁(·),h₂(·),...,h_N(·)]^T是目标节点位置μ＝[x,y,z]^T的函数，即对应发射端和接收端所在位置之间声速的传播时间(参见论文Ranging in an underwater medium with multiple isogradient sound speedprofile layers)；

同样以第一个传感器节点为参考节点，用参考节点的实际传播时间减去其他N-1个节点的实际传播时间，TDOA测量模型为：

Δt＝h'(μ)+ρ (42)

其中h'(·)＝[h₂₁'(·),h₃₁'(·),...,h_N1'(·)]^T，Δt＝[Δt₂₁,Δt₃₁,...,Δt_N1]^T，ρ＝[ρ₂₁,ρ₃₁,...,ρ_N1]^T，更具体可以写作：

h_i1'(·)＝h_i(·)-h₁(·) (43)

Δt_i1＝t_i-t₁ (44)

噪声向量的协方差矩阵可由式(46)得到:

其中

是一个对角线矩阵，其对角线值为

由于假定测量噪声是高斯的，目标位置μ的最大似然估计值可以写成：

显然，式(48)所代表的解是非线性的，可以用高斯牛顿法来求解；高斯牛顿法采用泰勒级数近似，通过多次迭代，使式(48)最小；其中，第k次迭代可以表示为：

其中

r_t＝h'(μ^(k))-Δt，更进一步可以写作：

其中：

其中：

步骤6)基于TDOA和FDOA联合估计声学定位方法，得到待定位目标节点的位置与速度信息：

当TDOA和FDOA的测量结果结合起来估计移动目标的位置和速度时，可以通过增加上述矩阵的维度数量来实现；需要估计的变量建模为Φ＝[x,y,z,v_x,v_y,v_z]^T；再次使用高斯牛顿法来解决该问题，第k次迭代可以表示为：

Φ^(k+1)＝Φ^(k)-(J^TR_ξ ^-1J)^-1J^TR_ξ ^-1r (53)

其中：

具体迭代求解过程如图1所示。

以水下航行器探测定位为例，当水下移动目标节点进入探测区域后，其发出的特定频率的噪声被各接收器获得，一般水面接收器为传感器节点。由于各传感器节点到目标节点位置的距离不一样，且存在多普勒效应，因此声音信号到达各传感器节点的时间与频率也有所不同，各水听器呈现出的是波形相似，强度不同，时序上相差一定平移量且频率不同的信号。利用互相关解算技术可以求出各传感器节点的接收波形相对于参考传感器节点波形的时间差与频率差。作为算法的输入量，我们还需要提前标定好该区域的声速值，各传感器节点的测量误差方差，以及各传感器节点的速度值与布放的具体坐标。根据上述信息利用本发明方法，可得出目标位置的坐标。计算结果随传感器节点测量误差的增大而增大，且移动目标与传感器节点的相对位置也会对测量精度造成影响。

本实施例仿真的传感器节点的具体设置见表1。四个传感器节点被设置为平行四边形结构，边长相距2公里。目标节点设置在800米深处，传感器节点位于距海面10.2米深处。目标节点的真实速度为v_μ＝[-2,1.5,4]^T m/s。所有的信号都从目标节点发送到传感器节点。仿真中使用的真实声速剖面图见图2。海面上的声速为1500m/s，1000米处的声速为1490m/s。真实声速剖面满足关系式c(z)＝1500-sqrt(z/10)。在此基础上，建立等梯度声速剖面模型。将真实声速剖面均匀地分为9层。每层的梯度满足关系式a_p＝(c_p(z_s)-c_p(z_d))/D_L，其中c_p(z_s)和c_p(z_d)是第p层最浅和最深处的声速，D_L是该层的层高。假设传感器节点之间是时间同步的，而目标节点和传感器节点之间是时间不同步的。目标节点向传感器节点发送的信号的载波频率为1000Hz。均方根误差(RMSE)可以用于衡量预测值与真实值之间的差异。

表1传感器节点的位置和速度

在图3和图4中，信号时延的测量噪声的标准差为σ² _t＝1×10^-8s²，信号频率的测量噪声的标准差为σ² _f＝1×10^-9Hz²。这两个噪声是相互独立的。TDOA和FDOA的测量值是通过向真实值添加噪声产生的。估计误差曲面由1000次蒙特卡罗仿真的统计平均值给出，曲面中网格的长度为100米。

图3和图4分别显示了四个传感器节点所包围区域内的位置和速度的估计误差曲面。直线传播模型和多层等梯度声速剖面模型的定位精度都在同一图中说明。从图3和图4中的估计误差曲面可以看出，平行四边形的两个锐角处的误差最大。越靠近平行四边形的中心点，误差值就越小。在位置和速度估计误差图中，直线传播模型的误差比多层模型的误差至少大20dB。

在图5和图6中，用RMSE来表示三种算法在直线传播假设模型下以及多层等梯度声速剖面模型下的定位性能，包括单独的TDOA和FDOA算法，以及TDOA和FDOA联合估计算法。时延测量的噪声是影响算法精度的一个重要因素。在仿真中，时延的噪声范围从1×10^-8s²到1×10^-4s²。多普勒频移测量的噪声被设定为时延测量噪声的十分之一，范围从1×10^-9Hz²到1×10^-5Hz²。时延的测量噪声与多普勒频移的测量噪声是不相关。目标节点的起始位置为μ＝[1000,800,800]^T。误差曲面是由10,000次蒙特卡罗仿真的统计平均值给出的。从图5和图6的仿真中我们可以看到，随着测量噪声的减少，在其他条件保持不变的情况下，算法的误差会减小。

从图6中TDOA定位的结果可以看出，当测量噪声较小时，直线传播模型的性能明显不如多层等梯度模型。当测量噪声较大时，两个模型的精度几乎相同。因为此时，影响定位精度的主要因素是测量噪声而不是直线传播模型带来的误差。图6中FDOA定位的结果表明，当测量噪声较小时，直线传播模型的误差主要是由声线传播时的掠射角角度不准确引起的。当测量噪声的影响比较小时，直线传播模型的性能明显不如多层模型。当测量噪声足够大时，两个模型的定位误差趋于一致。从图5中联合估计的结果可以明显看出，随着测量噪声的增加，定位算法的均方根误差增加。在这种情况下，定位精度往往由测量误差决定，而不是由直线传播模型的误差决定。

通过比较图5和图6的结果，我们可以发现，多层等梯度声速剖面模型在有声线弯曲的水下场景中可以给出更可靠和准确的目标节点位置和速度估计，整体上优于直线传播模型。然而，由于仿真中所选择的声速剖面的整体梯度相当小，所以直线传播模型带来的误差对估计结果的影响也很变相变小了，特别是当测量噪声较大时。

通过上述仿真实验的验证，可以得出结论，基于多层等梯度声速剖面模型的定位算法能够有效地跟踪声射线，修正声射线弯曲引起的估计误差，与传统的直线传播模型相比，能够给出更可靠、更准确的节点位置和速度估计。

显然，上述实施例仅仅是为清楚地说明所作的举例，而并非对实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其他不同形式的变化或变动。这里无需也无法把所有的实施方式予以穷举。而由此所引申出的显而易见的变化或变动仍处于本发明的保护范围。

Claims (7)

1.一种基于多层等梯度声速剖面模型的声学定位方法，其特征在于，包括以下步骤：

步骤1)建立多层等梯度声速剖面的数学模型；所述多层等梯度声速剖面的数学模型每层声速剖面的梯度相同，不同层之间的梯度会有变化，用P层声速剖面来接近真实的声速剖面，第p个声线层中的声速c_p(z)表示为：

式中，z代表水深，范围为第p层的起点深度

到终点深度

a_p是该层的声速梯度，b_p是该层声速的水平轴截距。

其中，根据声线在第p层的终点和第p+1层的起点的掠射角相等，进行声线跟踪。

步骤2)获取若干传感器节点的状态信息，并获取传感器节点之间接收到的FDOA信息或FDOA信息和TDOA信息；

步骤3)根据步骤2)获取的传感器节点的状态信息结合步骤1)建立的多层等梯度声速剖面的数学模型建立目标节点和所述若干传感器节点位置信息之间的到达频率模型或到达频率和到达时间模型；

步骤4)利用FDOA或TDOA与FDOA联合估计方法结合步骤2)获取的FDOA信息或FDOA信息和TDOA信息和步骤3)的模型估计得到待定位目标节点的位置、速度信息。

2.根据权利要求1所述的多层等梯度声速剖面模型的声学定位方法，其特征在于，所述步骤1)中，根据声线在第p层的终点和第p+1层的起点的掠射角相等，进行声线跟踪，具体为：不考虑海底和海面的声线反射情况，声线在第p层的终点和第p+1层的起点的掠射角相等：

其中掠射角的计算公式如下：

α_p＝arctan(Y_p)

β_p＝arctan(X_p)

Y_p＝L_p/X_p,当

式中，d_p表示水平方向上起止点的距离，X_p和Y_p是过程中定义的计算中间变量，α_p是实际声线与两节点直线传播路径之间的夹角，β_p是两节点之间直线路径与水平方向的夹角，根据掠射角计算公式以及三角函数和差化积的特性，得到下式：

基于X_p参数，获取各层的

和

进而实现声线跟踪。

3.根据权利要求1所述的多层等梯度声速剖面模型的声学定位方法，其特征在于，所述步骤2)中，若干传感器节点的状态信息包括位置和速度信息。

4.根据权利要求1所述的多层等梯度声速剖面模型的声学定位方法，其特征在于，所述步骤3)中根据步骤2)获取的传感器节点的状态信息结合步骤1)建立的多层等梯度声速剖面的数学模型建立待定位目标节点和所述若干传感器节点位置信息之间的到达频率模型具体包括：

通过多层等梯度声速剖面模型，从发射端到接收端的FOA的表达式为：

g＝g_s-g_D

其中g_s是考虑发射端运动的发射频率，g_D是接收端运动产生的多普勒频移，具体写作：

其中，f₀是载波频率，c_s和c_r分别是发射端和接收端所在位置深度处的声速，v_⊥s和v_⊥r分别是发射端和接收端的径向速度：

其中

是声线在xoy平面的投影与x轴的夹角，v_s＝[v_sx,v_sy,v_sz]^T和v_r＝[v_rx,v_ry,v_rz]^T分别为发射端和接收端的速度；

分别是发射端和接收端所在位置所处声速剖面模型层数对应的起始点掠射角和终止点掠射角。

根据上述关系求解FOA对目标节点位置和速度的偏导。

5.根据权利要求4所述的多层等梯度声速剖面模型的声学定位方法，其特征在于，将目标节点作为发射端，传感器节点作为接收端；假设目标和传感器节点的速度不为零，定义目标位置为μ＝[x,y,z]^T，目标节点在位置μ的速度为v_μ＝[v_x,v_y,v_z]^T＝[v_sx,v_sy,v_sz]^T。

6.根据权利要求5所述的多层等梯度声速剖面模型的声学定位方法，其特征在于，所述步骤4)中，利用FDOA估计得到目标节点的位置具体为：

建立传感器节点收到目标节点定位信号的FOA模型为：

f＝g(μ)+n

其中g(·)＝[g₁(·),g₂(·),...,g_N(·)]^T是信号的实际到达频率，N为传感器节点数量，f＝[f₁,f₂,...,f_N]^T是目标与每个传感器节点之间的测量到达频率，n是测量噪声，服从均值为零，方差为Σ＝[σ² _f1,σ² _f2,...,σ² _fN]^T的高斯分布；各个传感器节点的相应测量噪声相互独立；

用传感器参考节点的到达频率减去其他N-1个传感器节点的到达频率，FDOA测量模型为：

Δf＝g'(μ)+ζ

其中g'(·)＝[g₂₁'(·),g₃₁'(·),...,g_i1'(·),...,g_N1'(·)]^T，Δf＝[Δf₂₁,Δf₃₁,...,Δf_i1,...,Δf_N1]^T，ζ＝[ζ₂₁,ζ₃₁,...,ζ_i1,...,ζ_N1]^T，下标1为参考节点编号，更具体可以写作：

g_i1'(·)＝g_i(·)-g₁(·)

Δf_i1＝f_i-f₁

噪声向量的协方差矩阵为:

其中diag(σ² _f2,σ² _f3,...,σ² _fN)是一个对角线矩阵，1_N-1是单位矩阵；

假定测量噪声是高斯的，则目标节点位置μ的最大似然估计值为：

通过高斯牛顿法采用泰勒级数近似，通过多次迭代，使最大似然估计值最小获得目标节点位置；其中，第k次迭代可以表示为：

其中J_f＝▽g'(μ^(k))，r_f＝g'(μ^(k))-Δf：

其中：

是传感器节点对目标节点位置的偏导。

7.根据权利要求1所述的多层等梯度声速剖面模型的声学定位方法，其特征在于，所述步骤4)中，利用TDOA与FDOA联合估计得到目标节点的位置和速度信息具体为：

目标节点的待估计变量建模为Φ＝[x,y,z,v_x,v_y,v_z]^T，使用高斯牛顿法求解Φ的最大似然估计值获得目标节点的位置和速度信息，其中第k次迭代可以表示为：

Φ^(k+1)＝Φ^(k)-(J^TR_ξ ^-1J)^-1J^TR_ξ ^-1r

其中：

为TDOA测量模型的噪声向量的协方差矩阵；J_t＝▽h'(μ^(k))，

其中，TDOA测量模型表示为：

Δt＝h'(μ)+ρ

其中h'(·)＝[h₂₁'(·),h₃₁'(·),...,h_N1'(·)]^T，Δt＝[Δt₂₁,Δt₃₁,...,Δt_N1]^T，ρ＝[ρ₂₁,ρ₃₁,...,ρ_N1]^T，下标1为参考节点编号，更具体可以写作：

h_i1'(·)＝h_i(·)-h₁(·)

Δt_i1＝t_i-t₁

Images