电子罗盘自适应方位角矫正方法与流程

本发明涉及一种电子罗盘校正方法，尤其涉及一种电子罗盘自适应方位角矫正方法。

背景技术：

1、电子罗盘也称为数字指南针，其原理是基于地球磁场在罗盘三轴载体坐标系下磁场强度变化来实现对罗盘方位角进行解算的，当三轴磁场强度值发生变化，方位角也随即发生变化。地球磁场均匀且稳定，强度仅大约50000nt左右，十分容易受到罗盘安装载体附近的铁磁物质干扰，从而影响罗盘方位角精度。罗盘安装要求里明确对安装位置进行了指导说明，尽可能远离铁磁性物质、电机、大电流线缆等可能干扰地球磁场的环境，但实际工程环境条件几乎不可能达到无磁干扰环境，因此工程应用时务必要对磁干扰进行剔除，以提高罗盘输出方位角精度。传统的环境磁场校准方法，一般基于三轴传感器在安装位置附件磁场环境下按照一定运行轨迹所采集到的磁场数据，依据无磁干扰环境下理论上最优磁场数据进行反向拟合求解补偿因子，例如椭圆校准、椭球拟合校准。

2、以椭球拟合校准为例，该方法具有运算量小、实时性高、实用性强等特点。当无干扰时罗盘在原地旋转一圈其在水平面的磁场数据应该是一个标准圆，但由于磁场干扰的引入，导致最终测得磁场数据为一个椭圆，故可利用椭圆的标准方程拟合逼近求解变换因子，将椭圆变换回圆，从而实现对干扰磁场的校准。其校准流程：先利用倾角补偿算法补偿罗盘在水平面内的二维磁场数据，在对二维磁场数据进行外界干扰磁场补偿，从而解算得到正确的方位角。但其在实施校准时需要将罗盘完成一次水平旋转，采集实时磁场数据。在一般定向应用中，罗盘工作时会随载体一起旋转并实时输出载体方位角变化。当安装载体是铁磁性物质时，此时的铁磁干扰已经影响了地球磁场分布，为了达到较好的校准效果，理论上可以将罗盘和载体一起旋转来避免。但当安装载体巨大且无法移动时，直接单独旋转罗盘进行校准会引入误差，不能达到理想的校准效果。其次，载具上使用罗盘除了自身载体干扰外，还有使用过程中其它突发铁磁干扰，如车载应用里载具周围短期出现的铁磁干扰。传统的环境磁场校准方法无法对这类干扰进行有效识别和剔除，也就是说，在面临短暂出现的铁磁干扰并不会做出判断，当突发磁干扰发生时，罗盘处于失效状态，限制了罗盘应用。

技术实现思路

1、本发明的目的就在于提供一种解决上述问题，既能实现对突发磁干扰进行识别并实现自动干扰剔除，也能不依靠同时移动罗盘和安装载体的校准方式来提升校准精度，降低环境磁场校准时对安装载体移动的要求，达到对罗盘输出方位角矫正的，电子罗盘自适应方位角矫正方法。

2、为了实现上述目的，本发明采用的技术方案是这样的：一种电子罗盘自适应方位角矫正方法，包括以下步骤：

3、（1）电子罗盘置于校准地，对电子罗盘的三轴磁传感器和三轴加速度计进行温度校准，得到三轴磁传感器和三轴加速度计对应的温度校准参数矩阵 t jm和 t jg；

4、（2）对三轴磁传感器和三轴加速度计进行正交性校准，得到三轴磁传感器对应的正交误差矩阵 a pm、零偏误差矩阵 b pm，三轴加速度计对应的正交误差矩阵 a pg、零偏误差矩阵 b pg；

5、（3）对电子罗盘进行倾角解算以及倾斜补偿校准，得到俯仰角 η、横滚角 γ、倾斜补偿后的x轴磁场值 m ix、倾斜补偿后的y轴磁场值 m iy；

6、（4）在校准地搭建一标准磁场环境，完成参数计算与记录，包括步骤（41）~（43）；

7、（41）在标准磁场环境下，用椭圆校准方法对校准地的环境磁场校准，并构造电子罗盘的方位角计算式；

8、                               （1），

9、式中， α为方位角， m ’ ix、 m ’ iy分别为旋转偏置角 θ后的x轴磁场值和y轴磁场值， b cx为椭圆校准后x轴偏移量、 b cy为椭圆校准后y轴偏移量、 m cx为环境磁场补偿后的x轴磁场、 m cy为环境磁场补偿后的y轴磁场；

10、（42）查询校准地的地磁偏角 φ 0、根据地理北极和 φ 0确定校准地的地球磁北方向m0，使电子罗盘北向指向m0，完成校准地的磁北锁定；

