基于MPU-6050及蓝牙技术的姿态检测系统设计

白礼卓 鲜鹏飞 尹雪 马勇

摘   要：文章对姿态传感器国内外现状进行了分析，对其技术发展和存在的问题进行了简单介绍。提出了一种基于MPU-6050六轴姿态传感器（三轴陀螺仪、三轴加速度计）及蓝牙技术的设计方案，采用ARM和MPU-6050姿态传感器模块设计了一套姿态检测系统，手机可连接蓝牙模块并通过安卓手机APP实时显示姿态角数据。此外，研究了四元数及卡尔曼滤波等方法，对六轴传感器数据进行了静态修正，得到了更高精度的姿态角数据。

关键词：姿态检测;卡尔曼滤波算法;四元数;ARM;蓝牙技术

最近十年来，人机交互领域得到了快速的发展，其中包括智能驾驶汽车、动作捕捉（可穿戴设备）、无人机等。但有很多问题需要进一步解决，其中关键的问题有两个[1-2]：（1）姿态传感器的数据采集与数据融合。（2）实时进行数据的回传与分析。其中，姿态解算以及姿态控制、姿态角回传等是非常值得研究的关键技术，研究基于ARM的姿态解算算法及蓝牙通信技术，不仅具有广泛的理论意义，而且对实际的应用也非常重要。

1    理论基础

1.1  四元数法

四元数法在三维空间旋转变换中能有效避开万向节死锁问题且计算量小。四元数用英文字母Q表示，它由单位为1的实部q和以i、j、k表示基本单位的3个相互正交虚部共同构成[3]。载体姿态表达式可用这4个参数线性组合表示。Q的表达式通常写成：

载体的姿态更新，实际上就是要获得下一时刻载体的四元数。四元数包含了载体姿态的全部信息，因此也可以通过四元数的微分方程来获得新的四元数。

1.2  卡尔曼滤波算法

卡尔曼滤波算法（Kalman Filter Algorithm，KFA）实际上是求下一时刻数据最优解算法。在姿态解算过程中陀螺仪的积分误差不断地积累，计算出的姿态角会产生明显漂移，要修正姿态角最好的方法是融合加速度计和磁力计的观测值[4]。

假设某线性系统的状态方程为：

当前时刻t的系统真实值是Xt，Ft为状态转移矩阵，Ut-1是t-1时刻外部的控制量，Bt-1是控制矩阵，Wt-1为系统噪声[5]。系统的状态预测公式为：

卡尔曼滤波每个时刻系统的不确定性都用P来表示，为了表明观测的不确定性在t和t-1时刻间的传递，引入预测估计协方差矩阵，其中Q为过程噪声：

Pt-1是t-1时刻系统误差协方差，P-t是预测的t时刻系统误差协方差。

卡尔曼滤波不断地重复“预测值→实测值→最优估计值”的数学运算思路构建起最优的估计。

2    系统硬件设计

系统硬件总体设计方案如图1所示。

采用MPU-6050六轴传感器芯片，里面集成了加速度计和陀螺仪，将其与STM32单片机通过I2C接口连接。再将蓝牙模块HC05通过杜邦线与STM32单片机相连接，姿态数据通过蓝牙模块无线传输到手机APP。

3    系统软件设计

从图2中可知，最先开始配置时钟，再初始化MPU-6050传感器。再将MPU-6050中读取到的陀螺仪、加速度计数据存储到寄存器，准备让STM32内核处理器进行四元数归一化处理和卡尔曼滤波。同时进行蓝牙模块的初始化操作，成功则操作蓝牙模块进入MARSTER主机模式，尝试进行蓝牙匹配。最后手机端匹配蓝牙模块，成功则进行姿态数据传输，失败则返回INIT指令重启蓝牙模块。STM32主要负责完成姿态传感器数据采集、姿态解算和姿态输出，蓝牙模块负责读取STM32的姿态输出数据并且发送至手机，上位机软件负责PC端解析数据和3D航模实时显示[6]。

4    测试结果

本论文设计的手机APP主要实现显示MPU-6050原始数据，以及解算出来的姿态角AngelX，AngelY，AngelZ数据。手机APP显示结果如图3所示。

5    结语

在Matlab静态测试中，横滚角AngleROLL变化范围为-2.510 0°～-2.400 0°，俯仰角AnglePIT變化范围为0.870 0°～ 0.910 0°，偏航角AngleYAW变化范围为2.110 0°～2.140 0°。在检测偏航角时，当载体在静止状态时，可增大磁力计数据的权重，当载体在运动状态时，增大陀螺仪权重，互补了数据的误差，获得了更准确的姿态数据。此设计具有成本低、实时性的特点，在研究姿态数据回传上具有良好的可视性，在提高小型无人机飞行稳定性、提高智能驾驶准确性、更精确的动作捕捉（可穿戴设备）等方面有一定的意义。

[参考文献]

[1]胡茂晓.惯性动作捕捉前端设备与数据传输研究[D].济南：山东大学，2015.

[2]蒋硕硕.传感器技术的发展现状与应用前景探讨[J].电子技术与软件工程，2013（9）：23.

[3]文亮.四元数矩阵[M].长沙：国防科技大学出版社，2002.

[4]乔会敏，张嘉易，郝永平，等.一种微机械陀螺仪误差的高精度补偿方法[J].国外电子测量技术，2012（8）：18-20，30.

[5]DE RUITER A H J.Extended kalman filtering and nonlinear predictive filtering for spacecraft attitude determination[J].Aeronautical Journa，2015（7）：22-32.

[6]罗玮.一种新兴的蓝牙技术—超低功耗蓝牙技术[J].现代电信科技，2010（10）：31-34，38.

Abstract：In this paper， the status quo of attitude sensor at home and abroad is analyzed， and its technical development and existing problems are briefly introduced. A design scheme based on MPU-6050 six-axis attitude sensor（three-axis gyroscope， three-axis accelerometer）and Bluetooth technology is proposed. A set of attitude detection system is designed by ARM and MPU-6050 attitude sensor module. The mobile phone can be connected to the Bluetooth module. And the attitude angle data is displayed in real time through the Android mobile phone APP. In addition， methods such as quaternion and Kalman filtering are studied， and the six-axis sensor data is statically corrected to obtain higher-precision attitude angle data.

Key words：attitude detection; Kalman filtering algorithm; quaternion; ARM; Bluetooth technology