CN113432612B - 飞行体的导航方法、装置及*** - Google Patents

飞行体的导航方法、装置及*** Download PDF

Info

Publication number
CN113432612B
CN113432612B CN202110686092.3A CN202110686092A CN113432612B CN 113432612 B CN113432612 B CN 113432612B CN 202110686092 A CN202110686092 A CN 202110686092A CN 113432612 B CN113432612 B CN 113432612B
Authority
CN
China
Prior art keywords
time
noise
matrix
error
navigation
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
Application number
CN202110686092.3A
Other languages
English (en)
Other versions
CN113432612A (zh
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.)
Beijing Information Science and Technology University
Original Assignee
Beijing Information Science and Technology University
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 Beijing Information Science and Technology University filed Critical Beijing Information Science and Technology University
Priority to CN202110686092.3A priority Critical patent/CN113432612B/zh
Publication of CN113432612A publication Critical patent/CN113432612A/zh
Application granted granted Critical
Publication of CN113432612B publication Critical patent/CN113432612B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/20—Instruments for performing navigational calculations
    • 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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/165—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 combined with non-inertial navigation instruments
    • 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/18—Stabilised platforms, e.g. by gyroscope
    • 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
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/393—Trajectory determination or predictive tracking, e.g. Kalman filtering
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

Landscapes

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

Abstract

本发明公开了一种飞行体的导航方法、装置及***。其中，该方法包括：获取所述飞行体的运动状态信息；基于预先构建的组合导航模型和所述运动状态信息，对***噪声和量测噪声中的至少一个进行估计；基于所估计的***噪声和所述量测噪声中的至少一个，修正惯导***的惯导数据，并基于所修正的惯导数据对所述飞行体进行导航。本发明解决了由于导航时需要每次计算雅各比矩阵造成的实时性差的技术问题。

Description

