CN110779518B - 一种具有全局收敛性的水下航行器单信标定位方法 - Google Patents

Abstract

本发明涉及水下定位技术领域，特别涉及一种水下航行器的单信标定位方法。一种具有全局收敛性的水下航行器单信标定位方法，水下航行器搭载有水听器、多普勒测速仪、深度计、姿态航向参考系统及GPS；水声信标周期性广播水声信号；本发明通过状态增广，将离散状态的非线性单信标定位模型转化为线性时变模型；在未接收到水声信号时，通过水下航行器自身搭载设备获得水下航行器与水相对速度与姿态进行航位推算；接收到水声信号后，通过已知的水声信号发射时间获得水声信号传递时间，将其作为观测变量，同时综合航位推算数据以及各种传感器观测数据，基于Kalman滤波进行单信标定位的预测及更新。在满足定位模型可观测的前提下，本方法具有全局指数收敛性。

Description

一种具有全局收敛性的水下航行器单信标定位方法

技术领域

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

背景技术

精确的位置反馈是水下航行器完成既定水下任务的基础。由于水下电磁波信号衰减较快，广泛应用于陆地与天空定位的GNSS系统在水下无法应用。现有主流的水下定位方式包括以惯性导航为代表的航位推算方法以及以长基线定位为代表的水下声学定位方法。其中惯性导航设备往往会随时间增长产生较大累计误差，无法长时间用于水下定位，而高精度的惯性导航设备成本极高，限制了其在水下航行器中的应用。现有主流的水下声学定位方式包括长基线定位、超短基线定位、单信标定位等。长基线定位与超短基线定位发展均较为成熟，但其成本通常较高，且实时性通常较差，这限制了其在水下航行器中的应用。而新兴的水下单信标定位系统融合航位推算数据与单水声信标的测距信息，在定位成本和实时性方面均有较大的优势。目前的单信标定位系统均是以水声信号传递时间为观测，在水下声速已知的前提下，得到信标与水下航行器之间的地理斜距作为观测变量。但实际应用中，水声声速受到水下温度、盐度、密度等因素的影响，通常为时变未知的，精确的水声声速难以得到，这会导致水声测距误差，影响单信标定位系统的性能；此外，由于地理斜距的观测方程为非线性，目前的水下单信标定位系统通常采用非线性Kalman滤波或粒子滤波进行位置解算，这些非线性的滤波方法只具有局部收敛性，在水下航行器的初始位置误差或某一时刻的位置误差较大的情况下(例如：航行器在水下航行的过程中，在某一时间段内由于水下环境的恶劣性而失去了声学定位辅助，只依靠自身的航位推算而产生较大的累计误差，在恢复声学定位辅助之后会出现较大的位置误差)，难以保证后续滤波的收敛性，这也影响了现有水下单信标定位系统的实际应用。

发明内容

本发明的目的是：针对水下单信标定位当中水声声速的未知性、水下航行器初始位置误差或某一时刻的位置误差较大等问题，基于状态增广方法提出一种具有全局收敛性的水下航行器单信标定位方法。

本发明的技术方案是：一种具有全局收敛性的水下航行器单信标定位方法，水下航行器搭载有水听器、多普勒测速仪、深度计、姿态航向参考系统及GPS；水声信标周期性广播水声信号；所述方法包括以下步骤：

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

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

C.建立水下航行器的运动学模型以及观测模型并进行离散化，构建非线性单信标定位模型；

D.通过状态增广，将离散状态的非线性单信标定位模型转化为线性时变模型；

E.水下航行器在未接收到水声信号时，通过姿态航向参考系统以及多普勒测速仪获得水下航行器与水相对速度与姿态进行航位推算；水下航行器接收到水声信号后，通过已知的水声信号发射时间获得水声信号传递时间，将其作为观测变量，同时综合航位推算数据以及各种传感器观测数据，基于Kalman滤波进行单信标定位系统的预测以及更新。

在上述方案的基础上，具体的，所述步骤C中，运动学模型的建立方法为：

定义位置向量为：

p＝[x y z]^T

其中：x，y，z为水下航行器在水下局部惯性坐标系中的空间位置坐标；

定义海流速度向量为：

v_c＝[v_cx v_cy v_cz]^T

其中：v_cx，v_cy，v_cz为水下局部惯性坐标系中x，y，z三个方向未知的海流速度；

定义水下航行器对水速度向量为：

v_w＝[v_wx v_wy v_wz]^T

