「制御技術」カテゴリーアーカイブ

SICE BP（３慣性系）

投稿日時: 投稿者:

●原、千田、佐伯、野波：ロバスト制御のためのベンチマーク問題(I)
-３慣性系に対する位置制御・速度制御
●佐伯、千田、野波、原：ロバスト制御のためのベンチマーク問題(II)
-位置制御系の設計例

[1]　次図のような３慣性系を考えます。

これは次の運動方程式で表されます。

\displaystyle{(1) \begin{array}{l} \bullet j_1\ddot{\theta}_1(t)+d_1\dot{\theta}_1(t)=k_a(\theta_2(t)-\theta_1(t))+d_a(\dot{\theta}_2(t)-\dot{\theta}_1(t))+\tau(t)+\tau_{d1}\\ \bullet j_2\ddot{\theta}_2(t)+d_2\dot{\theta}_2(t)=-k_a(\theta_2(t)-\theta_1(t))-d_a(\dot{\theta}_2(t)-\dot{\theta}_1(t))\\+k_b(\theta_3(t)-\theta_2(t))+d_b(\dot{\theta}_3(t)-\dot{\theta}_2(t))+\tau_{d2}\\ \bullet j_3\ddot{\theta}_3(t)+d_3\dot{\theta}_3(t)=-k_b(\theta_3(t)-\theta_2(t))-d_b(\dot{\theta}_3(t)-\dot{\theta}_2(t))+\tau_{d3} \end{array} }

ここで、物理定数は次の値が想定されています。

物理定数 最小値 公称値 最大値
j_1 0.0009 0.001 0.0011
j_2 0.0009 0.001 0.0011
j_3 0.001 0.002 0.003
d_1 0.045 0.05 0.055
d_2 0.0009 0.001 0.0011
d_3 0.0014 0.007 0.035
d_a 0.0002 0.001 0.01
d_b 0.0009 0.001 0.0011
k_a 828 920 1012
k_b 72 80 88

●以下では、j_3,d_3,k_bの３つのパラメータ誤差を考慮して、回転体３に対してインパルス外乱が加わるときのシミュレーションを行ってみます。

図1

原論文にはさまざまな問題設定がなされていますが、ここでは、回転体３に対するインパルス外乱の下で、回転体３を速やかに整定させる制御系設計を検討します。ただし、操作入力には次の振幅制限

\displaystyle{(3)\quad \begin{array}{l} |u(t)|\le 3 \end{array} }

が課され、観測出力は回転体１の変位\theta_1(t)とします。

[2] 制御対象のLPVモデルを導出します。

\displaystyle{(4)\quad \begin{array}{l} \underbrace{ \left[\begin{array}{ccc} j_1 & 0 & 0\\ 0 & j_2 & 0\\ 0 & 0 & j_3 \end{array}\right] }_{J} \underbrace{ \left[\begin{array}{ccc} \ddot{\theta}_1(t)\\ \ddot{\theta}_2(t)\\ \ddot{\theta}_3(t) \end{array}\right] }_{\ddot{\xi}(t)} +\underbrace{ \left[\begin{array}{ccc} d_1+d_a & -d_a & 0\\ -d_a & d_2+d_a+d_b & -d_b\\ 0 & -d_b & d_3+d_b \end{array}\right] }_{D} \underbrace{ \left[\begin{array}{ccc} \dot{\theta}_1(t)\\ \dot{\theta}_2(t)\\ \dot{\theta}_3(t) \end{array}\right] }_{\dot{\xi}(t)}\\ +\underbrace{ \left[\begin{array}{ccc} k_a & -k_a & 0\\ -k_a & k_a+k_b & -k_b\\ 0 & -k_b & k_b \end{array}\right] }_{K} \underbrace{ \left[\begin{array}{ccc} {\theta}_1(t)\\ {\theta}_2(t)\\ {\theta}_3(t) \end{array}\right] }_{{\xi}(t)} = \underbrace{ \left[\begin{array}{ccc} 1\\ 0\\ 0 \end{array}\right] }_{E_{3\times1}}\tau(t) +\underbrace{ \left[\begin{array}{ccc} \tau_{d1}\\ \tau_{d2}\\ \tau_{d3} \end{array}\right] }_{\tau_{d}} \end{array} }

\displaystyle{(5)\quad \begin{array}{l} \left[\begin{array}{c} \dot{\xi}(t)\\ \ddot{\xi}(t) \end{array}\right] = \underbrace{ \left[\begin{array}{cccc} 0_{3\times3} & I_3\\ -J^{-1}K & -J^{-1}D \end{array}\right] }_{A(d_3/j_3,k_b/j_3)} \left[\begin{array}{c} {\xi}(t)\\ \dot{\xi}(t) \end{array}\right] + \left[\begin{array}{c} 0\\ J^{-1}E_{3\times1} \end{array}\right]\tau(t) + \left[\begin{array}{c} 0\\ J^{-1} \end{array}\right]\tau_d \end{array} }

\displaystyle{(6)\quad \begin{array}{l} J^{-1}D={ \left[\begin{array}{ccc} d_1/j_1+d_a/j_1 & -d_a/j_1 & 0\\ -d_a/j_2 & d_2/j_2+d_a/j_2+d_b/j_2 & -d_b/j_2\\ 0 & -d_b/j_3 & d_3/j_3+d_b/j_3 \end{array}\right]\\ =(d_1/j_1)D_1+(d_2/j_2)D_2+(d_3/j_3)D_3\\ +(d_a/j_1)D_4+(d_a/j_2)D_5+(d_b/j_2)D_6+(d_b/j_3)D_7\\ J^{-1}K= \left[\begin{array}{ccc} k_a/j_1 & -k_a/j_1 & 0\\ -k_a/j_2 & k_a/j_2+k_b/j_2 & -k_b/j_2\\ 0 & -k_b/j_3 & k_b/j_3 \end{array}\right]\\ =(k_a/j_1)K_1+(k_a/j_2)K_2+(k_b/j_2)K_3+(k_b/j_3)K_4\\ \end{array} }

\displaystyle{(6)\quad {\theta}_1(t)=\left[\begin{array}{cccc} E_{1\times 3} & 0_{1\times 3} \end{array}\right] \left[\begin{array}{c} {\xi}(t)\\ \dot{\xi}(t) \end{array}\right] }

ここで、\alpha_1\le\alpha\le\alpha_2、\beta_1\le\beta\le\beta_2に対する次の内分式に注目します。

\displaystyle{(8)\quad \begin{array}{l} \left[\begin{array}{c} \alpha\\ \beta \end{array}\right]= \underbrace{\frac{\alpha_2-\alpha}{\alpha_2-\alpha_1}\frac{\beta_2-\beta}{\beta_2-\beta_1}}_{p_{11}(\alpha,\beta)}\left[\begin{array}{c} \alpha_1\\ \beta_1 \end{array}\right]+ \underbrace{\frac{\alpha_2-\alpha}{\alpha_2-\alpha_1}\frac{\beta-\beta_1}{\beta_2-\beta_1}}_{p_{12}(\alpha,\beta)}\left[\begin{array}{c} \alpha_1\\ \beta_2 \end{array}\right]\\+ \underbrace{\frac{\alpha-\alpha_1}{\alpha_2-\alpha_1}\frac{\beta_2-\beta}{\beta_2-\beta_1}}_{p_{21}(\alpha,\beta)}\left[\begin{array}{c} \alpha_2\\ \beta_1 \end{array}\right]+ \underbrace{\frac{\alpha-\alpha_1}{\alpha_2-\alpha_1}\frac{\beta-\beta_1}{\beta_2-\beta_1}}_{p_{22}(\alpha,\beta)}\left[\begin{array}{c} \alpha_2\\ \beta_2 \end{array}\right] \\ \end{array} }

\displaystyle{(9)\quad p_{11}(\alpha,\beta)+p_{12}(\alpha,\beta)+p_{21}(\alpha,\beta)+p_{22}(\alpha,\beta)=1 }

\alpha=d_3/j_3,\beta=k_b/j_3のとき上の状態方程式は、端点モデル

\displaystyle{(10)\quad \begin{array}{l} \dot{x}(t)=\underbrace{A(\alpha_1,\beta_1)}_{A_{11}}x(t)+Bu(t)\\ \dot{x}(t)=\underbrace{A(\alpha_1,\beta_2)}_{A_{12}}x(t)+Bu(t)\\ \dot{x}(t)=\underbrace{A(\alpha_2,\beta_1)}_{A_{21}}x(t)+Bu(t)\\ \dot{x}(t)=\underbrace{A(\alpha_2,\beta_2)}_{A_{22}}x(t)+Bu(t) \end{array} }

を、p_{11},p_{12},p_{12},p_{22}によって重み付けして、LPVモデル

\displaystyle{(11)\quad \dot{x}(t)=\underbrace{(p_{11}A_{11}+p_{12}A_{12}+p_{21}A_{21}+p_{22}A_{22})}_{A(\alpha,\beta)}x(t)+Bu(t) }

として表されます。

[3]　状態FB

図2

[3]　出力FB

図3

演習13.3…Flipped Classroom

1^\circ 図1

MATLAB
SCILAB

2^\circ 図2

MATLAB
SCILAB

3^\circ 図3

MATLAB
SCILAB

２慣性系

投稿日時: 投稿者:

２慣性系の制御について、次の論文をフォローします。

松井義弘：PID制御による2慣性系の速度制御

２慣性系…Homework

[0]　次図のような２慣性系を考えます。

これは次の運動方程式で表されます。

\displaystyle{(1)\quad \begin{array}{l} j_1\dot{\omega}_1(t)+k\theta_1(t)=\tau_1(t)+k\theta_2(t)\\ j_2\dot{\omega}_2(t)+k\theta_2(t)=\tau_2(t)+k\theta_1(t) \end{array} }

ここで、\tau_1を操作トルク、\tau_2を外乱トルクとみなし、また

\displaystyle{(2)\quad \tau_s(t)=k(\theta_1(t)-\theta_2(t)) }

は軸トルクと呼ばれ、計測可能とします。これを用いると(1)は次式となります。

\displaystyle{(3)\quad \begin{array}{l} j_1\dot{\omega}_1(t)=-\tau_s(t)+\tau_1(t)\\ j_2\dot{\omega}_2(t)=\tau_s(t)+\tau_2(t) \end{array} }

以下では、一定の角速度での回転時に、大きさ1の定値外乱トルクが発生しても、望ましい角速度\omega^*での回転させる速度制御問題を検討します。回転体１の速度制御ですから基本はPI制御ですが、回転体２からの外乱トルクの影響を除くために、軸トルク（両軸のトルク差）のFBも考慮します（回転体２の角速度はFBしません）。両回転体の慣性モーメントの大小によって制御ゲインは変わることになります。

[1]　軸トルクFBを加えたPI制御

\displaystyle{(4)\quad \dot{\tau}_s(t)=k({\omega}_1(t)-{\omega}_2(t)) }

に注意して、次の状態方程式を得ます。

\displaystyle{(5)\quad \boxed{ \begin{array}{l} \underbrace{ \left[\begin{array}{c} \dot{\tau}_s(t)\\ \dot{\omega}_1(t)\\ \dot{\omega}_2(t) \end{array}\right] }_{\dot{\xi}(t)} = \underbrace{ \left[\begin{array}{cccc} 0 & k & -k\\ -1/j_1 & 0 & 0\\ 1/j_2 & 0 & 0 \end{array}\right] }_{A} \underbrace{ \left[\begin{array}{c} \tau_s(t)\\ \omega_1(t)\\ \omega_2(t) \end{array}\right] }_{\xi(t)} + \underbrace{ \left[\begin{array}{c} 0 \\ 1/j_1 \\ 0 \end{array}\right] }_{B} \tau_1(t) + \underbrace{ \left[\begin{array}{c} 0\\ 0\\ 1/j_2 \end{array}\right] }_{w} \\ \omega_1(t)= \underbrace{ \left[\begin{array}{cccc} 0 & 1 & 0 \end{array}\right] }_{C} \underbrace{ \left[\begin{array}{c} \tau_s(t)\\ \omega_1(t)\\ \omega_2(t) \end{array}\right] }_{\xi(t)}\\ \end{array}} }

ここで、\tau_1から\omega_1までの伝達関数

\displaystyle{(6)\quad G(s)=\frac{s^2+\omega_0^2}{j_1s(s^2+\omega_p^2)} }

から、次の共振角周波数\omega_pと反共振角周波数\omega_0を定義しておきます。

\displaystyle{(7)\quad \omega_p^2=\frac{(j_1+j_2)k}{j_1j_2}, \omega_0^2=\frac{k}{j_2} }

さて、制御目的が達成されているとすると

\displaystyle{(8)\quad \left[\begin{array}{c} 0\\ 0\\ -1/j_2\\ \omega^* \end{array}\right] = \underbrace{ \left[\begin{array}{cccc} 0 & k & -k & 0\\ -1/j_1 & 0 & 0 & 1/j_1\\ 1/j_2 & 0 & 0 & 0\\ 0 & 1 & 0 & 0 \end{array}\right] }_{S= \left[\begin{array}{cc} A & B\\ C & 0 \end{array}\right] } \left[\begin{array}{c} \tau_s(\infty)\\ \omega_1(\infty)\\ \omega_2(\infty)\\ \tau_1(\infty) \end{array}\right] }

より

\displaystyle{(9)\quad \underbrace{ \left[\begin{array}{c} \tau_s(\infty)\\ \omega_1(\infty)\\ \omega_2(\infty)\\ \tau_1(\infty) \end{array}\right] }_{\left[\begin{array}{c} \xi_\infty \\ \tau_{1,\infty} \end{array}\right]} = \underbrace{ \left[\begin{array}{cccc} 0 & 0 & j_2 & 0\\ 0 & 0 & 0 & 1\\ -1/k & 0 & 0 & 1\\ 0 & j_1 & j_2 &0\end{array}\right] }_{S^{-1}} \left[\begin{array}{c} 0\\ 0\\ -1/j_2\\ \omega^* \end{array}\right] = \left[\begin{array}{c} -1\\ \omega^*\\ \omega^*\\ -1 \end{array}\right] }

を得ます。したがって

\displaystyle{(10)\quad \left[\begin{array}{c} \dot{\xi}(t) \\ \omega_1(t)-\omega^* \end{array}\right] = \underbrace{ \left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right] }_{S} \left[\begin{array}{c} \xi(t)-\xi_\infty \\ \tau_1(t)-\tau_{1,\infty} \end{array}\right] }

に注意して、偏差系

\displaystyle{(11)\quad \frac{d}{dt} \left[\begin{array}{c} \xi(t)-\xi_\infty \\ \tau_1(t)-\tau_{1,\infty} \end{array}\right] = \underbrace{ \left[\begin{array}{cc} A & B \\ 0 & 0 \end{array}\right] }_{A_{E3}} \left[\begin{array}{c} \xi(t)-\xi_\infty \\ \tau_1(t)-\tau_{1,\infty} \end{array}\right] + \underbrace{ \left[\begin{array}{c} 0 \\ 1 \end{array}\right] }_{B_{E3}} \dot{\tau}_1(t) }

に対する安定化状態フィードバック

\displaystyle{(12)\quad \begin{array}{l} \dot{\tau}_1(t) = K_{E3} \left[\begin{array}{c} \xi(t)-\xi_\infty \\ \tau_1(t)-\tau_{1,\infty} \end{array}\right] = \underbrace{ K_{E3}S^{-1} }_{ \left[\begin{array}{cccc} F_1 & F_2 & F_3 & F_4 \end{array}\right]} \left[\begin{array}{c} \dot{\xi}(t) \\ \omega_1(t)-\omega^* \end{array}\right] \end{array} }

すなわち

\displaystyle{(13)\quad \boxed{\tau_1(t)=-F_1\tau_s(t)-\underbrace{F_2}_{K_P}\omega_1(t)-\underbrace{F_3}_{0}\omega_2(t) -\underbrace{F_4}_{K_I}\int_0^t(\omega_1(\tau)-\omega^*)d\tau} }

を求めます。

ここで本来は２次形式評価関数を設定したいところですが、ゲインに制約をつけるために、上記論文では閉ループ系の望ましい特性多項式を

\displaystyle{(14)\quad {\rm det}(sI_4-A_{E3}+B_{E3}K_{E3})=s^4+\underbrace{\beta_1\omega}_{\alpha_1} s^3+\underbrace{\beta_2\omega^2}_{\alpha_2} s^2+\underbrace{\beta_3\omega^3}_{\alpha_3} s+\underbrace{\beta_4\omega^4 }_{\alpha_4}}

と設定し、極配置問題を解いています。これは、たとえば

\displaystyle{(15)\quad \lambda(A_{E3}-B_{E3}K_{E3})=\{(-\zeta\pm\sqrt{1-\zeta^2})\omega,(-\zeta\pm\sqrt{1-\zeta^2})\omega\} }

とすると

\displaystyle{(16)\quad \begin{array}{l} \alpha_1=4\zeta\omega\\ \alpha_2=(4\zeta^2+2)\omega^2\\ \alpha_3=4\zeta\omega^3\\ \alpha_4=\omega^4\\ \end{array} }

の形式になること、また減衰係数\zetaと固有角周波数\omegaを指定できるなどの理由によると思われます。

上の望ましい特性多項式を達成する状態フィードバックゲインは、次式によって計算できます。

\displaystyle{(17)\quad \begin{array}{l} K_{E3}= \left[\begin{array}{cccc} 0 & 0 & 0 & 1 \end{array}\right]\\ \times \left[\begin{array}{cccc} B_{E3} & A_{E3}B_{E3} & A_{E3}^2B_{E3} & A_{E3}^3B_{E3} \end{array}\right]^{-1}\\ \times (A_{E3}^4+\alpha_1 A_{E3}^3+\alpha_2 A_{E3}^2+\alpha_3 A_{E3}+\alpha_4 I_4) \end{array} }

数式処理プログラムを用いて、

\displaystyle{(18)\quad k=j_1(w_p^2-w_0^2), j_2=j_1(\frac{w_p^2}{w_0^2}-1) }

を代入して、次のように計算できます（K_2,K_3,K_0,K_4は論文中の記号）。

\displaystyle{(19)\quad \begin{array}{l} F_1=-(\omega_0^2 \omega_p^2-\beta_2 \omega^2 \omega_0^2+\omega^4)/(\omega_0^2 \omega_p^2-\omega_0^4)=K_2\\ F_2=\beta_1 j_1 \omega=K_3\\ F_3=-(\beta_1 j_1 \omega \omega_0^2-\beta_3 j_1 \omega^3)/\omega_0^2=K_1\\ F_4=(j_1 \omega^4)/\omega_0^2=K_4 \end{array} }

実際

\displaystyle{(20)\quad \begin{array}{l} F_3=0 \\ \beta_1 \omega_0^2-\beta_3 \omega^2=0\\ \omega^2=\frac{\beta_1}{\beta_3}\omega_0^2 \end{array} }

\displaystyle{(21)\quad \begin{array}{l} F_2=j_1 \beta_1 \sqrt{\frac{\beta_1}{\beta_3}} \omega_0\\ \end{array} }

\displaystyle{(22)\quad \begin{array}{l} F_4=j_1 \frac{\beta_1^2}{\beta_3^2}\omega_0^2 \end{array} }

\displaystyle{(23)\quad \begin{array}{l} F_1=-\frac{1}{\omega_p^2-\omega_0^2}(\omega_p^2-\beta_2 \omega^2 +\frac{\omega^4}{\omega_0^2})\\ =-\frac{1}{\omega_p^2-\omega_0^2}(\omega_p^2-\beta_2 \frac{\beta_1}{\beta_3}\omega_0^2 +\frac{\beta_1^2}{\beta_3^2}\omega_0^2)\\ =-\frac{1}{\omega_p^2-\omega_0^2}(\omega_p^2-\omega_0^2+\omega_0^2-\beta_2 \frac{\beta_1}{\beta_3}\omega_0^2 +\frac{\beta_1^2}{\beta_3^2}\omega_0^2)\\ =-\frac{\omega_0^2}{\omega_p^2-\omega_0^2}(1-\beta_2 \frac{\beta_1}{\beta_3} +\frac{\beta_1^2}{\beta_3^2})-1\\ =\frac{j_1}{j_2}(\beta_2 \frac{\beta_1}{\beta_3} -\frac{\beta_1^2}{\beta_3^2}-1)-1\\ \end{array} }

まず、負荷の角速度をFBしなくて済むためには、(20)が必要となり、\omega自身には選択の余地はありません。興味深いのは、(23)から、軸トルクのFBゲインの符号が正にも負にもなることです。

MAXIMA 
/*BP.wxm*/
A:matrix([0,k,-k],[-1/j1,0,0],[1/j2,0,0]);
B:matrix([0],[1/j1],[0]);
C:matrix([0,1,0]);
D:matrix([0]);
S:addrow(addcol(A,B),addcol(C,D));
xinf:invert(S).matrix([0],[0],[1/j2],[-r]);
AE:addrow(addcol(A,B),[0,0,0,0]);
BE:matrix([0],[0],[0],[1]);
V:BE;
V:addcol(BE,AE.V);
V:addcol(BE,AE.V);
V:addcol(BE,AE.V);
p:s^4+b1*w*s^3+b2*w^2*s^2+b3*w^3*s+w^4;
q:expand( (s^2+2*z*w*s+w^2)^2 );
a4:coeff(p,s,0);
a3:coeff(p,s,1);
a2:coeff(p,s,2);
a1:coeff(p,s,3);
a0:coeff(p,s,4);
U:matrix([0,0,0,1]);
W0:a4*ident(4);
W1:a3*AE;
W2:a2*AE^^2;
W3:a1*AE^^3;
W4:a0*AE^^4;
W:W4+W3+W2+W1+W0;
KE:U.invert(V).W;
KS:ratsimp(KE.invert(S));
FE:KS,k=j1*(wp^2-w0^2),j2=j1*(wp^2/w0^2-1);
K1:ratsimp(FE[1][3]);
K2:ratsimp(FE[1][1]);
K3:ratsimp(FE[1][2]);
K4:ratsimp(FE[1][4]);
/*---*/
/*K1=-(b1*j1*w*w0^2-b3*j1*w^3)/w0^2*/
/*K2=-(w0^2*wp^2-b2*w^2*w0^2+w^4)/(w0^2*wp^2-w0^4)*/
/*K3=b1*j1*w*/
/*K4=(j1*w^4)/w0^2*/
MATLAB 
%cBP.m
%-----
 clear all, close all
 j1=1; w0=1; r=0.4;
 j2=r*j1; k=w0^2*j2; wp=sqrt(k*(j1+j2)/(j1*j2));
 A=[0 k -k;-1/j1 0 0;1/j2 0 0];
 B=[0;1/j1;0]; wd=[0;0;1/j2];
 C=[0 1 0];
%-----
 w=1; z=1/sqrt(2);
 p=conv([1 2*z*w w^2],[1 2*z*w w^2]);
 a1=p(2); a2=p(3); a3=p(4); a4=p(5);
 %a1=4*z*w; a2=(4*z^2+2)*w^2; a3=4*z*w^3; a4=w^4;
 AE=[A B;zeros(1,4)]; BE=[0;0;0;1];
 K1=[0 0 0 1];
 K2=[BE AE*BE AE^2*BE AE^3*BE];
 K3=AE^4+a1*AE^3+a2*AE^2+a3*AE+a4*eye(4,4);
 KE=K1*(K2\K3);
 AECL=AE-BE*KE;
 S=[A B;C 0];
 FE=KE/S;
 sys=ss(AECL,[wd;0],[[C 0];-FE],[]); 
 t=0:0.2:20;
 figure
 step(sys,t), grid
%-----
%  b1=4*z; b2=4*z^2+2; b3=4*z; b4=1;
%  F1=-(w0^2*wp^2-b2*w^2*w0^2+w^4)/(w0^2*wp^2-w0^4)
%  F2=b1*j1*w
%  F3=-(b1*j1*w*w0^2-b3*j1*w^3)/w0^2
%  F4=(j1*w^4)/w0^2
%-----静止・インパルス外乱（大きさ0.2）
 r=0
 w=0
 tau0=0
 om10=0
 om20=1/j2*0.2
 sim("nBP")
%-----静止・定値外乱（大きさ0.2）
 r=0
 w=0.2
 tau0=0
 om10=0
 om20=0
 sim("nBP")
%-----定速回転・インパルス外乱（大きさ0.2）
 r=1
 w=0
 tau0=0
 om10=0
 om20=1/j2*0.2
 sim("nBP")
%-----定速回転・定値外乱（大きさ0.2）
 r=1
 w=0.2
 tau0=0
 om10=0
 om20=0
 sim("nBP")
%-----
%eof

●数値微分による軸トルクを加えたPI制御

軸トルクは次式のように速度を１階微分して得ることができます。

\displaystyle{(24)\quad \boxed{\tau_s(t)=k(\theta_1(t)-\theta_2(t))=\tau_1(t)-j_1\frac{d}{dt}\omega_1(t)} }

これを用いると

(25)\quad \begin{array}{l} \displaystyle{\tau_1(t)=-F_1(\tau_1(t)-j_1\frac{d}{dt}\omega_1(t))-F_2\omega_1(t) -F_4\int_0^t(\omega_1(\tau)-\omega^*)d\tau}\\ \displaystyle{(1+F_1)\tau_1(t)=F_1j_1\frac{d}{dt}\omega_1(t)-F_2\omega_1(t)-F_4\int_0^t(\omega_1(\tau)-\omega^*)d\tau} \end{array} }