11、（43）获取此时环境磁场补偿后的x轴磁场 m cx0，环境磁场补偿后的y轴磁场 m cy0、俯仰角 η0、横滚角 γ0、正交性校准后的三轴磁场值 m pxyz0，代入方位角计算式解算 α，解算结果标记为 α0；

12、（5）在应用场所对电子罗盘进行自适应校准，包括步骤（51）~（56）；

13、（51）确定电子罗盘在应用场所的安装位置，查询安装位置的地磁偏角φ1，与φ0比对，当差值≤0.5°时，直接进入下一步，否则在安装位置搭建一标准磁场环境，重复步骤（4）更新 α0后再进入下一步；

14、（52）根据地理北极和 φ1确定安装位置的地球磁北方向m1，使电子罗盘北向指向m1，完成安装位置的磁北锁定；

15、（53）获取此时环境磁场补偿后的x轴磁场 m cx1，环境磁场补偿后的y轴磁场 m cy1、俯仰角 η1、横滚角 γ 1、正交性校准后的三轴磁场值 m pxyz1，代入方位角计算式解算α，解算结果标记为θ1；

16、（54）根据下式进行干扰磁场存在检测；

17、若 |m pz1－ m pz0 |≤δ m pz，安装位置无磁场干扰，方位角为θ1，无需矫正；

18、若 |m pz1－ m pz0|＞δ m pz，安装位置存在磁场干扰，进入步骤（55）；

19、式中， m pz1为 m pxyz1中的z轴磁场值， m pz0为 m pxyz0中的z轴磁场值，δ m pz为z轴磁场变化阈值，通过实验测得；

20、（55）在安装位置，旋转电子罗盘至任一位置2，按步骤（53）方法解算此时的 α，解算结果标记为θ2，计算自适应方位角ε2，ε2=θ1-θ2；

21、（56）根据下式对自适应方位角进行补偿，得到补偿后方位角α’；

22、                        （2），

23、                     （3），

24、公式（2）中，k1为方位角补偿置信因子，为补偿方位角；

25、公式（3）中， ω为标准磁场与干扰磁场矢量夹角，为位置2处干扰磁场下方位角，k2为补偿比例系数，为补偿方位角修正量。

26、作为优选，k1根据下式得到；

27、                                                 （4）。

28、作为优选， ω、、根据下式得到；

29、                        （5），

30、                      （6），

31、                       （7），

32、（8），

33、公式（6）中， m cx2、 m cy2分别为位置2处，环境磁场补偿后的x轴磁场 、环境磁场补偿后的y轴磁场；

34、公式（8）中，为俯仰角变化量，为横滚角变化量， m ’ px0、 m ’ py0、 m ’ pz0分别为x轴、y轴、z轴的位置变化计算值，∆ α=θ1 -α0为方位角变化量。

35、作为优选，步骤（1）中， t jg和 t jm根据下式得到：

36、                       （9），

37、式中， m xyz为三轴磁传感器测量值， g xyz为三轴加速度测量值； m txyz为温度校准后磁场值， g txyz为温度校准后加速度值。

38、作为优选，步骤（2）中， a pm、 b pm、 a pg、 b pg满足下式：

39、                       （10），

40、式中， m txyz为温度校准后磁场值， m pxyz为正交校准后磁场值， g txyz为温度校准后加速度值， g pxyz为正交校准后加速度值。

41、作为优选，步骤（3）中， η、 γ、 m ix、 m iy通过下式得到；

42、                        （11），

43、                   （12），

44、式中，g为重力加速度， g px、 g py、 g pz分别为正交校准后加速度值 g pxyz中的x轴加速度、y轴加速度、z轴加速度； m px、 m py、 m pz分别为正交校准后磁场值 m pxyz中的x轴磁场值、y轴磁场值、z轴磁场值。

45、作为优选，步骤（41）中，用椭圆校准方法得到椭圆校准参数，包括椭圆圆心坐标（ x0， y0）、长短轴（ a， b）、偏置角 θ，并根据下式计算得到 m ’ ix、 m ’ iy、 b cx、 b cy；

46、                （13），

47、                  （14）。

48、本发明的原理分析如下：

49、原理分析（1）：标准磁场环境下磁场分布与角度关系分析：

