基于STM32的平衡机器人PID控制系统设计

一、系统概述与核心原理

1. 系统定位

基于STM32的两轮自平衡机器人（Balance Bot）是自动控制理论的经典实践平台。系统通过MPU6050陀螺仪实时监测车身倾角，利用PID算法计算出电机补偿量，驱动直流电机保持车身直立不倒，并实现前进、后退与转向控制。

2. 控制原理

• 直立控制：基于角度闭环（PD控制），当车身前倾时，电机向前加速；后仰时向后加速。
• 速度控制：基于编码器闭环（PI控制），抵消直立控制带来的速度波动，实现稳态精度。
• 方向控制：差速转向，左右轮速度差实现转向。

3. 核心硬件选型

模块	型号	关键参数	接口方式
主控	STM32F103C8T6	72MHz Cortex-M3，3个通用定时器，2路ADC	核心控制器
姿态传感器	MPU6050	3轴陀螺仪+3轴加速度，±2000°/s，I2C接口	I2C1 (PB6/PB7)
电机驱动	TB6612FNG	双H桥，1.2A/路，内置死区控制	IN1-4 (PB0-PB3)，PWMA/B (PA8/PA9)
直流电机	520直流电机+霍尔编码器	减速比1:30，编码器分辨率390线/转	编码器A/B相 (PA0/PA1, PA6/PA7)
电源	18650锂电池×2	7.4V/2200mAh，带2A过流保护	7.4V直驱电机，5V降压给MCU

二、硬件设计方案

1. 核心电路连接

STM32F103C8T6
├── MPU6050 (I2C1)
│   ├── SCL = PB6
│   ├── SDA = PB7
│   └── INT = PA0 (数据就绪中断)
├── 电机驱动 (TB6612)
│   ├── 左电机：IN1=PB0, IN2=PB1, PWMA=PA8 (TIM1_CH1)
│   └── 右电机：IN3=PB2, IN4=PB3, PWMB=PA9 (TIM1_CH2)
├── 编码器 (TIM2/TIM3)
│   ├── 左编码器：A=PA0 (TIM2_CH1), B=PA1 (TIM2_CH2)
│   └── 右编码器：A=PA6 (TIM3_CH1), B=PA7 (TIM3_CH2)
└── 按键/指示灯
    ├── 启动/停止按键 = PC0
    └── 状态LED = PC1

2. 机械结构设计要点

• 重心位置：电池组安装在车体最下端，重心越低，平衡控制越容易（建议重心高度<15cm）。
• 轮子直径：推荐65-80mm，过大导致惯量增加，过小导致控制精度不足。
• 电机安装：电机轴线与车身对称轴重合，避免转向力矩不平衡。

三、软件设计与核心PID算法

1. 系统架构（前后台系统）

• 后台（中断）：
  • TIM1更新中断（1ms）：执行PID计算，更新电机PWM输出。
  • TIM2/TIM3编码器接口：硬件计数电机转速。
  • MPU6050中断（10ms）：读取原始数据，DMP解算姿态角。
• 前台（主循环）：OLED显示状态、按键扫描、蓝牙指令解析。

2. 核心PID算法实现

2.1 直立环PD控制器（最关键）

控制原理：角度偏差 → PD计算 → 电机PWM输出

#include "control.h"

// PID参数结构体
typedef struct {
    float Kp;       // 比例系数（决定响应速度）
    float Ki;       // 积分系数（消除稳态误差）
    float Kd;       // 微分系数（抑制超调/震荡）
    float integral;  // 积分累积
    float prev_error; // 上次误差（用于微分）
} PID_Controller_t;

