平衡车闭环控制实战：从倒立摆建模到PID工程调参

1. 这不是玩具，是闭环控制的物理教科书

平衡车和直立车，很多人第一眼觉得是“会自己站稳的轮子”，但真正拆开来看，它是一台实时运行的物理系统验证平台——你写的每一行代码，都在和重力、电机惯性、传感器噪声、机械结构形变这些真实世界的物理量掰手腕。我带过三届智能车校队，从2019年用STM32F407跑基础PID，到2023年用GD32H750做LQR+状态观测器融合，最深的体会是：所有“能跑起来”的代码背后，都藏着对刚体动力学、采样延迟、执行器饱和、传感器标定误差这四座大山的反复妥协。标题里写的“入门经验”，不是教你复制粘贴一段PID，而是帮你建立一套判断标准：什么时候该调P，什么时候该怀疑编码器零点漂移，为什么同样参数在夏天和冬天表现差一倍，以及——最关键的一点——当车突然倒向一边时，你该先看串口打印的陀螺仪原始值，还是先断电检查电机堵转电流。这篇文章不讲抽象理论，只讲我在实验室地板上摔坏过7块PCB板后，总结出的6个必须亲手验证的硬核节点。如果你刚拿到一块MPU6050+TB6612电机驱动板，正对着示例代码发懵；或者你已经跑通了基础平衡，但每次加速度稍大就抖得像筛糠——那你需要的不是新算法，而是把“代码”和“车体”之间那层模糊的映射关系，一锤一锤砸实。

2. 系统设计逻辑：从物理模型到代码落地的三层穿透

2.1 为什么必须从单摆模型开始理解？——绕不开的物理本质

所有直立车控制的核心，本质是倒立摆（Inverted Pendulum）问题。这不是竞赛命题组拍脑袋想出来的，而是由车体-轮轴-重心构成的刚体系统，在无外力时天然处于不稳定平衡点。它的运动方程推导过程，直接决定了你后续所有代码的结构骨架。我见过太多同学跳过这一步，直接抄PID参数表，结果调参调到崩溃才发现：自己连“角度误差”到底是用陀螺仪积分值还是加速度计融合值都没搞清。

真实车体的简化模型如下（忽略轮径、摩擦等二阶效应）：

θ'' = (m*g*L / J) * θ - (b/J) * θ' + (r/J) * u

其中：

• θ 是车身倾角（rad），θ' 和 θ'' 是角速度、角加速度
• m 是车体总质量（kg），g 是重力加速度（9.81 m/s²）
• L 是重心到轮轴中心的垂直距离（m），J 是绕轮轴的转动惯量（kg·m²）
• b 是等效阻尼系数（N·m·s/rad），r 是电机扭矩到轮轴的传动比（N·m/V）
• u 是电机驱动电压（V）

这个公式告诉你三件事：

1. θ'' 与 θ 成正比→ 系统天生不稳定（正反馈），必须靠控制器提供负反馈抵消；
2. θ'' 与 u 成正比→ 控制量直接影响加速度，但存在执行器饱和（u 最大值受限于电池电压）；
3. θ'' 与 θ' 成反比→ 阻尼项天然抑制振荡，但实际硬件中 b 极小，必须靠控制器人工注入阻尼。

提示：很多开源代码里直接用angle = atan2(acc_y, acc_z)计算倾角，这是严重错误。加速度计在动态过程中受离心力干扰极大，单独使用会导致低速时严重漂移。正确做法是用陀螺仪角速度积分初值，再用加速度计做慢速修正（互补滤波），或直接上卡尔曼滤波。我在2021年浙江赛区调试时，就因没处理好这个，导致车在匀速前进时缓慢前倾最终倒地。

2.2 控制架构选择：级联PID为何是入门唯一合理路径？

面对倒立摆模型，理论上可用LQR、MPC甚至强化学习（如TD3），但对新手而言，级联PID（Cascade PID）是唯一兼顾可解释性、调试直观性和工程鲁棒性的方案。它把控制分解为两个物理意义明确的环：

• 内环（速度环）：以电机编码器反馈的轮速为被控量，目标是让车轮按指令速度精准旋转。它解决的是“执行器响应滞后”问题，响应频率需达200Hz以上（采样周期≤5ms）。
• 外环（姿态环）：以车身倾角为被控量，输出目标轮速给内环。它解决的是“系统稳定性”问题，响应频率通常50Hz（采样周期20ms）即可。

