grbl1.1+Arduino CNCシールドV3.5+bCNCを使用中。
BluetoothモジュールおよびbCNCのPendant機能でスマホからもワイヤレス操作可能。
その他、電子工作・プログラミング、機械学習などもやっています。
MacとUbuntuを使用。

CNCマシン全般について:
国内レーザー加工機と中国製レーザー加工機の比較
中国製レーザーダイオードについて
CNCミリングマシンとCNCルーターマシンいろいろ
その他:
利用例や付加機能など:
CNCルーター関係:



*CNCマシンの制作記録は2016/04/10〜の投稿に書いてあります。


A*経路探索アルゴリズム

A*経路探索(覚書)
前から気になっていたA*経路探索を試してみました。Processingで書いています。
ネットで調べて見ると大体の仕組みは理解できるのですが、コードを見てもスッキリまとまりすぎで、初心者には少々分かりにくいという感じでした。コストやヒューリスティックというよりもオープンやクローズの配列やフラグをどう使うのかが分かりにくい部分でした。かなり試行錯誤してようやくそれっぽく動くようになったという感じです。
まずは、クラスは使わずにベタに書いて見ることにしました。そして配列もProcessingの場合であればArrayListを使うといいのかもしれませんが普通のArrayを使ってみました。というのも、初心者には単純な関数を使って、あまりスマートなコードにしない方が分かりやすいので。
Pythonならまた違った感じで書けそうなので、いくつか書き方を変えてみようかと思っています。

追記:具体的な手順を以下に追加しました。

探索の手順:
当初は、スタート地点から一歩ずつ進んでダメなら(壁や行き止まり、あるいはもう既に通った道)戻って、また違う道を選ぶというイメージでしたが、実際はそういう進み方ではありませんでした。歩きながら経験的に移動可能か不可能かを調べるのではなく、スコア(説明以下)の値を基準に探索箇所を飛び飛びで移動すると言う感じです。
具体的に以下で手順を説明します。

まず上図左のように5x5=25マス(ノード)の迷路があるとします。緑:start、ピンク:goal、黒いマス(ノード)は通行不可能な壁です。
各ノードを識別できるように、左上のノードからindex=0、右下のノードがindex=24になるように番号をふっておきます。そうすると、startはindex=11、goalはindex=19になります。
今回は斜め移動なしの設定にしたので、次に移動できるノードは上下左右のどれかになります。つまりスタート地点を探索拠点としindex=6、10、12、16を調べます。
各ノードにはいくつかの変数を用意して調査の度に記録しておきます。

index:そのノードの場所
status:未探索=0、オープン(探索拠点候補)=1、クローズド=2(探索拠点済み)
score:以下のcostとheulisticの合計値。
cost:スタート地点からの歩数(距離ではなく、一歩ずつ積算する)
heur:heuristic、ゴールまでのマンハッタン距離(ゴールまでの縦と横の移動ノード合計値)
prev:一歩前のノード(スタート地点は一歩前がないのでスタート地点の位置にしました)

このように各変数を記録すると上図右のようになります。いろいろあってわかりにくいかもしれませんが、これ以降の手順をひとつずつ見ていけば次第に分かってくるかと思います。

重要なのはcostとheuristicの合計値となるscoreの値です。毎回、探索した中から最小score値のノードを選びだし、そのノードが次の探索拠点(4近傍の中心地)となります。これは探索拠点先であって、実際の移動先とは異なります。探索手順と最終的な移動ルートの順番は別個のものと考えておいたほうがいいと思います。
costは、何歩目にそこに辿りつくかという情報です。4近傍は探索拠点(中心地)からみれば、次の一歩になるので、探索拠点のcostに毎回+1しておけばいいことになります。それが毎回積算されて、そこがスタートから何歩目かを知ることができます。
heuristicについては、例えばスタートとゴールのマンハッタン距離は右に3コマ下に1コマなので合計4コマという計算になります(壁があっても無視)。どのノードがゴールに近いかという目安として機能します。
原理的には、スタート地点からの歩数が少なく(無駄に遠回りせず)、かつゴールへ近そうなノードを選ぶということになります。
prevは一歩前のノードですが、4近傍は探索拠点(中心地)からみれば一歩先なので、4近傍における一歩前とは探索拠点のことになります。なので探索拠点のindex番号を入れていきます。prevはゴール発見後にルートを再構成する際に使います。



