CN110794409A - 一种可估计未知有效声速的水下单信标定位方法 - Google Patents

Abstract

本发明属于水下定位技术领域，特别涉及一种水下航行器的单信标定位方法。水下航行器在未接收到周期性水声信号时，通过自身配备的电子罗盘、深度计及读取自身的螺旋桨转速信息进行航位推算，并在接收到所搭载的多普勒测速仪测得的绝对速度观测后，构造海流速度观测量并通过Kalman滤波进行海流速度校正；水下航行器接收到水声信号后，考虑水下声速的未知性，将未知有效声速建模为Student’s t分布，基于扩展Kalman滤波算法及变分贝叶斯近似，以水声信号传递时间为观测变量，进行水下航行器的位置更新。相比于基于已知定常有效声速以及基于声速不确定性满足Gauss分布的水下单信标定位方法，本发明可以获得更加理想的定位性能，增强水下单信标定位系统的实际应用能力。

Description

一种可估计未知有效声速的水下单信标定位方法

技术领域

本发明属于水下定位技术领域，特别涉及一种水下航行器的单信标定位方法。

背景技术

精确的位置反馈是水下航行器完成既定水下任务的基础。由于水下电磁波信号衰减较快，广泛应用于陆地与天空定位的GNSS系统在水下无法应用。现有主流的水下定位方式包括以惯性导航为代表的航位推算方法以及以长基线定位为代表的水下声学定位方法。其中惯性导航设备往往会随时间增长产生较大累计误差，无法长时间用于水下定位，而高精度的惯性导航设备成本极高，限制了其在水下航行器中的应用。现有主流的水下声学定位方式包括长基线定位、超短基线定位、单信标定位等。长基线定位与超短基线定位发展均较为成熟，但其成本通常较高，且实时性通常较差，这限制了其在水下航行器中的应用。而新兴的水下单信标定位系统融合航位推算数据与单水声信标的测距信息，在定位成本和实时性方面均有较大的优势。水声测距是通过检测水声信号的传递时间乘以水声声速获得。目前的水下单信标定位方法通常假设水声声速完全已知，但实际的水声声速收到水域温度、盐度、密度、水深等因素的影响，通常为时变未知的。水声声速的设置误差会导致测距误差，进而会影响单信标定位系统定位精度。而现有基于未知有效声速的水下单信标定位方法均将有效声速不确定性建模为Gauss分布。但在实际的水下定位当中，受到水下恶劣环境的影响，水声声速不确定性通常为非Gauss的(会受到观测野值等因素的影响)。在这种情况下，传统的基于Gauss噪声对水声声速不确定性进行建模的单信标定位方法存在随机模型建模误差，这也会影响单信标定位系统的性能，进而影响其实际应用。

发明内容

本发明的目的是：针对水下单信标定位当中水声声速的未知性及其不确定性统计特性的非Gauss性，结合扩展Kalman滤波与变分贝叶斯近似提出一种可估计未知有效声速的水下单信标定位方法。

本发明的技术方案是：一种可估计未知有效声速的水下单信标定位方法：水下航行器搭载水听器、多普勒测速仪、深度计、电子罗盘及GPS；水声信标周期性广播水声信号，水下航行器通过所搭载的多普勒测速仪周期性的观测其绝对速度；水下航行器通过GPS获取初始位置，并且通过读取自身螺旋桨转速获得航行器与水的相对速度。水下航行器未接收到水声信号时通过自身携带的电子罗盘及读取自身螺旋桨转速信息进行航位推算；水下航行器在接收到多普勒测速仪提供的绝对速度观测时，通过结合螺旋桨转速信息及电子罗盘信息构造海流观测变量，基于Kalman滤波进行海流速度更新；水下航行器接收到水声信号后，考虑水下声速的未知性，将未知声速建模为Student’s t分布，基于扩展Kalman滤波算法及变分贝叶斯近似，以水声信号传递时间为观测变量，进行水下航行器的位置更新。本发明的步骤包括：

A.以定位区域内任意点为原点，东、北、天三个方向分别设为x，y，z轴，建立水下局部惯性坐标系；

B.通过水下航行器所搭载的GPS系统获取该所述水下航行器在水下局部惯性系当中的初始位置；

C.建立所述水下航行器的运动学模型以及观测模型并进行离散化；

D.建立有效声速的随机模型、函数模型以及预测模型；

E.水声信标周期性广播水声信号，水声信号发射时间及水声信标位置已知；所述水下航行器在未接收到水声信号时，通过自身配备的电子罗盘、深度计以及读取自身的螺旋桨转速信息进行航位推算，同时进行有效声速随机模型参数预测；所述水下航行器在接收到所搭载的多普勒测速仪测得的的绝对速度观测后，通过读取所述螺旋桨转速信息及所述电子罗盘信息，构造海流速度观测量并通过Kalman滤波进行海流速度校正；

F.所述水下航行器接收到水声信号后，记录接收时刻，根据已知的水声信号发射时刻及水声信标位置坐标，并考虑水下声速的未知性，以此基于扩展Kalman滤波算法及变分贝叶斯近似，以水声信号传递时间为观测变量，进行所述水下航行器的位置更新。

