CN111912402B - 基于地磁信息辅助gps的高旋转载体的测姿方法及装置 - Google Patents

Abstract

本发明提供了一种基于地磁信息辅助GPS的高旋转载体的测姿方法及装置，该方法利用地磁传感器输出的三轴模拟地磁信息、GPS接收机输出的位置信息、速度信息与时间信息和世界地磁模型输出的地磁分量得到实时的航向角、实时的俯仰角和解算出的滚转角，再基于解算出的滚转角、弹体的转速和采样计数推算得到实时的滚转角，从而得到弹体实时的姿态角。本发明解决了姿态解算周期大于弹体旋转周期的解算滞后问题，实现了在姿态解算滞后的情况下，通过采样点推算获取实时的姿态信息，从而有效解决了传统硬件运算速率不能满足高旋转弹体的实时姿态解算的工程问题。本发明还解决了因GPS接收机定位频率低而引起的与地磁传感器采样时间不同步问题。

Description

基于地磁信息辅助GPS的高旋转载体的测姿方法及装置

技术领域

本发明涉及导航信息处理技术领域，尤其涉及一种基于地磁信息辅助GPS 的高旋转载体的测姿方法及装置。

背景技术

高旋转弹体的智能化改造是精确制导武器的研究热点。智能化改造通常是增加制导与控制系统，利用实时导航参数对弹道进行精确修正，以提高弹体的打击精度和作战效能。

为实现高旋转弹体的精确制导与控制，需要获取弹体实时的姿态和位置、速度参数。随着卫星导航技术的发展，采用低成本、小型化、高动态的单天线 GPS接收机已成为测量高旋转弹体位置、速度的有效途径，但其无法提供姿态测量信息。目前国内外对高旋转弹体姿态测量的方法主要有惯性测量法、太阳方位角法、光电传感器检测法、地磁测量法等。其中，惯性测量法能够提供姿态和位置、速度参数，但由于受成本、体积和动态范围等限制，传统惯导系统已难以满足要求。太阳方位角法易受天气、日昼影响，无法满足弹体全天候、全天时连续使用的要求；光电传感器检测法通常需要在弹体截面对称位置开感光孔，易受弹体结构设计制约，且存在转速变化时测量误差大，易受天气和强杂散光干扰等不足。地磁测量法是利用地磁传感器敏感弹体姿态变化，并结合其它辅助信息来获取弹体姿态信息。尽管地磁传感器存在易受弹体铁磁物质、磁屏蔽干扰等因素影响，但其具有低成本、体积小、精度高、抗冲击抗过载能力强等诸多优点，且测量动态范围大、输出精细稳定，可利用地磁传感器采集弹体在地磁场的信号，并以GPS接收机测量输出作为辅助来获取姿态信息。

基于此，要同时获取高旋转弹体的姿态、位置和速度，可采用地磁+卫星导航的组合方案。利用地磁传感器敏感弹体在磁场中的姿态变化，而卫星导航可采用低成本、小型化、高动态的单天线GPS接收机，以获取高精度的位置、速度。理论上，利用地磁信息来辅助单天线GPS接收机对弹体进行姿态测量是可行的，但由于弹体旋转速率大，高旋转弹体的滚转角速率通常大于15转/秒，甚至高达300转/秒。且姿态解算运算量大，受硬件资源限制，姿态解算周期会远大于弹体旋转周期，姿态解算完成时弹体已旋转了一定角度，解算出的弹体姿态信息是解算前采样时刻的姿态信息，不具有实时性，不能满足弹体实时测姿要求。

发明内容

本发明提供了一种基于地磁信息辅助GPS的高旋转载体的测姿方法及装置，能够解决现有技术中因弹体旋转速率大而导致无法实时测量弹体姿态信息的技术问题。

根据本发明的一方面，提供了一种基于地磁信息辅助GPS的高旋转载体的测姿方法，所述方法包括：

采样与计数电路采集地磁传感器实时输出的三轴模拟地磁信息并对三轴模拟地磁信息的采样点数进行累加，且将当前采集时刻作为姿态解算周期的起始时标T_n；

采样与计数电路将三轴模拟地磁信息转换为弹体坐标系下在x轴、y轴、z 轴上的磁场数字分量Bx、By、Bz；

GPS接收机实时输出弹体的东向、北向与天向速度V_E、V_N、V_U和对应的时间信息；

姿态计算模块基于接收的弹体的东向、北向与天向速度V_E、V_N、V_U得到弹体的航向角φ和俯仰角θ；

