A*アルゴリズム

概要

最終更新：2020/01/23

このページではA*(エースター)アルゴリズムの説明をします。
A*アルゴリズムとはダイクストラ法のやり方をベースにしてより効率的に
最短経路を見つけることができる経路探索アルゴリズムです。
ダイクストラ法は現在のノードの距離と隣接しているノードまでのコストの合計を使用して
最短経路を見つけますが、この場合だとゴールから離れてしまう可能性があります。



上の図をダイクストラ法で探索するとStartの「V0」と隣接しているノード
「V1」「V2」「V3」はコストが低い順に実行されます。
そうすると「V1」「V3」「V2」の順番で探索が進みますが、
最もコストがかからない経路は「V2」からの経路です。
A*はこの問題を解決するために別の情報を追加して計算を行います。
その結果、より最適化された経路探索を実現しています。

サンプル

サンプルはここからダウンロードできます。
環境については以下の内容となっています。


	
開発環境
	

		
VSのバージョン
	
	

		
VisualStudio 2019
	

ヒューリスティックコスト

概要の項目で書いているダイクストラ法の計算に追加された情報のことを
「ヒューリスティックコスト(推定コスト)」と呼んでいます。
このコストはノード間のコストではなく、ゴールのノードに近づくために
用意されたコストで、よく使用されるのがゴールまでの距離です。



上の図で各セルで「1」と書いてあるのは移動コストです。
青のセル(2D)からオレンジのセル(1A)までの最短経路を見つける場合、
ダイクストラ法では青のセルから隣接しているノード「1D、2C、3D」は
移動コストが同じなので基本どれから行っても問題ないと判断されますが、
A*はヒューリスティックコストとして各ノードのGoalまでの距離を求めて
それを以下の計算式にあてはめます。


この結果、よりゴールに近いノードが選択されることになるので、
ダイクストラよりも少ない探索で最短経路を見つけることができます。
距離を求める方法では以下の二つの方法が有名です。

マンハッタン距離

マンハッタン距離はノードの各軸の距離を測り、それらの距離の合計を求め、
その値をヒューリスティックコストとして使用します。



	

		
セル
		
2Dまでのコスト
		
X軸距離
		
Y軸距離
		
ヒューリスティック
		
ノード間コスト
		
トータル
	
	

		
1D
		
0
		
3
		
0
		
3
		
1
		
4
	
	

		
2C
		
0
		
2
		
1
		
3
		
1
		
4
	
	

		
3D
		
0
		
3
		
2
		
5
		
1
		
6
	

今回は「1D」と「2C」のヒューリスティックコストが同じで、
「3D」だけコストが高いという結果になりました。

ユークリッド距離

ユークリッド距離は二点間の直線距離を測り、
その値をヒューリスティックコストとして使用します。



	

		
セル
		
2Dまでのコスト
		
ヒューリスティック
		
ノード間コスト
		
トータル
	
	

		
1D
		
0
		
3
		
1
		
4
	
	

		
2C
		
0
		
2.23
		
1
		
3.23
	
	

		
3D
		
0
		
3.60
		
1
		
4.70
	

ユークリッド距離では「2C」が最もコストが低く、
「3D」が最もコストが高いという結果になりました。
ユークリッド距離の方がマンハッタン距離よりも細かい結果になるので、
精度は高くなりますが、処理速度はマンハッタン距離の方が速いです。

オープンとクローズ

A*では探索候補のノードを「オープン」探索済みのノードを「クローズ」と呼んでいます。
例として「2D」がオープンにある場合のオープンとクローズの流れを説明したいと思います。



	

		
オープン
		
クローズ
	
	

		
2D
		
　
	

まず、オープンからノード「2D」を取得します。
取得された「2D」は「オープン」のリストから削除します。
※本来、取得するノードは最もトータルコストが低いノードが選ばれます。


	

		
オープン
		
クローズ
	
	

		
　
		
　
	

次に取得した「2D」の隣接ノードを調べます。
調査した結果、隣接ノードは「1D」「2C」「3D」の三つだということが分かったので、
各ノードのヒューリスティックコストを計算し、
その値を使用してトータルコストの算出も行います。


	

		
セル
		
2Dまでのコスト
		
ヒューリスティック
		
ノード間コスト
		
トータル
	
	

		
1D
		
0
		
3
		
1
		
4
	
	

		
2C
		
0
		
2.23
		
1
		
3.23
	
	

		
3D
		
0
		
3.60
		
1
		
4.70
	

