OpenRCFで倒立振子のシミュレーションをする方法

本稿では，OpenRCFで倒立振子のシミュレーションをする方法を解説していきます．

OpenRCFは下記のサイトからダウンロードできます．
https://mase.openrcf.site/

倒立振子のモデル

下図のようなモデルを考えます．ここでは台車に与える力 $f$ が制御入力となります．

後述するソースコード内では，各パラメータが次のように表現されています．

• 棒の長さ：PoleLength [m]
• 棒の傾き角度：Theta [rad]
• 棒の角速度：Omega [rad/s]
• 棒の質量：PoleMass [kg]
• 台車に与える力：Force [N]
• 台車の質量：BaseMass [kg]
• 台車の位置：Position [m]
• 台車の速度：Velocity [m/s]
• 数値計算をする際の微小時間（初期値は0.05）：SamplingTime [s]

ソースコード

倒立振子に関する変数と関数をまとめた InvertedPendulum Class は，本来は別ファイルに記述するのが一般的ですが，ここでは全て MainWindow.xaml.cs ファイルに記述した例を示します．MainWindow Class の上に InvertedPendulum Class（150行程度）が記述されていますが，その中身は理解していなくても問題ないため，何も考えずにコピー＆ペーストでOKです．

MainWindow.xaml.cs

class InvertedPendulum
{
    public float Position, Velocity;        
    public float Theta, Omega, Torque;                
    public float Force;        
    private readonly static float g = 9.80665f;
    public float Inertia { get; private set; } = 1;
    public float samplingTime = 0.05f;
    public float SamplingTime
    {
        get { return samplingTime; }
        set
        {
            if(isDynamicsEnabled)
            {
                Console.WriteLine("Error: SamplingTime setting must be before StartDynamics().");
            }
            else if(value < 0.001f)
            {
                Console.WriteLine("Error: SamplingTime is too small.");
            }
            else
            {
                samplingTime = value;
            }
        }
    }

    private Cuboid Pole = new Cuboid(0.1f, 0.1f, 0.8f);
    private Pillar Joint = new Pillar();
    private Cuboid Base = new Cuboid(0.4f, 0.4f, 0.2f);

    private float poleLength = 0.5f;
    public float PoleLength
    {
        get { return poleLength; }
        set
        {
            poleLength = value;
            Inertia = 0.25f * poleMass * poleLength * poleLength;
            Pole.SetSize(0.1f * poleLength, 0.1f * poleLength, poleLength);
            Pole.SetPositionOffset(0, 0, 0.5f * Pole.SizeZ);
            Joint.Radius = 0.075f * poleLength;
            Joint.Height = 0.12f * poleLength;
        }
    }

    private float poleMass = 0.2f;
    public float PoleMass
    {
        get { return poleMass; }
        set 
        {
            if (0 < value)
            {
                poleMass = value;
                Inertia = 0.25f * poleMass * poleLength * poleLength;
            }
            else
            {
                Console.WriteLine("Error: Mass must be greater than 0.");
            }
        }
    }

    private float baseMass = 0.2f;
    public float BaseMass
    {
        get { return baseMass; }
        set
        {
            if (0 < value) baseMass = value;
            else Console.WriteLine("Error: Mass must be greater than 0.");
        }
    }
        
    public InvertedPendulum(float poleLength = 0.8f)
    {          
        PoleLength = poleLength;
        Pole.Position = Joint.Position;
        Pole.Rotate = Joint.Rotate;
        Base.Position = Joint.Position;
        Base.SetPositionOffset(0, 0, -0.5f * Base.SizeZ);
        Joint.SetRotateOffset(0.5f * (float)Math.PI, 0, 0);
        Joint.Color.SetLightGray();            
        Joint.Position[2] = Base.SizeZ;
    }

    public void SetBaseSize(float sizeX, float sizeY, float sizeZ)
    {
        Base.SetSize(sizeX, sizeY, sizeZ);
        Base.SetPositionOffset(0, 0, -0.5f * Base.SizeZ);
        Joint.Position[2] = Base.SizeZ;
    }

