二轮平衡车源码解析：姿态解算、PID调参与避坑实践

简介：面向正在DIY二轮平衡车或入门嵌入式姿态控制的开发者，这里集中了一份本科阶段通过调试的完整方案，覆盖STM32硬件设计、电路连接与三类常见滤波算法源码，可直接作为个人小车上手的参考资料。软件部分均编译通过，包含互补滤波版、卡尔曼滤波版和DMP版，可对照学习不同姿态解算方法在真实底盘上的差异，也方便直接烧录后观察数据波形、调整PID参数。硬件资料同步给出电路原理图、接线方式和元器件说明，从主控、电机驱动到传感器均有对应内容，能减少从原理图到实物装配的踩坑环节。压缩包整体约148.18MB，主要文件为工程源码、原理图及连线说明文档，目录按硬件与算法分模块整理，查找高效。目前已有3128人学习下载，适合想在平衡车上做课程设计、竞赛调试或毕业设计参考的读者，直接基于这套资料可快速搭建属于自己的平衡小车。

1. 二轮平衡车全套源码：先想清楚你拿到的是什么东西

如果你搜到这份「二轮平衡车全套源码（超全）」，大概率不是想买一台成品小车，而是想自己从头搭一台能直立、能跑、能转向的平衡车。我见过太多人下载源码后第一件事就是打开主函数找main()，然后对着上千行代码发呆。我建议你换个顺序：先花半小时确认这套源码的硬件依赖、控制链路和文件组织，再决定是直接烧录还是按自己的板子改。因为二轮平衡车看起来是个玩具，但它背后的姿态解算、电机驱动、PID 串联这三段链路，和工业级运动控制没本质区别，坑也几乎一样。这套源码能帮你省掉从零开始写滤波和解算的时间，但前提是你得知道每一段代码对应哪个硬件、哪个参数、哪个故障。

我用一线调试的真实视角，把这个源码拆成五块讲清楚：硬件框架怎么对号入座、姿态解算代码怎么读、三段 PID 怎么调、踩过哪些坑、以及最后怎么验证你的车真的稳定。

2. 硬件框架与源码目录约定：先看接口，再谈移植

2.1 这套源码默认的硬件配置：MCU、IMU、电机编码器

绝大多数二轮平衡车源码基于 ARM Cortex-M 系列 MCU，常见的是某款主流 M4 内核芯片，主频在 168MHz 上下。IMU 几乎都是六轴：三轴陀螺仪加三轴加速度计，常见的型号是 MPU6050 或 ICM20602，用 I2C 接口挂在 MCU 上。电机驱动用直流减速电机加霍尔编码器，编码器信号接在定时器的编码器模式引脚上，PWM 输出接在另一组定时器通道上。电池一般是 11.1V 三节锂电，经过降压给 MCU 供电，直接给电机驱动模块供电。

你在看源码前，先把硬件手册翻出来对一下引脚定义。源码里的bsp.c或board.h这类文件会定义引脚映射，比如编码器 A 相接哪两个引脚、PWM 输出接哪两个引脚、I2C 接哪两个引脚。如果你的板子和源码默认的不一样，第一步不是改代码逻辑，而是先把这些宏定义改对，否则后面一切现象都是玄学。

我一般会先画一张表格，把自己的板子引脚和源码默认引脚逐一对照：

这张表看起来简单，但能避免一半的翻车。编码器引脚接错是最隐蔽的，因为车能走但不稳，你很难第一时间想到是引脚映射错了。

2.2 源码目录结构与关键文件：不要盲目打开main.c

拿到全套源码后，先看目录结构。常见做法是分成BSP（板级支持包）、App（应用层）、Control（控制算法）、Fusion（姿态融合）、Driver（外设驱动）。有的源码会混在一起，但控制算法一般放在单独目录。你优先打开下面这几个文件，其他可以先不看：

. ├── BSP │ ├── bsp_motor.c # 电机 PWM 输出与编码器读取 │ ├── bsp_mpu6050.c # IMU 驱动：I2C 读写、寄存器配置 │ └── bsp_uart.c # 串口打印与调试指令解析 ├── App │ ├── app_balance.c # 主循环状态机，调度解算和控制 │ └── app_control.c # 三段 PID：直立、速度、转向 ├── Fusion │ └── mahony.c # 姿态解算：互补滤波 / 四元数 ├── Control │ ├── pid.c # 位置式和增量式 PID │ └── pid.h # PID 结构体定义 └── main.c # 入口，初始化硬件并启动调度