具体的な話にもどると(上図右:先ほどと同じ図)、次の探索拠点は4近傍のうちで最小scoreのノードになります。上図右の場合index=12と16がscore=4で最小値になっています。複数の最小scoreノードがある場合はindex番号の若い順というルールにしておきます(他のルールでも可)。ということで、次回の探索先はindex=12になります。
スタート地点は探索拠点の役目を終了したのでグレーにしてあります。status=2というのが探索拠点済みという変数です。グレー(status=2)になると、原則的に壁と同じように調査対象からは外されます。
4近傍の一歩手前は探索拠点の位置であるため、4近傍のprevはどれもprev=11(index=11:スタート地点)となります。

上図左、赤枠の部分index=12が今回の探索拠点です。先程と同様に周囲4方向index=7、11、13、17を調べますが、index=11はstatus=2で探索拠点終了(グレー)なのでスキップ、index=13は壁なのでスキップ、新規に調査できるノードはindex=7と17だけです。status=0(未探索)のノードだけを探索するようにif文などで条件分けしておくといいと思います。ちなみに調査したノードはstatus=0(未探索状態)からstatus=1(次回の探索拠点候補)に変えておきます。
そして、最小score値のノードを探すのですが、これまで探索先拠点候補にあがっていたノードも含めて、その中から最小score値のノードを探します。つまり黄色いノードはすべて探索拠点候補(status=1:オープン)であり、status=1のノードの中からひとつ選び出すことになります(この部分が重要)。
そうすると、最小score=4でindex=16と17となり、番号の若いindex=16を次回の探索拠点にします。最小値ノード検索は、min()やindexOf()など使うといいのかもしれません。

上図右、index=16が探索拠点になり、その周囲を調べます。今回の新規ノードはindex=15、21の二つです。今までの探索先拠点候補(黄色)も含めて比較すると、相変わらずindex=17のscore=4が最小なので、引き続きindex=17が次回の探索先になります。
index=15、21のprevは、どちらともprev=16なので忘れずに記入しておきます。

上図左、index=17を探索拠点とし周囲を調査するとindex=22だけが新規のノードに加えられ探索拠点候補としてstatus=1にしておきます。index=17は用済みなのでstatus=2でグレー。新規開拓するたびにprevも記入しておきます。
そうするとたまたま黄色いノードすべてがscore=6で最小となるのですが、番号の若い方を優先してindex=6を次回の探索拠点にします。index=22を選べばすぐにゴールインできそうですが、若い番号順というルールにしたのでそれに従います。

上図右、今まで同様新規ノードを開拓します。index=1と5が探索拠点候補(黄色:status=1)として加わりました。
まだまだscore=6が最小なので、次は若い番号順でindex=7を探索先にします。

上図左、index=7が探索拠点です。周囲を調査しindex=2と8が探索拠点候補に加わりました。index=8も最小score=6です。番号の若い順でindex=8が次回の探索拠点になります。

上図右、index=8の周囲を調査しindex=3と9が探索拠点候補に加わりました。index=9もscore=6なので、若い番号順でindex=9が次回の探索拠点になります。

上図左、index=9は行き止まりですが、ゴールに近いため探索しようとしてしまいます。結果index=4が探索拠点候補に加わりましたがscore=8なので、引き続き最小score=6で若い番号順でindex=10が次の探索拠点になります。いちおうindex=9の行き止まりから方向転換したという感じです。

上図右、index=10が探索拠点ですが、もうすでに周囲は探索されているので、そのままindex=10は用済みとしてグレー(status=2)にして、引き続き最小score=6で若い番号順のindex=15を次の探索拠点にします。

上図左、index=15が探索拠点。index=20が探索拠点候補に加わりましたが、あいかわずscore=6が最小なので、次はindex=21が探索拠点になります。

上図右、index=21の周囲もすでに探索されているので、用済み(status=2)にして、最小score=6のindex=22を次の探索拠点にします。

上図左、ここまでくるとほぼ結果は見えますが、index=22が探索拠点となり、右隣のindex=23だけを新規に探索拠点候補に加えました。index=23が最小score=6で次回の探索拠点へ。

上図右、index=23が探索拠点、index=24が探索先候補に加えられ、score=6が最小なのでindex=24が次回の探索拠点へ。

