Готовимся к Micromouse: как роботу найти короткий путь к цели26.12.2024 13:30

Привет, Хабр! Я Денис Логашов, инженер-исследователь отдела автоматической обработки результатов моделирования и визуализации YADRO. В этой статье я расскажу о решении основной задачи в соревновании Micromouse: как роботу пользоваться сохраненной картой лабиринта для передвижения по нему и поиска кратчайшего пути. Это продолжение предыдущего материала, где мы учили робота карту составлять. 

Кратко опишу соревнование роботов Micromouse для тех, кто не читал первую часть. Есть квадратный лабиринт, поделенный на квадратные ячейки. В нем — фиксированные точки старта и финиша робота. Последняя — это, как правило, квадрат 2×2 ячейки с одним или несколькими входами. Задача робота-«микромыши» — как можно быстрее проехать от старта до финиша. На решение дается ограниченное время, за которое «мышь» может проходить лабиринт неограниченное число раз. Обновлять микрокод робота после демонстрации схемы лабиринта запрещено.

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

• основы программирования: переменные, условия, циклы, функции, битовые операции,

• основы модульной арифметики: взятие числа по модулю,

• начальное понимание объектов в программировании.

В спойлере — функции и переменные, которые мы использовали в прошлой части.

Исходные данные

# Переменные для направлений
up = 0
right = 1
down = 2
left = 3

# Переменные для отслеживания позиции робота 
# Принимают значения от 0 до 4 (для примера работаем с лабиринтом 5 х 5)
# Считаем, что робот изначально находится в нижнем левом углу (4, 0)
y = 4 
x = 0

# Переменная для отслеживания направления робота принимает значения от 0 до 3, что соответствует переменным для направлений
direction = 0

# Маски для каждой из стен в ячейке
up_wall = 0b00001000
right_wall = 0b00000100
down_wall = 0b00000010
left_wall = 0b00000001

# массив стен в лабиринте 5х5
walls[5][5]

# функция, которая изменяет направление робота внутри программы
func set_direction(turn_direction)

# функция, которая изменяет позицию робота внутри программы
func change_position()

# функция, которая выставляет значения для стен в текущей позиции робота
func set_walls(left_wall, front_wall, right_wall)

# функции, которые проверяют стенки относительно текущей позиции робота
func check_left_wall()
func check_front_wall()
func check_right_wall()

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

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

В прошлой статье с помощью модульной арифметики мы сократили функцию для изменения направления робота внутри программы:

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

Теперь посмотрим на функцию выставления стен на карте, которую прописали там же:

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
	…

Здесь с помощью модульной арифметики мы избавляемся от значительной части условий. Введем вспомогательный массив со всеми масками. Они записаны в определенном порядке: каждая соответствует направлениям, которые мы определили ранее.

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

Посмотрим на соответствие между новым массивом масок и текущим направлением робота. Когда робот смотрит вверх (direction = 0), передний сенсор соответствует верхней стенке в ячейке, а маска этой стенки находится в массиве под индексом 0. Когда смотрит вниз (direction = 2), передний датчик робота соответствует нижней стенке с маской под индексом 2. Нетрудно догадаться, к чему я веду: чтобы записать показания с переднего датчика, необходимо выбрать маску с индексом текущего направления.

front_mask = wall_masks[direction]

Мы уже рассматривали, как направления соотносятся с поворотами робота. С датчиками все так же. Чтобы получить маску для левого датчика, необходим индекс (direction + left) % 4, для правого — (direction + right) % 4.

Обновляя стенки в одной ячейке, мы можем обновлять эти стенки и в соседних ячейках. При этом для соседних стенок маска должна быть противоположной: у верхней стенки соответствующая стенка в соседней ячейке — нижняя, у левой — правая. В массиве наших масок индекс таких пар отличается на 2.

Помимо маски, у нас меняется и координата, которой нам нужно задать значения. Мы уже знаем, как меняются координаты в зависимости от направления робота, это мы рассматривали в функции change_position():  

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

На основе этой функции мы можем сформировать массив изменения координат. Первый индекс в этом массиве будет соответствовать направлению робота, а второй — координатам (y, x):

direction_changes[4][2] = [
	[-1, 0],
	[0, 1],
	[1, 0],
	[0, -1]
]

