概述

避障是智能小车最经典也最考验逻辑设计的功能之一。本文单独拆解一套基于 ESP32-S3 + 超声波（HC-SR04）+ 舵机 的避障模块，采用 连续扫描 + 四状态机 的设计：舵机在行进过程中持续左右扫动，超声波实时更新左/中/右三个方向的距离，主状态机据此做出前进、转向、后退、掉头的决策，全程非阻塞，不会因为测距而卡住小车控制。

---

一、系统接线部分

1.1 硬件清单

序号	元件名称	规格 / 型号	数量
1	主控板	ESP32-S3 开发板	1
2	超声波测距模块	HC-SR04	1
3	舵机	SG90 / MG90S	1
4	舵机云台/支架	配合超声波固定	1
5	电机驱动 + 电机	零知派小车底盘（驱动避障动作）	1 套
6	排线	2.54 4Pin排线	1
7	独立电源	给舵机/电机供电	1

---

1.2 接线方案表

避障模块本身只需要 3 个 GPIO：超声波的 Trig、Echo，以及舵机的信号线。

HC-SR04 超声波接线

HC-SR04 引脚	ESP32-S3 引脚	说明
VCC	5V	电源
Trig	GPIO 1	触发测距信号（输出）
Echo	GPIO 2	回波信号（输入）
GND	GND	接地

舵机接线

舵机线	连接位置	说明
信号线（橙/黄）	GPIO 42	PWM 控制信号
电源正（红）	5V	电源
电源负（棕/黑）	GND	接地

---

1.3 连接示意图

        舵机带动超声波左右转动，形成"扫描雷达"，超声波负责测距，两者配合就是整个避障的感知前端。

---

二、安装与使用教程

2.1 开源平台-搜索"避障模块"-代码下载自动打开

2.2 关键参数配置（config.h）

避障行为由一组距离/速度阈值控制，集中放在配置文件里方便调试：

// 超声波与舵机引脚
#define SR04_TRIG 1
#define SR04_ECHO 2
#define SERVO_PIN 42

// 避障距离阈值（单位 cm）
#define AVOID_DISTANCE_EMERGENCY 20   // 紧急停止距离
#define AVOID_DISTANCE_SLOW      35   // 减速/停车决策距离
#define AVOID_DISTANCE_SAFE      55   // 安全/恢复前进距离

// 避障速度
#define AVOID_SPEED_NORMAL       200  // 正常行驶速度
#define AVOID_SPEED_SLOW         150  // 慢速/后退速度
#define AVOID_SPEED_TURN         170  // 原地转向速度

// 扫描与脱困时间参数
#define AVOID_SWEEP_STEP_MS      400  // 连续扫描每步间隔(ms)
#define AVOID_BACK_MS            900  // 后退持续时间(ms)

2.3 连接-验证-上传

---

三、代码讲解部分

避障模块封装成一个 AvoidControl 类，对外只暴露 begin() / run() / stop() 三个接口。下面逐段讲解核心实现。

3.1 舵机初始化（MCPWM）

        舵机活动范围较大，且通电后会自动回到90°的位置，所以在固定齿轮配件时最好先通上电，且要尽可能平行于舵机本身安装，如下图。否则可能导致避障模式下，舵机活动范围太大导致支架损毁或者长时间卡住而烧毁

// 时基 1MHz，周期 20000us = 20ms = 50Hz
#define SERVO_TIMEBASE_RESOLUTION_HZ  1000000
#define SERVO_TIMEBASE_PERIOD         20000
#define SERVO_MIN_PULSEWIDTH_US       500    // 0度
#define SERVO_MAX_PULSEWIDTH_US       2500   // 180度

