【平衡小车编程实战】（六）从零到一：直立环PD与速度环PI的C语言实现与调试心法

1. 直立环PD控制器的实现精髓第一次接触平衡小车时我被直立环的魔法般的效果震撼到了——它能让一根金属杆像被施了定身术一样稳稳立住。后来才发现这背后的核心就是PD控制器。这里有个反直觉的现象为什么直立控制只用PD而不用完整的PID实测发现积分项(I)在这里反而会破坏稳定性因为角度偏差的持续累积会导致小车过度反应。直立环的公式看似简单PWM_out Vertical_Kp*(Angle-Med)Vertical_Kd*(gyro_Y-0);但调试时我踩过三个大坑极性判断错误有次把Kp和Kd的符号设反了结果小车像喝醉一样往地上栽。后来发现当角度向前倾斜时(RollMed)电机应该向前转才能恢复平衡。单位不统一MPU6050的角速度单位是度/秒而角度是度需要确保Kp和Kd的量纲匹配。我的经验值是Kp在80-120Kd在1.5-2.5范围较合适。机械中值校准Med_Angle这个参数必须精确我的土方法是让小车在不同平面测试取能保持静止时的角度平均值。调试时建议先用纯P控制用手扶住小车观察响应如果往左倒时电机往右转说明极性正确然后慢慢增加Kp直到出现小幅振荡最后加入Kd抑制振荡。用串口实时绘制角度波形会事半功倍我常用的是VOFA这个免费工具。2. 速度环PI控制的实战技巧速度环就像小车的隐形手当直立环工作时它会悄悄调整车速维持平衡。这里必须用PI控制——P项快速响应速度变化I项消除静差。但积分项是个双刃剑我曾在实验室熬到凌晨三点才搞明白积分饱和问题。来看这个关键实现// 低通滤波(α0.7) EnC_Err_Lowout (1-a)*Encoder_Err a*EnC_Err_Lowout_last; // 积分限幅 Encoder_SEncoder_S10000?10000:(Encoder_S(-10000)?(-10000):Encoder_S);这里有三个黄金法则低通滤波必不可少编码器的脉冲计数会有高频噪声直接用会导致速度环剧烈抖动。α取值0.6-0.8时效果最佳。积分限幅是安全阀我的小车曾因积分失控而暴走撞飞了桌上的咖啡杯。现在必设积分上限建议在5000-10000之间。采样周期要稳定用STM32的定时器触发中断我常用5ms周期。太短会加重计算负担太长会导致控制延迟。调试时有个小技巧先把直立环参数调好然后给小车一个轻微推力观察它能否自动回到平衡位置。好的速度环会让小车像有弹性一样缓缓复位。Kp从0.1开始慢慢增加Ki保持在Kp的1/10左右。3. 双环协同工作的核心逻辑当直立环和速度环同时工作时会出现奇妙的耦合现象。我的第一个版本小车会出现抽搐式前进后来发现是因为两个环的输出直接相加导致的。正确的做法应该是// 速度环输出作为直立环的期望角度偏移量 Vertical_outVertical(Velocity_outMed_Angle, Roll, gyrox);这种串级结构的意思是速度环计算出需要倾斜的角度直立环再努力维持这个角度。就像骑自行车时想前进就得稍微前倾。实测中要注意输出融合转向环的输出应该与速度环正交处理我的做法是左轮直立输出-转向输出右轮直立输出转向输出优先级设置直立环的响应速度应该是速度环的5-10倍这意味着直立环的采样频率要更高安全保护最后一定要加PWM限幅我的血泪教训是电机全速撞墙会导致齿轮崩齿4. 调试工具链的搭建心得工欲善其事必先利其器。经过五个版本的迭代我的调试工具链已经非常高效实时波形显示用DMA串口发送数据配合上位机软件显示波形。推荐用SEGGER的J-Scope可以直接读取STM32内存变量参数热更新通过串口命令实时修改PID参数不用每次改代码烧录。我写了个简单的协议解析器void USART1_IRQHandler(void) { if(USART_GetITStatus(USART1, USART_IT_RXNE)) { char cmd USART_ReceiveData(USART1); if(cmd P) Vertical_Kp 0.5f; else if(cmd p) Vertical_Kp - 0.5f; // 其他参数调整... } }机械校准工具用OLED屏幕显示实时角度和角速度配合按键调整机械中值。这个小改动让我调试效率提升了300%安全保护机制增加倾角超过30度自动切断电机的保护防止烧毁驱动芯片。代码里简单加个判断if(fabs(Roll) 30.0f) { MOTO1 0; MOTO2 0; }记得第一次成功让小车平衡时它稳稳立了3分钟后才因为电池没电倒下。那一刻突然理解了什么叫做控制的艺术——算法不是冰冷的公式而是让机械拥有生命的魔法。现在每次看到新手调参时焦头烂额的样子都会想起自己当初在实验室地板上追着失控小车跑的情形。调试PID就像教小孩子骑自行车既要放手让它自己找平衡又要在摔倒前及时扶住。