上図左、index=24が探索拠点。いままで通り周囲を調査するとgoal(index=19)を発見しました。ここで、探索アルゴルズムは終了です。あとはgoalにもprevを記入して、それぞれの一歩前のノードを見ていきます。

上図右、まずgoalにはprev=24が記入してあるので一歩前はindex=24となります。index=24にはprev=23が記入してあるので、さらに一歩戻ってindex=23へ。同様に遡るように辿っていくと、
goal(index=19、prev=24)
→index=24(prev=23)
→index=23(prev=22)
→index=22(prev=21)
→index=21(prev=16)
→index=16(prev=11)
→start(index=11)
とつながっていき、この順番(赤枠のルート)を反転すれば最終的なルートになります。

最小scoreのノードが複数あるときに、今回はindex番号の若い順で探索させましたが、この順番を変えればまた微妙に違ったルートになると思います。
上の例では、次回の探索拠点候補のノード(黄色)と探索拠点済みのノード(グレー)に関しては、二度目以降調査するときはスキップしていましたが、実際のアルゴリズム(以下)では、今回調べたcostと前回調べたcostを比較して、今回調べたcostのほうが少ない場合には内容を更新します。

ーーーーーーーーーーーーーーーーーーーーーーーーー

以下からは実際のプログラムの説明。

このページ下の方にブラウザ上で実行可能なサンプルを掲載しておきました(Chrome+Macでは動作確認とれています)。


マップについて:
マップの大きさは横列rows=10と縦列columns=10の変数を用意し10x10の正方形。変数rowsやcolumnsに数値を代入すれば変更可能にしてあります。
map[index]という配列を用意して、10x10ならば、最初の左上マスはmap[0]、右下の最後のマスはmap[10*10-1]となります。XY座標に置き換える際には、int X=index%rows、int Y=index/rowsとなります。map[index]=0:通路、1:壁、2:スタート地点、3:ゴール地点という感じで種類分けします。主には壁の有無による通過可能かどうか、あるいは表示用の色分けのために使っています。場合によっては以下のstatus用配列と一緒にしてしまってもいいかもしれません。

メインノードとサブノードについて:
メインノード(現在地:探索拠点)に対する周辺4つのサブノードを調べる部分は、今回はメインノードから見て北東南西の4方向を探索、同時に移動可能(斜め移動はなし)。単に北から時計回りに探索することにしました。メインノードの位置をmainNode(map[index]に代入されるindexの部分に相当する配列の番号)とすれば、北:mainNode-rows、東:mainNode+1、南:mainNode+rows、西:mainNode-1になります。これをsubNode[i]という配列(iは0〜subNodeの配列の長さまで)を用意して4箇所調べていきます。

ステータスについて:
今回は、status[index]という配列を用意して、各ノード(マス)が0:未探索、1:次の探索拠点候補に入れる(オープン状態)、2:探索終了(探索拠点候補から外す:クローズド状態)に分けます。探索対象ノードのステータスによって処理する内容が異なるのでそれの手がかりとして利用します。ネットなどで調べると、None、Open、Closedなどと種類わけされています。あるいは、Open用の配列、Closed用の配列を別々に用意している時もあります。Open用の配列は別途あると便利かもしれませんが、今回は一つのstatus[index]という配列でそれぞれのフラグを立てることで状態分けをしてしまおうと思います。毎回マップ上にある全てのノードをスキャンしないと、どれがどの状態であるか調べられないので、マップが巨大になると遅くなってしまうかもしれませんが、小さいマップであればおそらく問題ないでしょう。
未探索ノードであれば最初は0で、4方向を調べると同時にオープン状態status[subNode[i]]=1にします。オープン状態のノード中から最小値のスコアを持っているノードを選び出し(過去の探索でオープン状態にしたノードも含む)、次にそこにmainNodeは移動します。その時点でその最小値のスコアを持っていたノードは用済みとなって、status[index]=2でクローズドにします。用済みとなっても、後の探索手続きによってオープンに復活する場合もあります。

