CN110017809A - 利用地磁信息和光流传感器解算飞行器姿态的方法 - Google Patents

Abstract

本发明提供了一种利用地磁信息、GPS接收机和光流传感器解算飞行器姿态的方法，该方法通过采用自适应最小二乘滤波法，通过以磁阻传感器、GPS接收机和光流传感器的测量误差为权重，对测量噪声协方差矩阵进行调整，所述测量噪声协方差矩阵根据飞行器纵轴与地磁矢量的夹角变化确定,以实现对飞行器滚转角的最优估计。该方法通过地磁信息、GPS接收机和光流传感器配合使用，可以消除飞行器测量盲区的影响。

Description

利用地磁信息和光流传感器解算飞行器姿态的方法

技术领域

本发明涉及一种利用地磁信息、GPS接收机和光流传感器解算飞行体姿态的方法，特别涉及一种利用地磁信息、GPS接收机和光流传感器解算飞行体滚转角的空间盲区及固有偏差的方法。

背景技术

随着微电子技术的发展，磁传感器已广泛应用于磁场的检测与测量中。其中，磁阻传感器由于其尺寸小、灵敏度高、线性度优良、迟滞误差和零点温度漂移可矫正等特点，常应用于飞行体特别是小型无人机滚转姿态角解算系统中。由于无人机的飞行速度高、运动自由大，姿态角尤其是滚转角的解算精度直接影响到了无人机的飞行稳定性和飞行轨迹的准确性。在实际应用中发现，当无人机机体轴向靠近磁北向附近时，滚转姿态解算会出现较大误差，无法满足无人机多自由度全向飞行的要求。

该类误差主要是由于三轴磁阻传感器的其中一个敏感轴与无人机的机体轴固连，另外两轴与机体轴正交。当无人机的机体轴(x轴)靠近磁北向时，与机体轴垂直截面(Oyz面)上地磁场分量较小，导致y轴和z轴上磁阻传感器的测量值过于微弱，输出信号信噪比太低，从而导致滚转姿态解算存在误差。本领域人员普遍认为，可以通过提高传感器的测量精度和提升传感器输出信号的信噪比来减小甚至消除此误差。

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

本发明人发现，仅使用地磁传感器测量飞行体，特别是低速低空四旋翼无人机的滚转角时，会出现测量盲区

为了解决现有技术中的上述问题，本发明人提供了一种利用地磁信息、GPS接收机和光流传感器解算飞行体姿态的方法，滚转姿态角更精确的解算，同时避免仅使用地磁传感器测量滚转角带来的盲区，实现无人机多自由度全向飞行。

发明内容

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

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

第一方面，本发明提供一种利用地磁信息和光流传感器解算飞行器姿态的方法，所述方法中还包括GPS接收机，通过以磁阻传感器、GPS接收机和光流传感器的测量误差为权重，对测量噪声协方差矩阵进行调整。

该方法通过采用自适应最小二乘滤波法，所述测量噪声协方差矩阵根据飞行器纵轴与地磁矢量的夹角变化确定。

其中，所述GPS接收机和光流传感器每一时刻的测量值解算出的滚转角γ_A由以下式Ⅱ给出：

其中，t为测量时的时间；以飞行器特别是无人机起飞时刻为初始时间t₀；

ω_x为飞行器特别是无人机的滚转角速度；

进一步地，

其中，飞行器的飞行高度为h；光流传感器与GPS接收机测速差值在垂直飞行器纵轴特别是垂直无人机纵轴方向分量为ΔV。

利用GPS接收机和光流传感器每一时刻的测量值解算出的滚转角γ_A进行飞行器滚转角的最优解算如式Ⅰ所示方程式：

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

σ_A ²为GPS接收机和光流传感器冗余测量值的方差；

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

γ_A为根据GPS接收机和光流传感器每一时刻的测量值解算出的滚转角。

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

γ表示飞行器的真实滚转角；

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

其中，根据GPS接收机和光流传感器测量值解算出的滚转角γ_A＝γ+δγ_A；

γ表示无人机的真实滚转角；

δγ_A为解算误差，误差δγ_A～N(0,σ_A ²)，σ_A ²根据选用的GPS接收机和光流传感器的性能参数确定。

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

