グリッド化したマップ上で，グラフ理論を使った経路探索を行いました．
マップをグリッド化する際のイメージは以下です．

各ノードは上下左右のノードと接続され，グラフのエッジ重みにはノード間の距離(0.1[m])を設定しています．経路探索にはダイクストラ法を使いました．
探索結果は以下です．

また，上下左右以外に斜め（対角上の）ノードにも接続させた場合の経路探索結果は以下です．

この方法ではグラフを単純にマップをグリッド化することで作成しているだけなので，実際のロボットや障害物の位置は各グリッド点一番近い点等で決めるしかありません．
実際にその手法で経路を計画した結果が以下です．

グリッドの離散化粒度によっては通れるはずのスペースを通れないと判断してしまったり，逆に通れないスペースを通れると判断してしまう可能性があります．
グリッドを細かくすると明らかに不要なノードも増えて，計算時間も増えてしまいます．

なので，マップを単純にグリッド化することでグラフを得るのではなく，ボロノイ図や可視グラフからグラフ構造を得ることが実用上，有用であると思います．

今回の問題設定ではあんまり障害物ギリギリの経路を走行するよりか，障害物との距離に余裕を持たせて走行させたいのでボロノイ図からグラフを作成してみようと思います(次回以降)

ボロノイ図や可視グラフを用いるためにには障害物が多角形で表現されている方が良いのでまずは障害物の点群から凸包を作成するロジックを実装しようと思います(アンドリューの手法を実装予定)