Планирование пути алгоритмом A*

Как робот находит кратчайший проход через лабиринт препятствий — быстрее, чем перебирая всё подряд.

A* — алгоритм поиска кратчайшего пути на графе, который направляет перебор к цели с помощью эвристики, оставаясь оптимальным.

Карту разбивают на сетку клеток (занятые — препятствия, свободные — проходимые). Задача — найти путь от старта к цели. A* на каждом шаге выбирает клетку с минимальной оценкой:

$$ f(n) = g(n) + h(n) $$

где $g(n)$ — реальная стоимость пути от старта до клетки $n$, $h(n)$ — эвристика, оптимистичная оценка оставшегося пути до цели. Для движения по сетке без диагоналей берут манхэттенское расстояние:

$$ h(n) = |x_n - x_{goal}| + |y_n - y_{goal}| $$

Почему A* оптимален

Если эвристика допустима (никогда не переоценивает остаток — манхэттен это гарантирует на сетке), A* находит кратчайший путь, но исследует куда меньше клеток, чем слепой перебор, потому что приоритет отдаётся направлению к цели.

Считаем

Найдём путь в сетке 5×5 (символ # — стена, S — старт, G — цель).

import heapq

grid = [
    "S....",
    "###.#",
    "....#",
    ".###.",
    "....G",
]
rows, cols = len(grid), len(grid[0])

def find(ch):
    for r in range(rows):
        for c in range(cols):
            if grid[r][c] == ch:
                return (r, c)

start, goal = find("S"), find("G")
def h(p):
    return abs(p[0]-goal[0]) + abs(p[1]-goal[1])

open_heap = [(h(start), 0, start)]
g = {start: 0}
came = {}
while open_heap:
    f, gc, cur = heapq.heappop(open_heap)
    if cur == goal:
        break
    for dr, dc in [(-1,0),(1,0),(0,-1),(0,1)]:
        nr, nc = cur[0]+dr, cur[1]+dc
        if 0 <= nr < rows and 0 <= nc < cols and grid[nr][nc] != "#":
            ng = gc + 1
            if (nr,nc) not in g or ng < g[(nr,nc)]:
                g[(nr,nc)] = ng
                came[(nr,nc)] = cur
                heapq.heappush(open_heap, (ng + h((nr,nc)), ng, (nr,nc)))

path = []
cur = goal
while cur != start:
    path.append(cur)
    cur = came[cur]
path.append(start)
path.reverse()
print("длина пути (шагов):", len(path) - 1)
print("путь:", path)

Вывод:

длина пути (шагов): 14
путь: [(0, 0), (0, 1), (0, 2), (0, 3), (1, 3), (2, 3), (2, 2), (2, 1), (2, 0), (3, 0), (4, 0), (4, 1), (4, 2), (4, 3), (4, 4)]

Как работает под капотом

Очередь с приоритетом (heapq) всегда выдаёт клетку с наименьшим $f$ — это и направляет поиск. $g$ хранит лучшую известную стоимость до каждой клетки; если нашли путь короче, обновляем. Словарь came запоминает «откуда пришли», чтобы в конце восстановить маршрут обратным проходом от цели. Если бы $h \equiv 0$, A* выродился бы в алгоритм Дейкстры — корректный, но без направленности к цели.

Частые ошибки

• Завышать эвристику (брать недопустимую) — A* перестаёт гарантировать кратчайший путь.
• Не проверять границы сетки и стены — робот «пройдёт сквозь стену».
• Забыть обновлять $g$ при нахождении более короткого пути к уже виденной клетке.

Итог

• A* ищет кратчайший путь, направляя перебор эвристикой: $f = g + h$.
• На сетке без диагоналей допустимая эвристика — манхэттенское расстояние.
• Очередь с приоритетом выдаёт самую перспективную клетку.
• При $h \equiv 0$ A* превращается в Дейкстру.