基于STM32与MPU6050的自平衡小车：从PID控制到姿态解算的嵌入式实践

1. 项目概述：从零到一，打造你的第一台自平衡小车

几年前我第一次接触平衡车项目时，被那种“违反直觉”的稳定感深深吸引。一个看似随时会倒下的两轮结构，竟然能通过电机和算法的配合稳稳立住，甚至载人行驶。这背后，是微控制器实时处理传感器数据、快速做出决策并驱动执行机构的经典闭环控制案例。对于电子、嵌入式或自动化领域的学习者来说，亲手做一台平衡车，是理解PID控制、姿态解算、电机驱动等核心概念的绝佳实践。

这个项目的主角是STM32和MPU6050。STM32作为意法半导体的主流ARM Cortex-M系列微控制器，以其丰富的外设、强大的性能和活跃的社区生态，成为众多电子爱好者和工程师的首选。而MPU6050则是一款集成了三轴陀螺仪和三轴加速度计的六轴运动处理传感器，价格亲民，数据接口标准，是获取物体姿态信息的入门神器。将两者结合，我们就能搭建一个感知自身倾斜角度、并通过驱动轮子来对抗倾斜、维持平衡的智能系统。

整个制作过程，你会经历硬件选型与焊接、传感器数据采集与滤波、姿态角解算、PID控制器设计与调试、电机驱动与PWM控制等多个环节。这不仅仅是焊几根线、写几行代码那么简单，它要求你对系统有一个整体的认识，并理解每个环节如何影响最终“站不站得稳”这个结果。无论你是在校学生想做一个亮眼的毕业设计，还是工程师想夯实自己的嵌入式控制功底，亦或是纯粹的硬件DIY爱好者，这个项目都能带给你满满的成就感和技术提升。

2. 核心硬件选型与电路设计思路

硬件是系统的骨架，稳定的硬件平台是算法得以流畅运行的基础。在开始焊接之前，我们需要对每个模块的选型理由和连接方式有清晰的认识。

2.1 主控芯片：为什么是STM32？

在众多单片机中，我选择了STM32F103C8T6，也就是常说的“蓝色药丸”核心板。这个选择基于几个现实的考量。首先，它基于Cortex-M3内核，主频72MHz，对于需要每秒数百次循环读取传感器、计算PID并更新电机输出的平衡车应用来说，性能完全够用且游刃有余。其次，它拥有足够多的定时器资源来生成多路高精度的PWM波驱动电机，同时其SPI和I2C接口方便与MPU6050通信。最重要的是，它的生态极其丰富，标准库、HAL库、LL库以及各种第三方教程和开源项目俯拾皆是，极大降低了学习和调试的门槛。

注意：STM32F103系列有多个型号，如C8T6（64K Flash，20K RAM）和RCT6（256K Flash，48K RAM）。对于平衡车项目，C8T6的资源已经足够。购买时请认准正版芯片或信誉好的店铺，市面上有些劣质仿冒芯片可能导致程序运行不稳定。

2.2 姿态传感器：MPU6050的潜力与局限

MPU6050是这个项目的“眼睛”。它通过I2C接口与主控通信，实时提供三个轴的角速度（陀螺仪）和加速度数据。陀螺仪测量的是旋转的角速度，积分后可以得到角度变化，但存在累积误差（漂移）；加速度计测量的是包括重力在内的各轴加速度，在静止或匀速运动时，可以通过反三角函数计算出相对于重力方向的倾斜角，但对运动引起的加速度非常敏感，动态下误差大。

因此，单一使用任一种数据都无法获得稳定、准确的姿态角。这就需要我们进行“传感器融合”。MPU6050内部自带一个数字运动处理器（DMP），可以运行官方的传感器融合算法，直接输出解算好的四元数或欧拉角，这大大简化了我们的工作。如果不用DMP，我们也可以在STM32上实现互补滤波或卡尔曼滤波算法来融合数据。对于初学者，强烈建议先从使用DMP开始，快速获得稳定角度数据，把精力集中在控制算法上。

2.3 动力与执行机构：电机、驱动与电源

