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

摘要

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

说明书

技术领域

本发明涉及一种空间三维姿态测量方法，特别是涉及一种基于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\alpha }}=\frac{1}{k_{\mathrm{g\alpha }}}(g_{\alpha \_\mathrm{last}}-g_{\mathrm{d\alpha }}),a_{\mathrm{R\alpha }}=\frac{1}{k_{\mathrm{a\alpha }}}(a_{\alpha \_\mathrm{last}}-a_{\alpha 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_{\bar{k}}={\mathrm{Aq}}_{k-1}$和$P_{\bar{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{\Delta t}& -\frac{g_{\mathrm{Ry}}}{2}\mathrm{\Delta t}& -\frac{g_{\mathrm{Rz}}}{2}\mathrm{\Delta t}\\ \frac{g_{\mathrm{Rx}}}{2}\mathrm{\Delta t}& 1& \frac{g_{\mathrm{Rz}}}{2}\mathrm{\Delta t}& \frac{g_{\mathrm{Ry}}}{2}\mathrm{\Delta t}\\ \frac{g_{\mathrm{Ry}}}{2}\mathrm{\Delta t}& -\frac{g_{\mathrm{Rz}}}{2}\mathrm{\Delta t}& 1& \frac{g_{\mathrm{Rz}}}{2}\mathrm{\Delta t}\\ \frac{g_{\mathrm{Rz}}}{2}\mathrm{\Delta t}& \frac{g_{\mathrm{Ry}}}{2}\mathrm{\Delta t}& -\frac{g_{\mathrm{Rx}}}{2}\mathrm{\Delta 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_{\bar{k}}{H}^T{({\mathrm{HP}}_{\bar{k}}{H}^T+R)}^{-1},$观测更新$q_k=q_{\bar{k}}+K_k(z_k-{\mathrm{Hq}}_{\bar{k}})$和协方 差更新$P_k=(I-K_{k}H)P_{\bar{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维单位矩阵。