CN115031731A - 一种基于相对安装位置关系的多惯导协同导航方法及装置 - Google Patents

一种基于相对安装位置关系的多惯导协同导航方法及装置 Download PDF

Info

Publication number
CN115031731A
CN115031731A CN202210723122.8A CN202210723122A CN115031731A CN 115031731 A CN115031731 A CN 115031731A CN 202210723122 A CN202210723122 A CN 202210723122A CN 115031731 A CN115031731 A CN 115031731A
Authority
CN
China
Prior art keywords
inertial navigation
navigation
standby
navigation system
collaborative
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.)
Pending
Application number
CN202210723122.8A
Other languages
English (en)
Inventor
王亚波
刘贤俊
许彩
杨建�
袁克非
邓林
杨毅
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Third Military Representative Office Of Military Representative Bureau Of Chinese People's Liberation Army Naval Equipment Department In Wuhan
WUHAN SECOND SHIP DESIGN AND RESEARCH INSTITUTE
Original Assignee
Third Military Representative Office Of Military Representative Bureau Of Chinese People's Liberation Army Naval Equipment Department In Wuhan
WUHAN SECOND SHIP DESIGN AND RESEARCH INSTITUTE
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Third Military Representative Office Of Military Representative Bureau Of Chinese People's Liberation Army Naval Equipment Department In Wuhan, WUHAN SECOND SHIP DESIGN AND RESEARCH INSTITUTE filed Critical Third Military Representative Office Of Military Representative Bureau Of Chinese People's Liberation Army Naval Equipment Department In Wuhan
Priority to CN202210723122.8A priority Critical patent/CN115031731A/zh
Publication of CN115031731A publication Critical patent/CN115031731A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/183—Compensation of inertial measurements, e.g. for temperature effects
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G01C21/203—Specially adapted for sailing ships

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种基于相对安装位置关系的多惯导协同导航方法及装置，包括以下步骤：基于多惯导系统中各惯导的相对安装位置关系，获取主用惯导与备用惯导的导航参数之间的关联关系；获取所述主用惯导与备用惯导之间的相对距离信息和相对姿态信息，基于所述主用惯导与备用惯导的导航参数之间的关联关系构建协同导航系统的量测方程；对所述多惯导系统的误差进行扩展卡尔曼滤波，构建扩展卡尔曼滤波状态方程；基于所述扩展卡尔曼滤波状态方程及所述协同导航系统的量测方程，生成导航信息。本发明基于多套惯导间的相对安装位置关系，利用多套惯导间的相对姿态和相对距离信息构建观测量，实现多惯导的协同导航，有效提高导航系统精度和性能。

Description