其中：v_wx，v_wy，v_wz分别为水下局部惯性坐标系中x，y，z三个方向水下航行器与水的相对速度，通过姿态航向参考系统以及多普勒测速仪所测数据计算得到，计算公式为：

其中：

为多普勒测速仪测得的随体坐标系下水下航行器与水的相对速度矢量，

为随体坐标系向局部惯性坐标系旋转的旋转矩阵，其矩阵元素与姿态航向参考系统所测得的水下航行器姿态角、航向角相关；

的计算公式为：

其中：

θ,ψ分别为水下航行器的横滚角、俯仰角及航向角，由姿态航向参考系统测得；

记v_e为水下未知的有效声速；

求解未知p，v_c及v_e的时间导数，并考虑相应的不确定性，得到水下航行器的运动学模型：

其中：ω_p＝[ω_px ω_py ω_pz]^T为水下航行器在x，y，z方向的位置不确定性，ω_c＝[ω_cx ω_cy ω_cz]^T为x，y，z方向的海流不确定性；ω_e为有效声速不确定性。

在上述方案的基础上，具体的，所述步骤C中，观测模型的建立方法为：

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

记水声信标发射水声信号的时刻为T_e，记水声信标在水下局部惯性坐标系中的空间位置坐标为s＝[X_Te Y_Te Z_Te]^T，水下航行器接收到该水声信号的时刻为T_a，X_Te，Y_Te，Z_Te，T_e以及T_a均为已知量，观测方程为：

其中：v_t为对应的观测噪声；

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

根据多普勒测速仪测得的随体坐标系下水下航行器的绝对速度

结合姿态航向参考系统测得的水下航行器姿态和航向，计算得到水下航行器绝对速度在水下局部惯性坐标系下的表示：

其中：v_g＝[v_gx v_gy v_gz]^T为水下航行器绝对速度在局部惯性坐标系下x，y，z三个方向的分量；

根据v_g以及v_w，计算得到海流速度观测量为：

m_c＝v_g-v_w

其中：m_c＝[m_cx m_cy m_cz]^T表示三个方向的海流观测量；

海流观测方程为线性，满足m_c＝v_c+v_vc；

其中：v_vc为海流观测噪声向量，v_vc＝[v_vcx v_vcy v_vcz]^T，其中v_vcx为x方向的海流观测噪声；v_vcy为y方向的海流观测噪声；v_vcz为z方向的海流观测噪声；

S3.建立深度观测模型；

记水下航行器所搭载的深度计观测量为m_z，则其观测方程为

m_z＝ap+v_z

其中：a＝[0 0 1]，v_z为深度计观测噪声。

在上述方案的基础上，具体的，所述步骤C中，运动学模型以及观测模型离散化方法为：

S1.运动学模型离散化；

以变量加上下标符号k为离散时间索引，以ΔT为离散间隔，运动学模型离散为：

p_k+1＝p_k+ΔTv_c,k+u_k+ω_p,k

v_c,k+1＝v_c,k+ω_c,k

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

其中：u_k为控制向量，为已知量，由水下航行器根据多普勒测速仪及姿态航向参考系统观测量进行航位推算得到；ω_p,k,ω_c,k,ω_e,k表示离散状态下的过程噪声；

S2.观测模型离散化；

假设水下航行器在k时刻接收到水声信号，离散后的水声信号传递时间观测方程为：

其中，v_t,k为水声信号传递时间观测噪声；

假设在每一个离散时间点处均可以得到海流速度观测，离散后观测方程为：

m_vc，k＝v_c,k+v_vc,k

其中，v_vc,k为k时刻海流速度观测噪声；

同样，假设在每一个离散时间点处均可以得到深度计观测，离散后观测方程为：

m_z,k＝ap_k+v_z,k

其中，v_z,k为k时刻深度计观测噪声。

在上述方案的基础上，具体的，所述步骤D包括：

S1.运动学模型处理；

定义离散状态变量：

根据离散时间的水下航行器运动学模型，可以得到：

x_1,k+1＝x_1,k+u_kx_3,k+ΔTx_2,k+ω_1,k

x_2,k+1＝x_2,k+ω_2,k

x_3,k+1＝x_3,k+ω_3,k

其中：ω_1,k,ω_2,k,ω_3,k分别为对应的过程噪声；

进一步定义离散状态变量：

得到：

x_5,k+1＝x_5,k+ω_5,k

