STM32自平衡小车设计与PID控制实现

1. 项目概述

两轮自平衡小车作为嵌入式系统开发的经典项目，完美展现了如何通过单片机实现复杂的动态平衡控制。这个看似简单的装置，实际上融合了传感器技术、控制算法和机电一体化设计等多个领域的知识。我最近完成了一个基于STM32F103C8T6单片机的自平衡小车项目，实测效果相当不错，静止状态下倾角误差可以控制在±0.8°以内，即使在受到轻微推力干扰后，也能在2秒内快速恢复平衡。

这种小车之所以吸引人，是因为它打破了传统轮式机器人需要三个以上支撑点的限制，仅靠两个轮子就能保持直立。这种特性使得它在狭窄空间内具有极佳的机动性，非常适合用于教学演示、室内巡检等场景。不过要实现这种"独轮车"般的平衡效果，对控制系统的实时性和精度要求非常高，这也是项目最具挑战性的部分。

2. 系统架构设计

2.1 整体方案规划

在设计之初，我首先明确了系统的核心需求：不仅要实现基本平衡功能，还要具备运动控制能力，同时保证30分钟以上的续航。经过多次方案比较，最终确定了如图1所示的系统架构。整个系统采用分层设计，包括感知层、控制层、执行层和交互层四个部分。

感知层负责采集小车的姿态信息，这是整个系统的基础。控制层是系统的大脑，负责处理传感器数据并生成控制指令。执行层将电信号转化为机械运动，而交互层则提供人机接口。这种分层架构不仅逻辑清晰，也便于后期的调试和维护。

2.2 核心器件选型

在器件选择上，我特别注重性价比和性能的平衡。主控芯片选用STM32F103C8T6，这款72MHz主频的Cortex-M3内核单片机，不仅价格亲民（约10元），而且具有丰富的外设资源，完全能满足实时控制的需求。

姿态传感器选择了MPU6050，它集成了三轴加速度计和三轴陀螺仪，通过I2C接口与主控通信。这款传感器最大的优势是内置了数字运动处理器(DMP)，可以硬件解算姿态数据，大大减轻了主控的计算负担。实测发现，直接使用原始数据时，由于噪声影响，角度计算误差较大；而启用DMP后，静态倾角误差可以控制在0.5°以内。

电机选用N20直流减速电机，减速比为13:1，额定电压6V，空载转速200RPM。搭配TB6612FNG驱动芯片，这款驱动芯片支持1.2A的持续电流输出，具有低功耗、高效率的特点，特别适合小型直流电机控制。

3. 硬件电路设计

3.1 主控电路设计

STM32最小系统电路包括复位电路、时钟电路和调试接口。为了确保系统稳定性，我在电源输入端增加了10μF的钽电容和0.1μF的陶瓷电容组合，有效抑制了电源噪声。Boot0和Boot1引脚通过10kΩ电阻下拉，确保芯片从主闪存启动。

调试接口采用标准的SWD四线制（SWDIO、SWCLK、GND、VCC），相比传统的JTAG接口，SWD占用引脚更少，调试效率更高。实际开发中，这个接口不仅用于程序下载，还能实时监控变量值，大大提高了调试效率。

3.2 传感器接口设计

MPU6050的接口电路设计有几个关键点需要注意：首先，I2C总线的上拉电阻取值很关键，经过测试，4.7kΩ的电阻在3.3V电压下能提供最佳的信号质量；其次，在VCC引脚附近要放置0.1μF的去耦电容，以滤除高频噪声；最后，AD0引脚通过10kΩ电阻接地，将器件地址设置为0x68。

注意：MPU6050对电源噪声非常敏感，建议使用LDO稳压器单独供电。我在初期测试时曾遇到数据跳变的问题，后来发现是共用电源导致，改为独立供电后问题立即解决。

3.3 电机驱动电路

TB6612FNG驱动电路的设计要点包括：

1. VM引脚接7.4V锂电池正极，这是电机驱动电压
2. VCC引脚接5V逻辑电压
3. 每个电机需要三个控制信号：PWM（速度）、IN1和IN2（方向）
4. STBY引脚接高电平使能驱动芯片

为了保护电路，我在电机两端并联了续流二极管，有效抑制了电机启停时产生的反电动势。此外，还在电源输入端串联了自恢复保险丝，防止电机堵转时电流过大损坏器件。

4. 软件算法实现

4.1 姿态解算算法

姿态解算是自平衡小车的核心技术之一。MPU6050提供的加速度计和陀螺仪数据各有优缺点：加速度计在静态时精度高，但动态响应慢；陀螺仪动态响应快，但存在漂移问题。因此需要将两者数据融合，才能得到准确的姿态信息。

我尝试了三种融合算法：

1. 互补滤波：实现简单，计算量小，但参数调整困难
2. 卡尔曼滤波：效果最好，但实现复杂
3. Mahony滤波：折中方案，效果接近卡尔曼滤波，但计算量更小

最终选择了Mahony滤波算法，它在保证精度的同时，计算量适中，非常适合在STM32上实时运行。算法核心代码如下：