为什么不能只用单环PID？举个实例：当车体前倾1°时，单环PID会直接输出一个较大电压让电机加速后退。但电机从静止到达到目标转速需要时间（电感、转动惯量导致），这段时间内车体继续前倾，等电机真转起来时，倾角可能已扩大到3°，控制器又猛加电压……形成“超调-震荡-失控”死循环。而级联结构中，外环只负责“告诉内环该跑多快”，内环则专注“用最快响应把速度拉到目标值”，两者职责分离，抗干扰能力提升3倍以上。

注意：内外环采样周期必须严格满足“内环≥3倍外环”。我们曾用20ms外环+10ms内环，结果车体高频抖动。实测发现，当内环响应跟不上外环指令变化率时，会产生相位滞后，等效于在系统中引入纯滞后环节，直接恶化稳定性裕度。最终改为外环20ms、内环5ms（STM32F4主频168MHz下轻松实现），抖动消失。

2.3 硬件选型的隐藏陷阱：传感器与执行器的匹配真相

代码写得再漂亮，硬件不匹配就是空中楼阁。新手常犯的三个致命错误：

1. MPU6050的I²C时钟频率设为400kHz：看似提速，实则导致陀螺仪数据丢帧。MPU6050内部ADC采样率为1kHz，I²C读取速率超过其数据更新率，会读到重复值或未更新值。正确做法是I²C时钟设为100kHz，配合DMP（数字运动处理器）模式，直接读取融合后的四元数，避免主控CPU做复杂滤波。

2. 电机驱动芯片选TB6612而非DRV8871：TB6612最大持续电流1.2A，而12V/370电机堵转电流常达3A。我们曾用TB6612跑测试，连续工作2分钟后驱动芯片热关断，车体缓慢前倾倒地。换用DRV8871（持续电流3.6A）后，温升稳定在65℃。

3. 编码器分辨率盲目追求高线数：2500线编码器在100rpm时，AB相脉冲频率达4166Hz。若主控定时器捕获频率上限仅1MHz，则无法准确计数。实测发现，1000线编码器在平衡车常用转速范围（0~200rpm）内，脉冲频率最高3333Hz，STM32通用定时器完全可胜任，且信号更干净。

实操心得：在PCB打样前，务必用示波器抓取电机驱动信号和编码器AB相波形。我们曾遇到编码器A相有毛刺导致计数错误，根源是电机电源与编码器电源未隔离，开关噪声耦合进信号线。解决方案是在编码器电源入口加10μF钽电容+100nF陶瓷电容，并用磁珠隔离。

3. 核心代码解析：从初始化到PID参数整定的全链路拆解

3.1 初始化阶段：时序精度决定成败的底层根基

平衡车对时序的要求远超普通嵌入式项目。一个20ms的外环控制周期，若实际执行时间波动±3ms，等效于给系统注入随机扰动。以下是STM32F4系列关键初始化配置（基于HAL库，但原理通用）：

// 1. 系统时钟：主频168MHz，APB1=42MHz（定时器时钟源） RCC_OscInitTypeDef RCC_OscInitStruct = {0}; RCC_ClkInitTypeDef RCC_ClkInitStruct = {0}; __HAL_RCC_PLL_CONFIG(RCC_PLLP_DIV2); // PLL主频=8MHz*168/8=168MHz HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5); // 2. 定时器TIM2（外环，20ms）：预分频8399，自动重载值16799 → 168MHz/(8400*16800)=1ms基准，中断触发50Hz htim2.Instance = TIM2; htim2.Init.Prescaler = 8399; // 168MHz/8400 = 20kHz htim2.Init.CounterMode = TIM_COUNTERMODE_UP; htim2.Init.Period = 16799; // 20kHz/16800 = 1.19Hz? 错！应为20kHz/1000=20Hz → Period=999 // 正确计算：期望20ms周期 → 频率50Hz → 基准时钟20kHz → Period = 20000/1 - 1 = 19999? 不对！ // 实际：TIMxCLK = APB1*2 = 42MHz*2 = 84MHz（TIM2在APB1，但倍频） // 所以：Prescaler = 84-1 = 83 → TIMxCLK = 1MHz，Period = 20000-1 = 19999 → 1MHz/20000 = 50Hz // 结论：必须查RM手册确认TIMxCLK实际频率，不能凭经验估算！ // 3. 定时器TIM3（内环，5ms）：独立时钟源，避免外环中断抢占导致内环抖动 htim3.Instance = TIM3; htim3.Init.Prescaler = 83; // 84MHz/84 = 1MHz htim3.Init.Period = 4999; // 1MHz/5000 = 200Hz → 5ms周期

