ノーズパンチ機構が動くようになりました．

ピストンークランク機構が何とかシャーシ内にのりました．

↓が機構駆動時の拡大動画．

このままだとバッテリーはどう頑張っても搭載できないので，上にスペースを作って無理やりのせようと思います．

あとはボディをどうやってつくるか．．．

完全に自己満足ですが，娘にアンパンマン号をつくってあげようと思っています．

既存製品の組み合わせでシャシーと通信環境までは何とか作成できました．
PS3コントローラでBluetooth接続で遠隔操作させる仕様です．

機械加工等出来る環境ではないのでタミヤの工作キットを基本パーツに頑張りました．

実際のアンパンマン号はジャムおじさんが天才過ぎるので多機能ですが，あとノーズパンチという鼻が飛び出る機構と，目が光るようにLEDはつけたいと思っています．

問題はバッテリーとノーズパンチ機構のレイアウトと，ボディをどうやって作成するか．．．
DMM 3Dプリンタで作ってみたいけど，まずはペットボトルでどこまで高いクオリティのボディが作成可能か頑張ってみようと思います．

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

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

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

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

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

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

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

ロボットの車輪速は以下のように，青いロボットが向きだけ変えた後に，しばらく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])を設定しています．経路探索にはダイクストラ法を使いました．
探索結果は以下です．

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

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

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

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

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

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