CN104075699B - 三维固态电子罗盘及其传感器的零点和比例系数核正方法 - Google Patents

Abstract

本发明公开了一种三维固态电子罗盘及其传感器的零点和比例系数校正方法，电子罗盘包括三轴地磁传感器电路、三轴加速度传感器电路、通讯接口电路、电源转换电路、MCU处理器电路以及三轴地磁传感器零点与比例系数估计模块和三轴加速度传感器零点与比例系数估计模块，根据三轴地磁传感器电路、三轴加速度传感器电路的检测结果分别估计得到三轴地磁传感器和零点和比例系数比值以及三轴加速度传感器和零点和比例系数比值。本发明，集成了不依赖于校正设备的加速度传感器零点与比例系数估计模块以及地磁传感器零点与比例系数估计模块，方便多旋翼飞行器在野外进行加速度传感器与地磁传感器的校正，提高了电子罗盘俯仰角、横滚角以及航向角的测量精度。

Description

三维固态电子罗盘及其传感器的零点和比例系数核正方法

技术领域

本发明涉及多旋翼无人飞行器，具体涉及用于多旋翼无人飞行器的三维固态电子罗盘及其传感器的零点和比例系数校正方法。

背景技术

近年来，具备可垂直起降、悬停以及低空低速飞行能力的多旋翼无人飞行器已经成为应用与研究的一种主流无人飞行器，此类无人飞行器在侦察、搜索救援、交通监控、灾情监测、航拍、地理测绘、遥测遥感等领域有着非常广泛的应用前景。多旋翼无人飞行器的横、纵向速度控制依赖于航向角测量信息对GPS速度的分解。目前，绝大部分无人飞行器都是通过三维电子罗盘对地磁的测量获得航向角信息的。因此，三维电子罗盘在多旋翼无人飞行器控制系统中占有重要的地位。

由于地磁场是微弱磁场，传感器制造工艺、硬磁干扰、信号调理电路、模数转换器所引入的零点偏差和比例系数误差都会使磁传感器测量值偏离地磁真值，影响航向角测量精度，因此需要对每个地磁传感器的零点和比例系数进行校正。现有技术对于三维电子罗盘的校正主要有几下几种：

(1)中国发明专利CN102197277A(专利申请号200980141990.8)提出了一种用于确定电子罗盘零点偏移的方法：首先借助于三轴磁传感器求得所述电子罗盘的第一坐标系中的第一磁场强度，然后由所述第一磁场强度计算平行于地球表面的第二坐标系中的经倾斜补偿的第二磁场强度，最后使试探函数匹配于经补偿的第二磁场强度进而确定出零点偏移；中国发明专利CN102288170B(专利申请号201110198784.X)提出了一种水下航行器内电子罗盘的校正方法，通过搜索的方法确定使校准轨迹与参考轨迹之间误差最小的硬磁干扰参数。这两种方法对水平面上两个地磁分量的零点进行了校正，无法对3个地磁传感器各自的零点偏差和比例系数误差进行完全校正。

(2)中国发明专利CN101438131B(专利申请号200780016572.7)提出了一种校准方法及电子罗盘，在三维空间中利用地磁传感器的至少4个输出作出至少两个三角形，求出上述三角形的各自外接圆，再求出通过各个外接圆中线法线矢量的交点作为基准点(磁传感器零点)；中国发明专利CN102460069A(专利申请号201080030411.5)提出了一种用于便携式设备中的电子罗盘的校准技术，用于计算和去除来自3轴磁传感器输出的干扰局部偏移磁场的影响以获得地磁场精确读数的技术，所述三维校准器基于由传感器获得的限定大致球面的一组磁场测量计算器偏移场(零点偏差矢量)；中国发明专利CN102364354A(申请号201110166034.4)提出了一种磁性传感器装置和电子罗盘设备，将数字磁数据输出到数据处理装置，该数据处理装置根据由多个数字磁数据形成的统计学群体来导出数字磁数据的偏移量(零点偏差)。上述三种方法都只对三个磁传感器的零点偏差进行了校正，无法校正磁传感器的比例系数误差。

(3)中国发明专利CN101241009B(申请号200710304535.8)提出了一种磁电子罗盘误差补偿方法，以精确知道自身方位角的无磁转台为校正工具，基于变分圆分布和周期性假设的误差补偿算法，建立测量角误差的周期函数，以补偿周围铁磁物质等因素所引起的测量误差；中国发明专利CN102297687B(申请号201110124791.5)提出了一种电子罗盘的标定方法，以电子罗盘测量值与转台转动角度为训练样本，采用改进的自适应差分进化算法对神经网络的权值进行优化，从而得到较为精确的误差模型来补偿电子罗盘测量值；中国发明专利CN102818564A(申请号201210273633.0)提出了一种三维电子罗盘的标定方法，使用一个三维无磁旋转平台，利用三维电子罗盘内置的磁传感器和倾角传感器在不同空间方向的输出值变化对三维电子罗盘进行标定；中国发明专利CN103175520A(申请号201310090322.5)提出了一种电子罗盘校正方法、装置和设备，将多个传感器安装于几何体(正多面体或球体)框架的面上并进行三维球面旋转，对采集得到的磁力数据通过椭球处理得到校正参数的方法，以校正三维磁传感器的零点与比例系数。上述四种方法相比前述方法采用了更准确的传感器误差模型，可以取得更好的校正效果，但需要额外的校正设备，如转台或几何体框架。由于地磁传感器的误差参数易受温度、外围干扰磁场变化的影响而需要频繁校正，这些依赖校正设备的方法显然难以满足实际应用需要。

(4)中国发明专利CN1624425B(申请号200410001581.7)提出了一种用在电子罗盘内的自动校准方法，使用接收到的地磁数据找到该地磁传感器的单独的轴的传感器信号的最大和最小值以计算偏移和标度值；中国发明专利CN102589537A(申请号201210055586.2)提出了一种有磁环境下无人机的电子罗盘校正方法，磁力计获得电子罗盘x、y、z三轴的相对磁通量，滤波处理后送入极值获取单元获得三轴磁通量的极大值和极小值进行椭球拟合校正。这两种方法在理论上都能较好地校正三轴磁传感器的零点偏差和比例系数误差，而在实际应用中所述磁通量极大值与极小值难以精确得到，因此校正效果受限。

(5)中国发明专利CN102252689A(申请号201010176346.9)提出了一种电子罗盘全方位校准方法，利用IGRF模型计算地磁场强度，结合软硬磁修正的递归最小二乘估计器，计算软磁干扰的上三角矩阵与硬磁干扰向量，这种方法需要IGRF模型的辅助估计9个参数，计算量和存储量都较大，不适合单片机应用。

