CN103776451A - 一种基于mems的高精度三维姿态惯性测量系统以及测量方法 - Google Patents

Abstract

一种基于MEMS的高精度三维姿态惯性测量系统以及测量方法，涉及一种基于MEMS的高精度三维姿态惯性测量方法。本发明是为了解决现有的三维姿态惯性测量设备采用传感器成本高、精度低的问题。三轴陀螺仪计传感器用于将测量的角速度数据发送给ARM处理器，三轴加速度计传感器用于将测量的加速度数据发送给ARM处理器，三轴磁强计传感器用于将测量的磁强数据发送给ARM处理器，温度传感器用于测量将获得的三轴陀螺仪计传感器温漂数据发送给ARM处理器，ARM处理器用来对接收到的数据分别进行前置低通数字滤波、前端数据处理和拓展卡尔曼滤波处理，获得欧拉角三维姿态惯性数据或者四元数三维姿态惯性数据。它可用于导航控制领域。

Description

一种基于MEMS的高精度三维姿态惯性测量系统以及测量方法

技术领域

本发明涉及一种空间三维姿态测量方法，特别是涉及一种基于MEMS的高精度三维姿态惯性测量方法。

背景技术

无人机、机器人、机械云台、车辆船舶、虚拟现实、人体运动分析等在近年都取得了迅猛的发展。而在这些应用中，三维姿态与方位的自主测量显得极为重要。现有的三维姿态与方位传感器中，MEMS传感器成本低廉，但是测量数据由于存在较大的零偏、温漂等误差较大不能满足以上应用对于精度的要求；而一些IMU模块，在精度上能满足要求，但是成本高昂，限制了在实际中的应用。

发明内容

本发明是为了解决现有的三维姿态惯性测量设备采用传感器成本高、精度低的问题。现提供一种基于MEMS的高精度三维姿态惯性测量系统以及该测量方法。

一种基于MEMS的高精度三维姿态惯性测量系统，它包括微机电式惯性测量器件，它还包括ARM处理器和温度传感器，

所述的微机电式惯性测量器件包括三轴陀螺仪计传感器、三轴加速度计传感器和三轴磁强计传感器，

三轴陀螺仪计传感器，用于测量获得角速度，并将测量获得的角速度数据发送给ARM处理器，

三轴加速度计传感器，用于测量获得加速度，并将测量获得的加速度数据发送给ARM处理器，

三轴磁强计传感器，用于测量获得磁强，并将测量获得的磁强数据发送给ARM处理器，

温度传感器，用于测量获得三轴陀螺仪计传感器的温漂，并将测量获得的温漂数据发送给ARM处理器，

ARM处理器，用来对接收到的角速度数据、加速度数据、磁强数据和温度传感器数据分别进行前置低通数字滤波、前端数据处理和拓展卡尔曼滤波处理，还用于对滤除外界高频干扰的所有数据进行数据融合，获得欧拉角三维姿态惯性数据或者四元数三维姿态惯性数据。

该系统采用一个ARM处理器采集三轴陀螺仪计传感器、三轴加速度计传感器和三轴磁强计传感器的测量数据及采集温度传感器获得三轴陀螺仪计传感器的温漂，再通过对测量的数据进行前置低通数字滤波与前端数据处理和进行拓展卡尔曼滤波，滤除外界高频干扰并进行数据融合，对外实时输出四元数或欧拉角三维姿态方位数据，该设备采用一个ARM处理器，集合一个三轴陀螺仪计传感器、一个三轴加速度计传感器和一个三轴磁强计传感器并装载一个温度传感器，其测量精度同比现有的采用方位传感器要提高2倍以上，并且该系统采用三轴陀螺仪计传感器、三轴加速度计传感器和三轴磁强计传感器同比现有的采用方位传感器的成本低。它可用于导航控制领域。

附图说明

图1为具体实施方式一所述的一种基于MEMS的高精度三维姿态惯性测量系统的原理示框图；

图2为具体实施方式五所述的一种基于MEMS的高精度三维姿态惯性测量系统的测量方法的流程图。

