今回は前回作成した二重振子モデルにある２つのジョイントに比例微分制御器を付けて直立に立たせてみます．
ジョイントのセンサー情報の出力と制御トルクの追加
フィードバック制御を行うためには関節角度や角速度が必要となります．
ジョイントウィンドウのSensingのPositionとVelocityの項目にチェックを付けるとqポート（位置）とwポート（角速度）が使用できるようになります．
またActuationのTorqueの設定を”Provided by input”にすることでtポートが使用できるようになります．
今回，比例微分制御器はサブシステムとして作成しました．
サブシステムは普通にブロックをつなげた後に，サブシステムにしたい複数のブロックを選択し右クリックで”選択からサブシステムを作成”を選ぶことで作成できます．
楕円のRef，六角形のAngle，Angular Veloctyが入力です．楕円はsimlinkの入力，六角形はSimscapeブロックからの入力を表しています．
SimscapeのブロックとSimulinkのブロックは少し性質が異なります．
特に配線の矢印の色も黒(Simlink)，白(Simscape)と区別されていて，互いに異なるブロックをつなぐためには変換機(PS-SImulink ConverterやSimulink-PS Converter)が必要となります．
出力はSimscape型でトルク\((\tau=-K\theta-B\dot{\theta})\)を返しています．
サブシステムを右クリックし”マスク＞マスクの作成”と進むことで，サブシステムにマスクを生成できます．
今回はどのリンクもK=200, B=10としました．
作成した倒立二重振子のシミュレーションは以下の動画のようになりました．
補足(2022/07/15追記)
上記のサブシステムではSimscapeの信号である物理量(PS:Physical Signal)をSimulinkの値に変換していましたが，物理量のままゲインをかけたり，和をとるブロックがあることに後から気づきました．
これらのブロックはライブラリブラウザの”Simscape/Foundation Library/Physical Signals/Functions”の中にあります．
和や差をとる際には物理量の単位をそろえないといけない点には注意してください．
例えば今回の場合は比例ゲインによるトルクと微分ゲインによるトルクがどちらも[N*m]の単位になるように，Gain-Kブロックの単位を[N*m/rad]，Gain-Bブロックの単位を[N*m*s/rad]にしています．
また，Simlink-PS Comverterの出力の単位も[rad]に設定する必要がありました．
補足の補足
上の図ではRefがSimulinkのブロックになっていますが，入力元をPS ConstantにすることでここもPSブロックにできます（多分そっちの方がきれい）．
僕みたいな初心者は一度”Simscape/Foundation Library/Physical Signals”の下の各フォルダを眺めておくと実際に使うときにこんなのあったなと思いだして使えるようになる気がします．
今回学んだこととして，とりあえずSimscapeで探して，ほしいブロックがなかった時だけSimlinkのブロックを使うのがいいということに気づきました．探してみると意外とSimscapeでも欲しいブロックが見つかります．
コメント