二轮平衡车全套源码：卡尔曼滤波与PID三环控制实战

简介这是一套面向电子爱好者与嵌入式初学者的二轮平衡车DIY完整资料包涵盖从硬件搭建到软件调试的全链路内容。资源定位为STM32平台平衡车的开源学习套件包含互补滤波、卡尔曼滤波、DMP三种姿态解算方案的源码三套工程均编译通过、可直接烧录验证适合希望动手制作平衡小车或深入理解滤波算法差异的读者。压缩包大小148.18MB内置全部平衡小车相关资料类型上以Keil工程源码、电路原理图、PCB文件、连线说明文档为主硬件与软件配套呈现可以大幅降低DIY门槛。已有3129人学习浏览整体经过本科阶段完整调试验证是一份少有的“从零到跑通”的参考资源。读者可以获得实物接线指引、电路设计源文件以及三种滤波方案的对比实现既能直接复刻样车也能基于源码二次开发用于课程设计、毕业设计或兴趣项目。1. 平衡车全套源码从零开始搭一辆能站稳的两轮车第一次接触二轮平衡车的人往往以为难点在机械组装等真把套件拼好、电机转起来才发现真正让人头疼的是让它站稳。一个常见现象是车能启动但两三秒内就朝一个方向冲出去或者原地高频抖动这就是典型的角度环与速度环没配合好。市面上能跑的平衡车源码不少但大多数要么只给姿态解算片段要么把 PID 调参写成玄学让新手连入口都找不到。这套二轮平衡车全套源码的价值在于它把从传感器读取、姿态解算到双环/三环控制、PWM 输出的完整链路都串起来了适合正在做课设、电赛备赛或想入门嵌入式控制的人直接照抄再改。2. 姿态解算选型卡尔曼滤波、DMP 与互补滤波的取舍和落地代码2.1 三种解算方案的适用边界为什么不是越复杂越好平衡车要解决的核心问题是MPU6050 输出的加速度计和陀螺仪数据各自有硬伤。加速度计在静止时能准确给出重力方向但车体一震动高频噪声会直接把角度信号淹没陀螺仪积分得到的角度在短时间内很平滑但积分漂移会随时间累积几秒后角度就开始“飘”。姿态解算的全部工作就是把这两个都不完美的信号融合成一个相对可信的角度。卡尔曼滤波的思路是把角度和陀螺仪零偏作为状态量每次拿加速度计的角度做测量更新拿陀螺仪的角速度做预测更新。它收敛快、动态响应好但需要反复调协方差矩阵的过程噪声和测量噪声调不好就会出现角度滞后或者过冲。DMP 是 MPU6050 内部的硬件解算引擎DMP 库初始化后直接读四元数再转欧拉角CPU 占用极小但它把融合过程当黑匣子输出是相对姿态想改滤波特性基本无从下手。互补滤波则用一个系数 alpha 把两个信号按频率高低切开陀螺仪负责高频段加速度计负责低频段。代码只有几行在平衡车这种垂直安装、运行范围不到 30 度的场景里完全够用而且调试只要调一个 alpha。我的建议很简单做课设或功能演示优先互补滤波代码短、好解释、调参直观做竞赛或追求极限动态响应用卡尔曼滤波DMP 库适合不想碰矩阵运算、把精力全放在 PID 上的人。这套源码把三种方案都放了切换时只需要改融合函数入口不用动上层控制逻辑。2.2 卡尔曼滤波实现一维模型与核心代码卡尔曼滤波在平衡车场景里可以简化为一维模型——只对俯仰角pitch做融合横滚角同理但角度变化幅度远小于俯仰可以先不处理。状态量有两个角度 angle 和陀螺仪零偏 bias。预测方程用的是陀螺仪角速度减掉零偏后的值测量方程用的是加速度计算出的角度。下面是一份可直接嵌入单片机的卡尔曼滤波 C 代码typedef struct { float angle; // 融合后的角度输出 float bias; // 陀螺仪零偏 float P[2][2]; // 协方差矩阵 float Q_angle; // 角度过程噪声 float Q_bias; // 零偏过程噪声 float R_measure; // 测量噪声 } Kalman_t; void Kalman_Init(Kalman_t *kf) { kf-angle 0.0f; kf-bias 0.0f; kf-P[0][0] 0.0f; kf-P[0][1] 0.0f; kf-P[1][0] 0.0f; kf-P[1][1] 0.0f; kf-Q_angle 0.001f; kf-Q_bias 0.003f; kf-R_measure 0.03f; } float Kalman_Update(Kalman_t *kf, float new_angle, float new_rate, float dt) { float rate new_rate - kf-bias; kf-angle dt * rate; kf-P[0][0] dt * (dt * kf-P[1][1] - kf-P[0][1] - kf-P[1][0] kf-Q_angle); kf-P[0][1] - dt * kf-P[1][1]; kf-P[1][0] - dt * kf-P[1][1]; kf-P[1][1] kf-Q_bias * dt; float S kf-P[0][0] kf-R_measure; float K[2]; K[0] kf-P[0][0] / S; K[1] kf-P[1][0] / S; float y new_angle - kf-angle; kf-angle K[0] * y; kf-bias K[1] * y; float P00_temp kf-P[0][0]; float P01_temp kf-P[0][1]; kf-P[0][0] - K[0] * P00_temp; kf-P[0][1] - K[0] * P01_temp; kf-P[1][0] - K[1] * P00_temp; kf-P[1][1] - K[1] * P01_temp; return kf-angle; }这套代码的逻辑是每毫秒调用一次 Update传入加速度计解算出的角度 new_angle 和陀螺仪的原始角速度 new_rate。注意代码里 P 矩阵的更新方式和网络上流传的简化版不太一样这里是完整保留了协方差矩阵的递推关系在车体低转速时能明显减少角度输出的抖动。参数调整的经验是如果车体反应迟钝、倒下前角度还没跟上就把 Q_angle 调大一点如果角度噪声大、车体原地微抖就把 R_measure 调大。 dt 必须和实际调用周期一致如果主循环是 2ms 调用一次dt 就传 0.002写死 0.001 会让滤波输出失真这也是很多人移植滤波代码后发现角度“慢半拍”的直接原因。2.3 互补滤波与 DMP更轻量的替代方案互补滤波的实现思路非常直观陀螺仪积分后的角度在短时间内可信加速度计直接解算的角度在长时间内可信所以每次取 0.98 倍的陀螺仪角度加上 0.02 倍的加速度计角度一秒内完成约 50 次这样的递推后陀螺仪漂移就被不断“拉回”到加速度计参考轴上。典型代码如下#define ALPHA 0.98f float Complementary_Filter(float acc_angle, float gyro_rate, float dt) { static float angle 0.0f; angle ALPHA * (angle gyro_rate * dt) (1.0f - ALPHA) * acc_angle; return angle; }ALPHA 越大滤波结果越信任陀螺仪角度平滑但滞后明显ALPHA 越小角度越容易受振动影响但跟随加速度计的速度更快。平衡车上一般从 0.95 起步试车体前后倾斜时角度响应迟钝就减小到 0.90高频抖动就回调到 0.98。它虽然没有卡尔曼滤波那种严格的统计最优性但在平衡车这种控制周期只有 5ms、角度变化率有限的场景里性能差异几乎感知不到。DMP 的接入路径是官方库配合 I2C 读取。初始化时调用 dmp_init 并配置采样率主循环里通过 dmp_read_fifo 获取四元数再用四元数转欧拉角的公式得到 pitch。需要注意三点一是 DMP 初始化后必须先调用 dmp_set_fifo_rate 把输出频率设到 100Hz 左右太高的频率会导致 FIFO 频繁溢出二是读取时要用 dmp_get_pitch 这类接口而不是直接拿原始四元数算避免单位转换错误三是 DMP 的 yaw 会随机漂移但平衡车控制根本不依赖 yaw只取 pitch 和 roll 就够了。3. 源码架构拆解数据流、工程结构与启动顺序3.1 从传感器到 PWM 输出的完整数据链路这套源码的工程结构并不复杂核心可以分成四层传感器层负责初始化 MPU6050 并读取原始数据解算层负责把原始数据变成角度和角速度控制层跑 PID 三环输出层把控制量换算成 PWM 占空比。数据从传感器到电机输出完整链路是MPU6050 原始数据 → 加速度计/陀螺仪分量提取 → 姿态解算 → 机械角度与角速度 → 直立环 PD → 速度环 PI → 转向环 PD → 左右电机 PWM。这条链路里最容易断的位置在解算层和控制层之间。解算层输出的是“物理角度”单位是度控制层期望的输入是“相对于机械中位”的角度偏差。如果车体安装时传感器不是绝对水平解算出的角度就带了固定偏置这时必须做机械零点校准把这组偏置值记录在结构体里否则直立环会一直朝着一个方向推电机车一上电就冲出去。源码里用了 Angle_Offset 结构体来存三轴零偏这个设计很实用换车架时只需要重新校准不需要改控制代码。3.2 主循环与控制周期设计整个控制实时性依赖定时器中断而不是主循环 while(1) 里裸跑。源码用定时器产生 5ms 中断中断里依次执行读取 MPU6050 原始数据、姿态解算更新、直立环计算、速度环计算、转向环计算、最终 PWM 输出。这段代码是核心中的核心按顺序写清楚对后期调参非常关键。主循环部分通常长这样int main(void) { System_Init(); // 时钟、GPIO、定时器初始化 MPU6050_Init(); // 传感器初始化含自检 Kalman_Init(kf); // 选择卡尔曼滤波时启用 PID_Init(); // 三环 PID 参数初始化 Encoder_Init(); // 编码器捕获初始化 Motor_Init(); // PWM 定时器初始化 MPU6050_Calibrate(); // 静止校准获取零点偏置 while (1) { // 主循环只做低优先级任务 // 串口打印调试数据、按键检测、OLED 显示 Debug_Print_Angle(); Delay(10); } }这里有一个值得注意的设计控制部分全部塞进中断回调主循环只做调试信息输出和按键扫描这样 CPU 不会被耗时的浮点运算卡住控制节拍稳定。代码里涉及到一个关键的结构体 Balance_Control_t里面保存了角度目标值、速度累加值、上一次误差等状态每次中断更新它主循环读它避免数据竞争。3.3 模块划分与关键参数速查表源码把功能拆成了独立的模块文件命名和职责对应得非常清楚。下表列出主要文件、核心接口和修改参数时的关注点方便拿到源码后快速定位文件模块核心接口参数修改点mpu6050.cMPU6050_Init / MPU6050_Read低通滤波带宽、采样率分频kalman.cKalman_Init / Kalman_UpdateQ_angle / Q_bias / R_measurepid.cPID_Calc / PID_Reset三环 Kp / Ki / Kd、积分限幅motor.cMotor_SetPWM / Motor_InitPWM 频率、死区补偿、方向翻转encoder.cEncoder_Read / Encoder_Init编码器线数倍频、方向判定control.cBalance_Control_Run目标角度、速度环输出限幅这套源码里一个值得学习的点电机和编码器的方向判定用宏定义统一管理更换电机或把编码器装到另一侧时只需要改一个宏不用在 PID 输出里到处改正负号。初次移植时最容易翻车的就是方向问题电机转了但编码器计数方向反了直立环直接变成正反馈车越倒越加速。4. 三环 PID 控制参数整定流程与输出限幅实战4.1 直立环、速度环、转向环的分工与串联方式平衡车控制系统的框架是三个控制环叠加在同一个输出通道上。最内层是直立环输入是当前角度与目标角度的偏差输出是电机力矩目标是让车体保持垂直外层是速度环输入是目标速度与当前速度的偏差输出不是直接叠加到 PWM 上而是改变直立环的目标角度最外层是转向环输入是转向目标与当前偏航角速度的偏差输出叠加到左右电机的差模分量上。这种串联方式的核心设计意图是直立环必须最灵敏响应速度要快所以用 PD 控制Kp 负责把角度偏差转化成恢复力矩Kd 提供阻尼防止过冲速度环用 PI 控制因为速度误差的消除需要积分项来克服静摩擦力转向环和直立环一样用 PD因为转向动作本身是瞬态的不需要积分。三个环的输出叠加顺序是先算直立环得到基础 PWM再叠加速度环修正量最后叠加转向环差模量等效于左电机 直立环 速度环 转向环右电机 直立环 速度环 - 转向环。4.2 参数整定流程从静止到运动的三个阶段调参顺序强烈建议按下面的步骤走跳过任何一步都会让后面排查变得痛苦。第一步先把车体用绳子吊起来或者用手扶着让车轮悬空第二步调直立环 Kp从小到大逐步加直到用手把车体掰到一边时能感觉到明显的回复力矩并且车体不再一松手就软塌塌地倒下第三步加 Kd抑制车体在目标角度附近的振荡标准是车体被瞬时拨动后能在 1 到 2 个周期内收敛而不是持续高频抖动第四步把车放到地面调速度环 Kp让它能抵抗轻轻的推拉而不倒第五步加速度环 Ki消除长时间匀速漂移最后调转向环让车体能按照遥控指令平滑转向而不打转。调参过程中一个可行的中间状态判断方法代码里加一个串口调试输出每 10ms 输出一组数据格式为当前角度、目标角度、直立环输出、速度环输出、最终 PWM。调参时盯着这几个数值的关系看如果车体静止时直立环输出不为零说明目标角度和实际零点不一致如果速度环输出持续朝一个方向增长说明积分项在饱和需要检查限幅。源码里提供了这种调试宏开关默认注释掉打开后编译即可。直立环和速度环的代码通常是这样串联的int16_t Balance_Control_Run(float angle, float gyro, int16_t speed_left, int16_t speed_right) { float angle_err g_balance.angle_target - angle; float angle_out g_pid_balance.Kp * angle_err g_pid_balance.Kd * (-gyro); float speed_avg (speed_left speed_right) / 2.0f; float speed_err g_balance.speed_target - speed_avg; float speed_out g_pid_speed.Kp * speed_err g_pid_speed.Ki * g_pid_speed.integral; g_pid_speed.integral speed_err * 0.005f; if (g_pid_speed.integral 1000.0f) g_pid_speed.integral 1000.0f; if (g_pid_speed.integral -1000.0f) g_pid_speed.integral -1000.0f; float pwm_base angle_out speed_out; return (int16_t)pwm_base; }这段代码里有几个关键细节直立环的微分项用的是陀螺仪的角速度 gyro而不是角度误差的差分这样能避免对加速度计噪声做微分放大的问题速度环的积分项手动做了限幅防止车被持续外力推着时积分无限增大导致外力消失后车疯狂朝反方向冲出去速度环输出是叠加到 angle_out 上的这正好实现了“速度环通过改变目标角度来影响直立环”的设计意图。限幅参数的经验范围可以这样记直立环输出在 -700 到 700 之间速度环输出在 -300 到 300 之间转向环输出在 -200 到 200 之间最终 PWM 输出限幅在 -900 到 900。这些数值不是固定的具体要看电源电压和电机额定参数。如果 PWM 满偏值本身设定是 1000就不要让最终输出长期贴着 1000 跑否则电机处于饱和状态速度环和转向环都会失效车体会呈现明显的“失控感”。5. 避坑排查五种高频故障现象原因与解决记录5.1 电机高频“嗞嗞”声且车体剧烈抖动现象上电后电机没有正常转动而是发出尖锐的高频啸叫同时车体以肉眼可见的频率剧烈抖动用手按住车体时能感觉到明显的振动力度但一松手就倒向一侧。原因PWM 频率设置过低常见值是几百赫兹。电机驱动电路和电机的电感在这种频率下产生明显的电流波动导致力矩输出断续而直立环又把这种断续当成角度误差来响应形成正反馈振荡。解决把 PWM 定时器的频率配置成 10kHz 到 20kHz 之间。在源码的 motor.c 里修改定时器分频和重载值让 PWM 频率进入人耳听不到的超声范围。修改后电机声音会变得很干净车体的高频抖动会立刻消失。检查手段是用示波器看 PWM 波形频率或者听声音变化明显可感知。5.2 MPU6050 初始化失败I2C 通信卡死现象程序运行到 MPU6050_Init 时卡住串口没有打印任何信息或者打印了初始化失败提示复位后依旧如此。原因排除了接线错误后最常见的是 I2C 总线上没有上拉电阻。部分开发板内部已经集成上拉但外接 MPU6050 模块时如果连接线过长、接触不良总线 SCL/SDA 处于高阻态单片机检测不到从设备应答。解决给 SCL 和 SDA 各加一个 4.7k 欧姆上拉电阻到 3.3V另外 I2C 速率不要太高源码里默认配置为 400kHz把分频系数改大降速到 100kHz 试试能提高抗干扰能力。检查手段是单独写一个只扫描 I2C 设备地址的测试函数看能否返回 0x68 或 0x69如果返回值不对优先怀疑上拉和接线。5.3 静止时角度输出缓慢漂移现象车体静止放在桌面上串口打印的姿态角度数值每隔几秒就变化零点几度时间越长漂移越明显导致直立环误以为车体在倾斜。原因最直接的原因是陀螺仪零偏未校准。MPU6050 出厂时存在温漂和个体差异的零偏如果不做静止校准陀螺仪积分会把这个固定偏差不断累积导致角度随时间漂移。另外一个常见原因是加速度计未做低通滤波高频振动噪声被直接引入角度计算。解决第一步在初始化后让车体完全静止 1 秒以上采样陀螺仪输出求平均值作为零偏补偿值第二步确认滤波系数是否正确把加速度计原始数据经过低通滤波后再算角度。这套源码里已经实现了 mpu6050_offset_cal 函数调用位置在主循环之前使用时要确认车体确实静止否则会把运动状态当成零偏记录下来。5.4 电池电压下降后车子突然变软或朝一个方向加速现象满电时车体直立稳定运行几分钟后电池电压降低车体开始慢慢前倾直立环输出不足以把它拉回来最后朝前加速倒下。原因这是典型的 PWM 输出未做电源电压补偿。当电池电压下降时同样占空比下电机实际电压降低、力矩变小但控制环的 PID 计算依然按满电压的等效关系输出相当于控制器对执行机构的控制能力打了折扣系统增益随电压变化。解决在读取电池电压的 AD 值后按电压比例对最终 PWM 输出做修正常见做法是维持一个基准电压比如 12V 时输出 1000 对应满占空比当电压降到 10V 时输出限幅值同步按 10/12 缩放保持电机的实际出力不变。这样电压变化时控制参数基本不受影响。源码里预留了 voltage_compensate 接口没有实现完整逻辑建议自行填充 AD 采集代码。5.5 装好后转向环正常一加速度环就左右甩尾现象单独调直立环时车体很稳手轻轻推也能恢复直立。但开启速度环后车体开始前后往复摆动幅度越来越大最后变成左右甩尾式的失稳。原因速度环积分系数设置过大或者速度环输出限幅值设得太高。速度环输出改变的是直立环的目标角度如果目标角度变化幅度过大、变化速度过快直立环会认为车体严重偏离目标产生一个很大的输出导致电机猛烈加速反过来又让速度误差更大形成恶性循环。解决把速度环的 Ki 减半再观察同时把速度环输出限幅从 300 收窄到 150让速度环对目标角度的改变更温和。另外确认速度环的反馈方向是否正确——车体向前运动时编码器值应为负速度误差为负输出应为正如果方向反了速度环会持续加大车速导致失控。判断方法用手推动车体观察串口打印的速度值符号和方向是否一致不对的话修改编码器方向宏定义。6. 进阶用法上电自检、数据观测与站立自锁调试技巧最后分享三个在这套源码基础上常用的进阶技巧能显著减少调试时间和维护成本。第一个是上电自检流程在进入主循环之前依次执行传感器初始化、零偏校准、电机 PWM 输出置零、编码器读数清零然后延时 500ms 等待电源稳定再解除电机刹车。这个流程看起来简单但能避免一个问题——很多车一上电就突然加速是因为电机在初始化瞬间收到了一个残留在 PWM 寄存器里的旧值。正确做法是初始化最后把 PWM 输出强制写到 0再做一次编码器清零确保第一帧控制输出时所有反馈都是干净的。第二个是数据观测方法。调平衡车必须能看到角度和输出不然全靠手感是玄学。建议用串口以 100Hz 频率输出一段格式化数据终端里按列显示角度、目标角度、直立环输出、速度环输出、最终 PWM。调试时重点看两个指标一是静止时角度和目标角度的差长期是否为零不为零就要重新做机械零点校准二是拨动车体一次后角度能否在两个来回内收敛到目标值附近如果连续振荡四次以上就说明 Kd 不够或者滤波环节的相位滞后太大。更快的方式是先用仿真工具对卡尔曼滤波和 PID 参数做一次离线验证很多数学工具都能直接导入传感器采集的数据调好参数再烧进单片机能明显减少在实物上反复烧录的次数。第三个是站立自锁功能。平衡车有两种常见工作模式上电即平衡、上电视为待机模式。待机模式更安全——上电后车子不转按下按键后才启动平衡算法。实现方式不复杂主循环检测按键按下置位 enable 标志控制中断里检查该标志没有置位时直接把 PWM 输出置零且不执行 PID 计算。这个习惯值得保留特别是调试阶段车子放在桌面时不会被意外启动的电机带跑。初始化时我一般会强制走一遍整体检查先看传感器数据是否稳定再看角度能否跟随手动倾斜再确认电机方向与编码器方向一致最后才松开手让它进入平衡状态。从那以后我每次移植新源码或者换新硬件都强制走这一遍节省过太多排查时间希望帮到你。整套源码的完整工程文件、依赖库和烧录说明都在压缩包里按 README 里的引脚映射接好线烧录后先调直立环就能站起来。本文还有配套的精品资源点击获取