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

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


背景技术：

2.电子罗盘中，会用到mems三轴加速度计和磁传感器。
3.关于mems三轴加速度计：目前mems三轴加速度计存在较大的测量噪声，可达数百mg，严重影响后续的数据处理。这些问题主要是因由以下几个方面产生的：1、未处理传感器输出数据，输出数据存在较大噪声，严重影响测量精度。
4.2、mems三轴加速度计自身存在零偏误差、刻度因子误差、非线性误差、三轴非正交误差以及安装误差，严重影响电子罗盘的倾斜角测量和补偿精度。
5.3、受到外界干扰磁场的影响，磁测数据与真实值偏差较大。
6.综上，mems三轴加速度计的误差主要来源于自身的零偏误差、刻度因子误差、非线性误差、以及安装使用时的安装误差等，如何对这些误差进行标定和补偿，是亟需解决的问题。
7.关于磁传感器：外界环境干扰磁场会造成磁场测量误差，故磁传感器测得的数据与实际数据也会存在偏差。
8.由于加速度和磁场强度均存在偏差，我们在用到二者之一或二者结合进行后续数据处理时，会导致数据偏差较大。


技术实现要素：

9.本发明的目的就在于提供一种解决上述问题，能克服电、三轴加速度计自身的零偏误差、刻度因子误差、非线性误差、以及安装使用时的安装误差的，电子罗盘传感器误差及安装误差快速标定与补偿方法。
10.为了实现上述目的，本发明采用的技术方案是这样的：一种电子罗盘传感器误差及安装误差快速标定与补偿方法，包括以下步骤；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
、gy、gz分别为mems三轴加速度计在x、y、z轴上的理论值；，g
x
、gy、gz分别为mems三轴加速度计在x、y、z轴上的优化测量值；s32.将mems三轴加速度计在六位置的理论值和优化测量值带入上式（1），求解得到误差补偿参数矩阵a4×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.将a4×3前三行构成一综合误差系数矩阵k3×3，第四行三个元素从左到右依次标记为g
x0
、g
y0
、g
z0
，分别表示x、y、z轴上的零偏误差；s62.将s5中的优化测量值、k3×3、g
x0
、g
y0
、g
z0
带入下式，计算得到mems三轴加速度计在x、y、z轴上的补偿测量值g'
x
、g'y、g'z；=k3×3ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（2）s7.采用椭圆拟合参数对优化磁场强度进行补偿，得到补偿磁场强度。
11.作为优选：s1中，所述六位置标定法具体为；s11将电子罗盘安装在三轴无磁转台上，使电子罗盘各敏感轴方向与三轴无磁转台一致，调整三轴无磁转台处于水平状态；s12接通电子罗盘电源，预热3分钟后，开始对mems三轴加速度计进行标定；s13调整三轴无磁转台，使mems三轴加速度计位于六位置中的第一个位置；s14数据采集；静止30秒，在静止状态下采集mems三轴加速度计的三轴输出数据，数据采集持续30秒；s15调整三轴无磁转台，使mems三轴加速度计分别位于六位置中的第2-6个位置，在每个位置按照步骤s14进行数据采集。
12.关于卡尔曼滤波：
卡尔曼滤波算法是工程应用中最为广泛的一种最优估计算法。对于电子罗盘中使用的mems三轴加速度计，其系统状态转移矩阵ak、观测矩阵hk、动态噪声wk、观测噪声vk均可认为是固定值，即ak=a，hk=h，wk=w，vk=v，因此可得到mems三轴加速度计的卡尔曼滤波方程如下：状态向量预测方程为： ；状态向量协方差矩阵预测方程为：；卡尔曼滤波增益方程为：；状态向量更新方程为：；状态向量协方差矩阵更新方程为：；其中，表示根据k-1时刻的三轴状态估计向量预测得到的k时刻的三轴状态预测向量，表示k-1时刻的三轴状态估计向量，表示由k-1时刻到k时刻的三轴状态预测误差协方差，表示k-1时刻的三轴状态估计误差协方差。
13.我们对将数据进行卡尔曼滤波后，能有效减小测量噪声。
14.关于mems三轴加速度计，误差主要来源于自身的零偏误差、刻度因子误差、非线性误差、三轴非正交误差以及安装使用时的安装误差等。由于三轴非正交误差与安装误差产生的影响类似，在进行标定与补偿时，可以将两者统一表示为安装误差。因此，根据mems三轴加速度计的主要误差来源及特点，对某一位置而言，可将mems三轴加速度计的输出误差数学模型表示为：
ꢀꢀꢀꢀꢀꢀ
（3）式中，g
x0
、g
y0
、g
z0
表示mems三轴加速度计在x、y、z轴的零偏值，单位为g；g
x
、gy、gz表示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三轴加速度计二阶非线性误差系数。
15.对于大部分mems三轴加速度计，其二阶非线性误差系数很小，标定与补偿时可忽略不计。因此不考虑mems三轴加速度计二阶非线性误差，可由式（3）得到简化后的mems三轴加速度计的输出误差数学模型为：
ꢀꢀꢀꢀꢀꢀꢀ
（4）为了便于计算误差补偿参数，将式（4）变换为：
ꢀꢀꢀꢀꢀꢀꢀꢀ
（5）记为mems三轴加速度计的综合误差系数矩阵，由于k3×3是3阶满秩方阵，k3×3可逆，则有：（6）当mems三轴加速度计安装状态确定后，不考虑温度变化影响，其综合误差系数矩阵k3×3及零偏误差可视为固定值，因此，式（6）可变换为：
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（7）为便于求解误差补偿参数，由式（7）可得mems三轴加速度计误差补偿数学模型为：
ꢀꢀꢀꢀꢀꢀꢀ
（8）记，表示误差补偿参数矩阵；基于式（8）的模型，我们带入每个位置对应的实测值、理论值，由于实测值、理论值都是确定的，所以算出来的误差补偿参数矩阵a4×3也是定值，后续在测试现场实用本发明产品时，对所有测得的加速度数据，采用误差补偿参数矩阵a4×3进行补偿即可。
16.关于最小二乘椭圆拟合，主要思路是求解出椭圆参数a，b，c，d和e，进一步计算椭圆拟合参数：椭圆圆心（x0，y0）、长轴a、短轴b和椭圆旋转角θ；以实现最终的误差补偿，对椭圆拟合采用最小二乘法，其思想是要使得观测点和估计点距离的平方和达到最小。其中一种实现流程可参见图3，通过以下方式实现：（1）先在所有样本点中随机选取5个样本点，采用最小二乘法求解出椭圆的参数，具体做法为：在二维平面坐标系中，任意位置椭圆可以采用圆锥曲线方程的代数形式表示，即：
；假设为二维平面内x、y方向的n（n≥5）个待拟合的样本点，根据最小二乘原理，应以求目标函数的最小值来确定参数a、b、c、d和e。
17.由极值定理，欲使f的值最小，必有由此可得方程组将所有样本点代入，求解线性方程组可得到a、b、c、d和e的值，然后，根据说明书附图3进行处理；（2）遍历所有样本点，求取各个点到已得到的椭圆之间的距离，如果小于某个阈值，则称该样本点为匹配点，在数组c记录该样本点的编号，遍历完毕后，求取对于该拟合椭圆的匹配点的个数n，这里阈值根据实际情况自定；（3）匹配点总个数 n赋值给匹配点最大值n
max
；（4）再次执行步骤（1）和步骤（2），比较匹配点总个数n与匹配点最大值n
max
，当n》n
max
时，将椭圆参数和记录匹配点编号的数组c 保存下来，分别赋值给数组ie和数组ic；（5）循环执行步骤（4）一定次数，则最后在ie保存了最优椭圆参数，在数组ic保存了在所有样本点中匹配点的编号，也就相应的可以得到不匹配点的编号。
18.考虑到算法的实时性和准确性，对样本点的选取采用随机方法。但是如果考虑所有的可能性，譬如总样本空间有300个点，选取5个不同的样本点一共有种可能，遍历所有样本点在时间上不允许，而且也不必要。要加速计算，只需选择其中的一部分子集，在样本性质并不清楚的情况下，使用随机方法抽取是一个很好的选择。
19.与现有技术相比，本发明的优点在于：本发明不仅能减小数据输出噪声，还能有效地补偿mems加速度计自身零偏、刻度因子、非正交及安装误差，并且对外界的磁场干扰进行补偿。精度较高，操作简单，易于实现，具有重要的工程实用价值。
附图说明
20.图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）为卡尔曼滤波后。
具体实施方式
21.下面将结合附图对本发明作进一步说明。
22.实施例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
、gy、gz分别为mems三轴加速度计在x、y、z轴上的理论值；，g
x
、gy、gz分别为mems三轴加速度计在x、y、z轴上的优化测量值；s32.将mems三轴加速度计在六位置的理论值和优化测量值带入上式（1），求解得到误差补偿参数矩阵a4×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.将a4×3前三行构成一综合误差系数矩阵k3×3，第四行三个元素从左到右依次标记为g
x0
、g
y0
、g
z0
，分别表示x、y、z轴上的零偏误差；s62.将s5中的优化测量值、k3×3、g
x0
、g
y0
、g
z0
带入下式，计算得到mems三轴加速度计在x、y、z轴上的补偿测量值g'
x
、g'y、g'z；=k3×3ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
（2）s7.采用椭圆拟合参数对优化磁场强度进行补偿，得到补偿磁场强度。
23.本实施例中，s1中，所述六位置标定法具体为；s11将电子罗盘安装在三轴无磁转台上，使电子罗盘各敏感轴方向与三轴无磁转台一致，调整三轴无磁转台处于水平状态；s12接通电子罗盘电源，预热3分钟后，开始对mems三轴加速度计进行标定；s13调整三轴无磁转台，使mems三轴加速度计位于六位置中的第一个位置；s14数据采集；静止30秒，在静止状态下采集mems三轴加速度计的三轴输出数据，数据采集持续30秒；s15调整三轴无磁转台，使mems三轴加速度计分别位于六位置中的第2-6个位置，在每个位置按照步骤s14进行数据采集。
24.实施例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
xgygz
1]=[0.93620.12610.06121]；=[g
xgygz
1]=[-1.08380.11500.04451]；依次类推。
[0025]
最终，我们求解出误差补偿参数矩阵a4×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），从仿真结果来看，采用椭圆拟合方法进行校准的效果显著。
[0026]
s5.同实施例1中s5；s6.对三轴加速度计进行补偿，包括步骤s61-s62；
s61.将前三行构成一综合误差系数矩阵 k3×3，第四行三个元素从左到右依次标记为g
x0
、g
y0
、g
z0
，分别表示x、y、z轴上的零偏误差；s62.将s5中的优化测量值、k3×3、g
x0
、g
y0
、g
z0
带入公式（2），得到下式，可根据下式，计算得到mems三轴加速度计在x、y、z轴上的补偿测量值g'
x
、g'y、g'z；s7.采用椭圆拟合参数对优化磁场强度进行补偿，得到补偿磁场强度。
[0027]
另外：关于采用卡尔曼滤波器进行滤波，我们参见图5-图7，从图5-图7中可以得出，经过卡尔曼滤波后，mems三轴加速度计的输出数据噪声标准差由0.0053g减少为0.0014g，输出数据噪声减小了74%，满足标定实验对输出数据噪声的要求。
[0028]
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。