CN103424115B - 微小型飞行器地面试验姿态记录仪 - Google Patents

Abstract

本发明提供了一种微小型飞行器地面试验姿态记录仪，结合了MENS陀螺仪，加速度计以及数字罗盘等传感器获得飞行器的原始飞行姿态数据，并在利用均值滤波方法处理的基础上根据自定义的数据格式对飞行姿态数据进行分割和打包，以整数形式通过无线发送模块实时发送到上位机的数据组合单元处理，利用姿态解算单元实现对飞行姿态数据融合，从而实现快速、实时的对飞行姿态的更新。姿态解算单元主要由四元数互补滤波方法组成，它能够有效结合陀螺仪动态性能和加速度计的静态响应特性，补偿陀螺仪的积分误差。可视化单元模块根据姿态解算后的数据，驱动基于OpenGl图形库开发的飞行器模型实现飞行姿态的可视化模拟，并利用波形绘制模块将角位置运动以波形曲线的形式显示。

Description

微小型飞行器地面试验姿态记录仪

技术领域

本发明属于微小型飞行器测控领域，利用本方法可实现微小型飞行器在地面试验过程中的姿态参数实时、无线记录与回放。

背景技术

微小型无人机，比如微小型四旋翼无人机在军事侦查、民用和科研等多个领域有重要的潜在应用价值。为了提高飞行控制系统的稳定性、验证各种控制算法的控制效果，有必要在试飞之前对样机进行地面试验，以积累控制参数与飞行器姿态变化之间的对应关系。鉴于微小型无人机尺寸和质量有限，若采用传统的、质量较大的姿态监测仪则会对测量结果的精度产生不可忽略的影响。

在微小型无人机中，用于测量飞行器的飞行姿态时，通常要使用陀螺仪、加速度计以及数字罗盘等分别用于测量角速度、加速度以及磁场强度等数据。由于陀螺仪在测量角速度变化时，需要对角速度进行积分。随着时间的累计会产生漂移误差。同时，陀螺仪测量的信息置信度较高，更容易受到干扰，需要解决因外部干扰而产生的突然跃变的数据。

此外，通常在描述飞行器的飞行姿态时，我们需要将角速度、加速度以及磁场强度在旋转矩阵的框架下计算，虽然利用四元数来描述三维空间中物体的运动可以大大减少计算量，但是为了适应测量微小型无人机的飞行姿态，需要对飞行姿态记录仪的下位机系统与上位机系统进行重新的分工，使得在地面的上位机系统能够通过无线传输的手段并整合较强的计算能力，能快速的将基于四元数的旋转矩阵换算出飞行器的姿态角，实现对飞行器的飞行姿态的实施测量和记录。

张丽杰等人提供了一种小型MEMS姿态测量系统，可满足飞行器加速度小于6g,角速度小于±300度/s，而姿态测量误差小于1度。在微小型飞行器的姿态测量系统还需要具有更大适应性的设计。

发明内容

针对上述问题，我们设计了以MEMS惯性导航器件为核心的姿态监测系统，其质量轻，对姿态测试结果的影响较小。同时采用了互补滤波算法，较好地解决了MEMS器件的零漂问题。添加了无线数传模块，实现了飞行器姿态测试数据的无线传输。采用OpenGl图形库开发了姿态动画演示软件，可实现飞行器俯仰角、横滚角和航向角等姿态数据的保存和回放功能。

本发明为了实现上述目的，可以使用以下方案：

