简介基于STM32的双轮平衡自动循迹小车项目为嵌入式方向在校生、电子设计竞赛选手及STM32入门者提供了一套完整可运行的工程源码与文档。项目集成了姿态传感器数据融合、双电机差速控制、红外循迹检测等核心算法代码结构清晰注释明确可直接在Keil中打开编译并烧录至硬件平台。压缩包内共278个文件以C语言源代码、头文件、Keil工程文件为主配合hex可执行文件、编译映射文件及备份配置便于查看构建过程和实现细节整体体积仅9.75MB轻量易获取。目前已有264人学习下载。该工程经过功能验证稳定可靠既适合作为毕业设计、课程设计或竞赛初期立项的参照方案也方便读者依据文档说明逐步调试硬件与算法进而在源码基础上扩展蓝牙遥控、视觉巡线等更高阶功能。1. 双轮平衡循迹小车从立项到跑通这是一条完整的STM32技术链路一个两轮自平衡小车本质上是“倒立摆”模型在嵌入式平台上的实时控制实现系统必须在毫秒级周期内完成姿态解算、平衡控制和运动决策。当“平衡”与“循迹”叠加在同一辆车上时真正的难点不是某个单一功能而是中断优先级、控制周期、传感器带宽之间的协同调度。我会按工程落地的顺序——硬件选型、姿态解算、PID整定、循迹策略、代码组织——把这条链路讲清楚。适合准备做课程设计或毕业设计的在校生也适合想快速搭建一个带源码参考的STM32项目的在职工程师。2. 平衡控制的数学基础与硬件选型2.1 倒立摆模型为什么两轮车能立住两轮平衡车在力学上等价于一个一阶倒立摆重心位于轮轴上方重力会使车体倾倒而车轮加速度能产生抵消倾倒趋势的惯性力。控制目标是让车体倾角始终收敛在零度附近。离散化后常用状态变量为倾角 $\theta$ 和倾角角速度 $\dot{\theta}$控制量为电机输出 PWM 占空比。工程上不直接求解状态空间方程而是用两个级联的 PID 环来做近似控制这也是目前绝大多数 STM32 平衡车项目的标准做法。外环是直立环输入为目标倾角与实际倾角的偏差输出为期望角速度内环是速度环输入为期望角速度与实际角速度的偏差输出为电机 PWM。2.2 STM32 型号选型与传感器搭配主控我推荐用 STM32F103C8T6 或 STM32F405RGT6。F103 价格低、资料全51 元的板子就能跑完整套逻辑F405 主频高适合后续加视觉循迹或者无线图传。两者的工程代码结构几乎一致CubeMX 里切换芯片型号后重新生成初始化代码即可。传感器选型在平衡车上决定了控制上限姿态传感器MPU6050带 DMP 输出四元数省去自己写互补滤波的数学推导。注意读取频率要稳定在 1kHz 左右I2C 时钟设为 400kHz用硬件 I2C 而非软件模拟。电机带霍尔编码器的 N20 减速电机减速比 1:30编码器分辨率 11 线配合 4 倍频后每圈 44 个脉冲。直接用带 AB 相的编码器便于测速和测方向。驱动TB6612FNG比 L298N 轻导通压降小PWM 频率能推到 20kHz。2.3 电源架构一个容易被忽视的瓶颈电机启动瞬间电流可达 1.5A而 STM32 和 MPU6050 的工作电流加起来不到 100mA。如果共用一组电源MPU6050 的 VCC 会被拉到低于 3.3V姿态数据瞬间飘掉车就会倒。正确做法是7.4V 锂电池直接进 TB6612 的 VM再经 LM2596 降压到 5V 给 STM32 板载稳压器或直接给编码器供电MPU6050 用 STM32 板上的 3.3V LDO。地线要单点接地电机电源地和数字地在驱动板附近汇合避免数字信号被电机换向噪声耦合。// 电源状态检测ADC 采样电池电压低于阈值时减小最大 PWM 限幅 #define BATTERY_LOW_THRESHOLD_MV 6800 uint16_t get_battery_mv(void) { ADC_HandleTypeDef hadc; // 假设使用 ADC1_IN0参考电压 3.3V分压比 1/3 HAL_ADC_Start(hadc); HAL_ADC_PollForConversion(hadc, 100); uint16_t raw HAL_ADC_GetValue(hadc); return (uint32_t)raw * 3300 * 3 / 4095; }这段代码的作用是让控制线程能感知电源状态。电池电压下降时电机能输出的最大驱动力也在下降如果仍然输出同样大小的 PID 控制量车会在低电量时震荡加剧。具体做法是将电压低于 6.8V 时的 PWM 限幅从 95% 降到 80%保持系统稳态裕度。3. 姿态解算与 PID 控制核心代码3.1 MPU6050 数据读取与卡尔曼/互补滤波MPU6050 输出的加速度计和陀螺仪数据单独都不能直接用加速度计高频噪声大陀螺仪存在温漂积分导致的角度漂移。需要把两者融合。开源社区最常见的融合方案是 Mahony 互补滤波代码量小在 F103 平台上只需约 300 个周期。// Mahony 互补滤波核心更新四元数 void mahony_update(float gx, float gy, float gz, float ax, float ay, float az, float halfT) { float norm; float vx, vy, vz; float ex, ey, ez; norm sqrtf(ax*ax ay*ay az*az); ax / norm; ay / norm; az / norm; // 从四元数计算重力方向估计 vx 2.0f * (q1*q3 - q0*q2); vy 2.0f * (q0*q1 q2*q3); vz q0*q0 - q1*q1 - q2*q2 q3*q3; // 叉积误差 ex ay * vz - az * vy; ey az * vx - ax * vz; ez ax * vy - ay * vx; // PI 补偿陀螺仪积分漂移 exInt ex * Ki * halfT; eyInt ey * Ki * halfT; ezInt ez * Ki * halfT; gx ex * Kp exInt; gy ey * Kp eyInt; gz ez * Kp ezInt; // 一阶龙格库塔更新四元数 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); // 归一化 norm sqrtf(q0*q0 q1*q1 q2*q2 q3*q3); q0 / norm; q1 / norm; q2 / norm; q3 / norm; // 转换成欧拉角pitch 即为车体倾角 pitch asinf(-2.0f * (q1*q3 - q0*q2)) * 57.29578f; }参数halfT是采样周期的一半由定时器中断周期决定若控制频率为 1kHz则halfT 0.0005f。Kp和Ki分别取 0.5 和 0.05 作为初始值。注意Ki不能给得太大否则陀螺仪零漂会被放大导致实际静止时角度慢漂。每次上电后要让小车静置两秒完成陀螺仪零偏校准。3.2 串级 PID直立环与速度环的耦合单环 PID 直接控制 PWM 会导致两个问题车没有前进动力时会持续倾斜去够目标倾角下坡时车会一直加速。标准解法是直立环外、速度环内速度环输出作为直立环的目标值修正量而不是直接叠加 PWM。// 控制周期 2ms 的定时器中断内 void balance_control_task(void) { // 速度环目标速度为外部指令速度实际速度为编码器测速 float speed_err target_speed - current_speed; speed_i speed_err * 0.02f; // 积分分离速度误差大时清掉积分 if (fabsf(speed_err) 100.0f) speed_i 0.0f; speed_i fmaxf(fminf(speed_i, 3000.0f), -3000.0f); float speed_out speed_err * 1.2f speed_i * 0.3f; // 直立环目标倾角 0 速度环输出角度修正 float target_angle 0.0f speed_out * 0.002f; float angle_err target_angle - pitch; float gyro_rate get_gyro_y(); // 注意陀螺仪轴向与倾角方向一致 float balance_out angle_err * 28.0f gyro_rate * 0.5f; // 死区补偿防止 PWM 小信号时电机不转 if (balance_out 0) balance_out 12.0f; if (balance_out 0) balance_out - 12.0f; // 限幅后输出至 TB6612 balance_out fmaxf(fminf(balance_out, 7200.0f), -7200.0f); set_motor_pwm(balance_out); }速度环积分项和时间步长的匹配是这里最容易出错的地方。控制周期是 2msspeed_i speed_err * 0.02f中的0.02f就是经过换算的K_i * dt。如果改了中断频率这个系数必须同步改。死区补偿值 12 对应约 3% 的占空比具体数值取决于电机的启动电流需要通过实测调出过大反而会在零位附近来回撞击。3.3 PID 参数整定顺序与方法参数整定顺序不能乱否则车永远立不稳。第一步整直立环 PD 给零P 从小到大加直到手扶车体时感受到明显抵抗但还没开始高频抖动第二步加 D抑制回正时的超调效果是倾斜后手能感觉到阻尼第三步调速度环先给 P观察车是否朝一个方向缓慢漂移再加 I 消除稳态误差。整定过程强烈建议用蓝牙串口实时查看pitch和目标角度曲线。常见的现象是P 过大时高频振荡P 过小时车倾斜后无返回力矩D 过大时手推车体感觉像在推弹簧回正慢且有过冲。现象原因调参方向低频晃动约 1Hz速度环 P 过小或 I 过大增大速度环 P减小 I高频抖动约 10Hz直立环 P 过大或采样周期不稳减小直立环 P检查中断优先级静止时缓慢倾斜角度零偏未校准重新校准陀螺仪零偏或减小 Ki推一下持续加速速度环输出饱和增大速度环积分限幅或检查速度方向4. 循迹算法与灰度传感器信号处理4.1 传感器布局与采样策略循迹传感器我用的是 4 路灰度模块如 TCRT5000左右各两路间距根据赛道黑线宽度调整常见黑线宽 2cm传感器中心距 1.5cm 比较合适。4 路数字量的输出组合能覆盖“直线、左偏、右偏、左急弯、右急弯、脱线”六种状态。如果预算允许8 路模拟输出可以提供更平滑的位置信息但 4 路数字量配合位置权重偏差算法已经足够稳定。// 4 路数字量灰度传感器映射为偏差值0 表示对中 // 传感器从左到右顺序为 S1 S2 S3 S4 int8_t get_line_error(void) { uint8_t s1 HAL_GPIO_ReadPin(S1_GPIO_Port, S1_Pin); uint8_t s2 HAL_GPIO_ReadPin(S2_GPIO_Port, S2_Pin); uint8_t s3 HAL_GPIO_ReadPin(S3_GPIO_Port, S3_Pin); uint8_t s4 HAL_GPIO_ReadPin(S4_GPIO_Port, S4_Pin); // 整体偏右 - 偏差为负整体偏左 - 偏差为正 if (s2 s3) return 0; // 中间两路压线 if (s1 s2) return 2; // 左偏偏差中等 if (s3 s4) return -2; // 右偏 if (s1) return 3; // 左急弯偏差大 if (s4) return -3; // 右急弯 if (s1 s4) return 0; // 脱线但两侧都有信号保持当前方向 if (!(s1 || s2 || s3 || s4)) { // 全无信号沿用上一周期偏差防止脱线时突然转向 return last_error; } last_error ...; // 更新为当前计算出的偏差 return ...; }这里的关键是脱线处理当没有传感器压到黑线时直接返回零偏差会让车走直线结果是车笔直冲出赛道。正确逻辑是在脱线时保持上一次的偏差方向让车沿着最后已知的曲线方向继续转弯直到重新捕获黑线。这个逻辑配合“全白且两侧都有信号”的场景能覆盖绝大多数 90 度弯和 T 字路口。4.2 循迹转向 PID 与速度耦合循迹转向环的输出是两轮速度差叠加到平衡车的目标速度上。注意耦合关系转向输出过大时重心偏移会导致平衡环产生相反的纠正动作车会有明显“甩头”现象。解决方式是将转向 PID 输出限幅在目标速度的 40% 以内并且在直线段降低基础速度、弯道段通过偏差值动态降速。// 循迹 PID 计算输出左右轮速度差单位 mm/s float track_pid(float error, float dt) { static float integral 0.0f; float derivative (error - last_error) / dt; // 积分分离偏差大于 2.5 时停止积分防止弯道积分饱和 if (fabsf(error) 2.5f) { integral error * dt; } else { integral 0.0f; } integral fmaxf(fminf(integral, 50.0f), -50.0f); float kp 0.8f, ki 0.05f, kd 0.15f; return error * kp integral * ki derivative * kd; }dt用循迹任务执行周期建议与速度环一致或者更慢。Kp 初始值拿不准时可以先让车走直线人为用遥控器给出一个固定偏差观察转向速度是否平滑。循迹 PID 的输出是速度差最终左右轮目标速度分别为base_speed speed_diff和base_speed - speed_diff再分别送入速度环。4.3 一个容易踩的坑灰度阈值与环境光TCRT5000 是反射式红外传感器检测阈值受环境光和赛道颜色影响很大。不要用固定阈值硬比较要在 main 函数启动时做一次自动校准连续采样 50 次求平均取白底平均值的 60% 作为阈值线。另外传感器距离地面的高度必须锁死在 5~10mm误差超过 2mm 会让阈值失去意义。// 动态阈值校准上电时执行 uint16_t calibrate_threshold(ADC_HandleTypeDef *adc) { uint32_t sum 0; for (int i 0; i 50; i) { HAL_ADC_Start(adc); HAL_ADC_PollForConversion(adc, 10); sum HAL_ADC_GetValue(adc); HAL_Delay(10); } uint16_t avg sum / 50; // 返回白底阈值的 60% 作为区分黑白的阈值 return (uint16_t)(avg * 0.6f); }用 ADC 采集模块的模拟输出而不是直接读数字引脚是更稳的做法。因为模拟量能提供中间态信息比如传感器压线一半时电压会处于中间值软件可以从这里算出更精确的横向偏差。5. STM32CubeMX 工程配置与代码架构组织5.1 定时器与中断优先级分配平衡控制对中断优先级的要求很严格。我这里给出一个经验上稳定的配置顺序优先级数值越小越优先外设功能中断源抢占优先级子优先级频率/周期编码器测速TIM2/TIM3 更新201kHz平衡主控制TIM4 更新10500Hz蓝牙串口USART230115200bps循迹决策TIM5 更新40100Hz平衡控制中断应设置为最高抢占优先级数字最小即使循迹或串口发生阻塞也能保证车的直立不被打断。编码器中断频率如果与平衡控制频率相同必须保证编码器先更新、平衡环后读取否则读到的速度是上一时刻的旧值会让速度环滞后一拍。在 CubeMX 中如果使用 F103 系列注意它有 4 个定时器可用于编码器模式TIM1/2/3/4其中 TIM1 是高级定时器配置时选择 Encoder Mode 后硬件会自动处理方向和计数。计数范围设为最大0xFFFF读取后自行转换为有符号数避免溢出临界点无法判断方向的问题。5.2 源码工程结构规划一个具备可维护性的平衡车工程不能把所有代码堆在 main.c 里。推荐按功能模块拆分为以下结构这种组织方式也能让你的源代码文档显得更专业Car_Balance/ ├─ Core/ │ ├─ Inc/ (main.h, tim.h, adc.h) │ └─ Src/ (main.c, tim.c, adc.c) ├─ Drivers/ │ ├─ BSP/ (mpu6050.c/h, tb6612.c/h, encoder.c/h) │ ├─ Algorithm/ (mahony.c/h, pid.c/h, kalman.c/h) │ └─ Controller/ (balance.c/h, track.c/h) ├─ App/ │ ├─ task.c/h (任务调度) │ └─ debug.c/h (串口打印与参数在线修改) └─ Doc/ ├─ hardware_schematic.pdf └─ wiring_guide.md关键原则是硬件驱动层BSP只做寄存器读写不包含控制逻辑算法层保持纯函数输入输出方便在 PC 端用 Python 复现验证控制器层调用算法层处理中断上下文中的时间戳和临界区保护。Doc目录里放一个接线表标注每根信号线对应的 STM32 引脚这比在代码里写注释更直观。5.3 参数在线调试不用改代码就能调 PID每次调参都重新编译烧录三轮下来就烦了。常见做法是用串口作为参数通道在中断中解析收到的指令帧。协议简单定义为$CMD,VALUE\r\n例如$P,28.5\r\n表示将直立环 Kp 改为 28.5。// 串口命令解析需在接收中断或 DMA 空闲中断中调用 void debug_parse_command(char *buf) { if (buf[0] ! $) return; float val atof(buf 2); switch (buf[1]) { case P: balance_kp val; break; case D: balance_kd val; break; case I: speed_ki val; break; case V: target_speed (int16_t)val; break; default: break; } }配合 PC 端的串口助手可以做到车在运行中直接调整参数观察车体响应。注意修改参数时要加滤波或斜坡防止参数跳变让车瞬间失控例如new_kp 0.2 * target_kp 0.8 * old_kp做一个一阶低通。6. 代码阅读指南与三个容易忽视的细节6.1 源码阅读顺序从 main 到中断三层链路拿到别人的平衡车源代码不要从 main 第一行往后读。正确的阅读顺序是先看中断向量表或定时器回调函数确认控制周期和控制链路入口再找传感器数据读取函数确认数据类型和数据更新频率最后回到 main 中的外设初始化顺序确认传感器上电时序和 GPIO 复用配置。具体到你的工程第一行应该读tim.c里的HAL_TIM_PeriodElapsedCallback找到balance_control_task的调用位置。这个函数每 2ms 执行一次内部流程是“读陀螺仪 - Mahony 更新 - 速度环 - 直立环 - PWM 输出”。理解了这条链整体代码就已经在你的脑内跑起来了。6.2 编码器方向与电机极性的匹配关系编码器方向反了的典型症状是车一上电就全速往一个方向冲。这是因为速度环反馈为正反馈理论上增益大于 1 就会发散。排查方法是手动转动左轮前进方向串口打印编码器计数值确认是增加再给电机一个正向 PWM确认轮子是向前转。如果方向相反优先在encoder.c的读取函数里交换 AB 相的符号而不是改电机接线。因为 TB6612 输出端接电机交换任意两条线也可以反转极性但更容易造成逻辑混乱在代码中统一处理更清晰。6.3 串口打印性能开销printf 重定向必须用 DMA默认的printf通过fputc轮询发送在 115200 波特率下一个字符需要约 87us打印 20 个字符就是 1.7ms正好吃掉一个控制周期。如果把printf放进控制循环里车立刻开始抖动。解决方案是重定向到 DMA 发送或使用ITM调试端口。最简单的做法是在main.c中实现HAL_UART_Transmit_DMA的格式化版本// 使用 vsnprintf DMA 的轻量日志不阻塞控制中断 void debug_log(const char *fmt, ...) { static char buf[128]; va_list args; va_start(args, fmt); int len vsnprintf(buf, sizeof(buf), fmt, args); va_end(args); // 非阻塞发送如果上次发送未完成则丢弃本次日志 if (huart2.gState HAL_UART_STATE_READY) { HAL_UART_Transmit_DMA(huart2, (uint8_t *)buf, len); } }这里的“丢弃日志”是刻意的。控制周期内日志不能阻塞丢了这帧下帧还能补上比让控制中断等待更重要。如果串口 DMA 不可用另一个方案是用ITM_SendChar通过 SWO 引脚输出到调试器完全零开销但需要配合 ARM 调试器使用。本文还有配套的精品资源点击获取
STM32双轮平衡循迹小车：从PID控制到姿态解算的完整技术链路
简介基于STM32的双轮平衡自动循迹小车项目为嵌入式方向在校生、电子设计竞赛选手及STM32入门者提供了一套完整可运行的工程源码与文档。项目集成了姿态传感器数据融合、双电机差速控制、红外循迹检测等核心算法代码结构清晰注释明确可直接在Keil中打开编译并烧录至硬件平台。压缩包内共278个文件以C语言源代码、头文件、Keil工程文件为主配合hex可执行文件、编译映射文件及备份配置便于查看构建过程和实现细节整体体积仅9.75MB轻量易获取。目前已有264人学习下载。该工程经过功能验证稳定可靠既适合作为毕业设计、课程设计或竞赛初期立项的参照方案也方便读者依据文档说明逐步调试硬件与算法进而在源码基础上扩展蓝牙遥控、视觉巡线等更高阶功能。1. 双轮平衡循迹小车从立项到跑通这是一条完整的STM32技术链路一个两轮自平衡小车本质上是“倒立摆”模型在嵌入式平台上的实时控制实现系统必须在毫秒级周期内完成姿态解算、平衡控制和运动决策。当“平衡”与“循迹”叠加在同一辆车上时真正的难点不是某个单一功能而是中断优先级、控制周期、传感器带宽之间的协同调度。我会按工程落地的顺序——硬件选型、姿态解算、PID整定、循迹策略、代码组织——把这条链路讲清楚。适合准备做课程设计或毕业设计的在校生也适合想快速搭建一个带源码参考的STM32项目的在职工程师。2. 平衡控制的数学基础与硬件选型2.1 倒立摆模型为什么两轮车能立住两轮平衡车在力学上等价于一个一阶倒立摆重心位于轮轴上方重力会使车体倾倒而车轮加速度能产生抵消倾倒趋势的惯性力。控制目标是让车体倾角始终收敛在零度附近。离散化后常用状态变量为倾角 $\theta$ 和倾角角速度 $\dot{\theta}$控制量为电机输出 PWM 占空比。工程上不直接求解状态空间方程而是用两个级联的 PID 环来做近似控制这也是目前绝大多数 STM32 平衡车项目的标准做法。外环是直立环输入为目标倾角与实际倾角的偏差输出为期望角速度内环是速度环输入为期望角速度与实际角速度的偏差输出为电机 PWM。2.2 STM32 型号选型与传感器搭配主控我推荐用 STM32F103C8T6 或 STM32F405RGT6。F103 价格低、资料全51 元的板子就能跑完整套逻辑F405 主频高适合后续加视觉循迹或者无线图传。两者的工程代码结构几乎一致CubeMX 里切换芯片型号后重新生成初始化代码即可。传感器选型在平衡车上决定了控制上限姿态传感器MPU6050带 DMP 输出四元数省去自己写互补滤波的数学推导。注意读取频率要稳定在 1kHz 左右I2C 时钟设为 400kHz用硬件 I2C 而非软件模拟。电机带霍尔编码器的 N20 减速电机减速比 1:30编码器分辨率 11 线配合 4 倍频后每圈 44 个脉冲。直接用带 AB 相的编码器便于测速和测方向。驱动TB6612FNG比 L298N 轻导通压降小PWM 频率能推到 20kHz。2.3 电源架构一个容易被忽视的瓶颈电机启动瞬间电流可达 1.5A而 STM32 和 MPU6050 的工作电流加起来不到 100mA。如果共用一组电源MPU6050 的 VCC 会被拉到低于 3.3V姿态数据瞬间飘掉车就会倒。正确做法是7.4V 锂电池直接进 TB6612 的 VM再经 LM2596 降压到 5V 给 STM32 板载稳压器或直接给编码器供电MPU6050 用 STM32 板上的 3.3V LDO。地线要单点接地电机电源地和数字地在驱动板附近汇合避免数字信号被电机换向噪声耦合。// 电源状态检测ADC 采样电池电压低于阈值时减小最大 PWM 限幅 #define BATTERY_LOW_THRESHOLD_MV 6800 uint16_t get_battery_mv(void) { ADC_HandleTypeDef hadc; // 假设使用 ADC1_IN0参考电压 3.3V分压比 1/3 HAL_ADC_Start(hadc); HAL_ADC_PollForConversion(hadc, 100); uint16_t raw HAL_ADC_GetValue(hadc); return (uint32_t)raw * 3300 * 3 / 4095; }这段代码的作用是让控制线程能感知电源状态。电池电压下降时电机能输出的最大驱动力也在下降如果仍然输出同样大小的 PID 控制量车会在低电量时震荡加剧。具体做法是将电压低于 6.8V 时的 PWM 限幅从 95% 降到 80%保持系统稳态裕度。3. 姿态解算与 PID 控制核心代码3.1 MPU6050 数据读取与卡尔曼/互补滤波MPU6050 输出的加速度计和陀螺仪数据单独都不能直接用加速度计高频噪声大陀螺仪存在温漂积分导致的角度漂移。需要把两者融合。开源社区最常见的融合方案是 Mahony 互补滤波代码量小在 F103 平台上只需约 300 个周期。// Mahony 互补滤波核心更新四元数 void mahony_update(float gx, float gy, float gz, float ax, float ay, float az, float halfT) { float norm; float vx, vy, vz; float ex, ey, ez; norm sqrtf(ax*ax ay*ay az*az); ax / norm; ay / norm; az / norm; // 从四元数计算重力方向估计 vx 2.0f * (q1*q3 - q0*q2); vy 2.0f * (q0*q1 q2*q3); vz q0*q0 - q1*q1 - q2*q2 q3*q3; // 叉积误差 ex ay * vz - az * vy; ey az * vx - ax * vz; ez ax * vy - ay * vx; // PI 补偿陀螺仪积分漂移 exInt ex * Ki * halfT; eyInt ey * Ki * halfT; ezInt ez * Ki * halfT; gx ex * Kp exInt; gy ey * Kp eyInt; gz ez * Kp ezInt; // 一阶龙格库塔更新四元数 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); // 归一化 norm sqrtf(q0*q0 q1*q1 q2*q2 q3*q3); q0 / norm; q1 / norm; q2 / norm; q3 / norm; // 转换成欧拉角pitch 即为车体倾角 pitch asinf(-2.0f * (q1*q3 - q0*q2)) * 57.29578f; }参数halfT是采样周期的一半由定时器中断周期决定若控制频率为 1kHz则halfT 0.0005f。Kp和Ki分别取 0.5 和 0.05 作为初始值。注意Ki不能给得太大否则陀螺仪零漂会被放大导致实际静止时角度慢漂。每次上电后要让小车静置两秒完成陀螺仪零偏校准。3.2 串级 PID直立环与速度环的耦合单环 PID 直接控制 PWM 会导致两个问题车没有前进动力时会持续倾斜去够目标倾角下坡时车会一直加速。标准解法是直立环外、速度环内速度环输出作为直立环的目标值修正量而不是直接叠加 PWM。// 控制周期 2ms 的定时器中断内 void balance_control_task(void) { // 速度环目标速度为外部指令速度实际速度为编码器测速 float speed_err target_speed - current_speed; speed_i speed_err * 0.02f; // 积分分离速度误差大时清掉积分 if (fabsf(speed_err) 100.0f) speed_i 0.0f; speed_i fmaxf(fminf(speed_i, 3000.0f), -3000.0f); float speed_out speed_err * 1.2f speed_i * 0.3f; // 直立环目标倾角 0 速度环输出角度修正 float target_angle 0.0f speed_out * 0.002f; float angle_err target_angle - pitch; float gyro_rate get_gyro_y(); // 注意陀螺仪轴向与倾角方向一致 float balance_out angle_err * 28.0f gyro_rate * 0.5f; // 死区补偿防止 PWM 小信号时电机不转 if (balance_out 0) balance_out 12.0f; if (balance_out 0) balance_out - 12.0f; // 限幅后输出至 TB6612 balance_out fmaxf(fminf(balance_out, 7200.0f), -7200.0f); set_motor_pwm(balance_out); }速度环积分项和时间步长的匹配是这里最容易出错的地方。控制周期是 2msspeed_i speed_err * 0.02f中的0.02f就是经过换算的K_i * dt。如果改了中断频率这个系数必须同步改。死区补偿值 12 对应约 3% 的占空比具体数值取决于电机的启动电流需要通过实测调出过大反而会在零位附近来回撞击。3.3 PID 参数整定顺序与方法参数整定顺序不能乱否则车永远立不稳。第一步整直立环 PD 给零P 从小到大加直到手扶车体时感受到明显抵抗但还没开始高频抖动第二步加 D抑制回正时的超调效果是倾斜后手能感觉到阻尼第三步调速度环先给 P观察车是否朝一个方向缓慢漂移再加 I 消除稳态误差。整定过程强烈建议用蓝牙串口实时查看pitch和目标角度曲线。常见的现象是P 过大时高频振荡P 过小时车倾斜后无返回力矩D 过大时手推车体感觉像在推弹簧回正慢且有过冲。现象原因调参方向低频晃动约 1Hz速度环 P 过小或 I 过大增大速度环 P减小 I高频抖动约 10Hz直立环 P 过大或采样周期不稳减小直立环 P检查中断优先级静止时缓慢倾斜角度零偏未校准重新校准陀螺仪零偏或减小 Ki推一下持续加速速度环输出饱和增大速度环积分限幅或检查速度方向4. 循迹算法与灰度传感器信号处理4.1 传感器布局与采样策略循迹传感器我用的是 4 路灰度模块如 TCRT5000左右各两路间距根据赛道黑线宽度调整常见黑线宽 2cm传感器中心距 1.5cm 比较合适。4 路数字量的输出组合能覆盖“直线、左偏、右偏、左急弯、右急弯、脱线”六种状态。如果预算允许8 路模拟输出可以提供更平滑的位置信息但 4 路数字量配合位置权重偏差算法已经足够稳定。// 4 路数字量灰度传感器映射为偏差值0 表示对中 // 传感器从左到右顺序为 S1 S2 S3 S4 int8_t get_line_error(void) { uint8_t s1 HAL_GPIO_ReadPin(S1_GPIO_Port, S1_Pin); uint8_t s2 HAL_GPIO_ReadPin(S2_GPIO_Port, S2_Pin); uint8_t s3 HAL_GPIO_ReadPin(S3_GPIO_Port, S3_Pin); uint8_t s4 HAL_GPIO_ReadPin(S4_GPIO_Port, S4_Pin); // 整体偏右 - 偏差为负整体偏左 - 偏差为正 if (s2 s3) return 0; // 中间两路压线 if (s1 s2) return 2; // 左偏偏差中等 if (s3 s4) return -2; // 右偏 if (s1) return 3; // 左急弯偏差大 if (s4) return -3; // 右急弯 if (s1 s4) return 0; // 脱线但两侧都有信号保持当前方向 if (!(s1 || s2 || s3 || s4)) { // 全无信号沿用上一周期偏差防止脱线时突然转向 return last_error; } last_error ...; // 更新为当前计算出的偏差 return ...; }这里的关键是脱线处理当没有传感器压到黑线时直接返回零偏差会让车走直线结果是车笔直冲出赛道。正确逻辑是在脱线时保持上一次的偏差方向让车沿着最后已知的曲线方向继续转弯直到重新捕获黑线。这个逻辑配合“全白且两侧都有信号”的场景能覆盖绝大多数 90 度弯和 T 字路口。4.2 循迹转向 PID 与速度耦合循迹转向环的输出是两轮速度差叠加到平衡车的目标速度上。注意耦合关系转向输出过大时重心偏移会导致平衡环产生相反的纠正动作车会有明显“甩头”现象。解决方式是将转向 PID 输出限幅在目标速度的 40% 以内并且在直线段降低基础速度、弯道段通过偏差值动态降速。// 循迹 PID 计算输出左右轮速度差单位 mm/s float track_pid(float error, float dt) { static float integral 0.0f; float derivative (error - last_error) / dt; // 积分分离偏差大于 2.5 时停止积分防止弯道积分饱和 if (fabsf(error) 2.5f) { integral error * dt; } else { integral 0.0f; } integral fmaxf(fminf(integral, 50.0f), -50.0f); float kp 0.8f, ki 0.05f, kd 0.15f; return error * kp integral * ki derivative * kd; }dt用循迹任务执行周期建议与速度环一致或者更慢。Kp 初始值拿不准时可以先让车走直线人为用遥控器给出一个固定偏差观察转向速度是否平滑。循迹 PID 的输出是速度差最终左右轮目标速度分别为base_speed speed_diff和base_speed - speed_diff再分别送入速度环。4.3 一个容易踩的坑灰度阈值与环境光TCRT5000 是反射式红外传感器检测阈值受环境光和赛道颜色影响很大。不要用固定阈值硬比较要在 main 函数启动时做一次自动校准连续采样 50 次求平均取白底平均值的 60% 作为阈值线。另外传感器距离地面的高度必须锁死在 5~10mm误差超过 2mm 会让阈值失去意义。// 动态阈值校准上电时执行 uint16_t calibrate_threshold(ADC_HandleTypeDef *adc) { uint32_t sum 0; for (int i 0; i 50; i) { HAL_ADC_Start(adc); HAL_ADC_PollForConversion(adc, 10); sum HAL_ADC_GetValue(adc); HAL_Delay(10); } uint16_t avg sum / 50; // 返回白底阈值的 60% 作为区分黑白的阈值 return (uint16_t)(avg * 0.6f); }用 ADC 采集模块的模拟输出而不是直接读数字引脚是更稳的做法。因为模拟量能提供中间态信息比如传感器压线一半时电压会处于中间值软件可以从这里算出更精确的横向偏差。5. STM32CubeMX 工程配置与代码架构组织5.1 定时器与中断优先级分配平衡控制对中断优先级的要求很严格。我这里给出一个经验上稳定的配置顺序优先级数值越小越优先外设功能中断源抢占优先级子优先级频率/周期编码器测速TIM2/TIM3 更新201kHz平衡主控制TIM4 更新10500Hz蓝牙串口USART230115200bps循迹决策TIM5 更新40100Hz平衡控制中断应设置为最高抢占优先级数字最小即使循迹或串口发生阻塞也能保证车的直立不被打断。编码器中断频率如果与平衡控制频率相同必须保证编码器先更新、平衡环后读取否则读到的速度是上一时刻的旧值会让速度环滞后一拍。在 CubeMX 中如果使用 F103 系列注意它有 4 个定时器可用于编码器模式TIM1/2/3/4其中 TIM1 是高级定时器配置时选择 Encoder Mode 后硬件会自动处理方向和计数。计数范围设为最大0xFFFF读取后自行转换为有符号数避免溢出临界点无法判断方向的问题。5.2 源码工程结构规划一个具备可维护性的平衡车工程不能把所有代码堆在 main.c 里。推荐按功能模块拆分为以下结构这种组织方式也能让你的源代码文档显得更专业Car_Balance/ ├─ Core/ │ ├─ Inc/ (main.h, tim.h, adc.h) │ └─ Src/ (main.c, tim.c, adc.c) ├─ Drivers/ │ ├─ BSP/ (mpu6050.c/h, tb6612.c/h, encoder.c/h) │ ├─ Algorithm/ (mahony.c/h, pid.c/h, kalman.c/h) │ └─ Controller/ (balance.c/h, track.c/h) ├─ App/ │ ├─ task.c/h (任务调度) │ └─ debug.c/h (串口打印与参数在线修改) └─ Doc/ ├─ hardware_schematic.pdf └─ wiring_guide.md关键原则是硬件驱动层BSP只做寄存器读写不包含控制逻辑算法层保持纯函数输入输出方便在 PC 端用 Python 复现验证控制器层调用算法层处理中断上下文中的时间戳和临界区保护。Doc目录里放一个接线表标注每根信号线对应的 STM32 引脚这比在代码里写注释更直观。5.3 参数在线调试不用改代码就能调 PID每次调参都重新编译烧录三轮下来就烦了。常见做法是用串口作为参数通道在中断中解析收到的指令帧。协议简单定义为$CMD,VALUE\r\n例如$P,28.5\r\n表示将直立环 Kp 改为 28.5。// 串口命令解析需在接收中断或 DMA 空闲中断中调用 void debug_parse_command(char *buf) { if (buf[0] ! $) return; float val atof(buf 2); switch (buf[1]) { case P: balance_kp val; break; case D: balance_kd val; break; case I: speed_ki val; break; case V: target_speed (int16_t)val; break; default: break; } }配合 PC 端的串口助手可以做到车在运行中直接调整参数观察车体响应。注意修改参数时要加滤波或斜坡防止参数跳变让车瞬间失控例如new_kp 0.2 * target_kp 0.8 * old_kp做一个一阶低通。6. 代码阅读指南与三个容易忽视的细节6.1 源码阅读顺序从 main 到中断三层链路拿到别人的平衡车源代码不要从 main 第一行往后读。正确的阅读顺序是先看中断向量表或定时器回调函数确认控制周期和控制链路入口再找传感器数据读取函数确认数据类型和数据更新频率最后回到 main 中的外设初始化顺序确认传感器上电时序和 GPIO 复用配置。具体到你的工程第一行应该读tim.c里的HAL_TIM_PeriodElapsedCallback找到balance_control_task的调用位置。这个函数每 2ms 执行一次内部流程是“读陀螺仪 - Mahony 更新 - 速度环 - 直立环 - PWM 输出”。理解了这条链整体代码就已经在你的脑内跑起来了。6.2 编码器方向与电机极性的匹配关系编码器方向反了的典型症状是车一上电就全速往一个方向冲。这是因为速度环反馈为正反馈理论上增益大于 1 就会发散。排查方法是手动转动左轮前进方向串口打印编码器计数值确认是增加再给电机一个正向 PWM确认轮子是向前转。如果方向相反优先在encoder.c的读取函数里交换 AB 相的符号而不是改电机接线。因为 TB6612 输出端接电机交换任意两条线也可以反转极性但更容易造成逻辑混乱在代码中统一处理更清晰。6.3 串口打印性能开销printf 重定向必须用 DMA默认的printf通过fputc轮询发送在 115200 波特率下一个字符需要约 87us打印 20 个字符就是 1.7ms正好吃掉一个控制周期。如果把printf放进控制循环里车立刻开始抖动。解决方案是重定向到 DMA 发送或使用ITM调试端口。最简单的做法是在main.c中实现HAL_UART_Transmit_DMA的格式化版本// 使用 vsnprintf DMA 的轻量日志不阻塞控制中断 void debug_log(const char *fmt, ...) { static char buf[128]; va_list args; va_start(args, fmt); int len vsnprintf(buf, sizeof(buf), fmt, args); va_end(args); // 非阻塞发送如果上次发送未完成则丢弃本次日志 if (huart2.gState HAL_UART_STATE_READY) { HAL_UART_Transmit_DMA(huart2, (uint8_t *)buf, len); } }这里的“丢弃日志”是刻意的。控制周期内日志不能阻塞丢了这帧下帧还能补上比让控制中断等待更重要。如果串口 DMA 不可用另一个方案是用ITM_SendChar通过 SWO 引脚输出到调试器完全零开销但需要配合 ARM 调试器使用。本文还有配套的精品资源点击获取
