CN114353791B - 基于未知洋流流速的组合导航方法及装置 - Google Patents
基于未知洋流流速的组合导航方法及装置 Download PDFInfo
- Publication number
- CN114353791B CN114353791B CN202210043462.6A CN202210043462A CN114353791B CN 114353791 B CN114353791 B CN 114353791B CN 202210043462 A CN202210043462 A CN 202210043462A CN 114353791 B CN114353791 B CN 114353791B
- Authority
- CN
- China
- Prior art keywords
- error
- doppler velocimeter
- coordinate system
- speed error
- navigation system
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 31
- 238000005259 measurement Methods 0.000 claims abstract description 38
- 238000000605 extraction Methods 0.000 claims abstract description 21
- 238000001914 filtration Methods 0.000 claims abstract description 16
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims description 15
- 238000004590 computer program Methods 0.000 claims description 14
- 239000011159 matrix material Substances 0.000 claims description 14
- 238000012546 transfer Methods 0.000 claims description 9
- 238000004364 calculation method Methods 0.000 claims description 8
- 230000037452 priming Effects 0.000 claims description 2
- 238000010586 diagram Methods 0.000 description 3
- 230000005540 biological transmission Effects 0.000 description 2
- 230000005484 gravity Effects 0.000 description 2
- 230000001360 synchronised effect Effects 0.000 description 2
- 238000007796 conventional method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 239000004973 liquid crystal related substance Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000000523 sample Substances 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
Landscapes
- Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
Abstract
本申请涉及一种基于未知洋流流速的组合导航方法、装置、计算机设备和存储介质。所述方法包括:选取导航坐标系、载体坐标系和多普勒测速仪坐标系求解组合导航系统的状态方程,得到捷联惯性导航系统与多普勒测速仪之间的初始速度误差;根据多普勒测速仪的发射信号进行打底判断,根据判断结果对初始速度误差进行计算,得到最终速度误差;对最终速度误差进行观测量提取,得到量测方程;根据状态方程和量测方程进行卡尔曼滤波,得到捷联惯性导航系统的误差向量和洋流流速的估计值;利用姿态误差和最优速度误差对捷联惯性导航系统的误差进行补偿,得到最佳导航结果。采用本方法能够提高导航精度。
Description
技术领域
本申请涉及组合导航领域,特别是涉及一种基于未知洋流流速的组合导航方法、装置、计算机设备和存储介质。
背景技术
随着导航技术的发展,利用水下潜航器进行水下重力测量是一种便捷的方式,并利用传感器为水下潜航器提供速度信息,在实际过程中,受洋流流速的影响,载体对水速度与载体对地速度偏差较大,洋流流速会影响传感器的测速精度。
然而,目前的传统方法,在进行水下导航重力测量时都需要依赖已知的洋流信息,并且不同区域不同水层的洋流速度差别较大,获取大面积海域的洋流速度难度也较大,使得测量结果精度差,误差大等问题。
发明内容
基于此,有必要针对上述技术问题,提供一种能够提高导航精度的基于未知洋流流速的组合导航方法、装置、计算机设备和存储介质。
一种基于未知洋流流速的组合导航方法,所述方法包括:
根据捷联惯性导航系统和多普勒测速仪构建组合导航系统;多普勒测速仪用于获取速度观测量;
选取导航坐标系、载体坐标系和多普勒测速仪坐标系求解组合导航系统的状态方程,得到捷联惯性导航系统与多普勒测速仪之间的初始速度误差;状态方程包括姿态误差模型、速度误差模型以及洋流流速模型;
根据多普勒测速仪的发射信号进行打底判断,根据判断结果对初始速度误差进行计算,得到最终速度误差;
对最终速度误差进行观测量提取,得到量测方程;
根据状态方程和量测方程进行卡尔曼滤波,得到捷联惯性导航系统的误差向量和洋流流速的估计值;误差向量包括姿态误差和最优速度误差;
利用姿态误差和最优速度误差对捷联惯性导航系统的误差进行补偿,得到最佳导航结果。
在其中一个实施例中,选取导航坐标系、载体坐标系和多普勒测速仪坐标系求解组合导航系统的状态方程,得到捷联惯性导航系统与多普勒测速仪之间的初始速度误差,包括:
其中,δvn为所述初始速度误差,n表示导航坐标系,为导航坐标系下准确的对底速度,/>为导航坐标系下捷联惯性导航系统的速度误差向量,/>为导航坐标系下多普勒测速仪的输出速度,/> 为由多普勒测速仪坐标系到载体坐标系的姿态转移矩阵,/>为多普勒测速仪坐标系下多普勒测速仪的输出速度,/>为/>的反对称矩阵,ψ为捷联惯性导航系统的姿态误差向量。
在其中一个实施例中,对多普勒测速仪的发射信号进行打底判断,包括:若多普勒测速仪发射的信号能打底,则为对底速度,若多普勒测速仪发射的信号不能打底,则为对水速度。
在其中一个实施例中,根据判断结果对速度误差进行计算,得到最终速度误差,包括:
若为对底速度,则/>得到
其中,δvn′为最终速度误差。
在另一个实施例中,若为对水速度,则/>得到
其中,为洋流在n系下的速度。
在其中一个实施例中,对最终速度误差进行观测量提取,得到量测方程,包括:对最终速度误差进行观测量提取,得到第一分量和第二分量;根据第一分量和第二分量构建量测方程。
在其中一个实施例中,根据第一分量和第二分量构建量测方程,包括:
其中,δv1′和δv2′分别为δvn′的第一和第二分量,H为量测矩阵,V为高斯白噪声。
一种基于未知洋流流速的组合导航装置,所述装置包括:
构建组合导航系统模块,用于根据捷联惯性导航系统和多普勒测速仪构建组合导航系统;多普勒测速仪用于获取速度观测量;
状态方程求解模块,用于选取导航坐标系、载体坐标系和多普勒测速仪坐标系求解组合导航系统的状态方程,得到捷联惯性导航系统与多普勒测速仪之间的初始速度误差;状态方程包括姿态误差模型、速度误差模型以及洋流流速模型;
初始速度误差计算模块,用于根据多普勒测速仪的发射信号进行打底判断,根据判断结果对初始速度误差进行计算,得到最终速度误差;
观测量提取模块,用于对最终速度误差进行观测量提取,得到量测方程;
卡尔曼滤波模块,用于根据状态方程和量测方程进行卡尔曼滤波,得到捷联惯性导航系统的误差向量和洋流流速的估计值;误差向量包括姿态误差和最优速度误差;
误差补偿模块,用于利用姿态误差和最优速度误差对捷联惯性导航系统的误差进行补偿,得到最佳导航结果。
一种计算机设备,包括存储器和处理器,所述存储器存储有计算机程序,所述处理器执行所述计算机程序时实现以下步骤:
根据捷联惯性导航系统和多普勒测速仪构建组合导航系统;多普勒测速仪用于获取速度观测量;
选取导航坐标系、载体坐标系和多普勒测速仪坐标系求解组合导航系统的状态方程,得到捷联惯性导航系统与多普勒测速仪之间的初始速度误差;状态方程包括姿态误差模型、速度误差模型以及洋流流速模型;
根据多普勒测速仪的发射信号进行打底判断,根据判断结果对初始速度误差进行计算,得到最终速度误差;
对最终速度误差进行观测量提取,得到量测方程;
根据状态方程和量测方程进行卡尔曼滤波,得到捷联惯性导航系统的误差向量和洋流流速的估计值;误差向量包括姿态误差和最优速度误差;
利用姿态误差和最优速度误差对捷联惯性导航系统的误差进行补偿,得到最佳导航结果。
一种计算机可读存储介质,其上存储有计算机程序,所述计算机程序被处理器执行时实现以下步骤:
根据捷联惯性导航系统和多普勒测速仪构建组合导航系统;多普勒测速仪用于获取速度观测量;
选取导航坐标系、载体坐标系和多普勒测速仪坐标系求解组合导航系统的状态方程,得到捷联惯性导航系统与多普勒测速仪之间的初始速度误差;状态方程包括姿态误差模型、速度误差模型以及洋流流速模型;
根据多普勒测速仪的发射信号进行打底判断,根据判断结果对初始速度误差进行计算,得到最终速度误差;
对最终速度误差进行观测量提取,得到量测方程;
根据状态方程和量测方程进行卡尔曼滤波,得到捷联惯性导航系统的误差向量和洋流流速的估计值;误差向量包括姿态误差和最优速度误差;
利用姿态误差和最优速度误差对捷联惯性导航系统的误差进行补偿,得到最佳导航结果。
上述基于未知洋流流速的组合导航方法、装置、计算机设备和存储介质,首先利用捷联惯性导航系统和多普勒测速仪构建组合导航系统,利用多普勒测速仪速度作为观测量,不依赖已知洋流流速,通过选取多个坐标系求解组合导航系统的状态方程,对得到的捷联惯性导航系统和多普勒测速仪之间的初始速度误差进行打底计算,得到最终速度误差,然后对其进行观测量提取并根据得到的量测方程和状态方程进行卡尔曼滤波,一方面利用得到的姿态误差和最优速度误差对捷联惯性导航系统的误差进行补偿来得到最佳导航结果,提高导航精度,另一方面还能估计未知洋流的流速。
附图说明
图1为一个实施例中一种基于未知洋流流速的组合导航方法的流程示意图;
图2为一个实施例中一种基于未知洋流流速的组合导航装置的结构框图;
图3为一个实施例中计算机设备的内部结构图。
具体实施方式
为了使本申请的目的、技术方案及优点更加清楚明白,以下结合附图及实施例,对本申请进行进一步详细说明。应当理解,此处描述的具体实施例仅仅用以解释本申请,并不用于限定本申请。
在一个实施例中,如图2所示,提供了一种基于未知洋流流速的组合导航方法,包括以下步骤:
步骤102,根据捷联惯性导航系统和多普勒测速仪构建组合导航系统;多普勒测速仪用于获取速度观测量。
捷联式惯性导航系统即SINS具有自主性强、隐蔽性好、抗干扰以及易安装在无人潜航器的优点,但其测量的姿态、速度以位置误差随时间发散,多普勒测速仪即DVL利用多普勒原理,通过向海底或水层发射超声波信号测量其传感器探头与水层或海底的相对运动速度,可以作为水下测速设备为SINS提供高精度的速度信息,将多普勒测速仪的速度作为观测量,不依赖洋流流速,通过对捷联式惯性导航系统和多普勒测速仪的速度误差进行修正,可以得到高精度的导航结果。
步骤104,选取导航坐标系、载体坐标系和多普勒测速仪坐标系求解组合导航系统的状态方程,得到捷联惯性导航系统与多普勒测速仪之间的初始速度误差;状态方程包括姿态误差模型、速度误差模型以及洋流流速模型;
洋流东速和北速在短时间内是恒定不变的,因此认为以及/>通过以上两个式子再根据姿态误差模型和速度误差模型即可得到系统的状态方程。
捷联惯性导航系统在进行导航时由于洋流流速的影响会产生一定的误差,通过选取多个坐标系,对组合导航系统的状态方程进行求解得到载体坐标系到导航坐标系的姿态转移矩阵,利用载体坐标系到导航坐标系的姿态转移矩阵、由多普勒测速仪坐标系到载体坐标系的姿态转移矩阵以及通过多普勒测速仪获取的多普勒测速仪坐标系下多普勒测速仪的输出速度相乘得到带误差的多普勒测速仪在导航坐标系下的速度,然后将捷联惯性导航系统的输出速度与多普勒测速仪在导航坐标系下的速度作差,得到两者之间的误差为组合导航系统误差。
步骤106,根据多普勒测速仪的发射信号进行打底判断,根据判断结果对初始速度误差进行计算,得到最终速度误差。
组合导航系统误差主要由多普勒测速仪输出对水速度引起的,在计算最终速度误差时,需要对多普勒测速仪的发射信号进行打底判断,若多普勒测速仪发射的信号能打底,则多普勒测速仪坐标系下多普勒测速仪的输出速度为对底速度,若多普勒测速仪发射的信号不能打底,则多普勒测速仪坐标系下多普勒测速仪的输出速度为对水速度,根据对多普勒测速仪坐标系下多普勒测速仪的输出速度的判断来计算组合导航系统误差的最终误差。
步骤108,对最终速度误差进行观测量提取,得到量测方程。
对最终速度误差进行观测量提取,提取出的分量构建量测方程。该量测方程是作为先验知识。
步骤110,根据状态方程和量测方程进行卡尔曼滤波,得到捷联惯性导航系统的误差向量和洋流流速的估计值;误差向量包括姿态误差和最优速度误差。
量测方程作为先验知识对状态方程中的状态变量来进行估计,得到状态变量的估计值,通过卡尔曼滤波后,可以得到系统的状态变量的最优估计,得到更加准确捷联惯性导航系统的误差向量和洋流流速的估计值。洋流流速包括洋流的东速和北速。
步骤112,利用姿态误差和最优速度误差对捷联惯性导航系统的误差进行补偿,得到最佳导航结果。
利用姿态误差和最优速度误差对捷联惯性导航系统的误差进行补偿,误差补偿后的捷联惯性导航系统能够进行精确导航,提供更加准确的导航信息。
上述基于未知洋流流速的组合导航方法,首先利用捷联惯性导航系统和多普勒测速仪构建组合导航系统,利用多普勒测速仪速度作为观测量,不依赖已知洋流流速,通过选取多个坐标系求解组合导航系统的状态方程,对得到的捷联惯性导航系统和多普勒测速仪之间的初始速度误差进行打底计算,得到最终速度误差,然后对其进行观测量提取并根据得到的量测方程和状态方程进行卡尔曼滤波,一方面利用得到的姿态误差和最优速度误差对捷联惯性导航系统的误差进行补偿来得到最佳导航结果,提高导航精度,另一方面还能估计未知洋流的流速。
在其中一个实施例中,选取导航坐标系、载体坐标系和多普勒测速仪坐标系求解组合导航系统的状态方程,得到捷联惯性导航系统与多普勒测速仪之间的初始速度误差,包括:
其中,δvn为所述初始速度误差,n表示导航坐标系,为导航坐标系下准确的对底速度,/>为导航坐标系下捷联惯性导航系统的速度误差向量,/>为导航坐标系下多普勒测速仪的输出速度,/> 为由多普勒测速仪坐标系到载体坐标系的姿态转移矩阵,/>为多普勒测速仪坐标系下多普勒测速仪的输出速度,/>为/>的反对称矩阵,ψ为捷联惯性导航系统的姿态误差向量。
选取导航坐标系即n系为东-北-天坐标系,载体坐标系即b系为右-前-上坐标系,多普勒测速仪坐标系即m系为右-前-上坐标系,求组合导航系统的状态方程,表示为其中,X为误差状态量,A为状态转移矩阵;B为系统噪声转移矩阵,W为系统状态噪声,/>为X的微分方程;具体定义如下:
其中,φE、φN和φU分别为SINS的俯仰角误差、横滚角误差以及航向角误差,根据φE、φN和φU的微分方程构成姿态误差模型,δVE、δVN分别为SINS(捷联惯性导航系统)的东速误差和北速误差,根据δVE、δVN的微分方程构成速度误差模型,和/>分别为洋流东速和洋流北速,根据/>和/>的微分方程构成洋流流速模型;
其中,fE、fN和fU分别为东向比力、北向比力和天向比力,h为深度(向上为正),ωie为地球自转角速率,L为地理纬度,VE、VN分别为对地东速和北速,RM和RN分别为子午圈和卯酉圈半径;
其中,为由b系到n系的姿态转移矩阵,C11、C12、C21和C22分别为/>的第1行第1列元素、第1行第2列元素、第2行第1列元素以及第2行第2列元素;
其中,εx、εy和εz分别为右向陀螺、前向陀螺以及垂向陀螺的随机噪声,和/>分别为右向加速度计和前向加速度计的随机噪声;
在其中一个实施例中,对多普勒测速仪的发射信号进行打底判断,包括:若多普勒测速仪发射的信号能打底,则为对底速度,若多普勒测速仪发射的信号不能打底,则为对水速度。
利用组合导航系统来对多普勒测速仪的发射信号进行打底判断,不同的速度使得捷联惯性导航系统与多普勒测速仪之间的速度误差会不一样,进行打底判断后,对多普勒测速仪的输出速度有了更精确的判断,在后续误差计算中,有利于提高速度误差的精确度,进而提高了导航结果的准确度。
在其中一个实施例中,根据判断结果对速度误差进行计算,得到最终速度误差,包括:
若为对底速度,则/>得到
其中,δvn′为最终速度误差。
在另一个实施例中,若为对水速度,则/>得到
其中,为洋流在n系下的速度。
在其中一个实施例中,对最终速度误差进行观测量提取,得到量测方程,包括:对最终速度误差进行观测量提取,得到第一分量和第二分量;根据第一分量和第二分量构建量测方程。
在其中一个实施例中,根据第一分量和第二分量构建量测方程,包括:
其中,δv1′和δv2′分别为δvn′的第一和第二分量,H为量测矩阵,V为高斯白噪声。
若为对底速度,/>其中和/>分别为/>的第一、第二和第三分量,δVE、δVN分别为捷联惯性导航系统的东速误差和北速误差,此时/>
若为对水速度,/> 此时/>
应该理解的是,虽然图1的流程图中的各个步骤按照箭头的指示依次显示,但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明,这些步骤的执行并没有严格的顺序限制,这些步骤可以以其它的顺序执行。而且,图1中的至少一部分步骤可以包括多个子步骤或者多个阶段,这些子步骤或者阶段并不必然是在同一时刻执行完成,而是可以在不同的时刻执行,这些子步骤或者阶段的执行顺序也不必然是依次进行,而是可以与其它步骤或者其它步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。
在一个实施例中,如图2所示,提供了一种基于未知洋流流速的组合导航装置,包括:构建组合导航系统模块202、状态方程求解模块204、初始速度误差计算模块206、观测量提取模块208、卡尔曼滤波模块210和误差补偿模块212,其中:
构建组合导航系统模块202,用于根据捷联惯性导航系统和多普勒测速仪构建组合导航系统;多普勒测速仪用于获取速度观测量;
状态方程求解模块204,用于选取导航坐标系、载体坐标系和多普勒测速仪坐标系求解组合导航系统的状态方程,得到捷联惯性导航系统与多普勒测速仪之间的初始速度误差;状态方程包括姿态误差模型、速度误差模型以及洋流流速模型;
初始速度误差计算模块206,用于根据多普勒测速仪的发射信号进行打底判断,根据判断结果对初始速度误差进行计算,得到最终速度误差;
观测量提取模块208,用于对最终速度误差进行观测量提取,得到量测方程;
卡尔曼滤波模块210,用于根据状态方程和量测方程进行卡尔曼滤波,得到捷联惯性导航系统的误差向量和洋流流速的估计值;误差向量包括姿态误差和最优速度误差;
误差补偿模块212,用于利用姿态误差和最优速度误差对捷联惯性导航系统的误差进行补偿,得到最佳导航结果。
在其中一个实施例中,状态方程求解模块204还用于选取导航坐标系、载体坐标系和多普勒测速仪坐标系求解组合导航系统的状态方程,得到捷联惯性导航系统与多普勒测速仪之间的初始速度误差,包括:
其中,δvn为所述初始速度误差,n表示导航坐标系,为导航坐标系下准确的对底速度,/>为导航坐标系下捷联惯性导航系统的速度误差向量,/>为导航坐标系下多普勒测速仪的输出速度,/> 为由多普勒测速仪坐标系到载体坐标系的姿态转移矩阵,/>为多普勒测速仪坐标系下多普勒测速仪的输出速度,/>为/>的反对称矩阵,ψ为捷联惯性导航系统的姿态误差向量。
在其中一个实施例中,初始速度误差计算模块206还用于若多普勒测速仪发射的信号能打底,则为对底速度,若多普勒测速仪发射的信号不能打底,则/>为对水速度。
在其中一个实施例中,初始速度误差计算模块206还用于根据判断结果对速度误差进行计算,得到最终速度误差,包括:
若为对底速度,则/>得到
其中,δvn′为最终速度误差。
在另一个实施例中,若为对水速度,则/>得到
其中,为洋流在n系下的速度。
在其中一个实施例中,观测量提取模块208还用于对最终速度误差进行观测量提取,得到第一分量和第二分量;根据第一分量和第二分量构建量测方程。
在其中一个实施例中,观测量提取模块208还用于根据第一分量和第二分量构建量测方程,包括:
其中,δv1′和δv2′分别为δvn′的第一和第二分量,H为量测矩阵,V为高斯白噪声。
关于一种基于未知洋流流速的组合导航装置的具体限定可以参见上文中对于一种基于未知洋流流速的组合导航方法的限定,在此不再赘述。上述一种基于未知洋流流速的组合导航装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中,也可以以软件形式存储于计算机设备中的存储器中,以便于处理器调用执行以上各个模块对应的操作。
在一个实施例中,提供了一种计算机设备,该计算机设备可以是终端,其内部结构图可以如图3所示。该计算机设备包括通过系统总线连接的处理器、存储器、网络接口、显示屏和输入装置。其中,该计算机设备的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统和计算机程序。该内存储器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该计算机设备的网络接口用于与外部的终端通过网络连接通信。该计算机程序被处理器执行时以实现一种基于未知洋流流速的组合导航方法。该计算机设备的显示屏可以是液晶显示屏或者电子墨水显示屏,该计算机设备的输入装置可以是显示屏上覆盖的触摸层,也可以是计算机设备外壳上设置的按键、轨迹球或触控板,还可以是外接的键盘、触控板或鼠标等。
本领域技术人员可以理解,图3中示出的结构,仅仅是与本申请方案相关的部分结构的框图,并不构成对本申请方案所应用于其上的计算机设备的限定,具体的计算机设备可以包括比图中所示更多或更少的部件,或者组合某些部件,或者具有不同的部件布置。
在一个实施例中,提供了一种计算机设备,包括存储器和处理器,该存储器存储有计算机程序,该处理器执行计算机程序时实现上述实施例中方法的步骤。
在一个实施例中,提供了一种计算机存储介质,其上存储有计算机程序,计算机程序被处理器执行时实现上述实施例中方法的步骤。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程,是可以通过计算机程序来指令相关的硬件来完成,所述的计算机程序可存储于一非易失性计算机可读取存储介质中,该计算机程序在执行时,可包括如上述各方法的实施例的流程。其中,本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用,均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器(ROM)、可编程ROM(PROM)、电可编程ROM(EPROM)、电可擦除可编程ROM(EEPROM)或闪存。易失性存储器可包括随机存取存储器(RAM)或者外部高速缓冲存储器。作为说明而非局限,RAM以多种形式可得,诸如静态RAM(SRAM)、动态RAM(DRAM)、同步DRAM(SDRAM)、双数据率SDRAM(DDRSDRAM)、增强型SDRAM(ESDRAM)、同步链路(Synchlink)DRAM(SLDRAM)、存储器总线(Rambus)直接RAM(RDRAM)、直接存储器总线动态RAM(DRDRAM)、以及存储器总线动态RAM(RDRAM)等。
以上实施例的各技术特征可以进行任意的组合,为使描述简洁,未对上述实施例中的各个技术特征所有可能的组合都进行描述,然而,只要这些技术特征的组合不存在矛盾,都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本申请的几种实施方式,其描述较为具体和详细,但并不能因此而理解为对发明专利范围的限制。应当指出的是,对于本领域的普通技术人员来说,在不脱离本申请构思的前提下,还可以做出若干变形和改进,这些都属于本申请的保护范围。因此,本申请专利的保护范围应以所附权利要求为准。
Claims (10)
1.一种基于未知洋流流速的组合导航方法,其特征在于,所述方法包括:
根据捷联惯性导航系统和多普勒测速仪构建组合导航系统;所述多普勒测速仪用于获取速度观测量;
选取导航坐标系、载体坐标系和多普勒测速仪坐标系求解所述组合导航系统的状态方程,得到捷联惯性导航系统与多普勒测速仪之间的初始速度误差;所述状态方程包括姿态误差模型、速度误差模型以及洋流流速模型;
根据所述多普勒测速仪的发射信号进行打底判断,根据判断结果对所述初始速度误差进行计算,得到最终速度误差;
对所述最终速度误差进行观测量提取,得到量测方程;
根据所述状态方程和所述量测方程进行卡尔曼滤波,得到捷联惯性导航系统的误差向量和洋流流速的估计值;所述误差向量包括姿态误差和最优速度误差;
利用所述姿态误差和最优速度误差对捷联惯性导航系统的误差进行补偿,得到最佳导航结果。
2.根据权利要求1所述的方法,其特征在于,选取导航坐标系、载体坐标系和多普勒测速仪坐标系求解所述组合导航系统的状态方程,得到捷联惯性导航系统与多普勒测速仪之间的初始速度误差,包括:
其中,δvn为所述初始速度误差,n表示导航坐标系,为导航坐标系下准确的对底速度,/>为导航坐标系下捷联惯性导航系统的速度误差向量,/>为导航坐标系下多普勒测速仪的输出速度,/> 为由多普勒测速仪坐标系到载体坐标系的姿态转移矩阵,/>为多普勒测速仪坐标系下多普勒测速仪的输出速度,/>为/>的反对称矩阵,ψ为捷联惯性导航系统的姿态误差向量。
3.根据权利要求2所述的方法,其特征在于,对所述多普勒测速仪的发射信号进行打底判断,包括:
若多普勒测速仪发射的信号能打底,则为对底速度,若多普勒测速仪发射的信号不能打底,则/>为对水速度。
4.根据权利要求3所述的方法,其特征在于,根据判断结果对所述速度误差进行计算,得到最终速度误差,包括:
若为对底速度,则/>得到
其中,δvn′为最终速度误差。
5.根据权利要求4所述的方法,其特征在于,所述还包括:
若为对水速度,则/>得到
其中,为洋流在n系下的速度。
6.根据权利要求5所述的方法,其特征在于,对所述最终速度误差进行观测量提取,得到量测方程,包括:
对所述最终速度误差进行观测量提取,得到第一分量和第二分量;
根据第一分量和第二分量构建量测方程。
7.根据权利要求6所述的方法,其特征在于,根据第一分量和第二分量构建量测方程,包括:
其中,δv1′和δv2′分别为δvn′的第一和第二分量,H为量测矩阵,V为高斯白噪声。
8.一种基于未知洋流流速的组合导航装置,其特征在于,所述装置包括:
构建组合导航系统模块,用于根据捷联惯性导航系统和多普勒测速仪构建组合导航系统;所述多普勒测速仪用于获取速度观测量;
状态方程求解模块,用于选取导航坐标系、载体坐标系和多普勒测速仪坐标系求解所述组合导航系统的状态方程,得到捷联惯性导航系统与多普勒测速仪之间的初始速度误差;所述状态方程包括姿态误差模型、速度误差模型以及洋流流速模型;
初始速度误差计算模块,用于根据所述多普勒测速仪的发射信号进行打底判断,根据判断结果对所述初始速度误差进行计算,得到最终速度误差;
观测量提取模块,用于对所述最终速度误差进行观测量提取,得到量测方程;
卡尔曼滤波模块,用于根据所述状态方程和所述量测方程进行卡尔曼滤波,得到捷联惯性导航系统的误差向量和洋流流速的估计值;所述误差向量包括姿态误差和最优速度误差;
误差补偿模块,用于利用所述姿态误差和最优速度误差对捷联惯性导航系统的误差进行补偿,得到最佳导航结果。
9.一种计算机设备,包括存储器和处理器,所述存储器存储有计算机程序,其特征在于,所述处理器执行所述计算机程序时实现权利要求1至7中任一项所述方法的步骤。
10.一种计算机可读存储介质,其上存储有计算机程序,其特征在于,所述计算机程序被处理器执行时实现权利要求1至7中任一项所述的方法的步骤。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210043462.6A CN114353791B (zh) | 2022-01-14 | 2022-01-14 | 基于未知洋流流速的组合导航方法及装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210043462.6A CN114353791B (zh) | 2022-01-14 | 2022-01-14 | 基于未知洋流流速的组合导航方法及装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114353791A CN114353791A (zh) | 2022-04-15 |
CN114353791B true CN114353791B (zh) | 2023-11-10 |
Family
ID=81092157
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210043462.6A Active CN114353791B (zh) | 2022-01-14 | 2022-01-14 | 基于未知洋流流速的组合导航方法及装置 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114353791B (zh) |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103017755A (zh) * | 2012-11-26 | 2013-04-03 | 哈尔滨工程大学 | 一种水下导航姿态测量方法 |
CN109855649A (zh) * | 2019-04-12 | 2019-06-07 | 哈尔滨工程大学 | 一种水下滑翔机平台运动轨迹被动确定方法 |
CN110031882A (zh) * | 2018-08-02 | 2019-07-19 | 哈尔滨工程大学 | 一种基于sins/dvl组合导航系统的外量测信息补偿方法 |
WO2019242336A1 (zh) * | 2018-06-22 | 2019-12-26 | 东南大学 | 一种水下滑翔器导航定位系统及上浮精度校正方法 |
CN112729291A (zh) * | 2020-12-29 | 2021-04-30 | 东南大学 | 一种深潜长航潜水器sins/dvl洋流速度估计方法 |
WO2021253488A1 (zh) * | 2020-06-17 | 2021-12-23 | 东南大学 | 一种水下重力无源导航系统 |
-
2022
- 2022-01-14 CN CN202210043462.6A patent/CN114353791B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103017755A (zh) * | 2012-11-26 | 2013-04-03 | 哈尔滨工程大学 | 一种水下导航姿态测量方法 |
WO2019242336A1 (zh) * | 2018-06-22 | 2019-12-26 | 东南大学 | 一种水下滑翔器导航定位系统及上浮精度校正方法 |
CN110031882A (zh) * | 2018-08-02 | 2019-07-19 | 哈尔滨工程大学 | 一种基于sins/dvl组合导航系统的外量测信息补偿方法 |
CN109855649A (zh) * | 2019-04-12 | 2019-06-07 | 哈尔滨工程大学 | 一种水下滑翔机平台运动轨迹被动确定方法 |
WO2021253488A1 (zh) * | 2020-06-17 | 2021-12-23 | 东南大学 | 一种水下重力无源导航系统 |
CN112729291A (zh) * | 2020-12-29 | 2021-04-30 | 东南大学 | 一种深潜长航潜水器sins/dvl洋流速度估计方法 |
Non-Patent Citations (2)
Title |
---|
A Fast SINS Initial Alignment Scheme for Underwater Vehicle Applications;LI W et al.;Journal of Navigation;全文 * |
考虑洋流影响的SINS/DVL组合导航算法;郭玉胜;付梦印;邓志红;邓继权;艾瀛涛;;中国惯性技术学报(06);全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN114353791A (zh) | 2022-04-15 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN107655476B (zh) | 基于多信息融合补偿的行人高精度足部导航方法 | |
EP2385347B1 (fr) | Procédé de définition d'un système de navigation | |
Dinc et al. | Integration of navigation systems for autonomous underwater vehicles | |
CN112146655B (zh) | 一种BeiDou/SINS紧组合导航系统弹性模型设计方法 | |
CN111024124B (zh) | 一种多传感器信息融合的组合导航故障诊断方法 | |
CN114777812B (zh) | 一种水下组合导航系统行进间对准与姿态估计方法 | |
CN113566850B (zh) | 惯性测量单元的安装角度标定方法、装置和计算机设备 | |
CN114689047A (zh) | 基于深度学习的组合导航方法、装置、系统及存储介质 | |
US20220057517A1 (en) | Method for constructing point cloud map, computer device, and storage medium | |
CN111722295A (zh) | 一种水下捷联式重力测量数据处理方法 | |
CN113092822B (zh) | 一种基于惯组的激光多普勒测速仪的在线标定方法和装置 | |
CN114353791B (zh) | 基于未知洋流流速的组合导航方法及装置 | |
CN115856922A (zh) | 一种松耦合陆地组合导航方法、装置、计算机设备和介质 | |
CN115900705A (zh) | 一种紧耦合陆地组合导航方法、装置、计算机设备和介质 | |
CN115950450A (zh) | 二维多普勒测速仪的标定方法、装置、计算机设备和介质 | |
CN114897942B (zh) | 点云地图的生成方法、设备及相关存储介质 | |
CN114019954B (zh) | 航向安装角标定方法、装置、计算机设备和存储介质 | |
CN114001730B (zh) | 融合定位方法、装置、计算机设备和存储介质 | |
CN113566849B (zh) | 惯性测量单元的安装角度标定方法、装置和计算机设备 | |
CN112325770B (zh) | 一种车端单目视觉测量相对精度置信度评估方法及系统 | |
CN117782114B (zh) | 车辆定位校正方法、装置、计算机设备和存储介质 | |
JP3863043B2 (ja) | 水中航走体における位置検出方法 | |
Tong et al. | Study on the algorithm of sea bottom terrain match | |
US20220358365A1 (en) | Tightly coupled end-to-end multi-sensor fusion with integrated compensation | |
CN117630965A (zh) | 矿下车辆定位方法、装置、电子设备及存储介质 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |