STM32实现MPU6050姿态解算

本文介绍基于STM32与MPU6050传感器的四旋翼无人机姿态解算方法，采用互补滤波融合加速度计与陀螺仪数据，并通过匿名上位机协议实现串口通信，支持实时监控与调试，适用于嵌入式飞控开发。

ss78901

1514人浏览 · 2025-11-04 09:08:11

ss78901 · 2025-11-04 09:08:11 发布

基于STM32的四旋翼无人机项目（二）：MPU6050姿态解算（含匿名上位机串口通讯版本代码）

在调试一个小型四轴飞行器时，你有没有遇到过这样的问题：刚一通电，电机突然猛转一下，机体剧烈抖动？或者飞行过程中姿态慢慢“跑偏”，明明没打舵却越飞越歪？这些问题的背后，往往不是电机或遥控的问题，而是 姿态感知系统出了偏差 。

对于任何自主飞行的多旋翼平台来说，准确知道自己“身体”朝哪个方向倾斜——也就是实时获取俯仰（Pitch）、横滚（Roll）和偏航（Yaw）角度——是实现稳定悬停、精准控制的前提。而这个任务，就落在了惯性测量单元IMU身上。MPU6050作为一款集成了三轴加速度计和三轴陀螺仪的经典传感器，因其成本低、体积小、接口简单，在学生项目、开源飞控中被广泛采用。

但别忘了，原始数据从传感器出来时是“粗糙”的：加速度计对振动敏感，陀螺仪积分会漂移。直接拿这些数据去控制电机，结果只能是失控。所以关键在于——如何融合这两类数据，得到一个既快速响应又长期稳定的姿态估计，并且还能方便地传到电脑上观察波形、调参数？

本文将带你完整走一遍基于STM32实现MPU6050姿态解算的技术路径。我们不依赖DMP硬件解算（省去授权麻烦），用软件完成互补滤波，并通过国内开发者常用的“匿名上位机”协议把姿态角实时发出去，形成闭环调试能力。所有代码均使用HAL库编写，结构清晰，可直接移植到你的项目中。

MPU6050基础配置与I²C通信

MPU6050本质上是一个通过I²C总线与主控通信的数字传感器模块。它内部包含两个核心部件：
- 加速度计 ：感受重力方向的变化，静态下可用于判断倾斜角度；
- 陀螺仪 ：测量绕各轴旋转的角速度，动态响应快但随时间积分会产生累积误差。

两者各有优劣，必须结合使用。而在使用之前，首先要让STM32能正确“唤醒”并配置它。

MPU6050的标准I²C地址为 0x68 （AD0接地）或 0x69 （AD0接高）。上电后需要进行一系列寄存器写入操作来设置采样率、量程等参数。以下是基于STM32 HAL库的初始化函数：

#include "mpu6050.h"
#include "i2c.h"

MPU6050_t mpu6050;

uint8_t MPU6050_Init(void) {
    uint8_t result;

    // 复位设备
    HAL_I2C_Mem_Write(&hi2c1, MPU6050_ADDR << 1, MPU6050_PWR_MGMT_1, 1, 0x80, 1, 100);
    HAL_Delay(100);

    // 唤醒（取消睡眠模式）
    HAL_I2C_Mem_Write(&hi2c1, MPU6050_ADDR << 1, MPU6050_PWR_MGMT_1, 1, 0x00, 1, 100);

    // 设置采样分频：1kHz采样率，50Hz低通滤波
    HAL_I2C_Mem_Write(&hi2c1, MPU6050_ADDR << 1, MPU6050_SMPLRT_DIV, 1, 0x09, 1, 100); 
    HAL_I2C_Mem_Write(&hi2c1, MPU6050_ADDR << 1, MPU6050_CONFIG, 1, 0x06, 1, 100);

    // 设置陀螺仪满量程 ±2000°/s
    HAL_I2C_Mem_Write(&hi2c1, MPU6050_ADDR << 1, MPU6050_GYRO_CONFIG, 1, 0x18, 1, 100);

    // 设置加速度计量程 ±2g
    HAL_I2C_Mem_Write(&hi2c1, MPU6050_ADDR << 1, MPU6050_ACCEL_CONFIG, 1, 0x00, 1, 100);

    // 验证设备ID
    result = HAL_I2C_Mem_Read(&hi2c1, MPU6050_ADDR << 1, MPU6050_WHO_AM_I, 1, &mpu6050.ID, 1, 100);

    return (mpu6050.ID == MPU6050_DEFAULT_ID) ? 0 : 1;
}

