平衡小车纯平衡控制实现

原创于 2025-11-04 15:50:07 发布 · 237 阅读

7 ·

CC 4.0 BY-SA版权

文章标签：

#平衡小车 #STM32 #PID控制 #互补滤波 #MPU6050

AI助手已提取文章相关产品：

平衡小车“纯平衡”控制系统的工程实现与深度解析

在嵌入式控制的世界里，自平衡两轮小车就像是一块试金石——它不追求炫酷的功能堆叠，却能精准检验开发者对传感器、算法和执行机构之间协同关系的理解。尤其是“纯平衡”模式，看似只是让一个小车直立不倒，实则浓缩了从物理感知到动态反馈的完整闭环逻辑。

这不仅仅是一个学生实验项目，更是理解现代控制系统设计思想的绝佳入口。当你看到两个轮子托着一根杆子稳稳站立时，背后其实是MEMS传感器、滤波算法、PID控制器与电机驱动精密配合的结果。而这一切，都可以在一片STM32上以毫秒级响应完成。

MPU6050 是这类系统中最常见的姿态传感器，集成了三轴加速度计和三轴陀螺仪，通过I²C接口即可接入主控MCU。它的优势不仅在于体积小巧、成本低廉，更在于出厂即完成芯片对齐，避免了分立传感器安装带来的机械误差问题。

实际使用中，我们通常将采样率设为125Hz左右（通过SMPLRT_DIV寄存器配置），既保证足够的动态响应能力，又不至于给主控带来过重负担。加速度计量程设为±2g，陀螺仪设为±500°/s，基本覆盖了平衡车运行时的倾角变化范围。同时启用片内低通滤波器（约5Hz带宽），有助于抑制高频振动噪声，这对后续控制稳定性至关重要。

但真正关键的问题是： 如何从原始数据中得到一个可靠的角度值？

加速度计可以测量重力方向，从而计算出静态倾角。比如用 atan2(ax, sqrt(ay*ay + az*az)) 就能得到相对于垂直方向的倾斜角度。然而一旦小车开始运动或受到震动，加速度信号就会混入非重力分量，导致角度误判。

相反，陀螺仪测量的是角速度，通过对时间积分可以获得角度变化趋势，短期精度非常高。但它有个致命缺点——存在零偏漂移。哪怕静止不动，微小的偏置也会随时间累积成显著误差。

于是自然想到：能不能把两者结合起来？

这就是互补滤波的核心思路。它不像卡尔曼滤波那样需要建立复杂的协方差模型，而是基于一个简单的观察：
- 加速度计适合看“长期”，因为它不会漂移；
- 陀螺仪适合看“短期”，因为它响应快且不受线性加速度影响。

因此可以用高通特性保留陀螺仪的动态响应，用低通特性吸收加速度计的稳态信息。公式如下：

fused_angle = alpha * (fused_angle + gyro_rate * dt) + (1 - alpha) * acc_angle;

其中 alpha 通常取0.95到0.98之间。这个数值本质上决定了你有多信任陀螺仪。如果环境振动大，就提高 alpha ，更多依赖积分结果；如果担心漂移，则降低 alpha ，更多依赖加速度计。

这段代码跑在STM32F1系列上，一次运算耗时不到50微秒，完全能在10ms控制周期内轻松完成。相比之下，卡尔曼滤波虽然理论上更优，但在资源受限的平台上反而可能因计算延迟引入相位滞后，得不偿失。

当然，也有一些细节值得注意。例如，在启动阶段必须先校准陀螺仪零偏——让小车静止几秒钟，采集上百组数据求平均，否则初始积分就会带着偏差一路跑偏。此外， acc_angle 的计算也要注意单位转换，最好预先把弧度转成角度，减少重复运算开销。

有了准确的姿态角，下一步就是决定怎么动。

最常用的手段就是PID控制。对于纯平衡系统来说，设定点就是0°（直立状态），控制器的任务是根据当前倾角误差生成合适的PWM输出，驱动电机反向运动来抵消倾倒趋势。

比例项（Kp）是最直观的部分。它直接放大误差，形成快速响应。比如小车前倾5度，Kp设为50，那输出就是250单位的PWM。但Kp太大容易引起振荡，你会看到小车来回“抽搐”，像喝醉了一样摇晃不止。

积分项（Ki）的作用是消除静差。有时候即使调好了Kp和Kd，小车还是会缓慢倒下，说明存在持续的小误差没有被纠正。这时候加入一点积分作用，让它慢慢“记住”这种偏差并逐步补偿，就能扶正车身。不过要小心积分饱和——长时间得不到纠正会导致积分值越积越大，一旦触发响应就会猛冲出去。所以一般都要做积分限幅，甚至加入抗饱和机制（anti-windup）。

微分项（Kd）则是系统的“阻尼器”。它关注误差的变化率，提前预测趋势并施加反向力矩，有效抑制超调。想象一下弹簧振子，没有阻尼的话会来回震荡，加上阻尼才能迅速稳定。在平衡车上，Kd太小会晃个不停，太大又会让系统变得迟钝。典型值往往比Kp还高，比如100~150，这是因为在高速采样下微分项本身数值较小，需要更强增益才能起效。

下面是标准的增量式PID实现：

float PID_Calculate(PID_Controller *pid, float setpoint, float measured, float dt) {
    float error = setpoint - measured;
    pid->integral += error * dt;

    // 防止积分饱和
    if (pid->integral > INTEGRAL_MAX) pid->integral = INTEGRAL_MAX;
    if (pid->integral < -INTEGRAL_MAX) pid->integral = -INTEGRAL_MAX;

    float derivative = (error - pid->prev_error) / dt;
    float output = pid->Kp * error + 
                   pid->Ki * pid->integral + 
                   pid->Kd * derivative;

    pid->prev_error = error;
    return output;
}

这里特别强调一点： dt 必须恒定 。如果你用的是裸机循环而不是定时器中断，系统负载波动可能导致采样间隔忽长忽短，微分项就会产生剧烈跳变，造成失控。强烈建议使用SysTick或通用定时器触发固定周期任务（如每10ms执行一次主控循环）。

最终的控制指令要落地为电机动作。

大多数平衡车采用TB6612FNG或L298N这类H桥驱动芯片。它们接收PWM信号和方向引脚输入，控制直流电机的转速与正反转。相比L298N，TB6612效率更高、发热更低，而且支持待机模式，更适合电池供电场景。

PWM频率建议设在10kHz以上，避开人耳敏感区间（2kHz~4kHz），防止发出刺耳的“滋滋”声。同时要注意死区时间设置，防止上下桥臂同时导通造成电源短路。

控制逻辑也很直接：
- 当小车前倾时，应加速后退，把重心拉回来；
- 后倾时则向前行驶；
- 左右轮同步动作，维持直线平衡。

封装好的电机接口大致如下：

void SetLeftMotor(int pwm, int direction) {
    if (direction > 0) {
        GPIO_Set(DIR_LEFT_1, HIGH);
        GPIO_Set(DIR_LEFT_2, LOW);
    } else {
        GPIO_Set(DIR_LEFT_1, LOW);
        GPIO_Set(DIR_LEFT_2, HIGH);
    }
    TIM_SetCompare(TIM3, Channel_1, abs(pwm));
}

PID输出的正值或负值直接映射为方向和PWM大小，简洁明了。

但别忘了安全机制。当倾角超过±30°时，说明已经无法挽回，继续输出PWM只会让轮子空转甚至损坏电机。此时应当立即关闭驱动，进入保护状态。同样，上电自检失败（如MPU6050通信异常）也不应启动系统。

在整个系统调试过程中，有几个常见问题值得特别留意：