零知派——STM32智能小车系列教程（二）：HC-SR04超声波测距 + SG90舵机云台原理与调试

篇次	内容
第一篇（已完成）	五路循迹模块（TCRT5000 + LM393/LM339）
第二篇（本文）	HC-SR04 超声波测距 + SG90 舵机云台三模式应用
第三篇	红外双目跟随模块（物体跟随功能）
第四篇	霍尔编码器测速与直线行驶 PID（含 PID 自整定）
第五篇	DHT11 温湿度、火焰传感器、风扇灭火模块
第六篇	蓝牙/串口双通道通信协议设计
终篇	整车项目架构总览，把前面六篇串起来讲整体设计思路

二、超声波测距模块硬件原理

HC-SR04 是一种基于声波反射时间差测距的传感器模块。左侧标有"T"的探头负责发出超声波脉冲，右侧标有"R"的探头负责接收反射回来的超声波

2.1 超声波发送+接收

模块背面集成了：发射驱动电路（提升信号幅度驱动压电探头）、接收放大电路（将微弱回波信号放大）、比较器电路（与阈值比较输出数字电平）以及定时逻辑（控制Trig触发和Echo脉冲宽度输出）

①HC-SR04 完整时序图

HC-SR04 的通信时序是本模块使用中最关键的部分，理解时序才能写出可靠的驱动代码

时序说明：

主控给 Trig 一个 ≥10μs 的高电平脉冲
模块自动发出 8 个 40kHz 的超声波脉冲
模块等待回波，Echo 引脚拉高
收到回波后 Echo 引脚拉低，高电平持续时间 = 超声波往返时间
两次测量之间间隔 ≥ 60ms，避免发射信号与回波信号相互干扰

②距离公式推导

常温（20°C）下声速：

Echo 高电平时间为 TT T 微秒，超声波走过的总路程（去+回）：

单程距离（障碍物到传感器的实际距离）：

这就是为什么代码里直接用 dur / 58 来计算距离，是对公式的工程化近似，误差小于0.5%，完全在传感器本身±3mm的精度范围内

③内部电路原理图

发射电路：主控芯片产生 40kHz 方波信号、通过三极管或 MOS 管驱动放大、驱动超声波发射探头（T）发出 40kHz 声波
接收电路：接收探头（R）将声波信号转换为微弱的电信号、经过多级放大电路放大、经过比较器整形为数字信号、送入主控芯片判断是否有回波

2.2 超时保护机制

当测距目标超出量程（> 400cm）或者探头朝向完全吸音的软质材料时，超声波不会反射回来，Echo引脚会一直保持低电平，程序如果没有超时保护会无限等待，阻塞整个 loop() 主循环

