ダイクストラ法とA*(A-star)探索を3Dメッシュ上で行いThree.jsで可視化をしてみた

経路探索の代表的なアルゴリズムであるダイクストラ法とA*探索の、探索過程を可視化してみました。

# アルゴリズムの解説

ダイクストラ法はスタートから全方位に探索してスタートから近い順に経路を確定させていき、ゴールに到達するまでこれを繰り返します。A*探索はゴールまでの距離を推定する**ヒューリスティック関数**を用いて探索を効率化します。

A*探索では、探索ノード`n`に対して評価値`f(n) = g(n) + h(n)`が小さい順に処理します。ここで

- `g(n)` = スタートから探索ノード`n`までの実際の経路コスト
- `h(n)` = 探索ノード`n`からゴールまでの推定経路コスト (ヒューリスティック)

です。探索が終了するまでゴールまでの経路コストはわからないので、`h(n)`をどう与えるかというところで探索の効率が変わってくるのですが、ユークリッド距離を使用した幾何学的な最短経路探索ではゴールまでの直線距離を使うことができます。また、`h(n) = 0`の場合がダイクストラ法になります。

評価値`f(n)`が小さい順に探索ノードを取り出すことが必要ですが、単純に配列に入れると、最小値を検索するとき・要素を削除するときに配列の長さに比例した時間がかかり、あまり効率が良くありません。要素の追加・最小値の検索と削除を高速に(対数時間で)処理するデータ構造は “優先順位付きキュー (priority queue)” と呼ばれ、2分ヒープによる実装がもっとも有名です。

## 探索コード

A*探索の Javascript コードを以下に示します[^3]。元のコードをここに記述しやすいように変更しています。`queue`は`f`を評価値とする優先順位付きキューです。

```js
// すべての頂点に Closed でないとマークする
for (let vertex of vertices)
	vertex.isClosed = false;

let start_node = {};

start_node.parent = null;
start_node.target = start;

start_node.g = 0;
start_node.f = 0; // 計算不要(すぐにキューから取り除かれるため)

let queue = new PriorityQueue();
queue.push(start_node);

while (!queue.isEmpty()) {

	// 最小の評価値 f をもつ探索ノードを取り出して、queue から削除する
	let node = queue.pop();

	// node がすでに Closed な頂点を指している場合は無視する
	if (node.target.isClosed) continue;

	// node が指す頂点に Closed とマークする
	node.target.isClosed = true;

	// スタートから node.target への最短経路は node.parent から来た
	node.target.previous = node.parent;

	// ゴールに到達したら、来た道をたどって経路を完成させる
	if (node.target === goal) {
		let total_path = [];
		let vertex = goal;
		while (vertex !== null) {
			total_path.push(vertex);
			vertex = vertex.previous;
		}
		return total_path;
	}

	// node.target に隣接する各ノードについて...
	for (neighbor of node.target.neighbors) {

		// Closed な頂点は展開しなくてよい
		if (neighbor.isClosed) continue;

		// 新たな探索ノードを生成して、キューに追加する
		let new_node = {};

		new_node.parent = node.target;
		new_node.target = neighbor;

		new_node.g = node.g + real_cost(node.target, neighbor);
		new_node.f = new_node.g + heuristic_cost(neighbor, goal);

		queue.push(new_node);
	}
}
```

**出発地点からの最短経路が確定している頂点**には**Closed**とマークします。**Closed**な頂点には**最短経路が直前にどの頂点を通ったか**を記録します。これを出発地点までたどることで最短経路が(逆向きで)得られます。
この情報はグラフデータの頂点に記録することもできますが、並列実行するときなどグラフデータに記録したくないときは`HashMap`的なものを使う形になると思います。後者の場合、`HashMap`にキーが存在する頂点が Closed となりますので、Closed であることを単独で管理する必要はありません。ここでは頂点に直接記録しています。なお、キューに挿入済みだが Closed でない頂点を Open であるといいます。

ここで**探索ノード`search_node`はグラフの頂点とは異なる**ことに注意してください。`search_node`は対応する頂点`target`のほかに、どの頂点から来たかを示す`parent`を保持しています。`target`への経路を複数キューに入れておき、その中でキューから最初に出てきた`search_node`に対応する`parent`が、最短経路がどこから来たのかを表します。なぜなら、`target`が同じならヒューリスティック`h`の値は等しいので、評価値`f=g+h`が最小というのは経路コスト`g`が最小であるからです。

※インターネットで調べると探索ノードが`parent`を保持していない実装例も結構ありました。この場合、Openな頂点に対しては、新たに得られた`f`とそれまでの`f`を比較し、必要に応じて`f, g`と経路を更新する必要があります[^1]。したがって、使用する priority queue は優先順位の変更、または要素の削除に対応している必要があります。(なお隣接頂点間の経路コストがすべて等しい場合、これは起こらないので無視でき、また単なる幅優先探索で計算できます。)上の擬似コードで示した手法ではこれは必要ありませんが、メモリ使用量は少し増えると思います。

## 許容的ヒューリスティック

