[发明专利]一种水下鲁棒自适应单信标定位方法

权利要求书

1.一种水下鲁棒自适应单信标定位方法，其特征在于，包括以下步骤：

A.根据航行器位置、海流速度以及有效声速不确定性噪声特性的不同，将系统状态分为位置状态以及参数状态，建立离散形式的运动学模型以及观测模型；

B.将位置状态建模为Student’s t分布，参数状态先验分布建模为高斯分布；同时考虑到观测野值，将海流观测噪声与水声信号传递时间观测噪声建模为Student’s t分布；将所有的高斯分布与Student’s t分布的方差矩阵、尺度矩阵和自由度参数看作随机变量，其先验分布建模为其对应的共轭先验；引入满足Gamma分布的辅助参数，将所有的Student’s t分布分解为分层高斯的形式；

C.水声信标周期性广播水声信号，其位置坐标以及信号发射时间已知；采用序贯更新的方式，对于每一个时间历元，执行如下步骤：

C1.当航行器接收到自身螺旋桨转速提供的航行器对水速度以及罗盘提供的航行器艏向角时，进行航位推算，根据位置状态的运动学模型得到位置状态的预测值；根据参数状态的运动学模型得到参数状态的预测值；根据噪声统计参数的慢时变特性，对噪声参数的超参数进行预测；

C2.当航行器接收到海流观测时，基于变分贝叶斯，即Variational Bayes，VB近似进行海流速度相关变量的更新，需要更新的变量包括：参数状态，参数状态的先验方差矩阵，海流观测噪声尺度矩阵，海流观测噪声自由度参数，海流观测辅助参数；

C3.当航行器接收到信标发射的水声信号时，根据信号接收的时刻以及已知的水声信号发射时刻，计算水声信号传递时间，基于VB近似进行水声信号传递时间相关变量的更新，水声信号传递时间相关变量包括：位置状态、参数状态、参数状态的先验方差矩阵、位置状态的先验分布尺度矩阵、水声信号传递时间观测噪声尺度矩阵、位置状态先验分布自由度参数、水声信号传递时间观测噪声自由度参数、位置状态先验分布辅助参数、水声信号传递时间观测辅助参数。

2.如权利要求1所述的一种水下鲁棒自适应单信标定位方法，其特征在于，所述步骤A采用以下方法：

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

其中：下标k表示变量在第k个时间历元；x_k，y_k为第k个时间历元水下航行器在水下局部惯性坐标系中的水平位置；

定义参数状态：

其中：v_cx,k，v_cy,k为第k个时间历元x与y方向的海流速度；v_e,k表示第k个时间历元的水声声速；

考虑水下航行器运动过程中模型噪声影响，建立位置状态的离散形式运动学模型为：

p_k+1＝p_k+Δt(v_w,k+H_cx_k)+ω_p,k

其中：Δt为离散时间间隔，v_w,k＝[v_wx,k v_wy,k]^T为航行器与水的相对速度矢量，v_wx,k与v_wy,k分别表示航行器与水的相对速度在x与y方向的投影值，为位置状态过程噪声矢量，H_c表达式为：

将航行器与水的相对速度矢量v_w,k看作已知的输入，可由下式计算得出：

其中：

为随体坐标系到水下局部惯性坐标系之间的旋转矩阵，为航行器艏向角，由电子罗盘测量得出，为航行器与水的相对速度在随体坐标系下的投影，由自身螺旋桨转速估计得到；

根据参数状态的慢时变特性，建立参数状态的离散形式运动学模型为：

x_k+1＝x_k+ω_x,k

其中：为参数状态过程噪声矢量；

根据多普勒测速仪，即Doppler Velocity Log,DVL测得的随体坐标系下水下航行器的绝对速度结合电子罗盘测得的艏向角计算得到水下航行器绝对速度在局部惯性坐标系下的表示

进一步，计算得到海流观测值为：

v_cm,k＝v_g,k-v_w,k

海流观测方程为：

v_cm,k＝H_cx_k+ν_c,k；

其中：为海流观测噪声；

水声信标周期性广播水声信号，水下航行器接收到水声信号后根据信号接收时刻以及水声信号发射时刻计算水声信号传递时间为：

T_tm,k＝T_a,k-T_e,k

其中：T_tm,k为水声信号传递时间，T_a,k为水下航行器测得的水声信号接收时刻，T_e,k为已知的水声信号发射时刻；

水声信号传递时间的观测方程为：

T_tm,k＝h_k(p_k,x_k)+ν_t,k

其中：

其中：b_k＝[b_x,k b_y,k]^T为已知的水声信标位置坐标，z_k与b_z,k分别为水下航行器与水声信标的深度坐标，假设其均由深度计精确测得，为已知量，x_k(3)表示矢量x_k的第三个维度值，ν_t,k为水声信号传递时间的观测噪声。