二轮平衡车全套源码解析：卡尔曼滤波、互补滤波与PID控制

简介面向打算自制二轮平衡车的开发者与电子爱好者这套资料打包了本科阶段完整调通的软硬件方案可直接用于从零搭建自己的平衡小车也适合对照学习STM32控制、姿态解算与电机驱动等关键环节。压缩包约148.18MB内容涵盖STM32硬件原理、电路图、连线方法以及互补滤波、卡尔曼滤波、DMP三种主流姿态解算源码软件均调试通过、无错误硬件资料齐全目录划分便于按需查找。已有3129人浏览学习适合正在入门或进阶平衡车控制的读者参考。三种滤波版本均可直接编译运行可对照阅读理解不同姿态解算的实现差异各模块代码结构清晰配合电路图与连线说明能帮助快速完成硬件搭建、程序烧录和参数调试节省DIY过程中的排查时间。整体是一套完整、可落地的二轮平衡车资料。1. 二轮平衡车全套源码别让车在调参路上先倒了第一次把源码包烧进板子那一刻原以为能看到小车稳稳立起来结果车在地上打了两个滚电机嘶吼着把轮子甩到最大转速两秒不到就翻了个底朝天。后来我才想明白二轮平衡车能不能站起来根本不是你烧录了哪个现成固件而是姿态解算准不准、控制周期稳不稳、三个环叠加的极性对不对。这套二轮平衡车全套源码把卡尔曼滤波、DMP、互补滤波三种姿态解算方案全部实现了一遍同时给出了完整的直立环、速度环、转向环和电机驱动代码硬件链路从 MPU6050 一路通到 TB6612 驱动板属于那种“直接能编译、跑完能改车”的全套工程。适合第一次做平衡车的毕设党、想把它移植到自平衡机器人底盘的开发者以及不想再靠抄 PID 参数过日子的控制入门者。2. 整体架构与工程目录从 MPU6050 到 TB6612 的一条控制链2.1 硬件组成与技术选型这套源码默认的硬件平台是常见的 STM32 平衡车方案主控选的是 STM32F103C8T672MHz 主频、64KB Flash对平衡车这种“控制周期 500Hz 以内”的应用绰绰有余。姿态传感器是 MPU6050 六轴惯性单元挂在 I2C1 总线上HAL 库的 I2C 驱动直接读写寄存器。电机驱动用的是 TB6612FNG双路 H 桥比 L298N 体积小、压降低逻辑输入 3.3V 可以直接接 STM32不需要额外电平转换。电机选择的是带霍尔编码器的直流减速电机左右轮各一个编码器通过定时器输入捕获模式读正交解码。电源部分用 2S 锂电池7.4V给电机驱动供电同时经 AMS1117-3.3 稳压给主控和传感器供电。源码里还有一个蓝牙串口模块的驱动默认接 USART2用于遥控和调参。整套东西加起来成本不高适合作为入门级的自平衡机器人平台。2.2 工程目录结构一套源码看全链路打开源码包之后里面的工程目录结构非常清晰是标准的 MDK5 分组方式。我把核心目录整理出来balance_car/ ├── Core/ │ ├── Inc/ // 系统头文件 │ └── Src/ │ ├── main.c // 主函数入口 │ ├── stm32f1xx_it.c // 中断服务函数 │ └── system_stm32f1xx.c ├── Drivers/ │ ├── CMSIS/ │ └── STM32F1xx_HAL_Driver/ // HAL 库驱动 ├── Hardware/ │ ├── mpu6050.c // MPU6050 读写与初始化 │ ├── tb6612.c // 电机 PWM 与方向控制 │ ├── encoder.c // 编码器速度采集 │ └── bt.c // 蓝牙串口通信 ├── Algorithm/ │ ├── kalman.c // 卡尔曼滤波姿态解算 │ ├── complementary.c // 互补滤波姿态解算 │ ├── dmp.c // MPU6050 DMP 解算 │ └── pid.c // PID 控制器 └── User/ ├── control.c // 三环控制主逻辑 └── debug.c // 串口调试输出这个目录层次把硬件驱动、算法、控制逻辑分得很开。Hardware 层只负责“读传感器、写电机”Algorithm 层只做纯数学计算User 层拼装逻辑。我要做二次开发基本只在 User 和 Algorithm 两个目录里加东西不用动 HAL 库本身这一点对新手特别友好。2.3 编译与烧录让代码先跑起来编译方式没什么玄学用 MDK5 打开工程根目录下的 .uvprojx 文件然后按三步走# 界面操作对应的三步 1. 魔术棒图标 - Device 选择 STM32F103C8 2. 魔术棒 - Debug 选择 ST-Link 仿真器Settings 里选 SW 方式 3. 编译按钮 F7通过后点 LOAD 下载下载之前确认 BOOT0 跳线帽在低电平位置复位后程序直接跑。上电后如果车没反应第一件事是打开串口调试助手接 USART1 的 PA9/PA10波特率 115200数据格式 8N1看有没有输出姿态角数据。源码里 debug.c 默认每秒打印一次融合后的 pitch 和 roll 角这个输出是判断传感器方向有没有接反的最快手段。3. 姿态解算核心卡尔曼滤波、DMP 与互补滤波的三选一问题3.1 互补滤波代码量最小的高频融合方案互补滤波的思路是把陀螺仪积分得到的角度视为“高频可信、低频漂移”把加速度计反解出来的角度视为“低频可信、高频振动大”然后给两个来源分配权重合并成一个角度。源码里 complementary.c 的核心代码只有一行// complementary.c float complementary_filter(float gyro_rate, float acc_angle, float dt) { float alpha 0.98f; // 陀螺仪权重 static float angle 0.0f; angle alpha * (angle gyro_rate * dt) (1.0f - alpha) * acc_angle; return angle; }这段代码每次被调用都会基于上一拍的角度加上陀螺仪角速度在 dt 时间内的积分再用小比例拉向加速度计解算的角度。alpha 取 0.98 表示 98% 信陀螺仪、2% 信加速度计在 500Hz 采样下等效低通截止频率在 1.6Hz 左右人的手推抖动频率远高于这个值会被滤掉。这个方案的优点是工程代码少、容易调、不占算力。缺点是 alpha 固定车快速倾翻时加速度计的水平分量会让交互角度失真只能靠缩短控制周期弥补。在我自己调车过程中互补滤波适合先把整个控制链路跑通确认三环极性正确之后再换更精细的卡尔曼方案。3.2 卡尔曼滤波方差估计与状态预测的实现卡尔曼滤波在平衡车这个场景里状态量是两个维度当前倾角和陀螺仪零漂。之所以把零漂也列为状态量是因为陀螺仪的静置输出并不是恒定的 0而是一个缓慢变化的随机量卡尔曼滤波器能实时估计它并修正积分结果。源码里 kalman.c 的关键实现如下// kalman.c void kalman_filter(kalman_t *k, float gyro_rate, float acc_angle, float dt) { // 先验预测角度按角速度积分零漂保持不变 k-angle (gyro_rate - k-bias) * dt; // 先验协方差预测 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_bias * dt; // 计算卡尔曼增益 float S k-P[0][0] k-R_measure; float K0 k-P[0][0] / S; float K1 k-P[1][0] / S; // 用加速度计角度作为观测值进行更新 float error acc_angle - k-angle; k-angle K0 * error; k-bias K1 * error; // 后验协方差更新 k-P[0][0] - K0 * k-P[0][0]; k-P[0][1] - K0 * k-P[0][1]; k-P[1][0] - K1 * k-P[0][0]; k-P[1][1] - K1 * k-P[0][1]; }这段代码里三个关键参数 Q_angle、Q_bias、R_measure 分别在 kalman_t 结构体中初始化。Q_angle 是角度过程噪声方差Q_bias 是零漂过程噪声方差R_measure 是加速度计观测噪声方差。R 越小代表越信任加速度计表现为融合出来的角度跟随加速度计越快但也更容易把振动噪声带进来Q_bias 越大代表认为零漂变化越快滤波器对零漂估计越激进。实际调参时我习惯把 R_measure 固定在 0.03 附近然后一点点加 Q_bias。Q_bias 从 0.0001 开始往上试加到静止时角度输出近似一条直线、用手快速摇晃也不发飘为止。调参本质是在“响应快”和“平滑”之间找一个平衡点没有一组参数通吃所有车换了电机、加了负载都要重新微调。3.3 DMP 解算读四元数绕开融合算法DMP 方案的风味完全不同它用 MPU6050 芯片内部的运动引擎直接算四元数主控只负责从 FIFO 里把结果读出来再做一次四元数到欧拉角的转换。这套源码的 dmp.c 里初始化步骤先调用 mpu_dmp_init()然后读取缓存中的四元数分量// dmp.c 四元数转欧拉角 long quat[4]; dmp_read_fifo(gyro, accel, quat, sensor_timestamp); float q0 quat[0] / 16384.0f; // DMP 输出需除以 2^14 float q1 quat[1] / 16384.0f; float q2 quat[2] / 16384.0f; float q3 quat[3] / 16384.0f; // 横滚角 roll绕 X 轴 float roll atan2f(2.0f * (q0 * q1 q2 * q3), 1.0f - 2.0f * (q1 * q1 q2 * q2)) * 57.29578f; // 俯仰角 pitch绕 Y 轴 float pitch asinf(2.0f * (q0 * q2 - q3 * q1)) * 57.29578f;DMP 的优势是主控几乎不参与融合计算省出来的 CPU 时间可以跑更复杂的控制算法。缺点也很明显DMP 自身依赖官方库升级硬件型号或换传感器时需要重新适配库函数而且 DMP 在出厂时做了特定方向标定如果传感器在 PCB 上安装方向跟库里的默认方向不同四元数转出来的欧拉角符号和轴序都会对不上。我给一个选型参考表格这个表在决定移植方向时非常有用方案主控开销调参难度输出数据适合场景互补滤波极小低只需调 alpha欧拉角入门、低成本教学卡尔曼滤波中等中三组方差要调欧拉角 零漂估计稳定的量产车DMP小低但方向标定成本高四元数 → 欧拉角需要省算力的复杂控制系统4. 控制算法拆解直立环、速度环与转向环的叠加逻辑4.1 直立环PD 控制先把车立住整个控制逻辑放在 User/control.c 里核心是三个环的叠加。直立环是内环跑 500Hz 控制周期目标是让车体倾角回到设定的平衡角附近。这里的微分项直接取陀螺仪的角速度而不是角度误差的差分因为陀螺仪角速度就是角度的导数比差分误差更干净这算是我调车过程中的一个深刻体会。// 直立环 PD float balance_p 25.0f; // 比例系数角度误差放大 float balance_d 1.2f; // 微分系数抑制角速度提供阻尼 float balance_out; float balance_pid(float angle, float gyro_rate) { float error angle - BALANCE_ANGLE; // 平衡点通常为 0 度 balance_out balance_p * error balance_d * gyro_rate; return balance_out; }参数说明balance_p 太大车会高频抖动甚至自激震荡balance_p 太小车发软手一推就趴下。balance_d 负责“压住角速度”调大后车会变“硬”但过大就会像踩了急刹车一样启动困难。我一般先把 d 调到 1 左右然后加 p直到车从手中松开能挺住 1 秒以上再回头微调 d。4.2 速度环PI 控制让车不溜坡速度环是外环跑 200Hz 周期。速度环的输出不直接给电机而是作为直立环的目标角度让车“故意”前倾来获得前进速度。这样设计的好处是速度控制必须经过直立环不会绕过平衡直接轰电机。// 速度环 PI float speed_p 4.0f; // 比例项速度误差转倾斜角度 float speed_i 0.1f; // 积分项消除稳态速度误差 float speed_integral 0.0f; float speed_pid(float current_speed, float target_speed) { float error target_speed - current_speed; speed_integral error * 0.005f; // 5ms 速度周期 // 积分限幅防止上电瞬间积累过大 if (speed_integral 1000.0f) speed_integral 1000.0f; if (speed_integral -1000.0f) speed_integral -1000.0f; return speed_p * error speed_i * speed_integral; }速度环输出会叠加到直立环的目标角度上目标角度 BALANCE_ANGLE speed_out。车的速度误差越大目标倾角就越大车越倾斜、加速越快。积分项的作用是消除长时间推车造成的速度静差但积分上限必须掐死否则车会慢慢往一个方向加速冲出去这也是常见的翻车原因之一。4.3 转向环与电机输出差速控制和 PWM 限幅转向环的目的是让车绕竖直轴自转它只需要用到陀螺仪的 Z 轴角速度做一次 PD 控制输出加到左右电机的差速上。// 转向环 float turn_p 2.0f; float turn_d 0.1f; float turn_out turn_p * gyro_z turn_d * gyro_z_derivative;最后叠加输出时注意左右轮的符号。直立环和速度环给的是两轮同向的基础力矩转向环给的是反向差动力矩int motor_left balance_out - speed_out turn_out; int motor_right balance_out - speed_out - turn_out; // PWM 限幅定时器 ARR3600可输出正负占空比 if (motor_left 3200) motor_left 3200; if (motor_left -3200) motor_left -3200; if (motor_right 3200) motor_right 3200; if (motor_right -3200) motor_right -3200; tb6612_set_pwm(motor_left, motor_right);这段代码里的符号容易翻车。如果电机线或者编码器线某一侧接反整个系统就变成正反馈车一通电就疯跑。我的血泪经验是第一次组装后先把速度环去掉只留直立环手动扶车确认直立环能让车回正再逐个接上速度环和转向环。千万别一次全上否则出了问题根本不知道是哪一环的极性错了。5. 常见问题排查与避坑PWM 死区、零点漂移和安装方向5.1 现象一电机嗡嗡响但车起不来现象是上电后电机在振动轮子却不持续转动车完全不能起身。串口输出显示 PID 输出已经有值但电机就是“憋着”不转。原因绝大多数是 PWM 占空比落在了电机死区里。直流电机带减速器和编码器后摩擦扭矩较大主控输出 5% 以内的占空比时电机转矩不足以克服静摩擦表现为抖动而不是旋转。PID 输出在这个区间内来回震荡时就会听到明显的“嗡嗡”声。解决办法是先做一次死区标定找到让轮子刚好持续转动的临界占空比然后做输出补偿所有非零 PWM 输出都抬高或降低一个死区常数。我在 tb6612.c 里加了一段// 死区补偿PWM 输出在死区内时强制跳到临界值 #define PWM_DEAD_BAND 150 if (pwm 0 pwm PWM_DEAD_BAND) pwm PWM_DEAD_BAND; if (pwm 0 pwm -PWM_DEAD_BAND) pwm -PWM_DEAD_BAND;5.2 现象二静止时角度一直缓慢漂移现象是车放平在桌面上串口输出的角度从小到大的缓慢漂移过十几秒偏出去好几度最终导致直立环输出异常车倒向一边。原因很直接陀螺仪零漂没有被纠正。MPU6050 的陀螺仪即使完全静止输出也不是理想的 0而是一个随机偏置。互补滤波里这个偏置会积分进角度卡尔曼滤波虽然在估计零漂但如果初始协方差设置不合理收敛速度跟不上。解决办法是上电后做“静止校准”检测到前 200 次采样角速度的绝对值都小于 0.5 度每秒时把这段时间的角速度平均值作为零漂值在传感器读取处减去。DMP 方案里则需要在 dmp_init 后调用 dmp_calibrate_accel 和 dmp_calibrate_gyro 完成内部校准。5.3 现象三车一上电就往一个方向加速冲出去现象是电池一接上车不管怎么扶都猛烈地往一个方向加速越跑越快像有人在后面推。这类问题的元凶通常是速度环或者电机的极性反了。直立环输出电机的力让车回正但如果电机方向的软件定义和物理接线反了直立环的回正力矩就变成倾倒力矩速度环同样存在这个问题当车因为扰动往前滑动速度环应该让车后倾减速但如果方向反了它会加大前倾角度。排查思路是拆闭环、单环验证。第一步先屏蔽速度环和转向环输出只留直立环手动把车扶到接近竖直位置再松手观察它是否能回正能回正说明直立环极性正确再依次接入速度环。检查代码里 tb6612.c 的 IN 引脚电平定义和 sensor 安装方向是否一致如果传感器安装在 PCB 反面pitch 角输出的符号会整体取反需要在初始化时做一次方向标定。5.4 现象四接上串口调试后车开始抽搐现象是车本来站得好好的一接上蓝牙模块或者打开串口调试助手开始打印之后车体开始轻微抖动甚至直接倒下。原因往往不是控制代码本身而是串口打印占用了主循环时间导致直立环控制周期不固定。如果姿态解算和 PID 是在 main 函数里的 while 循环中跑的一条耗时的 printf 输出就会让控制周期从 2ms 拉长到 3ms 甚至更长控制频率波动就等于 PID 参数突然漂移车自然站不稳。解决办法是把控制逻辑搬到定时器中断里跑中断优先级设为最高串口打印只放在后台轮询里或者用一个状态位控制打印频率比如每秒打印一次。我习惯把打印做成标记位方式// 调试模式每 200ms 才允许一次完整打印避免拖慢控制中断 if (debug_trigger control_cycle_ok) { debug_print_attitude(); debug_trigger 0; }6. 二次开发实战从遥控车到可调参平台6.1 加一个蓝牙遥控通道源码里已经预留了蓝牙串口的接收接口我可以顺着这个口做指令解析。协议做成最简单的逐字节解析收到$开头的一行指令中间是方向字符。代码如下// bt_parse.c 串口指令解析 void bt_parse(char *cmd) { switch (cmd[1]) { case F: speed_target 30.0f; break; // 前进 case B: speed_target -30.0f; break; // 后退 case L: turn_target 45.0f; break; // 左转 case R: turn_target -45.0f; break; // 右转 case S: speed_target 0.0f; break; // 停止 } }速度给定走速度环而不是直接改 PWM会让加减速平滑很多。我一般还会加一个线性斜坡限制比如设定速度只能以每秒 50 的速度变化防止手机端误触瞬间给大值把车甩飞。6.2 用 ADC 电位器做在线调参告别反复烧录调 PID 时最烦的就是改一个数字、重新编译烧录一次。我在这套工程上加了两个电位器用 ADC1 的两个通道读 0~3.3V映射到直立环 P 和 D 的范围。代码是这样的// User/tunable.c float adc_to_balance_p(void) { uint16_t val read_adc_channel(ADC_CHANNEL_1); return 10.0f (float)val / 4095.0f * 20.0f; // 映射到 10~30 }这样车跑在桌面上我一边用手推车看响应一边拧电位器几秒钟就能试出一组手感不错的参数。验证方法还是老规矩先静止看角度收敛再手持倾斜看阻尼最后放手站三秒才算过。从那以后我每次改完代码都强制走一遍这个流程先查零漂、再验极性、最后才放车跑。这个习惯帮我省了无数次“车突然冲向桌角”的返工时间希望帮到你。本文还有配套的精品资源点击获取