Мы можем усовершенствовать нашу функцию изменения позиции с помощью нового массива:

func change_position():
	# y – текущая позиция робота по вертикали (глобальная переменная)
	# x – текущая позиция робота по горизонтали (глобальная переменная)
	# direction – текущее направление робота (глобальная переменная)
	# direction_changes – массив изменения позиции(глобальная переменная)
	
	y = y + direction_changes[direction][0]
	x = x + direction_changes[direction][1]

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

func set_wall(wall_direction):
	# y — текущая позиция робота по вертикали (глобальная переменная)
	# x — текущая позиция робота по горизонтали (глобальная переменная)
	# direction — текущее направление робота (глобальная переменная)
	# walls — массив значений стенок (глобальная переменная)
	# wall_masks — массив масок стенок (глобальная переменная)
	# direction_changes — массив изменения позиции(глобальная переменная)
	# wall_direction — направление стенки относительно мыши
		
	wall_idx = (direction + wall_direction) % 4
	walls[y][x] = walls[y][x] | wall_masks[wall_idx]
	# wall_idx также соответствует направлению соседней ячейки, поэтому
	# мы можем пользоваться этим значением в массиве direction_changes
	neighbour_y = y + direction_changes[wall_idx][0]
	neighbour_x = x + direction_changes[wall_idx][1]
	neighbour_idx = (wall_idx + 2) % 4
	walls[neighbour_y][neighbour_x] = walls[neighbour_y][neighbour_x] | wall_masks[neighbour_idx]

В текущем виде эта функция неидеальна, так как мы используем в ней значения из direction_changes, но никак не проверяем результат. Можем столкнуться с тем, что потребуется выставить значения для стенок с индексами (–1, x) — очевидно, это неправильно. Как избежать таких ситуаций, рассмотрим далее.

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

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

up_wall_visited = 0b10000000
right_wall_visited = 0b01000000
down_wall_visited = 0b00100000
left_wall_visited = 0b00010000

wall_masks_visited[4] = [
	up_wall_visited ,
	right_wall_visited ,
	down_wall_visited ,
	left_wall_visited 
]

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

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

Теперь еще раз взглянем на измененную функцию set_wall для выставления стен. Ее главная проблема в том, что мы могли получить невалидные индексы –1 или 5. Получить мы их могли лишь на крайних ячейках, когда проверяли граничные стенки. Поскольку мы условились не обновлять значения в ячейках, где уже побывали, граничные стены не должны выставляться повторно, а значит, и на невалидные индексы мы натыкаться не будем.

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

func set_wall(wall_direction):
	# y — текущая позиция робота по вертикали (глобальная переменная)
	# x — текущая позиция робота по горизонтали (глобальная переменная)
	# direction — текущее направление робота (глобальная переменная)
	# walls — массив значений стенок (глобальная переменная)
	# wall_masks — массив масок стенок (глобальная переменная)
	# direction_changes — массив изменения позиции(глобальная переменная)
	# wall_direction — направление стенки относительно мыши
	# neigh — сокр. соседний

	wall_idx = (direction + wall_direction) % 4
	visited_mask = wall_masks_visited[wall_idx] 
	wall_checked = walls[y][x] & visited_mask == visited_mask 
	if not wall_checked:
		walls[y][x] = walls[y][x] | wall_masks[wall_idx] | wall_masks_visited[wall_idx]

		neigh_idx = (wall_idx + 2) % 4
		neigh_y = y + direction_changes[wall_idx][0]
		neigh_x = x + direction_changes[wall_idx][1]
		walls[neigh_y][neigh_x] = walls[neigh_y][neigh_x] | wall_masks[neigh_idx] | wall_masks_visited[neigh_idx]

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

func check_wall(wall_direction, y_pos, x_pos):
		# walls — массив значений стенок (глобальная переменная)
		# wall_masks — массив масок стенок (глобальная переменная)
		# wall_direction — направление стенки относительно лабиринта
		# y_pos — координата ячейки по вертикали
		# x_pos — координата ячейки по горизонтали

		# нормируем значение, если мы вдруг забыли это сделать перед вызовом
		wall_idx = wall_direction % 4
		wall_mask = wall_masks[wall_idx]
		return walls[y_pos][x_pos] & wall_mask == wall_mask

