Решение лабиринта методом заливки для роботов Micromouse

Заливка — это алгоритм, который выигрывает большинство соревнований Micromouse и служит отличным введением в планирование при неопределённости. В отличие от учебного поиска пути, робот для прохождения лабиринта начинает не с полной карты: он обнаруживает стены по мере движения. Заливка создана именно для этого. Посмотрите, как она работает, в симуляторе решения лабиринта.

Не путь, а поле расстояний

Заливка присваивает каждой клетке число: расстояние от неё до цели. Целевые клетки получают ноль. Затем каждой соседней незаблокированной клетке присваивается на единицу больше, чем её наименьшему соседу; значения распространяются от цели, пока не будет охвачена каждая достижимая клетка.

Робот никогда не хранит маршрут. Он просто находится в клетке, смотрит на соседей и переходит в любую, у которой число ровно на единицу меньше. Поле расстояний и есть план, поэтому оно легко пересчитывается при изменении карты.

Оптимизм насчёт неизвестных стен

Робот не видит весь лабиринт, поэтому заливка считает каждую ещё не обнаруженную стену открытой. Такой оптимизм даёт кратчайшую возможную оценку и не даёт роботу бесцельно исследовать лабиринт вместо движения к цели.

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

Почему первый проход не является кратчайшим путём

Распространённый сюрприз: заливка не гарантирует кратчайший маршрут при первом проходе. Действуя по оптимистичным предположениям, она нашла один из способов добраться до цели, а не обязательно лучший. Реальные роботы Micromouse используют две фазы:

1. Проход поиска, который исследует достаточно лабиринта, чтобы построить точную карту.
2. Скоростной проход по кратчайшему пути, который открывает завершённая карта.

На раннем этапе эффективное исследование важнее оптимальности первого прохода.

Заливка против BFS и A*

Поиск в ширину и A* планируют на графе, который уже известен. Заливка рассчитана на лабиринт, который ещё исследуется: она выполняет новую заливку при каждой обнаруженной стене. Математически заливка вычисляет расстояния поиском в ширину (BFS) — разница в том, что она рассчитана на постоянно меняющуюся карту и недорого пересчитывает её.

Она также превосходит простое слежение за стеной (всегда держаться одной стены), которое не может достичь цели в центре лабиринта и редко находит короткий маршрут.

Цикл «измерение — планирование — движение»

void planOneCell(Maze& map, Robot& robot) {
  map.observe(robot.cell(), robot.senseLeftFrontRight());
  map.floodFill(centerGoals);
  Cell next = map.bestDescendingNeighbor(robot.cell());
  robot.turnAndMoveTo(next);
}

Оставьте измерение, построение карты, планирование и перемещение отдельными шагами. Именно эта граница позволяет одному и тому же планировщику работать в браузерном симуляторе, на настольном компьютере и на ESP32 с реальными датчиками расстояния и энкодерами.

Заливка целиком

Поле расстояний представляет собой волну поиска в ширину от цели, и его достаточно коротко, чтобы записать полностью:

const uint8_t N = 16;                       // 16 x 16 maze
uint8_t dist[N][N];                         // distance to goal, per cell
uint8_t walls[N][N];                        // bitmask: 1=N, 2=E, 4=S, 8=W

// A simple ring buffer beats a std::queue on a microcontroller.
uint8_t queueR[N * N], queueC[N * N];

void floodFill(const uint8_t goals[][2], uint8_t goalCount) {
  memset(dist, 255, sizeof(dist));          // 255 = unreachable

  uint16_t head = 0, tail = 0;
  for (uint8_t g = 0; g < goalCount; g++) {
    const uint8_t r = goals[g][0], c = goals[g][1];
    dist[r][c] = 0;
    queueR[tail] = r; queueC[tail] = c; tail++;
  }

  const int8_t dr[4] = {-1, 0, 1, 0};       // N, E, S, W
  const int8_t dc[4] = { 0, 1, 0, -1};

  while (head < tail) {
    const uint8_t r = queueR[head], c = queueC[head];
    head++;

    for (uint8_t d = 0; d < 4; d++) {
      if (walls[r][c] & (1 << d)) continue;             // wall in the way

      const int8_t nr = r + dr[d], nc = c + dc[d];
      if (nr < 0 || nr >= N || nc < 0 || nc >= N) continue;

      if (dist[nr][nc] > dist[r][c] + 1) {              // 255 means unvisited
        dist[nr][nc] = dist[r][c] + 1;
        queueR[tail] = nr; queueC[tail] = nc; tail++;
      }
    }
  }
}