关键细节：TIM2和TIM3必须启用中断优先级分组，且TIM3（内环）优先级高于TIM2（外环）。否则当TIM2中断正在执行PID计算时，TIM3中断被延迟，内环响应滞后。我们曾将TIM3设为抢占优先级1，TIM2设为2，问题解决。另外，所有PID计算必须放在中断服务函数（ISR）中，严禁在主循环里调用，否则时序完全失控。

3.2 传感器数据融合：互补滤波的代码实现与参数陷阱

MPU6050原始数据必须经过融合才能得到可靠倾角。以下为经实测验证的互补滤波代码（C语言）：

#define GYRO_GAIN 0.98f // 陀螺仪权重 #define ACC_GAIN 0.02f // 加速度计权重 float angle = 0.0f; // 当前倾角（rad） float gyro_rate = 0.0f; // 陀螺仪角速度（rad/s） float acc_angle = 0.0f; // 加速度计倾角（rad） uint32_t last_time = 0; uint32_t current_time = 0; void IMU_Update(void) { // 1. 读取MPU6050原始数据（省略I²C读取代码） int16_t gx, gy, gz, ax, ay, az; MPU6050_Read_Raw(&gx, &gy, &gz, &ax, &ay, &az); // 2. 转换为物理量（假设陀螺仪灵敏度131 LSB/(deg/s)，加速度计16384 LSB/g） gyro_rate = (float)gy * 0.0174533f / 131.0f; // deg/s → rad/s acc_angle = atan2f((float)ay, (float)az); // 仅用Y/Z轴，X轴用于横滚（此处忽略） // 3. 时间差计算（单位：ms） current_time = HAL_GetTick(); float dt = (current_time - last_time) / 1000.0f; last_time = current_time; // 4. 互补滤波：陀螺仪积分 + 加速度计修正 angle += gyro_rate * dt; // 积分得到角度变化 angle = angle * GYRO_GAIN + acc_angle * ACC_GAIN; // 加权融合 // 5. 关键！角度限幅防积分饱和 if(angle > 0.349f) angle = 0.349f; // 20°上限 if(angle < -0.349f) angle = -0.349f; // -20°下限 }

陷阱警示：GYRO_GAIN和ACC_GAIN并非固定值。夏天室温35℃时，MPU6050陀螺仪零偏漂移达0.5°/s，此时若仍用0.98权重，积分误差会快速累积。我们的解决方案是：开机后前10秒采集陀螺仪静止零偏，动态调整GYRO_GAIN = 1.0f - (zero_drift * 0.1f)。实测表明，此法可将静态漂移从±3°压缩至±0.2°。

3.3 PID控制器实现：增量式PID的抗饱和与防积分 windup

平衡车最易忽视的环节是PID输出饱和处理。以下为防积分饱和的增量式PID代码（外环姿态控制）：