具体实施方式

具体实施方式一：参照图1具体说明本实施方式，本实施方式所述的一种基于MEMS的高精度三维姿态惯性测量系统，它包括微机电式惯性测量器件，它还包括ARM处理器4和温度传感器5，

所述的微机电式惯性测量器件包括三轴陀螺仪计传感器1、三轴加速度计传感器2和三轴磁强计传感器3，

三轴陀螺仪计传感器1，用于测量获得角速度，并将测量获得的角速度数据发送给ARM处理器4，

三轴加速度计传感器2，用于测量获得加速度，并将测量获得的加速度数据发送给ARM处理器4，

三轴磁强计传感器3，用于测量获得磁强，并将测量获得的磁强数据发送给ARM处理器4，

温度传感器5，用于测量获得三轴陀螺仪计传感器1的温漂，并将测量获得的温漂数据发送给ARM处理器4，

ARM处理器4，用来对接收到的角速度数据、加速度数据、磁强数据和温漂数据分别进行前置低通数字滤波、前端数据处理和拓展卡尔曼滤波处理，还用于对滤除外界高频干扰的所有数据进行数据融合，获得欧拉角三维姿态惯性数据输出或者四元数三维姿态惯性数据输出。

具体实施方式二：本实施方式与具体实施方式一所述的一种基于MEMS的高精度三维姿态惯性测量系统的不同点在于，ARM处理器4采用型号为STM32F405实现。

具体实施方式三：本实施方式与具体实施方式一所述的一种基于MEMS的高精度三维姿态惯性测量系统的不同点在于，三轴陀螺仪计传感器1采用型号为MPU6050实现。

具体实施方式四：本实施方式与具体实施方式一所述的一种基于MEMS的高精度三维姿态惯性测量系统的不同点在于，三轴加速度计传感器2和三轴磁强计传感器3采用型号为AK8975实现。

具体实施方式五：采用具体实施方式一所述的一种基于MEMS的高精度三维姿态惯性测量系统实现三维姿态惯性的测量方法，它包括以下步骤：

步骤一、将三轴陀螺仪计传感器1、三轴加速度计传感器2和三轴磁强计传感器3水平固定在转台上，使三轴陀螺仪计传感器1的敏感轴x、三轴加速度计传感器2的敏感轴x和三轴磁强计传感器3的敏感轴x相互平行，三轴陀螺仪计传感器1的敏感轴y、三轴加速度计传感器2的敏感轴y和三轴磁强计传感器3的敏感轴y相互平行，三轴陀螺仪计传感器1的敏感轴z、三轴加速度计传感器2的敏感轴z和三轴磁强计传感器3的敏感轴z相互平行，分别对三轴陀螺仪计传感器1、三轴加速度计传感器2和三轴磁强计传感器3进行标定，获取硬件平台的校准参数；

步骤二、系统进行初始化对准，ARM处理器4周期性读取三轴陀螺仪计传感器1敏感轴x、y、z轴的数据g_x，g_y，g_z、三轴加速度计传感器2敏感轴x、y、z轴的数据a_x，a_y，a_z和三轴磁强计传感器3敏感轴x、y、z轴的数据m_x,m_y,m_z和温度传感器的数据T；

步骤三、利用读到的三轴加速度计传感器2敏感轴的数据和三轴磁强计传感器3敏感轴的数据以及

利用俯仰角公式

滚转角公式

偏航角公式

四元数公式

和

进行欧拉角初始对准和四元数的初始化，获得初始化数据q₀,q₁,q₂,q₃，

其中，G₀为重力加速度；

步骤四、对步骤二中读取到的三轴陀螺仪计传感器1的各敏感轴数据、三轴加速度计传感器2的各敏感轴数据和三轴磁强计传感器3的各敏感轴数据进行前置低通滤波与前端数据处理，实现测量数据的精确化；

步骤五、利用步骤三得到的初始化数据q₀,q₁,q₂,q₃和步骤四得到的三轴加速度计传感器2精确化的数据采用拓展卡尔曼滤波器在△t时间间隔内进行拓展卡尔曼滤波的时间更新，得到预测四元数三维姿态惯性数据；