Сведем то, что реализовали в этом разделе:

# массив масок стен
wall_masks[4] = [up_wall, right_wall, down_wall, left_wall]

# маски просмотренных стен
up_wall_visited = 0b10000000
right_wall_visited = 0b01000000
down_wall_visited = 0b00100000
left_wall_visited = 0b00010000

# массив масок просмотренных стен
wall_masks_visited[4] = [
up_wall_visited ,
right_wall_visited ,
down_wall_visited ,
left_wall_visited 
]

# массив изменений координат для соседних ячеек
direction_changes[4][2] = [
	[-1, 0],
	[0, 1],
	[1, 0],
	[0, -1]
]

# функция выставления стенки по направлению относительно мыши
func set_wall(wall_direction)

# функция проверки стенки по направлению относительно лабиринта
func check_wall(wall_direction)

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

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

Представим, что старт находится в ячейке (4, 0), а финиш в ячейке (0, 2). Изначально для робота карта имеет только внешние стены.

Как нашему роботу решить, куда ехать? В этом нам поможет алгоритм FloodFill, или заливка. С его помощью мы будем «заливать» карту, чтобы понять, где находится ближайший путь к финишу. Используя данные заливки, робот будет стараться стать ближе к финишу после каждого движения.

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

В чем суть заливки? Представим, что мы выливаем воду в ячейку финиша, и затем она начинает разливаться дальше по лабиринту. Чем ближе ячейка к финишу с учетом обхода стенок, тем раньше там окажется вода.

Пример заливки

По этому принципу мы и будем заполнять нашу карту. Нам понадобится вспомогательный массив с расстояниями от каждой ячейки до финиша. Назовем его массивом весов (weights). Расстояния в этом массиве будут считаться в ячейках. На старте, когда робот еще не знает, как расположены стены, кратчайший путь выглядит так:

В стартовой ячейке нам нужно указать число 6 — именно столько ячеек роботу нужно проехать до финиша. Когда он переместится на одну ячейку вверх, останется 5 ячеек. В финишной ячейке искомое расстояние — 0. Вот как должны быть расположены числа в нашем вспомогательном массиве:

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

Для каждого из этих соседей мы установим значение равное 0 + 1, то есть 1.

Теперь нужно проверить соседей для только что выставленных ячеек. Чтобы отслеживать ячейки, для которых нужно выставлять соседей, потребуется структура данных «очередь» (queue).

Эта конструкция работает по принципу «первый зашел — первый вышел». Мы будем добавлять ячейки в эту структуру в том же порядке, в котором обновляли значения. Затем брать из этой структуры первый элемент в списке и обновлять значения для его соседей. Так мы обеспечим равномерность заполнения карты.

В работе с очередью часто используют методы push () для добавления элемента в конец очереди и pop (), чтобы вытащить из очереди первый элемент. Пример работы такой конструкции:

q = Queue()  # создали очередь
q.push(2)	# добавили элемент 2
q.push(4)
q.push(1)

value = q.pop() 	# value = 2, теперь в очереди находятся элементы 4, 1
value = q.pop()	# value = 4, теперь в очереди только элемент 1
value = q.pop()	# value = 1, теперь очередь пуста.

Чтобы проверять, есть ли элементы в очереди, используют метод empty (). Он возвращает true, если в очереди нет элементов.

Вернемся к примеру лабиринта. В начале алгоритма у нас была пустая очередь, мы выставили значение 0 в ячейке (0, 2) и поместили ее в очередь.

Дальше начинается наш алгоритм. Пока очередь не пуста, мы:

• берем из нее ячейку,

• выставляем значение «вес в текущей ячейке +1» для всех доступных соседей, у которых еще не было выставлено значение,

• добавляем в очередь каждого обновленного соседа. 

Вытаскиваем ячейку (0, 2) из очереди и обновляем каждого соседа, как условились выше. После этого помещаем соседей в очередь:

Берем ячейку (0, 1), выбираем всех доступных соседей, которых еще не обновили. Обновляем расстояние для них и добавляем этих соседей в очередь.

Теперь очередь имеет следующий вид:

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

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

