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

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

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

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

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

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

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

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

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

走行経路&速度結果

UKFを用いた位置推定結果

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

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

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

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

色々な事情があり，使用するロボットの台数が4台から2台へ変更になったため，今回はそれを反映させました．

また，画像からロボットの位置と姿勢を算出した際，ノイズがのることで状態FBが上手くかからなくなることの対策としてUKF(非線形カルマンフィルタ)を実装したのでその結果をUPしておきます．

上記のアニメーションを行った際の推定結果が以下がです．
今回は真値に対して正規分布に従う乱数を加えることでノイズを表現しています．

結構大きめのノイズをのせてみましたが，真値に近い値が推定できている事がわかります．

それぞれの状態量の推定結果が以下です．

X,Yそれぞれで見るとあまり大きなノイズに見えませんが，XY座標上にプロットしてみると真値から外れている様子がわかります．

デスクトップPCで計算できること，ロボットの台数が減ったことから計算時間には大分余裕がありそうなので，UKFの状態量点列を変換する際に単純なオイラー法ではなく4次のルンゲクッタで計算するようにしているので推定値の制度は大分良さそうです．
(実際はこんなにノイズのらないと思うし)

次はロボット同士の衝突回避を実装しようと思います．