CN109238262A - 一种航向姿态解算及罗盘校准抗干扰方法 - Google Patents

Abstract

本发明公开一种罗盘校准抗干扰方法有采用该方法的航向姿态解算方法，所述抗干扰方法首先进行三轴磁罗盘初始校准，确认当前测试环境适合建立基准矢量；然后利用当前磁罗盘所测出的载体系的三轴磁场强度建立基准矢量；最后进行基于基准矢量的滤波，保证整体滤波过程中均方误差最小，求得三轴磁场偏置，以便将该偏置做为干扰进行排除。由于进行了特定的初始化并获取当前磁场的基准矢量，使用该磁场基准矢量进行滤波解算航向姿态，能消除设备本身的硬磁干扰及周围环境的常值偏置磁场干扰及短期出现的干扰，在周围环境没有长时间出现的干扰磁场的情况下，能获得准确的航向姿态。

Description

一种航向姿态解算及罗盘校准抗干扰方法

技术领域

本发明涉及一种罗盘校准抗干扰方法及采用该方法的航向姿态解算方法，适用于对无人机等运动载体(载体指的是当前传感器所在的空间实体，可以是飞机、舰船等)，以获得长时间稳定的航向姿态。

背景技术

现有的航向姿态解算方案使用的是经过球面校准后的罗盘数据融合陀螺仪进行姿态解算，对于影响罗盘数据的各项因子并没有做深入的解析，而且在受到干扰时只对干扰数据做了简单的剔除或者滤波处理。这样的方案在无外界软磁和硬磁干扰的情况下可以得到比较精确的姿态解算结果。然而由于现实应用存在多样的情况，比如罗盘安装的位置、电流对磁场的影响、外界硬磁的干扰等，使得罗盘在航向姿态解算应用中存在多种未可知的情况，而这势必会对正常应用带来隐患。

例如，某无人机磁场矢量校正方法及系统，对磁罗盘的数据构成做了简单的建模，并将主要的重心放在了传感器的校准上，在初始校准的时候如果周围无明显的磁场干扰，得到的校准数据是可以正常使用的。校准后的罗盘数据同陀螺仪进行简单的互补滤波，并最终得到航向姿态。而这样的做法显然未将设备(即载体)本身的硬磁因素考虑在内，使得校准数据极易被初始磁场环境所影响。

发明内容

本发明的目的是提出一种罗盘校准抗干扰方法及采用该方法的航向姿态解算方法，以滤除磁场干扰，以获得运动载体准确的航向姿态。

为此，本发明提出的罗盘校准抗干扰方法包括如下步骤：S1、进行三轴磁罗盘初始校准，确认当前测试环境适合建立基准矢量；S2、利用当前磁罗盘所测出的载体系的三轴磁场强度建立基准矢量；S3、进行基于基准矢量的滤波，保证整体滤波过程中均方误差最小，求得三轴磁场偏置，以便将该偏置做为干扰进行排除。

在本发明的一些实施例中，还包括如下技术特征：

步骤S1中所述“当前测试环境适合建立基准矢量”是指三轴磁罗盘测量的磁场强度的幅值与地磁的磁场强度的偏差的绝对值小于设定值；所述初始校准包括如下步骤：

S11、在当前测试环境下进行三轴磁罗盘数据测量；S12、计算三轴磁罗盘测量的磁场强度的幅值以及该幅值与地磁的磁场强度的偏差的绝对值；S13、判断：如果此时的所述的偏差的绝对值小于预定的特定值，则初始校准成功；如果所述的偏差的绝对值不小于预定的特定值，则更换到新的测量环境做为当前测试环境重新进行上述步骤S11-S13，直到初始校准成功。

所述步骤S2中，建立基准矢量包括如下步骤：利用当前磁罗盘所测出的载体系的三轴磁场强度，计算在导航坐标系下的三轴对应的磁场参考矢量，即为基准矢量。

所述步骤S3中，所述滤波为基于磁场基准矢量的扩展卡尔曼滤波。

在所述基于磁场基准矢量的扩展卡尔曼滤波中，采用用来做三维空间的旋转变换的四元数和三轴陀螺漂移以及三轴的磁场偏置作为状态矢量，以三轴加速度计测量的加速度和三轴磁罗盘测量的磁场强度作为量测数据构建扩展卡尔曼滤波方程。

