CN113776525B

CN113776525B - 一种基于斜距差匹配的惯性/单声源被动组合导航方法

Info

Publication number: CN113776525B
Application number: CN202111019597.0A
Authority: CN
Inventors: 张涛; 张亮; 夏茂栋; 张佳宇; 刘射德
Original assignee: Southeast University
Current assignee: Southeast University
Priority date: 2021-09-01
Filing date: 2021-09-01
Publication date: 2023-12-05
Anticipated expiration: 2041-09-01
Also published as: CN113776525A

Abstract

本发明公开了一种基于斜距差匹配的惯性/单声源被动组合导航方法，本发明针对传统声学主动定位方式下信息容易暴露的缺点，采用一种单声源被动定位方式辅助惯性导航的组合导航技术，声源按照固定周期向外发射信号，通过接收到信号的时间差完成定位。对于航行器来说，只接收外界的声信号，而可以在不对外辐射信号的情况下实现自主定位，有效地提高了水下航行器导航定位的隐蔽性和安全性。

Description

一种基于斜距差匹配的惯性/单声源被动组合导航方法

技术领域

本发明涉及水下基于时间到达差定位方式的被动导航技术领域，以一种斜距差匹配方式实现高精度的被动导航定位功能，特别是涉及一种基于斜距差匹配的惯性/单声源被动组合导航方法。

背景技术

惯性导航系统(Inertial Navigation System，INS)由于其自主性好、隐蔽性强、短时间内精度高等优点，已广泛应用于水下航行器的自主导航与定位。惯性导航可以全天候的提供姿态、速度、位置的全方位信息，拥有其他导航传感器不可比拟的优点，因此成为水下航行器导航定位的首选。虽然惯性导航技术日益成熟，然而其定位误差随时间积累需要定期重调以保证一定精度的这一根本特性却无法改变，因此，世界各国都在探索惯性导航的系统误差校准方法。

由于声波在水下的传输距离远，信号衰减损失小，因而水声导航被广泛应用于水下航行器的导航与定位中。传统基于时间到达方式，需要收发两端设备按照严格的时钟同步，时钟偏置误差将对定位结果产生较大的影响。而基于时间到达差的定位技术，不需要严格的时钟同步，而是以各水听器接收到的声源信号到达时间差作为计算依据，因此可消除对严格时钟同步的依赖性。由于航行器只接收声信号，不需要向外发送询问信号，因此，是一种低成本、高效的被动定位方式。

综上，基于时间到达差的方式对惯性导航的误差进行校正，可以有效提高水下的导航定位精度。但是其难点在于，根据一个信号周期内的时间到达差得到的斜距差，往往变化很小，难以实现对位置误差的精确校正作用。因此，本发明提出一种斜距差匹配方式的惯性/单声源被动组合导航定位方法。通过建立滑动窗构建合适长度的虚拟基阵，采用基阵内的变化较大的斜距差完成对惯性导航的位置误差校正功能。

发明内容

针以上问题，本发明旨在提供一种基于斜距差匹配的惯性/单声源被动组合导航方法，通过设置滑动窗，根据窗口内不同时刻接收到声信号的惯导位置建立虚拟基阵，选定滑动窗最后一帧窗口的位置为参考基元位置，并计算各个位置相对参考基元位置的时间达到差，根据时间到达差建立卡尔曼滤波的量测模型，以此达到高精度的组合导航的目的。

本发明提供一种基于斜距差匹配的惯性/单声源被动组合导航方法，包括如下步骤：

(1)建立惯性/单声源组合导航的状态方程模型；

步骤(1)中，建立惯性/单声源组合导航的状态方程模型：

将惯导的位置误差、速度误差、姿态误差作为系统状态量，得：

其中，[φ_E φ_N φ_U]^T表示姿态的失准角误差，[δV_E δV_N δV_U]^T表示速度误差，[δL δλ δh]^T表示位置误差，[ε_x ε_y ε_z]^T表示陀螺仪的零偏，表示加速度计的零偏；