插值递推模块将接收的GPS接收机输出的时间信息中距姿态解算周期的起始时标T_n最近的时刻作为GPS定位时标

插值递推模块获取GPS接收机在定位时标

时弹体在大地直角坐标系下的位置信息和速度信息；

插值递推模块基于弹体在大地直角坐标系下的位置信息和速度信息获取弹体坐标系下在姿态解算周期的起始时标T_n时的经度L、纬度B和高度H；

世界地磁模型基于接收的姿态解算周期的起始时标T_n和弹体坐标系下在姿态解算周期的起始时标T_n时的经度L、纬度B和高度H获取导航坐标系下在x 轴、y轴、z轴上的地磁分量N_x、N_y、N_z；

滚转角姿态解算模块基于接收的弹体坐标系下在x轴、y轴、z轴上的磁场数字分量Bx、By、Bz、航向角φ、俯仰角θ和导航坐标系下在x轴、y轴、z轴上的地磁分量N_x、N_y、N_z得到解算出的滚转角γ′，同时将当前解算完成时刻作为姿态解算周期的结束时标T_n+1；

转速计算模块基于过零点法获取弹体的转速M；

采样与计数电路获取多个姿态解算周期中每一个姿态解算周期的采样点数；

滚转角姿态推算模块基于接收的解算出的滚转角γ′、弹体的转速M和多个姿态解算周期中每一个姿态解算周期的采样点数获取实时的滚转角γ；

其中，多个姿态解算周期内转速相同。

优选的，通过下式得到弹体的航向角φ和俯仰角θ：

优选的，插值递推模块基于弹体在大地直角坐标系下的位置信息和速度信息获取弹体坐标系下在姿态解算周期的起始时标T_n时的经度L、纬度B和高度H 包括：

插值递推模块基于弹体在大地直角坐标系下x轴的位置信息和x轴的速度信息获取弹体在姿态解算周期的起始时标T_n时x轴的位置；

插值递推模块基于弹体在大地直角坐标系下y轴的位置信息和y轴的速度信息获取弹体在姿态解算周期的起始时标T_n时y轴的位置；

插值递推模块基于弹体在大地直角坐标系下z轴的位置信息和z轴的速度信息获取弹体在姿态解算周期的起始时标T_n时z轴的位置；

插值递推模块对弹体在姿态解算周期的起始时标T_n时x轴、y轴和z轴的位置进行坐标转换得到弹体坐标系下在姿态解算周期的起始时标T_n时的经度L、纬度B和高度H。

优选的，通过下式获取弹体在姿态解算周期的起始时标T_n时x轴的位置：

通过下式获取弹体在姿态解算周期的起始时标T_n时y轴的位置：

通过下式获取弹体在姿态解算周期的起始时标T_n时z轴的位置：

式中，

分别为弹体在大地直角坐标系下x轴、y轴、z轴的位置信息，

分别为弹体在大地直角坐标系下x轴、y轴、z 轴的速度信息。

优选的，滚转角姿态解算模块基于弹体坐标系下在x轴、y轴、z轴上的磁场数字分量Bx、By、Bz、航向角φ、俯仰角θ和导航坐标系下在x轴、y轴、z 轴上的地磁分量N_x、N_y、N_z得到解算出的滚转角γ′包括：

滚转角姿态解算模块基于弹体坐标系下在x轴、y轴、z轴上的磁场数字分量Bx、By、Bz和导航坐标系下在x轴、y轴、z轴上的地磁分量N_x、N_y、N_z得到姿态转换矩阵

滚转角姿态解算模块基于姿态转换矩阵

航向角φ和俯仰角θ得到解算出的滚转角γ′。

优选的，通过下式得到姿态转换矩阵

通过下式得到解算出的滚转角γ′：

优选的，基于过零点法获取弹体的转速M包括：

获取三轴模拟地磁信息中的弹体径向的地磁模拟信号

获取弹体径向的地磁模拟信号

中两个相邻的过零点时刻；

基于两个相邻的过零点时刻获取弹体的转速M。

优选的，通过下式获取弹体的转速M：

式中，T_M0和T_M1为两个相邻的过零点时刻。

优选的，通过下式获取实时的滚转角γ：

式中，N_i为第(i-1)个姿态解算周期的采样点数，m≥2，且为整数。