进一步的，所述C步骤中，所述运动学模型的建立方法为：

定义状态向量为：

x＝[x y v_cx v_cy]^T

其中：x，y为所述水下航行器在所述水下局部惯性坐标系中的水平位置；v_cx，v_cy为未知的海流速度；

对x求导并加入所述水下航行器运动运动模型噪声影响，得到所述水下航行器的运动学模型：

其中：v_wx为所述水下航行器x方向的对水速度；v_wy为所述水下航行器y方向的对水速度；v_wx及v_wy通过读取所述螺旋桨转速与所述电子罗盘测得的航行器艏向角计算得出；ω_x为所述水下航行器在x方向的位置不确定性；ω_y为所述水下航行器在y方向的位置不确定性；ω_cx为x方向的海流不确定性；ω_cy为y方向的海流不确定性；

v_wx及v_wy的计算公式为：

式中v_w为根据所述螺旋桨转速得出的所述水下航行器对水速度，为所述电子罗盘测得的艏向角。

进一步的，所述C步骤中，所述观测模型的建立方法为：

S1.建立水声信号传递时间的观测模型；

设所述水下航行器获得所述水声信标发射水声信号的时刻为T_e，所述水声信标在所述水下局部惯性坐标系中的空间位置坐标为X_Te，Y_Te，Z_Te，所述水下航行器接收到该水声信号的时刻为T_a，观测方程为：

其中：ν_t为对应的观测噪声；z为所述水下航行器的深度，由深度计精确测得，为已知量；v_e为有效声速；

将观测方程记作m＝h(x,v_e)，其中：

S2.建立海流流速观测模型；

根据所述多普勒测速仪测得的所述水下航行器的绝对速度v_g，结合所述电子罗盘测得的艏向角

计算得到所述水下航行器绝对速度在局部惯性坐标系下的分量v_gx,v_gy；

根据v_gx,v_gy以及v_wx,v_wy，计算得到海流速度观测分量为：

海流观测量为线性，满足m_vc＝Hx+ν_vc；

其中：观测向量m_vc＝[m_cx m_cy]^T；m_cx，m_cy分别为x，y方向海流速度观测；ν_vc为海流观测噪声向量，ν_vc＝[ν_v,cx ν_v,cy]^T，其中ν_v,cx为x方向的海流不确定性；ν_v,cy为y方向的海流不确定性；H为海流观测矩阵，满足：

进一步的，所述C步骤中，所述运动学模型以及观测模型离散化方法为：

S1.运动学模型离散化；

以下标k为时间索引，以Δt＝t_k+1-t_k为离散间隔，运动学模型离散为：

x_k+1＝A_kx_k+B_ku_k+w_k

其中：A_k为运动学方程，满足：

B_k为控制方程，满足：

u_k为控制向量，满足u_k＝[v_wx,k v_wy,k]^T，为已知量；

w_k过程噪声向量，满足w_k＝[ω_x,k ω_y,k ω_cx,k ω_cy,k]^T，对应各个状态变量的不确定性；将系统状态x_k,y_k,v_cx,k,v_cy,k的过程噪声建模为零均值Gauss分布，其协方差矩阵满足：

其中，σ_w为所述水下航行器对水速度观测不确定性的标准差；σ_c为海流不确定性的标准差；

S2.观测模型离散化；

所述水下航行器在k-1至k间接收到该水声信号，将其假设为在k时刻接收到该水声信号，即离散后的水声信号传递时间观测方程为：

其中，ν_t,k为观测噪声，假设其满足方差为R_t,k的Gauss分布；考虑到有效声速v_e,k的时变未知性，将v_e,k也看作随机变量；离散形式的观测方程写作：

m_k＝h_k(x_k,v_e,k)，

由于海流观测的采样频率较高，假设在每一个离散时间点k处均可以得到海流速度观测，故离散后的海流速度观测方程为：

m_vc，k＝H_kx_k+ν_vc,k

其中，H_k为k时刻海流速度观测矩阵，满足：

ν_vc,k为k时刻海流观测噪声，为零均值Gauss分布，其观测噪声协方差矩阵记作：

其中，σ_vc,m为海流速度观测噪声的标准差。

进一步的，所述D步骤中，有效声速的随机模型、函数模型以及预测模型建立方法为：

考虑到恶劣海况下有效声速不确定性的非Gauss性，将有效声速初始先验分布建模为 Student’s t分布：

其中：St(x|a,b,c)表示以a为均值，b为尺度参数，c为自由度，满足Student’s t分布的随机变量x；

为初始声速估计均值；

为初始声速不确定性尺度参数；v₀为初始声速自由度参数；

将k-1时刻有效声速的后验分布建模为Student’s t分布：

其中：m_1:k表示从1到k时刻的观测量的集合；p(x|y)表示随机变量x以y为条件的概率密度函数；

分别为k-1时刻有效声速的后验均值、尺度参数及自由度；

有效声速的动态扩散的函数模型记作：

v_e,k＝v_e,k-1+ω_e,k-1