平衡车的“手脚”是电机和驱动电路。我推荐使用带有减速箱的直流编码电机。减速箱可以增大扭矩，让小车有足够的力量对抗倾斜。而编码器则用于测量电机的实际转速，为后续的速度环PID控制提供反馈，是实现平稳移动和转向的关键。

电机驱动芯片我选用的是TB6612FNG。相比传统的L298N，TB6612效率更高、发热更小、体积更小巧，并且支持大电流（连续1.2A，峰值3.2A）。它只需要两个PWM信号和一个方向信号就能控制一个电机的正反转和速度，接口非常简洁。

电源部分至关重要且容易被忽视。整个系统包括STM32（3.3V）、MPU6050（3.3V）、TB6612（逻辑部分3.3-5V，电机部分需更高电压）和电机。一个常见的方案是使用一块7.4V或11.1V的锂电池作为总电源，然后通过一个DC-DC降压模块（如LM2596）产生一个稳定的5V电压，再通过AMS1117-3.3等LDO芯片为STM32和传感器提供3.3V。TB6612的电机驱动电压（VM）直接接电池电压，以获得最大驱动力。

2.4 电路连接与布局心得

具体的引脚连接需要根据你的核心板型号来定，但思路是通用的。以下是一个基于STM32F103C8T6的参考连接表：

实操心得1：布线时，电机驱动线（连接TB6612 AO/BO到电机的线）要尽量粗、短，并且最好与信号线（如I2C、编码器线）分开走，避免电机大电流开关产生的噪声干扰敏感的传感器和编码器信号。电源入口处记得并联一个容量较大的电解电容（如470uF/16V）来缓冲电机启动时的电流冲击。

3. 软件架构与核心算法解析

硬件搭建好后，大脑（软件）的设计决定了小车的“智商”。平衡车的软件核心是一个多层闭环控制系统，我们需要从底层到上层逐一实现。

3.1 系统软件框架设计

一个稳定可靠的平衡车程序通常采用“前后台”或基于实时操作系统（如FreeRTOS）的结构。对于初学者，一个清晰的前后台循环配合定时中断就能很好地工作。我建议的软件框架如下：

1. 初始化层：配置系统时钟、GPIO、中断、定时器、PWM、I2C、编码器接口等所有外设。
2. 数据采集层：
   • 在定时中断（如1ms一次）中，通过I2C读取MPU6050的DMP数据，解算出当前车体的俯仰角（Pitch）和角速度。
   • 在另一个定时中断或主循环中，读取编码器数值，计算电机的实际转速。
3. 控制算法层：这是核心。在一个固定的控制周期（如5ms）中断中，执行控制算法。
   • 最内环是直立环（角度环）：根据目标角度（通常为0度，即竖直）和当前角度偏差，通过PID计算出一个基础电机输出值，目的是让车体保持直立。
   • 中间环是速度环：根据目标速度（通常为0，即静止）和编码器测得的速度偏差，通过另一个PID计算出一个补偿值。这个补偿值会叠加到直立环的输出上，目的是消除直立环的静差，让小车能在静止时真正站稳，而不是缓慢移动。
   • 最外环是转向环：根据遥控器指令或自动巡线等需求，计算出左右电机的速度差，实现转向。
4. 输出执行层：将最终计算出的左右电机PWM占空比和方向，写入对应的定时器寄存器，驱动电机转动。
5. 通信与调试层：通过串口将关键数据（如角度、角速度、PWM值、PID参数）发送到电脑上位机（如匿名科创地面站、SerialPlot等），方便实时观察波形和调试参数。

3.2 姿态解算：获取准确的倾斜角

如前所述，使用MPU6050的DMP库是最快最稳的方式。你需要从官方或开源社区获取inv_mpu.c、inv_mpu_dmp_motion_driver.c等文件，并移植到你的工程中。移植的关键是实现i2c_write和i2c_read函数，以及提供准确的延时函数。

成功初始化DMP后，你可以设置其输出频率（如100Hz），然后在中断中调用dmp_read_fifo函数来获取解算好的四元数。再将四元数转换为欧拉角，我们主要关心俯仰角pitch。