使用滑动窗口计算三轴磁场的的方差；当上述方差大于用户预设值时，则认为是高频变化的磁场；对于高频变化的磁场，通过加大步骤S3中滤波时所用的相应量测噪声项的办法来降低滤波器对当前磁场的融合比例；否则，则认为是低频变化的磁场，对于低频变化的磁场，将附加在磁场矢量上的低频磁场作为步骤S3中所说的偏置进行处理。

所述卡尔曼滤波中包括有量测噪声序列方差矩阵；并且所述抗干扰方法还包括如下步骤：使用滑动窗口计算三轴磁场的的方差；当上述方差大于用户预设值时，则认为是高频变化的磁场；对于高频变化的磁场，通过加大相应量测噪声项的办法来降低滤波器对当前磁场的融合比例；具体是将卡尔曼滤波中的量测噪声序列方差矩阵调整为以加大卡尔曼滤波中的量测噪声项，其中S_x，S_y，S_z为三轴磁场的方差数据。

当上述方差不大于用户预设值时，则此时判断三轴磁场的数据幅值是否小于特定值；如果数据幅值小于特定值，则直接进入卡尔曼滤波；如果数据幅值不小于特定值，则先进行常值偏置估计，然后再进行卡尔曼滤波。

使用当前传感器测量出来的磁场强度同经过旋转矩阵转换后的磁场基准矢量相减，以获得当前的三轴磁场偏置。

本发明还提出一种航向姿态解算方法，使用经过校准后的罗盘数据融合陀螺仪进行航向姿态解算，所述校准包括上述的抗干扰方法以过滤磁场干扰。

本发明还提出一种航向姿态解算装置，其特征在于使用上述的航向姿态解算方法。

本发明还提出一种嵌入式设备，包括CPU和存储器，CPU通过接口连接三轴磁罗盘并采集三轴磁罗盘的数据，存储器中存储有计算机程序，所述CPU可执行该计算机程序用于实现上述的抗干扰方法或姿态解算方法。

本发明还提出一种计算机存储介质，其存储有计算机程序，所述计算机程序可被执行用于实现上述的抗干扰方法或姿态解算方法。

本发明的有益效是：由于进行了特定的初始化并获取当前磁场的基准矢量，使用该磁场基准矢量进行滤波解算航向姿态，能消除设备本身的硬磁干扰及周围环境的常值偏置磁场干扰及短期出现的干扰，在周围环境没有长时间出现的干扰磁场的情况下，能获得准确的航向姿态。

由于其计算是基于基准矢量而非地磁矢量，则其计算不受地磁偏角及地磁倾角的影响，因此在无法获得在地磁偏角及地磁倾角的情况下仍能正常进行姿态解算，使得短距离运动的载体(即在载体运动范围不大的情况下)无需搭载地磁偏角及地磁倾角的确定装置。相反，现有技术中即使确定了地磁偏角及地磁倾角，由于对设备本身的软磁和硬磁及周转环境的常值偏置磁场没有考虑，在有这些磁场干扰时它也无法准确地进行姿态解算。

在一些实施例中，使用扩展卡尔曼滤波估计三轴磁罗盘的动态偏置，提高了后续的航向计算精度。

在一些实施例中，还对磁场干扰进行分类，使用了实时的动态磁干扰的设计方法，提高了系统在不同应用环境的适应能力，从而在各种环境下均能获得长时间稳定的航向姿态。

附图说明

图1是本发明实施例中磁场矢量的建立及卡尔曼滤波的流程示意图。

图2是本发明实施例对磁场干扰进行分类的流程示意图。

具体实施方式

本发明以软件方法(通过计算机程序)实现了航向姿态解算过程中的磁场抗干扰。在优选实施例中,软件安装在嵌入式设备的cpu里面，cpu通过IIC和SPI方式获取传感器(三轴磁罗盘)的数据后进行处理。在一些实施例中，也可以直接将控制器及程序安装于传感器(三轴磁罗盘)中，或者将程序安装于上位机中。但由于传感器是个特定的ASIC芯片，只能处理简单的数据处理功能，此方法只在做高附加值的传感器解决方案时才会比较适用。另外，由于在上位机运行需要通过sd卡等离线数据传输方式或者使用无线wifi等在线数据传输方式将数据传输到上位机，经过运算后再将计算之后的系数传输回cpu，但这样必然会导致设备的依赖性变高，在不同的应用场合(野外等)变得复杂。因此，如前已述，本发明下述实施例选择将软件安装在嵌入式设备的cpu里面。

