今まで紹介してきた手法ではロボットと静的な障害物の回避しか考えていなかったため，状況によっては以下のようにロボット同士が接触してしまう可能性があります．

ロボットが複数台いるとき，ロボット同士の接触を回避するのは単純ではありません．
それは現在地だけからは接触の判断はできず，時系列を考慮しないといけないのが一番の要因です．また接触回避の仕方も色々あるため，どの時刻でどういった方法で回避するかを決めるのは一筋縄ではいきません．

今回の状況では速度を調整するだけで大方の接触は回避できそうなのでまず速度調整を行い，それでも接触してしまいそうな場合は経路を再探索する方法で実装することを考えました．

各ロボットの目標速度と経路があるためそれらを使って，どの時刻のどの点で接触しそうかをおおざっぱに求めます．以下に接触点を予測した結果を示します．
先の動画と比べると概ね接触点は予測できている事がわかります．

次に，経路が短い方のロボットをそのまま動かし，経路が長い方のロボットを一定時間走行しないようにさせます．経路が短い方のロボットを先に移動させることで出口が塞がれてしまうことを防ぎます．また走行させない方のロボットの進行方向停止時間は，念のため，接触が終わると思われる時間に設定します．

以上を実装してみた結果が以下の動画です．

ロボットの停止時間が少し長めですが，ちゃんと接触が回避できていることがわかります．

ロボットの車輪速は以下のように，青いロボットが向きだけ変えた後に，しばらく0としていることがわかります．

一応，以上をもって出展するための経路計画，状態推定，経路追従制御ができるようになりました！
最後に結果をまとめて載せておきます．

走行経路&速度結果

UKFを用いた位置推定結果

UKFを用いた状態推定結果(青ロボット)

UKFを用いた状態推定結果(赤ロボット)

DWA等を実装してみたかったですが，もう出展まで時間もないので後回しにしたいと思います．実機で動いている姿をみるのが楽しみです．

※もちろん二つの経路が交差しても，接触する可能性がない場合は速度調整は入りません