Готовимся к Micromouse: как роботу построить карту лабиринта21.11.2024 10:30

Привет, Хабр! Меня зовут Денис Логашов, я инженер-исследователь отдела автоматической обработки результатов моделирования и визуализации YADRO. В этом году мне предложили поучаствовать в соревновании по робототехнике в дисциплине Micromouse, где роботизированной мыши нужно как можно быстрее найти путь в центр лабиринта и понять, что цель достигнута. Такие соревнования проводятся в разных странах уже почти 50 лет, и в этом году Micromouse вошел в программу фестиваля РобоФинист 2024 в Санкт-Петербурге, где мы заняли второе место.

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

Суть соревнования

В нашем варианте лабиринт строится на поле размером 16×16 ячеек. Ячейки квадратные, размер грани составляет 18 см. Зафиксирована точка старта: повернутый в определенном направлении робот начинает в определенной ячейке. Точка финиша — это, как правило, квадрат 2×2 ячейки с одним или несколькими входами.

Задача робота — найти кратчайший путь от старта до финиша и проехать по нему. Обновлять микрокод робота после демонстрации схемы лабиринта запрещено. На решение дается ограниченное время, за которое мышь может проходить лабиринт неограниченное число раз. Поэтому обычно робот сначала строит карту лабиринта, а потом уже пытается доехать до финиша по кратчайшему пути. Более подробно о соревнованиях Micromouse в целом можно почитать в другом посте.

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

Фото нашей «робомыши» с РобоФинист-2024

Анализ карты лабиринта

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

Любое число можно представить в виде последовательностей бит, то есть байтов. Ими мы и будем пользоваться в программе. Один байт содержит восемь бит: минимальное возможное число — 0b00000000 (это число 0, если мы говорим про беззнаковые числа), а максимальное число — 0b11111111 (это 255). 0b в этой записи показывает, что мы представляем число в виде последовательности бит.

Числами в их стандартном понимании мы оперировать не будем — нам это попросту не нужно. Мы рассматриваем каждый бит как некое состояние, true или false. В рамках одного байта получается восемь значений, которые могут нам сказать, выполняется ли некоторое условие или нет.

Условимся, что каждый бит в байте отвечает за наличие стенки с одной из четырех сторон вокруг робота. Если стенка есть, бит равен 1, если нет, то 0. Пятый бит в байте отвечает за верхнюю стенку (при взгляде на лабиринт сверху), шестой — за правую, седьмой — за нижнюю и восьмой — за левую. Такие битовые записи называют масками, и далее мы будем использовать понятие «маска стенки». Значения первых четырех бит рассмотрим позже.

Для удобства добавим все эти маски в переменные:

up_wall = 0b00001000
right_wall= 0b00000100
down_wall = 0b00000010
left_wall = 0b00000001

Будем считать, что изначально вся наша карта не имеет никаких стенок, то есть значения всех бит равны 0. Пусть исходный лабиринт имеет размер 5×5 ячеек:

Псевдокод для его инициализации выглядит следующим образом:

walls[5][5] 
for i = 0, i < 5, i++ 
  for j = 0, j < 5, j++ 
    walls[i][j] = 0

Основные операции с картой

Как нам устанавливать значения для стенок и потом проверять их? Воспользуемся битовыми операциями: нам понадобится побитовое ИЛИ (|) и побитовое И (&). Таблица истинности для них выглядит так:  

При побитовом ИЛИ мы получаем 1 в итоговом значении, если хотя бы один бит был равен 1. При побитовом И мы получаем 1, только если оба бита равны 1. Как эти операции применяются в нашем случае? Находясь в ячейке, робот считывает значения с датчиков, которые проверяют наличие стенок вокруг. Затем при помощи побитовой операции ИЛИ он может добавить соответствующую стенку в значение для текущей ячейки.

Напомню, что исходный размер лабиринта — 5×5 ячеек. Мы заранее можем выставить значения для внешних стенок. Например, для всех верхних стенок (если смотреть на лабиринт в двумерной проекции сверху) нужно воспользоваться маской up_wall (0b00001000). К исходным нулевым значениям мы добавляем новые с помощью операции ИЛИ:

0b00000000 | 0b00001000 = 0b00001000