步骤六、利用步骤四得到的精确化的三轴加速度计传感器2数据和精确化的三轴磁强计传感器3数据，在上一时刻得到的预测四元数三维姿态惯性数据处做线性化展开，进行拓展卡尔曼滤波的测量更新；

步骤七、判断是否需要四元数三维姿态惯性数据输出，如果判断结果为是，则执行步骤八，如果判断结果为否，则执行步骤九；

步骤八、输出四元数三维姿态惯性数据，完成一次测量；

步骤九、将四元数三维姿态惯性数据转换成欧拉角三维姿态惯性数据进行输出，完成一次测量。

本实施方式中，利用精确化各传感器的测量数据，采用拓展卡尔曼滤波算法，基于四元数描述进行自适应滤波和数据融合，给出器件在相对于本地导航坐标系中的高精度的姿态和方位信息。

在实际应用中，可以通过不断的重复上述过程实现连续测量。

具体实施方式六：本实施方式与具体实施方式五所述的一种基于MEMS的高精度三维姿态惯性测量系统实现三维姿态惯性的测量方法的不同点在于，在步骤一中获得三轴陀螺仪计传感器1、三轴加速度计传感器2和三轴磁强计传感器3校准参数的过程为：

获得三轴陀螺仪计传感器1的校准参数的过程为：将电路板水平放置在转台上，首先测量系统上电后进行ARM处理器和周围电路、总线的初始化后，ARM处理器连续读取三轴陀螺仪计传感器各敏感轴（x，y，z）的数据和温度传感器5的数据，得到四个数组g_x[n]，g_y[n]，g_z[n]和T[n]，

利用Matlab数据拟合工具箱按公式：

g_dα=a_αT³+b_αT²+c_αT+d_α，

拟合陀螺仪的零漂和温漂，得到陀螺仪的零漂d_x,d_y,d_z，一次温漂常数c_x,c_y,c_z，二次温漂常数b_x,b_y,b_z和三次温漂常数a_x,a_y,a_z，

式中，g_dα为α轴上的漂移，包括零漂和温漂，α=x,y,z，

然后依次将三轴陀螺仪计传感器1的敏感轴x，y，z与转台的转轴保持平行，以不同的标称转速转动转台，记录下各转速下三轴陀螺仪计传感器1的敏感轴x，y，z的输出g_α，并以g_α=k_gαg_Bα+g_dα，α=x,y,z的方式拟合陀螺仪的量程系数k_gα，α=x,y,z，

式中，g_α为敏感轴上的实际输出，g_Bα为标称转速，g_dα为陀螺仪漂移；

获得三轴加速度计传感器2的校准参数的过程为：从不同角度将电路板静置，待静置后用ARM处理器连续读取三轴加速度计各敏感轴（x，y，z）的数据a_x[n]，a_y[n]，a_z[n]，

利用Matlab工具箱，按照(k_axa_x[n]+a_x0)²+(k_aya_y[n]+a_y0)²+(k_aza_z[n]+a_z0)²=G₀ ²的方式拟合出加速度计的量程系数k_aα和零偏常数a_α0，α=x,y,z，其中G₀为重力加速度记；

获得三轴磁强计传感器3的校准参数的过程为：将磁强计放置在转台上，匀速转速一圈，用ARM处理器连续读取三轴磁强计传感器3各敏感轴（x，y，z）的数据，得到一个数组m[n]={m_x[n],m_y[n],m_z[n]}，设取磁场向量模为1，利用Matlab工具箱，按照(m-c)^T×(m^T×U)^T(m-c)=1的方式进行最小二乘拟合，其中，m=m[n]，三维向量c=(c_x0,c_y0,c_z0)是磁场强度的椭球球心偏移量，c_α0，α=x,y,z为各轴上的偏移量；矩阵U是标度因子与非正交度校正矩阵。