すべての経路を調べ尽くしたわけではなく、経路コスト`g`が最小といっても`search_node`が生成された中での最小なので、「本当にこれが最短経路か？」という疑問が出てきます。

実は、**ヒューリスティックがゴールまでの実際の経路コストを上回ることがない**、という**許容的(admissible) と呼ばれる条件を満たせば、A*探索は常に真の最短経路を見つける**ことが保証されます。これを理解するため、次の図を考えます。

![0.png](https://qiita-image-store.s3.amazonaws.com/0/157335/28f2a8f0-bfbf-bb69-b4ab-5bcdfe57df8f.png)

S から G への最短経路を赤の経路`S→A→B→G`とします。この経路を進むときの探索ノードの評価値を考えると、

```
f(S→A)     = g(S→A)     + h(A) ≤ g(S→A→B→G)
f(S→A→B)   = g(S→A→B)   + h(B) ≤ g(S→A→B→G)
f(S→A→B→G) = g(S→A→B→G) + h(G) = g(S→A→B→G)
```

となります(探索ノードを経路で表しています)。ここで、最短ではない経路、例えば`S→C→D→E→G`を考えます。

```
f(S→C→D→E→G) = g(S→C→D→E→G) > g(S→A→B→G)
```

なので、この値は`f(S→A)`や`f(S→A→B)`より大きくなり、したがって`S→C→D→E→G`の経路が完成する前に

- (A がClosedになる) → (B がClosedになる) → (S→A→B→G の経路が完成する)

が起こり、最短経路`S→A→B→G`が先に見つかることになります。これは`S→C→D→E→G`の代わりに任意の非最短経路について成り立つので、非最短経路が先に見つかることはありません。もしヒューリスティックが許容的でなければ、

```
f(S→A) > f(S→C→D→E→G)
```

のようなことが起こり得ます。この場合、頂点Aの評価値の大きさがボトルネックになって S→A→B→G の経路を見つけられないことになります。

直線距離によるヒューリスティックは許容的なので、この場合のA*探索が見つける経路は真の最短経路になります。

# 成果物

成果物はこちら。Google Chrome, Firefox, Safari, Microsoft Edge で動作確認済みですが、Microsoft Edge ではタッチパネルには非対応となります。ES6で書いているため Internet Explorer には対応しておりません。

https://raysphere24.github.io/AStarSearchDemo/

緑色で表示した辺は SearchNode が生成された辺を表します。また、2点間の経路探索のほかに、1点を指定して最短経路のツリーを表示する機能も用意しました。モデルは有名な Stanford Bunny です。

ソースコードはこちらにあります。

https://github.com/Raysphere24/AStarSearchDemo

## 使用した言語・ライブラリ

ブラウザ上で体験できるように、言語は Javascript を採用しました[^2]。

JavaScript には標準の priority queue が無いので、mourner 氏の [tinyqueue](https://github.com/mourner/tinyqueue) を使用しました。

可視化には [three.js](http://threejs.org) を使用しました。これは WebGL をラップして簡単に使えるようにしたもので、シェーダーなどを書かなくても使えるようになっています。グラフィックスオブジェクトをライブラリが保持して描画する、Retained Mode タイプのライブラリです。

# 実行例

## ダイクストラ法とA*探索の比較

<img width="300" height="300" alt="1.png" src="https://qiita-image-store.s3.amazonaws.com/0/157335/3bcdcfcd-1f71-9b63-6c48-7cc78a3490b6.png">
<img width="300" height="300" alt="2.png" src="https://qiita-image-store.s3.amazonaws.com/0/157335/29965a07-885c-798a-e1fb-e3f1a1e53230.png">

経路が直線的であれば、探索の効率は大きく違います。

## ゴールを少し動かしただけで経路が大きく変わる例 (A*探索)

<img width="300" height="300" alt="3.png" src="https://qiita-image-store.s3.amazonaws.com/0/157335/3e5b461e-3290-26a2-f63b-032acdc56cff.png">
<img width="300" height="300" alt="4.png" src="https://qiita-image-store.s3.amazonaws.com/0/157335/4ad7e05d-e792-f231-f3a9-7af84752b411.png">

尻尾側がゴールです。ゴールを隣の頂点に動かすと経路が大きく変わることがわかります。
また、この例では直線距離と真の距離との誤差が大きいため、A*探索でもかなり広範囲に探索していることがわかります。

## 上の状況を最短経路木で観察する

<img width="300" height="300" alt="5.png" src="https://qiita-image-store.s3.amazonaws.com/0/157335/b41ecce0-cc05-b736-5b82-41c00dc4d2bf.png">

尻尾に到達する経路が両脇を通っていることがわかります。

# 参考資料
- [A* search algorithm - Wikipedia](https://en.wikipedia.org/wiki/A*_search_algorithm)
- エージェントアプローチ 人工知能 第2版

[^1]: WikipediaのA*探索の記事にある擬似コードもこの戦略をとっています。
[^2]: もともとC++とOpenGLを用いて実装したものを移植しました。
[^3]: 当初擬似コードを投稿しましたが、シンタックスハイライトの要望がきたため Javascript の文法に変更しました。