MATLABで現代制御その3:MATLABで設計した状態フィードバック制御器をUnity上で実装してシミュレーション

投稿日:2022年8月18日
最終更新日:2022年9月3日

MATLABで設計した状態フィードバック制御器をUnityで実装&動かしてみる

前回までMATLABで以下の台車型倒立振子の状態フィードバック制御器を設計して、MATLABで初期状態の応答をシミュレーションしてみたり、Simulinkで外部入力を加えた時の応答をシミュレーションしてみた。

ただ、波形を見てるだけだとちょっと味気ない。ということで、今回はUnityで台車型倒立振子のモデルをつくり、MATLABで設計した制御器を実装してアニメーションで動作を確認してみたので、その方法をメモしておく。

誤解が無いように書いておくと、今回のやり方はMATLABとUnityを通信させて制御しているわけではなく、MATLAB上で計算したフィードバックゲインKを使って、C#で実装したコードをUnity上で動かす、という感じ。

台車型倒立振子のモデリングやMATLABでの状態フィードバック制御器の設計については以下を参照。

MATLAB/Simulinkで遊んでみる まとめ

 

開発環境

MATLAB online(MATLAB R2022a)

Simulink

Control System Toolbox

Unity 2021.3.7f1

Windows 10

 

MATLABで状態フィードバックのゲインを計算

こちらは以前のおさらいなので詳細は割愛するが、以下のmファイルMATLABを実行して、以前定義した各状態変数x、dx/dt、θ、dθ/dtに対する状態フィードバック制御のゲインKを計算しておく。

% Parameters
M = 10;
m = 1;
L = 2;
g = 9.8;

% State Space
A = [0 1 0 0
     0 0 -3*m*g/(4*M+m) 0
     0 0 0 1
     0 0 6*g*(M+m)/(L*(4*M+m)) 0];
B = [0
     4/(4*M+m)
     0
     -6/(L*(4*M+m))];

% Check Controllability
Co = ctrb(A,B);
rank(Co)

% Pole/Feedback Gain
p = [-0.5 -1.0 -1.5 -2.0];
K = place(A,B,p)

これを実行すると、以下のようなフィードバックゲインKが得られた。

K = -2.0918 -8.716 -230.1724 -79.9546

実際に実機を作って動かす場合には各状態変数x、dx/dt、θ、dθ/dtを全てセンサーで測定するのは現実的ではないが、Unityであればオブジェクト(台車や振子のモデル)に付加したrigidbodyから各状態変数の値を取得できる。

そのため、ここで計算したゲインKを、Unity上で取得した各状態変数x、dx/dt、θ、dθ/dtにかけて足し合わせた入力uを台車に加えれば、状態フィードバック制御のシミュレーションができる。

 

Unityで台車型倒立振子をモデリング

まずは台車のモデリングから。Unityは最新のLTS版であるUnity 2021.3.7f1を使用した。今回は2Dでプロジェクトを作成した。あまり細かく説明しないが、Inspectorの設定だけ張っておくのでやりたい人は参考にしてください。まず、床は2D Object→Sprites→Squareで配置してRigidbodyを追加。

次に台車を、先ほどと同じくSquareでCartという名前で配置してRigidbodyを付加した。台車のタイヤは無くても摩擦を0に設定すればOKなので割愛。質量(Mass)はMATLABの計算で使った値と合わせている。

振子の棒もSquareでPoleという名前で配置して、Rigidbody&Hinge Joint 2Dで台車と接続した。こちらも寸法や質量(Mass)はMATLABの計算で使った値と合わせている。

ButtonとTextも配置しているが、こちらは次の節で書く。

また、Edit→Project SettingsからPhysic 2Dの設定を開き、重力加速度を-9.8(デフォルトは-9.81だが、MATLABで計算したときの値に合わせた)、摩擦を0にしたphysics materialをDefault Materialに設定して、MATLABでのモデルにパラメータが合うようにした。

 

台車を動かす制御器をC#で実装する