Вот и весь алгоритм. В лабиринте 16×16 он посещает не более 256 клеток и на Arduino выполняется намного быстрее миллисекунды — поэтому повторная заливка при обнаружении каждой новой стены совершенно доступна, и никто не возится с обновлениями.

Запись стены с обеих сторон

Самая распространённая ошибка связана не с самой заливкой. Стена является общей для двух клеток, и запись только с одной стороны создаёт карту, на которой робот может пройти сквозь неё в одном направлении, но не в обратном:

void setWall(uint8_t r, uint8_t c, uint8_t dir) {
  walls[r][c] |= (1 << dir);

  const int8_t dr[4] = {-1, 0, 1, 0};
  const int8_t dc[4] = { 0, 1, 0, -1};
  const int8_t nr = r + dr[dir], nc = c + dc[dir];
  if (nr < 0 || nr >= N || nc < 0 || nc >= N) return;

  walls[nr][nc] |= (1 << ((dir + 2) % 4));   // the same wall, from the other side
}

Выражение (dir + 2) % 4 задаёт противоположное направление: север превращается в юг, восток — в запад. Симптом ошибки — робот строит маршрут прямо сквозь стену, которую сам же записал; наблюдать за этим действительно трудно.

Память на Arduino

Лабиринт 16×16 в 2 KB SRAM Uno получается тесным, но работоспособным, а расчёт стоит выполнить заранее:

Очередь — самая дорогая часть, и её проще всего уменьшить: один массив байтов с упакованными r<<4|c вдвое сокращает объём до 256 B. Если памяти всё ещё мало, упаковка карты стен по 2 бита на клетку вернёт ещё 192 B. На этом этапе большинство предпочитает ESP32 или Mega, и это разумный выбор — но само упражнение по упаковке тоже поучительно.

Выбор целевых клеток

Стандартная цель Micromouse — блок 2×2 в центре, а не одна клетка, и это не мелочь. Задание расстояния 0 для всех четырёх клеток заставляет робот направляться к той грани блока, до которой он может дойти дешевле всего; это быстрее и надёжнее, чем фиксировать одну клетку, к которой, возможно, придётся подходить с неудобной стороны.

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

Правильное исследование

Заливка говорит роботу, куда идти с учётом того, что он знает. Она ничего не говорит о том, что ему следует узнать, и робот, который всегда только спускается по градиенту, достигнет цели, не увидев большую часть лабиринта. Поэтому «кратчайший путь» скоростного прохода окажется кратчайшим лишь среди коридоров, по которым робот случайно прошёл.

Обычный соревновательный компромисс — вторая или третья строка: добраться до цели, затем пересчитать поле расстояний относительно начала и пройти этот маршрут; это естественно раскрывает другой набор коридоров. Два прохода обычно дают карту, достаточно хорошую, чтобы скоростной проход оказался в пределах нескольких процентов от оптимального.

Диагностика

Пятая строка особенно важна, и её стоит сказать прямо: алгоритм — не самая сложная часть Micromouse. Заливка занимает сорок строк, и её можно полностью проверить в симуляторе без оборудования. Сложность возникает потому, что после десяти клеток робот уже не находится там, где, по его мнению, находится; идеальный план, выполненный из вымышленной позиции, приводит к столкновению со стеной.

Попробуйте сами

Шаг за шагом пройдите симулятор решения лабиринта и следите, как поле расстояний пересчитывается каждый раз при обнаружении стены. Изменение чисел — самый быстрый способ понять, почему у робота всегда есть план.