根据本发明的另一方面，提供了一种基于地磁信息辅助GPS的高旋转载体的测姿装置，所述装置包括采样与计数电路、GPS接收机、姿态计算模块、插值递推模块、世界地磁模型、滚转角姿态解算模块、转速计算模块、滚转角姿态推算模块和地磁传感器；

采样与计数电路用于采集地磁传感器实时输出的三轴模拟地磁信息并对三轴模拟地磁信息的采样点数进行累加，且将当前采集时刻作为姿态解算周期的起始时标T_n；

采样与计数电路还用于将三轴模拟地磁信息转换为弹体坐标系下在x轴、y 轴、z轴上的磁场数字分量Bx、By、Bz；

GPS接收机用于实时输出弹体的东向、北向与天向速度V_E、V_N、V_U和对应的时间信息；

姿态计算模块用于基于接收的弹体的东向、北向与天向速度V_E、V_N、V_U得到弹体的航向角φ和俯仰角θ；

插值递推模块用于将接收的GPS接收机输出的时间信息中距姿态解算周期的起始时标T_n最近的时刻作为GPS定位时标

插值递推模块还用于获取GPS接收机在定位时标

时弹体在大地直角坐标系下的位置信息和速度信息；

插值递推模块还用于基于弹体在大地直角坐标系下的位置信息和速度信息获取弹体坐标系下在姿态解算周期的起始时标T_n时的经度L、纬度B和高度H；

世界地磁模型用于基于接收的姿态解算周期的起始时标T_n和弹体坐标系下在姿态解算周期的起始时标T_n时的经度L、纬度B和高度H获取导航坐标系下在x轴、y轴、z轴上的地磁分量N_x、N_y、N_z；

滚转角姿态解算模块用于基于接收的弹体坐标系下在x轴、y轴、z轴上的磁场数字分量Bx、By、Bz、航向角φ、俯仰角θ和导航坐标系下在x轴、y轴、 z轴上的地磁分量N_x、N_y、N_z得到解算出的滚转角γ′，同时将当前解算完成时刻作为姿态解算周期的结束时标T_n+1；

转速计算模块用于基于过零点法获取弹体的转速M；

采样与计数电路还用于获取多个姿态解算周期中每一个姿态解算周期的采样点数；

滚转角姿态推算模块用于基于接收的解算出的滚转角γ′、弹体的转速M和多个姿态解算周期中每一个姿态解算周期的采样点数获取实时的滚转角γ；

其中，多个姿态解算周期内转速相同。

应用本发明的技术方案，利用地磁传感器输出的三轴模拟地磁信息、GPS 接收机输出的位置信息、速度信息与时间信息和世界地磁模型输出的地磁分量得到实时的航向角、实时的俯仰角和解算出的滚转角，再基于解算出的滚转角、弹体的转速和采样计数推算得到实时的滚转角，从而得到弹体实时的姿态角。本发明解决了姿态解算周期大于弹体旋转周期的解算滞后问题，实现了在姿态解算滞后的情况下，通过采样点推算获取实时的姿态信息，从而有效解决了传统硬件运算速率不能满足高旋转弹体的实时姿态解算的工程问题。本发明还解决了因GPS接收机定位频率低而引起的与地磁传感器采样时间不同步问题。

附图说明

所包括的附图用来提供对本发明实施例的进一步的理解，其构成了说明书的一部分，用于例示本发明的实施例，并与文字描述一起来阐释本发明的原理。显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1示出了根据本发明的一种实施例提供的基于地磁信息辅助GPS的高旋转载体的测姿方法的流程示意图；

图2示出了根据本发明的一种实施例提供的基于地磁信息辅助GPS的高旋转载体的测姿方法的姿态解算时序图；

图3示出了根据本发明的一种实施例提供的基于地磁信息辅助GPS的高旋转载体的测姿方法的GPS接收机定位输出和姿态解算时序同步图；

图4示出了根据本发明的一种实施例提供的基于地磁信息辅助GPS的高旋转载体的测姿装置的原理框图。

附图标记说明

10、采样与计数电路； 20、GPS接收机；

30、姿态计算模块； 40、插值递推模块；

50、世界地磁模型； 60、滚转角姿态解算模块；

70、转速计算模块； 80、滚转角姿态推算模块；

90、地磁传感器。

具体实施方式

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。以下对至少一个示例性实施例的描述实际上仅仅是说明性的，决不作为对本发明及其应用或使用的任何限制。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

