CN102506898A - 一种基于遗传算法的惯性/地磁传感器标定方法 - Google Patents

Abstract

本发明公开了一种基于遗传算法的惯性/地磁传感器标定方法。具体是对集成了三轴微加速度传感器、三轴微陀螺仪传感器和三轴地磁传感器的姿态跟踪单元进行标定的方法。包括：1)用自行设计的二轴无磁转台来采集全姿态下的传感器数据；2)利用三轴微加速度传感器和三轴地磁传感器在静止情况下输出数据的模值恒定原理来标定自身的零漂、比例因子和正交参数；3)利用三轴微加速度传感器和三轴地磁传感器数据叉乘向量的空间旋转角度来标定它们的对准参数；4)基于标定的三轴微加速度传感器来标定三轴微陀螺仪的零漂参数；5)基于三轴微陀螺仪传感器数据和转台所提供的旋转角度标定其比例因子和正交参数。本发明提供了一种快速、简洁的标定方法。

Description

一种基于遗传算法的惯性/地磁传感器标定方法

技术领域

本发明属于三维姿态跟踪领域，尤其涉及一种针对于内部包含三轴微加速度传感器、三轴微陀螺仪传感器和三轴地磁传感器等惯性/地磁传感器的姿态跟踪单元的误差参数补偿的标定方法。

背景技术

随着微电子机械系统(MEMS)的发展，特别是微惯性技术的发展，载体的位姿获取已经从原来的平台惯导发展到捷联惯导和组合导航技术，其系统的体积、重量和成本大大降低，因此采用惯性/地磁技术进行姿态的跟踪检测有着广泛的前景。最近，苹果公司的iphone手机以及安卓手机都增加了MEMS三轴微加速度传感器，三轴地磁传感器用于获取手机姿态从而设计更自然的人机交互应用。目前，国内外已经设计出了相当多的微惯性姿态测量装置用于姿态测量跟踪，本申请人在专利号为“ZL200810114391.4”的中国专利“基于ZigBee无线单片机的微惯性测量装置”中提供了一种可用于运动载体姿态测量的装置，该专利在本申请中引入作为参考。在该专利中，采用六轴微惯性传感器(集成了三轴微陀螺仪传感器和三轴微加速度传感器)和三轴磁场传感器来测量运动载体的姿态。

MEMS传感器虽然在微型化，低功耗方面有着无可比拟的优点，但是也存在自身固有的缺点。首先是传感器内部存在固有的噪声干扰；其次是现有的技术无法将三轴微加速度传感器、三轴微陀螺仪传感器以及三轴地磁传感器集成到单芯片上，从而产生了传感器在安装过程中的对准误差；最后MEMS传感器的使用环境和温度也会影响其内部参数，因此MEMS传感器在使用之前需要进行参数标定。

针对三轴微加速度传感器、三轴微陀螺仪传感器和三轴地磁传感器参数的标定，通常情况下，需要一个精密的三轴机械旋转台来准确的获取各个方位的姿态数据，通过转台准确的方位数据与微惯性单元输出的方位数据进行比较，来获取微惯性单元内部的三轴微加速度传感器、三轴微陀螺仪传感器和三轴地磁传感器的标定参数(S.Bonnet，C.Bassompierre，C.Godin，S.Lesecq，A.Barraud“Calibration methods for inertial and magneticsensors”，Sensors and Actuators A：Physical，Vol 156，Issue 2，2009，pp.302-311)。但是，这种传统的方法不仅需要高精度的转台，还需要设计一个复杂的数据处理方法，并且高精度转台的电机驱动部分容易引入磁场干扰。国内方面，北京航空航天大学、中北大学在微惯性传感器标定方面做出了很多工作。但是国内的研究主要从事的是单个微惯性传感器自身的标定，并且传感器标定过程中需要多自由度高精密转台，从而使得传感器能达到军事应用的水平。最近，中北大学提出了“一种微惯性测量组合现场快速标定方法”(中国专利：20100200418.9，201010134734.0)，该方法可以简单快速的对加速度传感器进行标定。但是该方法没有给出磁传感器和陀螺仪传感器的标定方法；也没有给出三轴微加速度传感器、三轴微陀螺仪传感器和三轴地磁传感器之间的坐标系对准的方法；而且该方法的采样数据是随机的。

发明内容

