基于STM32的姿态测量系统设计

摘 要： 以无人机航姿测量系统小型化、量轻化为背景，设计了一种基于微惯性单元MEMS的姿态测量系统。系统以STM32F103C8T6为主控制器，通过I2C总线分别采集惯性测量单元MPU6050和数字罗盘HMC5883L的测量数据，利用数据融合算法解算无人机当前姿态。对于MEMS温漂和噪声干扰的问题，提出了一种基于四元数的互补滤波算法，对测量的姿态数据进行补偿修正。实验结果表明，该姿态测量系统简单可靠、性能稳定、精确度高，成功完成了姿态的最优控制。

关键词： MEMS； STM32； 姿态测量系统； 互补滤波

中图分类号： TN98?34； TP249 文献标识码： A 文章编号： 1004?373X（2016）09?0012?03

Abstract： An attitude measurement system based on micro inertial unit MEMS was designed by taking the miniaturization and light weight UAV attitude measurement system as the background. The STM32F103C8T6 is taken as the main controller in the system to acquire the measurement data of the inertial measurement unit MPU6050 and digital compass HMC5883L through I2C bus， and then the data fusion algorithm is used to solve the UVA current attitude. For the problems of MEMS temperature drift and noise interference， a complementary filtering algorithm based on quaternion is proposed to compensate and correct the measured attitude data. The experimental results show that the attitude measurement system is simple and reliable， and has stable property and high precision. The attitude optimal control was successfully completed.

Keywords： MEMS； STM32； attitude measurement system； complementary filtering

0 引 言

小型无人机在国防建设和国民经济中具有广阔的应用前景，是当前的研究热点之一[1]。姿态的精确测量则是获得良好飞行品质的基础。体积小、重量轻、集成度高的MEMS传感器是完成微型姿态测量系统的最佳选择，但同时这类传感器有一定的缺陷。陀螺仪具有良好的高频动态响应特性，但受振动容易引起测量噪声，且输出存在温漂和累计误差，解算时姿态易发散，只适合做短时间的测量；加速度计与磁阻传感器在低频段动态响应特性好，能得到不随时间累计误差的姿态角，但在高频段动态响应特性差。因此综合陀螺仪、加速度和磁阻传感器各自的频率响应优势，从频域角度对3个传感器数据进行融合，有利于使传感器信息在时间和空间上相互补充，提高传感信息的可靠性[2]。

系统由主控制器（STM32F103C8）、六自由度惯性测量单元（MPU6050）和数字罗盘（HMC5883L）组成。MEMS主要作用是实时监测机器人的原始姿态信息并将模拟测量信号转化为数字信号输出，主控制器及其电路的主要任务是实时采集传感器输出的数字测量信号以及融合原始姿态数据，得出俯仰角、翻滚角和航向角的姿态信息[3]。姿态控制系统硬件结构如图1所示。

1 主要模块

1.1 微处理器模块

处理器模块是整个控制系统的核心，主要任务是提取各个传感器的信息，利用数据融合算法解算出当前姿态。姿态测量系统中数据集与处理的主控制器采用STM32F103C8T6，该芯片基于超低功耗的CortexM3内核，32位字宽，工作频率高达72 MHz，拥有80个快速I/O端口，9个标准的通信接口（2个I2C接口，3个USURT，2个SPI，一个CAN接口）提供了丰富的外设资源，便于系统的开发和扩展（STM32）。因此，该处理器模块满足对多传感器数据同步集以及数据融合算法运算速度的要求。

1.2 姿态传感器模块

MPU6050六轴传感器芯片集成了3轴MEMS陀螺仪和3轴MEMS加速度计，测量范围达±16 g，其高分辨率（3.9 mg/LSB）能够测量不到1°的倾斜角度变化。芯片正常工作时，陀螺仪和加速度计分别采集[x]轴，[y]轴和[z]轴的电压值，然后通过一个16位A/D转换器，转换成数字信号传送到处理器，但此时得到的值并不是实际的角度和角速度值，还必须经过一定的比例关系进行转换，才能得到实际的角度和角速度值[4]。

MHC5883L是由美国Honeywell公司生产的三轴磁场强度敏感器件，内置ASIC放大器，工作频率达160 Hz，罗盘航向精度精确到1°～2°，能在±8高斯的磁场中实现 5毫高斯分辨率，I2C总线输出。通过磁阻传感器测量地磁信息，主要用于测量地磁场矢量，定位设备的方向。