除非另外具体说明，否则在这些实施例中阐述的部件和步骤的相对布置、数字表达式和数值不限制本发明的范围。同时，应当明白，为了便于描述，附图中所示出的各个部分的尺寸并不是按照实际的比例关系绘制的。对于相关领域普通技术人员已知的技术、方法和设备可能不作详细讨论，但在适当情况下，所述技术、方法和设备应当被视为授权说明书的一部分。在这里示出和讨论的所有示例中，任何具体值应被解释为仅仅是示例性的，而不是作为限制。因此，示例性实施例的其它示例可以具有不同的值。应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步讨论。

图1示出了根据本发明的一种实施例提供的基于地磁信息辅助GPS的高旋转载体的测姿方法的流程示意图。如图1所示，本发明提供了一种基于地磁信息辅助GPS的高旋转载体的测姿方法，所述方法包括：

S1、采样与计数电路采集地磁传感器实时输出的三轴模拟地磁信息并对三轴模拟地磁信息的采样点数进行累加，且将当前采集时刻作为姿态解算周期的起始时标T_n；

S2、采样与计数电路将三轴模拟地磁信息转换为弹体坐标系下在x轴、y 轴、z轴上的磁场数字分量Bx、By、Bz；

S3、GPS接收机实时输出弹体的东向、北向与天向速度V_E、V_N、V_U和对应的时间信息；

S4、姿态计算模块基于接收的弹体的东向、北向与天向速度V_E、V_N、V_U得到弹体的航向角φ和俯仰角θ；

S5、插值递推模块将接收的GPS接收机输出的时间信息中距姿态解算周期的起始时标T_n最近的时刻作为GPS定位时标

S6、插值递推模块获取GPS接收机在定位时标

时弹体在大地直角坐标系下的位置信息和速度信息；

S7、插值递推模块基于弹体在大地直角坐标系下的位置信息和速度信息获取弹体坐标系下在姿态解算周期的起始时标T_n时的经度L、纬度B和高度H；

S8、世界地磁模型基于接收的姿态解算周期的起始时标T_n和弹体坐标系下在姿态解算周期的起始时标T_n时的经度L、纬度B和高度H获取导航坐标系下在x轴、y轴、z轴上的地磁分量N_x、N_y、N_z；

S9、滚转角姿态解算模块基于接收的弹体坐标系下在x轴、y轴、z轴上的磁场数字分量Bx、By、Bz、航向角φ、俯仰角θ和导航坐标系下在x轴、y轴、 z轴上的地磁分量N_x、N_y、N_z得到解算出的滚转角γ′，同时将当前解算完成时刻作为姿态解算周期的结束时标T_n+1；

S10、转速计算模块基于过零点法获取弹体的转速M；

S11、采样与计数电路获取多个姿态解算周期中每一个姿态解算周期的采样点数；

S12、滚转角姿态推算模块基于接收的解算出的滚转角γ′、弹体的转速M 和多个姿态解算周期中每一个姿态解算周期的采样点数获取实时的滚转角γ；

其中，多个姿态解算周期内转速相同。

应用本发明的技术方案，利用地磁传感器输出的三轴模拟地磁信息、GPS 接收机输出的位置信息、速度信息与时间信息和世界地磁模型输出的地磁分量得到实时的航向角、实时的俯仰角和解算出的滚转角，再基于解算出的滚转角、弹体的转速和采样计数推算得到实时的滚转角，从而得到弹体实时的姿态角。本发明解决了姿态解算周期大于弹体旋转周期的解算滞后问题，实现了在姿态解算滞后的情况下，通过采样点推算获取实时的姿态信息，从而有效解决了传统硬件运算速率不能满足高旋转弹体的实时姿态解算的工程问题。本发明还解决了因GPS接收机定位频率低而引起的与地磁传感器采样时间不同步问题。

根据本发明的一种实施例，通过下式得到弹体的航向角φ和俯仰角θ：

在本发明中，在弹体的侧滑角和攻角趋近于0时，姿态计算模块基于接收的弹体的东向、北向与天向速度V_E、V_N、V_U得到弹体的航向角φ和俯仰角θ，此时得到的航向角φ和俯仰角θ的精度更好。

根据本发明的一种实施例，插值递推模块基于弹体在大地直角坐标系下的位置信息和速度信息获取弹体坐标系下在姿态解算周期的起始时标T_n时的经度 L、纬度B和高度H包括：

插值递推模块基于弹体在大地直角坐标系下x轴的位置信息和x轴的速度信息获取弹体在姿态解算周期的起始时标T_n时x轴的位置；