一种基于相对安装位置关系的多惯导协同导航方法及装置
技术领域
本发明涉及舰艇导航应用领域，具体涉及一种基于相对安装位置关系的多惯导协同导航方法及装置。
背景技术
惯性导航系统具有独立自主、导航信息完备、隐蔽性强等优点，是舰艇导航领域必不可少的导航手段。然而，受制于惯导工作机理，其定位误差随时间累积，需要外部辅助信息进行修正。对于大型舰艇而言，为了保证导航系统的可靠性，通常冗余配备多套惯导系统，选择其中一套惯导作为主用惯导，其他作为备用惯导。
鉴于多套惯导的安装方式近似为刚性连接，主用惯导与备用惯导间的相对安装位置信息关系稳定、可靠，且可以事先精确测量。在无外部辅助信息条件下，利用多惯导间的相对安装位置关系，以此构建惯导间导航参数的联系，实现多惯导协同导航，实现主用惯导与备用惯导精度的整体提升。
发明内容
有鉴于此，本发明提供了一种基于相对安装位置关系的多惯导协同导航方法及装置，针对惯导定位误差随时间累积的问题，结合舰艇冗余配置多套惯导系统的特点，在无外部辅助信息条件下实现导航精度提升。
为实现本发明的目的，本发明所采用的技术方案是：
一种基于相对安装位置关系的多惯导协同导航方法，包括以下步骤：
步骤S1：基于多惯导系统中各惯导的相对安装位置测量，建立主用惯导与备用惯导的导航参数之间的数学关系；
步骤S2：获取所述主用惯导与备用惯导之间的相对距离信息和相对姿态信息，基于所述主用惯导与备用惯导的导航参数之间的数学关系构建协同导航系统的量测方程；
步骤S3：对所述多惯导系统的误差进行扩展卡尔曼滤波，构建扩展卡尔曼滤波状态方程；基于所述扩展卡尔曼滤波状态方程及所述协同导航系统的量测方程，生成导航信息。
优选地，所述步骤S1，包括：
步骤S11：通过测量各惯导的相对距离Δr12，构建所述主用惯导与所述备用惯导位置间的数学关系，则有
Figure BDA0003706505400000021
式中，
Figure BDA0003706505400000022
Figure BDA0003706505400000023
Figure BDA0003706505400000024
和
Figure BDA0003706505400000025
分别表示所述主用惯导和所述备用惯导实际输出的纬度、经度信息，L1、λ1和L2、λ1分别表示所述主用惯导和所述备用惯导理想或预期输出的纬度、经度信息，δL1、δλ1和δL2、δλ2分别表示所述主用惯导和所述备用惯导的纬度误差、经度误差；Rn、Re分别表示地球子午圈、卯酉圈曲率半径；h＝(h1+h2)/2表示惯导输出载体的平均高度信息；
Figure BDA0003706505400000026
表示惯导输出载体的平均纬度信息，h1、h2分别表示所述主用惯导、所述备用惯导输出载体的高度。本实施例中，使用长度测量工具获取距离信息；
步骤S12：使用标校装置进行安装标校，使得
Figure BDA0003706505400000027
构建所述主用惯导的姿态矩阵和所述备用惯导的姿态矩阵之间的数学关系，则有
Figure BDA0003706505400000031
式中，
Figure BDA0003706505400000032
Figure BDA0003706505400000033
b1和b2分别表示所述主用惯导和所述备用惯导的载体坐标系；I表示单位阵，
Figure BDA0003706505400000034
表示所述主用惯导载体坐标系相对所述备用惯导载体坐标系的姿态矩阵；
Figure BDA0003706505400000035
为所述主用惯导载体坐标系b1相对导航坐标系n的计算姿态矩阵，
Figure BDA0003706505400000036
为所述备用惯导载体坐标系b2相对导航坐标系n的计算姿态矩阵，φ1 n为所述主用惯导的姿态失准角,
Figure BDA0003706505400000037
为所述备用惯导的姿态失准角，
Figure BDA0003706505400000038
为所述主用惯导载体坐标系b1相对导航坐标系n的理想姿态矩阵，
Figure BDA0003706505400000039
为所述备用惯导载体坐标系b2相对导航坐标系n的理想姿态矩阵。
优选地，所述步骤S2，包括：
步骤S21：选取状态变量，所述状态变量包括所述主用惯导及所述备用惯导的姿态误差、速度误差、位置误差、陀螺常值误差和加速度计常值误差，构建的状态变量X表示如下：
Figure BDA00037065054000000310
式中，φi n表示姿态失准角，δVi n表示速度误差，
Figure BDA00037065054000000311
表示位置误差，δhi为高度误差，
Figure BDA00037065054000000312
表示陀螺常值误差，
Figure BDA00037065054000000313
表示加速度计常值误差，1≤i≤num，num为所述主用惯导数量与所述备用惯导数量的和，num＝2；
步骤S22：构建协同导航系统的量测方程：
令
Figure BDA0003706505400000041
Figure BDA0003706505400000042
则有
Figure BDA0003706505400000043
其中，Δθ12为构建的所述主用惯导与所述备用惯导间姿态误差角向量，Δθx、Δθy、Δθz分别为姿态误差角向量Δθ12三个元素；
获得协同导航系统的量测方程：
Z＝[Δθx Δθy Δθz Δr12]T＝h(X)+V (9)
式中，Z为量测变量，V为量测噪声向量；h(X)为量测函数，基于所述主用惯导与所述备用惯导的导航参数之间的数学关系获取。
优选地，所述步骤S3，包括：
步骤S31：基于所述协同导航系统的误差传播方程，构建扩展卡尔曼滤波的状态方程
Figure BDA0003706505400000044
其中，F为状态转移矩阵，G为协同导航系统的干扰矩阵，W为协同导航系统的随机干扰向量；
步骤S32：获取离散化的扩展卡尔曼滤波状态方程和量测方程：
Figure BDA0003706505400000045
式中，Xk为k时刻的状态估计，Xk-1为k-1时刻的状态估计，Zk为k时刻的观测值，φk,k-1为非奇异状态一步转移矩阵，Γk,k-1为所述协同导航系统过程噪声输入矩阵，h(Xk)为Xk的量测函数，Wk-1是所述协同导航系统随机过程噪声序列，Vk是协同导航系统随机量测噪声序列；
步骤S33：给定初始值
Figure BDA0003706505400000051
和P0，根据k时刻的观测值Zk，实时递推计算得到k时刻的状态估计
Figure BDA0003706505400000052
具体求解过程如下：
Figure BDA0003706505400000053
式中，
Figure BDA0003706505400000054
和P0分别表示一步预测状态估计和估计误差方差阵的初始值，
Figure BDA0003706505400000055
为k-1时刻的状态估计，
Figure BDA0003706505400000056
和Pk,k-1分别表示一步预测的状态估计和估计误差方差阵；Kk表示滤波增益矩阵；
Figure BDA0003706505400000057
为量测矩阵；
Figure BDA0003706505400000058
为
Figure BDA0003706505400000059
的量测函数，Rk为系统量测噪声的方差矩阵，Pk-1为k-1的估计误差方差阵，Qk-1为协同导航系统过程噪声的方差矩阵；
步骤S34：基于k时刻的状态估计
Figure BDA00037065054000000510
修正所述主用惯导与所述备用惯导输出的导航参数，生成导航信息。
一种基于相对安装位置关系的多惯导协同导航装置，包括：
关系获取模块：配置为基于多惯导系统中各惯导的相对安装位置测量，建立主用惯导与备用惯导的导航参数之间的数学关系；
量测方式构建模块：配置为获取所述主用惯导与备用惯导之间的相对距离信息和相对姿态信息，基于所述主用惯导与备用惯导的导航参数之间的数学关系构建协同导航系统的量测方程；
导航信息生成模块：配置为对所述多惯导系统的误差进行扩展卡尔曼滤波，构建扩展卡尔曼滤波状态方程；基于所述扩展卡尔曼滤波状态方程及所述协同导航系统的量测方程，生成导航信息。
一种计算机可读存储介质，所述存储介质中存储有多条指令；所述多条指令，用于由处理器加载并执行如前所述方法。
一种电子设备，所述电子设备，包括：
处理器，用于执行多条指令；
存储器，用于存储多条指令；
其中，所述多条指令，用于由所述存储器存储，并由所述处理器加载并执行如前所述方法。
有益效果：与现有技术相比，本发明的技术方案具有以下有益的技术效果：
1、本发明提出了一种基于相对安装位置关系的多惯导协同导航方法，该方法仅仅只需利用多套惯导间的相对安装位置关系，不需要额外的外部辅助导航传感器或信息源即可实现协同导航，方法简便可靠。
2、本发明使用的量测信息为多惯导之间的相对安装位置信息，相比与传统的多平台协同导航，多惯导间的相对姿态和相对距离信息更精确可靠、更新频率更高、信息源更丰富、且不需要实时测量。
3、本发明提出的基于相对安装位置关系的多惯导协同导航系统能够有效地实现导航系统定位精度的提升。
附图说明
图1为根据本发明实施例的基于相对安装位置关系的多惯导协同导航方法流程示意图。
图2为根据本发明实施例的两套惯导系统的协同导航模型图。
图3为根据本发明实施例的基于相对安装位置关系的多惯导协同导航装置结构示意图。
具体实施方式
下面结合附图和实施例，对本发明的技术方案作进一步说明。
本发明提出一种基于相对安装位置关系的多惯导协同导航方法，该方法在不额外增加外部辅助导航传感器或信息源的条件下，仅仅只需基于两套惯导之间的相对安装位置关系，利用两套惯导之间的相对姿态和相对距离信息去抑制惯导系统误差发散，实现两套惯导的有机融合，有效提高导航系统精度和性能。如图1-图2所示，本发明所述的一种基于相对安装位置关系的多惯导协同导航方法，包括以下步骤：
步骤S1：基于多惯导系统中各惯导的相对安装位置测量，建立主用惯导与备用惯导的导航参数之间的数学关系；
步骤S2：获取所述主用惯导与备用惯导之间的相对距离信息和相对姿态信息，基于所述主用惯导与备用惯导的导航参数之间的数学关系构建协同导航系统的量测方程；
步骤S3：对所述多惯导系统的误差进行扩展卡尔曼滤波，构建扩展卡尔曼滤波状态方程；基于所述扩展卡尔曼滤波状态方程及所述协同导航系统的量测方程，生成导航信息。
所述步骤S1：基于多惯导系统中各惯导的相对安装位置测量，建立主用惯导与备用惯导的导航参数之间的数学关系，包括：
步骤S11：通过测量各惯导的相对距离Δr12，构建所述主用惯导与所述备用惯导位置间的数学关系，则有
Figure BDA0003706505400000071
式中，
Figure BDA0003706505400000072
Figure BDA0003706505400000081
Figure BDA0003706505400000082
和
Figure BDA0003706505400000083
分别表示所述主用惯导和所述备用惯导实际输出的纬度、经度信息，L1、λ1和L2、λ1分别表示所述主用惯导和所述备用惯导理想或预期输出的纬度、经度信息，δL1、δλ1和δL2、δλ2分别表示所述主用惯导和所述备用惯导的纬度误差、经度误差；Rn、Re分别表示地球子午圈、卯酉圈曲率半径；h＝(h1+h2)/2表示惯导输出载体的平均高度信息；
Figure BDA0003706505400000084
表示惯导输出载体的平均纬度信息，h1、h2分别表示所述主用惯导、所述备用惯导输出载体的高度。本实施例中，使用长度测量工具获取距离信息。
步骤S12：使用标校装置进行安装标校，使得
Figure BDA0003706505400000085
构建所述主用惯导的姿态矩阵和所述备用惯导的姿态矩阵之间的数学关系，则有
Figure BDA0003706505400000086
式中，
Figure BDA0003706505400000087
Figure BDA0003706505400000088
b1和b2分别表示所述主用惯导和所述备用惯导的载体坐标系；I表示单位阵，
Figure BDA0003706505400000089
表示所述主用惯导载体坐标系相对所述备用惯导载体坐标系的姿态矩阵；
Figure BDA00037065054000000810
为所述主用惯导载体坐标系b1相对导航坐标系n的计算姿态矩阵，
Figure BDA00037065054000000811
为所述备用惯导载体坐标系b2相对导航坐标系n的计算姿态矩阵，φ1 n为所述主用惯导的姿态失准角,
Figure BDA00037065054000000812
为所述备用惯导的姿态失准角，
Figure BDA00037065054000000813
为所述主用惯导载体坐标系b1相对导航坐标系n的理想姿态矩阵，
Figure BDA00037065054000000814
为所述备用惯导载体坐标系b2相对导航坐标系n的理想姿态矩阵。
为了保证空间基准的一致性，使用标校装置进行安装标校。
所述步骤S2：基于所述主用惯导与备用惯导的导航参数之间的数学关系构建协同导航系统的量测方程，包括：
步骤S21：选取状态变量，所述状态变量包括所述主用惯导及所述备用惯导的姿态误差、速度误差、位置误差、陀螺常值误差和加速度计常值误差，构建的状态变量X表示如下：
Figure BDA0003706505400000091
式中，φi n表示姿态失准角，δVi n表示速度误差，
Figure BDA0003706505400000092
表示位置误差，δhi为高度误差，
Figure BDA0003706505400000093
表示陀螺常值误差，
Figure BDA0003706505400000094
表示加速度计常值误差，1≤i≤num，num为所述主用惯导数量与所述备用惯导数量的和，num＝2。
本实施例中，由所述多个惯导组成协同导航系统，构建的状态变量X是30维的，num＝2。
步骤S22：构建协同导航系统的量测方程：
令
Figure BDA0003706505400000095
Figure BDA0003706505400000096
则有
Figure BDA0003706505400000097
其中，Δθ12为构建的所述主用惯导与所述备用惯导间姿态误差角向量，Δθx、Δθy、Δθz分别为姿态误差角向量Δθ12三个元素，可以通过矩阵C计算获得。
基于式(1)和式(8)可以获得协同导航系统的量测方程：
Z＝[Δθx Δθy Δθz Δr12]T＝h(X)+V (9)
式中，Z为量测变量，V为量测噪声向量；h(X)为量测函数，基于所述主用惯导与所述备用惯导的导航参数之间的数学关系获取，具体表达式由式(1)和式(8)获取。本实施例的目的在于选取合适的量测变量Z，获取协同导航系统的量测方程。
所述步骤S3：对所述多惯导系统的误差进行扩展卡尔曼滤波，构建扩展卡尔曼滤波状态方程；基于所述扩展卡尔曼滤波状态方程及所述协同导航系统的量测方程，生成导航信息，包括：
步骤S31：基于所述协同导航系统的误差传播方程，构建扩展卡尔曼滤波的状态方程
Figure BDA0003706505400000101
其中，F为状态转移矩阵，G为协同导航系统的干扰矩阵，W为协同导航系统的随机干扰向量。
步骤S32：获取离散化的扩展卡尔曼滤波状态方程和量测方程：
Figure BDA0003706505400000102
式中，Xk为k时刻的状态估计，Xk-1为k-1时刻的状态估计，Zk为k时刻的观测值，φk,k-1为非奇异状态一步转移矩阵，Γk,k-1为所述协同导航系统过程噪声输入矩阵，h(Xk)为Xk的量测函数，Wk-1是所述协同导航系统随机过程噪声序列，Vk是协同导航系统随机量测噪声序列。本实施例中，通过离散化处理式(10)和式(9)，得到离散化的扩展卡尔曼滤波状态方程和量测方程。
步骤S33：给定初始值
Figure BDA0003706505400000103
和P0，根据k时刻的观测值Zk，实时递推计算得到k时刻的状态估计
Figure BDA0003706505400000104
具体求解过程如下：
Figure BDA0003706505400000111
式中，
Figure BDA0003706505400000112
和P0分别表示一步预测状态估计和估计误差方差阵的初始值，
Figure BDA0003706505400000113
为k-1时刻的状态估计，
Figure BDA0003706505400000114
和Pk,k-1分别表示一步预测的状态估计和估计误差方差阵；Kk表示滤波增益矩阵；
Figure BDA0003706505400000115
为量测矩阵；
Figure BDA0003706505400000116
为
Figure BDA0003706505400000117
的量测函数，Rk为协同导航系统量测噪声的方差矩阵，Pk-1为k-1的估计误差方差阵，Qk-1为协同导航系统过程噪声的方差矩阵。
步骤S34：基于k时刻的状态估计
Figure BDA0003706505400000118
修正所述主用惯导与所述备用惯导输出的导航参数，生成导航信息。从而实现抑制惯导误差发散的目的。
进一步地，修正所述主用惯导与所述备用惯导中输出的导航参数的方式为：
利用估计的欧拉失准角去修正所述主用惯导与所述备用惯导输出的姿态矩阵，具体方法如下：
Figure BDA0003706505400000119
Figure BDA00037065054000001110
利用估计的水平位置误差修正所述主用惯导与所述备用惯导输出的水平位置，具体方法如下：
Figure BDA00037065054000001111
Figure BDA00037065054000001112
本发明还提供了一种基于相对安装位置关系的多惯导协同导航装置，如图3所示，该装置包括：
关系获取模块：配置为基于多惯导系统中各惯导的相对安装位置测量，建立主用惯导与备用惯导的导航参数之间的数学关系；
量测方式构建模块：配置为获取所述主用惯导与备用惯导之间的相对距离信息和相对姿态信息，基于所述主用惯导与备用惯导的导航参数之间的数学关系构建协同导航系统的量测方程；
导航信息生成模块：配置为对所述多惯导系统的误差进行扩展卡尔曼滤波，构建扩展卡尔曼滤波状态方程；基于所述扩展卡尔曼滤波状态方程及所述协同导航系统的量测方程，生成导航信息。
以上的具体实施例仅描述了本发明的设计原理，该描述中的部件形状，名称可以不同，不受限制。所以，本发明领域的技术人员可以对前述实施例记载的技术方案进行修改或等同替换；而这些修改和替换未脱离本发明创造宗旨和技术方案，均应属于本发明的保护范围。

