机器人避障算法：感知、扫描、决策、转向

一台机器人能读出干净的距离、能扫描出空旷方向之后，还剩一块：决定该做什么的逻辑。这就是一台避障机器人的大脑——一个短短的循环，往前开，注意到有东西太近，看看周围，然后朝着空处转。它比听上去简单，而把它做对主要靠的是不把自己困住。

反应式，不是规划式

这台机器人没有地图。它对眼下面前的东西作出反应：感知、决策、动作、重复。这就是反应式导航，而它是正确的第一步——便宜、健壮，足以在杂乱房间里游荡而不撞到任何东西。它和路径规划不是一回事，后者是机器人用地图提前选一条路线（那是占据栅格上的 A*的地盘）。反应式避让是地基，规划在后面。

停车阈值

一切都取决于一个数：**多近算太近？**选一个距离，机器人到了就停下重新考虑。太小，它还没来得及反应就蹭上障碍；太大，它对什么都停，神经质地蠕行。合适的值取决于速度——更快的机器人必须更早反应，因为它在能停住之前会走过更多地面。一台速度温和的小机器人，15–25 cm 是个合理的起点。

状态机

测距传感器负责躲开大部分障碍；碰撞开关负责抓住它看不见的那些，而每一台无人值守的机器人都希望两样都有。

表达这个行为最干净的方式是一个有限状态机：几个命名的状态，加上它们之间清晰的转换规则。它把一堆缠绕的嵌套 if 换成你能读、能调试的东西。（通用模式见机器人行为的有限状态机。）

enum State { DRIVE, SCAN, TURN, REVERSE };
State state = DRIVE;
const int STOP_CM = 20;

void loop() {
  switch (state) {
    case DRIVE:
      driveForward();
      if (medianCm() < STOP_CM) { stopMotors(); state = SCAN; }
      break;

    case SCAN: {
      int a = bestAngle();            // servo sweep from the scan tutorial
      long open = distanceAt(a);
      if (open < STOP_CM)      state = REVERSE;   // nowhere to go
      else { turnToward(a); state = TURN; }
      break;
    }

    case TURN:
      if (turnComplete()) state = DRIVE;          // opening cleared, resume
      break;

    case REVERSE:
      backUp(400);                                // ms
      state = SCAN;                               // then look again
      break;
  }
}

转弯方向怎么选

扫描已经把难的部分做完了：它返回最空旷的方位。朝那边转。turnToward(angle) 这个调用把“左边空旷”映射成左转、“右边空旷”映射成右转，用的是差速驱动——一个轮子前进、另一个后退，大致原地旋转。

避开振荡和死胡同

有两种失效模式会把天真的机器人困住，而两者的修法都很简单：

• 在一个左右看起来差不多的拐角上左右反复摇摆。加一个最小转弯时长，让机器人对一次转弯做出承诺，而不是每轮循环都重新决定。
• 楔进一个每个方向都堵的死胡同。这正是 REVERSE 状态存在的理由：退开，然后重新扫描。一个小小的卡住计数器——扫了好几次都没有进展就退得更狠——能破掉最糟糕的陷阱。

整定：速度与反应距离

有两个旋钮决定机器人感觉是平稳还是惊慌：行驶速度和停车阈值。提高速度就必须提高阈值才能保住安全的停车距离——延迟项按 v·t、刹车项按 v²/2a，见延迟与刹车距离——这也是为什么速度翻倍会让阈值涨不止一倍。找到好配对最快的办法，是在一个犯错不花钱的地方改它们——避障模拟器——然后才把数值搬到硬件上。

迟滞：两个阈值，不是一个

上面那个状态机用的是单一的 STOP_CM，而这是第一件该修的事。一台在某个阈值附近徘徊的机器人，会因为细微的测量变化翻转比较结果，每秒钟在 DRIVE 和 SCAN 之间翻好多次。它会结巴，而不是做出决断。

const int STOP_CM   = 20;   // stop when closer than this
const int RESUME_CM = 30;   // do not drive again until clearer than this

两者之间的间隙就是迟滞，它强迫行为做出决断。同一个模式能修掉机器人代码里几乎所有由阈值驱动的振荡，不只是这一个——边缘检测器、电量低警告、对接接近都是。

关于间隙大小有一条有用的规则：**把它取得比你的测量噪声加上机器人在一轮循环里走过的距离更大。**对一台 0.3 m/s、传感器周期 60 ms 的机器人来说，那是 18 mm 的行程加上超声波散布的两三厘米——所以 10 cm 的间隙是宽裕的，3 cm 的间隙仍然会抖。

