入門 · レッスン 8 / 10

A*によるグリッド上の経路計画

占有格子地図、4近傍と8近傍の移動、許容的ヒューリスティックを解説し、heapqを使った純粋なPythonでA*プランナを実装します。展開したセル数を数えてDijkstra法と比較します。

入門40 分更新 2026年11月14日無料期間 2027年1月31日

このレッスンで学ぶこと

間取りを占有格子地図として表現し、4近傍と8近傍のそれぞれについて、セルの近傍を正しい移動コストとともに列挙する
二分ヒープを使ってA*を実装し、ヒューリスティックがゼロのときDijkstra法に帰着することを示す
許容的ヒューリスティックを定義し、移動の集合に合わせてマンハッタン距離・ユークリッド距離・オクタイル距離を選ぶ
同じマップで各ヒューリスティックが展開するセル数を測定し、その大小関係を説明する
許容的でないヒューリスティックが最適解より長い経路を返す例を示す

自己位置がわかっている移動ロボットでも、何にもぶつからずに別の場所へ行く方法は決めなければなりません。最も単純で実用的な答えは、床を正方形のセルに区切り、障害物を含むセルに印を付け、スタートからゴールまでの自由なセルの連なりのうちコストが最小のものを探索することです。このレッスンは、Pythonの辞書を使い慣れていて、多くの移動ロボット用プランナの土台にある探索を理解したい読者を対象としています。標準ライブラリのheapqだけを使い、純粋なPython約40行でA*を書きます。そして、良いヒューリスティックがDijkstra法と比べてどれだけ処理を減らすかを測定します。

占有格子地図

占有格子地図（occupancy grid）は、平面を一定サイズのセルに分割し、セルごとに自由・占有・未知のいずれであるかを保持します。セルのサイズはロボットとセンサに合わせて選びます。地図作成システムはLiDARのスキャンからこの格子を構築し、セルごとに占有確率を保持します。経路計画では、この確率をしきい値で「占有か否か」に二値化します。このレッスンでは格子を文字列で表し、#を占有、.を自由とします。こうするとコードの中でマップを目で確認できます。

実機では2つの点が重要です。第一に、ロボットは点ではありません。プランナはすべての障害物をロボットの半径だけ膨らませ（インフレーション）、自由なセルを通る経路でロボットの本体が障害物に触れないようにします。第二に、格子上の経路は鋭い角を持つセル中心の列です。差動二輪ロボットは各角でその場旋回すれば追従できますが、多くのシステムは経路を平滑化するか、ロボットの動力学を考慮するローカルコントローラに経路を渡します。

近傍と移動コスト

探索には、あるセルから1回の移動で到達できるセルと、その移動のコストが必要です。

セル n の近傍。左: 4近傍では上下左右に移動でき、コストは1セルです。右: 8近傍では斜めにも移動でき、コストは2の平方根、約1.41セルです。斜め移動は、間を通過する2つのセルのどちらかが占有されていると禁止します。このため経路が角をすり抜けることはありません。

4近傍では、縦横3セルずつの斜め方向の変位に6回の移動が必要です。8近傍では斜め移動3回、約4.24で済み、これは直線距離に等しくなります。8近傍のほうが短くギザギザの少ない経路になりますが、その代わりセルごとに調べる近傍が増えます。

角に関する規則は実機で重要です。この規則がないと、角で接する2つの占有セルの間を斜め移動が通り抜けてしまいます。そこはロボットが通れない隙間です。

Dijkstra法とA*

Dijkstra法（1959年）は、辺のコストが非負のグラフで最小コストの経路を見つけます。フロンティアのセルをgの順に並べた優先度付きキューを持ちます。gはスタートからの既知の最良経路のコストです。最もコストの小さいセルを繰り返し取り出して確定（展開）済みとし、各近傍にそのセルを経由する経路を提示します。ゴールが初めてキューから取り出された時点で、そのコストは最適です。Dijkstra法は、その方向がゴールに向かうかどうかに関係なく、スタートの周りを全方向に等コストの輪状に探索します。