\displaystyle{(26)\quad %\begin{array}{l} \boxed{\tau_1(t)=-\underbrace{\frac{F_2}{1+F_1}}_{F'_2}\omega_1(t)+\underbrace{\frac{F_1}{1+F_1}}_{F'_1}j_1\frac{d}{dt}\omega_1(t)-\underbrace{\frac{F_4}{1+F_1}}_{F'_4}\int_0^t(\omega_1(\tau)-\omega^*)d\tau} %\end{array} }

これはPID制御となっており、軸トルクのFBは不要となっています。

●外乱オブザーバによる軸トルクを加えたPI制御

外乱オブザーバの仕組みは、数値微分を用いた軸トルクを１次フィルタを通したものに等しいことが知られています。これは

\displaystyle{(27)\quad \boxed{\begin{array}{l} \dot{\theta}_R(t)=-\frac{1}{T_R}\theta_R(t)+\frac{1}{T_R}\underbrace{\tau_s(t)}_{\tau_1(t)-j_1\frac{d}{dt}\omega_1(t)} \end{array}} }

を用いて

\displaystyle{(28)\quad \boxed{\tau_1(t)=-F_1 \theta_R(t)-F_2\omega_1(t) -F_4\underbrace{\int_0^t(\omega_1(\tau)-\omega^*)d\tau}_{x_I(t)}} }

のように実施されます。以下では、これもPID制御であることを示します。

\displaystyle{(29)\quad \begin{array}{l} \dot{\theta}_R(t)=-\frac{1}{T_R}\theta_R(t)+\frac{1}{T_R}(-F_1 \theta_R(t)-F_2\omega_1(t) -F_4x_I(t)-j_1\frac{d}{dt}\omega_1(t))\\ =-\frac{1}{T_R}(1+F_1 )\theta_R(t)-\frac{F_4}{T_R}x_I(t) -\frac{1}{T_R}(F_2\omega_1(t)+j_1\frac{d}{dt}\omega_1(t))\\ =-\underbrace{\frac{1+F_1 }{T_R}}_{\frac{1}{T_D}}\theta_R(t)-\underbrace{\frac{F_4}{1+F_1 }}_{F'_4}\frac{1}{T_D}x_I(t) -\underbrace{\frac{F_2}{1+F_1 }}_{F'_2}\frac{1}{T_D}\omega_1(t) -\underbrace{\frac{j_1}{1+F_1 }}_{j'_1}\frac{1}{T_D}\frac{d}{dt}\omega_1(t) \end{array} }

のように書けるので、コントローラの状態空間表現は次式となります（簡単のため\omega^*=0としています）。

\displaystyle{(30)\quad \begin{array}{l} \left[\begin{array}{c} \dot{x}_I(t)\\ \dot{\theta}_R(t) \end{array}\right] = \underbrace{ \left[\begin{array}{cccc} 0 & 0\\ -\frac{F'_4}{T_D} & -\frac{1}{T_D} \end{array}\right] }_{A_F} \left[\begin{array}{c} x_I(t)\\ \theta_R(t) \end{array}\right] + \underbrace{ \left[\begin{array}{cc} 1\\ -\frac{1}{T_D}(F'_2+j'_1\frac{d}{dt}) \end{array}\right] }_{B_F}\omega_1(t)\\ \tau_1(t)= \underbrace{ \left[\begin{array}{cccc} -F_4 & -F_1 \end{array}\right] }_{C_F} \left[\begin{array}{c} x_I(t)\\ \theta_R(t) \end{array}\right] +\underbrace{(-F_2)}_{D_F}\omega_1(t) \end{array} }

これからコントローラの伝達関数が次式のように計算できます。

\displaystyle{(31)\quad \begin{array}{l} K(s)=\underbrace{ \left[\begin{array}{cccc} -F_4 & -F_1 \end{array}\right] }_{C_F} \underbrace{ \left[\begin{array}{cccc} s & 0\\ \frac{F'_4}{T_D} & s+\frac{1}{T_D} \end{array}\right]^{-1} }_{(sI-A_F)^{-1}} \underbrace{ \left[\begin{array}{cc} 1\\ -\frac{1}{T_D}(F'_2+j'_1s) \end{array}\right] }_{B_F} + \underbrace{(-F_2)}_{D_F}\\ = \left[\begin{array}{cccc} -F_4 & -F_1 \end{array}\right] \underbrace{ \left[\begin{array}{cccc} s & 0\\ \frac{F'_4}{T_D} & s+\frac{1}{T_D} \end{array}\right]^{-1} }_{ \frac{1}{s(s+\frac{1}{T_D})} \left[\begin{array}{cccc} s+\frac{1}{T_D} & 0\\ -\frac{F'_4}{T_D} & s \end{array}\right] } \left[\begin{array}{cc} 1\\ -\frac{1}{T_D}(F'_2+j'_1s) \end{array}\right] -F_2\\ =-F_4\frac{1}{s}+F_1 \frac{F'_4}{T_D}\frac{1}{s(s+\frac{1}{T_D})} +F_1 \frac{1}{s+\frac{1}{T_D}}\frac{1}{T_D}(F'_2+j'_1s)-F_2\\ =-(1+F_1 )(F'_2+F'_4\frac{1}{s})+F_1 \frac{1}{T_Ds+1}(F'_4\frac{1}{s}+F'_2+j'_1s)\\ =(-1-F_1 +F_1 \frac{1}{T_Ds+1}})(F'_2+F'_4\frac{1}{s})+F_1'\frac{j_1s}{T_Ds+1}}\\ =(-1-F_1 \frac{T_Ds}{T_Ds+1}})(F'_2+F'_4\frac{1}{s})+F_1'\frac{j_1s}{T_Ds+1}}\\ =(-1-F'_1\frac{T_Rs}{T_Ds+1}})(F'_2+F'_4\frac{1}{s})+F_1'\frac{j_1s}{T_Ds+1}}\\ =-F'_2-F'_4\frac{1}{s}-\frac{T_Rs}{T_Ds+1}}F'_1F'_2-\frac{T_Ds+1-T_Ds}{T_Ds+1}}T_RF'_1F'_4+F_1'\frac{j_1s}{T_Ds+1}}\\ =-F'_2-T_RF'_1F'_2-F'_4\frac{1}{s}-\frac{T_Rs}{T_Ds+1}}F'_1F'_2+\frac{T_Ds}{T_Ds+1}}T_RF'_1F'_4+F_1'\frac{j_1s}{T_Ds+1}}\\ =-\underbrace{(F'_2+T_RF'_1F'_2)}_{K_P}-\underbrace{F'_4}_{K_I}\frac{1}{s}-\frac{s}{T_Ds+1}}\underbrace{(T_RF'_1F'_2-T_DT_RF'_1F'_4-F_1'j_1)}}_{K_D}\\ %=F_P+F_I\frac{1}{s}+\frac{F_Ds}{Ts+1} \end{array} }

外乱オブザーバによる軸トルクを加えたPI制御は、１次フィルタ付微分動作をもつPID制御となっていることが分かります。

[2]　１次フィルタを通すPI制御

\displaystyle{(32)\quad \begin{array}{l} \dot{\tau}_1(t)=-\frac{1}{T_F}\tau_1(t)+\frac{1}{T_F}u(t) \end{array} }

\displaystyle{(33)\quad \boxed{ \begin{array}{l} \underbrace{ \left[\begin{array}{c} \dot{\tau}_s(t)\\ \dot{\omega}_1(t)\\ \dot{\omega}_2(t)\\ \dot{\tau}_1(t) \end{array}\right] }_{\dot{\xi}(t)} = \underbrace{ \left[\begin{array}{cccc} 0 & k & -k & 0\\ -1/j_1 & 0 & 0& 1/j_1\\ 1/j_2 & 0 & 0& 0\\ 0 & 0 & 0 & -1/T \end{array}\right] }_{A} \underbrace{ \left[\begin{array}{c} \tau_s(t)\\ \omega_1(t)\\ \omega_2(t)\\ \tau_1(t) \end{array}\right] }_{\xi(t)}\\ + \underbrace{ \left[\begin{array}{c} 0 \\ 0 \\ 0 \\ 1/T \end{array}\right] }_{B} u(t) + \underbrace{ \left[\begin{array}{c} 0\\ 0\\ 1/j_2\\ 0 \end{array}\right] }_{w}\\ \dot{\theta}_1(t)= \underbrace{ \left[\begin{array}{cccc} 0 & 1 & 0& 0 \end{array}\right] }_{C} \underbrace{ \left[\begin{array}{c} \tau_s(t)\\ \omega_1(t)\\ \omega_2(t)\\ \tau_1(t) \end{array}\right] }_{\xi(t)}\\ \end{array}} }

\displaystyle{(34)\quad \left[\begin{array}{c} 0\\ 0\\ -1/j_2\\ 0\\ \omega^* \end{array}\right] = \underbrace{ \left[\begin{array}{ccccc} 0 & k & -k & 0& 0\\ -1/j_1 & 0 & 0 & 1/j_1& 0\\ 1/j_2 & 0 & 0 & 0& 0\\ 0 & 0 & 0 & -1/T& 1/T\\ 0 & 1 & 0 & 0& 0 \end{array}\right] }_{S} \left[\begin{array}{c} \tau_s(\infty)\\ \omega_1(\infty)\\ \omega_2(\infty)\\ \tau_1(\infty)\\ u(\infty) \end{array}\right] }

\displaystyle{(35)\quad \underbrace{ \left[\begin{array}{c} \tau_s(\infty)\\ \omega_1(\infty)\\ \omega_2(\infty)\\ \tau_1(\infty)\\ u(\infty) \end{array}\right] }_{\left[\begin{array}{c} \xi_\infty \\ u_\infty \end{array}\right]} = \underbrace{ \left[\begin{array}{ccccc} 0 & 0 & j_2 & 0& 0\\ 0 & 0 & 0 & 0& 1\\ -1 & 0 & 0 & 0& 1\\ 0 & j_1 & j_2 &0& 0\\ 0 & j_1 & j_2 &T& 0 \end{array}\right] }_{S^{-1}} \left[\begin{array}{c} 0\\ 0\\ -1/j_2\\ 0\\ \omega^* \end{array}\right] = \left[\begin{array}{c} -1\\ \omega^*\\ \omega^*\\ -1\\ -1 \end{array}\right] }

\displaystyle{(36)\quad \left[\begin{array}{c} \dot{\xi}(t) \\ \omega_1(t)-\omega^* \end{array}\right] = \underbrace{ \left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right] }_{S} \left[\begin{array}{c} \xi(t)-\xi_\infty \\ u(t)-u_\infty \end{array}\right] }

\displaystyle{(37)\quad \frac{d}{dt} \left[\begin{array}{c} \xi(t)-\xi_\infty \\ u(t)-u_\infty \end{array}\right] = \underbrace{ \left[\begin{array}{cc} A & B \\ 0 & 0 \end{array}\right] }_{A_{E3}} \left[\begin{array}{c} \xi(t)-\xi_\infty \\ u(t)-u_\infty \end{array}\right] + \underbrace{ \left[\begin{array}{c} 0 \\ 1 \end{array}\right] }_{B_{E3}} \dot{u}(t) }

\displaystyle{(38)\quad \dot{u}(t) = K_{E3} \left[\begin{array}{c} \xi(t)-\xi_\infty \\ u(t)-u_\infty \end{array}\right] =\underbrace{ K_{E3}S^{-1} }_{ \left[\begin{array}{ccccc} K_2 & K_3 & K_1 & K_0& K_4 \end{array}\right]} \left[\begin{array}{c} \dot{\xi}(t) \\ \omega_1(t)-\omega^* \end{array}\right] }

(39)\quad \boxed{\begin{array}{l} \displaystyle{u(t)=-\underbrace{K_2}_{0}\tau_s(t)-\underbrace{K_3}_{K_P}\omega_1(t)-\underbrace{K_1}_{0}\omega_2(t)-\underbrace{K_0}_{0}\tau_1(t)}\\ \displaystyle{-\underbrace{K_4}_{K_I=\frac{K_P}{T_I}}\int_0^t(\omega_1(\tau)-\omega^*)d\tau} \end{array}} }

\displaystyle{(40)\quad {\rm det}(sI_5-A+BF)=s^5+\beta_1\omega s^4+\beta_2\omega^2 s^3+\beta_3\omega^3 s^2+\beta_4\omega^4 s+\beta_5\omega^5 }

\displaystyle{(41)\quad \lambda(A_{E3}-B_{E3}K_{E3})=\{(-\zeta\pm\sqrt{1-\zeta^2})\omega,(-\zeta\pm\sqrt{1-\zeta^2})\omega, \omega\} }

\displaystyle{(42)\quad \begin{array}{l} \alpha_1=(4\zeta+1)\omega\\ \alpha_2=(4\zeta^2+4\zeta+1)\omega^2\\ \alpha_3=(4\zeta^2+4\zeta+1)\omega^3\\ \alpha_4=(4\zeta+1)\omega^4\\ \alpha_5=\omega^5 \end{array} }

\displaystyle{(43)\quad \begin{array}{l} K_{E3}= \left[\begin{array}{ccccc} 0 & 0 & 0 & 0 &1 \end{array}\right]\\ \times \left[\begin{array}{ccccc} B_{E3} & A_{E3}B_{E3} & A_{E3}^2B_{E3} & A_{E3}^3B_{E3}& A_{E3}^4B_{E3} \end{array}\right]^{-1}\\ \times (A_{E3}^5+\alpha_1A_{E3}^4+\alpha_2A_{E3}^3+\alpha_3A_{E3}^2+\alpha_4A_{E3}+\alpha_5I_n) \end{array} }

\displaystyle{(44)\quad \begin{array}{l} K_2=-(T \beta_1 \omega \omega_0^2 \omega_p^2-T \beta_3 \omega^3 \omega_0^2+T \omega^5)/(\omega_0^2 \omega_p^2-\omega_0^4)\\ K_3=T \beta_2 j_1 \omega^2-T j_1 \omega_p^2\\ K_1=(T j_1 \omega_0^2 \omega_p^2-T \beta_2 j_1 \omega^2 \omega_0^2+T \beta_4 j_1 \omega^4)/\omega_0^2\\ K_0=T \beta_1 \omega-1\\ K_4=(T j_1 \omega^5)/\omega_0^2 \end{array} }

\displaystyle{(45)\quad K_0=0\Rightarrow T=\frac{1}{\beta_1 \omega} }

\displaystyle{(46)\quad \begin{array}{l} K_1=0\\ \omega_0^2 \omega_p^2-\beta_2 \omega^2 \omega_0^2+\beta_4 \omega^4=0\\ \end{array} }

\displaystyle{(47)\quad \begin{array}{l} K_2=0\\ \beta_1 \omega_0^2 \omega_p^2-\beta_3 \omega^2 \omega_0^2+ \omega^4=0\\ \end{array} }

\displaystyle{(48)\quad \begin{array}{l} (\beta_1\beta_4-1)\omega_0^2\omega_p^2-(\beta_3\beta_4-\beta_2)\omega^2 \omega_0^2=0\\ \omega^2=\frac{\beta_1\beta_4-1}{\beta_3\beta_4-\beta_2}\omega_p^2 \end{array} }

\displaystyle{(49)\quad \begin{array}{l} (\beta_3-\beta_1\beta_2)\omega^2\omega_0^2+(\beta_1 \beta_4-1)\omega^4=0\\ \omega^2=\frac{\beta_1\beta_2-\beta_3}{\beta_1 \beta_4-1}\omega_0^2 \end{array} }

\displaystyle{(50)\quad \begin{array}{l} \omega^4=\frac{\beta_1\beta_4-1}{\beta_3\beta_4-\beta_2}\frac{\beta_1\beta_2-\beta_3}{\beta_1 \beta_4-1}\omega_0^2\omega_p^2 =\frac{\beta_1\beta_2-\beta_3}{\beta_3\beta_4-\beta_2}\omega_0^2\omega_p^2\\ \end{array} }

\displaystyle{(51)\quad \begin{array}{l} \frac{\omega_p^2}{\omega_0^2}=\frac{\beta_3\beta_4-\beta_2}{\beta_1\beta_4-1}\frac{\beta_1\beta_2-\beta_3}{\beta_1 \beta_4-1} =\frac{(\beta_3\beta_4-\beta_2)(\beta_1\beta_2-\beta_3)}{(\beta_1\beta_4-1)^2} \end{array} }

\displaystyle{(52)\quad \begin{array}{l} K_3=T j_1(\beta_2 \omega^2- \omega_p^2)=\frac{1}{\beta_1 \omega}j_1(\beta_2 \frac{\beta_1\beta_4-1}{\beta_3\beta_4-\beta_2}\omega_p^2- \omega_p^2)\\ =\frac{j_1}{\beta_1 \omega}(\beta_2 \frac{\beta_1\beta_4-1}{\beta_3\beta_4-\beta_2}-1) \omega_p^2=\frac{j_1}{\beta_1 \omega}\frac{\beta_1\beta_2-\beta_3}{\beta_3\beta_4-\beta_2} \beta_4\omega_p^2\\ =\frac{j_1}{\beta_1 \omega} \frac{\omega^4}{\omega_0^2\omega_p^2}\beta_4\omega_p^2=\frac{j_1 \beta_4}{\beta_1} \frac{\omega^3}{\omega_0^2} \end{array} }

\displaystyle{(53)\quad \begin{array}{l} K_4=\frac{1}{\beta_1 \omega}\frac{j_1\omega}{\omega_0^2}\omega^4 =\frac{j_1}{\beta_1}\frac{\omega^4}{\omega_0^2} \end{array} }

\displaystyle{(54)\quad T_I=\frac{K_3}{K_4}=\frac{j_1 \beta_4}{\beta_1} \frac{\omega^3}{\omega_0^2}\frac{\beta_1}{j_1}\frac{\omega_0^2}{\omega^4} =\frac{\beta_4}{\omega} }

\displaystyle{(55)\quad \begin{array}{l} K_I=K_4T_I=\frac{j_1}{\beta_1}\frac{\omega^4}{\omega_0^2}\frac{\beta_4}{\omega} =\frac{j_1 \omega\beta_4}{\beta_1}\frac{\omega^2}{\omega_0^2}\\ =\frac{j_1 \omega\beta_4}{\beta_1 \omega_0^2}\frac{\beta_1\beta_2-\beta_3}{\beta_1\beta_4-1}\omega_0^2 =\frac{j_1 \omega\beta_4}{\beta_1}\frac{\beta_1\beta_2-\beta_3}{\beta_1\beta_4-1} \end{array} }

MAXIMA 
/*BP.wxm*/
A:matrix([0,k,-k,0],[-1/j1,0,0,1/j1],[1/j2,0,0,0],[0,0,0,-1/T]);
B:matrix([0],[0],[0],[1/T]);
C:matrix([0,1,0,0]);
D:matrix([0]);
S:addrow(addcol(A,B),addcol(C,D));
Sinv:invert(S);
xinf:Sinv.matrix([0],[0],[-1/j2],[0],[r]);
AE:addrow(addcol(A,B),[0,0,0,0,0]);
BE:matrix([0],[0],[0],[0],[1]);
V:BE;
V:addcol(BE,AE.V);
V:addcol(BE,AE.V);
V:addcol(BE,AE.V);
V:addcol(BE,AE.V);
p:s^5+b1*w*s^4+b2*w^2*s^3+b3*w^3*s^2+b4*w^4*s+w^5;
q:expand( (s^2+2*z*w*s+w^2)^2*(s+w) );
a5:coeff(p,s,0);
a4:coeff(p,s,1);
a3:coeff(p,s,2);
a2:coeff(p,s,3);
a1:coeff(p,s,4);
a0:coeff(p,s,5);
U:matrix([0,0,0,0,1]);
W0:a5*ident(5);
W1:a4*AE;
W2:a3*AE^^2;
W3:a2*AE^^3;
W4:a1*AE^^4;
W5:a0*AE^^5;
W:W5+W4+W3+W2+W1+W0;
KE:U.invert(V).W;
KS:ratsimp(KE.Sinv);
FE:KS,k=j1*(wp^2-w0^2),j2=j1*(wp^2/w0^2-1);
K2:ratsimp(FE[1][1]);
K3:ratsimp(FE[1][2]);
K1:ratsimp(FE[1][3]);
K0:ratsimp(FE[1][4]);
K4:ratsimp(FE[1][5]);
/*---*/
/*K2=-(T*b1*w*w0^2*wp^2-T*b3*w^3*w0^2+T*w^5)/(w0^2*wp^2-w0^4)*/
/*K3=T*b2*j1*w^2-T*j1*wp^2*/
/*K1=(T*j1*w0^2*wp^2-T*b2*j1*w^2*w0^2+T*b4*j1*w^4)/w0^2*/
/*K0=T*b1*w-1*/
/*K4=(T*j1*w^5)/w0^2*/
MATLAB 
%cBP_PI.m

[3]　１次フィルタ付微分動作をもつPID制御

\displaystyle{(56)\quad \begin{array}{l} \dot{x}_{D}(t)=-\frac{1}{T_D}x_{D}(t)+\frac{1}{T_D}\omega_1(t) \end{array} }

\displaystyle{(57)\quad \boxed{ \begin{array}{l} \underbrace{ \left[\begin{array}{c} \dot{\tau}_s(t)\\ \dot{\omega}_1(t)\\ \dot{\omega}_2(t)\\ \dot{x}_{D}(t)\\ \end{array}\right] }_{\dot{\xi}(t)} = \underbrace{ \left[\begin{array}{cccc} 0 & k & -k & 0\\ -1/j_1 & 0 & 0& 0\\ 1/j_2 & 0 & 0& 0\\ 0 & 1/T_D & 0 & -1/T_D \end{array}\right] }_{A} \underbrace{ \left[\begin{array}{c} \tau_s(t)\\ \omega_1(t)\\ \omega_2(t)\\ x_{D}(t)\\ \end{array}\right] }_{\xi(t)}\\ + \underbrace{ \left[\begin{array}{c} 0 \\ 1/j_1 \\ 0 \\ 0 \end{array}\right] }_{B} \tau_1(t) + \underbrace{ \left[\begin{array}{c} 0\\ 0\\ 1/j_2\\ 0 \end{array}\right] }_{w}\\ \omega_1(t)= \underbrace{ \left[\begin{array}{cccc} 0 & 1 & 0& 0 \end{array}\right] }_{C} \underbrace{ \left[\begin{array}{c} \tau_s(t)\\ \omega_1(t)\\ \omega_2(t)\\ x_{D}(t)\\ \end{array}\right] }_{\xi(t)}\\ \end{array}} }

\displaystyle{(58)\quad \left[\begin{array}{c} 0\\ 0\\ -1/j_2\\ 0\\ \omega^* \end{array}\right] = \underbrace{ \left[\begin{array}{ccccc} 0 & k & -k & 0& 0\\ -1/j_1 & 0 & 0& 0& 1/j_1 \\ 1/j_2 & 0 & 0& 0& 0\\ 0 & 1/T_D & 0 & -1/T_D& 0\\ 0 & 1 & 0 & 0& 0 \end{array}\right] }_{S} \left[\begin{array}{c} \tau_s(\infty)\\ \omega_1(\infty)\\ \omega_2(\infty)\\ x_{D}(\infty)\\ \tau_1(\infty) \end{array}\right] }

\displaystyle{(59)\quad \underbrace{ \left[\begin{array}{c} \tau_s(\infty)\\ \omega_1(\infty)\\ \omega_2(\infty)\\ x_{D}(\infty)\\ \tau_1(\infty) \end{array}\right] }_{\left[\begin{array}{c} \xi_\infty \\ \tau_\infty \end{array}\right]} = \underbrace{ \left[\begin{array}{ccccc} 0 & 0 & j_2 & 0& 0\\ 0 & 0 & 0 & 0& 1\\ -1/k & 0 & 0 & 0& 1\\ 0 & 0 & 0 &-1/T_D& 0\\ 0 & j_1 & j_2 &0& 0 \end{array}\right] }_{S^{-1}} \left[\begin{array}{c} 0\\ 0\\ -1/j_2\\ 0\\ \omega^* \end{array}\right] = \left[\begin{array}{c} -1\\ \omega^*\\ \omega^*\\ \omega^*\\ -1 \end{array}\right] }

\displaystyle{(60)\quad \left[\begin{array}{c} \dot{\xi}(t) \\ \omega_1(t)-\omega^* \end{array}\right] = \underbrace{ \left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right] }_{S} \left[\begin{array}{c} \xi(t)-\xi_\infty \\ \tau_1(t)-\tau_\infty \end{array}\right] }

\displaystyle{(61)\quad \frac{d}{dt} \left[\begin{array}{c} \xi(t)-\xi_\infty \\ \tau_1(t)-\tau_\infty \end{array}\right] = \underbrace{ \left[\begin{array}{cc} A & B \\ 0 & 0 \end{array}\right] }_{A_{E3}} \left[\begin{array}{c} \xi(t)-\xi_\infty \\ \tau_1(t)-\tau_\infty \end{array}\right] + \underbrace{ \left[\begin{array}{c} 0 \\ 1 \end{array}\right] }_{B_{E3}} \dot{u}(t) }

\displaystyle{(62)\quad \begin{array}{l} \dot{\tau}_1(t)(t) = K_{E3} \left[\begin{array}{c} \xi(t)-\xi_\infty \\ \tau_1(t)-\tau_\infty \end{array}\right]\\ =\underbrace{ K_{E3}S^{-1} }_{ \left[\begin{array}{ccccc} F_1(K_2) & F_2(K_3) & F_3(K_1) & F_4(K_0)& F_5(K_4) \end{array}\right]} \left[\begin{array}{c} \dot{\xi}(t) \\ \omega_1(t)-\omega^* \end{array}\right] \end{array} }

(63)\quad \boxed{\begin{array}{l} \displaystyle{\tau_1(t)=-\underbrace{F_1}_{0}\tau_s(t)-\underbrace{F_2}_{K_P+K_D/T}\omega_1(t)-\underbrace{F_3}_{0}\omega_2(t)-\underbrace{F_4}_{-K_D/T}x_D(t)}\\ \displaystyle{-\underbrace{F_5}_{K_I}\int_0^t(\omega_1(\tau)-\omega^*)d\tau} \end{array}} }

\displaystyle{(64)\quad {\rm det}(sI_5-A+BF)=s^5+\beta_1\omega s^4+\beta_2\omega^2 s^3+\beta_3\omega^3 s^2+\beta_4\omega^4 s+\omega^5 }

\displaystyle{(65)\quad \begin{array}{l} F_1=-((T^2 \omega_0^4+\omega_0^2) \omega_p^2+(-T^2 \beta_2 \omega^2+T \beta_1 \omega-1) \omega_0^4\\ +(T^2 \beta_4 \omega^4-T \beta_3 \omega^3) \omega_0^2+T \omega^5)/((T^2 \omega_0^4+\omega_0^2) \omega_p^2-T^2 \omega_0^6-\omega_0^4)\\ F_2=(T \beta_1 j_1 \omega-j_1)/T\\ F_3=-((T^2 \beta_1 j_1 \omega-T j_1) \omega_0^4+(T \beta_2 j_1 \omega^2-T^2 \beta_3 j_1 \omega^3) \omega_0^2\\ +T^2 j_1 \omega^5-T \beta_4 j_1 \omega^4)/(T^2 \omega_0^4+\omega_0^2)\\ F_4=-(T^5 j_1 \omega^5-T^4 \beta_4 j_1 \omega^4+T^3 \beta_3 j_1 \omega^3-T^2 \beta_2 j_1 \omega^2\\+T \beta_1 j_1 \omega-j_1)/(T^3 \omega_0^2+T)\\ F_5=(T j_1 \omega^5)/\omega_0^2\\ \end{array} }