Далее берем маску правой стенки 0b00000100. Почти все операции будут выглядеть так же, но в правом верхнем углу мы получим:

0b00001000 | 0b00000100 = 0b00001100

Выставим все значения по периметру:

Псевдокод для выставления начальных стенок:

for i = 0, i < 5, i++
  walls[i][0] = walls[i][0] | left_wall
  walls[i][4] = walls[i][4] | right_wall
  walls[0][i] = walls[0][i] | up_wall
  walls[4][i] = walls[4][i] | down_wall

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

Обновление карты при движении робота

При работе с двумерным массивом важно понимать, что первая координата описывает строку, то есть координату y, а вторая — столбец, то есть x.

Рассмотрим, как робот будет собирать информацию о карте при движении. Предположим, что он находится в ячейке (4, 0) и повернут вверх.

В этой позиции робот видит две стенки, правую и левую. Это соответствует маскам right_wall (0b00000100) и left_wall (0b00000001). Теперь нам нужно добавить эти биты к нашей карте. Как и при выставлении внешних стен, мы продолжаем пользоваться операцией ИЛИ.

https://habrastorage.org/getpro/habr/upload_files/f44/367/4c3/f443674c3e87195afb5bc771db933bb6.png

В этой ячейке мы уже предварительно получили 0b00000011, зафиксировав внешние стенки лабиринта снизу и слева. Теперь добавим правую:

0b00000011 | 0b00000100 = 0b00000111

Если теперь второй раз добавить левую стенку, результат не изменится, поскольку мы это уже делали:

0b00000111 | 0b00000001 = 0b00000111

В виде псевдокода получаем:

walls[4][0] = walls[4][0] | right_wall
walls[4][0] = walls[4][0] | left_wall

Теперь наша карта выглядит так:

Далее робот решает ехать прямо и перемещается в ячейку (3, 0).

Сенсоры определяют наличие стен спереди и слева. Текущее значение в этой ячейке 0b00000001. Берем маски up_wall и left_wall, выполняем битовые операции с ними — то есть с 0b00001000 и 0b00000001. Получаем 0b00001001. 

Следующим движением роботу нужно будет проехать одну ячейку вправо.

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

Для текущей позиции маской левой стенки относительно робота будет up_wall, передней стенки — right_wall и правой стенки — down_wall. Здесь обнаружена только стенка спереди, так что выставляем только это значение:

0b00000000 | 0b00000100 = 0b00000100 

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

С учетом позиции робота нам понадобятся маски right_wall для левого, down_wall для переднего и left_wall для правого датчика. Все три стенки зафиксированы роботом, так что мы получаем выражение:

0b00000010 | 0b00000100 | 0b00000010  | 0b00000001 = 0b00000111 

И напоследок еще один возможный случай: робот доехал до ячейки (4, 2) и смотрит влево.

Здесь нам нужны будут маски down_wall, left_wall и up_wall. Получаем следующее выражение:

0b00000010 | 0b00000010 | 0b00000001 | 0b00001000 = 0b00001011

В виде псевдокода выставление стен на карте будет выглядеть так:  

func set_walls(left_wall, front_wall, right_wall):
# y – текущая позиция робота по вертикали (глобальная переменная)
# x – текущая позиция робота по горизонтали (глобальная переменная)
# direction – текущее направление робота (глобальная переменная)
# walls - массив значений стенок (глобальная переменая)
# left_wall – имеет значение true, если робот увидел стенку слева
# front_wall – имеет значение true, если робот увидел стенку спереди
# right_wall – имеет значение true, если робот увидел стенку справа
    # если робот смотрит вверх, то выставляем значения для левой, верхней и правой 
    # стенки в ячейке соответственно
	if direction == up:
		if left_wall:
			walls[y][x] = walls[y][x] | left_wall
		if front_wall:
			walls[y][x] = walls[y][x] | up_wall
		if right_wall:
			walls[y][x] = walls[y][x] | right_wall

 	# если робот смотрит вправо, то выставляем значения для верхней, правой и нижней
 	# стенки в ячейке соответственно
	if direction == right: 
		if left_wall:
			walls[y][x] = walls[y][x] | up_wall
		if front_wall:
			walls[y][x] = walls[y][x] | right_wall
		if right_wall:
			walls[y][x] = walls[y][x] | down_wall

 	if direction == down:
		…
 	if direction == left:
		…