其中：ω_4,k,ω_5,k,ω_6,k分别为对应的过程噪声；

由离散状态的水声信号传递时间观测模型可得，在无观测噪声的前提下，有 m_t,k＝x_4,k,m_t,k+1＝x_4,k+1；在k+1时刻，水下航行器获得m_t,k+1后，将其看作已知量，作为线性增广模型扩散方程的参数，可以得到：

定义状态向量及噪声向量：

则x_k的运动学方程为：

x_k+1＝A_kx_k+ω_k

其中：

其中：I₃表示三维的单位矩阵，0_m×n表示元素均为0，维度为m行n列的矩阵；

S2.观测模型处理；

由x_4,k的定义可得：

m_t,k＝x_4,k+v_t,k

由x_1,k,x_2,k,x_3,k,x_5,k,x_6,k的定义可得：

x_2,k＝v_c,kx_3,k

当水下航行器获得海流观测m_vc，k时，将其看作已知量，结合海流观测方程，可得：

0＝x_2,k-m_vc,kx_3,k+v_s1,k

其中：v_s,1，v_s,2及v_s,3为对应的观测噪声，其与海流观测噪声和线性增广模型状态的关系为：

v_s1,k＝v_vc,kx_3,k

同样，由x_1,k,x_3,k的定义以及深度计观测方程，可得：

0＝ax_1,k-m_z,kx_3,k+v_s4,k

其中：v_s,4为对应观测噪声，且v_s4,k＝v_z,kx_3,k；

构造观测向量及观测噪声向量为：

对应的观测方程为：

m_k＝C_kx_k+v_k

其中：

在上述方案的基础上，具体的，所述步骤E中，水下航行器进行航位推算的方法为：

记航位推算的周期为ΔT_DR，ΔT_DR由姿态航向参考系统以及多普勒测速仪二者采样周期较大值决定，ΔT_DR小于线性增广模型离散时间间隔ΔT；

在k时刻至k+1时刻时间段内，局部惯性坐标系下水下航行器对水相对运动矢量为：

其中：

采用矩形法进行数值积分，得到：

其中：v_w,i表示k时刻至k+1时刻时间间隔内水下航行器对水速度的各个采样值，n表示 k时刻至k+1时刻时间间隔内总航位推算采样数。

在上述方案的基础上，具体的，所述步骤E中，通过Kalman滤波进行定位模型的预测以及更新方法为：

S1.预测过程；

水下航行器在k+1时刻接收到水声信号，计算出该时刻的水声信号传递时间m_t,k+1；同时，由航位推算可以得到u_k，进而构造出相应的增广线性模型矩阵A_k；由Kalman滤波的预测环节，可得增广线性模型状态先验估计为：

其中：

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

与P_k+1|k分别为k+1时刻的先验状态和先验方差；Q_k为k时刻的过程噪声协方差矩阵，为对称正定阵，其具体参数由过程噪声ω_k的统计特性决定，可通过线下调制获得；

S2.更新过程；

水下航行器在k+1时刻接收到水声信号，计算出该时刻的水声信号传递时间m_t,k+1；同时，根据多普勒测速仪以及姿态航向参考系统的观测值计算出k+1时刻的海流速度m_vc,k+1；根据深度计观测值得到k+1时刻的水下航行器的深度m_z,k+1；构造观测矩阵C_k+1及观测向量 m_k+1，根据Kalman滤波的更新环节，可得增广线性系统状态后验估计为：

P_k+1|k+1＝P_k+1|k-K_k+1C_k+1P_k+1|k

其中：K_k+1为Kalman增益；R_k+1为k+1时刻的观测噪声协方差矩阵，为对称正定阵，其具体参数由观测噪声v_k的统计特性决定，可通过线下调制获得。

在上述方案的基础上，进一步的，根据所述步骤E所得到的增广线性模型后验状态估计，可以计算得到原始非线性模型后验状态估计，计算方法为：

其中：

与

分别为k+1时刻有效声速、位置以及海流速度的后验估计；为了进一步保证定位模型的稳定性，对有效声速的估计进行限幅，即选择：

其中：v_m与v_M分别为有效声速的下界及上界，根据实际情况设定；sat(x,a,b)为限幅函数，其输出为：