A*（Hart, Nilsson and Raphael、1968年）は、代わりにf = g + hでキューを並べます。ここでhは、あるセルからゴールまでの残りコストのヒューリスティックな推定値です。ゴールに近そうなセルほど早くキューから取り出されるので、探索はゴールの方向に偏ります。すべてのセルでhがゼロなら、A*はDijkstra法とまったく同じです。

hが許容的（admissible）、すなわち真の残りコストを決して過大評価しないなら、A*は依然として最適経路を返します。より強い性質である一貫性（consistency）は、どの移動でもhの減少量がその移動コスト以下であることを指します。ヒューリスティックが一貫性を持てば、一度展開したセルを再び展開する必要はありません。以下のヒューリスティックはいずれも、組み合わせる移動の集合に対して一貫性を持ちます。

ヒューリスティック	オフセット dx, dy に対する式	許容的となる対象
ゼロ	0	任意（これはDijkstra法）
マンハッタン	dx + dy	4近傍グリッドのみ
ユークリッド	(dx² + dy²) の平方根	4近傍・8近傍のグリッド、連続空間
オクタイル	max(dx, dy) + (√2 - 1) min(dx, dy)	4近傍・8近傍のグリッド

障害物のない4近傍グリッドではマンハッタン距離が、障害物のない8近傍グリッドではオクタイル距離が、正確なコストに等しくなります。真のコストに等しいヒューリスティックなら、無駄な展開をほとんどせずに探索がゴールへまっすぐ進みます。障害物があると真のコストはこれらのどれよりも大きくなり、そこで探索に余分な処理が生じます。

純粋なPythonによるA*

マップは11×24セルで、スタート側に開いたC字形の壁があります。ゴールに向かう探索はまずこのくぼみに入り込み、引き返す必要があります。このため、ヒューリスティックの違いがよくわかります。プランナはキューにheapqを使います。Pythonのヒープにはキーを減少させる操作がありません。そこで、キュー内のセルを更新する代わりに、より低いコストの新しいコピーをプッシュし、古いコピーはヒープから取り出された時点で読み飛ばします。f値が等しい場合はカウンタで順序を決め、ヒープが2つのセルを比較せずに済むようにします。

import heapq
import math

GRID_TEXT = """
........................
........................
......##########........
...............#........
...............#........
..S............#....G...
...............#........
...............#........
......##########........
........................
........................
""".strip().splitlines()

grid = [[c == "#" for c in row] for row in GRID_TEXT]
rows, cols = len(grid), len(grid[0])
start = next((r, c) for r, row in enumerate(GRID_TEXT) for c, ch in enumerate(row) if ch == "S")
goal = next((r, c) for r, row in enumerate(GRID_TEXT) for c, ch in enumerate(row) if ch == "G")

MOVES_4 = [(-1, 0, 1.0), (1, 0, 1.0), (0, -1, 1.0), (0, 1, 1.0)]

def manhattan(a, b):
    return abs(a[0] - b[0]) + abs(a[1] - b[1])

def euclidean(a, b):
    return math.hypot(a[0] - b[0], a[1] - b[1])

def octile(a, b):
    dx, dy = abs(a[0] - b[0]), abs(a[1] - b[1])
    return max(dx, dy) + (math.sqrt(2.0) - 1.0) * min(dx, dy)

def zero(a, b):
    return 0.0          # A* with h = 0 is Dijkstra's algorithm

