平衡小车MPU6050姿态解算：互补滤波与卡尔曼滤波3种实现方案对比

平衡小车MPU6050姿态解算：互补滤波与卡尔曼滤波3种实现方案对比

两轮自平衡小车的核心挑战在于实时准确地估计车身倾角。作为系统的"眼睛"，MPU6050传感器提供的原始数据充满噪声和漂移，直接使用会导致PID控制失效。本文将深入解析互补滤波、Mahony滤波和卡尔曼滤波三种主流算法，通过实测数据对比其性能差异，并提供可直接移植的STM32 HAL库实现方案。

1. 姿态解算为何成为平衡小车的命门

当第一次看到两轮小车稳稳直立时，很多人会惊叹其神奇。但真正动手制作时，90%的失败案例都源于姿态估计不准。MPU6050输出的加速度计和陀螺仪数据存在本质缺陷：

• 加速度计：低频特性好但动态响应差。静止时能准确反映倾角，但小车运动时会被线性加速度污染。实测显示，即使5°的静态误差在加速瞬间可能飙升至30°以上。
• 陀螺仪：高频响应快但存在积分漂移。短时间角度变化测量精确，但10秒积分误差就可能超过10°。

// MPU6050原始数据读取示例（HAL库） HAL_I2C_Mem_Read(&hi2c1, MPU6050_ADDR, ACCEL_XOUT_H, 1, &raw_data[0], 6, 100); int16_t accel_x = (raw_data[0] << 8) | raw_data[1]; // 加速度计X轴 int16_t gyro_y = (raw_data[4] << 8) | raw_data[5]; // 陀螺仪Y轴

数据融合的黄金法则：用加速度计校正陀螺仪的低频漂移，用陀螺仪弥补加速度计的高频噪声。三种算法对此给出了不同解法：

提示：选择算法时需权衡实时性与精度。STM32F103在72MHz主频下，三种算法的单次执行时间分别为0.1ms、0.3ms和1.2ms。

2. 互补滤波：简单高效的工程解决方案

互补滤波如同"电子三明治"，将加速度计和陀螺仪数据按固定比例混合。其经典实现仅需一行代码：

float angle = 0.98 * (angle + gyro_y * dt) + 0.02 * accel_angle;

参数优化关键：

• 0.98/0.02权重：对应截止频率约5Hz。权重比需根据采样周期调整：

  α = τ/(τ + dt) \quad 其中τ=1/(2πf_c)

• 动态调参技巧：当检测到加速度计数据突变（abs(accel_angle - prev_angle) > 15°）时，临时降低其权重至0.01。

实测数据对比：

进阶改进——非线性互补滤波：

float error = accel_angle - angle; float adaptive_weight = fabs(error) < 10 ? 0.02 : 0.005; angle += (gyro_y + K * error) * dt;

3. Mahony滤波：四元数姿态解算的轻量级实现

Mahony滤波通过梯度下降法将加速度计数据作为观测基准，修正陀螺仪偏差。相比互补滤波，其优势在于：

1. 直接输出三维姿态（roll/pitch/yaw）
2. 自动估计陀螺仪零偏
3. 更适合存在剧烈运动的场景

核心算法步骤：

1. 四元数微分更新：

   q0 += 0.5 * (-q1*gx - q2*gy - q3*gz) * dt; q1 += 0.5 * ( q0*gx + q2*gz - q3*gy) * dt; // ...其余分量类似

2. 加速度计校正：

   float ex = ay*vz - az*vy; // 误差向量 float ey = az*vx - ax*vz; // 积分误差用于零偏补偿 gyro_bias_x += Ki * ex * dt; gx += Kp * ex + gyro_bias_x;

注意：Kp决定收敛速度，Ki影响稳态精度。典型值Kp=2.0,Ki=0.05。

参数整定流程：

1. 将Kp从0逐步增大，直到车身出现高频抖动
2. 加入Ki消除静态误差，但不超过Kp/20
3. 在STM32CubeMX中配置定时器触发滤波计算（建议500Hz）