// 直立环PID（PD控制）
// 输入：target_angle（目标角度，通常0°=直立），current_angle（当前角度）
// 输出：电机PWM值（-1000 ~ +1000）
int16_t Vertical_PID(float target_angle, float current_angle) {
    static PID_Controller_t pid = {
        .Kp = 800.0f,    // 比例系数：800（需根据机械结构调整）
        .Ki = 0.0f,      // 直立环通常不加积分（避免稳态抖动）
        .Kd = 2.5f,      // 微分系数：2.5（抑制车身晃动）
        .integral = 0.0f,
        .prev_error = 0.0f
    };
    
    float error = target_angle - current_angle;  // 角度偏差
    float derivative = error - pid.prev_error;   // 微分项（变化率）
    
    // PD输出 = Kp*误差 + Kd*微分
    int16_t output = (int16_t)(pid.Kp * error + pid.Kd * derivative);
    
    pid.prev_error = error;  // 保存本次误差
    return output;
}

2.2 速度环PI控制器（辅助稳定）

控制原理：编码器速度偏差 → PI计算 → 直立环角度补偿

// 速度环PI控制器
// 输入：target_speed（目标速度），current_speed（当前速度，编码器计数/秒）
// 输出：角度补偿值（叠加到直立环目标角度）
int16_t Velocity_PID(int16_t target_speed, int16_t current_speed) {
    static PID_Controller_t pid = {
        .Kp = 8.0f,     // 比例系数：8
        .Ki = 0.4f,     // 积分系数：0.4（消除速度静差）
        .Kd = 0.0f,     // 速度环不加微分
        .integral = 0.0f,
        .prev_error = 0.0f
    };
    
    int16_t error = target_speed - current_speed;
    pid.integral += error;  // 积分累积
    
    // 积分限幅（防止积分饱和）
    if (pid.integral > 1000) pid.integral = 1000;
    if (pid.integral < -1000) pid.integral = -1000;
    
    // PI输出 = Kp*误差 + Ki*积分
    int16_t output = (int16_t)(pid.Kp * error + pid.Ki * pid.integral);
    return output;
}

2.3 转向环P控制器（差速控制）

// 转向环P控制器（简单比例控制）
// 输入：turn_angle（转向角度，-100~+100，负值左转，正值右转）
// 输出：左右轮速度差
int16_t Turn_PID(int16_t turn_angle) {
    float Kp_turn = 5.0f;  // 转向比例系数
    return (int16_t)(Kp_turn * turn_angle);
}

3. 主控制循环（1ms定时中断）

#include "stm32f10x.h"
#include "mpu6050.h"
#include "encoder.h"
#include "motor.h"

// 全局变量
float current_angle = 0.0f;      // 当前车身角度（来自MPU6050）
int16_t left_speed = 0, right_speed = 0;  // 编码器速度
int16_t vertical_out = 0, velocity_out = 0, turn_out = 0;  // PID输出

// TIM1更新中断（1ms执行一次，控制核心）
void TIM1_UP_IRQHandler(void) {
    if (TIM_GetITStatus(TIM1, TIM_IT_Update) != RESET) {
        // 1. 读取传感器数据
        current_angle = MPU6050_GetAngle();  // 获取车身倾角（DMP解算）
        left_speed = Encoder_GetLeftSpeed();   // 左轮编码器速度
        right_speed = Encoder_GetRightSpeed(); // 右轮编码器速度
        
        // 2. 直立环PID（核心：保持车身直立）
        vertical_out = Vertical_PID(0.0f, current_angle);  // 目标角度0°
        
        // 3. 速度环PID（辅助：消除速度静差）
        int16_t avg_speed = (left_speed + right_speed) / 2;
        velocity_out = Velocity_PID(0, avg_speed);  // 目标速度0（原地平衡）
        
        // 4. 转向环PID（差速控制）
        turn_out = Turn_PID(0);  // 目标转向0（直行）
        
        // 5. 合成最终PWM输出
        int16_t left_pwm = vertical_out + velocity_out - turn_out;
        int16_t right_pwm = vertical_out + velocity_out + turn_out;
        
        // 6. 输出限幅（防止PWM溢出）
        if (left_pwm > 1000) left_pwm = 1000;
        if (left_pwm < -1000) left_pwm = -1000;
        if (right_pwm > 1000) right_pwm = 1000;
        if (right_pwm < -1000) right_pwm = -1000;
        
        // 7. 设置电机PWM
        Motor_SetPWM(left_pwm, right_pwm);
        
        TIM_ClearITPendingBit(TIM1, TIM_IT_Update);
    }
}