// 示例：从DMP读取数据并计算Pitch角（部分代码） if (dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more)) { // 读取失败处理 } if (sensors & INV_WXYZ_QUAT) { // 将四元数转换为欧拉角（单位：度） float q0 = quat[0] / q30; // q30是DMP输出数据的缩放因子 float q1 = quat[1] / q30; float q2 = quat[2] / q30; float q3 = quat[3] / q30; // 计算俯仰角Pitch (绕Y轴旋转) pitch = asin(-2 * q1 * q3 + 2 * q0 * q2) * 57.3f; // 57.3是弧度转度数的系数 }

实操心得2：DMP输出的数据稳定性很高，但上电后需要几秒钟的初始化时间。在程序开始时，确保小车静止放置，让DMP完成自检和校准。此外，安装MPU6050时，要确保其芯片平面与小车的主平面平行，否则解算出的角度基准会不对，需要在你计算的pitch角上加上一个固定的安装误差补偿值。

3.3 控制核心：PID算法的理解与实现

PID控制器是平衡车的灵魂。它根据设定值（Setpoint）和实际值（Feedback）的偏差（Error），通过比例（P）、积分（I）、微分（D）三个环节的运算，得到控制输出（Output）。

• 比例P：与当前偏差成正比。P越大，纠正偏差的力度越大，响应越快，但过大会引起振荡。
• 积分I：与偏差的累积和成正比。用于消除静态误差（比如小车总是缓慢朝一个方向走），但过大会导致系统反应迟钝和超调。
• 微分D：与偏差的变化率成正比。具有预见性，能抑制振荡，提高稳定性，但对噪声敏感。

在平衡车中，我们通常使用两个串联的PID：直立环（角度环）和速度环。直立环是基础，它让小车有站起来的趋势；速度环是修正，它让小车站住后不会偷偷溜走。

