一种基于斜距差匹配的惯性/单声源被动组合导航方法
阅读说明：本技术 一种基于斜距差匹配的惯性/单声源被动组合导航方法 (Inertia/single sound source passive combination navigation method based on slope distance difference matching ) 是由 张涛 张亮 夏茂栋 张佳宇 刘射德 于 2021-09-01 设计创作，主要内容包括：本发明公开了一种基于斜距差匹配的惯性/单声源被动组合导航方法,本发明针对传统声学主动定位方式下信息容易暴露的缺点,采用一种单声源被动定位方式辅助惯性导航的组合导航技术,声源按照固定周期向外发射信号,通过接收到信号的时间差完成定位。对于航行器来说,只接收外界的声信号,而可以在不对外辐射信号的情况下实现自主定位,有效地提高了水下航行器导航定位的隐蔽性和安全性。(The invention discloses an inertia/single sound source passive combined navigation method based on slope distance difference matching, aiming at the defect that information is easy to expose in the traditional acoustic active positioning mode, the invention adopts a combined navigation technology of assisting inertial navigation in a single sound source passive positioning mode, a sound source transmits signals outwards according to a fixed period, and positioning is completed through the time difference of the received signals. For an underwater vehicle, the underwater vehicle can only receive external sound signals, and can realize autonomous positioning without radiating external signals, so that the navigation and positioning concealment and safety of the underwater vehicle are effectively improved.)
技术领域
本发明涉及水下基于时间到达差定位方式的被动导航技术领域，以一种斜距差匹配方式实现高精度的被动导航定位功能，特别是涉及一种基于斜距差匹配的惯性/单声源被动组合导航方法。
背景技术
惯性导航系统(Inertial Navigation System，INS)由于其自主性好、隐蔽性强、短时间内精度高等优点，已广泛应用于水下航行器的自主导航与定位。惯性导航可以全天候的提供姿态、速度、位置的全方位信息，拥有其他导航传感器不可比拟的优点，因此成为水下航行器导航定位的首选。虽然惯性导航技术日益成熟，然而其定位误差随时间积累需要定期重调以保证一定精度的这一根本特性却无法改变，因此，世界各国都在探索惯性导航的系统误差校准方法。
由于声波在水下的传输距离远，信号衰减损失小，因而水声导航被广泛应用于水下航行器的导航与定位中。传统基于时间到达方式，需要收发两端设备按照严格的时钟同步，时钟偏置误差将对定位结果产生较大的影响。而基于时间到达差的定位技术，不需要严格的时钟同步，而是以各水听器接收到的声源信号到达时间差作为计算依据，因此可消除对严格时钟同步的依赖性。由于航行器只接收声信号，不需要向外发送询问信号，因此，是一种低成本、高效的被动定位方式。
综上，基于时间到达差的方式对惯性导航的误差进行校正，可以有效提高水下的导航定位精度。但是其难点在于，根据一个信号周期内的时间到达差得到的斜距差，往往变化很小，难以实现对位置误差的精确校正作用。因此，本发明提出一种斜距差匹配方式的惯性/单声源被动组合导航定位方法。通过建立滑动窗构建合适长度的虚拟基阵，采用基阵内的变化较大的斜距差完成对惯性导航的位置误差校正功能。
发明内容
针以上问题，本发明旨在提供一种基于斜距差匹配的惯性/单声源被动组合导航方法，通过设置滑动窗，根据窗口内不同时刻接收到声信号的惯导位置建立虚拟基阵，选定滑动窗最后一帧窗口的位置为参考基元位置，并计算各个位置相对参考基元位置的时间达到差，根据时间到达差建立卡尔曼滤波的量测模型，以此达到高精度的组合导航的目的。
本发明提供一种基于斜距差匹配的惯性/单声源被动组合导航方法，包括如下步骤：
(1)建立惯性/单声源组合导航的状态方程模型；
步骤(1)中，建立惯性/单声源组合导航的状态方程模型：
将惯导的位置误差、速度误差、姿态误差作为系统状态量，得：
其中，[φE φN φU]T表示姿态的失准角误差，[δVE δVN δVU]T表示速度误差，[δL δλ δh]T表示位置误差，[εx εy εz]T表示陀螺仪的零偏，表示加速度计的零偏；
因此，组合导航的状态方程为：
其中，F(t)表示状态转移矩阵，根据捷联惯导误差方程设置，W(t)表示过程噪声矩阵；
(2)建立滑动窗，在滑动窗内基于惯导的位置构建虚拟基阵；
步骤(2)中，建立滑动窗，在滑动窗内基于惯导的位置构建虚拟基阵，具体为：
设置滑动窗窗口大小，窗口内所有接收到声信号时刻的惯导位置共同构成一个虚拟的基阵，以滑动窗内的最后一个惯导位置为参考位置，根据窗口内不同时刻位置下航行器接收到的声信号到达时间，计算相对于参考位置的到达时间差，根据已知的声速可以得到斜距差。在获得一组斜距差的基础上，确定一个最大斜距差以及其对应的信号接收时间，以最大斜距差作为滤波器的量测值。
(3)建立滑动窗内的惯性/单声源组合导航斜距差量测方程模型；
步骤(3)中，建立滑动窗内的惯性/单声源组合导航斜距差量测方程模型，具体为：
所确定的最大斜距差所在时刻为tk，参考位置的时间为tk+i，分别对应的惯导位置为和此时惯导对应的误差状态为和通过状态转移矩阵统一不同时刻的误差状态，得到：
其中，
考虑惯导误差情况下，滑动窗内的最大斜距差表示为：
其中，表示声源位置；
将在地球直角坐标系中对位置进行求解，他们在地球坐标系e系下的相对位置矢量pe为：
其中，RN为卯酉圈曲率半径，e为椭圆偏心率；
将pe转换到导航系下，其矢量表示为：
其中，表示由地球坐标系到导航系的坐标转换矩阵；
考虑惯导位置误差的情况下，表示为：
其中，表示的误差，δpn表示惯导位置误差，Ce表示为：
其中，RNh＝RN+h，[L λ h]分别表示经度、纬度和高度。
BS表示为
其中，Sa＝sin(a)，Sa＝cos(a),a＝L或λ；
因此，斜距差表示为
其中，表示tk时刻位置误差，表示tk+i时刻位置误差，Ce,k+i表示tk+i时刻的Ce矩阵值，表示tk+i时刻的矩阵值，BS,k+i表示tk+i时刻的BS矩阵值， 表示tk+i时刻的pe矩阵值，Ce，k表示tk时刻的Ce矩阵值，表示tk时刻的矩阵值，BS,k表示tk时刻的BS矩阵值，
表示tk时刻的pe矩阵值，r1表示声源与tk时刻船的位置之间的真实距离，r2表示声源与tk+i时刻船的位置之间的真实距离；
测量得到的斜距差表示为：
其中，δr表示测量的误差；
因此，基于斜距差的量测方程表示为：
其中，H1＝[01×6 HPD1 01×6]， H2＝[01×6 HPD2 01×6]， {*}7:9表示向量的第7-9列，
(4)根据(1)-(3)建立的状态方程以及量测方程，利用卡尔曼滤波方法进行误差的更新与反馈。
作为本发明进一步改进，步骤(4)中，具体为：
(a)状态一步预测：
中，表示k-1时刻的状态估计，Fk-1表示k-1时刻到k时刻的状态转移矩阵；
(b)状态一步预测均方误差：
其中，Qk-1表示过程噪声矩阵，Pk-1表示k-1时刻的均方根误差；
(c)滤波增益：
其中，Hk表示k时刻的量测矩阵，Rk表示量测噪声矩阵；
(d)状态估计：
其中，zk表示观测量；
(e)状态估计均方误差：Pk＝Pk-KkHkPk|(k-1)。
有益效果：
根据以上步骤所建立的基于斜距差匹配的惯性/单声源被动组合导航定位方法，可以克服传统主动式定位中，信息易暴露，定位隐蔽性差的缺点，提高特殊应用环境下的定位安全性、隐蔽性。
附图说明
图1为滑动窗内虚拟基阵示意图。
具体实施方式
下面结合附图与具体实施方式对本发明作进一步详细描述：
本发明旨在提供一种基于斜距差匹配的惯性/单声源被动组合导航方法，通过设置滑动窗，根据窗口内不同时刻接收到声信号的惯导位置建立虚拟基阵，选定滑动窗最后一帧窗口的位置为参考基元位置，并计算各个位置相对参考基元位置的时间达到差，根据时间到达差建立卡尔曼滤波的量测模型，以此达到高精度的组合导航的目的。
本发明针对传统主动式声学定位信息易暴露的缺点，设计了基于斜距差匹配的惯性/单声源被动组合导航定位方法，其中滑动窗内虚拟基阵示意图如图1所示，
具体方法如下：
步骤1：建立惯性/单声源组合导航的状态方程模型，具体为：
将惯导的位置误差、速度误差、姿态误差作为系统状态量，得：
其中，[φE φN φU]T表示姿态的失准角误差，[δVE δVN δVU]T表示速度误差，[δL δλ δh]T表示位置误差，[εx εy εz]T表示陀螺仪的零偏，表示加速度计的零偏。
因此，组合导航的状态方程为：
其中，F(t)表示状态转移矩阵，根据捷联惯导误差方程设置，W(t)表示过程噪声矩阵。
步骤2：建立滑动窗，在滑动窗内基于惯导的位置构建虚拟基阵，具体为：
设置滑动窗窗口大小，窗口内所有接收到声信号时刻的惯导位置共同构成一个虚拟的基阵。以滑动窗内的最后一个惯导位置为参考位置，根据窗口内不同时刻位置下航行器接收到的声信号到达时间，计算相对于参考位置的到达时间差，根据已知的声速可以得到斜距差。在获得一组斜距差的基础上，确定一个最大斜距差以及其对应的信号接收时间，以最大斜距差作为滤波器的量测值。
步骤3：建立滑动窗内的惯性/单声源组合导航斜距差量测方程模型，具体为；
步骤2中所确定的最大斜距差所在时刻为tk，参考位置的时间为tk+i，分别对应的惯导位置为和此时惯导对应的误差状态为和通过状态转移矩阵统一不同时刻的误差状态，可以得到：
其中，
考虑惯导误差情况下，滑动窗内的最大斜距差可以表示为：
其中，表示声源位置。
将在地球直角坐标系中对位置进行求解，他们在地球坐标系e系下的相对位置矢量pe为：
其中，RN为卯酉圈曲率半径，e为椭圆偏心率。
将pe转换到导航系下，其矢量表示为：
其中，表示由地球坐标系到导航系的坐标转换矩阵。
考虑惯导位置误差的情况下，可以表示为：
其中，表示的误差，δpn表示惯导位置误差，Ce表示为：
其中，RNh＝RN+h，[L λ h]分别表示经度、纬度和高度。
BS表示为
其中，Sa＝sin(a)，Sa＝cos(a)，a＝L或λ。
因此，斜距差可以表示为
其中，表示tk时刻位置误差，表示tk+i时刻位置误差。Ce,k+i表示tk+i时刻的Ce矩阵值，表示tk+i时刻的矩阵值，BS,k+i表示tk+i时刻的BS矩阵值， 表示tk+i时刻的pe矩阵值。Ce,k表示tk时刻的Ce矩阵值，表示tk时刻的矩阵值，BS,k表示tk时刻的BS矩阵值， 表示tk时刻的pe矩阵值。r1表示声源与tk时刻船的位置之间的真实距离，r2表示声源与tk+i时刻船的位置之间的真实距离
测量得到的斜距差可以表示为：
其中，δr表示测量的误差。
因此，基于斜距差的量测方程可以表示为：
其中，H1＝[01×6 HPD1 01×6]， H2＝[01×6 HPD2 01×6]， {*}7:9表示向量的第7-9列。
步骤4：根据步骤1-3建立的状态方程以及量测方程，利用卡尔曼滤波方法进行误差的更新与反馈，具体为；
(a)状态一步预测：
其中，表示k-1时刻的状态估计，Fk-1表示k-1时刻到k时刻的状态转移矩阵。
(b)状态一步预测均方误差：
其中，Qk-1表示过程噪声矩阵，Pk-1表示k-1时刻的均方根误差。
(c)滤波增益：
其中，Hk表示k时刻的量测矩阵，Rk表示量测噪声矩阵。
(d)状态估计：
其中，zk表示观测量。
(e)状态估计均方误差：Pk＝Pk-KkHkPk|(k-1)。
以上所述，仅是本发明的较佳实施例之一，并非是对本发明作任何其他形式的限制，而依据本发明的技术实质所作的任何修改或等同变化，仍属于本发明所要求保护的范围。