基于STM32的MPU9255姿态解算算法的实现

东华理工大学机械与电子工程学院 王孚贵 朱兆优 李 星 赖云波

基于STM32的MPU9255姿态解算算法的实现

东华理工大学机械与电子工程学院 王孚贵 朱兆优 李 星 赖云波

本文采用STM32微控制器与MPU9255多轴姿态传感器结合，设计完成了水下机器人姿态检测和信息解算。通过MPU9255传感器内置的陀螺仪、加速度计和磁力计，对系统的三轴角速度、三轴加速度、三轴磁感应强度进行信息采集，利用四元数姿态解算方法，对所测量的数据进行分析与计算，进而解算成水下机器人的姿态信息，并采用PID算法输出PWM信号，实现对水下机器人系统的有效控制。文中给出信息采集、解算方法和程序算法思路，具有较好的实用价值。

STM32微控制器；MPU9255传感器；四元数；姿态解算

0 引言

MPU9255是内部集成陀螺仪、加速度计、磁力计的9轴姿态传感器。通过陀螺仪检测物体处于运动状态时的角速度，再利用算法解算成物体当前处于的角度，加速度计和磁力计分别测量加速度和磁感应强度，在数据算法中，四元数解算算法能够利用加速度数据对陀螺仪测量的存在误差的角度数据进行补偿，有效的减少误差，使得测量物体的姿态信息更加准确。通过微控制器实时解算姿态角。

1 系统设计

对于物体姿态信息检测是以STM32微控制器构成的硬件系统，通过其内部集成的I2C接口与MPU9255进行通信，通过接口读取MPU9255姿态传感器各轴的数据，读取到的数据是未进行算法处理的基本姿态信息，进而进行算法解算。解算完成后通过串口发送给上位机显示，并输出脉冲宽度调制信号用以执行机构调整当前物体的运动状态。姿态信息检测系统设计框图如图1所示。

图1 姿态检测系统设计框图

MPU9255是集成三轴陀螺仪、三轴加速度计和三轴磁力计的九轴物体姿态检测传感器，检测数据全数字输出，其高度的集成性避免了单体传感器组合时数据间产生的误差问题，并且减少了单体传感器带来的体积和系统功耗问题。

2 基于四元数的姿态解算

四元数是一种常见的表示旋转的方式。四元数中通过绕一个定义在参考坐标系的向量µ的单次转动来实现一个坐标系到另一个坐标系的变换，其中向量µ既表示旋转轴的长度，也表示旋转角，四元数Q通常表示为：

并且四元数的旋转矩阵为（b系到R系的坐标变换矩阵）：

四元数的解算姿态角的流程如图2所示。

图2 四元数解算过程

2.1 四元数初始化

通过STM2微控制器在任意个连续的采样周期内测量MPU9255传感器的输出数据，求得其输出数据的平均值，作为四元数解算姿态信息的基本数据。

2.2 修正陀螺仪数据误差

三轴重力加速度计在运动过程中容易受机体的振动而产生较大的误差，产生较大的噪声，短时间内的可靠性不高。陀螺仪在运动过程中比较稳定，且噪声波动小，但是对陀螺仪数据长时间积分会导致数据误差，产生漂移。所以需要通过加速度计和电子磁力计测量的数据来修正由陀螺仪数据快速解算得到的存在误差的姿态，最后得到准确的物体的姿态角度。

根据四元数矩阵以及用来定义物体姿态信息的欧拉角，转换物体的地理坐标系的重力向量，设所测姿态物体的从地理坐标系旋转到机体坐标系上的重力加速度为：

通过陀螺仪的积分来获得旋转角度，然后通过加速度和磁力计的比例和积分运算来修正陀螺仪的积分结果。

在一个三维的空间内，因为重力加速度的存在，加速度计提供了一个水平位置的绝对参考，但是它无法给提供一个方向的参考。而磁力计提供了一个正北方向的绝对参考。如果磁力传感器各轴的数均是0，那么忽略该磁力计磁数据。否则在磁力计数据归一化处理的时候，会导致除以0的错误。

2.3 数据融合

为了快速修正因积分而产生误差的陀螺仪数据，需要通过PI系数进行修正，经过大量实际实验的验证，当Ki=2.0f，Kp=0.005f的时候，对陀螺仪积分误差的补偿效果最好。

2.4 更新四元数

更新四元数实质即为对四元数的微分方程进行求解。对于四元数的微分方程，使用修正后的陀螺仪数据对时间积分，得到用四元数表示的姿态，然后求解四元数。因为需要满足解算结果的精确，所以采用一阶的龙格库塔法对四元数微分方程进行求解。

2.5 规范化处理

因为微处理器在实时更新并计算四元数的原因，所以存在浮点型数据计算不完整的情况而存在误差，误差使得在计算中的数据逐渐没有了规范化特性，所以在每一次对四元数进行计算更新之后，需要重新对四元数的规范化进行处理。

2.6 姿态角与欧拉角的转换

最后，通过欧拉角对机体姿态进行描述，利用四元数与欧拉角的公式进行转换，欧拉角包括俯仰角、滚转角、偏航角：

由于arctan和arcsin的结果是，这并不能覆盖所朝向（对于俯仰角的取值范围已经满足），因此需要用atan2来代arctan。

由四元数计算出的欧拉角单位是弧度，需要乘以57.3将弧度转为角度，完成欧拉角单位的转换。至此，已将四元数转换成精确的姿态信息。

3 结论与分析

实验部分是通过MPU9255实测的数据与四元数算法解算出的数据进行对比，以达到对四元数解算算法的验证。在MPU9255的实测数据的采集过程中，将MPU9255传感器安装在三轴转台上，转台由光栅码盘计算转过的角度，将采集到的转台的三轴角度与微控制器经过四元数算法采集到的MPU9255传感器的数据进行对比。

通过数据的对比，发现四元数解算算法能准确的解算出MPU9255传感器的姿态角，并能有效的抑制传感器的漂移误差，使解算出来的数据更接近真实值。

基于四元数对MPU9255姿态解算的方法中，充分的利用三轴陀螺仪、三轴加速度计以及三轴磁力计的数据对机体姿态角度进行修正，使测量的姿态角度更加的精确，经过程序代码的验证，证明了四元数解算算法的有效性和准确性，有着广泛的应用前景。

[1]张荣辉,贾宏光,陈涛．基于四元数的捷联式惯性导航系统的姿态解算[J]．光学精密工程,2008(10)：1963-1968．

[2]邓正隆．惯性技术[J]．哈尔滨工业大学出版社,2006．

[3]王巍．惯性技术研究现状及发展趋势[J]．自动化学报,2013,39(6)：723-729．

[4]夏圣,许勇．基于MEMS组合模块的姿态检测系统设计[J]．单片机与嵌入式系统应用,2014(4)：52-55．

[5]马艳红,胡军．姿态四元数相关问题[J]．空间控制技术与应用,2008(03)：55-60．