【免费开源】STM32G431九轴姿态传感器IMU融合卡尔曼滤波四元数解算开源项目分享
一、项目背景
九轴 IMU（加速度计+陀螺仪+磁力计）是无人机、平衡车、AR/VR、运动捕捉等设备的核心传感器。本项目"基于 STM32G431 的九轴姿态传感器"使用主流的 ICM20948 / MPU9250 等芯片，配合 STM32G431 的高性能 Cortex-M4 + FPU，完整实现：传感器原始数据采集、三轴校准、Mahony/EKF 姿态融合算法、四元数解算与欧拉角输出，以及 USB-CDC 上位机姿态显示。
二、项目流程图
三、硬件接线
| ICM20948 | STM32G431 |
|---|---|
| SCL/SCK | PB6 |
| SDA/SDI | PB7 |
| INT | PA0 |
| nCS | PA4 |
四、核心代码：Mahony 互补滤波
#include<math.h>typedefstruct{floatw,x,y,z;}Quat;staticQuat q={1,0,0,0};staticfloatKp=2.0f,Ki=0.005f;staticfloatex=0,ey=0,ez=0;voidMahony_Update(floatgx,floatgy,floatgz,floatax,floatay,floataz,floatdt){floatn=sqrtf(ax*ax+ay*ay+az*az);if(n<1e-6f)return;ax/=n;ay/=n;az/=n;floatvx=2*(q.x*q.z-q.w*q.y);floatvy=2*(q.w*q.x+q.y*q.z);floatvz=q.w*q.w-q.x*q.x-q.y*q.y+q.z*q.z;floatex0=ay*vz-az*vy;floatey0=az*vx-ax*vz;floatez0=ax*vy-ay*vx;ex+=ex0*dt;ey+=ey0*dt;ez+=ez0*dt;gx+=Kp*ex0+Ki*ex;gy+=Kp*ey0+Ki*ey;gz+=Kp*ez0+Ki*ez;floatdx=0.5f*(-q.x*gx-q.y*gy-q.z*gz)*dt;floatdy=0.5f*(q.w*gx+q.y*gz-q.z*gy)*dt;floatdz=0.5f*(q.w*gy-q.x*gz+q.z*gx)*dt;floatdw=0.5f*(q.w*gz+q.x*gy-q.y*gx)*dt;q.w+=dx;q.x+=dy;q.y+=dz;q.z+=dw;n=sqrtf(q.w*q.w+q.x*q.x+q.y*q.y+q.z*q.z);q.w/=n;q.x/=n;q.y/=n;q.z/=n;}voidQuat2Euler(float*r,float*p,float*y){*r=atan2f(2*(q.w*q.x+q.y*q.z),1-2*(q.x*q.x+q.y*q.y))*57.2958f;*p=asinf(2*(q.w*q.y-q.z*q.x))*57.2958f;*y=atan2f(2*(q.w*q.z+q.x*q.y),1-2*(q.y*q.y+q.z*q.z))*57.2958f;}五、关键技术点
5.1 G4 高速时钟
G431 主频 170MHz，FPU 单精度浮点运算速度极快，1kHz 姿态融合 CPU 占用 <10%。
5.2 椭球校准
磁力计因周围铁磁干扰需要做椭球拟合，把数据从椭球归一化到单位球，否则航向角误差会非常大。
5.3 时间戳精度
建议使用 TIM2 32 位定时器输出 1us 时间戳，姿态融合 dt 取真实时间，而非简单设为 0.001s。
六、应用场景
- 四轴飞行器/穿越机姿态控制；
- 平衡车直立环；
- AR/VR 头盔姿态追踪；
- 工业机械臂末端姿态校准；
- 运动手环。
七、调试经验
- 加速度零偏在常温下会有 ±50mg 偏差，开机要做静态校准；
- Yaw 角必须依赖磁力计，否则会无界漂移；
- 板子安装方向必须明确，否则解算结果会反向；
- 使用 USB-CDC 发送数据，比 UART 快，方便上位机三维可视化。
八、总结
本项目代码精炼但功能完整，基于 STM32G431 的高性能为复杂的 IMU 融合提供了优秀的算力基础。完整源码、上位机姿态可视化软件、磁力计校准工具均已打包，是学习 IMU 融合算法的最佳实战项目。