本发明提供了一种微小型飞行器地面试验姿态记录仪，包含下位机系统和上位机系统，下位机系统包括：控制单元，惯性检测单元以及无线发送模块；上位机系统包括：姿态解算单元，串口通信模块以及无线接收模块；其中，下位机系统与上位机系统之间通过无线发送模块与无线接收模块实现通信；控制单元由单片机组成；惯性检测单元由MEMS陀螺仪，加速度计以及数字罗盘组成，MEMS陀螺仪获得三轴角速度数据，加速度计获得三轴加速度数据，数字罗盘获得三轴磁场强度数据；串口通信模块实现了上位机系统中各个模块之间的串口通信的协议；姿态解算单元由计算机实现，包含可视化模块以及波形绘制模块；姿态解算单元分别将三轴角速度数据通过对角速度积分检测飞行器在飞行过程中的角度变化，将加速度数据和磁场强度数据，即通过飞行器各个轴上产生的重力分量以及与地球磁场间夹角的分量得到飞行器与惯性坐标系之间的夹角数据，即姿态角数据，并将夹角数据转换成基于四元数的旋转矩阵的方式来描述飞行器的飞行姿态；其特征在于：由单片机控制的下位机系统还包含均值滤波单元、数据打包单元和数据分割单元，均值滤波单元将MEMS陀螺仪获得的角速度数据进行数据均值滤波，消除MEMS陀螺仪因漂移输出的偏移后，连同磁场强度数据和加速度数据由数据打包单元进行打包，并由单片机将数据转换为整数的形式，数据分割单元将打包的数据中每一帧完整的数据分割成两部分后，并设置了起始位、数据位、停止位和校正位，通过无线发送模块将打包的数据发送到上位系统；上位机系统还包含数据组合单元；姿态解算单元还包含四元数互补滤波模块；以及滑动滤波模块；其中，数据组合单元将从下位机系统接受到打包的数据，对相邻接受到的数据进行检验，按照所述起始位、数据位、停止位和校正位进行数据组合成完整的角速度数据、磁场强度数据以及加速度数据，滑动滤波模块将角速度数据中发生突然跃变的数据剔除，将剔除跃变后的角速度数据发送到姿态解算单元进行姿态解算，姿态解算单元首先根据加速度数据和磁场强度数据加速度向量和磁场向量初始化，根据参考系的重力向量和磁场向量的四元数换算得到飞行器机体坐标系的重力单位向量和磁场单位向量，然后四元数互补滤波模块根据姿态解算单元针对MEMS陀螺仪测量得到的角速度进行积分计算后的姿态与加速度计和数字罗盘计算得到的姿态之间计算误差向量，再采用互补滤波的算法，即根据误差向量乘以滤波系数用于修正对于MEMS陀螺仪的角速度积分而累积的误差，将修正后的角速度积分构建基于四元数的旋转矩阵并进行规范化计算实现飞行姿态的更新，最后根据四元数换算成描述飞行器飞行姿态的姿态角。

本发明所涉及的飞行器飞行试验姿态记录仪，可视化模块根据当前姿态角的数据驱动图形绘制实时的虚拟物体的姿态从而实现虚拟可视化功能，波形绘制模块根据当前姿态角的数据绘制姿态角变化的曲线，即包括：俯仰角的曲线、航向角的曲线以及翻滚角的曲线。

另外，滑动滤波模块将角速度数据中复数次测量数据组成数据组为基准，每测量到一次数据后存放为数据组第一个数据，然后移除数据组中最后一个数据，如果在一组测量数据中，发生突然跃变的数据，将发生突然跃变的数据剔除，并将剔除跃变后的先前角速度数据发送到姿态解算单元进行姿态解算。

附图说明

图1是微小型飞行器地面试验姿态记录仪系统的原理图。

图2是在下机位完成的飞行姿态初始数据采集控制主程序流程图。

图3是上位机系统实现飞行姿态解算、波形绘制和飞行姿态可视化的应用程序流程图。

具体实施方式

以下结合附图对本发明所涉及的一种微小型飞行试验姿态记录仪的优选实施例做详细的阐述，但本发明不仅限于该实施例。为了使公众对本发明有彻底的了解，在以下本发明的优选实施例中详细说明了具体细节。

图1为微小型飞行器地面试验姿态记录仪系统的原理图。