有益效果：本发明通过状态增广，将水下单信标定位模型中非线性的水声信号传递时间观测模型转化为线性时变模型，构造出线性时变单信标定位模型，通过Kalman滤波进行位置解算。在保证系统可观测的前提下，本发明具有全局收敛性，也就是说，当水下航行器的初始位置误差或某一时刻的位置误差较大时，本发明所提出的方法仍可以保证定位误差收敛性。

附图说明

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

图2为本发明与传统基于扩展Kalman滤波(EKF)进行位置解算的水下单信标定位方法所得均方定位误差比较；

图3为本发明与传统基于扩展Kalman滤波(EKF)进行位置解算的水下单信标定位方法所得均方有效声速误差比较。

具体实施方式

实施例1，参见附图1，一种具有全局收敛性的水下航行器单信标定位方法，水下航行器搭载有水听器、多普勒测速仪、深度计、姿态航向参考系统及GPS；水声信标周期性广播水声信号；所述方法包括以下步骤：

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

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

C.建立水下航行器的运动学模型以及观测模型并进行离散化，构建非线性单信标定位模型。

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

定义位置向量为：

p＝[x y z]^T

其中：x，y，z为水下航行器在水下局部惯性坐标系中的空间位置坐标；

定义海流速度向量为：

v_c＝[v_cx v_cy v_cz]^T

其中：v_cx，v_cy，v_cz为水下局部惯性坐标系中x，y，z三个方向未知的海流速度；

定义水下航行器对水速度向量为：

v_w＝[v_wx v_wy v_wz]^T

其中：v_wx，v_wy，v_wz分别为水下局部惯性坐标系中x，y，z三个方向水下航行器与水的相对速度，通过姿态航向参考系统以及多普勒测速仪所测数据计算得到，计算公式为：

其中：

为多普勒测速仪测得的随体坐标系下水下航行器与水的相对速度矢量，

为随体坐标系向局部惯性坐标系旋转的旋转矩阵，其矩阵元素与姿态航向参考系统所测得的水下航行器姿态角、航向角相关；

的计算公式为：

其中：

θ,ψ分别为水下航行器的横滚角、俯仰角及航向角，由姿态航向参考系统测得；

记v_e为水下未知的有效声速；

求解未知p，v_c及v_e的时间导数，并考虑相应的不确定性，得到水下航行器的运动学模型：

其中：ω_p＝[ω_px ω_py ω_pz]^T为水下航行器在x，y，z方向的位置不确定性，ω_c＝[ω_cx ω_cy ω_cz]^T为x，y，z方向的海流不确定性；ω_e为有效声速不确定性。

观测模型的建立方法为：

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

记水声信标发射水声信号的时刻为T_e，记水声信标在水下局部惯性坐标系中的空间位置坐标为s＝[X_Te Y_Te Z_Te]^T，水下航行器接收到该水声信号的时刻为T_a，X_Te，Y_Te，Z_Te，T_e以及T_a均为已知量，观测方程为：

其中：v_t为对应的观测噪声；

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

根据多普勒测速仪测得的随体坐标系下水下航行器的绝对速度

结合姿态航向参考系统测得的水下航行器姿态和航向，计算得到水下航行器绝对速度在水下局部惯性坐标系下的表示：

其中：v_g＝[v_gx v_gy v_gz]^T为水下航行器绝对速度在局部惯性坐标系下x，y，z三个方向的分量；

根据v_g以及v_w，计算得到海流速度观测量为：

m_c＝v_g-v_w

其中：m_c＝[m_cx m_cy m_cz]^T表示三个方向的海流观测量；

海流观测方程为线性，满足m_c＝v_c+v_vc；

其中：v_vc为海流观测噪声向量，v_vc＝[v_vcx v_vcy v_vcz]^T，其中v_vcx为x方向的海流观测噪声；v_vcy为y方向的海流观测噪声；v_vcz为z方向的海流观测噪声；

S3.建立深度观测模型；

记水下航行器所搭载的深度计观测量为m_z，则其观测方程为

m_z＝ap+v_z

其中：a＝[0 0 1]，v_z为深度计观测噪声。

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

S1.运动学模型离散化；

以变量加上下标符号k为离散时间索引，以ΔT为离散间隔，运动学模型离散为：

p_k+1＝p_k+ΔTv_c,k+u_k+ω_p,k

v_c,k+1＝v_c,k+ω_c,k

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

其中：u_k为控制向量，为已知量，由水下航行器根据多普勒测速仪及姿态航向参考系统观测量进行航位推算得到；ω_p,k,ω_c,k,ω_e,k表示离散状态下的过程噪声；