Claims (7)

1.一种基于相对安装位置关系的多惯导协同导航方法，其特征在于，包括：
步骤S1：基于多惯导系统中各惯导的相对安装位置测量，建立主用惯导与备用惯导的导航参数之间的数学关系；
步骤S2：获取所述主用惯导与备用惯导之间的相对距离信息和相对姿态信息，基于所述主用惯导与备用惯导的导航参数之间的数学关系构建协同导航系统的量测方程；
步骤S3：对所述多惯导系统的误差进行扩展卡尔曼滤波，构建扩展卡尔曼滤波状态方程；基于所述扩展卡尔曼滤波状态方程及所述协同导航系统的量测方程，生成导航信息。
2.如权利要求1所述的方法，其特征在于，所述步骤S1，包括：
步骤S11：通过测量各惯导的相对距离Δr12，构建所述主用惯导与所述备用惯导位置间的数学关系，则有
Figure FDA0003706505390000011
式中，
Figure FDA0003706505390000012
Figure FDA0003706505390000013
Figure FDA0003706505390000014
和
Figure FDA0003706505390000015
分别表示所述主用惯导和所述备用惯导实际输出的纬度、经度信息，L1、λ1和L2、λ1分别表示所述主用惯导和所述备用惯导理想或预期输出的纬度、经度信息，δL1、δλ1和δL2、δλ2分别表示所述主用惯导和所述备用惯导的纬度误差、经度误差；Rn、Re分别表示地球子午圈、卯酉圈曲率半径；h＝(h1+h2)/2表示惯导输出载体的平均高度信息；
Figure FDA0003706505390000016
表示惯导输出载体的平均纬度信息，h1、h2分别表示所述主用惯导、所述备用惯导输出载体的高度；
步骤S12：使用标校装置进行安装标校，使得
Figure FDA0003706505390000021
构建所述主用惯导的姿态矩阵和所述备用惯导的姿态矩阵之间的数学关系，则有
Figure FDA0003706505390000022
式中，
Figure FDA0003706505390000023
Figure FDA0003706505390000024
b1和b2分别表示所述主用惯导和所述备用惯导的载体坐标系；I表示单位阵，
Figure FDA0003706505390000025
表示所述主用惯导载体坐标系相对所述备用惯导载体坐标系的姿态矩阵；
Figure FDA0003706505390000026
为所述主用惯导载体坐标系b1相对导航坐标系n的计算姿态矩阵，
Figure FDA0003706505390000027
为所述备用惯导载体坐标系b2相对导航坐标系n的计算姿态矩阵，φ1 n为所述主用惯导的姿态失准角,
Figure FDA0003706505390000028
为所述备用惯导的姿态失准角，
Figure FDA0003706505390000029
为所述主用惯导载体坐标系b1相对导航坐标系n的理想姿态矩阵，
Figure FDA00037065053900000210
为所述备用惯导载体坐标系b2相对导航坐标系n的理想姿态矩阵。
3.如权利要求2所述的方法，其特征在于，所述步骤S2，包括：
步骤S21：选取状态变量，所述状态变量包括所述主用惯导及所述备用惯导的姿态误差、速度误差、位置误差、陀螺常值误差和加速度计常值误差，构建的状态变量X表示如下：
Figure FDA00037065053900000211
式中，φi n表示姿态失准角，δVi n表示速度误差，
Figure FDA00037065053900000212
表示位置误差，δhi为高度误差，
Figure FDA00037065053900000213
表示陀螺常值误差，
Figure FDA00037065053900000214
表示加速度计常值误差，1≤i≤num，num为所述主用惯导数量与所述备用惯导数量的和，num＝2；
步骤S22：构建协同导航系统的量测方程：
令
Figure FDA0003706505390000031
Figure FDA0003706505390000032
则有
Figure FDA0003706505390000033
其中，Δθ12为构建的所述主用惯导与所述备用惯导间姿态误差角向量，Δθx、Δθy、Δθz分别为姿态误差角向量Δθ12三个元素；
获得协同导航系统的量测方程：
Z＝[Δθx Δθy Δθz Δr12]T＝h(X)+V (9)
式中，Z为量测变量，V为量测噪声向量；h(X)为量测函数，基于所述主用惯导与所述备用惯导的导航参数之间的数学关系获取。
4.如权利要求1-3中任一项所述的方法，其特征在于，所述步骤S3，包括：
步骤S31：基于所述协同导航系统的误差传播方程，构建扩展卡尔曼滤波的状态方程
Figure FDA0003706505390000034
其中，F为状态转移矩阵，G为协同导航系统的干扰矩阵，W为协同导航系统的随机干扰向量；
步骤S32：获取离散化的扩展卡尔曼滤波状态方程和量测方程：
Figure FDA0003706505390000035
式中，Xk为k时刻的状态估计，Xk-1为k-1时刻的状态估计，Zk为k时刻的观测值，φk,k-1为非奇异状态一步转移矩阵，Γk,k-1为所述协同导航系统过程噪声输入矩阵，h(Xk)为Xk的量测函数，Wk-1是所述协同导航系统随机过程噪声序列，Vk是协同导航系统随机量测噪声序列；
步骤S33：给定初始值
Figure FDA0003706505390000041
和P0，根据k时刻的观测值Zk，实时递推计算得到k时刻的状态估计
Figure FDA0003706505390000042
具体求解过程如下：
Figure FDA0003706505390000043
式中，
Figure FDA0003706505390000044
和P0分别表示一步预测状态估计和估计误差方差阵的初始值，
Figure FDA0003706505390000045
为k-1时刻的状态估计，
Figure FDA0003706505390000046
和Pk,k-1分别表示一步预测的状态估计和估计误差方差阵；Kk表示滤波增益矩阵；
Figure FDA0003706505390000047
为量测矩阵；
Figure FDA0003706505390000048
为
Figure FDA0003706505390000049
的量测函数，Rk为系统量测噪声的方差矩阵，Pk-1为k-1的估计误差方差阵，Qk-1为协同导航系统过程噪声的方差矩阵；
步骤S34：基于k时刻的状态估计
Figure FDA00037065053900000410
修正所述主用惯导与所述备用惯导输出的导航参数，生成导航信息。
5.一种基于相对安装位置关系的多惯导协同导航装置，其特征在于，包括：
关系获取模块：配置为基于多惯导系统中各惯导的相对安装位置测量，建立主用惯导与备用惯导的导航参数之间的数学关系；
量测方式构建模块：配置为获取所述主用惯导与备用惯导之间的相对距离信息和相对姿态信息，基于所述主用惯导与备用惯导的导航参数之间的数学关系构建协同导航系统的量测方程；
导航信息生成模块：配置为对所述多惯导系统的误差进行扩展卡尔曼滤波，构建扩展卡尔曼滤波状态方程；基于所述扩展卡尔曼滤波状态方程及所述协同导航系统的量测方程，生成导航信息。
6.一种计算机可读存储介质，所述存储介质中存储有多条指令；所述多条指令，用于由处理器加载并执行如权利要求1-4中任一项所述方法。
7.一种电子设备，其特征在于，所述电子设备，包括：
处理器，用于执行多条指令；
存储器，用于存储多条指令；
其中，所述多条指令，用于由所述存储器存储，并由所述处理器加载并执行如权利要求1-4中任一项所述方法。
CN202210723122.8A 2022-06-21 2022-06-21 一种基于相对安装位置关系的多惯导协同导航方法及装置 Pending CN115031731A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210723122.8A CN115031731A (zh) 2022-06-21 2022-06-21 一种基于相对安装位置关系的多惯导协同导航方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210723122.8A CN115031731A (zh) 2022-06-21 2022-06-21 一种基于相对安装位置关系的多惯导协同导航方法及装置

