CN107870001B - 一种基于椭球拟合的磁力计校正方法 - Google Patents

Abstract

本发明公开了一种基于椭球拟合的磁力计校正方法，包括如下步骤：1、根据磁力计所受到误差影响，建立磁力计误差模型：2、根据收集到的磁力计测量数据，将磁力计误差模型转化为磁力计椭球误差模型；3、计算磁力计椭球误差模型中的参数，并计算出椭球拟合的残差；4、确定满意区间，并计算满意值、满意区间概率，当满意区间概率小于满意值，去除去噪点，返回步骤3利用剩余数据重新计算磁力计椭球误差模型中的参数，并计算出椭球拟合的残差，再次执行步骤4，直到满意区间概率大于满意值，进而求出椭球拟合的误差，完成磁力计校正。该方法可以有效的提高磁力计校正及补偿精度，且该方法可靠性强、成本低、精度高、计算过程简洁。

Description

一种基于椭球拟合的磁力计校正方法

技术领域

本发明属于导航仪器校正领域，具体涉及一种基于椭球拟合的磁力计校正方法。

背景技术

在导航系统中，加速度计和陀螺仪组成惯性测量单元(IMU)，在某些精度要求较高的情况下，加速度计和陀螺仪自身的误差及漂移造成IMU输出的姿态和航向参数精度无法满足导航系统的要求。由于磁力计具有估计误差不积累的优点，因此磁力计常常被用来补偿陀螺仪的漂移从而提高姿态角的测量精度。由惯性测量单元(IMU)和磁力计组成的导航系统，可以提高姿态角及航向角的精度。但是，磁力计的输出会受到自身以及周围磁场环境干扰而失真产生误差，如偏置误差、比例因子误差、非正交误差、安装误差、硬磁和软磁误差。因此，在磁力计使用之前必须进行校正和补偿。

发明内容

发明目的：针对现有技术中存在的问题，本发明提供了一种基于椭球拟合的磁力计校正方法，该方法可以有效的提高磁力计校正及补偿精度，且该方法可靠性强、成本低、精度高、计算过程简洁。

技术方案：本发明采用如下技术方案：

一种基于椭球拟合的磁力计校正方法，包括如下步骤：

(1)根据磁力计所受到误差影响，建立磁力计误差模型：

其中是磁力计的输出矢量，B^b是当地磁场矢量在载体坐标系中的分量，b₀是总偏置误差矢量，M＝C_mC_nC_s(I_3×3+C_si)是总变换矩阵，C_s是比例因子误差矩阵，C_m是安装误差矩阵，C_n是非正交误差矩阵，C_si是软磁误差矩阵，I_3×3是3×3维单位矩阵，M^-1为矩阵M的逆矩阵；

(2)根据磁力计误差特性以及收集到的磁力计测量数据，将磁力计误差模型转化为磁力计椭球误差模型：

(3)计算磁力计椭球误差模型中的参数，并计算出椭球拟合的残差；

(4)根据残差的统计信息确定满意区间，并计算满意值、满意区间概率，所述满意值为服从正态分布的变量落入满意区间的概率，满意区间概率为测量数据落入满意区间的概率，当满意区间概率小于预设的满意值，去除去噪点，返回步骤(3)利用剩余数据重新计算磁力计椭球误差模型中的参数，并计算出椭球拟合的残差，再次执行步骤(4)，对残差结果进行分布计算分析，直到满意区间概率大于预设的满意值，进而求出椭球拟合的误差，完成磁力计校正。

步骤(2)中磁力计椭球误差模型的代数形式为：

其中α＝(a,b,c,d,e,f,p,q,r,s)为椭球参数，v为磁力计误差模型中的磁力计的输出矢量v_x,v_y,v_z分别为v在三维坐标系中三个坐标轴上的分量。

通过最小二乘法计算磁力计椭球误差模型中的椭球参数α。

所述椭球拟合的残差R为：R＝S*α₀；其中S为磁力计输出的N个测量值构成的矩阵，S＝(v₁,v₂,v₃,...,v_N)，v_i是第i个测量值；其中α′＝(a′,b′,c′,d′,e′,f′,p′,q′,r′,s′)为步骤(3)中计算得到的磁力计椭球误差模型中的参数，α′(10)是α′的第10列的元素，即α′(10)＝s′，X₀为椭球球心，

步骤(4)包括如下步骤：

(4.1)求出椭球拟合残差的平均值和标准差

(4.2)确定满意区间为计算满意区间概率P，P为测量数据落入满意区间的概率；计算满意值Q，Q为服从正态分布的变量x落入满意区间的概率；