typedef struct { float Target; // 目标值 float Feedback; // 反馈值 float Kp, Ki, Kd; // PID参数 float Error; // 当前误差 float LastError; // 上次误差 float Integral; // 积分项 float Output; // 输出值 float IntegralLimit; // 积分限幅 float OutputLimit; // 输出限幅 } PID_TypeDef; float PID_Calculate(PID_TypeDef *pid) { pid->Error = pid->Target - pid->Feedback; // 比例项 float p_out = pid->Kp * pid->Error; // 积分项（抗饱和处理） pid->Integral += pid->Error; if (pid->Integral > pid->IntegralLimit) pid->Integral = pid->IntegralLimit; if (pid->Integral < -pid->IntegralLimit) pid->Integral = -pid->IntegralLimit; float i_out = pid->Ki * pid->Integral; // 微分项（常用不完全微分，或对反馈值微分） float d_out = pid->Kd * (pid->Error - pid->LastError); pid->LastError = pid->Error; // 总和并限幅 pid->Output = p_out + i_out + d_out; if (pid->Output > pid->OutputLimit) pid->Output = pid->OutputLimit; if (pid->Output < -pid->OutputLimit) pid->Output = -pid->OutputLimit; return pid->Output; }

直立环PID：Target设为0度，Feedback就是MPU6050解算出的pitch角。其输出直接决定电机的力度和方向。速度环PID：Target设为0（期望静止），Feedback由编码器测得的电机转速经过处理得到（如左右轮速度的平均值）。速度环的输出不是直接给电机，而是叠加到直立环的目标值上，或者作为一个修正量加到直立环的输出上。这是一种“串级PID”的变体，速度环的输出改变了直立环的平衡点，从而让小车通过微微前倾或后仰来加速或减速，最终使速度趋于零。

4. 分步实现与调试全记录

理论需要实践来验证。下面我们一步步完成从工程创建到小车站立的整个过程。

4.1 工程创建与基础外设驱动

首先，使用STM32CubeMX工具初始化项目非常高效。选择你的芯片型号（STM32F103C8Tx），在图形化界面中配置：

• SYS: Debug选择Serial Wire（用于ST-Link下载调试）。
• RCC: HSE选择Crystal/Ceramic Resonator（如果你的板子有外部晶振）。
• 时钟树: 将HCLK设置为72MHz。
• GPIO: 配置TB6612的方向控制引脚（如PB12, PB13, PB14, PB15）为输出模式。
• 定时器:
  • 配置TIM1的CH1和CH4为PWM Generation模式，用于驱动两个电机。预分频和自动重载值设置为产生频率在10-20kHz的PWM（超出人耳听觉范围，电机噪音小）。
  • 配置TIM2和/或TIM3为Encoder Interface模式，用于读取左右电机的编码器。
  • 配置一个基本定时器（如TIM4）用于产生系统时基，例如1ms中断。
• I2C: 配置I2C1为标准模式（100kHz）或快速模式（400kHz），用于连接MPU6050。
• USART: 配置一个串口（如USART1）用于打印调试信息，波特率115200。

生成代码后，在Keil或STM32CubeIDE中打开项目。首先确保基础功能正常：编写测试代码，让LED闪烁，用逻辑分析仪或示波器检查PWM波形是否正确，读取编码器值是否随电机转动变化，通过I2C扫描函数确认能检测到MPU6050的地址（0x68或0x69）。

4.2 MPU6050 DMP库的移植与角度读取

这是关键一步。将DMP库文件添加到你的工程，并实现i2c_write和i2c_read函数（通常直接调用HAL库的HAL_I2C_Mem_Write和HAL_I2C_Mem_Read）。按照库提供的示例流程进行初始化：

1. 初始化I2C。
2. 复位MPU6050。
3. 设置陀螺仪和加速度计量程（例如，陀螺仪±2000°/s，加速度计±2g）。
4. 加载DMP固件。
5. 配置DMP输出速率（如100Hz），并启用所需输出（如四元数）。
6. 使能DMP。

成功初始化后，在定时中断（如1ms）中调用读取函数，并计算pitch角。通过串口将pitch角实时发送到上位机软件，观察波形。用手缓慢倾斜小车模块，看角度变化是否平滑、响应是否及时。静止时，角度曲线应是一条平稳的直线，噪声越小越好。

4.3 直立环PID的单独调试

在调试PID之前，务必确保机械结构牢固，电池电量充足，并将小车车轮悬空（用架子架起来），防止调试时小车突然失控冲出。

首先只调试直立环。将速度环和转向环的输出置零。

1. 参数初始化：将直立环PID的参数Kp,Ki,Kd全部设为0。速度环目标Target设为0。
2. 先调P（比例）：逐渐增大Kp。你会发现，当Kp小到一定程度时，用手推动车轮，车轮会缓慢地朝你推的反方向转动，试图“抵抗”你的推动，但力度很弱。继续增大Kp，这个抵抗的力度和速度会变强。找到一个临界点：当Kp增大到某个值时，你一推车轮，车轮会快速地来回振荡几次才停下。这个值就是临界Kp。将最终使用的Kp设为这个临界值的50%-70%。例如，临界Kp是120，那么可以先设定Kp=60。
3. 再调D（微分）：微分项能抑制振荡。在已有Kp的基础上，逐渐加入正的Kd。你会发现车轮的响应变得更“沉稳”，用手快速拨动时，振荡会明显减弱。Kd能提高系统的阻尼。但Kd过大会导致系统反应迟钝。通常Kd的值在Kp的十分之一到几十分之一之间试探。
4. 最后调I（积分）：在平衡车直立环中，积分项Ki通常很小，甚至可以为0。因为我们的目标角度是0，且传感器有噪声，积分容易累积误差导致系统不稳定。可以先设为0。

调试时，通过串口实时绘制pitch角、pitch角速度、直立环输出Output的波形至关重要。观察当你给小车一个扰动（用手轻推）时，这些量的变化是否迅速且平稳。

4.4 加入速度环与参数整定

当直立环能让小车在悬空时有明显的“平衡反应”后，可以尝试将小车放在地上（做好保护，防止跑飞）。此时只有直立环的小车可能会朝一个方向加速跑出去直到倒下，因为它没有“想停下来”的意愿。

此时加入速度环。速度环的Feedback是编码器测得的转速（单位可以是编码器计数/控制周期）。速度环的Target设为0。速度环的Output不是直接给电机，而是加到直立环的Target上。即：直立环目标角度 = 0 + 速度环输出值

这样，当小车因为微小偏差开始移动时，编码器测到速度，速度环输出一个值，这个值会改变直立环的平衡点。比如小车向前移动，速度环输出一个负值，使得直立环的目标角度变成微微向后倾，小车就会向后加速以纠正向前的速度，最终达到静止平衡。

速度环的调试同样遵循PID顺序：

1. 先调P：从小开始，慢慢增大。目标是让小车在受到轻微推力后，能自己慢慢回到原点停下，而不是持续振荡或跑飞。速度环的P通常比直立环的P小很多。
2. 再调I：速度环的I项很重要，用于消除静态误差。如果调好P后，小车总是停不住（有恒定的速度），就需要加入I。I值也要从小加起，过大会引起低频振荡。
3. D项：速度环的D项一般不加，因为编码器速度信号噪声较大，微分会放大噪声。

这个过程需要极大的耐心。一个实用的技巧是“阶跃响应法”：用手将小车抬离地面，然后突然放下，观察小车的反应。理想情况是它轻微晃动几下就稳稳站住。如果倒下，可能是直立环P不够或D不够；如果持续前后摆动，可能是直立环P太大或速度环参数不合适；如果朝一个方向匀速跑开，可能是速度环P太小或I不够。

5. 常见问题排查与进阶优化

即使按照步骤操作，你也一定会遇到各种问题。下面是我在多次制作中踩过的坑和解决方案。

5.1 硬件与基础驱动问题排查

5.2 软件与算法调试技巧

• 角度数据波动大：首先检查MPU6050是否安装牢固，避免振动。其次，可以软件上对DMP输出的角度进行低通滤波，例如angle_filtered = 0.9 * angle_filtered + 0.1 * angle_new。但DMP本身已经融合了数据，通常不需要额外滤波。
• 小车平衡点不在正中：可能是机械重心不在两轮轴线上。可以通过在软件中给直立环的目标角度Target加一个固定的偏移量来补偿。例如，小车总是向后倒，就把Target设为0.5度（前倾）。
• 响应迟钝或振荡：这是PID参数不匹配的典型表现。响应迟钝可能是P太小或D太大；振荡可能是P太大或D太小。使用上位机观察波形至关重要。观察当你施加扰动时，角度偏差、输出PWM的变化曲线。理想的响应应该是快速上升并平滑地回到零点，超调量小。
• 速度环引起低频摆动：小车能站住，但会以大约1-2秒的周期缓慢地前后摆动。这通常是速度环的I值过大引起的。尝试减小速度环的Ki，或者给速度环的积分项加上一个泄漏系数（即每次计算时，让积分项乘以一个略小于1的数，如0.995，防止其无限累积）。

5.3 从“能站”到“站得稳”的进阶优化

当小车基本能站立后，还可以从以下几个方面提升其性能：

1. 控制周期优化：确保你的主要控制循环（读取传感器、计算PID、更新电机）在一个固定的、尽可能短的周期内执行（如2ms或5ms）。使用定时器中断来触发这个循环，保证实时性。
2. 数据同步：确保在同一个控制周期内使用的角度、角速度、编码器数据是同一时刻采集的，避免使用“过时”的数据进行计算，否则会引入相位滞后，影响稳定性。
3. 输出平滑：对最终输出给电机的PWM值进行限幅和斜坡处理。突然的PWM大幅跳变会导致电机扭矩突变，引起振动。可以限制单个控制周期内PWM的最大变化量。
4. 加入“死区”处理：当角度偏差和角速度都非常小时，可以令电机输出为零。这样可以避免电机因微小的噪声而持续嗡嗡作响，也能节省电量。
5. 电池电压补偿：电池电压下降会导致电机在相同PWM下的转速下降。可以实时监测电池电压，并对输出的PWM值进行比例补偿，使小车性能在不同电量下保持一致。

最后，机械结构的对称性、重心的高低、轮胎的抓地力都会极大影响最终效果。一个重心低、结构紧凑、轮子摩擦力适中的小车，调试起来会事半功倍。这个项目最大的魅力在于，它完美地体现了“软硬结合”的思想。每一个参数的调整，都能直观地反映在小车的动态行为上。当你经过无数次调试，看到它终于稳稳立住的那一刻，所有的努力都值了。