针对上述现有技术中存在的问题，本发明提出了一种基于遗传算法的惯性/地磁传感器标定方法。具体是对集成了三轴微加速度传感器、三轴微陀螺仪传感器和三轴地磁传感器组成的姿态跟踪单元的一种快速、简洁的标定方法。

本发明提出的一种惯性/地磁传感器标定方法，具体包括如下步骤：

步骤1：将待标定的包含惯性/地磁传感器的姿态跟踪单元置于二轴无磁转台的水平转台上，启动所述姿态跟踪单元；

步骤2：采集各传感器在二轴无磁转台上的静态数据和动态数据；

步骤3：分离采集得到各传感器的静态数据和动态数据；

步骤4：基于分离得到各传感器的静态数据和动态数据，对姿态跟踪单元中的惯性/地磁传感器进行标定。

本发明方法的优点在于：1)无需高精密的无磁转台以及复杂的标定流程。标定过程中只需要借助于一个能提供相对45°角的无磁两自由度的转台。2)根据在任意静止姿态下三轴微加速度传感器和三轴地磁传感器模值不变特性，获取加速度计和磁力计的零漂、比例因子和正交矩阵，该方法可在任意姿态下采集数据，故对标定平台无任何位置精度要求，只要平台是无磁材料即可；3)利用加速度传感器和磁力计传感器数据的叉乘向量的相对旋转角度和对准矩阵特性，获得磁力计到加速度计对准矩阵参数，加速度传感器和磁力计传感器的联合标定，充分运用了这两类传感器在确定空间姿态的相关性；4)利用陀螺仪零漂与自身所受加速度的关联性，求得陀螺仪的零漂因子，利用自身加速度数据来对自身的陀螺仪标定，具有一定的互标定优点；5)根据陀螺积分后确定的角度误差模型与实际旋转角度建立目标函数，求得陀螺仪的比例因子和正交矩阵；6)本发明中所有的标定参数是通过遗传算法求得的，采用遗传算法能够快速的找到能满足标定精度的各传感器标定参数。

附图说明

图1为本发明所提出的标定方法所使用的姿态跟踪单元的组成示意图。

图2为经典三轴传感器误差模型。

图3为本发明的二轴无磁转台的结构示意图。

图4为本发明的二轴无磁转台的实物图。

图5为本发明的基于遗传算法的姿态跟踪单元标定方法流程图。

图6为三轴磁传感器标定前后的模值对比图。

图7为三轴加速度传感器和三轴磁传感器的对准效果图。

图8为三轴微陀螺仪传感器标定前后的旋转角度对比图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本发明进一步详细说明。

图1示出本发明进行标定的姿态跟踪单元的组成。该姿态跟踪单元集成了三轴加速度传感器、三轴微陀螺仪传感器和三轴磁传感器。三组传感器所在的正交轴构成了该单元的X轴、Y轴和Z轴的三轴坐标系。

图2示出经典三轴传感器误差模型。三轴MEMS传感器的输出为向量y_k，实际的传感器输入数据为u_k，其中，k代表传感器的类型，k＝{a，m，g}，a、m、g分别代表三轴加速度传感器、三轴地磁传感器、三轴微陀螺仪传感器。传感器原始输入数据u_k首先与传感器的零漂因子b_k相减，然后依次与比例因子S_k、正交矩阵T_k和M_k相乘，就得到了标定后的输出数据y_k。这里将传感器误差分为两类：机械误差和电量误差，机械误差指机械加工和传感器组装造成的误差，主要指轴不正交带来的正交误差和传感器坐标系与外壳坐标系不重合造成的对准误差；电量误差是每个轴上的传感器固有误差，主要指零漂和比例因子误差。在图2所示的模型中，如果令S_k、T_k、M_k和b_k分别表示比例因子、正交矩阵、对准矩阵和零漂因子，那么输出y_k的表达式为：

${\stackrel{\→}{y}}_k=S_k{T}_k{M}_k{\stackrel{\→}{u}}_k+{\stackrel{\→}{b}}_k---\left(1\right)$

其中，矩阵S_k为传感器的比例因子，其表示形式如下所示：

$S_k=\left[\begin{array}{ccc}{s}_{\mathrm{kx}}& 0& 0\\ 0& s_{\mathrm{ky}}& 0\\ 0& 0& s_{\mathrm{kz}}\end{array}\right]---\left(2\right)$

