CN115597571A - 电子罗盘传感器误差及安装误差快速标定与补偿方法 - Google Patents

Abstract

本发明公开了一种电子罗盘传感器误差及安装误差快速标定与补偿方法，涉及电子罗盘的误差补偿领域，其方法为：先选择一电子罗盘，得到MEMS三轴加速度计在每个位置处x、y、z轴上的理论值和初始测量值、和x、y轴上多个样本点的初始磁场强度，送入卡尔曼滤波器；再求解MEMS三轴加速度计的误差补偿参数矩阵、以及磁场数据的椭圆拟合参数，分别对MEMS三轴加速度计和磁传感器数据进行补偿，本发明不仅能减小数据输出噪声，还能有效地补偿MEMS加速度计自身零偏、刻度因子、非正交及安装误差，并且对外界的磁场干扰进行补偿。精度较高，操作简单，易于实现，具有重要的工程实用价值。

Description

电子罗盘传感器误差及安装误差快速标定与补偿方法

技术领域

本发明涉及电子罗盘的误差补偿领域，尤其涉及一种电子罗盘传感器误差及安装误差快速标定与补偿方法。

背景技术

电子罗盘中，会用到MEMS三轴加速度计和磁传感器。

关于MEMS三轴加速度计：目前MEMS三轴加速度计存在较大的测量噪声，可达数百mg，严重影响后续的数据处理。这些问题主要是因由以下几个方面产生的：

1、未处理传感器输出数据，输出数据存在较大噪声，严重影响测量精度。

2、MEMS三轴加速度计自身存在零偏误差、刻度因子误差、非线性误差、三轴非正交误差以及安装误差，严重影响电子罗盘的倾斜角测量和补偿精度。

3、受到外界干扰磁场的影响，磁测数据与真实值偏差较大。

综上，MEMS三轴加速度计的误差主要来源于自身的零偏误差、刻度因子误差、非线性误差、以及安装使用时的安装误差等，如何对这些误差进行标定和补偿，是亟需解决的问题。

关于磁传感器：外界环境干扰磁场会造成磁场测量误差，故磁传感器测得的数据与实际数据也会存在偏差。

由于加速度和磁场强度均存在偏差，我们在用到二者之一或二者结合进行后续数据处理时，会导致数据偏差较大。

发明内容

本发明的目的就在于提供一种解决上述问题，能克服电、三轴加速度计自身的零偏误差、刻度因子误差、非线性误差、以及安装使用时的安装误差的，电子罗盘传感器误差及安装误差快速标定与补偿方法。

为了实现上述目的，本发明采用的技术方案是这样的：一种电子罗盘传感器误差及安装误差快速标定与补偿方法，包括以下步骤；

S1.选择一电子罗盘，所述电子罗盘包括MEMS三轴加速度计和磁传感器；

通过六位置标定法，得到MEMS三轴加速度计在每个位置处x、y、z轴上的理论值和初始测量值；

罗盘沿x、y轴形成的平面旋转一周，由磁传感器得到x、y轴上多个样本点的初始磁场强度；

S2.将初始测量值和初始磁场强度值分别送入卡尔曼滤波器中处理，得到MEMS三轴加速度计在每个位置处x、y、z轴上的优化测量值、和x、y轴上多个样本点的优化磁场强度；

S3.求解MEMS三轴加速度计的误差补偿参数矩阵，包括步骤S31-S32；

S31.建立MEMS三轴加速度计的误差补偿参数矩阵求取模型；

（1）

式中，

和

分别为三轴加速度计在第i个位置的理论值和第j个位置的优化测量值；

，g _x 、g _y 、g _z 分别为MEMS三轴加速度计在x、y、z轴上的理论值；

，G _x 、G _y 、G _z 分别为MEMS三轴加速度计在x、y、z轴上的优化测量值；

S32.将MEMS三轴加速度计在六位置的理论值和优化测量值带入上式（1），求解得到误差补偿参数矩阵A _4×3；

S4.在xy平面上的设置椭圆方程，对样本点采用最小二乘椭圆拟合算法，求解椭圆参数A，B，C，D和E，进一步得到椭圆拟合参数并保存，所述椭圆拟合参数包括椭圆圆心（x0，y0）、长轴a、短轴b和椭圆旋转角θ；

S5.将S1中电子罗盘置于实测场景中，按步骤S1、S2得到实测场景下，MEMS三轴加速度计在每个位置处x、y、z轴上的优化测量值、和x、y轴上多个样本点的优化磁场强度；

S6.对三轴加速度计进行补偿，包括步骤S61-S62；