当飞行器纵轴与当地地磁矢量夹角小于W时，采用式Ⅰ所示的利用磁阻传感器的测量值解算出的滚转角γ_M和GPS接收机与光流传感器测量值解算出的滚转角γ_A进行加权平均获得飞行器滚转角的最优解算值

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

根据本发明提供的利用地磁信息和GPS接收机和光流传感器解算飞行器姿态的方法，具有以下有益效果：

(1)本发明通过地磁信息、GPS接收机和光流传感器配合使用来消除测量盲区的影响；

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

(3)本发明提供的方法无人机适用于低速低空四旋翼无人机，可以达到滚转姿态角更精确的解算，同时避免仅使用地磁传感器测量滚转角带来的盲区。

附图说明

图1示出地面起飞坐标系与北天东坐标系的关系图；

图2示出准机体坐标系与地面起飞坐标系的关系图；

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

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

图5示出地磁总量M与地面起飞坐标系Oxyz及北天东坐标系Ox_Ny_Uz_E的关系图；

图6-1示出实施例1中无人机起飞方向与磁北夹角0°～50°时磁场在无人机机体截面上投影占总磁场强度百分比的变化图；

图6-2示出实施例1中无人机起飞方向与磁北夹角90°～180°时磁场在无人机机体截面上投影占总磁场强度百分比的变化图；

图6-3示出实施例1中无人机起飞方向与磁北夹角2°～6°时滚转角解算误差对俯仰角误差的敏感程度；

图6-4示出实施例1中无人机俯仰角0°～-30°时滚转角解算误差对偏航角误差的敏感程度；

图6-5示出实施例1利用三轴地磁传感器和三轴地磁传感器+GPS接收机+光流传感器解算滚转角的解算结果与真实值的对比图。

具体实施方式

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

以下详述本发明。

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

本发明因此提供了一种利用地磁信息、GPS接收机和光流传感器解算飞行器姿态的方法，所述飞行器是指由人类制造、能飞离地面、在空间飞行并由人来控制的在大气层内或大气层外空间(太空)飞行的器械飞行物。所述飞行器包括航空器、航天器、火箭、无人机和制导武器，优选为无人机，更优选为滚转稳定的无人机。

现有技术中常常利用飞行器上安装的磁阻传感器获得机体姿态的测量数据，该数据是基于北天东坐标系Ox_Ny_Uz_E、地面起飞坐标系Oxyz、准机体坐标系Ox₄y₄z₄和机体坐标系Ox₁y₁z₁，以及坐标系间的转换得到的。

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

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

地面起飞坐标系与北天东坐标系的关系如图1所示，图示方位角为正。

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

所述机体坐标系的原点O与无人机质心重合，Ox₁轴与机体纵轴重合，指向头部为正；Oy₁轴位于飞行器纵向对称面内与Ox₁轴垂直；Oz₁轴与其他两轴重合构成右手坐标系。

准机体坐标系与地面起飞坐标系的两个欧拉角记为俯仰角θ和偏航角ψ，两坐标系关系如图2所示，图示俯仰角和偏航角为正。

地面起飞坐标系到准机体坐标系的坐标转换关系为：

其中，

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

其中，

另外，根据国际惯例，使用北东地坐标系对地磁场进行描述，如图4所示。北东地坐标系是与地球表面固连的坐标系，原点O取在发射点，Ox_N位于水平面内指向地理北极；OZ_D垂直水平面朝地；Oy_E与其他两轴垂直并构成右手坐标系。其中，M为磁场强度(地磁总量)，I为磁倾角，D为磁偏角，图示方向磁倾角和磁偏角为正值。

另外，根据美国海洋和大气管理局(NOAA)2015年公布的2015-2020世界地磁场模型，可以计算出世界各地地磁场强度及磁倾角和磁偏角的数据，具体数据可在NOAA官网(www.ngdc.noaa.gov)查询。

根据前述内容，地磁总量M与地面起飞坐标系Oxyz及北天东坐标系Ox_Ny_Uz_E的关系如图5所示，地磁场总量M在地面起飞坐标系Oxyz的投影分量M_x、M_y和M_z为：

地磁场总量M在机体坐标系下的投影分量M_x1、M_y1和M_z1为

M_x、M_y和M_z的值可以通过世界地磁场模型获得当地的地磁信息，而M_x1、M_y1和M_z1的值则可以通过捷联在飞行器上的磁阻传感器(地磁传感器)，特别是三轴地磁传感器来获得。

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

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

其中，

式(6)可展开为