\displaystyle{(66)\quad \begin{array}{l} F_1=0\Rightarrow\\ (T^2 \omega_0^4+\omega_0^2) \omega_p^2+(-T^2 \beta_2 \omega^2+T \beta_1 \omega-1) \omega_0^4\\ +(T^2 \beta_4 \omega^4-T \beta_3 \omega^3) \omega_0^2+T \omega^5=0\\ F_1'=-T \omega^5-T^2 \beta_4 \omega_0^2 \omega^4+T \beta_3 \omega_0^2 \omega^3+T^2 \beta_2 \omega_0^4 \omega^2-T \beta_1 \omega_0^4 \omega\\ +(-T^2 \omega_0^4-\omega_0^2) \omega_p^2+\omega_0^4=0 \end{array} }

\displaystyle{(67)\quad \begin{array}{l} \frac{1}{j_1T}F_3=0\Rightarrow\\ (T^2 \beta_1 j_1 \omega-T j_1) \omega_0^4+(T \beta_2 j_1 \omega^2-T^2 \beta_3 j_1 \omega^3) \omega_0^2\\ +T^2 j_1 \omega^5-T \beta_4 j_1 \omega^4=0\\ F_3'=-T \omega^5+\beta_4 \omega^4+T \beta_3 \omega_0^2 \omega^3-\beta_2 \omega_0^2 \omega^2-T \beta_1 \omega_0^4 \omega+\omega_0^4=0 \end{array} }

\displaystyle{(68)\quad \begin{array}{l} \frac{F_1'-F_3'}{T^2\omega_0^2+1} =-\beta_4 \omega^4+\beta_2 \omega_0^2 \omega^2-\omega_0^2 \omega_p^2=0 \end{array} }

\displaystyle{(69)\quad \begin{array}{l} F_1'=-T \omega^5-T^2 \omega_0^2(\beta_2 \omega_0^2 \omega^2-\omega_0^2 \omega_p^2)+T \beta_3 \omega_0^2 \omega^3+T^2 \beta_2 \omega_0^4 \omega^2-T \beta_1 \omega_0^4 \omega\\ +(-T^2 \omega_0^4-\omega_0^2) \omega_p^2+\omega_0^4=0\\ -T \omega^5+T \beta_3 \omega_0^2 \omega^3 -T \beta_1 \omega_0^4 \omega +\omega_0^4-\omega_0^2 \omega_p^2=0\\ \end{array} }

\displaystyle{(70)\quad \begin{array}{l} F_5'=-T \omega^5+(\beta_2 \omega_0^2 \omega^2-\omega_0^2 \omega_p^2)+T \beta_3 \omega_0^2 \omega^3-\beta_2 \omega_0^2 \omega^2-T \beta_1 \omega_0^4 \omega+\omega_0^4=0\\ -T \omega^5+T \beta_3 \omega_0^2 \omega^3-T \beta_1 \omega_0^4 \omega+\omega_0^4-\omega_0^2 \omega_p^2=0 \end{array} }

\displaystyle{(71)\quad \begin{array}{l} T=\frac{\omega_0^4-\omega_0^2 \omega_p^2}{\omega^5- \beta_3 \omega_0^2 \omega^3+\beta_1 \omega_0^4 \omega} =\frac{\beta_4(\omega_0^4-\omega_0^2 \omega_p^2)} {\omega(\beta_2\omega_0^2\omega^2-\omega_p^2\omega_0^2- \beta_3\beta_4 \omega_0^2 \omega^2+\beta_1\beta_4 \omega_0^4)}\\ =\frac{\beta_4(\omega_0^2-\omega_p^2)} {\omega((\beta_2-\beta_3\beta_4)\omega^2+\beta_1\beta_4 \omega_0^2-\omega_p^2)} \end{array} }

\displaystyle{(72)\quad \omega_p^2=\frac{(j_1+j_2)k}{j_1j_2}, \omega_0^2=\frac{k}{j_2} \Rightarrow k=j_1(w_p^2-w_0^2), j_2=j_1(w_p^2/w_0^2-1) }

\displaystyle{(73)\quad F_5=T j_1 \frac{j_2}{k}\omega^5=T j_1 \frac{\omega^5}{\omega_0^2}=K_I }

\displaystyle{(74)\quad \begin{array}{l} F_2=j_1\frac{T \beta_1 \omega-1}{T}=K_P+\frac{K_D}{T} \Rightarrow j_1(T \beta_1 \omega-1)=K_PT+K_D \end{array} }

-T \omega^5+T \beta_3 \omega_0^2 \omega^3-T \beta_1 \omega_0^4 \omega+\omega_0^4-\omega_0^2 \omega_p^2=0
\beta_3 \omega^3=\frac{\omega^5}{\omega_0^2}+\beta_1 \omega_0^2 \omega+\frac{1}{T}(\omega_p^2-\omega_0^2)

-\beta_4 \omega^4+\beta_2 \omega_0^2 \omega^2-\omega_0^2 \omega_p^2=0
\beta_2 \omega^2=\beta_4 \frac{\omega^4}{\omega_0^2}+ \omega_p^2

\displaystyle{(75)\quad \begin{array}{l} F_4=-\frac{j_1 j_2}{T^3 k+T j_2}(T^5 \omega^5-T^4 \beta_4 \omega^4+T^3 \beta_3 \omega^3-T^2 \beta_2 \omega^2 +T \beta_1 \omega - 1)=-\frac{K_D}{T}\\ K_D=\frac{j_1}{T^2\omega_0^2+1}(T^5 \omega^5-T^4 \beta_4 \omega^4+T^3 \beta_3 \omega^3-T^2 \beta_2 \omega^2 +T \beta_1 \omega - 1)\\ (T^2\omega_0^2+1)K_D=j_1(T^5 \omega^5-T^4 \beta_4 \omega^4+T^3 (\frac{\omega^5}{\omega_0^2}+\beta_1 \omega_0^2 \omega+\frac{1}{T}(\omega_p^2-\omega_0^2))\\ -T^2 (\beta_4 \frac{\omega^4}{\omega_0^2}+ \omega_p^2) +T \beta_1 \omega - 1)\\ =j_1(T^3 \omega^5 \frac{(T^2\omega_0^2+1)}{\omega_0^2} -T^2 \beta_4 \omega^4 \frac{(T^2\omega_0^2+1)}{\omega_0^2} + (\beta_1 T^3\omega_0^2 \omega-T^2\omega_0^2) +T\beta_1\omega-1)\\ =j_1(T^3 \omega^5 \frac{(T^2\omega_0^2+1)}{\omega_0^2} -T^2 \beta_4 \omega^4 \frac{(T^2\omega_0^2+1)}{\omega_0^2} +(T\beta_1\omega-1)(T^2\omega_0^2+1))\\ K_D=j_1(T^3 \omega^5 \frac{1}{\omega_0^2} -T^2 \beta_4 \omega^4 \frac{1}{\omega_0^2} +(T\beta_1\omega-1))\\ =j_1(T^3 \omega^5 \frac{1}{\omega_0^2} -T^2 \beta_4 \omega^4 \frac{1}{\omega_0^2}) +K_PT+K_D\\ 0=j_1(T^3 \omega^5 \frac{1}{\omega_0^2} -T^2 \beta_4 \omega^4 \frac{1}{\omega_0^2} +(T\beta_1\omega-1))\\ =j_1(T^3 \omega^5 \frac{1}{\omega_0^2} -T^2 \beta_4 \omega^4 \frac{1}{\omega_0^2}) +K_PT\\ K_P=-j_1(T^2 \omega^5 \frac{1}{\omega_0^2} -T \beta_4 \omega^4 \frac{1}{\omega_0^2}) =-K_IT+j_1T \beta_4 \omega^4 \frac{1}{\omega_0^2} \end{array} }

\displaystyle{(76)\quad \begin{array}{l} K_I=\frac{j_1T\omega^5}{\omega_0^2}\\ K_P=\frac{\beta_4j_1T\omega^4}{\omega_0^2}-K_IT =\frac{\beta_4j_1T\omega^4}{\omega_0^2}-\frac{j_1T^2\omega^5}{\omega_0^2} =-\frac{j_1}{\omega_0^2}(T^2\omega^5-T\beta_4\omega^4)\\ K_D=\beta_1j_1T\omega-j_1-K_PT =j_1(T\beta_1\omega-1)+\frac{j_1}{\omega_0^2}(T^3\omega^5-T^2\beta_4\omega^4) \end{array} }

MAXIMA 
/*BP.wxm*/
A:matrix([0,k,-k,0,0],[-1/j1,0,0,0,0],[1/j2,0,0,0,0],[0,1,0,0,0],[0,1/T,0,0,-1/T]);
B:matrix([0],[1/j1],[0],[0],[0]);
V:B;
V:addcol(B,A.V);
V:addcol(B,A.V);
V:addcol(B,A.V);
V:addcol(B,A.V);
p:s^5+b1*w*s^4+b2*w^2*s^3+b3*w^3*s^2+b4*w^4*s+w^5;
q:expand( (s^2+2*z*w*s+w^2)^2*(s+w) );
a5:coeff(p,s,0);
a4:coeff(p,s,1);
a3:coeff(p,s,2);
a2:coeff(p,s,3);
a1:coeff(p,s,4);
a0:coeff(p,s,5);
U:matrix([0,0,0,0,1]);
W0:a5*ident(5);
W1:a4*A;
W2:a3*A^^2;
W3:a2*A^^3;
W4:a1*A^^4;
W5:a0*A^^5;
W:W5+W4+W3+W2+W1+W0;
K:ratsimp(U.invert(V).W);
F:K,k=j1*(wp^2-w0^2),j2=j1*(wp^2/w0^2-1);
F1:ratsimp(F[1][1]);
F2:ratsimp(F[1][2]);
F3:ratsimp(F[1][3]);
F4:ratsimp(F[1][4]);
F5:ratsimp(F[1][5]);
/*---*/
/*F1=-((T^2*w0^4+w0^2)*wp^2+(-T^2*b2*w^2+T*b1*w-1)*w0^4+(T^2*b4*w^4-T*b3*w^3)*w0^2+T*w^5)/
  ((T^2*w0^4+w0^2)*wp^2-T^2*w0^6-w0^4)*/
/*F2=(T*b1*j1*w-j1)/T*/
/*F3=-((T^2*b1*j1*w-T*j1)*w0^4+(T*b2*j1*w^2-T^2*b3*j1*w^3)*w0^2+T^2*j1*w^5-T*b4*j1*w^4)/
 (T^2*w0^4+w0^2)*/
/*F4=(T*j1*w^5)/w0^2*/
/*F5=-(T^5*j1*w^5-T^4*b4*j1*w^4+T^3*b3*j1*w^3-T^2*b2*j1*w^2+T*b1*j1*w-j1)/(T^3*w0^2+T)*/
MATLAB 
%cBP_pid.m
%-----
 clear all, close all
 j1=1; w0=1; r=0.4;
 j2=r*j1; k=w0^2*j2; wp=sqrt(k*(j1+j2)/(j1*j2));
 A=[0 k -k;-1/j1 0 0;1/j2 0 0];
 B=[0;1/j1;0]; wd=[0;0;1/j2];
 C=[0 1 0];
% sys=ss(A,B,C,[]); 
% figure
% margin(sys), grid
%-----
 z=1/sqrt(2);
%-----
 b1=4*z+1; b2=4*z^2+4*z+2; b3=4*z^2+4*z+2; b4=4*z+1;
 check1=b2^2/(4*b4)-wp^2/w0^2
%ax^2+bx+c=0 -> x=(-b+-sqrt(b^2-4ac))/(2a)
 w=(b2*w0^2+sqrt(b2^2*w0^4-4*b4*w0^2*wp^2))/(2*b4)
 check2=w^2-(b1*b4*w0^2-wp^2)/(b3*b4-b2)
 TD=b4*(w0^2-wp^2)/(w*((b2-b3*b4)*w^2+b1*b4*w0^2-wp^2))
 KP=-j1/w0^2*(TD^2*w^5-TD*b4*w^4)
 KD=j1*(TD*b1*w-1)+j2/w0^2*(TD^3*w^5-TD^2*b4*w^4)
 KI=j1*TD*w^5/w0^2
%-----静止・インパルス外乱（大きさ0.2）
r=0
w=0
tau0=0
om10=0
om20=1/j2*0.2
sim("nBP_pid")
%-----静止・定値外乱（大きさ0.2）
r=0
w=0.2
tau0=0
om10=0
om20=0
sim("nBP_pid")
%-----定速回転・インパルス外乱（大きさ0.2）
r=1
w=0
tau0=0
om10=0
om20=1/j2*0.2
sim("nBP_pid")
%-----定速回転・定値外乱（大きさ0.2）
r=1
w=0.2
tau0=0
om10=0
om20=0
sim("nBP_pid")
%-----
%eof

LQGI制御

投稿日時: 投稿者:

LQGI制御…Homework

[1]　可制御かつ可観測なn次系を考えます。

\displaystyle{(1)\quad \dot{x}(t)=Ax(t)+Bu(t)+w\quad(x(t),w\in{\bf R}^n,u(t)\in{\bf R}^m) }

\displaystyle{(2)\quad y(t)=C_Mx(t)\quad(y(t)\in{\bf R}^p) }

ここで、状態方程式には、操作入力u(t)のほかに、定値外乱wが加わっていること、出力方程式における行列CをC_Mと書いたことに注意します。いま、出力変数の一部やそれらの組合せからなる新しいm個の被制御変数を

\displaystyle{(3)\quad z(t)=C_Sy(t)=\underbrace{C_SC_M}_{C}x(t)\quad(z(t)\in{\bf R}^m) }

のように選び((A,C)は可観測対)、定値外乱があるにも拘わらず、制御目的

\displaystyle{(4)\quad z(t)\ \rightarrow\ r \quad (t\rightarrow\infty) }

を達成したいとします。ここで、定数ベクトルrはm個の設定値からなる。もし制御目的(4)が物理的に可能とすると、ある状態x_\inftyと入力u_\inftyが確定し

\displaystyle{(5)\quad \left[\begin{array}{c} -w \\ r \end{array}\right] = \left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right] \left[\begin{array}{c} x_\infty \\ u_\infty \end{array}\right] }

の関係を満足しているはずです。したがって、どのようなwとrに対しても、x_\inftyとu_\inftyが定まるように、被制御変数(3)を

\displaystyle{(6)\quad {\rm rank}\, \underbrace{ \left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right] }_{S}=n+m }

が成り立つように選ぶものとします。

●さて、制御目的(4)を達成するために、つぎの積分動作を加えた状態フィードバックを考えました。

\displaystyle{(7)\quad u(t)=-Fx(t)-F_Ix_I(t)}

ただし

\displaystyle{(8)\quad \dot{x}_I(t)=z(t)-r \qquad (x_I(0)=0) }

実際には、式(7)の代わりに、状態オブザーバ

\displaystyle{(9)\quad \dot{\hat{x}}(t)=(A-HC_M)\hat{x}(t)+Hy(t)+Bu(t) }

の出力を用いて

\displaystyle{(10)\quad u(t)=-F\hat{x}(t)-F_Ix_I(t) }

を実施することになります。ここでの積分動作を加えたオブザーバベース コントローラは、(10)を(9)に代入した

\displaystyle{(11)\quad \dot{\hat{x}}(t)=(A-HC_M-BF)\hat{x}(t)-BF_Ix_I(t)+Hy(t) }

と、(8)を合わせて、つぎのように表されます。

\displaystyle{(12)\quad \boxed{\begin{array}{rl} \left[\begin{array}{c} \dot{\hat{x}}(t) \\ \dot{x}_I(t) \end{array}\right] =& \underbrace{ \left[\begin{array}{cc} A-HC_M-BF & -BF_I \\ 0 & 0 \end{array}\right] }_{A_K} \left[\begin{array}{c} \hat{x}(t) \\ x_I(t) \end{array}\right]\\[10mm] &+ \underbrace{ \left[\begin{array}{cc} H & 0\\ C_S & -I_m \end{array}\right] }_{B_K} \left[\begin{array}{c} y(t) \\ r \end{array}\right] \end{array}} }

\displaystyle{(13)\quad \boxed{u(t)= \underbrace{- \left[\begin{array}{cc} F & F_I \end{array}\right] }_{C_K} \left[\begin{array}{c} \hat{x}(t) \\ x_I(t) \end{array}\right]} }

これによる閉ループ系は、(10)を(1)に代入した

\displaystyle{(14)\quad \dot{x}(t)=Ax(t)-BF\hat{x}(t)-BF_Ix_I(t)+w(t) }

と、(8)、(3)を合わせて

\displaystyle{(15)\quad \boxed{\left[\begin{array}{c} \dot{x}(t) \\ \dot{x}_I(t) \\ \dot{\hat{x}}(t) \end{array}\right] = \left[\begin{array}{ccc} A & -BF_I & -BF \\ C & 0 & 0 \\ HC_M & -BF_I & A-HC_M-BF \end{array}\right] \left[\begin{array}{c} x(t) \\ x_I(t) \\ \hat{x}(t) \end{array}\right] + \left[\begin{array}{c} w \\ -r \\ 0 \end{array}\right]} }

のように表されます。そのブロック線図を図1に示します。

図1 積分動作を加えたオブザーバベースコントローラによる閉ループ系

●いま、閉ループ系(15)に、座標変換

\displaystyle{(16)\quad \left[\begin{array}{c} x(t) \\ x_I(t) \\ e(t) \end{array}\right] = \left[\begin{array}{ccc} I_n & 0 & 0 \\ 0 & I_m & 0 \\ -I_n & 0 & I_n \end{array}\right] \left[\begin{array}{c} x(t) \\ x_I(t) \\ \hat{x}(t) \end{array}\right] }

を行えば

\displaystyle{(17)\quad \boxed{\left[\begin{array}{c} \dot{x}(t) \\ \dot{x}_I(t) \\\hline \dot{e}(t) \end{array}\right] = \underbrace{ \left[\begin{array}{cc|c} A-BF & -BF_I & -BF \\ C & 0 & 0 \\\hline 0 & 0 & A-HC_M \end{array}\right] }_{ A_{EF}'= \left[\begin{array}{c|c} A_{EF} & - \left[\begin{array}{cc} B \\ 0 \end{array}\right] F \\[5mm] \hline 0 & \widehat{A} \end{array}\right] } \left[\begin{array}{c} x(t) \\ x_I(t) \\\hline e(t) \end{array}\right] + \left[\begin{array}{c} w \\ -r \\\hline -w \end{array}\right]} }

となり、閉ループ系の固有値は、積分動作を加えた状態フィードバックだけの閉ループ系の固有値と状態オブザーバの固有値からなります。

ここで、A_{EF}は安定行列であるとします。このとき、(17)より

\displaystyle{(18)\quad \left[\begin{array}{c} x(t) \\ x_I(t) \\\hline e(t) \end{array}\right] \ \rightarrow\ \underbrace{ \left[\begin{array}{c|c} A_{EF}^{-1} & -A_{EF}^{-1} \left[\begin{array}{cc} B \\ 0 \end{array}\right] F\widehat{A}^{-1} \\[5mm]\hline 0 & \widehat{A}^{-1} \end{array}\right] }_{A_{EF}'\,^{-1}} \left[\begin{array}{c} -w \\ r \\\hline w \end{array}\right] \quad (t\rightarrow\infty) }

ここで

\displaystyle{(19)\quad S^{-1} \left[\begin{array}{cc} B \\ 0 \end{array}\right] = \left[\begin{array}{cc} 0 \\ I_m \end{array}\right] }

\displaystyle{(20)\quad A_{EF}^{-1} = \left[\begin{array}{cc} I_n & 0 \\ -F_I^{-1}F & -F_I^{-1} \end{array}\right] S^{-1} }

を用いて

\displaystyle{(21)\quad x(t)\ \rightarrow\ x_\infty \quad (t\rightarrow\infty) }

\displaystyle{(22)\quad -F_Ix_{I}(t)\ \rightarrow\ F(x_\infty+e_\infty)+u_\infty \quad (t\rightarrow\infty) }

\displaystyle{(23)\quad e(t)\ \rightarrow\ e_\infty=\widehat{A}^{-1}w \quad (t\rightarrow\infty) }

を得ます。したがって、定値外乱が存在するときは状態オブザーバに関して定常偏差が残るにもかかわらず、制御目的(4)が成り立つことがわかります。

[2]　以下に、偏差系E3をLQG制御により安定化して、積分動作を加えたオブザーバベースコントローラを構成する手順を示します。

アルゴリズム　＜LQGI制御＞

ステップ1　被制御変数の決定

\left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right]が正則となるように(C=C_SC_M)、セレクタ行列C_S(m\times p)を決めます。

ステップ2　偏差系の安定化

偏差系

\displaystyle{(24)\quad \frac{d}{dt} %\underbrace{ \left[\begin{array}{c} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] %}_{{\tilde x}_E(t)-{\tilde x}_{E\infty}} = \underbrace{ \left[\begin{array}{cc} A & B \\ 0 & 0 \end{array}\right] }_{A_{E}} %\underbrace{ \left[\begin{array}{c} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] %}_{{\tilde x}_E(t)-{\tilde x}_{E\infty}} + \underbrace{ \left[\begin{array}{c} 0 \\ I_m \end{array}\right] }_{B_{E}} {\dot u}(t) }

を、状態フィードバック

\displaystyle{(25)\quad {\dot u}(t)=- \left[\begin{array}{cc} K & K_I \end{array}\right] \left[\begin{array}{c} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] }

によるLQ制御で安定化します。その際、評価関数としては

\displaystyle{(26)\quad \int_0^\infty \left( \left[\begin{array}{c} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right]^T Q_E \left[\begin{array}{c} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] +{\dot u}^T(t)R_E{\dot u}(t)\right)\,dt }

を用いる。ただし、(A_E,Q_E^{\frac{1}{2}})は可観測対とします。

さらに、FとF_Iを、次式から計算します。

\displaystyle{(27)\quad \left[\begin{array}{cc} F & F_I \end{array}\right] = \left[\begin{array}{cc} K & K_I \end{array}\right] \left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right]^{-1} }

ステップ3　オブザーバゲインHの決定

行列B'を、(A,B')が可制御対となるように選び、重み行列W>0とV>0を指定し

\displaystyle{(28)\quad \Gamma A^T+A\Gamma-\Gamma C_M^TV^{-1}C_M\Gamma+B'WB'^T=0 }

を解いて、\Gamma>0を求め、オブザーバゲインHをつぎのように定めます。

\displaystyle{(29)\quad H=V^{-1}C_M\Gamma }

ステップ4　LQGIコントローラの構成

C_S、F、F_I、Hから、つぎを構成します。

\displaystyle{(30)\quad \dot{x}_K(t)=A_Kx_K(t)+B_K \left[\begin{array}{c} y(t) \\ r \end{array}\right] }

\displaystyle{(31)\quad u(t)=C_Kx_K(t) }

ただし

\displaystyle{(32)\quad A_K= \left[\begin{array}{cc} A-HC_M-BF & -BF_I \\ 0\,(m\times n) & 0\,(m\times m) \end{array}\right] }

\displaystyle{(33)\quad B_K= \left[\begin{array}{cc} H & 0\,(n\times m)\\ C_S & -I_m \end{array}\right] }

\displaystyle{(34)\quad C_K=- \left[\begin{array}{cc} F & F_I \end{array}\right] }

この手順で設計された積分動作を加えた状態フィードバックによる制御方式をLQGI制御(LQG control with integral action)と呼びます。

LQI制御の応用

投稿日時: 投稿者:

LQI制御の応用…Homework

[1]　倒立振子は制御なしには平衡状態を保てないので、制御技術の有効性を示すのに、よく研究されています。次図は様々な倒立振子を示しています。

図1 様々な倒立振子

これらの倒立振子について、非線形シミュレータを開発し、平衡状態まわりの挙動を表す線形状態方程式を得ていました。これらに基づいて、以下では、LQ制御系を設計します。

[2] CIP

平衡状態\theta^*=0周りの挙動を表す線形状態方程式は次式で与えられます。

\displaystyle{(2.1)\quad \frac{d}{dt}\left[\begin{array}{c} r(t)\\ \theta(t)\\ \dot{r}(t)\\ \dot{\theta}(t) \end{array}\right] = \underbrace{ \left[\begin{array}{cccc} 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1\\ 0 & -\frac{3gm}{4M+m} & 0 & 0\\ 0 & \frac{3(M+m)g}{(4M+m)\ell} & 0 & 0\\ \end{array}\right] }_{A} \underbrace{ \left[\begin{array}{c} r(t)\\ \theta(t)\\ \dot{r}(t)\\ \dot{\theta}(t) \end{array}\right] }_{x(t)} + \underbrace{ \left[\begin{array}{c} 0\\ 0\\ \frac{4}{4M+m}\\ \frac{3}{(4M+m)\ell} \end{array}\right] }_{B} \underbrace{F(t)}_{u(t)} }

これに対して、倒立位置をr_cに変更可能とする、積分動作をもつ状態フィードバック

\displaystyle{(2.2)\quad u(t)= \underbrace{- \left[\begin{array}{cccc} f_1 & f_2 & f_3 & f_4 \end{array}\right] }_{-F} \underbrace{ \left[\begin{array}{c} r(t)\\ \theta(t)\\ \dot{r}(t)\\ \dot{\theta}(t) \end{array}\right] }_{x(t)} +F_I\int_0^t (r(t)-r_c)dt }

を、LQI制御のアルゴリズムに沿って設計します。

ステップ1　被制御変数の決定

制御目的は倒立位置をr_cに変更し、安定化することですから

\displaystyle{(2.3)\quad \underbrace{r(t)}_{z(t)}= \underbrace{ \left[\begin{array}{cccc} 1 & 0 & 0 & 0 \end{array}\right] }_{C} \underbrace{ \left[\begin{array}{c} r(t)\\ \theta(t)\\ \dot{r}(t)\\ \dot{\theta}(t) \end{array}\right] }_{x(t)} }

のように選びます。このとき、明らかに次式が成り立ちます。

\displaystyle{(2.4)\quad \underbrace{\left[\begin{array}{c} 0\\ 0\\ 0\\ 0\\\hline r_c \end{array}\right] }_{\left[\begin{array}{c} -w \\ r_c \end{array}\right]} = \underbrace{\left[\begin{array}{cccc|c} 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 1 & 0\\ 0 & -\frac{3gm}{4M+m} & 0 & 0 & \frac{4}{4M+m}\\ 0 & \frac{3(M+m)g}{(4M+m)\ell} & 0 & 0 & \frac{3}{(4M+m)\ell}\\\hline 1 & 0 & 0 & 0 & 0 \end{array}\right] }_{S=\left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right]} \underbrace{\left[\begin{array}{c} r_c\\ 0\\ 0\\ 0\\\hline 0 \end{array}\right] }_{\left[\begin{array}{c} x_\infty \\ u_\infty \end{array}\right]} }

ここで、Sは正則であることから、倒立位置を変えたときの状態x_\inftyと入力u_\inftyが一意に定まります。

ステップ2　偏差系の安定化

偏差系

\displaystyle{(2.5)\quad \frac{d}{dt} %\underbrace{ \left[\begin{array}{c} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] %}_{{\tilde x}_E(t)-{\tilde x}_{E\infty}} = \underbrace{ \left[\begin{array}{cc} A & B \\ 0 & 0 \end{array}\right] }_{A_{E}} %\underbrace{ \left[\begin{array}{c} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] %}_{{\tilde x}_E(t)-{\tilde x}_{E\infty}} + \underbrace{ \left[\begin{array}{c} 0 \\ 1 \end{array}\right] }_{B_{E}} {\dot u}(t) }

を、状態フィードバック

\displaystyle{(2.6)\quad {\dot u}(t)=- \left[\begin{array}{cc} K & K_I \end{array}\right] \left[\begin{array}{c} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] }

によるLQ制御で安定化します。その際、評価関数としてはたとえば

(2.7)\quad \begin{array}{l} \displaystyle{J=\int_0^\infty (\underbrace{\frac{1}{M_r^2}}_{q_1^2}(r(t)-r_c)^2+\underbrace{\frac{1}{M_\theta^2}}_{q_2^2}\theta^2(t) }\\ \displaystyle{+\underbrace{\frac{1}{M_F^2}}_{q_5^2}u^2(t)+\rho^2\underbrace{\frac{T_F^2}{M_F^2}}_{R}\dot{u}^2(t))\,dt}\\ \end{array} }

\displaystyle{(2.8)\quad |r(t)|<M_r,\ |\theta(t)|<M_\theta,\ |F(t)|<M_F,\ |\dot{F}(t)|<\frac{M_F}{T_F} }

を選び、これを最小にするような(2.6)を設計します。

ステップ3　積分動作を加えた状態フィードバックの構成

次式から積分動作を加えた状態フィードバック(2.2)を構成します。

\displaystyle{(2.9)\quad \left[\begin{array}{cc} F & F_I \end{array}\right] = \left[\begin{array}{cc} K & K_I \end{array}\right] \left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right]^{-1} }

MATLAB 
%cCIP_lqi.m
%-----
 clear all, close all
 global mc m ell g th0
 mc=1; m=0.1; ell=0.2; g=9.8;
 th0=0/180*pi %input('th(0) = <0,180> ')/180*pi;
%-----
 A=[zeros(2,2) eye(2);zeros(2,4)];
 A(3,1)=0; 
 A(3,2)=-3*m*g/(m+4*mc); 
 A(4,1)=0; 
 A(4,2)=3*(m+mc)*g/((m+4*mc)*ell);  
 B=zeros(4,1);
 B(3)=4/(m+4*mc);
 B(4)=-3/((m+4*mc)*ell); 
%-----
 Tcart=1; Mr=0.5;
 Tpend=1/4*2*pi*sqrt(4/3*ell/g); Mth=3/180*pi;
 Tamp=0.1; Mamp=1; 
 CM=eye(2,4); CS=[1 0]; C=CS*CM; 
 n=size(A,1); m=size(C,1); r=size(B,2);
%-----
 AE=[A B;zeros(m,n+m)];
 BE=[zeros(n,m);eye(m)];
 CE=eye(n+m);
 QE=diag([1/Mr 1/Mth Tcart/Mr*0 Tpend/Mth*0 1/Mamp].^2);
 RE=(Tamp/Mamp)^2;
 [KE,pcl]=opt(AE,BE,CE,QE,RE);
 FE=KE/[A B;C zeros(m)];
 pcl, F=FE(:,1:n), FI=FE(:,n+1:n+m)
%-----
 CE=diag([100,180/pi])*eye(2,5);
 sysE=ss(AE-BE*KE,[],[CE;-KE],[]);
 t=0:0.05:5;
 rc=0.5
 xE0=[-rc;0;0;0;0];
 initial(sysE,xE0,t), grid
%-----
 xs=[0;0;0;0];
 us=0;
 sim("nCIP_lqi")
%-----

[3] CIP2

平衡状態\theta^*=0と平衡入力F^*=(M+m)g\sin\alpha周りの挙動を表す線形状態方程式は次式で与えられます。

(3.1)\quad \begin{array}{l} \displaystyle{\frac{d}{dt}\left[\begin{array}{c} r(t)\\ \theta(t)\\ \dot{r}(t)\\ \dot{\theta}(t) \end{array}\right] = \underbrace{\left[\begin{array}{cccc} 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1\\ 0 & -\frac{6\cos\alpha mg}{8M+(5-3\cos2\alpha)m} & 0 & 0\\ 0 & \frac{6(M+m)g}{(8M+(5-3\cos2\alpha)m)\ell} & 0 & 0\\ \end{array}\right] }_{A(\alpha)} \underbrace{ \left[\begin{array}{c} r(t)\\ \theta(t)\\ \dot{r}(t)\\ \dot{\theta}(t) \end{array}\right] }_{x(t)}}\\ \displaystyle{+ \underbrace{\left[\begin{array}{c} 0\\ 0\\ \frac{8}{8M+(5-3\cos2\alpha)m}\\ \frac{6\cos\alpha}{(8M+(5-3\cos2\alpha)m)\ell} \end{array}\right] }_{B(\alpha)} \underbrace{(F(t)-F^*)}_{u(t)}} \end{array}

これに対して、倒立位置をr_cに変更可能とする、積分動作をもつ状態フィードバック

\displaystyle{(3.2)\quad u(t)= \underbrace{- \left[\begin{array}{cccc} f_1 & f_2 & f_3 & f_4 \end{array}\right] }_{-F} \underbrace{ \left[\begin{array}{c} r(t)\\ \theta(t)\\ \dot{r}(t)\\ \dot{\theta}(t) \end{array}\right] }_{x(t)} +F_I\int_0^t (r(t)-r_c)dt }

を、LQI制御のアルゴリズムに沿って設計します。

●レールの傾斜角\alphaは未知とするのが実際的です。そこで

\displaystyle{(3.3)\quad \begin{array}{l} \dot{x}(t)=A(\alpha)x(t)+B(\alpha)(F(t)-F^*)\\ =A(0)x(t)+B(0)(F(t)-F^*)\\ +\underbrace{(A(\alpha)-A(0))}_{\Delta A}x(t)+\underbrace{(B(\alpha)-B(0))}_{\Delta B}(F(t)-F^*)\\ =A(0)x(t)+B(0)F(t)+\underbrace{\Delta Ax(t)+\Delta BF(t)-B(\alpha)F^*}_{w} \end{array} }

として、レールの傾斜がないときの(2.1)に外乱wが加わっていると考えます。もちろんwは定値ではないのですが、どれくらいの効果があるかシミュレーションで確かめてみます。

MATLAB 
%cCIP2_lqi.m
%-----
 clear all, close all
 global mc m ell g alpha th0
 mc=1; m=0.1; ell=0.2; g=9.8;
 alpha=3/180*pi;
 th0=3/180*pi %input('ths   = <0,180> ')/180*pi;
%-----
 A=[zeros(2,2) eye(2);zeros(2,4)];
 A(3,1)=0; 
 A(3,2)=-6*cos(alpha)*m*g/(8*mc+(5-3*cos(2*alpha))*m); 
 A(4,1)=0; 
 A(4,2)=6*(m+mc)*g/((8*mc+(5-3*cos(2*alpha))*m)*ell);  
 B=zeros(4,1);
 B(3)=8/(8*mc+(5-3*cos(2*alpha))*m);
 B(4)=-6*cos(alpha)/((8*mc+(5-3*cos(2*alpha))*m)*ell); 
%----- alpha=0の設計モデル
 A=[zeros(2,2) eye(2);zeros(2,4)];
 A(3,1)=0; 
 A(3,2)=-3*m*g/(m+4*mc); 
 A(4,1)=0; 
 A(4,2)=3*(m+mc)*g/((m+4*mc)*ell);  
 B=zeros(4,1);
 B(3)=4/(m+4*mc);
 B(4)=-3/((m+4*mc)*ell); 
%-----
 Tcart=1; Mr=0.5;
 Tpend=1/4*2*pi*sqrt(4/3*ell/g); Mth=3/180*pi;
 Tamp=0.1; Mamp=1; 
 CM=eye(2,4); CS=[1 0]; C=CS*CM; 
 n=size(A,1); m=size(C,1); r=size(B,2);
%-----
 AE=[A B;zeros(m,n+m)];
 BE=[zeros(n,m);eye(m)];
 CE=eye(n+m);
 QE=diag([1/Mr 1/Mth Tcart/Mr*0 Tpend/Mth*0 1/Mamp].^2);
 RE=(Tamp/Mamp)^2;
 [KE,pcl]=opt(AE,BE,CE,QE,RE);
 FE=KE/[A B;C zeros(m)];
 pcl, F=FE(:,1:n), FI=FE(:,n+1:n+m)
%-----
 CE=diag([100,180/pi])*eye(2,5);
 sysE=ss(AE-BE*KE,[],[CE;-KE],[]);
 t=0:0.05:5;
 rc=0.5
 xE0=[-rc;0;0;0;0];
 initial(sysE,xE0,t), grid
%-----
 xs=[0;0;0;0];
 us=(mc+m)*g*sin(alpha)*0;
 sim("nCIP2_lqi")
%-----
%eof

[4] AIP

平衡状態\theta_1^*=\alpha,\ \theta_2^*=0と平衡入力\tau^*=-(m_1+2m_2)\ell_1g\sin\alpha周りの挙動を表す線形状態方程式は次式で与えられます。

(4.1)\quad \begin{array}{l} \displaystyle{\frac{d}{dt}\left[\begin{array}{c} \theta_1(t)\\ \theta_2(t)\\ \dot{\theta}_1(t)\\ \dot{\theta}_2(t) \end{array}\right] =}\\ \displaystyle{\underbrace{(\left[\begin{array}{cccc} 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1\\ \frac{3(m_1+2m_2)g}{(4m_1+3m_2)\ell_1} & -\frac{9m_2g}{2(4m_1+3m_2)\ell_1} & 0 & 0\\ -\frac{9(m_1+2m_2)g}{2(4m_1+3m_2)\ell_1} & \frac{9m_2g}{(4m_1+3m_2)\ell_2} & 0 & 0\\ \end{array}\right]+\Delta A) }_{A(\alpha)=A+\Delta A} \underbrace{\left[\begin{array}{c} \theta_1(t)\\ \theta_2(t)\\ \dot{\theta}_1(t)\\ \dot{\theta}_2(t) \end{array}\right] }_{x(t)}\\ \displaystyle{+ \underbrace{(\left[\begin{array}{c} 0\\ 0\\ \frac{6}{2(4m_1+3m_2)\ell_1^2}\\ -\frac{9}{2(4m_1+3m_2)\ell_1\ell_2} \end{array}\right]+\Delta B) }_{B(\alpha)=B+\Delta B} \underbrace{(\tau(t)-\tau^*)}_{u(t)}}\\ =Ax(t)+B\tau(t)+\underbrace{\Delta Ax(t)+\Delta B\tau(t)-B(\alpha)\tau^*}_{w} \end{array}

これに対して、アームの角度を\alphaに変更可能とする、積分動作をもつ状態フィードバック

\displaystyle{(4.2)\quad \tau(t)= \underbrace{- \left[\begin{array}{cccc} f_1 & f_2 & f_3 & f_4 \end{array}\right] }_{-F} \underbrace{ \left[\begin{array}{c} \theta_1(t)\\ \theta_2(t)\\ \dot{\theta}_1(t)\\ \dot{\theta}_2(t) \end{array}\right] }_{x(t)} +F_I\int_0^t (\theta_1(t)-\alpha)dt }

を、LQI制御のアルゴリズムに沿って設計します。

MATLAB 
%cAIP_lq.m
%-----
 clear all, close all
 global m1 m2 ell1 ell2  g th10 th20
 m1=0.1; m2=0.2; ell1=0.1; ell2=0.2; g=9.8;
 th10=0/180*pi %input('th1(0) = <0,180> ')/180*pi;
 th20=0/180*pi %input('th2(0) = <0,180> ')/180*pi;  
%----- th10=0 における線形モデル
 A=[zeros(2,2) eye(2);zeros(2,4)];
 A(3,1)=3*(2*m2+m1)*g/((3*m2+4*m1)*ell1); 
 A(3,2)=-9*m2*g/(2*(3*m2+4*m1)*ell1); 
 A(4,1)=-9*(2*m2+m1)*g/(2*(3*m2+4*m1)*ell2); 
 A(4,2)=3*(3*m2+m1)*g/((3*m2+4*m1)*ell2);  
 B=zeros(4,1);
 B(3)=3/((3*m2+4*m1)*ell1^2);
 B(4)=-9/(2*(3*m2+4*m1)*ell1*ell2);
%-----
 Tpend1=1/4*2*pi*sqrt(4/3*ell1/g); Mth1=3/180*pi;
 Tpend2=1/4*2*pi*sqrt(4/3*ell2/g); Mth2=3/180*pi;
 Tamp=0.1; Mamp=1; 
 CM=eye(2,4); CS=[1 0]; C=CS*CM; 
 n=size(A,1); m=size(C,1); r=size(B,2);
%-----
 AE=[A B;zeros(m,n+m)];
 BE=[zeros(n,m);eye(m)];
 CE=eye(n+m);
 QE=diag([1/Mth1 1/Mth2 Tpend1/Mth1*0 Tpend2/Mth2*0 1/Mamp].^2);
 RE=(Tamp/Mamp)^2;
 [KE,pcl]=opt(AE,BE,CE,QE,RE);
 FE=KE/[A B;C zeros(m)];
 pcl, F=FE(:,1:n), FI=FE(:,n+1:n+m)
%-----
 CE=diag([180/pi,180/pi])*eye(2,5);
 sysE=ss(AE-BE*KE,[],[CE;-KE],[]);
 t=0:0.01:1;
 th1c=10/180*pi
 xE0=[-th10;-th20;0;0;0];
 initial(sysE,xE0,t), grid
%-----
 xs=[0;0;0;0];
 us=-(m1+2*m2)*ell1*g*sin(th1c)
 sim("nAIP_lqi")
%-----
%eof

[5] PIP

平衡状態\theta_1^*=0,\ \theta_2^*=0周りの挙動を表す線形状態方程式は次式で与えられます。

(5.1)\quad \begin{array}{l} \displaystyle{\frac{d}{dt}\left[\begin{array}{c} r(t)\\ \theta_1(t)\\ \theta_2(t)\\ \dot{r}(t)\\ \dot{\theta}_1(t)\\ \dot{\theta}_2(t) \end{array}\right] =}\\ \displaystyle{\underbrace{\left[\begin{array}{cccccc} 0 & 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 0 & 0 & 1\\ 0 & -\frac{3m_1g}{4M+m_1+m_2} & -\frac{3m_2g}{4M+m_1+m_2} & 0 & 0 & 0\\ 0 & \frac{3(4m+4m_1+m_2)g}{4(4M+m_1+m_2)\ell_1} & \frac{9m_2g}{4(4M+m_1+m_2)\ell_1} & 0 & 0& 0\\ 0 & \frac{9gm_1}{4(4M+m_1+m_2)\ell_2} & \frac{3(4m+m_1+4m_2)g}{4(4M+m_1+m_2)\ell_2} & 0 & 0& 0\\ \end{array}\right]}_{A} \left[\begin{array}{c} r(t)\\ \theta_1(t)\\ \theta_2(t)\\ \dot{r}(t)\\ \dot{\theta}_1(t)\\ \dot{\theta}_2(t) \end{array}\right]}\\ \displaystyle{+ \underbrace{ \left[\begin{array}{c} 0\\ 0\\ 0\\ \frac{4}{4M+m_1+m_2}\\ -\frac{3}{(4M+m_1+m_2)\ell_1}\\ -\frac{3}{(4M+m_1+m_2)\ell_2} \end{array}\right] }_{B} \underbrace{F(t)}_{u(t)} \end{array}

これに対して、倒立位置をr_cに変更可能とする、積分動作をもつ状態フィードバック

\displaystyle{(5.2)\quad u(t)= \underbrace{- \left[\begin{array}{cccccc} f_1 & f_2 & f_3 & f_4 & f_5 & f_6 \end{array}\right] }_{-F} \underbrace{ \left[\begin{array}{c} r(t)\\ \theta_1(t)\\ \theta_2(t)\\ \dot{r}(t)\\ \dot{\theta}_1(t)\\ \dot{\theta}_2(t) \end{array}\right] }_{x(t)} +F_I\int_0^t (r(t)-r_c)dt }

を、LQI制御のアルゴリズムに沿って設計します。

MATLAB 
%cPIP_lqi.m
%-----
 clear all, close all
 global mc m1 m2 ell1 ell2 g th10 th20
 mc=1; m1=0.1; m2=0.2; ell1=0.15; ell2=0.2; g=9.8; 
 th10=0/180*pi %input('th1(0) = <0,180> ')/180*pi;
 th20=-1/180*pi %input('th2(0) = <0,180> ')/180*pi;  
%-----
 A=[zeros(3,3) eye(3);zeros(3,6)];
 A(4,1)=0; 
 A(4,2)=-3*m1*g/(m1+m2+4*mc); 
 A(4,3)=-3*m2*g/(m1+m2+4*mc);  
 A(5,1)=0; 
 A(5,2)=(12*m1+3*m2+12*mc)*g/((4*m1+4*m2+16*mc)*ell1);  
 A(5,3)=9*m2*g/((4*m1+4*m2+16*mc)*ell1);   
 A(6,1)=0; 
 A(6,2)=9*m1*g/((4*m1+4*m2+16*mc)*ell2);  
 A(6,3)=(3*m1+12*m2+12*mc)*g/((4*m1+4*m2+16*mc)*ell2);   
 B=zeros(6,1);
 B(4)=4/(m1+m2+4*mc);
 B(5)=-3/((m1+m2+4*mc)*ell1);
 B(6)=-3/((m1+m2+4*mc)*ell2); 
%-----
 Tcart=1; Mr=0.5;
 Tpend1=1/4*2*pi*sqrt(4/3*ell1/g); Mth1=3/180*pi;
 Tpend2=1/4*2*pi*sqrt(4/3*ell2/g); Mth2=3/180*pi;
 Tamp=0.1; Mamp=1; 
 CM=eye(3,6); CS=[1 0 0]; C=CS*CM; 
 n=size(A,1); m=size(C,1); r=size(B,2);
%-----
 AE=[A B;zeros(m,n+m)];
 BE=[zeros(n,m);eye(m)];
 CE=eye(n+m);
 QE=diag([1/Mr 1/Mth1 1/Mth2 Tcart/Mr*0 Tpend1/Mth1*0 Tpend2/Mth2*0 1/Mamp].^2);
 RE=(Tamp/Mamp)^2;
 [KE,pcl]=opt(AE,BE,CE,QE,RE);
 FE=KE/[A B;C zeros(m)];
 pcl, F=FE(:,1:n), FI=FE(:,n+1:n+m)
%-----
 CE=diag([100,180/pi,180/pi])*eye(3,7);
 sysE=ss(AE-BE*KE,[],[CE;-KE],[]);
 t=0:0.05:5;
 rc=0.5
 xE0=[-rc;0;0;0;0;0;0];
 initial(sysE,xE0,t), grid
%-----
 xs=[0;0;0;0;0;0];
 us=0;
 sim("nPIP_lqi")   
%-----
%eof

[6] DIP

平衡状態\theta_1^*=0,\ \theta_2^*=0周りの挙動を表す線形状態方程式は次式で与えられます。

(6.1)\quad \begin{array}{l} \displaystyle{ \frac{d}{dt}\left[\begin{array}{c} r(t)\\ \theta_1(t)\\ \theta_2(t)\\ \dot{r}(t)\\ \dot{\theta}_1(t)\\ \dot{\theta}_2(t) \end{array}\right] =}\\ \displaystyle{ \left[\begin{array}{cccccc} 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & -\frac{3m_1g}{Mm_1+m_1^2+(3M+m1)m_2} & -\frac{3m_2g}{Mm_1+m_1^2+(3M+m1)m_2} \\ 0 & \frac{3(4Mm_1+4m_1^2+3m_2^2+3(18M+13m_1)m_2)g}{(4Mm_1+m_1^2+(3M+m1)m_2)\ell_1} & \frac{9(2M+m_1)m_2g}{(4Mm_1+m_1^2+(3M+m1)m_2)\ell_1} \\ 0 & \frac{9(2Mm_1+m_1^2+3(2M+m_1)m_2)g}{(4Mm_1+m_1^2+(3M+m_1)m_2)\ell_2} & \frac{3(4Mm_1+m_1^2+12(3M+m_1)m_2)g}{(4Mm_1+m_1^2+(3M+m_1)m_2)\ell_2} \\ \end{array}\right.}\\ \displaystyle{\left.\begin{array}{cccccc} 1 & 0 & 0\\ 0 & 1 & 0\\ 0 & 0 & 1\\ 0 & 0 & 0\\ 0 & 0 & 0\\ 0 & 0 & 0\\ \end{array}\right] \left[\begin{array}{c} r(t)\\ \theta_1(t)\\ \theta_2(t)\\ \dot{r}(t)\\ \dot{\theta}_1(t)\\ \dot{\theta}_2(t) \end{array}\right]}\\ \displaystyle{+ \underbrace{\left[\begin{array}{c} 0\\ 0\\ 0\\ \frac{4m_1+3m_2}{4Mm_1+m_1^2+(3M+m1)m_2}\\ -\frac{3(2m_1+m2)}{2Mm_1+m_1^2+(3M+m1)m_2}\\ \frac{3m_1}{2(4Mm_1+m_1^2+(3M+m_1)m_2)\ell_2} \end{array}\right] }_{B} \underbrace{F(t)}_{u(t)}} \end{array}

これに対して、倒立位置をr_cに変更可能とする、積分動作をもつ状態フィードバック

\displaystyle{(6.2)\quad u(t)= \underbrace{- \left[\begin{array}{cccccc} f_1 & f_2 & f_3 & f_4 & f_5 & f_6 \end{array}\right] }_{-F} \underbrace{ \left[\begin{array}{c} r(t)\\ \theta_1(t)\\ \theta_2(t)\\ \dot{r}(t)\\ \dot{\theta}_1(t)\\ \dot{\theta}_2(t) \end{array}\right] }_{x(t)} +F_I\int_0^t (r(t)-r_c)dt }

を、LQI制御のアルゴリズムに沿って設計します。

MATLAB 
%cDIP_lqi.m
%-----
 clear all, close all
 global mc m1 m2 ell1 ell2 g th10 th20
 mc=1; m=0.1; m1=m; m2=m; ell=0.15; ell1=ell; ell2=ell; g=9.8;
 th10=1/180*pi %input('th1(0) = <0,180> ')/180*pi;
 th20=0 %input('th2(0) = <0,180> ')/180*pi;   
 %-----
 A=[zeros(3,3) eye(3);zeros(3,6)];
 A(4,1)=0; 
 A(4,2)=-18*m*g/(2*m+7*mc); 
 A(4,3)=3*m*g/(4*m+14*mc);  
 A(5,1)=0; 
 A(5,2)=(15*m+12*mc)*g/((2*m+7*mc)*ell);  
 A(5,3)=-(9*m+18*mc)*g/((8*m+28*mc)*ell);   
 A(6,1)=0; 
 A(6,2)=-(9*m+18*mc)*g/((2*m+7*mc)*ell);  
 A(6,3)=(15*m+48*mc)*g/((8*m+28*mc)*ell);   
 B=zeros(6,1);
 B(4)=7/(2*m+7*mc);
 B(5)=-9/((4*m+14*mc)*ell);
 B(6)=3/((4*m+14*mc)*ell);
%------
 Tcart=1; Mr=0.5;
 Tpend1=1/4*2*pi*sqrt(4/3*ell1/g); Mth1=3/180*pi;
 Tpend2=1/4*2*pi*sqrt(4/3*ell2/g); Mth2=3/180*pi;
 Tamp=0.1; Mamp=1; 
 CM=eye(3,6); CS=[1 0 0]; C=CS*CM; 
 n=size(A,1); m=size(C,1); r=size(B,2);
%-----
 AE=[A B;zeros(m,n+m)];
 BE=[zeros(n,m);eye(m)];
 CE=eye(n+m);
 QE=diag([1/Mr 1/Mth1 1/Mth2 Tcart/Mr*0 Tpend1/Mth1*0 Tpend2/Mth2*0 1/Mamp].^2);
 RE=(Tamp/Mamp)^2;
 [KE,pcl]=opt(AE,BE,CE,QE,RE);
 FE=KE/[A B;C zeros(m)];
 pcl, F=FE(:,1:n), FI=FE(:,n+1:n+m)
%-----
 CE=diag([100,180/pi,180/pi])*eye(3,7);
 sysE=ss(AE-BE*KE,[],[CE;-KE],[]);
 t=0:0.05:5;
 rc=0.5
 xE0=[-rc;0;0;0;0;0;0];
 initial(sysE,xE0,t), grid
%-----
 xs=[0;0;0;0;0;0];
 us=0;
 sim("nDIP_lqi")   
%-----
%eof

LQI制御

投稿日時: 投稿者:

LQI制御…Homework

[1]　次の可制御かつ可観測なn次系を考えます。

\displaystyle{(1)\quad \dot{x}(t)=Ax(t)+Bu(t)+w\quad(x(t),w\in{\bf R}^n,u(t)\in{\bf R}^m) }

\displaystyle{(2)\quad y(t)=C_Mx(t)\quad(y(t)\in{\bf R}^p) }

ここで、状態方程式には、操作入力u(t)のほかに、定値外乱wが加わっていること、出力方程式における行列CをC_Mと書いたことに注意します。いま、出力変数の一部やそれらの組合せからなる新しいm個の被制御変数(controlled variables)を

\displaystyle{(3)\quad z(t)=C_Sy(t)=\underbrace{C_SC_M}_{C}x(t)\quad(z(t)\in{\bf R}^m) }

のように選び((A,C)は可観測対)、定値外乱があるにも拘わらず、制御目的

\displaystyle{(4)\quad z(t)\ \rightarrow\ r \quad (t\rightarrow\infty) }

を達成したいとします。ここで、定数ベクトルrはm個の設定値からなる。もし制御目的(4)が物理的に可能とすると、ある状態x_\inftyと入力u_\inftyが確定し

\displaystyle{(5)\quad \boxed{\left[\begin{array}{c} -w \\ r \end{array}\right] = \left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right] \left[\begin{array}{c} x_\infty \\ u_\infty \end{array}\right]} }

の関係を満足しているはずです。したがって、どのようなwとrに対しても、x_\inftyとu_\inftyが定まるように、被制御変数(3)を

\displaystyle{(6)\quad \boxed{{\rm rank}\, \underbrace{ \left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right] }_{S}=n+m} }

が成り立つように選ぶものとします。

図1 積分動作を加えた状態フィードバックによる閉ループ系

[2]　さて、制御目的(4)を達成するために、図1に示すような、つぎの積分動作を加えた状態フィードバックを考えます。

\displaystyle{(7)\quad \boxed{u(t)=-Fx(t) -F_I\underbrace{\int_0^t(z(\tau)-r)\,d\tau}_{x_I(t)}} }

ここで、第2項は積分動作を表しています。このようにx_I(t)を定義すると

\displaystyle{(8)\quad \boxed{\dot{x}_I(t)=z(t)-r \qquad (x_I(0)=0)} }

を得ます。(1)と(8)を合わせて

\displaystyle{(9)\quad %\underbrace{ \left[\begin{array}{c} \dot{x}(t) \\ \dot{x}_I(t) \end{array}\right] %}_{\dot{x}_E(t)} = %\underbrace{ \left[\begin{array}{cc} A & 0 \\ C & 0 \end{array}\right] %}_{A_E} %\underbrace{ \left[\begin{array}{c} x(t) \\ x_I(t) \end{array}\right] %}_{x_E(t)} + %\underbrace{ \left[\begin{array}{c} B \\ 0 \end{array}\right] %}_{B_E} u(t) + %\underbrace{ \left[\begin{array}{c} w \\ -r \end{array}\right] %}_{w_E} }

を得ます。(9)に、(7)すなわち

\displaystyle{(10)\quad u(t)=- %\underbrace{ \left[\begin{array}{cc} F & F_I \end{array}\right] %}_{F_E} %\underbrace{ \left[\begin{array}{c} x(t) \\ x_I(t) \end{array}\right] %}_{x_E(t)} }

を代入すると、閉ループ系は、つぎのように表されます。

\displaystyle{(11)\quad \boxed{ %\underbrace{ \left[\begin{array}{c} \dot{x}(t) \\ \dot{x}_I(t) \end{array}\right] %}_{\dot{x}_E(t)} = \underbrace{ \left[\begin{array}{cc} A-BF & -BF_I \\ C & 0 \end{array}\right] }_{A_{EF}} %\underbrace{ \left[\begin{array}{c} x(t) \\ x_I(t) \end{array}\right] %}_{x_E(t)} + %\underbrace{ \left[\begin{array}{c} w \\ -r \end{array}\right] %}_{w_E} }}

いま、A_{EF}は安定行列であるとします。このとき、A_{EF}は正則であり、つぎのように書けます。

\displaystyle{(12)\quad A_{EF} = \underbrace{ \left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right] }_{S} - \left[\begin{array}{cc} B \\ 0 \end{array}\right] \left[\begin{array}{cc} F & F_I+I_m \end{array}\right] }

