本发明涉及水下定位技术领域，特别涉及一种水下航行器的单信标定位方法。一种具有全局收敛性的水下航行器单信标定位方法，水下航行器搭载有水听器、多普勒测速仪、深度计、姿态航向参考系统及GPS；水声信标周期性广播水声信号；本发明通过状态增广，将离散状态的非线性单信标定位模型转化为线性时变模型；在未接收到水声信号时，通过水下航行器自身搭载设备获得水下航行器与水相对速度与姿态进行航位推算；接收到水声信号后，通过已知的水声信号发射时间获得水声信号传递时间，将其作为观测变量，同时综合航位推算数据以及各种传感器观测数据，基于Kalman滤波进行单信标定位的预测及更新。在满足定位模型可观测的前提下，本方法具有全局指数收敛性。
1.一种具有全局收敛性的水下航行器单信标定位方法，水下航行器搭载有水听器、多普勒测速仪、深度计、姿态航向参考系统及GPS；水声信标周期性广播水声信号；其特征在于，所述方法包括以下步骤：A.以定位区域内任意点为原点，东、北、天三个方向分别设为x，y，z轴，建立水下局部惯性坐标系；
B.通过搭载的GPS获取该水下航行器在水下局部惯性坐标系当中的初始位置；
C.建立水下航行器的运动学模型以及观测模型并进行离散化，构建非线性单信标定位模型；
其中：x，y，z为水下航行器在水下局部惯性坐标系中的空间位置坐标；
vc＝[vcx vcy vcz]其中：vcx，vcy，vcz为水下局部惯性坐标系中x，y，z三个方向未知的海流速度；
vw＝[vwx vwy vwz]其中：vwx，vwy，vwz分别为水下局部惯性坐标系中x，y，z三个方向水下航行器与水的相对速度，通过姿态航向参考系统以及多普勒测速仪所测数据计算得到，计算公式为：其中： 为多普勒测速仪测得的随体坐标系下水下航行器与水的相对速度矢量， 为随体坐标系向局部惯性坐标系旋转的旋转矩阵，其矩阵元素与姿态航向参考系统所测得的水下航行器姿态角、航向角相关； 的计算公式为：其中：θ,ψ分别为水下航行器的横滚角、俯仰角及航向角，由姿态航向参考系统测得；
求解未知p，vc及ve的时间导数，并考虑相应的不确定性，得到水下航行器的运动学模型：
其中：ωp＝[ωpx ωpy ωpz]为水下航行器在x，y，z方向的位置不确定性，ωc＝[ωcx T
ωcy ωcz]为x，y，z方向的海流不确定性；ωe为有效声速不确定性；
记水声信标发射水声信号的时刻为Te，记水声信标在水下局部惯性坐标系中的空间位T
置坐标为s＝[XTe YTe ZTe] ，水下航行器接收到该水声信号的时刻为Ta，XTe，YTe，ZTe，Te以及Ta均为已知量，观测方程为：其中：vt为对应的观测噪声；
根据多普勒测速仪测得的随体坐标系下水下航行器的绝对速度 结合姿态航向参考系统测得的水下航行器姿态和航向，计算得到水下航行器绝对速度在水下局部惯性坐标系下的表示：
其中：vg＝[vgx vgy vgz]为水下航行器绝对速度在局部惯性坐标系下x，y，z三个方向的分量；
根据vg以及vw，计算得到海流速度观测量为：mc＝vg‑vw
其中：mc＝[mcx mcy mcz]表示三个方向的海流观测量；
其中：vvc为海流观测噪声向量，vvc＝[vvcx vvcy vvcz] ，其中vvcx为x方向的海流观测噪声；vvcy为y方向的海流观测噪声；vvcz为z方向的海流观测噪声；
记水下航行器所搭载的深度计观测量为mz，则其观测方程为mz＝ap+vz
运动学模型以及观测模型离散化方法为：S1.运动学模型离散化；
以变量加上下标符号k为离散时间索引，以ΔT为离散间隔，运动学模型离散为：pk+1＝pk+ΔTvc,k+uk+ωp,kvc,k+1＝vc,k+ωc,kve,k+1＝ve,k+ωe,k其中：uk为控制向量，为已知量，由水下航行器根据多普勒测速仪及姿态航向参考系统观测量进行航位推算得到；ωp,k,ωc,k,ωe,k表示离散状态下的过程噪声；
假设水下航行器在k时刻接收到水声信号，离散后的水声信号传递时间观测方程为：其中，vt,k为水声信号传递时间观测噪声；
假设在每一个离散时间点处均能够得到海流速度观测，离散后观测方程为：mvc，k＝vc,k+vvc,k其中，vvc,k为k时刻海流速度观测噪声；
同样，假设在每一个离散时间点处均能够得到深度计观测，离散后观测方程为：mz,k＝apk+vz,k其中，vz,k为k时刻深度计观测噪声；
D.通过状态增广，将离散状态的非线性单信标定位模型转化为线性时变模型；
根据离散时间的水下航行器运动学模型，得到：x1,k+1＝x1,k+ukx3,k+ΔTx2,k+ω1,kx2,k+1＝x2,k+ω2,kx3,k+1＝x3,k+ω3,k其中：ω1,k,ω2,k,ω3,k分别为对应的过程噪声；
x5,k+1＝x5,k+ω5,k其中：ω4,k,ω5,k,ω6,k分别为对应的过程噪声；
由离散状态的水声信号传递时间观测模型得到，在无观测噪声的前提下，有mt,k＝x4,k,mt,k+1＝x4,k+1；在k+1时刻，水下航行器获得mt,k+1后，将其看作已知量，作为线性增广模型扩散方程的参数，得到：
其中：I3表示三维的单位矩阵，0m×n表示元素均为0，维度为m行n列的矩阵；
mt,k＝x4,k+vt,k由x1,k,x2,k,x3,k,x5,k,x6,k的定义得到：x2,k＝vc,kx3,k当水下航行器获得海流观测mvc，k时，将其看作已知量，结合海流观测方程，得到：
0＝x2,k‑mvc,kx3,k+vs1,k其中：vs,1，vs,2及vs,3为对应的观测噪声，其与海流观测噪声和线性增广模型状态的关系为：
vs1,k＝vvc,kx3,k同样，由x1,k,x3,k的定义以及深度计观测方程，得到：
0＝ax1,k‑mz,kx3,k+vs4,k其中：vs,4为对应观测噪声，且vs4,k＝vz,kx3,k；
E.水下航行器在未接收到水声信号时，通过姿态航向参考系统以及多普勒测速仪获得水下航行器与水相对速度与姿态进行航位推算；水下航行器接收到水声信号后，通过已知的水声信号发射时间获得水声信号传递时间，将其作为观测变量，同时综合航位推算数据以及各种传感器观测数据，基于Kalman滤波进行单信标定位系统的预测以及更新。
2.如权利要求1所述的一种具有全局收敛性的水下航行器单信标定位方法，其特征在于，所述步骤E中，水下航行器进行航位推算的方法为：记航位推算的周期为ΔTDR，ΔTDR由姿态航向参考系统以及多普勒测速仪二者采样周期较大值决定，ΔTDR小于线性增广模型离散时间间隔ΔT；
在k时刻至k+1时刻时间段内，局部惯性坐标系下水下航行器对水相对运动矢量为：其中： 采用矩形法进行数值积分，得到：其中：vw,i表示k时刻至k+1时刻时间间隔内水下航行器对水速度的各个采样值，n表示k时刻至k+1时刻时间间隔内总航位推算采样数。
3.如权利要求2所述的一种具有全局收敛性的水下航行器单信标定位方法，其特征在于，所述步骤E中，通过Kalman滤波进行定位模型的预测以及更新方法为：S1.预测过程；
水下航行器在k+1时刻接收到水声信号，计算出该时刻的水声信号传递时间mt,k+1；同时，由航位推算得到uk，进而构造出相应的增广线性模型矩阵Ak；由Kalman滤波的预测环节，得到增广线性模型状态先验估计为：其中： 与Pk|k分别为k时刻的后验状态和后验方差； 与Pk+1|k分别为k+1时刻的先验状态和先验方差；Qk为k时刻的过程噪声协方差矩阵，为对称正定阵，其具体参数由过程噪声ωk的统计特性决定，通过线下调制获得；
水下航行器在k+1时刻接收到水声信号，计算出该时刻的水声信号传递时间mt,k+1；同时，根据多普勒测速仪以及姿态航向参考系统的观测值计算出k+1时刻的海流速度mvc,k+1；
根据深度计观测值得到k+1时刻的水下航行器的深度mz,k+1；构造观测矩阵Ck+1及观测向量mk+1，根据Kalman滤波的更新环节，得到增广线性系统状态后验估计为：Pk+1|k+1＝Pk+1|k‑Kk+1Ck+1Pk+1|k其中：Kk+1为Kalman增益；Rk+1为k+1时刻的观测噪声协方差矩阵，为对称正定阵，其具体参数由观测噪声vk的统计特性决定，通过线下调制获得。
4.如权利要求3所述的一种具有全局收敛性的水下航行器单信标定位方法，其特征在于，根据所述步骤E所得到的增广线性模型后验状态估计，计算得到原始非线性模型后验状态估计，计算方法为：
其中： 与 分别为k+1时刻有效声速、位置以及海流速度的后验估计；为了进一步保证定位模型的稳定性，对有效声速的估计进行限幅，即选择：其中：vm与vM分别为有效声速的下界及上界，根据实际情况设定；sat(x,a,b)为限幅函数，其输出为：
一种具有全局收敛性的水下航行器单信标定位方法
技术领域
[0001] 本发明涉及水下定位技术领域，特别涉及一种水下航行器的单信标定位方法。
背景技术
[0002] 精确的位置反馈是水下航行器完成既定水下任务的基础。由于水下电磁波信号衰减较快，广泛应用于陆地与天空定位的GNSS系统在水下无法应用。现有主流的水下定位方
式包括以惯性导航为代表的航位推算方法以及以长基线定位为代表的水下声学定位方法。
其中惯性导航设备往往会随时间增长产生较大累计误差，无法长时间用于水下定位，而高
精度的惯性导航设备成本极高，限制了其在水下航行器中的应用。现有主流的水下声学定
位方式包括长基线定位、超短基线定位、单信标定位等。长基线定位与超短基线定位发展均
较为成熟，但其成本通常较高，且实时性通常较差，这限制了其在水下航行器中的应用。而
新兴的水下单信标定位系统融合航位推算数据与单水声信标的测距信息，在定位成本和实
时性方面均有较大的优势。目前的单信标定位系统均是以水声信号传递时间为观测，在水
下声速已知的前提下，得到信标与水下航行器之间的地理斜距作为观测变量。但实际应用
中，水声声速受到水下温度、盐度、密度等因素的影响，通常为时变未知的，精确的水声声速
难以得到，这会导致水声测距误差，影响单信标定位系统的性能；此外，由于地理斜距的观
测方程为非线性，目前的水下单信标定位系统通常采用非线性Kalman滤波或粒子滤波进行
位置解算，这些非线性的滤波方法只具有局部收敛性，在水下航行器的初始位置误差或某
一时刻的位置误差较大的情况下(例如：航行器在水下航行的过程中，在某一时间段内由于
水下环境的恶劣性而失去了声学定位辅助，只依靠自身的航位推算而产生较大的累计误
差，在恢复声学定位辅助之后会出现较大的位置误差)，难以保证后续滤波的收敛性，这也
影响了现有水下单信标定位系统的实际应用。
发明内容
[0003] 本发明的目的是：针对水下单信标定位当中水声声速的未知性、水下航行器初始位置误差或某一时刻的位置误差较大等问题，基于状态增广方法提出一种具有全局收敛性
的水下航行器单信标定位方法。
[0004] 本发明的技术方案是：一种具有全局收敛性的水下航行器单信标定位方法，水下航行器搭载有水听器、多普勒测速仪、深度计、姿态航向参考系统及GPS；水声信标周期性广
播水声信号；所述方法包括以下步骤：
[0005] A.以定位区域内任意点为原点，东、北、天三个方向分别设为x，y，z轴，建立水下局部惯性坐标系；
[0006] B.通过搭载的GPS获取该水下航行器在水下局部惯性坐标系当中的初始位置；
[0007] C.建立水下航行器的运动学模型以及观测模型并进行离散化，构建非线性单信标定位模型；
[0008] D.通过状态增广，将离散状态的非线性单信标定位模型转化为线性时变模型；
[0009] E.水下航行器在未接收到水声信号时，通过姿态航向参考系统以及多普勒测速仪获得水下航行器与水相对速度与姿态进行航位推算；水下航行器接收到水声信号后，通过
已知的水声信号发射时间获得水声信号传递时间，将其作为观测变量，同时综合航位推算
数据以及各种传感器观测数据，基于Kalman滤波进行单信标定位系统的预测以及更新。
[0010] 在上述方案的基础上，具体的，所述步骤C中，运动学模型的建立方法为：
[0011] 定义位置向量为：
[0012] p＝[x y z]T
[0013] 其中：x，y，z为水下航行器在水下局部惯性坐标系中的空间位置坐标；
[0014] 定义海流速度向量为：
[0015] vc＝[vcx vcy vcz]T
[0016] 其中：vcx，vcy，vcz为水下局部惯性坐标系中x，y，z三个方向未知的海流速度；
[0017] 定义水下航行器对水速度向量为：
[0018] vw＝[vwx vwy vwz]T
[0019] 其中：vwx，vwy，vwz分别为水下局部惯性坐标系中x，y，z三个方向水下航行器与水的相对速度，通过姿态航向参考系统以及多普勒测速仪所测数据计算得到，计算公式为：
[0020]
[0021] 其中： 为多普勒测速仪测得的随体坐标系下水下航行器与水的相对速度矢量，为随体坐标系向局部惯性坐标系旋转的旋转矩阵，其矩阵元素与姿态航向参考系统所
测得的水下航行器姿态角、航向角相关； 的计算公式为：
[0022]
[0023] 其中：θ,ψ分别为水下航行器的横滚角、俯仰角及航向角，由姿态航向参考系统测得；
[0024] 记ve为水下未知的有效声速；
[0025] 求解未知p，vc及ve的时间导数，并考虑相应的不确定性，得到水下航行器的运动学模型：
[0026]
[0027]
[0028]
[0029] 其中：ωp＝[ωpx ωpy ωpz]T为水下航行器在x，y，z方向的位置不确定性，ωc＝T
[ωcx ωcy ωcz]为x，y，z方向的海流不确定性；ωe为有效声速不确定性。
[0030] 在上述方案的基础上，具体的，所述步骤C中，观测模型的建立方法为：
[0031] S1.建立水声信号传递时间的观测模型；
[0032] 记水声信标发射水声信号的时刻为Te，记水声信标在水下局部惯性坐标系中的空T
间位置坐标为s＝[XTe YTe ZTe] ，水下航行器接收到该水声信号的时刻为Ta，XTe，YTe，ZTe，Te
以及Ta均为已知量，观测方程为：
[0033]
[0034] 其中：vt为对应的观测噪声；
[0035] S2.建立海流流速观测模型；
[0036] 根据多普勒测速仪测得的随体坐标系下水下航行器的绝对速度 结合姿态航向参考系统测得的水下航行器姿态和航向，计算得到水下航行器绝对速度在水下局部惯性坐
标系下的表示：
[0037]
[0038] 其中：vg＝[vgx vgy vgz]T为水下航行器绝对速度在局部惯性坐标系下x，y，z三个方向的分量；
[0039] 根据vg以及vw，计算得到海流速度观测量为：
[0040] mc＝vg‑vw
[0041] 其中：mc＝[mcx mcy mcz]T表示三个方向的海流观测量；
[0042] 海流观测方程为线性，满足mc＝vc+vvc；
[0043] 其中：vvc为海流观测噪声向量，vvc＝[vvcx vvcy vvcz]T，其中vvcx为x方向的海流观测噪声；vvcy为y方向的海流观测噪声；vvcz为z方向的海流观测噪声；
[0044] S3.建立深度观测模型；
[0045] 记水下航行器所搭载的深度计观测量为mz，则其观测方程为
[0046] mz＝ap+vz
[0047] 其中：a＝[0 0 1]，vz为深度计观测噪声。
[0048] 在上述方案的基础上，具体的，所述步骤C中，运动学模型以及观测模型离散化方法为：
[0049] S1.运动学模型离散化；
[0050] 以变量加上下标符号k为离散时间索引，以ΔT为离散间隔，运动学模型离散为：
[0051] pk+1＝pk+ΔTvc,k+uk+ωp,k
[0052] vc,k+1＝vc,k+ωc,k
[0053] ve,k+1＝ve,k+ωe,k
[0054] 其中：uk为控制向量，为已知量，由水下航行器根据多普勒测速仪及姿态航向参考系统观测量进行航位推算得到；ωp,k,ωc,k,ωe,k表示离散状态下的过程噪声；
[0055] S2.观测模型离散化；
[0056] 假设水下航行器在k时刻接收到水声信号，离散后的水声信号传递时间观测方程为：
[0057]
[0058] 其中，vt,k为水声信号传递时间观测噪声；
[0059] 假设在每一个离散时间点处均可以得到海流速度观测，离散后观测方程为：
[0060] mvc，k＝vc,k+vvc,k
[0061] 其中，vvc,k为k时刻海流速度观测噪声；
[0062] 同样，假设在每一个离散时间点处均可以得到深度计观测，离散后观测方程为：
[0063] mz,k＝apk+vz,k
[0064] 其中，vz,k为k时刻深度计观测噪声。
[0065] 在上述方案的基础上，具体的，所述步骤D包括：
[0066] S1.运动学模型处理；
[0067] 定义离散状态变量：
[0068]
[0069] 根据离散时间的水下航行器运动学模型，可以得到：
[0070] x1,k+1＝x1,k+ukx3,k+ΔTx2,k+ω1,k
[0071] x2,k+1＝x2,k+ω2,k
[0072] x3,k+1＝x3,k+ω3,k
[0073] 其中：ω1,k,ω2,k,ω3,k分别为对应的过程噪声；
[0074] 进一步定义离散状态变量：
[0075]
[0076] 得到：
[0077]
[0078] x5,k+1＝x5,k+ω5,k
[0079]
[0080] 其中：ω4,k,ω5,k,ω6,k分别为对应的过程噪声；
[0081] 由离散状态的水声信号传递时间观测模型可得，在无观测噪声的前提下，有 mt,k＝x4,k,mt,k+1＝x4,k+1；在k+1时刻，水下航行器获得mt,k+1后，将其看作已知量，作为线性增广
模型扩散方程的参数，可以得到：
[0082]
[0083] 定义状态向量及噪声向量：
[0084]
[0085] 则xk的运动学方程为：
[0086] xk+1＝Akxk+ωk
[0087] 其中：
[0088]
[0089] 其中：I3表示三维的单位矩阵，0m×n表示元素均为0，维度为m行n列的矩阵；
[0090] S2.观测模型处理；
[0091] 由x4,k的定义可得：
[0092] mt,k＝x4,k+vt,k
[0093] 由x1,k,x2,k,x3,k,x5,k,x6,k的定义可得：
[0094] x2,k＝vc,kx3,k
[0095]
[0096]
[0097] 当水下航行器获得海流观测mvc，k时，将其看作已知量，结合海流观测方程，可得：
[0098] 0＝x2,k‑mvc,kx3,k+vs1,k
[0099]
[0100]
[0101] 其中：vs,1，vs,2及vs,3为对应的观测噪声，其与海流观测噪声和线性增广模型状态的关系为：
[0102] vs1,k＝vvc,kx3,k
[0103]
[0104]
[0105] 同样，由x1,k,x3,k的定义以及深度计观测方程，可得：
[0106] 0＝ax1,k‑mz,kx3,k+vs4,k
[0107] 其中：vs,4为对应观测噪声，且vs4,k＝vz,kx3,k；
[0108] 构造观测向量及观测噪声向量为：
[0109]
[0110] 对应的观测方程为：
[0111] mk＝Ckxk+vk
[0112] 其中：
[0113]
[0114] 在上述方案的基础上，具体的，所述步骤E中，水下航行器进行航位推算的方法为：
[0115] 记航位推算的周期为ΔTDR，ΔTDR由姿态航向参考系统以及多普勒测速仪二者采样周期较大值决定，ΔTDR小于线性增广模型离散时间间隔ΔT；
[0116] 在k时刻至k+1时刻时间段内，局部惯性坐标系下水下航行器对水相对运动矢量为：
[0117]
[0118] 其中： 采用矩形法进行数值积分，得到：
[0119]
[0120] 其中：vw,i表示k时刻至k+1时刻时间间隔内水下航行器对水速度的各个采样值，n表示 k时刻至k+1时刻时间间隔内总航位推算采样数。
[0121] 在上述方案的基础上，具体的，所述步骤E中，通过Kalman滤波进行定位模型的预测以及更新方法为：
[0122] S1.预测过程；
[0123] 水下航行器在k+1时刻接收到水声信号，计算出该时刻的水声信号传递时间mt,k+1；同时，由航位推算可以得到uk，进而构造出相应的增广线性模型矩阵Ak；由Kalman滤波的预
测环节，可得增广线性模型状态先验估计为：
[0124]
[0125]
[0126] 其中： 与Pk|k分别为k时刻的后验状态和后验方差； 与Pk+1|k分别为k+1时刻的先验状态和先验方差；Qk为k时刻的过程噪声协方差矩阵，为对称正定阵，其具体参数
由过程噪声ωk的统计特性决定，可通过线下调制获得；
[0127] S2.更新过程；
[0128] 水下航行器在k+1时刻接收到水声信号，计算出该时刻的水声信号传递时间mt,k+1；同时，根据多普勒测速仪以及姿态航向参考系统的观测值计算出k+1时刻的海流速度
mvc,k+1；根据深度计观测值得到k+1时刻的水下航行器的深度mz,k+1；构造观测矩阵Ck+1及观测
向量 mk+1，根据Kalman滤波的更新环节，可得增广线性系统状态后验估计为：
[0129]
[0130]
[0131] Pk+1|k+1＝Pk+1|k‑Kk+1Ck+1Pk+1|k
[0132] 其中：Kk+1为Kalman增益；Rk+1为k+1时刻的观测噪声协方差矩阵，为对称正定阵，其具体参数由观测噪声vk的统计特性决定，可通过线下调制获得。
[0133] 在上述方案的基础上，进一步的，根据所述步骤E所得到的增广线性模型后验状态估计，可以计算得到原始非线性模型后验状态估计，计算方法为：
[0134]
[0135]
[0136]
[0137] 其中： 与 分别为k+1时刻有效声速、位置以及海流速度的后验估计；为了进一步保证定位模型的稳定性，对有效声速的估计进行限幅，即选择：
[0138]
[0139] 其中：vm与vM分别为有效声速的下界及上界，根据实际情况设定；sat(x,a,b)为限幅函数，其输出为：
[0140]
[0141] 有益效果：本发明通过状态增广，将水下单信标定位模型中非线性的水声信号传递时间观测模型转化为线性时变模型，构造出线性时变单信标定位模型，通过Kalman滤波
进行位置解算。在保证系统可观测的前提下，本发明具有全局收敛性，也就是说，当水下航
行器的初始位置误差或某一时刻的位置误差较大时，本发明所提出的方法仍可以保证定位
误差收敛性。
附图说明
[0142] 图1为本发明的步骤流程图；
[0143] 图2为本发明与传统基于扩展Kalman滤波(EKF)进行位置解算的水下单信标定位方法所得均方定位误差比较；
[0144] 图3为本发明与传统基于扩展Kalman滤波(EKF)进行位置解算的水下单信标定位方法所得均方有效声速误差比较。
具体实施方式
[0145] 实施例1，参见附图1，一种具有全局收敛性的水下航行器单信标定位方法，水下航行器搭载有水听器、多普勒测速仪、深度计、姿态航向参考系统及GPS；水声信标周期性广播
水声信号；所述方法包括以下步骤：
[0146] A.以定位区域内任意点为原点，东、北、天三个方向分别设为x，y，z轴，建立水下局部惯性坐标系。
[0147] B.通过搭载的GPS获取该水下航行器在水下局部惯性坐标系当中的初始位置。
[0148] C.建立水下航行器的运动学模型以及观测模型并进行离散化，构建非线性单信标定位模型。
[0149] 运动学模型的建立方法为：
[0150] 定义位置向量为：
[0151] p＝[x y z]T
[0152] 其中：x，y，z为水下航行器在水下局部惯性坐标系中的空间位置坐标；
[0153] 定义海流速度向量为：
[0154] vc＝[vcx vcy vcz]T
[0155] 其中：vcx，vcy，vcz为水下局部惯性坐标系中x，y，z三个方向未知的海流速度；
[0156] 定义水下航行器对水速度向量为：
[0157] vw＝[vwx vwy vwz]T
[0158] 其中：vwx，vwy，vwz分别为水下局部惯性坐标系中x，y，z三个方向水下航行器与水的相对速度，通过姿态航向参考系统以及多普勒测速仪所测数据计算得到，计算公式为：
[0159]
[0160] 其中： 为多普勒测速仪测得的随体坐标系下水下航行器与水的相对速度矢量，为随体坐标系向局部惯性坐标系旋转的旋转矩阵，其矩阵元素与姿态航向参考系统所测
得的水下航行器姿态角、航向角相关； 的计算公式为：
[0161]
[0162] 其中：θ,ψ分别为水下航行器的横滚角、俯仰角及航向角，由姿态航向参考系统测得；
[0163] 记ve为水下未知的有效声速；
[0164] 求解未知p，vc及ve的时间导数，并考虑相应的不确定性，得到水下航行器的运动学模型：
[0165]
[0166]
[0167]
[0168] 其中：ωp＝[ωpx ωpy ωpz]T为水下航行器在x，y，z方向的位置不确定性，ωc＝T
[ωcx ωcy ωcz]为x，y，z方向的海流不确定性；ωe为有效声速不确定性。
[0169] 观测模型的建立方法为：
[0170] S1.建立水声信号传递时间的观测模型；
[0171] 记水声信标发射水声信号的时刻为Te，记水声信标在水下局部惯性坐标系中的空T
间位置坐标为s＝[XTe YTe ZTe] ，水下航行器接收到该水声信号的时刻为Ta，XTe，YTe，ZTe，Te
以及Ta均为已知量，观测方程为：
[0172]
[0173] 其中：vt为对应的观测噪声；
[0174] S2.建立海流流速观测模型；
[0175] 根据多普勒测速仪测得的随体坐标系下水下航行器的绝对速度 结合姿态航向参考系统测得的水下航行器姿态和航向，计算得到水下航行器绝对速度在水下局部惯性坐
标系下的表示：
[0176]
[0177] 其中：vg＝[vgx vgy vgz]T为水下航行器绝对速度在局部惯性坐标系下x，y，z三个方向的分量；
[0178] 根据vg以及vw，计算得到海流速度观测量为：
[0179] mc＝vg‑vw
[0180] 其中：mc＝[mcx mcy mcz]T表示三个方向的海流观测量；
[0181] 海流观测方程为线性，满足mc＝vc+vvc；
[0182] 其中：vvc为海流观测噪声向量，vvc＝[vvcx vvcy vvcz]T，其中vvcx为x方向的海流观测噪声；vvcy为y方向的海流观测噪声；vvcz为z方向的海流观测噪声；
[0183] S3.建立深度观测模型；
[0184] 记水下航行器所搭载的深度计观测量为mz，则其观测方程为
[0185] mz＝ap+vz
[0186] 其中：a＝[0 0 1]，vz为深度计观测噪声。
[0187] 运动学模型以及观测模型离散化方法为：
[0188] S1.运动学模型离散化；
[0189] 以变量加上下标符号k为离散时间索引，以ΔT为离散间隔，运动学模型离散为：
[0190] pk+1＝pk+ΔTvc,k+uk+ωp,k
[0191] vc,k+1＝vc,k+ωc,k
[0192] ve,k+1＝ve,k+ωe,k
[0193] 其中：uk为控制向量，为已知量，由水下航行器根据多普勒测速仪及姿态航向参考系统观测量进行航位推算得到；ωp,k,ωc,k,ωe,k表示离散状态下的过程噪声；
[0194] S2.观测模型离散化；
[0195] 假设水下航行器在k时刻接收到水声信号，离散后的水声信号传递时间观测方程为：
[0196]
[0197] 其中，vt,k为水声信号传递时间观测噪声；
[0198] 假设在每一个离散时间点处均可以得到海流速度观测，离散后观测方程为：
[0199] mvc，k＝vc,k+vvc,k
[0200] 其中，vvc,k为k时刻海流速度观测噪声；
[0201] 同样，假设在每一个离散时间点处均可以得到深度计观测，离散后观测方程为：
[0202] mz,k＝apk+vz,k
[0203] 其中，vz,k为k时刻深度计观测噪声。
[0204] D.通过状态增广，将离散状态的非线性单信标定位模型转化为线性时变模型。
[0205] S1.运动学模型处理；
[0206] 定义离散状态变量：
[0207]
[0208] 根据离散时间的水下航行器运动学模型，可以得到：
[0209] x1,k+1＝x1,k+ukx3,k+ΔTx2,k+ω1,k
[0210] x2,k+1＝x2,k+ω2,k
[0211] x3,k+1＝x3,k+ω3,k
[0212] 其中：ω1,k,ω2,k,ω3,k分别为对应的过程噪声；
[0213] 进一步定义离散状态变量：
[0214]
[0215] 得到：
[0216]
[0217] x5,k+1＝x5,k+ω5,k
[0218]
[0219] 其中：ω4,k,ω5,k,ω6,k分别为对应的过程噪声；
[0220] 由离散状态的水声信号传递时间观测模型可得，在无观测噪声的前提下，有 mt,k＝x4,k,mt,k+1＝x4,k+1；在k+1时刻，水下航行器获得mt,k+1后，将其看作已知量，作为线性增广
模型扩散方程的参数，可以得到：
[0221]
[0222] 定义状态向量及噪声向量：
[0223]
[0224] 则xk的运动学方程为：
[0225] xk+1＝Akxk+ωk
[0226] 其中：
[0227]
[0228] 其中：I3表示三维的单位矩阵，0m×n表示元素均为0，维度为m行n列的矩阵；
[0229] S2.观测模型处理；
[0230] 由x4,k的定义可得：
[0231] mt,k＝x4,k+vt,k
[0232] 由x1,k,x2,k,x3,k,x5,k,x6,k的定义可得：
[0233] x2,k＝vc,kx3,k
[0234]
[0235]
[0236] 当水下航行器获得海流观测mvc，k时，将其看作已知量，结合海流观测方程，可得：
[0237] 0＝x2,k‑mvc,kx3,k+vs1,k
[0238]
[0239]
[0240] 其中：vs,1，vs,2及vs,3为对应的观测噪声，其与海流观测噪声和线性增广模型状态的关系为：
[0241] vs1,k＝vvc,kx3,k
[0242]
[0243]
[0244] 同样，由x1,k,x3,k的定义以及深度计观测方程，可得：
[0245] 0＝ax1,k‑mz,kx3,k+vs4,k
[0246] 其中：vs,4为对应观测噪声，且vs4,k＝vz,kx3,k；
[0247] 构造观测向量及观测噪声向量为：
[0248]
[0249] 对应的观测方程为：
[0250] mk＝Ckxk+vk
[0251] 其中：
[0252]
[0253] E.水下航行器在未接收到水声信号时，通过姿态航向参考系统以及多普勒测速仪获得水下航行器与水相对速度与姿态进行航位推算；水下航行器接收到水声信号后，通过
已知的水声信号发射时间获得水声信号传递时间，将其作为观测变量，同时综合航位推算
数据以及各种传感器观测数据，基于Kalman滤波进行单信标定位系统的预测以及更新。
[0254] 水下航行器进行航位推算的方法为：
[0255] 记航位推算的周期为ΔTDR，ΔTDR由姿态航向参考系统以及多普勒测速仪二者采样周期较大值决定，ΔTDR小于线性增广模型离散时间间隔ΔT；
[0256] 在k时刻至k+1时刻时间段内，局部惯性坐标系下水下航行器对水相对运动矢量为：
[0257]
[0258] 其中： 采用矩形法进行数值积分，得到：
[0259]
[0260] 其中：vw,i表示k时刻至k+1时刻时间间隔内水下航行器对水速度的各个采样值，n表示 k时刻至k+1时刻时间间隔内总航位推算采样数。
[0261] 通过Kalman滤波进行定位模型的预测以及更新方法为：
[0262] S1.预测过程；
[0263] 水下航行器在k+1时刻接收到水声信号，计算出该时刻的水声信号传递时间mt,k+1；同时，由航位推算可以得到uk，进而构造出相应的增广线性模型矩阵Ak；由Kalman滤波的预
测环节，可得增广线性模型状态先验估计为：
[0264]
[0265]
[0266] 其中： 与Pk|k分别为k时刻的后验状态和后验方差； 与Pk+1|k分别为k+1时刻的先验状态和先验方差；Qk为k时刻的过程噪声协方差矩阵，为对称正定阵，其具体参数
由过程噪声ωk的统计特性决定，可通过线下调制获得；
[0267] S2.更新过程；
[0268] 水下航行器在k+1时刻接收到水声信号，计算出该时刻的水声信号传递时间mt,k+1；同时，根据多普勒测速仪以及姿态航向参考系统的观测值计算出k+1时刻的海流速度
mvc,k+1；根据深度计观测值得到k+1时刻的水下航行器的深度mz,k+1；构造观测矩阵Ck+1及观测
向量 mk+1，根据Kalman滤波的更新环节，可得增广线性系统状态后验估计为：
[0269]
[0270]
[0271] Pk+1|k+1＝Pk+1|k‑Kk+1Ck+1Pk+1|k
[0272] 其中：Kk+1为Kalman增益；Rk+1为k+1时刻的观测噪声协方差矩阵，为对称正定阵，其具体参数由观测噪声vk的统计特性决定，可通过线下调制获得。
[0273] 根据所述步骤E所得到的增广线性模型后验状态估计，可以计算得到原始非线性模型后验状态估计，计算方法为：
[0274]
[0275]
[0276]
[0277] 其中： 与 分别为k+1时刻有效声速、位置以及海流速度的后验估计；为了进一步保证定位模型的稳定性，对有效声速的估计进行限幅，即选择：
[0278]
[0279] 其中：vm与vM分别为有效声速的下界及上界，根据实际情况设定；sat(x,a,b)为限幅函数，其输出为：
[0280]
[0281] 实施例2，通过仿真数据对实施例1所述的方法进行验证。
[0282] 作为比较，本实施例同时展示了传统基于扩展Kalman滤波(EKF)进行位置解算的水下单信标定位方法的定位结果。总体仿真时长为5000秒，在整个运动过程中仿真的有效
声速为恒定的1500米/秒，三个方向的海流速度均为0.1米/秒。仿真的水声信号发射周期为
10秒，同样，海流观测周期、深度观测周期与系统离散周期也为10秒。航位推算的周期为
0.1s(相当于10Hz采样频率)。
[0283] 仿真的各个传感器噪声参数如下表
[0284]
[0285] 在数值验证的过程中，滤波器初始参数设置为：(1)x与y两个方向位置初始误差均为 100米；(2)z方向位置初始误差为10m；(3)x，y与z三个方向的海流初值均为0米/秒； (4)
有效声速的初始值为1400米/秒；(5)海流不确定性标准差为0.1米/秒；(6)航行器对水速度
‑5 ‑15
观测不确定性标准差为10 米/秒；(7)所提出方法有效声速不确定性的标准差为10 米/
‑5
秒，基于EKF的传统水下单信标定位系统有效声速不确定性的标准差为10 米/秒；(8) 水声
‑15
信号传递时间不确定性的标准差为10 米/秒；(9)水声信号传递时间观测噪声标准差为
‑5
10 秒；(10)海流观测噪声标准差为0.02米/秒；(11)深度计观测噪声标准差为0.001米；
(12)有效声速的上下界vM与vm分别为1600米/秒与1400米/秒。500独立的次Monte Carlo 仿
真的结果被用来验证本发明所提出的方法。两种定位方法定位性能评价指标为均方定位误
差RMSΔH以及均方有效声速误差 二者计算方式如下：
[0286]
[0287]
[0288] 其中： 及 分别为第i次Monte Carlo仿真中真实以及估计的航行器位置坐标，及 分别为第i次Monte Carlo仿真中真实以及估计的有效声速，M＝500表示总的仿真
次数。根据图2以及图3，可以看出在初始位置误差较大的情况下，所提出的方法可以较快的
收敛到一个较小值，而传统基于EKF的单信标定位方法无法收敛，会出现滤波发散。
[0289] 虽然，上文中已经用一般性说明及具体实施例对本发明作了详尽的描述，但在本发明基础上，可以对之作一些修改或改进，这对本领域技术人员而言是显而易见的。因此，
在不偏离本发明精神的基础上所做的这些修改或改进，均属于本发明要求保护的范围。
