本发明属于水下定位技术领域，特别涉及一种水下航行器的定位方法。水声信标周期性广播水声信号；水下航行器在未接收到水声信号时，通过自身配备的电子罗盘、深度计以及读取自身的螺旋桨转速信息进行航位推算，并在接收到所搭载的多普勒测速仪测得的的绝对速度观测后，构造海流速度观测量并通过Kalman滤波进行海流速度校正；水下航行器接收到水声信号后，考虑水下声速的未知性、水声信标位置误差及水声信号收发端的时钟漂移，以此基于扩展Kalman滤波算法及期望最大化算法，以水声信号传递时间为观测变量，进行水下航行器的位置更新。本发明可以保证水下航行器在存在时钟漂移、信标位置及声速设置误差的情况下仍然得到理想的定位性能。
1.一种水下航行器的水下信标定位方法，其特征在于，包括以下步骤：A.以定位区域内任意点为原点，东、北、天三个方向分别设为x，y，z轴，建立水下局部惯性坐标系；
B.通过所述水下航行器所搭载的GPS系统获取该所述水下航行器在水下局部惯性系当中的初始位置；
C.建立所述水下航行器的运动学模型以及观测模型并进行离散化；
x＝[x y vcx vcy]其中：x，y为所述水下航行器在所述水下局部惯性坐标系中的水平位置；vcx，vcy为未知的海流速度；
对x求导并加入所述水下航行器运动运动模型噪声影响，得到所述水下航行器的运动学模型：
其中：vwx为所述水下航行器x方向的对水速度；vwy为所述水下航行器y方向的对水速度；
vwx及vwy通过读取螺旋桨转速与电子罗盘测得的航行器艏向角计算得出；ωx为所述水下航行器在x方向的位置不确定性；ωy为所述水下航行器在y方向的速度不确定性；ωcx为x方向的海流不确定性；ωcy为x方向的海流不确定性；
vwx及vwy的计算公式为： 式中vw为根据所述螺旋桨转速得出的所述水下航行器对水速度， 为所述电子罗盘测得的艏向角；
所述观测模型的建立方法为：S1.建立水声信号传递时间的观测模型；
设所述水下航行器获得所述水声信标发射水声信号的时刻为Te，所述水声信标在所述水下局部惯性坐标系中的空间位置坐标为XTe，YTe，ZTe，所述水下航行器接收到该水声信号的时刻为Ta，观测方程为：其中：vt为对应的观测噪声；z为所述水下航行器的深度，ZTe为所述水声信标的深度，z与ZTe均由深度计精确测得为已知量；ΔTt为水声信号收发端的时钟漂移，ve为有效声速，在水声声速、所述水声信标位置坐标及水声信号收发端时钟漂移未知时，XTe，YTe，ΔTt，ve均为T
未知量；将XTe，YTe，ΔTt，ve看作未知的系统参数，定义参数集为θ＝[XTe YTeΔTt ve] ，则观测方程记作Tt＝h(x,θ)，其中，S2.建立海流流速观测模型；
根据多普勒测速仪测得的所述水下航行器的绝对速度vg，结合所述电子罗盘测得的艏向角 计算得到所述水下航行器绝对速度在局部惯性坐标系下的分量vgx,vgy；
根据vgx,vgy以及vwx,vwy，计算得到海流速度分量为：海流观测量为线性，满足m＝Hx+vvc；
其中：观测向量m＝[mcx mcy] ；mcx，mcy分别为x，y方向海流速度观测；vvc为海流观测噪声T
向量，vvc＝[vcx vcy] ，其中vcx为x方向的海流不确定性；vcy为y方向的海流不确定性；H为海流观测矩阵，满足：
所述运动学模型以及观测模型离散化方法为：S1.运动学模型离散化；
以符号k为时间索引，以Δt＝tk+1‑tk为离散间隔，运动学模型离散为：xk+1＝Akxk+Bkuk+wk其中：Ak为运动学方程，满足：Bk为控制方程，满足：
uk为控制向量，满足uk＝[vwx,k vwy,k]，为已知量；
wk过程噪声向量，满足wk＝[ωx,k ωy,k ωcx,k ωcy,k]；
对应各个状态变量的不确定性，过程噪声协方差矩阵满足：其中，σw为所述水下航行器对水速度观测不确定性的标准差；σc为海流不确定性的标准差；
所述水下航行器在k‑1至k间接收到该水声信号，将其假设为在k时刻接收到该水声信号，即离散后的水声信号传递时间观测方程为：其中，vt,k为观测噪声，假设其满足方差为Rt,k的Gauss分布；考虑到XTe YTe ΔTt,k ve,k的T
时变未知性，记未知参数集θk＝[XTe YTe ΔTt,k ve,k]，观测方程可以记作：mt,k＝hk(xk,θk)，假设在每一个离散时间点k处均可以得到观测，故离散后观测方程为：mvc，k＝Hkxk+vvc,k其中，Hk为k时刻速度与加速度观测矩阵，满足：vvc,k为k时刻海流观测噪声，为零均值Gauss分布，其观测噪声协方差矩阵满足：2
D.所述水声信标周期性广播水声信号，水声信号发射时间及水声信标位置已知，但存在一定偏差；所述水下航行器在未接收到水声信号时，通过自身配备的电子罗盘、深度计以及读取自身的螺旋桨转速信息进行航位推算，并在接收到所搭载的多普勒测速仪测得的的绝对速度观测后，通过读取所述螺旋桨转速信息及所述电子罗盘信息，构造海流速度观测量并通过Kalman滤波进行海流速度校正；
所述水下航行器利用自身配备的电子罗盘、深度计以及读取自身的螺旋桨转速信息进行航位推算的方法为：
其中： 与Pk‑1|k‑1分别为k‑1时刻的后验状态和后验方差； 与Pk|k‑1分别为k时刻的先验状态和先验方差；
所述水下航行器接收到所述多普勒测速仪测得的绝对速度观测后，进行海流速度校正的方法为：
Pk|k＝Pk|k‑1‑KkHkPk|k‑1其中：Kk为Kalman增益；
E.所述水下航行器接收到水声信号后，记录接收时刻，根据已知的水声信号发射时刻及水声信标位置坐标，并考虑水下声速的未知性、水声信标位置误差及水声信号收发端的时钟漂移，以此基于扩展Kalman滤波算法及期望最大化算法，以水声信号传递时间为观测变量，进行所述水下航行器的位置更新；
通过扩展Kalman滤波及期望最大化算法进行水声信号传递时间校正的方法为：S1.期望最大化算法求期望；
其中： 表示θk在第l次迭代中的近似值；Ex[·]表示相对于x的期望； 表示随机变量a的概率密度函数，函数与 相关；m1：k表示从1到k时刻的观测量；p(x|y)表示随机变量x以y为条件的条件期望；
在已知 与m1:k的前提下，定义下一次迭代的参数估计值为：假设共进行N次迭代，则最终参数估计解为：S1.3求解联合概率密度；
由于系统观测噪声及过程噪声均为Gauss白噪声，系统联合概率密度可以分解为：由于运动学模型为线性，当k‑1时刻系统后验为Gauss分布时，k时刻的先验分布也满足Gauss分布，即：其中，N(x；μ,Σ)表示以μ为均值向量，以Σ为方差矩阵，满足Gauss分布的随机变量x；
对非线性的水声信号传递时间观测方程mt,k＝hk(xk,θk)进行线性化，只保留一阶项，得到：
由此得到线性化后的水声信号传递时间观测方程为：在已知 与m1:k的前提下，记状态后验估计为：选择线性化展开点 相应的雅克比矩阵为：其中：
根据扩展Kalman滤波流程，可以得到后验估计参数为：S1.5求解目标函数；
选择 作为线性展开点，根据 以及 得到：其中： 表示与θk无关的常数；
其中： 表示所述水声信标位置坐标与水声声速的名义值，在迭代过程中一直保持不变，通过更新水声信号收发端的时钟漂移ΔTt,k来间接补偿信标位置坐标与水声声速设置误差带来的影响。
一种水下航行器的水下信标定位方法
技术领域
[0001] 本发明属于水下定位技术领域，特别涉及一种水下航行器的定位方法。
背景技术
[0002] 精确的位置反馈是水下航行器完成既定水下任务的基础。由于水下电磁波信号衰减较快，广泛应用于陆地与天空定位的GNSS系统在水下无法应用。现有主流的水下定位方
式包括以惯性导航为代表的航位推算方法以及以长基线定位为代表的水下声学定位方法。
其中惯性导航设备往往会随时间增长产生较大累计误差，无法长时间用于水下定位，而高
精度的惯性导航设备成本极高，限制了其在水下航行器中的应用。现有主流的的水下声学
定位方式包括长基线定位、超短基线定位、单信标定位等。长基线定位与超短基线定位发展
均较为成熟，但其成本通常较高，且实时性通常较差，这限制了其在水下航行器中的应用。
而新兴的水下单信标定位系统融合航位推算数据与单水声信标的测距信息，在定位成本和
实时性方面均有较大的优势。但目前的单信标定位系统均基于水声信号收发端的精确时钟
同步，且要求水声信标的位置坐标及水声声速完全已知。但实际应用中，水声信标的位置通
常是通过在任务执行前利用水面船进行多点测距获得，信标位置通常会存在校正误差；水
声声速收到水下温度、盐度、密度等因素的影响，通常为时变未知的，精确的水声声速难以
得到；而纵使安装高精度的原子钟，水声信号收发端仍会存在随着时间累计的时钟漂移，对
于在水下长时间执行任务的航行器而言，由时钟漂移造成的测距误差不可忽视。以上的实
际三个问题均会引起水声测距偏差，进而导致定位误差，恶化单信标定位系统性能。
发明内容
[0003] 本发明的目的是：针对水下单信标定位当中水声声速的未知性、水下信标位置偏差及水声信号收发端的时钟漂移的问题，基于期望最大化方法提出一种水下航行器的水下
信标定位方法。
[0004] 本发明的技术方案是：水下航行器搭载水听器、多普勒测速仪、深度计、电子罗盘及GPS；水声信标周期性广播水声信号，水下航行器通过所搭载的多普勒测速仪周期性的观
测其绝对速度；水下航行器通过GPS获取初始位置，并且通过读取自身螺旋桨转速获得航行
器与水的相对速度。水下航行器未接收到水声信号时通过自身携带的电子罗盘及读取自身
螺旋桨转速信息进行航位推算；水下航行器在接收到多普勒测速仪提供的绝对速度观测
时，通过结合螺旋桨转速信息及电子罗盘信息构造海流观测变量，基于Kalman滤波进行海
流速度更新；水下航行器接收水声信号后记录信号到达时间，并通过期望最大化方法及扩
展Kalman滤波进行定位位置校准。本发明的步骤包括：
[0005] 一种水下航行器的水下信标定位方法，包括以下步骤：
[0006] A.以定位区域内任意点为原点，东、北、天三个方向分别设为x，y，z轴，建立水下局部惯性坐标系；
[0007] B.通过水下航行器所搭载的GPS系统获取该水下航行器在水下局部惯性系当中的初始位置；
[0008] C.建立水下航行器的运动学模型以及观测模型并进行离散化；
[0009] D.水声信标周期性广播水声信号，水声信号发射时间及水声信标位置已知，但存在一定偏差；水下航行器在未接收到水声信号时，通过自身配备的电子罗盘、深度计以及读
取自身的螺旋桨转速信息进行航位推算，并在接收到所搭载的多普勒测速仪测得的的绝对
速度观测后，通过读取螺旋桨转速信息及电子罗盘信息，构造海流速度观测量并通过
Kalman滤波进行海流速度校正；
[0010] E.水下航行器接收到水声信号后，记录接收时刻，根据已知的水声信号发射时刻及水声信标位置坐标，并考虑水下声速的未知性、水声信标位置误差及水声信号收发端的
时钟漂移，以此基于扩展Kalman滤波算法及期望最大化算法，以水声信号传递时间为观测
变量，进行水下航行器的位置更新。
[0011] 具体的，上述C步骤中，运动学模型的建立方法为：
[0012] 定义状态向量为：
[0013] x＝[x y vcx vcy]T
[0014] 其中：x，y为水下航行器在水下局部惯性坐标系中的水平位置；vcx，vcy为未知的海流速度；
[0015] 对x求导并加入水下航行器运动运动模型噪声影响，得到水下航行器的运动学模型：
[0016]
[0017] 其中：vwx为水下航行器x方向的对水速度；vwy为水下航行器y方向的对水速度；vwx及vwy通过读取螺旋桨转速与电子罗盘测得的航行器艏向角计算得出；ωx为水下航行器在x
方向的位置不确定性；ωy为水下航行器在y方向的速度不确定性；ωcx为x方向的海流不确
定性；ωcy为x方向的海流不确定性；
[0018] vwx及vwy的计算公式为： 式中vw为根据螺旋桨转速得出的水下航行器对水速度， 为电子罗盘测得的艏向角。
[0019] 具体的，上述C步骤中，观测模型的建立方法为：
[0020] S1.建立水声信号传递时间的观测模型；
[0021] 设水下航行器获得水声信标发射水声信号的时刻为Te，水声信标在水下局部惯性坐标系中的空间位置坐标为XTe，YTe，ZTe，水下航行器接收到该水声信号的时刻为Ta，观测方
程为：
[0022]
[0023] 其中：vt为对应的观测噪声；z为水下航行器的深度，ZTe为水声信标的深度，z与ZTe均由深度计精确测得为已知量；ΔTt为水声信号收发端的时钟漂移，ve为有效声速，在水声
声速、水声信标位置坐标及水声信号收发端时钟漂移未知时，XTe，YTe，ΔTt，ve均为未知量；
T
将XTe，YTe，ΔTt，ve看作未知的系统参数，定义参数集为θ＝[XTe YTe ΔTt ve] ，则观测方程
记作Tt＝h(x,θ)，其中，
[0024] S2.建立海流流速观测模型；
[0025] 根据多普勒测速仪测得的水下航行器的绝对速度vg，结合电子罗盘测得的艏向角计算得到水下航行器绝对速度在局部惯性坐标系下的分量vgx,vgy；
[0026] 根据vgx,vgy以及vwx,vwy，计算得到海流速度分量为：
[0027]
[0028] 海流观测量为线性，满足m＝Hx+vvc；
[0029] 其中：观测向量m＝[mcx mcy]T；mcx，mcy分别为x，y方向海流速度观测；vvc为海流观测T
噪声向量，vvc＝[vcx vcy] ，其中vcx为x方向的海流不确定性；vcy为y方向的海流不确定性；H
为海流观测矩阵，满足：
[0030]
[0031] 具体的，上述C步骤中，运动学模型以及观测模型离散化方法为：
[0032] S1.运动学模型离散化；
[0033] 以符号k为时间索引，以Δt＝tk+1‑tk为离散间隔，运动学模型离散为：
[0034] xk+1＝Akxk+Bkuk+wk
[0035] 其中：Ak为运动学方程，满足：
[0036]
[0037] Bk为控制方程，满足：
[0038]
[0039] uk为控制向量，满足uk＝[vwx,k vwy,k]T，为已知量；
[0040] wk过程噪声向量，满足wk＝[ωx,k ωy,k ωcx,k ωcy,k]T；
[0041] 对应各个状态变量的不确定性，过程噪声协方差矩阵满足：
[0042]
[0043] 其中，σw为水下航行器对水速度观测不确定性的标准差；σc为海流不确定性的标准差；
[0044] S2.观测模型离散化；
[0045] 水下航行器在k‑1至k间接收到该水声信号，将其假设为在k时刻接收到该水声信号，即离散后的水声信号传递时间观测方程为：
[0046]
[0047] 其中，vt,k为观测噪声，假设其满足方差为Rt,k的Gauss分布(标准差记作σt,m)；考虑T
到XTe YTe ΔTt,k ve,k的时变未知性，记未知参数集θk＝[XTe YTe ΔTt,k ve,k] ，观测方程可
以记作mt,k＝hk(xk,θk)，
[0048]
[0049] 由于多普勒测速仪、电子罗盘、航行器螺旋桨转速采样频率较高，假设在每一个离散时间点k处均可以得到观测，故离散后观测方程为：
[0050] mvc，k＝Hkxk+vvc,k
[0051] 其中，Hk为k时刻速度与加速度观测矩阵，满足：
[0052]
[0053] vvc,k为k时刻海流观测噪声，为零均值Gauss分布，其观测噪声协方差矩阵满足：
[0054]
[0055] 其中，σvc,m2为海流速度观测噪声的标准差。
[0056] 具体的，上述D步骤中，水下航行器利用自身配备的电子罗盘、深度计以及读取自身的螺旋桨转速信息进行航位推算的方法为：
[0057]
[0058]
[0059] 其中： 与Pk‑1|k‑1分别为k‑1时刻的后验状态和后验方差(航行器进行更新后的导航状态参数)； 与Pk|k‑1分别为k时刻的先验状态和先验方差(航行器未进行更新时
的导航状态参数)。
[0060] 具体的，上述D步骤中，水下航行器接收到多普勒测速仪测得的绝对速度观测后，进行海流速度校正的方法为：
[0061]
[0062]
[0063] Pk|k＝Pk|k‑1‑KkHkPk|k‑1
[0064] 其中：Kk为Kalman增益。
[0065] 具体的，上述步骤E中，通过扩展Kalman滤波及期望最大化算法进行水声信号传递时间校正的方法为：
[0066] S1.期望最大化算法求期望；
[0067] S1.1定义目标函数；
[0068]
[0069] 其中： 表示θk在第l次迭代中的近似值；Ex[·]表示相对于x的期望； 表示随机变量a的概率密度函数，函数与 相关；m1:k表示从1到k时刻的观测量；p(x|y)表示随
机变量x以y为条件的条件期望；
[0070] S1.2定义参数估计值解；
[0071] 在已知 与m1:k的前提下，定义下一次迭代的参数估计值为：
[0072]
[0073] 假设共进行N次迭代，则最终参数估计解为：
[0074]
[0075] S1.3求解联合概率密度；
[0076] 由于系统观测噪声及过程噪声均为Gauss白噪声，系统联合概率密度可以分解为：由于运动学模型为线性，当k‑1时刻系
统后验为Gauss分布时，k时刻的先验分布也满足Gauss分布，即：
[0077]
[0078] 其中，N(x；μ,Σ)表示以μ为均值向量，以Σ为方差矩阵，满足Gauss分布的随机变量x；先验参数 与Pk|k‑1由所述步骤5当中的航位推算流程得出；
[0079] S1.4求解后验状态分布；
[0080] 对非线性的水声信号传递时间观测方程mt,k＝hk(xk,θk)进行线性化，只保留一阶项，得到：
[0081]
[0082] 其中：
[0083] 为观测方程的雅克比矩阵；
[0084] 由此得到线性化后的水声信号传递时间观测方程为：
[0085]
[0086] 在已知 与m1:k的前提下，记状态后验估计为：
[0087]
[0088] 选择线性化展开点 相应的雅克比矩阵为：
[0089]
[0090] 其中：
[0091]
[0092] 根据扩展Kalman滤波流程，可以得到后验估计参数为：
[0093]
[0094]
[0095]
[0096] S1.5求解目标函数；
[0097] 选择 作为线性展开点，根据 以及 得到：
[0098]
[0099] 其中： 表示与θk无关的常数；
[0100]
[0101]
[0102]
[0103]
[0104] 根据以上关系，将Fk简化为：
[0105]
[0106] 其中：
[0107]
[0108] S2.期望最大化算法求极值步骤；
[0109] 为 的极大值的必要条件为：
[0110]
[0111] 计算得到：
[0112]
[0113] 当 时，以上极值条件满足；
[0114] 令：
[0115]
[0116] 其中： 表示水声信标位置坐标与水声声速的名义值，在迭代过程中一直保持不变，通过更新水声信号收发端的时钟漂移ΔTt,k来间接补偿信标位置坐标与水声
声速设置误差带来的影响。
[0117] 通过以上参数更新方案，可以保证期望最大化方法的极值条件满足。
[0118] 有益效果：本发明通过结合Kalman滤波、扩展Kalman滤波与期望最大化方法，将未知的水声声速、水声信标位置坐标与水声信号收发端的时钟漂移看作未知系统参数，实现
了导航状态与系统参数的同时估计，可以保证水下航行器在存在时钟漂移、信标位置及声
速设置误差的情况下仍然得到理想的定位性能。
附图说明
[0119] 图1为本发明的步骤流程图；
[0120] 图2、图3为利用本发明进行实测数据数值验证的结果。
具体实施方式
[0121] 实施例1，参见附图1：一种水下航行器的水下信标定位方法，包括以下步骤：
[0122] A.以定位区域内任意点为原点，东、北、天三个方向分别设为x，y，z轴，建立水下局部惯性坐标系；
[0123] B.通过水下航行器所搭载的GPS系统获取该水下航行器在水下局部惯性系当中的初始位置；
[0124] C.建立水下航行器的运动学模型以及观测模型并进行离散化；
[0125] 运动学模型的建立方法为：
[0126] 定义状态向量为：
[0127] x＝[x y vcx vcy]T
[0128] 其中：x，y为水下航行器在水下局部惯性坐标系中的水平位置；vcx，vcy为未知的海流速度；
[0129] 对x求导并加入水下航行器运动运动模型噪声影响，得到水下航行器的运动学模型：
[0130]
[0131] 其中：vwx为水下航行器x方向的对水速度；vwy为水下航行器y方向的对水速度；vwx及vwy通过读取螺旋桨转速与电子罗盘测得的航行器艏向角计算得出；ωx为水下航行器在x
方向的位置不确定性；ωy为水下航行器在y方向的速度不确定性；ωcx为x方向的海流不确
定性；ωcy为x方向的海流不确定性；
[0132] vwx及vwy的计算公式为： 式中vw为根据螺旋桨转速得出的水下航行器对水速度， 为电子罗盘测得的艏向角；
[0133] 观测模型的建立方法为：
[0134] S1.建立水声信号传递时间的观测模型；
[0135] 设水下航行器获得水声信标发射水声信号的时刻为Te，水声信标在水下局部惯性坐标系中的空间位置坐标为XTe，YTe，ZTe，水下航行器接收到该水声信号的时刻为Ta，观测方
程为：
[0136]
[0137] 其中：vt为对应的观测噪声；z为水下航行器的深度，ZTe为水声信标的深度，z与ZTe均由深度计精确测得为已知量；ΔTt为水声信号收发端的时钟漂移，ve为有效声速，在水声
声速、水声信标位置坐标及水声信号收发端时钟漂移未知时，XTe，YTe，ΔTt，ve均为未知量；
T
将XTe，YTe，ΔTt，ve看作未知的系统参数，定义参数集为θ＝[XTe YTe ΔTt ve] ，则观测方程
记作Tt＝h(x,θ)，其中，
[0138] S2.建立海流流速观测模型；
[0139] 根据多普勒测速仪测得的水下航行器的绝对速度vg，结合电子罗盘测得的艏向角计算得到水下航行器绝对速度在局部惯性坐标系下的分量vgx,vgy；
[0140] 根据vgx,vgy以及vwx,vwy，计算得到海流速度分量为：
[0141]
[0142] 海流观测量为线性，满足m＝Hx+vvc；
[0143] 其中：观测向量m＝[mcx mcy]T；mcx，mcy分别为x，y方向海流速度观测；vvc为海流观测T
噪声向量，vvc＝[vcx vcy] ，其中vcx为x方向的海流不确定性；vcy为y方向的海流不确定性；H
为海流观测矩阵，满足：
[0144]
[0145] 运动学模型以及观测模型离散化方法为：
[0146] S1.运动学模型离散化；
[0147] 以符号k为时间索引，以Δt＝tk+1‑tk为离散间隔，运动学模型离散为：
[0148] xk+1＝Akxk+Bkuk+wk
[0149] 其中：Ak为运动学方程，满足：
[0150]
[0151] Bk为控制方程，满足：
[0152]
[0153] uk为控制向量，满足uk＝[vwx,k vwy,k]T，为已知量；
[0154] wk过程噪声向量，满足wk＝[ωx,k ωy,k ωcx,k ωcy,k]T；
[0155] 对应各个状态变量的不确定性，过程噪声协方差矩阵满足：
[0156]
[0157] 其中，σw为水下航行器对水速度观测不确定性的标准差；σc为海流不确定性的标准差；
[0158] S2.观测模型离散化；
[0159] 水下航行器在k‑1至k间接收到该水声信号，将其假设为在k时刻接收到该水声信号，即离散后的水声信号传递时间观测方程为：
[0160]
[0161] 其中，vt,k为观测噪声，假设其满足方差为Rt,k的Gauss分布(标准差记作σt,m)；考虑T
到XTe YTe ΔTt,k ve,k的时变未知性，记未知参数集θk＝[XTe YTe ΔTt,k ve,k] ，观测方程可
以记作mt,k＝hk(xk,θk)，
[0162]
[0163] 由于多普勒测速仪、电子罗盘、航行器螺旋桨转速采样频率较高，假设在每一个离散时间点k处均可以得到观测，故离散后观测方程为：
[0164] mvc，k＝Hkxk+vvc,k
[0165] 其中，Hk为k时刻速度与加速度观测矩阵，满足：
[0166]
[0167] vvc,k为k时刻海流观测噪声，为零均值Gauss分布，其观测噪声协方差矩阵满足：
[0168]
[0169] 其中，σvc,m2为海流速度观测噪声的标准差；
[0170] D.水声信标周期性广播水声信号，水声信号发射时间及水声信标位置已知，但存在一定偏差；水下航行器在未接收到水声信号时，通过自身配备的电子罗盘、深度计以及读
取自身的螺旋桨转速信息进行航位推算，并在接收到所搭载的多普勒测速仪测得的的绝对
速度观测后，通过读取螺旋桨转速信息及电子罗盘信息，构造海流速度观测量并通过
Kalman滤波进行海流速度校正；
[0171] 水下航行器利用自身配备的电子罗盘、深度计以及读取自身的螺旋桨转速信息进行航位推算的方法为：
[0172]
[0173]
[0174] 其中： 与Pk‑1|k‑1分别为k‑1时刻的后验状态和后验方差(航行器进行更新后的导航状态参数)； 与Pk|k‑1分别为k时刻的先验状态和先验方差(航行器未进行更新时
的导航状态参数)；
[0175] 水下航行器接收到多普勒测速仪测得的绝对速度观测后，进行海流速度校正的方法为：
[0176]
[0177]
[0178] Pk|k＝Pk|k‑1‑KkHkPk|k‑1
[0179] 其中：Kk为Kalman增益。
[0180] E.水下航行器接收到水声信号后，记录接收时刻，根据已知的水声信号发射时刻及水声信标位置坐标，并考虑水下声速的未知性、水声信标位置误差及水声信号收发端的
时钟漂移，以此基于扩展Kalman滤波算法及期望最大化算法，以水声信号传递时间为观测
变量，进行水下航行器的位置更新；
[0181] 通过扩展Kalman滤波及期望最大化算法进行水声信号传递时间校正的方法为：
[0182] S1.期望最大化算法求期望；
[0183] S1.1定义目标函数；
[0184]
[0185] 其中： 表示θk在第l次迭代中的近似值；Ex[·]表示相对于x的期望； 表示随机变量a的概率密度函数，函数与 相关；m1:k表示从1到k时刻的观测量；p(x|y)表示随机
变量x以y为条件的条件期望；
[0186] S1.2定义参数估计值解；
[0187] 在已知 与m1:k的前提下，定义下一次迭代的参数估计值为：
[0188]
[0189] 假设共进行N次迭代，则最终参数估计解为：
[0190]
[0191] S1.3求解联合概率密度；
[0192] 由于系统观测噪声及过程噪声均为Gauss白噪声，系统联合概率密度可以分解为：由于运动学模型为线性，当k‑1时刻系
统后验为Gauss分布时，k时刻的先验分布也满足Gauss分布，即：
[0193]
[0194] 其中，N(x；μ,Σ)表示以μ为均值向量，以Σ为方差矩阵，满足Gauss分布的随机变量x；先验参数 与Pk|k‑1由所述步骤5当中的航位推算流程得出；
[0195] S1.4求解后验状态分布；
[0196] 对非线性的水声信号传递时间观测方程mt,k＝hk(xk,θk)进行线性化，只保留一阶项，得到：
[0197]
[0198] 其中：
[0199] 为观测方程的雅克比矩阵；
[0200] 由此得到线性化后的水声信号传递时间观测方程为：
[0201]
[0202] 在已知 与m1:k的前提下，记状态后验估计为：
[0203]
[0204] 选择线性化展开点 相应的雅克比矩阵为：
[0205]
[0206] 其中：
[0207]
[0208] 根据扩展Kalman滤波流程，可以得到后验估计参数为：
[0209]
[0210]
[0211]
[0212] S1.5求解目标函数；
[0213] 选择 作为线性展开点，根据 以及 得到：
[0214]
[0215] 其中： 表示与θk无关的常数；
[0216]
[0217]
[0218]
[0219]
[0220] 根据以上关系，将Fk简化为：
[0221]
[0222] 其中：
[0223]
[0224] S2.期望最大化算法求极值步骤；
[0225] 为 的极大值的必要条件为：
[0226]
[0227] 计算得到：
[0228]
[0229] 当 时，以上极值条件满足；
[0230] 令：
[0231]
[0232] 其中： 表示水声信标位置坐标与水声声速的名义值，在迭代过程中一直保持不变，通过更新水声信号收发端的时钟漂移ΔTt,k来间接补偿信标位置坐标与水声
声速设置误差带来的影响。
[0233] 通过以上参数更新方案，可以保证期望最大化方法的极值条件满足。
[0234] 实施例2，本发明的算法伪代码总结为：
[0235]
[0236] 实施例3，利用实施例1所述的方法通过实验数据进行验证。
[0237] 作为比较，本实施例同时展示了传统的水下单信标定位方法的定位结果。实验数据的搜集方法为：水面船搭载GPS、水听器以及罗盘，在水面上进行二维运动。GPS观测到的
水面船运动轨迹作为真实参考，水听器接收固定在水底的水声信标所发射的水声信号，并
以此获得水声信号传递时间。由于水面船未安装多普勒测速仪，采用GPS轨迹进行差分结合
电子罗盘测得的艏向角来仿真多普勒测速仪所观测的航行器对地速度。试验当中水声信号
发射周期约为30秒(少数几个信号出现观测丢包)，海流观测周期为1秒，离散时间间隔Δt
同样设置为1秒。为了验证本发明所提出的方法，在试验数据中人为加入信标的位置误差以
及水声信号收发端的时钟漂移，其中x与y两个方向信标位置误差均未20米，时钟漂移规律
设置为ΔTt,k＝0.005kΔt/3600，相当于时钟漂移为5毫秒/小时。
[0238] 在数值验证的过程中，滤波器初始参数设置为：(1)x与y两个方向海流初始误差均为0.5米/秒；(2)名义声速 为1520米/秒；(3)名义时钟漂移为0秒；(4)海流不确定性标
准差σc为0.01米/秒；(5)航行器对水速度观测不确定性标准差σw为0.1米/秒；(6)水声信号
传递时间观测噪声标准差σt,m为0.001秒；(7)海流观测噪声标准差σvc,m为0.01米/秒；(8)传
统水下单信标定位应用中斜距观测噪声标准差σr,m为5米；(9)本发明所提出方法的迭代次
数N设置为10。
[0239] 附图2为试验中水面船的真实运动轨迹、本发明的方法与传统水下单信标定位方法所估计出的运动轨迹、真实以及名义水声信标位置。附图3表示两种方法的水平定位误差
随时间变化的曲线，定位误差计算公式为
[0240] 本发明所提出方法与传统水下单信标定位方法的平均均方定位误差分别为0.2497米与20.4466米。根据图2、3以及两种方法的平均均方定位误差，可以看出在存在时
钟漂移、信标位置偏差及水声声速设置误差时，本发明所提出的方法可以获得比传统水下
单信标定位方法更好的结果。时钟漂移、信标位置偏差及水声声速设置误差对本发明所提
出方法的定位性能几乎没有影响。
[0241] 虽然，上文中已经用一般性说明及具体实施例对本发明作了详尽的描述，但在本发明基础上，可以对之作一些修改或改进，这对本领域技术人员而言是显而易见的。因此，
在不偏离本发明精神的基础上所做的这些修改或改进，均属于本发明要求保护的范围。