S_kx、S_ky和S_kz分别表示x、y和z轴上k类传感器的比例因子。正交化矩阵T_k用于将传感器数据转化到一个正交坐标系下，该矩阵是通过Gram-Schmidt正交化过程构建的，其表示形式如下所示：

$T_k=\left[\begin{array}{ccc}1& 0& 0\\ \cos {\mathrm{\α}}_k& 1& 0\\ \cos {\mathrm{\β}}_k& \cos {\mathrm{\γ}}_k& 1\end{array}\right]---\left(3\right)$

设X、Y和Z代表正交坐标系，X′、Y′和Z′为非正交坐标系(轴X′、Y′和Z′与传感器实际安装方向同向)，则矩阵T_k中的元素α_k表示X′和Y′轴间的夹角，β_k表示X′和Z′轴间的夹角，γ_k表示Y和Z′轴间的夹角。假设X、Y和Z轴的对准误差用欧拉角表示为φ，

ψ，则对准矩阵M_k为：

向量b_k为零漂因子，表示为：

$b_k=\left[\begin{array}{c}{b}_x\\ b_y\\ b_z\end{array}\right]---\left(6\right)$

其中，b_x、b_y和b_z分别表示x、y和z轴上的传感器的零漂因子。

图3是二轴无磁转台的结构示意图。如图3所示，所述二轴无磁转台包括：1-垂直定位销、2-垂直转台、3-水平转台、4-水平定位销、5-水平定位孔、6-垂直转轴、7-垂直定位孔、8-水平转轴、9-侧面支撑板。

水平转台3通过位于垂直转台2上的水平转轴8和垂直转台2连接；绕水平转轴8转动水平转台3，位于水平转台3上的水平定位销4可分别与位于垂直转台2上的八个呈45°分布的水平定位孔5对准。垂直转台2通过位于侧面支撑板9上的垂直转轴6和侧面支撑板9连接，绕垂直转轴6转动垂直转台2，位于垂直转台2上的垂直定位销1可分别与位于侧面支撑板9上的八个呈45°分布的垂直定位孔7对准。该二轴无磁转台的实物图如图4所示。

图5为本发明的基于遗传算法的姿态跟踪单元标定方法的方法流程图。本发明所提出的基于遗传算法的姿态跟踪单元标定方法具体包括以下步骤：

步骤1：启动姿态跟踪单元。

将待标定的姿态跟踪单元放置在图3所示的二轴无磁转台的水平转台3上，使姿态跟踪单元的Z轴向上，启动姿态跟踪单元。

步骤2：采集姿态跟踪单元中的各传感器在二轴无磁转台上的静态数据和动态数据。

步骤2中对于各传感器的静态数据和动态数据的采集具体为依次采集各传感器在不同水平定位孔处和不同垂直定位孔处的静态数据和相邻两个水平定位孔之间的动态数据。

设S_vh(v，h＝1、2、3…8)表示垂直转台2在第v个垂直定位孔7和水平转台3在第h个水平定位孔5处的静态数据，D_vh1h2表示静态位置S_vh1和S_vh2之间的动态运动数据(这里的静态数据和动态数据均表示三轴地磁传感器、三轴微加速度传感器和三轴微陀螺仪传感器数据)。设置垂直定位销1在第v(v＝1、2…8)个垂直定位孔7处，采集水平定位销4在8个水平定位孔5处的姿态跟踪单元的静态数据S_vh和相邻两个水平定位孔5之间转动的姿态跟踪单元的动态数据D_vh1h2，这样在每个垂直定位孔7处可以得到8组姿态跟踪单元的静态数据S_vh和7组动态数据D_vh1h2，故在8个垂直定位孔7下一共获取64组姿态跟踪单元的静态数据S_vh和56组动态数据D_vh1h2。具体的数据采集过程为：

对于垂直定位孔1，采集的数据为：S₁₁-D₁₁₂-S₁₂-D₁₂₃-S₁₃-D₁₃₄-S₁₄-D₁₄₅-S₁₅-D₁₅₆-S₁₆-D₁₆₇-S₁₇-D₁₇₈-S₁₈；

对于垂直定位孔2，采集的数据为：S₂₁-D₂₁₂-S₂₂-D₂₂₃-S₂₃-D₂₃₄-S₂₄-D₂₄₅-S₂₅-D₂₅₆-S₂₆-D₂₆₇-S₂₇-D₂₇₈-S₂₈；