S2.观测模型离散化；

假设水下航行器在k时刻接收到水声信号，离散后的水声信号传递时间观测方程为：

其中，v_t,k为水声信号传递时间观测噪声；

假设在每一个离散时间点处均可以得到海流速度观测，离散后观测方程为：

m_vc，k＝v_c,k+v_vc,k

其中，v_vc,k为k时刻海流速度观测噪声；

同样，假设在每一个离散时间点处均可以得到深度计观测，离散后观测方程为：

m_z,k＝ap_k+v_z,k

其中，v_z,k为k时刻深度计观测噪声。

D.通过状态增广，将离散状态的非线性单信标定位模型转化为线性时变模型。

S1.运动学模型处理；

定义离散状态变量：

根据离散时间的水下航行器运动学模型，可以得到：

x_1,k+1＝x_1,k+u_kx_3,k+ΔTx_2,k+ω_1,k

x_2,k+1＝x_2,k+ω_2,k

x_3,k+1＝x_3,k+ω_3,k

其中：ω_1,k,ω_2,k,ω_3,k分别为对应的过程噪声；

进一步定义离散状态变量：

得到：

x_5,k+1＝x_5,k+ω_5,k

其中：ω_4,k,ω_5,k,ω_6,k分别为对应的过程噪声；

由离散状态的水声信号传递时间观测模型可得，在无观测噪声的前提下，有 m_t,k＝x_4,k,m_t,k+1＝x_4,k+1；在k+1时刻，水下航行器获得m_t,k+1后，将其看作已知量，作为线性增广模型扩散方程的参数，可以得到：

定义状态向量及噪声向量：

则x_k的运动学方程为：

x_k+1＝A_kx_k+ω_k

其中：

其中：I₃表示三维的单位矩阵，0_m×n表示元素均为0，维度为m行n列的矩阵；

S2.观测模型处理；

由x_4,k的定义可得：

m_t,k＝x_4,k+v_t,k

由x_1,k,x_2,k,x_3,k,x_5,k,x_6,k的定义可得：

x_2,k＝v_c,kx_3,k

当水下航行器获得海流观测m_vc，k时，将其看作已知量，结合海流观测方程，可得：

0＝x_2,k-m_vc,kx_3,k+v_s1,k

其中：v_s,1，v_s,2及v_s,3为对应的观测噪声，其与海流观测噪声和线性增广模型状态的关系为：

v_s1,k＝v_vc,kx_3,k

同样，由x_1,k,x_3,k的定义以及深度计观测方程，可得：

0＝ax_1,k-m_z,kx_3,k+v_s4,k

其中：v_s,4为对应观测噪声，且v_s4,k＝v_z,kx_3,k；

构造观测向量及观测噪声向量为：

对应的观测方程为：

m_k＝C_kx_k+v_k

其中：

E.水下航行器在未接收到水声信号时，通过姿态航向参考系统以及多普勒测速仪获得水下航行器与水相对速度与姿态进行航位推算；水下航行器接收到水声信号后，通过已知的水声信号发射时间获得水声信号传递时间，将其作为观测变量，同时综合航位推算数据以及各种传感器观测数据，基于Kalman滤波进行单信标定位系统的预测以及更新。

水下航行器进行航位推算的方法为：

记航位推算的周期为ΔT_DR，ΔT_DR由姿态航向参考系统以及多普勒测速仪二者采样周期较大值决定，ΔT_DR小于线性增广模型离散时间间隔ΔT；

在k时刻至k+1时刻时间段内，局部惯性坐标系下水下航行器对水相对运动矢量为：

其中：

采用矩形法进行数值积分，得到：

其中：v_w,i表示k时刻至k+1时刻时间间隔内水下航行器对水速度的各个采样值，n表示 k时刻至k+1时刻时间间隔内总航位推算采样数。

通过Kalman滤波进行定位模型的预测以及更新方法为：

S1.预测过程；

水下航行器在k+1时刻接收到水声信号，计算出该时刻的水声信号传递时间m_t,k+1；同时，由航位推算可以得到u_k，进而构造出相应的增广线性模型矩阵A_k；由Kalman滤波的预测环节，可得增广线性模型状态先验估计为：

其中：

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

与P_k+1|k分别为k+1时刻的先验状态和先验方差；Q_k为k时刻的过程噪声协方差矩阵，为对称正定阵，其具体参数由过程噪声ω_k的统计特性决定，可通过线下调制获得；

