MATLABで現代制御その5:台車型倒立振子のサーボシステムをUnity上で実装してシミュレーション

投稿日:2022年10月17日
最終更新日:2022年10月17日

MATLABで設計したサーボシステムをUnityで実装&動かしてみる

前回はMATLABで台車型倒立振子のサーボシステムを設計して、Simulinkでシミュレーションしてみた。

今回は以前状態フィードバック制御のときにつくったUnityの台車型倒立振子のモデルを使って、サーボシステムをUnity上で実装して動かしてみる。Unityの台車型倒立振子のモデルのつくり方は以下を参照。

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

 

開発環境

MATLAB online(MATLAB R2022a)

Simulink

Control System Toolbox

Unity 2021.3.7f1

Windows 10

 

サーボシステムのゲイン

前回のおさらいだが、以下のようなサーボシステムを想定して、MATLABでフィードバックゲインを求めた。状態変数は、台車の変位(x)、速度(dx/dt)、振子の角度(θ)、角速度(dθ/dt)で、入力:台車に加える力(u)になっており、変位xを目標値に追従させるようなサーボシステムになっている。

Matlabで計算したフィードバックゲインK、Gは以下の通り。

K = -89.2517 -93.5492 -677.8023 -247.7323
G = 41.8367

このゲインを使って、Unity上でC#のスクリプトを実装し、台車型倒立振子を目標値に追従させる。

 

台車を目標値に追従させるサーボ制御器をC#で実装する

以前Unityで状態フィードバック制御をおこなった時にMoveCart.csというスクリプトをつくったが、これを少しいじってMoveCartServo.csというスクリプトをつくる。以下が作成したスクリプト。

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

public class MoveCartServo : MonoBehaviour
{
    private GameObject refObj;
    [SerializeField]
    private float x;
    [SerializeField]
    private float deltaX;
    [SerializeField]
    private float theta;
    [SerializeField]
    private float deltaTheta;
    public TextMeshProUGUI xRefText;
    public TextMeshProUGUI xText;

    private float xRef = 0.0f;
    private float x1 = 0.0f;
    private float x2 = 0.0f;
    private float x3 = 0.0f;
    private float ku = 0.0f;
    private float u = 0.0f;

    private float k1 = -89.2517f;
    private float k2 = -93.5492f;
    private float k3 = -677.8023f;
    private float k4 = -247.7323f;
    private float g = 41.8367f;

    // Start is called before the first frame update
    void Start()
    {
        refObj = GameObject.Find( "Pole" );
        xRefText.text = "xRef = 0";
        xText.text = "x = 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;

        x1 = xRef - x;
        x2 = x2 + x1 * Time.deltaTime;
        x3 = -g * x2;
        ku = k1 * x + k2 * deltaX + k3 * (-theta) + k4 * (-deltaTheta);
        u = x3 - ku;

        xRefText.text = "xRef = " + xRef.ToString(); 
        xText.text = "x = " + x.ToString();
        
        force = new Vector2(u, 0.0f);
        rb.AddForce (force);
        
    }

    public void OnClickLeft() {
        xRef = xRef - 1.0f;
    }

    public void OnClickRight() {
        xRef = xRef + 1.0f;
    }
}

前回のスクリプトと被る部分も多いので詳細な説明は割愛するが、前回の状態フィードバック制御から積分器が追加されているため、C#のスクリプト内で積分をしてやる必要がある。

積分の実装方法は色々あると思うが、今回はシンプルにオイラー法で実装した。Time.deltaTimeは 直前のフレームと今のフレーム間で経過した時間を返すので、これに目標値と変位の差分をとったx1をかけて加算している。

x2 = x2 + x1 * Time.deltaTime;

また、Unityの実行画面でボタンにより目標値を±1ずつ変えられるように、以下の処理を追加している。Unity上でボタンを押して処理を実行させる方法は後で書く。

    public void OnClickLeft() {
        xRef = xRef - 1.0f;
    }

    public void OnClickRight() {
        xRef = xRef + 1.0f;
    }

さらに、Unityの画面上に変位xの目標値xRefと、倒立振子の実際の変位xをテキストで表示している。Unity上で文字を表示させる方法についても後で書く。

        xRefText.text = "xRef = " + xRef.ToString(); 
        xText.text = "x = " + x.ToString();

 

Unityで倒立振子にスクリプトを追加する

さきほどつくったモデルをUnity上に設定していく。基本的には以前つくったモデルと変わらないが、ボタンとテキストの内容を変えて

  • ButtonLeft:変位の目標値を-1するボタン
  • ButtonRight:変位の目標値を+1するボタン
  • TextRefX:変位の目標値を表示するテキスト
  • TextX:現在の倒立振子の変位を表示するテキスト

を追加している。まずは、Cartオブジェクトを選択してMoveCartServo.csを追加する。

次にButtonLeftを選択し、以下の画像のようにOnclickにCartをドラッグ&ドロップしたあと、MoveCartServo.OnClickLeftを設定する。これでボタンを押すことで、MoveCartServoのOnClickLeft関数が実行される。ButtonRightについても同様に設定する。

最後に、再びCartを選択して、Move Cart ServoのX Ref Text、X Textに、TextRefオブジェクト、Textオブジェクトをそれぞれドラッグ&ドロップする。これで画面上に目標値と実際の変位が表示される。

これで準備は完了。

 

実行して動かしてみる

後はUnityで動かすだけ。再生ボタンを押して、ボタンを適当に押して目標値を変えてみた様子が以下の動画。ボタンで設定した目標値xRefの値に対して、台車が動いて追従していることがわかる。

やはり波形だけでなく、アニメーションで動いている様子が見えると楽しい。

次回は、いったんMATLABに戻って台車型倒立振子の最適レギュレータを設計してみたいと思う。

 

参考文献

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

 

MATLAB/Simulink記事まとめ

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

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

 


投稿者: wakky

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

コメントを残す

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

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