利用地磁信息和角速率陀螺解算飞行器姿态的方法与流程

本发明涉及一种利用地磁信息和角速率陀螺解算飞行体姿态的方法，特别涉及一种利用地磁信息和角速率陀螺解算飞行体滚转角的空间盲区及固有偏差的方法。

背景技术：

磁阻传感器由于其尺寸小、成本低、可抗干扰和抗高过载等特性，被广泛应用于飞行体特别是导弹滚转姿态测量系统中。但当导弹轴向方向靠近磁北向附近时，滚转姿态解算会出现较大误差。

产生这些误差的原因，普遍认为是由于当导弹的弹体轴(x轴)靠近磁北向时，与弹体轴垂直截面(Oyz面)上地磁场分量较小，导致y轴和z轴上磁阻传感器的测量值过于微弱，输出信号信噪比太低，从而导致滚转姿态解算存在误差。本领域人员普遍认为，可以通过提高传感器的测量精度和提升传感器输出信号的信噪比来减小甚至消除此误差。

然而，本发明人通过基于磁阻传感器的弹体姿态测量技术，理论推导出了依靠磁阻传感器信息解算导弹滚转角的算法，发现了滚转角误差的主要来源，不仅与磁阻传感器的测量误差有关，还与导弹的俯仰角和偏航角的测量误差有关。即使磁阻传感器的测量精度达到理想的状态，当弹体轴与磁北向夹角较小时，因俯仰角和偏航角的测量存在误差，也会导致滚转角的解算误差在一定程度上趋于发散。

为了解决现有技术中的上述问题，本发明人提供了一种利用地磁信息和角速率陀螺解算飞行体姿态的方法。

技术实现要素：

为了解决上述问题，本发明人进行了锐意研究，结果发现：通过地磁信息和角速率陀螺配合使用，采用自适应最小二乘滤波法，可以消除测量盲区的影响，从而完成了本发明。

本发明的目的在于提供以下方面：

第一方面，本发明提供一种利用地磁信息和角速率陀螺解算飞行器姿态的方法，该方法通过采用自适应最小二乘滤波法，根据飞行器纵轴与地磁矢量的夹角变化确定测量噪声协方差矩阵，以实现对飞行器滚转角的最优解算。

所述方法中以磁阻传感器和角速率陀螺的测量误差为权重，对测量噪声协方差矩阵进行调整。

所述方法包括使用式Ⅰ所示方程式进行飞行器滚转角的最优解算：

式Ⅰ

σM²为磁阻传感器解算值的方差；

σA²为角速率陀螺冗余测量值的方差；

γM为根据磁阻传感器的测量值解算出的滚转角；

γA为根据角速率陀螺每一时刻的测量值解算出的滚装角。

其中，所述角速率陀螺每一时刻的测量值解算出的滚转角γA由以下式Ⅱ给出：

式Ⅱ

其中，t为测量时的时间；以飞行器发射瞬间的时间为初始时间t0；

ωx为飞行器的滚转角速度矢量。

其中，根据磁阻传感器的测量值解算出的滚转角γM＝γ+δγM；

γ表示导弹的真实滚转角；

δγM为磁阻传感器测定滚转角的解算误差，误差δγM～N(0,σM²)，其中σM²由飞行器的俯仰和偏航误差角决定。

其中，根据角速率陀螺测量值解算出的滚转角γA＝γ+δγA；

γ表示导弹的真实滚转角；

δγA为解算误差，误差δγA～N(0,σA²)，σA²根据选用的角速率陀螺的性能参数确定。

当飞行器纵轴与当地地磁矢量夹角大于W时，仅采用磁阻传感器的测量值来解算滚转角的最优解算值

当飞行器纵轴与当地地磁矢量夹角小于W时，采用式Ⅰ所示的利用磁阻传感器的测量值解算出的滚转角γM和陀螺角测量值解算出的滚转角γA进行加权平均获得飞行器滚转角的最优解算值

所述W为8～15°，更优选为9～13°。

第二方面，本发明提供了一种飞行器姿态控制系统，所述系统采用上述方法对飞行器的姿态进行控制。

根据本发明提供的利用地磁信息和角速率陀螺解算飞行器姿态的方法，具有以下有益效果：

(1)本发明通过地磁信息和角速率陀螺配合使用来消除测量盲区的影响；

(2)本发明提供的利用地磁信息和角速率陀螺解算飞行器姿态的方法通过采用自适应最小二乘滤波法，根据飞行器纵轴与地磁矢量的夹角变化调整测量噪声协方差矩阵，以实现对飞行器滚转角的最优估计。

