Планирование пути A* по сетке занятости

Планировщик робота отвечает на другой вопрос, чем регулятор обхода препятствий. Планировщик спрашивает: «Какие свободные клетки соединяют начало с целью с наименьшей стоимостью?» Регулятор спрашивает: «Какое движение безопасно прямо сейчас?» A* — это алгоритм поиска по графу, который объединяет уже понесённую стоимость с оценкой оставшейся стоимости, поэтому его разумно использовать как первый планировщик для двумерной сетки занятости.

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

Превратите карту в граф

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

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

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

Поймите три оценки A*

Для каждого узла n A* отслеживает:

• g(n): известную стоимость от начала до n;
• h(n): эвристическую оценку от n до цели;
• f(n) = g(n) + h(n): оценку общей стоимости маршрута через n.

Открытое множество содержит обнаруженные узлы, которые ещё могут быть раскрыты. На каждом шаге удаляйте узел с наименьшим f. Если это цель, восстановите маршрут по ссылкам на родителей к началу. Иначе пересчитайте стоимость каждого допустимого перехода: если новое g дешевле записанной стоимости, обновите стоимость и родителя.

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

h = |goalX - x| + |goalY - y|

При движении в восьми направлениях с диагональной стоимостью √2 используйте октильное расстояние. Евклидово расстояние в открытом пространстве также не превосходит истинной стоимости пути. Завышение может ускорить поиск, но лишает гарантии оптимального маршрута.

Реализуйте поиск с очередью с приоритетом

Стандартной библиотеки Python достаточно для компактной эталонной реализации:

from heapq import heappop, heappush

def astar(start, goal, neighbors, heuristic):
    frontier = [(heuristic(start, goal), 0, start)]
    came_from = {start: None}
    cost = {start: 0}

    while frontier:
        _, current_cost, current = heappop(frontier)
        if current_cost != cost.get(current):
            continue  # stale queue entry
        if current == goal:
            break

        for next_cell, step_cost in neighbors(current):
            new_cost = current_cost + step_cost
            if new_cost < cost.get(next_cell, float("inf")):
                cost[next_cell] = new_cost
                came_from[next_cell] = current
                heappush(frontier, (
                    new_cost + heuristic(next_cell, goal),
                    new_cost,
                    next_cell,
                ))

    if goal not in came_from:
        return None

    path, cell = [], goal
    while cell is not None:
        path.append(cell)
        cell = came_from[cell]
    return list(reversed(path))

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

Запретите диагональное срезание углов

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

Для перехода из (x, y) в (x+1, y+1) проверьте, что (x+1, y) и (x, y+1) доступны для прохода. Затем примените расширение зоны, занимаемой роботом, как отдельный слой безопасности.

Расширьте препятствия для реального робота

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

Жёсткий радиус расширения помечает ближайшие клетки как заблокированные. Градиентное поле стоимости добавляет большую стоимость возле стен, побуждая держаться дальше от них, не объявляя каждую близкую клетку непроходимой. Выбранный радиус должен учитывать выступ корпуса, неопределённость локализации и ошибку отслеживания.

Разрешение карты меняет число клеток: inflationCells = ceil(inflationMetres / resolution). Явно проверьте преобразование. Радиус 0.20 m при 0.05 m/cell составляет четыре клетки, а не 0.20 клетки.

Добавьте полезные стоимости прохода

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

Не добавляйте штраф за поворот, не включив ориентацию в состояние. Если прибытие в одну и ту же клетку лицом на восток и на север даёт разную будущую стоимость, (x, y) больше не описывает узел; используйте (x, y, heading) или примите, что штраф является только предпочтением постобработки.

Систематически проверяйте планировщик

Начните с крошечных карт, ответ на которых очевиден:

1. Пустая сетка: путь должен приближаться к оптимуму по прямой.
2. Одна стена с одним проходом: маршрут обязан пройти через проход.
3. Заблокированная цель: вернуть отсутствие пути без зацикливания.
4. Начало равно цели: вернуть единственную начальную клетку.
5. Препятствия, соприкасающиеся по диагонали: срезание угла запрещено.
6. Узкий коридор меньше расширенной зоны робота: пути нет.

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

Диагностируйте типичные сбои планировщика

• Маршрут пересекает стену: неверна индексация сетки, порядок строк и столбцов или проверка диагонали.
• Маршрут срезает углы: препятствия не расширены или разрешено диагональное срезание углов.
• A возвращает более длинный маршрут, чем ожидалось:* не совпадают единицы эвристики и стоимости шага либо принимаются устаревшие стоимости.
• Поиск потребляет слишком много памяти: чрезмерно высокое разрешение сетки или дубликаты в очереди никогда не отбрасываются.
• Робот не может пройти корректный путь: план игнорирует размер робота, радиус поворота, ограничения регулятора или ошибку локализации.
• Путь дрожит после каждого обновления датчиков: глобальное планирование перезапускается из-за небольших изменений без гистерезиса стоимости или политики перепланирования.

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