def astar(start, goal, moves, h):
    """Return (path, cost, nodes expanded). Cells are (row, col); True in grid means occupied."""
    g = {start: 0.0}
    parent = {start: None}
    open_heap = [(h(start, goal), 0, start)]   # (f, tie-break counter, cell)
    closed = set()
    counter = 0
    while open_heap:
        _, _, cell = heapq.heappop(open_heap)
        if cell in closed:
            continue                 # stale heap entry, a cheaper copy was already expanded
        closed.add(cell)
        if cell == goal:
            path = []
            while cell is not None:
                path.append(cell)
                cell = parent[cell]
            return path[::-1], g[goal], len(closed)
        for dr, dc, step in moves:
            nr, nc = cell[0] + dr, cell[1] + dc
            if not (0 <= nr < rows and 0 <= nc < cols) or grid[nr][nc]:
                continue
            new_g = g[cell] + step
            if new_g < g.get((nr, nc), math.inf):
                g[(nr, nc)] = new_g
                parent[(nr, nc)] = cell
                counter += 1
                heapq.heappush(open_heap, (new_g + h((nr, nc), goal), counter, (nr, nc)))
    return None, math.inf, len(closed)

def show(path):
    canvas = [list(row) for row in GRID_TEXT]
    for r, c in path[1:-1]:
        canvas[r][c] = "*"
    print("\n".join("".join(row) for row in canvas))

free_cells = sum(not occ for row in grid for occ in row)
print(f"grid {rows} x {cols}, free cells {free_cells}, start {start}, goal {goal}")
for name, h in [("zero", zero), ("Euclidean", euclidean), ("octile", octile), ("Manhattan", manhattan)]:
    path, cost, expanded = astar(start, goal, MOVES_4, h)
    print(f"4-connected, h = {name:9s}  cost {cost:5.1f}  path cells {len(path):3d}  expanded {expanded:3d}")
show(astar(start, goal, MOVES_4, manhattan)[0])

grid 11 x 24, free cells 239, start (5, 2), goal (5, 20)
4-connected, h = zero       cost  26.0  path cells  27  expanded 223
4-connected, h = Euclidean  cost  26.0  path cells  27  expanded 196
4-connected, h = octile     cost  26.0  path cells  27  expanded 190
4-connected, h = Manhattan  cost  26.0  path cells  27  expanded 158
........................
.....************.......
.....*##########*.......
.....*.........#*.......
.....*.........#*.......
..S***.........#****G...
...............#........
...............#........
......##########........
........................
........................

どのヒューリスティックもコスト26の経路を見つけます。4つとも4近傍グリッドで許容的なので、当然の結果です。違いは処理量にあります。Dijkstra法は239個の自由なセルのうち223個を展開します。使うセルが27個だけの経路を見つけるために、マップのほぼ全体を調べることになります。マンハッタン距離はこれを158個に減らします。ユークリッド距離とオクタイル距離はその中間です。4近傍グリッドでは、どちらもマンハッタン距離より大きく過小評価するためです。許容的なヒューリスティックが厳しい（真の値に近い）ほど、A*が展開するセルは少なくなります。

経路自体は壁の上側を回ります。このマップにはコスト26の経路が多数あり（ステップ数が同じ階段状の経路はどれもそうです）、A*がどれを返すかはタイブレークの順序で決まります。これはグリッド探索の性質であり、バグではありません。

8近傍と4つのヒューリスティック

2つ目のブロックでは斜め移動を許し、角をすり抜ける斜め移動は禁止して、4つのヒューリスティックをもう一度実行します。続いて、オクタイル距離とマンハッタン距離だけで2つ目のクエリを実行します。

import heapq
import math

GRID_TEXT = """
........................
........................
......##########........
...............#........
...............#........
..S............#....G...
...............#........
...............#........
......##########........
........................
........................
""".strip().splitlines()
grid = [[c == "#" for c in row] for row in GRID_TEXT]
rows, cols = len(grid), len(grid[0])
start, goal = (5, 2), (5, 20)
SQRT2 = math.sqrt(2.0)
MOVES_8 = [(-1, 0, 1.0), (1, 0, 1.0), (0, -1, 1.0), (0, 1, 1.0),
           (-1, -1, SQRT2), (-1, 1, SQRT2), (1, -1, SQRT2), (1, 1, SQRT2)]

def free(r, c):
    return 0 <= r < rows and 0 <= c < cols and not grid[r][c]