計算が完了したら、各ノードをオープンリストに追加しますが、
この時に以下の条件のいずれかを満たしている必要があります。

「1D」「2C」「3D」は①にあてはまっているので、オープンリストに追加します。


	

		
オープン
		
クローズ
	
	

		
1D
		
　
	
	

		
2C
		
　
	
	

		
3D
		
　
	

最後に役目を終えた「2D」をクローズリストに追加します。


	

		
オープン
		
クローズ
	
	

		
1D
		
2D
	
	

		
2C
		
　
	
	

		
3D
		
　
	

これでオープンとクローズの流れは完了しましたが、
条件が成立した際の補足を次の項目で行います。

①．オープンとクローズリストにない

①の条件が当てはまった場合、オープンにもクローズにも
ノードが存在していないのでオープンリストにそのまま追加して問題ありません。

②．オープンにあるかつ新しいノードの方がトータルコストが低い

②の条件があてはまっていた場合は以下のどちらかの対応をします。

オープンリスト側に同じノードが存在しないようにするための対応なので、
プログラム上ではやりやすい方で対応してください。

③．クローズにあるかつ新しいノードの方がトータルコストが低い

③の条件があてはまった場合は、以下の対応を行います。

流れ

A*実行時は以下の流れで行われます。

※一部の項目は「オープンリストとクローズリスト」の説明と重複しています。

①．グラフを用意する

まずはグラフを用意します。

今回のグラフのノードで必要な情報は以下の二つです。

②．スタートとゴールのノードを決める

グラフの用意が出来たらA*を使用して経路探索を行う
スタートとゴールのノードを決めます。

③．初期化する

実装内容によって必要な情報があればここでA*の処理が
本格的に動作する前のここで行ってください。

初期化の必要がある情報は例えば今回のサンプルでは各ノードに
ヒューリスティックコストやトータルコストの情報を覚えさせているので、
A*が開始される前に初期化を行う必要があります。

他にはオープンやクローズリストなども前回の情報が残る処理方法で
実装される方はここで初期化してください。

④．スタートノードをオープンに追加する

スタートノードが決まったら、オープンリストとクローズリストの
初期化を行ってからオープンリストにスタートノードを追加します。


	

		
オープン
		
クローズ
	
	

		
2D
		
　
	

⑤．オープンリストからノードを取得する

オープンリストからノードを取得します。
ここで取得するノードはリスト内で最もトータルコストが小さいノードを取得してください。
また、取得したノードは⑦が終了する前までにオープンリストから削除し、
クローズリストに追加してください。

⑥．ゴール判定を行う

取得したノードがゴールノードかどうかを判定します。
判定の結果、ゴールだった場合は経路探索は終了、
ゴールでなかった場合は、このまま⑦に進んでください。

⑦．隣接ノードをオープンリストに追加する

取得したノードの隣接ノードをオープンリストに追加します。
こちらの追加方法は「オープンリストとクローズリスト」の項目で
詳しく書いていますので、そちらを確認してください。

⑧．オープンリストをソートする

次にオープンリストでノードを取得した際に最小コストを取得しやすくするため、
リストをトータルコストで昇順ソートを行います。

⑨．⑤～⑧をオープンリストが空になるまで繰り返す

⑤～⑧の手順をオープンリストが空になるまで繰り返します。
もし、この手順⑨でオープンリストが空になったことで終了した場合、
最短経路を発見できなかったと判断します。

実装方法

流れの項目で書いてある内容の実装方法をサンプルコードの一部から抜粋して説明します。

①．グラフを用意する

まずはグラフを用意します。
グラフにはノードが必要なので、ノード用の構造体を宣言します。
今回のサンプルではノード用の構造体に以下のメンバを追加しています。

// ノード
struct Node
{
	Node()
	{
	}

	Node(int x, int y) :
		Position(x, y)
	{
	}

	Cell Position;		// ノード座標
	std::vector Edges;	// 隣接ノード(辺)
	float HeuristicCost;	// ヒューリスティックコスト
	float TotalCost;	// コスト(ヒューリスティックコスト込み)
};

今回のサンプルでは経路用のデータを新しく用意さずにNodeのみで
経路探索を完結するようにしているので、「ヒューリスティックコスト」と
「トータルコスト」も追加しています。
このように「ノード座標」と「隣接ノード」はグラフとして最低限必要な情報なので、
実装する上で必要な情報があれば追加を行ってください。

次はNodeを構造体をグラフのサイズ分(今回は横5、縦5の計25)だけ用意します。

// グラフ
Node Graph[MapHeight][MapWidth];