    public void Draw()
    {
        Pole.Draw();
        Joint.Draw();
        Base.Draw();
    }

    private void CalcDynamics()
    {
        float CosTh = (float)Math.Cos(Theta);
        float SinTh = (float)Math.Sin(Theta);
        float force = Force - PoleMass * g * CosTh * SinTh;

        Velocity += SamplingTime * force / (BaseMass + PoleMass);
        Position += SamplingTime * Velocity;
        Joint.Position[0] = Position;

        float poleForce = PoleMass / (BaseMass + PoleMass) * force;
        Torque = 0.5f * poleLength * (PoleMass * g * SinTh - poleForce * CosTh);
        Omega += SamplingTime * Torque / Inertia;
        Theta += SamplingTime * Omega;
        Joint.Rotate.SetRy(Theta);
    }

    private bool isDynamicsEnabled = false;
    public void StartDynamics()
    {
        if (isDynamicsEnabled == false)
        {
            Parallel.RunEndless(CalcDynamics, (uint)(1000 * SamplingTime));
            isDynamicsEnabled = true;
        }
        else
        {
            Console.WriteLine("Warning: Dynamics is already enabled.");
        }                
    }
        
    public void Reset()
    {            
        Force = 0;
        Position = 0;
        Velocity = 0;
        Theta = 0;
        Omega = 0;
    }
}

public partial class MainWindow : Window
{
    InvertedPendulum InvertedPendulum = new InvertedPendulum();

    void Setup()
    {
        InvertedPendulum.PoleLength = 0.8f;
        InvertedPendulum.PoleMass = 0.2f;
        InvertedPendulum.BaseMass = 0.2f;            
    }

    void Draw()
    {
        InvertedPendulum.Draw();           
    }

    void Button1_Click(object sender, RoutedEventArgs e)
    {            
        InvertedPendulum.StartDynamics();            
    }
     
    void Button2_Click(object sender, RoutedEventArgs e)
    {
        InvertedPendulum.Theta += 0.01f;            
    }
    
    void P_Control()
    {
        float Kp = 12;
        InvertedPendulum.Force = Kp * InvertedPendulum.Theta;
    }
 
    void Button3_Click(object sender, RoutedEventArgs e)
    {
        Parallel.RunEndless(P_Control, 50);
    }
    
    void Button4_Click(object sender, RoutedEventArgs e)
    {
        InvertedPendulum.Reset();
    }

}

上記のソースコードは，OpenRCF ver2.3 を前提に記述しています（旧バージョンのOpenRCFでは，関数Setup() が Initialize() という名前になっているため，一部修正が必要です）．
このプログラムを実行すると，下図のように倒立振子が出現します．

ボタン１を押すと，倒立振子のシミュレーション（数値計算）がバックグラウンドで開始されます．しかし，棒の傾き角度（Theta）が 0 なので，この時点では動かないです．
ボタン２を押すと，Theta に微小な量が加算され，その後は倒立振子が大きく動きだします．
ボタン３を押すと，P制御によって制御入力 $f$（InvertedPendulum.Force）が与えられます．ただし，棒の傾き角度（Theta）が小さいときにP制御を開始しないと，倒立振子が吹っ飛びます．吹っ飛んだ場合は，ボタン４を押して状態をリセットしましょう．

補足

各パラメータは「InvertedPendulum.変数名」で取得できます．例えば，現在の位置（Position）を取得したい場合は，InvertedPendulum.Position で取得できます．

関数 StartDynamics() を実行すると，バックグラウンドで 50 [ms] に1回のペースで数値計算が実行されます．この周期は SamplingTime の値を書き換えることで変更できます．

台車の大きさは，関数 SetBaseSize(sizeX, sizeY, sizeZ) で変更できます．引数が順に幅，奥行，高さとなっており，単位はメートルです．