具体实施方式七：本实施方式与具体实施方式五所述的一种基于MEMS的高精度三维姿态惯性测量系统实现三维姿态惯性的测量方法的不同点在于，对步骤四中的三轴陀螺仪计传感器1初始化数据、三轴加速度计传感器2初始化数据和三轴磁强计传感器3初始化数据进行前置低通滤波与前端数据处理的过程为：

对前置低通滤波进行一阶低通数字滤波，得到：

g_{α_last}=λ_gg_{α_last}+(1-λ_g)g_α，a_{α_last}=λ_aa_{α_last}+(1-λ_a)a_α，m_{α_last}=λ_mm_{α_last}+(1-λ_m)m_α，α=x,y,z，0≤λ_g≤1,0≤λ_a≤1，0≤λ_m≤1，

其中，λ_g,λ_a,λ_m分别为三轴陀螺仪计传感器1、三轴加速度计传感器2和三轴磁强计传感器3的低通滤波常数，g_{α_last}为上一次低通滤波后的三轴陀螺仪计传感器1α轴数据，g_α是最新一次测量得到的三轴陀螺仪计传感器1α各轴原始数据，a_{α_last}为上一次低通滤波后

的三轴加速度计传感器2α轴数据，a_α是最新一次测量得到的三轴加速度计传感器2α各轴原始数据，m_{α_last}为上一次低通滤波后的三轴磁强计传感器3α各轴数据，m_α是最新一次测量得到的三轴磁强计传感器3α各轴原始数据，

根据公式： $g_{\mathrm{R\α}}=\frac{1}{k_{\mathrm{g\α}}}(g_{\mathrm{\α}\_\mathrm{last}}-g_{\mathrm{d\α}}),a_{\mathrm{R\α}}=\frac{1}{k_{\mathrm{a\α}}}(a_{\mathrm{\α}\_\mathrm{last}}-a_{\mathrm{\α}0}),$ m_R=U(m_{α_last}-c)，α=x,y,z，

对测量数据进行动态补偿，实现测量数据的精确化，

式中，g_dα为陀螺仪实时漂移，g_{α_last}为上一次测量得到的三轴陀螺仪计传感器1α各轴原始数据，a_{α_last}为上一次低通滤波后的三轴加速度计传感器2α轴数据，m_{α_last}为上一次低通滤波后的三轴磁强计传感器3α各轴数据，g_Rα，a_Rα，m_R为精确化后的数据。

具体实施方式八：本实施方式与具体实施方式五所述的一种基于MEMS的高精度三维姿态惯性测量系统三维姿态惯性的测量方法的不同点在于，对步骤五中利用步骤三得到的初始化数据q₀,q₁,q₂,q₃和步骤四得到的三轴加速度计传感器2精确化的数据采用拓展卡尔曼滤波器在△t时间间隔内进行拓展卡尔曼滤波的时间更新，得到预测四元数三维姿态惯性数据的过程为：

根据公式：

$q_{\stackrel{\‾}{k}}={\mathrm{Aq}}_{k-1}$ 和 $P_{\stackrel{\‾}{k}}={\mathrm{AP}}_{k-1}{A}^T+Q,$

得到预测四元数

和卡尔曼滤波的协方差预测矩阵

式中，

为当前更新时刻的一步预测四元数，q_k-1=[q_k-10,q_k-11,q_k-12,q_k-13]^T为上一更新时刻的四元数，矩阵 $A=\left[\begin{array}{cccc}1& -\frac{g_{\mathrm{Rx}}}{2}\mathrm{\Δt}& -\frac{g_{\mathrm{Ry}}}{2}\mathrm{\Δt}& -\frac{g_{\mathrm{Rz}}}{2}\mathrm{\Δt}\\ \frac{g_{\mathrm{Rx}}}{2}\mathrm{\Δt}& 1& \frac{g_{\mathrm{Rz}}}{2}\mathrm{\Δt}& \frac{g_{\mathrm{Ry}}}{2}\mathrm{\Δt}\\ \frac{g_{\mathrm{Ry}}}{2}\mathrm{\Δt}& -\frac{g_{\mathrm{Rz}}}{2}\mathrm{\Δt}& 1& \frac{g_{\mathrm{Rz}}}{2}\mathrm{\Δt}\\ \frac{g_{\mathrm{Rz}}}{2}\mathrm{\Δt}& \frac{g_{\mathrm{Ry}}}{2}\mathrm{\Δt}& -\frac{g_{\mathrm{Rx}}}{2}\mathrm{\Δt}& 1\end{array}\right],$ △t为卡尔曼滤波的更新时间间隔，△t取定时器的时长，