附图说明

图1示出发射坐标系与北天东坐标系的关系图；

图2示出准弹体坐标系与发射坐标系的关系图；

图3示出弹体坐标系和准弹体坐标系的关系图；

图4示出北东地坐标系下地磁场的描述图；

图5示出地磁总量M与发射坐标系Oxyz及北天东坐标系OxNyUzE的关系图；

图6-1示出实施例1中导弹发射方向与磁北夹角0°～80°时磁场强度在弹截面上的分量的变化图；

图6-2示出实施例1中导弹发射方向与磁北夹角90°～180°时磁场强度在弹截面上的分量；

图6-3示出实施例1中导弹发射方向与磁北夹角2°～8°时滚转角解算误差对俯仰角误差敏感程度；

图6-4示出实施例1中导弹发射方向与磁北夹角0°～80°时滚转角解算误差对俯仰角误差敏感程度；

图6-5示出实施例1中导弹发射方向与磁北夹角90°～180°时滚转角解算误差对俯仰角误差敏感程度；

图6-6示出实施例1中导弹俯仰角0°～-40°时滚转角解算误差对偏航角误差敏感程度；

图6-7示出实施例1中导弹俯仰角-50°～-90°时滚转角解算误差对偏航角误差敏感程度；

图6-8示出实施例1利用三轴地磁传感器和三轴地磁传感器+角速率陀螺解算滚转角的解算结果与真实值的对比图。

具体实施方式

下面通过对本发明进行详细说明，本发明的特点和优点将随着这些说明而变得更为清楚、明确。

以下详述本发明。

本发明提供了一种利用地磁信息和角速率陀螺解算飞行器姿态的方法，所述飞行器是指由人类制造、能飞离地面、在空间飞行并由人来控制的在大气层内或大气层外空间(太空)飞行的器械飞行物。所述飞行器包括航空器、航天器、火箭、导弹和制导武器，优选为导弹。

飞行器在飞行过程中会产生滚动，获得实时有效的飞行器姿态参数，扩展飞行器可用状态对优化飞行器的控制系统非常重要。

现有技术中常常利用飞行器上安装的磁阻传感器获得弹体姿态的测量数据，该数据是基于北天东坐标系OxNyUzE、发射坐标系Oxyz、准弹体坐标系Ox4y4z4和弹体坐标系Ox1y1z1，以及坐标系间的转换得到的。

其中，所述北天东坐标系是与地球表面固连的坐标系，原点O取在发射点，OxN位于水平面内指向地理北极；OyU垂直水平面朝天；OzE与其他两轴垂直并构成右手坐标系。

所述发射坐标系也是与地球表面固连的坐标系，原点O取在发射点，Ox轴位于水平面内指向飞行器发射方向，与OxN轴夹角记为方位角Oy轴与Ox轴垂直指向上为正；Oz轴与其他两轴成右手坐标系。

所述准弹体坐标系的原点O与飞行器质心重合，Ox4轴与弹体纵轴重合，指向头部为正；Oy4轴位于包含Ox4轴的铅垂平面内指向上为正；Oz4轴与其他两轴重合构成右手坐标系。

所述弹体坐标系的原点O与导弹质心重合，Ox1轴与弹体纵轴重合，指向头部为正；Oy1轴位于飞行器纵向对称面内与Ox1轴垂直；Oz1轴与其他两轴重合构成右手坐标系。

发射坐标系与北天东坐标系的关系如图1所示，图示方位角为正。

准弹体坐标系与发射坐标系的两个欧拉角记为俯仰角和偏航角ψ，两坐标系关系如图2所示，图示俯仰角和偏航角为正。

发射坐标系到准弹体坐标系的坐标转换关系为：

其中，

弹体坐标系和准弹体坐标系的欧拉角记为滚转角γ，两坐标系关系如图3所示，图示滚转角为正。

准弹体坐标系到弹体坐标系的转换关系为

其中，

另外，根据国际惯例，使用北东地坐标系对地磁场进行描述，如图4所示。其中，M为磁场强度，I为磁倾角，D为磁偏角，图示方向磁倾角和磁偏角为正值。

北东地坐标系是与地球表面固连的坐标系，原点O取在发射点，OxN位于水平面内指向地理北极；OZD垂直水平面朝地；OyE与其他两轴垂直并构成右手坐标系。

