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 状態量グラフ

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

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

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