S2.更新过程；

水下航行器在k+1时刻接收到水声信号，计算出该时刻的水声信号传递时间m_t,k+1；同时，根据多普勒测速仪以及姿态航向参考系统的观测值计算出k+1时刻的海流速度m_vc,k+1；根据深度计观测值得到k+1时刻的水下航行器的深度m_z,k+1；构造观测矩阵C_k+1及观测向量 m_k+1，根据Kalman滤波的更新环节，可得增广线性系统状态后验估计为：

P_k+1|k+1＝P_k+1|k-K_k+1C_k+1P_k+1|k

其中：K_k+1为Kalman增益；R_k+1为k+1时刻的观测噪声协方差矩阵，为对称正定阵，其具体参数由观测噪声v_k的统计特性决定，可通过线下调制获得。

根据所述步骤E所得到的增广线性模型后验状态估计，可以计算得到原始非线性模型后验状态估计，计算方法为：

其中：

与

分别为k+1时刻有效声速、位置以及海流速度的后验估计；为了进一步保证定位模型的稳定性，对有效声速的估计进行限幅，即选择：

其中：v_m与v_M分别为有效声速的下界及上界，根据实际情况设定；sat(x,a,b)为限幅函数，其输出为：

实施例2，通过仿真数据对实施例1所述的方法进行验证。

作为比较，本实施例同时展示了传统基于扩展Kalman滤波(EKF)进行位置解算的水下单信标定位方法的定位结果。总体仿真时长为5000秒，在整个运动过程中仿真的有效声速为恒定的1500米/秒，三个方向的海流速度均为0.1米/秒。仿真的水声信号发射周期为10秒，同样，海流观测周期、深度观测周期与系统离散周期也为10秒。航位推算的周期为0.1s(相当于10Hz采样频率)。

仿真的各个传感器噪声参数如下表

在数值验证的过程中，滤波器初始参数设置为：(1)x与y两个方向位置初始误差均为 100米；(2)z方向位置初始误差为10m；(3)x，y与z三个方向的海流初值均为0米/秒； (4)有效声速的初始值为1400米/秒；(5)海流不确定性标准差为0.1米/秒；(6)航行器对水速度观测不确定性标准差为10^-5米/秒；(7)所提出方法有效声速不确定性的标准差为10^-15米/秒，基于EKF的传统水下单信标定位系统有效声速不确定性的标准差为10^-5米/秒；(8) 水声信号传递时间不确定性的标准差为10^-15米/秒；(9)水声信号传递时间观测噪声标准差为10^-5秒；(10)海流观测噪声标准差为0.02米/秒；(11)深度计观测噪声标准差为0.001米；(12)有效声速的上下界v_M与v_m分别为1600米/秒与1400米/秒。500独立的次Monte Carlo 仿真的结果被用来验证本发明所提出的方法。两种定位方法定位性能评价指标为均方定位误差RMS_ΔH以及均方有效声速误差

二者计算方式如下：

其中：

及

分别为第i次Monte Carlo仿真中真实以及估计的航行器位置坐标，

及

分别为第i次Monte Carlo仿真中真实以及估计的有效声速，M＝500表示总的仿真次数。根据图2以及图3，可以看出在初始位置误差较大的情况下，所提出的方法可以较快的收敛到一个较小值，而传统基于EKF的单信标定位方法无法收敛，会出现滤波发散。

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

Claims (4)

1.一种具有全局收敛性的水下航行器单信标定位方法，水下航行器搭载有水听器、多普勒测速仪、深度计、姿态航向参考系统及GPS；水声信标周期性广播水声信号；其特征在于，所述方法包括以下步骤：

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

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

C.建立水下航行器的运动学模型以及观测模型并进行离散化，构建非线性单信标定位模型；

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

定义位置向量为：

p＝[x y z]^T

其中：x，y，z为水下航行器在水下局部惯性坐标系中的空间位置坐标；

定义海流速度向量为：

v_c＝[v_cx v_cy v_cz]^T

其中：v_cx，v_cy，v_cz为水下局部惯性坐标系中x，y，z三个方向未知的海流速度；

定义水下航行器对水速度向量为：

v_w＝[v_wx v_wy v_wz]^T

其中：v_wx，v_wy，v_wz分别为水下局部惯性坐标系中x，y，z三个方向水下航行器与水的相对速度，通过姿态航向参考系统以及多普勒测速仪所测数据计算得到，计算公式为：

