MATLABとSimulinkで台車型倒立振子のサーボシステムを設計する
前回まで、台車型倒立振子のモデルに対して状態フィードバックをかけて各状態変数を0に収束させた。今回は、倒立を維持したまま任意に設定した目標の位置に台車を動かすようなサーボシステムをMATLABで設計してみる。対象はいつものように
- 状態変数：台車の変位(x)、速度(dx/dt)、振子の角度(θ)、角速度(dθ/dt)
- 入力：台車に加える力(u)
として、以下の台車型倒立振子を対象とする。線形化した台車型倒立振子のモデルは以下のサイトのものをそのまま使わせて頂いているm(_ _)m
台車型倒立振子を安定にフィードバック制御する｜Tajima Robotics
開発環境
MATLAB online (MATLAB R2022a)
Simulink
Control System Toolbox
設計するサーボシステム
ブロック図を書くのが面倒なので笑、Simulinkで作成したブロック図を以下にのせておく。
基本的には一般的な状態フィードバック+積分器で構成されたサーボシステム。今回は外乱も入れている。なお、SimulinkのState Spaceブロックの出力は4つの状態変数なのでCは4行4列の単位行列とし、そのあとで台車の変位だけを出力するようにC = [1 0 0 0]としている。
このサーボシステムについて状態方程式を拡張して、K、GのゲインをMATLABで求める。また、State Spaceブロックの各パラメータA、B、Dは次の節でMATLABで計算したものを指定している。（ここで指定するのは拡張したAe、Beではなく、元々のA、Bなので注意）
MATLABでサーボシステムのゲインを計算
今回は以下のようにMATLABでmファイルを実行してゲインKとGを計算した。
% 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))];
C = [1 0 0 0];
D = zeros(4,1);
Ae = [A zeros(4,1)
-C zeros(1,1)];
Be = [B
zeros(1,1)];
% Check Controllability
Co = ctrb(Ae,Be);
rank(Co)
% Pole/Feedback Gain
p = [-1+1j -1-1j -2+1j -2-1j -3];
Ke = place(Ae,Be,p);
K = Ke(1,1:4)
G = Ke(1,5)
極pは適当に決定して、placeで計算されたKeをK、Gに分割している。この計算の結果、次のゲインが得られた。
K = -89.2517 -93.5492 -677.8023 -247.7323
G =41.8367
Simulinkでシミュレーションしてみる
mファイルを実行してワークスペースに各パラメータが保存されたら、さきほどのSimulinkのモデルを開いてシミュレーションを実行する。以下が実行してみた結果。T=1でステップ関数で目標値を与えたあと、T=5で外乱を与えているが、外乱を除去しながら目標値に追従できていることがわかる。
Simulinkで動作が確認できたので、いつものように次回はUnityでサーボシステムを実行して、台車型倒立振子の動作をシミュレーションしてみたいと思う。
参考文献
この本のMATLAB/Simulink 6か月ライセンスを使ってやってます↓
Interface 2022年 9月号
サーボ制御については以下を参考にさせて頂きましたm(_ _)m
MATLAB/Simulink記事まとめ
MATLABとSimulinkの記事は以下にまとめてます。