对于垂直定位孔3，采集的数据为：S₃₁-D₃₁₂-S₃₂-D₃₂₃-S₃₃-D₃₃₄-S₃₄-D₃₄₅-S₃₅-D₃₅₆-S₃₆-D₃₆₇-S₃₇-D₃₇₈-S₃₈；

对于垂直定位孔4，采集的数据为：S₄₁-D₄₁₂-S₄₂-D₄₂₃-S₄₃-D₄₃₄-S₄₄-D₄₄₅-S₄₅-D₄₅₆-S₄₆-D₄₆₇-S₄₇-D₄₇₈-S₄₈；

对于垂直定位孔5，采集的数据为：S₅₁-D₅₁₂-S₅₂-D₅₂₃-S₅₃-D₅₃₄-S₅₄-D₅₄₅-S₅₅-D₅₅₆-S₅₆-D₅₆₇-S₅₇-D₅₇₈-S₅₈；

对于垂直定位孔6，采集的数据为：S₆₁-D₆₁₂-S₆₂-D₆₂₃-S₆₃-D₆₃₄-S₆₄-D₆₄₅-S₆₅-D₆₅₆-S₆₆-D₆₆₇-S₆₇-D₆₇₈-S₆₈；

对于垂直定位孔7，采集的数据为：S₇₁-D₇₁₂-S₇₂-D₇₂₃-S₇₃-D₇₃₄-S₇₄-D₇₄₅-S₇₅-D₇₅₆-S₇₆-D₇₆₇-S₇₇-D₇₇₈-S₇₈；

对于垂直定位孔8，采集的数据为：S₈₁-D₈₁₂-S₈₂-D₈₂₃-S₈₃-D₈₃₄-S₈₄-D₈₄₅-S₈₅-D₈₅₆-S₈₆-D₈₆₇-S₈₇-D₈₇₈-S₈₈。

步骤3：分离采集得到的各传感器静态数据和动态数据。

实际采集过程中所记录的数据是整个步骤2中的所有数据，即数据文件里包括了所需要的64组静态数据S_vh和56组动态数据D_vh1h2，这就需要将我们所需要的动态数据和静态数据从步骤2采集到的数据中分离开来。采用姿态跟踪单元在静止情况下，三轴微陀螺仪传感器输出的三轴数据接近零的特性，可以设置适当的阈值(这里设0.01)以分离出64组静态数据S_vh，由于动态数据D_vh1h2在静态数据S_vh1和S_vh2之间，故参考静态数据可以分离出需要的56组动态数据D_vh1h2。

步骤4：基于分离得到各传感器的静态数据和动态数据，对姿态跟踪单元中的惯性/地磁传感器进行标定。

所述步骤4进一步包括：

步骤41：基于步骤3分离得到三轴地磁传感器和三轴微加速度传感器的静态数据，利用三轴微加速度传感器和三轴地磁传感器在任意静止姿态下的三轴数据模值恒定的原理和遗传算法标定三轴地磁传感器和三轴微加速度传感器的零漂、比例因子、正交参数，并得到校正后的三轴地磁传感器和三轴微加速度传感器的静态数据。

利用理想的三轴地磁传感器和三轴微加速度传感器在任意静止姿态下数据的模值恒定特性，建立目标函数，如下式所示：

$\min \underset{i=1}{\overset{n}{\mathrm{\Σ}}}\sqrt{{(N_i-\stackrel{\‾}{N})}^2}---\left(7\right)$

其中，n为采集的三轴地磁传感器或三轴微加速度传感器的数据集大小，i为采集的数据集的序列号，

是三轴地磁传感器或三轴微加速度传感器的数据的模值，N_i0、N_i1和N_i2分别代表三轴传感器的X、Y和Z轴数据，

是实际磁场或重力场场强。

因为传感器数据的模值与对准矩阵M_k无关，故设M_k矩阵为单位阵，64组静态数据S_vh中的三轴地磁传感器或三轴微加速度传感器数据作为原始输入数据u_k代入式(1)得到的64组输出数据作为Y代入目标函数式(7)，取磁场强度为1guass或重力场强度为1g作为

采用遗传算法对式(7)做数据拟合，就可以求得式(1)中的三轴地磁传感器或三轴微加速度传感器的正交矩阵T_k、比例因子S_k和零漂因子b_k，即得到三轴地磁传感器和三轴微加速度传感器的标定参数T_a、T_m、S_a、S_m、b_a、b_m。64组静态数据S_vh经过比例因子、零漂和正交化校正后的数据记为S_{vh_TSB}。