如图1所示，本发明所涉及的微小型飞行器地面试验姿态记录仪系统100包含上位机系统300和下位机系统200两部分。其中，下位机优选了以STC89C52单片机204为控制单元，MEMS陀螺仪201、加速度计202及数字罗盘203为惯性检测单元，还基于单片机204设计了均值滤波单元206、数据打包单元205以及数据分割单元207。上位机300由VB开发并运行在PC机301上应用程序构成，上位机系统300还包括：数据组合单元304、姿态解算单元302、可视化模块305以及波形绘制模块305。另外，姿态解算单元302还包含了四元数互补滤波模块3021、滑动滤波模块3022。上位机300与下位机200之间通过NRD24L01无线发送模块208和NRD24L01无线接受模块303实现通信。由于涉及到上位机300的对飞行器的飞行姿态解算，要求数据质量要求较高，同时传输的数据量较大。因此飞行姿态记录仪系统100选用2.4GISM频段、配有大功率PA和LNA芯片的NRD24L01无线发送模块208以及NRD24L01无线接收303传输数据。NRD24L01模块带射频开关、带通滤波器、全双向射频功放功能，从而使它的发射效率和灵敏度达到最高、谐波和干扰达到最小、传输距离达到最远。它的数据传输速率高达2Mbps、支持6个节点组网、支持125个射频通道、极低的功耗，这样可以保障系统的稳定性和抗干扰能力。

飞行姿态记录仪系统100中涉及的惯性检测单元的传感器都是通过IIC通信协议，单片机204作为下位机控制芯片，能够模拟IIC通信实现对于传感器原始数据的采集处理，并进行数据分割，以满足无线模块发送规范，由于不涉及到大量的数据运算，因而其12M运行频率能够满足实时数据传输的要求。

惯性检测单元通过MEMS陀螺仪201、加速度计202、数字罗盘203采集原始数据，原始数据包括：角速度数据、加速度数据以及磁场强度数据。这三款传感器内部都含有三个正交单轴芯片，因而能够实现全方位的数据测量。MEMS陀螺仪201获得三轴角速度数据，加速度计202获得三轴加速度数据，数字罗盘203获得三轴磁场强度数据。

微小型飞行器由于重量、尺寸等因素，在携带飞行姿态检测设备时需要选择体积小，重量轻的元件，本发明优选了微机电系统。微机电系统(MEMS，Micro-Electronic-MechanicalSystems)技术具有很多优势，基于此技术发展起来的MEMS传感器具有体积小、重量轻和价格便宜等优点，近年来在低成本导航系统中得到广泛应用。本系统所用的MEMS传感器优选了包括L3G4200D陀螺仪、ADXl45加速度计和HMC5883L数字罗盘，能够满足本系统对高实时性和高精度的要求。

MEMS陀螺仪201选用ST(意法半导体)公司的MEMS芯片(L3g4200d)，最大支持量程为2000dps，其最小灵敏度较高，约为8.75mdps/LSB，数据输出频率较高，约为800HZ。

加速度计202采用的ADI公司生产的ADXL345三轴加速度传感器。最大支持量程为±16g，全分辨率可达3.9mg/LSB，数据输出频率可达800HZ。

数字罗盘203为霍尼韦尔公司生产的HMC5883L三轴数字罗盘，其测量范围从毫高斯到8高斯(gauss)。内置自检功能并带置位/复位和偏置驱动器用于消磁、自测和偏移补偿。

飞行姿态记录仪系统100中微控制器端，即下位机系统200的数据采集与均值滤波单元所采用的均值滤波算法采用C51语言编写，由单片机204实现控制。计算机端，即上位机系统300应用程序是基于VB6.0平台开发的。

图2为在下机位完成的飞行姿态初始数据采集控制主程序流程图。

