二轮平衡车全套源码：卡尔曼滤波与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 模块划分与关键参数速查表

源码把功能拆成了独立的模块文件，命名和职责对应得非常清楚。下表列出主要文件、核心接口和修改参数时的关注点，方便拿到源码后快速定位：

这套源码里一个值得学习的点：电机和编码器的方向判定用宏定义统一管理，更换电机或把编码器装到另一侧时，只需要改一个宏，不用在 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 里的引脚映射接好线，烧录后先调直立环就能站起来。

本文还有配套的精品资源，点击获取