Направления вниз и влево попробуйте добавить самостоятельно. Ответ можно найти в спойлере ниже.

Код для выставления стен

func set_walls(left_wall, front_wall, right_wall):
# y – текущая позиция робота по вертикали (глобальная переменная)
# x – текущая позиция робота по горизонтали (глобальная переменная)
# direction – текущее направление робота (глобальная переменная)
# walls - массив значений стенок (глобальная переменая)
# left_wall – имеет значение true, если робот увидел стенку слева
# front_wall – имеет значение true, если робот увидел стенку спереди
# right_wall – имеет значение true, если робот увидел стенку справа
	if direction == up:
		if left_wall:
			walls[y][x] = walls[y][x] | left_wall
		if front_wall:
			walls[y][x] = walls[y][x] | up_wall
		if right_wall:
			walls[y][x] = walls[y][x] | right_wall
	if direction == right: 
		if left_wall:
			walls[y][x] = walls[y][x] | up_wall
		if front_wall:
			walls[y][x] = walls[y][x] | right_wall
		if right_wall:
			walls[y][x] = walls[y][x] | down_wall
 	if direction == down:
		if left_wall:
			walls[y][x] = walls[y][x] | right_wall
		if front_wall:
			walls[y][x] = walls[y][x] | down_wall
		if right_wall:
			walls[y][x] = walls[y][x] | left_wall
 	if direction == left:
		   if left_wall:
			walls[y][x] = walls[y][x] | down_wall
		if front_wall:
			walls[y][x] = walls[y][x] | left_wall
		if right_wall:
			walls[y][x] = walls[y][x] | up_wall

Отслеживание направления робота

Из примеров выше видно, что направление робота — это важная часть алгоритма составления карты. Для описания здесь можно использовать одну целочисленную переменную, которая принимает четыре значения: 0 (вверх), 1 (вправо), 2 (вниз) и 3 (влево). Для управления полезно иметь функцию, которая изменяет направление робота в зависимости от его действий.

# текущее направление робота
direction = 0 
# значения для направлений робота
up = 0 
right = 1
down = 2
left = 3

func set_direction(action):
# direction – текущее направление робота (глобальная переменная)
# action – обозначение для движений робота, например "left” (L), "forward” (F), 
# "right” (R), "around”(A)

	if direction == up:
 		# при action == F ничего не изменяется
		if action == R:
			direction = right
		if action == A:
			direction = down
		if action == L:
			direction = left

	if direction == right:
 		# при action == F ничего не изменяется
		if action == R:
			direction = down
		if action == A:
			direction = left
		if action == L:
			direction = up

	if direction == down:
		…

	if direction == left:
		…

Попробуйте прописать направление вниз и влево самостоятельно. А затем проверьте себя в спойлере ниже.

Код для направления робота

func set_direction(action):
# direction – текущее направление робота (глобальная переменная)
# action – обозначение для движений робота, например "left” (L), "forward” (F), 
# "right” (R), "around”(A)
	if direction == up:
		if action == R:
			direction = right
		if action == A:
			direction = down
		if action == L:
			direction = left
	if direction == right:
		if action == R:
			direction = down
		if action == A:
			direction = left
		if action == L:
			direction = up
	if direction == down:
		if action == R:
			direction = left
		if action == A:
			direction = up
		if action == L:
			direction = right
	  if direction == left:
		if action == R:
			direction = up
		if action == A:
			direction = right
		if action == L:
			direction = down

Проверка стен на карте

Как убедиться, есть ли на карте какая-либо известная нам стенка? Проверить нужный бит в ячейке мы можем через операцию битового И. Возьмем для примера ячейку с координатами (4, 2):

Предположим, что в ней робот смотрит влево (direction = 3):

В этой ячейке были определены три стенки: верхняя, нижняя и левая. Пусть алгоритм робота велит ему повернуть направо. Как понять, есть ли стенка справа от робота?

Мы знаем, что для робота правая стенка является верхней в ячейке, поэтому берем соответствующую маску up_wall. Применим операцию И:

0b00001011 & 0b00001000 = 0b00001000

Чтобы проверить метод, проигнорируем положение робота и подставим маску right_wall:

0b00001011 & 0b00000100 = 0b00000000

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

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

func check_left_wall():
# y – текущая позиция робота по вертикали (глобальная переменная)
# x – текущая позиция робота по горизонтали (глобальная переменная)
# direction – текущее направление робота (глобальная переменная)
# walls - массив значений стенок (глобальная переменная)
 	is_wall = false
	if direction == up:
		is_wall = (walls[y][x] & left_wall) == left_wall
	if direction == right:
		is_wall = (walls[y][x] & up_wall) == up_wall
	if direction == down:
		is_wall = (walls[y][x] & right_wall) == right_wall
	if direction == left:
		is_wall = (walls[y][x] & down_wall) == down_wall
	return is_wall

func check_front_wall():
	…

func check_right_wall():
	…

Функции для передней стенки и для правой стенки предлагаю реализовать самостоятельно. Ответ в спойлере.

Проверка стенок на карте

func check_front_wall():
	is_wall = false
	if direction == up:
		is_wall = (walls[y][x] & up_wall) == up_wall
	if direction == right:
		is_wall = (walls[y][x] & right_wall) == right_wall
	if direction == down:
		is_wall = (walls[y][x] & down_wall) == down_wall
	if direction == left:
		is_wall = (walls[y][x] & left_wall) == left_wall
	return is_wall

func check_right_wall():
	is_wall = false
	if direction == up:
		is_wall = (walls[y][x] & right_wall) == right_wall
	if direction == right:
		is_wall = (walls[y][x] & down_wall) == down_wall
	if direction == down:
		is_wall = (walls[y][x] & left_wall) == left_wall
	if direction == left:
		is_wall = (walls[y][x] & up_wall) == up_wall
	return is_wall

Передвижение робота

С картой разобрались, осталось научить робота передвигаться относительно лабиринта, который построен в его памяти. Как изменяется направление, мы уже рассмотрели. Теперь будем изменять координаты робота — научим реагировать на команду «пройди одну ячейку вперед».

Это реализуется через простые операции с вычитанием и прибавлением индексов. Когда робот движется по карте наверх, каждую ячейку его координата y уменьшается на 1, при движении вправо увеличивается на 1 координата х и наоборот. Вот как будет выглядеть наша функция:

func change_position():
# y – текущая позиция робота по вертикали (глобальная переменная)
# x – текущая позиция робота по горизонтали (глобальная переменная)
# direction – текущее направление робота (глобальная переменная)
	if direction == up:
		y = y – 1
	if direction == right:
		x = x + 1
	if direction == down:
 		y = y + 1 
	if direction == left:
		x = x - 1

Пример алгоритма исследования лабиринта

Теперь у нас есть все компоненты, чтобы построить карту реального лабиринта и затем проверять стенки при помощи этой карты. Рассмотрим простой алгоритм исследования лабиринта на основе правила левой руки. Это значит, что робот сначала пытается проехать одну ячейку влево, если там нет стенки. В противном случае он пробует другие направления, последовательно поворачивая на 90 градусов по часовой стрелке. Если за три такие попытки он определяет рядом стенку, то разворачивается и едет обратно.

Предположим, что наш робот физически умеет:

• проезжать одну клетку вперед при помощи функции forward (),

• поворачивать налево при помощи turn_left (),

• поворачивать направо через turn_right (),

• считывать показания с левого, переднего и правого датчика при помощи get_left_wall (), get_front_wall () и get_right_wall () соответственно.

Тогда наш алгоритм будет выглядеть так:

