03｜平衡小车控制链：MPU6050、编码器与三级 PID

平衡小车必须同时处理“不要倒下”“保持目标速度”和“按需要转向”三个问题。它们不能由一个 PID 参数包办，而要按不同时间尺度组织成直立、速度和转向控制链。

整体控制链

示例把三个控制量叠加到左右电机：

姿态角 + Y轴角速度 → 直立环 ┐
左右编码器速度     → 速度环 ├→ 左右 PWM → 电机
Z轴角速度          → 转向环 ┘

直立环负责快速纠正倾倒趋势；速度环缓慢改变平衡点，让车辆前进或后退；转向环制造左右差速。

示例主流程中 Turn_out = 0，说明转向结构被预留，但当前片段没有启用完整转向控制。

电机方向与 PWM

电机函数先根据命令正负控制方向 GPIO，再把绝对值写入 PWM 比较值。左右轮的符号需要结合电机安装方向校正。

PWM 上限必须小于等于定时器周期，并满足驱动器和电机限制。示例数值用于说明控制流程，不能当作当前硬件的安全参数。

首次测试应断开控制环，架空车轮，用很小的受限命令分别验证左右正反转。若方向错误，闭环会形成正反馈并迅速失控。

编码器读取

示例使用 GPIO 中断累加编码器计数，并根据另一相信号判断方向。周期任务读取后清零，得到固定时间窗内的脉冲变化。

要转换为 RPM 或线速度，仍需要每圈脉冲数、解码倍率、减速比和轮径。完成这些参数的实物标定前，不能把计数直接称为速度。

清零读取与中断累加之间还要考虑原子性。若 MCU 在多字节访问时可能被中断，读取和清零应放入短临界区或使用安全交换方式。

MPU6050 数据路径

驱动通过 I²C 写寄存器完成复位、量程和滤波配置，再连续读取加速度与角速度寄存器并换算。

多个 I²C 片段使用等待状态寄存器的 while，没有超时、NACK 处理和总线恢复。传感器断线时，这类循环可能永久阻塞整个非抢占式系统。

移植时每个等待点都应有截止时间，并向上返回错误。连续失败后可以重新初始化总线或进入安全停机，但不能继续沿用旧姿态驱动电机。

直立环

直立环使用期望角度、实际俯仰角和 Y 轴角速度：

vertical = Kp × (target_angle - pitch)
         + Kd × gyro_y

这是典型 PD 结构。角度误差提供恢复力矩，角速度提供阻尼。陀螺仪正方向必须和角度变化方向一致，否则微分项会放大倾倒。

期望角不一定是几何零度，它还会受机械重心、传感器安装偏差影响。应先在安全支架上标定静态平衡点。

速度环

速度环把左右编码器相加作为整体速度误差，先低通，再积分：

speed_error = encoder_left + encoder_right
filtered = 0.7 × old + 0.3 × new
integral = clamp(integral + filtered)
velocity = Kp × filtered + Ki × integral

速度环输出与直立环相加，等价于缓慢改变车辆想要保持的倾角。外环应比直立内环慢，否则两个环会争夺控制权。

堵转、抱车或车轮悬空都会破坏正常反馈，积分限幅和故障复位必须明确。

左右输出叠加

示例执行：

base = vertical + velocity
motor_left  = base - turn
motor_right = base + turn

叠加后必须统一限幅。不能只分别限制三个环，因为它们相加后仍可能超过执行器范围。

若输出被限幅，速度环积分仍可能累积；工程实现应把饱和状态反馈给积分逻辑。

推荐验证顺序

1. 验证 I²C 错误能超时返回。
2. 静止记录 MPU6050 原始值、单位换算与姿态角。
3. 验证编码器方向和两侧计数一致性。
4. 架空或使用安全支架，仅调直立环。
5. 直立环稳定后，低带宽加入速度环。
6. 最后启用转向环，并验证叠加限幅。

平衡车具有快速运动风险。测试区域应清空，电机输出应受限，并保留人工断电方式；具体限流值需由实际电源、驱动器和电机确定。

接入真实平衡车前还要确认什么

本篇总结

• 平衡控制由快速直立环、较慢速度环和左右转向差速共同组成。
• 编码器计数必须经过分辨率、减速比和轮径标定后才有物理单位。
• I²C 等待没有超时是关键可靠性风险。
• 三个环的输出叠加后仍需统一限幅和抗积分饱和。
• 公式和代码只能说明控制结构；引脚、电气参数和稳定性必须在当前硬件上验证。