此外，除了地磁传感器，电子罗盘用于测量倾角的加速度计的零点和比例系数也易受温度变化影响而引入误差，需要经常校正，而上述各电子罗盘相关的专利都没有涉及到。对于加速度计的校正，中国发明专利CN101629970B(申请号200910160117.5)提出了一种用于加速度传感器的误差校正方法和误差校正装置，通过向振动质量块施压，测量振动质量块偏移引起的第一电流和第二电流，求得校正值；中国发明专利CN101852817B(申请号201010164389.5)提出了一种正交双高精度加速度计的标定方法，利用多面体棱镜、光栅分度头、光电自准值仪获得两个加速度计在0、90、180、120度4个角度位置上的采样输出，并采用正交双表法获得误差模型的各个参数，完成标定；中国发明专利CN101968496A(申请号201010221061.2)提出了一种加速度传感器的全自动校正系统，基于该系统测量并计算得到各标准g值下不同温度时加速度传感器对应的待校正g值与标准g值的偏差值，求出偏差值与温度的对应曲线，对曲线进行多项式拟合求出校正系数；中国发明专利CN103090882A(申请号201310006526.6)提出了一种加速度计实现高精度倾角测量中的灵敏轴非正交补偿校正方法，首先在垂直平面内设置四个两两垂直的校正位置点，通过夹角函数，在四个象限内，分别前后计算两灵敏轴对应于校正点的夹角作为非正交值；中国发明专利CN103399177A(申请号201310356585.6)提出了一种加速度传感器实现倾角测量应用中的灵敏轴参数多点方程式校正标定方法，采用多点标定和方程组计算方法来精密校正标定灵敏轴参数，从而得到精确的零点和灵敏度值。上述五种方法都依赖专用的校正设备，难以满足野外应用的需要。

由此可见，现有的三维电子罗盘及其校正方法存在依赖专用的校正设备，校正效果差，难以满足野外应用的需要。

发明内容

本发明所要解决的问题是三维电子罗盘的校正依赖专用的校正设备，校正效果差，难以满足野外应用的需要的问题。

为了解决上述技术问题，本发明所采用的技术方案是提供一种用于多旋翼无人飞行器的三维固态电子罗盘，包括三轴地磁传感器电路、三轴加速度传感器电路、通讯接口电路、电源转换电路和MCU处理器电路，三轴地磁传感器电路与MCU处理器电路通过I²C总线连接，用于检测地磁场分量；三轴加速度传感器电路与MCU处理器电路的AD采样输入管脚连接，用于检测重力分量；通讯接口电路与MCU处理器电路通过第一、第二UART通讯接口连接，实现TTL-RS232电平转换与USB-UART协议转换，为上位机提供RS232和USB通讯接口；电源转换电路将3.5V-5.5V的输入电压转换成+3.3V的稳定电压，用于为三轴地磁传感器电路、三轴加速度传感器电路、通讯接口电路和MCU处理器电路供电，其特征在于，还包括：

三轴地磁传感器零点与比例系数估计模块，根据三轴地磁传感器电路的检测结果估计三轴地磁传感器的零点b_x,h、b_y,h、b_z,h和比例系数比值r_x,h/r_y,h、r_x,h/r_z,h；

三轴加速度传感器零点与比例系数估计模块，根据三轴加速度传感器电路的检测结果估计三轴加速度传感器的零点b_x,g、b_y,g、b_z,g和比例系数比值r_x,g/r_y,g、r_x,g/r_z,g。

在上述三维固态电子罗盘中，三轴地磁传感器电路包括三轴地磁传感器芯片U1，第一电容C1和第二电容器C2并联后，一端接三轴地磁传感器芯片U1的S1引脚和+3.3V电压，另一端接地；第三电容C3一端接三轴地磁传感器芯片U1的C1引脚，另一端接地；第四电容C4的两端分别连接三轴地磁传感器芯片U1的SETC引脚和SETP引脚。

在上述三维固态电子罗盘中，三轴地磁传感器芯片U1型号为HMC5883L。

在上述三维固态电子罗盘中，三轴加速度传感器电路包括三轴加速度传感器芯片U2，三轴加速度传感器芯片U2采用意法半导体公司生产的带模拟电压信号输出的MEMS加速度传感芯片LIS3L06AL，三轴加速度传感器芯片U2的VDD引脚接+3.3V电压和第五电容C5和第六电容C6，第五电容C5和第六电容C6的另一端分别接地；三轴加速度传感器芯片U2的GND引脚接地，并通过第七电容C7连接到三轴加速度传感器芯片U2的AZ引脚；三轴加速度传感器芯片U2的AY和AX引脚分别通过第八、第九电容接地，第七、第八和第九电容将三轴加速度计的输出电压低通滤波到10Hz；三轴加速度传感器芯片U2的FS引脚通过第一电阻R1连接到+3.3V电压，将三轴加速度传感器芯片U2的量程选择管脚FS上拉到+3.3V，使得三轴加速度传感器芯片U2工作在±6g量程。

在上述三维固态电子罗盘中，通讯接口电路包括RS2329针COM接口J1、RS-232收发器芯片U3、A型USB接口J2和USB-UART桥接芯片U4；

RS-232收发器芯片U3的C1+和C1-引脚之间设有第十电容，C2+和C2-引脚之间设有第十二电容，VCC和GND引脚之间设有第十四电容，V+引脚通过第十一电容接地，V-引脚通过第十三电容接地，GND引脚接地，VCC引脚接+3.3V电压，T2OUT引脚和R2IN引脚分别连接RS2329针COM接口J1的第2和第3端子；

USB-UART桥接芯片U4通过USB-UART协议转换电路与A型USB接口J2连接。

本发明还提供了一种三维固态电子罗盘的传感器零点和比例系数核正方法，包括以下步骤：

步骤10：初始化三轴地磁传感器和三轴加速度传感器的零点与比例系数，其中：

三轴地磁传感器的初始化零点与比例系数最小二乘估计器初始化为h₀＝[h_x0h_y0h_z0]^T、P_h＝1000×I₅和c_h＝[01010]^T，h_x0、h_y0、h_z0为进入校正状态时三轴地磁传感器的输出；P_h为三轴地磁传感器的估计误差协方差阵；I₅为三轴地磁传感器的5维单位阵；c_h为要估计的三轴地磁传感器的参数向量；