为卡尔曼滤波的协方差矩阵预测，P_k-1为上一步的协方差矩阵，Q为四维过程激励噪声协方差矩阵。

具体实施方式九：本实施方式与具体实施方式五所述的一种基于MEMS的高精度三维姿态惯性测量系统实现三维姿态惯性的测量方法的不同点在于，对步骤六中的利用步骤四得到的精确化的三轴加速度计传感器2数据和精确化的三轴磁强计传感器3数据，在上一时刻得到的预测四元数三维姿态惯性数据处做线性化展开，进行拓展卡尔曼滤波的测量更新的过程为：

将观测方程放在q_k-1=[q_k-10,q_k-11,q_k-12,q_k-13]^T附近进行泰勒展开并取线性化部分得到观测矩阵H，再利用观测矩阵H进行更新，

得到卡尔曼增益矩阵 $K_k=P_{\stackrel{\‾}{k}}{H}^T{({\mathrm{HP}}_{\stackrel{\‾}{k}}{H}^T+R)}^{-1},$ 观测更新 $q_k=q_{\stackrel{\‾}{k}}+K_k(z_k-{\mathrm{Hq}}_{\stackrel{\‾}{k}})$ 和协方差更新 $P_k=(I-K_{k}H)P_{\stackrel{\‾}{k}},$

式中， $C_n^b=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]$ 为从大地坐标系到测量本地坐标系的转移矩阵，z为重力加速度和地磁观测量，z=a_Rα=m_R，v是重力加速度和地磁的参考矢量，分别取单位向量即为[0,0,1]^T和[0,1,0]^T，R为三维观测噪声协方差矩阵，I为4×4维单位矩阵。

Claims (9)

1.一种基于MEMS的高精度三维姿态惯性测量系统，它包括微机电式惯性测量器件，其特征在于，它还包括ARM处理器（4）和温度传感器（5），

所述的微机电式惯性测量器件包括三轴陀螺仪计传感器（1）、三轴加速度计传感器（2）和三轴磁强计传感器（3），

三轴陀螺仪计传感器（1），用于测量获得角速度，并将测量获得的角速度数据发送给ARM处理器（4），

三轴加速度计传感器（2），用于测量获得加速度，并将测量获得的加速度数据发送给ARM处理器（4），

三轴磁强计传感器（3），用于测量获得磁强，并将测量获得的磁强数据发送给ARM处理器（4），

温度传感器（5），用于测量获得三轴陀螺仪计传感器（1）的温漂，并将测量获得的温漂数据发送给ARM处理器（4），

ARM处理器（4），用来对接收到的角速度数据、加速度数据、磁强数据和温漂数据分别进行前置低通数字滤波、前端数据处理和拓展卡尔曼滤波处理，还用于对滤除外界高频干扰的所有数据进行数据融合，获得欧拉角三维姿态惯性数据输出或者四元数三维姿态惯性数据输出。

2.根据权利要求1所述的一种基于MEMS的高精度三维姿态惯性测量系统，其特征在于，ARM处理器采用型号为STM32F405实现。

3.根据权利要求1所述的一种基于MEMS的高精度三维姿态惯性测量系统，其特征在于，三轴陀螺仪传感器采用型号为MPU6050实现。

4.根据权利要求1所述的一种基于MEMS的高精度三维姿态惯性测量系统，其特征在于，三轴加速度计传感器和三轴磁强计传感器采用型号为AK8975实现。

5.采用权利要求1的一种基于MEMS的高精度三维姿态惯性测量系统实现三维姿态惯性的测量方法，其特征在于，它包括以下步骤：

