今までやってきた経路計画と制御を組み合わせて，目的地まで移動するシミュレーションを行いました．
机は横1.2[m]，縦0.7[m]を想定しており，以下のように0.1[m]ごとにグリッド化しました(斜め方向も接続，ロボットの一辺も約0.1[m])．エッジの重みはノード間の距離をそのまま使用しています．

机の端は走行してほしくない(落ちる)ので大外のノードは除去します．

また，障害物が存在するときはその周辺ノードも除去します．

以上のグラフの作り方でシミュレーションした結果は↓です．

(赤丸が目的地，緑丸が障害物をあらわしています)

左上のロボットの経路抜き出したのが↓です．

X軸が0.5[m]と0.6[m]あたりでロボットが通れそうな空間があるのですが，今回の0.1[m]間隔のグリッドでは通れないという判断になってしまっています．

グリッド間隔を0.05[m]に変えて経路を計算するとちゃんと↓の経路が求まります．

このように離散化粒度が荒いと通れる場所が通れないと判断され，遠回りをしてしまいます．この問題を解決するには上記のようにもっと細かく離散化すれば良いのですが，今の単純な障害物周辺の4点ノードを除去する方法では，今度はロボットが通れないスペースも経路計算してしまうこともあります．なので，障害物の周辺の除去するノードをロボットの大きさを考慮して決めてあげなければいけません(configuration空間を考えてあげる)．

離散化粒度が違うと以下のように大きく経路が異なることもあるのでconfiguration空間上でなるべく離散化粒度を細かく設定してあげようと思います．

【残課題】

①ロボットの大きさを考慮して除去ノードを決められるようにする．

②制御を位置偏差が出にくいものに変える(ギリギリの隙間を通るときが今の制御だと危ない)

③速度計画をする(動画で左上と左下のロボットが途中で重なってしまっている)

④行動計画ロジックを考える(状況に応じてどのロボットを動かすか？を決める)

①②③に関してはDynamic Window Approachをマルチ用に改良するともしかしたら解決するかもしれないけどとりあえず今の方針で進めてみようと思います．