三轴加速度传感器的零点与比例系数最小二乘估计器初始化为g₀＝[g_x0g_y0g_z0]^T，P_g＝1000×I₅，c_g＝[01010]^T，g_x0、g_y0、g_z0为进入校正状态时三轴加速度传感器的输出；P_g为三轴加速度传感器的估计误差协方差阵；I₅为三轴加速度传感器的5维单位阵；c_g为要估计的三轴加速度传感器的参数向量；

步骤20：分别将三轴地磁传感器和三轴加速度传感器绕三轴旋转，并采集三轴地磁传感器的当前数据h＝[h_xh_yh_z]^T和三轴加速度传感器当前数据g＝[g_xg_yg_z]^T；

步骤30：分别构建y_h、a_h和y_g、a_g，其中：

$y_h=h_x^2-h_{x0}^2,a_h=\left[\begin{array}{ccccc}2(h_x-h_{x0})& h_{y0}^2-h_y^2& 2(h_y-h_{y0})& h_{z0}^2-h_z^0& 2(h_z-h_{z0})\end{array}\right];$

$y_g=g_x^2-g_{x0}^2,a_g=\left[\begin{array}{ccccc}2(g_x-g_{x0})& g_{y0}^2-g_y^2& 2(g_y-g_{y0})& g_{z0}^2-g_z^0& 2(g_z-g_{z0})\end{array}\right];$

步骤40：计算三轴地磁传感器的增益估计参数c_h＝c_h+K_h(y_h-a_hc_h)，并更新其误差协方差阵P_h＝(I₅-K_ha_h)P_h；计算三轴加速度传感器的增益估计参数c_g＝c_g+K_g(y_g-a_gc_g)，并更新其误差协方差阵P_g＝(I₅-K_ga_g)P_g；

步骤50：向上位机发送三轴地磁传感器的估计参数c_h和三轴加速度传感器的估计参数c_g，并通过上位机观察c_h和c_g是否收敛；

如果c_h和c_g未收敛，则返回步骤20继续估计c_h和c_g；

如果c_h收敛，则结束地磁传感器校正，保存估计的c_h和三轴地磁传感器的零点b_x,h、b_y,h、b_z,h及比例系数比值r_x,h/r_y,h、r_x,h/r_z,h，b_x,h＝c_h(1)，b_y,h＝c_h(3)/c_h(2)，b_z,h＝c_h(5)/c_h(4)， c_h(1)、c_h(2)、c_h(3)、c_h(4)、c_h(5)为c_h的5个分量；

如果c_g收敛，则结束加速度计校正，保存估计的c_g和三轴加速度传感器的零点b_x,g、b_y,g、b_z,g及比例系数比值r_x,g/r_y,g、r_x,g/r_z,g，b_x,g＝c_g(1)，b_y,g＝c_g(3)/c_g(2)，b_z,g＝c_g(5)/c_g(4)， c_g(1)、c_g(2)、c_g(3)、c_g(4)、c_g(5)为c_g的5个分量。

在上述方法中，上位机通过RS232串口或USB串口发送三轴地磁传感器和三轴加速度传感器的校正开始及结束命令。

在上述方法中，利用重力场、地磁场与姿态角的关系计算三个姿态角，具体计算公式为：

$\mathrm{\θ}=-\arctan \left(\frac{g_x-b_{x,g}}{\sqrt{c_g\left(2\right){(g_y-b_{y,g})}^2+c_g\left(4\right){(g_z-b_{z,g})}^2}}\right);$

${\hat{X}}_H=(h_x-b_{x,h})\cos \mathrm{\θ}+\sqrt{c_h\left(2\right)}(h_y-b_{y,h})\sin \mathrm{\θ}\sin \mathrm{\φ}+\sqrt{c_h\left(4\right)}(h_z-b_{z,h})\sin \mathrm{\θ}\cos \mathrm{\φ};$

${\hat{Y}}_H=\sqrt{c_h\left(2\right)}(h_y-b_{y,h})\cos \mathrm{\φ}-\sqrt{c_h\left(4\right)}(h_z-b_{z,h})\sin \mathrm{\φ};$

其中：θ、φ、ψ分别为俯仰角、横滚角、航向角。

本发明，集成了不依赖于校正设备的加速度传感器零点与比例系数估计模块和不依赖于校正设备的地磁传感器零点与比例系数估计模块，方便多旋翼飞行器在野外进行加速度传感器与地磁传感器的校正，提高了电子罗盘俯仰角、横滚角、以及航向角的测量精度。

附图说明

图1为本发明的结构示意图；

图2为本发明三轴地磁传感器电路101的电路图；

图3为本发明三轴加速度传感器电路201的电路图；

图4为本发明通讯接口电路301的电路图；

图5为本发明电源转换电路401的电路图；

图6为本发明MCU处理器电路501的电路图；

图7为本发明MCU处理器UART1接收中断处理函数程序流程图；

图8为本发明MCU处理器UART2接收中断处理函数程序流程图；

图9为本发明MCU处理器系统定时器中断处理函数程序流程图。

具体实施方式

本发明提供了一种用于多旋翼无人飞行器的三维固态电子罗盘，可以不依赖外部校正设备实现地磁传感器与加速度传感器零点与比例系数的校正，使得电子罗盘的航向测量精度满足多旋翼无人飞行器自主飞行的需要。下面结合说明书附图对本发明做出详细的说明。

如图1所示，本发明提供的用于多旋翼无人飞行器的三维固态电子罗盘包括：三轴地磁传感器电路101、三轴加速度传感器电路201、通讯接口电路301、电源转换电路401、MCU处理器电路501以及三轴地磁传感器零点与比例系数估计模块和三轴加速度传感器零点与比例系数估计模块。

三轴地磁传感器电路101与MCU处理器电路501通过I²C总线连接，用于检测地磁场分量；三轴加速度传感器电路201的输出端与MCU处理器电路501的AD采样输入管脚连接，用于检测重力分量；通讯接口电路301与MCU处理器电路501通过第一、第二UART通讯接口连接，实现TTL-RS232电平转换与USB-UART协议转换，为上位机提供RS232与USB通讯接口；电源转换电路401将3.5V-5.5V的输入电压转换成+3.3V的稳定电压，用于为三轴地磁传感器电路101、三轴加速度传感器电路201、通讯接口电路301，MCU处理器电路501的供电。三轴地磁传感器零点与比例系数估计模块，根据三轴地磁传感器电路的检测结果估计三轴地磁传感器的零点b_x,h、b_y,h、b_z,h和比例系数比值r_x,h/r_y,h、r_x,h/r_z,h；三轴加速度传感器零点与比例系数估计模块，根据三轴加速度传感器电路的检测结果估计三轴加速度传感器的零点b_x,g、b_y,g、b_z,g和比例系数比值r_x,g/r_y,g、r_x,g/r_z,g。