4. 卡尔曼滤波：最优估计的理论实践

卡尔曼滤波将姿态估计建模为状态预测+测量校正的过程。对于单轴倾角估计，状态向量包含角度和角速度：

x_k = \begin{bmatrix} θ \\ \dot{θ} \end{bmatrix} = A \cdot x_{k-1} + B \cdot u_k + w_k

五个核心方程：

1. 状态预测：

   angle += gyro_y * dt - Q_angle * P[0][0]; P[0][0] += dt * (P[1][1] - P[0][1] - P[1][0] + Q_gyro);

2. 卡尔曼增益计算：

   float S = P[0][0] + R_angle; K[0] = P[0][0] / S;

3. 状态更新：

   angle += K[0] * (accel_angle - angle); P[0][0] -= K[0] * P[0][0];

参数矩阵初始化：

float Q_angle = 0.001; // 过程噪声协方差 float Q_gyro = 0.003; float R_angle = 0.5; // 测量噪声协方差 float P[2][2] = {{1, 0}, {0, 1}};

实测性能对比（STM32F407@168MHz）：

5. 工程实现：STM32 HAL库全流程解析

5.1 硬件配置要点

// MPU6050初始化（I2C接口） void MPU6050_Init(void) { uint8_t check = HAL_I2C_IsDeviceReady(&hi2c1, MPU6050_ADDR, 5, 100); if (check == HAL_OK) { HAL_I2C_Mem_Write(&hi2c1, MPU6050_ADDR, PWR_MGMT_1, 1, 0x00, 1, 100); // 唤醒 HAL_I2C_Mem_Write(&hi2c1, MPU6050_ADDR, ACCEL_CONFIG, 1, 0x08, 1, 100); // ±4g HAL_I2C_Mem_Write(&hi2c1, MPU6050_ADDR, GYRO_CONFIG, 1, 0x18, 1, 100); // ±2000°/s } }

5.2 定时器中断服务例程

// 1kHz中断服务函数 void TIM2_IRQHandler(void) { static uint8_t sample_count = 0; if (__HAL_TIM_GET_FLAG(&htim2, TIM_FLAG_UPDATE)) { __HAL_TIM_CLEAR_FLAG(&htim2, TIM_FLAG_UPDATE); // 每1ms读取传感器 MPU6050_Read_All(); // 每5ms执行滤波 if (++sample_count >= 5) { sample_count = 0; angle = Kalman_Filter(accel_angle, gyro_y); } // PID计算与电机控制 motor_pwm = PID_Calculate(angle); __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, motor_pwm); } }

5.3 三种算法的C语言实现对比

互补滤波完整实现：

float Complementary_Filter(float acc_angle, float gyro_rate) { static float angle = 0; angle = 0.98 * (angle + gyro_rate * DT) + 0.02 * acc_angle; return angle; }

Mahony滤波关键代码：