这段代码完成了几个关键动作：
- 先复位芯片；
- 关闭睡眠模式使其进入工作状态；
- 设定采样率为1kHz（通过SMPLRT_DIV=9，即1kHz/(9+1)=100Hz用于后续处理）；
- 启用低通滤波减少高频噪声；
- 指定陀螺仪和加速度计的工作范围。

最后通过读取 WHO_AM_I 寄存器确认是否成功连接。如果返回值为 0x68 ，说明通信正常。

实际部署时建议加上超时重试机制，避免因I²C总线异常导致初始化失败。

数据采集与单位转换

初始化完成后，就可以开始周期性读取传感器数据了。MPU6050的数据输出格式为16位补码，存储在连续的寄存器中。我们可以一次性读取14字节（包括XYZ加速度、温度、XYZ角速度），然后拆分成各个变量。

uint8_t MPU6050_Read_Accel_Gyro(void) {
    uint8_t data[14];

    if (HAL_I2C_Mem_Read(&hi2c1, MPU6050_ADDR << 1, MPU6050_ACCEL_XOUT_H, 1, data, 14, 100) != HAL_OK) {
        return 1;
    }

    mpu6050.Acc.X = (int16_t)(data[0] << 8 | data[1]);
    mpu6050.Acc.Y = (int16_t)(data[2] << 8 | data[3]);
    mpu6050.Acc.Z = (int16_t)(data[4] << 8 | data[5]);
    mpu6050.Temp = (int16_t)(data[6] << 8 | data[7]);
    mpu6050.Gyro.X = (int16_t)(data[8] << 8 | data[9]);
    mpu6050.Gyro.Y = (int16_t)(data[10] << 8 | data[11]);
    mpu6050.Gyro.Z = (int16_t)(data[12] << 8 | data[13]);

    // 转换为物理单位
    mpu6050.AccReal.X = mpu6050.Acc.X / 16384.0f;  // ±2g -> 16384 LSB/g
    mpu6050.AccReal.Y = mpu6050.Acc.Y / 16384.0f;
    mpu6050.AccReal.Z = mpu6050.Acc.Z / 16384.0f;

    mpu6050.GyroReal.X = mpu6050.Gyro.X / 16.4f;   // ±2000°/s -> 16.4 LSB/(°/s)
    mpu6050.GyroReal.Y = mpu6050.Gyro.Y / 16.4f;
    mpu6050.GyroReal.Z = mpu6050.Gyro.Z / 16.4f;

    return 0;
}

注意这里的转换系数要根据实际设定的量程调整：
- 加速度计±2g对应16384 LSB/g；
- 陀螺仪±2000°/s对应约16.4 LSB每度每秒。

很多初学者容易忽略这一点，导致角度计算完全错误。另外，强烈建议在系统启动阶段进行 静止零偏校准 ：让无人机平放不动3~5秒，记录此时陀螺仪输出的平均值作为偏移量，在后续计算中予以扣除，否则积分漂移会非常严重。

互补滤波实现姿态融合

现在我们有了经过标定的加速度和角速度数据，接下来就是最关键的一步： 姿态解算 。

理论上，可以通过陀螺仪积分得到角度变化，但由于存在零偏和噪声，长时间运行会导致显著漂移；而加速度计虽然在静态时能提供可靠的倾角参考，但在飞行震动或加速状态下会被干扰。因此，我们需要一种方法来“扬长避短”。