因此，组合导航的状态方程为：

其中，F(t)表示状态转移矩阵，根据捷联惯导误差方程设置，W(t)表示过程噪声矩阵；

(2)建立滑动窗，在滑动窗内基于惯导的位置构建虚拟基阵；

步骤(2)中，建立滑动窗，在滑动窗内基于惯导的位置构建虚拟基阵，具体为：

设置滑动窗窗口大小，窗口内所有接收到声信号时刻的惯导位置共同构成一个虚拟的基阵，以滑动窗内的最后一个惯导位置为参考位置，根据窗口内不同时刻位置下航行器接收到的声信号到达时间，计算相对于参考位置的到达时间差，根据已知的声速可以得到斜距差。在获得一组斜距差的基础上，确定一个最大斜距差以及其对应的信号接收时间，以最大斜距差作为滤波器的量测值。

(3)建立滑动窗内的惯性/单声源组合导航斜距差量测方程模型；

步骤(3)中，建立滑动窗内的惯性/单声源组合导航斜距差量测方程模型，具体为：

所确定的最大斜距差所在时刻为t_k，参考位置的时间为t_k+i，分别对应的惯导位置为和/>此时惯导对应的误差状态为/>和/>通过状态转移矩阵统一不同时刻的误差状态，得到：

其中，

考虑惯导误差情况下，滑动窗内的最大斜距差表示为：

其中，表示声源位置；

将在地球直角坐标系中对位置进行求解，他们在地球坐标系e系下的相对位置矢量p^e为：

其中，R_N为卯酉圈曲率半径，e为椭圆偏心率；

将p^e转换到导航系下，其矢量表示为：

其中，表示由地球坐标系到导航系的坐标转换矩阵；

考虑惯导位置误差的情况下，表示为：

其中，表示/>的误差，δpⁿ表示惯导位置误差，C_e表示为：

其中，R_Nh＝R_N+h，[L λ h]分别表示经度、纬度和高度。

B_S表示为

其中，S_a＝sin(a),C_a＝cos(a),a＝L或λ；

因此，斜距差表示为

其中，表示t_k时刻位置误差，/>表示t_k+i时刻位置误差，C_e,k+i表示t_k+i时刻的C_e矩阵值，/>表示t_k+i时刻的/>矩阵值，B_S,k+i表示t_k+i时刻的B_S矩阵值，

表示t_k+i时刻的p^e矩阵值，C_e,k表示t_k时刻的C_e矩阵值，/>表示t_k时刻的/>矩阵值，B_S,k表示t_k时刻的B_S矩阵值，/>

表示t_k时刻的p^e矩阵值，r₁表示声源与t_k时刻船的位置之间的真实距离，r₂表示声源与t_k+i时刻船的位置之间的真实距离；

测量得到的斜距差表示为：

其中，δr表示测量的误差；

因此，基于斜距差的量测方程表示为：

其中，H₁＝[0_1×6 H_PD1 0_1×6]，H₂＝[0_1×6 H_PD2 0_1×6]， {*}_7:9表示向量的第7-9列，/>

(4)根据(1)-(3)建立的状态方程以及量测方程，利用卡尔曼滤波方法进行误差的更新与反馈。

作为本发明进一步改进，步骤(4)中，具体为：

(a)状态一步预测：

中，表示k-1时刻的状态估计，F_k-1表示k-1时刻到k时刻的状态转移矩阵；

(b)状态一步预测均方误差：

其中，Q_k-1表示过程噪声矩阵，P_k-1表示k-1时刻的均方根误差；

(c)滤波增益：

其中，H_k表示k时刻的量测矩阵，R_k表示量测噪声矩阵；

(d)状态估计：

其中，z_k表示观测量；

(e)状态估计均方误差：P_k＝P_k-H_kH_kP_k|(k-1)。

有益效果：