其中：ω_e,k-1为有效声速的不确定性，同样满足Student’s t分布

p(ω_e,k-1)＝St(ω_e,k-1|0,Q_e,k-1,v₁)

其中Q_e,k-1与v₁分别为有效声速不确定性的尺度参数与自由度；将声速不确定性自由度建模为与k-1时刻有效声速后验自由度相等，即v₁＝v_k-1；据此，将有效声速的概率扩散方程记作：

p(v_e,k|v_e,k-1)＝St(v_e,k|v_e,k-1,Q_e,k-1,v_k-1)

结合上式，获得有效声速的预测模型为：

其中：分别为k时刻有效声速的先验均值、尺度参数及自由度，其计算公式为：

v_k＝v_k-1

根据启发式Gauss模型，将有效声速的先验概率密度函数分解为：

p(α_k)＝G(α_k|v_k/2,v_k/2)

其中：N(x|μ,Σ)表示以μ为均值，Σ为协方差矩阵，满足Gauss分布的随机向量x；G(x|a,b)表示以a,b为参数，满足Gama分布的随机变量x；α_k为辅助参数。

进一步的，所述E步骤中，所述水下航行器利用自身配备的电子罗盘、深度计以及读取自身的螺旋桨转速信息进行航位推算的方法为：

根据Kalman滤波的预测环节，将系统状态的先验分布近似为Gauss分布：

其中：与P_k|k-1分别为k时刻的先验状态和先验方差，其计算方法为：

其中：

与P_k-1|k-1分别为k-1时刻的后验状态和后验方差。

进一步的，所述E步骤中，所述水下航行器接收到所述多普勒测速仪测得的绝对速度观测后，进行海流速度校正的方法为：

P_k|k＝P_k|k-1-K_kH_kP_k|k-1

其中：K_k为海流更新的Kalman增益。

进一步的，所述步骤F中，通过扩展Kalman滤波及变分贝叶斯近似进行水声信号传递时间校正的方法为：

S1.建立观测似然函数及有效声速后验概率密度分布模型；

根据离散形式的水声信号传递时间观测模型，得到水声信号传递时间的观测似然密度为：

p(m_k|v_e,k,x_k)＝N(m_k|h_k(x_k,v_e,k),R_k)

将有效声速后验分布建模为Student’s t分布，并分解为：

S2.定义状态变量及有效声速后验估计近似值；

通过变分贝叶斯近似，将有效声速，系统状态以及辅助参数的联合后验估计近似为：

p(x_k,v_e,k,α_k|m_1:k)≈q(x_k)q(v_e,k)q(α_k)

以最小化近似前与近似后两个概率密度函数之间的Kullback-Leibler散度(KLD)最小化为目标，得到近似解为：

其中：log(·)表示对数函数；θ表示x_k,v_e,k,α_k中的任意元素；E_x[·]表示相对于x的期望；上标(-θ)表示整个集合当中除了θ以外的其它元素；c_θ表示与θ无关的常数；采用固定点迭代来求解q(θ)；

S3.求解状态变量及有效声速后验估计近似值；

S3.1求解联合概率密度对数值；

其对数形式表示为：

S3.2求解辅助参数α_k估计值；

令θ＝α_k，得到：

其中变量加上标(i)表示该变量在第i次迭代中的估计值；

进而得到：

取α_k在第i+1次迭代中的估计值为期望值，即：

S3.3求解有效声速v_e,k估计值；

以

为展开点，对非线性的水声信号传递时间观测方程m_k＝h_k(x_k,v_e,k)进行线性化，只保留一阶项，得到：

其中：

为观测方程的雅克比矩阵；

由此得到线性化后的水声信号传递时间观测方程为：

令θ＝v_e,k，得到：

其中：

根据Kalman滤波更新式，得到有效声速更新方程为：

其中

为第i+1次迭代中有效声速更新的Kalman增益；

S3.4求解系统状态x_k估计值；

以

为展开点，对非线性的水声信号传递时间观测方程m_k＝h_k(x_k,v_e,k)进行线性化，只保留一阶项，得到：

其中：

为观测方程的雅克比矩阵；

由此得到线性化后的水声信号传递时间观测方程为：

令θ＝x_k，得到：

其中：

根据Kalman滤波更新式，得到系统状态更新方程为：

其中：

为第i+1次迭代中系统状态更新的Kalman增益；

记总迭代次数为N，则最终有效声速均值及尺度参数、系统状态均值及协方差矩阵的估计值分别为：

有益效果：本发明通过结合Kalman滤波、扩展Kalman滤波及变分贝叶斯近似，将未知有效声速不确定性建模为Student’s t分布，以水声信号传递时间为观测变量，进行水下航行器的位置更新。相比于基于已知定常有效声速以及基于声速不确定性满足Gauss分布的水下单信标定位方法，本发明能够更好的跟踪水声声速变化的趋势，进而也可以得到更好的定位结果，增强水下单信标定位系统的实际应用能力。

附图说明

图1为本发明的步骤流程图；

图2为水面船的真实运动轨迹、本发明与两种传统水下单信标定位方法所估计出的运动轨迹比较；