步骤42：基于步骤41得到的校正后的三轴地磁传感器和三轴微加速度传感器的静态数据，对三轴地磁传感器到三轴微加速度传感器的坐标系的对准矩阵进行标定，并得到经过对准标定后的三轴地磁传感器和三轴微加速度传感器的静态数据。

设三轴地磁传感器数据为向量D_M，三轴微加速度传感器数据为向量D_A，则对于叉乘向量P＝D_M×D_A，P垂直于D_M和D_A。将姿态跟踪单元绕空间任意轴旋转，则向量P的相对旋转角度δ为：

$\mathrm{\δ}=\arccos \left(\frac{P_0\·P_i}{\left|\right|P_0\·P_i\left|\right|}\right)---\left(8\right)$

其中P₀为初始姿态下的叉乘向量；P_i为旋转后的叉乘向量。若三轴地磁传感器和三轴微加速度传感器的坐标系重合，则向量P的相对旋转角度与实际的旋转角度是相同的，根据这个特性，建立目标函数：

$\min \underset{i=1}{\overset{m}{\mathrm{\Σ}}}\left(\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{({{\mathrm{\δ}}_{\mathrm{ij}}}^{\′}-{\mathrm{\δ}}_{\mathrm{ij}})}^2\right)---\left(9\right)$

其中，δ_ij′和δ_ij分别表示在第i垂直定位孔和第j水平定位孔处的姿态相对在第i垂直定位孔和第1水平定位孔处的待标定的相对旋转角度和实际姿态相对旋转角，m表示垂直定位孔个数，n表示水平定位孔个数，i表示垂直定位销的位置，j表示水平定位销的位置。

因为此处标定只涉及到对准标定，故设T_k和S_k为单位阵，b_k为零向量，将校正后的静态数据S_{vh_TSB}作为原始输入数据u_k代入式(1)，得到三轴微加速度传感器数据的表达式(10)和三轴地磁传感器数据表达式(11)：

${\stackrel{\→}{y}}_a=M_a{\stackrel{\→}{u}}_a---\left(10\right)$

${\stackrel{\→}{y}}_m=M_m{\stackrel{\→}{u}}_m---\left(11\right)$

其中，M_a表示加速度传感器坐标系到自身坐标系的对准矩阵，M_m代表磁力计传感器坐标系到加速度传感器坐标系的对准矩阵。

将

和

作为D_M和D_A，代入P＝D_M×D_A和式(8)，便可求得δ_ij′，而δ_ij的表达式为：

δ_ij＝(j-1)×45，j＝1，2，…，8    (12)

令M_a为单位阵(此处只标定三轴地磁传感器到三轴微加速度传感器的坐标系的对准矩阵)，并将δ_ij′和式(12)代入目标函数(9)，采用遗传算法对其进行数据拟合，便能求得三轴地磁传感器的坐标系到三轴微加速度传感器的坐标系的对准矩阵M_m。经过地磁传感器坐标系到微加速度传感器坐标系的对准后，从S_{vh_TSB}得到的64组静态数据记为S_{vh_MTSB}。

步骤43：基于步骤3分离得到三轴微陀螺仪传感器的静态数据和步骤41中标定后的三轴微加速度传感器的静态数据，对三轴微陀螺仪传感器的零漂进行标定。

三轴微陀螺仪传感器的零漂与自身所受加速度的关联性如下式所示：

$\left[\begin{array}{c}{b}_x\\ b_y\\ b_z\end{array}\right]=\left[\begin{array}{ccccccc}{L}_{11}& L_{12}& L_{13}& L_{14}& L_{15}& L_{16}& L_{17}\\ L_{21}& L_{22}& L_{23}& L_{24}& L_{25}& L_{26}& L_{27}\\ L_{31}& L_{32}& L_{33}& L_{34}& L_{35}& L_{36}& L_{37}\end{array}\right]\×\left[\begin{array}{c}1\\ Y_{\mathrm{Ax}}\\ Y_{\mathrm{Ax}}^2\\ Y_{\mathrm{Ay}}\\ Y_{\mathrm{Ay}}^2\\ Y_{\mathrm{Az}}\\ Y_{\mathrm{Az}}^2\end{array}\right]---\left(13\right)$