图2是本发明中三轴地磁传感器电路101的电路图。如图2所示，三轴地磁传感器电路101由三轴地磁传感器芯片U1以及四个电容器C1、C2、C3和C4组成，三轴地磁传感器芯片U1采用霍尼韦尔公司生产的带I²C数字输出的磁阻型弱磁传感芯片HMC5883L，第一电容C1和第二电容器C2并联后，一端接三轴地磁传感器芯片U1的S1引脚和+3.3V电压，另一端接地；第三电容C3一端接三轴地磁传感器芯片U1的C1引脚，另一端接地；第四电容C4的两端分别连接三轴地磁传感器芯片U1的SETC引脚和SETP引脚。电容器C1和C2起电源滤波作用；电容器C3起蓄能作用，为三轴地磁传感器芯片U1内置的置位/复位电路提供电荷；电容器C4配合三轴地磁传感器芯片U1内置的置位/复位电路产生正向与反向脉冲电流，以提高地磁传感器的线性度并降低交叉轴效应与温度效应。

图3是本发明中三轴加速度传感器电路201的电路图，如图3所示，三轴加速度传感器电路201由三轴加速度传感器芯片U2以及五个电容器C5、C6、C7、C8、C9和电阻器R1组成，三轴加速度传感器芯片U2采用意法半导体公司生产的带模拟电压信号输出的MEMS加速度传感芯片LIS3L06AL，三轴加速度传感器芯片U2的VDD引脚接+3.3V电压和第五电容C5和第六电容C6，第五电容C5和第六电容C6的另一端分别接地；三轴加速度传感器芯片U2的GND引脚接地，并通过第七电容C7连接到三轴加速度传感器芯片U2的AZ引脚；三轴加速度传感器芯片U2的AY和AX引脚分别通过第八、第九电容C8、C9接地，第七、第八和第九电容将三轴加速度计的输出电压低通滤波到10Hz；三轴加速度传感器芯片U2的FS引脚通过第一电阻R1连接到+3.3V电压，将三轴加速度传感器芯片U2的量程选择管脚FS上拉到+3.3V，使得三轴加速度传感器芯片U2工作在±6g量程。

图4是本发明中通讯接口电路301的电路图，如图4所示，通讯接口电路301由RS2329针COM接口J1、RS-232收发器芯片U3、A型USB接口J2、USB-UART桥接芯片U4以及九个电容器C10、C11、C12、C13、C14、C15、C16、C17、C18组成，RS2329针COM接口J1、RS-232收发器芯片U3、电容器C10、C11、C12、C13、C14构成RS232电平转换电路，实现MCU处理器电路501通讯UART通讯接口1TTL电平与RS232电平之间的转换，其中：RS-232收发器芯片U3的C1+和C1-引脚之间设有第十电容，C2+和C2-引脚之间设有第十二电容，VCC和GND引脚之间设有第十四电容，V+引脚通过第十一电容接地，V-引脚通过第十三电容接地，GND引脚接地，VCC引脚接+3.3V电压，T2OUT引脚和R2IN引脚分别连接RS2329针COM接口J1的第2和第3端子。RS-232收发器芯片U3采用美信公司生产的MAX3232芯片。A型USB接口J2、USB-UART桥接芯片U4、电容器C15、C16、C17、C18构成USB-UART协议转换电路，实现MCU处理器电路501通讯UART通讯接口2的UART通讯协议与USB通讯协议的桥接转换；USB-UART桥接芯片U4采用FTDI公司的FT232R芯片。

图5是本发明中电源转换电路401的电路图，如图5所示，电源转换电路401由电源输入接口J3、线性稳压电源芯片U5、电容器C19、C20、C21组成；线性稳压电源芯片U5采用德州仪器公司的TPS73633低压降稳压器芯片；电容器C19、C20、C21起稳定电压作用。

图6是本发明中MCU处理器电路501的电路图，如图6所示，MCU处理器电路501由MCU处理器芯片U6、JTAG调试接口J4、晶体振荡器OSC1(24MHz)以及电容器C22、C23、C24、C25、C26、C27、C28、C29和电阻器R2、R3、R4组成；MCU处理器芯片U6采用意法半导体公司生产的基于Cortex-M3内核的32位嵌入式微控制器芯片STM32F100C8；JTAG调试接口J4用于连接JTAG调试器实现程序下载与在线调试；电容器C22、C23、C24、C25、C26、C27、C28起稳定电源电压作用；电容器C29起滤波作用，防止MCU处理器芯片U6因信号干扰而复位；电阻器R2使MCU处理器芯片U6的BOOT0管脚下拉接地，使程序从MCU处理器芯片U6内置的Flash存储器开始运行；电阻器R3、R4为I²C总线的上拉电阻。

本发明提供的三维固态电子罗盘的传感器零点和比例系数核正方法，包括以下步骤：

步骤10：初始化三轴地磁传感器和三轴加速度传感器的零点与比例系数，其中：

三轴地磁传感器的初始化零点与比例系数最小二乘估计器初始化为h₀＝[h_x0h_y0h_z0]^T、P_h＝1000×I₅和c_h＝[01010]^T，h_x0、h_y0、h_z0为进入校正状态时三轴地磁传感器的输出；P_h为三轴地磁传感器的估计误差协方差阵；I₅为三轴地磁传感器的5维单位阵；c_h为要估计的三轴地磁传感器的参数向量；

三轴加速度传感器的零点与比例系数最小二乘估计器初始化为g₀＝[g_x0g_y0g_z0]^T，P_g＝1000×I₅，c_g＝[01010]^T，g_x0、g_y0、g_z0为进入校正状态时三轴加速度传感器的输出；P_g为三轴加速度传感器的估计误差协方差阵；I₅为三轴加速度传感器的5维单位阵；c_g为要估计的三轴加速度传感器的参数向量；

步骤20：分别将三轴地磁传感器和三轴加速度传感器绕三轴旋转，并采集三轴地磁传感器的当前数据h＝[h_xh_yh_z]^T和三轴加速度传感器当前数据g＝[g_xg_yg_z]^T；

步骤30：分别构建y_h、a_h和y_g、a_g，其中：

$y_h=h_x^2-h_{x0}^2,a_h=\left[\begin{array}{ccccc}2(h_x-h_{x0})& h_{y0}^2-h_y^2& 2(h_y-h_{y0})& h_{z0}^2-h_z^0& 2(h_z-h_{z0})\end{array}\right];$