void AvoidControl::begin() {
    pinMode(SR04_TRIG, OUTPUT);
    pinMode(SR04_ECHO, INPUT);

    // 创建 MCPWM timer
    mcpwm_timer_handle_t timer = NULL;
    mcpwm_timer_config_t timer_config = {
        .group_id      = 0,
        .clk_src       = MCPWM_TIMER_CLK_SRC_DEFAULT,
        .resolution_hz = SERVO_TIMEBASE_RESOLUTION_HZ,
        .count_mode    = MCPWM_TIMER_COUNT_MODE_UP,
        .period_ticks  = SERVO_TIMEBASE_PERIOD,
    };
    mcpwm_new_timer(&timer_config, &timer);

    // 创建 operator 并连接 timer
    mcpwm_oper_handle_t oper = NULL;
    mcpwm_operator_config_t oper_config = { .group_id = 0 };
    mcpwm_new_operator(&oper_config, &oper);
    mcpwm_operator_connect_timer(oper, timer);

    // 创建 comparator（用于改占空比，即脉宽）
    mcpwm_comparator_config_t cmp_config = {};
    cmp_config.flags.update_cmp_on_tez = true;
    mcpwm_new_comparator(oper, &cmp_config, &servoComparator);

    // 创建 generator，绑定到舵机引脚
    mcpwm_gen_handle_t generator = NULL;
    mcpwm_generator_config_t gen_config = { .gen_gpio_num = SERVO_PIN };
    mcpwm_new_generator(oper, &gen_config, &generator);

    // 周期开始拉高，比较点拉低 → 标准舵机 PWM
    mcpwm_generator_set_action_on_timer_event(generator,
        MCPWM_GEN_TIMER_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP,
            MCPWM_TIMER_EVENT_EMPTY, MCPWM_GEN_ACTION_HIGH));
    mcpwm_generator_set_action_on_compare_event(generator,
        MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP,
            servoComparator, MCPWM_GEN_ACTION_LOW));

    mcpwm_timer_enable(timer);
    mcpwm_timer_start_stop(timer, MCPWM_TIMER_START_NO_STOP);

    currentServoAngle = 90;
    servoWrite(90);   // 回中
    // ... 状态变量初始化
}

        MCPWM 这套新版 API 是模块化的：timer 产生时基 → operator 挂在 timer 上 → comparator 决定翻转点 → generator 把波形输出到引脚。看起来步骤多，但好处是和电机用的 LEDC 完全是两套硬件，互不抢占。

3.2 舵机写角度

改变舵机角度，本质就是改变 comparator 的比较值（也就是高电平脉宽，单位 μs）。500μs 对应 0°，2500μs 对应 180°，线性映射：

static inline uint32_t angle_to_us(int angle) {
    return SERVO_MIN_PULSEWIDTH_US +
           ((SERVO_MAX_PULSEWIDTH_US - SERVO_MIN_PULSEWIDTH_US) * angle) / 180;
}

void AvoidControl::servoWrite(int angle) {
    angle = constrain(angle, 0, 180);
    int delta = abs(angle - currentServoAngle);
    currentServoAngle = angle;

    uint32_t us = angle_to_us(angle);
    mcpwm_comparator_set_compare_value(servoComparator, us);

    // 按转动幅度动态等待，确保舵机转到位再测距
    // 每度约3ms，最少80ms，最多500ms
    int waitMs = constrain(delta * 3, 80, 500);
    delay(waitMs);
}

        这里有个细节：转动幅度越大，等待时间越长。因为舵机转 90° 比转 10° 需要更多时间，如果不等它到位就立刻测距，量到的是"半路"的距离，数据就废了。

3.3 超声波测距

        标准的 HC-SR04 时序：Trig 拉高 10μs 触发，然后用 pulseIn 读 Echo 高电平持续时间，距离 = 时间 × 声速 / 2。

int AvoidControl::getDistance() {
    digitalWrite(SR04_TRIG, LOW);
    delayMicroseconds(2);
    digitalWrite(SR04_TRIG, HIGH);
    delayMicroseconds(10);
    digitalWrite(SR04_TRIG, LOW);

    long dur = pulseIn(SR04_ECHO, HIGH, 25000);  // 25ms 超时
    if (dur == 0) return 200;                    // 超时视为前方空旷
    return constrain((int)(dur * 0.034 / 2), 0, 200);
}

  0.034 是声速换算系数（340m/s = 0.034cm/μs）。加 25ms 超时是为了防止前方无障碍时 pulseIn 一直死等，超时就返回 200cm（当作"很远/空旷"）。