Код функции для заполнения карты расстояний лабиринта:

# координаты финиша
finish_y = 0
finish_x = 2
# массив расстояний
weights[5][5]

# инициализируем его значениями -1
for i = 0, i < 5, i++ 
	for j = 0, j < 5, j++ 
	weights[i][j] = -1

func floodfill():
	# finish_y  — позиция финиша по вертикали (глобальная переменная)
	# finish_x  — позиция финиша по горизонтали (глобальная переменная)
	# weights — массив весов (глобальная переменная)
	# direction_changes — массив изменения позиции(глобальная переменная)
	
	# заполним массив значениями –1, это позволит проверить какие значения были
	# выставлены, а какие нет
	for i = 0, i < 5, i++ 
		for j = 0, j < 5, j++ 
			weights[i][j] = -1
	# создадим очередь для ячеек
	q = Queue()
	# выставим значения для финишной ячейки и добавим ее в очередь
	weights[finish_y][finish_x] = 0
	q.push((finish_y, finish_x))
	while not q.empty():
		# вытаскиваем первую ячейку в очереди
		cur_y, cur_x = q.pop()
		# проверяем каждого доступного соседа
		for dir = 0, dir < 4, dir++:
			# проверяем стену
			if not check_wall(dir, cur_y, cur_x):
				neighbour_y = cur_y + direction_changes[dir][0]
				neighbour_x = cur_x + direction_changes[dir][1]
				# проверяем, было ли уже выставлено значение
				if weights[neighbour_y][neighbour_x] == -1:
					weights[neighbour_y][neighbour_x] = weights[cur_y][cur_x] + 1
				q.push((neighbour_y, neighbour_x))

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

Алгоритм поиска пути

Алгоритм поиска пути довольно прост: мы проверяем доступных соседей в ячейке и выбираем соседа с наименьшим значением. Оно всегда на 1 меньше, чем расстояние в текущей ячейке. Как только мы находим первое такое значение, то отправляем робота в эту ячейку.

Напишем функцию, которая будет возвращать направление, куда нам необходимо ехать, относительно лабиринта:

func find_path():
	# y — текущая позиция робота по вертикали (глобальная переменная)
	# x — текущая позиция робота по горизонтали (глобальная переменная)
	# weights — массив весов (глобальная переменная)
	# direction_changes — массив изменения позиции (глобальная переменная)

	for dir = 0, dir < 4, dir++:
		# проверяем стену
		if not check_wall(dir, y, x):
			neighbour_y = y + direction_changes[dir][0]
			neighbour_x = x + direction_changes[dir][1]
		# проверяем значение в соседней ячейке
			if weights[neighbour_y][neighbour_x] < weights[y][x]:
				return dir

Однако тут появляется несостыковка. Функция возвращает направление относительно лабиринта, а не робота, которого нужно поворачивать. Мы знаем текущее направление робота, из переменной directionи направление, которое дает нам функция:

move_direction = find_path()

Мы можем проверить каждое из направлений и посмотреть, как должна измениться переменная direction, чтобы соответствовать move_direction.Сейчас мы предполагаем, что робот физически может выполнять следующие действия: проехать одну ячейку вперед через forward(), повернуть на 90 градусов направо через turn_right() и налево через turn_left(). С учетом этого наша функция будет выглядеть так:

func move_robot():
# direction — текущее направление робота (глобальная переменная)

	move_direction = find_path()

	if (direction + right) % 4 == move_direction:
		turn_right()
		set_direction(right)
	else if (direction + down) % 4 == move_direction:
		turn_right()
		turn_right()
		set_direction(down)
	else if (direction + left) % 4 == move_direction:
		turn_left()
		set_direction(left)
	
	# всегда проезжаем одну ячейку 
	forward()
	change_position()

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

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

Начнем с простого примера использования всех рассмотренных нами подходов. На старте наш робот знает лишь, что:

• лабиринт имеет размер 5×5,

• робот находится в позиции (4, 0), а финиш находится в позиции (0, 2).

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

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

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

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

Следующая доступная ячейка для робота — (3, 1). Здесь мы находим еще одну стену, которая опять не влияет на карту весов.

