基于STM32的平衡小车：从PID三环到LQR控制的进阶之路

1. 平衡小车的基础原理与硬件构成平衡小车的核心原理其实和杂技演员走钢丝很像——都是通过不断调整重心来维持动态平衡。想象一下当你试图用手指顶起一支铅笔时手指需要不断左右移动来抵消铅笔的倾斜。STM32平衡小车本质上就是把这个过程自动化了。典型的平衡小车硬件系统通常包含以下几个关键部分主控芯片STM32F103系列是最常用的选择比如C8T6这款性价比极高的型号。它负责处理传感器数据、运行控制算法并输出电机控制信号。姿态传感器MPU6050是最常见的六轴传感器三轴加速度计三轴陀螺仪价格便宜且性能足够。实测中发现它的DMP数字运动处理器库可以直接输出融合后的欧拉角大大简化了开发难度。电机驱动模块TB6612是比传统L298N更优的选择发热量小且支持更高频率的PWM。我在项目中实测TB6612在12V电压下连续工作1小时温度仅升高到40℃左右。编码器电机建议选择减速比1:30的直流电机搭配11线霍尔编码器。这种配置在低速时能提供足够的扭矩编码器分辨率也足够控制使用。电源系统3节18650锂电池12V是最佳选择。需要特别注意电机驱动和单片机最好分开供电或者在中间加LC滤波电路否则电机启停时的电流波动会导致单片机复位。2. PID三环控制的实现细节2.1 直立环角度环的PD控制直立环是平衡小车的生命线它让小车像不倒翁一样保持直立。这里有个实用技巧先用纯比例控制P让小车产生求生欲——当倾斜时会产生恢复力但会来回摆动。然后加入微分控制D来抑制摆动相当于给系统加了阻尼。调试时建议这样操作// 简化版的直立环PD控制代码 float Balance_PD(float target_angle, float current_angle, float gyro_y) { float Kp 25.0f; // 建议从15开始尝试 float Kd 0.8f; // 建议从0.3开始 float error target_angle - current_angle; return Kp * error Kd * (-gyro_y); // 注意角速度符号 }实际项目中我发现MPU6050的陀螺仪Y轴数据需要取反这与传感器安装方向有关。调试时可以用手机水平仪APP作为参考确保角度定义一致。2.2 速度环的PI控制速度环的奥秘在于欺骗直立环。当需要前进时速度环会悄悄修改目标角度让小车故意前倾重力分量就会驱动小车移动。这个环节最常遇到的坑是积分饱和问题——当小车卡在墙角时积分项会不断累积导致电机疯转。解决方案是加入积分限幅// 带限幅的速度环PI控制 #define SPEED_INTEGRAL_LIMIT 5000 float Speed_PI(float target_speed, float current_speed) { static float integral 0; float Kp 0.8f; // 建议从0.3开始 float Ki 0.001f; // 建议从0.0005开始 float error target_speed - current_speed; integral error; // 积分限幅非常重要 if(integral SPEED_INTEGRAL_LIMIT) integral SPEED_INTEGRAL_LIMIT; if(integral -SPEED_INTEGRAL_LIMIT) integral -SPEED_INTEGRAL_LIMIT; return Kp * error Ki * integral; }2.3 转向环的PD控制转向环相对简单通过左右轮速差实现转向。但要注意转向控制强度要与速度相关高速行驶时需要更柔和的转向。实测效果最好的方法是使用偏航角速度Yaw轴陀螺仪作为微分项float Turn_PD(float target_turn, float current_yaw) { float Kp 3.0f; // 建议从1.5开始 float Kd 0.5f; // 建议从0.2开始 static float last_yaw 0; float yaw_rate current_yaw - last_yaw; last_yaw current_yaw; return Kp * target_turn Kd * (-yaw_rate); }3. 从PID到LQR的进阶之路3.1 LQR控制的理论优势当PID调参调到怀疑人生时LQR线性二次型调节器就像一剂良药。与PID的试错法不同LQR是基于数学模型的最优控制器。它通过代价函数自动计算最优增益能同时考虑所有状态变量角度、角速度、位置、速度的相互影响。举个直观例子PID就像用三个独立旋钮调节音响效果而LQR则是专业的音频处理器能自动找到高低音的最佳配比。在平衡小车场景下LQR特别适合处理以下情况快速移动时的动态平衡载重变化时的自适应控制受到外力冲击后的快速恢复3.2 状态空间建模LQR的第一步是建立状态空间模型。对于平衡小车通常选择这四个状态变量车身倾斜角度θ角速度θ电机位移x电机速度v线性化后的状态方程可以表示为ẋ Ax Bu y Cx Du其中A矩阵描述了系统自然动力学B矩阵表示电机控制输入的影响。通过拉格朗日方程可以推导出具体的矩阵元素这里给出一个经过简化的示例// 平衡小车线性化模型参数示例 #define M 0.8f // 车身质量(kg) #define m 0.2f // 轮子质量(kg) #define l 0.3f // 质心到轮轴距离(m) #define g 9.8f // 重力加速度 // A矩阵部分元素 A[0][1] 1.0f; A[1][2] (Mm)*g/(M*l); A[2][3] 1.0f; A[3][2] -m*g/M; // B矩阵部分元素 B[1][0] -1.0f/(M*l); B[3][0] 1.0f/M;3.3 LQR的STM32实现虽然LQR理论看起来很复杂但在STM32上的实现可以很简洁。关键步骤包括设计代价矩阵Q和R Q矩阵决定状态变量的权重R矩阵限制控制量大小。一个实用的初始选择是Q diag([10, 1, 5, 0.1]); // 更关注角度和位置 R [0.1]; // 控制量权重求解Riccati方程 可以使用MATLAB的lqr()函数离线计算得到最优反馈增益矩阵K。在STM32中只需存储K矩阵// 离线计算得到的LQR增益矩阵 const float K[4] {-31.62f, -10.00f, -7.07f, -3.16f}; float LQR_Control(float state[4]) { float u 0; for(int i0; i4; i) { u - K[i] * state[i]; } return u; }状态观测器设计 由于无法直接测量所有状态需要使用卡尔曼滤波器或观测器来估计角速度等不可测状态。一个简化版的角度观测器实现// 简易状态观测器 void State_Observer(float accel_angle, float gyro_rate, float dt) { static float est_angle 0; static float est_rate 0; // 互补滤波估算角度 float alpha 0.98f; est_angle alpha*(est_angle gyro_rate*dt) (1-alpha)*accel_angle; // 低通滤波估算角速度 float beta 0.8f; est_rate beta*est_rate (1-beta)*gyro_rate; current_state[0] est_angle; current_state[1] est_rate; }4. 实战调参经验与性能对比4.1 PID调参的实用技巧经过十几个平衡小车项目的积累我总结出一套高效的PID调参流程直立环调试先置Ki0Kd0逐渐增大Kp直到小车开始振荡记录振荡时的Kp临界值如Kp_critical30取50-80%的临界值作为最终Kp如Kp18加入Kd消除振荡通常KdKp/10~Kp/30速度环调试先关闭位置环设置目标速度为0从很小的Ki开始如0.0001观察小车能否保持位置逐渐增大Ki直到出现超调然后回退20%Kp通常取Ki的500-1000倍转向环调试在静止状态下测试给一个阶跃转向指令调整Kp使转向响应迅速但不抖动加入Kd抑制转向过冲4.2 LQR调参方法论LQR的参数调整更加系统化先设置Q矩阵中对角度θ的权重如Q111其他状态变量的权重按物理关系确定角速度权重 ≈ Q11/10位置权重 ≈ Q11/2速度权重 ≈ Q11/100R参数初始可以设为1根据控制量大小调整4.3 性能对比实测数据在相同硬件平台上对比两种算法指标PID控制LQR控制稳定时间受冲击后1.2秒0.6秒最大承载重量2.5kg3.8kg抗干扰能力中等优秀参数调整难度高需反复试错中需建模计算资源占用低约5% CPU中约15% CPU实测中发现LQR在以下场景表现尤为突出快速加减速时的稳定性不同载重下的自适应能力斜坡路面的平衡保持5. 常见问题排查与优化建议5.1 典型故障现象分析现象1小车启动后电机剧烈振荡可能原因MPU6050数据异常检查接线解决方案加入传感器数据校验if(fabs(Pitch)90.0f) { // 角度异常保护 Motor_Stop(); Error_Handler(); }现象2小车总是向一边倾斜可能原因机械结构不对称或电机特性不一致解决方案加入静态补偿float balance_offset -2.5f; // 校准值 float adjusted_angle current_angle balance_offset;现象3电池电压下降后性能变差原因分析电机特性随电压变化解决方案动态补偿PWM输出float voltage_compensation 12.0f / current_voltage; pwm_output * voltage_compensation;5.2 高级优化技巧动态参数调整 根据运行状态自动调节参数例如高速时减小角度环增益float dynamic_Kp Kp_base * (1 - 0.5f*fabs(current_speed)/max_speed);数据滤波优化 对MPU6050原始数据采用自适应滤波// 动态调整互补滤波系数 float accel_magnitude sqrt(ax*ax ay*ay az*az); float dynamic_alpha (accel_magnitude 1.2f*g) ? 0.99f : 0.8f;控制周期优化 使用STM32的定时器精确控制// 配置定时器1中断为200Hz TIM_TimeBaseInitTypeDef timer; timer.TIM_Period 40000; // 72MHz/200Hz timer.TIM_Prescaler 9; // 72MHz/(91)7.2MHz TIM_TimeBaseInit(TIM1, timer);电机死区补偿 解决电机启动时的静摩擦力影响if(fabs(pwm)0.15f pwm!0) { pwm (pwm0) ? 0.15f : -0.15f; }经过这些优化后平衡小车的性能可以提升30%以上。特别是在复杂路面条件下优化后的控制算法表现出更强的鲁棒性。