ヒューリスティックについて:
ネットなどの説明を読むと難しそうなことが書いてありますが、この場合は単に現在地(探索地:サブノード)からゴールまでの距離のことです。いくつか求める方法がありますが、今回の場合は4方向移動なので、マンハッタン距離(縦横の移動マス数)で求めることにしました。ゴールのindex番号をXY成分に置き換えてgx=goalIndex%rows、gy=goalIndex/rows、同様にサブノードもsubx=subNode[i]%rowsとsuby=subNode[i]/rowsで求め、それぞれの差分の合計となります。これもまた各ノードに対するheuristic[index]という配列を用意して、
heuristic[subNode[i]]=abs(goalIndex%rows-subNode[i]%rows)+abs(goalIndex/rows+subNode[i]/rows)
絶対値abs()を使って常にプラスになるようにしてあります。このほか、マンハッタン距離ではなく、三平方の定理で直線距離を求める方法や斜め移動可能なマンハッタン距離を変形したような求め方もあるようです。

コストについて:
コストはスタート地点から現在探索中のサブノードまでの実際の道のりです。heuristicの時のマンハッタン距離とは求め方が異なります。当初はコストについてもマンハッタン距離で求めていましたが、遠回りしないといけないような長い壁があるときは、距離的にゴールに近い方向ばかりを探索してしまって、迂回した方が実際は近いのにそう動いてくれなかったという時がありました。おそらく実際の道のりと架空の距離との違いでそうなってしまうのでしょう。マンハッタン距離や三平方の定理による直線距離は数式で簡単に求められますが、この実際のコストは歩いた道のりを毎回記録していかなければ求められないので、やや面倒です。
まずは、一歩進むごとにコストを+1するためのcost[index]配列を用意します。スタート地点ではコストは0です(スタート地点からの道のりなので)。スタート地点でサブノード(4方向)を調べた時に、メインノードのコストに+1したコスト数をサブノードに記録しておきます。cost[subNode[i]]=cost[mainNode]+1という感じです。最初の一歩はmainNodeがスタート地点なので0+1でcost[subNode[i]]=1です。2歩目以降は、常に前回探索したサブノードが今回のメインノードとなるために、現在のメインノードのコストに+1すれば、新たなサブノードのコストが求められるというわけです。このようにして探索したすべてのノードのコストを記録し続けます。

スコアについて:
スコアはscore=cost+heuristicとなります。原則的には、この合計したスコアの値が最小のものから探索拠点になるという手順になります(ただしオープン状態のサブノードのみ)。初めて訪れたサブノードには、この値を記録しておきますが、costとheuristicを記録する配列などがあれば、別々に記録しておいても構わないと思います。スコアの値によって次の探索候補にするかしないかの手がかりになりますが、同時にそのマスがオープンかクローズドかによっても手続きが変わってきます。

一歩前のノードの記録について:
これもprevNode[index]などと配列を用意しておき、移動ごとにそのノードに記録していきます。探索中の各サブノードの一歩前のノードは、その中心にあるmainNodeなので、そのメインノードの位置(index番号)を記録しておけばいいということです。最終的にゴールに辿り着いた時に、ゴールの一歩前、そしてその一歩前という感じで、スタート地点まで逆向きに辿って行くことができます。そのルートを空の配列にゴールから一つずつ入れていき、最後に反転すればスタートからゴールまでのルートが出来上がるというわけです。このために必要な配列となります。
ただし、探索中に前回調べたサブノードに遭遇した時(迂回している時など)、そのサブノードには前回の一歩前のノードが記録されており、今回のmainNodeとは異なる時があります。その場合には、前回か今回のどちらかが遠回りをしている場合が考えられるので、サブノードの今回の調べたcostと前回のcostの値を比較します。もし今回のcostの方が少なければ、前回は遠回りしていた可能性があるので、今回のcostに書き換えます。ならびに、一歩前のノードも今回のmainNodeに書き換えておきます。さらに、そのサブノードがクローズド(用済み)になっていた場合はオープン(再度探索可能対象にする)に切り替えます。