Следующая ближайшая к финишу ячейка, доступная для перехода — (2, 1).

Карта весов пока не изменилась, дальше робот может перейти в ячейку (2, 2). Здесь открывается сразу две новых стенки. На примере (2, 2) еще раз поэтапно взглянем на работу алгоритма заливки:

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

С этой картой робот может выбрать другой, более быстрый путь:

Между ячейками (1, 0) и (1, 1) есть еще одна стенка, о которой робот узнает, если пройдет этот путь. После этого прохода карта весов будет выглядеть так:

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

Хотя потребовалось два прохождения, чтобы проверить разные маршруты, наш подход все равно будет давать большую экономию по времени, особенно в более крупном лабиринте 16×16.

Воплощаем логику в коде

Опишем алгоритм из предыдущего раздела в виде функции. Напомню, что при помощи функций get_left_wall (), get_front_wall () и get_right_wall () робот считывает показания с датчиков, чтобы определить наличие стен. 

func solve_maze():
	# y — текущая позиция робота по вертикали (глобальная переменная)
	# x — текущая позиция робота по горизонтали (глобальная переменная)
	# weights — массив весов (глобальная переменная)

	while weights[y][x] != 0:
		is_left_wall = get_left_wall()
		is_front_wall = get_front_wall()
		is_right_wall = get_right_wall()
		# устанавливаем стенки относительно мыши
		if is_left_wall :
			set_wall(left)
		if is_front_wall :
			set_wall(up)
		if is_right_wall :
			set_wall(right)
	# обновляем карту весов после того, как обновили стены
	floodfill()
	# передвигаем робота в следующую ячейку
	move_robot()

Так наш робот сможет доехать от старта до финиша, при этом построив часть карты лабиринта и собрав информацию для быстрых заездов. Сводим все воедино.

Полный код робота Micromouse

# Переменные для направлений
up = 0
right = 1
down = 2
left = 3

# Переменные для отслеживания позиции робота 
y = 4
x = 0

# 
finish_y = 0
finish_x = 2

# Переменная для отслеживания направления робота
direction = 0

# Маски для каждой из стен в ячейке
up_wall = 0b00001000
right_wall = 0b00000100
down_wall = 0b00000010
left_wall = 0b00000001

# Маски просмотренных стен
up_wall_visited = 0b10000000
right_wall_visited = 0b01000000
down_wall_visited = 0b00100000
left_wall_visited = 0b00010000

# Массив стен, если рассматриваем лабиринт 5х5
walls[5][5]
walls[5][5] 
for i = 0, i < 5, i++ 
  for j = 0, j < 5, j++ 
    walls[i][j] = 0

# Массив расстояний
weights[5][5]
# инициализируем его значениями -1
for i = 0, i < 5, i++ 
  for j = 0, j < 5, j++ 
    weights[i][j] = -1


wall_masks = [up_wall, right_wall, down_wall, left_wall]
wall_masks_visited = [up_wall_visited, right_wall_visited, down_wall_visited, left_wall_visited]

direction_changes[4][2] = [
	[-1, 0],
	[0, 1],
	[1, 0],
	[0, -1]
]

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

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


func change_position():
	# y — текущая позиция робота по вертикали (глобальная переменная)
	# x — текущая позиция робота по горизонтали (глобальная переменная)
	# direction — текущее направление робота (глобальная переменная)
	# direction_changes — массив изменения позиции(глобальная переменная)

	y = y + direction_changes[direction][0]
	x = x + direction_changes[direction][1]


func set_wall(wall_direction):
	# y — текущая позиция робота по вертикали (глобальная переменная)
	# x — текущая позиция робота по горизонтали (глобальная переменная)
	# direction — текущее направление робота (глобальная переменная)
	# walls — массив значений стенок (глобальная переменная)
	# wall_masks — массив масок стенок (глобальная переменная)
	# direction_changes — массив изменения позиции(глобальная переменная)
	# wall_direction — направление стенки относительно мыши

	wall_idx = (direction + wall_direction) % 4
	visited_mask = wall_masks_visited[wall_idx] 
	wall_checked = walls[y][x] & visited_mask == visited_mask 
	if not wall_checked:
		walls[y][x] = walls[y][x] | wall_masks[wall_idx] | wall_masks_visited[wall_idx]

		neigh_idx = (wall_idx + 2) % 4
		neigh_y = y + direction_changes[wall_idx][0]
		neigh_x = x + direction_changes[wall_idx][1]
		walls[neigh_y][neigh_x] = walls[neigh_y][neigh_x] | wall_masks[neigh_idx] | wall_masks_visited[neigh_idx]