用意した構造体変数を使用して各ノードの隣接ノードを設定し、グラフを完成させます。

// ノードの作成
void CreateGraph()
{
	for (int y = 0; y < MapHeight; y++)
	{
		for (int x = 0; x < MapWidth; x++)
		{
			Graph[y][x].Position.X = x;
			Graph[y][x].Position.Y = y;

			Cell adjacent_cells[] = 
			{
				Cell(x, y - 1),
				Cell(x - 1, y),
				Cell(x + 1, y),
				Cell(x, y + 1),
			};

			// 隣接ノードの追加
			for (const Cell& cell : adjacent_cells)
			{
				if (IsCellWithinTheRange(cell.X, cell.Y) == true &&
					CostTable[cell.Y][cell.X] == 1)
				{
					Graph[y][x].Edges.push_back(&Graph[cell.Y][cell.X]);
				}
			}
		}
	}
}

これでグラフの準備が完了したの②に進みます。

②．スタートとゴールのノードを決める

A*を使用するために、経路探索のスタートとゴールノードを決めます。
これはA*実行関数を用意して引数に指定すればいいと思います。

// A*実行
AStar(start, goal);

サンプルではAStar関数が実行関数になっているので
第一引数がスタートノード、第二引数がゴールノードです。
これでA*で経路探索が実行できるので③に進みます。

③．初期化する

今回のサンプルで初期化が必要な情報は各ノードの
ヒューリスティックコストとトータルコストです。
※初期化が必要な情報は実装内容によって異なります。

// コスト初期化
void InitCost()
{
	for (int y = 0; y < MapHeight; y++)
	{
		for (int x = 0; x < MapWidth; x++)
		{
			Graph[y][x].HeuristicCost = Infinity;
			Graph[y][x].TotalCost = 0;
		}
	}
}

初期化が完了したら④に進みます。

④．スタートノードをオープンに追加する

③までの段階ではオープンリストは空なので、オープンリストに
スタートノードを追加します。

// スタートノードの指定
open_list.push_back(&Graph[start_cell.Y][start_cell.X]);

これで準備が整ったので⑤からA*による経路探索を開始します。

⑤．オープンリストからノードを取得する

オープンリストから経路調査するノードを一つ取得します。
この時に取得するノードは最もトータルコストが低いノードを取得してください。

// ノード取得
Node* search_node = (*open_list.begin());
// 探索リストから除外
open_list.erase(open_list.begin());

ノードが取得できたので⑥に進みます。
また、取得したノードはオープンリストに必要がなくなったので、
リストから削除します。

⑥．ゴール判定を行う

取得したノードがゴールノードかどうかを確認します。


// ゴールに到達したら終わり
if (IsEqualCell(search_node->Position, goal_node->Position) == true)
{
	// クローズリストに最後のノードを追加する
	close_list.push_back(search_node);
	break;
}

ゴールノードに到達したら、取得したノードをクローズに追加してください。
これは経路復元をやりやすくするために必要な処理です。

⑦．隣接ノードをオープンリストに追加する

取得したノードの隣接ノードの有無を確認します。

for (Node* adjacent_node : search_node->Edges)
{

}

隣接ノードがあった場合、まずはヒューリスティックコストを算出します。

// 計算を行っていなかった場合だけ計算
if (adjacent_node->HeuristicCost == Infinity)
{
	// ヒューリスティクスコスト計算
	adjacent_node->HeuristicCost = CalculateHeuristic(adjacent_node, goal_node);
}

計算が終わったヒューリスティックコストをトータルコスト計算に使用します。

// ノード間コスト
float edge_cost = CostTable[adjacent_node->Position.Y][adjacent_node->Position.X];
// 取得ノードのトータルコスト
float total_cost = search_node->TotalCost;
/*
	トータルコスト算出
		ノード間コスト + ヒューリスティックコスト + 取得ノードのトータルコスト
*/
float cost = edge_cost + adjacent_node->HeuristicCost + total_cost;

コスト算出が終了したら、リストに追加できるのかの確認を行った上で
オープンリストにノードを追加します。
※算出したトータルコストはadjacent_nodeのTotalCostに代入しないでください。
　まだ、このコストが最低コストと決まっていません。