其中，矩阵L为零漂更正矩阵，向量b，Y_A分别表示三轴微陀螺仪传感器的零漂因子和校正后的三轴微加速度传感器数据。为使三轴微陀螺仪传感器在静止姿态下数据输出尽量接近零，建立目标函数：

$\min \underset{i=1}{\overset{n}{\mathrm{\Σ}}}{(b_i-{\mathrm{LY}}_i)}^T(b_i-{\mathrm{LY}}_i)---\left(14\right)$

其中，n为采集数据集总大小，i代表数据集序列号。

将64组静态数据S_vh中的三轴微陀螺仪传感器数据作为b_i，将S_{vh_MTSB}中对应的三轴微加速度传感器数据作为Y_i代入目标函数(14)，采用遗传算法对其进行数据拟合，便能得到三轴微陀螺仪传感器的零漂与自身加速度的关联矩阵L。这样就能通过三轴微加速度传感器数据求得三轴微陀螺仪传感器的零漂因子b_g。

步骤44：基于步骤3分离得到三轴微陀螺仪传感器的动态数据和二轴无磁转台提供的旋转角度，对三轴陀螺仪传感器的比例因子和正交矩阵进行标定。

三轴陀螺仪传感器数据积分确定的相对角度如式(15)、(16)和(17)所示：

${\mathrm{\α}}_i^{\′}=\sqrt{{{\mathrm{\α}}_{\mathrm{gx}}}^2+{{\mathrm{\α}}_{\mathrm{gy}}}^2+{{\mathrm{\α}}_{\mathrm{gz}}}^2}---\left(15\right)$

${\mathrm{\α}}_{\mathrm{gi}}=\underset{0}{\overset{t}{\∫}}{g_i}^{\′}\×\mathrm{dt}(i=x,y,z)---\left(16\right)$

$\left[\begin{array}{c}{g^{\′}}_x\\ {g^{\′}}_y\\ {g^{\′}}_z\end{array}\right]=T_g\×S_g\×(\left[\begin{array}{c}{g}_x\\ g_y\\ g_z\end{array}\right]-B_g)---\left(17\right)$

其中，g_x、g_y和g_z表示陀螺仪的原始数据，g′_x、g′_y和g′_z表示陀螺仪的标定后的数据，B_g、T_g、S_g分别为陀螺仪的零漂、正交矩阵和比例因子矩阵，t为采样时间，α_gx、α_gy、α_gz表示分别绕三轴微陀螺仪传感器的X、Y和Z轴的旋转角度，α_i′表示三轴微陀螺仪传感器的相对旋转角度。建立陀螺仪积分相对角度和实际旋转相对角度的优化目标函数：

$\min \underset{i=1}{\overset{n}{\mathrm{\Σ}}}{({{\mathrm{\α}}_i}^{\′}-{\mathrm{\α}}_i)}^2---\left(18\right)$

其中，n为采集的数据集总大小，i代表数据集序列号。

B_g取步骤43中得到的三轴陀螺仪传感器的零漂因子b_g，g_x、g_y和g_z取56组动态数据D_vh1h2中的三轴微陀螺仪传感器数据，因为水平定位孔呈45°分布，故每组动态数据相当于是一个旋转45°的运动，故这里实际旋转角度α_i为45°，将这些数据代入式(15)-(18)中，采用遗传算法对目标函数式(18)求解，就能得到三轴微陀螺仪传感器的比例因子S_g和正交矩阵T_g。

至此，就完成了对姿态跟踪单元中的惯性/地磁传感器的标定。

图6是三轴地磁传感器自身标定前后的数据效果对比，虚线表示原始模值数据，实线表示标定后的模值数据，可以看到地磁传感器经过标定后，其模值波动相对标定前有明显的减小。

图7是三轴加速度传感器和三轴地磁传感器对准的效果图。虚线表示的是标定之前叉乘向量旋转角度，实线是标定之后叉乘向量旋转角度。