func check_wall(wall_direction, y_pos, x_pos):
	# walls — массив значений стенок (глобальная переменная)
	# wall_masks — массив масок стенок (глобальная переменная)
	# wall_direction — направление стенки относительно лабиринта
	# y_pos — координата ячейки по вертикали
	# x_pos — координата ячейки по горизонтали

	# нормируем значение, если мы вдруг забыли это сделать перед вызовом
	wall_idx = wall_direction % 4
	wall_mask = wall_masks[wall_idx]
	return walls[y_pos][x_pos] & wall_mask == wall_mask

func floodfill():
	# finish_y — позиция финиша по вертикали (глобальная переменная)
	# finish_x — позиция финиша по горизонтали (глобальная переменная)
	# weights — массив весов (глобальная переменная)
	# direction_changes — массив изменения позиции(глобальная переменная)
	
	# заполним массив значениями -1, это позволит проверить какие значения были
	# выставлены, а какие нет
	for i = 0, i < 5, i++ 
	  for j = 0, j < 5, j++ 
	    weights[i][j] = -1
	# создадим очередь для ячеек
	q = Queue()
	# выставим значения для финишной ячейки и добавим ее в очередь
	weights[finish_y ][finish_x ] = 0
	q.push((finish_y, finish_x))
	while not q.empty():
		# вытаскиваем первую ячейку в очереди
		cur_y, cur_x = q.pop()
		# проверяем каждого доступного соседа
		for dir = 0, dir < 4, dir++:
			# проверяем стену
			if not check_wall(dir, cur_y, cur_x ):
				neighbour_y = cur_y + direction_changes[dir][0]
				neighbour_x = cur_x + direction_changes[dir][1]
				# проверяем, было ли уже выставлено значение
				if weights[neighbour_y][neighbour_x] == -1:
					weights[neighbour_y][neighbour_x] = weights[cur_y][cur_x] + 1
					q.push((neighbour_y, neighbour_x))


func find_path():
	# y — текущая позиция робота по вертикали (глобальная переменная)
	# x — текущая позиция робота по горизонтали (глобальная переменная)
	# weights — массив весов (глобальная переменная)
	# direction_changes — массив изменения позиции (глобальная переменная)

	for dir = 0, dir < 4, dir++:
		# проверяем стену
		if not check_wall(dir, y, x):
			neighbour_y = y + direction_changes[dir][0]
			neighbour_x = x + direction_changes[dir][1]
			# проверяем значение в соседней ячейке
			if weights[neighbour_y][neighbour_x] < weights[y][x]:
				return dir


func move_robot():
	# direction — текущее направление робота (глобальная переменная)

	move_direction = find_path()

	if (direction + right) % 4 == move_direction:
		turn_right()
		set_direction(right)
	else if (direction + down) % 4 == move_direction:
		turn_right()
		turn_right()
		set_direction(down)
	else if (direction + left) % 4 == move_direction:
		turn_left()
		set_direction(left)
	
	# всегда проезжаем одну ячейку 
	forward()
	change_position()

func solve_maze():
	# y — текущая позиция робота по вертикали (глобальная переменная)
	# x — текущая позиция робота по горизонтали (глобальная переменная)
	# weights — массив весов (глобальная переменная)

	while weights[y][x] != 0:
		is_left_wall = get_left_wall()
		is_front_wall = get_front_wall()
		is_right_wall = get_right_wall()
		# устанавливаем стенки относительно мыши
		if is_left_wall :
			set_wall(left)
		if is_front_wall :
			set_wall(up)
		if is_right_wall :
			set_wall(right)
		# обновляем карту весов после того, как обновили стены
		floodfill()
		# передвигаем робота в следующую ячейку
		move_robot()


# вызываем основную функцию для прохождения лабиринта
solve_maze()

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

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

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

© Habrahabr.ru