typedef struct { float Kp, Ki, Kd; float setpoint; // 目标角度（rad） float error_last; // 上次误差 float integral; // 积分项 float output_max; // 输出限幅（对应电机PWM最大值） float output_min; // 输出限幅下限 } PID_Controller_t; PID_Controller_t pid_outer = {15.0f, 0.8f, 0.15f, 0.0f, 0.0f, 0.0f, 1000.0f, -1000.0f}; float PID_Calculate(PID_Controller_t* pid, float feedback) { float error = pid->setpoint - feedback; // 1. 比例项 float p_term = pid->Kp * error; // 2. 积分项（带抗饱和） float i_term = pid->Ki * error; // 只有当输出未饱和时才累加积分 if((pid->integral + i_term) < pid->output_max && (pid->integral + i_term) > pid->output_min) { pid->integral += i_term; } // 3. 微分项（用误差微分，避免设定值突变引起冲击） float d_term = pid->Kd * (error - pid->error_last); pid->error_last = error; float output = p_term + pid->integral + d_term; // 4. 输出限幅 if(output > pid->output_max) output = pid->output_max; if(output < pid->output_min) output = pid->output_min; return output; } // 外环调用示例 float target_speed = PID_Calculate(&pid_outer, angle); // angle来自IMU_Update()

实操心得：Ki参数绝不能凭经验设置。我们采用“临界比例度法”现场整定：先置Ki=Kd=0，逐步增大Kp直至车体持续等幅振荡，记录此时Kp临界值Ku（如Ku=25），则推荐初始Ki = 0.5*Ku/Tu（Tu为振荡周期，实测约0.8s），即Ki≈15.6。此法比Ziegler-Nichols公式更贴合实际物理系统。

3.4 电机驱动与PWM生成：死区时间与换向逻辑的硬伤规避

平衡车电机驱动必须考虑换向死区，否则H桥上下管直通短路。以下是基于STM32高级定时器TIM1的互补PWM配置：

// TIM1_CH1/CH1N生成互补PWM，死区时间=100ns（需根据MOSFET开关时间调整） htim1.Instance = TIM1; htim1.Init.Prescaler = 0; // 168MHz直接分频 htim1.Init.Period = 999; // 168MHz/1000 = 168kHz PWM频率 HAL_TIM_PWM_Init(&htim1); // 使能死区生成 TIM_BDTRInitTypeDef sBreakDeadTimeConfig; sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE; sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE; sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF; sBreakDeadTimeConfig.DeadTime = 100; // 单位：时钟周期，168MHz下100=595ns sBreakDeadTimeConfig.BreakState = TIM_BREAK_DISABLE; sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH; sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE; HAL_TIMEx_ConfigBreakDeadTime(&htim1, &sBreakDeadTimeConfig); // 启动PWM通道 HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1); HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_1);

血泪教训：死区时间必须实测确定。我们曾用100ns死区，结果电机启动时有“咔哒”异响，示波器显示上下管重叠导通200ns。更换为300ns死区后，异响消失，MOSFET温升降低40%。建议用示波器抓取HO和LO引脚波形，确保无重叠。

4. 实操全流程：从上电到稳定平衡的12个关键节点验证

4.1 上电自检：5分钟完成硬件功能确认

不要急着烧录代码，先做硬件级验证：

1. 电源纹波测试：用示波器DC耦合测量电机驱动芯片VCC，负载下纹波应＜100mVpp。我们曾因电源滤波电容虚焊，导致电机驱动异常，表现为车体间歇性抖动。

2. 编码器信号验证：手动匀速旋转车轮，用逻辑分析仪抓取AB相波形，确认：

   • A、B相相位差90°（正交编码）
   • Z相（索引脉冲）每圈出现1次（如有）
   • 无丢脉冲（对比理论脉冲数与实测数）

3. MPU6050通信确认：用ST-Link Utility读取MPU6050的WHO_AM_I寄存器（0x68），返回值应为0x68。若失败，检查I²C上拉电阻（推荐4.7kΩ）、SCL/SDA是否接反。

4. 电机转向校验：给电机施加正向PWM，观察车轮旋转方向是否与代码中定义一致。曾有队伍因电机接线反相，导致PID输出符号错误，车体向错误方向加速倾倒。

注意：所有自检必须在无闭环控制下进行！切勿在未验证传感器/执行器前开启PID，否则极易损坏硬件。

4.2 开环调试：让车“学会走路”前的必要铺垫

开环阶段目标是建立“输入PWM→输出轮速”的定量关系：

// 测试代码：固定PWM占空比，记录10秒内编码器脉冲数 uint32_t start_count = encoder_count; HAL_Delay(10000); uint32_t end_count = encoder_count; float rpm = (end_count - start_count) * 60.0f / (1000.0f * 1000.0f); // 假设1000线编码器

实测数据示例（12V供电）：

关键结论：50%以下PWM时，RPM与占空比近似线性；50%以上因电机反电动势增大，线性度下降。因此PID输出应映射到30%~70%占空比区间，避开非线性区。

4.3 闭环初调：三步法锁定基础参数

第一步：关闭积分与微分（Ki=Kd=0），仅调Kp

• 从小值开始（Kp=1），观察车体响应：轻微前倾时，车轮是否缓慢后退？
• 逐步增大Kp，直到车体能快速回正但不过冲（临界稳定）。此时Kp≈8~12。

第二步：加入微分（Kd>0），抑制超调

• 保持Kp不变，从Kd=0.01开始增加，观察车体回正时的“刹车感”。
• 当车体回正后无明显 overshoot（超调角＜0.5°），Kd≈0.1~0.2。

第三步：加入积分（Ki>0），消除静差

• 此时车体可能缓慢前倾（静差），从Ki=0.05开始增加。
• 当静差消失且无低频振荡，Ki≈0.5~0.8。

独家技巧：调参时用手机慢动作录像（240fps），逐帧观察车体倾角变化。比肉眼判断精确10倍，能清晰看到0.1°级的微小振荡。

4.4 稳定性验证：温度、电压、负载三维度压力测试

参数调好不等于万事大吉，必须做极限验证：

1. 温度测试：在35℃环境连续运行30分钟，记录倾角漂移量。合格标准：漂移＜0.3°。若超标，需优化陀螺仪零偏补偿算法。

2. 电压测试：电池从12.6V（满电）放电至10.8V（低压保护），观察平衡性能。若低压时明显抖动，说明PID参数未做电压前馈补偿。解决方案：在PID输出中叠加voltage_compensation = (12.6f - battery_volt) * 50.0f。

3. 负载测试：在车体顶部加100g砝码，观察响应速度。若回正时间延长＞50%，说明转动惯量J变化未被控制器感知。此时需重新计算LQR增益或手动增大Kp。

经验之谈：所有测试必须记录原始数据。我们建立了一个Excel模板，自动绘制“倾角-时间”曲线，并计算超调量、调节时间、稳态误差。没有数据支撑的“感觉良好”，在竞赛现场必栽跟头。

5. 常见故障排查：21个真实问题与秒级定位法

5.1 传感器类故障速查表

5.2 执行器类故障速查表

5.3 控制算法类故障速查表

真实案例：2022年某高校队伍在决赛现场，车体突然无法平衡。我们3分钟定位：用串口打印发现angle值在-0.5°到+0.5°间跳变，而陀螺仪原始值稳定。最终查明是MPU6050的DMP模式未正确初始化，导致四元数输出异常。解决方案：重刷DMP固件并严格按官方流程初始化。

5.4 环境干扰类故障终极排查法

当所有硬件、代码检查无误，故障仍存在时，按此顺序排查：

1. 电磁干扰（EMI）：用AM收音机靠近车体，听是否有“滋滋”声。若有，说明电机驱动噪声耦合进传感器线路。解决方案：电机电源与传感器电源用地线隔离，信号线走线远离电机线。

2. 机械共振：用手轻敲车体，听是否有“嗡”声。若有，说明结构刚度不足。解决方案：在轮轴与车架连接处加橡胶垫，或增加横向支撑杆。

3. 地面不平：在瓷砖、木地板、地毯上分别测试。我们曾发现车体在地毯上无法平衡，根源是地毯弹性导致轮速反馈失真。解决方案：改用光电编码器替代霍尔编码器，或增加轮速滑移补偿算法。

最后忠告：永远相信仪器，不信感觉。示波器、逻辑分析仪、万用表是你的第三只眼。我见过太多人花3天调PID，最后发现是编码器A相虚焊——用万用表通断档10秒解决。

6. 进阶延伸：从平衡到智能的三条可行路径

6.1 路径一：状态观测器升级——用Luenberger观测器替代互补滤波

当车体加速度较大时，互补滤波的倾角估计误差会显著增大。Luenberger观测器通过构建系统状态方程，用电机电压和编码器速度反推不可测状态（如角加速度），精度提升3倍。核心思想是：

x_hat' = A*x_hat + B*u + L*(y - C*x_hat)

其中：

• x_hat = [θ, θ']^T 是估计状态向量
• y = [θ_acc, ω_gyro]^T 是传感器融合输出
• L 是观测器增益矩阵，可通过极点配置法设计

实测表明，在0.5g加速度下，Luenberger观测器倾角误差＜0.1°，而互补滤波达0.8°。

6.2 路径二：控制律升级——LQR最优控制的工程化落地

LQR通过求解Riccati方程，给出全局最优状态反馈增益K。但直接套用理论公式会因模型失配导致性能下降。我们的工程化方案是：

1. 用实测数据辨识系统参数（m, L, J, b）
2. 构建离散化状态空间模型
3. 设定Q（状态权重）和R（控制权重）矩阵：Q = diag([100, 10])，R = 0.1
4. 调用MATLABdlqr()函数求解K
5. 将K映射为PID参数：Kp = K(1), Kd = K(2)

此法在2023年国赛中，使车体抗扰性提升40%，尤其在斜坡启动时优势明显。

6.3 路径三：功能扩展——视觉导航的轻量化集成

加装OV7670摄像头后，可实现循迹、避障。但裸奔OpenCV在STM32上不可能。我们的方案是：

• 用FPGA做图像预处理（二值化、轮廓提取）
• STM32只接收FPGA输出的“黑线中心坐标”
• 外环PID中加入前馈项：target_speed += k_ff * (center_x - 320)

实测在100lux光照下，循迹速度达1.2m/s，CPU占用率＜15%。

我的体会：智能车竞赛的本质，不是比谁算法新，而是比谁能把理论扎实落地。那些在实验室地板上摔过的每一次，都在帮你建立对物理世界的敬畏感。当你能看着示波器上的波形，脑中自动浮现出对应的微分方程时，你就真正入门了。