实施例一

如图1所示，是本发明一个实施例的流程示意图。图1所示实施例的抗干扰方法包括如下步骤：

首次使用罗盘之前要先进行初始校准：三轴磁罗盘数据采集(测量)后，进行三轴磁罗盘测量的磁场强度的幅值M_m计算以及磁场强度偏差绝对值B_m的计算，如果此时的B_m小于某一特定值则认为此时载体周围的磁场环境无较大干扰，可以建立此地区范围内的磁场基准矢量。如果超出范围，则会提示用户换个环境重新校准，并给出提示信息。此时判断的是B_m是不是小于用户设定的特定值，此处所谓特定值是一由用户掌握的变量，比如可以设定为罗盘所能测得的最小单位一个或几个刻度。

如果初始化成功(初始校准成功)，则进入基准矢量m_r计算步骤。基准矢量建立后，本实施可直接进入基于基准矢量的卡尔曼滤波。

具体基准矢量建立及卡尔曼滤波的详细步骤说明在下文中阐述。其中磁场幅值按下面公式(1-4)进行计算，磁场强度偏差绝对值B_m按照下面公式(1-5)计算。

可见，其中初始校准对校准环境是有要求的，要尽量在无干扰的情况下，周围磁场强度应远小于真实的地球磁场强度(真实的地球磁场强度B_E的大小为45μT左右)，否则初始校准不能成功，需要换个环境再行校准。

本实施例由于进行了特定的初始化、建立了基准矢量并进行了卡尔曼滤波，其方法不受设备(即载体)本身的硬磁及初始磁场环境的影响，也能滤除短期磁场干扰，适用于周环境没有长期出现的变化磁场干扰的情况。

实施例2

如图2所示，是本发明另一个实施例的流程示意图。

在周环境有长期出现的变化磁场干扰的情况下，本实施例先对干扰磁场进行分类，然后针对不同种类的磁场干扰，进行不同的处理。包括如下步骤：

在建立好基准矢量后，使用滑动窗口计算数据的方差，如果方差大于等于设定的特定值，则说明高频磁场噪声较大，则增大罗盘测量噪声进行卡尔曼滤波计算。

如果方差小于设定的特定值，则说明高频磁场噪声较小，但可能仍有低频磁场干扰，则此时判断三轴磁场的数据幅值(图2中所谓“数据幅值”，可以是当前实测的三轴磁场偏置的幅值，也可以采用滑动窗口的均值)是否小于特定值；如果数据幅值小于特定值，则说明也没有低频磁场干扰，直接进入卡尔曼滤波即可。如果数据幅值不小于特定值，则说明有低频磁场干扰，此时需先进行常值偏置估计，然后再进行卡尔曼滤波。

图2中的方差对应于下面公式(3-1)的S_x S_y S_z，而所谓的“某一特定值”是一个可由用户指定的参数，它是一个可由用户掌握的变量，图2中“特定值”可掌握在0值附近，比如可相当于罗盘灵敏度的一个最小刻度值。

针对使用环境中出现的磁场干扰，现有技术的处理方式只是简单的对数据进行过滤或者降低权重，这对于短时出现的干扰是可以满足应用的，但对于长期出现的磁场干扰(比如在高压电线附近，出现的磁场干扰远超地球的磁场强度；在矿山附近，也会出现磁场干扰)并不能进行有效的处理。而这显然不适用于真实的应用场景。本实施例可以解决这一问题。

以上是对两个实施例的简要的说明。为了更好地理解上述实施例，下面对图1、2所示实施例及其原理做进一步详细说明：

1、磁罗盘磁场建模并计算当前的基准矢量。

由于设备(即载体)本身具有软磁和硬磁干扰，并且周围环境也时常存在干扰，有些干扰是常值偏置磁场，无法通过滤波消除。这样，在存在着这些干扰的情况下，地磁向量无法被设备上的罗盘测准，因此即使通过GPS等手段确定了地磁偏角及地磁倾角，也无法准确地进行姿态解算。为此，本发明实施例提出基准矢量的概念，先对磁罗盘磁场建模并计算当前的基准矢量，然后再进行基于基准矢量的卡尔曼滤波，从而消除设备本身的软磁和硬磁干扰以及周围环境中的常值磁场干扰。

以下对磁罗盘磁场建模和计算当前的基准矢量进行举例说明。

先建立导航坐标系：以下以当前的导航坐标系为N-E-D(北-东-地)坐标系为例进行说明。