其中：

为多普勒测速仪测得的随体坐标系下水下航行器与水的相对速度矢量，

为随体坐标系向局部惯性坐标系旋转的旋转矩阵，其矩阵元素与姿态航向参考系统所测得的水下航行器姿态角、航向角相关；

的计算公式为：

其中：

θ,ψ分别为水下航行器的横滚角、俯仰角及航向角，由姿态航向参考系统测得；

记v_e为水下未知的有效声速；

求解未知p，v_c及v_e的时间导数，并考虑相应的不确定性，得到水下航行器的运动学模型：

其中：ω_p＝[ω_px ω_py ω_pz]^T为水下航行器在x，y，z方向的位置不确定性，ω_c＝[ω_cxω_cy ω_cz]^T为x，y，z方向的海流不确定性；ω_e为有效声速不确定性；

观测模型的建立方法为：

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

记水声信标发射水声信号的时刻为T_e，记水声信标在水下局部惯性坐标系中的空间位置坐标为s＝[X_Te Y_Te Z_Te]^T，水下航行器接收到该水声信号的时刻为T_a，X_Te，Y_Te，Z_Te，T_e以及T_a均为已知量，观测方程为：

其中：v_t为对应的观测噪声；

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

根据多普勒测速仪测得的随体坐标系下水下航行器的绝对速度

结合姿态航向参考系统测得的水下航行器姿态和航向，计算得到水下航行器绝对速度在水下局部惯性坐标系下的表示：

其中：v_g＝[v_gx v_gy v_gz]^T为水下航行器绝对速度在局部惯性坐标系下x，y，z三个方向的分量；

根据v_g以及v_w，计算得到海流速度观测量为：

m_c＝v_g-v_w

其中：m_c＝[m_cx m_cy m_cz]^T表示三个方向的海流观测量；

海流观测方程为线性，满足m_c＝v_c+v_vc；

其中：v_vc为海流观测噪声向量，v_vc＝[v_vcx v_vcy v_vcz]^T，其中v_vcx为x方向的海流观测噪声；v_vcy为y方向的海流观测噪声；v_vcz为z方向的海流观测噪声；

S3.建立深度观测模型；

记水下航行器所搭载的深度计观测量为m_z，则其观测方程为

m_z＝ap+v_z

其中：a＝[0 0 1]，v_z为深度计观测噪声；

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

S1.运动学模型离散化；

以变量加上下标符号k为离散时间索引，以ΔT为离散间隔，运动学模型离散为：

p_k+1＝p_k+ΔTv_c,k+u_k+ω_p,k

v_c,k+1＝v_c,k+ω_c,k

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

其中：u_k为控制向量，为已知量，由水下航行器根据多普勒测速仪及姿态航向参考系统观测量进行航位推算得到；ω_p,k,ω_c,k,ω_e,k表示离散状态下的过程噪声；

S2.观测模型离散化；

假设水下航行器在k时刻接收到水声信号，离散后的水声信号传递时间观测方程为：

其中，v_t,k为水声信号传递时间观测噪声；

假设在每一个离散时间点处均能够得到海流速度观测，离散后观测方程为：

m_vc，k＝v_c,k+v_vc,k

其中，v_vc,k为k时刻海流速度观测噪声；

同样，假设在每一个离散时间点处均能够得到深度计观测，离散后观测方程为：

m_z,k＝ap_k+v_z,k

其中，v_z,k为k时刻深度计观测噪声；

D.通过状态增广，将离散状态的非线性单信标定位模型转化为线性时变模型；

S1.运动学模型处理；

定义离散状态变量：

根据离散时间的水下航行器运动学模型，得到：

x_1,k+1＝x_1,k+u_kx_3,k+ΔTx_2,k+ω_1,k

x_2,k+1＝x_2,k+ω_2,k

x_3,k+1＝x_3,k+ω_3,k

其中：ω_1,k,ω_2,k,ω_3,k分别为对应的过程噪声；

进一步定义离散状态变量：

得到：

x_5,k+1＝x_5,k+ω_5,k

其中：ω_4,k,ω_5,k,ω_6,k分别为对应的过程噪声；

由离散状态的水声信号传递时间观测模型得到，在无观测噪声的前提下，有m_t,k＝x_4,k,m_t,k+1＝x_4,k+1；在k+1时刻，水下航行器获得m_t,k+1后，将其看作已知量，作为线性增广模型扩散方程的参数，得到：