飞行体的导航方法、装置及***
技术领域
本发明涉及计算机导航领域，具体而言，涉及一种飞行体的导航方法、装置及***。
背景技术
高速自旋飞行体在制导过程中，存在着一定的困难。火箭弹的滚动角度较大，卫星在捕捉信号时难度较大；火箭弹飞行过程中横滚角过大，需要较大的动态测量范围，对陀螺仪的精度要求较高。
采用组合导航方案，通常使用EKF滤波方法。在构建模型的过程中，由于***为非线性***，需要进行线性化，但使用雅各比矩阵运算，对实时性有一定影响；此外，在使用EKF滤波算法过程中，无法对***噪声矩阵Q、量测噪声矩阵R进行实时估计，并且在EKF应用过程中为固定值，滤波结果存在误差。
针对上述的问题，目前尚未提出有效的解决方案。
发明内容
本发明实施例提供了一种飞行体的导航方法、装置及***，以至少解决由于导航时需要每次计算雅各比矩阵造成的实时性差的技术问题。
根据本发明实施例的一个方面，提供了一种飞行体的导航方法，包括：获取所述飞行体的运动状态信息；基于预先构建的组合导航模型和所述运动状态信息，对***噪声和量测噪声中的至少一个进行估计；基于所估计的***噪声和所述量测噪声中的至少一个，修正惯导***的惯导数据，并基于所修正的惯导数据对所述飞行体进行导航。
根据本发明实施例的另一方面，还提供了一种飞行体的导航装置，包括：获取模块，被配置为获取所述飞行体的运动状态信息；估计模块，被配置为基于预先构建的组合导航模型和所述运动状态信息，对***噪声和量测噪声中的至少一个进行估计；导航模块，被配置为基于所估计的***噪声和所述量测噪声中的至少一个，修正惯导***的惯导数据，并基于所修正的惯导数据对所述飞行体进行导航。
根据本发明实施例的另一方面，还提供了一种飞行体的导航***，包括：飞行体；以及如上所述的导航装置，被配置为对所述飞行体进行导航。
根据本发明实施例的另一方面，还提供了一种计算机可读存储介质，其上存储有程序，在所述程序被运行时，使得处理器执行如上所述的方法。
在本发明实施例中，获取所述飞行体的运动状态信息；基于预先构建的组合导航模型和所述运动状态信息，对***噪声和量测噪声中的至少一个进行估计；基于所估计的***噪声和所述量测噪声中的至少一个，修正惯导***的惯导数据，并基于所修正的惯导数据对所述飞行体进行导航，实现了提高导航实时性的技术效果，进而解决了由于导航时需要每次计算雅各比矩阵造成的实时性差的技术问题。
附图说明
此处所说明的附图用来提供对本发明的进一步理解，构成本申请的一部分，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：
图1是根据本发明实施例的基于AEKF的高速自旋飞行体组合导航方法的流程图；
图2是根据本发明实施例的一种飞行体的导航方法的流程图；
图3是根据本发明实施例的一导航装置的结构示意图；
图4是根据本发明实施例的一导航***的结构示意图；
图5是根据本发明实施例的另一导航***的结构示意图；
图6是根据本发明实施例的计算机设备的结构示意图。
具体实施例
为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。
需要说明的是，本发明的说明书和权利要求书及上述附图中的术语″第一″、″第二″等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语″包括″和″具有″以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、***、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
实施例1
根据本发明实施例，提供了一种基于AEKF(自适应卡尔曼滤波算法)的高速自旋飞行体组合导航方法的流程图，如图1所示，该方法包括：
步骤S101，***初始化，设定相关导航参数。
步骤S102，进行初始对准，获取飞行体初始的速度、位置和姿态。
步骤S103，建立组合导航模型。
1)状态量构建
为提高组合导航的实时性，采用对***方程和量测方程的非线性函数作泰勒级数展开并仅保留线性项，获得线性模型。
与传统EKF(扩展卡尔曼滤波)建模方法不同，可以避免每次计算雅各比矩阵，直接选取误差量作为状态变量。例如，选取以下的***的误差量作为状态变量：位置误差ΔP、速度误差Δv、姿态误差
Figure GDA0003163277490000031
加速度计零偏误差Δa和陀螺零偏误差Δω，并建立状态方程如下：
Figure GDA0003163277490000041
式(1)中ΔPk和ΔPk+1分别为k时刻、k+1时刻的位置误差；ΔVk和ΔVk+1分别为k时刻、k+1时刻的速度误差；
Figure GDA0003163277490000042
和
Figure GDA0003163277490000043
分别为k时刻、k+1时刻的姿态误差；Δak和Δak+1分别为k时刻、k+1时刻的加速度计零偏误差；Δωk和Δωk+1分别为k时刻、k+1时刻的陀螺零偏误差；B1、B2为加速度计零偏误差、陀螺零偏误差的比例系数；Wk·a、Wk·ω分别为k时刻加速度计、陀螺误差的随机***动态噪声，且其均值和方差满足
Figure GDA0003163277490000044
为零均值白噪声序列；k表示时刻，Δt为时间，
Figure GDA0003163277490000045
为变化矩阵，n表示导航坐标系，b表示载体坐标系。St为反对称矩阵。
将上述等式写为标准的状态方程Xk+1＝f[Xk，k]+Wk形式得：
Figure GDA0003163277490000046
2)观测量构建
建立量测方程Zk+1＝h[Xk+1，k+1]，其中，观测量为载体的位置、速度，具体为式(3)所示：
Figure GDA0003163277490000047
其中，Zk+1为k+1时刻观测量，h为观测矩阵，Xk+1为k+1时刻***的状态向量，k为当前计算时刻。式(3)中量测***噪声，均值和方差满足
Figure GDA0003163277490000051
为零均值白噪声序列；I为3x3的单位矩阵。
步骤S104，SINS姿态解算。
SINS(捷联惯导***)姿态解算。通过加速度计和陀螺仪获取载体的加速度和角速度后，经过坐标变换，由载体坐标系变换至导航坐标系。通过加速计和陀螺仪的自带偏差修正获取的数据，解算出载体当前的位置、速度和姿态信息。
步骤S105，判断GPS信息是否更新，进入组合导航阶段。
1)判断GPS数据是否更新，在更新时刻进入组合导航；
2)若GPS数据未更新，则使用惯导***的惯导数据进行导航，并等待下次GPS数据更新；
3)若GPS数据更新，则首先对GPS数据进行坐标变换，以保证GPS数据与惯导数据处于同一坐标系下。
步骤S106，组合导航阶段。
本实施例采用的滤波方法为AEKF(自适应扩展卡尔曼滤波算法)方法，对***噪声和量测噪声进行估计，得到动态变化的噪声矩阵，提高组合导航的精度。由于噪声的不可知性，对EKF的算法造成影响，故此引入自适应。
1)AEKF算法具体为：
进行状态预测：
Figure GDA0003163277490000052
Figure GDA0003163277490000053
其中，
Figure GDA0003163277490000054
为***状态量的一步预测，
Figure GDA0003163277490000055
为上一时刻的***状态量，Φk.k-1为状态转移的一步预测矩阵，
Figure GDA0003163277490000056
为***噪声的均值，k为当前计算时刻，Pk，k-1为***协方差的一步预测矩阵，
Figure GDA0003163277490000061
为状态转移一步预测的转置矩阵，Pk-1为上一时刻的***协方差矩阵，Гk-1为***噪声驱动阵，
Figure GDA0003163277490000062
为上一时刻的***噪声协方差矩阵，
Figure GDA0003163277490000063
为***噪声驱动阵的转置矩阵，T为时间。
残差表示为：
Figure GDA0003163277490000064
其中，εk为k时刻的残差，Zk为K时刻观测量，H为观测矩阵，
Figure GDA0003163277490000065
为***状态量的一步预测，
Figure GDA0003163277490000066
为***量测噪声的均值。
进行量测更新：
Figure GDA0003163277490000067
Figure GDA0003163277490000068
Pk＝[I-KkHk]Pk，k-1 (9)
上式中，Kk为扩展卡尔曼增益，Pk.k-1为***协方差的一步预测矩阵，
Figure GDA0003163277490000069
为观测矩阵的转置，Hk为观测矩阵，
Figure GDA00031632774900000610
为k时刻***状态量，
Figure GDA00031632774900000611
为***状态量的一步预测，εk为残差，Pk为k时刻的***协方差矩阵，I为单位矩阵，
Figure GDA00031632774900000612
为***量测噪声的协方差矩阵。
自适应估计器为：
Figure GDA00031632774900000613
Figure GDA00031632774900000614
Figure GDA00031632774900000615
Figure GDA00031632774900000616
其中，其中，
Figure GDA00031632774900000617
分别为k时刻的量测噪声的协方差矩阵、量测噪声的均值；
Figure GDA00031632774900000618
为k-1时刻的量测噪声的协方差矩阵，εk为残差，
Figure GDA00031632774900000619
为残差的转置矩阵，T为矩阵转置符号，Hk为观测矩阵，Pk，k-1为***协方差的一步预测矩阵，
Figure GDA00031632774900000620
为观测矩阵的转置矩阵，
Figure GDA0003163277490000071
为k-1时刻的量测噪声的均值，dk＝(1-b)/(1-bk+1)，(0＜b＜1)为遗忘因子，其作用为加强对新数据对滤波的作用。
使用AEKF算法，对噪声进行在线估计，解决了EKF无法估计噪声的问题。
2)算法改进
使用AKEF对***噪声协方差和量测噪声协方差同时进行调节，但实际应用过程中，二者不能同时估计出来。需要已知矩阵
Figure GDA0003163277490000072
或者矩阵
Figure GDA0003163277490000073
进而估计另外一个。若是在协同噪声协方差矩阵
Figure GDA0003163277490000074
和量测协方差
Figure GDA0003163277490000075
均未知时同时更新矩阵，则容易引起滤波发散。因此，通常只对量测噪声进行估计。
在本实施例中，对上述AEKF算法的实施例进行了进一步的改进，对自适应器的量测噪声估计公式中添加系数，具体为：
Figure GDA0003163277490000076
Figure GDA0003163277490000077
其中，
Figure GDA0003163277490000078
分别为k时刻的量测噪声的协方差矩阵、量测噪声的均值，dk＝(1-b)/(1-bk+1)，(0＜b＜1)为遗忘因子，
Figure GDA0003163277490000079
为k-1时刻的量测噪声的协方差矩阵，εk为残差，
Figure GDA00031632774900000710
为残差的转置矩阵，T为矩阵转置符号，Hk为观测矩阵，Pk，k-1为***协方差的一步预测矩阵，
Figure GDA00031632774900000711
为观测矩阵的转置，b为系数，取值范围为(0，1)，k表示时刻，
Figure GDA00031632774900000712
为k-1时刻的量测噪声的均值，Zk为***观测量，Hk为观测矩阵，
Figure GDA00031632774900000713
为***状态量一步预测。
通过对AEKF算法进行改进，提高了导航精度。
3)根据卫星数据修正惯导数据，更新AEKF中的参数状态协方差矩阵P、卡尔曼增益K，以及量测噪声R矩阵，并基于修正后的惯导数据对飞行体进行导航。
实施例2
根据本发明实施例，提供了另一种飞行体的导航方法的流程图，如图2所示，该方法包括：
步骤S202，获取所述飞行体的运动状态信息。
例如，通过GPS***获取飞行体的速度、姿态、位置信息等，和/或，通过惯导***获取飞行体的速度、姿态、位置信息等。
步骤S204，基于预先构建的组合导航模型和所述运动状态信息，对***噪声和量测噪声中的至少一个进行估计。
其中，组合导航模型是通过以下方法构建的：基于所述惯导***的误差量构建状态量；基于GPS***的量测参数构建观测量。
在一个示例性实施例中，通过以下方法来构建状态量：在一个从所述惯导***获取所述误差量，其中，所述误差量包括位置误差ΔP、速度误差Δv、姿态误差
Figure GDA0003163277490000081
加速度计零偏误差Δa和陀螺零偏误差Δω；基于上述这些所述误差量，建立状态方程作为所述状态量。并且，基于GPS***的量测参数，构建量测方程。所构建的量测方程和状态方程即为组合导航模型。
步骤S206，基于所估计的***噪声和所述量测噪声中的至少一个，修正惯导***的惯导数据，并基于所修正的惯导数据对所述飞行体进行导航。
在构建了组合导航模型之后，基于预先构建的组合导航模型和所述运动状态信息，对***噪声和量测噪声中的至少一个进行估计。例如，对所述状态量进行状态预测；对所述观测量进行量测更新；基于所预测的状态量和所更新的观测量，利用自适应估计器对所述***噪声和量测噪声中的至少一个进行估计。
实施例3
根据本公开实施例，还提供了一种用于实施上述实施例1和实施例2中的方法的导航装置，如图3所示，该导航装置300包括：
获取模块32，被配置为获取所述飞行体的运动状态信息。
估计模块34，被配置为基于预先构建的组合导航模型和所述运动状态信息，对***噪声和量测噪声中的至少一个进行估计；
导航模块36，被配置为基于所估计的***噪声和所述量测噪声中的至少一个，修正惯导***的惯导数据，并基于所修正的惯导数据对所述飞行体进行导航。
在本公开的一个实施例中，导航装置300还包括模型建立模块，被配置为基于所述惯导***的误差量构建状态量，基于GPS***的量测参数构建观测量，以建立组合导航模型。
例如，从所述惯导***获取所述误差量，其中，所述误差量包括位置误差ΔP、速度误差Δv、姿态误差
Figure GDA0003163277490000091
加速度计零偏误差Δa和陀螺零偏误差Δω；基于所述误差量，建立如下的状态方程作为所述状态量：
Figure GDA0003163277490000092
其中，ΔPk和ΔPk+1分别为k时刻、k+1时刻的位置误差；ΔVk和ΔVk+1分别为k时刻、k+1时刻的速度误差；
Figure GDA0003163277490000093
和
Figure GDA0003163277490000094
分别为k时刻、k+1时刻的姿态误差；Δak和Δak+1分别为k时刻、k+1时刻的加速度计零偏误差；Δωk和Δωk+1分别为k时刻、k+1时刻的陀螺零偏误差；B1为加速度计零偏误差；B2为陀螺零偏误差的比例系数；wk·a为k时刻加速度计误差的随机***动态噪声；Wk·ω为陀螺误差的随机***动态噪声，k表示时刻，Δt为时间，St为反对称矩阵，
Figure GDA0003163277490000095
为变化矩阵，n表示导航坐标系，b表示载体坐标系。
例如，基于GPS***的量测参数构建如下的量测方程，作为所述观测量：
Figure GDA0003163277490000096
其中，Zk+1为k+1时刻观测量，h为观测矩阵，Xk+1为k+1时刻***的状态向量，I为单位矩阵，ΔPk+1为k+1时刻的位置误差；ΔVk+1为k+1时刻的速度误差。
该导航装置300还包括判断模块，用于判断是否进入组合导航。例如，判断GPS***的数据是否更新；在所述GPS***的数据更新的情况下，进入GPS和惯导***组合导航的状态；在所述GPS***的数据未更新的情况下，使用所述惯导***的数据进行导航，并等待GPS数据更新，然后在所述GPS数据更新后，进入GPS和惯导***组合导航的状态。
该导航装置300的估计模块36还包括状态预测单元、量测更新单元和自适应估计单元，其中，状态预测单元被配置为对所述状态量进行状态预测；量测单元被配置为对所述观测量进行量测更新；自适应估计单元被配置为基于所预测的状态量和所更新的观测量，利用自适应估计器对所述***噪声和量测噪声中的至少一个进行估计。
在一个示例性实施例中，状态预测单元基于以下公式，对所述状态量进行状态预测：
Figure GDA0003163277490000101
Figure GDA0003163277490000102
其中，
Figure GDA0003163277490000103
为***状态量的一步预测，
Figure GDA0003163277490000104
为上一时刻的***状态量，Φk.k-1为状态转移的一步预测矩阵，
Figure GDA0003163277490000105
为***噪声的均值，k为当前计算时刻，Pk，k-1为***协方差的一步预测矩阵，
Figure GDA0003163277490000106
为状态转移一步预测的转置矩阵，Pk-1为上一时刻的***协方差矩阵，Γk-1为***噪声驱动阵，
Figure GDA0003163277490000107
为上一时刻的***噪声协方差矩阵，
Figure GDA0003163277490000108
为***噪声驱动阵的转置矩阵。
在一个示例性实施例中，量测更新单元被配置为基于以下公式，对所述观测量进行量测更新：
Figure GDA0003163277490000109
Figure GDA00031632774900001010
Pk＝[I-KkHk]Pk，k-1
其中，Kk为扩展卡尔曼增益，Pk.k-1为***协方差的一步预测矩阵，
Figure GDA0003163277490000111
为观测矩阵的转置，Hk为观测矩阵，
Figure GDA0003163277490000112
为k时刻***状态量，
Figure GDA0003163277490000113
为***状态量的一步预测，εk为残差，Pk为k时刻的***协方差矩阵，I为单位矩阵，
Figure GDA0003163277490000114
为***量测噪声的协方差矩阵。
在一个示例性的实施例中，估计模块34包括量测噪声估计单元和***噪声估计单元。
量测噪声估计单元例如可以基于以下公式，对所述量测噪声进行估计：
Figure GDA0003163277490000115
Figure GDA0003163277490000116
其中，
Figure GDA0003163277490000117
分别为k时刻的量测噪声的协方差矩阵、量测噪声的均值；dk＝(1-b)/(1-bk+1)，(0＜b＜1)为遗忘因子，
Figure GDA0003163277490000118
为k-1时刻的量测噪声的协方差矩阵，εk为残差，
Figure GDA0003163277490000119
为残差的转置矩阵，T为矩阵转置符号，Hk为观测矩阵，Pk，k-1为***协方差的一步预测矩阵，
Figure GDA00031632774900001110
为观测矩阵的转置矩阵，
Figure GDA00031632774900001111
为k-1时刻的量测噪声的均值。
在其他的实施例中，量测噪声估计单元例如还可以基于以下公式，对所述量测噪声进行估计：
Figure GDA00031632774900001112
Figure GDA00031632774900001113
其中，
Figure GDA00031632774900001114
分别为k时刻的量测噪声的协方差矩阵、量测噪声的均值，dk＝(1-b)/(1-bk+1)，(0＜b＜1)为遗忘因子，
Figure GDA00031632774900001115
为k-1时刻的量测噪声的协方差矩阵，εk为残差，
Figure GDA00031632774900001116
为残差的转置矩阵，T为矩阵转置符号，Hk为观测矩阵，Pk，k-1为***协方差的一步预测矩阵，
Figure GDA00031632774900001117
为观测矩阵的转置，b为系数，取值范围为(0，1)，k表示时刻，
Figure GDA00031632774900001118
为k-1时刻的量测噪声的均值，Zk为***观测量，Hk为观测矩阵，
Figure GDA00031632774900001119
为***状态量一步预测。
***噪声估计单元例如可以基于以下公式，对所述***噪声进行估计：
Figure GDA0003163277490000121
Figure GDA0003163277490000122
其中，
Figure GDA0003163277490000123
和
Figure GDA0003163277490000124
分别为k时刻的***噪声的协方差矩阵、***噪声的均值，dk＝(1-b)/(1-bk+1)，(0＜b＜1)为遗忘因子，
Figure GDA0003163277490000125
为k-1时刻的***噪声协方差矩阵，Kk为扩展卡尔曼增益，εk为残差，
Figure GDA0003163277490000126
为残差的转置矩阵，
Figure GDA0003163277490000127
为扩展卡尔曼增益的转置矩阵，Pk为K时刻***协方差矩阵，Fk，k-1为***状态转移的一步预测矩阵，Pk-1为k-1时刻***协方差矩阵，
Figure GDA0003163277490000128
为***状态转移的一步预测矩阵的转置矩阵，
Figure GDA0003163277490000129
和
Figure GDA00031632774900001210
分别为k时刻和k-1时刻的***噪声的均值，
Figure GDA00031632774900001211
为k时刻***状态量，Fk，k-1为***状态转移的一步预测矩阵，
Figure GDA00031632774900001212
为k-1时刻***状态量，k为当前计算时刻，T为时间。
最后，根据卫星数据修正惯导数据，更新AEKF中的参数状态协方差矩阵P、卡尔曼增益K，以及量测噪声R矩阵，并基于修正后的惯导数据对飞行体进行导航。
实施例4
根据本公开实施例，还提供了一种用于实施上述导航的服务器，如图4所示，该服务器包括一种导航***400，该导航***包括如上所述的导航装置300、惯性传感器42和数据库44。
惯性传感器42被载体例如飞行体携带，并被配置为采集载体的运动状态信息。
导航装置300被配置为获取惯性传感器42所采集的载体的运动状态信息，基于预先构建的组合导航模型和所述运动状态信息，对***噪声和量测噪声中的至少一个进行估计，并基于所估计的***噪声和所述量测噪声中的至少一个，修正惯导***的惯导数据，并基于所修正的惯导数据对所述飞行体进行导航。
惯性传感器42和导航装置300之间可以通过网络连接，例如，使用WiFi、4G、5G等无线技术构建的无线网络。
可选地，本实施例中的具体示例可以参考上述实施例1和实施例2和实施例3中所描述的示例，本实施例在此不再赘述。
实施例5
根据本公开实施例，还提供了另外一种用于实施上述导航的服务器，如图5所示，该服务器包括一种导航***，该导航***包括SINS***52、GPS***54、信号合成点56和AEKF滤波器58。
SINS***52通过加速度计和陀螺仪获取飞行体的加速度和角速度后，经过坐标变换，由载体坐标系变换至导航坐标系。通过加速计和陀螺仪的自带偏差修正获取的数据，解算出飞行体当前的位置、速度和姿态信息。GPS***54也获取飞行体的位置和速度信息。
SINS***52和GPS***54采集的运动状态信息被输入到信号合成点56。信号合成点56判断GPS数据是否更新，在更新时刻指示进入组合导航；若GPS数据未更新，则使用惯导***的惯导数据进行导航，并指示等待下次GPS数据更新；若GPS数据更新，则首先对GPS数据进行坐标变换，以保证GPS数据与惯导数据处于同一坐标系下。
AEKF滤波器58被配置为基于预先构建的组合导航模型和所述运动状态信息，对***噪声和量测噪声中的至少一个进行估计，并基于所估计的***噪声和所述量测噪声中的至少一个，修正惯导***的惯导数据，并基于所修正的惯导数据对所述飞行体进行导航。
在本实施例中，处理器56和AEKF滤波器58相当于实施例4中的导航装置。
实施例6
本公开的实施例还提供了一种存储介质。可选地，在本实施例中，上述存储介质可以实施上述实施例1和实施例2中所描述的方法。
可选地，在本实施例中，上述存储介质可以位于惯性导航***的网络中的多个网络设备中的至少一个网络设备。
可选地，在本实施例中，上述存储介质可以包括但不限于：U盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、移动硬盘、磁碟或者光盘等各种可以存储程序代码的介质。
可选地，在本实施例中，处理器根据存储介质中已存储的程序代码执行上述实施例1和2中的方法。
可选地，本实施例中的具体示例可以参考上述实施例1和实施例2中所描述的示例，本实施例在此不再赘述。
实施例7
下面参考图6，其示出了适于用来实现本公开实施方式的计算机设备800的结构示意图。图6示出的计算机设备仅仅是一个示例，不应对本公开实施方式的功能和使用范围带来任何限制。
如图6所示，计算机设备800包括中央处理单元(CPU)801，其可以根据存储在只读存储器(ROM)802中的程序或者从存储部分808加载到随机访问存储器(RAM)803中的程序而执行各种适当的动作和处理。在RAM803中，还存储有设备800操作所需的各种程序和数据。CPU801、ROM802以及RAM803通过总线804彼此相连。输入/输出(I/O)接口805也连接至总线804。
以下部件连接至I/O接口805：包括键盘、鼠标等的输入部分806；包括诸如阴极射线管(CRT)、液晶显示器(LCD)等以及扬声器等的输出部分807；包括硬盘等的存储部分808；以及包括诸如LAN卡、调制解调器等的网络接口卡的通信部分809。通信部分809经由诸如因特网的网络执行通信处理。驱动器810也根据需要连接至I/O接口805。可拆卸介质811，诸如磁盘、光盘、磁光盘、半导体存储器等等，根据需要安装在驱动器810上，以便于从其上读出的计算机程序根据需要被安装入存储部分808。
根据本公开的实施方式，上文参考流程图描述的过程可以被实现为计算机软件程序。例如，本公开的实施方式包括一种计算机程序产品，其包括承载在计算机可读介质上的计算机程序，该计算机程序包含用于执行流程图所示的方法的程序代码。在这样的实施方式中，该计算机程序可以通过通信部分809从网络上被下载和安装，和/或从可拆卸介质811被安装。在该计算机程序被中央处理单元(CPU)801执行时，执行本公开的方法中限定的上述功能。需要说明的是，本公开的计算机存储介质可以是计算机可读信号介质或者计算机可读存储介质或者是上述两者的任意组合。计算机可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的***、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子可以包括但不限于：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机访问存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本公开中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行***、装置或者器件使用或者与其结合使用。而在本公开中，计算机可读的信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读的信号介质还可以是计算机可读存储介质以外的任何计算机可读介质，该计算机可读介质可以发送、传播或者传输用于由指令执行***、装置或者器件使用或者与其结合使用的程序。计算机可读介质上包含的程序代码可以用任何适当的介质传输，包括但不限于：无线、电线、光缆、RF等等，或者上述的任意合适的组合。
附图中的流程图和框图，图示了按照本公开各种实施方式的***、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段、或代码的一部分，该模块、程序段、或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个接连地表示的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或操作的专用的基于硬件的***来实现，或者可以用专用硬件与计算机指令的组合来实现。
描述于本公开实施方式中所涉及到的模块或单元可以通过软件的方式实现，也可以通过硬件的方式来实现。所描述的模块或单元也可以设置在处理器中，这些模块或单元的名称在某种情况下并不构成对该模块或单元本身的限定。
本公开提出了新的模型构建，解决了传统EKF模型构建需要每次计算雅各比矩阵而造成的实时性差的问题；并使用了AEKF算法，对噪声进行在线估计，解决了EKF无法估计噪声的问题，并对AEKF算法进行改进，提高了导航精度，从而具有实时性增强、导航***误差降低、精度提升的有益效果。
本公开实施例还可以配置为：
(1).一种飞行体的导航方法，包括：
获取所述飞行体的运动状态信息；
基于预先构建的组合导航模型和所述运动状态信息，对***噪声和量测噪声中的至少一个进行估计；
基于所估计的***噪声和所述量测噪声中的至少一个，修正惯导***的惯导数据，并基于所修正的惯导数据对所述飞行体进行导航。
(2).根据项1所述的导航方法，其中，所述组合导航模型是通过以下方法构建的：
基于所述惯导***的误差量构建状态量；
基于GPS***的量测参数构建观测量。
(3).根据项2所述的导航方法，其中，基于所述惯导***的误差量构建状态量包括：
从所述惯导***获取所述误差量，其中，所述误差量包括位置误差ΔP、速度误差Δv、姿态误差
Figure GDA0003163277490000161
加速度计零偏误差Δa和陀螺零偏误差Δω；
基于所述误差量，建立如下的状态方程作为所述状态量：
Figure GDA0003163277490000171
ΔPk和ΔPk+1分别为k时刻、k+1时刻的位置误差；ΔVk和ΔVk+1分别为k时刻、k+1时刻的速度误差；
Figure GDA0003163277490000172
和
Figure GDA0003163277490000173
分别为k时刻、k+1时刻的姿态误差；Δak和Δak+1分别为k时刻、k+1时刻的加速度计零偏误差；Δωk和Δωk+1分别为k时刻、k+1时刻的陀螺零偏误差；B1为加速度计零偏误差；B2为陀螺零偏误差的比例系数；Wk·a为k时刻加速度计误差的随机***动态噪声；Wk·ω为陀螺误差的随机***动态噪声，k表示时刻，Δt为时间，St为反对称矩阵，
Figure GDA0003163277490000175
为变化矩阵，n表示导航坐标系，b表示载体坐标系。
(4).根据项2所述的导航方法，其中，基于GPS***的量测参数构建观测量包括：基于GPS***的量测参数构建如下的量测方程，作为所述观测量：
Figure GDA0003163277490000174
其中，Zk+1为k+1时刻观测量，h为观测矩阵，Xk+1为k+1时刻***的状态向量，I为单位矩阵，ΔPk+1为k+1时刻的位置误差；ΔVk+1为k+1时刻的速度误差。
(5).根据项1至4中任一项所述的导航方法，其中，在修正惯导***的惯导数据之前，所述方法还包括：
判断GPS***的数据是否更新；
在所述GPS***的数据更新的情况下，进入GPS和惯导***组合导航的状态；
在所述GPS***的数据未更新的情况下，使用所述惯导***的数据进行导航，并等待GPS数据更新，然后在所述GPS数据更新后，进入GPS和惯导***组合导航的状态。
(6).根据项2至5中任一项所述的导航方法，其中，基于预先构建的组合导航模型和所述运动状态信息，对***噪声和量测噪声中的至少一个进行估计包括：
对所述状态量进行状态预测；
对所述观测量进行量测更新；
基于所预测的状态量和所更新的观测量，利用自适应估计器对所述***噪声和量测噪声中的至少一个进行估计。
(7).根据项6所述的导航方法，其中，对所述状态量进行状态预测包括：基于以下公式，对所述状态量进行状态预测：
Figure GDA0003163277490000181
Figure GDA0003163277490000182
其中，
Figure GDA0003163277490000183
为***状态量的一步预测，
Figure GDA0003163277490000184
为上一时刻的***状态量，Φk.k-1为状态转移的一步预测矩阵，
Figure GDA0003163277490000185
为***噪声的均值，k为当前计算时刻，Pk，k-1为***协方差的一步预测矩阵，
Figure GDA0003163277490000186
为状态转移一步预测的转置矩阵，Pk-1为上一时刻的***协方差矩阵，Γk-1为***噪声驱动阵，
Figure GDA0003163277490000187
为上一时刻的***噪声协方差矩阵，
Figure GDA0003163277490000188
为***噪声驱动阵的转置矩阵。
(8).根据项7所述的导航方法，其中，对所述观测量进行量测更新包括：基于以下公式，对所述观测量进行量测更新：
Figure GDA0003163277490000189
Figure GDA00031632774900001810
Pk＝[I-KkHk]Pk，k-1
其中，Kk为扩展卡尔曼增益，Pk.k-1为***协方差的一步预测矩阵，
Figure GDA0003163277490000191
为观测矩阵的转置，Hk为观测矩阵，
Figure GDA0003163277490000192
为k时刻***状态量，
Figure GDA0003163277490000193
为***状态量的一步预测，εk为残差，Pk为k时刻的***协方差矩阵，I为单位矩阵，
Figure GDA0003163277490000194
为***量测噪声的协方差矩阵。
(9).根据项6至8中任一项所述的导航方法，其中，基于以下公式，对所述量测噪声进行估计：
Figure GDA0003163277490000195
Figure GDA0003163277490000196
其中，
Figure GDA0003163277490000197
分别为k时刻的量测噪声的协方差矩阵、量测噪声的均值；dk＝(1-b)/(1-bk+1)，(0＜b＜1)为遗忘因子，
Figure GDA0003163277490000198
为k-1时刻的量测噪声的协方差矩阵，εk为残差，
Figure GDA0003163277490000199
为残差的转置矩阵，T为矩阵转置符号，Hk为观测矩阵，Pk，k-1为***协方差的一步预测矩阵，
Figure GDA00031632774900001910
为观测矩阵的转置矩阵，
Figure GDA00031632774900001911
为k-1时刻的量测噪声的均值。
(10).根据项6至8中任一项所述的导航方法，其中，基于以下公式，对所述量测噪声进行估计：
Figure GDA00031632774900001912
Figure GDA00031632774900001913
其中，
Figure GDA00031632774900001914
分别为k时刻的量测噪声的协方差矩阵、量测噪声的均值，dk＝(1-b)/(1-bk+1)，(0＜b＜1)为遗忘因子，
Figure GDA00031632774900001915
为k-1时刻的量测噪声的协方差矩阵，εk为残差，
Figure GDA00031632774900001916
为残差的转置矩阵，T为矩阵转置符号，Hk为观测矩阵，Pk，k-1为***协方差的一步预测矩阵，
Figure GDA00031632774900001917
为观测矩阵的转置，b为系数，取值范围为(0，1)，k表示时刻，
Figure GDA00031632774900001918
为k-1时刻的量测噪声的均值，Zk为***观测量，Hk为观测矩阵，
Figure GDA00031632774900001919
为***状态量一步预测。
(11).根据项6至8中任一项所述的导航方法，其中，基于以下公式，对所述***噪声进行估计：
Figure GDA0003163277490000201
Figure GDA0003163277490000202
其中，
Figure GDA0003163277490000203
和
Figure GDA0003163277490000204
分别为k时刻的***噪声的协方差矩阵、***噪声的均值，dk＝(1-b)/(1-bk+1)，(0＜b＜1)为遗忘因子，
Figure GDA0003163277490000205
为k-1时刻的***噪声协方差矩阵，Kk为扩展卡尔曼增益，εk为残差，
Figure GDA0003163277490000206
为残差的转置矩阵，
Figure GDA0003163277490000207
为扩展卡尔曼增益的转置矩阵，Pk为K时刻***协方差矩阵，Fk，k-1为***状态转移的一步预测矩阵，Pk-1为k-1时刻***协方差矩阵，
Figure GDA0003163277490000208
为***状态转移的一步预测矩阵的转置矩阵，
Figure GDA0003163277490000209
和
Figure GDA00031632774900002010
分别为k时刻和k-1时刻的***噪声的均值，
Figure GDA00031632774900002011
为k时刻***状态量，Fk，k-1为***状态转移的一步预测矩阵，
Figure GDA00031632774900002012
为k-1时刻***状态量，k为当前计算时刻，T为时间。
(12).一种飞行体的导航装置，包括：
获取模块，被配置为获取所述飞行体的运动状态信息；
估计模块，被配置为基于预先构建的组合导航模型和所述运动状态信息，对***噪声和量测噪声中的至少一个进行估计；
导航模块，被配置为基于所估计的***噪声和所述量测噪声中的至少一个，修正惯导***的惯导数据，并基于所修正的惯导数据对所述飞行体进行导
(13).一种飞行体的导航***，包括：
飞行体；以及
如项12所述的导航装置，被配置为对所述飞行体进行导航。
(14).一种计算机可读存储介质，其上存储有程序，在所述程序被运行时，使得处理器执行如项1至11中任一项所述的方法。
上述本公开实施方式序号仅仅为了描述，不代表实施方式的优劣。
上述实施方式中的集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在上述计算机可读取的存储介质中。基于这样的理解，本公开的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在存储介质中，包括若干指令用以使得一台或多台计算机设备(可为个人计算机、服务器或者网络设备等)执行本公开各个实施方式所述方法的全部或部分步骤。
在本公开的上述实施方式中，对各个实施方式的描述都各有侧重，某个实施方式中没有详述的部分，可以参见其他实施方式的相关描述。
在本申请所提供的几个实施方式中，应该理解到，所揭露的客户端，可通过其它的方式实现。其中，以上所描述的装置实施方式仅仅是示意性的，例如所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个***，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，单元或模块的间接耦合或通信连接，可以是电性或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施方式方案的目的。
另外，在本公开各个实施方式中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
以上所述仅是本公开的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本公开原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本公开的保护范围。