Publications (1)

Publication Number Publication Date
CN115031731A true CN115031731A (zh) 2022-09-09

Family

ID=83126918

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210723122.8A Pending CN115031731A (zh) 2022-06-21 2022-06-21 一种基于相对安装位置关系的多惯导协同导航方法及装置

Country Status (1)

Country Link
CN (1) CN115031731A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116481564A (zh) * 2023-03-11 2023-07-25 中国人民解放军国防科技大学 基于Psi角误差修正模型的极地双惯导协同标定方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116481564A (zh) * 2023-03-11 2023-07-25 中国人民解放军国防科技大学 基于Psi角误差修正模型的极地双惯导协同标定方法
CN116481564B (zh) * 2023-03-11 2024-02-23 中国人民解放军国防科技大学 基于Psi角误差修正模型的极地双惯导协同标定方法

Similar Documents

Publication Publication Date Title
CN112097763B (zh) 一种基于mems imu/磁力计/dvl组合的水下运载体组合导航方法
CN108731670A (zh) 基于量测模型优化的惯性/视觉里程计组合导航定位方法
CN110926464B (zh) 一种基于双模式的惯性导航方法及系统
CN113465628B (zh) 惯性测量单元数据补偿方法及系统
CN111982106A (zh) 导航方法、装置、存储介质及电子装置
CN114413895B (zh) 光纤陀螺旋转惯导联合定位方法、装置、设备及介质
CN116147624B (zh) 一种基于低成本mems航姿参考系统的船舶运动姿态解算方法
CN110873563B (zh) 一种云台姿态估计方法及装置
CN110058288A (zh) 无人机ins/gnss组合导航系统航向误差修正方法及系统
CN115031731A (zh) 一种基于相对安装位置关系的多惯导协同导航方法及装置
CN115856922A (zh) 一种松耦合陆地组合导航方法、装置、计算机设备和介质
CN113566850B (zh) 惯性测量单元的安装角度标定方法、装置和计算机设备
CN114323007A (zh) 一种载体运动状态估计方法及装置
US8566055B1 (en) Gyro indexing compensation method and system
CN111207734B (zh) 一种基于ekf的无人机组合导航方法
CN117073720A (zh) 弱环境与弱动作控制下快速视觉惯性标定与初始化方法及设备
CN108692727B (zh) 一种带有非线性补偿滤波器的捷联惯导系统
CN114019954B (zh) 航向安装角标定方法、装置、计算机设备和存储介质
CN115950450A (zh) 二维多普勒测速仪的标定方法、装置、计算机设备和介质
CN115900705A (zh) 一种紧耦合陆地组合导航方法、装置、计算机设备和介质
CN114895340A (zh) 双天线gnss/ins组合导航系统的定位方法和装置
CN113483762A (zh) 一种位姿优化方法及设备
CN113566849B (zh) 惯性测量单元的安装角度标定方法、装置和计算机设备
CN111964689A (zh) 深海惯导方位快速校正的方法
CN116774263B (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