よって、A_{EF}の逆行列は、公式

\displaystyle{(13)\quad(P-XQ^{-1}Y)^{-1}=P^{-1}+P^{-1}X(Q-YP^{-1}X)^{-1}YP^{-1}}

を用いて

\displaystyle{(14)\quad \begin{array}{ll} A_{EF}^{-1} =& S^{-1}+ S^{-1} \left[\begin{array}{cc} B \\ 0 \end{array}\right] \left(I_m- \left[\begin{array}{cc} F & F_I+I_m \end{array}\right] S^{-1} \left[\begin{array}{cc} B \\ 0 \end{array}\right] \right)^{-1} \\[5mm] &\times \left[\begin{array}{cc} F & F_I+I_m \end{array}\right] S^{-1} \end{array} }

ここで、

\displaystyle{(15)\quad S^{-1} \left[\begin{array}{cc} B \\ 0 \end{array}\right] = \left[\begin{array}{cc} 0 \\ I_m \end{array}\right] }

に注意して、整理すると

\displaystyle{(16)\quad \boxed{A_{EF}^{-1} = \left[\begin{array}{cc} I_n & 0 \\ -F_I^{-1}F & -F_I^{-1} \end{array}\right] S^{-1}} }

のように計算されます。ところで、(11)から

\displaystyle{(17)\quad \left[\begin{array}{c} x(t) \\ x_I(t) \end{array}\right] \ \rightarrow\ A_{EF}^{-1} \left[\begin{array}{c} -w \\ r \end{array}\right] \quad (t\rightarrow\infty) }

を得ます。この第1ブロック行は、(5)より

\displaystyle{(18)\quad x(t)\ \rightarrow\ \left[\begin{array}{cc} I_n & 0 \end{array}\right] S^{-1} \left[\begin{array}{c} -w \\ r \end{array}\right] =x_\infty \quad (t\rightarrow\infty) }

となって、(5)の第2ブロック行から制御目的(4)が成り立ちます。また、(17)の第2ブロック行から

\displaystyle{(19)\quad -F_Ix_{I}(t)\ \rightarrow\ \left[\begin{array}{cc} F & I_m \end{array}\right] S^{-1} \left[\begin{array}{c} -w \\ r \end{array}\right] = Fx_\infty+u_\infty \quad (t\rightarrow\infty) }

を得ます。ここで、設定値rは既知だから、rに関係した項\left[\begin{array}{cc} F & I_m \end{array}\right] S^{-1} \left[\begin{array}{c} 0 \\ r \end{array}\right]をフィードフォワードして、速応性を改善できます。すなわち、制御目的(4)を達成する制御方式は

\displaystyle{(20)\quad u(t)=-Fx(t) +F_I\underbrace{\int_0^t(r-z(\tau))\,d\tau}_{-x_I(t)} +F_rr }

のように表され、ここで、F_rはつぎのように決定できます。

\displaystyle{(21)\quad F_r= \left[\begin{array}{cc} F & I_m \end{array}\right] S^{-1} \left[\begin{array}{c} 0 \\ I_m \end{array}\right] }

[3]　これまで、閉ループ系(11)において、A_{EF}は安定行列であるとしていました。ここでは、これを満足させるための具体的手段として、先に学んだLQ制御を使うことを考えます。LQ制御の議論における閉ループ系は自励系(入力をもたない系)を前提にしていましたが、本章における閉ループ系は入力をもつことに注意が必要です。この前提を満足させるために、定常状態との差をとって得られる偏差系(error system)が用いられます。

制御目的(4)が達成されたとき成り立つ(5)より

\displaystyle{(22)\quad \left[\begin{array}{c} 0 \\ 0 \end{array}\right] = \left[\begin{array}{cc} A & 0 \\ C & 0 \end{array}\right] \left[\begin{array}{c} x_\infty \\ x_{I\infty} \end{array}\right] + \left[\begin{array}{c} B \\ 0 \end{array}\right] u_\infty + \left[\begin{array}{c} w \\ -r \end{array}\right] }

を得ます(x_{I\infty}は定数ベクトル)。まず、(9)から(22)を引いて、つぎの偏差系を得ます。

偏差系E1：
\displaystyle{(23)\quad \boxed{\frac{d}{dt} %\underbrace{ \left[\begin{array}{c} x(t)-x_\infty \\ x_I(t)-x_{I\infty} \end{array}\right] %}_{x_E(t)-x_{E\infty}} = \underbrace{ \left[\begin{array}{cc} A & 0 \\ C & 0 \end{array}\right] }_{A_{E1}} %\underbrace{ \left[\begin{array}{c} x(t)-x_\infty \\ x_I(t)-x_{I\infty} \end{array}\right] %}_{x_E(t)-x_{E\infty}} + \underbrace{ \left[\begin{array}{c} B \\ 0 \end{array}\right] }_{B_{E1}} (u(t)-u_\infty)} }

この両辺を微分すれば、状態変数の中の定数ベクトルを除くことができて

偏差系E2：
\displaystyle{(24)\quad \boxed{\frac{d}{dt} %\underbrace{ \left[\begin{array}{c} {\dot x}(t) \\ z(t)-r \end{array}\right] %}_{\dot{x}_E(t)} = \underbrace{ \left[\begin{array}{cc} A & 0 \\ C & 0 \end{array}\right] }_{A_{E2}} %\underbrace{ \left[\begin{array}{c} {\dot x}(t) \\ z(t)-r \end{array}\right] %}_{\dot{x}_E(t)} + \underbrace{ \left[\begin{array}{c} B \\ 0 \end{array}\right] }_{B_{E2}} {\dot u}(t)} }

を得ます。さらに、(1)と(3)をまとめた

\displaystyle{(25)\quad \left[\begin{array}{c} {\dot x}(t)-w \\ z(t) \end{array}\right] = \underbrace{ \left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right] }_{S} \left[\begin{array}{c} x(t) \\ u(t) \end{array}\right] }

から(5)を引いて、つぎの関係式が成り立ちます。

\displaystyle{(26)\quad \boxed{\left[\begin{array}{c} {\dot x}(t) \\ z(t)-r \end{array}\right] = \underbrace{ \left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right] }_{S} \left[\begin{array}{c} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right]} }

これに基づいて、偏差系E2に座標変換を行えば

偏差系E3：
\displaystyle{(27)\quad \boxed{\frac{d}{dt} %\underbrace{ \left[\begin{array}{c} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] %}_{{\tilde x}_E(t)-{\tilde x}_{E\infty}} = \underbrace{ \left[\begin{array}{cc} A & B \\ 0 & 0 \end{array}\right] }_{A_{E3}} %\underbrace{ \left[\begin{array}{c} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] %}_{{\tilde x}_E(t)-{\tilde x}_{E\infty}} + \underbrace{ \left[\begin{array}{c} 0 \\ I_m \end{array}\right] }_{B_{E3}} {\dot u}(t)} }

を得ます。ここで、つぎの関係式を用いました。

\displaystyle{(28)\quad \underbrace{ \left[\begin{array}{cc} A & 0 \\ C & 0 \end{array}\right] }_{A_{E2}} \underbrace{ \left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right] }_{S} = \underbrace{ \left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right] }_{S} \underbrace{ \left[\begin{array}{cc} A & B \\ 0 & 0 \end{array}\right] }_{A_{E3}} }

\displaystyle{(29)\quad \underbrace{ \left[\begin{array}{c} B \\ 0 \end{array}\right] }_{B_{E2}} = \underbrace{ \left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right] }_{S} \underbrace{ \left[\begin{array}{c} 0 \\ I_m \end{array}\right] }_{B_{E3}} }

●これら３つの偏差系E1,E2,E3はすべて可制御ですが、どれを用いるのがよいのでしょう？

偏差系E1に対する安定化状態フィードバックを

\displaystyle{(30)\quad u(t)-u_\infty=- \underbrace{\left[\begin{array}{cc} F & F_I \end{array}\right] }_{F_{E1}} \left[\begin{array}{c} x(t)-x_\infty \\ x_I(t)-x_{I\infty} \end{array}\right] }

とします。これによりA_{EF}=A_{E1}-B_{E1}F_{E1}を安定行列とすることはできますが、(30)は実装上の難があります。

偏差系E2に対する安定化状態フィードバックを

\displaystyle{(31)\quad {\dot u}(t)=- \underbrace{\left[\begin{array}{cc} F & F_I \end{array}\right] }_{F_{E2}} \left[\begin{array}{c} {\dot x}(t) \\ z(t)-r \end{array}\right] }

とします。これによりA_{EF}=A_{E2}-B_{E2}F_{E2}を安定行列とすることはできますが、LQ設計時の重み係数の決定に難があります。

偏差系E3に対する安定化状態フィードバックを

\displaystyle{(32)\quad {\dot u}(t)=- \underbrace{\left[\begin{array}{cc} K & K_I \end{array}\right] }_{K_{E3}} \left[\begin{array}{c} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] }

とします。これによりA_{E3}-B_{E3}K_{E3}を安定行列とできます。一方、(32)は(26)を用いると

\displaystyle{(33)\quad {\dot u}(t)=- \underbrace{\left[\begin{array}{cc} K & K_I \end{array}\right] \left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right]^{-1} }_{K_{E3}S^{-1}} \left[\begin{array}{c} {\dot x}(t) \\ z(t)-r \end{array}\right] }

と書けます。したがって

\displaystyle{(34)\quad A_{EF}=A_{E2}-B_{E2}F_{E2}=SA_{E3}S^{-1}-SB_{E3}K_{E3}S^{-1}=S(A_{E3}-B_{E3}K_{E3})S^{-1} }

も安定行列となります。そして、

\displaystyle{(35)\quad \boxed{\left[\begin{array}{cc} F & F_I \end{array}\right] = \left[\begin{array}{cc} K & K_I \end{array}\right] \left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right]^{-1}} }

とおいて、(33)の両辺を積分すれば、制御測(7)が得られます。このことは偏差系E3の優位性を示唆しているといえます。

[4]　以下に、偏差系E3をLQ制御により安定化して、積分動作を加えた状態フィードバックを構成する手順を示します。

アルゴリズム　＜LQI制御＞

ステップ1　被制御変数の決定

\left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right]が正則となるように(C=C_SC_M)、セレクタ行列C_S(m\times p)を決めます（一般に、多入力多出力系の場合、どの操作変数でどの被制御変数を制御するのかについて、物理的に実現可能な1対1対応を考えることが重要です。その際、被制御変数はフィードバックされるので観測量の中から選ばれなけばなりません）。

ステップ2　偏差系の安定化

偏差系

\displaystyle{(36)\quad \frac{d}{dt} %\underbrace{ \left[\begin{array}{c} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] %}_{{\tilde x}_E(t)-{\tilde x}_{E\infty}} = \underbrace{ \left[\begin{array}{cc} A & B \\ 0 & 0 \end{array}\right] }_{A_{E}} %\underbrace{ \left[\begin{array}{c} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] %}_{{\tilde x}_E(t)-{\tilde x}_{E\infty}} + \underbrace{ \left[\begin{array}{c} 0 \\ I_m \end{array}\right] }_{B_{E}} {\dot u}(t) }

を、状態フィードバック

\displaystyle{(37)\quad {\dot u}(t)=- \left[\begin{array}{cc} K & K_I \end{array}\right] \left[\begin{array}{c} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] }

によるLQ制御で安定化します。その際、評価関数としては

\displaystyle{(38)\quad \int_0^\infty \left( \left[\begin{array}{c} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right]^T Q_E \left[\begin{array}{c} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] +{\dot u}^T(t)R_E{\dot u}(t)\right)\,dt }

を用います。ただし、(A_E,Q_E^{\frac{1}{2}})は可観測対とします。