步骤一、将三轴陀螺仪计传感器（1）、三轴加速度计传感器（2）和三轴磁强计传感器（3）水平固定在转台上，使三轴陀螺仪计传感器（1）的敏感轴x、三轴加速度计传感器（2）的敏感轴x和三轴磁强计传感器（3）的敏感轴x相互平行，三轴陀螺仪计传感器（1）的敏感轴y、三轴加速度计传感器（2）的敏感轴y和三轴磁强计传感器（3）的敏感轴y相互平行，三轴陀螺仪计传感器（1）的敏感轴z、三轴加速度计传感器（2）的敏感轴z和三轴磁强计传感器（3）的敏感轴z相互平行，分别对三轴陀螺仪计传感器（1）、三轴加速度计传感器（2）和三轴磁强计传感器（3）进行标定，获取硬件平台的校准参数；

步骤二、系统进行初始化对准，ARM处理器（4）周期性读取三轴陀螺仪计传感器（1）敏感轴x、y、z轴的数据g_x，g_y，g_z、三轴加速度计传感器（2）敏感轴x、y、z轴的数据a_x，a_y，a_z和三轴磁强计传感器（3）敏感轴x、y、z轴的数据m_x,m_y,m_z和温度传感器的数据T；

步骤三、利用读到的三轴加速度计传感器（2）敏感轴的数据和三轴磁强计传感器（3）敏感轴的数据以及

利用俯仰角公式

滚转角公式

偏航角公式

四元数公式

和

进行欧拉角初始对准和四元数的初始化，获得初始化数据q₀,q₁,q₂,q₃，

其中，G₀为重力加速度；

步骤四、对步骤二中读取到的三轴陀螺仪计传感器（1）的各敏感轴数据、三轴加速度计传感器（2）的各敏感轴数据和三轴磁强计传感器（3）的各敏感轴数据进行前置低通滤波与前端数据处理，实现测量数据的精确化；

步骤五、利用步骤三得到的初始化数据q₀,q₁,q₂,q₃和步骤四得到的三轴加速度计传感器（2）精确化的数据采用拓展卡尔曼滤波器在△t时间间隔内进行拓展卡尔曼滤波的时间更新，得到预测四元数三维姿态惯性数据；

步骤六、利用步骤四得到的精确化的三轴加速度计传感器（2）数据和精确化的三轴磁强计传感器（3）数据，在上一时刻得到的预测四元数三维姿态惯性数据处做线性化展开，进行拓展卡尔曼滤波的测量更新；

步骤七、判断是否需要四元数三维姿态惯性数据输出，如果判断结果为是，则执行步骤八，如果判断结果为否，则执行步骤九；

步骤八、输出四元数三维姿态惯性数据，完成一次测量；

步骤九、将四元数三维姿态惯性数据转换成欧拉角三维姿态惯性数据进行输出，完成一次测量。

6.根据权利要求5所述的一种基于MEMS的高精度三维姿态惯性测量系统实现三维姿态惯性的测量方法，其特征在于，在步骤一中获得三轴陀螺仪计传感器（1）、三轴加速度计传感器（2）和三轴磁强计传感器（3）校准参数的过程为：

获得三轴陀螺仪计传感器（1）的校准参数的过程为：将电路板水平放置在转台上，首先测量系统上电后进行ARM处理器和周围电路、总线的初始化后，ARM处理器连续读取三轴陀螺仪计传感器各敏感轴（x，y，z）的数据和温度传感器（5）的数据，得到四个数组g_x[n]，g_y[n]，g_z[n]和T[n]，

利用Matlab数据拟合工具箱按公式：

g_dα=a_αT³+b_αT²+c_αT+d_α，

拟合陀螺仪的零漂和温漂，得到陀螺仪的零漂d_x,d_y,d_z，一次温漂常数c_x,c_y,c_z，二次温漂常数b_x,b_y,b_z和三次温漂常数a_x,a_y,a_z，

式中，g_dα为α轴上的漂移，包括零漂和温漂，α=x,y,z，