void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az) { float recipNorm; float vx, vy, vz; float ex, ey, ez; // 归一化加速度计 recipNorm = 1.0 / sqrt(ax*ax + ay*ay + az*az); ax *= recipNorm; ay *= recipNorm; az *= recipNorm; // 计算误差向量 vx = 2*(q1*q3 - q0*q2); vy = 2*(q0*q1 + q2*q3); vz = q0*q0 - q1*q1 - q2*q2 + q3*q3; ex = (ay*vz - az*vy); ey = (az*vx - ax*vz); // 积分误差 gyro_bias_x += ex * Ki * DT; gx += Kp * ex + gyro_bias_x; // ...更新四元数 }

卡尔曼滤波结构体：

typedef struct { float Q_angle; // 过程噪声协方差 float Q_gyro; // 角速度噪声协方差 float R_angle; // 测量噪声协方差 float P[2][2]; // 误差协方差矩阵 float K[2]; // 卡尔曼增益 float angle; // 最优估计角度 } Kalman_Filter; float Kalman_Update(Kalman_Filter* kf, float new_angle, float new_gyro) { // 预测步骤 kf->angle += (new_gyro - kf->angle) * DT; kf->P[0][0] += DT * (kf->P[1][1] - kf->P[0][1] - kf->P[1][0] + kf->Q_angle); // 更新步骤 float S = kf->P[0][0] + kf->R_angle; kf->K[0] = kf->P[0][0] / S; kf->angle += kf->K[0] * (new_angle - kf->angle); kf->P[0][0] -= kf->K[0] * kf->P[0][0]; return kf->angle; }

6. 场景化测试与参数调优指南

6.1 静态性能测试

将小车固定在倾斜10°的支架上，记录30秒内角度波动：

6.2 动态响应测试

施加5°阶跃干扰，测量响应时间：

6.3 抗冲击测试

用橡皮锤敲击车身，记录恢复时间：

调优建议：

1. 互补滤波：增大权重比（如0.99/0.01）可提升动态性能，但会降低静态精度
2. Mahony滤波：Kp决定收敛速度，每增加0.5可缩短20%响应时间
3. 卡尔曼滤波：R_angle越小对加速度计信任度越高，但抗干扰能力下降

7. 深入原理：从传感器特性到算法本质

7.1 MPU6050噪声特性分析

通过采集静止状态下2000组数据，得到传感器噪声分布：

7.2 算法频率响应对比

通过扫频测试绘制伯德图：

• 互补滤波：明显截止频率（如5Hz），高于此频率时陀螺仪主导
• Mahony滤波：平滑过渡带，无明确截止频率
• 卡尔曼滤波：最优估计使得各频段误差最小化

7.3 计算复杂度拆解

在STM32F103上实测单次计算指令周期：

8. 进阶优化：融合编码器与多传感器数据

当小车长时间运行时，单纯依赖IMU会导致累积误差。引入编码器信息可显著提升性能：

// 速度-角度观测融合 void Sensor_Fusion(float* angle, float encoder_speed) { static float drift_comp = 0; // 编码器推算的角度变化 float encoder_delta = 0.01 * encoder_speed * DT; // 动态调整陀螺仪零偏 drift_comp += 0.001 * (*angle - encoder_delta); *angle = 0.95 * (*angle + gyro_y * DT - drift_comp) + 0.05 * encoder_delta; }

多传感器融合效果对比：

9. 常见问题与调试技巧

问题1：小车静止时角度抖动

• 检查电源噪声（示波器观察3.3V纹波应<50mV）
• 降低加速度计权重（互补滤波）或增大R_angle（卡尔曼）

问题2：快速运动时角度滞后

• 提高采样频率至500Hz以上
• 在Mahony滤波中增大Kp（每次增加0.5）

问题3：转向时角度漂移

• 校准陀螺仪零偏（静止时采集1000点取平均）
• 在卡尔曼滤波中增加Q_gyro（提升过程噪声）

示波器调试建议：

1. 同时捕获accel_angle、gyro_integral和fused_angle
2. 观察阶跃响应是否过冲
3. 检查各传感器数据是否同步

10. 实战案例：从零构建平衡小车

硬件清单：

• STM32F103C8T6最小系统板
• MPU6050模块（带DMP功能）
• TB6612电机驱动
• 12V减速电机（带编码器）
• 2000mAh锂电池

软件架构：

/main ├── /Drivers │ ├── mpu6050.c # 传感器驱动 │ └── pid.c # 三环PID控制 ├── /Algorithm │ ├── kalman.c # 卡尔曼滤波 │ └── mahony.c # 姿态解算 └── /Application ├── control.c # 主控制循环 └── monitor.c # 状态监控

初始化序列：

void Main_Init(void) { HAL_Init(); SystemClock_Config(); MX_GPIO_Init(); MX_TIM1_Init(); // PWM输出 MX_I2C1_Init(); MX_USART1_UART_Init(); MPU6050_Init(); Kalman_Init(&kalman_filter); PID_Init(&angle_pid, 80, 0, 2); // Kp=80, Ki=0, Kd=2 HAL_TIM_Base_Start_IT(&htim2); // 启动1kHz定时器 }