插值递推模块基于弹体在大地直角坐标系下y轴的位置信息和y轴的速度信息获取弹体在姿态解算周期的起始时标T_n时y轴的位置；

插值递推模块基于弹体在大地直角坐标系下z轴的位置信息和z轴的速度信息获取弹体在姿态解算周期的起始时标T_n时z轴的位置；

插值递推模块对弹体在姿态解算周期的起始时标T_n时x轴、y轴和z轴的位置进行坐标转换得到弹体坐标系下在姿态解算周期的起始时标T_n时的经度L、纬度B和高度H。

根据本发明的一种实施例，通过下式获取弹体在姿态解算周期的起始时标 T_n时x轴的位置：

通过下式获取弹体在姿态解算周期的起始时标T_n时y轴的位置：

通过下式获取弹体在姿态解算周期的起始时标T_n时z轴的位置：

式中，

分别为弹体在大地直角坐标系下x轴、y轴、z轴的位置信息，

分别为弹体在大地直角坐标系下x轴、y轴、z 轴的速度信息。

根据本发明的一种实施例，滚转角姿态解算模块基于弹体坐标系下在x轴、 y轴、z轴上的磁场数字分量Bx、By、Bz、航向角φ、俯仰角θ和导航坐标系下在x轴、y轴、z轴上的地磁分量N_x、N_y、N_z得到解算出的滚转角γ′包括：

滚转角姿态解算模块基于弹体坐标系下在x轴、y轴、z轴上的磁场数字分量Bx、By、Bz和导航坐标系下在x轴、y轴、z轴上的地磁分量N_x、N_y、N_z得到姿态转换矩阵

滚转角姿态解算模块基于姿态转换矩阵

航向角φ和俯仰角θ得到解算出的滚转角γ′。

根据本发明的一种实施例，通过下式得到姿态转换矩阵

通过下式得到解算出的滚转角γ′：

根据本发明的一种实施例，基于过零点法获取弹体的转速M包括：

获取三轴模拟地磁信息中的弹体径向的地磁模拟信号

获取弹体径向的地磁模拟信号

中两个相邻的过零点时刻；

基于两个相邻的过零点时刻获取弹体的转速M。

根据本发明的一种实施例，通过下式获取弹体的转速M：

式中，T_M0和T_M1为两个相邻的过零点时刻。

根据本发明的一种实施例，通过下式获取实时的滚转角γ：

式中，N_i为第(i-1)个姿态解算周期的采样点数，m≥2，且为整数，mod(·,·) 为取模函数，γ′和γ的单位均为弧度。

其中，通过下式得到第(i-1)个姿态解算周期的采样点数N_i：

N_i＝(T_i+1-T_i)/f_s；

式中，T_i为第(i-1)个姿态解算周期的开始时标，T_i+1为第(i-1)个姿态解算周期的结束时标，f_s为采样频率。

在本发明中，解算出的滚转角γ′为当前姿态解算周期的起始时标时的滚转角，已失去了时效性。因此，通过上式对解算出的滚转角γ′进行推算，从而得到实时的滚转角γ，进而得到弹体实时的姿态信息。

图2示出了根据本发明的一种实施例提供的基于地磁信息辅助GPS的高旋转载体的测姿方法的姿态解算时序图。设弹体转速在两个姿态解算周期内保持不变，则每两个姿态解算周期推算出一个实时的滚转角γ，如图2所示，图中的 T₀、T₁、T₂、T₃、T₄分别表示第一至第五个姿态解算周期的起始时标。同时，当前的姿态解算周期的起始时标也是上一个姿态解算周期的结束时标，当前的姿态解算周期的结束时标也是下一个姿态解算周期的起始时标。图中的 N₀、N₁、N₂、N₃分别表示第一至第四个姿态解算周期的采样点数。γ′₀、γ′₁、γ′₂、γ′₃分别表示第一至第四个姿态解算周期解算出的滚转角。图中的γ₁、γ₂、γ₃分别表示第二至第四个姿态解算周期推算出的滚转角，即实时的滚转角。

此时，根据第一个姿态解算周期解算出的滚转角γ′₀、第一个姿态解算周期的采样点数N₀、第二个姿态解算周期的采样点数N₁和弹体的转速M得到第二个姿态解算周期的实时的滚转角γ₁，如下式所示：

γ₁＝γ′₀+(mod(N₀,M)/M)×2π)+(mod(N₁,M)/M)×2π)。