50、在干净地磁环境下，经过倾角补偿后的电子罗盘xy平面磁场分布与罗盘角度关系如图2所示，图2中，n为地理北极、m为地球磁北(也称标准磁场方向)、e为地理东，圆圈中心的长方形区域指电子罗盘，长方形区域的箭头指向，为电子罗盘的y轴方向（也称为罗盘北），与罗盘北垂直且标记 m x的方向，为电子罗盘的x轴方向。 m x、 m y为本发明 m xyz中x轴与y轴的测量值，也是地磁磁场在电子罗盘水平面的磁场大小，φ为地理北极与地球磁北的夹角，该值一般为固定常数，当坐标位置发生较大改变时会改变，ε为地球磁北与罗盘北向的夹角。

51、原理分析（2）：干扰磁场环境下的磁场分布与角度关系分析：

52、干扰物会产生干扰磁场，受干扰磁场影响，磁场在xy平面的分布已经发生改变，此时罗盘角度关系与xy平面磁场分布如图3所示。图中，i为干扰磁场矢量，ix与iy分别为干扰磁场经过倾角补偿后在xy平面内x轴与y轴的磁场分量，其中 β1为干扰磁场作用下的方位角，在图3中为罗盘北与i之间的夹角。 ε1为标准磁场作用下的方位角，在图3中为m与罗盘北的夹角。mtotal为标准磁场与干扰磁场构成的合磁场，mtotal作用下的罗盘方位角为θ1，其值满足关系式：arctanθ1=(mx+ix)/(my+iy)。图中存在以下两个固定角度关系：

53、关系1：由于只讨论静磁干扰，故标准磁场与干扰磁场的矢量和夹角ω大小不变，其值满足关系ω=ε1+β1。图3中，ω为m方向和i方向之间的夹角。

54、关系2：因为只讨论静磁干扰，故标准磁场与合磁场方向不会改变，两者磁场夹角δ保持不变，即δ=ε1+θ1的值保持不变。

55、原理分析（3）：干扰磁场下罗盘旋转后磁场分布与角度关系分析：

56、假设在图3中得到的标准磁场方位角为ε1，合磁场方位角为θ1。旋转到新位置后，在图4中合磁场方位角θ2，可由关系式arctanθ2=(mx+ix)/(my+iy)计算得到。由于图4在图3基础上，只旋转了电子罗盘，标准磁场和干扰磁场未改变，故上述关系2中δ不变，在图4中，δ=ε2+θ2，就可以知道新位置处的标准磁场方位角为ε2=δ-θ2=ε1+θ1-θ2。

57、原理分析（4）：干扰磁场下罗盘旋转后方位角矫正补偿分析：

58、在图5中，以罗盘中心为原点，若已知标准磁场m的矢量坐标为（mx，my），合磁场mtotal的矢量坐标为（m tx，mty）,则可得到干扰磁场i的矢量坐标为（ix，iy），即（mtx-mx，mty-my）。根据平面内矢量夹角关系，矢量a与矢量b的夹角∠aob应满足cos∠aob=（a×b）/(|a|×|b|)，则可以求得各磁场之间的夹角。假设图3中标准磁场方位角为ε1，干扰磁场下的方位角为 β1，两个磁场的夹角和根据关系1可知，二者夹角和不变并满足ω=ε1+β1。若旋转后新位置处的干扰磁场方位角为β2，则根据关系1，可以得到标准磁场下的方位角ε2=ε1+β1-β2=ω-β2。则可以通过这个关系，同样求得一个方位角ε2。该值与原理分析（3）中通过关系2得到的ε2表示的是同一个角度，但两者计算途径不同，故可以用这2个值进行矫正补偿。

59、原理分析（5）：干扰磁场检测原理分析

60、如图6所示，在罗盘静止无干扰时，电子罗盘在z轴方向的测量数值是地球磁场在z轴方向的投影分量，记为mz，此时，即使在平面内旋转罗盘，由于地磁线与z轴的夹角不变，mz值也不会变化。当干扰物出现时，干扰物在在z轴方向也会产生投影分量记为iz，故电子罗盘在z轴方向的测量数值是地球磁场与干扰磁场在z轴方向投影分量的矢量和ztotal。干扰物固定不变的前提条件下，ztotal的值基本保持不变，当干扰物移除时，该值恢复到mz。由此通过软件设置一个z轴磁场变化阈值即可检测干扰源是否存在。

61、与现有技术相比，本发明的优点在于：针对电子罗盘常规校准方法在用户端应用受限制、校准效果不理想、以及无法自动识别干扰且干扰存在时罗盘输出失效的情况，在现有传统的校准方法基础上整合了自适应校准算法，既能实现对磁干扰进行识别并实现自动干扰剔除，也能不依靠同时移动罗盘和安装载体的校准方式来提升校准精度，降低环境磁场校准时对安装载体移动的要求，达到对罗盘输出方位角矫正的目的，增加了罗盘在应用端的适应能力。