kawazu_gkgkのブログ

井の中の蛙が勉強して得たことを記録する．

Algodooによる台車型倒立振子の安定化制御

System Control Algodoo

Algodoo

Algodooという2次元物理シミュレータがあったので台車型倒立振子を作成してみた．
Algodoo公式サイト

AlgodooにはThymeという独自のスクリプト言語(@ウィキ - Thyme)が存在し，これを利用して制御システムを組むことができる．

制御システム構築に最低限必要なThyme知識

スクリプトを記述する場所には，F10キーで表示されるスクリプトウィンドウと各オブジェクトのスクリプトメニューの2種類がある．グローバルな変数・関数はスクリプトウィンドウ，各オブジェクトに持たせる変数・関数は対象オブジェクトのスクリプトメニューに書き込む．
f:id:kawazu_gkgk:20200627232732p:plain:w500
f:id:kawazu_gkgk:20200627232741p:plain:w500
上がスクリプトウィンドウ，下が各オブジェクトのスクリプトメニューである．

変数は以下のように定義，代入する．

var := 0; //定義
var = 10; //代入

関数は以下のように定義する．

func := {command1;command2;}; //引数なしの関数
func := (arg1,arg2)=>{command1;command2;}; //引数ありの関数

条件分岐は以下のように三項演算子を使用する．if文はない模様．

condition ? {command1} : {command2};

conditionがTrueの場合command1の処理を実行，Falseの場合command2の処理を実行する．処理自体は';'で区切れば何行分でも入れることができる．なお条件式（condtition部）でBool型変数を使用すると正常に動作しないので注意（Algodoo公式ページのフォーラムにも書いてあった）．

また，定義済みの関数を変更する場合は変数の代入と同様':='ではなく'='を使用して変更後の関数を再度代入する．

特定オブジェクト内で定義する変数・関数名は先頭にアンダースコア'_'，グローバル変数・関数の場合は変数・関数名の前に"Scene.my."を付ける必要がある．

各オブジェクトはそのオブジェクトの特性や運動の状態（位置，速度等）の変数を最初から持っている．これはオブジェクトのスクリプトメニューから見ることができる．
このスクリプトメニューのリスト中には何も入っていない関数がいくつか存在する．それらは何らかのイベントをトリガにして実行される関数である．以下に制御システムを組むのに有用そうなものを記す．

  • update
    シミュレーションのアップデート毎（デフォルトでは60Hz）に実行される．制御器の周期実行に使える．シミュレーション停止中も実行される．
  • postStep
    シミュレーションのアップデート毎（デフォルトでは60Hz）に実行される．制御器の周期実行に使える．updateとは違いシミュレーション停止中は実行されない．
  • onClick
    オブジェクトがクリックされた際に実行される．スイッチ等に使える．

f:id:kawazu_gkgk:20200627233135p:plain:w500

台車型倒立振子の作成

倒立振子本体は四角ブロックにタイヤ用の円形ブロック2つと振子用の棒をヒンジでくっつけ，片輪のヒンジをモータにしただけの至って簡単なものとする．
f:id:kawazu_gkgk:20200627234139p:plain:w300
台車のホイールベースは短いと台車の加・減速時に駆動輪が浮きやすくなるため，長くとる方がよい．また，振子の棒は長い方がいい加減な制御でも立てやすい．

オブジェクト同士が衝突するかどうかを設定するため，各オブジェクトのメニューから衝突レイヤを適切に設定（タイヤと地面は衝突判定を付けるためどうレイヤ，振子とその他オブジェクトは衝突しないようにレイヤを別ける等）する必要もある．

この倒立振子本体にセンサとして振子の角度を取得するオブジェクト，台車の位置を取得するオブジェクトを張り付ける（後述）．

以下，各パーツの構成とスクリプトを記す．

グローバル変数・関数

F10キーで現れるスクリプトウィンドウで以下変数及び関数を定義する．
Scene.my.atan2は前述のPhunの@ウィキから拝借してきたもので，振子の角度算出のために使用する．
Scene.my.clockは制御更新のタイミングで0->1になる信号として使用する．信号値は後述のパルスジェネレータで更新し，各センサ・アクチュエータの出力値更新周期に使用する．
Scene.my.tqはモータに要求するトルク値（制御入力）として使用する．
Scene.my.omg~Scene.my.y2は角度センサの出力値として使用する．オブジェクトの角度をそのまま取得できる変数は存在しないようなので，2点の座標から角度を算出する方法をとり，制御器内でScene.my.atan2を使用して角度を取得する．なお角速度は元から変数が用意されているのでそれを使用する．

Scene.my.atan2 := (y, x)=>{ x == 0 ? { y > 0 ? {math.pi/2} : {-math.pi/2}} : { x > 0 ? {math.atan(y/x)} : { y > 0 ? {math.atan(y/x) + math.pi} : {math.atan(y/x) - math.pi}}}};
Scene.my.clock := 0;
Scene.my.tq := 0;
Scene.my.omg :=0;
Scene.my.x1 :=0;
Scene.my.x2 :=0;
Scene.my.y1 :=0;
Scene.my.y2 :=0;

