MATLABで設計したサーボシステムをUnityで実装＆動かしてみる
前回はMATLABで台車型倒立振子のサーボシステムを設計して、Simulinkでシミュレーションしてみた。
今回は以前状態フィードバック制御のときにつくったUnityの台車型倒立振子のモデルを使って、サーボシステムをUnity上で実装して動かしてみる。Unityの台車型倒立振子のモデルのつくり方は以下を参照。
開発環境
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の記事は以下にまとめてます。
