２慣性系

投稿日時: 投稿者:

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

松井義弘：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