$y_g=g_x^2-g_{x0}^2,a_g=\left[\begin{array}{ccccc}2(g_x-g_{x0})& g_{y0}^2-g_y^2& 2(g_y-g_{y0})& g_{z0}^2-g_z^0& 2(g_z-g_{z0})\end{array}\right];$

步骤40：计算三轴地磁传感器的增益估计参数c_h＝c_h+K_h(y_h-a_hc_h)，并更新其误差协方差阵P_h＝(I₅-K_ha_h)P_h；计算三轴加速度传感器的增益估计参数c_g＝c_g+K_g(y_g-a_gc_g)，并更新其误差协方差阵P_g＝(I₅-K_ga_g)P_g；

步骤50：向上位机发送三轴地磁传感器的估计参数c_h和三轴加速度传感器的估计参数c_g，并通过上位机观察c_h和c_g是否收敛；

如果c_h和c_g未收敛，则返回步骤20继续估计c_h和c_g；

如果c_h收敛，则结束校正，保存估计的c_h和三轴地磁传感器的零点b_x,h、b_y,h、b_z,h及比例系数比值r_x,h/r_y,h、r_x,h/r_z,h，b_x,h＝c_h(1)，b_y,h＝c_h(3)/c_h(2)，b_z,h＝c_h(5)/c_h(4)， c_h(1)、c_h(2)、c_h(3)、c_h(4)、c_h(5)为c_h的5个分量；

如果c_g收敛，则结束校正，保存估计的c_g和三轴加速度传感器的零点b_x,g、b_y,g、b_z,g及比例系数比值r_x,g/r_y,g、r_x,g/r_z,g，b_x,g＝c_g(1)，b_y,g＝c_g(3)/c_g(2)，b_z,g＝c_g(5)/c_g(4)， c_g(1)、c_g(2)、c_g(3)、c_g(4)、c_g(5)为c_g的5个分量。

下面具体说明本发明提供的方法的具体实现过程：

三轴地磁传感器的零点、比例系数比值及估计参数c_h、三轴加速度传感器的零点、比例系数比值及估计参数c_g保存在MCU处理器芯片U6的最后一页Flash存储器中。在硬件初始化过程中，连接三轴地磁传感器电路101的PB10和PB11管脚设置为复用上拉(AF_PP)模式；连接三轴加速度传感器电路201的PA4、PA5和PA6管脚设置为模拟输入(AIN)模式；与通讯接口电路301连接的PA9、PA10、PA2和PA3管脚设置为复用推挽(AF_PP)模式。MCU处理器芯片U6的系统定时器(Systick)设置为每50毫秒产生一次中断，对应电子罗盘20Hz的信息输出频率。MCU处理器芯片U6的I²C总线1设置为400KHz时钟频率的主机模式，用于采集三轴地磁传感器芯片U1的测量数据。MCU处理器芯片U6的UART1和UART2模块均设置为38400bps、8位数据位、1位停止位、无奇偶校验位，设置DMA发送工作模式，开启接收中断。MCU处理器芯片U6在上电复位后从Flash中读取三轴地磁传感器的零点、比例系数比值及估计参数c_h、三轴加速度传感器的零点、比例系数比值及估计参数c_g。在硬件初始化和传感器参数读取完成后，主程序进入死循环，之后的操作均由中断完成。

图7是本发明MCU处理器UART1接收中断处理函数程序流程图。MCU处理器UART1接收中断处理函数程序的主要功能是接收RS232串口发送的传感器校正命令，具体命令格式如下：

三轴地磁传感器校正开始命令：*CMS<cr><lf>

三轴地磁传感器校正结束命令：*CME<cr><lf>

三轴加速度传感器校正开始命令：*CAS<cr><lf>

三轴加速度传感器校正开始命令：*CAE<cr><lf>

其中<cr>和<lf>表示ASCII码中的回车和换行。MCU处理器UART1接收中断处理函数程序对接收到的字符进行解析，如果接收到地磁传感器校正开始命令，则初始化地磁传感器零点与比例系数最小二乘估计器并进入地磁传感器校正状态；如果接收到地磁传感器校正结束命令，则保存估计的地磁传感器零点、比例系数比值、估计参数c_h到Flash存储器并退出地磁传感器校正状态；如果接收到加速度传感器校正开始命令，则初始化加速度传感器零点与比例系数最小二乘估计器并进入加速度传感器校正状态；如果接收到加速度传感器校正结束命令，则保存估计的加速度传感器零点、比例系数比值、估计参数c_g到Flash存储器并退出加速度传感器校正状态。

图8是本发明中MCU处理器UART2接收中断处理函数程序流程图。MCU处理器UART2接收中断处理函数程序的主要功能是接收USB串口发送的传感器校正命令，具体命令格式与UART1的命令格式相同：

三轴地磁传感器校正开始命令：*CMS<cr><lf>

三轴地磁传感器校正结束命令：*CME<cr><lf>

三轴加速度传感器校正开始命令：*CAS<cr><lf>

三轴加速度传感器校正开始命令：*CAE<cr><lf>

MCU处理器UART2接收中断处理函数程序对接收到的字符进行解析，如果接收到地磁传感器校正开始命令，则初始化地磁传感器零点与比例系数最小二乘估计器并进入地磁传感器校正状态；如果接收到地磁传感器校正结束命令，则保存估计的地磁传感器零点、比例系数比值、估计参数c_h到Flash存储器并退出地磁传感器校正状态；如果接收到加速度传感器校正开始命令，则初始化加速度传感器零点与比例系数最小二乘估计器并进入加速度传感器校正状态；如果接收到加速度传感器校正结束命令，则保存估计的加速度传感器零点、比例系数比值、估计参数c_g到Flash存储器并退出加速度传感器校正状态。

地磁传感器零点与比例系数最小二乘估计器初始化为：h₀＝[h_x0h_y0h_z0]^T，P_h＝1000×I₅，c_h＝[01010]^T。其中：h_x0、h_y0、h_z0为进入校正状态时三轴地磁传感器的输出；P_h为估计误差协方差阵；I₅为5维单位阵；c_h为要估计的参数向量，其初始化值的物理含义为三个地磁传感器零点均为0，比例系数相等。

加速度传感器零点与比例系数最小二乘估计器初始化为：g₀＝[g_x0g_y0g_z0]^T，P_g＝1000×I₅，c_g＝[01010]^T。其中：g_x0、g_y0、g_z0为进入校正状态时三轴加速度传感器的输出；P_g为估计误差协方差阵；I₅为5维单位阵；c_g为要估计的参数向量，其初始化值的物理含义为三个加速度传感器零点均为0，比例系数相等。