4. MPU6050姿态解算（DMP硬件解算）

#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"

// MPU6050初始化与DMP配置
uint8_t MPU6050_Init(void) {
    // 1. 初始化MPU6050
    mpu_init();
    
    // 2. 配置DMP（数字运动处理器）
    dmp_load_motion_driver_firmware();  // 加载DMP固件
    dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));
    dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL | 
                      DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL);
    dmp_set_fifo_rate(10);  // 100ms输出一次数据
    
    // 3. 设置陀螺仪量程±2000°/s，加速度计量程±2g
    mpu_set_gyro_range(MPU_GYRO_FS_2000);
    mpu_set_accel_range(MPU_ACCEL_FS_2);
    
    return 0;
}

// 获取车身倾角（通过DMP解算四元数，转换为欧拉角）
float MPU6050_GetAngle(void) {
    short gyro[3], accel[3];
    long quat[4];
    float pitch, roll, yaw;
    
    if (dmp_read_fifo(gyro, accel, quat, &sensor_timestamp) == 0) {
        // 四元数转换为欧拉角
        dmp_get_euler_angles(quat, &pitch, &roll, &yaw);
        return roll;  // 返回车身倾角（roll角）
    }
    return 0.0f;
}

参考代码 stm32 平衡机器人采用PID程序 www.youwenfan.com/contentcst/123511.html

四、PID参数整定指南（核心难点）

1. 整定步骤（必须按顺序执行）

步骤	操作	目标	现象
1. 直立环Kp	从0逐渐增大Kp	车身能短暂直立	车身剧烈震荡，像倒立摆
2. 直立环Kd	从0逐渐增大Kd	抑制震荡，平稳直立	车身不再剧烈晃动，轻微摆动
3. 速度环Kp	从0逐渐增大Kp	消除速度静差	用手推车身，能缓慢回位
4. 速度环Ki	从0逐渐增大Ki	完全消除静差	车身静止不动，无漂移

2. 典型参数参考（需根据实际机械结构调整）

参数	直立环(Kp)	直立环(Kd)	速度环(Kp)	速度环(Ki)
参考值	600~1200	1.5~4.0	5~15	0.2~0.8
过大现象	剧烈震荡	高频抖动	速度突变	积分饱和
过小现象	无法直立	反应迟钝	速度漂移	无法回位

3. 调试技巧

• 分步调试：先调直立环（速度环Ki=0），再调速度环。
• 手动辅助：调试时用手扶着车身，防止摔坏电机。
• 数据监控：通过串口打印角度、PWM值，观察系统响应曲线。

五、系统调试与扩展

1. 调试步骤

阶段	操作	工具
硬件调试	测量电机PWM波形，验证编码器计数方向	示波器、万用表
姿态调试	验证MPU6050角度输出是否与实际倾角一致	串口助手、串口绘图工具
PID整定	按步骤整定直立环、速度环参数	示波器观察角度响应曲线
功能测试	测试前进、后退、转向功能	实际场地测试

2. 扩展功能

• 蓝牙遥控：添加HC-05蓝牙模块，通过手机APP控制平衡车移动。
• 超声波避障：添加HC-SR04超声波，实现自动避障功能。
• OLED显示：实时显示角度、速度、电池电压等信息。
• 自动充电：添加红外寻线传感器，实现自动回充功能。

六、总结

基于STM32的平衡机器人通过MPU6050+DMP实现精准姿态解算，采用“直立环PD+速度环PI”的串级控制结构，确保车身稳定直立。