MATLAB/Simscape Multibodyでの台車型倒立振子シミュレーション

Last updated at 2022-12-05Posted at 2022-12-03

MATLAB/Simulink Advent Calender 2022の4日目になります。
3日目は、@kentaPtさんの「iPhone LiDARで取得した3次元点群を地図上にプロットしてみよう」でした。MATLABで3次元データも扱えるとは、大体の工学分野はMATLABだけでいいのではないか、という気がしてきます。myスマホがiphone SE(第一世代)なので、試せないのが残念です。

背景

MATLABツールボックスの一つであるSimscape Multibodyについて、MathWorksが公開している入門文書は、最終的にリンク機構を例に説明されていました。
しかし、制御工学勢からするとリンク機構はなじみが薄いです。そこで、みんな大好き倒立振子の実装を通して、別視点からの入門記事を提供します。
フライホイール型の倒立振子は、コチラで実装された方がいらっしゃるため、今回は台車型の倒立振子を対象とします。

本記事の対象読者は、MathWorksが公開している次の2つのチュートリアルは実践されたものとしています。

実装過程

前知識

Simscape MultibodyはZ軸を基準としていることが多いです。例えば、回転接続であるRevolute Jointは、問答無用でZ軸回りの回転となります。そのため、各オブジェクトのどこが何軸かを意識しながら、モデル作成を進めることが上達の近道です。

基本要素への分解

シミュレーション対象を構成要素に分解していきます。オブジェクトは振子と台車になります。また、オブジェクト間はそれぞれ回転軸および直線軸で接続されています。

振子　→台車と回転軸で接続する
台車　→振子と回転軸で接続する、地上と直線軸で接続する

方針

はじめに、振子オブジェクトを作成します。
次に、単純な振子を作成し、重力によって減衰振動することを確認します。
平行して、台車を作成します。
最後に台車と振子を繋げます。

振子を作る

smnewコマンドで新しいシミュレーション環境を作成します
チュートリアルで実施したように、まずは棒を作ります。ここでは、アルミ材料を想定したパラメータ設定をしました。

今回の振子オブジェクトは、片側しか接続しないため、座標(Rigid Transform)も片端にだけ設定します。
位置だけでなく、角度も同じRigid Transformで変換します。Rotatin About設定がFollower Axesの場合は、座標変換前の座標系で3軸を回転します。
地面との間を回転軸(Revolute Joint)で接続します。回転軸はZ軸回りでの回転となるので、回転軸と振子の回転とが合うようにRevolute Jointの前後で座標変換②と戻し③をしています。F⇒Bにした場合は、同じブロックでB⇒Fをすれば、結果として元の座標系に戻ります。座標系が見ずらいため、①のブロックで原点からx方向に20mmほどズラしています。
回転軸のダンピング係数(Damping Coefficient)は1e-4 N*m/(deg/s)に設定しました

※Revolute Jointの向きを逆にしていました。以降のモデルでは修正しています。

この時点でのシミュレーション例
振子オブジェクトが完成したので、今後のことを考えてサブシステム化します。

台車を作る

次は台車単体を実装します。ひとまず振子をコメントアウトします。
直線案内(Prismatic Joint)で台車と地面を接続します。Prismatic JointがZ方向のみに移動可能な直線運動を規定するので、Jointの前後に座標変換を加えて、固定座標系のX軸方向の拘束とします。

※Rigid Transformの設定は、どちらも同じ設定で、方向(接続端子)のみが違います。
External Force and Torqueブロックによって、台車に力を加えます。
ここでは、正弦波状にトルクを加えて、台車が動くことを確認しました。

振子と台車をつなげる

台車と振子ができたので、２つを繋げます(実際には振子のコメントアウトを解除します)。正弦波の振幅を大きくしてあげれば、台車の移動に伴って、振子も揺れていることが確認できます。この時に振子の初期角度は0度に設定しておきます。

制御装置を付与する

制御対象を作れたので、次は状態フィードバック系を実装します。状態量として、台車の位置と速度、振子の角度と角速度をフィードバックします。
Prismatic Jointの設定でSensingのPositionとVelocityを有効化します。
振子のRevolute Jointも同様にSensing設定でPositionとVelocityを設定します。
振子のブロックから、角度と角速度を外部に出力します。
状態フィードバック制御器をSimulinkで作ります。振子角度は、鉛直下向きが0radなので、鉛直上向きが0radとなるようにΠ[rad]だけオフセットしています(Revolute Jointの前後の座標変換をうまくやれば不要になる)。