3.4 连续扫描（核心非阻塞设计）

这是整套避障最关键的设计。舵机不是"停下来测三个方向"，而是在小车行进过程中按固定节奏一步步扫，每一步转到一个角度、测一次距离、归类到左/中/右。

扫描序列：右(30°) → 中(90°) → 左(150°) → 中(90°) → 循环。

const int AvoidControl::SWEEP_ANGLES[4] = {30, 90, 150, 90};

void AvoidControl::updateSweep() {
    unsigned long now = millis();
    if (now - lastSweepTime < AVOID_SWEEP_STEP_MS) return;  // 没到节奏，直接返回

    // 转到当前步的目标角度
    servoWrite(SWEEP_ANGLES[sweepStep]);

    // 测距（舵机已到位）
    int d = getDistance();

    // 按角度归类到对应方向
    int angle = SWEEP_ANGLES[sweepStep];
    if (angle <= 45) {
        distRight  = d;
    } else if (angle >= 135) {
        distLeft   = d;
    } else {
        distCenter = d;
    }

    Serial.printf("[扫描] 角度=%3d°  左=%3dcm 中=%3dcm 右=%3dcm\n",
                  angle, distLeft, distCenter, distRight);

    sweepStep     = (sweepStep + 1) % 4;
    lastSweepTime = now;
}

        通过 millis() 时间判断控制节奏（AVOID_SWEEP_STEP_MS 每步间隔），让扫描成为"后台持续进行"的过程，主状态机随时可以读取 distLeft / distCenter / distRight 这三个最新值，不必停车等测距。

3.5 决策函数

        当中央方向被挡住时，根据左右两侧距离决定往哪边转。这里加了一个"惯性权重"防止小车在两个方向之间来回摇摆：

AvoidControl::AvoidState AvoidControl::decideTurnState() {
    // 三方皆堵 → 后退
    if (distLeft   < AVOID_DISTANCE_SLOW &&
        distCenter < AVOID_DISTANCE_EMERGENCY &&
        distRight  < AVOID_DISTANCE_SLOW) {
        lastTurnDir = 0;
        return STATE_BACKING;
    }

    // 惯性加成：上次方向 +10cm 权重，防止反复横跳
    int scoreLeft  = distLeft  + (lastTurnDir == -1 ? 10 : 0);
    int scoreRight = distRight + (lastTurnDir == +1 ? 10 : 0);

    if (scoreLeft >= scoreRight) {
        lastTurnLeft = true;
        lastTurnDir  = -1;
    } else {
        lastTurnLeft = false;
        lastTurnDir  = +1;
    }
    return STATE_TURNING;
}

3.6 主状态机

        避障的行为由四个状态构成：前进、转向、后退、掉头。每个状态有自己的进入/退出条件，由 run() 在每次主循环中驱动：

void AvoidControl::run() {
    if (!active) {
        // 首次进入：初始化状态
        active = true;
        currentState = STATE_FORWARD;
        // ... 复位扫描和距离
        servoWrite(90);
    }

    // ① 连续扫描（非阻塞，按节奏推进）
    updateSweep();

    // ② 状态机驱动
    switch (currentState) {
        case STATE_FORWARD:  handleForward();  break;
        case STATE_TURNING:  handleTurning();  break;
        case STATE_BACKING:  handleBacking();  break;
        case STATE_UTURN:    handleUTurn();    break;
    }
}

        以"前进"状态为例，它采用四级距离策略：

