mpu9250九轴AHRS姿态解算程序. 包含模拟i2c驱动和加速度陀螺仪磁力计九轴数据融合算法。最近在搞一个无人机项目硬件选型时相中了MPU9250这款九轴传感器。这货集成了加速度计、陀螺仪和磁力计但要把原始数据变成可用姿态角还真得费点功夫。今天就跟大伙唠唠我的实现过程手把手带你们从I2C驱动写到姿态解算。先说硬件连接很多教程都直接调用Arduino的Wire库但实际项目中经常需要模拟I2C。下面这段GPIO模拟I2C的代码建议收藏// 模拟I2C起始信号 void I2C_Start(void) { SDA_HIGH; SCL_HIGH; delay_us(5); SDA_LOW; delay_us(5); SCL_LOW; } // 发送字节函数 void I2C_SendByte(uint8_t data) { for(int i0; i8; i) { (data 0x80) ? SDA_HIGH : SDA_LOW; SCL_HIGH; delay_us(5); SCL_LOW; data 1; } // 省略ACK检测... }这哥们干了两件事先拉高时钟线再拉低数据线生成起始信号然后用循环左移操作把数据位逐个拍出去。注意时钟线每次跳变都要留点延时实测STM32F103在72MHz主频下用5us延时刚好稳定。读取原始数据时得注意寄存器地址和字节顺序。MPU9250的加速度计和陀螺仪数据在0x3B~0x48地址段void MPU_GetData(float *acc, float *gyro) { uint8_t buf[14]; I2C_ReadBytes(MPU_ADDR, 0x3B, 14, buf); // 加速度计数据处理 acc[0] (int16_t)(buf[0]8 | buf[1]) / 16384.0f * 9.8f; acc[1] (int16_t)(buf[2]8 | buf[3]) / 16384.0f * 9.8f; acc[2] (int16_t)(buf[4]8 | buf[5]) / 16384.0f * 9.8f; // 陀螺仪数据处理 gyro[0] (int16_t)(buf[8]8 | buf[9]) / 131.0f * (M_PI/180.0f); // 类似处理其他轴... }这里有个坑点MPU9250默认量程是±2g和±250dps所以用16384LSB/g和131LSB/dps的系数转换。磁力计在AUXI2C接口上读取时要先切到Bypass模式不然会卡死。mpu9250九轴AHRS姿态解算程序. 包含模拟i2c驱动和加速度陀螺仪磁力计九轴数据融合算法。数据融合才是重头戏。这里我用了改进的互补滤波核心思路是用加速度计修正陀螺仪漂移void update_ahrs(float dt) { // 陀螺仪积分得到姿态角 roll_gyro gyro[0] * dt; // 加速度计补偿 float roll_acc atan2(accY, accZ) * RAD_TO_DEG; // 互补滤波融合 roll 0.98f * (roll gyro[0]*dt) 0.02f * roll_acc; }这个比例系数0.98和0.02需要根据实际运动状态动态调整。当加速度计数据可信度低时比如高速旋转要适当降低其权重。后来升级到四元数法时发现用Mahony算法效果更好void MahonyUpdate(float gx, float gy, float gz, float ax, float ay, float az) { float recipNorm; float halfvx, halfvy, halfvz; float halfex, halfey, halfez; // 加速度计归一化 recipNorm 1.0f / sqrt(ax*ax ay*ay az*az); ax * recipNorm; ay * recipNorm; az * recipNorm; // 计算误差 halfvx q1*q3 - q0*q2; halfvy q0*q1 q2*q3; halfvz q0*q0 - 0.5f q3*q3; // 积分误差 halfex (ay * halfvz - az * halfvy) * Ki * dt; // 类似处理其他轴... // 应用反馈 gx twoKp * halfex halfex * Ki; // 四元数更新... }这个算法的精髓在于通过PI控制器修正陀螺仪偏差Kp和Ki参数需要根据传感器噪声调整。实测在四旋翼飞行中当Kp0.5、Ki0.1时姿态角收敛最快。磁力计校准是个细致活。我写了个自动校准程序让设备绕8字旋转两分钟while(calib_cnt 2000) { MPU_GetMag(mx, my, mz); mag_max.x fmax(mag_max.x, mx); mag_min.x fmin(mag_min.x, mx); // 类似处理Y/Z轴... calib_cnt; } // 计算偏移量和比例因子 mag_offset.x (mag_max.x mag_min.x)/2.0f; mag_scale.x (mag_max.x - mag_min.x)/2.0f;校准后的数据要做椭圆拟合补偿硬铁干扰不过对于普通应用取最大最小值做线性校准已经够用。最后把四元数转成欧拉角时注意万向锁问题void get_euler(float *roll, float *pitch, float *yaw) { *roll atan2f(2.0f*(q0*q1 q2*q3), 1.0f - 2.0f*(q1*q1 q2*q2)); *pitch asinf(2.0f*(q0*q2 - q3*q1)); *yaw atan2f(2.0f*(q0*q3 q1*q2), 1.0f - 2.0f*(q2*q2 q3*q3)); }实测发现俯仰角超过±85度时横滚角开始漂移这时需要切换到四元数直接运算。姿态解算后的数据通过串口发到上位机用Python做个3D可视化界面看着姿态球实时转动的那一刻调试的苦逼都值了。