Claims (8)

1.一种飞行体的导航方法，包括：
获取所述飞行体的运动状态信息；
基于预先构建的组合导航模型和所述运动状态信息，对***噪声和量测噪声中的至少一个进行估计；
基于所估计的***噪声和量测噪声中的至少一个，修正惯导***的惯导数据，并基于所修正的惯导数据对所述飞行体进行导航；
其中，所述组合导航模型是通过以下方法构建的：基于所述惯导***的误差量构建状态量，建立状态方程；基于GPS***的量测参数构建观测量，建立量测方程；
其中，基于所述惯导***的误差量构建状态量，建立状态方程，包括：从所述惯导***获取所述误差量，其中，所述误差量包括位置误差ΔP、速度误差Δv、姿态误差
Figure FDA0003760094860000011
加速度计零偏误差Δa和陀螺零偏误差Δω；选取所述误差量作为所述状态量，建立如下的状态方程：
Figure FDA0003760094860000012
ΔPk和ΔPk+1分别为k时刻、k+1时刻的位置误差；ΔVk和ΔVk+1分别为k时刻、k+1时刻的速度误差；
Figure FDA0003760094860000013
和
Figure FDA0003760094860000014
分别为k时刻、k+1时刻的姿态误差；Δak和Δak+1分别为k时刻、k+1时刻的加速度计零偏误差；Δωk和Δωk+1分别为k时刻、k+1时刻的陀螺零偏误差；B1为加速度计零偏误差的比例系数；B2为陀螺零偏误差的比例系数；Wk·a为k时刻加速度计误差的随机***动态噪声；Wk·ω为k时刻陀螺误差的随机***动态噪声，k表示时刻，Δt为时间，St为反对称矩阵，
Figure FDA0003760094860000021
为变化矩阵，n表示导航坐标系，b表示载体坐标系；
其中，采用AEKF算法，基于以下公式，对所述量测噪声进行估计：
Figure FDA0003760094860000022
Figure FDA0003760094860000023
其中，
Figure FDA0003760094860000024
分别为k时刻的量测噪声的协方差矩阵、量测噪声的均值，dk＝(1-b)/(1-bk+1)为遗忘因子，
Figure FDA0003760094860000025
为k-1时刻的量测噪声的协方差矩阵，εk为残差，
Figure FDA0003760094860000026
为残差的转置矩阵，T为矩阵转置符号，Hk为观测矩阵，Pk，k-1为***协方差的一步预测矩阵，
Figure FDA0003760094860000027
为观测矩阵的转置，b为系数，取值范围为(0，1)，k表示时刻，
Figure FDA0003760094860000028
为k-1时刻的量测噪声的均值，Zk为***观测量，Hk为观测矩阵，
Figure FDA0003760094860000029
为***状态量的一步预测。
2.根据权利要求1所述的导航方法，其中，基于GPS***的量测参数构建观测量包括：基于GPS***的量测参数构建如下的量测方程：
Figure FDA00037600948600000210
其中，Zk+1为k+1时刻观测量，h为观测矩阵，Xk+1为k+1时刻***的状态向量，l为单位矩阵，ΔPk+1为k+1时刻的位置误差；ΔVk+1为k+1时刻的速度误差。
3.根据权利要求1至2中任一项所述的导航方法，其中，在修正惯导***的惯导数据之前，所述方法还包括：
判断GPS***的数据是否更新；
在所述GPS***的数据更新的情况下，进入GPS和惯导***组合导航的状态；
在所述GPS***的数据未更新的情况下，使用所述惯导***的数据进行导航，并等待GPS数据更新，然后在所述GPS数据更新后，进入GPS和惯导***组合导航的状态。
4.根据权利要求3所述的导航方法，其中，基于预先构建的组合导航模型和所述运动状态信息，对***噪声和量测噪声中的至少一个进行估计包括：
对所述状态量进行状态预测；
对所述观测量进行量测更新；
基于所预测的状态量和所更新的观测量，利用自适应估计器对所述***噪声和量测噪声中的至少一个进行估计。
5.根据权利要求4所述的导航方法，其中，对所述状态量进行状态预测包括：基于以下公式，对所述状态量进行状态预测：
Figure FDA0003760094860000031
Figure FDA0003760094860000032
其中，
Figure FDA0003760094860000033
为***状态量的一步预测，
Figure FDA0003760094860000034
为上一时刻的***状态量，Φk，k-1为状态转移的一步预测矩阵，
Figure FDA0003760094860000035
为***噪声的均值，k为当前计算时刻，Pk，k-1为***协方差的一步预测矩阵，
Figure FDA0003760094860000036
为状态转移一步预测矩阵的转置矩阵，Pk-1为上一时刻的***协方差矩阵，Γk-1为***噪声驱动阵，
Figure FDA0003760094860000037
为上一时刻的***噪声协方差矩阵，
Figure FDA0003760094860000038
为***噪声驱动阵的转置矩阵。
6.一种飞行体的导航装置，包括：
获取模块，被配置为获取所述飞行体的运动状态信息；
估计模块，被配置为基于预先构建的组合导航模型和所述运动状态信息，对***噪声和量测噪声中的至少一个进行估计；
导航模块，被配置为基于所估计的***噪声和量测噪声中的至少一个，修正惯导***的惯导数据，并基于所修正的惯导数据对所述飞行体进行导航；
其中，所述组合导航模型是通过以下方法构建的：基于所述惯导***的误差量构建状态量，建立状态方程；基于GPS***的量测参数构建观测量，建立量测方程；
其中，基于所述惯导***的误差量构建状态量，建立状态方程，包括：从所述惯导***获取所述误差量，其中，所述误差量包括位置误差ΔP、速度误差Δv、姿态误差
Figure FDA0003760094860000041
加速度计零偏误差Δa和陀螺零偏误差Δω；选取所述误差量作为所述状态量，建立如下的状态方程：
Figure FDA0003760094860000042
ΔPk和ΔPk+1分别为k时刻、k+1时刻的位置误差；ΔVk和ΔVk+1分别为k时刻、k+1时刻的速度误差；
Figure FDA0003760094860000043
和
Figure FDA0003760094860000044
分别为k时刻、k+1时刻的姿态误差；Δak和Δak+1分别为k时刻、k+1时刻的加速度计零偏误差；Δωk和Δωk+1分别为k时刻、k+1时刻的陀螺零偏误差；B1为加速度计零偏误差的比例系数；B2为陀螺零偏误差的比例系数；Wk·a为k时刻加速度计误差的随机***动态噪声；Wk·ω为k时刻陀螺误差的随机***动态噪声，k表示时刻，Δt为时间，St为反对称矩阵，
Figure FDA0003760094860000045
为变化矩阵，n表示导航坐标系，b表示载体坐标系；
其中，采用AEKF算法，基于以下公式，对所述量测噪声进行估计：
Figure FDA0003760094860000051
Figure FDA0003760094860000052
其中，
Figure FDA0003760094860000053
分别为k时刻的量测噪声的协方差矩阵、量测噪声的均值，dk＝(1-b)/(1-bk+1)为遗忘因子，
Figure FDA0003760094860000054
为k-1时刻的量测噪声的协方差矩阵，εk为残差，
Figure FDA0003760094860000055
为残差的转置矩阵，T为矩阵转置符号，Hk为观测矩阵，Pk，k-1为***协方差的一步预测矩阵，
Figure FDA0003760094860000056
为观测矩阵的转置，b为系数，取值范围为(0，1)，k表示时刻，
Figure FDA0003760094860000057
为k-1时刻的量测噪声的均值，Zk为***观测量，Hk为观测矩阵，
Figure FDA0003760094860000058
为***状态量的一步预测。
7.一种飞行体的导航***，包括：
飞行体；以及
如权利要求6所述的导航装置，被配置为对所述飞行体进行导航。
8.一种计算机可读存储介质，其上存储有程序，在所述程序被运行时，使得处理器执行如权利要求1至5中任一项所述的方法。
CN202110686092.3A 2021-06-21 2021-06-21 飞行体的导航方法、装置及*** Active CN113432612B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110686092.3A CN113432612B (zh) 2021-06-21 2021-06-21 飞行体的导航方法、装置及***

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110686092.3A CN113432612B (zh) 2021-06-21 2021-06-21 飞行体的导航方法、装置及***