S61.将A _4×3前三行构成一综合误差系数矩阵K _3×3，第四行三个元素从左到右依次标记为G _x0、G _y0、G _z0，分别表示x、y、z轴上的零偏误差；

S62.将S5中的优化测量值、K_3×3、G _x0、G _y0、G _z0带入下式，计算得到MEMS三轴加速度计在x、y、z轴上的补偿测量值g'_x、g'_y、g'_z；

=K_3×3

（2）

S7.采用椭圆拟合参数对优化磁场强度进行补偿，得到补偿磁场强度。

作为优选：S1中，所述六位置标定法具体为；

S11将电子罗盘安装在三轴无磁转台上，使电子罗盘各敏感轴方向与三轴无磁转台一致，调整三轴无磁转台处于水平状态；

S12接通电子罗盘电源，预热3分钟后，开始对MEMS三轴加速度计进行标定；

S13调整三轴无磁转台，使MEMS三轴加速度计位于六位置中的第一个位置；

S14数据采集；静止30秒，在静止状态下采集MEMS三轴加速度计的三轴输出数据，数据采集持续30秒；

S15调整三轴无磁转台，使MEMS三轴加速度计分别位于六位置中的第2-6个位置，在每个位置按照步骤S14进行数据采集。

关于卡尔曼滤波：

卡尔曼滤波算法是工程应用中最为广泛的一种最优估计算法。对于电子罗盘中使用的MEMS三轴加速度计，其系统状态转移矩阵A _k 、观测矩阵H _k 、动态噪声W _k 、观测噪声V _k 均可认为是固定值，即A _k =A，H _k =H，W _k =W，V _k =V，因此可得到MEMS三轴加速度计的卡尔曼滤波方程如下：

状态向量预测方程为：

；

状态向量协方差矩阵预测方程为：

；

卡尔曼滤波增益方程为：

；

状态向量更新方程为：

；

状态向量协方差矩阵更新方程为：

；

其中，

表示根据k-1时刻的三轴状态估计向量预测得到的k时刻的三轴状态预测向量，

表示k-1时刻的三轴状态估计向量，

表示由k-1时刻到k时刻的三轴状态预测误差协方差，

表示k-1时刻的三轴状态估计误差协方差。

我们对将数据进行卡尔曼滤波后，能有效减小测量噪声。

关于MEMS三轴加速度计，误差主要来源于自身的零偏误差、刻度因子误差、非线性误差、三轴非正交误差以及安装使用时的安装误差等。由于三轴非正交误差与安装误差产生的影响类似，在进行标定与补偿时，可以将两者统一表示为安装误差。因此，根据MEMS三轴加速度计的主要误差来源及特点，对某一位置而言，可将MEMS三轴加速度计的输出误差数学模型表示为：

（3）

式中，g _x0、g _y0、g _z0表示MEMS三轴加速度计在x、y、z轴的零偏值，单位为g；g _x、g _y、g _z表示MEMS三轴加速度计在x、y、z轴的理论值，单位为g；S _gxx、S _gyy 、S _gzz表示MEMS三轴加速度计的刻度因子系数；K _gxy、K _gxz 、K _gyx、K _gyz、K _gzx、K _gzy表示MEMS三轴加速度计的安装误差系数，含三轴非正交误差系数；K _gx2、K _gy2、K _gz2表示MEMS三轴加速度计二阶非线性误差系数。

对于大部分MEMS三轴加速度计，其二阶非线性误差系数很小，标定与补偿时可忽略不计。因此不考虑MEMS三轴加速度计二阶非线性误差，可由式（3）得到简化后的MEMS三轴加速度计的输出误差数学模型为：

（4）

为了便于计算误差补偿参数，将式（4）变换为：

（5）

记

为MEMS三轴加速度计的综合误差系数矩阵，由于K _3×3是3阶满秩方阵，K _3×3可逆，则有：

（6）

当MEMS三轴加速度计安装状态确定后，不考虑温度变化影响，其综合误差系数矩阵K _3×3及零偏误差可视为固定值，因此，式（6）可变换为：

（7）

为便于求解误差补偿参数，由式（7）可得MEMS三轴加速度计误差补偿数学模型为：

（8）

记

，表示误差补偿参数矩阵；

基于式（8）的模型，我们带入每个位置对应的实测值、理论值，由于实测值、理论值都是确定的，所以算出来的误差补偿参数矩阵A_4×3也是定值，后续在测试现场实用本发明产品时，对所有测得的加速度数据，采用误差补偿参数矩阵A_4×3进行补偿即可。