パルスジェネレータ

制御周期を決めるための周期信号を発生させるオブジェクト．postStepでシミュレーションループ毎にカウントアップし，一定周期でグローバル変数の制御更新フラグをTrueにする．
スクリプト(変数・関数の定義)はスクリプトメニューの一番上にある入力ウィンドウに書き込む．定義後はスクリプトメニュー下部に定義した変数・関数の内容が表示されるので，内容を変更する場合はそちらを直接変更してもOK．

_updcnt := 0;
_main := {_updcnt = (_updcnt + 1) % 1;_updcnt == 0?{Scene.my.clock = 1}:{Scene.my.clock = 0};};

関数_mainはpostStepで呼ぶようにする（以下オブジェクトでも同様）．
f:id:kawazu_gkgk:20200627231744p:plain:w300
この設定では制御周期はシミュレーション自体のデフォルト画面更新周期1/60s*1サイクル=約0.033sとなる．離散系の制御器設計をまだよく勉強していないためなるべく連続系に近くなるようにしているが，_updcnt = (_updcnt + 1) % 5といったようにすれば制御周期を長くすることもできる．

振子角度センサ

振子の両端に張り付ける．

  • 上側
_sendsig := {Scene.my.x2 = pos(0);Scene.my.y2 = pos(1);};
_main := {Scene.my.clock == 1?{_sendsig;}:{};};
  • 下側
_sendsig := {Scene.my.x1 = pos(0);Scene.my.y1 = pos(1);Scene.my.omg = angvel;};
_main := {Scene.my.clock == 1?{_sendsig;}:{};};

f:id:kawazu_gkgk:20200627232405p:plain:w500 pos(0)，pos(1)ははじめから各オブジェクトに設定されている変数の一つであり，それぞれオブジェクトのx軸，y軸座標（絶対位置）が入っている．
angvelはposそのオブジェクトの角速度が入っている変数である．角速度はどちらからとっても問題ないが，ここでは下側のオブジェクトから取得する．理由は特にない．

モータ

モータとして設定している回転軸のスクリプトメニューに以下を入力する．

_maxrpm := 100;
_driver := {Scene.my.tq < 0?{ccw = true;motorTorque = (-Scene.my.tq);}:{ccw = false;motorTorque = Scene.my.tq;};motorSpeed = _maxrpm;};
_main := {Scene.my.clock == 1?{_driver}:{};};

モータへは制御器で計算する要求トルクをセットするが，マイナストルクを直接セットすることができない．モータの逆回転はオブジェクトの変数ccwをTrueにする必要がある．

台車位置センサ及び制御器

台車に小さい四角形を張り付け，このオブジェクトの位置を台車位置として取得する．位置はオブジェクトの中央が参照されるようなので貼り付け位置に注意する．
制御構造は全状態フィードバックとする．

_ka = -156.0;
_kav = -57.5;
_kp = 5.18;
_kv = 10.8;
_preproc := {_angle = - Scene.my.atan2((Scene.my.x2 - Scene.my.x1),(Scene.my.y2 - Scene.my.y1));};
_controller := {Scene.my.tq = _ka*_angle + _kav*Scene.my.omg + _kp*pos(0) + _kv*vel(0)};
_main := {Scene.my.clock == 1?{_preproc;_controller;}:{};};

使用したモデル及び制御器設計はJupyter Notebookで書きgithubのリポジトリに公開したのでそちらを参照．ただしモデル導出は省略している．
作成した台車型倒立振子の寸法や重量で適正なフィードバックゲインは変わるため，このゲインをそのまま使って安定化できる保証はない．基本的に設計しなおす必要がある．
制御器設計ツールはMatlabやScilab等があるが，今回の設計ではPython-Controlを使用した．

なお，もうちょっと頑張れば台車位置の追従制御とかもできそうだが今回はレギュレーションのみである．

出来上がったもの

Youtubeに倒立振子の動作確認動画を上げた．

作成後に気付いたがホイールベース3m，振子高さ3mというなかなかの大きさで総重量10kg程度しかないトンデモ制御対象である．

注意点

台車型倒立振子本体をいい加減に作ると駆動力でタイヤが浮く，タイヤがすぐスリップする，振子が直立状態で角度ゼロにならないといったことが起こり，安定化を難しくする． ロバストな制御器を設計するのも大事だが，ハードそのものをしっかり作った方が効果的な場合もある．

今後の展望

今回作成した倒立振子の問題点としてタイヤのスリップに弱いことが上げられる．特に加減速時に駆動輪が浮くことで制御が働かなくなり不安定化するケースが多かった．

スリップ対策にはサスペンション追加という手もあるが，制御系を作ることが目的のためトラクションコントロールを作成したい．

また，Algodooという手軽な物理シミュレータで制御システムを作ることができることを確認できたため，スライディングモード制御の実装なども試したい．

修正
  • 2020/7/3
    角度・角速度の座標系が時計回りで正になっていたものを反時計回りで正になるように修正（スクリプト及び制御器設計Jupyter Notebook）