如图2所示，在下位机完成对微小型无人机的飞行姿态初始数据采集控制主程序流程如下，姿态初始数据的采集程序主要包括串口通信、无线通信模块、L3G4200D型MEMS陀螺仪201、ADXL345加速度计202、HMC5883L数字罗盘203等模块初始化及中断处理函数。其中为了降低串口数据传输时间，波特率设置为19200bps,系统工作频率为12MHZ。

步骤S1-201:

对下位机系统200和各个模块及单元进行初始化，在微小型无人机上安装硬件及保证正确连线，将单片机与惯性检测单元的MENS传感器，即，L3G4200D型MEMS陀螺仪201、ADXL345加速度计202、HMC5883L数字罗盘203等模块初始化以及保证无线发送模块208接线并上电。

在本实施例对惯性检测单元的传感器，包括L3G4200D型MEMS陀螺仪201、ADXL345加速度计202、HMC5883L数字罗盘203的数据输出平率设置为800Hz，用于解决系统误差。

步骤S1-202:

对无线发送模块208配制发送模式。

步骤S1-203:

惯性检测单元开始获取数据，即MEMS陀螺仪201获得三轴角速度数据，加速度计202获得三轴加速度数据，数字罗盘203获得三轴磁场强度数据。

步骤S1-204:

对MEMS陀螺仪201获得三轴角速度数据进行均值滤波。

微小型无人机的实时动态响应，要求传感器采样频率高及AD转换转换时间短。考虑到单片机204的运算能力，微小型飞行器地面试验姿态记录仪系统100将MEMS陀螺仪201获得的角速度数据由均值滤波单元进行均值滤波，消除MEMS陀螺仪201因漂移输出的偏移。为了方便数据的计算和处理，单片机204将获得的角速度数据、加速度数据以及磁场强度数据以整数形式发送到上位机系统300，即将小数点后的数全都进位到整数位，单片机204也同时把磁场强度数据和加速度数据转换为整数形式。然后，将消除偏移后的角速度数据连同加速度数据和磁场强度数据由数据打包单元进行打包。

步骤S1-205:

利用中断处理函数判断是否串口中断，如果是，进入下一步骤，否，返回。

步骤S1-206:

对打包的数据进行数据分割。

无线发送模块208和无线接收模块203因为发送和接收数据的长度有限制，而由下位机200的惯性检测单元采集的数据超过了长度限制，因而数据分割单元207将打包的数据中每一帧完整的数据分割成两部分并在帧头与帧尾设置了检验规则。

无线模块与微控制器和计算机301均通过串口连接，因而初始状态需要匹配串口参数使上下位机达到同步效果，实现串口通信的功能。为确保数据完整性，自定义了一个通信协议，一帧完整数据由64个字节组成，包含起始位、数据位、停止位和校验位。由于涉及到3个3轴传感器采集的数据和采集时间一共10个数据，每个数据之间以逗号分隔开。由于无线模块每次只能发送32个字节，因而程序对发送数据分隔为两个32位数组BUF[1]和BUF[2],发送起始位位于BUF[1]第一位，停止位位于BUF[2]最后一位。上位机第一次接收到数据会检测其是否包含停止位，如果包含则说明这帧数据为BUF[2]予以丢弃。每次接收到BUF[1]则存入缓冲区，等待接收BUF[2]并组合成一帧完整数据经校验无误后用于姿态运算。为了确保传输效果，无线模块数据发送间隔需要一定时间的延迟，经过多次反复测试延迟10ms效果最佳。

步骤S1-207：

无线发送模块208将分割后的数据发送到上位机300。

步骤S1-208：

进行数据缓冲清空后，进入步骤S1-205进行串口中断的判断。

由于在处理数据转换和计算及数据融合放到上位机300处理，尽量减少单片机204的负载，降低主程序运行周期，以满足由地面实时处理飞行器姿态并记录的要求。考虑到国家和国际对无线电的管制、无线传输的距离、传输的数据量以及实际应用环境障碍物等因素，本系统选用的无线模块对频段、功率和跳频等都有一定的要求。