ステップ3　積分動作を加えた状態フィードバックの構成

つぎの積分動作を加えた状態フィードバックを構成します。

\displaystyle{(39)\quad \boxed{u(t)=-Fx(t)-F_I\int_0^t(z(\tau)-r)\,d\tau} }

ただし

\displaystyle{(40)\quad \boxed{\left[\begin{array}{cc} F & F_I \end{array}\right] = \left[\begin{array}{cc} K & K_I \end{array}\right] \left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right]^{-1}} }

この手順で設計された積分動作を加えた状態フィードバックによる制御方式をLQI制御(LQ control with integral action)と呼びます。

LQG制御

投稿日時: 投稿者:

LQG制御…Homework

[1]　次のようなオブザーバベース・コントローラによる閉ループ系を考えます。ここで，新しい入力wとvがそれぞれW>0とV>0の平方根行列（X>0(\ge0)に対しYY=Xを満足する行列Y>0(\ge0)をX^{\frac{1}{2}}で表す）により重み付けられて，n次系の入力側（B'を介して）と出力側に設置されています。また新しい出力z=C'xと入力uが取り出されており，それぞれQ>0とR>0の平方根行列により重み付けられています。

図１　LQG制御系設計の枠組み

可制御かつ可観測な制御対象

\displaystyle{(1)\quad \left\{\begin{array}{ll} \dot{x}(t)=Ax(t)+Bu(t)+B'W^{\frac{1}{2}}w(t)&(x(t)\in{\bf R}^n,u(t),w(t)\in{\bf R}^m)\\ y(t)=Cx(t)+V^{\frac{1}{2}}v(t)&(y(t),v(t)\in{\bf R}^p)\\ z(t)=C'x(t)&(z(t)\in{\bf R}^p) \end{array}\right. }

を安定化するオブザーバベース・コントローラ

\displaystyle{(2)\quad \left\{\begin{array}{l} \dot{\hat{x}}(t)=(A-HC-BF)\hat{x}(t)+Hy(t)\\ u(t)=-F\hat{x}(t) \end{array}\right. }

を２次形式評価関数

\displaystyle{(3)\quad J=\int_0^\infty(z^T(t)Qz(t)+u^T(t)Ru(t))\,dt\quad (Q>0,R>0) }

を最小にするように、FとHを決定する問題を考えます。これらは

\displaystyle{(4)\quad \boxed{{\bf CARE}:\Pi A+A^T\Pi-\Pi BR^{-1}B^T\Pi+C'^TQC'=0} }

\displaystyle{(5)\quad \boxed{{\bf FARE}:\Gamma A^T+A\Gamma-\Gamma C^TV^{-1}C\Gamma+B'WB'^T=0} }

を満足する\Pi>0と\Gamma>0を用いて、次のように与えられます。

\displaystyle{(6)\quad \boxed{F=R^{-1}B^T\Pi} }

\displaystyle{(7)\quad \boxed{H=(V^{-1}C\Gamma)^T} }

このような制御方式をLQG制御と呼びます。

[2]　上の安定な閉ループ系は次式で表されます。

\displaystyle{(8)\quad \boxed{\begin{array}{l} \left[\begin{array}{cc} \dot{x}(t) \\ \dot{\hat{x}}(t) \end{array}\right]= \underbrace{ \left[\begin{array}{cc} A & -BF \\ HC&A-HC-BF \end{array}\right] }_{A_{CL1}} \left[\begin{array}{cc} {x}(t) \\ \hat{x}(t) \end{array}\right] + \underbrace{ \left[\begin{array}{cc} B'W^{\frac{1}{2}} & 0 \\ 0 & HV^{\frac{1}{2}} \end{array}\right] }_{B_{CL1}} \left[\begin{array}{cc} {w}(t) \\ {v}(t) \end{array}\right]\\ \left[\begin{array}{cc} {y'}(t) \\ {u'}(t) \end{array}\right] = \underbrace{ \left[\begin{array}{cc} Q^{\frac{1}{2}}C' & 0 \\ 0 & -R^{\frac{1}{2}}F \end{array}\right] }_{C_{CL1}} \left[\begin{array}{cc} {x}(t) \\ \hat{x}(t) \end{array}\right] \end{array}} }

これに座標変換

\displaystyle{(9)\quad \left[\begin{array}{cc} {x}(t) \\ {e}(t) \end{array}\right] = \left[\begin{array}{cc} I_n & 0 \\ -I_n & I_n \end{array}\right] \left[\begin{array}{cc} {x}(t) \\ \hat{x}(t) \end{array}\right],\quad \left[\begin{array}{cc} {x}(t) \\ \hat{x}(t) \end{array}\right] = \left[\begin{array}{cc} I_n & 0 \\ I_n & I_n \end{array}\right] \left[\begin{array}{cc} {x}(t) \\ {e}(t) \end{array}\right] }

を行うと

\displaystyle{(10)\quad \boxed{\begin{array}{l} \left[\begin{array}{cc} \dot{x}(t) \\ \dot{e}(t) \end{array}\right]= \underbrace{ \left[\begin{array}{cc} A-BF & -BF \\ 0 & A-HC \end{array}\right] }_{A_{CL2}} \left[\begin{array}{cc} {x}(t) \\ {e}(t) \end{array}\right] +\underbrace{ \left[\begin{array}{cc} B'W^{\frac{1}{2}} & 0 \\ -B'W^{\frac{1}{2}} & HV^{\frac{1}{2}} \end{array}\right] }_{B_{CL2}} \left[\begin{array}{cc} {w}(t) \\ {v}(t) \end{array}\right]\\ \left[\begin{array}{cc} {y'}(t) \\ {u'}(t) \end{array}\right] = \underbrace{ \left[\begin{array}{cc} Q^{\frac{1}{2}}C' & 0 \\ -R^{\frac{1}{2}}F & -R^{\frac{1}{2}}F \end{array}\right] }_{C_{CL2}} \left[\begin{array}{cc} {x}(t) \\ {e}(t) \end{array}\right] \end{array}} }

となります。このとき閉ループ系のインパルス応答は次式で与えられます。

\displaystyle{(11)\quad G_{CL}(t) =C_{CL1} \exp(A_{CL1}t) B_{CL1} =C_{CL2} \exp(A_{CL2}t) B_{CL2} }

●i=1,2に対して、評価関数Jの総和は次式で与えられます。

(12)\quad \begin{array}{l} \displaystyle{\int_0^\infty{\rm tr}(G_{CL}^T(t)G_{CL}(t))\,dt\quad (Q>0,R>0)}\\ \displaystyle{={\rm tr}(\int_0^\infty B_{CLi}^T\exp(A_{CLi}^Tt)C_{CLi}^TC_{CLi}\exp(A_{CLi}t)B_{CLi}\,dt)}\\ \displaystyle{={\rm tr}(B_{CLi}^T\int_0^\infty \exp(A_{CLi}^Tt)\underbrace{C_{CLi}^TC_{CLi}}_{Q_{CLi}}\exp(A_{CLi}t)\,dt\,B_{CLi})}\\ \displaystyle{={\rm tr}(B_{CLi}^T\underbrace{\int_0^\infty \exp(A_{CLi}^Tt)Q_{CLi}\exp(A_{CLi}t)\,dt}_{\Pi}\,B_{CLi})}\\ \displaystyle{={\rm tr}(\Pi B_{CLi}B_{CLi}^T)} \end{array} }

ここで\Pi>0は次式を満足します。

\displaystyle{(13)\quad \Pi A_{CLi}+A_{CLi}^T\Pi+Q_{CLi}=0 }

ラグランジュの未定定数法を適用するために、次の評価関数を考えます。

\displaystyle{(14)\quad J'={\rm tr}(\Pi B_{CLi}B_{CLi}^T)+{\rm tr}((\Pi A_{CLi}+A_{CLi}^T\Pi+Q_{CLi})\Gamma) }

これを最小化する場合の必要条件は、次式となります。

\displaystyle{(15)\quad \frac{\partial J'}{\partial\Gamma}=0,\ \frac{\partial J'}{\partial\Pi}=0,\ \frac{\partial J'}{\partial F}=0,\ \frac{\partial J'}{\partial H}=0 }

以下では、次の分割を考えます。

\displaystyle{(16)\quad \Pi=\left[\begin{array}{cc} \Pi_{1}&\Pi_{3} \\ \Pi_{3}^T&\Pi_{2} \end{array}\right],\ \Gamma=\left[\begin{array}{cc} \Gamma_{1}&\Gamma_{3} \\ \Gamma_{3}^T&\Gamma_{2} \end{array}\right] }

[3]　i=1の場合について、必要条件を一つ一つ調べていきます。

●\frac{\partial J'}{\partial\Gamma}=0

\displaystyle{(17)\quad \frac{\partial J}{\partial\Gamma}=\Pi A_{CL1}+A_{CL1}^T\Pi+Q_{CL1}=0 }

において

\displaystyle{(18)\quad \begin{array}{l} \Pi A_{CL1}= \left[\begin{array}{cc} \Pi_{1}&\Pi_{3} \\ \Pi_{3}^T&\Pi_{2} \end{array}\right] \left[\begin{array}{cc} A & -BF \\ HC&A-HC-BF \end{array}\right]\\ = \left[\begin{array}{cc} \Pi_{1}A+\Pi_{3}HC & -\Pi_{1}BF+\Pi_{3}(A-HC-BF)\\ \Pi_{3}^TA+\Pi_{2}HC & -\Pi_{3}^TBF+\Pi_{2}(A-HC-BF) \end{array}\right] \end{array} }

\displaystyle{(19)\quad \begin{array}{l} A_{CL1}^T\Pi =(\Pi A_{CL1})^T\\ =\left[\begin{array}{cc} A^T\Pi_{1}+C^TH^T\Pi_{3}^T&\\ -F^TB^T\Pi_{1}+(A-HC-BF)^T\Pi_{3}^T & \end{array}\right.\\ \left.\begin{array}{cc} A^T\Pi_{3}+C^TH^T\Pi_{2}\\ -F^TB^T\Pi_{3}+(A-HC-BF)^T\Pi_{2} \end{array}\right] \end{array} }

\displaystyle{(20)\quad Q_{CL1}= \left[\begin{array}{cc} C'^TQC' & 0 \\ 0 & F^TRF \end{array}\right] }

を代入して、次を得ます。

\displaystyle{(21)\quad \Pi_{1}A+\Pi_{3}HC+A^T\Pi_{1}+C^TH^T\Pi_{3}^T+C'^TQC'=0 }

\displaystyle{(22)\quad -\Pi_{1}BF+\Pi_{3}(A-HC-BF)+A^T\Pi_{3}+C^TH^T\Pi_{2}=0 }

\displaystyle{(23)\quad \begin{array}{l} -\Pi_{3}^TBF+\Pi_{2}(A-HC-BF) \\ -F^TB^T\Pi_{3}+(A-HC-BF)^T\Pi_{2}+F^TRF=0 \end{array} }

(22)+(23)より

\displaystyle{(24)\quad \begin{array}{l} (\Pi_{3}+\Pi_{2})(A-HC)+(A-BF)^T(\Pi_{3}+\Pi_{2})\\ +\underbrace{F^TRF-(\Pi_{1}+\Pi_{3}+\Pi_{3}^T+\Pi_{2})BF}_{0\ (assumed)}=0\\ \Rightarrow \Pi_{3}+\Pi_{2}=0\Rightarrow \Pi_{3}=-\Pi_{2}\Rightarrow \Pi_{3}^T=\Pi_{3} \end{array} }

(23)より

\displaystyle{(25)\quad \begin{array}{l} \Pi_{2}(A-HC)+(A-HC)^T\Pi_{2}+F^TRF\\ \underbrace{-(\Pi_{3}^T+\Pi_{2})BF-F^TB^T(\Pi_{3}+\Pi_{2})}_{0}=0\\ \Rightarrow \Pi_{2}>0 \end{array} }

(21)+(22)より

\displaystyle{(26)\quad \begin{array}{l} (\Pi_{1}+\Pi_{3})(A-BF)+A^T(\Pi_{1}+\Pi_{3})\\ +\underbrace{C^TH^T(\Pi_{3}^T+\Pi_{2})}_{0}+C'^TQC'+F^TRF-F^TB^T(\Pi_{1}+\Pi_{3})=0 \end{array} }

\displaystyle{(27)\quad \begin{array}{l} (\Pi_{1}+\Pi_{3})(A-BF)+(A-BF)^T(\Pi_{1}+\Pi_{3})+C'^TQC'+F^TRF=0\\ \Rightarrow \Pi_{1}+\Pi_{3}=\Pi_{1}-\Pi_{2}>0 \end{array} }

●\frac{\partial J'}{\partial\Pi}=0

\displaystyle{(28)\quad \frac{\partial J}{\partial\Pi}=B_{CL1}B_{CL1}^T+\Gamma A_{CL1}^T+A_{CL1}\Gamma=0 }

において

\displaystyle{(29)\quad \begin{array}{l} A_{CL1}\Gamma= \left[\begin{array}{cc} A & -BF \\ HC&A-HC-BF \end{array}\right] \left[\begin{array}{cc} \Gamma_{1}&\Gamma_{3} \\ \Gamma_{3}^T&\Gamma_{2} \end{array}\right]\\ =\left[\begin{array}{cc} A\Gamma_{1} -BF\Gamma_{3}^T &A\Gamma_{3} -BF\Gamma_{2}\\ HC\Gamma_{1}+(A-HC-BF)\Gamma_{3}^T &HC\Gamma_{3}+(A-HC-BF)\Gamma_{2} \end{array}\right] \end{array} }

\displaystyle{(30)\quad \begin{array}{l} \Gamma A_{CL1}^T =(A_{CL1}\Gamma)^T\\ =\left[\begin{array}{cc} \Gamma_{1}A^T -\Gamma_{3}F^TB^T & \Gamma_{1}C^TH^T+\Gamma_{3}(A-HC-BF)^T\\ \Gamma_{3}^TA^T -\Gamma_{2}F^TB^T & \Gamma_{3}^TC^TH^T+\Gamma_{2}(A-HC-BF)^T \end{array}\right] \end{array} }

\displaystyle{(31)\quad B_{CL1}B_{CL1}^T =\left[\begin{array}{cc} B'WB'^T & 0 \\ 0 & HVH^T \end{array}\right] }

を代入して、次を得ます。

\displaystyle{(32)\quad A\Gamma_{1} -BF\Gamma_{3}^T+\Gamma_{1}A^T -\Gamma_{3}F^TB^T+B'WB'^T=0 }

\displaystyle{(33)\quad A\Gamma_{3} -BF\Gamma_{2}+\Gamma_{1}C^TH^T+\Gamma_{3}(A-HC-BF)^T=0 }

\displaystyle{(34)\quad \begin{array}{l} HC\Gamma_{3}+(A-HC-BF)\Gamma_{2}\\ +\Gamma_{3}^TC^TH^T+\Gamma_{2}(A-HC-BF)^T+HVH^T=0 \end{array} }

(33)-(34)より

\displaystyle{(35)\quad \begin{array}{l} (A-HC)(\Gamma_{3}-\Gamma_{2})+(\Gamma_{3}-\Gamma_{2})(A-BF)^T\\ +\underbrace{(\Gamma_{1}-\Gamma_{3}-\Gamma_{3}^T+\Gamma_{2})C^TH^T-HVH^T}_{0\ (assumed)}=0\\ \Rightarrow \Gamma_{3}-\Gamma_{2}=0\Rightarrow \Gamma_{3}=\Gamma_{2}\Rightarrow \Gamma_{3}^T=\Gamma_{3} \end{array} }

(34)より

\displaystyle{(36)\quad \begin{array}{l} (A-BF)\Gamma_{2}+\Gamma_{2}(A-BF)^T+H^TVH\\ +\underbrace{HC(\Gamma_{3}-\Gamma_{2})+(\Gamma^T_{3}-\Gamma_{2})C^TH^T}_{0}=0\Rightarrow \Gamma_{2}>0 \end{array} }

(32)-(33)より

\displaystyle{(37)\quad \begin{array}{l} (\Gamma_{1}-\Gamma_{3})(A-HC)^T+A(\Gamma_{1}-\Gamma_{3})\\ +\underbrace{BF(\Gamma_{3}^T-\Gamma_{2})}_{0}+B'WB'^T+HVH^T-HC(\Gamma_{1}-\Gamma_{3})=0 \end{array} }

\displaystyle{(38)\quad \begin{array}{l} (\Gamma_{1}-\Gamma_{3})(A-HC)^T+(A-HC)(\Gamma_{1}-\Gamma_{3})+B'WB'^T+HVH^T=0\\ \Rightarrow \Gamma_{1}-\Gamma_{3}=\Gamma_{1}-\Gamma_{2}>0 \end{array} }

●準備１

\displaystyle{(39)\quad \Pi B_{CL1}B_{CL1}^T= \left[\begin{array}{cc} \Pi_{1}&\Pi_{3} \\ \Pi_{3}^T&\Pi_{2} \end{array}\right] \left[\begin{array}{cc} B'WB'^T & 0 \\ 0 & HVH^T \end{array}\right] }

(1)   \begin{eqnarray*} \end{eqnarray*}

\displaystyle{(40)\quad {\rm tr}(\Pi B_{CL1}B_{CL1}^T)={\rm tr}(\Pi_{1}B'WB'^T+\Pi_{2}HVH^T) }

\displaystyle{(41)\quad \frac{\partial {\rm tr}(\Pi B_{CL1}B_{CL1}^T)}{\partial H}=2\Pi_{2}HV }

●準備２

\displaystyle{(42)\quad \begin{array}{l} \Pi A_{CL1}\Gamma= \left[\begin{array}{cc} \Pi_{1}&\Pi_{3} \\ \Pi_{3}^T&\Pi_{2} \end{array}\right] \left[\begin{array}{cc} A & -BF \\ HC&A-HC-BF \end{array}\right] \Gamma\\ =\left[\begin{array}{cc} \Pi_{1}A+\Pi_{3}HC & -\Pi_{1}BF+\Pi_{3}(A-HC-BF)\\ \Pi_{3}^TA+\Pi_{2}HC & -\Pi_{3}^TBF+\Pi_{2}(A-HC-BF) \end{array}\right] \left[\begin{array}{cc} \Gamma_{1}&\Gamma_{3} \\ \Gamma_{3}^T&\Gamma_{2} \end{array}\right] \end{array} }

\displaystyle{(43)\quad \begin{array}{l} {\rm tr}(\Pi A_{CL1}\Gamma)\\ ={\rm tr}(\Pi_{1}A\Gamma_{1}+\Pi_{3}HC\Gamma_{1} -\Pi_{1}BF\Gamma_{3}^T+\Pi_{3}(A-HC-BF)\Gamma_{3}^T)\\ +{\rm tr}(\Pi_{3}^TA\Gamma_{3}+\Pi_{2}HC\Gamma_{3} -\Pi_{3}^TBF\Gamma_{2}+\Pi_{2}(A-HC-BF)\Gamma_{2}) \end{array} }

\displaystyle{(44)\quad \frac{\partial {\rm tr}(\Pi A_{CL1}\Gamma)}{\partial F} =-B^T\Pi_1\Gamma_{3}-B^T\Pi_3^T\Gamma_3-B^T\Pi_{3}\Gamma_{2}-B^T\Pi_{2}\Gamma_{2} }

\displaystyle{(45)\quad \frac{\partial {\rm tr}(\Pi A_{CL1}\Gamma)}{\partial H}=\Pi_{3}^T\Gamma_{1}C^T-\Pi_{3}^T\Gamma_{3}C^T+\Pi_{2}\Gamma_{3}^TC^T-\Pi_{2}\Gamma_{2}C^T }

●準備３

\displaystyle{(46)\quad \begin{array}{l} A_{CL1}^T\Pi\Gamma=(\Pi A_{CL1})^T\Gamma=\\ \left[\begin{array}{cc} A^T\Pi_{1}+C^TH^T\Pi_{3}^T&\\ -F^TB^T\Pi_{1}+(A-HC-BF)^T\Pi_{3}^T & \end{array}\right.\\ \left.\begin{array}{cc} A^T\Pi_{3}+C^TH^T\Pi_{2}\\ -F^TB^T\Pi_{3}+(A-HC-BF)^T\Pi_{2} \end{array}\right] \left[\begin{array}{cc} \Gamma_{1}&\Gamma_{3} \\ \Gamma_{3}^T&\Gamma_{2} \end{array}\right] \end{array} }

\displaystyle{(47)\quad \begin{array}{l} {\rm tr}(A_{CL1}^T\Pi\Gamma)\\ ={\rm tr}(A^T\Pi_{1}\Gamma_{1}+C^TH^T\Pi_{3}^T\Gamma_{1}+A^T\Pi_{3}\Gamma_{3}^T+C^TH^T\Pi_{2}\Gamma_{3}^T)\\ +{\rm tr}( -F^TB^T\Pi_{1}\Gamma_{3}+(A-HC-BF)^T\Pi_{3}^T\Gamma_{3} \\ -F^TB^T\Pi_{3}\Gamma_{2}+(A-HC-BF)^T\Pi_{2}\Gamma_{2}) \end{array} }

\displaystyle{(48)\quad \frac{\partial {\rm tr}(\Pi A_{CL1}\Gamma)}{\partial F}=-B^T\Pi_1\Gamma_{3}-B^T\Pi_3^T\Gamma_3-B^T\Pi_{3}\Gamma_{2}-B^T\Pi_{2}\Gamma_{2} }

\displaystyle{(49)\quad \frac{\partial {\rm tr}(\Pi A_{CL1}\Gamma)}{\partial H}=\Pi_{3}^T\Gamma_{1}C^T-\Pi_{3}^T\Gamma_{3}C^T+\Pi_{2}\Gamma_{3}^TC^T-\Pi_{2}\Gamma_{2}C^T }

●準備４

\displaystyle{(50)\quad Q_{CL1}^T\Gamma= \left[\begin{array}{cc} C^TQC & 0 \\ 0 & F^TRF \end{array}\right] \left[\begin{array}{cc} \Gamma_{1}&\Gamma_{3} \\ \Gamma_{3}^T&\Gamma_{2} \end{array}\right] }

\displaystyle{(51)\quad {\rm tr}(Q_{CL1}\Gamma)={\rm tr}(C^TQC\Gamma_{1}+F^TRF\Gamma_{2}) }

\displaystyle{(52)\quad \frac{\partial {\rm tr}(Q_{CL1}\Gamma)}{\partial F}=2RF\Gamma_{2} }

●\frac{\partial J'}{\partial F}=0

\displaystyle{(53)\quad \begin{array}{l} \frac{\partial J}{\partial F}=2RF\Gamma_{2}-2B^T(\Pi_1+\underbrace{\Pi_3^T}_{-\Pi_2})\underbrace{\Gamma_3}_{\Gamma_2}-2B^T\underbrace{(\Pi_{3}+\Pi_{2})}_{0}\Gamma_{2}=0\\ \qquad\Downarrow\Pi=\Pi_1-\Pi_2>0\\ 2(RF-B^T\Pi)\Gamma_{2}=0\\ \qquad\Downarrow\Gamma_2>0\\ \boxed{F=R^{-1}B^T\Pi}\\ \qquad\Downarrow\Pi(A-BF)+(A-BF)^T\Pi+C'^TQC'+F^TRF=0\\ \boxed{\Pi A+A^T\Pi-\Pi BR^{-1}B^T\Pi+C'^TQC'=0} \end{array} }

●\frac{\partial J'}{\partial H}=0

\displaystyle{(54)\quad \begin{array}{l} \frac{\partial J}{\partial H}=2\Pi_{2}HV+2\underbrace{\Pi_{3}^T}_{-\Pi_{2}}(\Gamma_{1}-\underbrace{\Gamma_{3}}_{\Gamma_{2}})C^T+2\Pi_{2}\underbrace{(\Gamma_{3}^T-\Gamma_{2})}_{0}C^T=0\\ \qquad\Downarrow\Gamma=\Gamma_1-\Gamma_2>0\\ 2\Pi_{2}(HV-\Gamma C^T)=0\\ \qquad\Downarrow\Pi_2>0\\ \boxed{H=(V^{-1}C\Gamma)^T}\\ \qquad\Downarrow(A-HC)\Gamma+\Gamma(A-HC)^T+B'WB'^T+HVH^T=0\\ \boxed{\Gamma A^T+A\Gamma-\Gamma C^TV^{-1}C\Gamma+B'WB'^T=0} \end{array} }

補遺　上述の議論では、次についての検討が必要です。

検討事項１　(13)の妥当性
検討事項２　(24),(35)における仮定の妥当性
検討事項３　i=2の場合の導出
検討事項４　十分性の証明

LQ制御の応用

投稿日時: 投稿者:

LQ制御の応用…Homework

[1]　倒立振子は制御なしには平衡状態を保てないので、制御技術の有効性を示すのに、よく研究されています。次図は様々な倒立振子を示しています。

図1 様々な倒立振子

これらの倒立振子について、非線形シミュレータを開発し、平衡状態まわりの挙動を表す線形状態方程式を得ていました。これらに基づいて、以下では、LQ制御系を設計します。

[2] CIP

平衡状態\theta^*=0周りの挙動を表す線形状態方程式は次式で与えられます。

\displaystyle{(2.1)\quad \frac{d}{dt}\left[\begin{array}{c} r(t)\\ \theta(t)\\ \dot{r}(t)\\ \dot{\theta}(t) \end{array}\right] = \underbrace{ \left[\begin{array}{cccc} 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1\\ 0 & -\frac{3gm}{4M+m} & 0 & 0\\ 0 & \frac{3(M+m)g}{(4M+m)\ell} & 0 & 0\\ \end{array}\right] }_{A} \underbrace{ \left[\begin{array}{c} r(t)\\ \theta(t)\\ \dot{r}(t)\\ \dot{\theta}(t) \end{array}\right] }_{x(t)} + \underbrace{ \left[\begin{array}{c} 0\\ 0\\ \frac{4}{4M+m}\\ \frac{3}{(4M+m)\ell} \end{array}\right] }_{B} \underbrace{F(t)}_{u(t)} }

これに対する状態フィードバック

\displaystyle{(2.2)\quad u(t)= \underbrace{- \left[\begin{array}{cccc} f_1 & f_2 & f_3 & f_4 \end{array}\right] }_{-F} \underbrace{ \left[\begin{array}{c} r(t)\\ \theta(t)\\ \dot{r}(t)\\ \dot{\theta}(t) \end{array}\right] }_{x(t)} }

を、評価関数

\displaystyle{(2.3)\quad J=\int_0^\infty (\underbrace{\frac{1}{M_r^2}}_{q_1^2}r^2(t)+\underbrace{\frac{1}{M_\theta^2}}_{q_2^2}\theta^2(t)+\rho^2\underbrace{\frac{1}{M_F^2}}_{R}u^2(t))\,dt }

\displaystyle{(2.4)\quad |r(t)|<M_r,\ |\theta(t)|<M_\theta,\ |F(t)|<M_F }

を最小にするように設計します。ここで、(2.4)は各変数が平衡状態周りで取り得る範囲を表しています。これらの逆数を重み係数に用いる方法をInverse Weighting法と呼ぶことがあります。\rhoは状態変数と入力変数の間のバランスをとる役割を果たします。また、上の評価関数はあくまで一例であり、他にも候補があり、その選択は設計者に任せられています。

MATLAB 
%cCIP_lq.m
%-----
 clear all, close all
 global mc m ell g th0
 mc=1; m=0.1; ell=0.2; g=9.8;
 th0=3/180*pi %input('th(0) = <0,180> ')/180*pi;
%-----
 A=[zeros(2,2) eye(2);zeros(2,4)];
 A(3,1)=0; 
 A(3,2)=-3*m*g/(m+4*mc); 
 A(4,1)=0; 
 A(4,2)=3*(m+mc)*g/((m+4*mc)*ell);  
 B=zeros(4,1);
 B(3)=4/(m+4*mc);
 B(4)=-3/((m+4*mc)*ell); 
%-----
 C=eye(2,4);
 Mr=0.5; Mth=3/180*pi; MF=1; rho=1;
 Q=diag([1/Mr^2,1/Mth^2]); R=rho^2*1/MF^2;
 [F,pcl]=opt(A,B,C,Q,R)
 C=diag([100,180/pi])*C;
 sys=ss(A-B*F,B,[C;-F],[]);
 t=0:0.05:5;
 x0=[0;th0;0;0];
 initial(sys,x0,t), grid
%-----
 xs=[0;0;0;0];
 us=0;
 sim("nCIP_lq")
%-----
%end

[3] CIP2

平衡状態\theta^*=0と平衡入力F^*=(M+m)g\sin\alpha周りの挙動を表す線形状態方程式は次式で与えられます。

(3.1)\quad \begin{array}{l} \displaystyle{\frac{d}{dt}\left[\begin{array}{c} r(t)\\ \theta(t)\\ \dot{r}(t)\\ \dot{\theta}(t) \end{array}\right] = \underbrace{\left[\begin{array}{cccc} 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1\\ 0 & -\frac{6\cos\alpha mg}{8M+(5-3\cos2\alpha)m} & 0 & 0\\ 0 & \frac{6(M+m)g}{(8M+(5-3\cos2\alpha)m)\ell} & 0 & 0\\ \end{array}\right] }_{A} \underbrace{ \left[\begin{array}{c} r(t)\\ \theta(t)\\ \dot{r}(t)\\ \dot{\theta}(t) \end{array}\right] }_{x(t)}}\\ \displaystyle{+ \underbrace{\left[\begin{array}{c} 0\\ 0\\ \frac{8}{8M+(5-3\cos2\alpha)m}\\ \frac{6\cos\alpha}{(8M+(5-3\cos2\alpha)m)\ell} \end{array}\right] }_{B} \underbrace{(F(t)-F^*)}_{u(t)}} \end{array}

これに対する状態フィードバック

\displaystyle{(3.2)\quad u(t)= \underbrace{- \left[\begin{array}{cccc} f_1 & f_2 & f_3 & f_4 \end{array}\right] }_{-F} \underbrace{ \left[\begin{array}{c} r(t)\\ \theta(t)\\ \dot{r}(t)\\ \dot{\theta}(t) \end{array}\right] }_{x(t)} }

を、評価関数

\displaystyle{(3.3)\quad J=\int_0^\infty (\underbrace{\frac{1}{M_r^2}}_{q_1^2}r^2(t)+\underbrace{\frac{1}{M_\theta^2}}_{q_2^2}\theta^2(t)+\rho^2\underbrace{\frac{1}{M_F^2}}_{R}u^2(t))\,dt }

\displaystyle{(3.4)\quad |r(t)|<M_r,\ |\theta(t)|<M_\theta,\ |F(t)-F^*|<M_F }

を最小にするように設計します。

MATLAB 
%cCIP2_lq.m
%-----
 clear all, close all
 global mc m ell g alpha th0
 mc=1; m=0.1; ell=0.2; g=9.8;
 alpha=3/180*pi;
 th0=3/180*pi %input('ths   = <0,180> ')/180*pi;
%-----
 A=[zeros(2,2) eye(2);zeros(2,4)];
 A(3,1)=0; 
 A(3,2)=-6*cos(alpha)*m*g/(8*mc+(5-3*cos(2*alpha))*m); 
 A(4,1)=0; 
 A(4,2)=6*(m+mc)*g/((8*mc+(5-3*cos(2*alpha))*m)*ell);  
 B=zeros(4,1);
 B(3)=8/(8*mc+(5-3*cos(2*alpha))*m);
 B(4)=-6*cos(alpha)/((8*mc+(5-3*cos(2*alpha))*m)*ell); 
%-----
 C=eye(2,4);
 Mr=0.5; Mth=3/180*pi; MF=1; rho=1;
 Q=diag([1/Mr^2,1/Mth^2]); R=rho^2*1/MF^2;
 [F,pcl]=opt(A,B,C,Q,R)
 C=diag([100,180/pi])*C;
 sys=ss(A-B*F,B,[C;-F],[]);
 t=0:0.05:5;
 x0=[0;th0;0;0];
 initial(sys,x0,t), grid
%-----
 xs=[0;0;0;0];
 us=(mc+m)*g*sin(alpha);
 sim("nCIP2_lq")
%-----
%end

●いまどれくらいレールが傾けられたか（\alphaの値）は未知とします。この場合F^*=(M+m)g\sin\alphaの設定ができないので、これを零として制御系のシミュレーションを実行してみます。そうすると安定化された倒立位置がずれてしまいます（平衡状態の移動）。これを防ぐにはLQI制御を適用する必要があります。

[4] AIP

平衡状態\theta_1^*=0,\ \theta_2^*=0と平衡入力\tau^*=-(m_1+2m_2)\ell_1g\sin\theta_1^*=0周りの挙動を表す線形状態方程式は次式で与えられます。

(4.1)\quad \begin{array}{l} \displaystyle{\frac{d}{dt}\left[\begin{array}{c} \theta_1(t)-\theta_1^*\\ \theta_2(t)\\ \dot{\theta}_1(t)\\ \dot{\theta}_2(t) \end{array}\right] =}\\ \displaystyle{\underbrace{\left[\begin{array}{cccc} 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1\\ \frac{3(m_1+2m_2)g}{(4m_1+3m_2)\ell_1} & -\frac{9m_2g}{2(4m_1+3m_2)\ell_1} & 0 & 0\\ -\frac{9(m_1+2m_2)g}{2(4m_1+3m_2)\ell_1} & \frac{9m_2g}{(4m_1+3m_2)\ell_2} & 0 & 0\\ \end{array}\right] }_{A} \left[\begin{array}{c} \theta_1(t)-\theta_1^*\\ \theta_2(t)\\ \dot{\theta}_1(t)\\ \dot{\theta}_2(t) \end{array}\right]\\ \displaystyle{+ \underbrace{\left[\begin{array}{c} 0\\ 0\\ \frac{6}{2(4m_1+3m_2)\ell_1^2}\\ -\frac{9}{2(4m_1+3m_2)\ell_1\ell_2} \end{array}\right] }_{B} \underbrace{(\tau(t)-\tau^*)}_{u(t)}} \end{array}

これに対する状態フィードバック

\displaystyle{(4.2)\quad u(t)= \underbrace{- \left[\begin{array}{cccc} f_1 & f_2 & f_3 & f_4 \end{array}\right] }_{-F} \underbrace{ \left[\begin{array}{c} \theta_1(t)-\theta_1^*\\ \theta_2(t)\\ \dot{\theta}_1(t)\\ \dot{\theta}_2(t) \end{array}\right] }_{x(t)} }

を、評価関数

\displaystyle{(4.3)\quad J=\int_0^\infty (\underbrace{\frac{1}{M_{\theta_1}^2}}_{q_1^2}(\theta_1(t)-\theta_1^*)^2(t)+\underbrace{\frac{1}{M_{\theta_2}^2}}_{q_2^2}\theta^2(t)+\rho^2\underbrace{\frac{1}{M_\tau^2}}_{R}u^2(t))\,dt }

\displaystyle{(4.4)\quad |\theta_1(t)-\theta_1^*|<M_{\theta_1},\ |\theta_2(t)|<M_{\theta_2},\ |\tau(t)-\tau^*|<M_\tau }

を最小にするように設計します。

MATLAB 
%cAIP_lq.m
%-----
 clear all, close all
 global m1 m2 ell1 ell2  g th10 th20
 m1=0.1; m2=0.2; ell1=0.1; ell2=0.2; g=9.8;
 th10=0/180*pi %input('th1(0) = <0,180> ')/180*pi;
 th20=-3/180*pi %input('th2(0) = <0,180> ')/180*pi;  
%----- th10=0 における線形モデル
 A=[zeros(2,2) eye(2);zeros(2,4)];
 A(3,1)=3*(2*m2+m1)*g/((3*m2+4*m1)*ell1); 
 A(3,2)=-9*m2*g/(2*(3*m2+4*m1)*ell1); 
 A(4,1)=-9*(2*m2+m1)*g/(2*(3*m2+4*m1)*ell2); 
 A(4,2)=3*(3*m2+m1)*g/((3*m2+4*m1)*ell2);  
 B=zeros(4,1);
 B(3)=3/((3*m2+4*m1)*ell1^2);
 B(4)=-9/(2*(3*m2+4*m1)*ell1*ell2);
%-----
 C=eye(2,4);
 Mth1=3/180*pi; Mth2=3/180*pi; MF=1; rho=1;
 Q=diag([1/Mth1^2,1/Mth2^2]); R=rho^2*1/MF^2;
 [F,pcl]=opt(A,B,C,Q,R)
 C=diag([180/pi,180/pi])*C;
 sys=ss(A-B*F,B,[C;-F],[]);
 t=0:0.01:1;
 x0=[th10;th20;0;0];
 initial(sys,x0,t), grid
%-----
 xs=[0;0;0;0];
 us=-(m1+2*m2)*ell1*g*sin(th10)
 sim("nAIP_lq")
%-----
%end

●平衡状態\theta_1^*\ne 0,\ \theta_2^*=0に移動させるためには、LQI制御を適用する必要があります。

[5] PIP

平衡状態\theta_1^*=0,\ \theta_2^*=0周りの挙動を表す線形状態方程式は次式で与えられます。

(5.1)\quad \begin{array}{l} \displaystyle{\frac{d}{dt}\left[\begin{array}{c} r(t)\\ \theta_1(t)\\ \theta_2(t)\\ \dot{r}(t)\\ \dot{\theta}_1(t)\\ \dot{\theta}_2(t) \end{array}\right] =}\\ \displaystyle{\underbrace{\left[\begin{array}{cccccc} 0 & 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 0 & 0 & 1\\ 0 & -\frac{3m_1g}{4M+m_1+m_2} & -\frac{3m_2g}{4M+m_1+m_2} & 0 & 0 & 0\\ 0 & \frac{3(4m+4m_1+m_2)g}{4(4M+m_1+m_2)\ell_1} & \frac{9m_2g}{4(4M+m_1+m_2)\ell_1} & 0 & 0& 0\\ 0 & \frac{9gm_1}{4(4M+m_1+m_2)\ell_2} & \frac{3(4m+m_1+4m_2)g}{4(4M+m_1+m_2)\ell_2} & 0 & 0& 0\\ \end{array}\right]}_{A} \left[\begin{array}{c} r(t)\\ \theta_1(t)\\ \theta_2(t)\\ \dot{r}(t)\\ \dot{\theta}_1(t)\\ \dot{\theta}_2(t) \end{array}\right]}\\ \displaystyle{+ \underbrace{ \left[\begin{array}{c} 0\\ 0\\ 0\\ \frac{4}{4M+m_1+m_2}\\ -\frac{3}{(4M+m_1+m_2)\ell_1}\\ -\frac{3}{(4M+m_1+m_2)\ell_2} \end{array}\right] }_{B} \underbrace{F(t)}_{u(t)} \end{array}

これに対する状態フィードバック

\displaystyle{(5.2)\quad u(t)= \underbrace{- \left[\begin{array}{cccccc} f_1 & f_2 & f_3 & f_4 & f_5 & f_6 \end{array}\right] }_{-F} \underbrace{ \left[\begin{array}{c} r(t)\\ \theta_1(t)\\ \theta_2(t)\\ \dot{r}(t)\\ \dot{\theta}_1(t)\\ \dot{\theta}_2(t) \end{array}\right] }_{x(t)} }

を、評価関数

\displaystyle{(5.3)\quad J=\int_0^\infty (\underbrace{\frac{1}{M_r^2}}_{q_1^2}r^2(t)+\underbrace{\frac{1}{M_{\theta_1}^2}}_{q_2^2}\theta_1^2(t)+\underbrace{\frac{1}{M_{\theta_2}^2}}_{q_2^2}\theta^2(t)+\rho^2\underbrace{\frac{1}{M_F^2}}_{R}u^2(t))\,dt }

\displaystyle{(5.4)\quad |r(t)|<M_r,\ |\theta_1(t)|<M_{\theta_1},\ |\theta_2(t)|<M_{\theta_2},\ |F(t)|<M_F }

を最小にするように設計します。

MATLAB 
%cPIP_lq.m
%-----
 clear all, close all
 global mc m1 m2 ell1 ell2 g th10 th20
 mc=1; m1=0.1; m2=0.2; ell1=0.15; ell2=0.2; g=9.8; 
 th10=0/180*pi %input('th1(0) = <0,180> ')/180*pi;
 th20=-1/180*pi %input('th2(0) = <0,180> ')/180*pi;  
%-----
 A=[zeros(3,3) eye(3);zeros(3,6)];
 A(4,1)=0; 
 A(4,2)=-3*m1*g/(m1+m2+4*mc); 
 A(4,3)=-3*m2*g/(m1+m2+4*mc);  
 A(5,1)=0; 
 A(5,2)=(12*m1+3*m2+12*mc)*g/((4*m1+4*m2+16*mc)*ell1);  
 A(5,3)=9*m2*g/((4*m1+4*m2+16*mc)*ell1);   
 A(6,1)=0; 
 A(6,2)=9*m1*g/((4*m1+4*m2+16*mc)*ell2);  
 A(6,3)=(3*m1+12*m2+12*mc)*g/((4*m1+4*m2+16*mc)*ell2);   
 B=zeros(6,1);
 B(4)=4/(m1+m2+4*mc);
 B(5)=-3/((m1+m2+4*mc)*ell1);
 B(6)=-3/((m1+m2+4*mc)*ell2); 
%------
 C=eye(3,6);
 Mr=0.5; Mth1=3/180*pi; Mth2=3/180*pi; MF=1; rho=1;
 Q=diag([1/Mr^2,1/Mth1^2,1/Mth2^2]); R=rho^2*1/MF^2;
 [F,pcl]=opt(A,B,C,Q,R);
 C=diag([100,180/pi,180/pi])*C;
 sys=ss(A-B*F,B,[C;-F],[]);
 t=0:0.05:5;
 x0=[0;th10;th20;0;0;0];
 initial(sys,x0,t), grid
%-----
 xs=[0;0;0;0;0;0];
 us=0;
 sim("nPIP_lq")   
%-----
%eof

[6] DIP

平衡状態\theta_1^*=0,\ \theta_2^*=0周りの挙動を表す線形状態方程式は次式で与えられます。

(6.1)\quad \begin{array}{l} \displaystyle{ \frac{d}{dt}\left[\begin{array}{c} r(t)\\ \theta_1(t)\\ \theta_2(t)\\ \dot{r}(t)\\ \dot{\theta}_1(t)\\ \dot{\theta}_2(t) \end{array}\right] =}\\ \displaystyle{ \left[\begin{array}{cccccc} 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & -\frac{3m_1g}{Mm_1+m_1^2+(3M+m1)m_2} & -\frac{3m_2g}{Mm_1+m_1^2+(3M+m1)m_2} \\ 0 & \frac{3(4Mm_1+4m_1^2+3m_2^2+3(18M+13m_1)m_2)g}{(4Mm_1+m_1^2+(3M+m1)m_2)\ell_1} & \frac{9(2M+m_1)m_2g}{(4Mm_1+m_1^2+(3M+m1)m_2)\ell_1} \\ 0 & \frac{9(2Mm_1+m_1^2+3(2M+m_1)m_2)g}{(4Mm_1+m_1^2+(3M+m_1)m_2)\ell_2} & \frac{3(4Mm_1+m_1^2+12(3M+m_1)m_2)g}{(4Mm_1+m_1^2+(3M+m_1)m_2)\ell_2} \\ \end{array}\right.}\\ \displaystyle{\left.\begin{array}{cccccc} 1 & 0 & 0\\ 0 & 1 & 0\\ 0 & 0 & 1\\ 0 & 0 & 0\\ 0 & 0 & 0\\ 0 & 0 & 0\\ \end{array}\right] \left[\begin{array}{c} r(t)\\ \theta_1(t)\\ \theta_2(t)\\ \dot{r}(t)\\ \dot{\theta}_1(t)\\ \dot{\theta}_2(t) \end{array}\right]}\\ \displaystyle{+ \underbrace{\left[\begin{array}{c} 0\\ 0\\ 0\\ \frac{4m_1+3m_2}{4Mm_1+m_1^2+(3M+m1)m_2}\\ -\frac{3(2m_1+m2)}{2Mm_1+m_1^2+(3M+m1)m_2}\\ \frac{3m_1}{2(4Mm_1+m_1^2+(3M+m_1)m_2)\ell_2} \end{array}\right] }_{B} \underbrace{F(t)}_{u(t)}} \end{array}

これに対する状態フィードバック

\displaystyle{(6.2)\quad u(t)= \underbrace{- \left[\begin{array}{cccccc} f_1 & f_2 & f_3 & f_4 & f_5 & f_6 \end{array}\right] }_{-F} \underbrace{ \left[\begin{array}{c} r(t)\\ \theta_1(t)\\ \theta_2(t)\\ \dot{r}(t)\\ \dot{\theta}_1(t)\\ \dot{\theta}_2(t) \end{array}\right] }_{x(t)} }

を、評価関数

\displaystyle{(6.3)\quad J=\int_0^\infty (\underbrace{\frac{1}{M_r^2}}_{q_1^2}r^2(t)+\underbrace{\frac{1}{M_{\theta_1}^2}}_{q_2^2}\theta_1^2(t)+\underbrace{\frac{1}{M_{\theta_2}^2}}_{q_2^2}\theta^2(t)+\rho^2\underbrace{\frac{1}{M_F^2}}_{R}u^2(t))\,dt }

\displaystyle{(6.4)\quad |r(t)|<M_r,\ |\theta_1(t)|<M_{\theta_1},\ |\theta_2(t)|<M_{\theta_2},\ |F(t)|<M_F }

を最小にするように設計します。

MATLAB 
%cDIP_lq.m
%-----
 clear all, close all
 global mc m1 m2 ell1 ell2 g th10 th20
 mc=1; m=0.1; m1=m; m2=m; ell=0.15; ell1=ell; ell2=ell; g=9.8;
 th10=1/180*pi %input('th1(0) = <0,180> ')/180*pi;
 th20=0 %input('th2(0) = <0,180> ')/180*pi;   
 %-----
 A=[zeros(3,3) eye(3);zeros(3,6)];
 A(4,1)=0; 
 A(4,2)=-18*m*g/(2*m+7*mc); 
 A(4,3)=3*m*g/(4*m+14*mc);  
 A(5,1)=0; 
 A(5,2)=(15*m+12*mc)*g/((2*m+7*mc)*ell);  
 A(5,3)=-(9*m+18*mc)*g/((8*m+28*mc)*ell);   
 A(6,1)=0; 
 A(6,2)=-(9*m+18*mc)*g/((2*m+7*mc)*ell);  
 A(6,3)=(15*m+48*mc)*g/((8*m+28*mc)*ell);   
 B=zeros(6,1);
 B(4)=7/(2*m+7*mc);
 B(5)=-9/((4*m+14*mc)*ell);
 B(6)=3/((4*m+14*mc)*ell);
%------
 C=eye(3,6);
 Mr=0.5; Mth1=3/180*pi; Mth2=3/180*pi; MF=1; rho=1;
 Q=diag([1/Mr^2,1/Mth1^2,1/Mth2^2]); R=rho^2*1/MF^2;
 [F,pcl]=opt(A,B,C,Q,R);
 C=diag([100,180/pi,180/pi])*C;
 sys=ss(A-B*F,B,[C;-F],[]);
 t=0:0.05:5;
 x0=[0;th10;th20;0;0;0];
 initial(sys,x0,t), grid
%-----
 xs=[0;0;0;0;0;0];
 us=0;
 sim("nDIP_lq")   
%-----
%eof

LQ制御

投稿日時: 投稿者:

LQ制御…Homework

[1]　可制御な制御対象

\displaystyle{(1)\quad %\left\{\begin{array}{ll} \dot{x}(t)=Ax(t)+Bu(t)\quad (x(t)\in{\bf R}^n,u(t)\in{\bf R}^m)\\ %z(t)=Cx(t)&(z(t)\in{\bf R}^q) %\end{array}\right. }

を安定化する状態フィードバック

\displaystyle{(2)\quad u(t)=-Fx(t) }

の決定法を考えます。一つの方法は，閉ループ系

\displaystyle{(3)\quad %\left\{\begin{array}{l} \dot{x}(t)=(A-BF)x(t) \\ %z(t)=Cx(t) %\end{array}\right. }

の時間応答に関する評価規範として，２次形式評価関数

(4)\quad \boxed{\begin{array}{l} \displaystyle{J=\int_0^\infty (z^T(t)Qz(t)+u^T(t)Ru(t))\,dt\quad (Q>0,R>0)}\\ \displaystyle{z(t)=Cx(t)\quad (z(t)\in{\bf R}^q)} \end{array}}

を設定し，これを最小化する問題を解くことです。ただし、(A,C)は可観測対とします。これによる状態フィードバックのゲイン行列Fは，リッカチ方程式

\displaystyle{(5)\quad {\boxed{\Pi A+A^T\Pi-\Pi BR^{-1}B^T\Pi+C^TQC=0}} }

の解\Pi>0を用いて，次式で与えられます。

\displaystyle{(6)\quad {\boxed{F=R^{-1}B^T\Pi}} }

この証明はNoteに示しています。

[2]　以下では、代表的な２次系に対して、評価関数を設定して、ゲイン行列Fを求めてみます。その際、リッカチ方程式は４つの解候補を持ちますが、\Pi>0の条件、すなわち

\displaystyle{(7)\quad \left[\begin{array}{cc} \pi_1 & \pi_3 \\ \pi_3 & \pi_2 \end{array}\right]>0\Leftrightarrow \pi_1>0,\ \pi_1\pi_2-\pi_3^2>0 }

を用いて（\pi_2>0）、解を１つに絞ることに注意してください。

●いま２次系（２重積分器）

\displaystyle{(8)\quad \boxed{\left[\begin{array}{c} \dot{x}_1(t) \\ \dot{x}_2(t) \end{array}\right] = \left[\begin{array}{cc} 0 & 1 \\ 0 & 0 \end{array}\right] \left[\begin{array}{c} x_1(t) \\ x_2(t) \end{array}\right] + \left[\begin{array}{c} 0 \\ 1 \end{array}\right] u(t)} }

を安定化する状態フィードバックu(t)=-f_1x_1(t)-f_2x_2(t)を，評価関数

\displaystyle{(9)\quad J=\int_0^\infty (x_1^2(t)+x_2^2(t)+u^2(t))\,dt }

を最小にするように求めると

\displaystyle{(10)\quad \boxed{ f_1=1,\quad f_2=\sqrt{3}\right) }}

となります。実際、リッカチ方程式

\displaystyle{(11)\quad \begin{array}{l} \left[\begin{array}{cc} \pi_1 & \pi_3 \\ \pi_3 & \pi_2 \end{array}\right] \left[\begin{array}{cc} 0 & 1 \\ 0 & 0 \end{array}\right] + \left[\begin{array}{cc} 0 & 0 \\ 1 & 0 \end{array}\right] \left[\begin{array}{cc} \pi_1 & \pi_3 \\ \pi_3 & \pi_2 \end{array}\right]\\ - \left[\begin{array}{cc} \pi_1 & \pi_3 \\ \pi_3 & \pi_2 \end{array}\right] \left[\begin{array}{c} 0 \\ 1 \end{array}\right] \left[\begin{array}{cc} 0 & 1 \end{array}\right] \left[\begin{array}{cc} \pi_1 & \pi_3 \\ \pi_3 & \pi_2 \end{array}\right] + \left[\begin{array}{cc} 1 & 0 \\ 0 & 1 \end{array}\right]\\ = \left[\begin{array}{cc} 0 & 0 \\ 0 & 0 \end{array}\right] \end{array} }

を要素ごとに整理して

\displaystyle{(12)\quad \left\{ \begin{array}{l} -\pi_3^2+1=0\\ \pi_1-\pi_2\pi_3=0\\ 2\pi_3-\pi_2^2+1=0 \end{array} \right. }

を得る。これは，まず第1式より\pi_3が２つ，つぎに第3式より\pi_2が２つ，さらに第2式より\pi_1が１つ定まり，つぎのように４組の解をもつ。すなわち

\displaystyle{(13)\quad \left\{\begin{array}{llll} \pi_3= 1 & \Rightarrow \left\{\begin{array}{l} \pi_2= \sqrt{3}\\ \pi_2=-\sqrt{3} \end{array}\right. & \left.\begin{array}{l} \Rightarrow\pi_1= \sqrt{3}\\ \Rightarrow\pi_1=-\sqrt{3} \end{array}\right. & \left.\begin{array}{ll} (*) &○\\ (**) &× \end{array}\right. \\ \pi_3=-1 & \Rightarrow \left\{\begin{array}{llll} \pi_2= j \\ \pi_2=-j \end{array}\right. & \hspace{-3mm} \left.\begin{array}{l} \Rightarrow\pi_1=-j\\ \Rightarrow\pi_1=j \end{array}\right. & \left.\begin{array}{ll} (***) &×\\ (****) &× \end{array}\right. \end{array}\right. }

ここで，(*)だけが，\pi_1,\pi_2>0,\ \pi_1\pi_2-\pi_3^2>0を満たします。したがって

\displaystyle{(14)\quad \left[\begin{array}{cc} f_1 & f_2 \end{array}\right]= \left[\begin{array}{cc} 0 & 1 \end{array}\right] \left[\begin{array}{cc} \pi_1 & \pi_3 \\ \pi_3 & \pi_2 \end{array}\right]= \left[\begin{array}{cc} \pi_3 & \pi_2 \end{array}\right]= \left[\begin{array}{cc} 1 & \sqrt{3} \end{array}\right] }

例題1　２次系（無定位系）

\displaystyle{(15)\quad \boxed{\left\{\begin{array}{l} %\underbrace{ \left[\begin{array}{c} \dot{x}_1(t) \\ \dot{x}_2(t) \end{array}\right] %}_{\dot{x}(t)} = %\underbrace{ \left[\begin{array}{cc} 0 & 1 \\ 0 & -2\zeta\omega_n \end{array}\right] %}_{A} %\underbrace{ \left[\begin{array}{c} x_1(t) \\ x_2(t) \end{array}\right] + %\underbrace{ \left[\begin{array}{c} 0 \\ \omega_n^2 \end{array}\right] %}_{B} \\ y(t)= %\underbrace{ \left[\begin{array}{cc} 1 & 0 \end{array}\right] %}_{C} %\underbrace{ \left[\begin{array}{c} x_1(t) \\ x_2(t) \end{array}\right] \end{array}\right.} }

を安定化する状態フィードバックu(t)=-f_1x_1(t)-f_2x_2(t)を，評価関数

\displaystyle{(16)\quad J=\int_0^\infty (q^2y^2(t)+r^2u^2(t))\,dt }

を最小にするように求めると

\displaystyle{(17)\quad \boxed{ f_1=\frac{q}{r},\quad f_2=\frac{2}{\omega_n}\left(-\zeta+\sqrt{\zeta^2+\frac{1}{2}\frac{q}{r}}\right) }}

また

\displaystyle{(18)\quad J=\int_0^\infty (q_1^2x_1^2(t)+q_2^2x_2^2(t)+r^2u^2(t))\,dt }

を最小にするように求めると

\displaystyle{(19)\quad \boxed{\left\{\begin{array}{l} f_1=\frac{q_1}{r} \\ f_2=\frac{2}{\omega_n}\left(-\zeta+\sqrt{\zeta^2+\frac{1}{2}\frac{q_1}{r}+ \left(\frac{q_2}{r}\right)^2\left(\frac{\omega_n}{2}\right)^2 }\right) \end{array}\right.} }

例題2　２次系

\displaystyle{(20)\quad \boxed{\left\{\begin{array}{l} %\underbrace{ \left[\begin{array}{c} \dot{x}_1(t) \\ \dot{x}_2(t) \end{array}\right] %}_{\dot x} = %\underbrace{ \left[\begin{array}{cc} 0 & 1 \\ -\omega_n^2 & -2\zeta\omega_n \end{array}\right] %}_{A} %\underbrace{ \left[\begin{array}{c} x_1(t) \\ x_2(t) \end{array}\right] %}_{x} + %\underbrace{ \left[\begin{array}{c} 0 \\ \omega_n^2 \end{array}\right] %}_{B} u(t) \\ y(t)= %\underbrace{ \left[\begin{array}{cc} 1 & 0 \end{array}\right] %}_{C} %\underbrace{ \left[\begin{array}{c} x_1(t) \\ x_2(t) \end{array}\right] \end{array}\right.} }

を安定化する状態フィードバックu(t)=-f_1x_1(t)-f_2x_2(t)を，評価関数

\displaystyle{(16)\quad J=\int_0^\infty (q^2y^2(t)+r^2u^2(t))\,dt }

を最小にするように求めると

\displaystyle{(21)\quad \boxed{\left\{\begin{array}{l} f_1=-1+\sqrt{1+\left(\frac{q}{r}\right)^2}\\ f_2=-\frac{2}{\omega_n}\left(-\zeta+\sqrt{\zeta^2-\frac{1}{2}+\frac{1}{2}\sqrt{1+\left(\frac{q}{r}\right)^2}}\right) \end{array}\right.} }

また

\displaystyle{(18)\quad J=\int_0^\infty (q_1^2x_1^2(t)+q_2^2x_2^2(t)+r^2u^2(t))\,dt }

を最小にするように求めると

\displaystyle{(22)\quad \boxed{\left\{\begin{array}{l} f_1=-1+\sqrt{1+\left(\frac{q_1}{r}\right)^2}\\ f_2=-\frac{2}{\omega_n}\left(-\zeta+\sqrt{\zeta^2-\frac{1}{2}+\frac{1}{2}\sqrt{1+\left(\frac{q_1}{r}\right)^2}+\left(\frac{q_2}{r}\right)^2\left(\frac{\omega_n}{2}\right)^2}\right) \end{array}\right.} }

[3]　計算機でリッカチ方程式解くには、ハミルトン行列と呼ばれる

\displaystyle{(23)\quad \boxed{M=\left[\begin{array}{cc} A & -BR^{-1}B^T \\ C^TQC & -A^T \end{array}\right]} }

を考えます。このハミルトン行列の固有値は、実軸に対称ばかりでなく、虚軸にも対称となるという性質を持っています。これらのうち安定な固有値と対応する固有ベクトルを、次のように求めます。

\displaystyle{(24)\quad \begin{array}{l} \underbrace{ \left[\begin{array}{cc} A & -BR^{-1}B^T \\ -C^TQC & -A^T \end{array}\right]}_{M(2n\times 2n)} \underbrace{ \left[\begin{array}{c} V_1 \\ V_2 \end{array}\right]}_{V^-(2n\times n)}\\ = \underbrace{ \left[\begin{array}{c} V_1 \\ V_2 \end{array}\right]}_{V^-(2n\times n)} \underbrace{ {\rm diag}\{\lambda_1,\cdots,\lambda_n\} }_{\Lambda^-(n\times n)} \end{array} }

これから

\displaystyle{(25)\quad \boxed{\Pi=V_2V_1^{-1}} }

のように求められます。

●ハミルトン行列を経由してリッカチ方程式解くためのコードは次のようになります。

MATLAB 
%opt.m
%---------------------------------------
 function [F,p]=opt(A,B,C,Q,R)
%---------------------------------------
 n=size(A,1);
 w2=R\B';
 [v,p]=eig([A -B*w2;-C'*Q*C -A']);
 p=diag(p);
 [dummy,index]=sort(real(p));
 p=p(index(1:n));
 x=v(1:n,index(1:n));
 y=v(n+1:n+n,index(1:n));
 X=real(y/x);
 F=w2*X;
%---------------------------------------
%eof
SCILAB 
function [F,p]=opt(A,B,C,Q,R)
 w2=R\B';
 [v,p]=spec([A -B*w2;-C'*Q*C -A']); p=diag(p);
 [dummy,index]=gsort(real(p));
 n=size(A,1); j=index(n+1:n+n);
 p=p(j); V1=v(1:n,j); V2=v(n+1:n+n,j);
 X=real(V2/V1); F=w2*X;
endfunction

MATLABでは、関数lqrがリッカチ方程式解くために準備されています。

演習A52…Flipped Classroom

\displaystyle{ (26)\ %\underbrace{ \left[\begin{array}{c} \dot{x}_1(t) \\ \dot{x}_2(t) \end{array}\right] %}_{\dot{x}(t)} = %\underbrace{ \left[\begin{array}{cc} 0 & 1 \\ 0 & -1 \end{array}\right] %}_{A} %\underbrace{ \left[\begin{array}{c} x_1(t) \\ x_2(t) \end{array}\right] %}_{x(t)} + %\underbrace{ \left[\begin{array}{c} 0 \\ 1 \end{array}\right] %}_{B} u(t) }

\displaystyle{ (27)\ %\underbrace{ \left[\begin{array}{c} \dot{x}_1(t) \\ \dot{x}_2(t) \end{array}\right] %}_{\dot{x}(t)} = %\underbrace{ \left[\begin{array}{cc} 0 & 1 \\ -1 & 0 \end{array}\right] %}_{A} %\underbrace{ \left[\begin{array}{c} x_1(t) \\ x_2(t) \end{array}\right] %}_{x(t)} + %\underbrace{ \left[\begin{array}{c} 0 \\ 1 \end{array}\right] %}_{B} u(t) }

Note A52-1 行列による微分　

いま、任意の行列Xの(i,j)要素をx_{ij}=[X]_{ij}で表すとき、スカラ関数fを行列変数Xの各要素で微分して得られる行列を\displaystyle{[\frac{\partial f}{\partial X}]_{ij}=\frac{\partial f}{\partial x_{ij}}}で定義します。このとき、行列のトレースについて、次が成り立ちます。

\displaystyle{(1)\ {\rm tr}AB={\rm tr}BA}
\displaystyle{(2)\ \frac{\partial}{\partial X}{\rm tr}AXB=A^TB^T}
\displaystyle{(3)\ \frac{\partial}{\partial X}{\rm tr}AX^TB=BA}
\displaystyle{(4)\ \frac{\partial}{\partial X}{\rm tr}AXBX^T=A^TXB^T+AXB}

実際、

\displaystyle{(1)\ \sum_{i}[AB]_{ii}=\sum_{i}\sum_{j}a_{ij}b_{ji} =\sum_{j}\sum_{i}b_{ji}a_{ij}=\sum_{j}[BA]_{jj}}

\displaystyle{(2)\ [\frac{\partial}{\partial X}{\rm tr}AXB]_{ij} =\frac{\partial}{\partial x_{ij}}\sum_{k}[AXB]_{kk} =\frac{\partial}{\partial x_{ij}}\sum_{k}\sum_{i,j}a_{ki}x_{ij}b_{jk}}
\displaystyle{=\sum_{k}b_{jk}a_{ki}=[BA]_{ji}=[A^TB^T]_{ij}}

\displaystyle{(3)\ [\frac{\partial}{\partial X}{\rm tr}AX^TB]_{ij} =\frac{\partial}{\partial x_{ij}}\sum_{k}[AX^TB]_{kk} =\frac{\partial}{\partial x_{ij}}\sum_{k}\sum_{i,j}a_{ki}x_{ji}b_{jk}}
\displaystyle{=\sum_{k}b_{ik}a_{kj}=[BA]_{ij}}

\displaystyle{(4)\ \frac{\partial}{\partial X}{\rm tr}AXBX^T =\frac{\partial}{\partial X}{\rm tr}(AXB)X^T +\frac{\partial}{\partial X}{\rm tr}AX(BX^T)}
\displaystyle{=AXB+A^TXB^T}

Note A52-2 LQ制御問題の解法　

可制御かつ可観測なn次系

\displaystyle{(1)\quad \dot{x}(t)=Ax(t)+Bu(t),\ y(t)=Cx(t) }

に対する状態フィードバック

\displaystyle{(2)\quad u(t)=-Fx(t) }

による閉ループ系

\displaystyle{(3)\quad \begin{array}{l} \dot{x}(t)=(A-BF)x(t),\ z(t)=Cx(t)\\ A_F=A-BF:\ stable\ matrix \end{array} }

に対して、評価関数

\displaystyle{(4)\quad J=\int_0^\infty(z^T(t)Qz(t)+u^T(t)Ru(t))\,dt }

を最小化するようにFを決める問題を考えます。

閉ループ系における状態の振る舞いは次式で与えられます。

\displaystyle{(5)\quad x(t)=\exp({A_Ft})x(0) }

ここで、１次系の場合は初期状態はx(0)\ne0であればよかったのですが、一般の場合はインパルス応答となるようにBの列ベクトルB^{(i)}を考えます。各インパルス応答

\displaystyle{(6)\quad x^{(i)}(t)=\exp({A_Ft})B^{(i)} }

に対する評価関数Jの総和は

(7)\quad \begin{array}{l} \displaystyle{\sum\int_0^\infty x^{(i)}\,^T(t)(C^TQC+F^TRF)x^{(i)}(t)\,dt}\\ \displaystyle{=\sum B^{(i)}\,^T\underbrace{\int_0^\infty \exp(A_F^Tt)(C^TQC+F^TRF)\exp(A_Ft)\,dt}_{\Pi}B^{(i)}}\\ \displaystyle{=\sum B^{(i)}\,^T\Pi B^{(i)}={\rm tr}\ \Pi BB^T} \end{array} }

と書けます。いま\Gammaをラグランジュの未定定数として

\displaystyle{(8)\quad J'={\rm tr}\ \Pi x(0)x^T(0)+{\rm tr}\ ((\Pi A_F+A_F^T\Pi+C^TQC+F^TRF)\Gamma) }

を最小化する問題を考えます。ここで、制約条件は、リャプノフ方程式と呼ばれる

\displaystyle{(9)\quad \Pi A_F+A_F^T\Pi+C^TQC+F^TRF=0 }

ですが、\Pi>0はA_Fが安定行列を意味することに注意します。

そこで、必要条件として次を得ます。

\displaystyle{(11)\quad \frac{\partial J'}{\partial\Pi}&=&BB^T+\Gamma A_F^T+A_F\Gamma=0\Rightarrow \Gamma>0 }
\displaystyle{(12)\quad \frac{\partial J'}{\partial F}&=&2(RF-B^T\Pi)\Gamma=0\Rightarrow F=R^{-1}B^T\Pi }
\displaystyle{(13)\quad \frac{\partial J'}{\partial\Gamma}&=&\Pi A_F+A_F^T\Pi+C^TQC+F^TRF=0 }

ここで、第２式から得られるF=R^{-1}B^T\Piを第３式に代入して

\displaystyle{(14)\quad \begin{array}{l} \Pi (A-BR^{-1}B^T\Pi)+(A-BR^{-1}B^T\Pi)^T\Pi+C^TQ\nonumber\\ +(R^{-1}B^T\Pi)^TRR^{-1}B^T\Pi=0 \end{array} }

すなわち、リッカチ方程式と呼ばれる\Piの行列方程式

\displaystyle{(15)\quad \Pi A+A^T\Pi-\Pi BR^{-1}B^T\Pi+C^TQC=0 }

を得ます。これから\Pi>0を求めて、Fは

\displaystyle{(16)\quad F=R^{-1}B^T\Pi }

のように得られます。このような制御方式をLQ制御と呼びます。

一方、十分性の議論は次のように行われます。まず、被積分項は次のように表すことができます。

\displaystyle{(17)\quad \begin{array}{lll} &&y^TQy+u^TRu=(u+R^{-1}B^T\Pi x)^TR(u+R^{-1}B^T\Pi x)\nonumber\\ &&-\underbrace{\frac{d}{dt}x^T\Pi x}_{2x^T\Pi\dot{x}} \end{array} }

実際、右辺に\dot{x}=Ax+Buを代入し、リッカチ方程式を用いると

\displaystyle{(18)\quad \begin{array}{lll} &&{\rm RHS}=u^TRu+2x^T\Pi Bu+x^T\Pi BR^{-1}B^T\Pi x-2x^T\Pi(Ax+Bu)\nonumber\\ &&=u^TRu+x^T(\Pi A+A^T\Pi+C^TQC)x-2x^T\Pi Ax={\rm LHS} \end{array} }

したがって、上記の両辺を積分して

\displaystyle{(19)\quad J=\int_0^\infty (u+R^{-1}B^T\Pi x)^TR(u+R^{-1}B^T\Pi x)\,dt-\left[x^T\Pi x\right]_0^\infty }

を得ます。ここで、x(t)=\exp(A_Ft)x(0)\rightarrow 0\ (t\rightarrow\infty)を前提とするので

\displaystyle{(20)\quad J=\int_0^\infty (u+R^{-1}B^T\Pi x)^TR(u+R^{-1}B^T\Pi x)\,dt+x^T(0)\Pi x(0) }

を得ます。これからu=-R^{-1}B^T\Pi xが評価関数を最小化することが分かります。

Note A53-3a 例題１(17)の導出　
　
リッカチ方程式

\displaystyle{(1)\quad \begin{array}{l} \left[\begin{array}{cc} \pi_1 & \pi_3 \\ \pi_3 & \pi_2 \end{array}\right] \left[\begin{array}{cc} 0 & 1 \\ 0 & -2\zeta\omega_n \end{array}\right] + \left[\begin{array}{cc} 0 & 0 \\ 1 & -2\zeta\omega_n \end{array}\right] \left[\begin{array}{cc} \pi_1 & \pi_3 \\ \pi_3 & \pi_2 \end{array}\right]\\ - \left[\begin{array}{cc} \pi_1 & \pi_3 \\ \pi_3 & \pi_2 \end{array}\right] \left[\begin{array}{c} 0 \\ \omega_n^2 \end{array}\right] r^{-2} \left[\begin{array}{cc} 0 & \omega_n^2 \end{array}\right] \left[\begin{array}{cc} \pi_1 & \pi_3 \\ \pi_3 & \pi_2 \end{array}\right]\\ + \left[\begin{array}{c} 1 \\ 0 \end{array}\right] q^{2} \left[\begin{array}{cc} 1 & 0 \end{array}\right] % \left[\begin{array}{cc} % q_1^2 & 0 \\ % 0 & q_2^2 % \end{array}\right] = \left[\begin{array}{cc} 0 & 0 \\ 0 & 0 \end{array}\right] \end{array}} }

を要素ごとに整理して

\displaystyle{(2)\quad \left\{ \begin{array}{l} -r^{-2}\omega_n^4\pi_3^2+q^2=0\\ \pi_1-2\zeta\omega_n\pi_3-r^{-2}\omega_n^4\pi_2\pi_3=0\\ 2\pi_3-4\zeta\omega_n\pi_2-r^{-2}\omega_n^4\pi_2^2=0 \end{array}\right.} }

を得る。まず，第１式より\pi_3が

\displaystyle{(3)\quad \pi_3=\pm r\omega_n^{-2}q }

と求まる。つぎに，第３式より\pi_2は

\displaystyle{(4)\quad \pi_2= %\frac{-b\pm\sqrt{b^2-ac}}{a} r^2\omega_n^{-4}(-2\zeta\omega_n\pm\sqrt{(2\zeta\omega_n)^2\pm 2r^{-2}\omega_n^4r\omega_n^{-2}q}) }

となるが，\pi_2>0より

\displaystyle{(5)\quad \begin{array}{l} \pi_2=r^2\omega_n^{-3}(-2\zeta+\sqrt{4\zeta^2+ 2r^{-1}q})\\ \pi_3=r\omega_n^{-2}q \end{array} }

でなければならない。さらに，第２式より\pi_1は

\displaystyle{(6)\quad \begin{array}{l} \pi_1=(2\zeta\omega_n+r^{-2}\omega_n^4r^2\omega_n^{-3}(-2\zeta+\sqrt{4\zeta^2+ 2r^{-1}q}))r\omega_n^{-2}q\\ =r\omega_n^{-1}q\sqrt{4\zeta^2+ 2r^{-1}q \end{array} }

となる（\pi_1>0）。すなわち(1)の解として

\displaystyle{(7)\quad \left\{\begin{array}{l} \pi_1=r\omega_n^{-1}q\sqrt{2r^{-1}q+4\zeta^2}}\\ \pi_2=r^2\omega_n^{-3}(-2\zeta+\sqrt{2r^{-1}q+4\zeta^2})}\\ \pi_3=r\omega_n^{-2}q} \end{array}\right.} }

を得ます（このとき\pi_1\pi_2-\pi_3^2>0も満足されます）。したがって

\displaystyle{(8)\quad \begin{array}{l} \left[\begin{array}{cc} f_1 & f_2 \end{array}\right]=r^{-2} \left[\begin{array}{cc} 0 & \omega_n^2 \end{array}\right] \left[\begin{array}{cc} \pi_1 & \pi_3 \\ \pi_3 & \pi_2 \end{array}\right]=r^{-2}\omega_n^2 \left[\begin{array}{cc} \pi_3 & \pi_2 \end{array}\right]\\ = \left[\begin{array}{cc} \frac{q}{r} & \frac{2}{\omega_n}\left(-\zeta+\sqrt{\zeta^2+\frac{1}{2}\frac{q}{r}}\right) \end{array}\right] \end{array}} }

Note A53-3b 例題１(19)の導出

リッカチ方程式

\displaystyle{(1)\quad \begin{array}{l} \left[\begin{array}{cc} \pi_1 & \pi_3 \\ \pi_3 & \pi_2 \end{array}\right] \left[\begin{array}{cc} 0 & 1 \\ 0 & -2\zeta\omega_n \end{array}\right] + \left[\begin{array}{cc} 0 & 0 \\ 1 & -2\zeta\omega_n \end{array}\right] \left[\begin{array}{cc} \pi_1 & \pi_3 \\ \pi_3 & \pi_2 \end{array}\right]\\ - \left[\begin{array}{cc} \pi_1 & \pi_3 \\ \pi_3 & \pi_2 \end{array}\right] \left[\begin{array}{c} 0 \\ \omega_n^2 \end{array}\right] r^{-2} \left[\begin{array}{cc} 0 & \omega_n^2 \end{array}\right] \left[\begin{array}{cc} \pi_1 & \pi_3 \\ \pi_3 & \pi_2 \end{array}\right]\\ + \left[\begin{array}{cc} q_1^2 & 0 \\ 0 & q_2^2 \end{array}\right] = \left[\begin{array}{cc} 0 & 0 \\ 0 & 0 \end{array}\right] \end{array}} }

を要素ごとに整理して、上と同様にして導出されます。

Note A52-3c 例題２(21)の導出　

リッカチ方程式

\displaystyle{(1)\quad \begin{array}{ll} \left[\begin{array}{cc} \pi_1 & \pi_3 \\ \pi_3 & \pi_2 \end{array}\right] \left[\begin{array}{cc} 0 & 1 \\ -\omega_n^2 & -2\zeta\omega_n \end{array}\right] + \left[\begin{array}{cc} 0 & -\omega_n^2 \\ 1 & -2\zeta\omega_n \end{array}\right] \left[\begin{array}{cc} \pi_1 & \pi_3 \\ \pi_3 & \pi_2 \end{array}\right]\\ - \left[\begin{array}{cc} \pi_1 & \pi_3 \\ \pi_3 & \pi_2 \end{array}\right] \left[\begin{array}{c} 0 \\ \omega_n^2 \end{array}\right] r^{-2} \left[\begin{array}{cc} 0 & \omega_n^2 \end{array}\right] \left[\begin{array}{cc} \pi_1 & \pi_3 \\ \pi_3 & \pi_2 \end{array}\right]\\ + \left[\begin{array}{c} 1 \\ 0 \end{array}\right] q^2 \left[\begin{array}{cc} 1 & 0 \end{array}\right] = \left[\begin{array}{cc} 0 & 0 \\ 0 & 0 \end{array}\right] \end{array}}

を要素ごとに整理して

\displaystyle{(2)\quad \left\{\begin{array}{l} -2\omega_n^2\pi_3-r^{-2}\omega_n^4\pi_3^2+q^2=0 \\ \pi_1-2\zeta\omega_n\pi_3-\omega_n^2\pi_2-r^{-2}\omega_n^4\pi_2\pi_3=0 \\ 2\pi_3-4\zeta\omega_n\pi_2-r^{-2}\omega_n^4\pi_2^2=0 \end{array}\right.

を得る。まず，第１式より\pi_3が

\displaystyle{(3)\quad \pi_3=r^2\omega_n^{-2}(-1\pm\sqrt{1+r^{-2}q^2}) }

と求まる。つぎに，第３式より\pi_2は

\displaystyle{(4)\quad \begin{array}{l} \pi_2= %\frac{-b\pm\sqrt{b^2-ac}}{a} a=r^{-2}\omega_n^4, b=2\zeta\omega_n, c=-2\pi_3 r^2\omega_n^{-4}(-2\zeta\omega_n\pm\sqrt{(2\zeta\omega_n)^2\pm 2r^{-2}\omega_n^4r^2\omega_n^{-2}(-1\pm\sqrt{1+r^{-2}q^2})})\\ =r^2\omega_n^{-3}(-2\zeta\pm\sqrt{4\zeta^2\pm 2(-1\pm\sqrt{1+r^{-2}q^2})}) \end{array}} }

となるが，\pi_2>0より

\displaystyle{(5)\quad \begin{array}{l} \pi_2=r^2\omega_n^{-3}(-2\zeta+\sqrt{4\zeta^2+ 2(-1+\sqrt{1+r^{-2}q^2})})}\\ \pi_3=r^2\omega_n^{-2}(-1+\sqrt{1+r^{-2}q^2}) \end{array} }

でなければならない。さらに，第２式より\pi_1は

\displaystyle{(6)\quad \begin{array}{l} \pi_1=(2\zeta\omega_n+r^{-2}\omega_n^4\pi_2)\pi_3+\omega_n^2\pi_2 \\ =(2\zeta\omega_n+r^{-2}\omega_n^4r^2\omega_n^{-3}(-2\zeta+\sqrt{4\zeta^2+ 2(-1+\sqrt{1+r^{-2}q^2})})})\\ \times r^2\omega_n^{-2}(-1+\sqrt{1+r^{-2}q^2})\\ +\omega_n^2r^2\omega_n^{-3}(-2\zeta+\sqrt{4\zeta^2+ 2(-1+\sqrt{1+r^{-2}q^2})})}\\ =r^2\omega_n^{-1}(2\zeta+(-2\zeta+\sqrt{4\zeta^2+ 2(-1+\sqrt{1+r^{-2}q^2})})})(-1+\sqrt{1+r^{-2}q^2})\\ +r^2\omega_n^{-1}(-2\zeta+\sqrt{4\zeta^2+ 2(-1+\sqrt{1+r^{-2}q^2})})}\\ =r^2\omega_n^{-1}\sqrt{4\zeta^2+ 2(-1+\sqrt{1+r^{-2}q^2})}(-1+\sqrt{1+r^{-2}q^2})\\ +r^2\omega_n^{-1}(-2\zeta+\sqrt{4\zeta^2+2(-1+\sqrt{1+r^{-2}q^2})})}\\ =r^2\omega_n^{-1}(-2\zeta+\sqrt{1+r^{-2}q^2}\sqrt{4\zeta^2-2+2\sqrt{1+r^{-2}q^2}}) \end{array} }

となる（\pi_1>0）。すなわち(1)の解として

\displaystyle{(7)\quad \left\{\begin{array}{l} \pi_1=r^2\omega_n^{-1}(-2\zeta+\sqrt{1+r^{-2}q^2} \sqrt{4\zeta^2-2+2\sqrt{1+r^{-2}q^2}}) \\ \pi_2=r^2\omega_n^{-3}(-2\zeta+\sqrt{4\zeta^2-2+2\sqrt{1+r^{-2}q^2}}) \\ \pi_3=r^2\omega_n^{-2}(-1+\sqrt{1+r^{-2}q^2}) \end{array}\right.}

を得ます（このとき\pi_1\pi_2-\pi_3^2>0も満足されます）。したがって

\displaystyle{(8)\quad \begin{array}{l} \left[\begin{array}{cc} f_1 & f_2 \end{array}\right]=r^{-2} \left[\begin{array}{cc} 0 & \omega_n^2 \end{array}\right] \left[\begin{array}{cc} \pi_1 & \pi_3 \\ \pi_3 & \pi_2 \end{array}\right]=r^{-2}\omega_n^2 \left[\begin{array}{cc} \pi_3 & \pi_2 \end{array}\right]\\ = \left[\begin{array}{cc} -1+\sqrt{1+\left(\frac{q}{r}\right)^2} & -\frac{2}{\omega_n}\left(-\zeta+\sqrt{\zeta^2-\frac{1}{2}+\frac{1}{2}\sqrt{1+\left(\frac{q}{r}\right)^2}}\right) \end{array}\right] \end{array}} }

Note A52-3d 例題２(22)の導出　

リッカチ方程式

\displaystyle{(1)\quad \begin{array}{l} \left[\begin{array}{cc} \pi_1 & \pi_3 \\ \pi_3 & \pi_2 \end{array}\right] \left[\begin{array}{cc} 0 & 1 \\ -\omega_n^2 & -2\zeta\omega_n \end{array}\right] + \left[\begin{array}{cc} 0 & -\omega_n^2 \\ 1 & -2\zeta\omega_n \end{array}\right] \left[\begin{array}{cc} \pi_1 & \pi_3 \\ \pi_3 & \pi_2 \end{array}\right]\\ - \left[\begin{array}{cc} \pi_1 & \pi_3 \\ \pi_3 & \pi_2 \end{array}\right] \left[\begin{array}{c} 0 \\ \omega_n^2 \end{array}\right] r^{-2} \left[\begin{array}{cc} 0 & \omega_n^2 \end{array}\right] \left[\begin{array}{cc} \pi_1 & \pi_3 \\ \pi_3 & \pi_2 \end{array}\right]\\ + \left[\begin{array}{cc} q_1^2 & 0 \\ 0 & q_2^2 \end{array}\right] = \left[\begin{array}{cc} 0 & 0 \\ 0 & 0 \end{array}\right] \end{array}} }

を要素ごとに整理して、上と同様にして導出されます。

補遺　上述の議論では、次についての検討が必要です。

検討事項１　(9)の妥当性

リャプノフ方程式

投稿日時: 投稿者:

リャプノフ方程式…Homework

[1]　対象が平衡状態にあることは線形状態方程式\dot{x}(t)=Ax(t)+Bu(t)において、x=0, u=0を意味します。そこで平衡状態が乱されてx\ne0となる時刻をt=0にとると、線形状態方程式は次式となります。

\displaystyle{(1)\quad \dot{x}(t)=Ax(t)\qquad(x(0)\ne0) }

これに出力方程式

\displaystyle{(2)\quad y(t)=Cx(t) }

を考慮する場合も含めると、漸近安定性の判定法は次のようにまとめられます。

——————————————————————————————————————–
【漸近安定性の定義とその等価な条件】

定義DA:　\forall x(0)\ne 0: x(t)=\exp(At)x(0)\rightarrow 0\quad(t\rightarrow\infty)

条件A0:　\exp(At)\rightarrow 0\quad(t\rightarrow\infty)

条件A1:　{\rm Re}(\lambda_i(A))<0\quad(i=1,\cdots,n)

条件A2:　\exists \Pi>0: \Pi A+A^T\Pi+I_n=0

条件A3:　\exists \Pi>0: \boxed{\Pi A+A^T\Pi+C^TC=0}　ただし、(A,C)は可観測対
——————————————————————————————————————–

ここで、条件A2、条件A3における行列方程式はリャプノフ方程式と呼ばれます。
　＜定義DA\Leftrightarrow条件A0\Leftrightarrow条件A1＞
はすでに示しています。以下では、
　＜定義DA\Rightarrow条件A2\Rightarrow条件A1＞および
　＜定義DA\Rightarrow条件A3\Rightarrow条件A1＞
を示して全部の等価性を主張します。

●＜定義DA\Rightarrow条件A2\Rightarrow条件A1＞　定義DAより、(1)の解x(t)=\exp(At)x(0)の二乗面積は有界となり、

\displaystyle{(3)\quad \int_0^\infty \underbrace{x^T(t)x(t)}_{V(x(t))}dt=x^T(0)\underbrace{\int_0^\infty\exp(A^Tt)\exp(At)dt}_{\Pi}x(0)>0 }

から、\Pi>0が定まります。これは次のようにリャプノフ方程式を満足します。

(4)\quad \begin{array}{l} \Pi A+A^T\Pi\\ \displaystyle{=\int_0^\infty\exp(A^Tt)\exp(At)dt A+A^T \int_0^\infty\exp(A^Tt)\exp(At)dt}\\ \displaystyle{=\int_0^\infty\frac{d}{dt}(\exp(A^Tt)\exp(At))dt=\left[\exp(A^Tt)\exp(At)\right]_0^\infty\\ =\exp(A^T\infty)\underbrace{\exp(A\infty)}_{0}-\exp(A^T0)\underbrace{\exp(A0)}_{I_n}=-I_n}\\ \end{array} }

次に、条件A2が成り立つとします。Aが{\rm Re}(\lambda)\ge0を満たす固有値\lambdaをもつとし、これに対応する固有ベクトルをv\ne0とします。このとき

\displaystyle{(5)\quad \begin{array}{l} 0=v^H(\Pi A+A^T\Pi+I_n)v\\ =v^H\Pi Av+v^HA^T\Pi v+v^Hv\\ =\lambda v^H\Pi v+\lambda^*v^H\Pi v+v^Hv\\ =2\underbrace{{\rm Re}(\lambda)}_{\ge0} \underbrace{v^H\Pi v}_{>0}+\underbrace{v^Hv}_{>0}>0 \end{array} }

となって矛盾。したがって条件A1を得ます。

●安定行列A=\left[\begin{array}{cc} 0 & 1 \\ -2 & -3 \end{array}\right]に対して条件A2を確かめます。リャプノフ方程式は

\displaystyle{(6)\quad \underbrace{ \left[\begin{array}{cc} p_1 & p_3 \\ p_3 & p_2 \end{array}\right] }_{P} \underbrace{ \left[\begin{array}{cc} 0 & 1 \\ -2 & -3 \end{array}\right] }_{A} + \underbrace{ \left[\begin{array}{cc} 0 & -2 \\ 1 & -3 \end{array}\right] }_{A^T} \underbrace{ \left[\begin{array}{cc} p_1 & p_3 \\ p_3 & p_2 \end{array}\right] }_{P} =- \underbrace{ \left[\begin{array}{cc} 1 & 0 \\ 0 & 1 \end{array}\right] }_{I_2} }

となり、次の連立1次方程式に書き直すことができます。

\displaystyle{(7)\quad \left[\begin{array}{ccc} 0 & 0 & -4 \\ 1 & -2 & -3 \\ 0 & -6 & 2 \end{array}\right] \left[\begin{array}{c} p_1 \\ p_2 \\ p_3 \end{array}\right] = \left[\begin{array}{c} -1 \\ 0 \\ -1 \end{array}\right] }

これより、次を得ます。

\displaystyle{(8)\quad P= \left[\begin{array}{cc} 1.25 & 0.25 \\ 0.25 & 0.25 \end{array}\right]>0 }

●＜定義DA\Rightarrow条件A3\Rightarrow条件A1＞　定義DAより、(1),(2)の解y(t)=C\exp(At)x(0)の二乗面積は有界となり、

\displaystyle{(9)\quad \int_0^\infty \underbrace{y^T(t)y(t)}_{V(x(t))}dt=x^T(0)\underbrace{\int_0^\infty\exp(A^Tt)C^TC\exp(At)dt}_{\Pi}x(0)>0 }

から、\Pi>0が定まります。ここで、(A,C)は可観測対の条件が、可観測性グラミアンW_o(t)>0を保証し、\Pi=W_o(\infty)>0が成り立ちます。これは次のようにリャプノフ方程式を満足します。

(10)\quad \begin{array}{l} \Pi A+A^T\Pi\\ \displaystyle{=\int_0^\infty\exp(A^Tt)C^TC\exp(At)dt A+A^T \int_0^\infty\exp(A^Tt)C^TC\exp(At)dt}\\ \displaystyle{=\int_0^\infty\frac{d}{dt}(\exp(A^Tt)C^TC\exp(At))dt=\left[\exp(A^Tt)C^TC\exp(At)\right]_0^\infty\\ =\exp(A^T\infty)C^TC\underbrace{\exp(A\infty)}_{0}-\exp(A^T0)C^TC\underbrace{\exp(A0)}_{I_n}=-C^TC}\\ \end{array} }

次に、条件A3が成り立つとします。Aが{\rm Re}(\lambda)\ge0を満たす固有値\lambdaをもつとし、これに対応する固有ベクトルをv\ne0とします。このとき

\displaystyle{(11)\quad \begin{array}{l} 0=v^H(\Pi A+A^T\Pi+C^TC)v\\ =v^H\Pi Av+v^HA^T\Pi v+v^HC^TCv\\ =\lambda v^H\Pi v+\lambda^*v^H\Pi v+v^HC^TCv\\ =2\underbrace{{\rm Re}(\lambda)}_{\ge0} \underbrace{v^H\Pi v}_{>0}+\underbrace{v^HC^TCv}_{\ge0}\ge0 \end{array} }

となって矛盾。したがって条件A1を得ます。

Note A51 ラウス・フルビッツの安定判別法

次式で表されるn次自由系を考えます。

\displaystyle{(101)\quad \dot{x}(t)=Ax(t) }

この漸近安定性は

\displaystyle{(102)\quad \forall x(0)\ne0: x(t)=\exp(At)x(0)\,\rightarrow\,0\quad(t\rightarrow\infty) }

で定義され，A行列のすべての固有値の実部が負であることが必要十分条件であることはよく知られています。A行列の中に不確かなパラメータを含む場合，漸近安定性を保証する範囲を求めるために，Routh-Hurwitzの安定判別法が有用です。これは，A行列の特性多項式

\displaystyle{(103)\quad {\rm det}(\lambda I_n-A)=\underbrace{a_0}_{1}\lambda^n+a_1\lambda^{n-1}+\cdots+a_{n-1}\lambda+a_n }

の係数a_1,a_2,\cdots,a_nから得られるRouth表

\displaystyle{(104)\quad \boxed{\begin{array}{c|rrrrrr} s^{n} & a_0 & a_2 & a_4 & a_6 & \cdots \\ s^{n-1} & a_1 & a_3 & a_5 & a_7 & \cdots \\ s^{n-2} & \frac{a_1a_2-a_0a_3}{a_1}=b_1 & \frac{a_1a_4-a_0a_5}{a_1}=b_2 & \frac{a_1a_6-a_0a_7}{a_1}=b_3 & \cdots & \cdots \\ s^{n-3} & \frac{b_1a_3-a_1b_2}{b_1}=c_1 & \frac{b_1a_5-a_1b_3}{b_1}=c_2 & \cdots & \cdots & \vdots \\ s^{n-4} & \frac{c_1b_2-b_1c_2}{c_1}=d_1 & \frac{c_1b_3-b_1c_3}{c_1}=d_2 & \cdots & \cdots & \vdots \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\ s^{0} & & & & & \end{array}} }

の第１列の要素R_1=a_1,R_2=b_1,R_3=c_1,R_4=d_1,\cdotsがすべて正(a_0=1)であれば漸近安定，または係数a_1,a_2,a_3,\cdotsがすべて正(a_0=1)かつHurwitz行列式

\displaystyle{(105)\quad \boxed{\begin{array}{l} D_1=a_1 \\ D_2={\rm det}< \left[\begin{array}{cc} a_1 & a_3 \\ a_0 & a_2 \end{array}\right] \\ D_3={\rm det} \left[\begin{array}{ccc} a_1 & a_3 & a_5 \\ a_0 & a_2 & a_4 \\ 0 & a_1 & a_3 \end{array}\right] \\ \vdots \\ D_{n-1}={\rm det} \left[\begin{array}{ccccc} a_1 & a_3 & a_5 & \cdots & a_{2n-3} \\ a_0 & a_2 & a_4 & \cdots & a_{2n-4} \\ 0 & a_1 & a_3 & \cdots & a_{2n-5} \\ 0 & a_0 & a_2 & \cdots & a_{2n-6} \\ 0 & 0 & a_1 & \cdots & a_{2n-7} \\ \vdots & \vdots & \ddots & \ddots & \vdots \\ 0 & 0 & 0 & \cdots & a_{n-1} \\ \end{array}\right] \end{array}} }

がすべて正であれば漸近安定と判定します。両者の間には，次が成り立ちます。

\displaystyle{(106)\quad R_1=\Delta_1,\ R_2=\frac{\Delta_2}{\Delta_1},\cdots,R_n=\frac{\Delta_n}{\Delta_{n-1}} }

●たとえば、n=2,3の場合は、次の条件となります。

\lambda^2+a_1\lambda+a_2の場合は、a_1>0,a_2>0

\lambda^3+a_1\lambda^2+a_2\lambda+a_3の場合は、\displaystyle{a_1>0,a_2>\frac{a_3}{a_1},a_3>0}

証明

●Routh-Hurwitzの安定判別法の時間領域における証明については，P.C.Parksによって，自由系が漸近安定であるための必要十分条件

\displaystyle{(107)\quad \exists P>0: PA+A^TP=-H^TH\le0 }

に基づくものが示されています（ただし，(A,H)は可観測対）。彼は

\displaystyle{(108)\quad A=\left[\begin{array}{cccccc} 0 & 1 & 0 & \cdots & 0\\ 0 & 0 & 1 & \cdots & 0\\ \vdots & \vdots & \vdots & \ddots & \vdots\\ 0 & 0 & 0 & \cdots & 1\\ -a_n & -a_{n-1} & -a_{n-2} & \cdots & -a_1 \end{array}\right] }

を，ある正則行列Tで相似変換した行列

\displaystyle{(109)\quad B=\left[\begin{array}{ccccccc} 0 & 1 & \cdots & 0 & 0 & 0 \\ -b_n & 0 & \cdots & 0 & 0 & 0 \\ 0 & -b_{n-1} & \cdots & 0 & 0 & 0 \\ \vdots & \vdots & \ddots & \vdots & \vdots& \vdots\\ 0 & 0 & \cdots & -b_3 & 0 & 1 \\ 0 & 0 & \cdots & 0 & -b_2 & -b_1 \end{array}\right]=TAT^{-1} }

に対して

\displaystyle{(110)\quad \exists \Pi>0: \Pi B+B^T\Pi=-H^TH\le0 }

を満足する\PiとHを，次のように求めています。

\displaystyle{(111)\quad \Pi=\left[\begin{array}{ccccccc} b_1b_2\cdots b_n & 0 & \cdots & 0 \\ 0 & b_1b_2\cdots b_{n-1} & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots\\ 0 & 0 & \cdots & b_1 \end{array}\right] }

\displaystyle{(112)\quad H=\left[\begin{array}{cccc} 0 & \cdots & 0 & \sqrt{2}b_1 \end{array}\right] }

実際，\Pi Bを計算してみると

\displaystyle{(113)\quad \left[\begin{array}{ccccccc} b_1b_2\cdots b_n & 0 & \cdots & 0 \\ 0 & b_1b_2\cdots b_{n-1} & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots\\ 0 & 0 & \cdots & b_1 \end{array}\right] \left[\begin{array}{ccccccc} 0 & 1 & \cdots & 0 & 0 & 0 \\ -b_n & 0 & \cdots & 0 & 0 & 0 \\ 0 & -b_{n-1} & \cdots & 0 & 0 & 0 \\ \vdots & \vdots & \ddots & \vdots & \vdots& \vdots\\ 0 & 0 & \cdots & -b_3 & 0 & 1 \\ 0 & 0 & \cdots & 0 & -b_2 & -b_1 \end{array}\right] }
\displaystyle{ = \left[\begin{array}{ccccccc} 0 & b_1b_2\cdots b_n & \cdots & 0 & 0 & 0 \\ -b_1b_2\cdots b_n & 0 & \cdots & 0 & 0 & 0 \\ 0 & -b_1b_2\cdots b_{n-1} & \cdots & 0 & 0 & 0 \\ \vdots & \vdots & \ddots & \vdots & \vdots& \vdots\\ 0 & 0 & \cdots & -b_1b_2b_3 & 0 & b_1b_2 \\ 0 & 0 & \cdots & 0 & -b_1b_2 & -b_1^2 \end{array}\right] }

となります。したがって

\displaystyle{(114)\quad &&\Pi B+B^T\Pi= \left[\begin{array}{ccccccc} 0 & 0 & \cdots & 0 \\ 0 & 0 & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots\\ 0 & 0 & \cdots & -2b_1^2 \end{array}\right] =- \underbrace{ \left[\begin{array}{cccc} 0 \\ \vdots \\ 0 \\ \sqrt{2}b_1 \end{array}\right] }_{H^T} \underbrace{ \left[\begin{array}{cccc} 0 & \cdots & 0 & \sqrt{2}b_1 \end{array}\right] }_{H} \nonumber }

が得られます。明らかに，b_n\ne0のとき

\displaystyle{(115)\quad {\rm rank} \left[\begin{array}{ccccccc} B-\lambda I_n \\ H \end{array}\right] = {\rm rank} \left[\begin{array}{ccccccc} -\lambda & 1 & \cdots & 0 & 0 & 0 \\ -b_n & -\lambda & \cdots & 0 & 0 & 0 \\ 0 & -b_{n-1} & \cdots & 0 & 0 & 0 \\ \vdots & \vdots & \ddots & \vdots & \vdots& \vdots\\ 0 & 0 & \cdots & -b_3 & -\lambda& 1 \\ 0 & 0 & \cdots & 0 & -b_2 & -b_1-\lambda \\ 0 & 0 & \cdots & 0 & 0 & \sqrt{2}b_1 \end{array}\right]=n }

となり，(B,H)は可観測対です。また，関係式

\displaystyle{(116)\quad &&b_1=\Delta_1,\ b_2=\frac{\Delta_2}{\Delta_1},\ b_3=\frac{\Delta_3}{\Delta_1\Delta_2},\ b_4=\frac{\Delta_1\Delta_4}{\Delta_2\Delta_3},\ \cdots,\ b_n=\frac{\Delta_{n-3}\Delta_n}{\Delta_{n-2}\Delta_{n-1}} }

が成り立つことが示されており，\Piの正定性からRouth-Hurwitzの安定判別法の妥当性が出ます。

●以下では、２つの行列

\displaystyle{(117)\quad A=\left[\begin{array}{cccccc} 0 & 1 & 0 & \cdots & 0\\ 0 & 0 & 1 & \cdots & 0\\ \vdots & \vdots & \vdots & \ddots & \vdots\\ 0 & 0 & 0 & \cdots & 1\\ -a_n & -a_{n-1} & -a_{n-2} & \cdots & -a_1 \end{array}\right] }

\displaystyle{(118)\quad B=\left[\begin{array}{ccccccc} 0 & 1 & \cdots & 0 & 0 & 0 \\ -b_n & 0 & \cdots & 0 & 0 & 0 \\ 0 & -b_{n-1} & \cdots & 0 & 0 & 0 \\ \vdots & \vdots & \ddots & \vdots & \vdots& \vdots\\ 0 & 0 & \cdots & -b_3 & 0 & 1 \\ 0 & 0 & \cdots & 0 & -b_2 & -b_1 \end{array}\right] }

に対して

\displaystyle{(119)\quad B=TAT^{-1} }

を満足する座標変換行列Tを導出します。ここで

\displaystyle{(120)\quad {\rm det}(\lambda I_n-B)={\rm det}(\lambda I_n-A) =\lambda^n+a_1\lambda^{n-1}+\cdots+a_{n-1\lambda+}a_n }

が成り立ちます。

さて、次の関係式はよく知られています。

\displaystyle{(121)\quad A=V_A\Lambda V_A^{-1} }

ただし

\displaystyle{(122)\quad \Lambda=\left[\begin{array}{ccccccc} \lambda_1 & 0 & \cdots & 0 \\ 0 & \lambda_2 & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots\\ 0 & 0 & \cdots & \lambda_n \end{array}\right] }

\displaystyle{(123)\quad V_A=\left[\begin{array}{ccccccc} 1 & 1 & \cdots & 1 \\ \lambda_1 & \lambda_2 & \cdots & \lambda_n \\ \vdots & \vdots & \ddots & \vdots \\ \lambda_1^{n-1} & \lambda_2^{n-1} & \cdots & \lambda_n^{n-1} \end{array}\right] }

ここで，簡単のために，Aの固有値\{\lambda_1,\lambda_2,\cdots,\lambda_n\}は互いに相異なるものとします。このとき

\displaystyle{(124)\quad B=V_B\Lambda V_B^{-1} }

を満足するV_Bを求めれば，座標変換行列Tは

\displaystyle{(125)\quad T=V_BV_A^{-1} }

のように表されます。

(119)に関する補足

●n=2のとき

\displaystyle{ A=\left[\begin{array}{cccccc} 0 & 1 \\ -a_2 & -a_1 \end{array}\right] }
\displaystyle{ B=\left[\begin{array}{cccccc} 0 & 1 \\ -b_2 & -b_1 \end{array}\right] }

ですから，自明な場合

\displaystyle{ b_1=a_1=\Delta_1,\ b_2=a_2=\frac{\Delta_2}{\Delta_1} }

で，V_B=V_A，すなわちT=I_2となります。

●n=3のとき

\displaystyle{ A=\left[\begin{array}{cccccc} 0 & 1 & 0 \\ 0 & 0 & 1 \\ -a_3 & -a_2 & -a_1 \end{array}\right] }
\displaystyle{ B=\left[\begin{array}{cccccc} 0 & 1 & 0 \\ -b_3 & 0 & 1 \\ 0 & -b_2 & -b_1 \end{array}\right] }

となります。いま

\displaystyle{ \left[\begin{array}{cccccc} 0 & 1 & 0 \\ -b_3 & 0 & 1 \\ 0 & -b_2 & -b_1 \end{array}\right] \left[\begin{array}{c} v_1 \\ v_2 \\ v_3 \end{array}\right]= \lambda \left[\begin{array}{c} v_1 \\ v_2 \\ v_3 \end{array}\right] }

すなわち

\displaystyle{ \left\{\begin{array}{l} v_2=\lambda v_1 \\ v_3=\lambda v_2 +b_3v_1\\ \lambda v_3+b_1v_3+b_2v_2=0 \end{array}\right. }

となるv_1=1,v_2,v_3を求めると

\displaystyle{ \left[\begin{array}{c} v_1 \\ v_2 \\ v_3 \end{array}\right]= \left[\begin{array}{l} 1 \\ \lambda \\ \lambda^2+b_3 \end{array}\right] = \underbrace{ \left[\begin{array}{cccccc} 1 & 0 & 0 \\ 0 & 1 & 0 \\ b_3 & 0 & 1 \end{array}\right]}_{\Gamma} \left[\begin{array}{c} 1 \\ \lambda \\ \lambda^2 \end{array}\right] }

ただし

\displaystyle{ \lambda^3+b_1\lambda^2+(b_2+b_3)\lambda+b_1b_3=0 }

これは

\displaystyle{ \lambda^3+a_1\lambda^2+a_2\lambda+a_3=0 }

と一致しなければならないので

\displaystyle{ b_1=a_1,\ b_2=a_2-\frac{a_3}{a_1},\ b_3=\frac{a_3}{a_1} }

となります。また，Hurwitz行列式を用いて

\displaystyle{ b_1=\Delta_1,\ b_2=\frac{\Delta_2}{\Delta_1},\ b_3=\frac{\Delta_3}{\Delta_1\Delta_2} }

と表されるます。実際

\displaystyle{ \begin{array}{l} b_1=\Delta_1=a_1 \\ b_2=\frac{\Delta_2}{\Delta_1}=\frac{a_1a_2-a_3}{a_1}=a_2-\frac{a_3}{a_1} \\ b_3=\frac{\Delta_3}{\Delta_1\Delta_2}=\frac{\Delta_2a_3}{a_1\Delta_2}=\frac{a_3}{a_1} \end{array} }

以上から，V_B=\Gamma V_A，すなわちT=\Gammaとなります。

●n=4のとき

\displaystyle{ A=\left[\begin{array}{cccccc} 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ -a_4 & -a_3 & -a_2 & -a_1 \end{array}\right] }

\displaystyle{ B=\left[\begin{array}{cccccc} 0 & 1 & 0 & 0 \\ -b_4 & 0 & 1 & 0 \\ 0 & -b_3 & 0 & 1 \\ 0 & 0 & -b_2 & -b_1 \end{array}\right] }

となります。いま

\displaystyle{ \left[\begin{array}{cccccc} 0 & 1 & 0 & 0 \\ -b_4 & 0 & 1 & 0 \\ 0 & -b_3 & 0 & 1 \\ 0 & 0 & -b_2 & -b_1 \end{array}\right] \left[\begin{array}{c} v_1 \\ v_2 \\ v_3 \\ v_4 \end{array}\right]= \lambda \left[\begin{array}{c} v_1 \\ v_2 \\ v_3 \\ v_4 \end{array}\right] }

すなわち

\displaystyle{ \left\{\begin{array}{l} v_2=\lambda v_1 \\ v_3=\lambda v_2 +b_4v_1\\ v_4=\lambda v_3 +b_3v_2\\ \lambda v_4+b_1v_4+b_2v_3=0 \end{array}\right. }

となるv_1=1,v_2,v_3,v_4を求めると

\displaystyle{ \left[\begin{array}{c} v_1 \\ v_2 \\ v_3 \\ v_4 \end{array}\right]= \left[\begin{array}{l} 1 \\ \lambda \\ \lambda^2+b_4 \\ \lambda^3+(b_3+b_4)\lambda \end{array}\right] = \underbrace{ \left[\begin{array}{cccccc} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ b_4 & 0 & 1 & 0 \\ 0 & b_3+b_4 & 0 & 1 \end{array}\right]}_{\Gamma} \left[\begin{array}{c} 1 \\ \lambda \\ \lambda^2 \\ \lambda^3 \end{array}\right] }

ただし

\displaystyle{ \lambda^4+b_1\lambda^3+(b_2+b_3+b_4)\lambda^2+b_1(b_3+b_4)\lambda+b_2b_4=0 }

これは

\displaystyle{ \lambda^4+a_1\lambda^3+a_2\lambda^2+a_3\lambda+a_4=0 }

と一致しなければならないので

\displaystyle{ b_1=a_1,\ b_2=a_2-\frac{a_3}{a_1},\ b_3=\frac{a_3}{a_1}-\frac{a_4}{a_2-\frac{a_3}{a_1}},\ b_4=\frac{a_4}{a_2-\frac{a_3}{a_1}} }

となります。また，Hurwitz行列式を用いて

\displaystyle{ b_1=\Delta_1,\ b_2=\frac{\Delta_2}{\Delta_1},\ b_3=\frac{\Delta_3}{\Delta_1\Delta_2},\ b_4=\frac{\Delta_1\Delta_4}{\Delta_2\Delta_3} }

と表されます。実際

\displaystyle{ \begin{array}{l} b_1=\Delta_1=a_1 \\ b_2=\frac{\Delta_2}{\Delta_1}=\frac{a_1a_2-a_3}{a_1}=a_2-\frac{a_3}{a_1} \\ b_3=\frac{\Delta_3}{\Delta_1\Delta_2}=\frac{\Delta_2a_3-a_1^2a_4}{a_1\Delta_2}=\frac{a_3}{a_1}-\frac{a_4}{a_2-\frac{a_3}{a_1}} \\ b_4=\frac{\Delta_1\Delta_4}{\Delta_2\Delta_3}=\frac{a_1\Delta_3a_4}{\Delta_2\Delta_3}=\frac{a_4}{a_2-\frac{a_3}{a_1}} \end{array} }

以上から，V_B=\Gamma V_A，すなわちT=\Gammaとなります。

●n=5のとき
\displaystyle{ A=\left[\begin{array}{cccccc} 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 1 \\ -a_5 & -a_4 & -a_3 & -a_2 & -a_1 \end{array}\right] }

\displaystyle{ B=\left[\begin{array}{cccccc} 0 & 1 & 0 & 0 & 0 \\ -b_5 & 0 & 1 & 0 & 0 \\ 0 & -b_4 & 0 & 1 & 0 \\ 0 & 0 & -b_3 & 0 & 1 \\ 0 & 0 & 0 & -b_2 & -b_1 \end{array}\right] }

となります。いま

\displaystyle{ \left[\begin{array}{cccccc} 0 & 1 & 0 & 0 & 0 \\ -b_5 & 0 & 1 & 0 & 0 \\ 0 & -b_4 & 0 & 1 & 0 \\ 0 & 0 & -b_3 & 0 & 1 \\ 0 & 0 & 0 & -b_2 & -b_1 \end{array}\right] \left[\begin{array}{c} v_1 \\ v_2 \\ v_3 \\ v_4 \\ v_5 \end{array}\right]= \lambda \left[\begin{array}{c} v_1 \\ v_2 \\ v_3 \\ v_4 \\ v_5 \end{array}\right] }

すなわち

\displaystyle{ \left\{\begin{array}{l} v_2=\lambda v_1 \\ v_3=\lambda v_2 +b_5v_1\\ v_4=\lambda v_3 +b_4v_2\\ v_5=\lambda v_4 +b_3v_3\\ \lambda v_5+b_1v_5+b_2v_4=0 \end{array}\right. }

となるv_1=1,v_2,v_3,v_4,v_5を求めると

\displaystyle{ \left[\begin{array}{c} v_1 \\ v_2 \\ v_3 \\ v_4 \\ v_5 \end{array}\right]= \left[\begin{array}{l} 1 \\ \lambda \\ \lambda^2+b_5 \\ \lambda^3+(b_4+b_5)\lambda \\ \lambda^4+(b_3+b_4+b_5)\lambda^2+b_3b_5 \end{array}\right] }
\displaystyle{ = \underbrace{ \left[\begin{array}{cccccc} 1 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 \\ b_5 & 0 & 1 & 0 & 0 \\ 0 & b_4+b_5 & 0 & 1 & 0 \\ b_3b_5 & 0 & b_3+b_4+b_5 & 0 & 1 \end{array}\right]}_{\Gamma} \left[\begin{array}{c} 1 \\ \lambda \\ \lambda^2 \\ \lambda^3 \\ \lambda^4 \end{array}\right] \nonumber }

ただし

\displaystyle{ \lambda^5+b_1\lambda^4+(b_2+b_3+b_4+b_5)\lambda^3+b_1(b_3+b_4+b_5)\lambda^2+(b_2(b_4+b_5)+b_3b_5)\lambda+b_1b_3b_5=0\nonumber }

これは

\displaystyle{ \lambda^5+a_1\lambda^4+a_2\lambda^3+a_3\lambda^2+a_4\lambda+a_5=0 }

と一致しなければならないので

\displaystyle{ &&b_1=a_1,\ b_2=a_2-\frac{a_3}{a_1},\ b_3=\frac{a_3}{a_1}-\frac{a_4-\frac{a_5}{a_1}}{b_2},\ b_4=\frac{a_4-\frac{a_5}{a_1}}{b_2}-\frac{a_5}{a_1b_3},\ b_5=\frac{a_5}{a_1b_3} \nonumber }

となります。また，Hurwitz行列式を用いて

\displaystyle{ b_1=\Delta_1,\ b_2=\frac{\Delta_2}{\Delta_1},\ b_3=\frac{\Delta_3}{\Delta_1\Delta_2},\ b_4=\frac{\Delta_1\Delta_4}{\Delta_2\Delta_3},\ b_5=\frac{\Delta_2\Delta_5}{\Delta_3\Delta_4} }

と表される。実際

\displaystyle{ \begin{array}{l} b_1=\Delta_1=a_1 \\ b_2=\frac{\Delta_2}{\Delta_1}=\frac{a_1a_2-a_3}{a_1}=a_2-\frac{a_3}{a_1} \\ b_3=\frac{\Delta_3}{\Delta_1\Delta_2}=\frac{\Delta_2a_3-(a_1a_4-a_5)a_1}{a_1\Delta_2}=\frac{a_3}{a_1}-\frac{a_4-\frac{a_5}{a_1}}{b_2} \\ b_4=\frac{\Delta_1\Delta_4}{\Delta_2\Delta_3}=\frac{1}{b_2}(a_4-\frac{a_5}{a_1}-\frac{a_5(a_2-\frac{a_3}{a_1})}{R_3})=\frac{a_4-\frac{a_5}{a_1}}{b_2}-\frac{a_5}{a_1b_3} \\ b_5=\frac{\Delta_2\Delta_5}{\Delta_3\Delta_4}=\frac{\Delta_2\Delta_4a_5}{\Delta_3\Delta_4}=\frac{a_5}{a_1b_3} \end{array} }

以上から，V_B=\Gamma V_A，すなわちT=\Gammaとなる。

●ここまで調べれば、n=iの場合の(119)からn=i+1の場合の(119)を示すことができる思います（あとでやってみます）。

可検出性と可観測性

投稿日時: 投稿者:

可検出性と可観測性…Homework

[1]　制御対象のモデルが

\displaystyle{(1)\quad \left\{\begin{array}{ll} \dot{x}(t)=Ax(t)+Bu(t)&(x(t)\in{\bf R}^n,u(t)\in{\bf R}^m)\\ y(t)=Cx(t)&(y(t)\in{\bf R}^p)\right. \end{array} }

で与えられるとき、これに対する状態オブザーバは、微分方程式

\displaystyle{(2)\quad \dot{\hat{x}}(t)=(A-HC)\hat{x}(t)+Bu(t)+Hy(t),\ \hat{x}(0)=0\\ }

で与えられました。ここで、オブザーバゲインHをA-HCが安定行列になるように選ぶ必要がありました。でもこれはいつでも可能であるわけではありません。すなわちHをA-HCが安定行列になるように選べる場合に状態オブザーバが構成可能となり、このとき可検出性(detectability)が成り立つと言います。

A-HCを安定行列とするためには、双対システム

\displaystyle{(3)\quad \dot{w}(t)=A^Tw(t)+C^Tv(t)}

に対する状態フィードバック

\displaystyle{(4)\quad v(t)=-H^Tw(t)}

による閉ループ系

\displaystyle{(5)\quad \dot{w}(t)=(A^T-C^TH^T)w(t)=(A-HC)^Tw(t)}

を安定化して求めます。ということは、可検出性の条件は双対システムの可安定性と等価になります。すなわち可安定性の条件において

\displaystyle{(6)\quad A\leftarrow A^T,\ B\leftarrow C^T}

のように置き換えて、次が得られます。

【可検出性の定義とその等価な条件】

定義DD:　状態オブザーバを構成可能

条件D1:　{\rm rank}\, \left[\begin{array}{c} C \\ A-\lambda I_n \end{array}\right]=n　(\lambdaはAのすべての不安定固有値)

条件D2:　Cv=0,\ Av=\lambda v \Rightarrow v=0　(\lambdaはAのすべての不安定固有値)

可検出性の判定は行列AとCを用いて行われるので、可検出性が成り立つとき、対(A,C)は可検出対という言い方をします。

可安定性よりも強い条件として可制御性がありましたが、これに対応する概念は可観測性(observability)と言われ、これと等価な条件は次のようにまとめられます。

【可観測性の定義とその等価な条件】

定義DO:　任意有限時間[0,t]上の入出力データu(\tau),y(\tau)\tau\in[0,t]から，初期状態x(0)を一意に決定できる

条件O1:　\displaystyle{\underbrace{\int_0^t \exp(A^T\tau)C^TC\exp(A\tau)\,d\tau}_{W_o(t)}>0 \quad (\forall t>0)}

条件O2:　{\rm rank}\, \underbrace{ \left[\begin{array}{c} C \\ CA \\ \vdots\\ CA^{n-1} \end{array}\right] }_{observability\ matrix} =n

条件O3:　Hを選んで，A-HCの固有値を任意に設定可能

条件O4:　{\rm rank} \left[\begin{array}{c} C \\ A-\lambda I_n \end{array}\right]=n　(\lambdaはAのすべての固有値)

条件O5:　Cv=0,\ Av=\lambda v \Rightarrow v=0　(\lambdaはAのすべての固有値)

条件O6:　(A^T,C^T)は可制御対

可観測性の判定も行列AとCを用いて行われるので、可観測性が成り立つとき、対(A,C)は可観測対という言い方をします。条件O1の積分式を可観測性グラミアン行列、条件O2の行列を可観測性行列と呼びます。また条件O4を満足するAの固有値を\lambdaは可観測固有値、満足しない固有値を不可観測固有値と呼びます。

上で述べたように、可観測性の議論は、可制御性の議論において、AをA^Tに、BをC^Tに置き換えて行えばよいので、次が成り立ちます。

条件O6\Leftrightarrow条件O1\Leftrightarrow条件O2\Leftrightarrow条件O3\Leftrightarrow条件O4\Leftrightarrow条件O5

●＜定義DO\Rightarrow条件O1＞

適当な入力のもとでの出力は次式で表されます。

\displaystyle{(8)\quad y(t)=C\exp(At)x(0)+\int_0^tC\exp(A\tau)Bu(t-\tau)\,d\tau}

ここで、零状態応答は計算できますので、これを出力から引いたz(t)を次のようにおきます。

\displaystyle{(9)\quad \underbrace{y(t)-\int_0^tC\exp(A\tau)Bu(t-\tau)\,d\tau}_{z(t)}=C\exp(At)x(0)}

したがって、入出力データu(\tau),z(\tau)\tau\in[0,t]から，初期状態x(0)を一意に決定できるかが問題となります。

このとき、W_o(t)>0を否定して、矛盾を導きます。

(10)\quad \begin{array}{l} \displaystyle{\overline{(\forall v\ne0:v^TW_o(t)v>0)}}\\ \displaystyle{\Rightarrow \exists v\ne0:w^TW_o(t)v=0}\\ \displaystyle{\Rightarrow \exists v\ne0,\forall \tau\le t: C\exp(A\tau)v=0}\\ \displaystyle{\Rightarrow x(0)=v\Rightarrow z(t)=0} \end{array}

これは、入出力データu(\tau),z(\tau)\tau\in[0,t]から，初期状態x(0)を一意に決定できることにはならないことを示しています。

●＜条件O1\Rightarrow定義DO＞

上と同様に(9)を考えますと、入出力データu(\tau),z(\tau)\tau\in[0,t]から，初期状態x(0)は次式で計算できます。

\displaystyle{(11)\quad x(0)=W_o^{-1}(t)\int_0^t\exp(A^T\tau)C^Tz(\tau)\,d\tau}

実際、左辺は、次のようにx(0)となります。

\displaystyle{(12)\quad (\int_0^t\exp(A^T\tau)C^TC\exp(A\tau)\,d\tau)^{-1}\int_0^t\exp(A^T\tau)C^TC\exp(At)x(0)\,d\tau=x(0) }

以上で、可観測性\LeftrightarrowO1\LeftrightarrowO2が示されました。

Note A43 可安定性と可制御性

●状態フィーバックにより安定化できることを可安定性と言います。

【可安定性の定義とその等価な条件】

定義DS:　状態フィードバックにより安定化可能

条件S1:　{\rm rank}\, \left[\begin{array}{cc} B & A-\lambda I_n \end{array}\right] =n　(\lambdaはAのすべての不安定固有値)

条件S2:　B^Tw=0,\ A^Tw=\lambda w \Rightarrow w=0　(\lambdaはAのすべての不安定固有値)

可安定性の判定は行列AとBを用いて行われるので、可安定性が成り立つとき、対(A,B)は可安定対という言い方をします。

●可安定性の十分条件として次の可制御性が知られています。

【可制御性の定義とその等価な条件】

定義DC:　任意初期状態を，任意有限時間内に，任意状態に移動可能

条件C1:　\displaystyle{\int_0^t \exp(A\tau)BB^T\exp(A^T\tau)\,d\tau>0 \quad (\forall t>0)}

条件C2:　\boxed{{\rm rank}\, \left[\begin{array}{cccc} B & AB & \cdots & A^{n-1}B \end{array}\right]=n}

条件C3:　Fを選んで，A-BFの固有値を任意に設定可能

条件C4:　\boxed{{\rm rank}\, \left[\begin{array}{cc} B & A-\lambda I_n \end{array}\right] =n}　(\lambdaはAのすべての固有値)

条件C5:　B^Tw=0,\ A^Tw=\lambda w \Rightarrow w=0　(\lambdaはAのすべての固有値)

可制御性の判定も行列AとBを用いて行われるので、可制御性が成り立つとき、対(A,B)は可制御対という言い方をします。条件C1の積分式を可制御性グラミアン行列、条件C2の行列を可制御性行列と呼びます。また条件C4を満足するAの固有値を\lambdaは可制御固有値、満足しない固有値を不可制御固有値と呼びます。