尧图网页设计 
还记得第一次调平衡车项目我拿着MPU6050的数据愣是看了半天——角度值在那里像个醉汉一样晃陀螺仪的零漂又让角度慢慢躺平下去。那时候网上资料七零八落DMP库一调就崩最后靠着一份残缺的卡尔曼滤波代码硬是啃了下来。现在回头看从陀螺仪原始数据到稳定可用的姿态角这条路其实并不长只是中间有几个坎得迈过去。这篇文章我把这套基于STM32F4和HAL库的MPU6050实战方案完整写出来从底层I2C寄存器读写到陀螺仪/加速度计的校准与换算再到四元数概念梳理和卡尔曼滤波的C语言落地最后给出可以直接抄的完整代码。无论是刚接触MPU6050的新手还是想把DMP替换成自研解算算法的老手这篇文章都能省下你至少一周的调试时间。1. 拿到一颗MPU6050先搞清它到底能给你什么1.1 陀螺仪和加速度计的物理意义MPU6050这颗六轴传感器内部实际上集成了两颗独立的MEMS芯片。一颗是三轴陀螺仪测量的是绕X、Y、Z三个轴的旋转角速度单位是°/s另一颗是三轴加速度计测量的是沿三个轴的线性加速度单位是g9.8m/s²。这两个物理量看起来都很直观但它们各自的短板也特别明显。陀螺仪的角速度积分可以得到角度但只要有零点偏移积分就会不停累积误差——我之前测过一颗没有任何校准的MPU6050静止放置时Z轴陀螺仪读数能到2°/s积分一分钟角度就漂了120°这压根没法直接用。加速度计呢它对重力方向的分量感知非常准静止时可以算出精确的倾斜角但一旦有运动加速度混进来比如平衡车起步、机械臂挥动输出就会剧烈抖动根本没法直接用。所以姿态解算的核心思路就一句话用陀螺仪的短时精度来抵消加速度计的噪声用加速度计的长时稳定性来修正陀螺仪的漂移。这就是数据融合的价值也是卡尔曼滤波和互补滤波在这类项目里大显身手的原因。1.2 关于DMP那点事儿为什么很多人不想用DMPMPU6050自带一个Digital Motion ProcessorDMPInvenSense官方提供了DMP固件库可以直接输出四元数甚至欧拉角很多新手觉得用它最省事。但我必须说工程项目里我基本不用DMP原因有三个。第一DMP固件库封装得太严实你拿到的是一堆编译好的二进制文件加上不完整的头文件出了问题根本没法调试。它内部怎么融合的、融合参数是什么样、有没有针对你的安装方式进行调校全是一团黑。第二DMP库的移植性很差。官方给的库是针对老版MSP430写的底层接口你把它搬到STM32F4上要自己改写I2C读写函数和中断接口光是适配HAL库就得折腾半天典型的花钱费时还不讨好。第三DMP输出的姿态是它的标准答案你没法针对自己的机械结构做优化。比如你的设备安装时传感器歪了5°用自研算法可以直接在初始姿态里做修正DMP就比较难办。所以我强烈建议实战项目里自己写解算代码哪怕一开始只用互补滤波搞定了一两个轴之后再去上卡尔曼滤波。这个过程能让你真正理解姿态解算后续换传感器、换滤波算法都事半功倍。2. 从零开始配置STM32F4的HAL库环境2.1 建立工程CubeMX初始化I2C和串口用HAL库做STM32F4开发第一步就是CubeMX生成工程。这里有一个很关键的细节MPU6050通信协议是I2C而I2C总线的时钟、上拉电阻配置直接决定了后续读数据稳不稳定。CubeMX里你要干的活是这样的RCC选择HSE外部晶振主频配置到168MHzSTM32F407或180MHzSTM32F429具体看芯片型号。I2C1选择PB6/PB7引脚模式I2C速度选400kHz Fast Mode。MPU6050手册说明I2C时钟可以到400kHz实测下来400kHz比100kHz快得多而且只要电路设计得当稳得很。如果调试中发现数据不稳定再降回100kHz。USART2选择PA2/PA3异步模式115200-8-N-1用于把姿态数据发到电脑上看波形。顺便把SYS里的Debug设为Serial Wire免得烧录器占用引脚。CubeMX默认生成的HAL库会把I2C的时序参数都算好一般不需要手动改。但有一个点很多人会漏掉I2C的上拉电阻。MPU6050模块大多数板子上已经贴了2.2k或4.7k的上拉电阻但如果你是自己画的板子一定要在SCL和SDA上加4.7k上拉电阻到VCC。I2C总线协议要求开路输出没有上拉电阻是绝对跑不起来的。2.2 最小系统验证用HAL库读写MPU6050寄存器工程生成之后先别急着写姿态解算第一步先验证I2C能不能和MPU6050正常通信。MPU6050的I2C地址是7位地址0x68AD0引脚接地或0x69AD0引脚接高。多数模块默认接地也就是0x688位写地址就是0xD0读地址是0xD1。最稳妥的验证方式是读WHO_AM_I寄存器地址是0x75。正常返回值应该是0x68。对应的HAL库代码就三行uint8_t who_am_i 0; HAL_I2C_Mem_Read(hi2c1, 0x68 1, 0x75, I2C_MEMADD_SIZE_8BIT, who_am_i, 1, 100); // 如果 who_am_i 0x68说明I2C通信正常这里有一个新手特别容易翻车的地方HAL_I2C_Mem_Read的第三个参数是设备地址形参里要求的是8位地址也就是0x68左移一位变成0xD0而很多人直接填0x68结果读出来全是0xFF。如果WHO_AM_I读出来是0x68恭喜你芯片活着。接下来要做的是唤醒芯片。MPU6050上电后默认处于睡眠模式寄存器PWR_MGMT_1地址0x6B的bit6是SLEEP位置1就是睡眠。我们把它写成0x00使用内部8MHz振荡器并清除睡眠位uint8_t pwr_mgmt 0x00; HAL_I2C_Mem_Write(hi2c1, 0x68 1, 0x6B, I2C_MEMADD_SIZE_8BIT, pwr_mgmt, 1, 100);然后配置陀螺仪量程、加速度计量程和数字低通滤波器寄存器SMPLRT_DIV0x19设置采样率分频0x07表示采样率 1kHz / (1 7) 125Hz这个值对控制类应用足够了。要求更高响应速度可以设0x03得到250Hz。寄存器CONFIG0x1A的bit2:0设置数字低通滤波器带宽。推荐设置0x03截止频率约44Hz可以滤掉大部分振动噪声又不至于让角度响应太迟钝。注意这个低通滤波器对陀螺仪和加速度计同时生效。寄存器GYRO_CONFIG0x1B和ACCEL_CONFIG0x1C设置量程。我一般用±2000dps和±8g的组合原因后面细说。配完这些就可以读取六轴原始数据了。加速度计的每个轴16位数据在寄存器0x3B到0x40陀螺仪在0x43到0x48。每个轴拆成高字节和低字节两个8位寄存器读出来拼一下就完事。uint8_t data[14]; HAL_I2C_Mem_Read(hi2c1, 0x68 1, 0x3B, I2C_MEMADD_SIZE_8BIT, data, 14, 100); int16_t accel_x (data[0] 8) | data[1]; int16_t accel_y (data[2] 8) | data[3]; int16_t accel_z (data[4] 8) | data[5]; int16_t temp (data[6] 8) | data[7]; int16_t gyro_x (data[8] 8) | data[9]; int16_t gyro_y (data[10] 8) | data[11]; int16_t gyro_z (data[12] 8) | data[13];到这里你已经有最原始的传感器数据了。3. 原始数据的读取、单位换算和校准3.1 为什么把量程设置为±2000dps和±8g很多教程喜欢把量程设为±250dps理由是分辨率最高但实际应用里这个设置很容易踩坑。量程和灵敏度的关系是这样的MPU6050陀螺仪每个量程对应不同的刻度因子FS_SEL具体来说量程dps刻度因子LSB/°/s±250131±50065.5±100032.8±200016.4数值越大刻度因子越小意味着每个LSB代表的角速度越大原始值在相同角速度下的数值就越小。也就是说量程越大分辨率越低但能测的上限越大。我选择±2000dps的原因是平衡车、机械臂、无人机这类带电机振动的场景瞬间角速度经常超过500°/s甚至1000°/s。如果量程选低了陀螺仪会直接饱和输出卡在最大值这时候积分出来的角度就是灾难。选±2000dps虽然分辨率低一些但配合16位ADC最小分辨率是2000/327680.061°/s/LSB这个精度对姿态控制来说已经绰绰有余了。加速度计量程选±8g同理。MPU6050的加速度计在静止时理论上只受重力1g但电机启动瞬间的冲击加速度能到几个g量程选小了同样会饱和。±8g对应的刻度因子是4096 LSB/g静止时重力分量产生的原始值大约在4000左右能有足够的分辨率来分析姿态。3.2 一杯水法陀螺仪的静态零漂校准陀螺仪的零漂是姿态解算里最恶心的东西。每一颗MPU6050的零漂都不一样而且会随温度变化。所以代码里不能写死一个偏移量必须上电后动态校准。我的做法是上电静止校准也叫一杯水法——把设备平放在桌面上静止不动连续采样200次陀螺仪数据取平均作为零漂偏移量。因为静止时真实角速度是0平均值就是零漂。#define GYRO_CALI_SAMPLES 200 float gyro_offset[3] {0, 0, 0}; void MPU6050_GyroCalibration(void) { int32_t sum[3] {0, 0, 0}; int16_t raw[3]; for (int i 0; i GYRO_CALI_SAMPLES; i) { MPU6050_ReadGyro(raw); sum[0] raw[0]; sum[1] raw[1]; sum[2] raw[2]; HAL_Delay(2); } for (int i 0; i 3; i) { gyro_offset[i] (float)sum[i] / GYRO_CALI_SAMPLES; } }这里有个细节容易被忽略I2C读取MPU6050数据寄存器如果你连续两次读取的间隔太短可能读到的是同一组数据。MPU6050的数据寄存器是异步更新的读的时候不会锁存所以如果你的循环读得太快可能连续读到一模一样的值。解决方法是读取间隔大于传感器的输出速率比如采样率是125Hz读取间隔至少要8ms。校准采200个样每个间隔2ms看起来是400ms但读取频率可能已经超过传感器更新率了。稳妥起见我建议每次读取之间延时5ms以上。采集完的偏移量怎么存最省事的办法是直接存放在全局变量里每次解算前用原始读数减去偏移量即可gyro_x_calibrated (float)raw_gyro_x - gyro_offset[0];这种校准方式能消除绝大部分零漂但温度变化仍然会导致剩余漂移。追求极致的项目可以做温度补偿但对于绝大多数实战场景上电校准一次足够了配合卡尔曼滤波的修正能力角度能稳定保持很久。3.3 从原始值到物理单位的换算读到的16位原始值要换算成物理单位才能用于解算。换算公式非常简单陀螺仪角速度(°/s) 原始值 / 刻度因子。±2000dps对应刻度因子16.4即gyro_dps raw_gyro / 16.4。加速度(m/s²) 原始值 / 4096 * 9.8。有些简化算法直接以g为单位那就不乘9.8直接raw_accel / 4096重力加速度在Z轴上的投影应该在±1g范围内。在正式解算之前还要做一步归一化处理。因为安装偏差、量程误差等原因三个轴的比例因子不会完全一致最直观的验证方法是用加速度计的模长来判断静止时sqrt(ax² ay² az²)应该等于1g归一化后。如果偏差大于2%建议做一次六面校准分别让六个面朝上放置记录数据并求解三个轴的比例因子和零偏。不过对大多数应用来说归一化误差控制在1~2%以内直接用就够了。4. 姿态解算欧拉角、旋转矩阵和四元数先把概念捋清楚4.1 为什么不能直接把数据凑在一起输出很多新手拿到加速度计和陀螺仪的数据第一反应是把加速度计算出来的角度和陀螺仪积分算出来的角度直接平均。这样做出来的角度又慢又飘完全不能用。根本原因在于加速度计和陀螺仪的频域特性完全相反。加速度计输出的角度在静态时特别准但一有动态加速度就剧烈跳动——高频噪声大陀螺仪积分的角度短时特别平稳但长时间会漂走——低频误差大。数据融合的本质就是设计一个滤波器低频段信任加速度计高频段信任陀螺仪交叉频率附近的过渡要平滑。在了解融合算法之前得先把姿态的数学表达搞清楚。姿态角也叫欧拉角指的是物体相对世界坐标系的旋转角度分别绕X轴、Y轴、Z轴旋转得到横滚角Roll、俯仰角Pitch、偏航角Yaw。物体旋转到任意姿态可以按固定顺序分解成三次旋转比如先绕Z轴转Yaw再绕Y轴转Pitch最后绕X轴转Roll。这里有一个关键点必须强调欧拉角的旋转顺序会影响最终结果同一个姿态按不同顺序分解三个角度是不同的。所以你在代码里看到的Roll和Pitch一定是基于某个约定的旋转顺序推导的。大多数姿态解算算法都固定使用ZYX旋转顺序也就是先偏航、再俯仰、最后横滚。如果哪天花花绿绿的博文里角度对不上多半是旋转顺序定义不一致。4.2 从加速度计估算初始姿态角在使用融合算法之前需要有一个初始的姿态值。最常见的方法是利用加速度计在静止状态下计算Roll和Pitch。在静止状态下加速度计的测量模型是加速度计测到的加速度向量 重力向量在机体坐标系中的投影。也就是说静止时ax、ay、az的合成向量模长等于g方向指向地心。根据这个关系可以解出初始横滚角和俯仰角roll_acc atan2(ay, az) * 180 / PI; pitch_acc atan(-ax / sqrt(ay * ay az * az)) * 180 / PI;这个公式的来源是空间几何横滚角是Y轴与水平面的夹角由Y轴加速度和Z轴加速度的比例决定俯仰角是X轴加速度与Y、Z合成加速度的比例决定。注意这里算出来的角度在设备有线性加速度时是不准的所以只用来初始化或者用低权重参与融合。另外偏航角Yaw用加速度计是算不出来的因为重力向量不携带绕重力轴旋转的信息。要获得Yaw必须依赖磁力计或视觉里程计。很多平衡车项目只用Roll和Pitch就能跑这种场景下单靠MPU6050就完全够用。4.3 互补滤波先把最朴素的融合搞明白卡尔曼滤波出场之前我必须先讲讲互补滤波。因为互补滤波的代码极短逻辑极清晰先理解它再理解卡尔曼会有一种哦原来是这样的通透感。互补滤波公式长这样angle 0.98 * (angle gyro_rate * dt) 0.02 * acc_angle;就这么一行。它做的事情是大部分信任陀螺仪积分的角度同时用加速度计的角度做一个缓慢的修正。比例为0.98和0.02意味着陀螺仪占主导、加速度计做微调。这个比例对应的时间常数是tau dt * (1 - alpha) / alpha; // 当 dt0.01s, alpha0.98 时tau 0.01 * 0.02 / 0.98 ≈ 0.2s时间常数的物理含义是加速度计的修正作用经过这个时间后能把陀螺仪的积分误差校正掉63%。如果你觉得角度恢复太慢就减小alpha如果觉得角度噪声太大就增大alpha。这个滤波器就一个参数调起来特别顺手。但互补滤波有明显的天花板它假设传感器噪声是固定的、不随时间变化的。实际上MPU6050在运动过程中的振动强度一直在变静态和动态的最优alpha值并不一样。如果需要更好的效果就该上卡尔曼滤波了。4.4 卡尔曼滤波从原理到一维角度模型卡尔曼滤波的核心思想是把系统的状态估计拆成预测和更新两步。预测阶段用系统模型陀螺仪积分推算状态更新阶段用测量值加速度计角度修正预测结果。关键区别于互补滤波的地方在于卡尔曼滤波能动态调整该信任预测还是信任测量这个信任程度用协方差矩阵来量化。对于姿态解算这种场景我们不需要完整的3D姿态估计只对单个轴的角度做卡尔曼滤波就足够了。比如要估计横滚角Roll状态向量定义为x [angle, bias]其中angle是估计的角度bias是陀螺仪的漂移误差。系统的状态方程是angle_new angle (gyro_rate - bias) * dt 噪声 bias_new bias 噪声这个模型的意思是角度由陀螺仪积分得出但要去掉估计出的零漂零漂bias本身的变化非常缓慢当作随机游走处理。对应的两个关键噪声参数是过程噪声协方差Q代表你对状态模型的信任程度。Q越小越相信陀螺仪积分Q越大越允许状态快速变化。一般取0.001到0.01。测量噪声协方差R代表你对加速度计角度测量的信任程度。R越小越相信加速度计R越大越怀疑加速度计。一般取0.01到1。代码实现上我直接用标准的5个卡尔曼方程按一维情况简化编写。完整代码会在下一节给出。5. HAL库卡尔曼滤波代码实战5.1 代码结构完整代码直接抄下面这套代码我拆成四个文件mpu6050.h、mpu6050.c、kalman.h、kalman.c。默认你已经用CubeMX初始化了I2C1和USART2工程能编译通过。首先是mpu6050.h头文件#ifndef __MPU6050_H #define __MPU6050_H #include stm32f4xx_hal.h #define MPU6050_ADDR 0x68 #define MPU6050_ADDR_W ((MPU6050_ADDR 1) 0xFE) #define MPU6050_ADDR_R ((MPU6050_ADDR 1) | 0x01) #define MPU6050_REG_WHO_AM_I 0x75 #define MPU6050_REG_PWR_MGMT_1 0x6B #define MPU6050_REG_SMPLRT_DIV 0x19 #define MPU6050_REG_CONFIG 0x1A #define MPU6050_REG_GYRO_CONFIG 0x1B #define MPU6050_REG_ACCEL_CONFIG 0x1C #define MPU6050_REG_ACCEL_XOUT 0x3B #define MPU6050_REG_GYRO_XOUT 0x43 typedef struct { int16_t Accel_X_RAW; int16_t Accel_Y_RAW; int16_t Accel_Z_RAW; int16_t Gyro_X_RAW; int16_t Gyro_Y_RAW; int16_t Gyro_Z_RAW; float Ax, Ay, Az; float Gx, Gy, Gz; float Roll, Pitch, Yaw; float gyro_offset[3]; } MPU6050_t; uint8_t MPU6050_Init(I2C_HandleTypeDef *hi2c); uint8_t MPU6050_Read_All(I2C_HandleTypeDef *hi2c, MPU6050_t *mpu); void MPU6050_Calibrate_Gyro(I2C_HandleTypeDef *hi2c, MPU6050_t *mpu); float MPU6050_Get_Roll_Accel(MPU6050_t *mpu); float MPU6050_Get_Pitch_Accel(MPU6050_t *mpu); #endif然后是mpu6050.c#include mpu6050.h #include math.h #define GYRO_SCALE 16.4f // ±2000dps 对应的刻度因子 #define ACCEL_SCALE 4096.0f // ±8g 对应的刻度因子 #define GYRO_CALI_SAMPLES 200 static I2C_HandleTypeDef *mpu_i2c; static uint8_t MPU6050_WriteReg(uint8_t reg, uint8_t data) { return HAL_I2C_Mem_Write(mpu_i2c, MPU6050_ADDR_W, reg, I2C_MEMADD_SIZE_8BIT, data, 1, 100); } static uint8_t MPU6050_ReadRegs(uint8_t reg, uint8_t *buf, uint8_t len) { return HAL_I2C_Mem_Read(mpu_i2c, MPU6050_ADDR_R, reg, I2C_MEMADD_SIZE_8BIT, buf, len, 100); } uint8_t MPU6050_Init(I2C_HandleTypeDef *hi2c) { mpu_i2c hi2c; uint8_t who_am_i 0; MPU6050_ReadRegs(MPU6050_REG_WHO_AM_I, who_am_i, 1); if (who_am_i ! 0x68) return 1; MPU6050_WriteReg(MPU6050_REG_PWR_MGMT_1, 0x00); // 唤醒 HAL_Delay(50); MPU6050_WriteReg(MPU6050_REG_SMPLRT_DIV, 0x07); // 125Hz采样率 MPU6050_WriteReg(MPU6050_REG_CONFIG, 0x03); // 低通滤波44Hz MPU6050_WriteReg(MPU6050_REG_GYRO_CONFIG, 0x18); // ±2000dps MPU6050_WriteReg(MPU6050_REG_ACCEL_CONFIG, 0x10); // ±8g return 0; } void MPU6050_Calibrate_Gyro(I2C_HandleTypeDef *hi2c, MPU6050_t *mpu) { int32_t sum[3] {0, 0, 0}; uint8_t data[6]; for (int i 0; i GYRO_CALI_SAMPLES; i) { MPU6050_ReadRegs(MPU6050_REG_GYRO_XOUT, data, 6); sum[0] (int16_t)((data[0] 8) | data[1]); sum[1] (int16_t)((data[2] 8) | data[3]); sum[2] (int16_t)((data[4] 8) | data[5]); HAL_Delay(5); } mpu-gyro_offset[0] (float)sum[0] / GYRO_CALI_SAMPLES; mpu-gyro_offset[1] (float)sum[1] / GYRO_CALI_SAMPLES; mpu-gyro_offset[2] (float)sum[2] / GYRO_CALI_SAMPLES; } uint8_t MPU6050_Read_All(I2C_HandleTypeDef *hi2c, MPU6050_t *mpu) { uint8_t data[14]; if (MPU6050_ReadRegs(MPU6050_REG_ACCEL_XOUT, data, 14) ! HAL_OK) return 1; mpu-Accel_X_RAW (int16_t)((data[0] 8) | data[1]); mpu-Accel_Y_RAW (int16_t)((data[2] 8) | data[3]); mpu-Accel_Z_RAW (int16_t)((data[4] 8) | data[5]); mpu-Gyro_X_RAW (int16_t)((data[8] 8) | data[9]); mpu-Gyro_Y_RAW (int16_t)((data[10] 8) | data[11]); mpu-Gyro_Z_RAW (int16_t)((data[12] 8) | data[13]); mpu-Ax mpu-Accel_X_RAW / ACCEL_SCALE; mpu-Ay mpu-Accel_Y_RAW / ACCEL_SCALE; mpu-Az mpu-Accel_Z_RAW / ACCEL_SCALE; mpu-Gx (mpu-Gyro_X_RAW - mpu-gyro_offset[0]) / GYRO_SCALE; mpu-Gy (mpu-Gyro_Y_RAW - mpu-gyro_offset[1]) / GYRO_SCALE; mpu-Gz (mpu-Gyro_Z_RAW - mpu-gyro_offset[2]) / GYRO_SCALE; return 0; } float MPU6050_Get_Roll_Accel(MPU6050_t *mpu) { return atan2f(mpu-Ay, mpu-Az) * 180.0f / 3.14159265f; } float MPU6050_Get_Pitch_Accel(MPU6050_t *mpu) { return atanf(-mpu-Ax / sqrtf(mpu-Ay * mpu-Ay mpu-Az * mpu-Az)) * 180.0f / 3.14159265f; }这段代码把传感器初始化和数据读取分离得比较干净。有一个细节MPU6050_Calibrate_Gyro是放在Init之后、进入主循环之前调用的调用时设备必须静止。5.2 卡尔曼滤波核心代码接下来是核心的卡尔曼滤波代码。kalman.h定义滤波器结构体#ifndef __KALMAN_H #define __KALMAN_H typedef struct { float Q_angle; // 角度过程噪声协方差 float Q_bias; // 零漂过程噪声协方差 float R_measure; // 测量噪声协方差 float angle; // 当前角度估计值 float bias; // 当前零漂估计值 float P[2][2]; // 误差协方差矩阵 } Kalman_t; void Kalman_Init(Kalman_t *kal); float Kalman_GetAngle(Kalman_t *kal, float new_angle, float new_rate, float dt); #endifkalman.c的完整实现#include kalman.h void Kalman_Init(Kalman_t *kal) { kal-Q_angle 0.001f; kal-Q_bias 0.003f; kal-R_measure 0.03f; kal-angle 0.0f; kal-bias 0.0f; kal-P[0][0] 0.0f; kal-P[0][1] 0.0f; kal-P[1][0] 0.0f; kal-P[1][1] 0.0f; } float Kalman_GetAngle(Kalman_t *kal, float new_angle, float new_rate, float dt) { float rate new_rate - kal-bias; kal-angle dt * rate; kal-P[0][0] dt * (dt * kal-P[1][1] - kal-P[0][1] - kal-P[1][0] kal-Q_angle); kal-P[0][1] - dt * kal-P[1][1]; kal-P[1][0] - dt * kal-P[1][1]; kal-P[1][1] kal-Q_bias * dt; float S kal-P[0][0] kal-R_measure; float K[2]; K[0] kal-P[0][0] / S; K[1] kal-P[1][0] / S; float y new_angle - kal-angle; kal-angle K[0] * y; kal-bias K[1] * y; float P00_temp kal-P[0][0]; float P01_temp kal-P[0][1]; kal-P[0][0] - K[0] * P00_temp; kal-P[0][1] - K[0] * P01_temp; kal-P[1][0] - K[1] * P00_temp; kal-P[1][1] - K[1] * P01_temp; return kal-angle; }这套实现是标准的线性卡尔曼滤波针对角度零漂两状态模型的简化版本。它内部维护一个2x2的协方差矩阵P每次预测时加上过程噪声Q更新时用测量噪声R计算卡尔曼增益K最终输出修正后的角度估计值。这里有个初学者容易迷糊的点P矩阵的初始化为什么是0因为姿态解算的初始角度直接用加速度计算出来的值填充初始误差很小协方差初始为0是合理的。而如果你的初始角度完全未知那就应该把P初始化为一个较大的对角矩阵比如100让滤波器快速收敛。5.3 主循环把一切串起来主程序里的逻辑是这样#include mpu6050.h #include kalman.h MPU6050_t mpu; Kalman_t kalman_roll; Kalman_t kalman_pitch; uint32_t last_time; float dt; int main(void) { HAL_Init(); SystemClock_Config(); MX_GPIO_Init(); MX_I2C1_Init(); MX_USART2_UART_Init(); // 初始化传感器失败则进入错误处理 if (MPU6050_Init(hi2c1) ! 0) { while (1); } // 上电校准陀螺仪设备必须静止 MPU6050_Calibrate_Gyro(hi2c1, mpu); // 用加速度计的初始角度初始化卡尔曼滤波器 MPU6050_Read_All(hi2c1, mpu); Kalman_Init(kalman_roll); Kalman_Init(kalman_pitch); kalman_roll.angle MPU6050_Get_Roll_Accel(mpu); kalman_pitch.angle MPU6050_Get_Pitch_Accel(mpu); last_time HAL_GetTick(); while (1) { // 计算实际采样周期 dt uint32_t now HAL_GetTick(); dt (now - last_time) / 1000.0f; if (dt 0.02f) dt 0.02f; // 防止卡顿导致dt异常 last_time now; MPU6050_Read_All(hi2c1, mpu); float roll_meas MPU6050_Get_Roll_Accel(mpu); float pitch_meas MPU6050_Get_Pitch_Accel(mpu); float roll Kalman_GetAngle(kalman_roll, roll_meas, mpu.Gx, dt); float pitch Kalman_GetAngle(kalman_pitch, pitch_meas, mpu.Gy, dt); // 用串口把数据发出去供波形工具查看 char buf[64]; int len snprintf(buf, sizeof(buf), roll:%.2f pitch:%.2f\r\n, roll, pitch); HAL_UART_Transmit(huart2, (uint8_t *)buf, len, 100); // 控制主频大约100Hz解算 HAL_Delay(10); } }主循环里有一个非常重要的处理dt的计算。很多移植卡尔曼滤波失败的人问题就出在dt上。如果你固定用10ms去算但实际主循环因为其他任务卡顿跑了50ms卡尔曼里的积分就会出错角度会产生明显的跳变。用HAL_GetTick实时计算实际dt并加一个上限保护是一种稳妥的做法。5.4 安装方向改变时的坐标系调整这个坑我踩过不止一次。MPU6050在PCB上的安装方向不是固定的比如四轴飞控里它可能绕Z轴转了90°安装。这时候如果你直接把Y轴数据当作俯仰角就会得到完全错误的结果。处理方法是在代码里加一个坐标映射。假设你的MPU6050绕Z轴旋转了90°俯仰运动和Y轴陀螺仪测量方向一致那你要做的是// 安装方向旋转90°时的映射 mpu.Gy_for_pitch mpu.Gy; // 视实际方向决定正负号 mpu.Gx_for_roll -mpu.Gx;更好做法是在硬件设计阶段就明确传感器坐标轴和机体轴系的对应关系然后在代码注释里记录清楚避免后面自己都忘了当初怎么映射的。6. 调参经验卡尔曼滤波Q和R应该怎么调6.1 一个参数一个参数来从互补滤波参数起步卡尔曼滤波不是魔法调参不对效果可能还不如互补滤波。我的调参经验是先跑一组基准数据再依据现象针对性调整。第一步把Q_angle设为0.001、Q_bias设为0.003、R_measure设为0.03这是大多数MPU6050项目的常用起点。把设备放在桌面上静止观察输出角度的波动。如果角度噪声在±0.5°以内说明R_measure设置合理如果噪声超过±1°说明R_measure偏大需要调小让滤波器更相信加速度计。第二步拿起设备快速晃动。观察角度跟随是否滞后明显。若滞后明显说明Q_angle偏小滤波器太懒不信任陀螺仪积分需要加大Q_angle。若角度出现明显毛刺说明Q_angle太大滤波器太活跃需要减小。第三步把设备静止放置10分钟观察角度是否缓慢漂移。如果角度漂移超过2°说明Q_bias偏小滤波器对零漂的估计更新太慢需要加大Q_bias。如果角度出现游泳式的晃动说明Q_bias偏大零漂估计噪声加减太猛需要减小。调参的本质就是不断权衡响应速度和噪声抑制。没有一个参数组合是万能的动起来顺手、静下来不飘就是好参数。6.2 实测调参记录一组典型的参数演变过程我在某平衡车项目里记录的参数调整过程是这样的初始参数Q_angle0.001、Q_bias0.003、R_measure0.03静态测试角度噪声±0.3°用手快速摆动时角度滞后约120ms效果已经不错了。但装上车后电机振动让加速度计噪声明显增大角度出现了小幅高频抖动。我把R_measure从0.03调到0.08后高频抖动明显减弱但因为更不信任加速度计低速时的角度恢复变慢车在静止启动时初始角度偏差恢复时间从1秒增加到3秒。这时候我调整Q_angle到0.002让陀螺仪积分稍微多参与一些恢复速度提了回来。最终这组参数是Q_angle0.002、Q_bias0.003、R_measure0.06在这个场景下最顺手。但这套参数换到下一台机械结构更重的设备上又不一定适用所以调参这事没有捷径只能一遍遍试。6.3 输出串口波形千万别用printf盲调调参最重要的工具是数据可视化。用串口把角度发出来在电脑上打开VOFA或者SerialPlot这样的波形工具一边动作一边看曲线参数调起来一目了然。我习惯一次性输出四个变量加速度计直接算出的角度红色、卡尔曼滤波后的角度绿色、陀螺仪积分原始角度蓝色、当前滤波器状态的角度速度输入黄色。四路波形叠在一起一眼就能看出来滤波器的响应是否合理、滞后多少、静态噪声多大。没有波形工具就盲调参数等于闭着眼睛开车。7. 常见问题与调试实录7.1 I2C读不到WHO_AM_I这个问题占了我遇到问题的80%。排查路径按照这五步走九成能解决第一确认电源。MPU6050工作电压是3.3V但很多模块板载了稳压芯片VCC接5V也能正常工作。如果你用的是自己做的板子务必确认VDD引脚在3.3VVLOGIC也接3.3V。第二确认上拉电阻。用示波器看SCL和SDA引脚空闲时应该都是高电平。如果SDA一直是低那就是上拉电阻没焊或者虚焊。第三确认接线。SCL-PB6SDA-PB7千万别接反。因为I2C在通信不正常时会一直卡在总线状态接口不对也是同样现象。第四确认地址。AD0引脚的电平决定I2C地址AD0接地是0x68接VCC是0x69。如果你的模块AD0被默认上拉到高需要读0x69试试。第五降低I2C速度。把CubeMX里的I2C速度从400k改到100k。线太长、干扰太大时400k可能通信不稳定降速通常很管用。7.2 角度数据一直在漂角度漂移首先要区分是积分漂移还是校正漂移。快速晃动后回来角度能恢复说明卡尔曼正常工作仅仅有瞬时漂移对控制类应用问题不大。如果角度一直线性地漂移毫无回正趋势那大概率是校准没做好。检查套路是把设备静止打印陀螺仪三个轴的原始值应该在0附近有±10以内的波动是正常的。如果某个轴输出稳定在200以上说明校准没生效或者校准后用了错误的数据源。另一个隐蔽问题是校准时机。如果你在设备还在晃动时执行校准平均值就不是零漂而是零漂运动平均。所以校准代码必须放在系统上电后的静止阶段校准期间串口打出提示信息校准完成后再进入主循环。7.3 角度假死卡尔曼滤波输出不变了这是最诡异的问题之一卡尔曼滤波输出值突然固定在一个数无论怎么动设备都不变。我排查了两次才发现原因浮点溢出。当运行一段时间后如果P矩阵的元素因为数值误差累积出现异常卡尔曼增益K可能趋近于0导致测量修正量y * K近乎为0滤波输出就不再跟随真实角度了。还有就是数据读取错误I2C返回0xFF时换算出来是巨大的角速度或者加速度一下子把滤波器打蒙了。解决办法是在主循环里检查传感器的原始数据范围。正常的加速度原始值范围在±16384±8g陀螺仪在±32767。如果读出来超出这些范围说明I2C读取出错直接丢弃这帧数据。再加一个定时器看门狗如果连续1秒I2C读取失败重新初始化传感器并重新进行陀螺仪校准。7.4 MPU6050的电源噪声引起的角度抖动电源质量对MEMS传感器的输出影响很大。MPU6050的模拟电源AVDD如果和数字电源DVDD直接用同一个LDO输出而不做隔离电机的PWM噪声会窜进模拟链路表现为加速度计输出出现50Hz或PWM频率的抖动。解决办法是在AVDD引脚加一个10欧姆电阻再并一个10uF和0.1uF的退耦电容构成一个简单的RC低通滤波器。如果PCB空间富余直接用独立的LDO给MPU6050供电效果最好。这个细节在自绘板项目里特别重要开发板一般不太容易遇到这类问题。8. 拓展从一维到三维四元数姿态解算的升级路径8.1 为什么两轴卡尔曼不够用前面讲的角度解算方法本质上是把Roll和Pitch拆成两个独立的一维卡尔曼滤波。这在平衡车、两轴云台这种运动范围有限的场景下完全够用。但如果你想做完整的3D姿态跟踪比如四轴飞行器的姿态控制这套方案就有个硬伤它没有处理轴间耦合和旋转顺序的问题。欧拉角描述姿态有万向锁问题当Pitch接近±90°时Roll和Yaw的旋转轴重合会丢失一个自由度导致姿态解算发散。实际飞控在剧烈姿态变化时欧拉角会出现跳变或者翻转这就是万向锁的表现。8.2 四元数的优势与代码落地四元数用四个参数w, x, y, z表示旋转不会出现万向锁问题而且计算量比旋转矩阵更小。从MPU6050原始数据到四元数的更新公式核心的梯度下降法Madgwick滤波和互补滤波的思路是一样的用陀螺仪的四元数微分方程积分旋转再用加速度计的测量值构建误差函数通过梯度下降修正四元数。Madgwick算法的AHRS实现只有不到200行C代码在STM32F4上跑得飞快。我建议在把本文的卡尔曼滤波跑通之后花点时间把Madgwick的代码也移植过来配合HAL库读取的原始数据直接就能输出无万向锁问题的四元数姿态。很多飞控开源项目里的姿态解算都是这么做的。8.3 移植一套Madgwick姿态解算的具体步骤移植Madgwick算法到HAL库工程核心步骤就三步第一步把MadgwickAHRS.c和MadgwickAHRS.h拷进工程里面有两个关键函数MadgwickAHRSupdate带磁力计版本和MadgwickAHRSupdateIMU不带磁力计版本。MPU6050没有磁力计用后者。第二步在MPU6050_Read_All之后把加速度和角速度单位换算成算法要求的格式。Madgwick算法要求加速度单位是g角速度单位是rad/s而MPU6050读出来的是°/s记得乘以0.0174533。#define DEG2RAD 0.0174533f MadgwickAHRSupdateIMU( mpu.Gx * DEG2RAD, mpu.Gy * DEG2RAD, mpu.Gz * DEG2RAD, mpu.Ax, mpu.Ay, mpu.Az );第三步从四元数提取欧拉角公式是roll atan2f(2.0f * (q0 * q1 q2 * q3), 1.0f - 2.0f * (q1 * q1 q2 * q2)) * 180.0f / PI; pitch asinf(2.0f * (q0 * q2 - q3 * q1)) * 180.0f / PI; yaw atan2f(2.0f * (q0 * q3 q1 * q2), 1.0f - 2.0f * (q2 * q2 q3 * q3)) * 180.0f / PI;Madgwick算法里的beta参数默认0.1就相当于卡尔曼的Q/R比值beta越大越信任加速度计响应快但噪声大beta越小越平滑但滞后明显。实际调的时候先放0.1再看波形微调。9. 写在实验台边有一套稳定的六轴姿态解算方案对后面做平衡车、四轴、云台、体感遥控器都有直接的帮助。我自己前后在三个不同的项目里用过这套MPU6050HAL库卡尔曼滤波的方案每次重新拾起来总能在某个环节发现可以优化的点——要么是校准流程更合理了要么是滤波参数更适应具体机械特性了。这套组合的代码量不大但每个细节都值得反复推敲。最后再分享一个小技巧调试MPU6050的时候别着急写完整算法。先把原始数据用串口打出来放到VOFA画波形确认每一个轴的极性、方向都和你预想的一致再往上叠滤波算法。我见过太多同行在姿态解算阶段调试半天最后发现是传感器某一轴的原始数据正负号反了——那种挫败感真的特别伤。数据极性对了后续不管是卡尔曼滤波还是转入四元数解算都会顺畅很多。 STM32 HAL库实战：MPU6050数据采集与卡尔曼滤波姿态解算
还记得第一次调平衡车项目我拿着MPU6050的数据愣是看了半天——角度值在那里像个醉汉一样晃陀螺仪的零漂又让角度慢慢躺平下去。那时候网上资料七零八落DMP库一调就崩最后靠着一份残缺的卡尔曼滤波代码硬是啃了下来。现在回头看从陀螺仪原始数据到稳定可用的姿态角这条路其实并不长只是中间有几个坎得迈过去。这篇文章我把这套基于STM32F4和HAL库的MPU6050实战方案完整写出来从底层I2C寄存器读写到陀螺仪/加速度计的校准与换算再到四元数概念梳理和卡尔曼滤波的C语言落地最后给出可以直接抄的完整代码。无论是刚接触MPU6050的新手还是想把DMP替换成自研解算算法的老手这篇文章都能省下你至少一周的调试时间。1. 拿到一颗MPU6050先搞清它到底能给你什么1.1 陀螺仪和加速度计的物理意义MPU6050这颗六轴传感器内部实际上集成了两颗独立的MEMS芯片。一颗是三轴陀螺仪测量的是绕X、Y、Z三个轴的旋转角速度单位是°/s另一颗是三轴加速度计测量的是沿三个轴的线性加速度单位是g9.8m/s²。这两个物理量看起来都很直观但它们各自的短板也特别明显。陀螺仪的角速度积分可以得到角度但只要有零点偏移积分就会不停累积误差——我之前测过一颗没有任何校准的MPU6050静止放置时Z轴陀螺仪读数能到2°/s积分一分钟角度就漂了120°这压根没法直接用。加速度计呢它对重力方向的分量感知非常准静止时可以算出精确的倾斜角但一旦有运动加速度混进来比如平衡车起步、机械臂挥动输出就会剧烈抖动根本没法直接用。所以姿态解算的核心思路就一句话用陀螺仪的短时精度来抵消加速度计的噪声用加速度计的长时稳定性来修正陀螺仪的漂移。这就是数据融合的价值也是卡尔曼滤波和互补滤波在这类项目里大显身手的原因。1.2 关于DMP那点事儿为什么很多人不想用DMPMPU6050自带一个Digital Motion ProcessorDMPInvenSense官方提供了DMP固件库可以直接输出四元数甚至欧拉角很多新手觉得用它最省事。但我必须说工程项目里我基本不用DMP原因有三个。第一DMP固件库封装得太严实你拿到的是一堆编译好的二进制文件加上不完整的头文件出了问题根本没法调试。它内部怎么融合的、融合参数是什么样、有没有针对你的安装方式进行调校全是一团黑。第二DMP库的移植性很差。官方给的库是针对老版MSP430写的底层接口你把它搬到STM32F4上要自己改写I2C读写函数和中断接口光是适配HAL库就得折腾半天典型的花钱费时还不讨好。第三DMP输出的姿态是它的标准答案你没法针对自己的机械结构做优化。比如你的设备安装时传感器歪了5°用自研算法可以直接在初始姿态里做修正DMP就比较难办。所以我强烈建议实战项目里自己写解算代码哪怕一开始只用互补滤波搞定了一两个轴之后再去上卡尔曼滤波。这个过程能让你真正理解姿态解算后续换传感器、换滤波算法都事半功倍。2. 从零开始配置STM32F4的HAL库环境2.1 建立工程CubeMX初始化I2C和串口用HAL库做STM32F4开发第一步就是CubeMX生成工程。这里有一个很关键的细节MPU6050通信协议是I2C而I2C总线的时钟、上拉电阻配置直接决定了后续读数据稳不稳定。CubeMX里你要干的活是这样的RCC选择HSE外部晶振主频配置到168MHzSTM32F407或180MHzSTM32F429具体看芯片型号。I2C1选择PB6/PB7引脚模式I2C速度选400kHz Fast Mode。MPU6050手册说明I2C时钟可以到400kHz实测下来400kHz比100kHz快得多而且只要电路设计得当稳得很。如果调试中发现数据不稳定再降回100kHz。USART2选择PA2/PA3异步模式115200-8-N-1用于把姿态数据发到电脑上看波形。顺便把SYS里的Debug设为Serial Wire免得烧录器占用引脚。CubeMX默认生成的HAL库会把I2C的时序参数都算好一般不需要手动改。但有一个点很多人会漏掉I2C的上拉电阻。MPU6050模块大多数板子上已经贴了2.2k或4.7k的上拉电阻但如果你是自己画的板子一定要在SCL和SDA上加4.7k上拉电阻到VCC。I2C总线协议要求开路输出没有上拉电阻是绝对跑不起来的。2.2 最小系统验证用HAL库读写MPU6050寄存器工程生成之后先别急着写姿态解算第一步先验证I2C能不能和MPU6050正常通信。MPU6050的I2C地址是7位地址0x68AD0引脚接地或0x69AD0引脚接高。多数模块默认接地也就是0x688位写地址就是0xD0读地址是0xD1。最稳妥的验证方式是读WHO_AM_I寄存器地址是0x75。正常返回值应该是0x68。对应的HAL库代码就三行uint8_t who_am_i 0; HAL_I2C_Mem_Read(hi2c1, 0x68 1, 0x75, I2C_MEMADD_SIZE_8BIT, who_am_i, 1, 100); // 如果 who_am_i 0x68说明I2C通信正常这里有一个新手特别容易翻车的地方HAL_I2C_Mem_Read的第三个参数是设备地址形参里要求的是8位地址也就是0x68左移一位变成0xD0而很多人直接填0x68结果读出来全是0xFF。如果WHO_AM_I读出来是0x68恭喜你芯片活着。接下来要做的是唤醒芯片。MPU6050上电后默认处于睡眠模式寄存器PWR_MGMT_1地址0x6B的bit6是SLEEP位置1就是睡眠。我们把它写成0x00使用内部8MHz振荡器并清除睡眠位uint8_t pwr_mgmt 0x00; HAL_I2C_Mem_Write(hi2c1, 0x68 1, 0x6B, I2C_MEMADD_SIZE_8BIT, pwr_mgmt, 1, 100);然后配置陀螺仪量程、加速度计量程和数字低通滤波器寄存器SMPLRT_DIV0x19设置采样率分频0x07表示采样率 1kHz / (1 7) 125Hz这个值对控制类应用足够了。要求更高响应速度可以设0x03得到250Hz。寄存器CONFIG0x1A的bit2:0设置数字低通滤波器带宽。推荐设置0x03截止频率约44Hz可以滤掉大部分振动噪声又不至于让角度响应太迟钝。注意这个低通滤波器对陀螺仪和加速度计同时生效。寄存器GYRO_CONFIG0x1B和ACCEL_CONFIG0x1C设置量程。我一般用±2000dps和±8g的组合原因后面细说。配完这些就可以读取六轴原始数据了。加速度计的每个轴16位数据在寄存器0x3B到0x40陀螺仪在0x43到0x48。每个轴拆成高字节和低字节两个8位寄存器读出来拼一下就完事。uint8_t data[14]; HAL_I2C_Mem_Read(hi2c1, 0x68 1, 0x3B, I2C_MEMADD_SIZE_8BIT, data, 14, 100); int16_t accel_x (data[0] 8) | data[1]; int16_t accel_y (data[2] 8) | data[3]; int16_t accel_z (data[4] 8) | data[5]; int16_t temp (data[6] 8) | data[7]; int16_t gyro_x (data[8] 8) | data[9]; int16_t gyro_y (data[10] 8) | data[11]; int16_t gyro_z (data[12] 8) | data[13];到这里你已经有最原始的传感器数据了。3. 原始数据的读取、单位换算和校准3.1 为什么把量程设置为±2000dps和±8g很多教程喜欢把量程设为±250dps理由是分辨率最高但实际应用里这个设置很容易踩坑。量程和灵敏度的关系是这样的MPU6050陀螺仪每个量程对应不同的刻度因子FS_SEL具体来说量程dps刻度因子LSB/°/s±250131±50065.5±100032.8±200016.4数值越大刻度因子越小意味着每个LSB代表的角速度越大原始值在相同角速度下的数值就越小。也就是说量程越大分辨率越低但能测的上限越大。我选择±2000dps的原因是平衡车、机械臂、无人机这类带电机振动的场景瞬间角速度经常超过500°/s甚至1000°/s。如果量程选低了陀螺仪会直接饱和输出卡在最大值这时候积分出来的角度就是灾难。选±2000dps虽然分辨率低一些但配合16位ADC最小分辨率是2000/327680.061°/s/LSB这个精度对姿态控制来说已经绰绰有余了。加速度计量程选±8g同理。MPU6050的加速度计在静止时理论上只受重力1g但电机启动瞬间的冲击加速度能到几个g量程选小了同样会饱和。±8g对应的刻度因子是4096 LSB/g静止时重力分量产生的原始值大约在4000左右能有足够的分辨率来分析姿态。3.2 一杯水法陀螺仪的静态零漂校准陀螺仪的零漂是姿态解算里最恶心的东西。每一颗MPU6050的零漂都不一样而且会随温度变化。所以代码里不能写死一个偏移量必须上电后动态校准。我的做法是上电静止校准也叫一杯水法——把设备平放在桌面上静止不动连续采样200次陀螺仪数据取平均作为零漂偏移量。因为静止时真实角速度是0平均值就是零漂。#define GYRO_CALI_SAMPLES 200 float gyro_offset[3] {0, 0, 0}; void MPU6050_GyroCalibration(void) { int32_t sum[3] {0, 0, 0}; int16_t raw[3]; for (int i 0; i GYRO_CALI_SAMPLES; i) { MPU6050_ReadGyro(raw); sum[0] raw[0]; sum[1] raw[1]; sum[2] raw[2]; HAL_Delay(2); } for (int i 0; i 3; i) { gyro_offset[i] (float)sum[i] / GYRO_CALI_SAMPLES; } }这里有个细节容易被忽略I2C读取MPU6050数据寄存器如果你连续两次读取的间隔太短可能读到的是同一组数据。MPU6050的数据寄存器是异步更新的读的时候不会锁存所以如果你的循环读得太快可能连续读到一模一样的值。解决方法是读取间隔大于传感器的输出速率比如采样率是125Hz读取间隔至少要8ms。校准采200个样每个间隔2ms看起来是400ms但读取频率可能已经超过传感器更新率了。稳妥起见我建议每次读取之间延时5ms以上。采集完的偏移量怎么存最省事的办法是直接存放在全局变量里每次解算前用原始读数减去偏移量即可gyro_x_calibrated (float)raw_gyro_x - gyro_offset[0];这种校准方式能消除绝大部分零漂但温度变化仍然会导致剩余漂移。追求极致的项目可以做温度补偿但对于绝大多数实战场景上电校准一次足够了配合卡尔曼滤波的修正能力角度能稳定保持很久。3.3 从原始值到物理单位的换算读到的16位原始值要换算成物理单位才能用于解算。换算公式非常简单陀螺仪角速度(°/s) 原始值 / 刻度因子。±2000dps对应刻度因子16.4即gyro_dps raw_gyro / 16.4。加速度(m/s²) 原始值 / 4096 * 9.8。有些简化算法直接以g为单位那就不乘9.8直接raw_accel / 4096重力加速度在Z轴上的投影应该在±1g范围内。在正式解算之前还要做一步归一化处理。因为安装偏差、量程误差等原因三个轴的比例因子不会完全一致最直观的验证方法是用加速度计的模长来判断静止时sqrt(ax² ay² az²)应该等于1g归一化后。如果偏差大于2%建议做一次六面校准分别让六个面朝上放置记录数据并求解三个轴的比例因子和零偏。不过对大多数应用来说归一化误差控制在1~2%以内直接用就够了。4. 姿态解算欧拉角、旋转矩阵和四元数先把概念捋清楚4.1 为什么不能直接把数据凑在一起输出很多新手拿到加速度计和陀螺仪的数据第一反应是把加速度计算出来的角度和陀螺仪积分算出来的角度直接平均。这样做出来的角度又慢又飘完全不能用。根本原因在于加速度计和陀螺仪的频域特性完全相反。加速度计输出的角度在静态时特别准但一有动态加速度就剧烈跳动——高频噪声大陀螺仪积分的角度短时特别平稳但长时间会漂走——低频误差大。数据融合的本质就是设计一个滤波器低频段信任加速度计高频段信任陀螺仪交叉频率附近的过渡要平滑。在了解融合算法之前得先把姿态的数学表达搞清楚。姿态角也叫欧拉角指的是物体相对世界坐标系的旋转角度分别绕X轴、Y轴、Z轴旋转得到横滚角Roll、俯仰角Pitch、偏航角Yaw。物体旋转到任意姿态可以按固定顺序分解成三次旋转比如先绕Z轴转Yaw再绕Y轴转Pitch最后绕X轴转Roll。这里有一个关键点必须强调欧拉角的旋转顺序会影响最终结果同一个姿态按不同顺序分解三个角度是不同的。所以你在代码里看到的Roll和Pitch一定是基于某个约定的旋转顺序推导的。大多数姿态解算算法都固定使用ZYX旋转顺序也就是先偏航、再俯仰、最后横滚。如果哪天花花绿绿的博文里角度对不上多半是旋转顺序定义不一致。4.2 从加速度计估算初始姿态角在使用融合算法之前需要有一个初始的姿态值。最常见的方法是利用加速度计在静止状态下计算Roll和Pitch。在静止状态下加速度计的测量模型是加速度计测到的加速度向量 重力向量在机体坐标系中的投影。也就是说静止时ax、ay、az的合成向量模长等于g方向指向地心。根据这个关系可以解出初始横滚角和俯仰角roll_acc atan2(ay, az) * 180 / PI; pitch_acc atan(-ax / sqrt(ay * ay az * az)) * 180 / PI;这个公式的来源是空间几何横滚角是Y轴与水平面的夹角由Y轴加速度和Z轴加速度的比例决定俯仰角是X轴加速度与Y、Z合成加速度的比例决定。注意这里算出来的角度在设备有线性加速度时是不准的所以只用来初始化或者用低权重参与融合。另外偏航角Yaw用加速度计是算不出来的因为重力向量不携带绕重力轴旋转的信息。要获得Yaw必须依赖磁力计或视觉里程计。很多平衡车项目只用Roll和Pitch就能跑这种场景下单靠MPU6050就完全够用。4.3 互补滤波先把最朴素的融合搞明白卡尔曼滤波出场之前我必须先讲讲互补滤波。因为互补滤波的代码极短逻辑极清晰先理解它再理解卡尔曼会有一种哦原来是这样的通透感。互补滤波公式长这样angle 0.98 * (angle gyro_rate * dt) 0.02 * acc_angle;就这么一行。它做的事情是大部分信任陀螺仪积分的角度同时用加速度计的角度做一个缓慢的修正。比例为0.98和0.02意味着陀螺仪占主导、加速度计做微调。这个比例对应的时间常数是tau dt * (1 - alpha) / alpha; // 当 dt0.01s, alpha0.98 时tau 0.01 * 0.02 / 0.98 ≈ 0.2s时间常数的物理含义是加速度计的修正作用经过这个时间后能把陀螺仪的积分误差校正掉63%。如果你觉得角度恢复太慢就减小alpha如果觉得角度噪声太大就增大alpha。这个滤波器就一个参数调起来特别顺手。但互补滤波有明显的天花板它假设传感器噪声是固定的、不随时间变化的。实际上MPU6050在运动过程中的振动强度一直在变静态和动态的最优alpha值并不一样。如果需要更好的效果就该上卡尔曼滤波了。4.4 卡尔曼滤波从原理到一维角度模型卡尔曼滤波的核心思想是把系统的状态估计拆成预测和更新两步。预测阶段用系统模型陀螺仪积分推算状态更新阶段用测量值加速度计角度修正预测结果。关键区别于互补滤波的地方在于卡尔曼滤波能动态调整该信任预测还是信任测量这个信任程度用协方差矩阵来量化。对于姿态解算这种场景我们不需要完整的3D姿态估计只对单个轴的角度做卡尔曼滤波就足够了。比如要估计横滚角Roll状态向量定义为x [angle, bias]其中angle是估计的角度bias是陀螺仪的漂移误差。系统的状态方程是angle_new angle (gyro_rate - bias) * dt 噪声 bias_new bias 噪声这个模型的意思是角度由陀螺仪积分得出但要去掉估计出的零漂零漂bias本身的变化非常缓慢当作随机游走处理。对应的两个关键噪声参数是过程噪声协方差Q代表你对状态模型的信任程度。Q越小越相信陀螺仪积分Q越大越允许状态快速变化。一般取0.001到0.01。测量噪声协方差R代表你对加速度计角度测量的信任程度。R越小越相信加速度计R越大越怀疑加速度计。一般取0.01到1。代码实现上我直接用标准的5个卡尔曼方程按一维情况简化编写。完整代码会在下一节给出。5. HAL库卡尔曼滤波代码实战5.1 代码结构完整代码直接抄下面这套代码我拆成四个文件mpu6050.h、mpu6050.c、kalman.h、kalman.c。默认你已经用CubeMX初始化了I2C1和USART2工程能编译通过。首先是mpu6050.h头文件#ifndef __MPU6050_H #define __MPU6050_H #include stm32f4xx_hal.h #define MPU6050_ADDR 0x68 #define MPU6050_ADDR_W ((MPU6050_ADDR 1) 0xFE) #define MPU6050_ADDR_R ((MPU6050_ADDR 1) | 0x01) #define MPU6050_REG_WHO_AM_I 0x75 #define MPU6050_REG_PWR_MGMT_1 0x6B #define MPU6050_REG_SMPLRT_DIV 0x19 #define MPU6050_REG_CONFIG 0x1A #define MPU6050_REG_GYRO_CONFIG 0x1B #define MPU6050_REG_ACCEL_CONFIG 0x1C #define MPU6050_REG_ACCEL_XOUT 0x3B #define MPU6050_REG_GYRO_XOUT 0x43 typedef struct { int16_t Accel_X_RAW; int16_t Accel_Y_RAW; int16_t Accel_Z_RAW; int16_t Gyro_X_RAW; int16_t Gyro_Y_RAW; int16_t Gyro_Z_RAW; float Ax, Ay, Az; float Gx, Gy, Gz; float Roll, Pitch, Yaw; float gyro_offset[3]; } MPU6050_t; uint8_t MPU6050_Init(I2C_HandleTypeDef *hi2c); uint8_t MPU6050_Read_All(I2C_HandleTypeDef *hi2c, MPU6050_t *mpu); void MPU6050_Calibrate_Gyro(I2C_HandleTypeDef *hi2c, MPU6050_t *mpu); float MPU6050_Get_Roll_Accel(MPU6050_t *mpu); float MPU6050_Get_Pitch_Accel(MPU6050_t *mpu); #endif然后是mpu6050.c#include mpu6050.h #include math.h #define GYRO_SCALE 16.4f // ±2000dps 对应的刻度因子 #define ACCEL_SCALE 4096.0f // ±8g 对应的刻度因子 #define GYRO_CALI_SAMPLES 200 static I2C_HandleTypeDef *mpu_i2c; static uint8_t MPU6050_WriteReg(uint8_t reg, uint8_t data) { return HAL_I2C_Mem_Write(mpu_i2c, MPU6050_ADDR_W, reg, I2C_MEMADD_SIZE_8BIT, data, 1, 100); } static uint8_t MPU6050_ReadRegs(uint8_t reg, uint8_t *buf, uint8_t len) { return HAL_I2C_Mem_Read(mpu_i2c, MPU6050_ADDR_R, reg, I2C_MEMADD_SIZE_8BIT, buf, len, 100); } uint8_t MPU6050_Init(I2C_HandleTypeDef *hi2c) { mpu_i2c hi2c; uint8_t who_am_i 0; MPU6050_ReadRegs(MPU6050_REG_WHO_AM_I, who_am_i, 1); if (who_am_i ! 0x68) return 1; MPU6050_WriteReg(MPU6050_REG_PWR_MGMT_1, 0x00); // 唤醒 HAL_Delay(50); MPU6050_WriteReg(MPU6050_REG_SMPLRT_DIV, 0x07); // 125Hz采样率 MPU6050_WriteReg(MPU6050_REG_CONFIG, 0x03); // 低通滤波44Hz MPU6050_WriteReg(MPU6050_REG_GYRO_CONFIG, 0x18); // ±2000dps MPU6050_WriteReg(MPU6050_REG_ACCEL_CONFIG, 0x10); // ±8g return 0; } void MPU6050_Calibrate_Gyro(I2C_HandleTypeDef *hi2c, MPU6050_t *mpu) { int32_t sum[3] {0, 0, 0}; uint8_t data[6]; for (int i 0; i GYRO_CALI_SAMPLES; i) { MPU6050_ReadRegs(MPU6050_REG_GYRO_XOUT, data, 6); sum[0] (int16_t)((data[0] 8) | data[1]); sum[1] (int16_t)((data[2] 8) | data[3]); sum[2] (int16_t)((data[4] 8) | data[5]); HAL_Delay(5); } mpu-gyro_offset[0] (float)sum[0] / GYRO_CALI_SAMPLES; mpu-gyro_offset[1] (float)sum[1] / GYRO_CALI_SAMPLES; mpu-gyro_offset[2] (float)sum[2] / GYRO_CALI_SAMPLES; } uint8_t MPU6050_Read_All(I2C_HandleTypeDef *hi2c, MPU6050_t *mpu) { uint8_t data[14]; if (MPU6050_ReadRegs(MPU6050_REG_ACCEL_XOUT, data, 14) ! HAL_OK) return 1; mpu-Accel_X_RAW (int16_t)((data[0] 8) | data[1]); mpu-Accel_Y_RAW (int16_t)((data[2] 8) | data[3]); mpu-Accel_Z_RAW (int16_t)((data[4] 8) | data[5]); mpu-Gyro_X_RAW (int16_t)((data[8] 8) | data[9]); mpu-Gyro_Y_RAW (int16_t)((data[10] 8) | data[11]); mpu-Gyro_Z_RAW (int16_t)((data[12] 8) | data[13]); mpu-Ax mpu-Accel_X_RAW / ACCEL_SCALE; mpu-Ay mpu-Accel_Y_RAW / ACCEL_SCALE; mpu-Az mpu-Accel_Z_RAW / ACCEL_SCALE; mpu-Gx (mpu-Gyro_X_RAW - mpu-gyro_offset[0]) / GYRO_SCALE; mpu-Gy (mpu-Gyro_Y_RAW - mpu-gyro_offset[1]) / GYRO_SCALE; mpu-Gz (mpu-Gyro_Z_RAW - mpu-gyro_offset[2]) / GYRO_SCALE; return 0; } float MPU6050_Get_Roll_Accel(MPU6050_t *mpu) { return atan2f(mpu-Ay, mpu-Az) * 180.0f / 3.14159265f; } float MPU6050_Get_Pitch_Accel(MPU6050_t *mpu) { return atanf(-mpu-Ax / sqrtf(mpu-Ay * mpu-Ay mpu-Az * mpu-Az)) * 180.0f / 3.14159265f; }这段代码把传感器初始化和数据读取分离得比较干净。有一个细节MPU6050_Calibrate_Gyro是放在Init之后、进入主循环之前调用的调用时设备必须静止。5.2 卡尔曼滤波核心代码接下来是核心的卡尔曼滤波代码。kalman.h定义滤波器结构体#ifndef __KALMAN_H #define __KALMAN_H typedef struct { float Q_angle; // 角度过程噪声协方差 float Q_bias; // 零漂过程噪声协方差 float R_measure; // 测量噪声协方差 float angle; // 当前角度估计值 float bias; // 当前零漂估计值 float P[2][2]; // 误差协方差矩阵 } Kalman_t; void Kalman_Init(Kalman_t *kal); float Kalman_GetAngle(Kalman_t *kal, float new_angle, float new_rate, float dt); #endifkalman.c的完整实现#include kalman.h void Kalman_Init(Kalman_t *kal) { kal-Q_angle 0.001f; kal-Q_bias 0.003f; kal-R_measure 0.03f; kal-angle 0.0f; kal-bias 0.0f; kal-P[0][0] 0.0f; kal-P[0][1] 0.0f; kal-P[1][0] 0.0f; kal-P[1][1] 0.0f; } float Kalman_GetAngle(Kalman_t *kal, float new_angle, float new_rate, float dt) { float rate new_rate - kal-bias; kal-angle dt * rate; kal-P[0][0] dt * (dt * kal-P[1][1] - kal-P[0][1] - kal-P[1][0] kal-Q_angle); kal-P[0][1] - dt * kal-P[1][1]; kal-P[1][0] - dt * kal-P[1][1]; kal-P[1][1] kal-Q_bias * dt; float S kal-P[0][0] kal-R_measure; float K[2]; K[0] kal-P[0][0] / S; K[1] kal-P[1][0] / S; float y new_angle - kal-angle; kal-angle K[0] * y; kal-bias K[1] * y; float P00_temp kal-P[0][0]; float P01_temp kal-P[0][1]; kal-P[0][0] - K[0] * P00_temp; kal-P[0][1] - K[0] * P01_temp; kal-P[1][0] - K[1] * P00_temp; kal-P[1][1] - K[1] * P01_temp; return kal-angle; }这套实现是标准的线性卡尔曼滤波针对角度零漂两状态模型的简化版本。它内部维护一个2x2的协方差矩阵P每次预测时加上过程噪声Q更新时用测量噪声R计算卡尔曼增益K最终输出修正后的角度估计值。这里有个初学者容易迷糊的点P矩阵的初始化为什么是0因为姿态解算的初始角度直接用加速度计算出来的值填充初始误差很小协方差初始为0是合理的。而如果你的初始角度完全未知那就应该把P初始化为一个较大的对角矩阵比如100让滤波器快速收敛。5.3 主循环把一切串起来主程序里的逻辑是这样#include mpu6050.h #include kalman.h MPU6050_t mpu; Kalman_t kalman_roll; Kalman_t kalman_pitch; uint32_t last_time; float dt; int main(void) { HAL_Init(); SystemClock_Config(); MX_GPIO_Init(); MX_I2C1_Init(); MX_USART2_UART_Init(); // 初始化传感器失败则进入错误处理 if (MPU6050_Init(hi2c1) ! 0) { while (1); } // 上电校准陀螺仪设备必须静止 MPU6050_Calibrate_Gyro(hi2c1, mpu); // 用加速度计的初始角度初始化卡尔曼滤波器 MPU6050_Read_All(hi2c1, mpu); Kalman_Init(kalman_roll); Kalman_Init(kalman_pitch); kalman_roll.angle MPU6050_Get_Roll_Accel(mpu); kalman_pitch.angle MPU6050_Get_Pitch_Accel(mpu); last_time HAL_GetTick(); while (1) { // 计算实际采样周期 dt uint32_t now HAL_GetTick(); dt (now - last_time) / 1000.0f; if (dt 0.02f) dt 0.02f; // 防止卡顿导致dt异常 last_time now; MPU6050_Read_All(hi2c1, mpu); float roll_meas MPU6050_Get_Roll_Accel(mpu); float pitch_meas MPU6050_Get_Pitch_Accel(mpu); float roll Kalman_GetAngle(kalman_roll, roll_meas, mpu.Gx, dt); float pitch Kalman_GetAngle(kalman_pitch, pitch_meas, mpu.Gy, dt); // 用串口把数据发出去供波形工具查看 char buf[64]; int len snprintf(buf, sizeof(buf), roll:%.2f pitch:%.2f\r\n, roll, pitch); HAL_UART_Transmit(huart2, (uint8_t *)buf, len, 100); // 控制主频大约100Hz解算 HAL_Delay(10); } }主循环里有一个非常重要的处理dt的计算。很多移植卡尔曼滤波失败的人问题就出在dt上。如果你固定用10ms去算但实际主循环因为其他任务卡顿跑了50ms卡尔曼里的积分就会出错角度会产生明显的跳变。用HAL_GetTick实时计算实际dt并加一个上限保护是一种稳妥的做法。5.4 安装方向改变时的坐标系调整这个坑我踩过不止一次。MPU6050在PCB上的安装方向不是固定的比如四轴飞控里它可能绕Z轴转了90°安装。这时候如果你直接把Y轴数据当作俯仰角就会得到完全错误的结果。处理方法是在代码里加一个坐标映射。假设你的MPU6050绕Z轴旋转了90°俯仰运动和Y轴陀螺仪测量方向一致那你要做的是// 安装方向旋转90°时的映射 mpu.Gy_for_pitch mpu.Gy; // 视实际方向决定正负号 mpu.Gx_for_roll -mpu.Gx;更好做法是在硬件设计阶段就明确传感器坐标轴和机体轴系的对应关系然后在代码注释里记录清楚避免后面自己都忘了当初怎么映射的。6. 调参经验卡尔曼滤波Q和R应该怎么调6.1 一个参数一个参数来从互补滤波参数起步卡尔曼滤波不是魔法调参不对效果可能还不如互补滤波。我的调参经验是先跑一组基准数据再依据现象针对性调整。第一步把Q_angle设为0.001、Q_bias设为0.003、R_measure设为0.03这是大多数MPU6050项目的常用起点。把设备放在桌面上静止观察输出角度的波动。如果角度噪声在±0.5°以内说明R_measure设置合理如果噪声超过±1°说明R_measure偏大需要调小让滤波器更相信加速度计。第二步拿起设备快速晃动。观察角度跟随是否滞后明显。若滞后明显说明Q_angle偏小滤波器太懒不信任陀螺仪积分需要加大Q_angle。若角度出现明显毛刺说明Q_angle太大滤波器太活跃需要减小。第三步把设备静止放置10分钟观察角度是否缓慢漂移。如果角度漂移超过2°说明Q_bias偏小滤波器对零漂的估计更新太慢需要加大Q_bias。如果角度出现游泳式的晃动说明Q_bias偏大零漂估计噪声加减太猛需要减小。调参的本质就是不断权衡响应速度和噪声抑制。没有一个参数组合是万能的动起来顺手、静下来不飘就是好参数。6.2 实测调参记录一组典型的参数演变过程我在某平衡车项目里记录的参数调整过程是这样的初始参数Q_angle0.001、Q_bias0.003、R_measure0.03静态测试角度噪声±0.3°用手快速摆动时角度滞后约120ms效果已经不错了。但装上车后电机振动让加速度计噪声明显增大角度出现了小幅高频抖动。我把R_measure从0.03调到0.08后高频抖动明显减弱但因为更不信任加速度计低速时的角度恢复变慢车在静止启动时初始角度偏差恢复时间从1秒增加到3秒。这时候我调整Q_angle到0.002让陀螺仪积分稍微多参与一些恢复速度提了回来。最终这组参数是Q_angle0.002、Q_bias0.003、R_measure0.06在这个场景下最顺手。但这套参数换到下一台机械结构更重的设备上又不一定适用所以调参这事没有捷径只能一遍遍试。6.3 输出串口波形千万别用printf盲调调参最重要的工具是数据可视化。用串口把角度发出来在电脑上打开VOFA或者SerialPlot这样的波形工具一边动作一边看曲线参数调起来一目了然。我习惯一次性输出四个变量加速度计直接算出的角度红色、卡尔曼滤波后的角度绿色、陀螺仪积分原始角度蓝色、当前滤波器状态的角度速度输入黄色。四路波形叠在一起一眼就能看出来滤波器的响应是否合理、滞后多少、静态噪声多大。没有波形工具就盲调参数等于闭着眼睛开车。7. 常见问题与调试实录7.1 I2C读不到WHO_AM_I这个问题占了我遇到问题的80%。排查路径按照这五步走九成能解决第一确认电源。MPU6050工作电压是3.3V但很多模块板载了稳压芯片VCC接5V也能正常工作。如果你用的是自己做的板子务必确认VDD引脚在3.3VVLOGIC也接3.3V。第二确认上拉电阻。用示波器看SCL和SDA引脚空闲时应该都是高电平。如果SDA一直是低那就是上拉电阻没焊或者虚焊。第三确认接线。SCL-PB6SDA-PB7千万别接反。因为I2C在通信不正常时会一直卡在总线状态接口不对也是同样现象。第四确认地址。AD0引脚的电平决定I2C地址AD0接地是0x68接VCC是0x69。如果你的模块AD0被默认上拉到高需要读0x69试试。第五降低I2C速度。把CubeMX里的I2C速度从400k改到100k。线太长、干扰太大时400k可能通信不稳定降速通常很管用。7.2 角度数据一直在漂角度漂移首先要区分是积分漂移还是校正漂移。快速晃动后回来角度能恢复说明卡尔曼正常工作仅仅有瞬时漂移对控制类应用问题不大。如果角度一直线性地漂移毫无回正趋势那大概率是校准没做好。检查套路是把设备静止打印陀螺仪三个轴的原始值应该在0附近有±10以内的波动是正常的。如果某个轴输出稳定在200以上说明校准没生效或者校准后用了错误的数据源。另一个隐蔽问题是校准时机。如果你在设备还在晃动时执行校准平均值就不是零漂而是零漂运动平均。所以校准代码必须放在系统上电后的静止阶段校准期间串口打出提示信息校准完成后再进入主循环。7.3 角度假死卡尔曼滤波输出不变了这是最诡异的问题之一卡尔曼滤波输出值突然固定在一个数无论怎么动设备都不变。我排查了两次才发现原因浮点溢出。当运行一段时间后如果P矩阵的元素因为数值误差累积出现异常卡尔曼增益K可能趋近于0导致测量修正量y * K近乎为0滤波输出就不再跟随真实角度了。还有就是数据读取错误I2C返回0xFF时换算出来是巨大的角速度或者加速度一下子把滤波器打蒙了。解决办法是在主循环里检查传感器的原始数据范围。正常的加速度原始值范围在±16384±8g陀螺仪在±32767。如果读出来超出这些范围说明I2C读取出错直接丢弃这帧数据。再加一个定时器看门狗如果连续1秒I2C读取失败重新初始化传感器并重新进行陀螺仪校准。7.4 MPU6050的电源噪声引起的角度抖动电源质量对MEMS传感器的输出影响很大。MPU6050的模拟电源AVDD如果和数字电源DVDD直接用同一个LDO输出而不做隔离电机的PWM噪声会窜进模拟链路表现为加速度计输出出现50Hz或PWM频率的抖动。解决办法是在AVDD引脚加一个10欧姆电阻再并一个10uF和0.1uF的退耦电容构成一个简单的RC低通滤波器。如果PCB空间富余直接用独立的LDO给MPU6050供电效果最好。这个细节在自绘板项目里特别重要开发板一般不太容易遇到这类问题。8. 拓展从一维到三维四元数姿态解算的升级路径8.1 为什么两轴卡尔曼不够用前面讲的角度解算方法本质上是把Roll和Pitch拆成两个独立的一维卡尔曼滤波。这在平衡车、两轴云台这种运动范围有限的场景下完全够用。但如果你想做完整的3D姿态跟踪比如四轴飞行器的姿态控制这套方案就有个硬伤它没有处理轴间耦合和旋转顺序的问题。欧拉角描述姿态有万向锁问题当Pitch接近±90°时Roll和Yaw的旋转轴重合会丢失一个自由度导致姿态解算发散。实际飞控在剧烈姿态变化时欧拉角会出现跳变或者翻转这就是万向锁的表现。8.2 四元数的优势与代码落地四元数用四个参数w, x, y, z表示旋转不会出现万向锁问题而且计算量比旋转矩阵更小。从MPU6050原始数据到四元数的更新公式核心的梯度下降法Madgwick滤波和互补滤波的思路是一样的用陀螺仪的四元数微分方程积分旋转再用加速度计的测量值构建误差函数通过梯度下降修正四元数。Madgwick算法的AHRS实现只有不到200行C代码在STM32F4上跑得飞快。我建议在把本文的卡尔曼滤波跑通之后花点时间把Madgwick的代码也移植过来配合HAL库读取的原始数据直接就能输出无万向锁问题的四元数姿态。很多飞控开源项目里的姿态解算都是这么做的。8.3 移植一套Madgwick姿态解算的具体步骤移植Madgwick算法到HAL库工程核心步骤就三步第一步把MadgwickAHRS.c和MadgwickAHRS.h拷进工程里面有两个关键函数MadgwickAHRSupdate带磁力计版本和MadgwickAHRSupdateIMU不带磁力计版本。MPU6050没有磁力计用后者。第二步在MPU6050_Read_All之后把加速度和角速度单位换算成算法要求的格式。Madgwick算法要求加速度单位是g角速度单位是rad/s而MPU6050读出来的是°/s记得乘以0.0174533。#define DEG2RAD 0.0174533f MadgwickAHRSupdateIMU( mpu.Gx * DEG2RAD, mpu.Gy * DEG2RAD, mpu.Gz * DEG2RAD, mpu.Ax, mpu.Ay, mpu.Az );第三步从四元数提取欧拉角公式是roll atan2f(2.0f * (q0 * q1 q2 * q3), 1.0f - 2.0f * (q1 * q1 q2 * q2)) * 180.0f / PI; pitch asinf(2.0f * (q0 * q2 - q3 * q1)) * 180.0f / PI; yaw atan2f(2.0f * (q0 * q3 q1 * q2), 1.0f - 2.0f * (q2 * q2 q3 * q3)) * 180.0f / PI;Madgwick算法里的beta参数默认0.1就相当于卡尔曼的Q/R比值beta越大越信任加速度计响应快但噪声大beta越小越平滑但滞后明显。实际调的时候先放0.1再看波形微调。9. 写在实验台边有一套稳定的六轴姿态解算方案对后面做平衡车、四轴、云台、体感遥控器都有直接的帮助。我自己前后在三个不同的项目里用过这套MPU6050HAL库卡尔曼滤波的方案每次重新拾起来总能在某个环节发现可以优化的点——要么是校准流程更合理了要么是滤波参数更适应具体机械特性了。这套组合的代码量不大但每个细节都值得反复推敲。最后再分享一个小技巧调试MPU6050的时候别着急写完整算法。先把原始数据用串口打出来放到VOFA画波形确认每一个轴的极性、方向都和你预想的一致再往上叠滤波算法。我见过太多同行在姿态解算阶段调试半天最后发现是传感器某一轴的原始数据正负号反了——那种挫败感真的特别伤。数据极性对了后续不管是卡尔曼滤波还是转入四元数解算都会顺畅很多。