定义状态向量及噪声向量：

则x_k的运动学方程为：

x_k+1＝A_kx_k+ω_k

其中：

其中：I₃表示三维的单位矩阵，0_m×n表示元素均为0，维度为m行n列的矩阵；

S2.观测模型处理；

由x_4,k的定义得到：

m_t,k＝x_4,k+v_t,k

由x_1,k,x_2,k,x_3,k,x_5,k,x_6,k的定义得到：

x_2,k＝v_c,kx_3,k

当水下航行器获得海流观测m_vc，k时，将其看作已知量，结合海流观测方程，得到：

0＝x_2,k-m_vc,kx_3,k+v_s1,k

其中：v_s,1，v_s,2及v_s,3为对应的观测噪声，其与海流观测噪声和线性增广模型状态的关系为：

v_s1,k＝v_vc,kx_3,k

同样，由x_1,k,x_3,k的定义以及深度计观测方程，得到：

0＝ax_1,k-m_z,kx_3,k+v_s4,k

其中：v_s,4为对应观测噪声，且v_s4,k＝v_z,kx_3,k；

构造观测向量及观测噪声向量为：

对应的观测方程为：

m_k＝C_kx_k+v_k

其中：

E.水下航行器在未接收到水声信号时，通过姿态航向参考系统以及多普勒测速仪获得水下航行器与水相对速度与姿态进行航位推算；水下航行器接收到水声信号后，通过已知的水声信号发射时间获得水声信号传递时间，将其作为观测变量，同时综合航位推算数据以及各种传感器观测数据，基于Kalman滤波进行单信标定位系统的预测以及更新。

2.如权利要求1所述的一种具有全局收敛性的水下航行器单信标定位方法，其特征在于，所述步骤E中，水下航行器进行航位推算的方法为：

记航位推算的周期为ΔT_DR，ΔT_DR由姿态航向参考系统以及多普勒测速仪二者采样周期较大值决定，ΔT_DR小于线性增广模型离散时间间隔ΔT；

在k时刻至k+1时刻时间段内，局部惯性坐标系下水下航行器对水相对运动矢量为：

其中：

采用矩形法进行数值积分，得到：

其中：v_w,i表示k时刻至k+1时刻时间间隔内水下航行器对水速度的各个采样值，n表示k时刻至k+1时刻时间间隔内总航位推算采样数。

3.如权利要求2所述的一种具有全局收敛性的水下航行器单信标定位方法，其特征在于，所述步骤E中，通过Kalman滤波进行定位模型的预测以及更新方法为：

S1.预测过程；

水下航行器在k+1时刻接收到水声信号，计算出该时刻的水声信号传递时间m_t,k+1；同时，由航位推算得到u_k，进而构造出相应的增广线性模型矩阵A_k；由Kalman滤波的预测环节，得到增广线性模型状态先验估计为：

其中：

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

与P_k+1|k分别为k+1时刻的先验状态和先验方差；Q_k为k时刻的过程噪声协方差矩阵，为对称正定阵，其具体参数由过程噪声ω_k的统计特性决定，通过线下调制获得；

S2.更新过程；

水下航行器在k+1时刻接收到水声信号，计算出该时刻的水声信号传递时间m_t,k+1；同时，根据多普勒测速仪以及姿态航向参考系统的观测值计算出k+1时刻的海流速度m_vc,k+1；根据深度计观测值得到k+1时刻的水下航行器的深度m_z,k+1；构造观测矩阵C_k+1及观测向量m_k+1，根据Kalman滤波的更新环节，得到增广线性系统状态后验估计为：

P_k+1|k+1＝P_k+1|k-K_k+1C_k+1P_k+1|k

其中：K_k+1为Kalman增益；R_k+1为k+1时刻的观测噪声协方差矩阵，为对称正定阵，其具体参数由观测噪声v_k的统计特性决定，通过线下调制获得。

4.如权利要求3所述的一种具有全局收敛性的水下航行器单信标定位方法，其特征在于，根据所述步骤E所得到的增广线性模型后验状态估计，计算得到原始非线性模型后验状态估计，计算方法为：

其中：

与

分别为k+1时刻有效声速、位置以及海流速度的后验估计；为了进一步保证定位模型的稳定性，对有效声速的估计进行限幅，即选择：

其中：v_m与v_M分别为有效声速的下界及上界，根据实际情况设定；sat(x,a,b)为限幅函数，其输出为：

Images