オブジェクトが配置できたら、Cartに以下のようにMoveCart.csというスクリプトを追加した。

using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using TMPro;

public class MoveCart : MonoBehaviour
{
    private GameObject refObj;
    private float x;
    private float deltaX;
    private float theta;
    private float deltaTheta;
    public TextMeshProUGUI uText;

    private float k1 = -2.0918f;
    private float k2 = -8.716f;
    private float k3 = -230.1724f;
    private float k4 = -79.9546f;

    // Start is called before the first frame update
    void Start()
    {
        refObj = GameObject.Find( "Pole" );
        uText.text = "u = 0";
    }

    // Update is called once per frame
    void FixedUpdate()
    {
        Rigidbody2D rb = this.GetComponent();
        Vector2 force = new Vector2(0.0f, 0.0f);

        x = rb.position.x;
        deltaX = rb.velocity.x;
        theta = refObj.GetComponent().rotation * Mathf.Deg2Rad;
        deltaTheta = refObj.GetComponent().angularVelocity * Mathf.Deg2Rad;

        float u = -k1 * x + -k2 * deltaX - k3 * (-theta) - k4 * (-deltaTheta);
        uText.text = "u = " + u.ToString();
        
        force = new Vector2(u, 0.0f);
        rb.AddForce (force);
    }

    public void OnClickLeft() {
        Rigidbody2D rb = this.GetComponent();
        Vector2 force = new Vector2(0.0f, 0.0f);
        force = new Vector2(300f, 0.0f);
        rb.AddForce (force);
    }

    public void OnClickRight() {
        Rigidbody2D rb = this.GetComponent();
        Vector2 force = new Vector2(0.0f, 0.0f);
        force = new Vector2(-300f, 0.0f);
        rb.AddForce (force);
    }
}

一点補足しておくと、

float u = -k1 * x + -k2 * deltaX - k3 * (-theta) - k4 * (-deltaTheta);

について、まずMATLABで求めたKはフィードバックするときの符号はマイナスになるのでk1~k4の符号はマイナスになる。加えて、振子の棒の角度θは、状態変数を求めた時に定義した方向と、Unityで定義されている方向で符号が逆になるため、thetaとdeltaThetaの符号をマイナスにしている。

MATLABやSimulinkでやったような初期条件からの応答を見てもよかったのだが、せっかくなのでOnClickLeft/OnClickRightという関数を実装して、ボタンをクリックして台車を横から突っつけるような実装にした。ボタンはUIのButtonオブジェクトを追加し、追加したButtonの「Button」にMoveCart.csを付加したCartオブジェクトを設定し、ボタンをクリックした関数を実行できるようにした。

また、UIからTextも追加して、uText.text = “u = ” + u.ToString();でuの値を画面上に表示するようにした。ScriptからTextを変更する場合は、Publicで定義したMoveCartのuTextに、Textのオブジェクトを追加すればOK。

これで準備は完了。

 

実行して動かしてみる

後はUnityで動かすだけ。再生ボタンを押して、ボタンをクリックしてツンツンつついてみた様子が以下の動画。表示されているuの値が台車に加わり、倒立状態を維持できていることがわかる。

やはり動いている様子が見えると楽しい。前回の記事で書いたようにMATLAB OnlineだとMechanics Explorerでアニメーション表示ができないので、Unityを使うというのはなかなか良い気がする。今後も物理シミュレーターとしてUnityを活用していきたい。

 

参考文献

この本のMATLAB/Simulink 6か月ライセンスを使ってやってます↓
Interface 2022年 9月号

 

MATLAB/Simulink記事まとめ

MATLABとSimulinkの記事は以下にまとめてます。

MATLAB/Simulinkで遊んでみる まとめ

 


投稿者: wakky

映画と旅行が大好きなエンジニア。お酒、ゲーム、読書も好き。

コメントを残す

メールアドレスが公開されることはありません。 が付いている欄は必須項目です

このサイトはスパムを低減するために Akismet を使っています。コメントデータの処理方法の詳細はこちらをご覧ください