先读pid.h和mahony.c，因为这两个文件决定了控制链路的基本形态。main.c反而是最后才看的东西——它只是把初始化函数按顺序调一遍，真正干活的是app_control.c里每 5ms 或 10ms 执行一次的控制任务。

2.3 移植到自己的板子：三步改引脚，别改算法

我自己的习惯是：先把bsp_motor.c、bsp_mpu6050.c里的引脚宏全部改掉，然后编译看报错，再逐一调试外设。下面这段是bsp_motor.c里最常见的一段引脚配置代码：

void BSP_Motor_Init(void) { GPIO_InitTypeDef GPIO_InitStructure; TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_OCInitTypeDef TIM_OCInitStructure; // 使能定时器1和对应GPIO时钟 RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE); RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE); // 配置 PA8、PA9 为复用功能推挽输出(PWM输出引脚) GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8 | GPIO_Pin_9; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; GPIO_Init(GPIOA, &GPIO_InitStructure); // 将 PA8、PA9 连接到 TIM1 的 CH1、CH2 复用功能 GPIO_PinAFConfig(GPIOA, GPIO_PinSource8, GPIO_AF_TIM1); GPIO_PinAFConfig(GPIOA, GPIO_PinSource9, GPIO_AF_TIM1); // 定时器1配置：预分频和周期决定PWM频率 TIM_TimeBaseStructure.TIM_Prescaler = 84 - 1; // 84MHz/84 = 1MHz TIM_TimeBaseStructure.TIM_Period = 1000 - 1; // 1MHz/1000 = 1kHz PWM TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure); // PWM通道配置 TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; TIM_OCInitStructure.TIM_Pulse = 0; TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; TIM_OC1Init(TIM1, &TIM_OCInitStructure); TIM_OC2Init(TIM1, &TIM_OCInitStructure); TIM_Cmd(TIM1, ENABLE); TIM_CtrlPWMOutputs(TIM1, ENABLE); // 高级定时器必须使能主输出 }

这段代码的关键在于两个参数：TIM_Prescaler = 84 - 1和TIM_Period = 1000 - 1，它把 PWM 频率定在 1kHz。平衡车电机 PWM 频率一般建议 1kHz～10kHz，低于 1kHz 电机会有啸叫，高于 10kHz 驱动管的开关损耗会增加。用 IGBT 或大功率 MOSFET 驱动时尤其注意频率别太高。

参数说明：TIM_Prescaler是预分频值，84 - 1表示把 84MHz 的时钟除以 84，得到 1MHz；TIM_Period是计数周期，1000 - 1表示计数到 1000 才溢出，于是最终频率是 1MHz / 1000 = 1kHz。你把这两个值同时乘以 10，就能得到 10kHz 的 PWM，但前提是驱动模块支持这么高的开关频率。

2.4 编码器读取与方向判断：为什么车轮反转时你会怀疑人生

编码器代码是另一个移植重点。霍尔编码器输出两路正交方波，定时器工作在编码器模式时，硬件会自动根据 A、B 相先后顺序累计计数。源码里读取编码器值的函数通常是：

int16_t BSP_Encoder_Get_Left(void) { return (int16_t)TIM3->CNT; // 读取计数寄存器 } void BSP_Encoder_Clear_Left(void) { TIM3->CNT = 0; // 清零，供速度环周期累加使用 }

注意两个细节：一是TIM3->CNT是有符号的，正负方向由 A、B 相先后决定，不要把它当无符号数处理；二是每次读取完要清零，因为速度环需要的是“一个周期内转了多少圈”，而不是累计总量。我见过有人不清零，导致速度环积分越积越离谱，车原地抖动，排查了一下午。

如果两个轮子的编码器读数方向不一致，会出现什么现象？车能直立，但你推它一下，它不会前进，而是原地打转。原因是左右速度环反馈方向相反，控制器以为一个轮子超速、一个轮子欠速，于是持续输出相反的修正力矩。这个现象极其容易误判为 PID 参数问题，其实是硬件方向问题。最简单的修正方式是在初始化时判断方向，或者干脆在读取函数里把其中一个取反：

int16_t BSP_Encoder_Get_Right(void) { return -(int16_t)TIM4->CNT; // 如果右轮方向反了，在这里取反 }

3. 姿态解算源码走读：互补滤波那条主链路的参数都在哪

3.1 为什么平衡车不用卡尔曼滤波，而用互补滤波

姿态解算是平衡车的灵魂。小车直立的前提是知道自己倾斜了多少度，而倾斜角不能直接测量，只能通过陀螺仪积分或者加速度计反算。陀螺仪短时间准但会漂移，加速度计长时间稳但怕震动，互补滤波就是把两者按频率互补地融合在一起：低频信加速度计，高频信陀螺仪。

卡尔曼滤波在原理上更优，但它在嵌入式上有个实际问题：需要构造状态转移矩阵和测量矩阵，维数多了计算量大，而且几个噪声协方差矩阵的初值调起来非常玄学。平衡车这个场景，动态范围不大、模型相对固定，互补滤波的精度已经足够，而且代码短、容易调参。所以这套源码几乎可以肯定用的是互补滤波或它的四元数版本。

3.2 读懂 Mahony 互补滤波：核心代码就三行

下面这段是 Mahony 互补滤波的核心，它用加速度计修正陀螺仪积分带来的漂移：

void Mahony_Update(float gx, float gy, float gz, float ax, float ay, float az) { float norm; float vx, vy, vz; float ex, ey, ez; float halfT = 0.005f; // 采样周期的一半，按 5ms 控制周期算 // 1. 把加速度计测量值归一化，得到重力方向单位向量 norm = sqrt(ax*ax + ay*ay + az*az); ax /= norm; ay /= norm; az /= norm; // 2. 用当前四元数推算重力方向向量 vx = 2.0f * (q1*q3 - q0*q2); vy = 2.0f * (q0*q1 + q2*q3); vz = q0*q0 - q1*q1 - q2*q2 + q3*q3; // 3. 叉积求误差：陀螺仪积分导致的重力方向偏差 ex = ay*vz - az*vy; ey = az*vx - ax*vz; ez = ax*vy - ay*vx; // 4. 误差反馈到陀螺仪积分上，Kp 决定修正速度 gx += Kp * ex; gy += Kp * ey; gz += Kp * ez; // 5. 四元数一阶积分更新 q0 += halfT * (-q1*gx - q2*gy - q3*gz); q1 += halfT * ( q0*gx + q2*gz - q3*gy); q2 += halfT * ( q0*gy - q1*gz + q3*gx); q3 += halfT * ( q0*gz + q1*gy - q2*gx); }

这段代码里，Kp是滤波器的核心参数。Kp越大，加速度计对陀螺仪的修正越强，姿态角跟随越快，但对震动越敏感；Kp越小，姿态角越平滑，但陀螺仪漂移会导致角度慢慢飘走。一般经验值在5.0f ~ 20.0f之间，具体要看你的采样周期和 IMU 噪声水平。要注意的是，这里的Kp和控制 PID 的 Kp 是两回事，新手最容易混淆。

halfT = 0.005f表示采样周期的一半，如果你把控制周期改成 2ms，这里要改成0.002f，否则四元数积分会不准确，表现出来是静止时角度缓慢漂移。这个值改错的现象很隐蔽，因为车刚开机时是稳的，过几十秒才慢慢歪。

3.3 从四元数到欧拉角：翻滚角、俯仰角哪个是你的控制输入

平衡车只需要一个方向的倾斜角，通常叫俯仰角（Pitch）。四元数转欧拉角的代码基本长这样：

float q0, q1, q2, q3; // 全局四元数 float Get_Pitch(void) { // 从四元数计算俯仰角，结果单位是弧度 float pitch = asinf(-2.0f * (q1*q3 - q0*q2)) * 57.29578f; return pitch; }

注意这个asinf的范围问题：当俯仰角接近 ±90° 时，asinf的输入接近 ±1，此时角度分辨率变低，而且可能产生跳变。平衡车正常的倾斜范围在 ±30° 以内，不需要担心这个问题。但如果你把小车拿起来竖着转，角度显示可能会突然跳一个很大的值，这是正常的数学特性，不是代码 bug。

这里有一个非常常见的坑：有些源码会用atan2算俯仰角，有些用asin，两者在小角度下结果接近，但符号方向可能相反。如果你发现把车往前倾，角度输出是负的，而 PID 期望的是正的，整个控制就是正反馈，一上电就飞车。遇到这种情况不要急着调 PID，先看看角度符号方向是否和你的电机正方向匹配。

4. 三段串联 PID：直立、速度、转向的控制链路与参数设定

4.1 为什么是“串级”而不是“并联”：直立环是内环，速度环是外环

二轮平衡车有至少三个控制目标：保持直立、达到目标速度、按目标转向。这三个目标不是并列关系，而是嵌套关系。直立环是内环，它直接输出电机 PWM；速度环是外环，它输出的是角度目标值。也就是说，速度环不是直接操纵 PWM，而是告诉直立环“你要稍微往前倾一点”，直立环再去执行这个倾斜指令。这个结构叫串级 PID，内环快、外环慢。

转向环相对独立，它输出的是左右轮 PWM 的差值，叠加在直立环的输出上。用公式表达就是：

PWM_Left = Output_Pitch + Output_Speed + Output_Turn PWM_Right = Output_Pitch + Output_Speed - Output_Turn

Output_Pitch来自直立环，Output_Speed是速度环输出叠加在角度目标上的量，Output_Turn是转向环输出。这个结构决定了一个问题：转向环不能单独工作，它必须依赖直立环先立住。很多人在调完直立后直接调转向，发现转向没反应，其实是因为速度环还没调好，角度目标一直在变，转向量被淹没了。

4.2 直立环 PID：先调 P，再调 D，I 最好为零

直立环是整台车第一个要调的环。它的输入是当前俯仰角，输出是 PWM 修正量。直立环的控制目标是让角度回到 0°，所以它本质上是一个角度 PD 控制器，I 项通常不需要。

int16_t Pitch_PID(float angle, float gyro_rate) { static float last_angle = 0.0f; static float integral = 0.0f; // 1. 比例项：角度偏差直接决定修正力度 float P = Pitch_Kp * angle; // 2. 微分项：用陀螺仪角速度替代角度差分，信号更干净 float D = Pitch_Kd * gyro_rate; // 3. 积分项：平衡车直立不需要积分，抗静差靠角度本身就够 // 除非机械结构存在严重不对称，才需要加一点点 I float output = P + D; // 输出限幅，防止飞车 if (output > PWM_MAX) output = PWM_MAX; if (output < -PWM_MAX) output = -PWM_MAX; return (int16_t)output; }

这里最关键的细节是微分项用的是陀螺仪角速度，而不是角度差分。因为角度是从滤波输出的，本身有延迟，如果再对它差分，相当于二次放大噪声。陀螺仪输出的是角速度，它和角度微分是同一个物理量，但噪声特性更好。这个细节决定了你的 D 项能不能调大而不产生震荡。

调参顺序：先给Pitch_Kp一个很小的值，比如 5，观察车能不能“软绵绵”地往平衡点靠。如果车完全没反应，加大 Kp；如果车开始高频抖动，说明 Kp 太大或者缺少 D 项。当车能勉强立住但来回晃动时，开始加Pitch_Kd，每次加 0.5，直到晃动明显减弱。这个过程大概需要半小时。

4.3 速度环 PID：把“目标角度”变成“目标速度”的反馈

速度环的输入是当前速度与目标速度的差，输出是角度目标值的修正量。它不需要直接输出 PWM，而是修改直立环的平衡点。这是整台车最难调的部分，因为速度环和直立环之间存在耦合：你加一点速度环输出，直立环就以为车要往前倒，于是加大修正，速度环又觉得速度不够，继续加——这是极限环震荡的经典来源。

int16_t Speed_PID(int16_t current_speed, int16_t target_speed) { static float integral = 0.0f; float error = target_speed - current_speed; // 1. 积分项：累积速度偏差，克服静差 integral += Speed_Ki * error; // 2. 限幅：积分不能无限大，否则恢复太慢甚至过冲 if (integral > 100.0f) integral = 100.0f; if (integral < -100.0f) integral = -100.0f; // 3. 输出的是角度修正量，单位是度 float output = Speed_Kp * error + integral; // 4. 限幅：防止给的角度目标太离谱 if (output > 15.0f) output = 15.0f; if (output < -15.0f) output = -15.0f; return (int16_t)output; }

速度环的output直接加到角度目标值上，所以它的单位是“度”，而不是 PWM 值。这就是为什么速度环限制了±15度——如果目标倾斜角超过 15 度，车基本已经处于失控边缘了。速度环的正确调法是：先让车在原地直立稳定，然后手动把目标速度设为一个固定值，比如 0.2m/s，然后从小到大调Speed_Kp，观察车是否开始缓慢加速。如果车前后震荡加速，说明 Kp 太大；如果车不动，说明 Kp 太小。

速度环积分项上限100不是拍脑袋定的。我测试过，积分上限过大会导致车从静止启动时猛地往前冲，因为积分项已经积累到一个很大的值，即使速度已经达标，它还要先“泄洪”。所以积分限幅要和小车的响应速度匹配：底盘轻、响应快的车，积分限幅要小一些。

4.4 转向环 PID：这是三个环里最容易忽略的

转向环的输入是目标转向角速度与当前转向角速度的差，输出是左右轮差值。这里不需要用陀螺仪的 Z 轴角速度，直接用编码器左右轮速度差就能推算转向角速度：

int16_t Turn_PID(int16_t left_speed, int16_t right_speed, int16_t target_turn_rate) { int16_t current_turn_rate = (right_speed - left_speed) / 2; float error = target_turn_rate - current_turn_rate; float output = Turn_Kp * error; // 转向环一般只用 P 项，加 D 项容易引入噪声 if (output > 100.0f) output = 100.0f; if (output < -100.0f) output = -100.0f; return (int16_t)output; }

有人问转向环的 D 项去哪里了？其实转向 D 项就是左轮速度减去右轮速度再取反，本质上和当前转向速率是同一个量，用了反而容易振荡。转向环的核心是 Kp 和死区：如果你发现车向左转一点然后回正，来回摆动，说明 Kp 偏大；如果转向响应迟钝，可以加一点点Turn_Kd，但一般建议不需要。还有一个极容易被忽视的点：转向环的限幅不能和直立环一样大，否则在高速转向时左右轮差会把直立环的输出淹没，导致侧翻。

4.5 参数速查表：从零调通一套车的参考区间

给出一个参考参数表，基于常见 12V 减速电机、轮径 65mm、车重 1.5kg 左右的小车。这张表的作用是让你有一个安全起点，而不是直接照抄：

注意：这套参数不是万能模板，但它是一个能让你在二十分钟内跑起来的起点。调参的黄金法则是“每次只改一个参数，改完记录现象”。否则两个参数互相掩盖，你永远不知道当前车的问题到底是哪个环造成的。

5. 全套源码落地避坑清单：五条来自产线的血泪记录

5.1 上电飞车根本不是 PID 问题，而是电机方向反了

现象：一上电，车轮猛转，车像弹射一样冲出去，速度极快，根本不给调参机会。很多人第一反应是 Kp 太大，赶紧把 Kp 往下调，但即使调到 0，车依然在冲。

原因：直立环输出正负和电机转动方向不匹配。控制器检测到车往前倾，输出正 PWM 让轮子往前滚去“接住”车，结果电机机械方向是反的，轮子往后滚，相当于车越往前倾、控制器越把它推倒，形成正反馈。

解决：先做开环测试。把直立环输出旁路，直接手动给电机一个固定 PWM，确认左右轮前进方向一致；然后把车放在地面扶正，用手向前推一下，同时看编码器数值变化和电机输出的符号是否“试图”阻止你推。如果不一致，在代码里改电机方向宏或在编码器读取函数里取反。这是最优先要检查的一步，没有之一。

5.2 角度静止漂移：不是滤波问题，而是陀螺仪零偏没消除

现象：车静止放在桌面上，用上位机看角度，发现角度以每分钟几度的速度缓慢爬升或下降，完全不动也在变。调滤波的Kp没用，调到很大反而角度开始抖动。

原因：陀螺仪内部存在零偏（bias），即静止时输出不为零，而是有一个小的固定偏移。四元数积分会把这个偏移累积起来，表现为角度漂移。滤波器的Kp只能修正动态误差，无法完全消除静态零偏。

解决：在初始化时做陀螺仪零偏校准。常见做法是上电后让车静止 1 秒，读 100 次陀螺仪 Z 轴和 XY 轴数据取平均，把这个平均值存下来，之后每次读取陀螺仪时都减去这个零偏值。代码长这样：

float gyro_offset_x = 0.0f, gyro_offset_y = 0.0f; void IMU_Calibrate_Offset(void) { float sum_x = 0.0f, sum_y = 0.0f; int i; for (i = 0; i < 200; i++) { sum_x += Read_Gyro_X(); // 读取陀螺仪X轴原始值 sum_y += Read_Gyro_Y(); // 读取陀螺仪Y轴原始值 delay_ms(5); } gyro_offset_x = sum_x / 200.0f; gyro_offset_y = sum_y / 200.0f; } float Read_Gyro_Calibrated_X(void) { return Read_Gyro_X() - gyro_offset_x; // 校准后陀螺仪数据 }

参数说明：校准样本数200和采样间隔5ms加一起是 1 秒，时间太长用户体验差，太短零点稳定性差。如果你校准后还是漂移，检查是不是校准完成后车又被移动过，或者 IMU 芯片离电机太近、受到发热影响。注意，零偏是温度的函数，车刚开机和跑十分钟后的零偏会不一样，严谨的做法是在工作温度下校准，但 DIY 场景用开机校准就够。

5.3 编码器读数跳变导致速度环抽搐：定时器初始化顺序有讲究

现象：速度环调好后，车静止时电机有“哒哒哒”的轻微抽搐声，看编码器数值，静止时也在 ±3 以内跳变，而不是稳定为 0。这个跳变像噪声一样，被速度环积分后变成小幅震荡。

原因：编码器用的是定时器编码器模式，但定时器初始化时，计数器初始值不是 0，或者编码器模式配置和引脚复用没设好，导致在电机没动的情况下，计数器自己跳。还有一个隐蔽原因是：编码器引脚没有配置上拉输入，导致悬空噪声被当成有效电平。

解决：首先把编码器引脚的模式从GPIO_Mode_AF改成GPIO_Mode_IN并带上拉（GPIO_PuPd_UP），因为编码器模式并不需要复用功能输出，但必须有上拉输入才能保证静止电平稳定。其次，在配置完定时器后，手动将TIMx->CNT = 0清零一次。第三，如果跳变仍然存在，检查编码器线缆是否和电机 POWER 线并行走线，若是，将编码器线单独走线远离电机。

void BSP_Encoder_Init(void) { GPIO_InitTypeDef GPIO_InitStructure; // 编码器引脚用输入模式，带上拉 GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4 | GPIO_Pin_5; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN; GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; GPIO_Init(GPIOB, &GPIO_InitStructure); // 复用配置为定时器功能 GPIO_PinAFConfig(GPIOB, GPIO_PinSource4, GPIO_AF_TIM3); GPIO_PinAFConfig(GPIOB, GPIO_PinSource5, GPIO_AF_TIM3); // 清除计数寄存器，保证初始状态为0 TIM_SetCounter(TIM3, 0); }

我把这段代码放在这里，是想提醒你：编码器读取跳变在平衡车项目中不是疑难杂症，但它特别影响调参效率。你要先花十分钟把信号质量搞定，再去调速度环，否则每一次调参都会被“哒哒哒”的抽搐声干扰判断。我自己的习惯是，任何 PID 调试之前，先把所有外设的原始数据打印出来确认干净，再做控制。

5.4 电池电压不同，手感完全不一样：这是参考电压的锅

现象：刚充满电时，平衡车有力、容易控制；玩半小时后，明显变软，甚至无法直立。或者同一套代码，换一块电池测试，参数全乱，之前调好的 Kp 现在全错。

原因：一部分原因是电池电压下降导致电机最大转速下降，PWM 满占空比的输出扭矩变小；另一部分原因是 PWM 的参考电压变了，如果电机驱动的逻辑电源是从电池直接取的，那么同样的 PWM 占空比，在 12.6V 和 11.1V 下实际电压差了 12%，电机响应差了接近 20%。

解决：这是物理限制，改代码没法完全解决。但你能做两件事。第一，把电机的 PWM 输出改成“电流模式”或者“电压补偿模式”，即在代码里读取电池电压，算一个补偿系数乘到输出上。第二，更实用的做法是把直立环的输出限幅设置为随电压动态变化，低电压时减小限幅，防止电机因为扭矩不足而被 PID 输出推死。常见做法是加一层电池电压采集，代码长这样：

uint8_t Calc_Battery_Compensation(void) { float vbat = Read_Battery_Voltage(); // ADC采集电池电压 float comp = 1.0f; if (vbat < 11.0f) { comp = 0.85f; // 低压时降低输出限幅 } else if (vbat < 10.5f) { comp = 0.70f; // 更低压时限制更明显 } return (uint8_t)(PWM_MAX * comp); // 返回动态限幅值 }

如果你的项目要求“插上电就能玩”，不考虑电池电压，那就要把调试时的电池状态固定在一个区间，比如电量在 40% 到 60% 之间调参，这样至少保证大部分时间的表现居中可用。我自己的习惯是把电池电压挂在串口调试画面上，调参前先看一眼电压，电压太低不调，因为那是浪费时间。

5.5 滤波太“柔和”导致车永远在晃动：Kp 与控制周期不匹配

现象：车的角度波形看起来非常平滑，但车就是一直小幅度前后晃动，怎么调直立环 Kp 和 Kd 都无法消除。看代码发现滤波器的Kp是 2.0，而控制周期是 10ms。

原因：互补滤波的Kp决定了加速度计修正的速度。当控制周期快，滤波Kp太小，角度信息里包含了过多的陀螺仪积分成分，导致角度响应跟不上实际物理倾斜。表现出来就是角度波形平滑，但实际车已经来回晃了好几轮，滤波后才慢慢跟上。

解决：控制周期和滤波Kp需要匹配。控制周期 5ms 时，Kp用 8～10 起步；控制周期 10ms 时，Kp要相应增加，因为每次修正的间隔变长了，需要更大的修正量。一个快速判断方法是：把车拿在手里做快速俯仰运动，观察串口打印的角度是否跟手。如果角度有明显的滞后，说明Kp太小。如果你发现无论怎么调Kp，角度总有稳定的一拍延迟，那么问题可能在四元数更新的halfT上——它必须和控制周期的实际值一致。

6. 把源码跑透的最后一步：用数据回放习惯验证你的整车状态

调完这三段 PID，你的车应该能直立、能前进后退、能转向了。但这时候你只完成了 80%，剩下的 20% 是验证整车状态是否“健康”。我强烈建议你养成的第一个习惯是：每次调完参数，把俯仰角、角速度、左右编码器速度、目标速度这五路数据通过串口打出来，并录制下来。

你不需要一帧一帧看数据，只需要关注三段波形：第一段是俯仰角在 ±2° 以内是否稳定，第二段是左右轮速度是否在目标速度附近且没有持续振荡，第三段是转向输出是否频繁打满限幅。这三段波形能告诉你车的“体质”如何。参数都调好了，但俯仰角波形有间歇性尖峰，那可能是编码器线缆偶尔受到电机干扰；速度波形有稳定的正弦波，那可能是速度环带宽太低和直立环产生了耦合。

接下来做四个验证动作。第一个是推车恢复测试：车立住后，用手向前推一下，看它能否在 2 秒内恢复直立且不前后甩动。第二个是斜坡驻车测试：找一块 5 度左右的斜坡，让车停上去，看它会不会自己溜走。第三个是满载测试：在车顶放一个 500g 的负载，看它会不会变得迟钝或开始震荡，这能检验速度环的鲁棒性。第四个是电池低电压测试：把电池玩到接近没电，确认低压补偿逻辑正常工作。

我自己的习惯是准备一个独立的调试记录本，每改动一组参数就记录：参数值、室温、电池电压、现象描述。这个习惯救过我很多次。有时候你调了一小时参数，发现车越来越差，回到上一个“能直立”的版本时，如果没有记录，你只能凭记忆往回瞎摸。有了记录，你三分钟就能恢复到最优参数。另外，每次只调一个环、只改一个参数、改完立刻验证，是效率最高的路，虽然听起来慢，实际上半小时的成果比各种参数乱飞的三个小时还大。

最后说一个玄学以外的建议：所有参数在源码里都要有注释说明修改意图。比如#define Pitch_Kd 1.2f // 底盘偏重，已调大，过轻车需要减半。半年后你翻回来看这些代码，会发现这些注释比代码本身更值钱。希望这些从源码里榨出来的经验和教训能帮到你。

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