全体的な流れ:
まずメインループに入る前に初期設定の段階で、スタート地点、ゴール地点の位置(今回の場合は配列のインデックス番号)を決めておきます。そして、スタート地点をオープン状態にし、スコアも求めておき、以後のメインループ突入の下準備を整えておきます。
そしてメインループに入ります。まずオープン状態のノードからスコア最小値のノードを選び出します。1回目のループでは、スタート地点しかオープン状態になっていないため、自動的にスタート地点がスコア最小値のノードになります。スコア最小値ノードが決定されたら、このノードをクローズド(用済み)にしてしまいます。もしオープンリストやクローズドリストなどの配列を使っているなら、オープンリストからそのノードを消去して、クローズドリストに入れ直します。
このスコア最小値ノードが次回の探索用メインノードとなり、メインノードの周辺(4方向のサブノード)を順番に調べます。サブノード(壁やマップ範囲外は除く)のstatusが新規の場合はオープンにして次回の探索候補ノードにしておきます。同時にcost、heuristic、prevNode(一歩前のノード:つまり現在のメインノードの位置)も各配列に記録します。
サブノードが新規ばかりではなくオープンかクローズドの場合もあるので、その場合はそのノードの以前記録したcostを調べて今回のcostと比較します。迂回などにより、前回記録したcostが今回調べたcostと違う場合も発生します。クローズドで用済みになっていたとしても、今回のcostが前回調べた時のcostより小さい場合は、そのノードの内容を書き換えなければいけません。書き換えられたクローズド状態のノードは、再度探索拠点候補になるかもしれないので、オープン状態に戻しておき、当然そのノードの一歩前のノードprevNodeも今回のメインノードに書き換えておきます。
オープン状態のノードの場合は、前回のcostよりも今回のcostの方が小さければ、同様に今回の小さい方のcostに書き換えておき、一歩手前のノードも今回のメインノードに書き換えます。オープン状態なのでまだ探索拠点候補にしたままにしておきます。
あとはゴールを見つけるまでこの手順をループします。

スコア最小値ノードが複数ある場合:
新規のノードが常に次回の探索拠点候補(オープン状態)に加えられ、毎回最小スコアのノードを一つだけ選ぶため、オープンノードは徐々に増えていきます。最小値スコアのノードが複数ある場合は次の回に同スコアのノードを探索することになります。つまり一つのスコア最小値ノードを探索している間は、他のスコア最小値ノードは順番待ちしているという状態です。

この画像では、黄色が最終的なゴール(赤)までのルート、周辺の緑がクローズドされたノード、さらに周縁部の水色がオープン状態のノードです。これを見ると分かるように、オープン状態のノードは探索したエリアの外周付近にあり、これらのうちの最小スコアのノードが次のループで選択され、探索エリアを拡大していきます。この時、複数あるスコア最小値ノードのどれを優先するかは、あまり重要ではないようですが、確かにどれを選ぶかで道筋は多少変わってしまうこともあると思います。壁の右を通った方がいいのか、それとも左を通った方がいいのか、ただし結果的にどちらを通っても最終コストが同じであれば、どちらでもいいということになります。細かくは検証していないのでわかりませんが、ヒューリスティックは壁の有無に関係なしの距離なので、実際遠回りしなければいけない時はあてになりませんが、コストは実際の道のりなので、これまでのスタート地点からのコストが少ない方が、最終的には近道になるのだと思います。
例えば、現在3個のノードが同じ最小値スコアだったとします。そうすると、まずどれでもいいので(今回の場合は北東南西という順番にしていることや配列の若い番号順などの演算順による)一つをメインノードとしてその周辺4方向を探索します。その結果いくつかの新規ノードがオープン状態に加えられて、それらも含めてまた最小スコアのノードを選びます。この時、新規ノードの中に最小スコアがなければ、先ほどの最小スコアのノード(3個のうちの2個目)に移動して引き続き探索します。それでも最小スコアが新規ノードになければ、3個目の最小スコアのノードを調べます。途中で現在の最小スコアよりも小さいスコア値を持った新規ノードが現れれば、そのノードに移動していきますが、それでもない場合は、今回の最小スコアの3つのノードは全滅で、次に最小なノードを選ぶということ(処理的には戻る感じ)になります。大きな壁があったり、行き止まりや進む方向がずれている場合はこのような感じになると思います。途中でより最小のスコア値を持つ新規ノードが発見された場合は、当然そちらに移動していき、全体的な流れとしては、進みながら戻るということを繰り返します。戻るというのは、ゴールまでの最短道のりにふさわしくない場合に、方向転換して他の可能性を探し始めるということにつながります。その分、処理は増えてしまいます。
見た目的には、ゴールに近そうなノードだけを次々と突き進むように探索するイメージですが、最短ルートから大きく外れるような迂回が突然発生すれば、その分コストもヒューリスティックも増えてしまうため、距離的にゴールに近い部分だけでなく、すでにだいぶ前に探索した付近に戻ったりしながら探索エリアを拡大しつつゴールに向けての最短道のりを探します。