图3为本发明与两种传统水下单信标定位方法的水平定位误差随时间变化的曲线；

图4为真实声速值、基于传统方法1与本发明提出方法估计出的水声声速。

具体实施方式

实施例1，参见附图1，一种可估计未知有效声速的水下单信标定位方法，包括以下步骤：

A.以定位区域内任意点为原点，东、北、天三个方向分别设为x，y，z轴，建立水下局部惯性坐标系；

B.通过水下航行器所搭载的GPS系统获取该所述水下航行器在水下局部惯性系当中的初始位置；

C.建立所述水下航行器的运动学模型以及观测模型并进行离散化；

所述运动学模型的建立方法为：

定义状态向量为：

x＝[x y v_cx v_cy]^T

其中：x，y为所述水下航行器在所述水下局部惯性坐标系中的水平位置；v_cx，v_cy为未知的海流速度；

对x求导并加入所述水下航行器运动运动模型噪声影响，得到所述水下航行器的运动学模型：

其中：v_wx为所述水下航行器x方向的对水速度；v_wy为所述水下航行器y方向的对水速度；v_wx及v_wy通过读取所述螺旋桨转速与所述电子罗盘测得的航行器艏向角计算得出；ω_x为所述水下航行器在x方向的位置不确定性；ω_y为所述水下航行器在y方向的位置不确定性；ω_cx为x方向的海流不确定性；ω_cy为y方向的海流不确定性；

v_wx及v_wy的计算公式为：

式中v_w为根据所述螺旋桨转速得出的所述水下航行器对水速度，

为所述电子罗盘测得的艏向角；

所述观测模型的建立方法为：

S1.建立水声信号传递时间的观测模型；

设所述水下航行器获得所述水声信标发射水声信号的时刻为T_e，所述水声信标在所述水下局部惯性坐标系中的空间位置坐标为X_Te，Y_Te，Z_Te，所述水下航行器接收到该水声信号的时刻为T_a，观测方程为：

其中：ν_t为对应的观测噪声；z为所述水下航行器的深度，由深度计精确测得，为已知量；v_e为有效声速；

将观测方程记作m＝h(x,v_e)，其中：

S2.建立海流流速观测模型；

根据所述多普勒测速仪测得的所述水下航行器的绝对速度v_g，结合所述电子罗盘测得的艏向角

计算得到所述水下航行器绝对速度在局部惯性坐标系下的分量v_gx,v_gy；

根据v_gx,v_gy以及v_wx,v_wy，计算得到海流速度观测分量为：

海流观测量为线性，满足m_vc＝Hx+ν_vc；

其中：观测向量m_vc＝[m_cx m_cy]^T；m_cx，m_cy分别为x，y方向海流速度观测；ν_vc为海流观测噪声向量，ν_vc＝[ν_v,cx ν_v,cy]^T，其中ν_v,cx为x方向的海流不确定性；ν_v,cy为y方向的海流不确定性；H为海流观测矩阵，满足：

所述运动学模型以及观测模型离散化方法为：

S1.运动学模型离散化；

以下标k为时间索引，以Δt＝t_k+1-t_k为离散间隔，运动学模型离散为：

x_k+1＝A_kx_k+B_ku_k+w_k

其中：A_k为运动学方程，满足：

B_k为控制方程，满足：

u_k为控制向量，满足u_k＝[v_wx,k v_wy,k]^T，为已知量；

w_k过程噪声向量，满足w_k＝[ω_x,k ω_y,k ω_cx,k ω_cy,k]^T，对应各个状态变量的不确定性；将系统状态x_k,y_k,v_cx,k,v_cy,k的过程噪声建模为零均值Gauss分布，其协方差矩阵满足：

其中，σ_w为所述水下航行器对水速度观测不确定性的标准差；σ_c为海流不确定性的标准差；

S2.观测模型离散化；

所述水下航行器在k-1至k间接收到该水声信号，将其假设为在k时刻接收到该水声信号，即离散后的水声信号传递时间观测方程为：

其中，ν_t,k为观测噪声，假设其满足方差为R_t,k的Gauss分布；考虑到有效声速v_e,k的时变未知性，将v_e,k也看作随机变量；离散形式的观测方程写作：

m_k＝h_k(x_k,v_e,k)，

由于海流观测的采样频率较高，假设在每一个离散时间点k处均可以得到海流速度观测，故离散后的海流速度观测方程为：

m_vc，k＝H_kx_k+ν_vc,k

其中，H_k为k时刻海流速度观测矩阵，满足：

ν_vc,k为k时刻海流观测噪声，为零均值Gauss分布，其观测噪声协方差矩阵记作：

其中，σ_vc,m为海流速度观测噪声的标准差；

D.建立有效声速的随机模型、函数模型以及预测模型；

有效声速的随机模型、函数模型以及预测模型建立方法为：

考虑到恶劣海况下有效声速不确定性的非Gauss性，将有效声速初始先验分布建模为 Student’s t分布：

其中：St(x|a,b,c)表示以a为均值，b为尺度参数，c为自由度，满足Student’s t分布的随机变量x；

为初始声速估计均值；