当前时刻载体的在载体坐标系的姿态为att＝[φ θ ψ]，其中φ代表滚转角，θ代表俯仰角，ψ代表偏航角。可得由载体坐标系到导航坐标系的转换矩阵为：

式中的b代表的是载体坐标系，n代表的是导航坐标系。此式将载体在载体坐标系中的姿态转换到导航坐标系。

其中，载体坐标系是以机体自身为参考的，通常X轴指载体头部，Y轴则指向载体的右方，Z轴则指向载体下方。

导航坐标系是指用来确定载体所在的空间的一种描述方法，用来对载体进行定位结算和导航，以本专利为例，使用NED坐标系(N：正北方向，E：正东方向：D：垂直于地球表面并指向下)。

载体坐标系和导航坐标系之间通过三轴姿态角(俯仰角、滚转角、偏航角)构成的转换矩阵进行相互转换。如公式(1-1)中的

假设当前载体按照机体向前移动100米，三轴姿态角为[0,0,45],当前载体位置在载体坐标系可表示为[100,0,0],在导航坐标系下则为[70.7,70.7,0]。因为有姿态角的存在，所以需要将载体坐标系中的坐标转换为统一的导航系下的坐标，用来实现定位导航以及制导等功能。

地磁矢量在导航坐标系中的分量为

它代表的是无磁场干扰情况下地磁矢量在导航坐标系中的分量，其中的n代表是导航坐标系，E代表的是大地坐标系，α_nE为地磁偏角，γ_nE为地磁倾角，B_E为地磁的磁场强度。此式将地磁矢量从大地坐标系转换到导航坐标系。

当前罗盘测量到的磁场强度可分解为：

其中代表当前罗盘测量到的三轴磁场强度，代表的是当前载体坐标系三个坐标轴的磁场强度，b代表载体，m代表磁场，b_m代表当前载体的硬磁强度，它是在无外界干扰状态下，载体坐标系下的三轴磁场强度，I₃代表3*3的单位矩阵：

M_m代表当前载体的软磁强度，代表的是当前载体的材料因为外界磁场的变化产生的磁场强度。表示一个3*3由导航坐标系到载体坐标系的转换矩阵矢量：

其中代表的是的转置矩阵；表示扰动磁场,代表的扰动磁场在导航坐标系中的三轴分量，A代表扰动磁场，n代表导航坐标系；表示导航坐标系中的地磁场，代表的是地磁矢量在导航坐标系中的三轴分量，E代表大地，n代表导航坐标系。

从式1-3可以看出，当前罗盘测量到的三轴磁场强度与多个因素有关，包括：当前载体的硬磁强度b_m、当前载体的软磁强度M_m、扰动磁场地磁矢量等等，因此，当前软盘无法直接测量得到地磁矢量因此如果以地磁矢量为基准的话则无法实现准确的姿态解算，还需要用到其他方法来辅助，比如需要考虑的地磁偏角和地磁倾角等。

为此，本发明实施提出了基于基准矢量来进行解算的方法。本实施例的这种方法，把载体的硬磁强度b_m、当前载体的软磁强度M_m、扰动磁场和地磁矢量做为整体考虑，无须区分哪些部分是地磁矢量，哪些部分是其他矢量，因此就省去了传统磁罗盘的导航方法需要考虑的地磁偏角和地磁倾角，因此方法可以适用于地磁偏角和地磁倾角出现变化的情况。

需要说明的是，这是基于载体的活动区域局限于几十千米的工作半径的情况，在这样的工作区域内，基准矢量基本不会受地磁矢量变化的影响，因此可以不考虑地磁偏角和倾角。但当活动区域较大上百千米的时候，这时候需要不断调整基准矢量。其中一种方法是利用载体装备的GPS等设备，查知此时的经纬度，通过查表法计算地磁倾角和偏角，然后通过公式(1-2)计算此时的地磁分量，并重新更新磁场基准向量。

以下以假设当前的载体位于低纬度地区(正负50度，载体的大部分活动范围处于这个范围)、并且不考虑地磁偏角以及地磁倾角的影响为例，来进行说明。

在实施例一初始校准的时候，三轴磁罗盘测量的磁场强度的幅值可以表示为：

其中M_m表示当前三轴的磁场强度幅值，m_x,m_y,m_z为罗盘测量出的三轴的磁场强度。并定义磁场强度偏差绝对值

