亚星游戏官网-www.yaxin868.com

一种基于mems的高精度三维姿态惯性测量系统以及测量方法
【专利摘要】一种基于MEMS的高精度三维姿态惯性测量系统以及测量方法，涉及一种基于MEMS的高精度三维姿态惯性测量方法。本发明是为了解决现有的三维姿态惯性测量设备采用传感器成本高、精度低的问题。三轴陀螺仪计传感器用于将测量的角速度数据发送给ARM处理器，三轴加速度计传感器用于将测量的加速度数据发送给ARM处理器，三轴磁强计传感器用于将测量的磁强数据发送给ARM处理器，温度传感器用于测量将获得的三轴陀螺仪计传感器温漂数据发送给ARM处理器，ARM处理器用来对接收到的数据分别进行前置低通数字滤波、前端数据处理和拓展卡尔曼滤波处理，获得欧拉角三维姿态惯性数据或者四元数三维姿态惯性数据。它可用于导航控制领域。
【专利说明】一种基于MEMS的高精度三维姿态惯性测量系统以及测量方法
【技术领域】
[0001]本发明涉及一种空间三维姿态测量方法，特别是涉及一种基于MEMS的高精度三维姿态惯性测量方法。
【背景技术】
[0002]无人机、机器人、机械云台、车辆船舶、虚拟现实、人体运动分析等在近年都取得了迅猛的发展。而在这些应用中，三维姿态与方位的自主测量显得极为重要。现有的三维姿态与方位传感器中，MEMS传感器成本低廉，但是测量数据由于存在较大的零偏、温漂等误差较大不能满足以上应用对于精度的要求；而一些MU模块，在精度上能满足要求，但是成本高昂，限制了在实际中的应用。

【发明内容】

[0003]本发明是为了解决现有的三维姿态惯性测量设备采用传感器成本高、精度低的问题。现提供一种基于MEMS的高精度三维姿态惯性测量系统以及该测量方法。
[0004]一种基于MEMS的高精度三维姿态惯性测量系统，它包括微机电式惯性测量器件，它还包括ARM处理器和温度传感器，
[0005]所述的微机电式惯性测量器件包括三轴陀螺仪计传感器、三轴加速度计传感器和三轴磁强计传感器，
[0006]三轴陀螺仪计传感器，用于测量获得角速度，并将测量获得的角速度数据发送给ARM处理器，
[0007]三轴加速度计传感器，用于测量获得加速度，并将测量获得的加速度数据发送给ARM处理器，
[0008]三轴磁强计传感器，用于测量获得磁强，并将测量获得的磁强数据发送给ARM处理器，
[0009]温度传感器，用于测量获得三轴陀螺仪计传感器的温漂，并将测量获得的温漂数据发送给ARM处理器，
[0010]ARM处理器，用来对接收到的角速度数据、加速度数据、磁强数据和温度传感器数据分别进行前置低通数字滤波、前端数据处理和拓展卡尔曼滤波处理，还用于对滤除外界高频干扰的所有数据进行数据融合，获得欧拉角三维姿态惯性数据或者四元数三维姿态惯性数据。
[0011]该系统采用一个ARM处理器采集三轴陀螺仪计传感器、三轴加速度计传感器和三轴磁强计传感器的测量数据及采集温度传感器获得三轴陀螺仪计传感器的温漂，再通过对测量的数据进行前置低通数字滤波与前端数据处理和进行拓展卡尔曼滤波，滤除外界高频干扰并进行数据融合，对外实时输出四元数或欧拉角三维姿态方位数据，该设备采用一个ARM处理器，集合一个三轴陀螺仪计传感器、一个三轴加速度计传感器和一个三轴磁强计传感器并装载一个温度传感器，其测量精度同比现有的采用方位传感器要提高2倍以上，并且该系统采用三轴陀螺仪计传感器、三轴加速度计传感器和三轴磁强计传感器同比现有的采用方位传感器的成本低。它可用于导航控制领域。
【专利附图】