图9是本发明MCU处理器系统定时器中断处理函数程序流程图。MCU处理器的系统定时器设置为每50毫秒产生一次中断，使得电子罗盘的信息更新频率为20Hz。在其中的传感器数据采集与数字低通滤波步骤中，MCU处理器通过I²C总线采集三轴地磁传感器数据，通过集成的AD转换器采集三轴加速度传感器数据，这些传感器数据通过二阶巴特沃兹数字低通滤波器去噪后获得三轴地磁传感器数据h＝[h_xh_yh_z]^T与三轴加速度传感器数据g＝[g_xg_yg_z]^T。如果地磁传感器状态被UART1接收中断处理函数程序或UART2接收中断处理函数程序设置进入校正状态，则程序进入地磁传感器零点与比例系数估计模块运行。所述地磁传感器零点与比例系数估计模块的主要功能是估计三轴地磁传感器的零点b_x,h、b_y,h、b_z,h及比例系数比值r_x,h/r_y,h、r_x,h/r_z,h。

由于地磁场强度在距离变化不大的情况下可视为恒定的，理想情况下经过零点与比例系数校正的三轴地磁传感器输出应该分布在以原点为球心，地磁场强度为半径的球面上：

${\left(\frac{h_x-b_{x,h}}{r_{x,h}}\right)}^2+{\left(\frac{h_y-b_{y,h}}{r_{y,h}}\right)}^2+{\left(\frac{h_z-b_{z,h}}{r_{z,h}}\right)}^2={\left(R_h\right)}^2---\left(1\right)$

其中：R_h是恒定的地磁场强度。

同理，h₀＝[h_x0h_y0h_z0]^T也分布在同样的球面上。

${\left(\frac{h_{x0}-b_{x,h}}{r_{x,h}}\right)}^2+{\left(\frac{h_{y0}-b_{y,h}}{r_{y,h}}\right)}^2+{\left(\frac{h_{z0}-b_{z,h}}{r_{z,h}}\right)}^2={\left(R_h\right)}^2---\left(2\right)$

式(1)减式(2)可得：

y_h＝a_h×c_h(3)

$y_h=h_x^2-h_{x0}^2---\left(4\right)$

$a_h=\left[\begin{array}{ccccc}2(h_x-h_{x0})& h_{y0}^2-h_y^2& 2(h_y-h_{y0})& h_{z0}^2-h_z^2& 2(h_z-h_{z0})---\left(5\right)\end{array}\right]$

$c_h={\left[\begin{array}{ccccc}{b}_{x,h}& r_{x,h}^2/r_{y,h}^2& r_{x,h}^2{b}_{y,h}/r_{y,h}^2& r_{x,h}^2/r_{z,h}^2& r_{x,h}^2{b}_{z,h}/r_{z,h}^2\end{array}\right]}^T---\left(6\right)$

基于式(3-6)即可设计递推最小二乘算法估计与地磁传感器零点和比例系数相关的参数c_h，具体步骤为：

(1)上位机通过RS232串口或USB串口发送三轴地磁传感器校正开始命令(*CMS<cr><lf>)，初始化地磁传感器零点与比例系数最小二乘估计器为：h₀＝[h_x0h_y0h_z0]^T，P_h＝1000×I₅，c_h＝[01010]^T。

(2)将三轴地磁传感器分别绕三轴旋转；每过50毫秒，程序进入MCU处理器系统定时器中断一次；在中断中，程序通过传感器数据采集与数字低通滤波步骤获得地磁传感器数据h＝[h_xh_yh_z]^T后进入地磁传感器零点与比例系数估计模块；在地磁传感器零点与比例系数估计模块中，程序首先利用h＝[h_xh_yh_z]^T、h₀＝[h_x0h_y0h_z0]^T及式(4-5)构建y_h与a_h，然后计算增益最后估计参数c_h＝c_h+K_h(y_h-a_hc_h)并更新误差协方差阵P_h＝(I₅-K_ha_h)P_h。

(3)在地磁传感器校正信息发送模块中，程序通过RS232串口与USB串口向上位机发送估计参数c_h，用户通过上位机观察c_h是否收敛。如果c_h未收敛，程序自动返回步骤(2)继续估计c_h。如果c_h收敛，用户通过RS232串口或USB串口发送三轴地磁传感器校正结束命令(*CME<cr><lf>)，程序根据估计的c_h与式(6)计算地磁传感器零点与比例系数并保存到Flash存储器。具体计算公式为：b_x,h＝c_h(1)，b_y,h＝c_h(3)/c_h(2)，b_z,h＝c_h(5)/c_h(4)， 其中：c_h(1)、c_h(2)、c_h(3)、c_h(4)、c_h(5)为c_h的5个分量。

如果加速度传感器状态被UART1接收中断处理函数程序或UART2接收中断处理函数程序设置进入校正状态，则程序进入加速度传感器零点与比例系数估计模块运行。所述加速度传感器零点与比例系数估计模块的主要功能是估计三轴加速度传感器的零点b_x,g、b_y,g、b_z,g及比例系数比值r_x,g/r_y,g、r_x,g/r_z,g。

由于重力场强度在距离变化不大的情况下可视为恒定的，理想情况下经过零点与比例系数校正的三轴加速度传感器输出应该分布在以原点为球心，重力场强度(重力加速度值)为半径的球面上：

${\left(\frac{g_x-b_{x,g}}{r_{x,g}}\right)}^2+{\left(\frac{g_y-b_{y,g}}{r_{y,g}}\right)}^2+{\left(\frac{g_z-b_{z,g}}{r_{z,g}}\right)}^2={\left(R_g\right)}^2---\left(7\right)$

其中：R_g是恒定的重力场强度。同理，g₀＝[g_x0g_y0g_z0]^T也分布在同样的球面上。

${\left(\frac{g_{x0}-b_{x,g}}{r_{x,g}}\right)}^2+{\left(\frac{g_{y0}-b_{y,g}}{r_{y,g}}\right)}^2+{\left(\frac{g_{z0}-b_{z,g}}{r_{z,g}}\right)}^2={\left(R_g\right)}^2---\left(8\right)$

式(7)减式(8)可得：

y_g＝a_g×c_g(9)

$y_g=g_x^2-g_{x0}^2---\left(10\right)$

$a_g=\left[\begin{array}{ccccc}2(g_x-g_{x0})& g_{y0}^2-g_y^2& 2(g_y-g_{y0})& g_{z0}^2-g_z^2& 2(g_z-g_{z0})\end{array}\right]---\left(11\right)$