void AvoidControl::handleForward() {
    int dist = distCenter;  // 直接用扫描得到的中央距离

    if (dist < AVOID_DISTANCE_EMERGENCY) {
        // 紧急停止 → 立即决策
        CarDrive.stop();
        currentState   = decideTurnState();
        stateStartTime = millis();
    }
    else if (dist < AVOID_DISTANCE_SLOW) {
        // 减速区 → 停车决策
        CarDrive.stop();
        currentState   = decideTurnState();
        stateStartTime = millis();
    }
    else if (dist < AVOID_DISTANCE_SAFE) {
        // 预警区 → 慢速行驶，并根据侧方距离微调方向
        if (distLeft > distRight + 20) {
            CarDrive.run(AVOID_SPEED_SLOW - 20, AVOID_SPEED_SLOW); // 轻微左偏
        } else if (distRight > distLeft + 20) {
            CarDrive.run(AVOID_SPEED_SLOW, AVOID_SPEED_SLOW - 20); // 轻微右偏
        } else {
            CarDrive.run(AVOID_SPEED_SLOW, AVOID_SPEED_SLOW);
        }
    }
    else {
        // 安全区 → 全速前进
        CarDrive.run(AVOID_SPEED_NORMAL, AVOID_SPEED_NORMAL);
    }
}

        转向、后退、掉头三个状态同理，各自带超时兜底（比如转向超过 1800ms 还没找到出路就重新决策，掉头超过 3000ms 就再次后退），防止卡死在某个状态里。

---

四、项目结果演示

4.1 避障行为流程

启动避障模式
    ↓
舵机回中(90°)，开始连续扫描
    ↓
━━━━━━━━━━━━━━━━━━━━━━━━━━━
       前方空旷（>55cm）
━━━━━━━━━━━━━━━━━━━━━━━━━━━
全速前进，舵机持续左右扫描
    ↓
━━━━━━━━━━━━━━━━━━━━━━━━━━━
      前方预警（35~55cm）
━━━━━━━━━━━━━━━━━━━━━━━━━━━
减速慢行，并向较空旷一侧微调
    ↓
━━━━━━━━━━━━━━━━━━━━━━━━━━━
      前方受阻（<35cm）
━━━━━━━━━━━━━━━━━━━━━━━━━━━
停车 → 比较左右距离 → 转向较空旷侧
    ↓
━━━━━━━━━━━━━━━━━━━━━━━━━━━
        三方皆堵
━━━━━━━━━━━━━━━━━━━━━━━━━━━
后退脱困 → 原地掉头 → 重新寻路

4.2 串口监视器输出示例

[避障] 模式启动
[扫描] 角度= 30°  左=200cm 中=200cm 右=180cm
[扫描] 角度= 90°  左=200cm 中= 65cm 右=180cm
[前进] 前方=65cm
[扫描] 角度=150°  左=120cm 中= 65cm 右=180cm
[前进] 前方=45cm
[前进] 减速区 45cm → 决策
[决策] 左=120cm 中=45cm 右=180cm
[决策] 右转 (左120cm 右180cm)
[转向] 已转420ms 前方=160cm
[转向] 前方清空 → 前进

4.3 四级距离策略一览

中央距离	区域	动作
> 55cm	安全区	全速前进
35~55cm	预警区	减速慢行 + 侧向微调
20~35cm	减速区	停车 → 决策转向
< 20cm	紧急区	立即停止 → 决策

---

4.4 调试与排错技巧

避障模块在每个关键环节都埋了串口打印（[扫描]、[决策]、[前进]、[转向] 等），跑不通时不用瞎猜，照着串口日志一步步定位即可。下面是几个最实用的判断方法。

看「扫描日志」判断舵机和超声波是否正常

正常运行时，[扫描] 这一行应该有两个特征：角度在 30 → 90 → 150 → 90 之间循环变化，并且三个方向的距离会随着小车移动、障碍物远近而持续更新。

[扫描] 角度= 30°  左=200cm 中=200cm 右=180cm
[扫描] 角度= 90°  左=200cm 中= 65cm 右=180cm
[扫描] 角度=150°  左=120cm 中= 65cm 右=180cm

对照排查：

现象	可能原因
角度一直不变（卡在某个值）	舵机没转，多半是 MCPWM 没初始化成功、或舵机供电/接线问题
角度在变，但某方向距离恒为 200	该方向超声波没读到回波，检查 Trig/Echo 接线、是否共地
距离恒为 0 或乱跳	Echo 电平干扰、接线松动，或 SR04 供电不足（需 5V）
完全没有 [扫描] 打印	没进避障模式，检查模式切换逻辑