Publications (2)

Publication Number Publication Date
CN113432612A CN113432612A (zh) 2021-09-24
CN113432612B true CN113432612B (zh) 2022-10-14

Family

ID=77756880

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110686092.3A Active CN113432612B (zh) 2021-06-21 2021-06-21 飞行体的导航方法、装置及***

Country Status (1)

Country Link
CN (1) CN113432612B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114111773B (zh) * 2021-11-30 2023-11-17 北京信息科技大学 组合导航方法、装置、***及存储介质
CN114111797B (zh) * 2021-11-30 2024-02-20 北京信息科技大学 基于fpga的卡尔曼滤波器、ip核及导航用芯片

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6417802B1 (en) * 2000-04-26 2002-07-09 Litton Systems, Inc. Integrated inertial/GPS navigation system
CN105043385B (zh) * 2015-06-05 2018-10-26 北京信息科技大学 一种行人自主导航定位的自适应卡尔曼滤波方法
CN106950586A (zh) * 2017-01-22 2017-07-14 无锡卡尔曼导航技术有限公司 用于农机作业的gnss/ins/车辆组合导航方法
CN107621264B (zh) * 2017-09-30 2021-01-19 华南理工大学 车载微惯性/卫星组合导航***的自适应卡尔曼滤波方法
CN110926465A (zh) * 2019-12-11 2020-03-27 哈尔滨工程大学 一种mems/gps松组合导航方法