图3为上位机系统实现飞行姿态解算、波形绘制和飞行姿态可视化的应用程序流程图。

如图3所示，上位机系统300主要包含串口通信、数据显示、数据组合304、无线接收模块303、数据融合姿态解算302、Opengl可视化305、波形绘制306等模块。上位机系统300在实现飞行姿态解算、调用Opengl图形库来实现波形绘制和飞行姿态可视化具有如下步骤。

步骤S1-301：

对下位机系统300的初始化。打开运行在上位机系统300的软件，并给无线接收模块上电303。同时，设置端口号、波特率等参数、匹配上位机与上位机的通信参数，实现串口通信。

步骤S1-302：

调用Opengl图形库并初始化，用于实现可视化模块305以及波形绘制模块306。

步骤S1-303:

串口参数的设置。这里的串口设置使得无线发送模块208与无线接收模块303的同步效果，实现串口通信。串口通信主要基于VB6.0提供的Mscomm控件，提供两种通信处理方法，提供多种属性，满足用户开发需求。应用程序每接收到一帧完整数据，将依据自定义的通信协议自动提取原始数据在相应位置显示并存储，为姿态解算提供数据源。

惯性检测单元的传感器采集数据与上位机系统300的可视化响应之间的响应延迟是飞行姿态记录仪系统100的性能参数，主要由无线模块发送及发送间隙的时间延迟，程序运行周期等因素有关，总响应时间约为80ms，姿态数据数据更新速率于此相关。

步骤S1-304:

判断是否打开串口，如果是，则来到下一步在，如果是否，则回到步骤S1-303,进行串口参数的设置。

步骤S1-305:

数据接收。在设置好串口通信的设置后，上位机系统300的无线接收模块303实时接收到下位机200发送的数据。

步骤S1-306:

数据校验。上位机系统300由无线接收模块303收到的数据，再将数据发送给数据组合单元304。数据组合单元304对相邻接收到的数据进行检验，根据在分割数据单元207对一帧完整数据分割后设置的起始位、数据位、停止位和校正位的规则进行数据组合，完成匹配，合成一帧完整的角速度数据，或者加速度数据和磁场强度数据。

步骤S1-307:

对合并或组合的数据检验其完整性，如果数据不完整，则回到步骤S1-306，如果数据完整则来到下一步骤。

步骤S1-308:

数据显示，直接显示原始数据和经姿态解算单元301处理后的数据，不做可视化或波形的绘制。

步骤S1-309：

将完整的角速度数据、加速度数据和磁场强度数据由姿态解算单元302计算得到飞行器的姿态角用于描述飞行器的飞行姿态。首先，姿态解算单元302要将以整数形式接受到的数据还原成带小数点的原始数据。

飞行姿态是对四旋翼无人机空间姿态的描述方法，以地球惯性坐标系作为参考系，确定机体自身的坐标系(机体坐标系)飞行姿态。通常飞行姿态采用欧拉角方法描述即俯仰角(α)、航向角(β)及翻滚角(γ)。

动滤波模块3022将角速度数据中发生突然跃变的数据剔除，将剔除跃变后的角速度数据发送到姿态解算单元302进行姿态解算。滑动滤波模块3022将角速度数据中复数次测量数据，比如为4次组成数据组为基准，每测量到一次数据后存放为数据组第一个数据，然后移除数据组中最后一个数据。如果在一组测量数据中，发生突然跃变的数据，滑动滤波模块3022会认定是外部干扰造成的，将发生突然跃变的数据剔除，并将剔除跃变后的先前角速度数据，也就是最近3次采集的角速度数据发送到姿态解算单元302进行姿态解算，然后对MEMS陀螺仪201测量得到的角速度数据，姿态解算单元302通过对角速度积分，即得到角度的表达式：从而能够快速准确的检测飞行器在飞行过程中的角度变化。