图8是陀螺仪漂移更正效果图。在数据采集过程中，惯性/地磁传感器水平放置，然后绕Z轴重复旋转90°。然后对陀螺仪数据进行积分，得到陀螺仪漂移更正前后的姿态效果对比，图8中虚线代表陀螺标定前数据，实线代表标定后的数据。其中，上部图代表的是陀螺仪X轴标定前后积分的角度数据，由于惯性地磁传感器只是绕Z轴旋转，故数据越接近0度，说明标定性能越好；中部图代表的是陀螺仪Y轴标定前后积分的角度数据，与X轴相同，数据越接近0°，说明标定性能越好；下部图代表的是陀螺仪Z轴标定前后积分的角度数据，其数据在静止状态越接近0°或90°处时说明标定效果越明显。从图中的三个图都可以看出陀螺漂移现象明显减小，即标定效果是显著的。

以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种惯性/地磁传感器标定方法，其特征在于，该方法包括以下步骤：

步骤1：将待标定的包含惯性/地磁传感器的姿态跟踪单元置于二轴无磁转台的水平转台上，启动所述姿态跟踪单元；

步骤2：采集各传感器在二轴无磁转台上的静态数据和动态数据；

步骤3：分离采集得到各传感器的静态数据和动态数据；

步骤4：基于分离得到各传感器的静态数据和动态数据，对姿态跟踪单元中的惯性/地磁传感器进行标定。

2.如权利要求1所述的方法，其特征在于，所述姿态跟踪单元包含三轴微加速度传感器、三轴微陀螺仪传感器和三轴地磁传感器。

3.如权利要求1所述的方法，其特征在于，所述二轴无磁转台包括垂直转台、水平转台、水平定位孔、垂直转轴、垂直定位孔和水平转轴。

4.如权利要求1所述的方法，其特征在于，所述步骤2进一步包括：依次采集各传感器在二轴无磁转台上的不同水平定位孔处和不同垂直定位孔处的静态数据和相邻两个水平定位孔之间的动态数据。

5.如权利要求1所述的方法，其特征在于，所述步骤3中，通过设置阈值来分离各传感器的静态数据和动态数据。

6.如权利要求2所述的方法，其特征在于，所述步骤4进一步包括：

步骤41：基于分离得到三轴微加速度传感器和三轴地磁传感器的静态数据，利用三轴微加速度传感器和三轴地磁传感器在任意静止姿态下的三轴数据模值恒定的原理和遗传算法标定三轴地磁传感器和三轴微加速度传感器的零漂、比例因子、正交参数，得到校正后的三轴地磁传感器和三轴微加速度传感器的静态数据；

步骤42：基于步骤41校正后的三轴地磁传感器和三轴微加速度传感器的静态数据，对三轴地磁传感器到三轴微加速度传感器的坐标系的对准矩阵进行标定，并得到经过对准标定后的三轴地磁传感器和三轴微加速度传感器的静态数据；

步骤43：基于步骤3分离得到三轴微陀螺仪传感器的静态数据和步骤41中标定后的三轴微加速度传感器的静态数据，对三轴微陀螺仪传感器的零漂进行标定；

步骤44：基于步骤3分离得到三轴微陀螺仪传感器的动态数据和二轴无磁转台提供的旋转角度，对三轴陀螺仪传感器的比例因子和正交矩阵进行标定。

7.如权利要求6所述的方法，其特征在于，所述步骤41进一步包括：利用三轴微加速度传感器和三轴地磁传感器在任意静止姿态下的三轴数据模值恒定的原理，建立目标函数，采用遗传算法对所述目标函数进行数据拟合，以对三轴地磁传感器和三轴微加速度传感器的零漂、比例因子、正交参数进行标定。

8.如权利要求6所述的方法，其特征在于，所述步骤42进一步包括：利用三轴地磁传感器和三轴微加速度传感器的数据叉乘向量的相对旋转角度与实际的旋转角度相同的特性，建立目标函数，采用遗传算法对所述目标函数进行数据拟合，以对三轴地磁传感器到三轴微加速度传感器的坐标系的对准矩阵进行标定。

9.如权利要求6所述的方法，其特征在于，所述步骤43进一步包括：利用三轴微陀螺仪传感器的零漂与自身所受加速度的关联性建立目标函数，采用遗传算法对所述目标函数进行数据拟合，以对三轴微陀螺仪传感器的零漂进行标定。

10.如权利要求6所述的方法，其特征在于，所述步骤44进一步包括：根据三轴微陀螺仪传感器数据积分确定的相对角度与二轴无磁转台的实际旋转角度之间的误差模型，建立目标函数，采用遗传算法对所述目标函数进行求解，以对三轴陀螺仪传感器的比例因子和正交矩阵进行标定。

Images