根据以上步骤所建立的基于斜距差匹配的惯性/单声源被动组合导航定位方法，可以克服传统主动式定位中，信息易暴露，定位隐蔽性差的缺点，提高特殊应用环境下的定位安全性、隐蔽性。

附图说明

图1为滑动窗内虚拟基阵示意图。

具体实施方式

下面结合附图与具体实施方式对本发明作进一步详细描述：

本发明旨在提供一种基于斜距差匹配的惯性/单声源被动组合导航方法，通过设置滑动窗，根据窗口内不同时刻接收到声信号的惯导位置建立虚拟基阵，选定滑动窗最后一帧窗口的位置为参考基元位置，并计算各个位置相对参考基元位置的时间达到差，根据时间到达差建立卡尔曼滤波的量测模型，以此达到高精度的组合导航的目的。

本发明针对传统主动式声学定位信息易暴露的缺点，设计了基于斜距差匹配的惯性/单声源被动组合导航定位方法，其中滑动窗内虚拟基阵示意图如图1所示，

具体方法如下：

步骤1：建立惯性/单声源组合导航的状态方程模型，具体为：

其中，[φ_E φ_N φ_U]^T表示姿态的失准角误差，[δV_E δV_N δV_U]^T表示速度误差，[δL δλ δh]^T表示位置误差，[ε_x ε_y ε_z]^T表示陀螺仪的零偏，表示加速度计的零偏。

因此，组合导航的状态方程为：

其中，F(t)表示状态转移矩阵，根据捷联惯导误差方程设置，W(t)表示过程噪声矩阵。

步骤2：建立滑动窗，在滑动窗内基于惯导的位置构建虚拟基阵，具体为：

设置滑动窗窗口大小，窗口内所有接收到声信号时刻的惯导位置共同构成一个虚拟的基阵。以滑动窗内的最后一个惯导位置为参考位置，根据窗口内不同时刻位置下航行器接收到的声信号到达时间，计算相对于参考位置的到达时间差，根据已知的声速可以得到斜距差。在获得一组斜距差的基础上，确定一个最大斜距差以及其对应的信号接收时间，以最大斜距差作为滤波器的量测值。

步骤3：建立滑动窗内的惯性/单声源组合导航斜距差量测方程模型，具体为；

步骤2中所确定的最大斜距差所在时刻为t_k，参考位置的时间为t_k+i，分别对应的惯导位置为和/>此时惯导对应的误差状态为/>和/>通过状态转移矩阵统一不同时刻的误差状态，可以得到：

其中，

考虑惯导误差情况下，滑动窗内的最大斜距差可以表示为：

其中，表示声源位置。

其中，R_N为卯酉圈曲率半径，e为椭圆偏心率。

将p^e转换到导航系下，其矢量表示为：

其中，表示由地球坐标系到导航系的坐标转换矩阵。

考虑惯导位置误差的情况下，可以表示为：

其中，表示/>的误差，δpⁿ表示惯导位置误差，C_e表示为：

其中，R_Nh＝R_N+h，[_L λ h]分别表示经度、纬度和高度。

B_S表示为

其中，S_a＝sin(a),C_a＝cos(a),a＝L或λ。

因此，斜距差可以表示为

其中，表示t_k时刻位置误差，/>表示t_k+i时刻位置误差。C_e,k+i表示t_k+i时刻的C_e矩阵值，/>表示t_k+i时刻的/>矩阵值，B_S,k+i表示t_k+i时刻的B_S矩阵值，表示t_k+i时刻的p^e矩阵值。C_e,k表示t_k时刻的C_e矩阵值，/>表示t_k时刻的/>矩阵值，B_S,k表示t_k时刻的B_S矩阵值，/> 表示t_k时刻的p^e矩阵值。r₁表示声源与t_k时刻船的位置之间的真实距离，r₂表示声源与t_k+i时刻船的位置之间的真实距离

测量得到的斜距差可以表示为：

其中，δr表示测量的误差。

因此，基于斜距差的量测方程可以表示为：