今回は、クラスを使わないので、マップ描画用の配列、コスト用配列、ヒューリスティック用配列、一歩前のノードを記録する配列などを別々に用意しています。
ファンクションも敢えて分けないで、メインループの中に全部書いておきました。見にくいかもしれませんが、一応上から順に追っていけば、どのような手続きになっているかが分かると思います。
プログラム開始直後は、ループ待機状態で、マップ上をクリックすることで壁の有無を切り替え可能、キーを押すと探索開始。薄緑は探索済みエリア、黒は未探索エリア、グレーが壁。

//A*経路探索アルゴリズム
//
//マップ作成用の主な変数
int rows=10;//横列の数
int columns=rows;//縦列の数
int gridSize;//グリッドのサイズ
float wallDensity=0.25;//マップ上に25%の割合でランダムに壁を入れる変数

//探索手続きに必要な配列
int[] map=new int[rows*columns];//マップ用配列、0:何もなし、1:壁、2:スタート、3:ゴール
int[] status=new int[rows*columns];//各ノードの探索済み状態、0:未探索、1:オープン、2:クローズド
int[] cost=new int[rows*columns];//コスト用配列:各ノードのスタートからのステップ数(カウント制)
int[] heuristic=new int[rows*columns];//ヒューリスティック用配列:各ノードのゴールまでのマンハッタン距離
int[] prevNode=new int[rows*columns];//一歩前のノードの位置

//スタート、ゴール、探索開始点の変数
int startNode=0;//スタート地点のノード位置(インデックス番号)
int goalNode=rows*columns-1;//ゴール地点のノード位置(インデックス番号)
int mainNode;//探索中の現在地のノード位置(インデックス番号)

//探索終了後の発見ルート表示用の変数など
boolean found=false;//ゴール発見用フラグ
boolean start=false;//探索開始用フラグ
boolean routeStart=false;//ルート表示フラグ
int[] route={};//ルート用配列
int cnt=0;//ルート表示用カウンタ

//初期設定
void setup() {
  size(600, 600);//画面サイズ
  gridSize=width/rows;//マップの1グリッドの大きさ
  for (int i=0; i<rows*columns; i++) {//マップ用意
    if (random(1.0)<wallDensity) {//ランダムで壁配置
      map[i]=1;
    }
  }
  map[startNode]=2;//スタート地点
  map[goalNode]=3;//ゴール地点
  mainNode=startNode;//探索用メインノード
  status[startNode]=1;//スタート地点をオープンにしておく
  cost[startNode]=0;//スタート地点のコスト:0
  heuristic[startNode]=goalNode%rows+goalNode/rows;//スタート地点のヒューリスティック
  stroke(100,50,50);
  ellipseMode(CORNER);
}

