专利名称：基于多mems传感器的动态载体姿态测量系统的制作方法
技术领域：
本实用新型涉及载体姿态测量技术领域，尤其是涉及ー种基于多MEMS传感器的动态载体姿态测量系统。
背景技术：
陀螺测量系统ー种惯性测量系统，通过测量惯性空间的三维角速度、三维加速度及三维磁场強度，三轴的加速度计和磁场计跟踪静态的角度，利用三轴陀螺跟踪系统动态的角度，实时输出物体在空间中的姿态角。以往的姿态测量系统利用三轴地磁解耦和三轴加速度计，受外力加速度影响很大，在运动/振动等环境中，输出方向角误差较大，此外地磁传感器有缺点，它的绝对參照物是地磁场的磁力线，地磁的特点是使用范围大，但强度较低，约零点几高斯，非常容易受到其它磁体的干扰，如果融合了 Z轴陀螺仪的瞬时角度，就可以使系统数据更加稳定。加速度测量的是重力方向，在无外力加速度的情况下，能准确输出ROLL/PITCH两轴姿态角度并且此角度不会有累积误差，在更长的时间尺度内都是准确的。但是加速度传感器测角度的缺点是加速度传感器实际上是用MEMS技术检测惯性カ造成的微小形变，而惯性力与重力本质是一祥的，所以加速度计就不会区分重力加速度与外力加速度，当系统在三维空间做变速运动吋，它的输出就不正确了。
实用新型内容本实用新型所要解决的技术问题在于针对上述现有技术中的不足，提供一种基于多MEMS传感器的动态载体姿态测量系统，其结构简単，设计合理，智能化程度高，使用操作便捷，体积小，成本低，測量精度高，实用性強，应用范围广。为解决上述技术问题，本实用新型采用的技术方案是一种基于多MEMS传感器的动态载体姿态测量系统，其特征在于包括用于对动态载体的姿态进行实时检测的多MEMS传感器检测单元、用于对多MEMS传感器检测单元所检测到的信号进行分析处理的微处理器模块和上位计算机，所述上位计算机通过通信电路模块与微处理器模块相接并通信，所述微处理器模块的输入端接有用于实时采集多MEMS传感器检测单元实时所检测信号的信号采集单元，所述多MEMS传感器检测单元与所述信号采集单元相接；所述多MEMS传感器检测单元由测量轴与动态载体坐标系的横轴相重合的第一 MEMS加速度传感器、第一 MEMS角速率传感器和第一 MEMS磁强传感器，测量轴与动态载体坐标系的纵轴相重合的第二 MEMS加速度传感器、第二 MEMS角速率传感器和第二 MEMS磁强传感器，以及测量轴与动态载体坐标系的竖轴相重合的第三MEMS加速度传感器、第三MEMS角速率传感器和第三MEMS磁强传感器组成。上述的基于多MEMS传感器的动态载体姿态测量系统，其特征在于所述信号采集単元包括与多MEMS传感器检测单元输出端相接的信号多路复用电路模块和与信号多路复用电路模块输出端相接的A/D转换电路模块，所述A/D转换电路模块的输出端与所述微处理器模块的输入端相接。上述的基于多MEMS传感器的动态载体姿态测量系统,其特征在于所述微处理器模块为ARM微处理器STM32F103。上述的基于多MEMS传感器的动态载体姿态测量系统，其特征在于所述通信电路模块为串ロ通信电路模块。上述的基于多MEMS传感器的动态载体姿态测量系统,其特征在于所述第一 MEMS加速度传感器、第二 MEMS加速度传感器和第三MEMS加速度传感器均为芯片ADXL202。上述的基于多MEMS传感器的动态载体姿态测量系统,其特征在于所述第一 MEMS角速率传感器、第二 MEMS角速率传感器和第三MEMS角速率传感器均为芯片ADXRS 300。 上述的基于多MEMS传感器的动态载体姿态测量系统,其特征在于所述第一 MEMS磁强传感器、第二 MEMS磁强传感器和第三MEMS磁强传感器均为芯片HMC1022。上述的基于多MEMS传感器的动态载体姿态测量系统，其特征在于所述A/D转换电路模块为芯片AD976。本实用新型与现有技术相比具有以下优点I、本实用新型采用模块化、集成化的设计，结构简単，设计合理，实现方便。2、本实用新型的智能化程度高，使用操作便捷。3、本实用新型对动态载体的姿态进行实时检测的检测单元由多个MEMS传感器构成，不但降低了成本，也大幅减小了体积。4、本实用新型微处理器模块的数据处理速度快、数据处理能力强，提高了本实用新型的实时性和稳定性。5、本实用新型的測量精度高，实用性强，应用范围广，可以当作AHRS(姿态测量系统)使用，也可以当作VG (垂直參考系统)使用，还可以当作MU (惯性测量单元)应用。综上所述，本实用新型结构简単，设计合理，智能化程度高，使用操作便捷，体积小，成本低，測量精度高，实用性強，应用范围广。下面通过附图和实施例，对本实用新型的技术方案做进ー步的详细描述。
图I为本实用新型的电路原理框图。附图标记说明1-1 一第一 MEMS加速度传感器；1-2—第二 MEMS加速度传感器；1-3—第三MEMS加速度传感器；2-1—第一 MEMS角速率传感器；2-2—第二 MEMS角速率传感器；2-3—第三MEMS角速率传感器；3-1—第一 MEMS磁强传感器；3_2—第二 MEMS磁强传感器；3-3—第三MEMS磁强传感器；4一信号采集单元；4-1 一信号多路复用电路模块；4_2 — A/D转换电路模块；5 一微处通器模块；6—上位计算机；7—通イ目电路模块。
具体实施方式
如图I所示，本实用新型包括用于对动态载体的姿态进行实时检测的多MEMS传感器检测单元、用于对多MEMS传感器检测单元所检测到的信号进行分析处理的微处理器模块5和上位计算机6，所述上位计算机6通过通信电路模块7与微处理器模块5相接并通信，所述微处理器模块5的输入端接有用于实时采集多MEMS传感器检测单元实时所检测信号的信号采集单元4，所述多MEMS传感器检测单元与所述信号采集单元4相接；所述多MEMS传感器检测单兀由测量轴与动态载体坐标系的横轴相重合的第一 MEMS加速度传感器1-1、第一 MEMS角速率传感器2-1和第一 MEMS磁强传感器3_1，测量轴与动态载体坐标系的纵轴相重合的第MEMS加速度传感器1-2、第二 MEMS角速率传感器2_2和第MEMS磁强传感器3-2，以及测量轴与动态载体坐标系的竖轴相重合的第三MEMS加速度传感器1-3、第三MEMS角速率传感器2-3和第三MEMS磁强传感器3-3组成。如图I所示,本实施例中，所述信号采集单元4包括与多MEMS传感器检测单元输出端相接的信号多路复用电路模块4-1和与信号多路复用电路模块4-1输出端相接的A/D转换电路模块4-2,所述A/D转换电路模块4-2的输出端与所述微处理器模块5的输入端相
接。本实施例中，所述微处理器模块5为ARM微处理器STM32F103。所述通信电路模块7为串ロ通信电路模块。所述第一 MEMS加速度传感器1-1、第MEMS加速度传感器1_2和第三MEMS加速度传感器1-3均为芯片ADXL202。所述第一 MEMS角速率传感器2_1、第MEMS角速率传感器2-2和第三MEMS角速率传感器2-3均为芯片ADXRS300。所述第一 MEMS磁强传感器3-1、第二 MEMS磁强传感器3-2和第三MEMS磁强传感器3_3均为芯片HMC1022。所述A/D转换电路模块4-2为芯片AD976。利用本发明所述的基于多MEMS传感器的动态载体姿态测量系统进行动态载体测量的方法，包括以下步骤步骤一、对动态载体姿态的实时检测及同步传输通过多MEMS传感器检测单元中的第一 MEMS加速度传感器1-1、第二 MEMS加速度传感器1_2和第三MEMS加速度传感器1-3分别对动态载体在横轴、纵轴和竖轴上的加速度进行实时检测，并将实时所检测到的信号输出给信号采集单元4 ;通过多MEMS传感器检测单元中的第一 MEMS角速率传感器2_1、第二 MEMS角速率传感器2-2和第三MEMS角速率传感器2_3分别对动态载体在横轴、纵轴和竖轴上的角速率进行实时检測，并将实时所检测到的信号输出给信号采集单元4 ;通过多MEMS传感器检测单元中的第一 MEMS磁强传感器3_1、第二 MEMS磁强传感器3_2和第三MEMS磁强传感器3-3分别对动态载体在横轴、纵轴和竖轴上的磁强进行实时检測，并将实时所检测到的信号输出给信号采集单元4 ；步骤ニ、信号采集及预处理信号采集单元4对多MEMS传感器检测单元中的各个传感器所检测到的信号进行实时采集及A/D转换预处理，得到动态载体在横轴、纵轴和竖轴上的加速度值[GxGyGJ,动态载体在横轴、纵轴和竖轴上的角速率值[ x y J，以及动态载体在横轴、纵轴和竖轴上的磁场强度值[HxHyHz]，并将得到的数据实时上传给微处理器模块5进行记录；步骤三、所述微处理器模块5对其接收到的数据进行分析处理，得到动态载体的俯仰角Y、动态载体的横滚角P、动态载体的航向角a、用于表示动态载体姿态的旋转矩阵M、用于表示动态载体姿态的欧拉角[0小V]和用于表示动态载体姿态的四元数[q0qlq2q3]，并进行存储，其具体过程如下[0037]步骤301、得到动态载体的俯仰角Y :所述微处理器模块5根据公式ァzarctanh-j'H计算得到动态载体的俯仰角、并进行存储；步骤302、得到动态载体的横滚角P :所述微处理器模块5根据公式
/ "
计算得到动态载体的横滚角P并进行存储；步骤303、得到动态载体的航向角a，包括以下过程步骤3 0 3 1、所述微处理器模块5根据公式
I cosfi O -sto,l I _ A I I
1 pitch -Vllf
sin, § m%P I
Imi^凡,.巧Jt =Rr.%tm Hy /fj计算得到磁场强度值[HxHyHJ在地平坐标系中的
「I 0 0 IRrott = 0 cosy sinfI —0 -sinf COSfj
值[m]；
//步骤3032、所述微处理器模块5根据公式计算得到动态载体的航
Hl ,
向角a并进行存储；步骤304、得到用于表示动态载体姿态的的旋转矩阵M，包括以下过程步骤304 1、所述微处理器模块5根据公式 cos/ cosa-sm/3sm/sma cosfisina+smfism/cosa -sin^cos/
VE= -cos/Smacos/coscrsiny 计算得至1 J动态载体在
sin於cosa+cos卢sin/sincr sin^sina-cos/ sinfcoscr cosficos/
静止的地球坐标系统下的向量V_E ；
Fo my步骤3042、所述微处理器模块5根据公式F一g 0 - 计算得到动态载
[- , mx 0
体在局部坐标系统中的向量V_MIN ；步骤3043、所述微处理器模块5根据公式V_MIN = M. V_E计算得到用于表示动态
"Mn Mn Mn载体姿态的旋转矩阵M= M21 M22 M23并进行存储；
M31 M32 M33具体地，旋转矩阵M的计算过程如下首先，在公式V_MIN = M. V_E的左右两端都乘以矩阵V_E的逆矩a b €
阵 V_E_1,得到 V—MIN. V—E—1 = M. E，令步骤 3041 中，矩阵 VJC= e /，即
g h i
a=cos 3 cos a -sin ^ sin Y sin a，b=cos ^ sina +sin^ sin Y cosa，c=_sin ^ cos Y ,d = -cos ysina , e = co s Ycosa , f = sin Y , g = sin^ cosa +cos^ siny sina ,
p ,叫p bザ
h=sin & sin a -cos & sin Y cos a，i=cos & cos Y，则有M = mz 0 - COx d € f $
tox 0 g k I a b c 1ei hf (hi he) hf ~ ce而d e ブ= 7--.....Ji fg-id -(cg-ia) cd-af
g h i I - —I d/j.ge - (ah - gh) ae-bd其中，IV—EI = (aei+bfg+cdh) -(ceg+bdi+afh)
f0 - F ei - hf - (hi - he) hf - ce I因此有Mmz 0 -m.x 4fg~hi ~(cg~ia) cd-af
—丨卜mx 0 \dh-ge -(ah-gh) ae-bd J令A=ei_hf，B=-(bi_hc)，C=bf_ce，D=fg_id，E=_(cg_ia)，F=cd_af，G=dh_ge，H=-( ah-gf)，I=ae_bd ；
:所述微处理器模块5根 P= arcsin(-M13)
据公式j#=arctm(M23/M33)计算得到用于表示动态载体姿态的欧拉角[e由￥]并进行存 j y/ -arctan(M 12/M11)
储；步骤306、得到用于表示动态载体姿态的四元数[q0qlq2q3]，包括以下过程
'tesf\=M\ 1+M22+M33
test2=Mll-M22-M33步骤3061、所述微处理器模块5根据公式L 3 Mn + M22 .イジ计算得到
fes/4=-i/ll-M22 + AT33
testl、test2、test3 和 test4 四个数；步骤3062、所述微处理器模块5判断testl、test2、test3和test4四个数的大小，当testl为testl、test2、test3和test4四个数中的最大数时,所述处理器模块5根据公式
权利要求1.一种基于多MEMS传感器的动态载体姿态测量系统,其特征在于包括用于对动态载体的姿态进行实时检测的多MEMS传感器检测单元、用于对多MEMS传感器检测单元所检测到的信号进行分析处理的微处理器模块(5)和上位计算机(6)，所述上位计算机(6)通过通信电路模块(7 )与微处理器模块(5 )相接并通信，所述微处理器模块(5 )的输入端接有用于实时采集多MEMS传感器检测单元实时所检测信号的信号采集单元(4)，所述多MEMS传感器检测单元与所述信号采集单元(4)相接；所述多MEMS传感器检测单元由测量轴与动态载体坐标系的横轴相重合的第一 MEMS加速度传感器(1-1)、第一 MEMS角速率传感器(2_1)和第一 MEMS磁强传感器(3-1 )，测量轴与动态载体坐标系的纵轴相重合的第二 MEMS加速度传感器(1-2)、第二 MEMS角速率传感器(2-2)和第二 MEMS磁强传感器(3 _2)，以及测量轴与动态载体坐标系的竖轴相重合的第三MEMS加速度传感器(1-3)、第三MEMS角速率传感器(2_3)和第三MEMS磁强传感器(3-3 )组成。
2.按照权利要求I所述的基于多MEMS传感器的动态载体姿态测量系统，其特征在于所述信号采集单元(4)包括与多MEMS传感器检测单元输出端相接的信号多路复用电路模块(4-1)和与信号多路复用电路模块(4-1)输出端相接的A/D转换电路模块(4-2)，所述A/D转换电路模块(4-2)的输出端与所述微处理器模块(5)的输入端相接。
3.按照权利要求I或2所述的基于多MEMS传感器的动态载体姿态测量系统，其特征在于所述微处理器模块(5)为ARM微处理器STM32F103。
4.按照权利要求I或2所述的基于多MEMS传感器的动态载体姿态测量系统，其特征在于所述通信电路模块(7)为串ロ通信电路模块。
5.按照权利要求I或2所述的基于多MEMS传感器的动态载体姿态测量系统，其特征在于所述第一 MEMS加速度传感器(1-1 )、第二 MEMS加速度传感器(1-2 )和第三MEMS加速度传感器(1-3 )均为芯片ADXL202。
6.按照权利要求I或2所述的基于多MEMS传感器的动态载体姿态测量系统，其特征在于所述第一 MEMS角速率传感器(2-1 )、第二 MEMS角速率传感器(2_2)和第三MEMS角速率传感器(2-3)均为芯片ADXRS300。
7.按照权利要求I或2所述的基于多MEMS传感器的动态载体姿态测量系统，其特征在于所述第一 MEMS磁强传感器(3-1 )、第二 MEMS磁强传感器(3_2)和第三MEMS磁强传感器(3-3)均为芯片 HMC1022。
8.按照权利要求2所述的基于多MEMS传感器的动态载体姿态测量系统，其特征在于所述A/D转换电路模块(4-2)为芯片AD976。
专利摘要本实用新型公开了一种基于多MEMS传感器的动态载体姿态测量系统，包括多MEMS传感器检测单元、微处理器模块和上位计算机，上位计算机通过通信电路模块与微处理器模块相接，微处理器模块的输入端接有信号采集单元，多MEMS传感器检测单元与信号采集单元相接；多MEMS传感器检测单元由第一MEMS加速度传感器、第一MEMS角速率传感器和第一MEMS磁强传感器，第二MEMS加速度传感器、第二MEMS角速率传感器和第二MEMS磁强传感器，以及第三MEMS加速度传感器、第三MEMS角速率传感器和第三MEMS磁强传感器组成。本实用新型结构简单，设计合理，智能化程度高，使用操作便捷，体积小，成本低，测量精度高，实用性强，应用范围广。
文档编号G01C21/16GK202661077SQ20122035473
公开日2013年1月9日 申请日期2012年7月20日 优先权日2012年7月20日
发明者朱维 申请人:陕西航天长城测控有限公司