为初始声速不确定性尺度参数；v₀为初始声速自由度参数；

将k-1时刻有效声速的后验分布建模为Student’s t分布：

其中：m_1:k表示从1到k时刻的观测量的集合；p(x|y)表示随机变量x以y为条件的概率密度函数；

分别为k-1时刻有效声速的后验均值、尺度参数及自由度；

有效声速的动态扩散的函数模型记作：

v_e,k＝v_e,k-1+ω_e,k-1

其中：ω_e,k-1为有效声速的不确定性，同样满足Student’s t分布

p(ω_e,k-1)＝St(ω_e,k-1|0,Q_e,k-1,v₁)

其中Q_e,k-1与v₁分别为有效声速不确定性的尺度参数与自由度；将声速不确定性自由度建模为与k-1时刻有效声速后验自由度相等，即v₁＝v_k-1；据此，将有效声速的概率扩散方程记作：

p(v_e,k|v_e,k-1)＝St(v_e,k|v_e,k-1,Q_e,k-1,v_k-1)

结合上式，获得有效声速的预测模型为：

其中：分别为k时刻有效声速的先验均值、尺度参数及自由度，其计算公式为：

v_k＝v_k-1

根据启发式Gauss模型，将有效声速的先验概率密度函数分解为：

p(α_k)＝G(α_k|v_k/2,v_k/2)

其中：N(x|μ,Σ)表示以μ为均值，Σ为协方差矩阵，满足Gauss分布的随机向量x；G(x|a,b)表示以a,b为参数，满足Gama分布的随机变量x；α_k为辅助参数；

E.水声信标周期性广播水声信号，水声信号发射时间及水声信标位置已知；所述水下航行器在未接收到水声信号时，通过自身配备的电子罗盘、深度计以及读取自身的螺旋桨转速信息进行航位推算，同时进行有效声速随机模型参数预测；所述水下航行器在接收到所搭载的多普勒测速仪测得的的绝对速度观测后，通过读取所述螺旋桨转速信息及所述电子罗盘信息，构造海流速度观测量并通过Kalman滤波进行海流速度校正；

所述水下航行器利用自身配备的电子罗盘、深度计以及读取自身的螺旋桨转速信息进行航位推算的方法为：

根据Kalman滤波的预测环节，将系统状态的先验分布近似为Gauss分布：

其中：

与P_k|k-1分别为k时刻的先验状态和先验方差，其计算方法为：

其中：

与P_k-1|k-1分别为k-1时刻的后验状态和后验方差；

所述水下航行器接收到所述多普勒测速仪测得的绝对速度观测后，进行海流速度校正的方法为：

P_k|k＝P_k|k-1-K_kH_kP_k|k-1

其中：K_k为海流更新的Kalman增益；

F.所述水下航行器接收到水声信号后，记录接收时刻，根据已知的水声信号发射时刻及水声信标位置坐标，并考虑水下声速的未知性，以此基于扩展Kalman滤波算法及变分贝叶斯近似，以水声信号传递时间为观测变量，进行所述水下航行器的位置更新；

通过扩展Kalman滤波及变分贝叶斯近似进行水声信号传递时间校正的方法为：

S1.建立观测似然函数及有效声速后验概率密度分布模型；

根据离散形式的水声信号传递时间观测模型，得到水声信号传递时间的观测似然密度为：

p(m_k|v_e,k,x_k)＝N(m_k|h_k(x_k,v_e,k),R_k)

将有效声速后验分布建模为Student’s t分布，并分解为：

S2.定义状态变量及有效声速后验估计近似值；

通过变分贝叶斯近似，将有效声速，系统状态以及辅助参数的联合后验估计近似为：

p(x_k,v_e,k,α_k|m_1:k)≈q(x_k)q(v_e,k)q(α_k)

以最小化近似前与近似后两个概率密度函数之间的Kullback-Leibler散度(KLD)最小化为目标，得到近似解为：

其中：log(·)表示对数函数；θ表示x_k,v_e,k,α_k中的任意元素；E_x[·]表示相对于x的期望；上标(-θ)表示整个集合当中除了θ以外的其它元素；c_θ表示与θ无关的常数；采用固定点迭代来求解q(θ)；

S3.求解状态变量及有效声速后验估计近似值；

S3.1求解联合概率密度对数值；

其对数形式表示为：

S3.2求解辅助参数α_k估计值；

令θ＝α_k，得到：

其中变量加上标(i)表示该变量在第i次迭代中的估计值；

进而得到：

取α_k在第i+1次迭代中的估计值为期望值，即：

S3.3求解有效声速v_e,k估计值；

以

为展开点，对非线性的水声信号传递时间观测方程m_k＝h_k(x_k,v_e,k)进行线性化，只保留一阶项，得到：

其中：

为观测方程的雅克比矩阵；

由此得到线性化后的水声信号传递时间观测方程为：

令θ＝v_e,k，得到：

其中：

根据Kalman滤波更新式，得到有效声速更新方程为：

其中

为第i+1次迭代中有效声速更新的Kalman增益；

S3.4求解系统状态x_k估计值；

以