c复制void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az) {
    float recipNorm;
    float halfvx, halfvy, halfvz;
    float halfex, halfey, halfez;
    float qa, qb, qc;

    // 计算误差
    halfvx = q1 * q3 - q0 * q2;
    halfvy = q0 * q1 + q2 * q3;
    halfvz = q0 * q0 - 0.5f + q3 * q3;
    halfex = (ay * halfvz - az * halfvy);
    halfey = (az * halfvx - ax * halfvz);
    halfez = (ax * halfvy - ay * halfvx);

    // 积分误差
    integralFBx += Ki * halfex * dt;
    integralFBy += Ki * halfey * dt;
    integralFBz += Ki * halfez * dt;

    // 应用反馈
    gx += Kp * halfex + integralFBx;
    gy += Kp * halfey + integralFBy;
    gz += Kp * halfez + integralFBz;

    // 四元数更新
    gx *= (0.5f * dt);
    gy *= (0.5f * dt);
    gz *= (0.5f * dt);
    qa = q0;
    qb = q1;
    qc = q2;
    q0 += (-qb * gx - qc * gy - q3 * gz);
    q1 += (qa * gx + qc * gz - q3 * gy);
    q2 += (qa * gy - qb * gz + q3 * gx);
    q3 += (qa * gz + qb * gy - qc * gx);

    // 归一化
    recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0 *= recipNorm;
    q1 *= recipNorm;
    q2 *= recipNorm;
    q3 *= recipNorm;
}

4.2 PID控制算法

平衡控制采用双环PID结构：外环是角度环，内环是角速度环。这种结构相比单环PID具有更好的动态性能和抗干扰能力。

角度环PID参数整定过程：

1. 先将Ki和Kd设为0，逐步增大Kp直到小车出现小幅振荡
2. 保持Kp不变，逐步增大Kd抑制振荡
3. 最后加入Ki消除静差

经过多次调试，最终确定的PID参数为：

• 角度环：Kp=25, Ki=0.5, Kd=0.8
• 角速度环：Kp=1.2, Ki=0, Kd=0.05

PID算法的实现采用了位置式算法，并加入了积分限幅和输出限幅，防止积分饱和和输出过大：

c复制typedef struct {
    float Kp;
    float Ki;
    float Kd;
    float integral;
    float last_error;
    float max_output;
    float max_integral;
} PID_TypeDef;

float PID_Calculate(PID_TypeDef *pid, float target, float feedback) {
    float error = target - feedback;
    float p_out = pid->Kp * error;
    
    pid->integral += pid->Ki * error;
    // 积分限幅
    if(pid->integral > pid->max_integral) pid->integral = pid->max_integral;
    else if(pid->integral < -pid->max_integral) pid->integral = -pid->max_integral;
    
    float d_out = pid->Kd * (error - pid->last_error);
    pid->last_error = error;
    
    float output = p_out + pid->integral + d_out;
    // 输出限幅
    if(output > pid->max_output) output = pid->max_output;
    else if(output < -pid->max_output) output = -pid->max_output;
    
    return output;
}

4.3 运动控制实现

除了平衡控制外，小车还需要实现前进、后退和转向功能。我的实现方案是通过调整左右轮的速度差来实现转向，同时保持整体平衡。

具体控制逻辑如下：

1. 前进/后退指令会生成一个目标速度
2. 转向指令会生成一个目标角速度
3. 将目标速度与平衡控制输出叠加，得到最终电机控制量

这种分层控制结构使得小车在运动时也能保持良好的平衡性。实测表明，在0.3m/s的速度下进行转向，小车的倾角波动不超过2°，表现出良好的稳定性。

5. 系统调试与优化

5.1 调试工具与方法

在开发过程中，我使用了多种调试工具：

1. ST-Link调试器：用于程序下载和单步调试
2. J-Scope：实时监控关键变量
3. 蓝牙模块：无线传输调试数据
4. OLED显示屏：本地显示状态信息

特别值得一提的是J-Scope工具，它可以通过SWD接口实时采集单片机内部变量，并以波形形式显示在PC上。这对于PID参数调试特别有帮助，可以直观地观察系统响应特性。

5.2 常见问题与解决方案

在开发过程中遇到了不少问题，以下是几个典型的案例：

问题1：小车启动后立即倒下
原因：MPU6050初始化后需要约1秒的稳定时间，直接读取数据会导致角度计算错误
解决方案：增加2秒的启动延时，等待传感器稳定

问题2：小车在平衡点附近持续抖动
原因：PID参数中微分分量不足，无法有效抑制振荡
解决方案：适当增大Kd参数，并检查机械结构是否松动

问题3：长时间运行后平衡性能下降
原因：锂电池电压下降导致电机响应变慢
解决方案：增加电压补偿算法，根据电池电压调整PWM输出

5.3 性能测试结果

经过系统优化后，进行了全面的性能测试：

测试结果表明，系统各项指标均达到或超过了设计要求，特别是在平衡稳定性和响应速度方面表现优异。

6. 项目总结与展望

这个自平衡小车项目让我深刻体会到嵌入式系统开发的乐趣和挑战。从最初的方案设计，到中期的软硬件调试，再到最后的性能优化，每个环节都需要综合考虑多方面因素。特别是在PID参数整定过程中，经历了无数次失败和调整，才最终找到最优参数组合。

项目的成功离不开几个关键点：首先是选择了合适的传感器和滤波算法，确保了姿态检测的准确性；其次是采用了双环PID控制结构，兼顾了响应速度和稳定性；最后是注重细节优化，比如电源滤波、机械结构加固等，这些看似小的改进往往能带来质的提升。

未来还可以从以下几个方面进行改进：

1. 增加无线控制功能，如蓝牙或2.4G遥控
2. 改进机械结构，增强抗振动能力
3. 尝试更先进的控制算法，如模糊PID或自适应控制
4. 增加环境感知能力，实现自主避障