STM32自平衡小车PID控制与MPU6050数据融合实战-嵌云网-嵌入式AI开发资源站

STM32自平衡小车PID控制与MPU6050数据融合实战

老铁爱金衫

1. 项目概述

去年夏天，我花了整整一个月时间折腾这个STM32自平衡小车项目。从最初的硬件选型到最后的PID参数调试，踩了不少坑，也积累了不少实战经验。今天这篇主要分享软件层面的实现细节，特别是那些在官方文档里找不到的实用技巧。

这个平衡小车的核心思想其实很简单：通过MPU6050获取姿态数据，用PID算法计算出电机控制量，让小车在各种扰动下都能保持直立。但真正做起来，你会发现每个环节都有不少门道。比如MPU6050的数据怎么处理才稳定？PID参数怎么调才能既快速响应又不振荡？这些都是我接下来要详细讲解的内容。

2. 开发环境搭建

2.1 工具链选择

我使用的是Keil MDK v5作为主要开发环境，配合STM32CubeMX进行外设初始化。这里有个小技巧：CubeMX生成的代码会覆盖用户自定义部分，所以我通常只用它生成初始化代码，然后手动移植到主工程。

建议在CubeMX生成代码时勾选"Generate peripheral initialization as a pair of .c/.h files"选项，这样外设初始化代码会单独生成，方便移植。

2.2 工程配置要点

时钟配置：STM32F103C8T6最高支持72MHz主频，确保系统时钟正确配置
调试接口：建议启用SWD接口，方便在线调试
浮点运算：由于需要大量浮点计算，建议在工程设置中启用"Use Single Precision"选项

c复制// 系统时钟配置示例（在system_stm32f1xx.c中）
#define SYSCLK_FREQ_72MHz 72000000

3. 电机驱动实现

3.1 电机控制逻辑

电机驱动主要涉及PWM生成和方向控制。我使用的是TB6612电机驱动模块，它的控制逻辑非常直观：

c复制void Motor_Set(int16_t Speed, int16_t Speed_R) {
    // 左电机控制
    if(Speed>0){
        HAL_GPIO_WritePin(AIN1_GPIO_Port,AIN1_Pin,GPIO_PIN_SET);
        HAL_GPIO_WritePin(AIN2_GPIO_Port,AIN2_Pin,GPIO_PIN_RESET);
    } else {
        HAL_GPIO_WritePin(AIN1_GPIO_Port,AIN1_Pin,GPIO_PIN_RESET);
        HAL_GPIO_WritePin(AIN2_GPIO_Port,AIN2_Pin,GPIO_PIN_SET);
    }
    __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_3,abs(Speed));
    
    // 右电机控制（方向相反）
    if(Speed_R>0){
        HAL_GPIO_WritePin(BIN1_GPIO_Port,BIN1_Pin,GPIO_PIN_RESET);
        HAL_GPIO_WritePin(BIN2_GPIO_Port,BIN2_Pin,GPIO_PIN_SET);
    } else {
        HAL_GPIO_WritePin(BIN1_GPIO_Port,BIN1_Pin,GPIO_PIN_SET);
        HAL_GPIO_WritePin(BIN2_GPIO_Port,BIN2_Pin,GPIO_PIN_RESET);
    }
    __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_4,abs(Speed_R));
}

注意：不同电机的死区特性可能不同。我的MG513电机死区约3%，实测影响不大就没做补偿。如果你的电机死区较大（>5%），建议添加死区补偿。

3.2 安全保护机制

为了防止小车倾倒时电机持续运转，我添加了倾角保护：

c复制uint8_t Turn_Off(float angle) {
    if(angle<-30 || angle>30) {  // 倾角超过30度关闭电机
        Motor_Set(0,0);
        return 1;  // 异常状态
    }
    return 0;  // 正常状态
}

4. 传感器数据处理

4.1 MPU6050配置

MPU6050的配置有几个关键点：

I2C地址：AO脚接GND时地址为0xD0，接VCC时为0xD1
采样率：我设置为1kHz（SMPLRT_DIV=0x00）
滤波器：DLPF_CFG配置为0x03，平衡滤波效果和延迟

c复制void MPU6050_Init(void) {
    MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x01);  // 取消睡眠模式
    MPU6050_WriteReg(MPU6050_SMPLRT_DIV, 0x00);  // 1kHz采样率
    MPU6050_WriteReg(MPU6050_CONFIG, 0x03);      // DLPF配置
    MPU6050_WriteReg(MPU6050_GYRO_CONFIG, 0x18); // ±2000°/s量程
    MPU6050_WriteReg(MPU6050_ACCEL_CONFIG, 0x18);// ±16g量程
}