def neighbours(cell):
    r, c = cell
    for dr, dc, step in MOVES_8:
        if not free(r + dr, c + dc):
            continue
        if dr and dc and not (free(r + dr, c) and free(r, c + dc)):
            continue                     # no squeezing diagonally past a corner
        yield (r + dr, c + dc), step

def zero(a, b):
    return 0.0

def manhattan(a, b):
    return abs(a[0] - b[0]) + abs(a[1] - b[1])

def euclidean(a, b):
    return math.hypot(a[0] - b[0], a[1] - b[1])

def octile(a, b):
    dx, dy = abs(a[0] - b[0]), abs(a[1] - b[1])
    return max(dx, dy) + (SQRT2 - 1.0) * min(dx, dy)

def astar(start, goal, h):
    g, parent, closed, counter = {start: 0.0}, {start: None}, set(), 0
    open_heap = [(h(start, goal), 0, start)]
    while open_heap:
        _, _, cell = heapq.heappop(open_heap)
        if cell in closed:
            continue
        closed.add(cell)
        if cell == goal:
            path = []
            while cell is not None:
                path.append(cell)
                cell = parent[cell]
            return path[::-1], g[goal], len(closed)
        for nxt, step in neighbours(cell):
            new_g = g[cell] + step
            if new_g < g.get(nxt, math.inf):
                g[nxt], parent[nxt] = new_g, cell
                counter += 1
                heapq.heappush(open_heap, (new_g + h(nxt, goal), counter, nxt))
    return None, math.inf, len(closed)

results = {}
for name, h in [("zero", zero), ("Euclidean", euclidean), ("octile", octile), ("Manhattan", manhattan)]:
    path, cost, expanded = astar(start, goal, h)
    results[name] = path
    print(f"S to G, h = {name:9s}  cost {cost:6.3f}  expanded {expanded:3d}")

canvas = [list(row) for row in GRID_TEXT]
for r, c in results["octile"][1:-1]:
    canvas[r][c] = "*"
print("\n".join("".join(row) for row in canvas))

# A second query, from the top-left area to below the cup, shows Manhattan's overestimate biting.
for name, h in [("octile", octile), ("Manhattan", manhattan)]:
    path, cost, expanded = astar((1, 4), (9, 19), h)
    print(f"(1, 4) to (9, 19), h = {name:9s}  cost {cost:6.3f}  expanded {expanded:3d}")

S to G, h = zero       cost 21.899  expanded 212
S to G, h = Euclidean  cost 21.899  expanded 123
S to G, h = octile     cost 21.899  expanded 105
S to G, h = Manhattan  cost 21.899  expanded 108
........................
.....************.......
....*.##########.*......
...*...........#..*.....
...*...........#...*....
..S............#....G...
...............#........
...............#........
......##########........
........................
........................
(1, 4) to (9, 19), h = octile     cost 21.243  expanded 106
(1, 4) to (9, 19), h = Manhattan  cost 22.414  expanded  68

斜め移動により、経路は26セルから21.9セルに短くなり、壁の角に沿うようになります。この移動の集合で最も厳しい許容的ヒューリスティックであるオクタイル距離は、展開数が最も少なくなります（105）。ユークリッド距離は許容的ですが緩く、123です。Dijkstra法は依然としてマップの大半を展開します。

注目すべきはマンハッタン距離の行です。8近傍グリッドでは過大評価する場合があります。斜め1ステップのコストは1.41ですが、マンハッタン距離は2と見積もります。もはや許容的ではないので、Aは最適性の保証を失います。1つ目のクエリでは、たまたま108回の展開で最適コストを返しました。2つ目のクエリ、セル(1, 4)からセル(9, 19)では、展開数は106ではなく68でしたが、返した経路のコストは最適値21.243に対して22.414で、約5.5%長くなりました。許容的でないヒューリスティックは、経路の質と引き換えに速度を得ます。意図的にこれを行うプランナもあります（重み付きAは、許容的なhに1より大きい係数を掛けます）。移動の集合に合わないヒューリスティックを組み合わせて、意図せずこれを行ってしまうのはよくあるバグです。

