AbudoriLab.です。
今回は経路計画の記事第二弾として、ダイクストラ法について説明します。
第一弾として幅優先探索と深さ優先探索について説明した記事はこちらです：
【第一弾のあらすじ】
・基礎的な経路計画の方法として、幅優先探索と深さ優先探索を紹介しました。
・深さ優先探索では最良経路を得られると限らないので、幅優先探索を使うべきであることを示しました。
・しかし、幅優先探索も最良経路を求められるのはマス目の重みが平等な地図に限られるため、マス目の重みが異なる地図ではダイクストラ法などの別のアルゴリズムを用いるべきであることを説明しました。
それでは、今回の記事に入っていきましょう。
マス目の重みが異なる地図
前回の記事では下図のようなグリッドマップでの経路計画として、幅優先探索と深さ優先探索を紹介しました。
幅優先探索では全てのマス目の重みが平等という前提条件があり、この前提が崩れると最良な経路を計画できなくなります。
マス目の重みが平等とは、下図のような感じです。
スタートとゴール以外のマス目に重みが設定されていますが、その値が全て1で等しくなっています。（平等であればいいので、値は1でなくても何でもいいです。）
マス目の重みが平等な地図の特徴として、最良(=最短)経路が複数存在しやすいことが挙げられます。
上の地図における最短経路を二つ示します。
左はジグザグ、右は直線的な経路ですが、いずれもスタートからゴールまで通っているマス目の数は5で等しく、どちらも最良経路になります。
右：直線的な経路
いずれも累積重みは5
上図において緑記載した数字は累積重みという数値です。
累積重みの定義は下記の通りです：
あるマス目の累積重みとは、スタートからそのマス目までを進んだ経路上の重みの累積値になります。
定義よりスタート地点では0です。
上図右では経路上のマスの重みは全て1なので、一つ進むごとに累積重みは1ずつ増えていきます。
ゴールに辿り着くまでに5個のマス目を通過しているので、ゴールの累積重みは5となっています。
難しいことを言っているようですが、今回はマス目の重みが全て1なので、通ったマス目の数=累積重みになります。
ではマス目の重みが平等でない地図ではどうなるでしょうか。極端な例を挙げて考えてみます。
下図のグリッドマップはマス目の重み(黒字)が1 or 100(スタートとゴールでは0)になっています。
左：重みが1のマス目だけをうまく通り、累積重みは5
右：重みが1のマス目を全く通らず、累積重みは500
最良経路=マス目の累積重みが最小になる経路、を達成するには左図のように重みが1のマス目をジグザグに通るしかありません。
しかし幅優先探索は全ての重みが平等に見えてしまい、最良経路を計画できるとは限らないのです。
例えば、右図のように直線的に通過した場合ではマス目の数は5個で左と同じですが、重みが100のマス目しか通っていないので累積重みは500となってしまいます。
幅優先探索は右と左の違いを区別することが出来ませんので、累積重みに最悪100倍の差が生じうるということです。
このような設定では、ゴールでの累積重みが最小となるような経路を計画することが求められます。
マス目の重みが異なる地図の具体例
マス目の重みが異なる地図は、ロボットを考えるとどんな場合に現れるでしょうか。
例えば不整地のような各地点で凹凸がある場所を動くロボットを考えます。
凸凹の激しさや傾斜の大きさをマス目の重みとすれば、最良経路は凹凸の少ない、穏やかな経路となります。
占有格子地図では、障害物が存在する確率を各マス目の重みとします。
この重みが出来るだけ小さくなる経路計画を行えば、障害物を避けて移動を行うことができます。
つまり、重み付きの地図を用いることで、通ることが不可能ではないが、なるべく通りたくない場所（凸凹が激しい場所や、障害物の存在確率が高い場所）を避けられる経路計画が実現できるわけです。
それゆえ、リアルでのロボットの経路計画ではマス目の重みが異なる地図が必要となるケースが多いです。
重みがある場合に幅優先探索では最良経路が計算できないので、新しいアルゴリズムが必要になります。
ダイクストラ法とは
そんなマス目の重みの異なる地図でも、累積重みが最小となる経路が計画できるアルゴリズムがダイクストラ法です。
両者の特徴を一言で言うと下記の通りです。
幅優先探索：隣接するマス目から順番に探索していく
ダイクストラ法：隣接するマス目で、累積重みが小さいものから順番に探索していく
あまり正確ではないですが、下図にイメージレベルで両者の違いを表現しました。
幅優先探索は、探索開始地点から近場のマス目を手当たり次第にブワーっと総なめしていく探索でした。
ダイクストラ法では各マス目の累積重みを予め計算しておき、累積重みが小さい順に優先順位をつけて探索していきます。
探索に優先順位があることが、両者の差分となります。
幅優先探索は近い順から調べるのでスタートに隣接した4つのマス目に優先順位はありません。
とりあえず時計回りにスイープしています。
ダイクストラ法はマス目の累積重み（今回の図ではマス目自体の重みとニアリーイコールになっていますが）が小さい順に探索しています。
ダイクストラ法のアルゴリズム
ダイクストラ法のアルゴリズムを汎用的なグラフ表現において説明します。
どうしてもアルゴリズムや数学の話で難しくなってしまいますので、数式が苦手な方は本章はスキップして「実験」の章へ行きましょう。
説明に使用するグラフは下図の通りで、ノードA, B, C....を繋ぐ各エッジに重みが割り当てられています。
このようなグラフを重み付きグラフと言います。
次のノードに移動する時に数字の値だけコストがかかる、と考えてみてください。
このグラフにおいて、スタートからゴールまでの累積重みを最小化する経路を求める問題を考えます。
ダイクストラ法のアルゴリズムの流れをアニメーションで示したのが下図です。
まず累積重みをスタート：0, その他：∞で初期化し、累積重みを更新しながら、累積重みが最小の未訪問ノードをその都度探索していく、というのが大まかな流れとなっています。
累積重みという概念を導入することで、重み付きグラフに対しても最良経路を逐次探索できています。
最終的にゴールの累積重みは5で、これを達成する経路が最良経路となります。
今回の場合はS→C→F→Gです。
経路は、累積重みの更新時にどの隣接ノードの累積重みを使ったかを記憶しておくことで辿ることができます。（例えばGの累積重みは6→5で更新される時、Fの累積重み4+エッジの重み1で5と計算されるので、経路においてGの手前はFです。これを繰り返してスタートまで辿ることができます。）
グラフとグリッドマップは相互に変換することができます。
マス目に重みがある場合は、下図のようにノード間のエッジに重みがあるグラフとして表現ができます。
隣のマス目へ移動する際は、重み分だけコストが必要になります。
矢印が登場してややこしく見えますが（このようなグラフを有向重み付きグラフといいます）、グリッドマップで考える時は隣のマス目の重みを参照するだけですので、実際は単純な構造となっています。
実験
ダイクストラ法を実行可能なプログラムを作成しました。
マス目の重みが異なる地図でのダイクストラ法の挙動を観察してみましょう。
プログラムはpython3で記述されており、以下のリンクよりダウンロード可能です。
(なお今回の実装はnumpy芸で何とかした感が強く、あまり綺麗ではないです。。ダイクストラ法は本当は使うデータ構造で計算速度が結構違ってくるのですが、そこまで実装&解説できませんでした。興味のある方は調べてみてください。)
今回のプログラムで使用する地図は10x10の2次元格子地図で、前回の記事で用いた地図と基本的には一緒ですが、スタートとゴール以外のマス目の重みが1-9で設定されています。重みは緑色の濃淡で表現され、濃いほど大きく、薄いほど小さいです。
では早速プログラムを実行してみましょう。下のコマンドを実行してください。
$python3 dijkstra.py
実行した結果の動画が下図です。
近い順に平等に探索されていた幅優先探索と異なり、重みが小さそうな、薄い色のマス目から優先順位をつけて探索されていく様子がわかります。
地図に対してどのような経路が計画されたのか見てみましょう。
左の地図でオレンジの枠で囲った部分は重みが大きいマス目ですが、スタート、ゴール近くではこれらのマス目を避けてカクカクした経路になっていることが観察されます。
比較のために、前回の記事で紹介した幅優先探索の経路を再掲します。
幅優先探索ではマス目の重みを無視して経路を生成するので、上図オレンジで囲ったマス目を幾つか通ってしまっていますね。
プログラムでは以下の部分で乱数のシードを設定しているので地図が固定されていますが、ramdom.seed(3)の3の部分を別の数字にしたり、ramdom.seed(3)の行をコメントアウトすることでランダムな地図を生成して遊ぶことができます。
是非色々な地図でダイクストラ法を実行してみてください。
なお前回同様、GitHubのプログラムは結果のアニメーション表示が可能ですが、動画ファイルとして保存することはできません。
動画を保存するにあたっては下記リンクのスクリプトを利用させて頂きましたので、保存をしたい方はこちらのリンクを見て適切にコードを変更してください。
重みが平等な地図でのダイクストラ法
前の章で、幅優先探索は近場のマス目から順番に探索していく方法で、ダイクストラ法は累積重みが小さいマス目から順番に探索していく方法であることを述べました。
実は、全てのマス目の重みが平等の時は、「近場のマス目」=「累積重みが小さいマス目」となります。
すなわち、全てのマス目の重みが平等な地図上でダイクストラ法を実行すると、幅優先探索と同じ結果が得られます。これを実験で確かめてみましょう。
具体的には、地図の重みをランダムに設定している部分をコメントアウトし、全て1としてみましょう。
まとめ
本記事ではダイクストラ法による経路計画を紹介しました。
この手法は幅優先探索のように前提条件に依存せず、全てのケースで最良経路を計算できるアルゴリズムです。
しかし、スタートからくまなく探索していくダイクストラ法は、探索する地図が大きくなればなるほど計算時間がかかるようになります。
そこで次回の記事では計算量がよりスマートな経路計画である、A*(エースター)を紹介する予定です。