int SonarServo::getDistance()
{
    digitalWrite(UltraTrigPin, LOW);
    delayMicroseconds(2);
    digitalWrite(UltraTrigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(UltraTrigPin, LOW);

    const unsigned long TIMEOUT_US = 35000UL;  // 35ms超时 ≈ 6m量程
    unsigned long t0 = micros();
    // 等待Echo变高（超时则视为无效）
    while (!digitalRead(UltraEchoPin) && (micros() - t0) < TIMEOUT_US);
    if (digitalRead(UltraEchoPin) == LOW) return -1;  // 超时，返回无效值

    // 测量高电平持续时间
    t0 = micros();
    while (digitalRead(UltraEchoPin) && (micros() - t0) < TIMEOUT_US);
    unsigned long dur = micros() - t0;

    int cm = (int)(dur / 58);
    return (cm > 400) ? -1 : cm;  // 超出量程也返回-1
}

TIMEOUT_US = 35000 对应约 35ms，换算距离约 6m，超出HC-SR04实际量程，任何有效测量都会在超时之前完成，确保函数调用时间有上界

①测距范围与盲区

HC-SR04 标称的有效测距范围大约在 2cm 到 400cm 之间：

近距离盲区（约 2cm 以内）：发射和接收用的是同一类型的超声波换能器，发射脉冲结束后接收电路需要一定的恢复时间，太近的距离反射波会和发射的余振混在一起分辨不清楚
远距离限制（超过 400cm）：受限于超声波在空气中传播时的衰减，太远反射回来的信号强度不足以被稳定识别

②检测角度与盲点

HC-SR04 的超声波探头有一个发射锥角，通常在 15 度左右

它检测的不是一个点，而是一个扇形区域内的最近反射物体。如果赛道两侧或者前方有形状不规则、表面凹凸或者倾斜角度较大的障碍物，超声波反射方向会发生偏折，导致测距结果不准甚至完全测不到。这也是为什么单纯依赖超声波做避障时，常常需要配合舵机左右摆动多次测量取最近值

三、舵机云台控制原理

SG90 内部集成了直流电机、减速齿轮组和位置反馈电位器。与普通直流电机"给电就转、断电就停"不同，舵机的核心能力是角度伺服控制——给一个指令角度，舵机会自动转动到该角度并通过内部闭环保持在那里

3.1 PWM信号与角度

舵机依靠PWM信号的脉宽（而非占空比、也非频率）来确定目标角度。标准控制信号参数：

①PWM信号规范

SG90 舵机接收的 PWM 信号规范如下：

参数	值
PWM 频率	50Hz
PWM 周期	20ms
脉宽范围	0.5ms ~ 2.5ms

②脉宽与角度的对应关系

脉冲宽度	对应角度	说明
0.5 ms	0°	最小角度
1.5 ms	90°	中位（居中）
2.5 ms	180°	最大角度

③舵机内部闭环控制原理

SG90 舵机内部包含一个完整的闭环控制系统：

给定一个 PWM 脉宽，舵机就会转到对应的角度并保持住，直到收到新的脉宽指令。外力强行转动舵机输出轴时，舵机会主动抵抗并回到设定角度

3.2 舵机云台使用方式

项目里舵机不是用来驱动机械臂或者轮子转向，而是顶着超声波传感器做一个简单的云台：

舵机转到某个角度，超声波就朝那个方向测距；这种“测距前先转舵机、转到位再读数”的用法，本质上是用时间分割的方式实现了一个简易的扇形扫描雷

void SonarServo::sweepTo(int to, int stepMs)
{
    static int _current = SERVO_DEFAULT_POS;
    int step = (to > _current) ? 1 : -1;
    while (_current != to) {
        _current += step;
        _servo.write(_current);
        delay(stepMs);
    }
}

注意事项

舵机每转到一个新角度，都需要等待一定时间（约 100~200ms）让机械结构稳定下来
再触发超声波测距，否则转动过程中的震动会让测距结果出现明显抖动
项目 sweepTo() 函数中内置了步进延时

四、硬件接线说明

4.1 引脚定义

①HC-SR04 对外提供四个引脚

引脚	名称	方向	功能	本项目接法
1	VCC	输入	电源正极（5V）	接 5V
2	Trig	输入	触发信号输入	接 D31
3	Echo	输出	回波信号输出	接 D30
4	GND	—	电源地	接 GND

注意：Trig 和 Echo 方向相反——Trig 是主控发给模块的触发指令，Echo 是模块回传给主控的测距结果，不要接反

②SG90 引出三根线

线色	名称	功能	本项目接法
棕色/黑色	GND	电源地	接 GND
红色	VCC	电源正极（5V）	接 5V
橙色/黄色	SIG	PWM 控制信号	接 D15（ServoPWM1）

4.2 接线方案表

模块	引脚	零知派迷你板引脚	代码常量	说明
HC-SR04	VCC	5V	—	超声波模块需要 5V 供电
HC-SR04	GND	GND	—	共地
HC-SR04	Trig	D31	`UltraTrigPin`	触发信号，输出脉冲
HC-SR04	Echo	D30	`UltraEchoPin`	回波信号，读取高电平脉宽
SG90 舵机	红线 (VCC)	5V	—	舵机供电，建议独立供电
SG90 舵机	棕/黑线 (GND)	GND	—	共地
SG90 舵机	橙/黄线 (SIG)	D15	`ServoPWM1`	PWM 控制信号

请注意：超声波模块和舵机模块直插小车扩展板超声波引脚和舵机接口P1，超声波模块的VCC和GND方向请勿接反，否则导致模块烧毁

4.3 连接示意图

五、独立测试程序

下面这个程序只依赖超声波模块和舵机两路硬件，不需要电机、循迹等其他模块。引脚定义与主项目 pinsdefine.h 完全一致，可以直接在零知派迷你板上单独烧录测试

/**************************************************************************************
 * 文件: Sonar_Servo_Standalone_Test.ino
 * 作者：零知实验室（深圳市在芯间科技有限公司）
 * 功能：HC-SR04超声波测距 + SG90舵机云台 独立测试程序
 *       引脚定义与主项目 pinsdefine.h 完全一致，不修改任何引脚
 *       单独烧录，通过串口监视器实时观察角度-距离对应关系
 **************************************************************************************/
#include <Servo.h>

// ── 引脚（与主项目 pinsdefine.h 保持一致）──
#define UltraTrigPin  31
#define UltraEchoPin  30
#define ServoPWM1     15
#define SERVO_CENTER  80   // 实车中位角

Servo g_servo;

int getDistance()
{
    // 1. 拉低至少2μs，确保触发信号初始状态干净
    digitalWrite(UltraTrigPin, LOW);
    delayMicroseconds(2);
    // 2. 拉高至少10μs，触发一次测量
    digitalWrite(UltraTrigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(UltraTrigPin, LOW);

    // 3. 等待 Echo 变高（含超时保护35ms）
    const unsigned long TIMEOUT_US = 35000UL;
    unsigned long t0 = micros();
    while (!digitalRead(UltraEchoPin) && (micros() - t0) < TIMEOUT_US);
    if (!digitalRead(UltraEchoPin)) {
        Serial.println(F("[SONAR] 超时！无回波（障碍物超出量程或未接线）"));
        return -1;
    }

    // 4. 测量 Echo 高电平持续时间
    t0 = micros();
    while (digitalRead(UltraEchoPin) && (micros() - t0) < TIMEOUT_US);
    unsigned long dur = micros() - t0;

    // 5. 换算为厘米：距离(cm) = dur(μs) / 58
    int cm = (int)(dur / 58);
    if (cm > 400) {
        Serial.println(F("[SONAR] 超量程（>400cm）"));
        return -1;
    }
    return cm;
}

void setup()
{
    Serial.begin(115200);
    delay(200);

    pinMode(UltraTrigPin, OUTPUT);
    pinMode(UltraEchoPin, INPUT);

    g_servo.attach(ServoPWM1);
    g_servo.write(SERVO_CENTER);
    delay(500);   // 等舵机转到中位稳定

    Serial.println(F("=========================================="));
    Serial.println(F(" HC-SR04 + SG90 云台独立测试程序         "));
    Serial.println(F(" 串口波特率：115200                       "));
    Serial.println(F(" 舵机将在 30°~150° 之间自动来回扫描       "));
    Serial.println(F(" 用手在不同角度遮挡，观察距离数值变化     "));
    Serial.println(F("=========================================="));
}

void loop()
{
    // 舵机从30°到150°逐步扫描，每5°停留测一次距离
    static int  angle    = 30;
    static int  stepDir  = 1;       // +1 向右，-1 向左
    static bool sweeping = true;

    g_servo.write(angle);
    delay(200);   // 等舵机到位并消除振动

    int dis = getDistance();

    // 格式化输出，方便截图或做折线图
    Serial.print(F("[CLOUD] Angle="));
    Serial.print(angle);
    Serial.print(F("°\tDist="));
    if (dis < 0) Serial.println(F("无效"));
    else {
        Serial.print(dis);
        Serial.println(F("cm"));
    }

    // 推进角度
    angle += stepDir * 5;
    if (angle >= 150) { angle = 150; stepDir = -1; }
    if (angle <= 30)  { angle = 30;  stepDir =  1; }

    delay(60);   // 每次测量间隔≥60ms避免串扰
}

5.1 调试流程

①验证超声波基本功能

先不装舵机，只接HC-SR04，固定对准一面墙，观察串口输出的距离是否稳定（同一位置多次读数偏差应在±2cm内）

②验证舵机基本功能

先不接HC-SR04，只接舵机，烧录后观察舵机是否从30°开始做30°~150°的来回扫描，确认转动方向和幅度是否符合预期

③联合测试

两者都接好，用手在不同角度位置遮挡，对比距离读数随角度的变化，验证云台测距的完整功能，观察串口输出内容

5.2 视频演示

零知派迷你板——HC-SR04 超声波云台扫描与测距测试

本视频演示零知智能小车项目中 HC-SR04 超声波测距模块与 SG90 舵机云台的联合独立测试。舵机在 30°~150° 范围内往复扫描，超声波模块在每个角度下测量前方障碍物距离，串口监视器同步打印角度与对应距离数值

六、项目超声波-舵机云台功能实现

在 sc_perception.h/.cpp 中，SonarServo 类把超声波测距和舵机转动这两个操作封装在一起，封装的核心价值在于：

三个行为模式（避障、贴边、魔术手）都可以直接调用 g_sonar.getDistance() 和 g_sonar.sweepTo()，不需要各自重复写驱动代码，也不会出现两个行为模式同时操作同一个舵机对象的冲突（因为同一时刻只有一个行为模式在运行）

6.1 SonarServo 底层封装

SonarServo 类封装了超声波测距和舵机控制两个底层操作，对外提供三个公共方法

class SonarServo {
private:
    Servo _servo;
public:
    SonarServo();                    // 初始化舵机，默认居中
    int  getDistance();              // 单次测距，返回厘米数，-1 表示超时
    void sweepTo(int to, int stepMs = 8);  // 平滑转动到目标角度
    void setAngle(int angle);        // 直接设置角度
};

getDistance() 实现核心是 35ms 超时保护和 -1 无效值返回

内部位置记录设计

void SonarServo::sweepTo(int to, int stepMs)
{
    static int _current = SERVO_DEFAULT_POS;   // 静态变量记录当前角度
    int step = (to > _current) ? 1 : -1;
    while (_current != to) {
        _current += step;
        _servo.write(_current);
        delay(stepMs);
    }
}

_current 是一个 static 局部变量，函数调用结束后不会销毁，每次调用时都从上次停留的角度开始转动，而不是从0°或某个固定角度重新开始

sweepTo() 逐度步进的方式比直接 write(to) 更平滑，能有效减少舵机转动时的抖动

6.2 AvoidBehavior 超声波避障

避障模式的核心思路是一个 状态机 + 三点扫描 + IR 融合 的决策系统

①三点扫描策略

const int AvoidBehavior::SCAN_ANGLES[3] = {80, 130, 30};
//                                         前   左   右

注意：舵机 0° 朝右，180° 朝左，80° ≈ 正前方。

if (millis() - _scanTs >= SCAN_STEP_MS) {
    int d = g_sonar.getDistance();
    if (d > 0) _dist[_scanIdx] = d;   // 无效读数保持上次值
    _scanIdx = (_scanIdx + 1) % 3;
    g_sonar.setAngle(SCAN_ANGLES[_scanIdx]);
    _scanTs = millis();
}

三个角度轮换扫描，每 SCAN_STEP_MS（120ms）切换一个角度

②决策优先级

优先级	条件	动作
①	IR 双侧都有触发	立即后退
②	IR 左触发 + 左方超声波近	大幅右转
③	IR 右触发 + 右方超声波近	大幅左转
④	前方超声波近（≤28cm）	停车→左右决策→转向
⑤	仅 IR 左触发	右前进
⑥	仅 IR 右触发	左前进
⑦	全无障碍	自适应速度前进

单点测距只能知道“前方有没有障碍”，但不知道“障碍在左边还是右边”。三点扫描可以同时获得前方、左侧、右侧三个方向的距离信息，决策更智能

③前方障碍决策逻辑

if (dF > 0 && dF <= BLOCK_CM) {
    bool irBias = false;
    if      (irL && !irR) { irBias = true;  }   // 左有IR→偏向右转
    else if (irR && !irL) { irBias = false; }   // 右有IR→偏向左转
    else {
        irBias = (dR >= dL);                     // 右边空间更大→向右转
    }
    if (dL <= BLOCK_CM && dR <= BLOCK_CM) {
        g_motor.goWithDirec(carBack);            // 两侧都堵→后退
        return;
    }
    irBias ? g_motor.goWithDirec(carRight) : g_motor.goWithDirec(carLeft);
}

IR 与超声波的互补关系：

传感器	优势	劣势
超声波	测距精准（2~400cm）	响应慢，有盲区，锥角内检测
IR 红外	响应极快，近距离可靠	只能检测有无，不能测距

④自适应速度

无障碍时根据前方距离调整速度

if (dF > 0 && dF <= SLOW_CM) {
    int spd = map(constrain(dF, BLOCK_CM, SLOW_CM), BLOCK_CM, SLOW_CM, 60, 140);
    g_motor.motorRun(spd, spd);
} else {
    g_motor.goWithDirec(carForwardSlow);
}

前方越近（在 28~50cm 区间），速度越慢，避免急刹车式的不平滑控制

6.3 NearWallBehavior 贴边行驶

贴边模式要解决的问题是：让小车始终和一侧的墙壁保持固定距离前进——这是一个典型的闭环控制问题，用 PID 来实现

①左右扫描锁定墙侧

void NearWallBehavior::onEnter()
{
    g_sonar.sweepTo(30);
    delay(200);
    int dR = 0;
    for (int i = 0; i < 3; i++) { int d = g_sonar.getDistance(); if(d>0) dR += d; delay(30); }
    dR /= 3;

    g_sonar.sweepTo(150);
    delay(200);
    int dL = 0;
    for (int i = 0; i < 3; i++) { int d = g_sonar.getDistance(); if(d>0) dL += d; delay(30); }
    dL /= 3;

    _wallLeft = (dL <= dR);        // 选择距离更近的一侧贴墙
    g_sonar.setAngle(_wallLeft ? 150 : 30);   // 舵机锁定在该方向
}

确定贴边方向后，舵机就固定在那个角度不再转动，循环测量到墙壁的距离

②PID 闭环控制

void NearWallBehavior::update()
{
    int dis = g_sonar.getDistance();
    if (dis < 0) dis = (int)setpoint;
    dis = constrain(dis, 0, (int)(setpoint * 2));

    _input = dis;
    _pid.Compute();

    int lPwm = _wallLeft ? (int)(basePwm - _output) : (int)(basePwm + _output);
    int rPwm = _wallLeft ? (int)(basePwm + _output) : (int)(basePwm - _output);

    g_motor.motorRun(constrain(lPwm, -255, 255), constrain(rPwm, -255, 255));
}

PID参数说明

参数	默认值	作用
`Kp`	4.0	比例项：偏差越大，修正越猛
`Ki`	0.02	积分项：消除稳态偏差
`Kd`	0.2	微分项：抑制振荡
`setpoint`	17cm	目标距离
`basePwm`	70	基础速度

为什么贴边用PID三项而循迹只用PD两项？

对比项	循迹（PD）	贴边（PID）
偏差类型	瞬时传感器读值，没有固定偏置	持续测距，有稳定状态的误差
积分项作用	积分容易在连续弯道过冲	积分消除长时间行驶中的稳态偏差
是否需要I	不需要	需要

调试建议

先用纯 P（Ki=Kd=0）观察是否能大致维持设定距离
逐步加 I 消除长时间运行后的稳态偏差
最后加 D 抑制振荡

6.4 MagicBehavior 测距跟随

手掌在小车正前方移动，小车跟着手掌的远近自动前进后退——手靠近就后退、手远离就前进

void MagicBehavior::update()
{
    int dis = g_sonar.getDistance();
    if (dis < 0) { g_motor.stop(); return; }   // 无效读数停车

    int MAX_D = FAR + NEAR;   // = 18 + 13 = 31cm
    dis = constrain(dis, 0, MAX_D);

    if (dis > FAR) {
        // 手太远 → 前进，距离越远速度越快
        g_motor.motorRun(map(dis, FAR, MAX_D, 60, 120),
                         map(dis, FAR, MAX_D, 60, 120));
    } else if (dis < NEAR) {
        // 手太近 → 后退，距离越近速度越快
        g_motor.motorRun(-map(dis, 0, NEAR, 120, 60),
                         -map(dis, 0, NEAR, 120, 60));
    } else {
        // 死区 NEAR(13cm) ~ FAR(18cm) → 停车
        g_motor.stop();
    }
}

①死区设计

阈值	值	含义
`NEAR`	13cm	太近 → 后退
`FAR`	18cm	太远 → 前进
死区	13~18cm	保持静止（5cm 宽）

死区的作用是避免手稍微抖动一下小车就来回小幅度抽动，给操作者一定的容错空间。

②距离→速度映射

场景	映射	效果
距离 > 18cm	`map(dis, 18, 31, 60, 120)`	越远走得越快（最大 120）
距离 < 13cm	`map(dis, 0, 13, 120, 60)`	越近退得越快（最大 120）
13~18cm 之间	停车	容错死区

这种渐进式响应手感比较自然，不会出现突然加速或急停的突兀感

6.5 系统流程图

七、常见问题解答(FAQ)

Q1：贴边模式PID参数应该怎么从零开始整定？

A：推荐顺序：先置 Ki=0、Kd=0，只用P控制，从小值（比如1.0）开始逐步增大Kp，观察小车能不能大致维持在目标距离附近来回摆动。找到能维持但有明显振荡的Kp后，加入Kd（从0.1开始）抑制振荡，最后加入小量Ki（0.01~0.05）消除长距离行驶中的稳态偏差。全程可以通过蓝牙热发 WKP=/WKI=/WKD= 在不重烧的情况下实时调整