[发明专利]一种磁罗盘全罗差实时校准与补偿方法

权利要求书

1.一种磁罗盘全罗差实时校准与补偿方法，其特征在于：该方法分阶段进行，第一阶段使用载体不同姿态下的磁罗盘三轴磁场数据，基于地磁场的模不变性，由不同姿态下的磁罗盘三轴磁场测量数据对三轴磁传感器的全罗差进行自适应Kalman滤波估计，估计磁罗盘全罗差中的9个罗差参数；第二阶段基于水平坐标系中地磁场分量不变的约束条件，仅利用磁罗盘自身输出的信息进一步估计出剩余缺失的3个罗差参数，实现全罗差补偿；

具体包括如下步骤：

步骤1：在无磁场干扰的区域，用磁传感器测量该区域的总场值||H^b||，采集载体不同姿态下的磁罗盘三轴磁场数据；

步骤2：建立全罗差模型和校正模型

定义为地磁场矢量的真实值在载体坐标系中的投影，为磁罗盘三轴磁场的测量值；

地磁场的真实值H^b与磁罗盘三轴磁场的测量值H^m之间可用下列矩阵方程表示：

H^m＝CH^b+b+n

其中，矩阵C＝I+K，为软磁干扰系数矩阵，b＝[b_x b_y b_z]^T为硬磁干扰系数向量，向量n表示磁传感器噪声，可以用测量值的平均值来估计；相对于系统误差，测量噪声误差的数量级较小，为此，在建立标定与补偿模型时可先不予考虑n的影响；

为了消除上述系统误差，根据全罗差模型构建校正模型，校正模型以磁罗盘中三轴磁场测量值H^m作为模型输入，其模型的输出为

其中为地磁场的估计值，矩阵G是矩阵C的逆矩阵，若能利用H^m求得矩阵G的估计和向量b的估计，则可根据上式由磁罗盘三轴磁场测量数据获得地磁场矢量的估计值；

步骤3：构造Kalman滤波模型

对于步骤2中的校正模型的输出公式，有

||H^b||²＝[G(H^m-b)]^T[G(H^m-b)]

＝(H^m-b)^TΓ(H^m-b)

其中：矩阵γ是待估计参数；

展开方程，则

为磁罗盘三轴磁场的测量值；

以待估计参数作为状态变量，基于地磁场模的不变性，构造观测方程，得到如下的Kalman滤波模型：

y＝Hx+v

式中，v＝2(H^m-b)^TΓn+n^TΓn为测量噪声，其均值μ和方差σ可以表示为

μ＝E(v)＝sum(Γ.*∑)

σ²＝E(v²)-μ²

＝4[Γ(H^m-b)]^T∑[Γ(H^m-b)]

H和x分别为9元素行向量和9元素列向量，其各元素分别为：

h₉＝1

对Kalman滤波模型测量方程式离散化，可以得到：

式中，下标表示k时刻，噪声v_k是一个与状态相关的非高斯噪声，若采用传统的最小二乘算法无法实现参数的最优估计；

步骤4：采用自适应Kalman滤波进行状态估计；

步骤5：由步骤4得到的最优估计值按照下式计算矩阵K_G和矢量b的最优估计值和

1)计算矢量

2)计算矩阵Γ

3)由矩阵Γ来确定矩阵

Γ用来确定矩阵K_G，K_G由矩阵G的极分解给出，即

K_G＝(R_G)^TG

所以，由矩阵Γ的奇异值分解，可以得到：

步骤6：根据估计的误差矩阵和矢量带入校正模型，得到第一阶段校正后的地磁矢量估计值即

步骤7：第二阶段确定剩余罗差参数，实现全罗差补偿；

基于水平坐标系中地磁场分量不变的约束条件，仅利用磁罗盘自身输出的信息进一步估计出剩余缺失3个罗差参数，从而实现全罗差补偿；

地磁场矢量的模长与方向均不变，因此其竖直分矢量与水平分矢量均为常矢量，计算地磁场竖直分量：

其中F(θ,γ)＝[sinγ -sinθcosγ cosθcosγ]^T为旋转矩阵的第3行列向量；

根据空间轴系的转换关系，将R_G简化成3参数的方向余弦矩阵，得到正交矩阵R_G如下：

该式表示为一个关于参数β₁、β₂和β₃的非线性方程，估计出该参数方程，即可确定正交矩阵R_G，再根据G＝R_GK_G，确定矩阵G，从而完成实现全罗差补偿；

步骤8：根据估计的误差矩阵矢量和正交矩阵得到最终全罗差补偿后的磁罗盘三轴磁场即

步骤9：根据补偿后的磁罗盘的三轴磁场数据，结合三轴加速度数据按照以下公式计算得到磁罗盘的俯仰角、横滚角和磁航向角；

计算俯仰角θ和横滚角γ：

其中，为磁罗盘载体坐标系下测得三轴加速度；

将经过校正和补偿后的载体坐标系下磁罗盘三轴磁场数据和变换到地理坐标下得到水平方向两轴磁场和其计算公式如下：

利用下式计算磁航向角ψ：