依次循环，得到每一个姿态解算周期的实时的滚转角，从而得到每一个姿态解算周期的实时姿态信息。

图3示出了根据本发明的一种实施例提供的基于地磁信息辅助GPS的高旋转载体的测姿方法的GPS接收机定位输出和姿态解算时序同步图。如图3所示， T₀、T₁、T₂、T₃、T₄、T₅、T₆、T₇、T₈分别表示第一至第九个姿态解算周期的起始时标，

分别表示第一至第五个GPS定位时标， N₀、N₁、N₂、N₃、N₄、N₅、N₆、N₇分别表示第一至第八个姿态解算周期的采样点数。通过插值递推模块将接收的GPS接收机输出的时间信息中距姿态解算周期的起始时标T_n最近的时刻作为GPS定位时标

从而实现了地磁传感器的采样和GPS定位输出数据的同步。

图4示出了根据本发明的一种实施例提供的基于地磁信息辅助GPS的高旋转载体的测姿装置的原理框图。如图4所示，本发明提供了一种基于地磁信息辅助GPS的高旋转载体的测姿装置，所述装置包括采样与计数电路10、GPS接收机20、姿态计算模块30、插值递推模块40、世界地磁模型50、滚转角姿态解算模块60、转速计算模块70、滚转角姿态推算模块80和地磁传感器90；

采样与计数电路10用于采集地磁传感器90实时输出的三轴模拟地磁信息并对三轴模拟地磁信息的采样点数进行累加，且将当前采集时刻作为姿态解算周期的起始时标T_n；

采样与计数电路10还用于将三轴模拟地磁信息转换为弹体坐标系下在x 轴、y轴、z轴上的磁场数字分量Bx、By、Bz；

GPS接收机20用于实时输出弹体的东向、北向与天向速度V_E、V_N、V_U和对应的时间信息；

姿态计算模块30用于基于接收的弹体的东向、北向与天向速度V_E、V_N、V_U得到弹体的航向角φ和俯仰角θ；

插值递推模块40用于将接收的GPS接收机20输出的时间信息中距姿态解算周期的起始时标T_n最近的时刻作为GPS定位时标

插值递推模块40还用于获取GPS接收机20在定位时标

时弹体在大地直角坐标系下的位置信息和速度信息；

插值递推模块40还用于基于弹体在大地直角坐标系下的位置信息和速度信息获取弹体坐标系下在姿态解算周期的起始时标T_n时的经度L、纬度B和高度 H；

世界地磁模型50用于基于接收的姿态解算周期的起始时标T_n和弹体坐标系下在姿态解算周期的起始时标T_n时的经度L、纬度B和高度H获取导航坐标系下在x轴、y轴、z轴上的地磁分量N_x、N_y、N_z；

滚转角姿态解算模块60用于基于接收的弹体坐标系下在x轴、y轴、z轴上的磁场数字分量Bx、By、Bz、航向角φ、俯仰角θ和导航坐标系下在x轴、y 轴、z轴上的地磁分量N_x、N_y、N_z得到解算出的滚转角γ′，同时将当前解算完成时刻作为姿态解算周期的结束时标T_n+1；

转速计算模块70用于基于过零点法获取弹体的转速M；

采样与计数电路10还用于获取多个姿态解算周期中每一个姿态解算周期的采样点数；

滚转角姿态推算模块80用于基于接收的解算出的滚转角γ′、弹体的转速M 和多个姿态解算周期中每一个姿态解算周期的采样点数获取实时的滚转角γ；

其中，多个姿态解算周期内转速相同。

应用本发明的技术方案，利用地磁传感器输出的三轴模拟地磁信息、GPS 接收机输出的位置信息、速度信息与时间信息和世界地磁模型输出的地磁分量得到实时的航向角、实时的俯仰角和解算出的滚转角，再基于解算出的滚转角、弹体的转速和采样计数推算得到实时的滚转角，从而得到弹体实时的姿态角。本发明解决了姿态解算周期大于弹体旋转周期的解算滞后问题，实现了在姿态解算滞后的情况下，通过采样点推算获取实时的姿态信息，从而有效解决了传统硬件运算速率不能满足高旋转弹体的实时姿态解算的工程问题。本发明还解决了因GPS接收机定位频率低而引起的与地磁传感器采样时间不同步问题。

在本发明中，滚转角姿态推算模块80还用于接收弹体的航向角φ和俯仰角θ，并基于弹体的航向角φ、俯仰角θ和实时的滚转角γ获取弹体的实时姿态信息。