同じマップとクエリで、各ヒューリスティックについてA*が展開したセル数（4近傍と8近傍の移動）。値は2つのブロックの出力から取りました。8近傍グリッドでのマンハッタン距離（保証なし）を除き、すべての実行が最適な経路コストを返します。

実機では何が変わるか

実運用のナビゲーションスタックは同じ探索を土台にしつつ、いくつかの層を追加します。格子は同時自己位置推定と地図作成（SLAM）で構築した地図から得て、リアルタイムのセンサデータで更新します。障害物はロボットの半径に余裕分を加えた分だけ膨らませます。障害物の近くのセルには通行禁止ではなく高いコストを与え、経路が壁から十分な距離を保つようにします。セルのコストで地形・傾斜・進入禁止区域を表すこともできます。

グリッド探索には限界があります。解像度を固定すると、セル数はマップの大きさの2乗で増え、より高次元の問題では次元数を指数とするべき乗で増えます。6関節アームのコンフィギュレーション空間は、実用的な解像度では格子化できません。そこではRRTやPRMのようなサンプリングベースのプランナが代わりを担います。これらは動作計画のハブで紹介しています。また、格子上の経路はロボットの運動上の制約を無視します。自動車型のロボットはその場旋回ができないため、そのプランナはセルだけでなく向きを含む姿勢の空間で探索します。

演習

インフレーションの処理を追加してください。計画の前に、障害物から1セル以内（8近傍）にある自由なセルをすべて占有とします。8近傍・オクタイル距離の探索を再実行し、新しいコストと展開数を報告してください。経路が移動したのはなぜでしょうか。
壁の近くを禁止せずに、プランナが壁から離れた経路を好むようにしてください。障害物に隣接する各自由セルに、進入時の追加コスト2を与えます。どのヒューリスティックが許容的なままでしょうか。
2つ目のクエリでhを1.5 * octile（重み付きA*）に置き換え、コストと展開数を報告してください。コストは最適値の1.5倍以内に収まっているでしょうか。

まとめ

占有格子地図は地図をグラフに変換します。自由なセルがノード、許可された移動がコスト1または√2の辺です。
A*はg + hの順に探索します。hがゼロならDijkstra法です。許容的なhなら最適経路を返しつつ、展開するセルが少なくなります。
ヒューリスティックは移動の集合に合わせます。4近傍ならマンハッタン距離、8近傍ならオクタイル距離です。ユークリッド距離はどちらでも許容的ですが緩くなります。
このマップでは、Dijkstra法は212〜223セルを展開し、最も厳しい許容的ヒューリスティックを使ったA*は158セル（4近傍）と105セル（8近傍）を展開しました。
過大評価するヒューリスティック（斜め移動ありのマンハッタン距離）は、より長い経路を返すことがあります。ここでは5.5%長くなりました。

さらに読む

Planning Algorithms, Steven M. LaValle, Cambridge University Press, 2006：第2章「Discrete Planning」で、グラフ上の前向き探索、Dijkstra法、A*を扱います。本全体がオンラインで無料公開されています。
A Formal Basis for the Heuristic Determination of Minimum Cost Paths, Hart, Nilsson and Raphael, IEEE Transactions on Systems Science and Cybernetics, 1968：A*を提案し、ヒューリスティックが過大評価しなければ最小コストの経路を見つけることを証明した論文です。
A note on two problems in connexion with graphs, E. W. Dijkstra, Numerische Mathematik, 1959：最短経路アルゴリズムの原典で、わずか3ページです。
Modern Robotics, Lynch and Park, Cambridge University Press, 2017（第1版改訂版、2019年プレプリント）：第10章「Motion Planning」は、グリッド探索の次に読む章です。移動ロボットだけでなく、アームのコンフィギュレーション空間での計画も扱います。