其中，H₁＝[0_1×6 H_PD1 0_1×6]，H₂＝[0_1×6 H_PD2 0_1×6]，{*}_7:9表示向量的第7-9列。

步骤4：根据步骤1-3建立的状态方程以及量测方程，利用卡尔曼滤波方法进行误差的更新与反馈，具体为；

(a)状态一步预测：

其中，表示k-1时刻的状态估计，F_k-1表示k-1时刻到k时刻的状态转移矩阵。

(b)状态一步预测均方误差：

其中，Q_k-1表示过程噪声矩阵，P_k-1表示k-1时刻的均方根误差。

(c)滤波增益：

其中，H_k表示k时刻的量测矩阵，R_k表示量测噪声矩阵。

(d)状态估计：

其中，z_k表示观测量。

(e)状态估计均方误差：P_k＝P_k-K_kH_kP_k|(k-1)。

以上所述，仅是本发明的较佳实施例之一，并非是对本发明作任何其他形式的限制，而依据本发明的技术实质所作的任何修改或等同变化，仍属于本发明所要求保护的范围。

Claims

1.一种基于斜距差匹配的惯性/单声源被动组合导航方法，其特征在于，包括如下步骤：

(1)建立惯性/单声源组合导航的状态方程模型；

步骤(1)中，建立惯性/单声源组合导航的状态方程模型：

因此，组合导航的状态方程为：

(2)建立滑动窗，在滑动窗内基于惯导的位置构建虚拟基阵；

设置滑动窗窗口大小，窗口内所有接收到声信号时刻的惯导位置共同构成一个虚拟的基阵，以滑动窗内的最后一个惯导位置为参考位置，根据窗口内不同时刻位置下航行器接收到的声信号到达时间，计算相对于参考位置的到达时间差，根据已知的声速可以得到斜距差；在获得一组斜距差的基础上，确定一个最大斜距差以及其对应的信号接收时间，以最大斜距差作为滤波器的量测值；

其中，

考虑惯导误差情况下，滑动窗内的最大斜距差表示为：

其中，表示声源位置；

其中，R_N为卯酉圈曲率半径，e为椭圆偏心率；

将p^e转换到导航系下，其矢量表示为：

其中，表示由地球坐标系到导航系的坐标转换矩阵；

考虑惯导位置误差的情况下，表示为：

其中，表示/>的误差，δpⁿ表示惯导位置误差，C_e表示为：

其中，R_Nh＝R_N+h，[L λ h]分别表示经度、纬度和高度；

B_S表示为

其中，S_a＝sin(a)，C_a＝cos(a)，a＝L或λ；

因此，斜距差表示为

其中，表示t_k时刻位置误差，/>表示t_k+i时刻位置误差，C_e，k+i表示t_k+i时刻的C_e矩阵值，/>表示t_k+i时刻的/>矩阵值，B_S，k+i表示t_k+i时刻的B_S矩阵值，

表示t_k+i时刻的p^e矩阵值，C_e，k表示t_k时刻的C_e矩阵值，/>表示t_k时刻的/>矩阵值，B_S，k表示t_k时刻的B_S矩阵值，/>

测量得到的斜距差表示为：

其中，δr表示测量的误差；

因此，基于斜距差的量测方程表示为：

其中，H₁＝[0_1×6 H_PD1 0_1×6]，H₂＝[0_1×6 H_PD2 0_1×6]， {*}_7：9表示向量的第7-9列，/>

2.根据权利要求书1所述一种基于斜距差匹配的惯性/单声源被动组合导航方法，其特征在于：步骤(4)中，具体为：

(a)状态一步预测：

(b)状态一步预测均方误差：

(c)滤波增益：

其中，H_k表示k时刻的量测矩阵，R_k表示量测噪声矩阵；

(d)状态估计：

其中，z_k表示观测量；

(e)状态估计均方误差：P_k＝P_k-K_kH_kP_k|(k-1)。