看「决策日志」判断转向方向是否合理

每次受阻决策时会打印左右距离和最终选择，可以直接验证"是不是往更空旷的方向转了"：

[决策] 左=120cm 中=45cm 右=180cm
[决策] 右转 (左120cm 右180cm)

上面这例右侧 180cm 比左侧 120cm 空旷，选择右转，合理。如果你发现它总往更窄的一侧转，那就是决策逻辑或左右距离归类（distLeft/distRight）接反了。

看「状态切换日志」判断状态机是否健康

正常的一次避障，状态流转应该是连贯的：

[前进] 前方=45cm
[前进] 减速区 45cm → 决策
[决策] 右转 (左120cm 右180cm)
[转向] 已转420ms 前方=160cm
[转向] 前方清空 → 前进

如果你看到某一类日志反复刷屏、迟迟不切换到下一个状态（比如一直打印 [转向] 却始终不回到 [前进]），说明卡在该状态出不来——优先检查这个状态的退出条件是否永远无法满足，或者超时兜底是否生效。本项目给转向（1800ms）、掉头（3000ms）都加了超时退出，正是为了应对这种卡死。

一个小技巧：先静态测，再上路

调试初期不建议直接让小车跑。可以先把小车架空（轮子离地），用手在超声波前方比划远近，盯着串口看三个方向的距离变化和状态切换是否符合预期。确认"感知 + 决策"这条链路没问题后，再让它真正上地跑，能省去很多在地上追车的麻烦。

4.5 视频演示

---

五、避障原理与状态机设计讲解

5.1 为什么用"连续扫描"而不是"停车测距"

最常见的入门写法是：小车停下 → 舵机转到左、测一次 → 转到中、测一次 → 转到右、测一次 → 比较三个值 → 决定方向 → 继续走。

这种写法的问题是全程阻塞：每次决策都要停车，舵机转三次、测三次，耗时近 1 秒，小车走走停停，非常卡顿。

本项目改为连续扫描：舵机在行进中按固定节奏一步步扫，每一步只转一个角度、测一次，结果实时更新到左/中/右三个变量。主状态机随时读取这三个最新值做决策，不需要为了测距而停车。代价是单个方向的数据有轻微"滞后"（不是同一时刻测的三个方向），但对小车避障这种场景完全够用，换来的是流畅的连续行驶。

5.2 四状态机设计

避障行为抽象成四个状态，状态之间按条件迁移：

        ┌──────────────────────────────────────┐
        │                                       │
        ▼                                       │
   ┌─────────┐  前方受阻   ┌─────────┐  脱困成功  │
   │ FORWARD │───────────→│ TURNING │──────────┘
   │  前进   │←───────────│  转向   │
   └────┬────┘  前方清空   └─────────┘
        │
        │ 三方皆堵
        ▼
   ┌─────────┐   后退完成   ┌─────────┐
   │ BACKING │────────────→│  UTURN  │
   │  后退   │             │  掉头   │
   └─────────┘             └────┬────┘
        ▲                       │
        └───────────────────────┘
              掉头超时再次后退

每个状态独立处理自己的逻辑，并带超时兜底：转向最多 1800ms、掉头最多 3000ms，避免因为传感器误差或物理卡住而无限停留在某个状态。这是状态机设计里很重要的健壮性保障。

5.3 惯性权重：防止"鬼打墙"

如果纯粹按"哪边远往哪转"，当左右距离接近时，小车容易在两个方向之间反复横跳，原地抖动出不去（俗称"鬼打墙"）。

解决办法是给"上一次选择的方向"加一个权重（本项目是 +10cm）。这样只要两边差距不大，就倾向于延续上次的方向，避免来回摇摆，让转向决策更稳定。

5.4 重要的坑：舵机为什么用 MCPWM 而不是 LEDC

这是本项目（尤其在 ESP32-S3 上）最值得记录的一个坑。

