基于STM32的平衡车源码解析：从MPU6050姿态解算到串级PID调试

简介这是一份基于STM32的平衡车完整工程源码面向嵌入式开发者、机器人爱好者以及准备学习PID控制和传感器融合的初学者。工程以MPU6050惯性测量单元实现姿态检测结合直立PID与速度PID双闭环控制并引入卡尔曼滤波对传感器数据去噪与融合帮助读者掌握从底层驱动到上层控制算法的完整实现链路。压缩包共167个文件包含C源码、头文件、编译生成的hex/axf/map文件、Keil工程配置、原理图PDF、手机端APK及使用说明等总大小约13.35MB目录结构清晰。目前已有1165人学习下载。通过研读代码读者可理解STM32实时控制逻辑、MPU6050数据读取、卡尔曼滤波参数整定以及PID调参思路是入门智能车与移动机器人控制的高价值参考资料。 基于STM32的平衡车源码从原理到调试手把手教你写出一套能跑起来的程序做平衡车这个项目是我在嵌入式学习路上觉得“最有意思”的一步。它不像点灯、串口打印那样验证完就结束而是一个真正把传感器、电机、控制算法、实时系统串起来的综合项目。尤其是当小车第一次能在桌面上稳稳立住哪怕只有三秒那种成就感跟跑通一个串口中断完全是两个层次。这篇文章不打算放一份完整工程让你直接下载交差而是把我在实际编写“基于STM32的平衡车源码”过程中觉得最核心的思路、最容易被绕进去的坑、以及最终跑通的代码架构全部拆开讲清楚。如果你手里的板子是STM32F103系列传感器是MPU6050驱动是TB6612那这篇文章基本可以当一套注释版参考来用。1. 项目整体思路与源码架构设计先别急着写代码把整个系统的控制链路在脑子里过一遍车身直立的关键是保持重心投影落在轮轴附近传感器MPU6050负责测量车身倾角主控STM32根据当前倾角和角速度通过PID算法计算出电机的补偿力矩最后让两个轮子转起来把车身“推”回平衡位置。整个过程是一个典型的闭环负反馈系统。所以源码设计上主要分成四个模块姿态获取模块I2C读取MPU6050并进行滤波、控制算法模块直立环和速度环串级PID、电机驱动模块PWM输出控制TB6612、调试与显示模块串口输出、OLED可选。在实际编码中这四个模块分别对应mpu6050.c、pid.c、motor.c、uart_debug.c逻辑清晰后期调参也好定位问题。2. 硬件选型与引脚分配为什么这样配源代码写起来之前先要把硬件连接关系定死。很多新手做平衡车代码逻辑没问题但老是读不到MPU6050数据或者电机不转十有八九是引脚冲突或者供电不足而且这些硬件隐患在写代码时根本看不出来。我这边沿用了一套经过实测、稳定性很高也最适合教学的最小系统组合STM32F103C8T6 MPU6050IIC接口 TB6612FNG电机驱动。模块引脚对应STM32引脚说明MPU6050SCL / SDAPB6 / PB7使用I2C1硬件外设也可以软件模拟但不推荐MPU6050INTPA11可选用于数据就绪中断项目里我选择轮询读取TB6612PWMA / PWMBPA8 / PA9定时器TIM1的CH1/CH2输出PWMTB6612AIN1 / AIN2PB0 / PB1左电机方向控制TB6612BIN1 / BIN2PB10 / PB11右电机方向控制编码器左A/左B右A/右BPA6 / PA7 / PB3 / PB4使用TIM3和TIM4编码器模式选PB6/PB7作为I2C引脚有个好处它们是I2C1的硬件重映射引脚直接在CubeMX里勾选就行不用去操作寄存器映射。PWM用TIM1是因为TIM1是高级定时器频率在20kHz左右输出非常稳定对电机驱动来说这一点很关键。TB6612是经典的电机驱动芯片相对于L298N来说导通压降小很多3.3V逻辑电平直连STM32完全没问题而且体积小适合平衡车这种对重量敏感的结构。3. 姿态解算读写MPU6050容易栽的坑MPU6050读出加速度和角速度原始数据并不难难在把它变成精确的车身倾角。我见过很多初学源码直接拿atan2(AcceY, AcceZ)当倾角用但实际跑起来就发现那个角度在车身快速摆动姿态时会剧烈抖动完全没法用。这是因为加速度计容易受到运动加速度的干扰而陀螺仪虽然短时间准确但积分漂移没法消除。解决的办法是两个数据融合工程上最适合平衡车的方案是一阶互补滤波计算量小效果好放在STM32F103这种主频72MHz的控制器里毫无压力。// 互补滤波角度 0.98 * (上次角度 陀螺仪角速度 * dt) 0.02 * 加速度计角度 float balance_Angle 0.98f * (balance_Angle gx * dt) 0.02f * accel_angle;系数怎么选陀螺仪比重越大融合出的角度对瞬时振动越不敏感但跟随时会越滞后加速度计比重越大越跟手但噪声越多。在平衡车应用中的经验是0.98/0.02起始调如果车身高频抖动就降低陀螺仪权重如果角度跟踪慢就调高一些。采样周期一定要固定我当时设置的是5ms执行一次姿态更新对应dt 0.005f。还有一个特别特别容易踩的坑MPU6050的陀螺仪偏移量不做校准。如果不校准融合出来的角度就有偏置漂移车身会朝一个方向倾斜PID积分环再怎么补偿都会越跑越偏。正确的做法是上电后让车身保持静止连续取200次陀螺仪Z轴零偏值的平均值存成全局偏置每次读原始数据再减去零偏。4. 串级PID控制源码详解直立环和速度环的配合平衡车控制如果只加一个直立环逻辑上也能立起来但车身会朝一个方向慢慢跑远然后失去平衡摔倒。这时候就需要串级PID把速度环的输出作为直立环的目标角度偏置让小车既能保持平衡又能原地站在原地不掉速。速度环我采取的是增量式PID周期50ms一个控制节拍通过编码器统计两个轮子的平均速度与目标速度一般是0做差输出一个角度偏置加到直立环目标角度上。直立环采用位置式PD控制周期5ms执行一次输出作为PWM占空比指令。// 直立环核心代码5ms定时器中断中调用 int balance_pid(float angle, float gyro) { float error target_angle - angle; // target_angle由速度环提供 float pout Kp_angle * error; // 比例项 float dout Kd_angle * gyro; // 微分项用的是角速度不是角度微分 return (int)(pout dout); }看到没有直立环的微分项直接用的陀螺仪输出的角速度而不是用(angle - last_angle) / dt去算。两者效果上差不多但直接使用陀螺仪的值省了一步差分运算同时因为陀螺仪本身带宽高、噪声小微分质量会更好。速度环的代码逻辑在50ms中断里做// 速度环核心代码50ms定时器中断中调用 int velocity_pid(int target_speed) { int real_speed (encoder_left encoder_right) / 2; int error target_speed - real_speed; integral error; // 速度环积分项 integral constrain(integral, -5000, 5000); // 防止积分饱和 int output Kp_speed * error Ki_speed * integral; target_angle constrain(output, -3, 3); // 角度偏置限制在±3度 return target_angle; }速度环输出限制在±3度非常重要。如果不限制车身倾斜角度目标会被拉到很大这时候小车表面看起来在拼命追速度实际上已经失去直立控制能力了。做这个限制的底层逻辑是速度环永远只能修正车身姿态环的参考输入但不能喧宾夺主。5. 主循环与定时器分配中断优先级的艺术整个系统运行下来有三个任务在循环执行5ms的直立环姿态更新、50ms的速度环和串口调试、以及随时可能在跑的I2C读取。如果这些全部塞进主循环做delay那系统实时性会大打折扣PID控制的效果也会很差。官方推荐的做法是用定时器中断做固定节拍控制TIM6中断5ms最高优先级读取MPU6050数据、执行互补滤波、执行直立PD、输出PWMTIM7中断50ms普通优先级读取编码器、执行速度环、计算目标角度偏置、串口发送调试数据主循环里面只放一些非实时任务按键扫描、OLED刷新显示、状态灯翻转这里的思路其实可以理解成把“身体平衡”视为最高优先级的本能反应5ms节拍把“走路速度和方向”视为稍低一级的大脑决策50ms节拍。控制周期必须固定且可预测一旦被打断或抖动PID参数就全靠运气了。关于中断优先级的配置TIM6的抢占优先级要给到1子优先级1TIM7的抢占优先级给到2子优先级1。注意不要让I2C读取放到中断里面死等我用的方式是轮询加超时机制如果MPU6050不响应就跳过本次数据不阻塞中断执行。6. 首次上电调试全记录从“趴窝”到稳定站立我还记得第一次上电小车在1秒内“啪”的一声直接趴到地上当时第一反应是机械结构问题后来检查发现代码里电机方向写反了。这真的是新手做平衡车最容易忽略的点——MPU6050的安装方向、电机的正反转、PWM占空比的正负极性三者必须同时对齐任何一个不匹配都会直接拍地。调试顺序建议这样来处理离线测试电机方向把小车悬空通过串口指令分别给两个电机发送固定PWM确认占空比为正时轮子是往前转的记录正反转映射规则。判断角度极性用手缓慢前倾车身观察串口打印的角度值确保倾角方向和正方向定义一致如果反了把accel_angle取负。空转试探直立拔掉电机线只保留编码器供电和PWM信号手扶着车身在桌面上空转观察PWM输出是否随着车身倾斜反向输出。放手实测上电机手拉住车身尾部轻推一下给它一个直立启动的力矩看是否能站立同时观察串口曲线调增益。在实际调试过程中我遇到一个问题车能立住但总是持续往一个方向缓慢漂移。一开始怀疑加速度计零偏后来排查发现是编码器脉冲计数方向定义反了导致速度环把“往左走”误判断成“往右走”于是不断修正目标角度越修越偏。还有一个值得留意的地方是PWM死区。TB6612驱动在占空比很小的时候电机输出的力矩可能不足以克服静摩擦就会有“空转但车不动”的情况。我当时处理的方法很简单在PWM输出函数里判断如果计算出来的PWM绝对值小于20就直接输出0如果大于20再按计算值输出。这个小细节看似不起眼却直接影响小车能不能克服地面摩擦回复直立。7. PID调参实操让车从抖动到稳定的调整顺序参数整定是整个项目里极其磨人也是收获最大的一个环节。我先提供一个适合大多数车体结构的初调范围参数先给一个起始值作用Kp_angle100~180车辆回复直立的回复力强度太大车体会高频震荡Kd_angle1~2抑制车体角速度太大车体会迟钝甚至震Kp_speed-80~-120对速度误差的反应注意极性问题可能需要取负Ki_speed-1~-3消除静态偏差车身停住时保持零速调参的时候严格按照“先直立环、后速度环”的顺序。直立环PD的部分先把速度环全部禁用目标角度恒为0从较小的Kp_angle50起步观察车身是否能快速回正如果回正无力、一松手就塌就逐步增加Kp_angle每次大约增加20。当车体能实现来回摆动但不倒的状态时再加入Kd_angle抖动消失说明阻尼够了。直立环调好之后就能“用手扶着站直”了接下来才是速度环。速度环的Kp_speed每增加一点测试一下车会不会原地越走越远。正常的现象是如果轻推小车一下它会往前滑着走一点然后自动停下回到起点附近。如果它越走越快直到摔倒说明速度环的方向反了要把Kp_speed取反。整定过程中我用的一个非常实用的工具是串口波形。把角度、目标角度、PWM值、速度四个变量一起打印出来在电脑上用匿名上位机或者VOFA画波形能直观看到每个控制环在干什么。8. 避坑速查源代码编写与迭代中的高频问题这里把一些常见问题和排查手段做个速查表方便当作备忘现象可能原因排查与处理办法小车一上电就往一个方向猛冲电机方向或者倾角方向反了断开电机手动倾斜车身观察PWM是否反向再单独测试电机正反转小车高频抖动或“嗡嗡”响Kp增益过大或PWM频率过低把PWM频率提高到20kHz以上尝试降低Kp并补一点点Kd小车能立住但持续向一边漂移速度环方向反了或编码器方向不对打印编码器值轻推小车确认方向若反了把速度环系数取反快速摆动时角度数据飘得离谱互补滤波系数不适合或者采样时间不固定检查定时器中断是否卡死验证dt是否为固定5ms电机偶尔不响应或是无力TB6612供电不足电机电源需要独立供电避免和单片机共用同一路LDO电流不够会直接把电压拉垮最后一个很实际的建议代码写完后一定要做版本备份。每次调参感觉“差不多能站稳了”马上把参数和代码一起提交存档。我吃过的亏是在调参数时越改越差最后想回到之前稳的状态却忘了之前用的是什么值只能靠肉眼和经验硬猜。9. 写在最后的一段调试心得回头再看这个STM32平衡车源码项目真正考验人的不是写完代码那一下而是它把“传感器数据 → 算法处理 → 执行器输出 → 物理反馈”整个链路压缩在几毫秒内完成一遍闭环。这种实时性和鲁棒性的感觉是普通单片机外设练习完全给不了的。我个人调试过程中的一个小技巧是一开始不要追求“完美直立一整分钟”而是先定一个“能立3秒钟”的小目标。把直立环调稳把速度环的方向固化然后逐步拉长验证时间。每换一组新的PID参数我都会拿手机录一段视频回放看到底是哪里出了问题。视频放慢以后很多现场不容易观察到的细节反而会被暴露出来轮子反转、车身抖动、电机延迟响应全部一目了然。后续如果你想在这个基础上继续扩展可以考虑加入蓝牙遥控、转向环、甚至用MPU6050的Z轴角速度做一个车身原地旋转的控制。但前提是先把现有的双环PID打磨到位底层稳了上层花活才有得玩。本文还有配套的精品资源点击获取