func left_hand_algo():
	while true: 
		left_wall = get_left_wall()
		front_wall = get_front_wall()
		right_wall = get_right_wall()
 		set_walls(left_wall, front_wall, right_wall)

		if not check_left_wall():
			turn_left()
			set_direction("L”)
			forward()
			change_position()
		else if not check_front_wall():
			forward()
			change_position()
		else if not check_right_wall():
			turn_right()
			set_direction("R”)
			forward()
			change_position()
		else:
			turn_right()
 			turn_right()
			set_direction("A”) 
			forward()
			change_position()

Этот алгоритм позволит построить полную карту лабиринта, если этот лабиринт содержит лишь один путь от старта до финиша, чтобы в дальнейшем искать по этой карте короткий путь от начальной точки до конечной. Использование функций check_left_wall () и ей подобных здесь избыточно, так как мы можем просто взять только что полученные данные с датчиков. Я вставил их здесь исключительно ради демонстрации работы рассмотренных алгоритмов. В реальных соревнованиях Micromouse путей много и алгоритм несколько иной.

Упрощение функций

В предыдущих разделах мы научили робота строить карту. Далее будет немного материала для «продвинутых пользователей».

Сейчас все функции содержат очень много условий, которые могут быть упрощены при помощи модульной арифметики. В большинстве случаев нам потребуется модульная арифметика по модулю 4, так как у нас четыре направления.

На примере модуля 4 и будет идти дальнейшее объяснение. У нас есть переменная, которая отвечает за направление, и одно фиксированное значение для каждого из четырех направлений. Посмотрим внимательнее, как изменяются эти значения при поворотах.

Начнем с поворотов направо. Робот при старте смотрит наверх, то есть direction = 0. Далее робот повернет направо — мы фиксируем за этим направлением значение 1. После этого робот еще раз повернет направо: direction = 2. Несложно заметить, что после каждого поворота направо мы увеличиваем значение direction на 1.

Но что делать, если робот смотрел налево (direction = 3) и затем повернул направо? Мы получаем значение 4, хотя хотелось бы иметь 0. Что, если робот продолжит поворачивать направо? Мы будем получать значения 5, 6, 7 и так далее. Посмотрим, как эти значения сопоставляются с теми, которые нам нужны. 

Необходимые значения — это не что иное, как остаток от деления полученных значений на 4. Если робот смотрел налево и повернул направо, то мы получили direction = 4. Остаток от деления 4 на 4 равен 0, что соответствует направлению вверх. В большинстве языков программирования для получения остатка используется оператор деления по модулю, вызываемый через символ %. В итоге поворот направо мы можем записать так:  

direction = (direction + 1) % 4

В таком случае нам даже не нужно знать, в какую сторону смотрел робот до поворота.

Теперь о поворотах налево. Представим, что робот смотрит влево (direction = 3). При повороте налево мы получим значение 2, затем 1, затем 0. После значения 0 пойдут отрицательные числа, и магия с остатком от деления здесь с ходу не заработает. Попробуем построить таблицу для первых трех отрицательных чисел. Этого достаточно, так как мы уже выяснили, что у нас циклически повторяется только 4 значения.

Можно заметить, что нужные нам значения отличаются ровно на наш модуль (4). Запишем формулу поворота налево:

direction = (direction – 1 + 4) % 4

Если робот находится в позиции 0 («смотри вверх») и поворачивает налево, мы получаем direction = (0 — 1 + 4) % 4 = 3, что соответствует значению «смотрит налево». Расчет значений для других направлений тоже никак не изменился, поскольку мы прибавляем значение модуля (4% 4 = 0).

Упростим выражение:

direction = (direction + 3) % 4

Интересно, что для поворота направо и налево мы используем глобальные значения этих состояний — 1 и 3 соответственно. Вернемся к функции изменения направления и перепишем ее:

func set_direction(turn_direction):
# direction – текущее направление робота (глобальная переменная)
# turn_direction – установленные значения для направлений: 0 – прямо, 1 – вправо, 
# 2 – вниз, 3 - влево
 	direction = (direction + turn_direction) % 4

При помощи модульной арифметики у нас получилось сократить количество строк этой функции с 28 до одной. Другие рассмотренные функции (set_walls, check_left_wall, check_front_wall и check_right_wall) также могут быть упрощены, с помощью модульной арифметики и нескольких дополнительных массивов, например массива всех масок:

masks[4] = [up_wall, right_wall, down_wall, left_wall]

Оптимизация выставления стенок

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

Когда мы выставляем значение правой стенки в текущей ячейке, мы можем выставить и значение левой стенки в соседней. То есть после первого обновления стенки у нас поменяется не только значение ячейки (4, 0), но и значение ячейки (4, 1). Учтем только, что в соседней ячейке нам нужно будет выставить стенку в противоположном направлении, то есть позиция бита будет отличаться на 2. В итоге после первого обновления карта будет выглядеть следующим образом:

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

© Habrahabr.ru