基于STM32和MPU6050的姿态控制系统

范浩，徐龙，赖福明，蔡文广，吕明泽，李媛

（辽宁科技大学，辽宁 鞍山 114051）

0 引言

随着科技与生活方式的不断变化，无论是在工业制造领域还是民用领域，各类自动化，准智能化的产品层出不穷。随之而来的，是各种控制技术的大规模应用，尤其是传感器技术的飞速发展。加上物联网技术的快速发展，各种民用机器人和智能家居走上市场，其中不乏空中作业和各类变姿态作业设备[1-3]。此时就需要姿态控制系统的加入，并且要求具有高精度，高可靠性，低成本的特性。由以上因素，采用低成本的主控芯片和传感器，并结合新型滤波技术和相关算法提供高质量的姿态控制效果，可以大幅减少研发成本和生产成本。

1 系统原理

1.1 基本架构与流程

实现基本姿态控制需要陀螺仪传感器和MCU处理器两大部分，数据处理结束后可将数据上传至上位机进行具体操作，如图1所示。

1.2 MPU6050

MPU6050为全球首例整合性6轴运动处理组件，MPU6050的角速度全格感测范围为±250、±500、±1000与±2000°/sec(dps)，可准确追踪快速与慢速动作，并且，用户可程式控制的加速器全格感测范围为±2g、±4g、±8g、±16g。

可实现1000Hz采样频率，采样数据储存在内部寄存器中，可进行实时更新及读取。MPUI6050可使用IIC协议进行通讯，可输出X，Y，Z轴的角速度与角加速度。

1.3 STM32F103ZET6

STM32F103ZET6为ST公司出品的ARM架构32位MCU，使用Cortex-M3核心具有72MHz主频，拥有512KB Flash储存，可在-40℃-85℃环境中工作，具有八组GPIO接口，扩展性非常好，通过GPIO接口可实现复杂时序以及多种协议和驱动的构建，可以完全符合本次研究需求[4-5]。

2 硬件结构

2.1 MPU6050电路设计

MPU6050仅需极少的外围元件即可正常工作，且具有低功耗的特性。由于要与STM32F103ZET6进行IIC通讯，在其SDA与SCL脚位应当加4.7K的上拉电阻。

2.2 组合电路设计

由于STM32F103ZET6的硬件IIC具有先天稳定性缺陷，故采用软件模拟IIC的形式驱动MPU6050。将STM32F103ZET6的PB8 GPIO口定义为模拟SCL脚，PB9 GPIO口定义为模拟SDA脚，并上拉连接至MPU6050的SCL、SDA脚。双芯片均采用3.3v稳压供电，使用单电源即可。

3 功能实现

3.1 IIC总线

IIC（Inter－Integrated Circuit）总线是一种由PHILIPS公司在80年代开发的两线式串行总线，用于连接微控制器及其外围设备。IIC串行总线一般有两根信号线，一根是双向的数据线SDA，另一根是时钟线SCL，其时钟信号是由主控器件产生。所有接到IIC总线设备上的串行数据SDA都接到总线的SDA上，各设备的时钟线SCL接到总线的SCL上。对于并联在一条总线上的每个设备都有唯一的地址[6]。

3.2 IIC时序分析

起始条件：当时钟线SCL为高期间，数据线SDA由高到低的跳变。

停止条件：当时钟线SCL为高期间，数据线SDA由低到高的跳变。

应答信号为低电平时，规定为有效应答位（ACK），表示设备已经成功地接收了该字节。

应答信号为高电平时，规定为非应答位（NACK），一般表示设备接收该字节没有成功。

IIC总线进行数据传输时，SCL信号为高电平期间，SDA上的数据必须保持稳定；只有在SCL上的信号为低电平期间，SDA上的电平状态才允许变化。

需要注意的是，使用STM32内部时钟的计时器会引起IIC时序延时失败，无法初始化以及通讯，所以应当采用外部时钟进行延时程序的编写[7]。

3.3 MPU6050配置

对IIC接口以及MPU6050进行初始化配置后，即可读取寄存器数据，获得12个原始数据，将高八位和低八位合并后获得16进制数值，即可获得六个基础数据（ACCEL_X，ACCEL_Y，ACCEL_Z，GYRO_X，GYRO_Y，GYRO_Z）如需把原始的16位数据用重力加速度单位G表示，需要注意传感器的量程。默认测量范围是±2G，一共是4G的宽度，16位数据的最大读数为65536，除4就是16384，即1G加速度对应的数值。打印前把数据除以16384即可得到对应单位为G的数值，角速度的单位转换也同理[8]。

3.4 欧拉角获得

如图2所示，在取得ACCEL_X，ACCEL_Y，ACCEL_Z数值后通过公式Angle*=acos(ACCEL_*/G)，计算得到坐标轴与重力的夹角，到此全部运动参数均已得到，通过一定的控制逻辑即可获取物体运动与姿态的一切参数。

4 分析及误差处理

4.1 稳态误差消除

由于生产加工和芯片焊接产生的误差，原始数据会有一定的静态漂移量，可在一已知水平面上放置MPU6050进行稳态误差测量，当Z轴默认与水平面垂直时，Z轴加速度应为重力加速度G，而其余轴应为0，若数据不符合理论值，则将数据偏离量定义为Δ，将输出值-Δ即可得到校正后数值。

4.2 随机误差消除

考虑到一阶互补滤波具有明显相位滞后并且效果不佳，所以采用二阶互补滤波的方式进行滤波。对MPU6050来说，加速度计对载体的加速度比较敏感，取瞬时值计算倾角误差比较大，而陀螺仪积分得到的角度不受载体加速度的影响，但是随着时间的增加积分漂移和温度漂移带来的误差比较大。所以这两个传感器正好可以弥补相互的缺点。

5 上位机通讯测试

本系统采用CH340G芯片进行USB/串口转换，CH340G与大多数使用串口的应用程序兼容而不需要改变现有的代码。CH340G可以用来将现有的使用串行端口的外设升级到USB设备，将串行端口升级到PC。外置电平转换硬件，可提供包括RS232，RS422和RS485的接口。计算机通过串口接收到数据后通过编程语言可方便地进行相关调试工作。经测试，本套试验系统姿态角度误差±0.2°，达到良好的可用性以及可靠性。

6 结论

本文介绍了基于STM32和MPU6050的姿态控制系统，对系统的组建和调试进行了详细的阐述，对其中的几个关键问题进行了解决。试验结果证明本系统具有良好的可用性，丰富的功能以及较好的兼容性，可以实现速度加速度以及角度信息的获取与处理，基本可以实现任意运动状态的检测和反馈控制，在各类姿态控制平台上均可应用。