然后依次将三轴陀螺仪计传感器（1）的敏感轴x，y，z与转台的转轴保持平行，以不同的标称转速转动转台，记录下各转速下三轴陀螺仪计传感器（1）的敏感轴x，y，z的输出g_α，并以g_α=k_gαg_Bα+g_dα，α=x,y,z的方式拟合陀螺仪的量程系数k_gα，α=x,y,z，

式中，g_α为敏感轴上的实际输出，g_Bα为标称转速，g_dα为陀螺仪漂移；

获得三轴加速度计传感器（2）的校准参数的过程为：从不同角度将电路板静置，待静置后用ARM处理器连续读取三轴加速度计各敏感轴（x，y，z）的数据a_x[n]，a_y[n]，a_z[n]，

利用Matlab工具箱，按照(k_axa_x[n]+a_x0)²+(k_aya_y[n]+a_y0)²+(k_aza_z[n]+a_z0)²=G₀ ²的方式拟合出加速度计的量程系数k_aα和零偏常数a_α0，α=x,y,z，其中G₀为重力加速度记；

获得三轴磁强计传感器（3）的校准参数的过程为：将磁强计放置在转台上，匀速转速一圈，用ARM处理器连续读取三轴磁强计传感器（3）各敏感轴（x，y，z）的数据，得到一个数组m[n]={m_x[n],m_y[n],m_z[n]}，设取磁场向量模为1，利用Matlab工具箱，按照(m-c)^T×(m^T×U)^T(m-c)=1的方式进行最小二乘拟合，其中，m=m[n]，三维向量c=(c_x0,c_y0,c_z0)是磁场强度的椭球球心偏移量，c_α0，α=x,y,z为各轴上的偏移量；矩阵U是标度因子与非正交度校正矩阵。

7.根据权利要求5所述的一种基于MEMS的高精度三维姿态惯性测量系统实现三维姿态惯性的测量方法，其特征在于，对步骤四中的三轴陀螺仪计传感器（1）初始化数据、三轴加速度计传感器（2）初始化数据和三轴磁强计传感器（3）初始化数据进行前置低通滤波与前端数据处理的过程为：

对前置低通滤波进行一阶低通数字滤波，得到：

g_{α_last}=λ_gg_{α_last}+(1-λ_g)g_α，a_{α_last}=λ_aa_{α_last}+(1-λ_a)a_α，m_{α_last}=λ_mm_{α_last}+(1-λ_m)m_α，α=x,y,z，0≤λ_g≤1,0≤λ_a≤1，0≤λ_m≤1，

其中，λ_g,λ_a,λ_m分别为三轴陀螺仪计传感器（1）、三轴加速度计传感器（2）和三轴磁强计传感器（3）的低通滤波常数，g_{α_last}为上一次低通滤波后的三轴陀螺仪计传感器（1）α轴数据，g_α是最新一次测量得到的三轴陀螺仪计传感器（1）α各轴原始数据，a_{α_last}为上一次低通滤波后的三轴加速度计传感器（2）α轴数据，a_α是最新一次测量得到的三轴加速度计传感器（2）α各轴原始数据，m_{α_last}为上一次低通滤波后的三轴磁强计传感器（3）α各轴数据，m_α是最新一次测量得到的三轴磁强计传感器（3）α各轴原始数据，

根据公式： $g_{\mathrm{R\α}}=\frac{1}{k_{\mathrm{g\α}}}(g_{\mathrm{\α}\_\mathrm{last}}-g_{\mathrm{d\α}}),a_{\mathrm{R\α}}=\frac{1}{k_{\mathrm{a\α}}}(a_{\mathrm{\α}\_\mathrm{last}}-a_{\mathrm{\α}0}),$ m_R=U(m_{α_last}-c)，α=x,y,z，

对测量数据进行动态补偿，实现测量数据的精确化，

式中，g_dα为陀螺仪实时漂移，g_{α_last}为上一次测量得到的三轴陀螺仪计传感器（1）α各轴原始数据，a_{α_last}为上一次低通滤波后的三轴加速度计传感器（2）α轴数据，m_{α_last}为上一次低通滤波后的三轴磁强计传感器（3）α各轴数据，g_Rα，a_Rα，m_R为精确化后的数据。

