简介一套面向两轮平衡小车项目的控制系统学习资源适合自动控制、嵌入式及机器人方向的学生与开发者循序渐进学习。资源以姿态检测—数据融合—控制决策—电机执行为主线集中呈现比例积分微分控制、卡尔曼滤波、状态观测器等算法在硬件上的真实部署过程。压缩包共1420个文件容量约36.17MB主体为439个C源文件和379个头文件并配有工程配置、启动汇编、固件烧录文件以及电路原理图与说明文档核心工程为Mini_balance_car整体目录清晰按功能模块划分便于定位和对照调试。已有1746人学习下载。读者可获取可读的完整源码、硬件接线与算法注释内容覆盖从传感器读取、控制量输出到电机驱动验证的完整链路对自控理论学习具有很好的实践补充价值。1. 平衡小车car资源包一个倒立摆控制的完整副本平衡小车在课程设计和DIY圈子里从来不缺资料但多数是残缺品——缺库、缺工程文件、缺注释能直接跑起来的很少。这份压缩包值得拆是因为它把“能直立”的完整链路一次性装齐了MPU6050姿态解算、双环PID控制、电机驱动还有关键参数的预设值。它解决的是从“看得懂原理”到“车能立起来”的最后一公里适合刚做完基础实验、想在真车上验证控制算法的从业者也能帮你在PID参数上少走至少一星期的弯路。2. 姿态解算不是黑匣子MPU6050数据流与Kalman滤波实现2.1 为什么选Kalman滤波而不是互补滤波这份代码里姿态解算用的是Kalman滤波而不是更常见的互补滤波。原因很直接互补滤波在高频抖动场景下车体容易发飘角度输出滞后明显而Kalman滤波能实时估计陀螺仪零偏角度曲线更平滑直立环的P参数可以给得更大而不担心抖振。互补滤波的公式是 angle 0.98 * (angle gyro*dt) 0.02 * acc_angle只有一个权重系数好调但上限低。Kalman滤波引入了两个过程噪声参数 Q_angle、Q_bias 和一个量测噪声 R_measure调参维度更多但换来的是姿态数据在动态下的“跟手度”。对于平衡车这种需要快速响应的系统Kalman滤波的优先级更高。实际使用中如果硬件是STM32F103这类主频不高的芯片一维Kalman滤波每周期只做几次浮点乘加完全跑得动这也是它能被放进这个工程文件的原因。2.2 姿态数据流与代码骨架整体数据流是这样走的MPU6050通过I2C输出原始加速度和角速度加速度计用atan2换算成角度陀螺仪除以量程系数换算成°/s然后送进Kalman滤波融合最后得到单一的角度值供直立环使用。代码里比较核心的是下面这段。// kalman.c 核心更新函数角度单位统一为“度” float kalman_update(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] R_measure; float K0 k-P[0][0] / S; // 角度增益 float K1 k-P[1][0] / S; // 零偏增益 // 量测更新用加速度计角度修正预测值 float y acc_angle - k-angle; k-angle K0 * y; k-bias K1 * y; // 后验协方差更新 float P00 k-P[0][0] - K0 * k-P[0][0]; float P01 k-P[0][1] - K0 * k-P[0][1]; k-P[0][0] P00; k-P[0][1] P01; k-P[1][0] - K1 * k-P[0][0]; k-P[1][1] - K1 * k-P[0][1]; return k-angle; }这是一维Kalman滤波的标准实现状态量是角度和陀螺仪零偏。Q_angle、Q_bias是过程噪声协方差R_measure是加速度计量测噪声。简单理解Q越小越信任陀螺仪积分R越大越不信任加速度计。这套数值直接表现成姿态角曲线的平滑度和滞后程度也间接决定直立环P参数能不能给大。调参时先固定R不动只动Q观察角度曲线在高频抖动下的表现。MPU6050的读取部分同样关键寄存器地址和字节顺序写错会导致角度直接跳变。// MPU6050读取从0x3B寄存器开始连续读14字节跳过温度寄存器 void mpu_read_acc_gyro(short *ax, short *ay, short *az, short *gx, short *gy, short *gz) { uint8_t buf[14]; // 0x3B是ACCEL_XOUT_H连续读取保证加速度和角速度来自同一时刻 i2c_read_regs(MPU6050_ADDR, 0x3B, buf, 14); *ax (buf[0] 8) | buf[1]; // 大端字节序高位在前 *ay (buf[2] 8) | buf[3]; *az (buf[4] 8) | buf[5]; *gx (buf[8] 8) | buf[9]; *gy (buf[10] 8) | buf[11]; *gz (buf[12] 8) | buf[13]; }这里最容易犯的错是读错寄存器地址导致角度和角速度不是同一时刻采样融合出来就会出现“角度滞后半拍”的怪问题。这个代码里用连续读14字节的方式规避了两次单独读之间的时间差。2.3 采样时间dt与零偏校准Kalman滤波里的dt决定了积分的精度代码里用的是实际测量时间戳差分而不是固定值。原因是主循环里I2C读取时间受总线干扰会波动固定dt会导致积分漂移。void control_isr(void) { uint32_t now micros(); float dt (now - last_us) / 1000000.0f; // 单位换算成秒 last_us now; short ax, ay, az, gx, gy, gz; mpu_read_acc_gyro(ax, ay, az, gx, gy, gz); // 加速度计换算角度atan2(ay, az)比atan(ax/az)更稳避免除零跳变 float acc_angle atan2f((float)ay, (float)az) * 57.2958f; // 陀螺仪原始值转°/s±2000dps量程的灵敏度为16.4 LSB/(°/s) float gyro_rate (float)gy / 16.4f; float angle kalman_update(kf, gyro_rate, acc_angle, dt); }这里的acc_angle用的是atan2(ay, az)而不是atan(ax/az)因为后者在车体侧倾接近90度时会除零。虽然平衡车控制范围通常只有±30度但大角度扰动时能多做一重保护总是好的。dt的期望值是5ms对应200Hz的姿态更新频率。低于100Hz直立会明显发软车体像没吃饭一样高于500Hz对MCU负担大且多出来的更新对直立效果帮助有限。零偏校准放在上电初始化里静止采样200次求均值。陀螺仪零偏是Kalman滤波里bias变量的初始值如果直接给0需要上百次控制周期才能收敛这期间车体已经倒了。// 上电校准静止状态下采集200次角速度均值作为bias初值 float bias_init 0; for (int i 0; i 200; i) { short ax, ay, az, gx, gy, gz; mpu_read_acc_gyro(ax, ay, az, gx, gy, gz); bias_init (float)gy / 16.4f; delay(1); } bias_init / 200.0f; kf.bias bias_init;这个校准过程必须在车体完全静止时进行用手扶着或者放在水平桌面上都行。有个细节校准时长不要少于150ms否则角速度均值还没稳定bias初值本身就带偏差。这包代码里已经把校准函数放在初始化流程里了只需要确认宏定义CALIBRATE_GYRO被打开。3. 双环PID直立环、速度环、转向环的参数设计与联动3.1 控制结构速度环为什么叠加在直立环外面平衡车的控制结构不是三个独立PID各管各的而是串级叠加关系。直立环是最内层直接输出PWM让车体保持垂直速度环在直立环外面它的输出不是直接给电机而是修正直立环的输出基准。转向环在最外层只在左右轮之间做差分。直立环本身是个不稳定的倒立摆控制纯P控制就能让车体回正但会有持续振荡所以必须加D项。D项用陀螺仪角速度而不是角度差分是因为角速度本身就是角度的微分直接用陀螺仪原始值既省计算又没差分放大噪声的问题。关于速度环常见有两种接法一种是速度环输出叠加到PWM上另一种是速度环输出改变直立环的目标角度。这份代码用的是第一种好处是物理意义清晰、实现简单缺点是对直立环限幅要特别注意否则速度环输出过大时会把直立环的PWM占掉车体立刻倒。第二种接法更平滑但调试复杂度高。对这个资源的目标场景叠加方式是合理的。3.2 直立环PD实现// 直立环纯PD控制器输出限幅在满量程的60% float stand_pid(float angle, float gyro_rate) { float p_term KP_STAND * angle; float d_term KD_STAND * gyro_rate; // 角速度本身就是角度的微分 float out p_term d_term; // 限幅是关键给速度环和转向环留出输出空间 if (out MOTOR_LIMIT_UP) out MOTOR_LIMIT_UP; if (out -MOTOR_LIMIT_UP) out -MOTOR_LIMIT_UP; return out; }直立环P参数太大车体会高频抖振D参数太大电机会持续啸叫。注意这里的限幅值MOTOR_LIMIT_UP如果设到100%速度环和转向环就没有修正空间了车体会缓慢滑走。常见的做法是设成PWM满量程的60%这样叠加输出后还有足够的动态范围。陀螺仪角速度做D项有个典型的坑如果角速度单位或者方向反了直立环会从“阻尼”变成“助振”。检查方法很简单用手快速转动车体看串口输出的gyro_rate符号和转动方向是否对应。3.3 速度环PI与转向环PD// 速度环PI控制器目标是让车体静止在当前位置 float speed_pid(float target_speed, float current_speed) { float err target_speed - current_speed; static float integral 0; integral err * SPEED_KI; // 积分限幅防止长时间积分饱和导致回不来 if (integral SPEED_INT_LIMIT) integral SPEED_INT_LIMIT; if (integral -SPEED_INT_LIMIT) integral -SPEED_INT_LIMIT; float out err * SPEED_KP integral; return out; }速度环的积分限幅比输出限幅更重要。如果积分不限幅车体被外力推倒后重新扶正的过程中积分项可能已经积到非常大输出直接顶到限幅值直立环再努力也拉不回来。SPEED_INT_LIMIT的经验值建议不超过速度环输出限幅的一半。速度环极性反了的后果很直观一松手车就往一个方向加速冲越冲越倒根本不往回矫正。这是因为速度环误差的反向被当成正向处理了。验证方法用手扶着车体往左推如果串口显示速度值变大且PWM往反方向修正说明极性正确如果PWM反而推着车继续往左走把编码器两根线交换或者对速度取反。转向环就简单多了纯P就能工作。目标转向角度为0左右轮PWM差值与当前偏航角成正比。// 转向环纯P控制只做左右差分 float turn_pid(float target_angle, float current_angle) { float err target_angle - current_angle; return err * KP_TURN; // KD默认0够用 }// 三环合流输出 float stand_out stand_pid(g_angle, g_gyro_rate); float speed_out speed_pid(0, g_speed); // 静止目标速度是0 float turn_out turn_pid(0, g_yaw); float left_pwm stand_out speed_out - turn_out; float right_pwm stand_out speed_out turn_out;合流时转向项的符号决定了转向方向是否正确左右轮换一下就知道了。这个工程里直立环和速度环的输出极性如果都是按“正向直立、反向倾倒”约定转向环的符号与前面两者相反也很自然。3.4 参数预设值与调参顺序这套代码里预置的参数值是能直接上车的前提是硬件配置相近带编码器直流减速电机、6V或7.4V供电、PWM满量程7200左右。参数预设范围作用说明KP_STAND300~600直立环比例过小发软过大高频抖KD_STAND1~3直立环微分抑制振荡过大会啸叫SPEED_KP0.5~1速度环比例修正漂移速度SPEED_KI0.02~0.05速度环积分消除静态误差KP_TURN20~60转向环比例影响转向灵敏度MOTOR_LIMIT_UP60%满量程直立环限幅必须低于PWM满量程调参顺序从来都是从内往外先把速度环和转向环全部禁用只留直立环手扶着车体让它能“硬挺几秒”然后再叠加速度环最后加转向环。倒着调会找不到问题源头全是玄学。这一步如果跳过后面出问题基本都得返工。4. 从代码到能直立移植、接线、参数定位与第一次上电4.1 先看工程结构别急着改代码拿到压缩包解压后第一件事不是打开main.c而是先看目录结构。这类工程通常分三层硬件驱动层放I2C、定时器、PWM、编码器接口文件算法层放Kalman滤波和PID控制应用层放主循环和中断调度。确认了文件归属改一个引脚或者换一个MCU时才知道动哪里。# 定位关键参数的最快方式是全局搜索别翻文件 grep -rn MOTOR_LIMIT_UP\|KP_STAND\|SPEED_KP . --include*.c --include*.h全局搜索能一次性把所有控制参数集中列出来。这类工程里参数定义通常集中在control.h或者config.h里如果没集中工程写得太散老手也不会去逐个文件翻。4.2 硬件接线对照表把代码里的引脚宏和实物接线对应上是第一次上电前最不能省的一步。下面这份接线表是这类平衡车最常见的一套配置与工程文件的宏定义基本吻合。模块信号引脚对应MCU引脚备注MPU6050SDAPB7I2C1数据线需上拉电阻MPU6050SCLPB6I2C1时钟线左电机编码器A相/B相PA0 / PA1TIM2通道1/2右电机编码器A相/B相PA6 / PA7TIM3通道1/2左电机PWMIN1/IN2PA8 / PA9带死区互补输出右电机PWMIN3/IN4PA10 / PA11带死区互补输出串口调试TX/RXPA2 / PA39600bps或115200bps引脚接错是最常见的“代码没问题但车就是不转”的原因。尤其编码器A相B相接反速度环极性直接反掉表现就是松手后加速倒车。这块不排查清楚就调PID等于白忙。4.3 第一次上电三步法第一次上电不要直接松手更不要站在车旁边祈祷它自己站起来。按照三步法来第一步手扶车体保持垂直打开串口监视器看角度输出。正常情况下角度应在0度附近波动波动幅度不超过正负1度。如果角度跳变超过5度检查MPU6050的I2C接线和量程配置。第二步禁用速度环和转向环只保留直立环。用手扶着车体感受电机是否在持续对抗你的手——有一个明显的“往正中间推”的力就是对的。此时车体会小幅振荡这是正常现象叫极限环振荡。第三步逐步松手。每次松手时间从1秒开始递增如果直立时间超过5秒再打开速度环。速度环打开后车体可能会缓慢漂移这是积分项还没起作用属正常现象。// 调试输出10Hz打印别用100Hz否则串口会拖慢主循环 if (timer_10hz_flag) { printf(angle%.2f pwm%d speed%d\r\n, g_angle, pwm_out, g_speed); }4.4 串口输出频率对控制周期的影响很多新手在这个阶段会踩一个隐藏坑printf打印频率太高控制中断被串口阻塞拖慢导致dt被算错姿态解算跟着漂。常见做法是把调试输出频率控制在10Hz左右控制中断保持200Hz不变。这个代码里调试打印放主循环控制代码放在定时器中断里两者互不干扰。如果你拿到手的版本把打印放在中断里优先把它挪出去。5. 避坑与常见问题排查五条血泪踩坑记录5.1 上电后高频抖动声音像蜜蜂嗡嗡响现象车体在原位高频振动电机发出尖锐而细密的连续声响肉眼能看到车体在发抖幅度不大但频率极高。原因两种可能叠加。一是KP_STAND设置过大直立环比例增益过高微小角度误差被放大成大幅PWM修正二是姿态角本身滞后Kalman滤波的Q设置不当导致角度输出相位延迟直立环的D项没有起到阻尼作用。解决先把KP_STAND降一半确认抖动消失后再逐步加回来。如果降了KP还抖检查dt是否为固定值。常见做法是改成用时间戳差分固定dt在I2C被NACK阻塞时会引入相位误差。5.2 一松手就往一个方向加速倒下现象车体不是原地软倒而是加速往某个方向冲越冲越快直到趴下。原因速度环极性反了或者编码器A/B相接反。速度环的输出方向与误差方向相反才算正确接反的话误差越大修正越猛适得其反。解决先交换编码器A/B相最简单有效的验证方式。交换后重新上电推一下车体如果PWM输出方向变成“抵抗推力”说明极性已正常。如果交换后现象变成诱导方向也反过来那就是速度环符号本身需要取反。5.3 静止时角度缓慢漂移车体慢慢倾斜现象把车体放在完全水平的桌面上串口角度输出从0缓慢涨到5度再继续涨车体也跟着倾斜。原因陀螺仪零偏没有校准或者校准失效。Kalman滤波的bias初始值如果是0实际零偏不为0姿态角就会以恒定速率漂移。温度变化也会导致零偏漂移夏天空调房和室外跑出来的零偏能差不少。解决确保上电校准函数被执行校准期间车体绝对静止。如果车体放在桌面上仍持续漂移检查校准均值有没有被正确赋值到kf.bias而不是只算了均值没赋进去。5.4 车能立住但始终往一个方向偏转现象双轮都在转车体整体绕着垂直轴缓慢转向偏向方向固定。原因左右电机减速比不一致或者电池电量不足导致左右PWM实际输出电压有差异。另一种可能是转向环的零偏误差没处理偏航角长期有偏差。解决先检查机械装配用手转动左右轮对比阻力。如果机械没问题给转向环目标值引入一个小偏置把固定偏转抵消掉。正常情况下这个偏置值很小如果你需要加到很大才能抵消说明机械或电机存在硬伤。5.5 断电再上电后参数复位直立效果变差现象刚调好的参数立得很稳掉电重启后车体又变得发软或者抖动参数仿佛被重置。原因参数只存在RAM里没有写进Flash持久化。调试时每次修改肯定有效但重启后就丢。这个包里有Flash模拟EEPROM的读写函数很多人没启用。解决把三个PID参数定义成结构体一次性写入Flash上电时读取。我在类似项目里的习惯是加一个参数版本号读出来时校验版本号版本不匹配就用默认参数防止结构体定义改了之后读取了脏数据。注意5.4和5.5这两类问题排查时千万别先动PID参数。机械问题和存储问题是硬故障改参数只是临时掩盖症状本质问题还在。6. 进阶从“能站住”到“能跑起来”的三个调优动作车体能稳定直立之后顺滑的行走控制是下一步。这里分享三个调优动作都是在现有代码上小改就能见效的。第一个动作是给速度环输出加斜坡限幅。直接限幅会让车体启动时猛然前倾给人“蹿出去”的感觉。把速度环输出本身限幅改为按时间斜率限制每次控制周期最多变化一个固定的delta加速度就变得柔和可控。// 速度环输出斜坡限幅每周期最多变化SLOPE_LIMIT float speed_out_limited; if (speed_out last_speed_out SLOPE_LIMIT) speed_out_limited last_speed_out SLOPE_LIMIT; else if (speed_out last_speed_out - SLOPE_LIMIT) speed_out_limited last_speed_out - SLOPE_LIMIT; else speed_out_limited speed_out; last_speed_out speed_out_limited;SLOPE_LIMIT的经验值是满量程PWM的2%左右7200满量程对应144。太小则响应迟钝太大则失去了斜坡限幅的意义。第二个动作是转向环加一点D项。纯P转向在高速行走时会轻微振荡加D项能抑制这种航向摆动代价是转向会稍微变“硬”。KD_TURN建议从1开始加如果转向时车体出现明显侧倾振荡先减小转向环P而不是调D。第三个动作是参数一键保存。把整组PID参数打包成结构体写入Flash并加一个参数版本号。修改参数后按一下按键或者发一条串口指令就保存上电时自动读取并校验。这个习惯帮我省掉了大量重复调试时间。从那以后我每次拿到这类平衡车代码都会先花十分钟把直立环单独跑通、确认姿态角曲线不漂移不到0.5度再合速度环最后加转向环。这个顺序让我少拆了两辆车也让每一次调试都只面对一个变量。希望帮到你。本文还有配套的精品资源点击获取