MyEnigma

とある自律移動システムエンジニアのブログです。#Robotics #Programing #C++ #Python #MATLAB #Vim #Mathematics #Book #Movie #Traveling #Mac #iPhone

A*による最短経路探索MATLAB, Pythonプログラム

https://github.com/AtsushiSakai/PythonRoboticsGifs/blob/master/PathPlanning/AStar/animation.gif?raw=true

はじめに

先日、ダイクストラ法による最短経路探索の

MATLABプログラムを公開しましたが、

ダイクストラ法による最短経路探索MATLABプログラム - MY ENIGMA

今回は、このダイクストラ法を改良した

A* (エースター)というアルゴリズムによる

最短経路探索MATLAB, Pythonプログラムを公開したいと思います。

A*アルゴリズムとは、

A*アルゴリズムは、

ダイクストラ法のノード探索の方法を

改良したアルゴリズムです。


ダイクストラ法は

隣接するノードを順番に探索するアルゴリズムですので、

スタート地点とゴール地点が遠いと、

探索するノードの数が膨大になり、

計算コストが非常に大きくなるという問題がありました。


A*アルゴリズムは、

ヒューリスティック関数という関数を使用し、

最短経路らしいノードのみを探索します。


ヒューリスティック関数と言われると

難しそうに感じますが、

グリッドマップにおける最短経路探索では、

ヒューリスティック関数として、

"そのノードとゴールまでの距離"

を使用することが多いです。


このヒューリスティック関数を利用することにより、

ゴールが存在する方向のノードが優先的に探索されるようになり、

無駄なノードの探索を減少させることができます。


また、このヒューリスティック関数の値(推定コスト)が、

実際のコストを下回る場合は、

このA*アルゴリズムで、

最適(最短)の経路を見つけることができることが理論的に示されています。

これをA*の許容性(admissibility)と呼びます。

しかし、このヒューリスティック関数の値を小さく見積もると、

よりダイクストラ法に近づき、

最適経路を見つけることはできますが、計算コストが大きくなってしまいます。

したがって、できるだけヒューリスティック関数を実際のコストを

ギリギリ下回るように設定することが重要です。


一般的にA*のヒューリスティック関数には、

現在のノードとゴールまでの距離を利用することが多いですが、

これは実際のコストを必ず下回るためです。


下記の図は、

先日公開したダイクストラ法を用いて

最短経路探索を行った結果です。

赤いバツ(open)と黒いバツ(close)が

探索されたノードを表しています。

この結果を見るとわかる通り、

スタート地点が左下にあり、

ゴール地点が右下に存在しているにも関わらず、

上の方のノードも探索してしまっていることがわかります。


一方、A*アルゴリズムを使用すると、

冒頭の図のような結果になります。

スタート地点とゴール地点の間のノードのみが探索されており、

効率的なノード探索が行われていることがわかります。


A*とダイクストラ法のノード探索の違いについては、

下記のwikipediaの記事のgif動画もわかりやすいです。

Dijkstra's algorithm - Wikipedia, the free encyclopedia

A* search algorithm - Wikipedia, the free encyclopedia

A*よる経路探索アルゴリズムの流れ

1. ゴールノード(G )とスタートノード(S )を作成する。

また計算中のノードを格納しておくためのリスト(Openリスト)と、

計算済みのノードを格納しておくリスト(Closeリスト)を用意する。

2. スタートノードをOpenリストに追加する.

ここでスタートノードから、あるノード n を通って、

ゴールノードまでたどり着くときの最短経路を考えると、

このときこの最短経路のコストを f (n) とおくと、

g (n) をスタートノードから n までの最小コスト、

h (n) をn からゴールノードまでの最小コストとすると、

f (n)= g (n) + h (n) ---式i

となる。このとき

g*(S) = 0 であり, (スタートノードからスタートノードへのコストは0)

であり、先ほどの式から、

f*(S) = h*(S) となる。(スター*が付いたものは推定値を表す)

また Closeリストは空にする。

また常にh(n)=0と考えると、ダイクストラ法となる。

3. Openリストが空なら探索は失敗とする

(スタートからゴールにたどり着くような経路が存在しなかったことになる)。

4. Openリストに格納されているノードの中で最もf*(n) が小さいノードnを選択する。

(このようにコストが最小になるノードのみを探索することで、

効率的な探索が可能になる)

5. n = G であるなら探索を終了する。それ以外の場合は n を Close リストに移す。
6. n に隣接している全てのノード(ここでは隣接ノードを m とおく)

に対して以下の操作を行う

    6-1. f '(m) = g*(n) + h*(m) + COST(n,m) を計算する、

      ここでf'(m)は最小コストの候補値である。

      COST(n,m) はノード n から m へ移動するときのコストである。

      (今回は方向に関わらず、COST(n,m) =1であるとした)

      またg*(n) は 式iより、g*(n) = f*(n) - h*(n) で求めることができる。

    6-2. m の状態に応じて以下の操作を行う

       6-2-1. m が Openリストにも Closeリストにも含まれていない場合

           f*(m) = f '(m) とし m を Openリストに追加する。

          このとき m の親を n として記録する。

       6-2-2. m が Openリストにある場合、

          f '(m) < f*(m) であるなら f*(m) = f '(m) に置き換える。

          このとき記録してある m の親を n に置き換える。

       6-2-3. m が Closeリストにある場合、

          f '(m) < f*(m) であるなら f*(m) = f '(m) として

          m を Openリストに移動する。

          また記録してある m の親を n に置き換える。

7. 3-6 以降を繰り返す。
8. 探索終了後 G から親を順次たどっていくと S から G までの最短経路が得られる。

運動モデルにおけるコスト設定による複雑な経路探索

下記記事のダイクストラ法と同様に、

A*アルゴリズムにおいても、

運動モデルのコストをうまく設定することにより、

より複雑な経路探索を行うことができます。

ダイクストラ法による最短経路探索MATLABプログラム - MY ENIGMA

MATLAB プログラム

サンプルプログラムの仕様については、

下記のプログラムを読めばわかると思いますが、

スタートとゴールの位置を指定して、

境界データと障害物データを指定すると、

これらを避けた最短経路を作成してくれます。


またanimation関数をコメントアウトすることにより、

どのように探索が行われているのかを、

各ステップ毎に確認することができます。


プログラムは下記リンク先のGitHub上にあります。

github.com

 

Pythonサンプルプログラム

下記のリポジトリで、

A*のPythonサンプルコードを公開しています。

github.com

 

MyEnigma Supporters

もしこの記事が参考になり、

ブログをサポートしたいと思われた方は、

こちらからよろしくお願いします。

myenigma.hatenablog.com