为展开点，对非线性的水声信号传递时间观测方程m_k＝h_k(x_k,v_e,k)进行线性化，只保留一阶项，得到：

其中：

为观测方程的雅克比矩阵；

由此得到线性化后的水声信号传递时间观测方程为：

令θ＝x_k，得到：

其中：

根据Kalman滤波更新式，得到系统状态更新方程为：

其中：为第i+1次迭代中系统状态更新的Kalman增益；

记总迭代次数为N，则最终有效声速均值及尺度参数、系统状态均值及协方差矩阵的估计值分别为：

实施例2，利用实施例1所述的方法通过试验数据进行验证。

作为比较，本实施例同时展示了基于声速不确定性Gauss分布以及基于已知定常水声声速的水下单信标定位方法定位结果(分别记作传统方法1与传统方法2，传统方法1参考文献Z.Zhu and S.L.J.Hu,“Model and algorithm improvement on single beaconunderwater tracking,”IEEE Journal of Oceanic Engineering,vol.PP,no.99,pp.1–18,2017.)。

试验数据的搜集方法为：水面船搭载GPS、水听器以及罗盘，在水面上进行二维运动。 GPS观测到的水面船运动轨迹作为真实参考，水听器接收固定在水底的水声信标所发射的水声信号，并以此获得水声信号传递时间。由于水面船未安装多普勒测速仪，采用GPS轨迹进行差分结合电子罗盘测得的艏向角来仿真多普勒测速仪所观测的航行器对地速度。试验当中水声信号发射周期约为30秒(少数几个信号出现观测丢包)，海流观测周期为1秒，离散时间间隔Δt同样设置为1秒。

在数值验证的过程中，各种方法调制参数设置为：(1)x与y两个方向位置初始误差均为10米；(2)x与y两个方向海流初始误差均为0.05米/秒；(3)名义声速v_e,k为1540米/秒；(4)海流不确定性标准差σ_c为0.01米/秒；(5)航行器对水速度观测不确定性标准差σ_w为 0.1米/秒；(6)传统方法1与本发明所采用的水声声速不确定性的标准差为1米/秒，本发明所用的水声声速噪声尺度系数通过其标准差进行求解；(7)水声信号传递时间观测噪声标准差σ_t,m为0.001秒；(8)海流观测噪声标准差σ_vc,m为0.01米/秒；(9)传统方法2中斜距观测噪声标准差σ_r,m为5米；(10)本发明的迭代次数N设置为15，自由度参数为2.25。

附图2为试验中水面船的真实运动轨迹、本发明的方法与两种传统水下单信标定位方法所估计出的运动轨迹。附图3表示三种方法的水平定位误差随时间变化的曲线，定位误差计算公式为

附图4表示真实声速值、基于声速不确定性Gauss分布的水下单信标定位方法(传统方法1)与本发明提出方法的水声声速估计结果。两种传统方法与本发明所提出方法的平均均方定位误差分别为7.19米、13.09米与5.50米。而传统方法1与本发明方法的声速平均均方误差分别为6.87米/秒与3.47米/秒。平均均方定位误差与声速平均均方误差计算公式分别为：

其中：T和K分别表示总离散间隔数及总水声信号传递时间采样数。根据图2、3、4以及三种方法的平均均方定位误差、声速平均均方误差，可以看出本发明所提出的方法可以获得比传统水下单信标定位方法更好的结果。相比于基于声速不确定性Gauss分布的水下单信标定位方法(传统方法1)，本发明所提出方法可以更好的跟踪水声声速变化的趋势，进而也可以得到更好的定位结果。

实施例3，本发明的算法伪代码总结为：

虽然，上文中已经用一般性说明及具体实施例对本发明作了详尽的描述，但在本发明基础上，可以对之作一些修改或改进，这对本领域技术人员而言是显而易见的。因此，在不偏离本发明精神的基础上所做的这些修改或改进，均属于本发明要求保护的范围。

Claims (8)

1.一种可估计未知有效声速的水下单信标定位方法，其特征在于，包括以下步骤：

A.以定位区域内任意点为原点，东、北、天三个方向分别设为x，y，z轴，建立水下局部惯性坐标系；

B.通过水下航行器所搭载的GPS系统获取该所述水下航行器在水下局部惯性系当中的初始位置；

C.建立所述水下航行器的运动学模型以及观测模型并进行离散化；

D.建立有效声速的随机模型、函数模型以及预测模型；

E.水声信标周期性广播水声信号，水声信号发射时间及水声信标位置已知；所述水下航行器在未接收到水声信号时，通过自身配备的电子罗盘、深度计以及读取自身的螺旋桨转速信息进行航位推算，同时进行有效声速随机模型参数预测；所述水下航行器在接收到所搭载的多普勒测速仪测得的的绝对速度观测后，通过读取所述螺旋桨转速信息及所述电子罗盘信息，构造海流速度观测量并通过Kalman滤波进行海流速度校正；

F.所述水下航行器接收到水声信号后，记录接收时刻，根据已知的水声信号发射时刻及水声信标位置坐标，并考虑水下声速的未知性，以此基于扩展Kalman滤波算法及变分贝叶斯近似，以水声信号传递时间为观测变量，进行所述水下航行器的位置更新。