为了便于描述，在这里可以使用空间相对术语，如“在……之上”、“在……上方”、“在……上表面”、“上面的”等，用来描述如在图中所示的一个器件或特征与其他器件或特征的空间位置关系。应当理解的是，空间相对术语旨在包含除了器件在图中所描述的方位之外的在使用或操作中的不同方位。例如，如果附图中的器件被倒置，则描述为“在其他器件或构造上方”或“在其他器件或构造之上”的器件之后将被定位为“在其他器件或构造下方”或“在其他器件或构造之下”。因而，示例性术语“在……上方”可以包括“在……上方”和“在……下方”两种方位。该器件也可以其他不同方式定位(旋转90度或处于其他方位)，并且对这里所使用的空间相对描述作出相应解释。

此外，需要说明的是，使用“第一”、“第二”等词语来限定零部件，仅仅是为了便于对相应零部件进行区别，如没有另行声明，上述词语并没有特殊含义，因此不能理解为对本发明保护范围的限制。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种基于地磁信息辅助GPS的高旋转载体的测姿方法，其特征在于，所述方法包括：

采样与计数电路采集地磁传感器实时输出的三轴模拟地磁信息并对三轴模拟地磁信息的采样点数进行累加，且将当前采集时刻作为姿态解算周期的起始时标T_n；

采样与计数电路将三轴模拟地磁信息转换为弹体坐标系下在x轴、y轴、z轴上的磁场数字分量Bx、By、Bz；

GPS接收机实时输出弹体的东向、北向与天向速度V_E、V_N、V_U和对应的时间信息；

姿态计算模块基于接收的弹体的东向、北向与天向速度V_E、V_N、V_U得到弹体的航向角φ和俯仰角θ；

插值递推模块将接收的GPS接收机输出的时间信息中距姿态解算周期的起始时标T_n最近的时刻作为GPS定位时标

插值递推模块获取GPS接收机在定位时标

时弹体在大地直角坐标系下的位置信息和速度信息；

插值递推模块基于弹体在大地直角坐标系下的位置信息和速度信息获取弹体坐标系下在姿态解算周期的起始时标T_n时的经度L、纬度B和高度H；

世界地磁模型基于接收的姿态解算周期的起始时标T_n和弹体坐标系下在姿态解算周期的起始时标T_n时的经度L、纬度B和高度H获取导航坐标系下在x轴、y轴、z轴上的地磁分量N_x、N_y、N_z；

滚转角姿态解算模块基于接收的弹体坐标系下在x轴、y轴、z轴上的磁场数字分量Bx、By、Bz、航向角φ、俯仰角θ和导航坐标系下在x轴、y轴、z轴上的地磁分量N_x、N_y、N_z得到解算出的滚转角γ′，同时将当前解算完成时刻作为姿态解算周期的结束时标T_n+1；

转速计算模块基于过零点法获取弹体的转速M；

采样与计数电路获取多个姿态解算周期中每一个姿态解算周期的采样点数；

滚转角姿态推算模块基于接收的解算出的滚转角γ′、弹体的转速M和多个姿态解算周期中每一个姿态解算周期的采样点数获取实时的滚转角γ；

其中，多个姿态解算周期内转速相同。

2.根据权利要求1所述的方法，其特征在于，通过下式得到弹体的航向角φ和俯仰角θ：

3.根据权利要求1所述的方法，其特征在于，插值递推模块基于弹体在大地直角坐标系下的位置信息和速度信息获取弹体坐标系下在姿态解算周期的起始时标T_n时的经度L、纬度B和高度H包括：

插值递推模块基于弹体在大地直角坐标系下x轴的位置信息和x轴的速度信息获取弹体在姿态解算周期的起始时标T_n时x轴的位置；

插值递推模块基于弹体在大地直角坐标系下y轴的位置信息和y轴的速度信息获取弹体在姿态解算周期的起始时标T_n时y轴的位置；

插值递推模块基于弹体在大地直角坐标系下z轴的位置信息和z轴的速度信息获取弹体在姿态解算周期的起始时标T_n时z轴的位置；

插值递推模块对弹体在姿态解算周期的起始时标T_n时x轴、y轴和z轴的位置进行坐标转换得到弹体坐标系下在姿态解算周期的起始时标T_n时的经度L、纬度B和高度H。