//メインループ
void draw() {
  background(0);//背景(黒)
  //マップ描画開始(色分け、壁の有無)
  for (int i=0; i<rows*columns; i++) {
    if (map[i]==1) {//壁(グレー)
      fill(200);
    } else if (map[i]==2) {//スタート地点(緑)
      fill(100, 200, 100);
    } else if (map[i]==3) {//ゴール地点(赤)
      fill(200, 100, 100);
    } else {//通過可能エリア
      if(cost[i]+heuristic[i]>1){
        fill(50,100,50);//探索済み(薄緑)
      }else{
        fill(0);//未探索(黒)
      }
    }
    rect(i%rows*gridSize, i/rows*gridSize, gridSize, gridSize);//グリッド描画
  }
  fill(100, 250, 100);//探索用メインノード(緑:丸)
  ellipse(mainNode%rows*gridSize, mainNode/rows*gridSize, gridSize, gridSize);

  if (start) {//キーを押してループ開始
    //先ずはゴール発見後の処理から
    if (found) {//ゴールを発見した場合
      int node=goalNode;//ゴールからスタートまで逆戻りに辿るための変数
      if (routeStart==false) {//最終ルート表示フラグ
        while (node!=startNode) {//ゴールからルートを組み立てる
          route=append(route, node);//ルート用配列に追加していく
          node=prevNode[node];//各ノードの一歩前のノードを代入していく
        }
        route=append(route, startNode);//最後にスタート地点を追加
        route=reverse(route);//ルート用配列の順序を反転させる
        routeStart=true;//ルート描画開始用フラグ
      } else {//発見したルートを表示
        mainNode=route[cnt];//メインノードをルート配列順で表示
        cnt++;//ルート表示用カウンタ
        if (cnt>route.length-1) {
          cnt=0;
        }
      }
    } else {
      //ここから探索処理の開始
      int minScore=rows*columns;//最小スコア(ダミー変数)
      int minNode=-1;//最小スコアのノード(エラー検出用の-1)
      //オープン状態のノードの中から最小スコアのノードを探し出す処理
      for (int i=0; i<rows*columns; i++) {//最小スコアとそのノードを求める
        if (status[i]==1) {//探索済みオープン状態のノードの場合
          minScore=min(minScore, cost[i]+heuristic[i]);//最小スコア(コスト+ヒューリスティック)を求める
          if(minScore==cost[i]+heuristic[i]){//最小スコアの時のノードを求める
            minNode=i;//最小スコアのノード(インデックス番号)
          }
        }
      }
      if(minNode==-1){//もし最小スコアのノードがない場合
        println("NO WAY!");//探索失敗
        noLoop();//ループ停止
      }else{//探索可能な場合
        mainNode=minNode;//最小スコアのノードをメインノードにする
        status[minNode]=2;//メインノード(最小スコアノード)をクローズドにしておき、以後サブノードを探索
      }
      //サブノード(4方向)の探索開始
      int[] subNode={mainNode-rows, mainNode+1, mainNode+rows, mainNode-1};//サブノード:北東南西方向

      for (int i=0; i<subNode.length; i++) {
        //サブノードがマップ範囲外の場合の探索スキップ処理
        if (subNode[i]<0||subNode[i]>=rows*columns) {//マップ南北の領域外
          continue;//スキップ
        }
        if ((i==1&&subNode[i]%rows==0)||(i==3&&subNode[i]%rows==rows-1)) {//マップ東西の領域外
          continue;//スキップ
        }
        if (map[subNode[i]]==1) {//壁の場合
          continue;//スキップ
        }

        //先ずはゴール発見時の処理
        if (subNode[i]==goalNode) {//サブノードの一つがゴールの場合
          cost[subNode[i]]=cost[mainNode]+1;//コストを+1加えておく(メインノードのコスト+1)
          prevNode[subNode[i]]=mainNode;//一歩手前のノードをメインノードとして記録しておく
          mainNode=subNode[i];//メインノードを次のノード(ゴール)へ移動
          found=true;//発見用フラグ
          break;//探索ループ脱出
        }

        //ここからサブノードが探索可能な場合の処理
        if (status[subNode[i]]==0) {//サブノードが未探索の場合
          status[subNode[i]]=1;//サブノードをオープン状態にする
          cost[subNode[i]]=cost[mainNode]+1;//コストを+1加えておく
          //サブノードのマンハッタン距離を計算してヒューリスティック用の配列に記録しておく
          heuristic[subNode[i]]=abs(goalNode%rows-subNode[i]%rows)+abs(goalNode/rows-subNode[i]/rows);
          prevNode[subNode[i]]=mainNode;//一歩前のノードの記録
        } else if (status[subNode[i]]==1) {//探索済みでオープン状態のノードの場合
          if (cost[subNode[i]]>cost[mainNode]+1) {//現在のサブノードのコストが、以前探索した時のコストより小さい時
            cost[subNode[i]]=cost[mainNode]+1;//より小さい今回のコストを優先して値を書き換えておく
            prevNode[subNode[i]]=mainNode;//一歩前のノードも今回のメインノードとして記録しておく
          }
        } else if (status[subNode[i]]==2) {//探索済みでクローズドのノードの場合
          if (cost[subNode[i]]>cost[mainNode]+1) {//現在のサブノードのコストが、以前探索した時のコストより小さい時
            cost[subNode[i]]=cost[mainNode]+1;//より小さい今回のコストを優先して値を書き換えておく
            prevNode[subNode[i]]=mainNode;//一歩前のノードも書き換えておく
            status[subNode[i]]=1;//オープン状態に切り替えて、次回探索候補ノードに入れておく
          }
        }
      //以上で一回分の探索処理終了。ゴール発見まで繰り返しこの処理をループする
      }
    }
  }
}

//マップ上の各グリッドをクリックすることで壁の有無を切り替える
void mousePressed() {
  int id=mouseX/gridSize+mouseY/gridSize*rows;
  if (map[id]==0) {
    map[id]=1;
  } else if (map[id]==1) {
    map[id]=0;
  }
}
//キーを押すと探索開始/一時停止
void keyPressed(){
  start=!start;
}