2.如权利要求1所述的一种可估计未知有效声速的水下单信标定位方法，其特征在于，所述C步骤中，所述运动学模型的建立方法为：

定义状态向量为：

x＝[x y v_cx v_cy]^T

其中：x，y为所述水下航行器在所述水下局部惯性坐标系中的水平位置；v_cx，v_cy为未知的海流速度；

对x求导并加入所述水下航行器运动运动模型噪声影响，得到所述水下航行器的运动学模型：

其中：v_wx为所述水下航行器x方向的对水速度；v_wy为所述水下航行器y方向的对水速度；v_wx及v_wy通过读取所述螺旋桨转速与所述电子罗盘测得的航行器艏向角计算得出；ω_x为所述水下航行器在x方向的位置不确定性；ω_y为所述水下航行器在y方向的位置不确定性；ω_cx为x方向的海流不确定性；ω_cy为y方向的海流不确定性；

v_wx及v_wy的计算公式为：

式中v_w为根据所述螺旋桨转速得出的所述水下航行器对水速度，

为所述电子罗盘测得的艏向角。

3.如权利要求2所述的一种可估计未知有效声速的水下单信标定位方法，其特征在于，所述C步骤中，所述观测模型的建立方法为：

S1.建立水声信号传递时间的观测模型；

设所述水下航行器获得所述水声信标发射水声信号的时刻为T_e，所述水声信标在所述水下局部惯性坐标系中的空间位置坐标为X_Te，Y_Te，Z_Te，所述水下航行器接收到该水声信号的时刻为T_a，观测方程为：

其中：ν_t为对应的观测噪声；z为所述水下航行器的深度，由深度计精确测得，为已知量；v_e为有效声速；

将观测方程记作m＝h(x,v_e)，其中：

S2.建立海流流速观测模型；

根据所述多普勒测速仪测得的所述水下航行器的绝对速度v_g，结合所述电子罗盘测得的艏向角计算得到所述水下航行器绝对速度在局部惯性坐标系下的分量v_gx,v_gy；

根据v_gx,v_gy以及v_wx,v_wy，计算得到海流速度观测分量为：

海流观测量为线性，满足m_vc＝Hx+ν_vc；

其中：观测向量m_vc＝[m_cx m_cy]^T；m_cx，m_cy分别为x，y方向海流速度观测；ν_vc为海流观测噪声向量，ν_vc＝[ν_v,cx ν_v,cy]^T，其中ν_v,cx为x方向的海流不确定性；ν_v,cy为y方向的海流不确定性；H为海流观测矩阵，满足：

4.如权利要求3所述的一种可估计未知有效声速的水下单信标定位方法，其特征在于，所述C步骤中，所述运动学模型以及观测模型离散化方法为：

S1.运动学模型离散化；

以下标k为时间索引，以Δt＝t_k+1-t_k为离散间隔，运动学模型离散为：

x_k+1＝A_kx_k+B_ku_k+w_k

其中：A_k为运动学方程，满足：

B_k为控制方程，满足：

u_k为控制向量，满足u_k＝[v_wx,k v_wy,k]^T，为已知量；

w_k过程噪声向量，满足w_k＝[ω_x,k ω_y,k ω_cx,k ω_cy,k]^T，对应各个状态变量的不确定性；将系统状态x_k,y_k,v_cx,k,v_cy,k的过程噪声建模为零均值Gauss分布，其协方差矩阵满足：

其中，σ_w为所述水下航行器对水速度观测不确定性的标准差；σ_c为海流不确定性的标准差；

S2.观测模型离散化；

所述水下航行器在k-1至k间接收到该水声信号，将其假设为在k时刻接收到该水声信号，即离散后的水声信号传递时间观测方程为：

其中，ν_t,k为观测噪声，假设其满足方差为R_t,k的Gauss分布；考虑到有效声速v_e,k的时变未知性，将v_e,k也看作随机变量；离散形式的观测方程写作：

m_k＝h_k(x_k,v_e,k)，

由于海流观测的采样频率较高，假设在每一个离散时间点k处均可以得到海流速度观测，故离散后的海流速度观测方程为：

m_vc，k＝H_kx_k+ν_vc,k

其中，H_k为k时刻海流速度观测矩阵，满足：

ν_vc,k为k时刻海流观测噪声，为零均值Gauss分布，其观测噪声协方差矩阵记作：

其中，σ_vc,m为海流速度观测噪声的标准差。

5.如权利要求4所述的一种可估计未知有效声速的水下单信标定位方法，其特征在于，所述D步骤中，有效声速的随机模型、函数模型以及预测模型建立方法为：

考虑到恶劣海况下有效声速不确定性的非Gauss性，将有效声速初始先验分布建模为Student’s t分布：

其中：St(x|a,b,c)表示以a为均值，b为尺度参数，c为自由度，满足Student’s t分布的随机变量x；

为初始声速估计均值；

为初始声速不确定性尺度参数；v₀为初始声速自由度参数；

将k-1时刻有效声速的后验分布建模为Student’s t分布：