4.根据权利要求3所述的方法，其特征在于，通过下式获取弹体在姿态解算周期的起始时标T_n时x轴的位置：

通过下式获取弹体在姿态解算周期的起始时标T_n时y轴的位置：

通过下式获取弹体在姿态解算周期的起始时标T_n时z轴的位置：

式中，

分别为弹体在大地直角坐标系下x轴、y轴、z轴的位置信息，

分别为弹体在大地直角坐标系下x轴、y轴、z轴的速度信息。

5.根据权利要求1所述的方法，其特征在于，滚转角姿态解算模块基于弹体坐标系下在x轴、y轴、z轴上的磁场数字分量Bx、By、Bz、航向角φ、俯仰角θ和导航坐标系下在x轴、y轴、z轴上的地磁分量N_x、N_y、N_z得到解算出的滚转角γ′包括：

滚转角姿态解算模块基于弹体坐标系下在x轴、y轴、z轴上的磁场数字分量Bx、By、Bz和导航坐标系下在x轴、y轴、z轴上的地磁分量N_x、N_y、N_z得到姿态转换矩阵

滚转角姿态解算模块基于姿态转换矩阵

航向角φ和俯仰角θ得到解算出的滚转角γ′。

6.根据权利要求5所述的方法，其特征在于，通过下式得到姿态转换矩阵

通过下式得到解算出的滚转角γ′：

7.根据权利要求1所述的方法，其特征在于，基于过零点法获取弹体的转速M包括：

获取三轴模拟地磁信息中的弹体径向的地磁模拟信号

获取弹体径向的地磁模拟信号

中两个相邻的过零点时刻；

基于两个相邻的过零点时刻获取弹体的转速M。

8.根据权利要求7所述的方法，其特征在于，通过下式获取弹体的转速M：

式中，T_M0和T_M1为两个相邻的过零点时刻。

9.根据权利要求1所述的方法，其特征在于，通过下式获取实时的滚转角γ：

式中，N_i为第i+1个姿态解算周期的采样点数，m≥2，且为整数。

10.一种基于地磁信息辅助GPS的高旋转载体的测姿装置，其特征在于，所述装置包括采样与计数电路、GPS接收机、姿态计算模块、插值递推模块、世界地磁模型、滚转角姿态解算模块、转速计算模块、滚转角姿态推算模块和地磁传感器；

采样与计数电路用于采集地磁传感器实时输出的三轴模拟地磁信息并对三轴模拟地磁信息的采样点数进行累加，且将当前采集时刻作为姿态解算周期的起始时标T_n；

采样与计数电路还用于将三轴模拟地磁信息转换为弹体坐标系下在x轴、y轴、z轴上的磁场数字分量Bx、By、Bz；

GPS接收机用于实时输出弹体的东向、北向与天向速度V_E、V_N、V_U和对应的时间信息；

姿态计算模块用于基于接收的弹体的东向、北向与天向速度V_E、V_N、V_U得到弹体的航向角φ和俯仰角θ；

插值递推模块用于将接收的GPS接收机输出的时间信息中距姿态解算周期的起始时标T_n最近的时刻作为GPS定位时标

插值递推模块还用于获取GPS接收机在定位时标

时弹体在大地直角坐标系下的位置信息和速度信息；

插值递推模块还用于基于弹体在大地直角坐标系下的位置信息和速度信息获取弹体坐标系下在姿态解算周期的起始时标T_n时的经度L、纬度B和高度H；

世界地磁模型用于基于接收的姿态解算周期的起始时标T_n和弹体坐标系下在姿态解算周期的起始时标T_n时的经度L、纬度B和高度H获取导航坐标系下在x轴、y轴、z轴上的地磁分量N_x、N_y、N_z；

滚转角姿态解算模块用于基于接收的弹体坐标系下在x轴、y轴、z轴上的磁场数字分量Bx、By、Bz、航向角φ、俯仰角θ和导航坐标系下在x轴、y轴、z轴上的地磁分量N_x、N_y、N_z得到解算出的滚转角γ′，同时将当前解算完成时刻作为姿态解算周期的结束时标T_n+1；

转速计算模块用于基于过零点法获取弹体的转速M；

采样与计数电路还用于获取多个姿态解算周期中每一个姿态解算周期的采样点数；

滚转角姿态推算模块用于基于接收的解算出的滚转角γ′、弹体的转速M和多个姿态解算周期中每一个姿态解算周期的采样点数获取实时的滚转角γ；

其中，多个姿态解算周期内转速相同。

Images