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

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

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

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

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

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

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

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

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

走行経路&速度結果

UKFを用いた位置推定結果

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

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

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

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

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

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

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

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

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

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

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

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

今回出展するお片付けロボットは以下のような動きでゴミ等を回収します．
↓
http://kyu-robo.sakura.ne.jp/movie/work/01_RC-DC/mech_movie_1.mp4

動画を見ていただくとわかるように，ロボット一台だけではゴミをすくい上げることが難しいと思われます．

なので少なくとも二台のロボットで協力してゴミをすくい上げることを考えます．

一台が壁となり，もう一台がゴミをすくい上げるように，二台でゴミを挟み込めると良さそうです．

そこで今回は，ゴミを挟み込むような経路計画を行いました．

大したロジックではないのですが，概要を説明します．

まず，ゴミから近いロボットを二台選択します(ここはざっくりで良いのでゴミとロボット間の直線距離を単純に比較)．
次にゴミが縦長か横長かで上下で挟み込むか左右から挟み込むかを決定します．
回収するまではゴミ自体には触れないので，ゴミは目的であると同時に障害物としても扱います．
ゴミから近い二台のみを制御し，ゴミを挟み込む位置まで移動します．
今回は位置精度が出るように，初動のみ，姿勢制御を優先させました．
残りの二台は有事に備え，姿勢だけ合わせてスタンバイします．

横長のゴミを挟み込む様子が↓です．
赤丸がゴミオブジェクト（画像認識から得られる点群），緑の丸が障害物（画像認識から得られる点群）です．
ロボットの経路が重なるとロボット同士がぶつかるリスクも増えるので選ばれた二台のロボット位置関係から，どっちのロボットがどちらから挟み込むも決めています．

また，縦長のゴミを挟み込む様子が↓です．

まだ，ゴミと障害物が近接しているときのエラー処理等が入っていませんが，概ね上手く動いてくれそうです．

あとはエラー処理とロボット同士が衝突しないようなロジックを入れ込めば，ひとまず良さそうです．

今までやってきた経路計画と制御を組み合わせて，目的地まで移動するシミュレーションを行いました．
机は横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をマルチ用に改良するともしかしたら解決するかもしれないけどとりあえず今の方針で進めてみようと思います．

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

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

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

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

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

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

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

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

強化学習を用いた経路探索(offline)をやってみました．
試した強化学習はQ-LearningとSarsaです．
とりあえずお試しなので以下のようなざっくり離散化したconfiguration空間で経路を探索してみました．

とれる行動は上下左右の移動のみとして，障害物に-10，目的地に10の報酬を与えました．

Q-Learning，Sarsaそれぞれで計算した結果が以下となります．

両手法とも，方策はε-greedyとしε=0.2としました．学習率は0.2，割引率は0.9，学習の反復解数は500回としました．

結果，Q-Learningの方は毎回上手く経路が学習されたのですが，Sarsaの方は結構失敗することが多かったです．

このように一たびQ値が学習されれば，環境が変わらない限りはどこからスタートしてもゴールにたどり着く経路が得られるのが良いところだと思っていたのですが，学習時間が約12秒もかかってしまったのでやはりオンラインで搭載しようとすると，大した学習をしているわけではないのに，環境(障害物の位置など)が変わるたびにフリーズ感が出てしまうので実演には向かないかなぁと思いました．

また，Q値の学習のされ方が以下のような，求まった経路に引き込まれるようなものになったので，スタートポイントを変えるとゴールにはたどり着けますが，無駄に大回りをしてしまうような経路になってしまっています．

正直アルゴリズムを書いてみたかっただけなので，次は実演用のロジックを書こうと思います(online経路計画)．

Maker Faire Tokyo 2017に出展できることになりました．
http://makezine.jp/event/mft2017/

出展するものは机の上のお片付けロボットです．
http://kyu-robo.sakura.ne.jp/

私はほとんど活動に参加できていなかったのですが，ロボットの経路計画と制御で部分でお手伝いする予定です．

本番はROSを使う予定ですが，私が色々あってLinuxが使えない状況にあるので，Pythonでシミュレータを作成しました．

制御周期間は0次ホールドで入力を入れ続けるシミュレータで頑張ってアニメーションまで表示されるようにしました．

今回は経路ありきで制御を行う際のアルゴリズムを実装したので結果を載せておきます．

今回の出展するものは経路計画，制御指令値計算はマイコンで行うわけではなく，ホストPCから計算結果を飛ばす仕様です．なのでやはり制御屋としてはマイコンではできないような計算負荷の高い制御を実装してみたくなっちゃうんですよね．
そういうことで今回の制御はモデルベースの非線形最適制御を使っています！

特徴は以下です．
①拘束条件を考慮できる(非現実的な入力を計算することなどを防げる)
②経路がカクカクしたものであっても評価関数を自動で最小化するので割りと滑らかな走行が可能になる(無理な目標値を与えてもちゃんと制御してくれる)
③XY座標ベースの目標経路と目標速度を与えるだけで計算できる(目標ヨーレイトとかを計算しなくて良い)．

特にノンホロノミック拘束である車両ロボットであっても②のような特徴があるので経路生成にそんなに力をいれなくても良いことになります．
まぁ経路計画で楽をするなら制御を豪華にして，制御で楽をするなら経路計画を豪華にしなきゃいけない感じですね(今回は前者)．

制御器内で用いたモデルは以下で，シミュレータのプラントモデルと同じものを使用しています．

Fig.1 制御器内のモデル(導出)

Fig.2 制御器内のモデル(状態方程式)

シミュレーション結果は次のようになりました．

Fig.3 制御結果

②で述べたようにカクカクした適当な経路を与えても制御ができています．また初期値からいきなり姿勢がずれている場合でもその場で回転して姿勢を戻しつつ移動する様子が再現されています．

ただし，今回の設定した評価関数重みでは位置偏差に対する追従性が悪く，最終的な目標値にたどりついたときに位置偏差が残ってしまうものもあります．

お片付けロボットにおいて，位置合わせは重要なので，もう少し位置偏差に対する重みを大きくするか，もっと簡単な別の制御則で済ませてしまうかもしれませんw(出力ゼロ化制御など)．

今回車輪速制限を±0.05 [m/s]以下に設定したのですが，その拘束条件もちゃんと満たせている事が以下のグラフからわかります．

Fig.4 入力計算結果

走行速度も曲がる際に回転のために速度が0になるシーンはほとんどない状態で走行できている事がわかります．

Fig.5 軌道

Fig.6 状態量グラフ

次は経路計画アルゴリズムを実装したいと思います．

オフライン前提の強化学習とオンライン前提のボロノイ図⇒グラフ理論　あたりを試せればと思います．

文字通りの「机上の空論」とならないよう，ロボットが動くところまで頑張ります．