姿态解算单元302根据加速度计202获得的三轴加速度数据，即加速度计得到沿着飞行器的机体坐标系各个轴上产生的重力分量。

$\mathrm{\ψ}={\tan }^{-1}(a_x/\sqrt{{a_y}^2+{a_z}^2})---\left(2\right)$

${\mathrm{\θ}}_2={\tan }^{-1}(\sqrt{{a_x}^2+{a_y}^2}/a_z)---\left(3\right)$

根据上述公式(2)～(4)，姿态解算单元302计算出机体与惯性坐标系各个轴之间的夹角。

同时，数字罗盘203获得的三轴磁场强度数据，即飞行器与地球磁场间夹角的分量。当飞行器处于平衡状态且绕垂直与地面的Z轴转动时，由于旋转轴与重力方向重合，因而在水平面方向并不会产生相应的重力分量，此时数字罗盘能够检测出飞行器与地球磁场之间的夹角能够起到很好的补偿作用。

机体坐标系相对位置可由参考系通过旋转矩阵获得。将旋转矩阵以欧拉角方法表示如式(5)所示：

$T=\left[\begin{array}{ccc}\cos \mathrm{\α}\cos \mathrm{\γ}-\cos \mathrm{\β}\sin \mathrm{\α}\sin \mathrm{\γ}& \sin \mathrm{\α}\cos \mathrm{\γ}+\cos \mathrm{\β}\cos \mathrm{\α}\sin \mathrm{\γ}& \sin \mathrm{\β}\sin \mathrm{\γ}\\ -\cos \mathrm{\α}\sin \mathrm{\γ}-\cos \mathrm{\β}\sin \mathrm{\α}\cos \mathrm{\γ}& -\sin \mathrm{\α}\sin \mathrm{\γ}+\cos \mathrm{\β}\cos \mathrm{\α}\cos \mathrm{\γ}& \sin \mathrm{\β}\cos \mathrm{\γ}\\ \sin \mathrm{\β}\sin \mathrm{\α}& -\sin \mathrm{\β}\cos \mathrm{\α}& \cos \mathrm{\β}\end{array}\right]---\left(5\right)$

通过式(5)的表述，无人机的飞行姿态可通过旋转矩阵的方式描述。通过惯性检测单元传感器采集原理姿态数据通过姿态耦合处理，即可得到姿态角实现对于飞行轨迹的检测。由于采用旋转矩阵会涉及大量矩阵计算，会造成系统时间和空间的消耗，同时由于三角函数本身的性质会导致奇点的产生。故本发明方法采用四元数法对姿态矩阵进行描述。四元数是最简单的超复数，由实数和三个复数组成代表一个四维空间。常见的四元数数学表示如式(6)所示：

q＝q₀+q₁i+q₂j+q₃k(6)

式中，q₀为超复数的实部，q₁、q₂、q₃为超复数三个虚部且满足 $\sqrt{q_0^2+q_1^2+q_2^2+q_3^2}=1$

旋转矩阵以四元数的方法表示如式(7)所示：

$T=\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]---\left(7\right)$

，将四元数转换为欧拉角数学表示如式(8)所示：

