陀螺仪MPU6050、MPU6500姿态解算。软件滤波，四元素。工程是GD32F303芯片...-育师

陀螺仪MPU6050、MPU6500姿态解算。软件滤波，四元素。工程是GD32F303芯片的。 c语言文件，可以移植stm32等任意平台。带freertos系统。输出pitch，roll，yaw

最近在GD32F303上折腾MPU6050姿态解算，踩了不少坑总算把三轴姿态角给整出来了。这玩意儿说起来简单，真要自己动手从零搞起，没点耐心还真不行。先说硬件配置，MPU6050和MPU6500这俩兄弟用起来差不多，都是I2C接口，注意电源要稳，最好单独供电别跟数字电路混用。

先上个初始化代码片段：

void mpu_init(void) { i2c_write_byte(MPU6050_ADDR, MPU6050_PWR_MGMT_1, 0x80); //复位 vTaskDelay(50); i2c_write_byte(MPU6050_ADDR, MPU6050_SMPLRT_DIV, 0x07); i2c_write_byte(MPU6050_ADDR, MPU6050_CONFIG, 0x06); i2c_write_byte(MPU6050_ADDR, MPU6050_GYRO_CONFIG, 0x18); //±2000dps i2c_write_byte(MPU6050_ADDR, MPU6050_ACCEL_CONFIG, 0x10); //±8g }

这里有个细节要注意，GD32的I2C时序和STM32略有不同，如果发现通信失败，记得调整时钟频率配置。我当初就卡在这儿半天，最后发现是GD32的I2C时钟寄存器设置需要多右移一位。

姿态解算核心算法用四元数，配合互补滤波食用更佳。先上关键的结构体定义：

typedef struct { float q0, q1, q2, q3; float integralFBx, integralFBy, integralFBz; } Attitude_Quaternion;

互补滤波实现部分，这里用加速度计修正陀螺仪漂移：

void complementary_filter(IMU_Data *imu, Attitude_Quaternion *q, float dt) { // 加速度计归一化 float acc_norm = 1.0f / sqrtf(imu->ax*imu->ax + imu->ay*imu->ay + imu->az*imu->az); imu->ax *= acc_norm; imu->ay *= acc_norm; imu->az *= acc_norm; // 计算误差 float vx = 2*(q->q1*q->q3 - q->q0*q->q2); float vy = 2*(q->q0*q->q1 + q->q2*q->q3); float ez = vx*imu->ay - vy*imu->ax; // 积分反馈 q->integralFBz += Ki * ez * dt; imu->gz += q->integralFBz + Kp * ez; }

这个滤波参数Kp和Ki需要实测调整，建议从0.5开始慢慢调。有个偷懒的方法——接上调试器，看着波形调参最直观。

FreeRTOS任务拆分有讲究，建议分两个任务：一个高频任务专门读传感器数据（500Hz以上），另一个中频任务做解算（200Hz左右）。实测这样既能保证数据实时性，又不会让CPU负载太高。

最后转换欧拉角的部分要特别注意奇异点问题：

void quat_to_euler(Attitude_Quaternion *q, Euler_Angle *angle) { // roll float sinr_cosp = 2*(q->q0*q->q1 + q->q2*q->q3); float cosr_cosp = 1 - 2*(q->q1*q->q1 + q->q2*q->q2); angle->roll = atan2f(sinr_cosp, cosr_cosp); // pitch float sinp = 2*(q->q0*q->q2 - q->q3*q->q1); if(fabsf(sinp) >= 1) angle->pitch = copysignf(M_PI/2, sinp); // 处理90度奇异点 else angle->pitch = asinf(sinp); // yaw float siny_cosp = 2*(q->q0*q->q3 + q->q1*q->q2); float cosy_cosp = 1 - 2*(q->q2*q->q2 + q->q3*q->q3); angle->yaw = atan2f(siny_cosp, cosy_cosp); }

实测发现当pitch接近±90度时，roll和yaw会开始漂，这是欧拉角的固有问题。如果项目对全姿态有要求，建议直接用四元数做后续处理。

移植到其他平台时，重点关注三个地方：I2C/SPI驱动、定时器时钟源、浮点运算支持。GD32F303有硬件浮点，爽得很。要是换到没有FPU的芯片，记得把浮点运算改成定点数或者查表法。

最后吐槽下MPU6050的温漂，静止状态下yaw角每小时能飘个5-8度。解决办法是上磁力计做九轴融合，或者简单粗暴——每隔几分钟用加速度计重新校准零点。