(4.3)若概率P大于满意值Q，可直接进行椭球误差的求解；若概率P小于满意值Q，则去掉测量数据在满意区间之外的点，然后返回步骤(3)利用剩余数据重新计算磁力计椭球误差模型中的参数，并计算出椭球拟合的残差，再次执行步骤(4)，对残差结果进行分布计算分析，直到概率P大于满意值Q，进而求出椭球拟合的误差。有益效果：与现有技术相比，本发明公开的基于椭球拟合的磁力计校正方法具有以下优点：(1)在磁力计测量过程中会不可避免的存在一些误差较大的样本点，若误差较大的样本点较多，或者少数但较显著，会对测量精度影响很大。如果直接将包含这些点的所有样本点直接用最小二乘法进行椭球拟合，得到的拟合误差比较大；本发明公开的方法通过去除这些误差较大的样本点，使得拟合效果更好，同时磁力计校正精度更高。(2)本发明不需要提供新的设备和器件，充分利用了现有数据，在提高精度和稳定性的同时没有增加成本。

附图说明

图1为本发明公开方法的流程图。

具体实施方式

本发明公开了一种基于椭球拟合的磁力计校正方法下面结合附图进一步阐述本发明。

如图1所示，一种基于椭球拟合的磁力计校正方法，包括如下步骤：

(1)根据磁力计所受到误差影响，建立磁力计误差模型；

磁力计误差一般受比例因子误差、安装误差、非正交误差、软磁误差、硬磁误差的影响，磁力计误差模型的一般表达式为：

其中，C_s是比例因子误差矩阵，C_m是安装误差矩阵，C_n是非正交误差矩阵，C_si是软磁误差矩阵，是硬磁误差矢量，是软磁误差矢量，b^b是偏置误差矢量，B^b是当地磁场矢量在载体坐标系中的分量，是磁力计的输出矢量，ε₀是系统噪声矢量。上标“b”表示该矢量是在载体坐标系中进行测量。

简化表达式(1)和(2)可得到：

其中M＝C_mC_nC_s(I_3×3+C_si)是总变换矩阵，I_3×3是3×3维单位矩阵，是总偏置误差矢量，忽略高斯白噪声ε₀，得到如下表达式：

通过式(4)可以得到校正后的磁场强度：

(2)根据磁力计误差特性以及收集到的磁力计测量数据，将磁力计误差模型转化为磁力计椭球误差模型：

根据步骤(1)中的误差模型，为方便起见，将表达式(5)写成如下形式：

u＝L(v-b) (6)

由地磁场特性知，矢量场u的强度(即矢量u的模长)不变，即

|u|²＝L^TL＝const (7)

式(7)的物理含义是矢量u的变化轨迹是一个球面，将(6)带入(7)中：

u^Tu＝v^TL^TLv-2b^TL^TLv+b^TL^TLb＝const (8)

由线性代数相关知识可知，当矩阵L^TL为正定时，式(8)中关于v的二次型代表一个椭球面，而对于任意矩阵L而言，L^TL必定为对称半正定矩阵；而在传感器误差不是很大时，L^TL为严格对角占优，即任一行或任一列中非对角元之和严格小于该行或者该列中的对角元，这刚好是让L^TL为严格正定矩阵的一个充要条件。故此，可以认为v的变化轨迹是一个椭球面。

将式(8)转化为代数形式如下：

其中α＝(a,b,c,d,e,f,p,q,r,s)为椭球参数，v为磁力计误差模型中的磁力计的输出矢量v_x,v_y,v_z分别为v在三维坐标系中三个坐标轴上的分量。

(3)计算磁力计椭球误差模型中的参数，并计算出椭球拟合的残差；

本实施例中通过最小二乘法计算磁力计椭球误差模型中的椭球参数α。基于椭球的最小二乘拟合方法就是寻找一组椭球参数，使得测量数据v到该椭球面的代数距离平方和最小。即：取得最小值。化为矩阵的形式，D(σ)＝σ^TS^TSσ的最小值。

其中S为磁力计输出的N个测量值构成的矩阵，S＝(v₁,v₂,v₃,...,v_N)，v_i是第i个测量值；

为了使得拟合的曲面为椭球面，需要引入限制条件4ac-b>0，一般取4ac-b＝1，4ac-b＝1可表示为矩阵形式：

α^TCα＝1 (10)

其中C₂＝0_3*7,C₃＝0_7*3,C₄＝0_7*7；

引入拉格朗日乘子λ，椭球拟合转变成求约束条件下函数G(σ)＝σ^TS^TSσ+λ(1-σ^TCσ)的条件极值。对函数G(σ)一阶求导并令其导数为零，然后求解α的过程可以简化为求解：

S^TSα＝λCα (11)

式(11)中，存在唯一的为正的特征值，该特征值对应的特征向量α′＝(a′,b′,c′,d′,e′,f′,p′,q′,r′,s′)即为所求的α。

通过求出的α′可以求解出椭球球心其中令α′(10)是α′的第10列的元素，即α′(10)＝s′，则椭球拟合的残差为：

R＝S*α₀ (12)