$c_g={\left[\begin{array}{ccccc}{b}_{x,g}& r_{x,g}^2/r_{y,g}^2& r_{x,g}^2{b}_{y,g}/r_{y,g}^2& r_{x,g}^2/r_{z,g}^2& r_{x,g}^2{b}_{z,g}/r_{z,g}^2\end{array}\right]}^T---\left(12\right)$

基于式(9-12)即可设计递推最小二乘算法估计与加速度传感器零点和比例系数相关的参数c_g，具体步骤为：

(1)上位机通过RS232串口或USB串口发送三轴加速度传感器校正开始命令(*CAS<cr><lf>)，初始化加速度传感器零点与比例系数最小二乘估计器为：g₀＝[g_x0g_y0g_z0]^T，P_g＝1000×I₅，c_g＝[01010]^T。

(2)将三轴加速度传感器分别绕三轴旋转；每过50毫秒，程序进入MCU处理器系统定时器中断一次；在中断中，程序通过传感器数据采集与数字低通滤波步骤获得加速度传感器数据g＝[g_xg_yg_z]^T后进入加速度传感器零点与比例系数估计模块；在加速度传感器零点与比例系数估计模块中，程序首先利用g＝[g_xg_yg_z]^T、g₀＝[g_x0g_y0g_z0]^T及式(10-11)构建y_g与a_g，然后计算增益最后估计参数c_g＝c_g+K_g(y_g-a_gc_g)并更新误差协方差阵P_g＝(I₅-K_ga_g)P_g。

在加速度传感器校正信息发送模块中，程序通过RS232串口与USB串口向上位机发送估计参数c_g，用户通过上位机观察c_g是否收敛。如果c_g未收敛，程序自动返回步骤(2)继续估计c_g。如果c_g收敛，用户通过RS232串口或USB串口发送三轴加速度传感器校正结束命令(*CAE<cr><lf>)，程序根据估计的c_g与式(12)计算加速度传感器零点与比例系数并保存到Flash存储器。具体计算公式为：b_x,g＝c_g(1)，b_y,g＝c_g(3)/c_g(2)，b_z,g＝c_g(5)/c_g(4)， 其中：c_g(1)、c_g(2)、c_g(3)、c_g(4)、c_g(5)为c_g的5个分量。

如果地磁传感器与加速度传感器都没处于校正状态，则MCU处理器系统定时器中断处理函数程序依次运行俯仰角、横滚角与航向角计算模块，以及俯仰角、横滚角与航向角信息帧发送模块。

在俯仰角、横滚角与航向角计算模块中，程序利用重力场、地磁场与姿态角的关系计算三个姿态角。假设传感器零点与比例系数已知，地磁传感器与加速度传感器输出可表示为：

${\hat{h}}_x=(h_x-b_{x,h})/r_{x,h}---\left(13\right)$

${\hat{h}}_y=(h_y-b_{y,h})/r_{y,h}---\left(14\right)$

${\hat{h}}_z=(h_z-b_{z,h})/r_{z,h}---\left(15\right)$

${\hat{g}}_x=(g_x-b_{x,g})/r_{x,g}---\left(16\right)$

${\hat{g}}_y=(g_y-b_{y,g})/r_{y,g}---\left(17\right)$

${\hat{g}}_z=(g_z-b_{z,g})/r_{z,g}---\left(18\right)$

利用地球重力场与三轴加速度计测量重力之间的关系，俯仰角和横滚角可通过以下公式计算：

$\mathrm{\&theta;}=-\arctan \left(\frac{{\hat{g}}_x}{\sqrt{{\hat{g}}_y^2+{\hat{g}}_z^2}}\right)=-\arctan \left(\frac{g_x-b_{x,g}}{c_g\left(2\right){(g_y-b_{y,g})}^2+c_g\left(4\right){(g_z-b_{z,g})}^2}\right)---\left(19\right)$

$\frac{{\hat{g}}_y}{{\hat{g}}_z}=\frac{g_y-b_{y,g}}{g_z-b_{z,g}}\sqrt{\frac{c_g\left(2\right)}{c_g\left(4\right)}}---\left(20\right)$

利用地球地磁场与三轴地磁传感器测量磁场之间的关系，航向角可通过以下公式计算：

$\begin{array}{c}{X}_H={\hat{h}}_x\cos \mathrm{\&theta;}+{\hat{h}}_y\sin \mathrm{\&theta;}\sin \mathrm{\&phi;}+{\hat{h}}_z\sin \mathrm{\&theta;}\cos \mathrm{\&phi;}\\ =\frac{1}{r_{x,h}}[(h_x-b_{x,h})\cos \mathrm{\&theta;}+\sqrt{c_h\left(2\right)}(h_y-b_{y,h})\sin \mathrm{\&theta;}\sin \mathrm{\&phi;}+\sqrt{c_h\left(4\right)}(h_z-b_{z,h})\sin \mathrm{\&theta;}\cos \mathrm{\&phi;}]\end{array}---\left(22\right)$

$\begin{array}{c}{X}_H={\hat{h}}_y\cos \mathrm{\&phi;}-{\hat{h}}_z\sin \mathrm{\&phi;}\\ =\frac{1}{r_{x,h}}\left[\sqrt{c_h\left(2\right)}(h_y-b_{y,h}\right)\cos \mathrm{\&phi;}+\sqrt{c_h\left(4\right)}(h_z-b_{z,h})\sin \mathrm{\&phi;}]\end{array}---\left(23\right)$

$\frac{Y_H}{X_H}=\frac{\sqrt{c_h\left(2\right)}(h_y-b_{y,h})\cos \mathrm{\&phi;}-\sqrt{c_h\left(4\right)}(h_z-b_{z,h})\sin \mathrm{\&phi;}}{(h_x-b_{x,h})\cos \mathrm{\&theta;}+\sqrt{c_h\left(2\right)}(h_y-b_{y,h})\sin \mathrm{\&theta;}\sin \mathrm{\&phi;}+\sqrt{c_h\left(4\right)}(h_z-b_{z,h})\sin \mathrm{\&theta;}\cos \mathrm{\&phi;}}---\left(24\right)$

${\hat{X}}_H=(h_x-b_{x,h})\cos \mathrm{\&theta;}+\sqrt{c_h\left(2\right)}(h_y-b_{y,h})\sin \mathrm{\&theta;}\sin \mathrm{\&phi;}+\sqrt{c_h\left(4\right)}(h_z-b_{z,h})\sin \mathrm{\&theta;}\cos \mathrm{\&phi;}---\left(25\right)$

${\hat{Y}}_H=\sqrt{c_h\left(2\right)}(h_y-b_{y,h})\cos \mathrm{\&phi;}-\sqrt{c_h\left(4\right)}(h_z-b_{z,h})\sin \mathrm{\&phi;}---\left(26\right)$

