CN102759356A - 基于多mems传感器的动态载体姿态测量系统及方法 - Google Patents

Abstract

本发明公开了一种基于多MEMS传感器的动态载体姿态测量系统及方法，系统包括多MEMS传感器检测单元、微处理器模块和上位计算机，上位计算机通过通信电路模块与微处理器模块相接，微处理器模块的输入端接有信号采集单元；多MEMS传感器检测单元由三个MEMS加速度传感器、三个角速率传感器和三个磁强传感器组成；方法包括步骤：对动态载体姿态的实时检测及同步传输；信号采集及预处理；微处理器模块对数据进行分析处理，得到动态载体的俯仰角、横滚角、航向角、旋转矩阵、欧拉角和四元数并进行存储；处理结果输出及显示。本发明设计合理，操作便捷，测量实时性高、精度高，输出结果多样，实用性强，适应性强，推广应用价值高。

Description

基于多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转换预处理，得到动态载体在横轴、纵轴和竖轴上的加速度值[G_x G_y G_z]，动态载体在横轴、纵轴和竖轴上的角速率值[ω_x ω_y ω_z]，以及动态载体在横轴、纵轴和竖轴上的磁场强度值[H_x H_y H_z]，并将得到的数据实时上传给微处理器模块进行记录；

步骤三、所述微处理器模块对其接收到的数据进行分析处理，得到动态载体的俯仰角γ、动态载体的横滚角β、动态载体的航向角α、用于表示动态载体姿态的旋转矩阵M、用于表示动态载体姿态的欧拉角[θφψ]和用于表示动态载体姿态的四元数[q0 q1 q2 q3]，并进行存储，其具体过程如下：

步骤301、得到动态载体的俯仰角γ：所述微处理器模块根据公式

计算得到动态载体的俯仰角γ并进行存储;

步骤302、得到动态载体的横滚角β：所述微处理器模块根据公式计算得到动态载体的横滚角β并进行存储；

步骤303、得到动态载体的航向角α，包括以下过程：

步骤3031、所述微处理器模块根据公式 $\left\{\begin{array}{c}{P}_{\mathrm{pitch}}=\left[\begin{array}{ccc}\cos \mathrm{\β}& 0& -\sin \mathrm{\β}\\ 0& 1& 1\\ \sin \mathrm{\β}& 0& \cos \mathrm{\β}\end{array}\right]\\ {\left[\begin{array}{ccc}{H}_{l-x}& H_{l-y}& H_{l-z}\end{array}\right]}^T={R_{\mathrm{roll}}}^{-1}{P_{\mathrm{pitch}}}^{-1}{\left[\begin{array}{ccc}{H}_x& H_y& H_z\end{array}\right]}^T\\ R_{\mathrm{roll}}=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\γ}& \sin \mathrm{\γ}\\ 0& -\sin \mathrm{\γ}& \cos \mathrm{\γ}\end{array}\right]\end{array}\right.$ 计算得到磁场强度值[H_x H_y H_z]在地平坐标系中的值[H_l-x H_l-y H_l-z]；

步骤3032、所述微处理器模块根据公式

计算得到动态载体的航向角α并进行存储；

步骤304、得到用于表示动态载体姿态的的旋转矩阵M，包括以下过程：

步骤3041、所述微处理器模块根据公式 $V\_E=\left[\begin{array}{ccc}\cos \mathrm{\β}\cos \mathrm{\α}-\sin \mathrm{\β}\sin \mathrm{\γ}\sin \mathrm{\α}& \cos \mathrm{\β}\sin \mathrm{\α}+\sin \mathrm{\β}\sin \mathrm{\γ}\cos \mathrm{\α}& -\sin \mathrm{\β}\cos \mathrm{\γ}\\ -\cos \mathrm{\γ}\sin \mathrm{\α}& \cos \mathrm{\γ}\cos \mathrm{\α}& \sin \mathrm{\γ}\\ \sin \mathrm{\β}\cos \mathrm{\α}+\cos \mathrm{\β}\sin \mathrm{\γ}\sin \mathrm{\α}& \sin \mathrm{\β}\sin \mathrm{\α}-\cos \mathrm{\β}\sin \mathrm{\γ}\cos \mathrm{\α}& \cos \mathrm{\β}\cos \mathrm{\γ}\end{array}\right]$ 计算得到动态载体在静止的地球坐标系统下的向量V_E；

步骤3042、所述微处理器模块根据公式 $V\_\mathrm{MIN}=\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_z& {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z& 0& -{\mathrm{\ω}}_x\\ -{\mathrm{\ω}}_y& {\mathrm{\ω}}_x& 0\end{array}\right]$ 计算得到动态载体在局部坐标系统中的向量V_MIN；