(4)根据残差的统计信息确定满意区间，并计算满意值、满意区间概率，所述满意值为服从正态分布的变量落入满意区间的概率，满意区间概率为测量数据落入满意区间的概率，当满意区间概率小于预设的满意值，去除去噪点，返回步骤(3)利用剩余数据重新计算磁力计椭球误差模型中的参数，并计算出椭球拟合的残差，再次执行步骤(4)，对残差结果进行分布计算分析，直到满意区间概率大于预设的满意值，进而求出椭球拟合的误差，完成磁力计校正，具体步骤为：

(4.1)求出椭球拟合残差的平均值和标准差

(4.2)确定满意区间为计算满意区间概率P，P为测量数据落入满意区间的概率，本实施例中P为测量数据在满意区间的个数与测量数据总数的比值；计算满意值Q，Q为服从正态分布的变量x落入满意区间的概率；

(4.3)若概率P大于满意值Q，可直接进行椭球误差的求解；若概率P小于满意值Q，则去掉测量数据在满意区间之外的点，然后返回步骤(3)利用剩余数据重新计算磁力计椭球误差模型中的参数，并计算出椭球拟合的残差，再次执行步骤(4)，对残差结果进行分布计算分析，直到概率P大于满意值Q，进而求出椭球拟合的误差。满意区间中参数k的具体值可以根据实验得出。k的值越大，满意值Q的值越大，为了得到较好的去噪效果，Q不宜过大，否则不能有效的去除去噪点，同时Q不宜过小，否则数据不能得到有效的分析处理。根据正态分布的特性可知当k＝1时，Q＝68.27％；当k＝1.96时，Q＝95.45％。故k可在[1,1.96]中取值，而具体的k需要通过实验得出。可通过等分法将k的大小依次取k＝1，k＝1.1，k＝1.2…k＝1.9进行实验，通过实验迭代去噪之后得到椭球拟合的残差，通过残差对比可以说明椭球拟合的效果，以此来确定k的大小。

Claims (3)

1.一种基于椭球拟合的磁力计校正方法，其特征在于，包括如下步骤：

(1)根据磁力计所受到误差影响，建立磁力计误差模型：

其中是磁力计的输出矢量，B^b是当地磁场矢量在载体坐标系中的分量，b₀是总偏置误差矢量，M＝C_mC_nC_s(I_3×3+C_si)是总变换矩阵，C_s是比例因子误差矩阵，C_m是安装误差矩阵，C_n是非正交误差矩阵，C_si是软磁误差矩阵，I_3×3是3×3维单位矩阵，M^-1为矩阵M的逆矩阵；

(2)根据磁力计误差特性以及收集到的磁力计测量数据，将磁力计误差模型转化为磁力计椭球误差模型；

(3)计算磁力计椭球误差模型中的参数，并计算出椭球拟合的残差；

(4)根据残差的统计信息确定满意区间，并计算满意值、满意区间概率，所述满意值为服从正态分布的变量落入满意区间的概率，满意区间概率为测量数据落入满意区间的概率，当满意区间概率小于根据正态分布所求的满意值，去除去噪点，返回步骤(3)利用剩余数据重新计算磁力计椭球误差模型中的参数，并计算出椭球拟合的残差，再次执行步骤(4)，直到满意区间概率大于根据正态分布所求的满意值，进而求出椭球拟合的误差，完成磁力计校正；；

步骤(2)中磁力计椭球误差模型的代数形式为：

其中α＝(a,b,c,d,e,f,p,q,r,s)为椭球参数，v为磁力计误差模型中的磁力计的输出矢量v_x,v_y,v_z分别为v在三维坐标系中三个坐标轴上的分量；

所述椭球拟合的残差R为：R＝S*α₀；其中S为磁力计输出的N个测量值构成的矩阵，S＝(v₁,v₂,v₃,...,v_N)，v_i是第i个测量值；其中α′＝(a′,b′,c′,d′,e′,f′,p′,q′,r′,s′)为步骤(3)中计算得到的磁力计椭球误差模型中的参数，α′(10)是α′的第10列的元素，即α′(10)＝s′，X₀为椭球球心，

2.根据权利要求1所述的基于椭球拟合的磁力计校正方法，其特征在于，通过最小二乘法计算磁力计椭球误差模型中的椭球参数α。

3.根据权利要求1所述的基于椭球拟合的磁力计校正方法，其特征在于，步骤(4)包括如下步骤：

(4.1)求出椭球拟合残差的平均值和标准差

(4.2)确定满意区间为计算满意区间概率P，P为测量数据落入满意区间的概率；计算满意值Q，Q为服从正态分布的变量x落入满意区间的概率；

(4.3)若概率P大于满意值Q，可直接进行椭球误差的求解；若概率P小于满意值Q，则去掉测量数据在满意区间之外的点，然后返回步骤(3)利用剩余数据重新计算磁力计椭球误差模型中的参数，并计算出椭球拟合的残差，再次执行步骤(4)，对残差结果进行分布计算分析，直到概率P大于满意值Q，进而求出椭球拟合的误差。