B_m＝|M_m-B_E| (1-5)

B_E为地磁的磁场强度，大小为45μT左右。如果此时的B_m小于某一特定值则认为此时载体周围的磁场环境无较大干扰，可以建立此地区范围内的磁场基准矢量。如果超出范围，则会提示用户换个环境重新校准，并给出提示信息。此时判断的是B_m是不是小于用户设定的特定值，此处所谓特定值是一由用户掌握的变量，比如可以设定为罗盘所能测得的最小单位一个或几个刻度。

此即为图1中初始化的理论基础。

初始化之后即可建立基准矢量

m_r(即m_ref)代表的是当前活动区域内的基准磁场矢量，基准矢量指的是在区域内导航坐标系下的三轴对应的磁场参考矢量，此基准矢量在当前区域内是恒定的、可测量的。当前磁罗盘能测出上文所说的载体系的三轴磁场强度即而基准矢量即可通过式(1-6)求得。

相反，由于当前磁罗盘只能测出载体系的三轴磁场强度而实际上是由公式(1-3)所表达的各种分量所组成的，地磁矢量只是其中之一，所以地磁矢量实际上是不可被当前罗盘准确测得的；而在无法获取经纬度的条件下，不能获取地磁偏角和地磁倾角，则地磁矢量也无法通过计算得到。就算是获取了地磁偏角和倾角，只能获取理想地磁向量在导航系下的分量，但并不能代表这就是当前环境下的参考矢量，这个时候使用当前环境下测量出来的磁场建立出来的基准矢量会更符合实际情况。

因此，当载体的活动区域小于几百千米时，本实施例使用基准矢量的方法作为区域导航的参考具有巨大的优越性。

下文中会用磁场的基准矢量去求解磁场偏置。使用基准矢量的方法可建立在当前环境下的磁场参考，并可通过算法区分不同情况下的磁场干扰。

另外，如前所述，基准矢量建立后，由于载体运动，比如飞行到不同的纬度后，需要重新建立基准矢量，但只要载体配备GPS，有了GPS的数据，就可以直接使用查表法，重新计算基准矢量，但也可以按上述步骤从头重新建立一遍。

2、基于磁场基准矢量的扩展卡尔曼滤波

选用四元数q＝[q₀ q₁ q₂ q₃]和三轴陀螺漂移b_ω＝[b_ωx b_ωy b_ωz]以及三轴的磁场偏置b_m＝[b_mx b_my b_mz]作为状态矢量(共10维)。

三轴陀螺漂移指的是因为陀螺的工艺导致的测量数值不准确的情况，在本技术的应用中可近似为陀螺的常值零偏。四元数用来做三维空间的旋转变换(四元数是一种数学概念，用来描述现实空间的坐标表现方式，可以将q₀理解为幅值，q₁表示在x轴上的分量，q₂表示在y轴上的分量，q₃表示在z轴上的分量，四个数值表示了空间坐标)，相比于使用欧拉角来计算旋转矩阵，其计算量小，工程实现方便。磁场偏置指的是，因为外界磁场干扰或者磁罗盘工艺问题引起的磁场强度测量不准确的情况四元数可以初始化为[1 0 0 0](四元数初始值设为[1 0 0 0]，则通过四元数和角度的转换关系，便可以得到初始角度为[000].角速度的初始值不用给定，因为它可以从陀螺仪传感器得到。设置了四元数的初始值之后，姿态便可以由此初始值开始计算。)，三轴陀螺漂移初始化为[0 0 0]，三轴磁场偏置初始化为[0 0 0](此三者在卡尔曼滤波过程中会不断获得新值，其值就用来解算航向姿态)，以三轴加速度计测量的加速度a＝[a_x a_y a_z]和三轴磁罗盘测量的磁场强度m＝[m_x m_y m_z]作为量测数据构建扩展卡尔曼滤波方程(即下面2-1这个方程)。其中状态矢量X＝[q b_ω b_m]，量测矢量Z＝[a m]。

于是就有如下方程

对于滤波器来说，其中为系统状态的导数，f(X)为扩展卡尔曼滤波器系统状态的函数，w为系统噪声，Z为系统的量测状态，在当前系统中的量测状态指的就是上文所说的量测矢量Z＝[a m]，h(X)为系统的量测状态函数,v为量测噪声。

其中ω为三轴陀螺仪测量出来的角速度，b_ω为陀螺仪的常值零偏，为四元数的转换矩阵

