整套可直接移植 C 代码适用STM32 / ESP32 / 普通32位MCUIMU6轴加速度计陀螺仪如MPU6050、ICM42688流程原始数据读取 → 零偏校准 → 一阶低通预处理 → Mahony AHRS融合输出四元数 → 四元数转欧拉角坐标系约定行业通用X右Y前Z下roll(绕X)横滚pitch(绕Y)俯仰yaw(绕Z)偏航⚠️6轴IMU yaw会漂移想要稳定航向必须加磁力计扩展9轴1. 头文件 imu_ahrs.h#ifndef__IMU_AHRS_H#define__IMU_AHRS_H#includestdint.h#includemath.h#definePI3.141592653589793f#defineDEG2RAD(PI/180.0f)#defineRAD2DEG(180.0f/PI)// IMU采样周期【根据你的实际采样频率修改】// 例200Hz采样 → dt 0.005f#defineIMU_DT0.005f// Mahony参数#defineKp2.0f// 比例增益调大修正更快噪声变大#defineKi0.005f// 积分增益抑制稳态漂移// IMU数据结构体typedefstruct{// 原始ADC输出int16_tax_raw,ay_raw,az_raw;int16_tgx_raw,gy_raw,gz_raw;// 物理量floatax,ay,az;// gfloatgx,gy,gz;// rad/s// 零偏静止校准得到floatgx_bias,gy_bias,gz_bias;// 姿态四元数floatq0,q1,q2,q3;// 欧拉角 单位度floatroll,pitch,yaw;}IMU_t;externIMU_t imu;// 一阶低通滤波结构体typedefstruct{floatalpha;floatout;}LPF_t;// 函数声明voidIMU_Init(void);voidIMU_Gyro_Calibrate(uint16_tsample_cnt);voidIMU_RawDataProcess(void);floatLPF_Update(LPF_t*lpf,floatraw);voidMahony_AHRS_Update(floatax,floatay,floataz,floatgx,floatgy,floatgz);voidQuaternion_To_Euler(void);#endif2. 源文件 imu_ahrs.c#includeimu_ahrs.hIMU_t imu;// 低通滤波器实例加速度使用staticLPF_t lpf_ax{.alpha0.2f,.out0};staticLPF_t lpf_ay{.alpha0.2f,.out0};staticLPF_t lpf_az{.alpha0.2f,.out0};// Mahony内部积分误差staticfloatintegral_fb_x0,integral_fb_y0,integral_fb_z0;/** * brief 一阶指数低通滤波 * param lpf 滤波器实例 * param raw 原始输入 * retval 滤波后数值 */floatLPF_Update(LPF_t*lpf,floatraw){lpf-outlpf-alpha*raw(1.0f-lpf-alpha)*lpf-out;returnlpf-out;}/** * brief IMU初始化四元数初始姿态水平 */voidIMU_Init(void){imu.q01.0f;imu.q10.0f;imu.q20.0f;imu.q30.0f;imu.gx_bias0;imu.gy_bias0;imu.gz_bias0;}/** * brief 陀螺仪零偏校准 * param sample_cnt 采样点数建议500~1000 * 【使用条件IMU水平静止不要晃动】 */voidIMU_Gyro_Calibrate(uint16_tsample_cnt){int32_tsum_gx0,sum_gy0,sum_gz0;uint16_ti;for(i0;isample_cnt;i){// 这里需要你自行填充读取IMU原始gx_raw,gy_raw,gz_raw// Read_IMU_Raw(imu.ax_raw, imu.ay_raw, imu.az_raw,// imu.gx_raw, imu.gy_raw, imu.gz_raw);sum_gximu.gx_raw;sum_gyimu.gy_raw;sum_gzimu.gz_raw;}imu.gx_bias(float)sum_gx/sample_cnt;imu.gy_bias(float)sum_gy/sample_cnt;imu.gz_bias(float)sum_gz/sample_cnt;}/** * brief 原始数据转换、去零偏、低通预处理 * 【重要】根据你的IMU量程修改系数示例MPU6050 * accel ±2g scale 2.0f / 32768.0f * gyro ±250°/s scale 250.0f / 32768.0f */voidIMU_RawDataProcess(void){// 加速度原始值 - gfloataccel_scale2.0f/32768.0f;imu.aximu.ax_raw*accel_scale;imu.ayimu.ay_raw*accel_scale;imu.azimu.az_raw*accel_scale;// 加速度低通滤波imu.axLPF_Update(lpf_ax,imu.ax);imu.ayLPF_Update(lpf_ay,imu.ay);imu.azLPF_Update(lpf_az,imu.az);// 陀螺仪原始值 - °/s减去零偏再转为 rad/sfloatgyro_scale250.0f/32768.0f;floatgx_dps(imu.gx_raw-imu.gx_bias)*gyro_scale;floatgy_dps(imu.gy_raw-imu.gy_bias)*gyro_scale;floatgz_dps(imu.gz_raw-imu.gz_bias)*gyro_scale;imu.gxgx_dps*DEG2RAD;imu.gygy_dps*DEG2RAD;imu.gzgz_dps*DEG2RAD;}/** * brief Mahony AHRS 6轴融合算法 * ax,ay,az 单位ggx,gy,gz 单位rad/s */voidMahony_AHRS_Update(floatax,floatay,floataz,floatgx,floatgy,floatgz){floatq0imu.q0;floatq1imu.q1;floatq2imu.q2;floatq3imu.q3;floatnorm;floatvx,vy,vz;floatex,ey,ez;// 归一化加速度计normsqrtf(ax*axay*ayaz*az);if(norm0.0f)return;ax/norm;ay/norm;az/norm;// 预估重力方向由当前四元数推算vx2.0f*(q1*q3-q0*q2);vy2.0f*(q0*q1q2*q3);vzq0*q0-q1*q1-q2*q2q3*q3;// 误差 叉乘测量重力 叉乘 预估重力exay*vz-az*vy;eyaz*vx-ax*vz;ezax*vy-ay*vx;// 积分反馈integral_fb_xex*Ki*IMU_DT;integral_fb_yey*Ki*IMU_DT;integral_fb_zez*Ki*IMU_DT;// 修正角速度gxKp*exintegral_fb_x;gyKp*eyintegral_fb_y;gzKp*ezintegral_fb_z;// 四元数微分更新floatq0_dot0.5f*(-q1*gx-q2*gy-q3*gz);floatq1_dot0.5f*(q0*gxq2*gz-q3*gy);floatq2_dot0.5f*(q0*gy-q1*gzq3*gx);floatq3_dot0.5f*(q0*gzq1*gy-q2*gx);q0q0_dot*IMU_DT;q1q1_dot*IMU_DT;q2q2_dot*IMU_DT;q3q3_dot*IMU_DT;// 四元数归一化必须防止发散normsqrtf(q0*q0q1*q1q2*q2q3*q3);imu.q0q0/norm;imu.q1q1/norm;imu.q2q2/norm;imu.q3q3/norm;}/** * brief 四元数转欧拉角 roll pitch yaw角度 */voidQuaternion_To_Euler(void){floatq0imu.q0;floatq1imu.q1;floatq2imu.q2;floatq3imu.q3;// Roll X轴imu.rollatan2f(2.0f*(q0*q1q2*q3),q0*q0-q1*q1-q2*q2q3*q3)*RAD2DEG;// Pitch Y轴imu.pitch-asinf(2.0f*(q1*q3-q0*q2))*RAD2DEG;// Yaw Z轴 【6轴会漂移】imu.yawatan2f(2.0f*(q0*q3q1*q2),q0*q0q1*q1-q2*q2-q3*q3)*RAD2DEG;}3. 主循环调用示例main.c / 定时器中断⭐强烈建议放在固定周期定时器中断执行保证dt稳定#includeimu_ahrs.h// 外部函数需要你自己实现I2C/SPI读取IMU原始数据externvoidRead_IMU_Raw(int16_t*ax,int16_t*ay,int16_t*az,int16_t*gx,int16_t*gy,int16_t*gz);intmain(void){// 硬件初始化I2C/SPI、IMU寄存器配置省略IMU_Init();// 【校准步骤上电后保持IMU静止水平运行一次】IMU_Gyro_Calibrate(800);while(1){// 1.读取原始数据Read_IMU_Raw(imu.ax_raw,imu.ay_raw,imu.az_raw,imu.gx_raw,imu.gy_raw,imu.gz_raw);// 2.原始数据处理去零偏单位转换低通滤波IMU_RawDataProcess();// 3.Mahony融合得到四元数Mahony_AHRS_Update(imu.ax,imu.ay,imu.az,imu.gx,imu.gy,imu.gz);// 4.四元数转欧拉角Quaternion_To_Euler();// 此时可以使用 imu.roll imu.pitch imu.yaw// printf(roll:%.2f pitch:%.2f yaw:%.2f\r\n,imu.roll,imu.pitch,imu.yaw);// 严格保证采样周期 和 IMU_DT匹配// HAL_Delay(5); // dt0.005 200Hz}}重点修改说明必看否则数据错乱IMU量程系数代码内是MPU6050 ±2g / ±250dps如果你用ICM42688、BMI160修改accel_scale、gyro_scaleRead_IMU_Raw()函数需要你自行实现I2C/SPI读取寄存器原始int16这部分和硬件驱动相关IMU_DT采样频率200Hz → 0.005100Hz →0.01必须和实际一致坐标轴方向如果角度反向、左右颠倒修改原始数据正负号、调换ax/ay顺序参数调试指南Kp越大加速度修正越强运动时抖动变大静止姿态收敛更快推荐区间1.0 ~ 5.0Ki积分项抑制长时间静态漂移太大容易震荡推荐0 ~ 0.01低通alpha加速度噪声大 → 减小到0.1想要响应快 →提高到0.3常见问题角度震荡降低Kp或者减小低通alpha静止缓慢漂移适度增大Ki运动时角度乱晃Kp不要太大剧烈运动加速度计不可信yaw持续漂移6轴物理限制只能加磁力计升级9轴如果你告诉我MCU型号、IMU型号、通信方式I2C/SPI、采样频率我可以帮你补上完整IMU读取驱动直接编译运行。另外如果你想要 Madgwick 版本代码我也可以一并给出。
)