对转弯做出承诺

第二种振荡发生在决策层：车往左转，扫描现在看起来不一样了，它就往右转，于是原地来回摇摆。两个机制修掉它：

unsigned long turnStartedAt = 0;
const unsigned long MIN_TURN_MS = 250;    // commit for at least this long

case TURN:
  if (millis() - turnStartedAt < MIN_TURN_MS) break;   // no re-deciding yet
  if (medianCm() > RESUME_CM) { stopTurn(); state = DRIVE; }
  break;

一个最小转弯时长阻止机器人在转弯还没改变任何东西之前就重新评估，而它检查的是恢复阈值而不是停车阈值。

察觉自己卡住了

REVERSE 处理的是正前方的死胡同。它处理不了更难的那种情况：机器人技术上是动着的、扫着的、转着的，却哪儿也没去——一个凹进去的角落，或者一把它不停绕圈的椅子。

uint8_t scanCount = 0;
unsigned long lastProgressAt = 0;

// Called whenever the robot successfully drives forward for a while.
void notedProgress() { scanCount = 0; lastProgressAt = millis(); }

void escalate() {
  scanCount++;
  if (scanCount >= 3) {
    backUp(800);                    // back further
    pivot(random(120, 240));        // and turn a large, RANDOM amount
    scanCount = 0;
  } else if (millis() - lastProgressAt > 15000) {
    backUp(1200);
    pivot(180);                     // full about-turn
    lastProgressAt = millis();
  }
}

这里有两个值得点名的设计点。

**升级。**对障碍的第一次反应是小幅转弯；对同一种情况的第三次反应是大幅的。固定不变地重复同一个反应，正是让机器人待在角落里的原因。

**随机。**一台确定性的机器人掉进对称陷阱，每次都会做同样的选择，于是永远循环。脱困转弯里加一个随机分量就打破了这种对称，这也是最早那批扫地机器人采用随机反弹而不是聪明规则的原因——它对那些你没预料到的陷阱是真的健壮。

在每个状态里读碰撞开关

写出来的状态机只检查距离，而每一种测距传感器都有它看不见的表面。一台没有接触感知的机器人会把窗帘一直推到电机堵转。

接触应该在状态机之上处理，而不是在某个状态内部：

void loop() {
  updateBumpers();                       // debounced, from the bump-switch tutorial

  // Contact overrides whatever the robot thought it was doing.
  if (bumpLeft || bumpRight) {
    stopMotors();
    backUp(300);
    pivot(bumpLeft ? +60 : -60);         // turn AWAY from the side that hit
    state = SCAN;
    return;
  }

  switch (state) { /* ... as before ... */ }
}

注意符号：要朝背离那个被触发的开关的方向转。这正是两个开关比一个更值得占位置的确切原因——单个居中开关只能告诉你撞到了东西，转弯方向只能靠运气。

速度和阈值，做成一张表

停车阈值不是一个常数；它是速度的函数，因为刹车距离随速度的平方增长。对一台感知加决策延迟 80 ms、按 1.0 m/s² 刹车、90 mm 转弯半径、50 mm 余量的机器人：

速度从 0.3 翻到 0.6 m/s，阈值并没有翻倍——但它确实长了 76%；而到 0.9 m/s 时，机器人必须在半米以外反应。这就是为什么一台在步行速度下躲得可靠的机器人一加速就开始撞东西，也是为什么修法是随速度变化的阈值，而不是一个让机器人在所有地方都畏畏缩缩的固定大值。

int stopThresholdCm(float speedMps) {
  const float latency = speedMps * 0.08f;             // 80 ms
  const float braking = (speedMps * speedMps) / 2.0f; // a = 1.0 m/s^2
  return (int)((latency + braking + 0.09f + 0.05f) * 100.0f);
}

把状态打印出来就是调试工具

你能加的最有用的一行：

if (state != lastReportedState) {
  Serial.print(millis()); Serial.print(F(" -> "));
  Serial.println(STATE_NAMES[state]);
  lastReportedState = state;
}

在变化时打印而不是每轮循环打印，输出才读得下去；而它把一台“有时候行为怪”的机器人变成一份你能读的记录。这个行为里的几乎每一个缺陷，都会表现成一段讲不通的状态序列——一个永远不结束的 TURN，一个每秒触发四十次的 SCAN，一个立刻又进入的 REVERSE。

如果你没法从单个变量回答“这台机器人现在在干什么”，那这个状态机就没在干它的活。

故障排查