STM32二轮平衡车源码解析：从姿态解算到三环PID控制

简介面向嵌入式入门者及DIY平衡车爱好者这套基于STM32的二轮平衡小车软硬件方案完整可用能帮助读者从零搭建并调通自己的平衡小车。压缩包以ZIP格式打包整体约148.18MB内部涵盖可编译源码、电路原理图、连线方法等硬件资料目录结构清晰便于按需查阅。源码特别提供互补滤波、卡尔曼滤波与DMP三种姿态解算版本均经过调试无错误可在同一硬件平台上对比不同滤波算法的稳定性与响应差异这对理解飞控核心逻辑很有价值。配套硬件资料包含STM32主控电路、传感器接线及电源设计等信息软硬件结合可完成组装、下载、调试的全流程实践。目前已有3129人浏览学习适合想要动手DIY平衡小车、深入研习姿态估计与PID控制的学生或爱好者。1. 二轮平衡车源码从“永远倒下”到“稳稳站立”的完整工程第一次接触二轮平衡车的人多半会经历一个让人想摔键盘的时刻电机驱动正常、PWM 输出正常、传感器数据也确实打出来了但小车一上电就往一个方向猛冲然后“啪”地翻倒。我拆这套二轮平衡车源码时最深的感受是——它把直立这件事拆成了传感器采集、姿态解算、控制环三层而不是简单地把一堆调好的参数堆在一起。整套源码覆盖了主控驱动、MPU6050 姿态采集、DMP 固件移植、互补滤波、卡尔曼滤波以及直立环 速度环 转向环的完整控制链路对想做平衡车玩 PID、或者想理解倒立摆控制如何落地的嵌入式开发者来说是一份能直接跑起来的完整工程模板而不是零散的代码片段。适合两类人一是手里已经有车模但始终调不平的新手二是想研究姿态融合算法在单片机上如何落地的开发者。2. 硬件与技术栈STM32 MPU6050 直流电机选型逻辑2.1 整车硬件清单每个器件的角色分配读源码之前先搞清楚硬件角色后面看代码才不懵。这套工程的主控是 STM32F103C8T6一颗 72MHz 的 Cortex-M3 核心性能跑平衡车绰绰有余。姿态传感器用 MPU6050六轴三轴加速度 三轴陀螺仪通过 I2C 接口输出原始数据。电机是带霍尔编码器的直流减速电机减速比常见 1:30编码器用来测车速速度环全靠它反馈。器件型号作用接口主控STM32F103C8T6跑姿态解算与控制算法内部外设姿态传感器MPU6050提供加速度与角速度原始值I2C电机直流减速电机 霍尔编码器输出力矩、反馈轮速PWM GPIO电机驱动TB6612FNG将 PWM 信号转为电机驱动电流GPIO PWM无线调试HC-05 蓝牙模块无线查看姿态波形 / 遥控UART电源7.4V 锂电池整车供电电源管理这套选型很有代表性主控便宜、传感器常用、电机通用。它不是我见过最极致的硬件方案但绝对是适合学习和调试的。代码里驱动层全部采用寄存器操作不依赖 HAL 库这对理解底层时序有很大帮助。2.2 为什么用寄存器操作而不是 HAL 库现在的 STM32 开发大多用 CubeMX 生成 HAL 库工程但这套源码的驱动层是直接操作寄存器的。最典型的就是配置 GPIO直接操作GPIOA-CRL这样的寄存器位而不是调HAL_GPIO_Init()。// GPIO 初始化示例配置 PA0-PA3 为推挽输出50MHz GPIOA-CRL 0xFFFF0000; // 清掉低16位配置 GPIOA-CRL | 0x3333; // 0x3 推挽输出50MHz寄存器写法的好处是可控性强、编译后指令更短。在 10ms 控制周期里中断服务函数内的每一行代码都在跟时钟抢时间寄存器操作能省掉函数调用和结构体判空的开销。缺点也明显换芯片平台时驱动层几乎要全部重写。我的习惯是保留这套底层驱动不动算法层完全隔离硬件后面改成别的芯片只需要重写三四个文件。2.3 传感器数据链路I2C 总线上的原始六轴数据MPU6050 的原始数据是 16 位有符号整数。加速度计默认量程是 ±2g陀螺仪默认量程是 ±250°/s但平衡车场景通常会把陀螺仪配到 ±2000°/s防止快速倾倒时数据削顶。源码里上电第一件事就是读取 WHO_AM_I 寄存器地址 0x75确认 I2C 通信正常。uint8_t MPU6050_Check(void) { uint8_t id I2C_ReadByte(0xD0, 0x75); // 0xD0 是器件写地址 if (id 0x68) { // MPU6050 固定返回 0x68 return 0; // 通信正常 } return 1; }这里 0xD0 是 MPU6050 的 7 位地址 0x68 左移一位加上写标志位得到的字节地址0x75 是 WHO_AM_I 寄存器地址。源码里关于 I2C 时序的封装都在hardware/i2c.c里如果上电后卡在检查函数优先排查地址是否写对这是新人最容易翻车的地方。3. 源码结构与工程骨架一个完整工程该怎么读3.1 目录结构驱动层、算法层、控制层、应用层四层划分这套源码的目录划分值得学习它把底层驱动和上层算法完全分开了。拿到手先看目录别一头扎进 main.c 从头读那样容易被底层初始化带偏方向。BalanceCar/ ├── HARDWARE/ │ ├── i2c.c/.h # I2C 底层时序 │ ├── mpu6050.c/.h # MPU6050 驱动 │ ├── motor.c/.h # 电机 PWM 与方向控制 │ ├── encoder.c/.h # 编码器计数读取 │ └── delay.c/.h # 延时函数 ├── ALGORITHM/ │ ├── kalman.c/.h # 卡尔曼滤波 │ ├── complementary.c/.h # 互补滤波 │ └── dmp_mpu6050.c/.h # DMP 固件库移植 ├── CONTROL/ │ ├── balance.c/.h # 直立环 PD │ ├── speed.c/.h # 速度环 PI │ ├── turn.c/.h # 转向环 PD │ └── control.c/.h # 控制调度核心 ├── APP/ │ ├── main.c # 主函数与初始化 │ ├── uart.c/.h # 串口调试 │ └── sys.c/.h # 时钟与中断配置 └── README.md四层划分的逻辑很清晰HARDWARE 层负责跟芯片外设打交道ALGORITHM 层只做数学运算CONTROL 层把算法结果转为控制量APP 层负责初始化并启动整个系统。改车的时候改硬件就动 HARDWARE调姿态就动 ALGORITHM改手感就动 CONTROL互不干扰。3.2 定时中断10ms 控制周期的来源平衡车不是 main 函数里跑 while(1) 就能立的必须靠定时中断保证控制周期稳定。这套源码用定时器产生 10ms 中断在中断服务函数里执行完整的控制流程。10ms 是平衡车的常见控制周期换算成 100Hz 的控制频率对姿态环来说响应足够快对 STM32 来说负载也不高。void TIM2_IRQHandler(void) { if (TIM2-SR TIM_SR_UIF) { // 检查更新标志 TIM2-SR ~TIM_SR_UIF; // 清除标志位 Control_Loop(); // 执行三环控制 } }代码里用的是直接操作状态寄存器的方式清标志。如果用 HAL 库这里会变成HAL_TIM_IRQHandler加回调函数多一层间接调用。对 10ms 级别的中断来说那点开销不算什么但这套工程能把时序行为控制得更精确。3.3 上电初始化顺序先让传感器“稳定”再启动控制初始化顺序直接决定上电瞬间会不会翻车。源码里的顺序是配置时钟和 GPIO → 初始化 I2C → 校验 MPU6050 → 配置 PWM 定时器 → 初始化编码器 → 延时等待传感器稳定 → 开中断进入控制循环。int main(void) { SysTick_Init(); // 系统时钟与滴答定时器 GPIO_Init(); // 引脚复用配置 I2C_Init(); MPU6050_Init(); // 配置量程和时钟源 Motor_Init(); // PWM 定时器配置 Encoder_Init(); // 编码器输入捕获 UART_Init(115200); // 串口调试 delay_ms(500); // 等传感器内部稳定 MPU6050_GetData(mpu); // 读取初始角度作为零偏基准 Calibrate_Gyro(); // 陀螺仪零偏采集 TIM2_Enable(); // 开启控制中断 while(1) { // 主循环只做非实时任务比如蓝牙指令解析 } }delay_ms(500)这个延时很多人会忽略实际上很重要。MPU6050 刚上电时内部振荡器需要时间稳定如果立刻读数据前几十帧的角度是跳变的直接导致控制环第一拍输出异常。源码在启动前先采集了一组陀螺仪零偏均值这就是后面姿态解算能够站稳的第一道防线。4. 核心算法拆解从姿态解算到三环控制的完整链路4.1 三种姿态解算方案对比不是越复杂越好这套源码最值钱的部分是它同时提供了三种姿态解算的实现分别是 DMP 硬件解算、互补滤波、卡尔曼滤波。三者的核心矛盾都一样加速度计长期稳定但噪声大陀螺仪短期精确但会积分漂移需要用不同策略把两者融合。方案原理计算开销响应速度适用场景DMPMPU6050 内部硬件解算直接输出四元数极低快但受 FIFO 输出频率限制想快速出项目不深究算法细节互补滤波一阶高通滤波融合角速度低通融合加速度低中等调 alpha 即可单片机资源紧张、追求简洁卡尔曼滤波状态估计 协方差递推高快能估计陀螺仪零偏对姿态响应要求高、愿意调参数我的结论入门先从互补滤波跑通整车再换卡尔曼滤波感受差异。DMP 适合不想写解算代码的快速原型但如果你想理解控制链路的每一个环节手写滤波是必经之路。4.2 互补滤波一阶融合轻量级首选互补滤波只有一条核心公式融合系数用一个浮点数alpha表达。陀螺仪积分的角度在短时间可信加速度计计算的角度在长时间可信互补就是把两者按权重加起来。float complementary_filter(float gyro_rate, float acc_angle, float dt) { static float angle 0.0f; float alpha 0.95f; // 陀螺仪权重越大越信任积分 angle alpha * (angle gyro_rate * dt) (1.0f - alpha) * acc_angle; return angle; }alpha取 0.95 意味着积分占 95%、加速度修正占 5%。这个值不是拍脑袋定的alpha 越大角度越平滑但跟不上快速倾倒alpha 越小角度越贴近加速度计但噪声明显。我一般先给 0.95看波形再微调如果静态波形毛刺多就加大到 0.97如果动态响应迟钝就降到 0.92。互补滤波的优点是代码极短、占用资源几乎为零缺点是它对陀螺仪零偏没有估计能力如果零偏没校准好角度依然会漂。4.3 DMP 方案把姿态解算放进传感器内部DMP 是 MPU6050 芯片内部集成的数字运动处理器。开启 DMP 后传感器自己完成四元数解算主控只需要通过 FIFO 读取结果计算负担比手写滤波方案小很多。源码里dmp_mpu6050.c是一个完整的 DMP 驱动移植使用前提是先初始化 FIFO。void DMP_Init(void) { mpu_init(); mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL); // 开启六轴 mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL); // 启用 FIFO dmp_load_motion_driver_firmware(); // 加载 DMP 固件 dmp_set_fifo_rate(100); // 输出频率 100Hz dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT); // 输出四元数 }dmp_set_fifo_rate(100)对应 10ms 读取一次的节奏。这里有个坑DMP 初始化失败大多是因为 FIFO 配置顺序错了必须先mpu_init再mpu_configure_fifo顺序颠倒会导致dmp_load_motion_driver_firmware崩溃。DMP 输出的四元数精度是固定的不涉及两个滤波器的调参冲突适合快速交作业。但它的输出频率上限受传感器内部 FIFO 速率限制如果控制周期想做到 1msDMP 就不太合适了。4.4 卡尔曼滤波五步递推为什么它能更快跟上倾倒卡尔曼滤波是三种方案里实现最复杂的也是响应最快、抗漂移能力最强的。它本质是一个递推的状态估计器把角度和陀螺仪零偏作为两个状态变量用协方差矩阵描述估计的不确定性每一步先预测再修正。typedef struct { float Q_angle; // 角度噪声协方差 float Q_gyro; // 陀螺仪零偏噪声协方差 float R_angle; // 加速度计测量噪声 float P[2][2]; // 协方差矩阵 float K[2]; // 卡尔曼增益 float angle; // 融合后的角度 float bias; // 陀螺仪零偏估计值 } Kalman_t; float Kalman_Filter(Kalman_t *k, float new_angle, float new_rate, float dt) { // 第一步先验状态预测用陀螺仪积分角度并更新协方差 k-rate new_rate - k-bias; k-angle dt * k-rate; // 第二步更新协方差矩阵简化版 k-P[0][0] dt * (dt * k-P[1][1] - k-P[0][1] - k-P[1][0] k-Q_angle); k-P[0][1] - dt * k-P[1][1]; k-P[1][0] - dt * k-P[1][1]; k-P[1][1] k-Q_gyro * dt; // 第三步计算卡尔曼增益 float S k-P[0][0] k-R_angle; k-K[0] k-P[0][0] / S; k-K[1] k-P[1][0] / S; // 第四步用加速度计角度做观测修正 float y new_angle - k-angle; k-angle k-K[0] * y; k-bias k-K[1] * y; // 第五步更新协方差矩阵 k-P[0][0] - k-K[0] * k-P[0][0]; k-P[0][1] - k-K[0] * k-P[0][1]; k-P[1][0] - k-K[1] * k-P[0][0]; k-P[1][1] - k-K[1] * k-P[0][1]; return k-angle; }这套实现是嵌入式平衡车圈子里流传很广的版本核心逻辑是状态变量里多了一个bias陀螺仪零偏滤波过程中自动估计零偏并补偿积分误差。因为多了这个状态卡尔曼滤波能自己抵抗零漂不需要像互补滤波那样依赖外部校准值。代价是协方差矩阵的初始值需要调我建议Q_angle0.001、Q_gyro0.003、R_angle0.5起步然后在波形里观察角度滞后程度响应慢就调大 R_angle噪声多就调大 Q_angle。4.5 三环控制直立环 速度环 转向环的 PWM 叠加姿态解算最终喂给控制环。这套源码的三环结构是经典的嵌套 PID直立环 PD 负责让车辆保持倾角速度环 PI 负责让轮子跟上目标速度转向环 PD 负责让车头转向。三者输出叠加成最终的 PWM 值输出给左右电机。void Control_Loop(void) { // 读取并解算姿态 mpu.acc_angle atan2(mpu.acc_y, mpu.acc_z) * 57.3f; // 加速度计算倾角 mpu.gyro_rate mpu.gyro_x; // 陀螺仪角速度 float pitch Kalman_Filter(kalman, mpu.acc_angle, mpu.gyro_rate, 0.01f); // 速度环 float speed_now (encoder_left encoder_right) * 0.5f; static float speed_int 0; speed_int (speed_target - speed_now) * 0.01f; speed_int CLAMP(speed_int, -3000, 3000); // 防积分饱和 // 直立环 PD int16_t balance_out BALANCE_KP * pitch BALANCE_KD * mpu.gyro_rate; // 速度环 PI int16_t speed_out SPEED_KP * (speed_target - speed_now) SPEED_KI * speed_int; // 转向环 PD int16_t turn_out TURN_KP * (turn_target - mpu.gyro_z) TURN_KD * (-turn_speed); // 叠加输出 int16_t pwm_left balance_out speed_out - turn_out; // 左电机 PWM int16_t pwm_right balance_out speed_out turn_out; // 右电机 PWM Motor_Set(LEFT, CLAMP(pwm_left, -7199, 7199)); Motor_Set(RIGHT, CLAMP(pwm_right, -7199, 7199)); }这里的参数关系值得展开说。直立环 PD 中BALANCE_KP决定恢复力强弱Pitch是当前倾角小车往后倒时pitch为正输出的正 PWM 让电机前进把车身扶正BALANCE_KD是阻尼项角速度大时输出反向力防止震荡。速度环是为了解决一个物理问题如果只有直立环小车为了让角度归零会一直朝一个方向加速跑最终追不上重心而倒下。速度环会把“车子跑太快”变成“目标角度偏移”让车轻微倾斜来刹车就像人走路时身体前倾加速、后仰减速。转向环则简单得多它单独控制左右轮差速。三个环的优先级要分清直立环最大速度环其次转向环最小。所以叠加后左右轮都有balance_out speed_out作为基础转向环只产生差值。如果CLAMP限幅值设得太小比如 3000那么直立环输出直接削顶小车永远立不起来我一般先给 7199对应 100% 占空比等稳定后再逐步减小限幅防止过冲。5. 避坑与调试姿态漂移、电机死区与参数整定的实战排查5.1 现象静止时角度缓慢漂移小车总往一边加速现象是车子静止放在地面上没有碰它但电机缓慢转动角度值肉眼可见地越飘越远。原因大概率是陀螺仪零偏没有校准。陀螺仪在零角速度时输出并不为 0会有一个固定的偏置值。如果这个值被当作真实角速度积进去角度自然漂移。解决上电后静止采集 200 次陀螺仪原始值取平均作为零偏存到全局变量。卡尔曼滤波能在内部估计零偏但初始值如果偏太多收敛要花很长时间。如果用了互补滤波零偏必须靠这个方式去掉。源码里Calibrate_Gyro()函数干的就是这件事千万别注释掉。5.2 现象PWM 值很小时电机不转车体高频抖动现象是车体在平衡点附近持续小幅抖动发出“嗡嗡”声电机轴能看到微小的来回转动。原因不是 PID 参数过大而是电机存在死区——PWM 占空比低于某个阈值时输出力矩不足以克服静摩擦力。解决给输出加一个死区补偿值。当我计算出的 PWM 在 -100 到 100 之间时强制把输出抬高到 300 或 -300让电机越过死区。补偿值需要根据电机型号实测我常用办法是写一个测试程序从 0 开始逐步加大 PWM记录编码器刚刚开始转动的值作为死区阈值。5.3 现象上电瞬间电机猛跳一下然后翻车现象是每次通电电机在开始控制前就先猛转一下力度不大但会让车辆瞬间失去平衡。原因很典型上电时 PWM 定时器和 GPIO 的默认电平不确定在初始化完成之前电机驱动器的 IN 引脚处于悬空或随机状态产生无效的开关信号。解决在初始化流程中先配置电机方向引脚为推挽输出并拉低再使能 PWM 输出最后才打开控制中断。顺序不能反。另外建议在硬件上加一下拉电阻让驱动芯片在上电瞬间保持刹车态。源码里Motor_Init()已经处理了这个问题如果你从别处抄了驱动这个顺序最容易丢。5.4 现象满电和亏电时同一组参数表现完全不同现象是电池满电时车能立住但玩了一段时间电压下降后车开始频繁点头甚至倒下。原因非常实际电机 PWM 的占空比控制的是相对于电源电压的输出比例电池电压降低后同样占空比下电机实际获得的电压变小输出力矩下降。而直立环和速度环参数是基于满电时调的电压变了控制增益等于变了。解决在电源输入端加电压采样ADC 读取电池电压后对 PWM 输出做补偿将输出乘以一个系数例如满电 8.4V当前 7.2V系数就是 7.2/8.4。不需要精确补偿只要能减小倾覆的可能性就够了。如果你不想引入 ADC 逻辑最简单的办法是让车在固定电压段运行例如加稳压模块。5.5 现象串口波形正常但小车始终无法直立现象是最诡异的一种串口上位机里角度波形平滑、控制输出也有响应但实车就是站不住反复往一个方向倒下。排查到最后原因经常不在算法层而在执行层——电机方向接反或者编码器 A/B 相接反。解决先单独测试两个电机的正反转和编码器方向是否匹配。具体做法是给电机一个固定 PWM观察编码器数值的正负再反过来给 PWM确认编码器数值跟着翻转。如果直立环的方向和速度环的方向互相矛盾系统会一直打架。这个坑我栽过一次后来所有新车的调试流程都改成先验证执行器方向再调算法。6. 二次开发路线从“能站起来”到“能跑能避障”6.1 用串口上位机完成参数整定的第一步拿到源码后别直接上去调 PID先把串口调试跑起来。源码的 UART 模块会输出角度、角速度、左右编码器数值和最终 PWM用串口助手或者自己写一个简单的上位机绘图工具把角度波形打出来。调参数的时候眼睛盯着波形比盯着车体靠谱得多。先调BALANCE_KP从小到大慢慢加直到车体能够做出明显的“回正”动作。再调BALANCE_KD加到车体快速推倒时不会来回震荡。最后调速度环让车能原地稳住不跑偏。我习惯把SPEED_KI留到最后因为积分项最容易引发低频振荡。6.2 三个低成本改造方向遥控、避障、显示蓝牙遥控是最容易的改造。HC-05 在源码里已经初始化只需在 main 函数的 while(1) 里解析串口指令收到前后左右四个命令时改变speed_target和turn_target即可。加超声波避障也类似在车头装一个 HC-SR04测到 30cm 内有障碍就把speed_target设成 0优先保证直立不倒。如果想让调试体验更好加一块 OLED 显示当前角度和电量I2C 接口正好和 MPU6050 共用总线。// 蓝牙遥控指令解析伪代码 while (1) { if (UART_RxLen() 0) { uint8_t cmd UART_ReadByte(); switch (cmd) { case F: speed_target 100; turn_target 0; break; case B: speed_target -100; turn_target 0; break; case L: speed_target 0; turn_target 30; break; case R: speed_target 0; turn_target -30; break; case S: speed_target 0; turn_target 0; break; } } }6.3 参数整定顺序不想重蹈覆辙的调试习惯二次开发最忌讳一上来就改控制参数。我的固定流程是先跑一遍原始工程确认它本身能立住再一项项改代码每次只在单一方向做改动并保留上一组能工作的参数备份。从那以后我每一次拿到新的平衡车源码都强制先跑通默认参数、确认传感器方向、再动控制参数。倒立摆这种系统参数之间互相耦合一次改两个变量出了问题根本分不清是哪个环节引起的。把每一个改动都当成一次实验而不是一次“修改”调车才能是可控的而不是玄学。希望帮到你。本文还有配套的精品资源点击获取