$\left\{\begin{array}{c}\mathrm{\γ}=\arctan 2\[(2q_1{q}_2+2q_0{q}_3),(-2{q_2}^2-2{q_2}^2+1)\]*180/\mathrm{\π}\\ \mathrm{\α}=\arcsin (-2q_1{q}_3+2q_0{q}_2)*180/\mathrm{\π}\\ \mathrm{\β}=\arctan 2\[(2q_2{q}_3+2q_0{q}_1),(-2{q_1}^2-2{q_2}^2+1)\]*180/\mathrm{\π}\end{array}\right.---\left(8\right)$

由于MEMS陀螺仪201有良好的动态性能而加速度计202有着良好的静态新能。静态误差由于陀螺仪输出偏移导致姿态解算的累积误差。动态误差则是加速度计输出为重力分量与线性加速度的叠加量，导致误差向量存在偏差。造成陀螺仪姿态修正的偏移误差。四元数互补滤波单元3021采用基于四元数的互补滤波算法，将加速度向量和磁场向量进行直接四元数转换，然后将转换后四元数对陀螺仪输出的方向向量进行修正，达到了结合加速度计202静态性能良好的特点，产生对MEMS陀螺仪201积分累计误差进行补偿的效果。

首先，姿态解算单元302利用加速度数据和磁场强度数据得到一个初始化的加速度向量和磁场向量，并根据惯性参考系的重力向量和磁场向量四元数换算得到机体坐标系的重力单位向量，也就是加速度向量和磁场单位向量。姿态解算单元302分别先根据进行了滑动滤波模块3022作用的角速度数据进行积分后建立的基于四元数的旋转矩阵解算出的一组姿态角以及根据加速度计202和数字罗盘203的加速度数据和磁场强度数据互补后建立的基于四元数的旋转矩阵解算出另一组姿态角。

由于对MEMS陀螺仪的角速度进行积分，随着积分的进行累计MEMS陀螺仪角速度的误差，然后由四元数互补滤波模块3021计算由MEMS陀螺仪解算出的姿态角与结合了数字罗盘和加速度计解算得到的姿态角之间的误差向量。采用互补滤波算法，即根据误差向量乘以滤波系数用于MENS修正陀螺仪零漂,就是因角速度积分而累积的误差，同时，也剔除了高频运动中的加速度，提高了计算精度。将修正后的角速度积分构建四元数的旋转矩阵并进行规范化技术，实现飞行姿态的更新。

基于上述的旋转角与四元数之间的换算公式(8)以及表示旋转矩阵的公式(5)和(7)，可以计算得到一组新的姿态角的值，通过基于四元数的互补滤波算法进行数据耦合得到飞行姿态信息。基于这组新的姿态角的值来实现可视化和波形的绘制。四元数的算法运算量小，能够降低处理器的工作负荷，同时加以滑动滤波能够避免漂移及突变数据对于运算带来的误差，互补滤波算法有效的结合陀螺仪的动态性能及加速度计和数字罗盘的静态性能，提高系统的精度及可靠性。

本发明涉及的飞行姿态记录仪系统100能够满足加速度±16g，角速度小±2000dps(度/秒)，采用四元数互补滤波模块3021进行姿态解算，姿态测量误差能够达到0.3度。

步骤S1-310:

点击捕捉与绘图按钮，驱动Opengl图形库实时将当前姿态角信息显示并绘制波形曲线。

Opengl可视化功能能够根据姿态解算后的数据驱动Opengl图形库绘制

波形绘制模块依靠VB6.0提供的picture控件通过调用.Line方法，数据驱动实时绘制数据波形曲线。将经过姿态解算单元302通过公式(8)解算的姿态角，以时间为横轴分别绘制俯仰角的角度变化曲线、航向角的角度变化曲线以及翻滚角的角度变化曲线，用于反映飞行器的姿态角的变化。

步骤S1-311：

点击捕捉与绘图按钮，驱动Opengl图形库实时模拟当前目标的运动姿态。

Opengl可视化功能能够根据姿态解算后的数据驱动Opengl图形库绘制当前状态下目标物体的实时姿态实现虚拟可视化的功能。基于VB6.0平台的应用程序，根据姿态解算单元302通过公式(8)技术得到的姿态角，实现姿态动画演示，主要包含的是串口通信及姿态仿真其中姿态仿真采用的是基于OpenGl图形库绘制的飞行器模型，为了更加直观的了解姿态角的数据通过姿态数据驱动实时构建当前飞行器的运动姿态。

Claims (3)

1.一种微小型飞行器地面试验姿态记录仪，包含下位机系统和上位机系统，下位机系统包括：控制单元，惯性检测单元以及无线发送模块；所述上位机系统包括：姿态解算单元、串口通信模块以及无线接收模块；

其中，所述下位机系统与所述上位机系统之间通过所述无线发送模块与所述无线接收模块实现通信；

所述控制单元由单片机组成；

惯性检测单元由MEMS陀螺仪，加速度计以及数字罗盘组成，所述MEMS陀螺仪获得三轴角速度数据，所述加速度计获得三轴加速度数据，所述数字罗盘获得三轴磁场强度数据；

串口通信模块实现了上位机系统中各个模块之间的串口通信的协议；

所述姿态解算单元由计算机实现，包含可视化模块以及波形绘制模块；

所述姿态解算单元分别将三轴角速度数据通过对角速度积分检测所述飞行器在飞行过程中的角度变化；

将所述加速度数据和磁场强度数据，即通过所述飞行器各个轴上产生的重力分量以及与地球磁场间夹角的分量得到飞行器与惯性坐标系之间的夹角数据，即姿态角数据，并将所述夹角数据转换成基于四元数的旋转矩阵的方式来描述所述飞行器的飞行姿态；

其特征在于：

由所述单片机控制的所述下位机系统还包含均值滤波单元、数据打包单元和数据分割单元，所述均值滤波单元将所述MEMS陀螺仪获得的角速度数据进行数据均值滤波，消除所述MEMS陀螺仪因漂移输出的偏移后，连同磁场强度数据和加速度数据由所述数据打包单元进行打包，并由所述单片机将所述角速度数据、所述加速度数据以及所述磁场强度数据转换为整数的形式，

所述数据分割单元将打包的数据中每一帧完整的数据分割成两部分后，并设置了起始位、数据位、停止位和校正位，通过无线发送模块将打包的数据发送到所述上位机系统；

所述上位机系统还包含数据组合单元；

所述姿态解算单元还包含四元数互补滤波模块以及滑动滤波模块；

其中，所述数据组合单元将从所述下位机系统接收到所述打包的数据，对相邻接受到的数据进行检验，按照所述起始位、数据位、停止位和校正位进行数据组合成完整的角速度数据、磁场强度数据以及加速度数据，

滑动滤波模块将角速度数据中发生突然跃变的数据剔除，将剔除跃变后的角速度数据发送到所述姿态解算单元进行姿态解算，

所述姿态解算单元首先根据所述加速度数据和磁场强度数据得到初始化的加速度向量和磁场向量，根据参考系的重力向量和磁场向量的四元数换算得到所述飞行器机体坐标系的重力单位向量和磁场单位向量，然后所述四元数互补滤波模块计算由MEMS陀螺仪解算出的姿态角与结合了数字罗盘和加速度计解算得到的姿态角之间的误差向量，再采用互补滤波的算法，即根据所述误差向量乘以滤波系数用于修正对于所述MEMS陀螺仪的角速度积分而累积的误差，将修正后的角速度积分构建基于四元数的旋转矩阵并进行规范化计算实现飞行姿态的更新，最后根据四元数换算成描述所述飞行器飞行姿态的姿态角。

2.根据权利要求1所述的微小型飞行器地面试验姿态记录仪，其特征在于：

其中，所述可视化模块根据当前所述姿态角的数据驱动图形绘制实时的虚拟物体的姿态从而实现虚拟可视化功能，

所述波形绘制模块根据当前所述姿态角的数据绘制姿态角变化的曲线，即包括：俯仰角的曲线、航向角的曲线以及翻滚角的曲线。

3.根据权利要求1所述的微小型飞行器地面试验姿态记录仪，其特征在于：

其中，所述滑动滤波模块将角速度数据中复数次测量数据组成数据组为基准，每测量到一次数据后存放为所述数据组第一个数据，然后移除所述数据组中最后一个数据，如果在一组测量数据中，发生突然跃变的数据，将所述发生突然跃变的数据剔除，并将剔除跃变后的先前角速度数据发送到所述姿态解算单元进行姿态解算。