// オープンリストに追加
bool AddAdjacentNode(std::list& open_list, std::list& close_list, Node* adjacent_node, float cost)
{
	bool can_update = true;
	// クローズリストに同じノードがあってリストの方のコストが高いなら削除
	if (EraseNode(close_list, adjacent_node, cost) == EraseResult::CouldntErased)
	{
		can_update = false;
	}
	
	// オープンリストに同じノードがあってリストの方のコストが高いなら削除
	if (EraseNode(open_list, adjacent_node, cost) == EraseResult::CouldntErased)
	{
		can_update = false;
	}

	if (can_update == true)
	{
		open_list.push_back(adjacent_node);
		return true;
	}

	return false;
}

「EraseNode」は引数で渡したリスト内に削除条件に合ったノードがあれば
該当するノードをリストから削除する関数です。
戻り値として「削除した」「リスト側のコストの方が低くて削除してない」
「見つからなかった」のいずれかを返すようにしており、
どちらのリストにも「リスト側のコストの方が低くて削除してない」がない場合のみ
オープンリストに隣接ノードを追加するようにしています。
隣接ノードの追加が決定したら算出したトータルコストを
隣接ノードのトータルコストに反映します。

// ノード追加
if (AddAdjacentNode(open_list, close_list, adjacent_node, cost) == true)
{
	// トータルコストを更新
	adjacent_node->TotalCost = cost;
}

これで⑤で取得したノードの役目が終わったのでクローズリストに追加します。

// このノードの探索終了
close_list.push_back(search_node);

これでこの項目でやることはなくなったので⑧に進みます。

⑧．オープンリストをソートする

ここでは次に⑤の手順を行う際に最小のトータルコストを持つノードを
取得しやすくするためにリストをトータルコストで昇順ソートします。
ソートの方法はどのようなやり方でも構いませんし、
⑤で最小コストを調べて取得するならこの手順は飛ばしても問題ありません。

// 昇順ソート用関数
bool Less(Node* a, Node* b)
{
	if (a->TotalCost < b->TotalCost)
	{
		return true;
	}

	return false;
}

// 昇順ソート
open_list.sort(Less);

今回のサンプルでは昇順ソート用関数のLessを作成し、
C++のListコンテナのsort関数の引数としてLessを渡すことでソートをしています。
ソートが完了したら⑨に進みます。

⑨．⑤～⑧をオープンリストが空になるまで繰り返す

ここでは⑤～⑧までの処理をオープンリストに繰り返す処理を実装します。

// オープンリストがなくなるまで回す
while (open_list.empty() == false)
{
	// ⑤～⑧を実行
}

サンプルでは空になっているかのチェックはemptyを使用しています。
この関数はコンテナ(リスト)が空だった場合はtrue、
そうじゃなかった場合はfalseを返します。

経路復元

最短コストが分かったら次は経路復元を行う必要があります。
単純な経路復元の方法は、まず更新ノードを保存するためのノード配列を用意します。
配列のサイズは探索で使用しているノードの数と同じです。
この配列はノード情報ではなく、位置情報が分かればいいので、
位置情報専用のデータがあるならそちらで問題ありません。
※サンプルではCell構造体を使っています。


// 更新したノード位置保存用
Cell last_update_cells[MapHeight][MapWidth];

次は配列の初期化を行います。
位置情報を絶対に使用しない値で初期化してください。


Cell() :
	X(-1),
	Y(-1)
{

}

次は経路探索中でノードのトータルコストを更新した際に
オープンリストから取得したノードの情報を更新したノードの位置に代入します。


// ノード追加
if (AddAdjacentNode(open_list, close_list, adjacent_node, cost) == true)
{
	// トータルコストを更新
	adjacent_node->TotalCost = cost;

	// 経路を更新したセルを保存
	last_update_cells[adjacent_node->Position.Y][adjacent_node->Position.X] = search_node->Position;
}

これを最後まで継続すれば経路復元を行うための情報が集まります。
あとは、あつまった情報でゴールノードに代入されている位置情報を辿っていけば
最終的にスタートノードにたどり着きます。


// 経路復元
std::list route_list;

// ゴールセルから復元する
route_list.push_back(goal_cell);
while (route_list.empty() == false)
{
	Cell route = route_list.front();

	// スタート位置なら終了
	if (IsEqualCell(route, start_node->Position) == true)
	{
		// 復元した経路を表示
		for (Cell& cell : route_list)
		{
			printf("x = %d y = %d\n", cell.X, cell.Y);
		}
		break;
	}
	else
	{
		if (IsCellWithinTheRange(route.X, route.Y) == true)
		{
			// 追加
			route_list.push_front(last_update_cells[route.Y][route.X]);
		}
		else
		{
			printf("経路は見つからなかった\n");
			break;
		}
	}
}

これでA*の実装は完了です。