另外，根据美国海洋和大气管理局(NOAA)2015年公布的2015-2020世界地磁场模型(The US/UK World Magnetic Model for 2015-2020)，可以计算出世界各地地磁场强度及磁倾角和磁偏角的数据。例如，我国部分城市2018年1月5日的地磁场数据如表1-1所示。

表1-1我国部分城市2018年1月5日地磁场数据

根据前述内容，地磁总量M与发射坐标系Oxyz及北天东坐标系OxNyUzE的关系如图5所示，其中，

则地磁场总量M在发射坐标系Oxyz的投影分量Mx、My和Mz为：

地磁场总量M在弹体坐标系下的投影分量Mx1、My1和Mz1为

Mx、My和Mz的值可以通过世界地磁场模型获得当地的地磁信息，而My1和Mz1的值则可以通过捷联在飞行器上的磁阻传感器(地磁传感器)，特别是三轴地磁传感器来获得。

为解算滚转角γ，式(4)可化为

将式(3)代入式(5)，展开并化简可得。

其中，

式(6)可展开为

由式(7)可得

其中，

联立式(8)(9)可得

由以上分析可以得出，飞行器的滚转角的解算则由两项组成。其中，第一项记为γ1，其由y1轴和z1轴地磁信息所决定，称为测量项；第二项记为γ*，其由飞行过程中磁倾角、磁偏角、发射方位角、偏航角和俯仰角共同决定，为补偿项。则式(11)可表示为

γ＝γ1+γ* (12)

在飞行器发射时，磁倾角、磁偏角、发射方位角作为初始值装定进飞行器内，而偏航角可近似为0，结合磁阻传感器特别是三轴地磁传感器测得的地磁信息，则可解算出飞行器的滚转角和俯仰角。

飞行器发射后，当飞行器纵轴向与地磁矢量方向夹角较小时，垂直于飞行器纵轴的截面(弹截面)上地磁分量很小，当该分量远小于磁阻传感器特别是三轴地磁传感器自身噪声时，便无法准确解算出飞行器的滚转角。

这是由于现有的磁阻传感器仅考虑了磁场突变、磁阻传感器噪声和角速率陀螺的测量噪声，并未考虑偏航角及俯仰角的解算误差对飞行器滚转角解算的影响程度。

进一步地，当飞行器俯仰角且飞行器射向与磁北夹角小于W，即飞行器纵轴向在运动过程中与地磁场矢量方向夹角小于W时，无法利用地磁信息对飞行器的滚转角进行解算。

优选所述W为8～15°，更优选为9～13°，例如10°。

此时，利用地磁信息解算飞行器的滚转角时存在测量盲区，为了实现利用磁阻传感器特别是三轴地磁传感器无盲区测量飞行器的滚转角，本发明向飞行器的测量模块中特别引入了角速率陀螺，通过地磁信息和角速率陀螺配合使用来消除测量盲区的影响。

结合冗余设计的思想，本发明特别提出了一种利用地磁信息和角速率陀螺解算飞行器姿态的方法，所述方法通过采用自适应最小二乘滤波法，根据飞行器纵轴与地磁矢量的夹角变化确定测量噪声协方差矩阵，以实现对飞行器滚转角的最优估计。

优选地，所述方法中以磁阻传感器和角速率陀螺的测量误差为权重，对测量噪声协方差矩阵进行调整。

在一种优选的实施方式中，所述利用地磁信息和角速率陀螺解算飞行器姿态的方法包括使用式Ⅰ所示方程式进行解算，特别是飞行器滚转角的最优解算值为：

式Ⅰ

其中，σM²为磁阻传感器解算值的方差；或者，为磁阻传感器解算出滚转角误差的方差；

σA²为角速率陀螺冗余测量值的方差；或者，为角速率陀螺解算滚转角误差的方差；

γM为根据磁阻传感器的测量值解算出的滚转角；

γA为根据角速率陀螺每一时刻的测量值解算出的滚转角。

最好将角速率陀螺的敏感轴与飞行器的纵轴重合安装，此时，角速率陀螺每一时刻测量值解算出的滚转角可由以下式Ⅱ给出：

式Ⅱ

其中，t为测量时的时间；以飞行器发射瞬间的时间为初始时间t0；飞行器的滚转角速度矢量为ωx。

进一步地，根据角速率陀螺的飞行器滚转角测量值解算出的滚转角γA符合正态分布，根据正态分布的规律得到γA的计算公式γA＝γ+δγA，γ表示导弹的真实滚转角，δγA为解算误差，误差δγA～N(0,σA²)。其中σA²由角速率陀螺的性能参数决定，一般为已知量，根据选用的角速率陀螺确定σA²的值。

