CN113984049B - 飞行器的飞行轨迹的估计方法、装置及系统 - Google Patents
飞行器的飞行轨迹的估计方法、装置及系统 Download PDFInfo
- Publication number
- CN113984049B CN113984049B CN202111439193.7A CN202111439193A CN113984049B CN 113984049 B CN113984049 B CN 113984049B CN 202111439193 A CN202111439193 A CN 202111439193A CN 113984049 B CN113984049 B CN 113984049B
- Authority
- CN
- China
- Prior art keywords
- aircraft
- data
- stage
- satellite
- acquired
- 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 63
- 238000005070 sampling Methods 0.000 claims description 51
- 238000005259 measurement Methods 0.000 claims description 32
- 238000012545 processing Methods 0.000 claims description 31
- 238000010606 normalization Methods 0.000 claims description 24
- 238000004364 calculation method Methods 0.000 claims description 15
- 238000012937 correction Methods 0.000 claims description 14
- 230000008569 process Effects 0.000 claims description 11
- 238000000354 decomposition reaction Methods 0.000 claims description 3
- 238000013178 mathematical model Methods 0.000 claims description 3
- 239000011159 matrix material Substances 0.000 description 54
- 230000002159 abnormal effect Effects 0.000 description 5
- 238000001914 filtration Methods 0.000 description 5
- 230000006870 function Effects 0.000 description 5
- 238000005516 engineering process Methods 0.000 description 4
- 238000009499 grossing Methods 0.000 description 4
- 238000012546 transfer Methods 0.000 description 4
- 230000008901 benefit Effects 0.000 description 3
- 238000001514 detection method Methods 0.000 description 3
- 230000005484 gravity Effects 0.000 description 3
- 238000003672 processing method Methods 0.000 description 3
- 230000001133 acceleration Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 230000008030 elimination Effects 0.000 description 2
- 238000003379 elimination reaction Methods 0.000 description 2
- 230000007704 transition Effects 0.000 description 2
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 1
- PUAQLLVFLMYYJJ-UHFFFAOYSA-N 2-aminopropiophenone Chemical compound CC(N)C(=O)C1=CC=CC=C1 PUAQLLVFLMYYJJ-UHFFFAOYSA-N 0.000 description 1
- WHXSMMKQMYFTQS-UHFFFAOYSA-N Lithium Chemical compound [Li] WHXSMMKQMYFTQS-UHFFFAOYSA-N 0.000 description 1
- 230000006978 adaptation Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000007405 data analysis Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 230000009191 jumping Effects 0.000 description 1
- 229910052744 lithium Inorganic materials 0.000 description 1
- 230000005389 magnetism Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000005096 rolling process Methods 0.000 description 1
- 238000009987 spinning Methods 0.000 description 1
- 230000009897 systematic effect Effects 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
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/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/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
- G01C21/08—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving use of the magnetic field of the earth
Landscapes
- Engineering & Computer Science (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Life Sciences & Earth Sciences (AREA)
- Environmental & Geological Engineering (AREA)
- General Life Sciences & Earth Sciences (AREA)
- Geology (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种飞行器的飞行轨迹的估计方法、装置及系统。其中，该方法包括：在飞行器处于第一阶段的情况下，利用地磁测量所述飞行器的转速；在所述飞行器处于第二阶段的情况下，启动所述惯性传感器，并基于所述地磁测量的所述飞行器的转速和所述惯性传感器获取的所述飞行器的转速，来获取所述惯性传感器的工作状态；所述飞行器处于第三阶段的情况下，基于卫星获取的飞行器数据进行初始对准；所述飞行器处于第四阶段的情况下，基于所述地磁测量的所述飞行器的转速、姿态、位置、速度信息构建卡尔曼滤波器，并利用所述卡尔曼滤波器估计所述飞行器的飞行轨迹。本发明解决了相关技术中飞行轨迹估计不精确的技术问题。
Description
技术领域
本发明涉及导航领域，具体而言，涉及一种高速自旋飞行器的飞行轨迹的估计方法、装置及系统。
背景技术
惯性导航作为一种自主式的导航与定位技术，依靠其惯性器件(加速度计、陀螺)测量的载体运动信息来连续推算载体速度、位置和姿态，但是在高动态环境下传感器误差过大甚至超出传感器量程，难以正常工作，因此通常采用地磁和卫星辅助实现飞行器轨迹的轨迹和飞行器制导。
现有技术中提出了一种高动态自旋制导炮弹空中组合导航方法中，采用惯性与卫星组合导航的方法，但是其难以在导航系统工作前确定飞行器状态，确定系统开机时间。
现有技术中还提出了一种旋转体姿态解算方法中，依靠惯性和磁组合的姿态解算方法，但却未涉及系统误差和对传感器的误差测量。
针对上述的问题，目前尚未提出有效的解决方案。
发明内容
本发明实施例提供了一种飞行器的飞行轨迹的估计方法、装置及系统，以至少解决相关技术中飞行轨迹估计不精确的技术问题。
根据本发明实施例的一个方面，提供了一种飞行器的飞行轨迹的估计方法，包括：在飞行器处于第一阶段的情况下，利用地磁测量所述飞行器的转速，其中，所述第一阶段为惯性测量器件失效的阶段；在所述飞行器处于第二阶段的情况下，启动所述惯性传感器，并基于所述地磁测量的所述飞行器的转速和所述惯性传感器获取的所述飞行器的转速，来获取所述惯性传感器的工作状态，其中，所述第二阶段为所述地磁测量的所述飞行器的转速低于设定的阈值的阶段；所述飞行器处于第三阶段的情况下，基于卫星获取的飞行器数据进行初始对准，其中，所述第三阶段是能够通过卫星捕获到所述飞行器数据的阶段，所述飞行器数据包括所述飞行器的姿态、位置、速度信息；所述飞行器处于第四阶段的情况下，基于所述地磁测量的所述飞行器的转速、姿态、位置、速度信息构建卡尔曼滤波器，并利用所述卡尔曼滤波器估计所述飞行器的飞行轨迹，其中，所述第四阶段是所述飞行器达到弹道顶点后的阶段。
在本发明实施例中，分为四个阶段对飞行器的转速测量、初始对准和姿态位置速度解算，解决了相关技术中飞行轨迹估计不精确的技术问题，具有提高飞行轨迹估计精度的有益效果。
附图说明
此处所说明的附图用来提供对本发明的进一步理解，构成本申请的一部分，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：
图1是根据本发明第一实施例的一种飞行器的飞行轨迹估计方法的流程图；
图2A是根据本发明实施例的一种飞行器的飞行轨迹四个阶段的示意图；
图2B是根据本发明第二实施例的一种飞行器的飞行轨迹估计方法的流程图；
图3是根据本发明第三实施例的一种飞行器的飞行轨迹估计方法的流程图；
图4是根据本发明第四实施例的初始对准的方法的流程图；
图5是根据本发明第五实施例的一种飞行器的飞行轨迹估计方法的流程图；
图6是根据本发明第六实施例的惯导数据处理方法的流程图；
图7是根据本发明第七实施例的惯导数据处理方法的流程图；
图8是根据本发明第八实施例的一种飞行器的飞行轨迹估计方法的流程图；
图9是根据本发明施例的一种飞行器的飞行轨迹估计装置的结构示意图。
具体实施方式
为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。
需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
本申请的地磁信号也称为地磁信息或地磁数据，时间窗口也简称为窗口。
实施例1
根据本发明实施例，提供了一种飞行器的飞行轨迹的估计方法，如图1所示，该方法包括：
步骤S102，在飞行器处于第一阶段的情况下，利用地磁测量所述飞行器的转速，其中，所述第一阶段为惯性测量器件失效的阶段。
在一个示例性实施例中，通过所述地磁测量获取地磁信号，循环执行以下，直到所述地磁信号的所有原始值全部计算完毕：获取上一窗口的地磁数据的最大值、最小值和平均值；基于所获取的上一窗口的地磁数据的最大值、最小值和平均值，对当前窗口内实时获取的当前的地磁数据点进行归一化处理，并判断归一化处理后的当前的地磁数据点是否为零点，以找出当前窗口中的相邻两个零点；基于所述相邻两个零点计算所述飞行体在所述相邻两个零点对应的两个时刻内的转速；其中，所述地磁数据由离散的多个所述地磁数据点组成。
其中，判断归一化处理后的当前的地磁数据点是否为零点包括：获取所述归一化处理后的当前的地磁数据点的左侧的相邻时间间隔采样点的地磁数据值和右侧的相邻时间间隔采样点的地磁数据值；将左侧的相邻时间间隔采样点的地磁数据值和右侧的相邻时间间隔采样点的地磁数据值相乘，得到结果值；在所述结果值小于零的情况下，判断归一化处理后的当前的地磁数据点为零点，否则不是零点。
其中，基于所述相邻两个零点计算所述飞行体在所述相邻两个零点对应的两个时刻内的转速包括：将所述相邻两个零点对应的两个时刻的值作为数据分析的特征值；基于所述特征值计算所述两个时刻对应的正弦波的周期；基于所述正弦波的周期计算所述飞行体的转速。
在所述当前窗口为第一个时间窗口的情况下，在获取上一窗口的地磁数据的最大值、最小值和平均值之前，所述方法还包括：在所述地磁数据中找到所述地磁数据的第一个极大值和第一个极小值；基于所述第一个极大值出现的时间点和第一个极小值出现的时间点划定所述第一个时间窗口的范围；计算所述第一个时间窗口内的地磁数据的最大值、最小值和平均值，并将所计算出的所述第一个时间窗口内的地磁数据的最大值、最小值和平均值分别作为所述上一窗口的地磁数据的最大值、最小值和平均值。
例如，基于所获取的上一窗口的地磁数据的最大值、最小值和平均值，对当前窗口内实时获取的当前的地磁数据点进行归一化处理，并判断归一化处理后的当前的地磁数据点是否为零点，以找出当前窗口中的相邻两个零点包括：基于所获取的上一窗口的地磁数据的最大值、最小值和平均值，对当前窗口内实时获取的当前的地磁数据点进行归一化处理，并判断归一化处理后的当前的地磁数据点是否为零点，循环执行本步骤，直到找出当前窗口中的相邻两个零点；判断所述相邻两个零点间的距离是否小于距离阈值，如果是，则删除所述相邻两个零点，并获取下一组相邻两个零点，如果否，则保留所述相邻两个零点。
计算完当前窗口的地磁数据之后，所述方法还包括：基于所述当前窗口内的最后一组相邻两个零点对应的周期，来设定下一窗口的时间范围，并将下一窗口作为当前窗口，并跳转到所述获取上一窗口的地磁数据的最大值、最小值和平均值的步骤。其中，所述下一窗口的时间范围为当前窗口内的地磁数据的最后一组正弦图像单个周期时间的整数倍。
步骤S104，在所述飞行器处于第二阶段的情况下，启动所述惯性传感器，并基于所述地磁测量的所述飞行器的转速和所述惯性传感器获取的所述飞行器的转速，来获取所述惯性传感器的工作状态，其中，所述第二阶段为所述地磁测量的所述飞行器的转速低于设定的阈值的阶段。
例如，基于所述地磁测量的所述飞行器的转速与陀螺仪检测的所述飞行器的转速，来确认飞行轴向陀螺仪工作状态；通过已知弹道参数，判断加速度计工作状态；其中，所述惯性传感器包括所述陀螺仪和所述加速度计。
步骤S106，所述飞行器处于第三阶段的情况下，基于卫星获取的飞行器数据进行初始对准，其中，所述第三阶段是能够通过卫星捕获到所述飞行器数据的阶段，所述飞行器数据包括所述飞行器的姿态、位置、速度信息。
例如，通过所述卫星获取的所述飞行器的运动信息、卫星速度分解角度信息、名义弹道信息、惯性信息进行初始对准。
步骤S108，所述飞行器处于第四阶段的情况下，基于所述地磁测量的所述飞行器的转速、姿态、位置、速度信息构建卡尔曼滤波器，并利用所述卡尔曼滤波器估计所述飞行器的飞行轨迹，其中，所述第四阶段是所述飞行器达到弹道顶点后的阶段。
例如，通过所述地磁测量，估计所述飞行器的转速与转动差值，通过所述惯性传感器，获取所述飞行器的惯性信息，并通过所述卫星以及名义弹道，进行实时弹道测量与弹道预测，以确定所述卡尔曼滤波器的状态量和观测量；基于所述卡尔曼滤波器的状态量和观测量对所述惯性传感器的输出进行零偏误差修正，并对所述卫星的实时弹道测量与弹道预测进行弹道误差修正。
其中，所述状态量包括速度误差、位置误差、姿态误差和陀螺零偏；所述观测量包括位置、速度、俯仰、偏航、飞行轴向陀螺零偏。
本实施例中，利用矢量地磁进行空间姿态角和转速测量，判断飞行器运动状态后转入惯导组合导航阶段，之后，通过卫星获取实时飞行器三维位置、速度信息，实现了对飞行器的飞行轨迹的精确估计。
实施例2
根据本申请实施例，提供了另一种飞行器的飞行轨迹的估计方法。
图2A是根据本申请实施例的飞行器的估计飞行轨迹的四个阶段的示意图，如图2A所示，本申请实施例将飞行器的飞行划分为四个阶段。第一阶段，是飞行器起飞到转速从高速降到设定的阈值的阶段，第二阶段为转速进一步降低的阶段，例如低于设定的阈值并且未检测到卫星数据的阶段，第三阶段为检测到卫星数据的阶段，第四阶段为飞行器达到弹道顶点后的阶段。
图2B是根据本申请实施例的飞行器的飞行轨迹估计的方法的流程示意图，如图2B所示，该方法包括以下步骤：
步骤S202，在飞行器处于第一阶段的情况下，利用地磁测量所述飞行器的转速。
在第一阶段，飞行器处于高动态环境下，惯性传感器处于关机状态，该阶段主要依靠地磁测量实时计算飞行器转速，当飞行器转速低于设定阈值(20r/s)时，惯性传感器开机，进入第二阶段。
步骤S204，在飞行器处于第二阶段的情况下，启动所述惯性传感器。
在第二阶段，惯性传感器开机，三轴陀螺仪、三轴加速度计开电工作，利用地磁测量的飞行器(例如，弹体)转速与陀螺仪检测的弹体转速确认飞行轴向陀螺仪工作状态；通过已知弹道参数，判断加速度计工作状态；利用器件自身功能，确认工作状态。
步骤S206，在飞行器处于第三阶段的情况下，进行初始对准。
当捕获到卫星数据时，系统转入第三阶段，进行初始对准，通过卫星测量的运动信息、卫星速度分解角度信息、名义弹道信息、惯性信息复合完成初始对准。
步骤S208，在飞行器处于第四阶段的情况下，进入组合导航。
达到弹道顶点后，进入第四阶段，根据对转运行状态，转入组合导航与弹道修正阶段，构建基于转速、姿态、位置、速度误差状态量的12维EKF滤波器，实时估计弹体姿态、位置、速度信息，完成弹体参考姿态信息、位置信息计算。
本实施例中分为四个阶段对飞行器进行转速测量、初始对准和姿态位置速度解算，从而能够更精确地进行导航。
实施例3
根据本申请实施例，提供了又一种飞行器的飞行轨迹的估计方法，本实施例中的飞行器以弹体为例。如图3所示，该方法包括以下步骤：
步骤S302，在第一阶段，利用地磁信息判断弹体转速。
在第一阶段，利用标量地磁信息判断弹体转速和/或弹体旋转的角度差值，为第二阶段惯性传感器开机提供依据，为组合导航过程中陀螺仪零偏修正提供测量输入。
步骤S304，在第二阶段，确认器件的工作状态。
在第二阶段中，利用惯性传感器获取弹体转速低于启动阈值的惯性信息，确认飞行姿态轴向工作状态；通过已知弹道参数，判断加速度计工作状态；利用器件自身功能，确认工作状态。
步骤S306，在第三阶段，进行初始对准。
利用卫星获取实时弹体三维位置、速度信息；通过卫星测量的运动信息、卫星速度分解角度信息、名义弹道信息、惯性信息复合完成初始对准。
步骤S308，在第四阶段，进行组合导航。
在第四阶段中，构建基于转速、姿态、位置、速度误差状态量的滤波器，实时估计弹体姿态、位置、速度信息。
本实施例中判断转速低于惯性导航启动阈值时惯性系统开机，三轴陀螺仪、三轴加速度计开电工作，从而有效避免了高动态环境造成惯性器件失效的情况。
实施例4
根据本申请实施例，提供了一种初始对准的方法，本实施例中的飞行器以弹体为例。如图4所示，该方法包括以下步骤：
步骤S402，初始对准条件判断。
在初始对准条件判断时，运行三个独立的线程，线程1用于弹道判断，线程2用于转速判断，线程3用于卫星判断。
线程1获取弹道时间，并判断弹道时间是否接近阶段切换，如果没有接近阶段切换，跳转回弹道时间获取步骤。
线程2实时检测弹体的转速，并判断转速是否低于预设的阈值，例如20r/s。如果转速不低于20r/s，则跳转到实时转速检测步骤。
线程3获取卫星信息，并判断卫星信息是否有效。如果卫星信息无效，则跳转回卫星信息获取步骤。
如果接近阶段切换，且转速是否低于2预设的阈值、且卫星信息有效，则执行步骤S404。
步骤S404，初始对准。
线程4用于弹道估计。首先读取弹道装订参数，然后进行弹道对比，获取参考信息，估计弹道位置、弹体速度和弹体姿态。
线程5用于识别惯性信息。通过加速度计、标量地磁、陀螺仪实时采集信息之后，进行Quest快速对准，然后计算弹体姿态。
线程6用于卫星信息提取。利用弹体的经纬度、速度、飞行时间，进行复核计算，以得到弹道位置、弹体速度和弹体姿态。
线程7用于滤波融合。线程4、5、6输出的弹道位置、弹体速度、弹体姿态被输入到线程7，构建卡尔曼滤波器。
线程8用于基于所构建的卡尔曼滤波器进行状态判断，判断是否到达弹道顶点。
本实施例结合飞行器轨迹、实时转速检测和卫星信息的获取共同实现飞行器姿态的初始对准。
实施例5
根据本发明实施例，还提供了一种组合导航方法，如图5所示，该方法包括：
步骤S502，采集数据。
在第一阶段从标量地磁、第二阶段从陀螺仪、加速度计、第三阶段从名义弹道以及卫星采集相关数据，以进行相应的导航。
步骤S504，构建卡尔曼函数、进行误差修正及姿态解算。
在进入第四阶段后，进入组合导航阶段。
1)构建卡尔曼函数。
首先选取系统的误差量作为状态变量：位置误差ΔP、速度误差Δv、姿态误差加速度计零偏误差Δa和陀螺零偏误差Δω，建立状态方程如下：
式(1)中ΔPk和ΔPk+1分别为k时刻、k+1时刻的位置误差；ΔVk和ΔVk+1分别为k时刻、k+1时刻的速度误差；和/>分别为k时刻、k+1时刻的姿态误差；Δak和Δak+1分别为k时刻、k+1时刻的加速度计零偏误差；Δωk和Δωk+1分别为k时刻、k+1时刻的陀螺零偏误差；B1、B2为加速度计零偏误差、陀螺零偏误差的比例系数；Wk·a、Wk·ω分别为k时刻加速度计、陀螺误差的随机系统动态噪声，且其均值和方差满足/>为零均值白噪声序列；k表示时刻，Δt为时间，/>为变化矩阵，n表示导航坐标系，b表示载体坐标系。St为反对称矩阵。
将上述等式写为标准的状态方程Xk+1＝f[Xk，k]+Wk形式得：
其中，F为状态转译矩阵；I3*3为3x3的单位矩阵；为坐标转换矩阵。
其次，建立量测方程。
建立量测方程Zk+1＝h[Xk+1，k+1]，其中，量测量为飞行器的位置、速度，具体为式(3)所示：
其中，Zk+1为k+1时刻量测量，h为观测矩阵，Xk+1为k+1时刻系统的状态向量，k为当前计算时刻。式(4)中量测系统噪声，均值和方差满足为零均值白噪声序列；I为3x3的单位矩阵。
最后，构建扩展卡尔曼滤波算法(extended kalman filter，EKF)。
Pk，k-1＝fk，k-1Pk-1fT k，k-1+Qk-1 (3)
Pk＝(I-KkHk)Pk，k-1 (6)
式中Pk，k-1为状态预测协方差；Pk为状态估计协方差；Kk为卡尔曼增益矩阵；Qk为系统噪声的协方差矩阵；Rk为量测噪声的协方差矩阵。式(5)和式(6)为状态预测部分，式(7)、式(8)和式(9)为更新部分。
2)进行姿态解算。
将从传感器获取的加速度、陀螺仪信息(惯导数据)，用于SINS数据解算，得到位置、速度、姿态信息；
其中x_h(1∶6)为位置和速度，采用式(10)方式解算。x_h(7)、x_h(8)、x_h(9)为滚转角、俯仰角和偏航角，采用式(11)计算；式(11)中acc_t为消除重力后的惯导数据。Rb2t为方向余弦矩阵转欧拉角；Ts为采样时间。
3)进行误差修正。
使用卫星数据、地磁信号修正惯导数据。
计算误差量：
K＝(P*H′)/(H*P*H′+R)
z＝-x_h(1∶6，k)
dx＝K*z (9)
其中，z为GPS与SINS间数据的差值，K为卡尔曼滤波增益，P为状态方程协方差矩阵，H为观测矩阵，H’为观测矩阵的转置矩阵，R为量测噪声协方差，x_h为惯导系统解算出的导航信息矩阵；
修正导航数据：
Pk＝[I-KkHk]Pk，k-1 (14)
其中，x_h为惯导系统解算出的导航信息矩阵，dx为公式(9)中所得的扰动，R32为R矩第三行第二列数据，R33为R矩阵第三行第三列数据，R31为R矩阵第三行第一列数据，R21为R矩阵第二行第一列数据，R11为R矩阵第一行第一列数据，q 2dcm为四元数转方向余弦矩阵，quatk为k时刻四元数，k为当前计算时刻，Kk为扩展卡尔曼增益，Pk.k-1为系统协方差的一步预测矩阵，为观测矩阵的转置，Hk为观测矩阵，/>分别为k时刻的量测噪声的协方差矩阵、量测噪声的均值；εk为残差，/>为系统状态量的一步预测，/>为k时刻系统状态量，/>为系统状态量的一步预测，/>为上一时刻的系统状态量，Φk.k-1为状态转移的一步预测矩阵，/>为系统噪声的均值，Pk为k时刻的系统协方差矩阵，I为单位矩阵，/>为系统量测噪声的协方差矩阵，Pk，k-1为系统协方差的一步预测矩阵，/>为状态转移一步预测的转置矩阵，Pk-1为上一时刻的系统协方差矩阵，Γk-1为系统噪声驱动阵，/>为上一时刻的系统噪声协方差矩阵，/>为系统噪声驱动阵的转置矩阵，T为时间。
其中，为k-1时刻的量测噪声的协方差矩阵，εk为残差，/>为残差的转置矩阵，T为矩阵转置符号，Hk为观测矩阵，Pk.k-1为系统协方差的一步预测矩阵，/>为观测矩阵的转置矩阵，/>为k-1时刻的量测噪声的均值，dk＝(1-b)/(1-bk+1)，(0＜b＜1)为遗忘因子，其作用为加强对新数据对滤波的作用。
步骤S510，进行卡尔曼更新。
更新卡尔曼增益K：
K＝(P*H′)/(H*P*H′+R) (15)
更新滤波状态协方差矩阵P：
P＝F*P*F′+G*Q*G′
P＝(P+P′)/2 (16)
cov(：，k)＝diag(P) (17)
其中，P表示滤波状态协方差矩阵，F表示状态传递矩阵，F′表示状态传递矩阵的转置矩阵，G表示过程噪声增益矩阵，Q表示，G′表示过程噪声增益矩阵的转置矩阵，P′表示滤波状态协方差矩阵的转置矩阵，cov(：，k)表示状态协方差矩阵的对角线上k时刻数据。
更新状态传递矩阵F：
其中，E表示15x15的单位矩阵，Rb2t表示在四元数不为0的情况下四元数转方向余弦矩阵。
本实施例中，利用地磁信号和卫星数据，对惯导数据进行误差修正，并分阶段进行组合导航，从而解决了相关技术中导航不精确的问题，具有提高导航精确度的有益效果。
实施例6
根据本发明实施例，提供了又一种组合导航方法。本实施例与上述实施例中的不同之处在于对惯性传感器的加速度计获取的数据进行了归一化处理、平滑处理，其他处理步骤与上述实施例相同，此处不再赘述。本实施例将着重描述对惯导数据，例如加速度计数据，的处理。
如图6所示，对惯导数据的处理包括以下步骤：
步骤S601，设定第一个采样时间。
设定第一个采样时间的时间设定范围，并将第一个采样时间作为当前采样时间。将第一个采样时间的时间设定范围设置为200ms。
步骤S602，获取当前采样时间内的惯导数据的原始值的最大值、最小值和平均值。
步骤S603，去除原始值中的异常数据。
步骤S604，将处理后的惯导数据的原始值进行平滑处理。
在一个实施例中，在去除了异常数据后可对其原始值数据进行平滑处理，例如：每n个点取一次平均值，即
其中，ms代表平滑处理后的惯性传感器原始值，以此让惯性传感器的原始值达到便于处理的目的。
在其他实施例中，此步骤也可以省略。
步骤S605，将平滑处理后的惯导数据的原始值进行归一化处理。
基于以下公式进行归一化处理：
其中Qi为归一化处理后的数据值，ms为当前采样时间范围内的惯性传感器的平滑处理后的原始值，w(i)ave、w(i)max和W(i)min分别为采样时间范围内的惯性传感器的原始值的平均值、最大值和最小值。
在一个示例性实施例中，w(i)ave的计算过程为：
为了使计算结果更接近真值，其中n代表采样时间内的相邻两个最大值范围内惯性传感器原始值点的个数；r代表在采样时间内的惯性传感器原始值的异常数据的个数；R代表异常数据的值的总和。
步骤S606，计算当前采样时间内的转速。
通过采样时间内正弦波图像经过零点的次数x，可以得出在采样时间内内正弦波的周期：
其中，t表示采样时间值。
进一步计算飞行器的转速：
步骤S607，基于计算出的转速进行导航。
相较于其他已有导航技术，本实施例提供的技术使用处理后的惯导数据进行导航，使得在测量过程中所受干扰因素较少，导航更为可靠稳定且更精确。
实施例7
根据本发明实施例，提供了又一种组合导航方法。本实施例与实施例6中的不同之处在于基于当前采样时间预计了下一采样时间，其他处理步骤与实施例6中相同。
如图7所示，该惯导数据例如，加速度计数据的处理方法包括：
步骤S701，设定第一个采样时间。
设定第一个采样时间的时间设定范围，并将第一个采样时间作为当前采样时间。将第一个采样时间的时间设定范围设置为200ms。
步骤S702，获取当前采样时间内的惯导数据的原始值的最大值、最小值和平均值。
步骤S703，去除原始值中的异常数据。
步骤S704，将处理后的惯导数据的原始值进行平滑处理。
步骤S705，将平滑处理后的惯导数据的原始值进行归一化处理。
步骤S706，计算当前采样时间中的正弦周期，并基于正弦周期计算当前转速。
步骤S701至步骤S706与步骤S601至步骤S606相同，此处不再赘述。
步骤S707，基于当前采样时间，设定下一采样时间。
基于当前采样时间内的惯性传感器信号的正弦波的正弦周期，划定下一采样时间为λ/Ri，其中，λ是倍数。
步骤S708，基于计算出的转速进行导航。
在本实施例中，基于当前采样时间的采样数据的正弦波图像，来预测下一采样时间点，可以截取出更完整的采样数据的正弦波图像，从而使得采样数据更能反映真实情况，进而使得导航更精确。
实施例8
根据本发明实施例，还提供了一种组合导航方法。该组合导航方法第一阶段、第二阶段与第三阶段的处理方法与上述实施例方法基本相同，不同之处在于第四阶段，本实施例在第四阶段获取了导航数据之后，不仅仅依赖于导航数据进行导航，还基于预先设置的期望轨迹进行导航。
如图8所示，该导航方法包括：
步骤S802，构建组合导航模型。
步骤S804，构建扩展卡尔曼滤波算法。
步骤S806，进行姿态解算。
基于构建的组合导航模型和构建的扩展卡尔曼滤波算法，进行误差修正及姿态解算。
步骤S802至步骤S806与实施例6中的构建组合导航模型、构建扩展卡尔曼滤波算法和姿态解算的步骤相同，此处不再赘述。
步骤S808，计算期望轨迹。
在本实施例中，引入了期望飞行轨迹，组合导航的时候，不仅考虑修正后的惯导数据，还要基于期望飞行轨迹进行导航。
例如，在所述卫星数据未被更新的情况下，将螺旋线作为卫星数据未被更新的情况下的期望飞行轨迹，使用所述导航参数对所述飞行器进行导航；在所述卫星数据被更新的情况下，将经过卫星数据未被更新的情况下的期望飞行轨迹与基准圆的交点、并指向基准圆圆心的直线作为基准线，采用焦点连线平行于所述基准线的直线作为期望飞行轨迹，并基于组合导航数学模型，使用所述卫星数据对所述惯性传感器的惯导数据进行误差修正，并采用修正后的惯导数据进行惯性解算，得到所述飞行器的导航参数，以对所述飞行器进行导航。
具体地，基准圆的中心A可表示为：
A＝E-rAe＝F-(rA+rEF)e
＝(xF-(rA+rEF)cosλF，yF+(rA+rEF)sinλF)
其中，rA为基准圆的半径，E为未检测到卫星数据时的期望轨迹和基准圆的交点，e为方向向量，rEF为检测到卫星数据时的期望轨迹的距离，xF和yF为飞行器当前的X轴和Y轴坐标，λF为入射角。
由于螺旋线弹道轨迹经过启控点(xB，yB)，因此检测到卫星数据时的期望轨迹的距离rEF可表示为：
其中，rEF为检测到卫星数据时的期望飞行轨迹的长度，xF为飞行器当前的X轴坐标，yF为飞行器的Y轴坐标，xB为启控点的X轴坐标，yB为启控点的Y轴坐标，λF为入射角，θn1为期望倾角，λM为终端角，K为系数。
之后，确定所述基准线：
其中，bn2为多项式常数项系数，xF为所述基准圆的圆心的X轴坐标，yF为所述基准圆的圆心的Y轴坐标，xB为所述交点的X轴坐标，yB为所述交点的Y轴坐标，λF为入射角，K为常数系数。
在求出rEF和bn2之后，可以看出其轨迹为经过E点指向基准圆中心A，斜率为tanλF的直线，因此，检测到卫星数据时的期望飞行轨迹可以表示为：
其中，yn2为输出，bn2为常数项系数。
步骤S810，拟合期望飞行轨迹。
在计算出期望飞行轨迹之后，基于所获取的惯导数据拟合所计算出的期望飞行轨迹，以对飞行器进行导航。
本实施例中，利用上述滤波算法和卫星数据，对惯导数据进行误差修正，并基于修正后的惯导数据拟合期望飞行轨迹，从而解决了相关技术中导航不精确的问题，具有提高导航精确度的有益效果。
实施例9
根据本申请实施例，提供了一种飞行器的飞行轨迹的估计装置，如图9所示，该装置包括：第一阶段模块92、第二阶段模块94、第三阶段模块96和第四阶段模块98。
第一阶段模块92被配置为在飞行器处于第一阶段的情况下，利用地磁测量所述飞行器的转速，以估计所述飞行器的飞行轨迹，其中，所述第一阶段为惯性测量器件失效的阶段；
第二阶段模块94被配置在所述飞行器处于第二阶段的情况下，启动所述惯性传感器，并基于所述地磁测量的所述飞行器的转速和所述惯性传感器获取的所述飞行器的转速，来获取所述惯性传感器的工作状态，以估计所述飞行器的飞行轨迹，其中，所述第二阶段为所述地磁测量的所述飞行器的转速低于设定的阈值的阶段；
第三阶段模块96被配置为所述飞行器处于第三阶段的情况下，基于卫星获取的飞行器数据进行初始对准，以估计所述飞行器的飞行轨迹，其中，所述第三阶段是能够通过卫星捕获到所述飞行器数据的阶段，所述飞行器数据包括所述飞行器的姿态、位置、速度信息。在一个示例性实施例，可以通过所述卫星获取的所述飞行器的运动信息、卫星速度分解角度信息、名义弹道信息、惯性信息进行初始对准。
第四阶段模块98，被配置为所述飞行器处于第四阶段的情况下，基于所述地磁测量的所述飞行器的转速、姿态、位置、速度信息构建卡尔曼滤波器，并利用所述卡尔曼滤波器估计所述飞行器的飞行轨迹，其中，所述第四阶段是所述飞行器达到弹道顶点后的阶段。
在一个示例性实施例中，第一阶段模块92还被配置为：获取上一窗口的地磁数据的最大值、最小值和平均值；基于所获取的上一窗口的地磁数据的最大值、最小值和平均值，对当前窗口内实时获取的当前的地磁数据点进行归一化处理，并判断归一化处理后的当前的地磁数据点是否为零点，以找出当前窗口中的相邻两个零点；基于所述相邻两个零点计算所述飞行体在所述相邻两个零点对应的两个时刻内的转速；其中，所述地磁数据由离散的多个所述地磁数据点组成。
在一个示例性实施例中，第二阶段模块94还被配置为：基于所述地磁测量的所述飞行器的转速与陀螺仪检测的所述飞行器的转速，来确认飞行轴向陀螺仪工作状态；通过弹道参数，判断加速度计工作状态；其中，所述惯性传感器包括所述陀螺仪和所述加速度计。
在一个示例性实施例中，第二阶段模块94还被配置为：获取所述采样时间Ts内的惯导数据的原始值，对所述惯导数据的原始值进行归一化处理；对归一化处理后的所述惯导数据的原始值进行消除重力处理；基于所述消除重力处理后的惯导数据的原始值，计算所述采样时间Ts内正弦波的周期，基于所述采样时间内正弦波的周期确定下一采样时间Ts的时间设定范围，将所述下一采样时间作为所述采样时间Ts。
在一个示例性实施例中，第四阶段模块98还被配置为：通过所述地磁测量，估计所述飞行器的转速与转动差值，通过所述惯性传感器，获取所述飞行器的惯性信息，并通过所述卫星以及名义弹道，进行实时弹道测量与弹道预测，以确定所述卡尔曼滤波器的状态量和观测量；基于所述卡尔曼滤波器的状态量和观测量对所述惯性传感器的输出进行零偏误差修正，并对所述卫星的实时弹道测量与弹道预测进行弹道误差修正。在一个示例性实施例中，所述状态量包括速度误差、位置误差、姿态误差和陀螺零偏；所述观测量包括位置、速度、俯仰、偏航、飞行轴向陀螺零偏。
本实施例中的估计装置能够实现上述实施例的估计方法，此处不再赘述。
本实施例中，在惯性测量器件失效的高动态环境下，利用地磁辅助获取弹体转速，确定组合导航系统的开机时间。并且，结合飞行器轨迹、实时转速检测和卫星信息的获取共同实现飞行器姿态的初始对准。从而具有以下有益效果：判断转速低于惯性导航启动阈值时惯性系统开机，三轴陀螺仪、三轴加速度计开电工作，有效避免了高动态环境造成惯性器件失效的情况。结合卫星定位数据和装订的弹道参数，提高在系统初始对准的精度和速度。
实施例10
根据本申请实施例，提供了一种飞行器的飞行轨迹的估计系统，该系统包括实施例9中的飞行器的飞行轨迹的估计装置和飞行器。其中，估计装置的结构和实施例9中的相同，此处不再赘述。
实施例11
本发明的实施例还提供了一种存储介质，其上存储有程序，在该程序被执行时，使得计算机能够执行上述实施例中的任一项方法。
可选地，在本实施例中，上述存储介质可以包括但不限于：U盘、只读存储器(ROM，Read-Only Memory)、移动硬盘、磁碟或者光盘等各种可以存储程序代码的介质。
存储介质上可以包括微型数据记录模块，其主要包括通信单元、供电单元、逻辑控制单元、存储单元。微型数据记录模块可作为独立部件应用于整个导航系统。通过外部通信接口实时获取包含加速度、陀螺仪、地磁传感器和卫星接收机在内的传感器输出数据，和系统运行过程中产生的包含飞行姿态、飞行轨迹和导航位置在内的导航数据。经过逻辑控制单元进行信号处理与编解码，最终将传感器数据存储至非易失性存储器中。除此之外，记录仪采用可由导航系统电源供电或外部电源、锂电池等方式供电。本系统运行有相关程序接口，可添加修改相关功能，进行二次开发。
存储单元可选Nor flash、NAND Flash、sd卡、EMMC等作为存储介质。
上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。
以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。
Claims (9)
1.一种飞行器的飞行轨迹的估计方法，其特征在于，包括：
在飞行器处于第一阶段的情况下，利用地磁测量所述飞行器的转速，以估计所述飞行器的飞行轨迹，其中，所述第一阶段为惯性测量器件失效的阶段；
在所述飞行器处于第二阶段的情况下，启动惯性传感器，并基于所述地磁测量的所述飞行器的转速和所述惯性传感器获取的所述飞行器的转速，来获取所述惯性传感器的工作状态，以估计所述飞行器的飞行轨迹，其中，所述第二阶段为所述地磁测量的所述飞行器的转速低于设定的阈值的阶段；
所述飞行器处于第三阶段的情况下，基于卫星获取的飞行器数据进行初始对准，以估计所述飞行器的飞行轨迹，其中，所述第三阶段是能够通过卫星捕获到所述飞行器数据的阶段，所述飞行器数据包括所述飞行器的姿态、位置、速度信息；
所述飞行器处于第四阶段的情况下，基于所述地磁测量的所述飞行器的转速、姿态、位置、速度信息构建卡尔曼滤波器，并利用所述卡尔曼滤波器估计所述飞行器的飞行轨迹，其中，所述第四阶段是所述飞行器达到弹道顶点后的阶段；
其中，利用地磁测量所述飞行器的转速包括：获取上一窗口的地磁数据的最大值、最小值和平均值；基于所获取的上一窗口的地磁数据的最大值、最小值和平均值，对当前窗口内实时获取的当前的地磁数据点进行归一化处理，并判断归一化处理后的当前的地磁数据点是否为零点，以找出当前窗口中的相邻两个零点；基于所述相邻两个零点计算所述飞行器在所述相邻两个零点对应的两个时刻内的转速；其中，所述地磁数据由离散的多个所述地磁数据点组成；
其中，判断归一化处理后的当前的地磁数据点是否为零点包括：获取所述归一化处理后的当前的地磁数据点的左侧的相邻时间间隔采样点的地磁数据值和右侧的相邻时间间隔采样点的地磁数据值；将左侧的相邻时间间隔采样点的地磁数据值和右侧的相邻时间间隔采样点的地磁数据值相乘，得到结果值；在所述结果值小于零的情况下，判断归一化处理后的当前的地磁数据点为零点，否则不是零点；
其中，在所述第四阶段中，在所述基于卫星获取的飞行器数据未被更新的情况下，将螺旋线作为基于卫星获取的飞行器数据未被更新的情况下的期望飞行轨迹，使用导航参数对所述飞行器进行导航；在所述基于卫星获取的飞行器数据被更新的情况下，将经过基于卫星获取的飞行器数据未被更新的情况下的期望飞行轨迹与基准圆的交点、并指向基准圆圆心的直线作为基准线，采用焦点连线平行于所述基准线的直线作为第二期望飞行轨迹，并基于组合导航数学模型，使用所述基于卫星获取的飞行器数据对所述惯性传感器的惯导数据进行误差修正，并采用修正后的惯导数据进行惯性解算，得到所述飞行器的导航参数，以对所述飞行器进行导航；其中，所述基准圆圆心是基于以下参数确定的：所述飞行器当前的X轴和Y轴坐标、入射角、所述基准圆的半径、检测到卫星数据时的所述第二期望轨迹的距离。
2.根据权利要求1所述的方法，其特征在于，基于所述地磁测量的所述飞行器的转速和所述惯性传感器获取的所述飞行器的转速，来获取所述惯性传感器的工作状态，包括：
基于所述地磁测量的所述飞行器的转速与陀螺仪检测的所述飞行器的转速，来确认飞行轴向陀螺仪工作状态；
通过弹道参数，判断加速度计工作状态；
其中，所述惯性传感器包括所述陀螺仪和所述加速度计。
3.根据权利要求1所述的方法，其特征在于，基于卫星获取的飞行器数据进行初始对准包括：通过所述卫星获取的所述飞行器的运动信息、卫星速度分解角度信息、名义弹道信息、惯性信息进行初始对准。
4.根据权利要求1所述的方法，其特征在于，基于所述地磁测量的所述飞行器的转速、姿态、位置、速度信息构建卡尔曼滤波器包括：
通过所述地磁测量，估计所述飞行器的转速与转动差值，通过所述惯性传感器，获取所述飞行器的惯性信息，并通过所述卫星以及名义弹道，进行实时弹道测量与弹道预测，以确定所述卡尔曼滤波器的状态量和观测量；
基于所述卡尔曼滤波器的状态量和观测量对所述惯性传感器的输出进行零偏误差修正，并对所述卫星的实时弹道测量与弹道预测进行弹道误差修正。
5.根据权利要求4所述的方法，其特征在于，所述状态量包括速度误差、位置误差、姿态误差和陀螺零偏；所述观测量包括位置、速度、俯仰、偏航、飞行轴向陀螺零偏。
6.根据权利要求2所述的方法，其特征在于，所述加速度计采集的数据还经过消除重力处理。
7.一种飞行器的飞行轨迹的估计装置，其特征在于，包括：
第一阶段模块，被配置为在飞行器处于第一阶段的情况下，利用地磁测量所述飞行器的转速，以估计所述飞行器的飞行轨迹，其中，所述第一阶段为惯性测量器件失效的阶段；
第二阶段模块，被配置在所述飞行器处于第二阶段的情况下，启动惯性传感器，并基于所述地磁测量的所述飞行器的转速和所述惯性传感器获取的所述飞行器的转速，来获取所述惯性传感器的工作状态，以估计所述飞行器的飞行轨迹，其中，所述第二阶段为所述地磁测量的所述飞行器的转速低于设定的阈值的阶段；
第三阶段模块，被配置为所述飞行器处于第三阶段的情况下，基于卫星获取的飞行器数据进行初始对准，以估计所述飞行器的飞行轨迹，其中，所述第三阶段是能够通过卫星捕获到所述飞行器数据的阶段，所述飞行器数据包括所述飞行器的姿态、位置、速度信息；
第四阶段模块，被配置为所述飞行器处于第四阶段的情况下，基于所述地磁测量的所述飞行器的转速、姿态、位置、速度信息构建卡尔曼滤波器，并利用所述卡尔曼滤波器估计所述飞行器的飞行轨迹，其中，所述第四阶段是所述飞行器达到弹道顶点后的阶段；
其中，所述第一阶段模块还被配置为：获取上一窗口的地磁数据的最大值、最小值和平均值；基于所获取的上一窗口的地磁数据的最大值、最小值和平均值，对当前窗口内实时获取的当前的地磁数据点进行归一化处理，并判断归一化处理后的当前的地磁数据点是否为零点，以找出当前窗口中的相邻两个零点；基于所述相邻两个零点计算所述飞行器在所述相邻两个零点对应的两个时刻内的转速；其中，所述地磁数据由离散的多个所述地磁数据点组成；
其中，所述第一阶段模块还被配置为：判断归一化处理后的当前的地磁数据点是否为零点包括：获取所述归一化处理后的当前的地磁数据点的左侧的相邻时间间隔采样点的地磁数据值和右侧的相邻时间间隔采样点的地磁数据值；将左侧的相邻时间间隔采样点的地磁数据值和右侧的相邻时间间隔采样点的地磁数据值相乘，得到结果值；在所述结果值小于零的情况下，判断归一化处理后的当前的地磁数据点为零点，否则不是零点；
其中，所述估计装置还被配置为：在所述第四阶段中，在所述基于卫星获取的飞行器数据未被更新的情况下，将螺旋线作为基于卫星获取的飞行器数据未被更新的情况下的期望飞行轨迹，使用导航参数对所述飞行器进行导航；在所述基于卫星获取的飞行器数据被更新的情况下，将经过基于卫星获取的飞行器数据未被更新的情况下的期望飞行轨迹与基准圆的交点、并指向基准圆圆心的直线作为基准线，采用焦点连线平行于所述基准线的直线作为第二期望飞行轨迹，并基于组合导航数学模型，使用所述基于卫星获取的飞行器数据对所述惯性传感器的惯导数据进行误差修正，并采用修正后的惯导数据进行惯性解算，得到所述飞行器的导航参数，以对所述飞行器进行导航；其中，所述基准圆圆心是基于以下参数确定的：所述飞行器当前的X轴和Y轴坐标、入射角、所述基准圆的半径、检测到卫星数据时的所述第二期望轨迹的距离。
8.一种飞行器的飞行轨迹的估计系统，其特征在于，包括：
飞行器，设置有惯性传感器、地磁传感器；
如权利要求7所述的飞行轨迹的估计装置，被配置为估计所述飞行器的飞行轨迹。
9.一种计算机可读存储介质，其上存储有程序，在所述程序被执行时，使得处理器执行如权利要求1至6中任一项所述的方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111439193.7A CN113984049B (zh) | 2021-11-30 | 2021-11-30 | 飞行器的飞行轨迹的估计方法、装置及系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111439193.7A CN113984049B (zh) | 2021-11-30 | 2021-11-30 | 飞行器的飞行轨迹的估计方法、装置及系统 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113984049A CN113984049A (zh) | 2022-01-28 |
| CN113984049B true CN113984049B (zh) | 2024-01-26 |
Family
ID=79732598
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111439193.7A Active CN113984049B (zh) | 2021-11-30 | 2021-11-30 | 飞行器的飞行轨迹的估计方法、装置及系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113984049B (zh) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115079227A (zh) * | 2022-07-26 | 2022-09-20 | 武汉优米捷光电子制造有限责任公司 | 基于改进无迹卡尔曼滤波的自旋弹组合导航方法 |
| CN115855071B (zh) * | 2023-03-02 | 2023-04-25 | 北京理工大学 | 一种基于多传感器融合的射程解算方法及系统 |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US6081230A (en) * | 1994-11-29 | 2000-06-27 | Xanavi Informatics Corporation | Navigation system furnished with means for estimating error of mounted sensor |
| CN102589552A (zh) * | 2012-01-19 | 2012-07-18 | 北京华力创通科技股份有限公司 | 低成本组合导航系统的数据融合方法和装置 |
| CN102879793A (zh) * | 2012-09-28 | 2013-01-16 | 北京信息科技大学 | 超小型gps/ins/磁强计/气压计组合导航系统 |
| CN111351482A (zh) * | 2020-03-19 | 2020-06-30 | 南京理工大学 | 基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法 |
| CN111486865A (zh) * | 2019-01-29 | 2020-08-04 | 北京理工大学 | 传递对准滤波器、传递对准方法及应用其的制导飞行器 |
| CN111692919A (zh) * | 2020-01-16 | 2020-09-22 | 北京理工大学 | 超近射程的飞行器精确制导控制方法 |
| CN111912402A (zh) * | 2020-07-22 | 2020-11-10 | 北京自动化控制设备研究所 | 基于地磁信息辅助gps的高旋转载体的测姿方法及装置 |
| CN111966123A (zh) * | 2020-08-28 | 2020-11-20 | 北京信息科技大学 | 导航设备及飞行器 |
| CN113587746A (zh) * | 2021-10-08 | 2021-11-02 | 北京信息科技大学 | 基于地磁信息测量弹体的大跨度转速的方法、装置及系统 |
-
2021
- 2021-11-30 CN CN202111439193.7A patent/CN113984049B/zh active Active
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US6081230A (en) * | 1994-11-29 | 2000-06-27 | Xanavi Informatics Corporation | Navigation system furnished with means for estimating error of mounted sensor |
| CN102589552A (zh) * | 2012-01-19 | 2012-07-18 | 北京华力创通科技股份有限公司 | 低成本组合导航系统的数据融合方法和装置 |
| CN102879793A (zh) * | 2012-09-28 | 2013-01-16 | 北京信息科技大学 | 超小型gps/ins/磁强计/气压计组合导航系统 |
| CN111486865A (zh) * | 2019-01-29 | 2020-08-04 | 北京理工大学 | 传递对准滤波器、传递对准方法及应用其的制导飞行器 |
| CN111692919A (zh) * | 2020-01-16 | 2020-09-22 | 北京理工大学 | 超近射程的飞行器精确制导控制方法 |
| CN111351482A (zh) * | 2020-03-19 | 2020-06-30 | 南京理工大学 | 基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法 |
| CN111912402A (zh) * | 2020-07-22 | 2020-11-10 | 北京自动化控制设备研究所 | 基于地磁信息辅助gps的高旋转载体的测姿方法及装置 |
| CN111966123A (zh) * | 2020-08-28 | 2020-11-20 | 北京信息科技大学 | 导航设备及飞行器 |
| CN113587746A (zh) * | 2021-10-08 | 2021-11-02 | 北京信息科技大学 | 基于地磁信息测量弹体的大跨度转速的方法、装置及系统 |
Non-Patent Citations (7)
| Title |
|---|
| A High-Spin Ｒate Measurement Method for Projectiles Using a Magnetoresistive Sensor Based on Time- Frequency Domain Analysis;Zhihong Deng等;《sensors》;1-19 * |
| Real-time attitude propagation algorithm for high spinning flying bodies;Ning Liu等;《ELSEVIER》;1-16 * |
| Research on Integrated Navigation Method of High-speed Spinning Flying Body Based on EKF;Liu Ning等;《2021 40th Chinese Control Conference (CCC)》;3739-37444 * |
| SINS/CNS Integrated Navigation System for Ballistic Missile based on Maximum Correntropy Kalman Filter;Zhangming He等;《2018 Annual American Control Conference (ACC)》;1473-1478 * |
| 严丹等.《兵工学报》.2019,第40卷(第12期),第2447-2456页. * |
| 地磁信息的旋转弹姿态算法研究;苏中等;《弹箭与制导学报》;第31卷(第5期);33-35、38 * |
| 基于改进模糊PID的导弹飞行轨迹误差修正反馈控制;王军;《智能计算机与应用》;第7卷(第3期);第28页第2.2节 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113984049A (zh) | 2022-01-28 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113984049B (zh) | 飞行器的飞行轨迹的估计方法、装置及系统 | |
| CN110221328A (zh) | 一种组合导航方法和装置 | |
| Kneip et al. | Deterministic initialization of metric state estimation filters for loosely-coupled monocular vision-inertial systems | |
| JP5586994B2 (ja) | 位置標定装置、位置標定装置の位置標定方法および位置標定プログラム | |
| CN106415205B (zh) | 用于确定用户位置的朝向估计 | |
| CN110956665B (zh) | 车辆拐弯轨迹双向计算方法、系统、装置 | |
| Anjum et al. | Sensor data fusion using unscented kalman filter for accurate localization of mobile robots | |
| EP2502026A2 (en) | Spatial alignment determination for an inertial measurement unit (imu) | |
| CN101216321A (zh) | 捷联惯性导航系统的快速精对准方法 | |
| CN110715659A (zh) | 零速检测方法、行人惯性导航方法、装置及存储介质 | |
| CN112230242A (zh) | 位姿估计系统和方法 | |
| RU2762143C2 (ru) | Система определения курса и углового пространственного положения, выполненная с возможностью функционирования в полярной области | |
| CN107830871B (zh) | 一种补偿陀螺仪角速度数据的方法、装置、陀螺仪和系统 | |
| Samadzadegan et al. | Autonomous navigation of Unmanned Aerial Vehicles based on multi-sensor data fusion | |
| CN102937450A (zh) | 一种基于陀螺测量信息的相对姿态确定方法 | |
| CN110779514B (zh) | 面向仿生偏振导航辅助定姿的分级卡尔曼融合方法及装置 | |
| US8296015B2 (en) | Method and apparatus for decide turn condition using sensor | |
| CN106595669B (zh) | 一种旋转体姿态解算方法 | |
| CN113566850B (zh) | 惯性测量单元的安装角度标定方法、装置和计算机设备 | |
| Kessler et al. | Multi-Sensor indoor pedestrian navigation system with vision aiding | |
| Klein et al. | INS fine alignment with low-cost gyroscopes: Adaptive filters for different measurement types | |
| CN114111773B (zh) | 组合导航方法、装置、系统及存储介质 | |
| CN114842224A (zh) | 一种基于地理底图的单目无人机绝对视觉匹配定位方案 | |
| CN113483752A (zh) | 一种基于加速度匹配的航向快速初始化方法、电子设备、存储介质 | |
| CN108413970B (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 |