4.2 数据融合算法

使用互补滤波融合加速度计和陀螺仪数据：

c复制// 在1ms定时中断中执行
MPU6050_GetData(&AX, &AY, &AZ, &GX, &GY, &GZ);
GY += 20;  // 陀螺仪零偏校准

// 加速度计计算角度（瞬时值）
AngleAcc = -atan2(AX, AZ) / 3.14159 * 180;

// 陀螺仪积分角度（累积值）
AngleGyro = Angle_pitch + GY / 32768.0 * 2000 * 0.001;  // 0.001s时间间隔

// 互补滤波融合
float Alpha = 0.02;  // 滤波系数
Angle_pitch = Alpha * AngleAcc + (1-Alpha) * AngleGyro;

调试技巧：先用串口输出原始传感器数据，确保数据合理后再进行融合计算。常见问题包括I2C通信不稳定、传感器安装不水平等。

5. PID控制实现

5.1 直立环（PD控制）

直立环负责保持小车平衡，采用PD控制：

c复制int PD_value(float pitch, float Gy, float target) {
    int output = SKp*(pitch-target) + SKd*Gy;
    return I_xianfu(output, 80);  // 输出限幅
}

参数调试经验：

先调P参数，让小车能够响应倾斜但不振荡
再调D参数，抑制振荡并提高稳定性
我的参考值：SKp=3.92, SKd=0.02

5.2 速度环（PI控制）

速度环通过编码器反馈实现速度控制：

c复制int velocity_PI_value(float velocity) {
    static float filt_velocity = 0, last_filt_velocity = 0;
    static float velocity_sum = 0;
    
    // 低通滤波
    float a = 0.3;
    filt_velocity = a*velocity + (1-a)*last_filt_velocity;
    
    // 积分项限幅
    velocity_sum += filt_velocity;
    velocity_sum = I_xianfu(velocity_sum, 10000);
    
    last_filt_velocity = filt_velocity;
    return VKp*filt_velocity + VKi*velocity_sum;
}

注意：积分项必须限幅，否则会导致"积分饱和"现象，影响系统响应。

5.3 转向环（PD控制）

转向环处理小车的转向控制：

c复制int Turn_PD_value(int Target_Turn, float Gz) {
    return TKp * Target_Turn + TKd * Gz;
}

6. 系统整合与调试

6.1 主控制流程

c复制while(1) {
    // 1. 读取传感器数据
    MPU6050_GetData(&AX, &AY, &AZ, &GX, &GY, &GZ);
    
    // 2. 数据融合得到当前角度
    Angle_pitch = ...;
    
    // 3. 读取编码器获取速度
    encoder_time = ...;
    
    // 4. 计算各PID环输出
    V_out = velocity_PI_value(target_speed - encoder_time);
    pwm_out = PD_value(Angle_pitch, GY, target_angle+V_out);
    T_out = Turn_PD_value(Turn_out, GZ);
    
    // 5. 综合输出
    L_out = pwm_out + T_out/2;
    R_out = pwm_out - T_out/2;
    Motor_Set(-L_out, -R_out);
}

6.2 调试技巧

分步调试：先调直立环，再加速度环，最后加转向环
参数调整：每次只调整一个参数，小幅度变化（10%-20%）
安全措施：调试时用手扶着小车，准备随时断电
可视化工具：使用蓝牙模块将数据发送到上位机观察曲线

7. 常见问题排查

7.1 MPU6050数据异常

可能原因：

I2C通信不稳定：降低I2C时钟频率（100kHz）
电源噪声：增加电源滤波电容
传感器安装不牢固：确保传感器固定牢固

7.2 电机响应迟钝

解决方法：

检查PWM频率（建议10kHz左右）
检查电机死区，必要时添加补偿
提高PID的P参数

7.3 小车持续振荡

调整策略：

适当降低P参数
增加D参数
检查机械结构是否松动

8. 项目优化方向

加入蓝牙遥控：通过手机APP实时调整PID参数
实现路径跟踪：增加红外或摄像头传感器
低功耗优化：在静止时降低采样率
改用DMP库：提高姿态解算精度（但会增加复杂度）

这个项目最让我头疼的是PID参数调试，前后花了近两周时间。后来发现一个技巧：先用手机慢动作录像观察小车振荡情况，能更直观地理解参数影响。希望这些经验对正在做类似项目的朋友有所帮助。