2 基于四元数的姿态解算

2.1 姿态的坐标描述

3 多传感器的数据融合

数据融合算法设计是姿态测量系统中重要的环节。在本文的姿态测量系统中，互补滤波算法的主要思路是将加速度计和电子罗盘分别相对于重力加速度矢量与地磁场矢量的姿态角叠加到由陀螺仪高速积分得到的角增量中，利用翻滚角[?]补偿[x]轴的角速度[wx，]俯仰角[θ]补偿[y]轴的角速度[wy，]偏航角[φ]补偿[z]轴的角速度[4][wz。]设置互补滤波器的传递函数为：

式中：[G（s）]表示姿态的方向余弦矩阵；互补滤波器计算输出的姿态方向余弦矩阵为[R；][C0]表示由加速度计和数字罗盘观测到的姿态方向余弦矩阵；[WH]表示加速度计和数字罗盘的高频观测噪声；[C0=R+WH；][C1]表示由陀螺仪计算得到的姿态信息，[C1=R+WL，][WL]表示陀螺仪的低频累积误差。

因此，互补滤波算法在传感器数据整合上，能同时滤除低频和高频干扰，在一定程度上提高了系统精确度[5]。互补滤波器大多采用比例或比例积分PI补偿方法，[G（s）]若取常数[k，]则设计的低通滤波器和高通滤波器的截止频率为[f=k2π，]当[k]较大时，截止频率[f]较高，则滤除低频的、幅值较大的运动加速度效果较差；[k]较小时，截止频率[f]较低，易受陀螺仪的时间漂移造成误差影响[6]。由于系统具有一定的可预见性，这里参考文献[7]利用互补滤波器引入模糊机理实时推导参数[k]的方法。

4 实验结果与分析

在搭建的硬件平台上实现上述滤波算法。通过硬件电路采集各传感器的信号，并在STM32内将陀螺仪、加速度计、电子罗盘3个传感器的测量数据进行解算和滤波[2]。通过串口传输到PC机上，实时显示各姿态信息，最终运用Matlab实现数据的处理。观察采用互补滤波前后姿态角的变化。图2是俯仰角融合前后对比图，0～3 s为陀螺仪振动情况，融合前后的角度信息非常接近，3～6 s稳定情况下，信号具有偏差，该图有效地说明了加速度传感器的数据可以纠正稳态下的姿态信息。图3说明陀螺仪自身存在随机漂移误差，并且误差会随时间不断积累，相对于融合前，融合后波形稍有延迟，相位有一定的变化，数据稳定性有了很大改善。上述实验说明采用互补滤波进行数据融合，噪声振幅小，抗干扰能力增强，数据更加稳定和准确。

5 结 论

本文设计的基于STM32的姿态角测量系统，利用四元数法进行姿态解算，并通过互补滤波进行数据融合，有效综合了陀螺仪、加速度计、磁力计在姿态测量中的优缺点，不仅提高了姿态测量的精确度，而且增强了系统的总体性能和容错能力，为无人机完成各种飞行任务提供了根本保证。

参考文献

[1] 余国林，陈继平，余涛，等.无人机航空遥感平台机载作业控制系统设计[J].现代电子技术，2012，35（4）：132?135.

[2] 万晓凤，康利平，余运俊，等.互补滤波算法在四旋翼飞行器姿态解算中的应用[J].测控技术，2015，34（2）：8?10.

[3] 万良金.基于多传感器信息融合的机器人姿态测量技术研究[D].北京：北京交通大学，2015.

[4] 赖义汉，王凯.基于MPU6050的双轮平衡车控制系统设计[J].河南工程学院院报，2014，26（1）：53?57.

[5] 刘辉邦，褚金奎，支炜，等.基于STM32的无人机姿态测量系统设计[J].传感器与微系统，2013，32（8）：108?110.

[6] 郑健.基于9轴传感器的姿态参考系统研究与实现[D].成都：电子科技大学，2013.

[7] 吕印新，肖前贵，胡寿松.基于四元数互补滤波的无人机姿态解算[J].燕山大学学报，2014，38（2）：175?180.