此公式表示的是载体坐标系到四元数的转换关系，需要它将陀螺仪测量到的角速度转换为四元数[q₀ q₁ q₂ q₃]初始化为[1 0 0 0]，初始化后经卡尔曼的滤波器递推获得(卡尔曼的滤波器递推是通过公式(2-7)实现的)。“把角速度转换为四元数”指的是将角速度通过四元数转换矩阵(公式(2-3))转换成四元数的导数，见公式(2-2)的第一行。

通过上文中的公式计算出基准矢量m_r后，在后期的滤波器环节中就会按照公式(2-4)的关系进行系统的量测状态函数计算：

其中g_n代表地球重力矢量，g_n＝[0 0 g]，g为重力加速度。

对函数f(x)和h(x)求解雅克比矩阵得到F和H

由此便得到了系统线性化后的状态方程

进行离散卡尔曼滤波过程

其中矢量为系统状态矢量的一步预测值，矢量为上一时刻的估计值，矩阵Φ为矩阵F的离散化形式，Φ_k/k-1为一步转移矩阵，多一个上标T代表的是矩阵的转置，矩阵P_k/k-1为状态矢量的一步预测方差矩阵，矩阵P_k-1为状态矢量的上一时刻方差矩阵，矩阵Q_k-1为系统噪声方差阵。矩阵K_k为增益矩阵，矩阵H_k为量测矩阵，矩阵R_k为量测噪声序列方差矩阵，矩阵I为单位矩阵。P_k代表当前时刻估计出的均方误差，代表量测矩阵的转置，K为滤波增益，代表当前时刻的状态估计，Z_k代表当前时刻的量测状态。