由式(7)可得

其中，

联立式(8)(9)可得

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

γ＝γ₁+γ^* (12)

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

飞行器发射后，当飞行器纵轴向与地磁矢量方向夹角较小时，垂直于飞行器纵轴的截面(垂直于机体轴的截面)上地磁分量很小，当该分量远小于磁阻传感器特别是三轴地磁传感器自身噪声时，便无法准确解算出飞行器的滚转角。这是由于现有的磁阻传感器仅考虑了磁场突变、磁阻传感器噪声、GPS接收机和光流传感器的测量噪声，并未考虑偏航角及俯仰角的解算误差对飞行器滚转角解算的影响程度。

进一步地，当飞行器俯仰角θ＜-I且飞行器射向与磁北夹角小于W，即飞行器纵轴向在运动过程中与地磁场矢量方向夹角小于W时，无法利用地磁信息对飞行器的滚转角进行解算。所述W为8～15°，更优选为9～13°，例如10°。此时，利用地磁信息解算飞行器的滚转角时存在测量盲区，为了实现利用磁阻传感器特别是三轴地磁传感器无盲区测量飞行器的滚转角，本发明向飞行器的测量模块中特别引入了GPS接收机和光流传感器，通过地磁信息、GPS接收机和光流传感器配合使用来消除测量盲区的影响。

结合冗余设计的思想，本发明特别提出了一种利用地磁信息、GPS接收机和光流传感器解算飞行器姿态的方法，所述方法通过采用自适应最小二乘滤波法，根据飞行器纵轴与地磁矢量的夹角变化确定测量噪声协方差矩阵，以实现对飞行器滚转角的最优估计。特别是，当飞行器在低空飞行时，可以利用GPS接收机和光流传感器测速的差值来解算飞行器的滚转角速度，进而积分求得无人机的滚转角。

所述飞行器优选为无人机，更优选为低空低速飞行的无人机，最好为低空低速四旋翼无人机。其中，低空飞行是指飞行高度不高于5米，且为在陆地上飞行；低速指飞行速度在0.2马赫以内)。

优选地，在所述方法中以磁阻传感器、GPS接收机和光流传感器的测量误差为权重，对测量噪声协方差矩阵进行调整。

在一种优选的实施方式中，所述利用地磁信息、GPS接收机和光流传感器解算飞行器姿态的方法包括使用式Ⅰ所示方程式进行解算，特别是飞行器滚转角的最优解算值为：

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

σ_A ²为GPS接收机和光流传感器冗余测量值的方差；或者，为GPS接收机和光流传感器解算滚转角误差的方差

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

γ_A为根据GPS接收机和光流传感器每一时刻的测量值解算出的滚转角。

GPS接收机应安装朝天向，无遮挡物，以便接收GPS信号；光流传感器的敏感轴应指向大地，此时，GPS接收机和光流传感器每一时刻测量值解算出的滚转角可由以下式Ⅱ给出：

其中，t为测量时的时间；以飞行器特别是无人机起飞时刻为初始时间t₀；飞行器特别是无人机的滚转角速度为ω_x。

进一步地，

其中，飞行器的飞行高度为h；光流传感器与GPS接收机测速差值在垂直飞行器纵轴特别是垂直无人机纵轴方向分量为ΔV。

根据GPS接收机和光流传感器的飞行器滚转角测量值解算出的滚转角γ_A符合正态分布，根据正态分布的规律得到γ_A的计算公式γ_A＝γ+δγ_A，γ表示无人机的真实滚转角，δγ_A为解算误差，误差δγ_A～N(0,σ_A ²)。

其中σ_A ²由GPS接收机和光流传感器的性能参数决定，一般为已知量，根据选用的GPS接收机和光流传感器确定σ_A ²的值。

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

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

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

在一种更优选的实施方式中，为避免GPS接收机和光流传感器角速度积分造成的误差，当飞行器纵轴与当地地磁矢量夹角大于W时，仅采用磁阻传感器的测量值来解算滚转角的最优解算值

当飞行器纵轴与当地地磁矢量夹角小于W时，则采用式Ⅰ所示的利用磁阻传感器的测量值解算出的滚转角γ_M和GPS接收机与光流传感器测量值解算出的滚转角γ_A进行加权平均获得飞行器滚转角的最优解算值