其中：m_1:k表示从1到k时刻的观测量的集合；p(x|y)表示随机变量x以y为条件的概率密度函数；

v_k-1分别为k-1时刻有效声速的后验均值、尺度参数及自由度；

有效声速的动态扩散的函数模型记作：

v_e,k＝v_e,k-1+ω_e,k-1

其中：ω_e,k-1为有效声速的不确定性，同样满足Student’s t分布

p(ω_e,k-1)＝St(ω_e,k-1|0,Q_e,k-1,v₁)

其中Q_e,k-1与v₁分别为有效声速不确定性的尺度参数与自由度；将声速不确定性自由度建模为与k-1时刻有效声速后验自由度相等，即v₁＝v_k-1；据此，将有效声速的概率扩散方程记作：

p(v_e,k|v_e,k-1)＝St(v_e,k|v_e,k-1,Q_e,k-1,v_k-1)

结合上式，获得有效声速的预测模型为：

其中：

v_k分别为k时刻有效声速的先验均值、尺度参数及自由度，其计算公式为：

v_k＝v_k-1

根据启发式Gauss模型，将有效声速的先验概率密度函数分解为：

p(α_k)＝G(α_k|v_k/2,v_k/2)

其中：N(x|μ,Σ)表示以μ为均值，Σ为协方差矩阵，满足Gauss分布的随机向量x；G(x|a,b)表示以a,b为参数，满足Gama分布的随机变量x；α_k为辅助参数。

6.如权利要求5所述的一种可估计未知有效声速的水下单信标定位方法，其特征在于，所述E步骤中，所述水下航行器利用自身配备的电子罗盘、深度计以及读取自身的螺旋桨转速信息进行航位推算的方法为：

根据Kalman滤波的预测环节，将系统状态的先验分布近似为Gauss分布：

其中：

与P_k|k-1分别为k时刻的先验状态和先验方差，其计算方法为：

其中：

与P_k-1|k-1分别为k-1时刻的后验状态和后验方差。

7.如权利要求6所述的一种可估计未知有效声速的水下单信标定位方法，其特征在于，所述E步骤中，所述水下航行器接收到所述多普勒测速仪测得的绝对速度观测后，进行海流速度校正的方法为：

P_k|k＝P_k|k-1-K_kH_kP_k|k-1

其中：K_k为海流更新的Kalman增益。

8.如权利要求7所述的一种可估计未知有效声速的水下单信标定位方法，其特征在于，所述步骤F中，通过扩展Kalman滤波及变分贝叶斯近似进行水声信号传递时间校正的方法为：

S1.建立观测似然函数及有效声速后验概率密度分布模型；

根据离散形式的水声信号传递时间观测模型，得到水声信号传递时间的观测似然密度为：

p(m_k|v_e,k,x_k)＝N(m_k|h_k(x_k,v_e,k),R_k)

将有效声速后验分布建模为Student’s t分布，并分解为：

S2.定义状态变量及有效声速后验估计近似值；

通过变分贝叶斯近似，将有效声速，系统状态以及辅助参数的联合后验估计近似为：

p(x_k,v_e,k,α_k|m_1:k)≈q(x_k)q(v_e,k)q(α_k)

以最小化近似前与近似后两个概率密度函数之间的Kullback-Leibler散度(KLD)最小化为目标，得到近似解为：

其中：log(·)表示对数函数；θ表示x_k,v_e,k,α_k中的任意元素；E_x[·]表示相对于x的期望；上标(-θ)表示整个集合当中除了θ以外的其它元素；c_θ表示与θ无关的常数；采用固定点迭代来求解q(θ)；

S3.求解状态变量及有效声速后验估计近似值；

S3.1求解联合概率密度对数值；

其对数形式表示为：

S3.2求解辅助参数α_k估计值；

令θ＝α_k，得到：

其中变量加上标(i)表示该变量在第i次迭代中的估计值；

进而得到：

取α_k在第i+1次迭代中的估计值为期望值，即：

S3.3求解有效声速v_e,k估计值；

以

为展开点，对非线性的水声信号传递时间观测方程m_k＝h_k(x_k,v_e,k)进行线性化，只保留一阶项，得到：

其中：

为观测方程的雅克比矩阵；

由此得到线性化后的水声信号传递时间观测方程为：

令θ＝v_e,k，得到：

其中：

根据Kalman滤波更新式，得到有效声速更新方程为：

其中：

为第i+1次迭代中有效声速更新的Kalman增益；

S3.4求解系统状态x_k估计值；

以

为展开点，对非线性的水声信号传递时间观测方程m_k＝h_k(x_k,v_e,k)进行线性化，只保留一阶项，得到：

其中：

为观测方程的雅克比矩阵；

由此得到线性化后的水声信号传递时间观测方程为：

令θ＝x_k，得到：

其中：

根据Kalman滤波更新式，得到系统状态更新方程为：

其中：

为第i+1次迭代中系统状态更新的Kalman增益；

记总迭代次数为N，则最终有效声速均值及尺度参数、系统状态均值及协方差矩阵的估计值分别为：

Images