【倒立振子part20】連続系から離散系への変換について

これまで連続系での状態空間モデルを作成し最適制御を用いてフィードバックゲインを求めて安定化できることを確認した。
今回からは実際にマイコンで動かすために連続系から離散系への変換を行っていく。
手探りで進めていくので、誤った解釈があるかもしれないのでなるべく丁寧に理解していく。

ゼロ次ホールド
離散化した状態方程式
- 離散化した状態方程式の安定確認
離散化システムの最適制御
シュミレーション
次回

ゼロ次ホールド

マイコンを使用する際に測定値は一定のサンプリングレートで取り組むことができる。
実際に振子の角度やモータの角速度などは連続的に変化しているが、マイコンはサンプリングレート毎でしか測定できないため「次のサンプリングレートまでは前の値がそのまま引き継がれる」という考えゼロ次ホールドと呼ばれる。

離散化した状態方程式

連続系での状態方程式 $\dot{x}_{t}=Ax(t)+Bu(t)$ の解は計算式は省略するが下記の様に求められる。
$\displaystyle{x(t)=e^{A(t-t_0)}x(t_0)+e^{At}\int_{t_0}^{t} e^{-A\tau}Bu(\tau)d\tau}$

ここでサンプリングレートをTとすると、ある時刻tの次のステップ時刻t+Tの状態方程式の解 $x_{t+T}$ は次式で表せる。
ただし入力 $u(t)$ は離散化されてゼロ次ホールドされているとする。そのためk番目の入力という意味を込めて $u_k$ とする。
$\displaystyle{\begin{equation} \begin{split} x(t+T)&=e^{A(t+T-t_0)}x(t_0)+e^{A(t+T)}\int_{t_0}^{t+T} e^{-A\tau}Bu_kd\tau\\ &=e^{A(t+T-t_0)}x(t_0)+e^{A(t+T)}\int_{t_0}^{t} e^{-A\tau}Bu_kd\tau+e^{A(t+T)}\int_{t}^{t+T} e^{-A\tau}Bu_kd\tau\\ &=e^{AT}\left\{e^{A(t-t_0)}x(t_0)+e^{At}\int_{t_0}^{t} e^{-A\tau}Bu_kd\tau\right\}+e^{A(t+T)}\int_{t}^{t+T} e^{-A\tau}Bu_kd\tau\\ &=e^{AT}x(t)+e^{A(t+T)}\left\{\int_{t}^{t+T} e^{-A\tau}Bd\tau\right\}u_k\\ &=e^{AT}x(t)+e^{AT}\left\{\int_{0}^{T} e^{-At}dt\right\}Bu_k\\ \end{split} \end{equation}}$
以上よりある時刻tにおける状態 $x_k$ に対して次ステップ時間T進んだt+Tにおける状態を $x_{k+1}$ とすると下式で表すことができる。
$\displaystyle{x_{k+1}=e^{AT}x_k+e^{AT}\left\{\int_{0}^{T} e^{-At}dt\right\}Bu_k}$
これが離散化した状態方程式となる。状態方程式の形にすると
$x_{k+1}=Adx_k+Bdu_k$
$Ad=e^{AT}$
$\displaystyle{Bd=e^{AT}\left\{\int_{0}^{T} e^{-At}dt\right\}B=\left\{\int_{0}^{T} e^{A(T-t)}dt\right\}B}$

離散化した状態方程式の安定確認

連続時間システムでは漸近安定である必要十分条件は「行列Aのすべての固有値の実部が負であること」に対して離散時間システムでの必要十分条件は「行列Adのすべての固有値の絶対値が1より小さい」となる。(導出式は連続時間システムと同様なので省略)

離散化システムの最適制御

離散化された状態方程式のフィードバック制御でも連続系と同様で $u_k=K_dx_k$ として状態方程式の状態を安定化させることができる。
この時の状態フィードバックゲインである $K_d$ は下記で求めることができる。
●評価関数 $J_d$ は行列 $Q_d$ と定数 $R_d$ をを用いて
$\displaystyle{J_d=\sum_{k=0}^{\infty}({x_k}^TQ_dx_k+R_d{u_k}^2)}$
●この時、評価関数 $J_d$ の値を最小にするための状態フィードバック系は下記で表せる。
$u_k=K_dx_k$
$K_d=-(R_d+{B_d}^TP_dB_d)^{-1}{B_d}^TP_dA_d$
●上式の行列 $P_d$ は下記の「離散時間リカッチ代数方程式」を満たす正定値対称行列である。
${A_d}^TP_dA_d-P_d-{A_d}^TP_dB_d(R_d+{B_d}^TP_dB_d)^{-1}{B_d}^TP_dA_d+Q_d=0$