步骤3043、所述微处理器模块根据公式V_MIN＝M·V_E计算得到用于表示动态载体姿态的旋转矩阵 $M=\left[\begin{array}{ccc}M11& M12& M13\\ M21& M22& M23\\ M31& M32& M33\end{array}\right]$ 并进行存储；

步骤305、得到用于表示动态载体姿态的欧拉角[θφψ]：所述微处理器模块根据公式 $\left\{\begin{array}{c}\mathrm{\θ}=\arcsin (-M13)\\ \mathrm{\φ}=\arctan (M23/M33)\\ \mathrm{\ψ}=\arctan (M12/M11)\end{array}\right.$ 计算得到用于表示动态载体姿态的欧拉角[θφψ]并进行存储；

步骤306、得到用于表示动态载体姿态的四元数[q0 q1 q2 q3]，包括以下过程：

步骤3061、所述微处理器模块根据公式 $\left\{\begin{array}{c}\mathrm{test}1=M11+M22+M33\\ \mathrm{test}2=M11-M22-M33\\ \mathrm{test}3=-M11+M22-M33\\ \mathrm{test}4=-M11-M22+M33\end{array}\right.$ 计算得到test1、test2、test3和test4四个数；

步骤3062、所述微处理器模块判断test1、test2、test3和test4四个数的大小，当test1为test1、test2、test3和test4四个数中的最大数时，所述处理器模块根据公式 $\left\{\begin{array}{c}S=2\sqrt{1+M11+M22+M33}\\ \left[\begin{array}{c}q0\\ q1\\ q2\\ q3\end{array}\right]=\left[\begin{array}{c}S/4\\ (M23-M32)/S\\ (M31-M13)/S\\ (M12-M21)/S\end{array}\right]\end{array}\right.$ 计算得到用于表示动态载体姿态的四元数[q0 q1 q2 q3]并进行存储；当test2为test1、test2、test3和test4四个数中的最大数时，所述处理器模块根据公式 $\left\{\begin{array}{c}S=2\sqrt{1+M11-M22-M33}\\ \left[\begin{array}{c}q0\\ q1\\ q2\\ q3\end{array}\right]=\left[\begin{array}{c}(M32-M23)/S\\ -S/4\\ -(M21+M12)/S\\ -(M13+M31)/S\end{array}\right]\end{array}\right.$ 计算得到用于表示动态载体姿态的四元数[q0 q1 q2 q3]并进行存储；当test3为test1、test2、test3和test4四个数中的最大数时，所述处理器模块根据公式 $\left\{\begin{array}{c}S=2\sqrt{1-M11+M22-M33}\\ \left[\begin{array}{c}q0\\ q1\\ q2\\ q3\end{array}\right]=\left[\begin{array}{c}(M13-M31)/S\\ -(M21+M12)/S\\ -S/4\\ -(M32+M23)/S\end{array}\right]\end{array}\right.$ 计算得到用于表示动态载体姿态的四元数[q0 q1 q2 q3]并进行存储；当test4为test1、test2、test3和test4四个数中的最大数时，所述处理器模块根据公式 $\left\{\begin{array}{c}S=2\sqrt{1-M11-M22+M33}\\ \left[\begin{array}{c}q0\\ q1\\ q2\\ q3\end{array}\right]=\left[\begin{array}{c}(M21-M12)/S\\ -(M13+M31)/S\\ -(M32+M23)/S\\ -S/4\end{array}\right]\end{array}\right.$ 计算得到用于表示动态载体姿态的四元数[q0 q1 q2 q3]并进行存储；

步骤四、处理结果输出及显示：用户通过操作所述上位计算机输入显示命令，所述处理器模块通过通信电路模块接收从所述上位计算机上输入的显示命令并根据显示命令发送相应的数据给上位计算机，所述上位计算机通过通信电路模块接收处理器模块发送的数据并进行显示。

上述的方法，其特征在于：步骤四中用户通过操作所述上位计算机输入的显示命令包括显示模式设置命令和显示内容设置命令，所述显示模式设置命令包括测试模式设置命令和连续模式设置命令，所述处理器模块根据显示命令发送相应的数据给上位计算机且所述上位计算机通过通信电路模块接收处理器模块发送的数据并进行显示的方法如下：

当用户没有通过操作所述上位计算机输入显示模式设置命令时，所述处理器模块以测试模式发送相应的数据给上位计算机，上位计算机以测试模式对数据进行显示，即用户每输入一次显示内容设置命令，所述处理器模块就按照显示内容设置命令需要显示的内容发送一次数据给上位计算机，所述上位计算机就通过通信电路模块接收处理器模块发送的数据并进行一次显示；当用户输入多个显示内容设置命令时，所述处理器模块就通过通信电路模块接收从所述上位计算机上输入的多个显示内容设置命令并存储在命令缓存器中，然后，所述处理器模块再按照多个显示内容设置命令分别需要显示的内容和发出的先后顺序依次发送相应的数据给上位计算机，所述上位计算机就通过通信电路模块依次接收处理器模块发送的数据并依次进行显示；

当用户通过操作所述上位计算机输入显示模式设置命令时，所述处理器模块通过通信电路模块接收从所述上位计算机上输入的显示命令并对其接收到的显示模式设置命令进行判断，得出显示模式设置命令是测试模式设置命令，还是连续模式设置命令，当显示模式设置命令是测试模式设置命令时，所述处理器模块按照上述测试模式发送相应的数据给上位计算机，上位计算机以测试模式对数据进行显示；当显示模式设置命令是连续模式设置命令时，所述处理器模块以连续模式发送相应的数据给上位计算机，上位计算机以连续模式对数据进行显示，即所述处理器模块每存储一次数据就同步发送存储的数据给上位计算机，所述上位计算机通过通信电路模块接收处理器模块发送的数据并进行显示；

在所述上位计算机以连续模式对数据进行显示的过程中，当用户通过操作所述上位计算机输入显示内容设置命令时，所述处理器模块就通过通信电路模块接收从所述上位计算机上输入的显示内容设置命令并存储在命令缓存器中，并在连续模式的一个显示周期完成后，所述处理器模块按照显示内容设置命令需要显示的内容发送相应的数据给上位计算机，所述上位计算机就通过通信电路模块接收处理器模块发送数据并进行显示，当显示内容设置命令需要显示的内容显示完成后，所述上位计算机再继续以连续模式对数据进行显示。

本发明与现有技术相比具有以下优点：

1、本发明动态载体测量系统采用了模块化、集成化的设计，结构简单，设计合理，实现方便。

2、本发明的智能化程度高，使用操作便捷。

3、本发明对动态载体的姿态进行实时检测的检测单元由多个MEMS传感器构成，不但降低了成本，也大幅减小了体积。

4、本发明微处理器模块的数据处理速度快、数据处理能力强，提高了本发明的实时性和稳定性。

5、本发明首先采用了旋转矩阵来表示动态载体姿态，然后还能快速地将旋转矩阵转换成欧拉角和四元数，最终采用了旋转矩阵、欧拉角和四元数三种模式来表示动态载体姿态，表示方式多种多样，且测量的实时性高，能够满足不同用户的各种实际需求。

6、本发明具备连续模式、测试模式以及连续模式与测试模式的组合模式三种显示模式，能够根据用户的需求方便地转换显示模式和显示内容，适用性强。

7、本发明的测量精度高，实用性强，应用范围广，可以当作AHRS(姿态测量系统)使用，也可以当作VG(垂直参考系统)使用，还可以当作IMU（惯性测量单元）应用。

综上所述，本发明设计合理，使用操作便捷，测量实时性高，测量精度高，输出结果多样，实用性强，适应性强，推广应用价值高。

下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。

附图说明

图1为本发明动态载体姿态测量系统的结构示意图。

图2为本发明动态载体姿态测量方法的方法流程图。

附图标记说明:

具体实施方式

如图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组成。

如图1所示，本实施例中，所述信号采集单元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。

结合图2，本发明所述的基于多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转换预处理，得到动态载体在横轴、纵轴和竖轴上的加速度值[G_x G_y G_z]，动态载体在横轴、纵轴和竖轴上的角速率值[ω_x ω_y ω_z]，以及动态载体在横轴、纵轴和竖轴上的磁场强度值[H_x H_y H_z]，并将得到的数据实时上传给微处理器模块5进行记录；

步骤三、所述微处理器模块5对其接收到的数据进行分析处理，得到动态载体的俯仰角γ、动态载体的横滚角β、动态载体的航向角α、用于表示动态载体姿态的旋转矩阵M、用于表示动态载体姿态的欧拉角[θφψ]和用于表示动态载体姿态的四元数[q0 q1 q2 q3]，并进行存储，其具体过程如下：

步骤301、得到动态载体的俯仰角γ：所述微处理器模块5根据公式

计算得到动态载体的俯仰角γ并进行存储;

步骤302、得到动态载体的横滚角β：所述微处理器模块5根据公式

计算得到动态载体的横滚角β并进行存储；

步骤303、得到动态载体的航向角α，包括以下过程：

步骤3031、所述微处理器模块5根据公式 $\left\{\begin{array}{c}{P}_{\mathrm{pitch}}=\left[\begin{array}{ccc}\cos \mathrm{\β}& 0& -\sin \mathrm{\β}\\ 0& 1& 1\\ \sin \mathrm{\β}& 0& \cos \mathrm{\β}\end{array}\right]\\ {\left[\begin{array}{ccc}{H}_{l-x}& H_{l-y}& H_{l-z}\end{array}\right]}^T={R_{\mathrm{roll}}}^{-1}{P_{\mathrm{pitch}}}^{-1}{\left[\begin{array}{ccc}{H}_x& H_y& H_z\end{array}\right]}^T\\ R_{\mathrm{roll}}=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\γ}& \sin \mathrm{\γ}\\ 0& -\sin \mathrm{\γ}& \cos \mathrm{\γ}\end{array}\right]\end{array}\right.$ 计算得到磁场强度值[H_x H_y H_z]在地平坐标系中的值[H_l-x H_l-y H_l-z]；

步骤3032、所述微处理器模块5根据公式

计算得到动态载体的航向角α并进行存储；

步骤304、得到用于表示动态载体姿态的的旋转矩阵M，包括以下过程：

步骤3041、所述微处理器模块5根据公式 $V\_E=\left[\begin{array}{ccc}\cos \mathrm{\β}\cos \mathrm{\α}-\sin \mathrm{\β}\sin \mathrm{\γ}\sin \mathrm{\α}& \cos \mathrm{\β}\sin \mathrm{\α}+\sin \mathrm{\β}\sin \mathrm{\γ}\cos \mathrm{\α}& -\sin \mathrm{\β}\cos \mathrm{\γ}\\ -\cos \mathrm{\γ}\sin \mathrm{\α}& \cos \mathrm{\γ}\cos \mathrm{\α}& \sin \mathrm{\γ}\\ \sin \mathrm{\β}\cos \mathrm{\α}+\cos \mathrm{\β}\sin \mathrm{\γ}\sin \mathrm{\α}& \sin \mathrm{\β}\sin \mathrm{\α}-\cos \mathrm{\β}\sin \mathrm{\γ}\cos \mathrm{\α}& \cos \mathrm{\β}\cos \mathrm{\γ}\end{array}\right]$ 计算得到动态载体在静止的地球坐标系统下的向量V_E；

步骤3042、所述微处理器模块5根据公式 $V\_\mathrm{MIN}=\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_z& {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z& 0& -{\mathrm{\ω}}_x\\ -{\mathrm{\ω}}_y& {\mathrm{\ω}}_x& 0\end{array}\right]$ 计算得到动态载体在局部坐标系统中的向量V_MIN；

步骤3043、所述微处理器模块5根据公式V_MIN＝M·V_E计算得到用于表示动态载体姿态的旋转矩阵 $M=\left[\begin{array}{ccc}M11& M12& M13\\ M21& M22& M23\\ M31& M32& M33\end{array}\right]$ 并进行存储；

具体地，旋转矩阵M的计算过程如下：

首先，在公式V_MIN＝M·V_E的左右两端都乘以矩阵V_E的逆矩阵V_E^-1，得到V_MIN·V_E^-1=M·E，令步骤3041中，矩阵 $V\_E=\left[\begin{array}{ccc}a& b& c\\ d& e& f\\ g& h& i\end{array}\right],$ 即a=cosβcosα-sinβsinγsinα，b=cosβsinα+sinβsinγcosα，c=-sinβcosγ，d=-cosγsinα，e=cosγcosα，f=sinγ，g=sinβcosα+cosβsinγsinα，h=sinβsinα-cosβsinγcosα，i=cosβcosγ，则有 $M=\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_z& {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z& 0& -{\mathrm{\ω}}_x\\ -{\mathrm{\ω}}_y& {\mathrm{\ω}}_x& 0\end{array}\right]\·{\left[\begin{array}{ccc}a& b& c\\ d& e& f\\ g& h& i\end{array}\right]}^{-1},$ 而 ${\left[\begin{array}{ccc}a& b& c\\ d& e& f\\ g& h& i\end{array}\right]}^{-1}=\frac{1}{|V\_E|}\left[\begin{array}{ccc}\mathrm{ei}-\mathrm{hf}& -(\mathrm{bi}-\mathrm{hc})& \mathrm{bf}-\mathrm{ce}\\ \mathrm{fg}-\mathrm{id}& -(\mathrm{cg}-\mathrm{ia})& \mathrm{cd}-\mathrm{af}\\ \mathrm{dh}-\mathrm{ge}& -(\mathrm{ah}-\mathrm{gh})& \mathrm{ae}-\mathrm{bd}\end{array}\right]$ 其中，|V_E|=(aei+bfg+cdh)-(ceg+bdi+afh)

因此有: $M=\frac{1}{|V\_E|}\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_z& {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z& 0& -{\mathrm{\ω}}_x\\ -{\mathrm{\ω}}_y& {\mathrm{\ω}}_x& 0\end{array}\right]\·\left[\begin{array}{ccc}\mathrm{ei}-\mathrm{hf}& -(\mathrm{bi}-\mathrm{hc})& \mathrm{bf}-\mathrm{ce}\\ \mathrm{fg}-\mathrm{id}& -(\mathrm{cg}-\mathrm{ia})& \mathrm{cd}-\mathrm{af}\\ \mathrm{dh}-\mathrm{ge}& -(\mathrm{ah}-\mathrm{gh})& \mathrm{ae}-\mathrm{bd}\end{array}\right]$

令A=ei-hf，B=-(bi-hc)，C=bf-ce，D=fg-id，E=-(cg-ia)，F=cd-af，G=dh-ge，H=-(ah-gh)，I=ae-bd；

则有: $M=\frac{1}{|V\_E|}\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_z& {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z& 0& -{\mathrm{\ω}}_x\\ -{\mathrm{\ω}}_y& {\mathrm{\ω}}_x& 0\end{array}\right]\·\left[\begin{array}{ccc}A& B& C\\ D& E& F\\ G& H& I\end{array}\right]$

即: $M=\frac{1}{|V\_E|}\left[\begin{array}{ccc}-D{\mathrm{\ω}}_z+G{\mathrm{\ω}}_y& -E{\mathrm{\ω}}_z+H{\mathrm{\ω}}_y& -F{\mathrm{\ω}}_z+I{\mathrm{\ω}}_y\\ A{\mathrm{\ω}}_z-G{\mathrm{\ω}}_X& B{\mathrm{\ω}}_z-H{\mathrm{\ω}}_X& C{\mathrm{\ω}}_z-I{\mathrm{\ω}}_X\\ -A{\mathrm{\ω}}_Y+D{\mathrm{\ω}}_X& -B{\mathrm{\ω}}_Y+E{\mathrm{\ω}}_X& -C{\mathrm{\ω}}_Y+F{\mathrm{\ω}}_X\end{array}\right]$

步骤305、得到用于表示动态载体姿态的欧拉角[θφψ]：所述微处理器模块5根据公式 $\left\{\begin{array}{c}\mathrm{\θ}=\arcsin (-M13)\\ \mathrm{\φ}=\arctan (M23/M33)\\ \mathrm{\ψ}=\arctan (M12/M11)\end{array}\right.$ 计算得到用于表示动态载体姿态的欧拉角[θφψ]并进行存储；

步骤306、得到用于表示动态载体姿态的四元数[q0 q1 q2 q3]，包括以下过程：

步骤3061、所述微处理器模块5根据公式 $\left\{\begin{array}{c}\mathrm{test}1=M11+M22+M33\\ \mathrm{test}2=M11-M22-M33\\ \mathrm{test}3=-M11+M22-M33\\ \mathrm{test}4=-M11-M22+M33\end{array}\right.$ 计算得到test1、test2、test3和test4四个数；

步骤3062、所述微处理器模块5判断test1、test2、test3和test4四个数的大小，当test1为test1、test2、test3和test4四个数中的最大数时，所述处理器模块5根据公式 $\left\{\begin{array}{c}S=2\sqrt{1+M11+M22+M33}\\ \left[\begin{array}{c}q0\\ q1\\ q2\\ q3\end{array}\right]=\left[\begin{array}{c}S/4\\ (M23-M32)/S\\ (M31-M13)/S\\ (M12-M21)/S\end{array}\right]\end{array}\right.$ 计算得到用于表示动态载体姿态的四元数[q0 q1 q2 q3]并进行存储；当test2为test1、test2、test3和test4四个数中的最大数时，所述处理器模块5根据公式 $\left\{\begin{array}{c}S=2\sqrt{1+M11-M22-M33}\\ \left[\begin{array}{c}q0\\ q1\\ q2\\ q3\end{array}\right]=\left[\begin{array}{c}(M32-M23)/S\\ -S/4\\ -(M21+M12)/S\\ -(M13+M31)/S\end{array}\right]\end{array}\right.$ 计算得到用于表示动态载体姿态的四元数[q0 q1 q2 q3]并进行存储；当test3为test1、test2、test3和test4四个数中的最大数时，所述处理器模块5根据公式 $\left\{\begin{array}{c}S=2\sqrt{1-M11+M22-M33}\\ \left[\begin{array}{c}q0\\ q1\\ q2\\ q3\end{array}\right]=\left[\begin{array}{c}(M13-M31)/S\\ -(M21+M12)/S\\ -S/4\\ -(M32+M23)/S\end{array}\right]\end{array}\right.$ 计算得到用于表示动态载体姿态的四元数[q0 q1 q2 q3]并进行存储；当test4为test1、test2、test3和test4四个数中的最大数时，所述处理器模块5根据公式 $\left\{\begin{array}{c}S=2\sqrt{1-M11-M22+M33}\\ \left[\begin{array}{c}q0\\ q1\\ q2\\ q3\end{array}\right]=\left[\begin{array}{c}(M21-M12)/S\\ -(M13+M31)/S\\ -(M32+M23)/S\\ -S/4\end{array}\right]\end{array}\right.$ 计算得到用于表示动态载体姿态的四元数[q0 q1 q2 q3]并进行存储；

步骤四、处理结果输出及显示：用户通过操作所述上位计算机6输入显示命令，所述处理器模块5通过通信电路模块7接收从所述上位计算机6上输入的显示命令并根据显示命令发送相应的数据给上位计算机6，所述上位计算机6通过通信电路模块7接收处理器模块5发送的数据并进行显示。

本实施例中，步骤四中用户通过操作所述上位计算机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再继续以连续模式对数据进行显示。这样的显示模式为连续模式与测试模式的组合显示模式。

例如，测试模式设置命令为“0XA1”，连续模式设置命令为“0XA5”，显示内容设置命令包括用于对动态载体的航向角α、动态载体的俯仰角γ、动态载体的横滚角β、表示动态载体姿态的旋转矩阵M、表示动态载体姿态的欧拉角[θφψ]和表示动态载体姿态的四元数[q0 q1 q2 q3]进行显示的6组命令，每组显示内容设置命令均由显示命令ASCII码、执行周期数和优先级选择三部分组成，6组显示内容设置命令与显示内容的对应关系如表1所示：

表1 显示命令与显示内容对应关系表

	显示内容	显示命令ASCII码	执行周期数	优先级选择
					1	航向角α	Gyro_Yaw	0-FFFF	0-11可选
2	俯仰角γ	Gyro_Pitch	0-FFFF	0-11可选
					3	横滚角β	Gyro_Roll	0-FFFF	0-11可选
4	旋转矩阵M	Gyro	0-FFFF	0-11可选
					5	欧拉角[θφψ]	Acc	0-FFFF	0-11可选
6	四元数[q0 q1 q2 q3]	Mag	0-FFFF	0-11可选

具体实施时，用户一次可以通过操作所述上位计算机6输入表1中的6组命令中的一组、几组或者全部，其中，FFFF表示连续输出，0-FFFF之间的数表示输出次数并在每输出一次后就减1，优先级0-11可选，0优先级最高，11优先级最低。在所述上位计算机6以测试模式或连续模式对数据进行显示的过程中，当用户一次输入一组显示内容设置命令时，所述处理器模块5就按照显示内容设置命令需要显示的内容发送一次数据给上位计算机6，所述上位计算机6就通过通信电路模块7接收处理器模块5发送的数据并进行一次显示；当用户一次输入多组显示内容设置命令时，所述处理器模块5就通过通信电路模块7接收从所述上位计算机6上输入的多个显示内容设置命令并存储在命令缓存器中，然后，所述处理器模块5再按照多个显示内容设置命令分别需要显示的内容和发出的先后顺序依次发送相应的数据给上位计算机6，所述上位计算机6就通过通信电路模块7依次接收处理器模块5发送的数据并依次进行显示；例如，用户一次输入的显示内容设置命令为：Gyro_Yaw FFFF 0，Gyro_Pitch FFFF 1，Gyro_Roll FFFF 2，Gyro 1 3，所述处理器模块5就通过通信电路模块7接收从所述上位计算机6上输入的多个显示内容设置命令并存储在命令缓存器中，所述处理器模块5第一次依次发送给上位计算机6进行显示的数据为航向角α、俯仰角γ、横滚角β和旋转矩阵M，并对命令缓存器中Gyro的执行周期数减1，FFFF为连续输出，不做减1，然后，命令缓存器中的命令就变为了Gyro_Yaw FFFF 0，Gyro_Pitch FFFF 1，Gyro_Roll FFFF 2，所述处理器模块5第二次依次发送给上位计算机6进行显示的数据为航向角α、俯仰角γ和横滚角β，在没有新的显示内容设置命令输入时，所述处理器模块5以后各次依次发送给上位计算机6进行显示的数据均为航向角α、俯仰角γ和横滚角β，当有新的显示模式设置命令或显示内容设置命令输入时，所述处理器模块5根据新命令执行新动作。

综上所述，本发明首先采用了旋转矩阵来表示动态载体姿态，然后还能快速地将旋转矩阵转换成欧拉角和四元数，最终采用了旋转矩阵、欧拉角和四元数三种模式来表示动态载体姿态，具备连续模式、测试模式以及连续模式与测试模式的组合模式三种显示模式，能够根据用户的需求方便地转换显示模式和显示内容，适用性强。

以上所述，仅是本发明的较佳实施例，并非对本发明作任何限制，凡是根据本发明技术实质对以上实施例所作的任何简单修改、变更以及等效结构变化，均仍属于本发明技术方案的保护范围内。

Claims (10)

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.按照权利要求1所述的基于多MEMS传感器的动态载体姿态测量系统，其特征在于：所述信号采集单元（4）包括与多MEMS传感器检测单元输出端相接的信号多路复用电路模块（4-1）和与信号多路复用电路模块（4-1）输出端相接的A/D转换电路模块（4-2），所述A/D转换电路模块（4-2）的输出端与所述微处理器模块（5）的输入端相接。

3.按照权利要求1或2所述的基于多MEMS传感器的动态载体姿态测量系统，其特征在于：所述微处理器模块（5）为ARM微处理器STM 32F103。

4.按照权利要求1或2所述的基于多MEMS传感器的动态载体姿态测量系统，其特征在于：所述通信电路模块（7）为串口通信电路模块。

5.按照权利要求1或2所述的基于多MEMS传感器的动态载体姿态测量系统，其特征在于：所述第一MEMS加速度传感器（1-1）、第二MEMS加速度传感器（1-2）和第三MEMS加速度传感器（1-3）均为芯片ADXL202。

6.按照权利要求1或2所述的基于多MEMS传感器的动态载体姿态测量系统，其特征在于：所述第一MEMS角速率传感器（2-1）、第二MEMS角速率传感器（2-2）和第三MEMS角速率传感器（2-3）均为芯片ADXRS300。

7.按照权利要求1或2所述的基于多MEMS传感器的动态载体姿态测量系统，其特征在于：所述第一MEMS磁强传感器（3-1）、第二MEMS磁强传感器（3-2）和第三MEMS磁强传感器（3-3）均为芯片HMC1022。

8.按照权利要求2所述的基于多MEMS传感器的动态载体姿态测量系统，其特征在于：所述A/D转换电路模块（4-2）为芯片AD976。

9.一种利用如权利要求1所述系统的基于多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转换预处理，得到动态载体在横轴、纵轴和竖轴上的加速度值[G_x G_y G_z]，动态载体在横轴、纵轴和竖轴上的角速率值[ω_x ω_y ω_z]，以及动态载体在横轴、纵轴和竖轴上的磁场强度值[H_x H_y H_z]，并将得到的数据实时上传给微处理器模块（5）进行记录；

步骤三、所述微处理器模块（5）对其接收到的数据进行分析处理，得到动态载体的俯仰角γ、动态载体的横滚角β、动态载体的航向角α、用于表示动态载体姿态的旋转矩阵M、用于表示动态载体姿态的欧拉角[θφψ]和用于表示动态载体姿态的四元数[q0 q1 q2 q3]，并进行存储，其具体过程如下：

步骤301、得到动态载体的俯仰角γ：所述微处理器模块（5）根据公式

计算得到动态载体的俯仰角γ并进行存储;

步骤302、得到动态载体的横滚角β：所述微处理器模块（5）根据公式

计算得到动态载体的横滚角β并进行存储；

步骤303、得到动态载体的航向角α，包括以下过程：

步骤3031、所述微处理器模块（5）根据公式 $\left\{\begin{array}{c}{P}_{\mathrm{pitch}}=\left[\begin{array}{ccc}\cos \mathrm{\β}& 0& -\sin \mathrm{\β}\\ 0& 1& 1\\ \sin \mathrm{\β}& 0& \cos \mathrm{\β}\end{array}\right]\\ {\left[\begin{array}{ccc}{H}_{l-x}& H_{l-y}& H_{l-z}\end{array}\right]}^T={R_{\mathrm{roll}}}^{-1}{P_{\mathrm{pitch}}}^{-1}{\left[\begin{array}{ccc}{H}_x& H_y& H_z\end{array}\right]}^T\\ R_{\mathrm{roll}}=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\γ}& \sin \mathrm{\γ}\\ 0& -\sin \mathrm{\γ}& \cos \mathrm{\γ}\end{array}\right]\end{array}\right.$ 计算得到磁场强度值[H_x H_y H_z]在地平坐标系中的值[H_l-x H_l-y H_l-z]；

步骤3032、所述微处理器模块（5）根据公式

计算得到动态载体的航向角α并进行存储；

步骤304、得到用于表示动态载体姿态的的旋转矩阵M，包括以下过程：

步骤3041、所述微处理器模块（5）根据公式 $V\_E=\left[\begin{array}{ccc}\cos \mathrm{\β}\cos \mathrm{\α}-\sin \mathrm{\β}\sin \mathrm{\γ}\sin \mathrm{\α}& \cos \mathrm{\β}\sin \mathrm{\α}+\sin \mathrm{\β}\sin \mathrm{\γ}\cos \mathrm{\α}& -\sin \mathrm{\β}\cos \mathrm{\γ}\\ -\cos \mathrm{\γ}\sin \mathrm{\α}& \cos \mathrm{\γ}\cos \mathrm{\α}& \sin \mathrm{\γ}\\ \sin \mathrm{\β}\cos \mathrm{\α}+\cos \mathrm{\β}\sin \mathrm{\γ}\sin \mathrm{\α}& \sin \mathrm{\β}\sin \mathrm{\α}-\cos \mathrm{\β}\sin \mathrm{\γ}\cos \mathrm{\α}& \cos \mathrm{\β}\cos \mathrm{\γ}\end{array}\right]$ 计算得到动态载体在静止的地球坐标系统下的向量V_E；

步骤3042、所述微处理器模块（5）根据公式 $V\_\mathrm{MIN}=\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_z& {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z& 0& -{\mathrm{\ω}}_x\\ -{\mathrm{\ω}}_y& {\mathrm{\ω}}_x& 0\end{array}\right]$ 计算得到动态载体在局部坐标系统中的向量V_MIN；

步骤3043、所述微处理器模块（5）根据公式V_MIN＝M·V_E计算得到用于表示动态载体姿态的旋转矩阵 $M=\left[\begin{array}{ccc}M11& M12& M13\\ M21& M22& M23\\ M31& M32& M33\end{array}\right]$ 并进行存储；

步骤305、得到用于表示动态载体姿态的欧拉角[θφψ]：所述微处理器模块（5）根据公式 $\left\{\begin{array}{c}\mathrm{\θ}=\arcsin (-M13)\\ \mathrm{\φ}=\arctan (M23/M33)\\ \mathrm{\ψ}=\arctan (M12/M11)\end{array}\right.$ 计算得到用于表示动态载体姿态的欧拉角[θφψ]并进行存储；

步骤306、得到用于表示动态载体姿态的四元数[q0 q1 q2 q3]，包括以下过程：

步骤3061、所述微处理器模块（5）根据公式 $\left\{\begin{array}{c}\mathrm{test}1=M11+M22+M33\\ \mathrm{test}2=M11-M22-M33\\ \mathrm{test}3=-M11+M22-M33\\ \mathrm{test}4=-M11-M22+M33\end{array}\right.$ 计算得到test1、test2、test3和test4四个数；

步骤3062、所述微处理器模块（5）判断test1、test2、test3和test4四个数的大小，当test1为test1、test2、test3和test4四个数中的最大数时，所述处理器模块（5）根据公式 $\left\{\begin{array}{c}S=2\sqrt{1+M11+M22+M33}\\ \left[\begin{array}{c}q0\\ q1\\ q2\\ q3\end{array}\right]=\left[\begin{array}{c}S/4\\ (M23-M32)/S\\ (M31-M13)/S\\ (M12-M21)/S\end{array}\right]\end{array}\right.$ 计算得到用于表示动态载体姿态的四元数[q0 q1 q2 q3]并进行存储；当test2为test1、test2、test3和test4四个数中的最大数时，所述处理器模块（5）根据公式 $\left\{\begin{array}{c}S=2\sqrt{1+M11-M22-M33}\\ \left[\begin{array}{c}q0\\ q1\\ q2\\ q3\end{array}\right]=\left[\begin{array}{c}(M32-M23)/S\\ -S/4\\ -(M21+M12)/S\\ -(M13+M31)/S\end{array}\right]\end{array}\right.$ 计算得到用于表示动态载体姿态的四元数[q0 q1 q2 q3]并进行存储；当test3为test1、test2、test3和test4四个数中的最大数时，所述处理器模块（5）根据公式 $\left\{\begin{array}{c}S=2\sqrt{1-M11+M22-M33}\\ \left[\begin{array}{c}q0\\ q1\\ q2\\ q3\end{array}\right]=\left[\begin{array}{c}(M13-M31)/S\\ -(M21+M12)/S\\ -S/4\\ -(M32+M23)/S\end{array}\right]\end{array}\right.$ 计算得到用于表示动态载体姿态的四元数[q0 q1 q2 q3]并进行存储；当test4为test1、test2、test3和test4四个数中的最大数时，所述处理器模块（5）根据公式 $\left\{\begin{array}{c}S=2\sqrt{1-M11-M22+M33}\\ \left[\begin{array}{c}q0\\ q1\\ q2\\ q3\end{array}\right]=\left[\begin{array}{c}(M21-M12)/S\\ -(M13+M31)/S\\ -(M32+M23)/S\\ -S/4\end{array}\right]\end{array}\right.$ 计算得到用于表示动态载体姿态的四元数[q0 q1 q2 q3]并进行存储；

步骤四、处理结果输出及显示：用户通过操作所述上位计算机（6）输入显示命令，所述处理器模块（5）通过通信电路模块（7）接收从所述上位计算机（6）上输入的显示命令并根据显示命令发送相应的数据给上位计算机（6），所述上位计算机（6）通过通信电路模块（7）接收处理器模块（5）发送的数据并进行显示。

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）再继续以连续模式对数据进行显示。

Images