专利名称：基于多mems传感器的动态载体姿态测量系统及方法
技术领域：
本发明涉及载体姿态测量技术领域，尤其是涉及一种基于多MEMS传感器的动态载体姿态测量系统及方法。
背景技术：
现有技术中，姿态测量大多是通过测量载体在惯性空间中三轴地磁场强度值和三轴加速度值解算出载体在惯性空间中的姿态角。地磁场测量成本低，范围大，能够解算出载体的静态航向角，但地磁场强度较低，非常容易受到其它磁体的干扰。加速度测量的是重力方向，在无外力加速度的情况下，能准确解算出静态俯仰角和横滚角，并且角度不会有累积漂移，但是由于加速度传感器实际上是用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角速率传感器均为芯片ADXRS300。上述的基于多MEMS传感器的动态载体姿态测量系统,其特征在于所述第一 MEMS磁强传感器、第二 MEMS磁强传感器和第三MEMS磁强传感器均为芯片HMC1022。上述的基于多MEMS传感器的动态载体姿态测量系统，其特征在于所述A/D转换电路模块为芯片AD976。本发明还提供了一种数据处理速度快、实时性高、精度高、输出结果多样、适应性强的基于多MEMS传感器的动态载体姿态测量方法，其特征在于该方法包括以下步骤步骤一、对动态载体姿态的实时检测及同步传输通过多MEMS传感器检测单元中的第一 MEMS加速度传感器、第二 MEMS加速度传感器和第三MEMS加速度传感器分别对动态载体在横轴、纵轴和竖轴上的加速度进行实时检测，并将实时所检测到的信号输出给信号采集单元；通过多MEMS传感器检测单元中的第一 MEMS角速率传感器、第二 MEMS角速率传感器和第三MEMS角速率传感器分别对动态载体在横轴、纵轴和竖轴上的角速率进行实时检测，并将实时所检测到的信号输出给信号采集单元；通过多MEMS传感器检测单元中的第
一MEMS磁强传感器、第二 MEMS磁强传感器和第三MEMS磁强传感器分别对动态载体在横轴、纵轴和竖轴上的磁强进行实时检测，并将实时所检测到的信号输出给信号采集单元；步骤二、信号采集及预处理信号采集单元对多MEMS传感器检测单元中的各个传感器所检测到的信号进行实时采集及A/D转换预处理，得到动态载体在横轴、纵轴和竖轴上的加速度值[Gx Gy Gz]，动态载体在横轴、纵轴和竖轴上的角速率值[GJx Coy Co J，以及动态载体在横轴、纵轴和竖轴上的磁场强度值[Hx Hy HJ，并将得到的数据实时上传给微处理器模块进行记录；步骤三、所述微处理器模块对其接收到的数据进行分析处理，得到动态载体的俯仰角Y、动态载体的横滚角P、动态载体的航向角a、用于表示动态载体姿态的旋转矩阵M、用于表不动态载体姿态的欧拉角
和用于表不动态载体姿态的四兀数[q0 ql q2q3]，并进行存储，其具体过程如下步骤301、得到动态载体的俯仰角Y :所述微处理器模块根据公式/ =
KJz计算得到动态载体的俯仰角Y并进行存储；步骤302、得到动态载体的横滚角P :所述微处理器模块根据公式
权利要求
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微处理器STM 32F103。
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。
9.一种利用如权利要求I所述系统的基于多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转换预处理，得到动态载体在横轴、纵轴和竖轴上的加速度值[Gx Gy Gz]，动态载体在横轴、纵轴和竖轴上的角速率值[GJx Coy Co J，以及动态载体在横轴、纵轴和竖轴上的磁场强度值[Hx Hy HJ，并将得到的数据实时上传给微处理器模块(5)进行记录； 步骤三、所述微处理器模块(5)对其接收到的数据进行分析处理，得到动态载体的俯仰角Y、动态载体的横滚角P、动态载体的航向角ct、用于表$动态载体姿态的旋转矩阵M、 用于表不动态载体姿态的欧拉角
和用于表不动态载体姿态的四兀数[qO ql q2q3]，并进行存储，其具体过程如下 步骤301、得到动态载体的俯仰角y :所述微处理器模块(5)根据公式r = arctan(-Cj2计算得到动态载体的俯仰角Y并进行存储； 步骤302、得到动态载体的横滚角P :所述微处理器模块(5)根据公式P=arctan^JZ________2^7_______3_______)计算得到动态载体的横滚角 P 并进行存储； 步骤303、得到动态载体的航向角a，包括以下过程ms0 0 -si讀PpUdt — 0 11L sin# 0 cos# 步骤3031、所述微处理器模块(5)根据公式[//,、、. Hh H1J = Rmll %IKh l[Hx Hr HJr[I 0 0 ^Km = 0 cosf sin/[0 -sin/ cos:/—计算得到磁场强度值[Hx Hy HJ在地平坐标系中的值[Hh H1^y H1J ； 步骤3032、所述微处理器模块(5)根据公式》= arctan(#)计算得到动态载体的航向角《并进行存储； 步骤304、得到用于表示动态载体姿态的的旋转矩阵M，包括以下过程 步骤3041、所述微处理器模块(5)根据公式 cos^coscr-sin/ sinfsincr cos^sina+siiijffsiiifcosflr -sin於cosy V _E= -cosysinofcos/cos asiny 计算得至1J动态载sin/ cosa+cos/ sinysina sin/ sina-cos/9sin/cosa cosficosy体在静止的地球坐标系统下的向量V_E ；0 -m2 mr 步骤3042、所述微处理器模块(5)根据公式F — M/M= 0 - 计算得到动态载体二气' 咚0 _在局部坐标系统中的向量V_MIN ； 步骤3043、所述微处理器模块(5)根据公式V_MIN = M *V_E计算得到用于表示动态载 体姿态的旋转矩阵
10.按照权利要求9所述的方法，其特征在于步骤四中用户通过操作所述上位计算机(6)输入的显示命令包括显示模式设置命令和显示内容设置命令，所述显示模式设置命令包括测试模式设置命令和连续模式设置命令，所述处理器模块(5)根据显示命令发送相应的数据给上位计算机(6 )且所述上位计算机(6 )通过通信电路模块(7 )接收处理器模块(5 )发送的数据并进行显示的方法如下 当用户没有通过操作所述上位计算机(6)输入显示模式设置命令时，所述处理器模块(5)以测试模式发送相应的数据给上位计算机(6)，上位计算机(6)以测试模式对数据进行显示，即用户每输入一次显示内容设置命令，所述处理器模块(5 )就按照显示内容设置命令需要显示的内容发送一次数据给上位计算机(6)，所述上位计算机(6)就通过通信电路模块(7)接收处理器模块(5)发送的数据并进行一次显示；当用户输入多个显示内容设置命令时，所述处理器模块(5)就通过通信电路模块(7)接收从所述上位计算机(6)上输入的多个显示内容设置命令并存储在命令缓存器中，然后，所述处理器模块(5 )再按照多个显示内容设置命令分别需要显示的内容和发出的先后顺序依次发送相应的数据给上位计算机(6)，所述上位计算机(6)就通过通信电路模块(7)依次接收处理器模块(5)发送的数据并依次进行显示； 当用户通过操作所述上位计算机(6)输入显示模式设置命令时，所述处理器模块(5)通过通信电路模块(7)接收从所述上位计算机(6)上输入的显示命令并对其接收到的显示模式设置命令进行判断，得出显示模式设置命令是测试模式设置命令，还是连续模式设置命令，当显示模式设置命令是测试模式设置命令时，所述处理器模块(5 )按照上述测试模式发送相应的数据给上位计算机(6)，上位计算机(6)以测试模式对数据进行显示；当显示模式设置命令是连续模式设置命令时，所述处理器模块(5)以连续模式发送相应的数据给上位计算机(6)，上位计算机(6)以连续模式对数据进行显示，即所述处理器模块(5)每存储一次数据就同步发送存储的数据给上位计算机(6)，所述上位计算机(6)通过通信电路模块(7)接收处理器模块(5)发送的数据并进行显示； 在所述上位计算机(6)以连续模式对数据进行显示的过程中，当用户通过操作所述上位计算机(6 )输入显示内容设置命令时，所述处理器模块(5 )就通过通信电路模块(7 )接收从所述上位计算机(6)上输入的显示内容设置命令并存储在命令缓存器中，并在连续模式的一个显示周期完成后，所述处理器模块(5)按照显示内容设置命令需要显示的内容发送相应的数据给上位计算机(6 )，所述上位计算机(6 )就通过通信电路模块(7 )接收处理器模块(5 )发送数据并进行显示，当显示内容设置命令需要显示的内容显示完成后，所述上位计算机(6)再继 续以连续模式对数据进行显示。
全文摘要
本发明公开了一种基于多MEMS传感器的动态载体姿态测量系统及方法，系统包括多MEMS传感器检测单元、微处理器模块和上位计算机，上位计算机通过通信电路模块与微处理器模块相接，微处理器模块的输入端接有信号采集单元；多MEMS传感器检测单元由三个MEMS加速度传感器、三个角速率传感器和三个磁强传感器组成；方法包括步骤对动态载体姿态的实时检测及同步传输；信号采集及预处理；微处理器模块对数据进行分析处理，得到动态载体的俯仰角、横滚角、航向角、旋转矩阵、欧拉角和四元数并进行存储；处理结果输出及显示。本发明设计合理，操作便捷，测量实时性高、精度高，输出结果多样，实用性强，适应性强，推广应用价值高。
文档编号G01C21/16GK102759356SQ20121025236
公开日2012年10月31日 申请日期2012年7月20日 优先权日2012年7月20日
发明者朱维 申请人:陕西航天长城测控有限公司