简介：基于 STM32F407ZGT6 与 MPU6050 的二轮自平衡车项目，是一份面向课程设计、毕业设计及嵌入式进阶学习的完整技术资料。项目围绕速度环与位置环串级 PID 控制展开，能够帮助读者理解如何利用惯性传感器获取车身姿态，并通过闭环控制算法驱动电机维持平衡，是电子信息、自动化、计算机等专业学生完成自平衡类题目或学习运动控制算法的实用参考。压缩包共 188 个文件，大小约 5.33 MB，以 C 源码和头文件为主，同时包含 Keil MDK 工程配置、编译中间文件、可直接烧录的 hex/axf 固件，以及脚本和说明文档，从源代码到编译结果形成完整闭环，便于对照学习。目前已有 357 人学习下载。具体内容包括 STM32F4 标准外设驱动、MPU6050 姿态读取与处理、二轮车平衡控制逻辑、速度环与位置环参数整定思路等，读者可以沿工程结构对照源码和配置，掌握从传感器采集、姿态解算到 PWM 控制输出的全流程；开发者也可将其快速移植到自己的硬件平台，节省底层驱动与算法调试时间。
1. 立不住的二轮车与串级 PID：stm32f407zgt6 加 MPU6050 的控车基线
两轮车放在桌上就会倒，倒的原因是重心在支点上方。让它不倒，本质上是让电机在倾角误差的驱动下，把支撑点追着重心跑，让误差收敛到零。用 STM32F407ZGT6 做大脑，MPU6050 给倾角和角速度，直流减速电机加编码器当执行器，串级 PID 在这里不是课本里的名词：速度环负责给目标倾角，角度位置环负责快速回正，内环跑得比外环快，车才能立住又不乱跑。下面按姿态解算、串级 PID 结构、F407 时序实现、波形调参四条线展开，适合手里已经有小车、想从头重写控制逻辑而不是只抄参数的人。
2. MPU6050 姿态解算与读取：在 STM32F407ZGT6 上用 HAL 库跑通角度
2.1 从 stm32f407zgt6 原理图确认 I2C 引脚和 MPU6050 的地址
自平衡车的第一件事是知道车“现在往哪个方向倒”，MPU6050 给出的原始数据就是依据。它内部有一个三轴加速度计和一个三轴陀螺仪：加速度计感知重力方向，但怕振动；陀螺仪感知角速度，积分后会漂。两路数据互补才能形成可靠的倾角。
拿到原理图先不要写代码，确认三条：
- SCL/SDA 接在哪一对引脚。最常见是 I2C1 的 PB6/PB7，也有的板子放到 PF0/PF1 这种普通 IO 上模拟 I2C。模拟 I2C 也能跑，但控制周期 1ms 时软件时序不稳，优先用硬件 I2C。
- 总线上有没有上拉电阻。MPU6050 模块有的板载 4.7kΩ 上拉，有的没焊。F407 内部上拉只有几十 kΩ，线上没有外置上拉时通讯时好时坏。
- AD0 引脚决定器件地址。AD0 接低时地址是 0x68，接高是 0x69。原理图上这一个引脚的焊法决定后面 HAL 里
0x68 << 1是否正确。
MPU6050 供电 3.3V，逻辑电平也是 3.3V。接到 5V 上模块会发热，加速度计零偏会变大，直立环会一直得到一个小角度误差。
2.2 MPU6050 HAL 库初始化：唤醒、量程设置和原始数据读取
用 HAL 操作 MPU6050 的实质是 I2C 写寄存器和连续读数据。先唤醒芯片，再配置加速度计量程和陀螺仪量程。下面是能直接放进工程的最小初始化：
uint8_t mpu6050_init(void) { uint8_t reg, val; /* 寄存器 0x6B 是电源管理，复位后默认休眠，写 0 唤醒 */ reg = 0x6B; val = 0x00; if (HAL_I2C_Mem_Write(&hi2c1, 0x68 << 1, reg, I2C_MEMADD_SIZE_8BIT, &val, 1, 100) != HAL_OK) return 1; /* 寄存器 0x1C 设加速度计量程为 ±4g */ reg = 0x1C; val = 0x08; if (HAL_I2C_Mem_Write(&hi2c1, 0x68 << 1, reg, I2C_MEMADD_SIZE_8BIT, &val, 1, 100) != HAL_OK) return 2; /* 寄存器 0x1B 设陀螺仪量程为 ±2000dps */ reg = 0x1B; val = 0x18; if (HAL_I2C_Mem_Write(&hi2c1, 0x68 << 1, reg, I2C_MEMADD_SIZE_8BIT, &val, 1, 100) != HAL_OK) return 3; return 0; }读取加速度和角速度时，从 0x3B 开始的连续 14 字节里依次是加速度 X、Y、Z、温度、陀螺仪 X、Y、Z：
int mpu6050_read_raw(int16_t *acc, int16_t *gyro) { uint8_t buf[14]; if (HAL_I2C_Mem_Read(&hi2c1, 0x68 << 1, 0x3B, I2C_MEMADD_SIZE_8BIT, buf, 14, 100) != HAL_OK) return -1; acc[0] = (int16_t)((buf[0] << 8) | buf[1]); acc[1] = (int16_t)((buf[2] << 8) | buf[3]); acc[2] = (int16_t)((buf[4] << 8) | buf[5]); gyro[0] = (int16_t)((buf[8] << 8) | buf[9]); gyro[1] = (int16_t)((buf[10] << 8) | buf[11]); gyro[2] = (int16_t)((buf[12] << 8) | buf[13]); return 0; }HAL 的地址参数是 7 位地址左移一位，所以写0x68 << 1。量程选 ±4g、±2000dps 是因为平衡车倾角变化范围小，±4g 下加速度计灵敏度是 8192 LSB/g，角度分辨率更高；陀螺仪量程给到最大，起步瞬间电机反转带来的角速度尖峰才不会被削顶。
2.3 MPU6050 姿态解算：先用手写互补滤波，再考虑 DMP
只用陀螺仪积分角度，十几秒就会漂出去；只用加速度计算角度，每个控制周期都在跳。F407 上最省事的做法是互补滤波，核心代码只有一条：
/* dt = 0.001f，对应 1ms 控制周期；alpha 取 0.98 */ angle = alpha * (angle + gyro_y * dt) + (1.0f - alpha) * acc_angle;alpha 是陀螺仪权重。0.98 表示短期变化主要信任陀螺仪积分，加速度计只负责把长期漂移慢慢拉回来。dt 必须是真实中断周期，中断抖动大的时候角度会长出毛刺，这时候先用频率计确认 1ms 节拍。
加速度计算俯仰角的公式是：
acc_angle = atan2f(acc_y, acc_z);前提是车体前进方向为 Y 轴、宽度方向为 X 轴。如果模块朝向不同，要换 acc_x 或者对结果取反，不要在图省事的情况下改 PID 极性来掩盖符号错误。
很多资料推荐直接开 DMP，让 MPU6050 内部输出四元数。DMP 确实能省 CPU 占用，但初始化配置多，FIFO 中断和采样频率都要配合好，一旦输出跳变很难定位问题。先用手写互补滤波跑通整机，再用 DMP 替换，整个过程更顺。
2.4 数据手册里容易漏的：数字低通滤波和陀螺仪零偏
MPU6050 内部带可配置的低通滤波器，寄存器地址 0x1A。平衡车把 DLPF 设在 10Hz～42Hz 区间内即可；带宽太低会让陀螺仪响应滞后，带宽太高会让振动噪声直接进角度。默认配置下噪声严重时，选 21Hz 附近往往合适。
另一个必须处理的是零偏。模块通电后陀螺仪读数不归零，直接积分会让角度慢慢向一侧偏，低速下表现为车越站越歪。上电时静止采集 200 次角速度取平均，作为零偏值：
float gyro_offset = 0.0f; for (int i = 0; i < 200; i++) { mpu6050_read_raw(acc, gyro); gyro_offset += gyro[1]; HAL_Delay(5); } gyro_offset /= 200;这段采集一定要在机架静止时做。把车放在地上、手扶着不要动，采集完再进入主循环。注意这里默认绕 Y 轴角速度，如果实际安装时 Y 轴朝反方向，最后对 gyro_y 整体取反。
3. 串级 PID 的环路分工：角度位置环与速度环的时间间隔和参数整定
3.1 内外环的作用及时间间隔：为什么单个 PID 立不住
平衡车如果只用一个 PID 环，拿倾角误差直接算 PWM，角度误差大的时候输出饱和、角度误差小的时候输出过小，车会在平衡点来回穿，根本没有阻尼来刹车。串级 PID 把控制任务拆成两层：
- 内环是角度位置环，也叫直立环，以车体倾角作为位置反馈，输出直接作用到电机 PWM。
- 外环是速度环，读编码器轮速，输出一个很小的目标倾角偏置，让车在期望速度里跑。
两层之间是串联关系：速度环的输出不是 PWM，而是角度位置环目标值的一部分。这种结构和无人机串级 PID 类似，无人机是外环位置/速度给目标姿态，内环姿态快速跟随；平衡车是外环速度给目标倾角，内环角度快速回正。内外环的时间间隔至少要差 5 倍：内环 1ms～5ms，外环 10ms～20ms。如果速度环也跑 1ms，目标倾角被频繁改写，内环永远追不上目标，整车会高频抖振。
速度环输出必须限幅。一般限制在 ±0.05rad 以内，也就是约 ±3°。这个限幅是保护层：一旦车被外力推倒，速度环积分会在几秒内冲到最大值，若不加限幅，目标倾角可能变成 20°，直立环会全力朝一个方向加速，把车直接推飞。
无刷电机配 FOC 驱动时，速度环往下还会多一层电流环，设计思路变成电流环、速度环、位置环三层级联，本质还是内层快、外层慢。本文这个标题对应的常规方案是直流减速电机加编码器，速度环直接以编码器为反馈源即可，不需要额外的电流闭环。
3.2 速度环用增量式 PI、角度位置环用位置式 PD 的代码结构
角度位置环阻尼项用角速度相乘，速度环用增量式 PI 累加目标倾角。完整更新函数如下：
float balance_stack_update(BalancePID *b, float dt, uint8_t speed_tick) { if (speed_tick) { /* 速度环，每 10ms 执行一次，dt 传入 0.01f */ float err = b->target_speed - b->speed; float inc = b->kp_speed * (err - b->last_speed_err) + b->ki_speed * err * dt; b->last_speed_err = err; b->speed_out += inc; if (b->speed_out > b->speed_out_limit) b->speed_out = b->speed_out_limit; else if (b->speed_out < -b->speed_out_limit) b->speed_out = -b->speed_out_limit; } /* 角度位置环，每个 1ms 节拍都执行 */ float angle_err = (b->target_angle + b->speed_out) - b->angle; float out = b->kp_angle * angle_err + b->kd_angle * b->gyro_y; if (out > b->angle_limit) out = b->angle_limit; else if (out < -b->angle_limit) out = -b->angle_limit; return out; }速度环用增量式的理由：它输出的不是 PWM，而是“目标角度偏置”的累加量。写成增量式，每次只把微小的变化加到 speed_out 上，天然平滑；如果换成位置式，误将轮速误差直接转换成大角度偏置，内环会被瞬间打穿。直立环用位置式 PD，因为倾角误差不需要积分项，稳态不靠 I 项来顶。
D 项写法是kd_angle * gyro_y，这是把角速度直接当阻尼项用，等价于误差微分项。符号上，gyro_y 的正方向必须和三轴倾角的方向一致，否则阻尼变成正反馈，车会越抖越大。调车时发现震颤加剧，先换 Kd 符号，再检查陀螺仪安装朝向。
3.3 增量式速度环怎么调：先让直立环站稳，再放开速度环
参数顺序决定调试效率。把 speed_tick 固定在 0，先只调角度位置环：
- 手扶车体，向前推一下，观察 PWM 方向。电机应朝“追重心”的方向加速，而不是顺着推力方向猛冲。
- 逐渐增大 kp_angle，手能感到明显的回正力。继续增大，直到出现高频摆动。
- 增大 kd_angle 压制摆动，直到推一下车只回一次位，不来回甩头。
直立环能站稳后，把速度环放出来，kp_speed 先设 0，只给 ki_speed 一个很小的初始值，比如 0.001。再按需加 kp_speed。调试过程中常见表现对照如下表：
| 现象 | 原因 | 调整 |
|---|---|---|
| 手推时有阻力，但松手就飞快冲刺 | kp_angle 符号反 | 对 kp_angle 取反 |
| 车在原地剧烈抖动 | kd_angle 太小或方向反 | 先取反试，再考虑增大 |
| 车能立，但角度缓慢偏一侧 | 陀螺仪零偏未清 | 重新做零偏采集，检查机械水平 |
| 车启动后持续单方向加速 | 速度环符号反 | 对 kp_speed 取反 |
| 推一下后车晃两三次才停 | 速度环 kp_speed 偏大 | 减小 kp_speed，观察 speed_out 曲线 |
| 速度环限幅长期顶到上限 | ki_speed 饱和 | 降低 ki_speed，确认轮速单位 |
增量式速度环和位置式速度环的调法手感不同：位置式输出直接是 PWM，调大很快压垮直立环；增量式改变的是目标倾角，好调一点，但积分饱和藏在累加量里，要通过串口看 speed_out 是否长期贴在上限上判断。
4. 让 STM32F407ZGT6 把算法按时跑起来：编码器、PWM 和中断分配
4.1 用 TIM 编码器模式读电机转速，方向自动判断
直流减速电机后面的编码器一般是 AB 相正交输出。把 A 相接定时器的 CH1，B 相接 CH2，定时器工作在编码器模式下，计数方向会随转向自动变化，CPU 不需要干预。F407 上 TIM2、TIM3、TIM4、TIM5 都支持编码器模式，常见的引脚搭配是 TIM2 的 PA0/PA1 或者 TIM4 的 PB6/PB7。
HAL 初始化的核心配置：
TIM_Encoder_InitTypeDef enc = {0}; htim2.Instance = TIM2; htim2.Init.Period = 0xFFFF; /* 环形计数器 */ htim2.Init.Prescaler = 0; enc.EncoderMode = TIM_ENCODERMODE_TI1; enc.IC1Polarity = TIM_ICPOLARITY_RISING; enc.IC2Polarity = TIM_ICPOLARITY_RISING; HAL_TIM_Encoder_Init(&htim2, &enc); HAL_TIM_Encoder_Start(&htim2, TIM_CHANNEL_ALL);读取速度时不能直接用 CNT 值，要每周期取差：
int16_t now = (int16_t)__HAL_TIM_GET_COUNTER(&htim2); int16_t pulse = now - last_cnt; /* 有符号差值，正负代表方向 */ last_cnt = now; speed = (float)pulse / (enc_line * 4.0f) / dt; /* 换算成圈/秒 */pulse 除以“编码器线数 × 4”再除以 dt，得到当前轮速，单位是圈/秒。注意 last_cnt 更新必须放在中断里和速度环同一个节拍，不能在主循环里慢慢算，否则前后两次采样时间不一致，速度计算会忽大忽小。
4.2 PWM 输出和方向引脚：直流电机的驱动映射
带 H 桥的驱动板，比如 TB6612、DRV8833，通常是 PWM 引脚控占空比、DIR 引脚控方向。F407 上用一个通用定时器输出 PWM，频率设在 10kHz～20kHz，听不到电机啸叫，同时留够 MOSFET 开关余量。
/* APB1 定时器时钟 84MHz：period=8399, prescaler=0 时 PWM 频率 10kHz */ htim3.Init.Period = 8399; htim3.Init.Prescaler = 0; HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_1);方向引脚就是普通 GPIO，根据输出正负设置高低：
if (pwm_out >= 0) { HAL_GPIO_WritePin(DIR_GPIO_Port, DIR_Pin, GPIO_PIN_RESET); compare = pwm_out; } else { HAL_GPIO_WritePin(DIR_GPIO_Port, DIR_Pin, GPIO_PIN_SET); compare = -pwm_out; } __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_1, compare);这里 PWM 占空比限幅最好放在最外层，不给超过 80% 的高占空比。全占空比下电机电流大，电源会瞬间跌落，MPU6050 的加速度计跟着跳，整机反而更容易倒。留电 20% 余量，很多时候能避免莫名摔车。
4.3 1ms 控制周期和 10ms 速度环周期：用定时器中断分频
平衡车不要在主循环里做控制。常见做法是用基础定时器 TIM6/TIM7 产生 1ms 更新中断，在回调里读 IMU、做融合、跑直立环；速度环用计数器分频到 10ms。
/* TIM6：Prescaler=83，Period=999，APB1 84MHz 下更新周期 1ms */ static uint8_t speed_tick = 0; void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { if (htim->Instance == TIM6) { mpu6050_read_raw(acc, gyro); angle_fusion(0.001f); speed_tick++; if (speed_tick >= 10) { speed_tick = 0; pwm_out = balance_stack_update(&ctrl, 0.01f, 1); } else { pwm_out = balance_stack_update(&ctrl, 0.001f, 0); } set_motor(pwm_out); } }速度环在每 10 次节拍里跑一次，dt 传 0.01f，角度位置环每次都跑，dt 传 0.001f。这样内外环时间间隔固定为 10 倍，参数不会再被随机抖动干扰。中断优先级上，TIM6 要高于串口中断；串口发送用 DMA，不让打印阻塞控制。
4.4 用串口把 angle、speed_out 和 PWM 拉出来看
人眼看不出调参效果，串口示波器是必需品。格式化一行逗号分隔的数据发到上位机：
void debug_send(void) { char buf[64]; int n = snprintf(buf, sizeof(buf), "%.2f,%.2f,%.2f,%d\r\n", ctrl.angle, ctrl.speed, ctrl.speed_out, (int)pwm_out); HAL_UART_Transmit(&huart1, (uint8_t *)buf, n, 10); }上位机用支持逗号分隔曲线的串口示波器，把 angle、speed、speed_out、pwm_out 放在同一个时间轴上。判断标准：
- angle 曲线应是一条围绕目标角度小幅抖动的直线，不应有周期性大幅正弦；
- speed_out 曲线应是缓慢爬坡的平滑线，几个周期内反复翻转说明 kp_speed 偏大；
- pwm_out 换向处应是一簇短脉冲，如果满幅持续几百毫秒，说明外环积分在硬推直立环。
4.5 上电就倒这类故障的表格式排查
| 现象 | 可能原因 | 排查方法 |
|---|---|---|
| I2C 扫描不到 MPU6050 | 上拉缺失或 AD0 短路 | 量 SCL/SDA 波形，看地址是 0x68 还是 0x69 |
| 上电往一个方向冲 | 电机方向或陀螺仪符号反 | 先开环给一组固定 PWM，确认轮子转向和代码方向一致 |
| 手扶时高频振动 | kd_angle 不足或方向反 | 取反 kd_angle，再逐步加大 |
| 立住后慢慢歪向一侧 | 陀螺仪零偏漂移 | 静止重采零偏，检查模块紧固程度 |
| 运行中突然摔倒 | 电池电压跌落 | 量电机端电压，换大容量锂电池或用低占空比限幅 |
这些是倍率一致的问题，不带具体项目背景，按现象查引线最有效。
5. 调试收尾：用波形判断收敛，再往外层加位移位置环
5.1 三条曲线里的收敛迹象
调好的车，串口曲线上 angle 在零点附近 ±0.02rad 内波动，speed_out 平滑变化，pwm_out 换向有规律。如果 angle 呈衰减振荡但周期在拉长，说明速度环 kp_speed 偏大；如果 speed_out 出现锯齿，检查编码器脉冲是否被毛刺干扰，或者控制周期是否被串口打印拖长。
一个有用的验证方法：把车放在平坦地面上，松手后让它自平衡 10 秒，记录 speed_out 曲线的峰值。峰值小于限幅一半，说明外环还留有裕量；峰值经常顶满限幅，说明 kp_speed 接近临界，再调大会进入极限环。
5.2 从速度环再往外包一圈：位移位置环
如果需要车停在指定位置，就在速度环外再加一层位移位置环，形成“位移位置环 → 速度环 → 角度位置环”的三层结构：
/* 位移位置环，每 100ms 执行一次 */ position_err = target_position - position; target_speed = kp_pos * position_err; /* 速度环再根据 target_speed 输出目标倾角偏置 */位移位置环的执行周期比速度环更长，常见取 100ms 左右，周期太短会让位置误差直接变成剧烈加速。position 可以由编码器累计得到：每个控制周期把 pulse 累加并除以每圈计数，得到车体位移。位移环开启前先让两层内环保持平衡，否则三层一起散。
5.3 用一张调参记录表管理多组参数
| 日期 | kp_angle | kd_angle | kp_speed | ki_speed | 现象描述 | 下一步调整 |
|---|---|---|---|---|---|---|
| MM-DD | 数值 | 数值 | 数值 | 数值 | 直立、速度、限幅情况 | 具体动作 |
每次改参数只动一个系数，记录前后两条波形。调车最忌讳同时改直立环和速度环，因为串级结构里内环收敛速度直接影响外环观感，两个一起改，出问题不知道先查谁。
最后加位移环时，先用手扶着车，把 target_position 设为本地点，松手后观察它是否在 0.1m 半径内锁住；再把目标位置逐步改到 1m 外，看车是否先加速再平稳减速到位。每加一层环路，都先按住更内层的手感，再慢慢放开外层权限。
本文还有配套的精品资源，点击获取
