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

一、系统概述与核心原理1. 系统定位基于STM32的两轮自平衡机器人Balance Bot是自动控制理论的经典实践平台。系统通过MPU6050陀螺仪实时监测车身倾角利用PID算法计算出电机补偿量驱动直流电机保持车身直立不倒并实现前进、后退与转向控制。2. 控制原理直立控制基于角度闭环PD控制当车身前倾时电机向前加速后仰时向后加速。速度控制基于编码器闭环PI控制抵消直立控制带来的速度波动实现稳态精度。方向控制差速转向左右轮速度差实现转向。3. 核心硬件选型模块型号关键参数接口方式主控STM32F103C8T672MHz Cortex-M33个通用定时器2路ADC核心控制器姿态传感器MPU60503轴陀螺仪3轴加速度±2000°/sI2C接口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锂电池×27.4V/2200mAh带2A过流保护7.4V直驱电机5V降压给MCU二、硬件设计方案1. 核心电路连接STM32F103C8T6 ├── MPU6050 (I2C1) │ ├── SCL PB6 │ ├── SDA PB7 │ └── INT PA0 (数据就绪中断) ├── 电机驱动 (TB6612) │ ├── 左电机IN1PB0, IN2PB1, PWMAPA8 (TIM1_CH1) │ └── 右电机IN3PB2, IN4PB3, PWMBPA9 (TIM1_CH2) ├── 编码器 (TIM2/TIM3) │ ├── 左编码器APA0 (TIM2_CH1), BPA1 (TIM2_CH2) │ └── 右编码器APA6 (TIM3_CH1), BPA7 (TIM3_CH2) └── 按键/指示灯 ├── 启动/停止按键 PC0 └── 状态LED PC12. 机械结构设计要点重心位置电池组安装在车体最下端重心越低平衡控制越容易建议重心高度15cm。轮子直径推荐65-80mm过大导致惯量增加过小导致控制精度不足。电机安装电机轴线与车身对称轴重合避免转向力矩不平衡。三、软件设计与核心PID算法1. 系统架构前后台系统后台中断TIM1更新中断1ms执行PID计算更新电机PWM输出。TIM2/TIM3编码器接口硬件计数电机转速。MPU6050中断10ms读取原始数据DMP解算姿态角。前台主循环OLED显示状态、按键扫描、蓝牙指令解析。2. 核心PID算法实现2.1 直立环PD控制器最关键控制原理角度偏差 → PD计算 → 电机PWM输出#includecontrol.h// PID参数结构体typedefstruct{floatKp;// 比例系数决定响应速度floatKi;// 积分系数消除稳态误差floatKd;// 微分系数抑制超调/震荡floatintegral;// 积分累积floatprev_error;// 上次误差用于微分}PID_Controller_t;// 直立环PIDPD控制// 输入target_angle目标角度通常0°直立current_angle当前角度// 输出电机PWM值-1000 ~ 1000int16_tVertical_PID(floattarget_angle,floatcurrent_angle){staticPID_Controller_t pid{.Kp800.0f,// 比例系数800需根据机械结构调整.Ki0.0f,// 直立环通常不加积分避免稳态抖动.Kd2.5f,// 微分系数2.5抑制车身晃动.integral0.0f,.prev_error0.0f};floaterrortarget_angle-current_angle;// 角度偏差floatderivativeerror-pid.prev_error;// 微分项变化率// PD输出 Kp*误差 Kd*微分int16_toutput(int16_t)(pid.Kp*errorpid.Kd*derivative);pid.prev_errorerror;// 保存本次误差returnoutput;}2.2 速度环PI控制器辅助稳定控制原理编码器速度偏差 → PI计算 → 直立环角度补偿// 速度环PI控制器// 输入target_speed目标速度current_speed当前速度编码器计数/秒// 输出角度补偿值叠加到直立环目标角度int16_tVelocity_PID(int16_ttarget_speed,int16_tcurrent_speed){staticPID_Controller_t pid{.Kp8.0f,// 比例系数8.Ki0.4f,// 积分系数0.4消除速度静差.Kd0.0f,// 速度环不加微分.integral0.0f,.prev_error0.0f};int16_terrortarget_speed-current_speed;pid.integralerror;// 积分累积// 积分限幅防止积分饱和if(pid.integral1000)pid.integral1000;if(pid.integral-1000)pid.integral-1000;// PI输出 Kp*误差 Ki*积分int16_toutput(int16_t)(pid.Kp*errorpid.Ki*pid.integral);returnoutput;}2.3 转向环P控制器差速控制// 转向环P控制器简单比例控制// 输入turn_angle转向角度-100~100负值左转正值右转// 输出左右轮速度差int16_tTurn_PID(int16_tturn_angle){floatKp_turn5.0f;// 转向比例系数return(int16_t)(Kp_turn*turn_angle);}3. 主控制循环1ms定时中断#includestm32f10x.h#includempu6050.h#includeencoder.h#includemotor.h// 全局变量floatcurrent_angle0.0f;// 当前车身角度来自MPU6050int16_tleft_speed0,right_speed0;// 编码器速度int16_tvertical_out0,velocity_out0,turn_out0;// PID输出// TIM1更新中断1ms执行一次控制核心voidTIM1_UP_IRQHandler(void){if(TIM_GetITStatus(TIM1,TIM_IT_Update)!RESET){// 1. 读取传感器数据current_angleMPU6050_GetAngle();// 获取车身倾角DMP解算left_speedEncoder_GetLeftSpeed();// 左轮编码器速度right_speedEncoder_GetRightSpeed();// 右轮编码器速度// 2. 直立环PID核心保持车身直立vertical_outVertical_PID(0.0f,current_angle);// 目标角度0°// 3. 速度环PID辅助消除速度静差int16_tavg_speed(left_speedright_speed)/2;velocity_outVelocity_PID(0,avg_speed);// 目标速度0原地平衡// 4. 转向环PID差速控制turn_outTurn_PID(0);// 目标转向0直行// 5. 合成最终PWM输出int16_tleft_pwmvertical_outvelocity_out-turn_out;int16_tright_pwmvertical_outvelocity_outturn_out;// 6. 输出限幅防止PWM溢出if(left_pwm1000)left_pwm1000;if(left_pwm-1000)left_pwm-1000;if(right_pwm1000)right_pwm1000;if(right_pwm-1000)right_pwm-1000;// 7. 设置电机PWMMotor_SetPWM(left_pwm,right_pwm);TIM_ClearITPendingBit(TIM1,TIM_IT_Update);}}4. MPU6050姿态解算DMP硬件解算#includeinv_mpu.h#includeinv_mpu_dmp_motion_driver.h// MPU6050初始化与DMP配置uint8_tMPU6050_Init(void){// 1. 初始化MPU6050mpu_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加速度计量程±2gmpu_set_gyro_range(MPU_GYRO_FS_2000);mpu_set_accel_range(MPU_ACCEL_FS_2);return0;}// 获取车身倾角通过DMP解算四元数转换为欧拉角floatMPU6050_GetAngle(void){shortgyro[3],accel[3];longquat[4];floatpitch,roll,yaw;if(dmp_read_fifo(gyro,accel,quat,sensor_timestamp)0){// 四元数转换为欧拉角dmp_get_euler_angles(quat,pitch,roll,yaw);returnroll;// 返回车身倾角roll角}return0.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~12001.5~4.05~150.2~0.8过大现象剧烈震荡高频抖动速度突变积分饱和过小现象无法直立反应迟钝速度漂移无法回位3. 调试技巧分步调试先调直立环速度环Ki0再调速度环。手动辅助调试时用手扶着车身防止摔坏电机。数据监控通过串口打印角度、PWM值观察系统响应曲线。五、系统调试与扩展1. 调试步骤阶段操作工具硬件调试测量电机PWM波形验证编码器计数方向示波器、万用表姿态调试验证MPU6050角度输出是否与实际倾角一致串口助手、串口绘图工具PID整定按步骤整定直立环、速度环参数示波器观察角度响应曲线功能测试测试前进、后退、转向功能实际场地测试2. 扩展功能蓝牙遥控添加HC-05蓝牙模块通过手机APP控制平衡车移动。超声波避障添加HC-SR04超声波实现自动避障功能。OLED显示实时显示角度、速度、电池电压等信息。自动充电添加红外寻线传感器实现自动回充功能。六、总结基于STM32的平衡机器人通过MPU6050DMP实现精准姿态解算采用“直立环PD速度环PI”的串级控制结构确保车身稳定直立。