根据磁阻传感器的测量值解算出的滚转角γM符合正态分布，根据正态分布的规律得到γM的计算公式γM＝γ+δγM，其中，γ表示导弹的真实滚转角，δγM为磁阻传感器测定滚转角的解算误差，误差δγM～N(0,σM²)，其中σM²由飞行器的俯仰和偏航误差角决定。

在飞行过程中，俯仰和偏航误差角的范围为已知量。滚转角解算误差对偏航角和俯仰角误差的敏感程度数据表可以制作成数据库，在飞行器发射前就装定入飞行器载计算机中，这样，就可以根据俯仰和偏航的角误差实时获得σM²的值。

进一步地，当飞行器纵轴与当地磁场夹角较大时，σM²较小，γM占主要作用；当飞行器纵轴与当地磁场夹角较小时，σM²较大，γA占主要作用。通过这种方式，可避免因地磁解算滚转角所带来的空间盲区。

在一种更优选的实施方式中，为避免陀螺角速度积分造成的误差，当飞行器纵轴与当地地磁矢量夹角大于W时，仅采用磁阻传感器的测量值来解算滚转角的最优解算值当飞行器纵轴与当地地磁矢量夹角小于W时，则采用式Ⅰ所示的利用磁阻传感器的测量值解算出的滚转角γM和陀螺角测量值解算出的滚转角γA进行加权平均获得飞行器滚转角的最优解算值

本发明还提供了一种飞行器姿态控制系统，所述系统采用上述利用地磁信息和角速率陀螺解算飞行器姿态的方法对飞行器的姿态进行控制。

实施例

实施例1

表1-1中华阴地区的地磁信息为，磁场强度M＝52585.6nT，磁倾角I＝53°14′33″，磁偏角D＝﹣3°58′33″。为方便起见，记导弹发射方向与磁北向夹角为ψ，取磁北偏东向为正。

通过Matlab软件进行的仿真计算，得出了在不同发射方向和不同俯仰角下，磁场强度在弹截面上的分量，如图6-1和图6-2所示。

由图6-1和图6-2可得，当弹轴向与磁北向夹角越小时，或随着弹的俯仰运动，当弹轴向与磁感线方向夹角越小时，弹截面上磁场强度分量越小，反之亦然.

通过仿真得出了在不同发射方向和不同俯仰角下，俯仰角的误差对导弹滚转角解算精度的影响，如图6-3～6-5所示。

由图6-3～6-6可得，当弹轴向与磁北向夹角较小时，滚转角解算精度对俯仰角和偏航角的误差最为敏感。尤其当弹轴与磁北夹角在10°以内时，由图6-3可得，俯仰角或偏航角误差每增加1°误差，滚转角解算误差约50°。

通过仿真计算得出了在不同发射方向和不同俯仰角下，偏航角的误差对导弹滚转角解算精度的影响，如图6-6和图6-7所示，可知，随着弹轴向与磁北向夹角增大，滚转角解算误差对随俯仰角和偏航角误差敏感程度逐渐降低，当夹角180°时解算误差接近0。

将上述数据制作为数据库，装入弹载计算机中。

导弹上装载三轴地磁传感器和MEMS角速率陀螺，按照下式计算飞行过程中导弹滚转角的最优估算值：

其中，σM²为三轴地磁传感器解算出滚转角误差的方差；σA²为角速率陀螺解算滚转角误差的方差；γM为三轴地磁传感器解算出的滚转角；γA为角速率陀螺解算出的滚转角。

ψ为5°时，分别利用三轴地磁传感器和三轴地磁传感器+角速率陀螺解算滚转角，解算结果与真实值的对比如图6-8所示。

由图可知，利用三轴地磁传感器+角速率陀螺解算滚转角时解算出的结果与真实值的结果更为接近，偏差范围更小；而只利用三轴地磁传感器解算滚转角时解算出的结果与真实值偏差范围更大。

可见，利用三轴地磁传感器+角速率陀螺解算滚转角的解算结果更为准确。

以上结合具体实施方式和范例性实例对本发明进行了详细说明，不过这些说明并不能理解为对本发明的限制。本领域技术人员理解，在不偏离本发明精神和范围的情况下，可以对本发明技术方案及其实施方式进行多种等价替换、修饰或改进，这些均落入本发明的范围内。本发明的保护范围以所附权利要求为准。