最初舵机用的是常见的 ledcAttach / ledcWrite（LEDC 外设）来产生 PWM。在经典 ESP32 上没问题，但移植到 ESP32-S3 后，舵机死活不动，初始化甚至直接返回失败。

排查后发现根因：ESP32-S3 的 LEDC 只有 8 个通道（经典 ESP32 有 16 个）。而小车的 4 个电机用了 8 路 PWM，正好把 8 个 LEDC 通道占满。舵机作为"第 9 路"申请 LEDC 时，要么直接失败，要么被强行塞进电机正在用的定时器，导致舵机拿到的是电机的 12kHz 频率而不是 50Hz —— 表现就是"初始化显示成功，但舵机纹丝不动"。

解决方案：舵机改用 ESP32-S3 上和 LEDC 完全独立的 MCPWM 外设。MCPWM 是另一套专门用于电机/舵机控制的硬件，不占用 LEDC 的任何通道，从根本上避开了冲突。这也是 Espressif 官方例程驱动舵机时用 MCPWM 的原因 —— 舵机本来就该用它。

一句话总结：多路电机占满了 LEDC，舵机就别再挤 LEDC 了，换独立的 MCPWM。

---

六、常见问题解答（FAQ）

Q1：超声波一直返回 200 或 0，测不到距离？

检查以下几点：

• Trig / Echo 接线是否接反（Trig 是输出、Echo 是输入）

• HC-SR04 的 VCC 是否接的 5V（多数 SR04 需要 5V 才能正常工作）

• 是否共地

• 前方是否真的无障碍（无障碍时本代码故意返回 200 表示空旷）

---

Q2：舵机初始化显示成功，但完全不动？（重点）

如果你在 ESP32-S3 上、且小车有多路电机用了 LEDC，这几乎可以确定是 LEDC 通道被电机占满导致的。舵机用 LEDC 申请不到独立资源，或被塞进电机的定时器拿到错误频率。

解决办法：把舵机从 LEDC 改为 MCPWM 驱动（本文 3.1 的写法），MCPWM 和 LEDC 是两套独立硬件，不冲突。详见 [5.4]。

---

Q3：舵机旋转的范围不在正前方？

固定齿轮件前需要先通电保证舵机回到90°的位置，且安装时需要平行于舵机本身安装并拧紧螺丝，详情可以查看本文3.1节下有安装图

---

Q4：小车在障碍物前来回打转、出不去？

这是典型的"鬼打墙"，左右距离接近时反复横跳。本项目用惯性权重（上次方向 +10cm）来缓解。如果还严重，可以把权重调大，或者在转向状态里加更长的最短转向时间。

---

Q5：舵机扫描时小车很卡顿、走走停停？

多半是用了"停车测三个方向"的阻塞式写法。改用本文的连续扫描（3.4），通过 millis() 控制扫描节奏，让测距在行进中后台进行，主控不必停车等测距。

---

Q6：为什么舵机写角度后要 delay 一下？

因为舵机转动需要物理时间，转到位之前测的距离是"半路"的，不准。servoWrite 里按转动幅度动态 delay（每度约 3ms），确保舵机到位后再测距。这个 delay 在避障逻辑里是可接受的。

---

Q7：状态机卡在某个状态出不来怎么办？

本项目每个状态都加了超时兜底（转向 1800ms、掉头 3000ms 等）。如果你自己改逻辑后出现卡死，优先检查是不是某个状态缺少超时退出条件，或退出条件永远无法满足。

---

总结

本文单独拆解了智能小车的避障模块，核心是 超声波 + 舵机 的感知前端，配合 连续扫描 + 四状态机 的非阻塞控制逻辑。开发过程中解决了几个很有代表性的实际问题：

• 连续扫描替代阻塞式测距，让小车行驶更流畅；

• 四状态机 + 超时兜底，让避障行为健壮、不卡死；

• 惯性权重，解决左右横跳的"鬼打墙"问题；

• 最关键的，ESP32-S3 上舵机改用 MCPWM 而非 LEDC，绕开了多路电机占满 LEDC 通道的冲突。