[发明专利]一种航向姿态解算及罗盘校准抗干扰方法

权利要求书

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

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

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

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

其中，所述罗盘校准抗干扰方法还包括如下步骤：

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

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

否则，则认为是低频变化的磁场，对于低频变化的磁场，将附加在磁场矢量上的低频磁场作为步骤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.如权利要求5所述的罗盘校准抗干扰方法，其特征在于，所述卡尔曼滤波中包括有量测噪声序列方差矩阵R_k；并且所述抗干扰方法还包括如下步骤：

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

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

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

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