STM32两轮自平衡车：卡尔曼滤波与PID算法实战-嵌云网-嵌入式AI开发资源站

STM32两轮自平衡车：卡尔曼滤波与PID算法实战

胡大渔

1. 项目概述

两轮自平衡车作为经典的嵌入式控制实践项目，一直是电子工程师和创客们验证控制算法的理想平台。这个基于STM32F103C8的项目，通过卡尔曼滤波和PID算法的组合实现，展示了如何用低成本硬件构建稳定可靠的自平衡系统。

我在2018年第一次尝试制作平衡车时，花了整整两周时间才让车体勉强站立3秒钟。后来通过不断优化算法参数和硬件布局，最终实现了超过5分钟的稳定平衡。这个过程中积累的经验教训，正是我想通过本文分享的核心内容。

2. 硬件设计与选型

2.1 主控芯片选择

STM32F103C8T6作为主控芯片有几个明显优势：

72MHz主频足够处理传感器数据和运行控制算法
内置的硬件浮点运算单元大幅提升计算效率
丰富的外设接口（I2C、SPI、PWM等）完美适配项目需求
成本仅10元左右，性价比极高

提示：购买时注意区分正版和山寨芯片，我曾遇到过山寨芯片的ADC精度不足导致平衡失败的情况。

2.2 传感器模块配置

MPU6050是最常用的姿态传感器，包含三轴加速度计和三轴陀螺仪：

加速度计测量重力方向，但动态响应慢
陀螺仪测量角速度，但存在漂移问题
两者数据融合才能得到准确的姿态角

接线建议：

code复制VCC → 3.3V
GND → GND
SCL → PB6
SDA → PB7
INT → PB5(用于数据就绪中断)

2.3 电机驱动方案

TB6612FNG电机驱动芯片相比传统的L298N：

效率更高（可达95%）
体积更小
支持1.2A持续电流（峰值3.2A）
内置过热保护和低压检测

电机选型建议：

减速电机（建议减速比1:30-1:50）
编码器分辨率不低于12CPR
工作电压与电池匹配（常用7.4V锂电池）

3. 软件算法实现

3.1 卡尔曼滤波设计

卡尔曼滤波的核心是预测-更新两个阶段：

c复制// 预测阶段
x = F * x + B * u;
P = F * P * F' + Q;

// 更新阶段
y = z - H * x;
S = H * P * H' + R;
K = P * H' / S;
x = x + K * y;
P = (I - K * H) * P;

参数调优经验：

Q矩阵（过程噪声）初始值建议设为0.001
R矩阵（测量噪声）初始值建议设为0.1
实际调试时先固定R，逐步调整Q直到响应速度合适

3.2 PID控制器实现

位置式PID算法实现：

c复制typedef struct {
    float Kp, Ki, Kd;
    float integral;
    float prev_error;
} PID_Controller;

float PID_Update(PID_Controller* pid, float error, float dt) {
    pid->integral += error * dt;
    float derivative = (error - pid->prev_error) / dt;
    pid->prev_error = error;
    return pid->Kp * error + pid->Ki * pid->integral + pid->Kd * derivative;
}

参数整定步骤：

先将Ki和Kd设为0，逐步增大Kp直到车体开始振荡
取振荡时Kp值的60%作为基准
逐步增加Kd抑制振荡
最后加入少量Ki消除静差

3.3 传感器数据处理

MPU6050数据读取流程：

c复制void MPU6050_Read(float* accel, float* gyro) {
    uint8_t buf[14];
    HAL_I2C_Mem_Read(&hi2c1, MPU6050_ADDR, ACCEL_XOUT_H, 1, buf, 14, 100);
    
    accel[0] = (int16_t)(buf[0]<<8 | buf[1]) / 16384.0f;
    accel[1] = (int16_t)(buf[2]<<8 | buf[3]) / 16384.0f;
    accel[2] = (int16_t)(buf[4]<<8 | buf[5]) / 16384.0f;
    
    gyro[0] = (int16_t)(buf[8]<<8 | buf[9]) / 131.0f;
    gyro[1] = (int16_t)(buf[10]<<8 | buf[11]) / 131.0f;
    gyro[2] = (int16_t)(buf[12]<<8 | buf[13]) / 131.0f;
}

注意：原始数据需要校准偏移量，建议上电静止时自动校准30秒。

4. 系统整合与调试

4.1 控制周期优化

通过定时器中断实现精确的5ms控制周期：

c复制void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
    if(htim == &htim3) { // TIM3配置为5ms中断
        static uint32_t cnt = 0;
        if(++cnt >= 200) { // 每1秒输出调试信息
            cnt = 0;
            printf("Angle: %.2f\tOutput: %.2f\n", current_angle, motor_output);
        }
        
        Sensor_Update();
        Balance_Control();
        Motor_Output();
    }
}

4.2 电源管理设计

常见问题及解决方案：

问题现象	可能原因	解决方法
运行时突然重启	电池电压骤降	增加大容量电容(1000μF以上)
电机响应迟钝	电源功率不足	改用放电倍率更高的电池
传感器数据异常	电源噪声干扰	增加LC滤波电路

4.3 机械结构要点

车体设计经验：

重心高度控制在轮轴上方10-15cm
电机轴与轮子同心度误差<0.5mm
使用铝合金框架减轻重量
电池尽量安装在靠近轮轴的位置

5. 进阶优化方向

5.1 无线调试接口

添加HC-05蓝牙模块实现参数实时调整：

c复制void USART2_IRQHandler(void) {
    if(USART2->SR & USART_SR_RXNE) {
        char c = USART2->DR;
        if(c == 'p') pid.Kp += 0.1;
        else if(c == 'P') pid.Kp -= 0.1;
        // 类似处理其他参数...
    }
}

5.2 运动控制扩展

实现前进/后退控制的基本思路：

c复制float target_speed = 0; // 通过遥控器设置

void Speed_Control(float dt) {
    static float distance = 0;
    float speed = encoder_diff / dt;
    float speed_error = target_speed - speed;
    distance += speed_error * dt;
    
    float output = speed_pid.Kp * speed_error 
                 + speed_pid.Ki * distance
                 + speed_pid.Kd * (speed_error - speed_pid.prev_error) / dt;
    
    // 将输出叠加到平衡控制输出上
    balance_angle_offset = output * 0.1f; // 系数需要实验确定
}

5.3 性能优化技巧

提升运行效率的方法：

使用查表法替代三角函数计算
将卡尔曼滤波中的矩阵运算改为展开形式
启用STM32的硬件FPU加速浮点运算
合理使用DMA传输传感器数据

6. 常见问题排查

我在调试过程中遇到的典型问题及解决方法：

车体向一侧倾斜
- 检查MPU6050安装是否水平
- 重新校准加速度计零偏
- 检查电机输出是否对称
高频振荡无法消除
- 降低Kp增益
- 增加Kd增益
- 检查机械结构是否松动
响应延迟明显
- 检查控制周期是否稳定
- 降低卡尔曼滤波的Q参数
- 检查电机供电是否充足
静止时缓慢漂移
- 适当增加Ki增益
- 检查陀螺仪零偏是否准确
- 确保车体机械对称性

这个项目最关键的收获是理解了理论算法与实际实现的差距。书本上的PID公式可能只有三行，但要让一个物理系统真正稳定工作，需要考虑传感器噪声、机械延迟、电源波动等无数细节。调试过程中最重要的工具不是示波器，而是耐心——有时调整一个参数后需要观察十几分钟才能确定效果。