Алгоритм обхода препятствий: восприятие, скан, решение, поворот

Когда робот умеет считывать чистое расстояние и сканировать направление, свободное от препятствий, остаётся одна часть: логика, которая решает, что делать. Это мозг робота для обхода препятствий — короткий цикл, который движет робота вперёд, замечает слишком близкий объект, осматривается и поворачивает к свободному месту. Это проще, чем звучит, и правильная реализация в основном сводится к тому, чтобы не застрять.

Реактивно, а не по плану

У этого робота нет карты. Он реагирует на то, что находится перед ним прямо сейчас: воспринимает, решает, действует, повторяет. Это реактивная навигация, и она является правильным первым подходом — дешёвым, надёжным и достаточным для блуждания в загромождённой комнате без столкновений. Это не то же самое, что планирование пути, при котором робот использует карту для выбора маршрута заранее (это область 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, который немедленно входит снова.

Если по одной переменной нельзя ответить на вопрос «что робот делает прямо сейчас», значит, конечный автомат не выполняет свою работу.

Диагностика