关于最小二乘椭圆拟合，主要思路是求解出椭圆参数A，B，C，D和E，进一步计算椭圆拟合参数：椭圆圆心（x0，y0）、长轴a、短轴b和椭圆旋转角θ；以实现最终的误差补偿，对椭圆拟合采用最小二乘法，其思想是要使得观测点和估计点距离的平方和达到最小。其中一种实现流程可参见图3，通过以下方式实现：

（1）先在所有样本点中随机选取5个样本点，采用最小二乘法求解出椭圆的参数，具体做法为：

在二维平面坐标系中，任意位置椭圆可以采用圆锥曲线方程的代数形式表示，即：

；

假设

为二维平面内x、y方向的N（N≥5）个待拟合的样本点，根据最小二乘原理，应以求目标函数的最小值来确定参数A、B、C、D和E。

由极值定理，欲使F的值最小，必有

由此可得方程组

将所有样本点代入，求解线性方程组可得到A、B、C、D和E的值，然后，根据说明书附图3进行处理；

（2）遍历所有样本点，求取各个点到已得到的椭圆之间的距离，如果小于某个阈值，则称该样本点为匹配点，在数组C记录该样本点的编号，遍历完毕后，求取对于该拟合椭圆的匹配点的个数N，这里阈值根据实际情况自定；

（3）匹配点总个数 N赋值给匹配点最大值N_max；

（4）再次执行步骤（1）和步骤（2），比较匹配点总个数N与匹配点最大值N_max，当N>N_max时，将椭圆参数和记录匹配点编号的数组C 保存下来，分别赋值给数组I_e和数组I_c；

（5）循环执行步骤（4）一定次数，则最后在Ie保存了最优椭圆参数，在数组I_c保存了在所有样本点中匹配点的编号，也就相应的可以得到不匹配点的编号。

考虑到算法的实时性和准确性，对样本点的选取采用随机方法。但是如果考虑所有的可能性，譬如总样本空间有300个点，选取5个不同的样本点一共有

种可能，遍历所有样本点在时间上不允许，而且也不必要。要加速计算，只需选择其中的一部分子集，在样本性质并不清楚的情况下，使用随机方法抽取是一个很好的选择。

与现有技术相比，本发明的优点在于：

本发明不仅能减小数据输出噪声，还能有效地补偿MEMS加速度计自身零偏、刻度因子、非正交及安装误差，并且对外界的磁场干扰进行补偿。精度较高，操作简单，易于实现，具有重要的工程实用价值。

附图说明

图1为本发明流程图；

图2为MEMS三轴加速度计理论输出与六个位置的对应关系图，其中图2中（a）为第一个位置，（b）为第二个位置，（c）为第三个位置，（d）为第四个位置，（e）为第五个位置，（f）为第六个位置；

图3为寻找最优椭圆参数的流程图；

图4a为采用最小二乘椭圆拟合方法，用Matlab根据实验采集的数据绘制的散点图；

图4b为修正前后的磁场强度散点对比图；

图5为MEMS三轴加速度计x轴输出数据在卡尔曼滤波前后对比图，其中（a）为卡尔曼滤波前，（b）为卡尔曼滤波后；

图6为MEMS三轴加速度计y轴输出数据在卡尔曼滤波前后对比图，其中（a）为卡尔曼滤波前，（b）为卡尔曼滤波后；

图7为MEMS三轴加速度计z轴输出数据在卡尔曼滤波前后对比图，其中（a）为卡尔曼滤波前，（b）为卡尔曼滤波后。

具体实施方式

下面将结合附图对本发明作进一步说明。

实施例1：参见图1-图2，一种电子罗盘传感器误差及安装误差快速标定与补偿方法，包括以下步骤；

S1.选择一电子罗盘，所述电子罗盘包括MEMS三轴加速度计和磁传感器；

通过六位置标定法，得到MEMS三轴加速度计在每个位置处x、y、z轴上的理论值和初始测量值；

罗盘沿x、y轴形成的平面旋转一周，由磁传感器得到x、y轴上多个样本点的初始磁场强度；

S2.将初始测量值和初始磁场强度值分别送入卡尔曼滤波器中处理，得到MEMS三轴加速度计在每个位置处x、y、z轴上的优化测量值、和x、y轴上多个样本点的优化磁场强度；

S3.求解MEMS三轴加速度计的误差补偿参数矩阵，包括步骤S31-S32；

S31.建立MEMS三轴加速度计的误差补偿参数矩阵求取模型；

（1）

式中，

和