シュミレーション

離散化については上記でまとめたので実際のシステムでの離散化での初期応答のシュミレーションを行う。
上記の計算についてもscilabのコマンドを使用することで自動で計算できるため省略できる。
シュミレーションとしては、初期状態による応答、重み行列 $Q,R$ を変えたときの応答値の挙動についてもまとめる。

初期状態を変更してシュミレーション

サンプル周期と $T=0.005[s$ ]とする。
初期状態 $x0=1,5,10[deg$ ]の3パターンで分けてシュミレーションする。
この時の重み行列は
$Qd=Q\cdot{T}=\displaystyle{\begin{pmatrix}1000&0&0&0 \\0&1&0&0\\0&0&1&0 \\0&0&0&1 \end{pmatrix}\cdot0.005}$
$\displaystyle{Rd=R\cdot{T}=1000\cdot0.005}$
とする。

初期状態として5degとともかく10degも傾くことはないので初期応答としては問題ないと判断する。
連続システムに対して離散システムは応答性が悪くなっていることが分かった。

重み行列 $Q,R$ を変更してシュミレーション

次に初期状態を $5[deg$ ]として、重みを変更してシュミレーションを行う。

【q1の重みを1000として固定してRの重みを変更】

Rを小さくすることで速応答性が上がるため入力が高くなるためR=500ぐらいが限界かなと感じる。

scilabのプログラム

scilabのプログラムは下記の様にした。ただし状態空間モデルと連続系については前回と同様なので省略する。

dt=0.005;                           //サンプル周期[s]
pole_sys_d=dscr(pole_sys,dt);       //連続状態の線形システムpole_sysをdtで離散化
Ad=pole_sys_d(2);
Bd=pole_sys_d(3);
Cd=pole_sys_d(4);
V_d=cont_mat(pole_sys_d);           //可制御行列
disp("可制御行列のランク")
disp(rank(V_d));                    //可制御行列のランク確認

//最適制御の重み設計
q1=100;       //振子角度の重み
q2=0.1;       //ホイール角度の重み
q3=0.1;       //振子角速度の重み
q4=0.1;       //ホイール角速度の重み
Q=[q1,0,0,0;0,q2,0,0;0,0,q3,0;0,0,0,q4];               //直立状態を維持したいのでθ1とθ1'にかかる値を大きくする(ゲインを大きくする),逆にモータの回転については重要度は小さくする
R=1000;                                             //R値を大きくするとモータを激しく動かさない、R値を小さくするとモータを素早く動かして安定するシステム
Qd=Q*dt;
Rd=R*dt;

//離散化した最適制御
Pd=ricc(Ad,Bd*inv(Rd)*Bd',Qd,'disc');              //リカッチ方程式の解,contは連続、discは離散
Kd=inv(Rd+Bd'*Pd*Bd)*Bd'*Pd*Ad;                   //最適レギュレータ
ramda_d=abs(spec(Ad-Bd*Kd));                     //レギュレータ極(固有値)→離散系では絶対値が1より小さいときに漸近安定する
pole_sys_d_fb=syslin(dt,Ad-Bd*Kd,Bd,Cd);        //サンプル周期dtでの線形システムの定義
disp("離散リカッチ代数方程式の解 ")
disp(Xd)
disp("離散フィードバックゲイン ")
disp(Kd)
disp("離散フィードバックの固有値 ")
disp(ramda_d)

//状態シュミレーション
d0=-0.017*5;                                        //初期状態値
x0=[d0;0;0;0];                                       //単位はrad、1deg=0.017rad
t=0:0.01:2.5;
sys_d_x=flts(zeros(t),pole_sys_d_fb,x0);               //状態xの応答
Vm=-Kd*sys_d_x;                                          //システム入力u=モータ電圧Vはu=-Kx
Im=(Vm-kt*sys_d_x(4,:))/R;                              //モータの印可電流
subplot(3,1,1); plot(t,sys_d_x(1,:),'r');
xtitle('t-θ','時間[t]','本体傾き[rad]')
xgrid()
legend(['連続系';'離散系'],4);
subplot(3,1,2); plot(t,sys_d_x(2,:),'r');
xtitle('t-ω','時間[t]','本体傾き[rad/s]')
xgrid()
legend(['連続系';'離散系'],4);
subplot(3,1,3); plot(t,Vm,'r');
xtitle('t-V','時間[t]','印可電圧[V]')
xgrid()                          //システムの状態の1列目はθ1,2列目はθ1',3列目はθ2,4列目はθ2'
legend(['連続系';'離散系'],4);