简介基于 STM32F407ZGT6 与 MPU6050 的二轮自平衡车项目是一份面向课程设计、毕业设计及嵌入式进阶学习的完整技术资料。项目围绕速度环与位置环串级 PID 控制展开能够帮助读者理解如何利用惯性传感器获取车身姿态并通过闭环控制算法驱动电机维持平衡是电子信息、自动化、计算机等专业学生完成自平衡类题目或学习运动控制算法的实用参考。压缩包共 188 个文件大小约 5.33 MB以 C 源码和头文件为主同时包含 Keil MDK 工程配置、编译中间文件、可直接烧录的 hex/axf 固件以及脚本和说明文档从源代码到编译结果形成完整闭环便于对照学习。目前已有 357 人学习下载。具体内容包括 STM32F4 标准外设驱动、MPU6050 姿态读取与处理、二轮车平衡控制逻辑、速度环与位置环参数整定思路等读者可以沿工程结构对照源码和配置掌握从传感器采集、姿态解算到 PWM 控制输出的全流程开发者也可将其快速移植到自己的硬件平台节省底层驱动与算法调试时间。1. 立不住的二轮车与串级 PIDstm32f407zgt6 加 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、Zint 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 设在 10Hz42Hz 区间内即可带宽太低会让陀螺仪响应滞后带宽太高会让振动噪声直接进角度。默认配置下噪声严重时选 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 倍内环 1ms5ms外环 10ms20ms。如果速度环也跑 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 相接定时器的 CH1B 相接 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频率设在 10kHz20kHz听不到电机啸叫同时留够 MOSFET 开关余量。/* APB1 定时器时钟 84MHzperiod8399, prescaler0 时 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。/* TIM6Prescaler83Period999APB1 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_anglekd_anglekp_speedki_speed现象描述下一步调整MM-DD数值数值数值数值直立、速度、限幅情况具体动作每次改参数只动一个系数记录前后两条波形。调车最忌讳同时改直立环和速度环因为串级结构里内环收敛速度直接影响外环观感两个一起改出问题不知道先查谁。最后加位移环时先用手扶着车把 target_position 设为本地点松手后观察它是否在 0.1m 半径内锁住再把目标位置逐步改到 1m 外看车是否先加速再平稳减速到位。每加一层环路都先按住更内层的手感再慢慢放开外层权限。本文还有配套的精品资源点击获取
