A*経路探索をPythonで実装する｜ロボットの「最短ルート」を見つける

自律ロボットが「今いる場所から目的地まで、障害物を避けてどう進むか」を決めるのが経路計画です。その定番アルゴリズムが A（エースター）**。この記事ではグリッド地図の上で、AをPythonでゼロから実装します。最短経路を賢く見つける仕組みが、手を動かすとよくわかります。

A*の考え方: 実コスト＋見込み

A*は各マスを評価するとき、2つを足します。

• g … スタートからそのマスまでの実際のコスト
• h … そのマスからゴールまでの見込みコスト（ヒューリスティック）

f = g + h が小さいマスから優先的に調べる。この「ゴールの方向へ当たりをつける」のが、闇雲に広げる探索より速い理由です。

準備

pip install numpy

標準ライブラリの heapq（優先度付きキュー）も使います。

① グリッドとヒューリスティック

0 が通路、1 が障害物のグリッドを用意します。見込みコストにはマンハッタン距離を使います。

import heapq

grid = [
    [0, 0, 0, 0, 0],
    [0, 1, 1, 1, 0],
    [0, 0, 0, 1, 0],
    [1, 1, 0, 1, 0],
    [0, 0, 0, 0, 0],
]

def heuristic(a, b):
    return abs(a[0] - b[0]) + abs(a[1] - b[1])   # マンハッタン距離

② A*本体

f が小さいマスから取り出し、隣（上下左右）へ広げていきます。

def astar(grid, start, goal):
    rows, cols = len(grid), len(grid[0])
    open_set = [(0, start)]          # (f, マス)
    came_from = {}
    g = {start: 0}

    while open_set:
        _, current = heapq.heappop(open_set)
        if current == goal:
            # 経路を復元
            path = [current]
            while current in came_from:
                current = came_from[current]
                path.append(current)
            return path[::-1]

        r, c = current
        for dr, dc in [(-1,0),(1,0),(0,-1),(0,1)]:
            nr, nc = r + dr, c + dc
            if 0 <= nr < rows and 0 <= nc < cols and grid[nr][nc] == 0:
                tentative = g[current] + 1
                if tentative < g.get((nr, nc), float("inf")):
                    came_from[(nr, nc)] = current
                    g[(nr, nc)] = tentative
                    f = tentative + heuristic((nr, nc), goal)
                    heapq.heappush(open_set, (f, (nr, nc)))
    return None   # 到達不能

③ 実行して経路を見る

path = astar(grid, (0, 0), (4, 4))
print("経路:", path)
print("ステップ数:", len(path) - 1)

出力は 経路: [(0,0), (0,1), ... , (4,4)]、ステップ数8。障害物（1）を避けながら、左上から右下への最短経路が見つかりました。試しに heuristic を常に0にすると、A*は「ダイクストラ法」になり、同じ最短経路を出しつつ探索するマスが増えます。ヒューリスティックの効き目が体感できます。

まとめ

• A*は f = g + h（実コスト＋ゴールへの見込み）が小さいマスから探索する
• ヒューリスティックでゴール方向に当たりをつけるので、闇雲な探索より速い
• heapq の優先度付きキューで実装でき、障害物を避けた最短経路が出せる
• h は実コストを超えないこと（許容性）が正しさの条件

もう少し詳しく（背景と理論）

A* は Hart, Nilsson, Raphael (1968) が提案した最短経路探索で¹、評価値 f=g+h の h（ヒューリスティック）が実コストを過大評価しない許容性を満たすとき最適解を保証します。さらに h が三角不等式を満たす**無矛盾（consistent）なら、一度確定したノードを再訪しなくてよく効率的です²。h≡0 とすると A はダイクストラ法に一致します³。実ロボットの経路計画では、グリッドではなく連続空間を扱うため、RRT や PRM といったサンプリングベース手法や、動的環境向けの D 系が併用されます⁴。

次の一歩 🌸

周囲を認識する目はロボットビジョンの基礎、動作の実行はPID制御をPythonで実装、開発の土台はROS 2入門へどうぞ。

Footnotes

1. Hart, P. E., Nilsson, N. J., Raphael, B. (1968). "A Formal Basis for the Heuristic Determination of Minimum Cost Paths." IEEE Trans. Systems Science and Cybernetics, 4(2), 100–107. ↩

2. 許容的ヒューリスティック（h ≤ 真のコスト）で A* は最適。無矛盾（consistent: h(n) ≤ c(n,n') + h(n')）ならクローズド集合の再展開が不要になり効率が上がる。 ↩

3. h≡0 の A* はダイクストラ法（Dijkstra, 1959）と等価。ヒューリスティックでゴール方向に探索を絞るぶん、A* は展開ノードが少なくて済む。 ↩

4. 連続・高次元空間では RRT（LaValle, 1998）や PRM（Kavraki et al., 1996）などサンプリングベース計画、動的環境では D*/D* Lite が使われる。グリッド A* はその入門にあたる。 ↩