简介围绕MPU6050六轴传感器与STM32F103微控制器的IMU姿态解算实验资源面向嵌入式初学者及无人机、机器人、运动控制等方向开发者旨在帮助解决三维姿态角获取、传感器数据融合以及底层驱动编写等问题。资源共117个文件压缩包394KB包含完整的Keil工程与源码57个头文件、51个C源文件覆盖STM32标准外设库、I2C通信及数据处理模块2个hex可直接烧录运行uvprojx/uvoptx为工程配置另有源文件列表及批处理脚本方便工程管理。实验重点演示互补滤波与卡尔曼滤波两种算法代码中可查看陀螺仪与加速度计的原始读取、滤波融合、欧拉角输出等完整流程也能在串口终端观察俯仰、偏航、滚转角的实时变化适合对照学习姿态解算原理和嵌入式驱动开发。文件组织结构清晰.c/.h按外设驱动、系统配置、用户算法分层便于快速定位I2C初始化、数据读取和滤波实现等关键模块。已有4155人学习对需要快速搭建IMU实验平台、完成课程设计或移植参考代码的读者尤为实用。1. 实验16IMU姿态解算STM32F103与MPU6050的组合为什么值得做STM32F103平台上的IMU姿态解算看起来是课程实验实际调试时最容易被同一个问题卡住陀螺仪积分出来的yaw角静置也会慢慢漂加速度计解算出的roll和pitch在动态下噪声又大。姿态解算的本质是把陀螺仪的角速度积分、加速度计的重力方向参考以及磁力计如果有的话的绝对航向参考融合在一起。很多工程师在实验16这样的题目里会被“只要读MPU6050然后算角度”这种简化思路带偏最后做出来的姿态在静态下能看一运动就发散。这篇文章直接按我平时调通一套IMU姿态解算的路径来写从四元数补基础到STM32F103上读MPU6050原始数据再到互补滤波落地最后把yaw慢漂的处理方法讲清楚。适合正在用标准库或HAL库调MPU6050并且对姿态结果有进一步要求的读者。2. IMU姿态解算的数学基础陀螺仪积分、欧拉角与四元数为什么选四元数2.1 陀螺仪输出的是角速度姿态是积分得到的陀螺仪的核心输出是绕自身三个轴的角速度常用单位是deg/s或rad/s。MPU6050的陀螺仪原始值是16位有符号整数量程配置成±2000deg/s时灵敏度为16.4LSB/(deg/s)。姿态更新最简单的想法是对角速度积分当前角度等于上一时刻角度加上角速度乘以积分步长。比如只绕z轴旋转可以用yaw gz_radps * dt累积航向角。这个一轴积分在短时间内有效但陀螺仪的零偏会使积分结果随时间线性增长。更重要的是真实运动是三维的三个轴分别积分会丢失旋转轴耦合关系做出来的姿态在大角度运动时会明显变形。所以我建议一开始就不要走欧拉角积分路线直接使用四元数。四元数用四个参数表示三维旋转没有方向锁问题更新公式是线性运算非常适合STM32F103这种没有FPU的M3内核。使用标准数学库的浮点运算频率200Hz时占用CPU时间不长基本不影响其他任务。2.2 欧拉角的万向锁问题与四元数的几何意义欧拉角用roll、pitch、yaw三个分量描述姿态直观且容易调试。但pitch角接近±90°时roll和yaw的旋转轴重合系统失去一个自由度这就是万向锁现象。STM32F103的运算能力不算强欧拉角更新时还需要频繁计算三角函数实时性和稳定性都不理想。四元数则不同它把三维旋转编码成四维单位向量更新时只做乘法和加法归一化后即可保证旋转的稳定性。四元数q w xi yj zk的模长恒为1时代表纯旋转。q0为标量部分q1/q2/q3为虚部。绕某个轴旋转半角四元数就能表示该旋转。将三个轴上的角速度转换成四元数增量需要计算q_dot 0.5 * q ⊗ ω其中ω (0, gx, gy, gz)。离散化后就是一段非常紧凑的迭代公式。2.3 从角速度到四元数的微分方程以及加速度计提供的参考四元数离散更新使用一阶龙格库塔法公式为# 用Python先验证四元数更新逻辑后续照搬到STM32F103 import numpy as np def quaternion_update(q, gyro_radps, dt): w, x, y, z q gx, gy, gz gyro_radps # 四元数乘法q ⊗ (0, gx, gy, gz) qw -x*gx - y*gy - z*gz qx w*gx y*gz - z*gy qy w*gy z*gx - x*gz qz w*gz x*gy - y*gx # 一阶龙格库塔更新 q_new q 0.5 * dt * np.array([qw, qx, qy, qz]) # 归一化防止数值误差累积 return q_new / np.linalg.norm(q_new) # 静止但有常数零偏场景z轴角速度0.01 rad/sdt10ms q np.array([1.0, 0, 0, 0]) for _ in range(2000): q quaternion_update(q, [0, 0, 0.01], 0.01) yaw np.arctan2(2*(q[0]*q[3] q[1]*q[2]), 1 - 2*(q[2]*q[2] q[3]*q[3])) * 180 / np.pi print(f20秒后yaw: {yaw:.2f} deg)这段代码有两个关键参数gyro_radps必须从原始值除以灵敏度并换算成rad/sdt是积分步长单位秒必须与STM32F103上的中断周期一致。归一化如果只做一次长时间运行后四元数模长会偏离1导致姿态缩放所以每次更新都必须做。加速度计的作用不是直接积分得到姿态而是提供重力矢量在机体坐标系下的投影。静态时三轴加速度计模值接近1g通过投影方向可以解出roll和pitch也可以用来补偿陀螺仪的累积误差。下表是常见数据源在姿态解算中的互补特性数据源输出频域特性能修正的姿态陀螺仪角速度高频准确低频漂移所有轴但yaw无绝对参考加速度计线性加速度低频准确高频噪声大roll、pitch磁力计磁场强度易受软硬磁干扰yaw在MPU6050六轴方案中通常只用加速度计修正roll和pitch。yaw缺少一个不随时间漂移的绝对参考这是之后所有yaw慢漂问题的根源。3. STM32F103上读取MPU6050最小系统接线、I2C时序与原始数据预处理3.1 硬件接线与关键寄存器配置STM32F103最小系统直接驱动MPU6050最常见的是软件模拟I2C。我选择软件模拟的原因很实际F103硬件I2C在噪声环境中容易出现busy状态卡死排查起来比软件模拟麻烦。接线建议使用PB8作为SCLPB9作为SDAAD0引脚接地对应I2C从机地址0x68。MPU6050的VLOGIC引脚也要接3.3V否则内部电平转换异常数据总是0xFF。初始化时首先要按寄存器顺序配置电源和量程。下面这张表是每次调试都要核对的关键寄存器寄存器配置错误会导致姿态解算结果异常但不容易察觉。寄存器名称地址配置值作用PWR_MGMT_10x6B0x00唤醒传感器选择内部8MHz振荡器SMPLRT_DIV0x190x04分频器采样率1kHz/(14)200HzCONFIG0x1A0x03数字低通滤波器约44Hz截止GYRO_CONFIG0x1B0x18陀螺仪量程±2000deg/sACCEL_CONFIG0x1C0x10加速度计量程±8gSMPLRT_DIV设置成0x04配合内部1kHz采样率实际数据更新率是200Hz。这个值需要与后面姿态解算的dt一致。量程寄存器特别容易漏配比如默认陀螺仪量程是±250deg/s如果按照16.4的灵敏度换算姿态会以接近8倍的比例偏差偏转。3.2 I2C读取六轴数据的代码与寄存器表MPU6050的加速度计和陀螺仪数据分别从0x3B和0x43开始连续读取14字节包含加速度计三轴、温度、陀螺仪三轴。读取前要设置起始寄存器地址。以下是标准库下的读取代码底层I2C时序函数只需要实现Start、Stop、SendByte和ReadByte即可。// STM32F103 软件模拟I2C读取MPU6050 // PB8-SCL, PB9-SDA, 从机地址0x68 #define MPU6050_ADDR_W 0xD0 // 0x68左移1位写方向 #define MPU6050_ADDR_R 0xD1 // 0x68左移1位读方向 void MPU6050_Read_Gyro_Accel(int16_t *accel, int16_t *gyro) { uint8_t buf[14]; // 指定内部寄存器起始地址为0x3B I2C_Start(); I2C_SendByte(MPU6050_ADDR_W); I2C_SendByte(0x3B); I2C_Start(); // 重复起始 // 连续读取14字节 I2C_SendByte(MPU6050_ADDR_R); for (int i 0; i 14; i) { if (i 13) buf[i] I2C_ReadByte(1); // 非最后字节发送ACK else buf[i] I2C_ReadByte(0); // 最后字节发送NACK } I2C_Stop(); // 高字节在前 accel[0] (int16_t)((buf[0] 8) | buf[1]); // AX accel[1] (int16_t)((buf[2] 8) | buf[3]); // AY accel[2] (int16_t)((buf[4] 8) | buf[5]); // AZ gyro[0] (int16_t)((buf[8] 8) | buf[9]); // GX gyro[1] (int16_t)((buf[10] 8) | buf[11]); // GY gyro[2] (int16_t)((buf[12] 8) | buf[13]); // GZ }代码中的MPU6050_ADDR_W和MPU6050_ADDR_R已经包含读写标志位。I2C读多字节时每收到一个字节发ACK告诉从机继续发送最后一个字节必须发NACK从机才能释放总线。accel和gyro数组里的数值是原始LSB不是物理量后续必须做灵敏度换算。3.3 原始数据转物理量并做零偏估计原始数据转物理量的公式为物理值 原始LSB / 灵敏度。例如陀螺仪量程±2000deg/s时灵敏度为16.4那么原始值164对应10deg/s再乘以π/180就是rad/s。转换后陀螺仪单位用rad/s加速度单位用g方便后续四元数归一化和Mahony滤波。转换代码通常放在读取之后姿态解算之前// 量程±2000deg/s灵敏度16.4 // 量程±8g灵敏度4096 float gyro_gain 16.4f; float accel_gain 4096.0f; float gx gyro_raw[0] / gyro_gain * PI / 180.0f; float gy gyro_raw[1] / gyro_gain * PI / 180.0f; float gz gyro_raw[2] / gyro_gain * PI / 180.0f; float ax accel_raw[0] / accel_gain; float ay accel_raw[1] / accel_gain; float az accel_raw[2] / accel_gain;零偏估计在系统启动时做。让开发板水平静止连续读取100次陀螺仪原始值求平均再将平均值转成rad/s之后每次读取都减掉这个零偏值。这一步能去掉大部分yaw慢漂来源。需要说明的是加速度计也有零偏但重力矢量的模值可以通过sqrt(ax^2ay^2az^2)检查如果与1g偏差超过5%要先检查水平安装面和传感器是否损坏。4. 姿态解算实现互补滤波和四元数更新在STM32F103上的落地4.1 互补滤波为什么能同时抑制陀螺漂移和加计噪声陀螺仪短时间积分结果平滑但长期有漂移加速度计静态结果准但运动时携带大量振动噪声。互补滤波的思路是将两路信号按频率进行加权组合对陀螺仪使用高通滤波对加速度计使用低通滤波再相加。实现上通常不显式设计滤波器而是用PI控制器把加速度计与陀螺仪积分的误差反馈到陀螺仪角速度上。这就是Mahony互补滤波比梯度下降法更简洁参数也更容易调。互补滤波作用在四元数上时先计算由当前四元数推算出的重力向量再与加速度计实测重力向量做叉积。叉积结果代表两个向量之间的夹角误差。该误差通过比例项修正瞬时角速度通过积分项修正陀螺仪的常值零偏。这样一来roll和pitch被牢牢绑在重力加速度方向上动态时依然由陀螺仪主导不会因为加速度计噪声而剧烈跳动。4.2 核心代码Mahony滤波在STM32F103上的实现下面的代码是六轴姿态解算的完整核心不依赖MPU6050的DMP全部在MCU上运行。调用频率需要严格固定为200Hz也就是前面SMPLRT_DIV配置对应的采样率。// Mahony互补滤波Kp和Ki为关键参数 // 输入加速度计除以g后的分量陀螺仪rad/s分量 #define Kp 25.0f // 比例增益 #define Ki 3.0f // 积分增益 #define dt 0.005f // 采样周期200Hz static float q0 1.0f, q1 0.0f, q2 0.0f, q3 0.0f; static float integralFBx 0.0f, integralFBy 0.0f, integralFBz 0.0f; void Mahony_Update(float ax, float ay, float az, float gx, float gy, float gz) { float norm; float vx, vy, vz; float ex, ey, ez; // 归一化加速度计 norm sqrtf(ax*ax ay*ay az*az); if (norm 0.0001f) return; ax / norm; ay / norm; az / norm; // 从当前四元数推算重力参考方向 vx 2.0f*(q1*q3 - q0*q2); vy 2.0f*(q0*q1 q2*q3); vz q0*q0 - q1*q1 - q2*q2 q3*q3; // 加速度计与参考方向做叉积得到误差 ex ay*vz - az*vy; ey az*vx - ax*vz; ez ax*vy - ay*vx; // 积分误差用于消除陀螺零偏 integralFBx ex * Ki * dt; integralFBy ey * Ki * dt; integralFBz ez * Ki * dt; // 修正陀螺仪角速度 gx Kp*ex integralFBx; gy Kp*ey integralFBy; gz Kp*ez integralFBz; // 一阶龙格库塔法更新四元数 q0 (-q1*gx - q2*gy - q3*gz) * dt * 0.5f; q1 ( q0*gx q2*gz - q3*gy) * dt * 0.5f; q2 ( q0*gy - q1*gz q3*gx) * dt * 0.5f; q3 ( q0*gz q1*gy - q2*gx) * dt * 0.5f; // 四元数归一化 norm sqrtf(q0*q0 q1*q1 q2*q2 q3*q3); q0 / norm; q1 / norm; q2 / norm; q3 / norm; } void Quaternion_To_Euler(float *roll, float *pitch, float *yaw) { *roll atan2f(2.0f*(q0*q1 q2*q3), 1.0f - 2.0f*(q1*q1 q2*q2)) * 57.29578f; *pitch asinf(2.0f*(q0*q2 - q1*q3)) * 57.29578f; *yaw atan2f(2.0f*(q0*q3 q1*q2), 1.0f - 2.0f*(q2*q2 q3*q3)) * 57.29578f; }关键参数的影响如下表所示调试时可以按顺序调整。参数典型范围作用调整经验Kp2040决定加速度计纠正姿态的强度调大后静态恢复快但动态噪声变大过小则姿态慢漂Ki26决定对陀螺零偏的积分消除速度过大会把加速度计噪声积分进去导致晃动后回不到零dt0.005与中断频率严格一致不匹配会直接让姿态在静止时持续旋转采样频率100500Hz越高越能还原快速运动超过系统负载时会出现周期丢失表现和dt错误一样代码里gx、gy、gz是经过零偏补偿后的rad/s值ax、ay、az是除以g后的加速度值。Mahony滤波不保证yaw不发散因为叉积误差方程里没有yaw方向的绝对参考。如果系统放在FreeRTOS中运行Mahony_Update应该放在固定频率的定时器任务里而不是放在while主循环中。主循环中的系统调度会导致dt抖动yaw慢漂会变得更加不规律。4.3 验证姿态解算结果并观察yaw慢漂在STM32F103上写完代码后将欧拉角通过USART1输出格式为roll:%.2f pitch:%.2f yaw:%.2f用串口助手以200Hz保存数据。将开发板水平放置roll和pitch应该在0度附近小幅度波动波动范围一般在±1度以内。yaw会随着时间缓慢变化。一个设计良好的零偏校准程序可以把yaw慢漂控制在每分钟12度以内。如果没有做零偏校准这个值可能达到每分钟几十度那就是纯粹的陀螺零偏累积不是算法问题。5. 进阶手工零偏校准与四元数正交化验证把yaw慢漂压下去5.1 静态零偏校准的正确步骤零偏校准不能只靠几次读取的瞬时值。我会在main函数初始化后延时等待传感器稳定然后连续采集2000个陀螺仪原始样本丢弃前50个用剩余样本做平均值作为零偏。这个平均值除以灵敏度再转成rad/s就是静态零偏。动态使用中如果发现往同一个方向慢漂可以适当增大Ki但不要指望Ki完全消除静态零偏因为它本质上是把加速度计的误差积分对yaw无效。所以最好的办法是校准完零偏之后再进主循环。5.2 四元数正交化验证技巧一个很容易被忽略的细节是四元数模长归一化只能保证长度不变不能保证旋转矩阵严格正交。当积分步长偏大时四元数更新会产生轻微的非正交误差长期运行后roll和pitch之间会互相渗透。验证方法是将四元数转成旋转矩阵然后计算矩阵与自身转置的乘积结果应当接近单位矩阵。如果对角线偏出0.9951.005说明四元数更新步长过大应该减小dt或改用二阶龙格库塔法。5.3 和MPU6050 DMP的取舍MPU6050内部DMP可以直接输出四元数使用起来只有几行配置确实省心。但DMP的yaw依然会漂如果想继续优化又看不到内部状态反而比自研滤波难调。Mahony互补滤波的代码量不到100行Kp和Ki可调出现异常时还能打印中间变量。手写算法换来的是对每个参数确定性的掌控。后续如果引入磁力计或做IMU内参标定也只是在此基础之上增加一个观测维度。把这一套在STM32F103上跑通之后再迁移到其他M4内核平台时只是浮点运算更快算法结构完全不用变。本文还有配套的精品资源点击获取