分别为三轴加速度计在第i个位置的理论值和第j个位置的优化测量值；

，g _x 、g _y 、g _z 分别为MEMS三轴加速度计在x、y、z轴上的理论值；

，G _x 、G _y 、G _z 分别为MEMS三轴加速度计在x、y、z轴上的优化测量值；

S32.将MEMS三轴加速度计在六位置的理论值和优化测量值带入上式（1），求解得到误差补偿参数矩阵A _4×3；

S4.在xy平面上的设置椭圆方程，对样本点采用最小二乘椭圆拟合算法，求解椭圆参数A，B，C，D和E，进一步得到椭圆拟合参数并保存，所述椭圆拟合参数包括椭圆圆心（x0，y0）、长轴a，短轴b和椭圆旋转角θ；

S5.将S1中电子罗盘置于实测场景中，按步骤S1、S2得到实测场景下，MEMS三轴加速度计在每个位置处x、y、z轴上的优化测量值、和x、y轴上多个样本点的优化磁场强度；

S6.对三轴加速度计进行补偿，包括步骤S61-S62；

S61.将A _4×3前三行构成一综合误差系数矩阵K _3×3，第四行三个元素从左到右依次标记为G _x0、G _y0、G _z0，分别表示x、y、z轴上的零偏误差；

S62.将S5中的优化测量值、K_3×3、G _x0、G _y0、G _z0带入下式，计算得到MEMS三轴加速度计在x、y、z轴上的补偿测量值g'_x、g'_y、g'_z；

=K_3×3

（2）

S7.采用椭圆拟合参数对优化磁场强度进行补偿，得到补偿磁场强度。

本实施例中，S1中，所述六位置标定法具体为；

S11将电子罗盘安装在三轴无磁转台上，使电子罗盘各敏感轴方向与三轴无磁转台一致，调整三轴无磁转台处于水平状态；

S12接通电子罗盘电源，预热3分钟后，开始对MEMS三轴加速度计进行标定；

S13调整三轴无磁转台，使MEMS三轴加速度计位于六位置中的第一个位置；

S14数据采集；静止30秒，在静止状态下采集MEMS三轴加速度计的三轴输出数据，数据采集持续30秒；

S15调整三轴无磁转台，使MEMS三轴加速度计分别位于六位置中的第2-6个位置，在每个位置按照步骤S14进行数据采集。

实施例2：参见图1-图7，基于实施例1的方法，我们具体实验，实验流程、数据及结果如下：

S1：通过六位置标定法得到MEMS三轴加速度计在每个位置处x、y、z轴上的理论值和初始测量值，罗盘沿x、y轴形成的平面旋转一周，由磁传感器得到x、y轴上多个样本点的初始磁场强度；

S2：将S1中数据送入卡尔曼滤波器中处理，得到MEMS三轴加速度计在每个位置处x、y、z轴上的优化测量值、和x、y轴优化磁场强度；

其中，每个位置的理论值、以及优化测量值见下表1：

表1：每个位置的理论值、以及优化测量值统计表

x、y轴优化磁场强度见表2：

表2：x、y轴的优化磁场强度表

S3.求解MEMS三轴加速度计的误差补偿参数矩阵，包括步骤S31-S32；

S31.建立MEMS三轴加速度计的误差补偿参数矩阵求取模型；

（1）

将表1中的数据带入式（1）中，由于

，那么：

，

，依次类推；

，那么：

=[ G _x G _y G _z 1 ]= [0.9362 0.1261 0.0612 1 ]；

=[ G _x G _y G _z 1 ]= [-1.0838 0.1150 0.0445 1 ]；依次类推。

最终，我们求解出误差补偿参数矩阵A _4×3的值如下：

S4.同实施例1步骤S4，具体可通过以下方式实现：

根据椭圆拟合方法，建立了Matlab仿真模型，同时将电子罗盘放置在无磁转台上进行实验数据采集，然后用最小二乘椭圆拟合算法求解椭圆参数A，B，C，D和E，参见图4a，图4a中，实验数据为Matlab根据实验采集的数据绘制的散点图，实验数据拟合是用最小二乘椭圆拟合算法得到椭圆，可以看出，曲线拟合结果与数据点几乎一致，所以完全重叠，几乎看不出区别，拟合后，我们能得到椭圆参数长轴a=18056.18、短轴b=17417.49、中心坐标（x0=602.85，y0=-1403.18）。利用这个椭圆参数，我们可以对磁场数据进行补偿，补偿的结果参见图4b，图中2个椭圆，靠下的椭圆圆心没有居于零点，校准后为靠上的椭圆，校准后的长轴a=17489.64，b=17987.52，中心坐标（x0=0.00，y0=-0.00），从仿真结果来看，采用椭圆拟合方法进行校准的效果显著。