其中：θ、φ、ψ分别为俯仰角、横滚角、航向角，X_H、Y_H为地磁场分量在水平面上的投影。

在俯仰角、横滚角与航向角信息帧发送模块中，程序利用UART1和UART2的DMA发送功能向RS232串口和USB串口发送计算得到的俯仰角、横滚角、航向角数值。

本发明的三维固态电子罗盘与其他实现方案相比，集成了不依赖于校正设备的加速度传感器零点与比例系数估计模块和不依赖于校正设备的地磁传感器零点与比例系数估计模块，方便多旋翼飞行器在野外进行加速度传感器与地磁传感器的校正，提高了电子罗盘俯仰角、横滚角、以及航向角的测量精度；同时采用固态的磁阻传感器、MEMS加速度计为传感元件，采用高集成度的MCU处理器为主控芯片，降低系统成本的同时提高了系统可靠性；因此更加适合在多旋翼无人飞行器上使用，符合多旋翼无人飞行器技术发展的趋势。

本发明不局限于上述最佳实施方式，任何人应该得知在本发明的启示下作出的结构变化，凡是与本发明具有相同或相近的技术方案，均落入本发明的保护范围之内。

Claims (3)

1.三维固态电子罗盘的传感器零点和比例系数核正方法，其特征在于，包括以下步骤：

步骤10：初始化三轴地磁传感器和三轴加速度传感器的零点与比例系数，其中：

三轴地磁传感器的初始化零点与比例系数最小二乘估计器初始化为h₀＝[h_x0h_y0h_z0]^T、P_h＝1000×I₅和c_h＝[01010]^T，h_x0、h_y0、h_z0为进入校正状态时三轴地磁传感器的输出；P_h为三轴地磁传感器的估计误差协方差阵；I₅为三轴地磁传感器的5维单位阵；c_h为要估计的三轴地磁传感器的参数向量；

三轴加速度传感器的零点与比例系数最小二乘估计器初始化为g₀＝[g_x0g_y0g_z0]^T，P_g＝1000×I₅，c_g＝[01010]^T，g_x0、g_y0、g_z0为进入校正状态时三轴加速度传感器的输出；P_g为三轴加速度传感器的估计误差协方差阵；I₅为三轴加速度传感器的5维单位阵；c_g为要估计的三轴加速度传感器的参数向量；

步骤20：分别将三轴地磁传感器和三轴加速度传感器绕三轴旋转，并采集三轴地磁传感器的当前数据h＝[h_xh_yh_z]^T和三轴加速度传感器当前数据g＝[g_xg_yg_z]^T；

步骤30：分别构建y_h、a_h和y_g、a_g，其中：

$y_h=h_x^2-h_{x0}^2,a_h=\&lsqb;\begin{array}{ccccc}2(h_x-h_{x0})& h_{y0}^2-h_y^2& 2(h_y-h_{y0})& h_{z0}^2-h_z^2& 2(h_z-h_{z0})\end{array}\&rsqb;;$

$y_g=g_x^2-g_{x0}^2,a_g=\&lsqb;\begin{array}{ccccc}2(g_x-g_{x0})& g_{y0}^2-g_y^2& 2(g_y-g_{y0})& g_{z0}^2-g_z^2& 2(g_z-g_{z0})\end{array}\&rsqb;;$

步骤40：计算三轴地磁传感器的增益估计参数c_h＝c_h+K_h(y_h-a_hc_h)，并更新其误差协方差阵P_h＝(I₅-K_ha_h)P_h；计算三轴加速度传感器的增益估计参数c_g＝c_g+K_g(y_g-a_gc_g)，并更新其误差协方差阵P_g＝(I₅-K_ga_g)P_g；

步骤50：向上位机发送三轴地磁传感器的估计参数c_h和三轴加速度传感器的估计参数c_g，并通过上位机观察c_h和c_g是否收敛；

如果c_h和c_g未收敛，则返回步骤20继续估计c_h和c_g；

如果c_h收敛，则地磁传感器结束校正，保存估计的c_h和三轴地磁传感器的零点b_x,h、b_y,h、b_z,h及比例系数比值r_x,h/r_y,h、r_x,h/r_z,h，b_x,h＝c_h(1)，b_y,h＝c_h(3)/c_h(2)，b_z,h＝c_h(5)/c_h(4)， $r_{x,h}/r_{y,h}=\sqrt{c_h\left(2\right)},r_{x,h}/r_{z,h}=\sqrt{c_h\left(4\right)},$ c_h(1)、c_h(2)、c_h(3)、c_h(4)、c_h(5)为c_h的5个分量；

如果c_g收敛，则结束加速度计校正，保存估计的c_g和三轴加速度传感器的零点b_x,g、b_y,g、b_z,g及比例系数比值r_x,g/r_y,g、r_x,g/r_z,g，b_x,g＝c_g(1)，b_y,g＝c_g(3)/c_g(2)，b_z,g＝c_g(5)/c_g(4)， $r_{x,g}/r_{y,g}=\sqrt{c_g\left(2\right)},r_{x,g}/r_{z,g}=\sqrt{c_g\left(4\right)},c_g\left(1\right),c_g\left(2\right),$ c_g(3)、c_g(4)、c_g(5)为c_g的5个分量。

2.如权利要求书1所述的方法，其特征在于，上位机通过RS232串口或USB串口发送三轴地磁传感器和三轴加速度传感器的校正开始及结束命令。

3.如权利要求书1所述的方法，其特征在于，用重力场、地磁场与姿态角的关系计算三个姿态角，具体计算公式为：

$\mathrm{\&theta;}=-\arctan \left(\frac{g_x-b_{x,g}}{\sqrt{c_g\left(2\right){(g_y-b_{y,g})}^2+c_g\left(4\right){(g_z-b_{z,g})}^2}}\right);$

${\hat{X}}_H=(h_x-b_{x,h})cos\mathrm{\&theta;}+\sqrt{c_h\left(2\right)}(h_y-b_{y,h})sin\mathrm{\&theta;}\sin \mathrm{\&phi;}+\sqrt{c_h\left(4\right)}(h_z-b_{z,h})sin\mathrm{\&theta;}cos\mathrm{\&phi;};$

${\hat{Y}}_H=\sqrt{c_h\left(2\right)}(h_y-b_{y,h})cos\mathrm{\&phi;}-\sqrt{c_h\left(4\right)}(h_z-b_{z,h})sin\mathrm{\&phi;};$

其中：θ、φ、ψ分别为俯仰角、横滚角、航向角。