Also Published As

Publication number Publication date
CN113432612A (zh) 2021-09-24

Similar Documents

Publication Publication Date Title
CN113432612B (zh) 飞行体的导航方法、装置及***
Phuong et al. A DCM based orientation estimation algorithm with an inertial measurement unit and a magnetic compass
CN111551174A (zh) 基于多传感器惯性导航***的高动态车辆姿态计算方法及***
CN110887480B (zh) 基于mems传感器的飞行姿态估计方法及***
CN111156987A (zh) 基于残差补偿多速率ckf的惯性/天文组合导航方法
CN111721288B (zh) 一种mems器件零偏修正方法、装置及存储介质
CN111136660A (zh) 机器人位姿定位方法及***
CN112683269B (zh) 一种附有运动加速度补偿的marg姿态计算方法
CN109141411B (zh) 定位方法、定位装置、移动机器人及存储介质
CN110132287B (zh) 一种基于极限学习机网络补偿的卫星高精度联合定姿方法
CN110595434B (zh) 基于mems传感器的四元数融合姿态估计方法
CN114179825A (zh) 多传感器融合获取量测值置信度方法及自动驾驶车辆
JP2002323322A (ja) 慣性計測装置を用いた姿勢推定装置及び方法並びにプログラム
CN114323007A (zh) 一种载体运动状态估计方法及装置
CN117213480A (zh) 一种传递对准方法、***、设备及存储介质
CN111982126A (zh) 一种全源BeiDou/SINS弹性状态观测器模型设计方法
CN109655057B (zh) 一种六推无人机加速器测量值的滤波优化方法及其***
CN109459769B (zh) 一种自主定位方法与***
Kim et al. Drift error analysis caused by RLG dither axis bending
CN115727871A (zh) 一种轨迹质量检测方法、装置、电子设备和存储介质
CN113946151A (zh) 针对自动驾驶车辆的数据处理方法、装置和自动驾驶车辆
CN115667845A (zh) 用于移动载体的导航辅助方法
CN116539029B (zh) 一种水下动基座的基座定位方法、装置、存储介质及设备
CN114279426B (zh) 一种六轴优化的磁力计在线校准方法
CN113188566B (zh) 一种机载分布式pos数据融合方法

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