本发明还提供了一种飞行器姿态控制系统，所述系统采用上述利用地磁信息和GPS接收机与光流传感器解算飞行器姿态的方法对飞行器的姿态进行控制。

实施例

实施例1

昆明地区的地磁信息为，磁场强度M＝47706.0nT，磁倾角I＝38°51'0"，磁偏角D＝﹣1°31'54"。为方便起见，四旋翼无人机纵轴方向在水平面内投影与磁北向夹角为ψ，取磁北偏东向为正。

其中，符号区分如下：

ψ—无人机轴向在水平面内投影与磁北向夹角；

ψ—无人机轴向在水平面内投影与发射方向夹角(航偏角)。

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

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

通过仿真得出了在不同发射方向和不同俯仰角下，俯仰角的误差对无人机滚转角解算精度的影响，如图6-3所示。可知，当无人机纵轴向与磁北向夹角较小时，滚转角解算精度对俯仰角和偏航角的误差最为敏感。尤其当无人机纵轴与磁北夹角在10°以内时，由图6-3可得，俯仰角或偏航角误差每增加1°误差，滚转角解算误差约35°以上。

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

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

无人机上装载三轴地磁传感器、GPS接收机和光流传感器，按照下式计算飞行过程中无人机滚转角的最优估算值：

其中，σ_M ²为三轴地磁传感器解算出滚转角误差的方差；σ_A ²为GPS接收机和光流传感器解算滚转角误差的方差；γ_M为三轴地磁传感器解算出的滚转角；γ_A为GPS接收机和光流传感器解算出的滚转角。

ψ为5°时，分别利用三轴地磁传感器和三轴地磁传感器+GPS接收机+光流传感器解算滚转角，解算结果与真实值的对比如图6-5所示。

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

可见，利用三轴地磁传感器+GPS接收机+光流传感器解算滚转角的解算结果更为准确。

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

Claims (9)

1.一种利用地磁信息和光流传感器解算飞行器姿态的方法，其特征在于，

所述方法中还使用GPS接收机，通过以磁阻传感器、GPS接收机和光流传感器的测量误差为权重，对测量噪声协方差矩阵进行调整。

2.根据权利要求1所述的方法，其特征在于，所述测量噪声协方差矩阵根据飞行器纵轴与地磁矢量的夹角变化确定。

3.根据权利要求2所述的方法，其特征在于，根据GPS接收机和光流传感器每一时刻的测量值解算出的滚转角γ_A由以下式Ⅱ给出：

其中，t为测量时的时间；以飞行器特别是无人机起飞瞬间的时间为初始时间t₀；

ω_x为飞行器特别是无人机的滚转角速度；

进一步地，

其中，飞行器的飞行高度为h；光流传感器与GPS接收机测速差值在垂直飞行器纵轴特别是垂直无人机纵轴方向分量为ΔV。

4.根据权利要求3所述的方法，其特征在于，利用GPS接收机和光流传感器每一时刻的测量值解算出的滚转角γ_A进行飞行器滚转角的最优解算如式Ⅰ所示方程式：

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

σ_A ²为GPS接收机和光流传感器冗余测量值的方差；

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

γ_A为根据GPS接收机和光流传感器每一时刻的测量值解算出的滚转角。

5.根据权利要求3或4所述的方法，其特征在于，根据磁阻传感器的测量值解算出的滚转角γ_M＝γ+δγ_M；

γ表示无人机的真实滚转角；

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

6.根据权利要求3或4所述的方法，其特征在于，根据GPS接收机和光流传感器测量值解算出的滚转角γ_A＝γ+δγ_A；

γ表示无人机的真实滚转角；

δγ_A为解算误差，误差δγ_A～N(0,σ_A ²)，σ_A ²根据选用的GPS接收机和光流传感器的性能参数确定。

7.根据权利要求3或4所述的方法，其特征在于，当飞行器纵轴与当地地磁矢量夹角大于W时，仅采用磁阻传感器的测量值来解算滚转角的最优解算值

8.根据权利要求3或4所述的方法，其特征在于，当飞行器纵轴与当地地磁矢量夹角小于W时，采用式Ⅰ所示的利用磁阻传感器的测量值解算出的滚转角γ_M和GPS接收机与光流传感器测量值解算出的滚转角γ_A进行加权平均获得飞行器滚转角的最优解算值

9.根据权利要求7或8所述的方法，其特征在于，所述W为8～15°，更优选为9～13°。