分かりにくかったのは、各ノードのコストをスタート地点からステップを数えて累積して行く部分でした。最初のうちは、コストはスタート地点から探索地までのマンハッタン距離、ヒューリスティックも同じくマンハッタン距離で計算していましたが、それだとゴールには辿り着けても最短距離にはならなかったりしてやや変な動きをします。
探索はスコアの少ない順から未探索ノード、オープンノード、クローズドノード関わらず調べて行くので、どうやって複数の異なるノードのステップ数を距離ではなく道のりで数えればいいのか、ネットの説明を読んでもなかなか分かりづらかったという感じです。探索中の4方向のサブノードに対して、中心のメインノードはサブノードからすれば一歩手前の存在となるので、それを毎ループ記録しておけばいいということにやっと気がついたという感じです。最短距離は導き出せるようになったのですが、それでも探索順に癖があるので、処理回数を減らすためにはもう少し工夫が必要という感じです。
変数rowsに20を代入しグリッド数を増やして実行して見ると分かるのですが、この探索方法だと左上から右下に向かって対角線で進むというよりは、やや最短距離から下へ膨らんだルートを取りがちです(南下して東に進む傾向がある)。それでも一応最短距離にはなります。
ヒューリスティックの計算方法を変えると、対角線状に進むようにはなったのですが、探索範囲がかなり幅広に膨らんでしまいます。結果的には、前者の方が処理数は少なくて済むし、対角線ではないにしても最短距離にはなっているので、いいのかもしれません。まだ工夫の余地はあるのかもしれませんが、今のところこんな感じ。

追記: その後、少し改良して以下のようになりました。アルゴリズム部分はあまり変わっていません。
*表示画面内を空クリックしてから(フォーカスを与えてから)、キーを押して下さい。

・黄緑:スタート(ドラッグで移動可能)
・赤:ゴール(ドラッグで移動可能)
・グレー:壁(クリックで壁の有無を切り替える)
・黒:通路(クリックで壁の有無を切り替える)
・↑(UP)キー:グリッドを増やす(最大160x160グリッド)
・↓(DOWN)キー:グリッドを減らす(最小10x10グリッド)
・nキー:リセット
・その他のキーで探索開始/一時停止

*黄緑丸は現在の探索位置、緑は探索済み(クローズド)エリア、水色はオープン状態のノード、黄色が最終的なゴールまでのルートです。
*ProcessingのコードをP5.jsに書き換えたもの(以下)。


グリッド数を増やして実行すると、どのように探索しているかが分かると思います。とりあえずはゴールに向かって行くのですが、次第にコスト(スタート地点からの道のり)も増えてくるので、ゴールに向かうことが優先されるというよりは、一旦引き返して途中の探索エリアも拡張します。同じスコアのノードが探索エリアのアウトライン付近に分散しているのが分かるかと思います。先端(ゴールに最も近い探索エリア)を伸ばすためには、途中の探索エリアもある程度太くしていかないとダメという感じです。
A*の場合は、ゴールの位置がわかっている前提での最短距離(最短道のり)を導き出すアルゴリズムのようで最速というわけではないようです。この最短と最速というニュアンスは同じ意味だと思っていましたが、最速というのは道のりというよりは演算処理のことで、どれだけ少ない処理回数で答えを導き出すかという違う意味のようです。
当初は何かAIのような賢さがあるのかと思っていましたが、それほど複雑なことではなかったようです。どちらかというと条件に対して処理をしているだけという感じです。そもそもゴールの位置がわかっている前提なので、ゴールを探し出すというニュアンスとも違っています。
A*アルゴリズムの中のヒューリスティックを取り除けば、ダイクストラ法になるようです。ダイクストラ法の場合は、ゴールがわからない前提でスタート地点を中心に同心円状に探索していきます。実際どこにあるか分からないゴールを探しつつ最短距離を導き出すアルゴリズムなので少しAIっぽい感じはしますが、ゴールを見つけるまでは、ひたすら探索範囲を四方八方に拡大していかなければならないので、結局は量がなせる技という感じです。A*はヒューリスティックがある分、ゴールへの指向性があるという感じでしょうか。

0 件のコメント:

コメントを投稿

人気の投稿