8.根据权利要求5所述的一种基于MEMS的高精度三维姿态惯性测量系统实现三维姿态惯性的测量方法，其特征在于，对步骤五中利用步骤三得到的初始化数据q₀,q₁,q₂,q₃和步骤四得到的三轴加速度计传感器（2）精确化的数据采用拓展卡尔曼滤波器在△t时间间隔内进行拓展卡尔曼滤波的时间更新，得到预测四元数三维姿态惯性数据的过程为：

根据公式：

$q_{\stackrel{\‾}{k}}={\mathrm{Aq}}_{k-1}$ 和 $P_{\stackrel{\‾}{k}}={\mathrm{AP}}_{k-1}{A}^T+Q,$

得到预测四元数

和卡尔曼滤波的协方差预测矩阵

式中，

为当前更新时刻的一步预测四元数，q_k-1=[q_k-10,q_k-11,q_k-12,q_k-13]^T为上一更新时刻的四元数，矩阵 $A=\left[\begin{array}{cccc}1& -\frac{g_{\mathrm{Rx}}}{2}\mathrm{\Δt}& -\frac{g_{\mathrm{Ry}}}{2}\mathrm{\Δt}& -\frac{g_{\mathrm{Rz}}}{2}\mathrm{\Δt}\\ \frac{g_{\mathrm{Rx}}}{2}\mathrm{\Δt}& 1& \frac{g_{\mathrm{Rz}}}{2}\mathrm{\Δt}& \frac{g_{\mathrm{Ry}}}{2}\mathrm{\Δt}\\ \frac{g_{\mathrm{Ry}}}{2}\mathrm{\Δt}& -\frac{g_{\mathrm{Rz}}}{2}\mathrm{\Δt}& 1& \frac{g_{\mathrm{Rz}}}{2}\mathrm{\Δt}\\ \frac{g_{\mathrm{Rz}}}{2}\mathrm{\Δt}& \frac{g_{\mathrm{Ry}}}{2}\mathrm{\Δt}& -\frac{g_{\mathrm{Rx}}}{2}\mathrm{\Δt}& 1\end{array}\right],$ △t为卡尔曼滤波的更新时间间隔，△t取定时器的时长，

为卡尔曼滤波的协方差矩阵预测，P_k-1为上一步的协方差矩阵，Q为四维过程激励噪声协方差矩阵。

9.根据权利要求5所述的一种基于MEMS的高精度三维姿态惯性测量系统实现三维姿态惯性的测量方法，其特征在于，对步骤六中的利用步骤四得到的精确化的三轴加速度计传感器（2）数据和精确化的三轴磁强计传感器（3）数据，在上一时刻得到的预测四元数三维姿态惯性数据处做线性化展开，进行拓展卡尔曼滤波的测量更新的过程为：

将观测方程

放在q_k-1=[q_k-10,q_k-11,q_k-12,q_k-13]^T附近进行泰勒展开并取线性化部分得到观测矩阵H，再利用观测矩阵H进行更新，

得到卡尔曼增益矩阵 $K_k=P_{\stackrel{\‾}{k}}{H}^T{({\mathrm{HP}}_{\stackrel{\‾}{k}}{H}^T+R)}^{-1},$ 观测更新 $q_k=q_{\stackrel{\‾}{k}}+K_k(z_k-{\mathrm{Hq}}_{\stackrel{\‾}{k}})$ 和协方差更新 $P_k=(I-K_{k}H)P_{\stackrel{\‾}{k}},$

式中， $C_n^b=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]$ 为从大地坐标系到测量本地坐标系的转移矩阵，z为重力加速度和地磁观测量，z=a_Rα=m_R，v是重力加速度和地磁的参考矢量，分别取单位向量即为[0,0,1]^T和[0,1,0]^T，R为三维观测噪声协方差矩阵，I为4×4维单位矩阵。

Images