互补滤波是一种轻量级的解决方案，特别适合资源有限的MCU平台。其思想很简单：

“我相信陀螺仪的短期动态表现，但也相信加速度计的长期静态准确性。”

数学表达式如下：

Angle = α * (Previous_Angle + Gyro_rate * dt) + (1 - α) * Acc_angle

其中α通常取0.95~0.98之间，代表对陀螺仪的信任程度。dt是采样间隔。

下面是具体实现：

#define ALPHA 0.96f
#define DT    0.01f  // 10ms采样周期

float Pitch = 0.0f, Roll = 0.0f, Yaw = 0.0f;

void ComplementaryFilter(float ax, float ay, float az, float gx, float gy, float gz, float dt) {
    float pitch_acc = atan2(-ax, sqrt(ay*ay + az*az)) * RAD_TO_DEG;
    float roll_acc  = atan2( ay,  az) * RAD_TO_DEG;

    // 陀螺仪积分
    Pitch += gx * dt;
    Roll  += gy * dt;
    Yaw   += gz * dt;  // 注意：纯IMU无法修正Yaw漂移！

    // 滤波融合
    Pitch = ALPHA * Pitch + (1 - ALPHA) * pitch_acc;
    Roll  = ALPHA * Roll  + (1 - ALPHA) * roll_acc;
}

几点说明：
- atan2 函数用来计算反正切，避免除零风险；
- 加速度计只能估算Pitch和Roll，无法提供Yaw的绝对参考（因为水平面没有固定参照）；
- Yaw角只能靠陀螺仪积分维持，长期使用会有明显漂移，未来应引入磁力计或GPS辅助修正。

该函数建议放在定时器中断或主循环中以固定频率调用（如100Hz），确保dt准确。若系统负载较高，可用 HAL_GetTick() 差值估算真实时间间隔。

匿名上位机通信：让数据“看得见”

再好的算法如果没有可视化手段也难以调试。在国内嵌入式社区中，“匿名四轴上位机”是一款广受欢迎的开源调试工具，支持实时波形显示、姿态球动画、参数调节等功能。它的优势在于协议简单、免驱动、兼容性强。

其数据帧格式如下：

字段	内容
帧头	0xAA, 0xAA
功能码	0x02（状态信息）
数据长度	N（后续字节数）
数据体	实际数据（N字节）
校验和	所有数据异或校验

下面是如何封装一包包含三个姿态角的数据：

void ANO_DT_Send_Status(float pitch, float roll, float yaw) {
    uint8_t buf[22];
    int32_t temp;

    buf[0] = 0xAA;
    buf[1] = 0xAA;
    buf[2] = 0x02;
    buf[3] = 16;  // 数据长度：3个float（×2字节/float扩大100倍）+ 10字节备用

    temp = pitch * 100;
    buf[4] = temp & 0xFF;
    buf[5] = (temp >> 8) & 0xFF;

    temp = roll * 100;
    buf[6] = temp & 0xFF;
    buf[7] = (temp >> 8) & 0xFF;

    temp = yaw * 100;
    buf[8] = temp & 0xFF;
    buf[9] = (temp >> 8) & 0xFF;

    // 备用字段清零
    for (int i = 10; i < 18; i++) {
        buf[i] = 0;
    }

    // 校验和（从第2字节开始异或至倒数第二）
    uint8_t sum = 0;
    for (int i = 2; i < 18; i++) {
        sum ^= buf[i];
    }
    buf[18] = sum;

    HAL_UART_Transmit(&huart1, buf, 19, 100);
}

发送频率建议控制在20~50Hz之间，既能保证画面流畅，又不至于压垮串口带宽。波特率推荐115200bps。

连接后打开匿名上位机，选择对应COM口，即可看到实时更新的姿态曲线。你可以直观地观察滤波效果、判断是否存在抖动或漂移，甚至可以一边飞行一边调整α系数看响应变化。