【附图说明】
[0012]图1为【具体实施方式】一所述的一种基于MEMS的高精度三维姿态惯性测量系统的原理示框图；
[0013]图2为【具体实施方式】五所述的一种基于MEMS的高精度三维姿态惯性测量系统的测量方法的流程图。
【具体实施方式】
[0014]【具体实施方式】一:参照图1具体说明本实施方式，本实施方式所述的一种基于MEMS的高精度三维姿态惯性测量系统，它包括微机电式惯性测量器件，它还包括ARM处理器4和温度传感器5，
[0015]所述的微机电式惯性测量器件包括三轴陀螺仪计传感器1、三轴加速度计传感器
2和三轴磁强计传感器3，
[0016]三轴陀螺仪计传感器1，用于测量获得角速度，并将测量获得的角速度数据发送给ARM处理器4，
[0017]三轴加速度计传感器2，用于测量获得加速度，并将测量获得的加速度数据发送给ARM处理器4，
[0018]三轴磁强计传感器3，用于测量获得磁强，并将测量获得的磁强数据发送给ARM处理器4，
[0019]温度传感器5，用于测量获得三轴陀螺仪计传感器I的温漂，并将测量获得的温漂数据发送给ARM处理器4，
[0020]ARM处理器4，用来对接收到的角速度数据、加速度数据、磁强数据和温漂数据分别进行前置低通数字滤波、前端数据处理和拓展卡尔曼滤波处理，还用于对滤除外界高频干扰的所有数据进行数据融合，获得欧拉角三维姿态惯性数据输出或者四元数三维姿态惯性数据输出。
[0021]【具体实施方式】二:本实施方式与【具体实施方式】一所述的一种基于MEMS的高精度三维姿态惯性测量系统的不同点在于，ARM处理器4采用型号为STM32F405实现。
[0022]【具体实施方式】三:本实施方式与【具体实施方式】一所述的一种基于MEMS的高精度三维姿态惯性测量系统的不同点在于，三轴陀螺仪计传感器I采用型号为MPU6050实现。
[0023]【具体实施方式】四:本实施方式与【具体实施方式】一所述的一种基于MEMS的高精度三维姿态惯性测量系统的不同点在于，三轴加速度计传感器2和三轴磁强计传感器3采用型号为AK8975实现。
[0024]【具体实施方式】五:米用【具体实施方式】一所述的一种基于MEMS的高精度三维姿态惯性测量系统实现三维姿态惯性的测量方法，它包括以下步骤:
[0025]步骤一、将三轴陀螺仪计传感器1、三轴加速度计传感器2和三轴磁强计传感器3水平固定在转台上，使三轴陀螺仪计传感器I的敏感轴X、三轴加速度计传感器2的敏感轴X和三轴磁强计传感器3的敏感轴X相互平行,三轴陀螺仪计传感器I的敏感轴y、三轴加速度计传感器2的敏感轴y和三轴磁强计传感器3的敏感轴y相互平行，三轴陀螺仪计传感器I的敏感轴z、三轴加速度计传感器2的敏感轴z和三轴磁强计传感器3的敏感轴z相互平行，分别对三轴陀螺仪计传感器1、三轴加速度计传感器2和三轴磁强计传感器3进行标定，获取硬件平台的校准参数；
[0026]步骤二、系统进行初始化对准，ARM处理器4周期性读取三轴陀螺仪计传感器I敏感轴X、y、z轴的数据gx, gy, gz、三轴加速度计传感器2敏感轴X、y、z轴的数据ax, ay, az和三轴磁强计传感器3敏感轴X、y、z轴的数据mx, my, mz和温度传感器的数据T ；
[0027]步骤三、利用读到的三轴加速度计传感器2敏感轴的数据和三轴磁强计传感器3敏感轴的数据以及
[0028]利用俯仰角公式
【权利要求】
1.一种基于MEMS的高精度三维姿态惯性测量系统，它包括微机电式惯性测量器件，其特征在于，它还包括ARM处理器(4 )和温度传感器(5 )， 所述的微机电式惯性测量器件包括三轴陀螺仪计传感器(I)、三轴加速度计传感器(2)和三轴磁强计传感器(3)， 三轴陀螺仪计传感器(1)，用于测量获得角速度，并将测量获得的角速度数据发送给ARM处理器(4)， 三轴加速度计传感器(2)，用于测量获得加速度，并将测量获得的加速度数据发送给ARM处理器(4)， 三轴磁强计传感器(3)，用于测量获得磁强，并将测量获得的磁强数据发送给ARM处理器⑷， 温度传感器(5)，用于测量获得三轴陀螺仪计传感器(I)的温漂，并将测量获得的温漂数据发送给ARM处理器(4)， ARM处理器(4)，用来对接收到的角速度数据、加速度数据、磁强数据和温漂数据分别进行前置低通数字滤波、前端数据处理和拓展卡尔曼滤波处理，还用于对滤除外界高频干扰的所有数据进行数据融合，获得欧拉角三维姿态惯性数据输出或者四元数三维姿态惯性数据输出。
2.根据权利要求1所述的一种基于MEMS的高精度三维姿态惯性测量系统，其特征在于，ARM处理器采用型号为STM32F405实现。
3.根据权利要求1所述的一种基于MEMS的高精度三维姿态惯性测量系统，其特征在于，三轴陀螺仪传感器采用型号为MPU6050实现。
4.根据权利要求1所述的一种基于MEMS的高精度三维姿态惯性测量系统，其特征在于，三轴加速度计传感器和三轴磁强计传感器采用型号为AK8975实现。
5.采用权利要求1的一种基于MEMS的高精度三维姿态惯性测量系统实现三维姿态惯性的测量方法，其特征在于，它包括以下步骤: 步骤一、将三轴陀螺仪计传感器(I)、三轴加速度计传感器(2)和三轴磁强计传感器(3 )水平固定在转台上，使三轴陀螺仪计传感器(I)的敏感轴X、三轴加速度计传感器(2 )的敏感轴X和三轴磁强计传感器(3)的敏感轴X相互平行，三轴陀螺仪计传感器(I)的敏感轴y、三轴加速度计传感器(2)的敏感轴y和三轴磁强计传感器(3)的敏感轴y相互平行，三轴陀螺仪计传感器(I)的敏感轴z、三轴加速度计传感器(2)的敏感轴z和三轴磁强计传感器(3)的敏感轴z相互平行，分别对三轴陀螺仪计传感器(I)、三轴加速度计传感器(2)和三轴磁强计传感器(3)进行标定，获取硬件平台的校准参数； 步骤二、系统进行初始化对准，ARM处理器(4)周期性读取三轴陀螺仪计传感器(I)敏感轴X、y、z轴的数据gx, gy, gz、三轴加速度计传感器(2)敏感轴X、y、z轴的数据ax, ay, az和三轴磁强计传感器(3)敏感轴X、y、z轴的数据mx, my, mz和温度传感器的数据T ； 步骤三、利用读到的三轴加速度计传感器(2)敏感轴的数据和三轴磁强计传感器(3)敏感轴的数据以及 利用俯仰角公式
6.根据权利要求5所述的一种基于MEMS的高精度三维姿态惯性测量系统实现三维姿态惯性的测量方法，其特征在于，在步骤一中获得三轴陀螺仪计传感器(I )、三轴加速度计传感器(2)和三轴磁强计传感器(3)校准参数的过程为: 获得三轴陀螺仪计传感器(I)的校准参数的过程为:将电路板水平放置在转台上，首先测量系统上电后进行ARM处理器和周围电路、总线的初始化后，ARM处理器连续读取三轴陀螺仪计传感器各敏感轴(X，1，z)的数据和温度传感器(5)的数据，得到四个数组&[11]，gy[n], gz[n]和 T [η], 利用Matlab数据拟合工具箱按公式: gda=aaT3+baT2+caT+da, 拟合陀螺仪的零漂和温漂，得到陀螺仪的零漂dx，dy, dz，一次温漂常数cx，cy, cz, 二次温漂常数bx, by, bz和三次温漂常数ax, ay, az,式中，gda为α轴上的漂移，包括零漂和温漂，a=x，y，z, 然后依次将三轴陀螺仪计传感器(I)的敏感轴X，1，Z与转台的转轴保持平行，以不同的标称转速转动转台，记录下各转速下三轴陀螺仪计传感器(I)的敏感轴x,y, z的输出ga,并以ga=kgagBa+gda，a =x, y, z的方式拟合陀螺仪的量程系数kga，a =x, y, z, 式中，ga为敏感轴上的实际输出，gBa为标称转速，gda为陀螺仪漂移； 获得三轴加速度计传感器(2)的校准参数的过程为:从不同角度将电路板静置，待静置后用ARM处理器连续读取三轴加速度计各敏感轴(X, y, z)的数据ax[n], ay[n], az[n],
利用 Matlab 工具箱，按照(kaxax [n] +ax0)2+ (kayay [n] +ay0)2+ (kazaz [η] +az0)2=G02 的方式拟合出加速度计的量程系数kaa和零偏常数aa(l，a =χ, y, z，其中Gtl为重力加速度记； 获得三轴磁强计传感器(3)的校准参数的过程为:将磁强计放置在转台上，匀速转速一圈，用ARM处理器连续读取三轴磁强计传感器(3)各敏感轴(X，y, z)的数据，得到一个数组m[n] = {mx[n], my[n], mz[n]},设取磁场向量模为I,利用Matlab工具箱，按照(m_c)tX (mTXU)T(m-c)=l的方式进行最小二乘拟合,其中，m=m[n],三维向量c=(cxCI, cy0, cz0)是磁场强度的椭球球心偏移量，ca0, a =x, y, z为各轴上的偏移量；矩阵U是标度因子与非正交度校正矩阵。
7.根据权利要求5所述的一种基于MEMS的高精度三维姿态惯性测量系统实现三维姿态惯性的测量方法，其特征在于，对步骤四中的三轴陀螺仪计传感器(I)初始化数据、三轴加速度计传感器(2)初始化数据和三轴磁强计传感器(3)初始化数据进行前置低通滤波与前端数据处理的过程为: 对前置低通滤波进行一阶低通数字滤波，得到:
S a _last= λ gga Iast+ (1- λ g) g α，a a last-λ aaa —last+ (1-λ a) a α，ma last-λ mma _last+ (1-λ m)ma, a =X, y, Z,0 ≤λg≤ 1,0≤ λa≤ 1,0≤ λm≤l, 其中，Xg, Xa, λπ分别为三轴陀螺仪计传感器(I)、三轴加速度计传感器(2)和三轴磁强计传感器(3)的低通滤波常数，ga—last为上一次低通滤波后的三轴陀螺仪计传感器(I) α轴数据，ga是最新一次测量得到的三轴陀螺仪计传感器(I) a各轴原始数据，aa last为上一次低通滤波后的三轴加速度计传感器(2) α轴数据，aa是最新一次测量得到的三轴加速度计传感器(2) a各轴原始数据，ma last为上一次低通滤波后的三轴磁强计传感器(3) α各轴数据，ma是最新一次测量得到的三轴磁强计传感器(3) a各轴原始数据，
根据公式:
8.根据权利要求5所述的一种基于MEMS的高精度三维姿态惯性测量系统实现三维姿态惯性的测量方法，其特征在于，对步骤五中利用步骤三得到的初始化数据％，Q1, Q2, Q3和步骤四得到的三轴加速度计传感器(2)精确化的数据采用拓展卡尔曼滤波器在Λ t时间间隔内进行拓展卡尔曼滤波的时间更新，得到预测四元数三维姿态惯性数据的过程为:根据公式:
9.根据权利要求5所述的一种基于MEMS的高精度三维姿态惯性测量系统实现三维姿态惯性的测量方法，其特征在于，对步骤六中的利用步骤四得到的精确化的三轴加速度计传感器(2)数据和精确化的三轴磁强计传感器(3)数据，在上一时刻得到的预测四元数三维姿态惯性数据处做线性化展开，进行拓展卡尔曼滤波的测量更新的过程为: 将观测
【文档编号】G01C21/16GK103776451SQ201410076764
【公开日】2014年5月7日 申请日期:2014年3月4日 优先权日:2014年3月4日
【发明者】胡庆雷, 肖冰, 陈卓 申请人:哈尔滨工业大学