[发明专利]考虑量测异常的集群式多深海潜航器的协同定位方法

权利要求书

1.一种考虑量测异常的威力集群式多深海潜航器协同定位方法，其特征在于：包括以下步骤：

S1：建立从深海潜航器在当地地理坐标系下的状态误差方程和考虑主从深海潜航器的相对距离测量信息异常的量测方程；

S2：根据多深海潜航器协同定位的特点以及Krein空间线性估计理论，引入乘性噪声和参数不确定性，在考虑量测异常的情况下设计新形式Krein空间的协同导航的鲁棒后验滤波方程；

S3：设计自适应算法对滤波器建模的参数不确定性进行在线估计和补偿，提高协同定位物理过程与所建数学模型的匹配度。

2.如权利要求1所述的一种考虑量测异常的威力集群式多深海潜航器协同定位方法，其特征在于：所述步骤S1具体包括如下步骤：

S1-1:考虑一个15维系统状态量的主从潜航器协同定位离散系统：

其中，Δθ,Δφ,Δω表示从潜航器的姿态角误差，Δv_E,Δv_N,Δv_U代表从潜航器的东北天向速度误差，ΔL,Δλ,Δh表示从潜航器的纬度、经度、高度误差，代表从潜航器中惯性加计的常值偏移，代表从潜航器中惯性陀螺仪的常值偏移；

从潜航器惯导系统在当地地理坐标系下的离散时间误差方程为：

其中，为状态转移矩阵，X_k∈Rⁿ是为状态量，k是滤波时刻，τ_s是离散时间间隔，是从载体坐标系到导航坐标系的转换矩阵。且为捷联惯导系统连续时间误差方程中相应的系统矩阵；

S1-2:考虑主从深海潜航器的相对距离测量信息异常的量测方程为：

其中，Z_k∈R^m是为外信息量，L_m、λ_m、h_m为主深海潜航器的位置，L_s、λ_s、h_s为从深海潜航器的位置，H_k＝[Ο_3x6 I_3x3 Ο_3x6]为量测矩阵，ΔH_k为不确定参数阵，Γ为乘性噪声适配矩阵，v_1,k、v_2,k分别为互不相关的量测高斯白噪声。ΔR_λ、ΔR_L、ΔR_h为主从深海潜航器的东向相对距离、北向相对距离、垂向相对距离；

由于水下声波波速V的不确定性问题，会导致主从深海潜航器间相对距离测量的不确定：

其中，ΔV表示水下声波波速V的不确定，τ_L、τ_λ、τ_h表示水下声波传输时间。