S5.同实施例1中S5；

S6.对三轴加速度计进行补偿，包括步骤S61-S62；

S61.将

前三行构成一综合误差系数矩阵K _3×3，第四行三个元素从左到右依次标记为G _x0、G _y0、G _z0，分别表示x、y、z轴上的零偏误差；

S62.将S5中的优化测量值、K_3×3、G _x0、G _y0、G _z0带入公式（2），得到下式，可根据下式，计算得到MEMS三轴加速度计在x、y、z轴上的补偿测量值g'_x、g'_y、g'_z；

S7.采用椭圆拟合参数对优化磁场强度进行补偿，得到补偿磁场强度。

另外：关于采用卡尔曼滤波器进行滤波，我们参见图5-图7，从图5-图7中可以得出，经过卡尔曼滤波后，MEMS三轴加速度计的输出数据噪声标准差由0.0053g减少为0.0014g，输出数据噪声减小了74%，满足标定实验对输出数据噪声的要求。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (2)

1.一种电子罗盘传感器误差及安装误差快速标定与补偿方法，其特征在于：包括以下步骤；

S1.选择一电子罗盘，所述电子罗盘包括MEMS三轴加速度计和磁传感器；

通过六位置标定法，得到MEMS三轴加速度计在每个位置处x、y、z轴上的理论值和初始测量值；

罗盘沿x、y轴形成的平面旋转一周，由磁传感器得到x、y轴上多个样本点的初始磁场强度；

S2.将初始测量值和初始磁场强度值分别送入卡尔曼滤波器中处理，得到MEMS三轴加速度计在每个位置处x、y、z轴上的优化测量值、和x、y轴上多个样本点的优化磁场强度；

S3.求解MEMS三轴加速度计的误差补偿参数矩阵，包括步骤S31-S32；

S31.建立MEMS三轴加速度计的误差补偿参数矩阵求取模型；

（1）

式中，

和

分别为三轴加速度计在第i个位置的理论值和第j个位置的优化测量值；

，g _x 、g _y 、g _z 分别为MEMS三轴加速度计在x、y、z轴上的理论值；

，G _x 、G _y 、G _z 分别为MEMS三轴加速度计在x、y、z轴上的优化测量值；

S32.将MEMS三轴加速度计在六位置的理论值和优化测量值带入上式（1），求解得到误差补偿参数矩阵A _4×3；

S4.在xy平面上的设置椭圆方程，对样本点采用最小二乘椭圆拟合算法，求解椭圆参数A，B，C，D和E，进一步得到椭圆拟合参数并保存，所述椭圆拟合参数包括椭圆圆心（x0，y0）、长轴a，短轴b和椭圆旋转角θ；

S5.将S1中电子罗盘置于实测场景中，按步骤S1、S2得到实测场景下，MEMS三轴加速度计在每个位置处x、y、z轴上的优化测量值、和x、y轴上多个样本点的优化磁场强度；

S6.对三轴加速度计进行补偿，包括步骤S61-S62；

S61.将A _4×3前三行构成一综合误差系数矩阵K _3×3，第四行三个元素从左到右依次标记为G _x0、G _y0、G _z0，分别表示x、y、z轴上的零偏误差；

S62.将S5中的优化测量值、K_3×3、G _x0、G _y0、G _z0带入下式，计算得到MEMS三轴加速度计在x、y、z轴上的补偿测量值g'_x、g'_y、g'_z；

=K_3×3

（2）

S7.采用椭圆拟合参数对优化磁场强度进行补偿，得到补偿磁场强度。

2.根据权利要求1所述的电子罗盘传感器误差及安装误差快速标定与补偿方法，其特征在于：S1中，所述六位置标定法具体为；

S11将电子罗盘安装在三轴无磁转台上，使电子罗盘各敏感轴方向与三轴无磁转台一致，调整三轴无磁转台处于水平状态；

S12接通电子罗盘电源，预热3分钟后，开始对MEMS三轴加速度计进行标定；

S13调整三轴无磁转台，使MEMS三轴加速度计位于六位置中的第一个位置；

S14数据采集；静止30秒，在静止状态下采集MEMS三轴加速度计的三轴输出数据，数据采集持续30秒；

S15调整三轴无磁转台，使MEMS三轴加速度计分别位于六位置中的第2-6个位置，在每个位置按照步骤S14进行数据采集。

Images