(注：公式(2-2)(2-3)中的通过线性化也就是公式(2-5)得到F和H，F通过离散化公式(得到公式(2-7)中的Φ_k/k-1，H对应于公式(2-7)中的H_k。)

从卡尔曼滤波更新的矢量中即可得到三轴的磁场偏置b_m，以及更新的四元数q＝[q₀ q₁ q₂ q₃]和三轴陀螺漂移b_ω＝[b_ωx b_ωy b_ωz]，并使他们在整体滤波过程中均方误差最小。

卡尔曼滤波保证了整体滤波过程中磁场偏置b_m等的均方误差最小。在其他一些实施例中，还有其它简单的的比如互补滤波，但这种不能保证整体均方误差最小。卡尔曼滤波并不是保证均方误差最小的唯一方法，在一些实施例中，还可以采用最小二乘法。本实施例使用卡尔曼滤波是因为它在最小二乘法的基础上可以根据数据质量计算增益，进而影响数据融合效果。

从卡尔曼滤波更新的矢量中即可得到三轴的磁场偏置b_m，这个偏置指的是经过卡尔曼递推之后并且保证整个系统均方误差最小的偏置数据，其将代入到公式(2-4)中。姿态解算使用的是卡尔曼滤波每一步更新之后的系统状态中的四元数进行计算的。而单位四元数可以表示旋转，因此可以用于姿态解算。

3、动态磁场抗干扰算法。

当载体经过类似高压线输电线路或者矿山时，磁罗盘会检测到磁场变化，针对动态的磁场干扰采用如下的流程来处理。(注：在背景技术中我们主要指出的是长期出现的磁场干扰，长期代表的是出现磁场干扰的持续时间较长，此处的动态磁场干扰指的是磁场的磁感应强度是在变化的，它也属于长期出现的磁场干扰。)

首先使用滑动窗口计算数据的方差

建立一长度为n的环形缓冲区，用来存放接收数据。之所以说是环形缓冲区，是因为数据是一维的，缓冲区用完长度后，从第一个数据开始覆盖，从数据的存储方式来说是环形的。使用“环形缓冲区”这一说法可以让人理解真实的数据存储和使用流程。

假设当前采样窗口为n，其中数据所在的缓冲区为buf[n]＝[x₁ … x_n]，则其均值为其方差为

方差和均值计算后主要用来计算当前磁场的量测噪声即下面公式(3-1)中的R_{m_n}，计算R_{m_n}之后会替换公式(2-7)中的R_k中的磁场量测噪声。

针对动态磁场分为两种情况，一种是高频变化的磁场，一种是可以归类为低频变化的磁场。

高频变化的磁场其方差也就是数据的波动要明显大于低频变化磁场，因此本实施例通过方差进行磁场类别的判断。当上述方差大于用户预设值时，则可认为是高频变化的磁场。

对于高频变化的磁场，通过加大相应量测噪声的办法来降低滤波器对当前磁场的融合比例，则当前磁场的量测噪声可以表示为：

其中R_m为滤波器的初始磁场量测噪声，S_x，S_y，S_z为三轴磁场的方差数据。

公式(3-1)表示加大了量测噪声，主要用在公式(2-7)中，用来替换R_k中的属于磁场噪声的部分。

因此，对于长期出现的磁场干扰，如果是高频变化的磁场，可以通过上面的抗干扰算法(3-1)来改变公式中(2-7)的R_k值，加入此时处于干扰状态，通过(3-1)可以得到R_{m_n}变大，从而使得公式(2-7)中的R_k值变大，使得K_k值变小，对量测数据的使用权值就会下降。

对于低频变化的磁场，将附加在磁场矢量上的低频磁场作为偏置进行处理，然后使用基准磁场矢量进行航向姿态的解算和航向陀螺的偏置估计(因为公式(2-2)的系统中定义了陀螺的偏置，所以随着卡尔曼滤波器的运行，系统状态中的四个四元数和陀螺偏置便可以计算出来，然后就可以通过四个四元数计算出姿态角(“通过四个四元数计算出姿态角”指的是通过第四部分的转换矩阵将递推得到的四元数转换成姿态角)。详见下文描述)。当前的低频磁场偏置被包含在前文所说的三轴磁场偏置中，三轴磁场偏置包括传感器本身的磁场偏置和外界的低频磁场干扰偏置。(本发明考虑的是飞行器的飞行范围在几十千米内，并没安装GPS设备。如果当前的设备安装了GPS便可以根据经纬度，查表计算当前的磁偏角和磁倾角，重新计算基准矢量即可。)

即，如果此时的磁场干扰是低频的话，就可以使用当前传感器测量出来的磁场强度同经过旋转矩阵转换后的磁场基准矢量(m_r)相减，便可以获得当前的三轴磁场偏置。公式及过程如下：

假设当前传感器的测量出的三轴磁感应强度为当前的基准矢量为当前的转换矩阵为则当前的磁场偏置(此处的m_b是通过罗盘传感器测量出来的磁场偏置数值，而b_m表示的是经过卡尔曼递推得到的磁场偏置数值，且满足均方误差最小,二者含义不同，因此采用不同的标号)，其中表示的是m_r的转置处理过后的数据送入上文所说的卡尔曼滤波器中，即：把经过磁场干扰处理过后的数据即磁场偏置数据送入到公式(2-7)中的矢量中的三轴磁场偏置b_m中。

4、航向姿态解算

通过公式(2-7)得到了不断更新递推的四个四元数、陀螺偏置和磁场偏置，通过四元数可以进一步求解航向姿态：

计算出四元数q＝[q₀ q₁ q₂ q₃]之后，求解旋转矩阵

三轴姿态为姿态为att＝[φθψ]

由此便计算出了航向姿态ψ。

本实施例具有如下优点：

(1)通过对当前磁场环境的判断，提高了校准的正确率。

(2)使用的滤波方法可以对参数进行最优估计。

(3)提高了复杂磁场环境下算法的可靠性。

本发明方法经过试验，基准矢量的建立、扩展卡尔曼滤波方法、以及动态抗干扰算法经过了实验的检验，证明可行。

Claims (15)

1.一种罗盘校准抗干扰方法，其特征在于包括如下步骤：

S1、进行三轴磁罗盘初始校准，使得当前测试环境适合建立基准矢量；

S2、利用当前磁罗盘测出载体系的三轴磁场强度，根据测得的磁场强度建立基准矢量；

S3、进行基于基准矢量的滤波，保证整体滤波过程中均方误差最小，求得三轴磁场偏置，将该偏置做为干扰进行排除。

2.如权利要求1所述的罗盘校准抗干扰方法，其特征在于：

步骤S1中所述“当前测试环境适合建立基准矢量”是指三轴磁罗盘测量的磁场强度的幅值与地磁的磁场强度的偏差的绝对值小于设定值；所述初始校准包括如下步骤：

S11、在当前测试环境下进行三轴磁罗盘数据测量；

S12、计算三轴磁罗盘测量的磁场强度的幅值M_m以及该幅值M_m与地磁的磁场强度B_E的偏差的绝对值B_m；

S13、判断：如果此时的所述的偏差的绝对值B_m小于预定的特定值，则初始校准成功；如果所述的偏差的绝对值B_m不小于预定的特定值，则更换到新的测量环境做为当前测试环境重新进行上述步骤S11-S13，直到初始校准成功。

3.如权利要求1所述的罗盘校准抗干扰方法，其特征在于：

所述步骤S2中，建立基准矢量包括如下步骤：利用当前磁罗盘所测出的载体系的三轴磁场强度计算在导航坐标系下的三轴对应的磁场参考矢量，即为基准矢量m_r。

4.如权利要求1所述的罗盘校准抗干扰方法，其特征在于：

所述步骤S3中，所述滤波为基于磁场基准矢量的扩展卡尔曼滤波。

5.如权利要求4所述的罗盘校准抗干扰方法，其特征在于：

在所述基于磁场基准矢量的扩展卡尔曼滤波中，采用用来做三维空间的旋转变换的四元数q＝[q₀ q₁ q₂ q₃]和三轴陀螺漂移b_ω＝[b_ωx b_ωy b_ωz]以及三轴的磁场偏置b_m＝[b_mx b_myb_mz]作为状态矢量，以三轴加速度计测量的加速度a＝[a_x a_y a_z]和三轴磁罗盘测量的磁场强度m＝[m_x m_y m_z]作为量测数据构建扩展卡尔曼滤波方程。

6.如权利要求1所述的罗盘校准抗干扰方法，其特征在于还包括如下步骤：

使用滑动窗口计算三轴磁场的的方差；

当上述方差大于用户预设值时，则认为是高频变化的磁场；对于高频变化的磁场，通过加大步骤S3中滤波时所用的相应量测噪声项的办法来降低滤波器对当前磁场的融合比例；

否则，则认为是低频变化的磁场，对于低频变化的磁场，将附加在磁场矢量上的低频磁场作为步骤S3中所说的偏置进行处理。

7.如权利要求5所述的罗盘校准抗干扰方法，其特征在于，所述卡尔曼滤波中包括有量测噪声序列方差矩阵R_k；并且所述抗干扰方法还包括如下步骤：

使用滑动窗口计算三轴磁场的的方差；

当上述方差大于用户预设值时，则认为是高频变化的磁场；对于高频变化的磁场，通过加大相应量测噪声项的办法来降低滤波器对当前磁场的融合比例；具体是将卡尔曼滤波中的量测噪声序列方差矩阵R_k调整为：

以加大卡尔曼滤波中的量测噪声项，其中S_x，S_y，S_z为三轴磁场的方差数据。

8.如权利要求5所述的罗盘校准抗干扰方法，其特征在于还包括如下步骤：

使用滑动窗口计算三轴磁场的的方差；

当上述方差不大于用户预设值时，则此时判断三轴磁场的数据幅值是否小于特定值；如果数据幅值小于特定值，则直接进入卡尔曼滤波；如果数据幅值不小于特定值，则先进行常值偏置估计，然后再进行卡尔曼滤波。

9.如权利要求8所述的罗盘校准抗干扰方法，其特征在于：所述常值偏置估计方法包括如下步骤：

使用当前传感器测量出来的磁场强度同经过旋转矩阵转换后的磁场基准矢量m_r相减，以获得当前的三轴磁场偏置。

10.一种航向姿态解算方法，使用经过校准后的罗盘数据融合陀螺仪进行航向姿态解算，其特征在于，所述校准包括采用如权利要求1-9所述的抗干扰方法以过滤磁场干扰。

11.一种航向姿态解算装置，其特征在于使用如权利要求10所述的航向姿态解算方法。

12.一种嵌入式设备，包括CPU和存储器，CPU通过接口连接三轴磁罗盘并采集三轴磁罗盘的数据，存储器中存储有计算机程序，所述CPU可执行该计算机程序用于实现如权利要求1-9所述的方法。

13.一种嵌入式设备，包括CPU和存储器，CPU通过接口连接三轴磁罗盘并采集三轴磁罗盘的数据，存储器中存储有计算机程序，所述CPU可执行该计算机程序用于实现如权利要求10所述的方法。

14.一种计算机存储介质，其存储有计算机程序，所述计算机程序可被执行用于实现如权利要求1-9所述的方法。

15.一种计算机存储介质，其存储有计算机程序，所述计算机程序可被执行用于实现如权利要求10所述的方法。