STM32三路超声波跟随小车：从测距到测方位的完整实现与避坑指南

简介面向嵌入式学习者这是一份STM32三路超声波自动跟随小车完整工程资源围绕自动跟随场景提供基于ARM Cortex-M内核微控制器的完整控制程序涵盖多路超声波测距、三向目标方位判断与电机驱动控制三个超声波传感器分别布置于车前方不同位置通过距离差感知目标位置并调整左右电机转速实现较稳定的动态跟随。压缩包共309个文件约8.45MB以.c/.h源文件与硬件配置头文件为核心同时包含uvproj/eww工程文件、hex/axf烧录固件、bat脚本及编译中间文件可导入Keil或IAR直接查看与重新编译。已有2403人学习/下载项目覆盖超声波触发与回波捕获、GPIO中断配置、主循环距离计算、三路传感器布局与跟随算法框架源码结构清晰适合电子竞赛备赛、智能车实践及嵌入式入门。借助自带工程配置和调试会话文件可较快完成固件烧录与实车调参并继续优化算法或移植至其他STM32型号。1. 这是“测距”还是“测方位”三路超声波小车的真正门槛很多人在做跟随小车的第一版时都只在正前方装一颗超声波。结果小车只能跟直线——目标一转弯它就分不清“人往左拐了”还是“人后退了”要么直直撞上去要么原地打转。STM32三路超声波自动跟随小车这个名字看起来只是比单路多了两个探头实际上方案的核心是把跟随从“测距”升级成“测方位”左、前、右三路回波一起读才能判断目标在左边还是右边、该往哪转。这套方案难度不高一颗 STM32F103C8T6 最小系统板加三颗 HC-SR04 就能跑起来适合刚学完串口和 GPIO、想做一个能动、会转向的完整小车的阶段。下面按我实际搭这套车的顺序讲原理、接线、源码和踩过的坑。2. 三路超声波的跟随原理为什么单路会跟丢2.1 HC-SR04 的测距模型和单路方案的先天不足先明确一点HC-SR04 测距靠的是“给 TRIG 引脚一个 10us 以上的高电平模块发出 8 个 40kHz 超声波脉冲然后 ECHO 引脚输出一段高电平高电平持续时间就是声波往返时间”。所以距离公式是固定的距离(cm) ECHO 高电平时间(us) × 0.034 / 2 ≈ 时间(us) × 0.017这个模型在单路方案下有两个致命问题。第一它只能告诉你“前方有东西距离多远”但无法告诉你“这个东西在往哪边移动”。第二超声波的波束角大约是 15 度一颗传感器测到的是前方一个圆锥形区域内的最近反射点当目标从正前方偏到侧面时只要圆锥还能扫到目标边缘读数就还在变化小车无法从距离变化里区分“目标左移”和“目标后退”。所以单路小车最常见的表现是目标静止时它能慢慢靠近并停下目标一旦侧移它会先加速冲向目标原来站的位置然后因为失去反射信号而急停。这在室内跟人的场景里几乎不可用。要解决必须从“测一个点”变成“测一个面”也就是三路布局。2.2 左、前、右三路布局与方位判定逻辑三路传感器我一般装在车头同一高度间距 5~8cm左探头朝左前方约 30 度、中探头正前方、右探头朝右前方约 30 度。这样做的目的不是追求“三个距离都精确”而是让三个距离之间出现可判断的差异。具体判定逻辑按优先级可以这样写先看中路距离如果中路小于停车阈值不管左右怎么变先停车避免撞上如果中路在跟随区间内再看左路减右路的差值差值大于死区说明目标偏左小于负死区说明目标偏右在死区之内说明目标基本正对就直行。如果中路距离超过丢失阈值进入搜索状态。这套逻辑用很少的代码就能实现而且比直接拿三路距离做 PID 更稳。判断条件判断结果小车动作中路 20cm目标太近停车中路 20~60cm左路-右路 8cm目标偏左左转差速中路 20~60cm左路-右路 -8cm目标偏右右转差速中路 20~60cm差值和在 ±8cm 内目标正对直行中路 80cm目标丢失减速原地搜索这里有个容易被忽略的点差值必须做多帧确认。超声波单次读数有 ±2cm 的抖动如果只取一帧就判断方向目标明明站在正前方差值也可能偶尔超过死区小车就会左右乱摆。我一般连续取 3 帧方向判断结果一致才执行转向动作。2.3 跟随距离分区为什么我不建议直接上连续 PID很多做驱动的同学拿到距离数据第一反应就是用 PID让速度随距离连续变化。但超声波的噪声特性让连续 PID 很容易翻车HC-SR04 在室内墙面反射、目标衣服吸音、地面多径反射的条件下单帧读数抖动经常超过 ±2cm把这个抖动直接送进 PIDPWM 会跟着抖小车走起来一窜一窜的。所以我更常用的方式是“分区 缓启动”把距离分成停车区、跟随区、丢失区三个区间在每个区间里给固定 PWM区间切换时用斜坡函数让 PWM 渐变而不是突变。这样小车在跟随区里匀速走目标突然停住时能够快速进入停车区不会因为 P 项过大而撞上去。等整机跑稳了再在左右转向差速上叠加 PID而不是在直行速度上叠加。跟随距离分区参数表参数默认值说明STOP_DIST20cm小于该值立即停车避开 HC-SR04 近距离盲区HOLD_DIST60cm超过该值认为目标脱离安全跟随范围LOST_DIST80cm超过该值进入搜索状态原地旋转扫描TURN_DEADZONE8cm左右路差值死区小于该值不转向FOLLOW_SPEED60/100直行 PWM占空比给 60 左右留出调节余量2.4 三路同时工作的隐藏成本供电与触发时序三路不是简单地把三颗传感器并联到 5V。单颗 HC-SR04 工作电流约 15mA三颗就是 45mA而且每次触发瞬间电流会跳变。如果直接用 STM32 板载 LDO 的 5V 给三路供电测距时 5V 会被拉低回波幅度下降读数开始飘。更麻烦的是三颗传感器如果同时触发声波会互相串扰中路发出的超声波打在墙上反射回来可能被左路探头当成自己的回波接收然后算出一个完全离谱的距离。所以三路方案在时序上必须分时触发不能一起测。我一般每轮只触发一路等这一路 ECHO 完全结束后再触发下一路三路之间间隔至少 5ms。供电方面5V 母线上加 100uF 以上电解电容或者干脆用一个独立的 5V 降压模块给传感器供电这是让三路读数稳定的第一步。3. STM32 主控选型与接线把三路传感器焊上车头之前3.1 主控和传感器的选型组合跟随小车的主控最常见的是 STM32F103C8T6 最小系统板48 脚Flash 64KBRAM 20KB跑这套逻辑绰绰有余。传感器就用 HC-SR04不是因为性能好而是因为足够普及、坏了换新成本低。三路 HC-SR04 加上一块 TB6612FNG 驱动、两个直流减速电机、一个 2S 锂电就是很标准的配置。这里要提醒一点HC-SR04 的 ECHO 引脚输出的是 5V 高电平而 F103 的 GPIO 工作电压是 3.3V。直接接到非 5V 容忍引脚上轻则读数异常重则烧引脚。选引脚时先确认该引脚带 FT5V 容忍标注或者用 1k 电阻串联加 2k 电阻对地分压把 5V 分到约 3.3V。分压方案虽然多两个电阻但省心。3.2 引脚分配表与接线顺序我习惯的引脚分配如下三路 TRIG 和 ECHO 各占一个 GPIO电机控制占两路 PWM 和四个方向引脚信号引脚说明TRIG_L / ECHO_LPA0 / PA1左路触发与回波TRIG_F / ECHO_FPA2 / PA3中路触发与回波TRIG_R / ECHO_RPA4 / PA5右路触发与回波左电机 PWMPA6接驱动板 AIN1/PWM 通道右电机 PWMPA7接驱动板 BIN1/PWM 通道方向引脚PB0 PB1 PB10 PB11接驱动板 IN1 IN2 IN3 IN4接线顺序我一般按“先逻辑地、后功率地、再信号线”来。先把 STM32 的 GND、电机驱动的 GND、电池负极全部连在一起再给驱动板上电最后接超声波的 TRIG 和 ECHO。如果先把信号线全接好再上功率电驱动板浪涌电流容易把 3.3V 逻辑电平拉乱甚至让 MCU 复位。3.3 驱动与供电TB6612FNG 和双电源共地驱动芯片我推荐 TB6612FNG对比 L298N 它的优势是导通压降小、发热低L298N 在 12V 供电、电流稍大时烫得不敢摸。TB6612 的 VM 接电池正极VCC 接 3.3V 或 5V 逻辑电源STBY 引脚必须拉高否则电机不转。如果手上只有 L298N接线思路一样但注意别用 L298N 板载 5V 给主控供电——那个 5V 是线性稳压电机一转电压就往下掉MCU 跟着复位是经典翻车现场。供电结构我一般是这样电池正极 → TB6612 VM电机电源 电池正极 → 5V 降压模块 → STM32 5V 引脚 三路 HC-SR04 VCC 电池负极 → TB6612 GND、降压模块 GND、STM32 GND 全部相连这里最关键的是“共地不共电源”逻辑电源和功率电源共地但 5V 不要从电机驱动板上取除非你确认那个 5V 的输出电流足够且纹波可控。三颗 HC-SR04 同时触发瞬间需要大约 60mA 左右的脉冲电流5V 降压模块选输出 1A 以上比较稳妥。3.4 上电前的静态检查清单每次组装完新板子我习惯先做一遍静态检查再上电。第一万用表量 5V 和 GND 之间有没有短路这个最致命短路一上电就烧。第二量三个 HC-SR04 的 VCC 和 GND 两端电压确认都在 4.8~5.2V 之间。第三检查 ECHO 引脚是否都接到了 5V 容忍引脚或分压电阻上这一步能用放大镜看电路连线确认。第四电机和传感器分开上电测试先传感器后电机如果插上电机电源后传感器读数异常优先怀疑共地问题而不是传感器坏了。4. 测距与跟随状态机三路分时采样和差速转向的源码思路4.1 用 DWT 实现微秒级延时和超时计时HC-SR04 的触发脉冲要求 10us 以上而 HAL_Delay 最小粒度是 1ms没法直接用。我一般用 Cortex-M3 内核的 DWT 计数器做微秒延时顺便用它给 ECHO 回波做超时判断避免程序堵死在等待回波的循环里。#include core_cm3.h static uint8_t dwt_ready 0; void delay_us(uint32_t us) { if (!dwt_ready) { CoreDebug-DEMCR | CoreDebug_DEMCR_TRCENA_Msk; DWT-CYCCNT 0; DWT-CTRL | DWT_CTRL_CYCCNTENA_Msk; dwt_ready 1; } uint32_t start DWT-CYCCNT; uint32_t ticks us * (SystemCoreClock / 1000000); while ((DWT-CYCCNT - start) ticks); }这段代码在工程里只需要出现一次。DWT 计数器是内核自带的不占用定时器资源也不会跟 PWM 输出抢外设。SystemCoreClock 在 F103 上通常是 72MHz所以 ticks 算出来是 us 数乘以 72也就是 1us 对应 72 个计数。需要注意 DWT-CYCCNT 是 32 位计数器在 72MHz 下大约 59 秒回绕一次对于单次测距几十毫秒的窗口完全够用不需要处理回绕。4.2 单路测距函数轮询 ECHO 并带超时保护单路测距的代码我封装成函数传入 TRIG 和 ECHO 的端口及引脚号返回距离值失败返回 -1。这样三路共用一套逻辑后面想扩展到第五路也是复制调用而已。int32_t measure_cm(GPIO_TypeDef *trig_port, uint16_t trig_pin, GPIO_TypeDef *echo_port, uint16_t echo_pin) { uint32_t t_start, t_end, dt_us; HAL_GPIO_WritePin(trig_port, trig_pin, GPIO_PIN_RESET); delay_us(2); HAL_GPIO_WritePin(trig_port, trig_pin, GPIO_PIN_SET); delay_us(12); // 规范要求 10us 以上留 2us 余量 HAL_GPIO_WritePin(trig_port, trig_pin, GPIO_PIN_RESET); t_start DWT-CYCCNT; while (HAL_GPIO_ReadPin(echo_port, echo_pin) GPIO_PIN_RESET) { if ((DWT-CYCCNT - t_start) 3000000) return -1; // 约 41ms 超时 } t_start DWT-CYCCNT; while (HAL_GPIO_ReadPin(echo_port, echo_pin) GPIO_PIN_SET) { if ((DWT-CYCCNT - t_start) 3000000) return -1; // 高电平超时保护 } t_end DWT-CYCCNT; dt_us (t_end - t_start) / (SystemCoreClock / 1000000); return dt_us * 17 / 1000; // cm us * 0.034 / 2 ≈ us * 17 / 1000 }超时时间 3000000 个计数约等于 41ms对应测量距离约 7 米室内跟随场景足够。返回 -1 后上层要处理这个非法值不能直接拿去做差速判断。距离换算用整数运算避免浮点dt_us 乘以 17 再除以 1000等价于乘以 0.017误差在 1mm 量级对跟随场景完全够用。浮点运算是可以用的但在多处调用时整数运算更省内核时间。4.3 三路分时采样中值滤波对抗单帧毛刺三路采样不能在同一时刻触发否则会串扰。我每一轮按左、中、右的顺序每路之间间隔 2ms每路内部连续测 3 次取中值。中值滤波比均值滤波更适合超声波因为噪声是离群值——偶尔出现一个 1cm 或者 200cm 的读数均值会被拉偏中值直接忽略它。#define FILTER_N 3 int32_t buf[FILTER_N]; int32_t median_filter(GPIO_TypeDef *trig_port, uint16_t trig_pin, GPIO_TypeDef *echo_port, uint16_t echo_pin) { for (int i 0; i FILTER_N; i) { buf[i] measure_cm(trig_port, trig_pin, echo_port, echo_pin); delay_us(2000); // 每路间隔 2ms降低同频串扰 if (buf[i] 0) buf[i] 999; // 超时值放大让中值逻辑自动丢弃 } for (int i 1; i FILTER_N; i) { int32_t key buf[i]; int j i - 1; while (j 0 buf[j] key) { buf[j 1] buf[j]; j--; } buf[j 1] key; } return buf[FILTER_N / 2]; }注意这里把超时值改成 999而不是直接丢弃。因为一次测距超时可能是目标暂时丢失但也可能是传感器被遮挡把 999 放进滤波队列参与排序中值结果会变成一个很大值上层逻辑自然把它归入“丢失”分区比单独写异常分支更省事。中值排序用的插入排序数组只有 3 个元素开销可以忽略。4.4 跟随状态机停车、直行、差速转向、搜索有了三路距离剩下就是状态机。我定义的跟随状态机只有四个状态停车、直行、左转、右转丢失时原地旋转搜索。转向不用原地打转而是用差速——内侧轮降速、外侧轮保持这样小车走的是弧线跟人更自然。typedef enum { STOP_STATE, FORWARD_STATE, TURN_LEFT_STATE, TURN_RIGHT_STATE, SEARCH_STATE } follow_state_t; #define STOP_DIST 20 #define HOLD_DIST 60 #define LOST_DIST 80 #define TURN_DEADZONE 8 #define FORWARD_PWM 60 #define TURN_INNER_PWM 35 #define TURN_OUTER_PWM 85 void set_motor(int left_pwm, int right_pwm) { // 内部实现取决于驱动板左 PWM 接 PA6右 PWM 接 PA7 // 方向引脚由正负号决定这里只做占空比输出 } void follow_loop(void) { int32_t dist_l median_filter(TRIG_L_PORT, TRIG_L_PIN, ECHO_L_PORT, ECHO_L_PIN); int32_t dist_f median_filter(TRIG_F_PORT, TRIG_F_PIN, ECHO_F_PORT, ECHO_F_PIN); int32_t dist_r median_filter(TRIG_R_PORT, TRIG_R_PIN, ECHO_R_PORT, ECHO_R_PIN); int32_t diff dist_l - dist_r; if (dist_f STOP_DIST) { set_motor(0, 0); // 太近先停 } else if (dist_f LOST_DIST) { set_motor(-TURN_INNER_PWM, TURN_OUTER_PWM); // 原地旋转搜索 } else if (dist_f HOLD_DIST diff TURN_DEADZONE) { set_motor(TURN_INNER_PWM, TURN_OUTER_PWM); // 目标偏左左弧线 } else if (dist_f HOLD_DIST diff -TURN_DEADZONE) { set_motor(TURN_OUTER_PWM, TURN_INNER_PWM); // 目标偏右右弧线 } else { set_motor(FORWARD_PWM, FORWARD_PWM); // 正对直行 } }这里几个参数值得说清楚。FORWARD_PWM 用 60 而不是 100是因为直行时左右轮如果有转速差小车会跑偏留 40 的余量给转向补偿。转向时内侧轮 35、外侧轮 85差速 50 足够让小车在室内地砖上画出明显弧线如果转向太猛把内侧 PWM 提到 50 减小差速。STOP_DIST 设 20cm 还有一个原因是 HC-SR04 有 2cm 盲区近距离读数本来就不可靠与其让状态机在 2cm~5cm 区间反复横跳不如直接停死。5. 三路跟随小车最常见的 5 个翻车点和排查清单5.1 三路回波互相串扰距离读数忽远忽近现象只触发中路左路的读数会跟着跳有时候三路同时显示相同的距离值明显是听到了同一个回波。 原因传感器间距太近或者触发间隔太短。中路发出的声波经墙面反射后被左路探头接收左路 ECHO 误判为自己发出的波的回波。 解决三轮触发之间至少间隔 2ms我实际用的是 5ms测试时把三路传感器之间用薄海绵隔开减少空气传导串扰。还有一个技巧是每路连续测 3 次时3 次内部之间间隔拉长到 5ms不要连续触发。5.2 电机一转MCU 就复位或者超声波读数瞬间乱跳现象小车静止时三路距离都正常一踩油门LCD 花屏、串口乱码严重时开发板直接重启。 原因电机启动瞬间电流可达 1A 以上如果逻辑电源和功率电源没分开5V 被瞬间拉低到 3V 以下单片机欠压复位。共地没做好还会让 ECHO 信号参考点漂移回波电平判断错乱。 解决功率级和逻辑级分开供电电池正极先到 TB6612 VM再单独用降压模块给 STM32 和传感器供电GND 全部连在一起。电池容量也要够2S 锂电至少 1000mAh电压瞬间跌落时会好很多。5.3 目标站得很近小车反而直接撞上去现象人站在车前 10cm 左右小车应该停车但它却往前顶或者反复前进后退。 原因HC-SR04 的近距离盲区在 2cm 左右5cm 以内读数开始不稳定。三路布局下目标太近时超声波打在人体上反射角度变大回波幅度变弱读数可能突然跳到 100cm 以上状态机误判为目标丢失于是加速向前找目标。 解决STOP_DIST 不要设太激进20cm 是底线。另外在代码里做一次保护如果上一帧读数小于 15cm而当前帧突然变成大于 50cm先把当前帧标记为无效用上一帧结果输出防止单帧跳变导致小车撞人。5.4 目标转弯时小车左右乱摆像喝醉了现象人站在正前方不动小车自己左右扭人一直往前走小车走 S 形。 原因超声波读数抖动大单帧差值超过了 TURN_DEADZONE状态机在直行和转向之间频繁切换。也有可能是 TURN_DEADZONE 设太小5cm 的差值在噪声面前没有意义。 解决先看滤波确认用的是中值滤波而不是均值再把 TURN_DEADZONE 从 5cm 提到 8cm给噪声留出余量。还不行就在状态机里做“多帧确认”连续 3 帧都是同一个转向判断才执行转向其中一帧不一致就维持当前状态。5.5 ECHO 引脚电平不匹配传感器换一个烧一个现象传感器单独接 Arduino 没问题接到 STM32 上测几次就读数异常换个传感器又能用然后再坏。 原因HC-SR04 的 ECHO 输出 5V 高电平STM32 引脚不是所有都 5V 容忍非容忍引脚被 5V 灌入后 IO 保护二极管导通长期会损坏引脚。 解决选引脚时在芯片数据手册里查 FT 标注或者干脆用电阻分压ECHO 串 1k 电阻后接 STM32 引脚引脚对地接 2k 电阻5V 分压后约 3.3V。这个方案通用且便宜比电平转换芯片省事唯一的代价是回波上升沿变缓但在 40kHz 超声信号下完全不影响测距精度。6. 进阶从三路固定探头到舵机扫描与 PID 平滑跟随三路固定探头能解决“目标在左边还是右边”的问题但它有个物理边界左右探头的夹角是固定的目标一旦跑到小车 45 度角以外左右路都会失去反射中路距离又没变状态机就只能直行直到目标彻底跟丢。想突破这个边界常见做法是把三路固定探头换成“单探头 舵机扫描”舵机每 50ms 转到左、中、右三个角度各测一次相当于用时间换空间。舵机扫描的代码改造量不大把 GPIO 触发改成舵机 PWM 转角然后把角度对应的距离存数组。但要注意舵机转动需要稳定时间转角后至少等 20ms 再触发测距否则读数会带上上一角度的残影。我实际测试过单探头扫描在三路同样的 50ms 周期下单角度测距周期会拉长到 150ms 左右跟随响应变慢所以扫描模式更适合慢速跟随不适合追着跑。如果想让跟随更顺滑可以在转向差速上叠加 PID。先保持分区框架不变把“目标偏左”的输出从固定差速改成“基础差速 PID 计算量”输入是左路减右路的差值。实际调参时先只调 P从 8 开始试太小转向跟不上太大小车点头然后加一点 D取值 0.5~1 用来抑制差值突变I 项在跟随场景一般不用因为超声波噪声会直接积分放大。调参时务必拿串口把左路、右路、差值、PWM 四个值全部打印出来对着曲线调比盲调快得多。验证方法我一般做三件事第一是静态标定把目标物放在 30cm、60cm、100cm 三个位置看串口读数和卷尺读数偏差超过 3cm 检查传感器高度和朝向第二是绕桩测试在地面摆两个间隔 80cm 的桩让小车跟随人绕 S 形记录它是否撞桩或丢目标第三是连续运行 20 分钟重点听电机驱动芯片温度以及观察 STM32 有没有意外复位。最后一件事的教训来自我自己的血泪经验有次驱动板发热把旁边 5V 降压模块的焊锡都烤软了小车跑着跑着突然断电复位查了两天才发现是散热问题。现在我的习惯是每次给新板子上电之前先拿起万用表量一遍 5V 和 GND再摸一遍驱动板温度——这两个动作救回来的板子比什么玄学优化都多。希望帮到你。本文还有配套的精品资源点击获取