モデル規範による追従制御

モデル規範による追従制御…Homework

[1] 次の状態方程式で表される制御対象を考えます。

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

この状態が理想的なモデル

\displaystyle{(2)\quad \dot{x}_m(t)=A_mx_m(t)+B_mr(t) }

の状態を追従するように、すなわち追従誤差e(t)について

\displaystyle{(3)\quad e(t)=x(t)-x_m(t)\rightarrow 0 \quad(t\rightarrow\infty) }

となるように制御則を決定したいとします。これは状態オブザーバの設計によく似ていますが、モデル(2)の入力が新規の入力r(t)であることが違います。

追従誤差のダイナミックスは

\displaystyle{(4)\quad \begin{array}{l} %\bullet \dot{e}(t)=Ax(t)+Bu(t)-(A_mx_m(t)+B_mr(t))\\ %=Ax(t)-Ax_m(t)+Ax_m(t)-A_mx_m(t)+Bu(t)-B_mr(t)\\ %=Ae(t)+(A-A_m)x_m(t)+Bu(t)-B_mr(t)\\ %\bullet \dot{e}(t)=Ax(t)+Bu(t)-(A_mx_m(t)+B_mr(t))\\ =Ax(t)-A_mx(t)+A_mx(t)-A_mx_m(t)+Bu(t)-B_mr(t)\\ =A_me(t)+(A-A_m)x(t)+Bu(t)-B_mr(t) \end{array} }

となります。いま条件

\displaystyle{(5)\quad \begin{array}{l} (BB^\dag-I)(A-A_m)=0\Leftrightarrow B\underbrace{B^\dag(A-A_m)}_{F}=A-A_m\\ (BB^\dag-I)B_m=0\Leftrightarrow B\underbrace{B^\dag B_m}_{G}=B_m \end{array} }

を仮定すると(B^\dagBB^\dag B=Bを満たす疑似逆行列)、制御則

\displaystyle{(6)\quad u(t)=-Ke(t)-Fx(t)+Gr(t) }

の下では、誤差方程式は次式となります。

\displaystyle{(7)\quad \dot{e}(t)=(A_m-BK)e(t) }

実際、

\displaystyle{(8)\quad \begin{array}{l} \dot{e}=A_me+(A-A_m)x+Bu-B_mr\\ =A_me+(A-A_m)x+B(-Ke-Fx+Gr)-B_mr\\ =A_me+(A-A_m)x-BKe-BB^\dag(A-A_m)x+BB^\dag B_mr-B_mr\\ =(A_m-BK)e-\underbrace{(BB^\dag-I)(A-A_m)}_{0}x+\underbrace{(BB^\dag -I)B_m}_{0}r\\ \end{array} }

以上から、制御則(6)の設計の指針として、まずFGは(5)より

\displaystyle{(9)\quad \begin{array}{l} A_m=A-BF\\ B_m=BG \end{array} }

だから、望ましいモデル(2)を(1)に対する状態フィードバックと入力変換で得るものとします。特にGrから出力

\displaystyle{(10)\quad y(t)=Hx(t) }

までの定常ゲイン

\displaystyle{(11)\quad \left\{\begin{array}{l} 0=(A-BF)x(\infty)+BGr(\infty)\\ y(\infty)=Hx(\infty) \end{array}\right. \Rightarrow y(\infty)=\underbrace{-H(A-BF)^{-1}BG}_{I}r(\infty) }

が単位行列となるように、次式で決めます。

\displaystyle{(12)\quad G=-(H(A-BF)^{-1}B)^{-1} }

次にKですが、(7)が安定行列であることが前提となり、望ましいモデルより、追従誤差は速いダイナミックスをもつことが必要であることに注意します。

[2] そこでスライディングモード制御を適用することを考えます。まずスイッチング関数を

\displaystyle{(13)\quad s(t)=Se(t) }

とするとき、スライディングモード時は

\displaystyle{(14)\quad Se(t)=0\Rightarrow S\dot{e}(t)=0 }

すなわち

\displaystyle{(15)\quad S\dot{e}(t)=S(A_me(t)+(A-A_m)x(t)+Bu(t)-B_mr(t))=0 }

が成り立ち、等価制御は次式となります。

\displaystyle{(16)\quad u_{eq}(t)=-(SB)^{-1}S(A_me(t)+(A-A_m)x(t)-B_mr(t)) }

このとき追従誤差のダイナミックスは次式となります。

\displaystyle{(17)\quad \begin{array}{l} \dot{e}=A_me+(A-A_m)x+Bu-B_mr\\ =A_me+(A-A_m)x+B(-(SB)^{-1}S(A_me+(A-A_m)x-B_mr))-B_mr\\ =(I-B(SB)^{-1}S)(A_me+(A-A_m)x-B_mr)\\ =(I-B(SB)^{-1}S)(A_me+BFx-BGr)\\ =\underbrace{(I-B(SB)^{-1}S)A_m}_{A_{eq}}e \end{array} }

したがって、スライディングモード制御を適用したときの、全体の制御則は、次式で表されます。

\displaystyle{(18)\quad { \begin{array}{l} u(t)=-Fx(t)+Gr(t)+u_\ell(t)+u_n(t)\\ u_\ell(t)=-(SB)^{-1}(SA_m-\Phi S)e(t)\\ u_n(t)=-(SB)^{-1}\rho(t,e)\frac{P_2s(t)}{||P_2s(t)||} \end{array}} }

ただし

\displaystyle{(19)\quad P_2\Phi+\Phi^TP_2=-I }

演習…Flipped Classroom

MATLAB
%cCIP_smcm.m
%-----
 clear all, close all
 global mc m ell g th0
 mc=1; m=0.1; ell=0.2; g=9.8;
 ths=0; %input('ths   = <0,180> ')/180*pi;
 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); 
 H=[1 0 0 0];
%-----
 lambda=[-0.8*1.5 0.5 -1.5*1.5 1];
 nocomp=2;
 specpos=rand(4,4);
 specent=rand(4,4);
 F=vplace2(A,B,lambda,nocomp,specpos,specent)       
 pl=eig(A-B*F)
%-----
 H=[1 0 0 0];
 Am=A-B*F;
 G=-inv(H*(Am\B));
 Bm=B*G;
%-----
 Mr=0.5; Mth=3/180*pi; 
 Tc=1; Tpen=0.25*2*pi*sqrt(2*ell/g);
 Q=diag([1/Mr^2,1/Mth^2,Tc^2/Mr^2,Tpen^2/Mth^2]);
 S=swflqr(A,B,Q) 
%-----
 Phi=-0.1;
 P2=0.5*inv(-Phi);
 Check=P2*Phi+Phi*P2
 P2S=P2*S;
 Leq=inv(S*B)*S*A;
 LPhi=-inv(S*B)*Phi*S;
 L=Leq+LPhi;
 rho=1;
 Ln=inv(S*B)*rho;
%-----
 x0=[0;th0*0;0;0];
 sim('CIP_smcm_2015a.mdl')
%-----
%eof
SCILAB

C42 出力数>=入力数


SM出力フィードバック…Homework

[1] 制御対象の状態空間表現として次式を考えます。

\displaystyle{(1)\quad \begin{array}{l} \dot{x}(t)=Ax(t)+Bu(t)+f(t,x,u)\\ y(t)=Cx(t)\\ (x(t)\in{\rm\bf R}^n, u(t)\in{\rm\bf R}^m, y(t)\in{\rm\bf R}^p, f(t,x,u)\in{\rm\bf R}^n) \end{array} }

ただし、次のマッチング条件が成り立つとします。

\displaystyle{(2)\quad \begin{array}{l} f(t,x,u)=B\xi(t,x,u)\\ ||\xi(t,x,u)||<k_1||u||+\alpha(t,y) \end{array} }

このとき、適当なF\in{\rm\bf R}^{m\times p}に対して、SM超平面

\displaystyle{(3)\quad {\cal S}=\{x\in{\rm\bf R}^n: Fy=\underbrace{FC}_{S}x=0\}  }

を考えます。ここで、p\ge mの場合を考え、

\displaystyle{(4)\quad {\rm rank}(CB)=m }

を仮定します。さらに、(1)において座標変換x_c=T_cxを行って、C行列が[0\ I_p]となるようにしておきます。

\displaystyle{(5)\quad \underbrace{ \left[\begin{array}{c} \dot x_{c1}(t)\\ \dot y(t) \end{array}\right] }_{\dot{x}_c(t)} = \underbrace{ \left[\begin{array}{cc} A_{c11} & A_{c12} \\ A_{c21} & A_{c22} \\ \end{array}\right] }_{A_c=T_c A T_c^{-1}} \underbrace{ \left[\begin{array}{c} x_{c1}(t)\\ y(t) \end{array}\right] }_{x_c(t)} + \underbrace{ \left[\begin{array}{c} B_{c1}\\ B_{c2} \end{array}\right] }_{B_c=T_c B} (u(t)+\xi(t,x,u)) }

\displaystyle{(6)\quad y(t)= \underbrace{ \left[\begin{array}{cc} 0_{p\times n-p} & I_p \\ \end{array}\right] }_{C_c=CT_c^{-1}} \underbrace{ \left[\begin{array}{c} x_{c1}(t)\\ y(t) \end{array}\right] }_{x_c(t)} }

ここで、(4)より

\displaystyle{(7)\quad {\rm rank}(CB)={\rm rank}(CT_c^{-1}T_cB)={\rm rank}(C_cB_c)=B_{c2}=m }

したがって、ある直交行列T\in{\rm R}^{p\times p}により

\displaystyle{(8)\quad T^TB_{c2} = \left[\begin{array}{c} 0_{p-m\times m}\\ {B}_{b22} \end{array}\right] }

とすることができます(B_{c2}の特異値分解から)。このTを用いた座標変換

\displaystyle{(9)\quad \begin{array}{l} \underbrace{ \left[\begin{array}{c} x_{b1}(t)\\ x_{b2}(t) \end{array}\right] }_{x_b(t)} = \underbrace{ \left[\begin{array}{cc} I_{n-p} & -B_{c1}(B_{c2}^TB_{c2})^{-1}B_{c2}^T \\ 0_{p\times n-p} & T^T \\ \end{array}\right] }_{T_b} \underbrace{ \left[\begin{array}{c} x_{c1}(t)\\ y(t) \end{array}\right] }_{x_c(t)}\\ \Leftrightarrow \underbrace{ \left[\begin{array}{c} x_{c1}(t)\\ y(t) \end{array}\right] }_{x_c(t)} = \underbrace{ \left[\begin{array}{cc} I_{n-p} & B_{c1}(B_{c2}^TB_{c2})^{-1}B_{c2}^T \\ 0_{p\times n-p} & T \\ \end{array}\right] }_{T_b^{-1}} \underbrace{ \left[\begin{array}{c} x_{b1}(t)\\ x_{b2}(t) \end{array}\right] }_{x_b(t)} \end{array} }

を行って、次をの標準形を得ます。

\displaystyle{(10)\quad \underbrace{ \left[\begin{array}{c} \dot{x}_{b1}(t)\\ \dot{x}_{b2}(t) \end{array}\right] }_{\dot{x}_b(t)} = \underbrace{ \left[\begin{array}{cc} {A}_{b11} & {A}_{b12} \\ {A}_{b21} & {A}_{b22}\\ \end{array}\right] }_{A_b=T_b A_c T_b^{-1}} \underbrace{ \left[\begin{array}{c} x_{b1}(t)\\ x_{b2}(t) \end{array}\right] }_{x_b(t)} + \underbrace{ \left[\begin{array}{c} 0_{n-p\times m}\\\hline 0_{p-m\times m}\\ \bar{B}_{b22} \end{array}\right] }_{B_b=T_bB_c} (u(t)+\xi(t,x,u)) }

\displaystyle{(11)\quad y(t) = \underbrace{ \left[\begin{array}{cc} 0_{p\times n-p} & T \end{array}\right] }_{C_b=C_cT_b^{-1}} \underbrace{ \left[\begin{array}{c} x_{b1}(t)\\ x_{b2}(t) \end{array}\right] }_{x_b(t)} }

ここで、FC_bを次のように表しておきます。

\displaystyle{(12)\quad FC_b= \left[\begin{array}{cc} 0_{m\times n-p} & FT \end{array}\right] =\left[\begin{array}{ccc} 0_{m\times n-p} & F_1 & F_2 \end{array}\right] =\left[\begin{array}{cc} F_1C_1 & F_2 \end{array}\right] }

ただし

\displaystyle{(13)\quad C_1= \left[\begin{array}{cc} 0_{p-m\times n-p} & I_{p-m} \end{array}\right] }

C41 出力数=入力数


SM出力フィードバック…Homework

[1] 制御対象の状態空間表現として次式を考えます。

\displaystyle{(1)\quad \begin{array}{l} \dot{x}(t)=Ax(t)+Bu(t)+f(t,x,u)\\ y(t)=Cx(t)\\ (x(t)\in{\rm\bf R}^n, u(t)\in{\rm\bf R}^m, y(t)\in{\rm\bf R}^p, f(t,x,u)\in{\rm\bf R}^n) \end{array} }

ただし、次のマッチング条件が成り立つとします。

\displaystyle{(2)\quad \begin{array}{l} f(t,x,u)=B\xi(t,x,u)\\ ||\xi(t,x,u)||<k_1||u||+\alpha(t,y) \end{array} }

このとき、適当なF\in{\rm\bf R}^{m\times p}に対して、SM超平面

\displaystyle{(3)\quad {\cal S}=\{x\in{\rm\bf R}^n: Fy=\underbrace{FC}_{S}x=0\}  }

を考えます。ここで、入力数=出力数(m=p)および

\displaystyle{(4)\quad {\rm det}(SB)={\rm det}(FCB)={\rm det}(F){\rm det}(CB)\ne0 \Leftrightarrow {\rm det}(F)\ne0, {\rm det}(CB)\ne0 }

を仮定します。さらに、(1)はつぎの標準形であるとします。

\displaystyle{(5)\quad \underbrace{ \left[\begin{array}{c} \dot x_1(t)\\ \dot x_2(t) \end{array}\right] }_{\dot{x}(t)} = \underbrace{ \left[\begin{array}{cc} A_{11} & A_{12} \\ A_{21} & A_{22} \\ \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\\ B_2 \end{array}\right] }_{B} (u(t)+\xi(t,x,u)) }

\displaystyle{(6)\quad y(t)= \underbrace{ \left[\begin{array}{cc} C_1 & C_2 \\ \end{array}\right] }_{C} \underbrace{ \left[\begin{array}{c} x_1(t)\\ x_2(t) \end{array}\right] }_{x(t)} = \underbrace{C_2 \left[\begin{array}{cc} M & I \\ \end{array}\right] }_{C} \underbrace{ \left[\begin{array}{c} x_1(t)\\ x_2(t) \end{array}\right] }_{x(t)} \ (M=C_2^{-1}C_1) }

これに対して、座標変換

\displaystyle{(7)\quad \underbrace{ \left[\begin{array}{c} x_1(t)\\ y(t) \end{array}\right] }_{\bar{x}(t)} = \underbrace{ \left[\begin{array}{cc} I & 0 \\ C_1 & C_2 \\ \end{array}\right] }_{T_y} \underbrace{ \left[\begin{array}{c} x_1(t)\\ x_2(t) \end{array}\right] }_{x}\\ \Leftrightarrow \underbrace{ \left[\begin{array}{c} x_1(t)\\ x_2(t) \end{array}\right] }_{x(t)} = \underbrace{ \left[\begin{array}{cc} I & 0 \\ -C_2^{-1}C_1 & C_2^{-1} \\ \end{array}\right] }_{T_y^{-1}} \underbrace{ \left[\begin{array}{c} x_1(t)\\ y(t) \end{array}\right] }_{\bar{x}(t)} }

を行って、次式を得ます。

\displaystyle{(8)\quad \underbrace{ \left[\begin{array}{c} \dot x_1(t)\\ \dot y(t) \end{array}\right] }_{\dot{\bar{x}}(t)} = \underbrace{ \left[\begin{array}{cc} \bar{A}_{11} & \bar{A}_{12} \\ \bar{A}_{21} & \bar{A}_{22}\\ \end{array}\right] }_{\bar{A}=T_y A T_y^{-1}} \underbrace{ \left[\begin{array}{c} x_1(t)\\ y(t) \end{array}\right] }_{\bar{x}(t)} + \underbrace{ \left[\begin{array}{c} 0\\ \bar{B}_2 \end{array}\right] }_{\bar{B}=T_yB} (u(t)+\xi(t,x,u)) }

\displaystyle{ \begin{array}{l} (8.1)\quad \bar{A}_{11}=A_{11}-A_{12}M\\ (8.2)\quad \bar{A}_{12}=A_{12}C_2^{-1}\\ (8.3)\quad \bar{A}_{21}=C_2(M\bar{A}_{11}+A_{21}-A_{22}M)\\ (8.4)\quad \bar{A}_{22}=C_2(M{A}_{12}+A_{22})C_2^{-1}\\  (8.5)\quad \bar{B}_2=C_2B_2\\ \end{array} }

\displaystyle{(9)\quad y(t) = \underbrace{ \left[\begin{array}{cc} 0 & I \end{array}\right] }_{\bar{C}=CT_y^{-1}} \underbrace{ \left[\begin{array}{c} x_1(t)\\ y(t) \end{array}\right] }_{\bar{x}(t)} }

●このとき、SMC則

\displaystyle{(10)\quad { u(t)=\underbrace{-\gamma Fy(t)}_{linear\ control}\underbrace{-\rho(t,y)\frac{Fy(t)}{||Fy(t)||}}_{switching\ component}} }
ただし

\displaystyle{ \begin{array}{l} (10.1)\quad \gamma>\gamma_0\\ (10.2)\quad F=\bar{B}_2^TP_2 \Leftrightarrow P\bar{B}=\bar{C}^TF^T\\ (10.3)\quad \rho(t,y)=\frac{1}{1-k_1}(k_1\gamma||Fy||+\alpha(t,y)+\gamma_2)\\ \end{array} }

を(\gamma,P_2,\gamma_2は設計パラメータ)、2次安定性

\displaystyle{{(11)\quad  V(s)= \underbrace{ \left[\begin{array}{c} x_1(t)\\ y(t) \end{array}\right] }_{\bar{x}(t)} \underbrace{ \left[\begin{array}{cc} P_1 & 0 \\ 0 & P_2 \\ \end{array}\right] }_{P} \underbrace{ \left[\begin{array}{c} x_1(t)\\ y(t) \end{array}\right] }_{\bar{x}(t)} &\Rightarrow \dot{V}(s)\le \bar{x}^T(t)L(\gamma)\bar{x}(t) }

ただし

\displaystyle{ \begin{array}{l} (11.1)\quad L(\gamma)=PA_0+A_0^TP<0\\ (11.2)\quad A_0=\bar{A}-\gamma \bar{B}F\bar{C}\\ \end{array} }

が成り立つように決定します(P_1>0, P_2>0)。

●実際、まず\gamma

\displaystyle{ \begin{array}{l} (12)\quad \gamma>\gamma_0 \end{array} }

ただし

\displaystyle{ \begin{array}{l} (12.1)\quad \gamma_0=\frac{1}{2}\lambda_{max}(F^{-T}(Q_3+Q_2^TQ_1^{-1}Q_2)F^{-1})\\ (12.2)\quad Q_1=-(P_1{\bar A}_{11}+{\bar A}_{11}^TP_1)>0\\ (12.3)\quad Q_2=P_1{\bar A}_{12}+{\bar A}_{21}^TP_2\\ (12.4)\quad Q_3=P_2\bar{A}_{22}+\bar{A}_{22}^TP_2\\ \end{array} }

を満足するように選ぶことで、(11.1)を得ます。これは

\displaystyle{(13)\quad  \begin{array}{l} A_0= \underbrace{ \left[\begin{array}{cc} \bar{A}_{11} & \bar{A}_{12} \\ \bar{A}_{21} & \bar{A}_{22} \\ \end{array}\right] }_{\bar{A}}-\gamma \underbrace{ \left[\begin{array}{c} 0\\ \bar{B}_2 \end{array}\right] }_{\bar{B}} F \underbrace{ \left[\begin{array}{cc} 0 & I \end{array}\right] }_{\bar{C}}\\ = \left[\begin{array}{cc} \bar{A}_{11} & \bar{A}_{12} \\ \bar{A}_{21} & \bar{A}_{22}-\gamma \bar{B}_2F \\ \end{array}\right] \end{array} }

\displaystyle{(14)\quad  \begin{array}{l} PA_0= \underbrace{ \left[\begin{array}{cc} P_1 & 0 \\ 0 & P_2 \\ \end{array}\right] }_{P} \underbrace{ \left[\begin{array}{cc} \bar{A}_{11} & \bar{A}_{12} \\ \bar{A}_{21} & \bar{A}_{22}-\gamma \bar{B}_2F \end{array}\right] }_{A_0}\\ = \left[\begin{array}{cc} P_1\bar{A}_{11} & P_1\bar{A}_{12} \\ P_2\bar{A}_{21} & P_2\bar{A}_{22}-\gamma P_2\bar{B}_2F \end{array}\right] \end{array} }

\displaystyle{(15)\quad  \begin{array}{l} L(\gamma)=PA_0+A_0^TP\\ = \left[\begin{array}{cc} P_1\bar{A}_{11}+\bar{A}_{11}^TP_1 & P_1\bar{A}_{12}+\bar{A}_{21}^TP_2\\ \bar{A}_{12}^TP_1+P_2\bar{A}_{21} & P_2\bar{A}_{22}+\bar{A}_{22}^TP_2+* \end{array}\right]\\ =\left[\begin{array}{cc} -Q_1 & Q_2 \\ Q_2^T & Q_3-2\gamma F^TF \\ \end{array}\right]\\ \quad(*=-\gamma P_2\bar{B}_2F-\gamma (P_2\bar{B}_2F)^T=-2\gamma F^TF) \end{array} }

と表せるので

\displaystyle{(15)\quad  \begin{array}{l} L(\gamma)<0 \\ \Leftrightarrow  Q_3-2\gamma F^TF+Q_2^TQ_2^{-1}Q_2<0\\ \Leftrightarrow  2\gamma F^TF>Q_3+Q_2^TQ_2^{-1}Q_2\\ \Leftrightarrow  2\gamma I>F^{-T}(Q_3+Q_2^TQ_2^{-1}Q_2)F^{-1}\\ \Leftrightarrow  2\gamma >\lambda_{max}(F^{-T}(Q_3+Q_2^TQ_2^{-1}Q_2)F^{-1}) \end{array} }

となることから分かります。

●次に2次安定性(11)を示すために

\displaystyle{(16)\quad  \begin{array}{l} \dot{\bar{x}}=\bar{A}\bar{x}+\bar{B}(-\gamma F\bar{C}\bar{x}-u_{sw}+\xi) \end{array} }

に注意して

\displaystyle{(17)\quad  \begin{array}{l} \dot{V}=\dot{\bar{x}}^TP\bar{x}+\bar{x}^TP\dot{\bar{x}}=2\bar{x}^TP\dot{\bar{x}}\\ =2\bar{x}^TP(\bar{A}\bar{x}-\gamma \bar{B}F\bar{C}\bar{x}-\bar{B}u_{sw}+\bar{B}\xi)\\ =\bar{x}^T(P\bar{A}+\bar{A}^TP-2\gamma P\bar{B}F\bar{C})\bar{x}+2^TPB(\xi-u_{sw})\\ =\bar{x}^T(P\bar{A}+\bar{A}^TP-2\gamma (F\bar{C})^TF\bar{C})\bar{x}+2\bar{x}^T(F\bar{C})^T(\xi-u_{sw})\\ =\bar{x}^TL(\gamma)\bar{x}+2\bar{x}^T(F\bar{C})^T(\xi-u_{sw})\\ =\bar{x}^TL(\gamma)\bar{x}+2y^TF^T(\xi-u_{sw})\\ =\bar{x}^TL(\gamma)\bar{x}+2y^TF^T\xi-2y^TF^Tu_{sw}\\ \le \bar{x}^TL(\gamma)\bar{x}+2||Fy||||\xi||-2||Fy||||u_{sw}||\\ =\bar{x}^TL(\gamma)\bar{x}+2||Fy||||\xi||-2\rho(t,y)||Fy||\\ < \bar{x}^TL(\gamma)\bar{x}+2||Fy||(k_1||u||+\alpha(t,y)-\rho(t,y))\\ \end{array} }

ここで

\displaystyle{(18)\quad  \begin{array}{l} \rho(t,y)=k_1\rho(t,y)+k_1\gamma||Fy||+\alpha(t,y)+\gamma_2\\ \ge k_1||-\gamma Fy-\rho(t,y)\frac{Fy}{||Fy||}||+\alpha(t,y)+\gamma_2\\ \ge k_1||u||+\alpha(t,y)+\gamma_2\\ \end{array} }

を用いて

\displaystyle{(19)\quad  \begin{array}{l} \dot{V} < \bar{x}^TL(\gamma)\bar{x}-2||Fy||(\rho(t,y)-k_1||u||-\alpha(t,y))\\ < \bar{x}^TL(\gamma)\bar{x}-2\gamma_2||Fy||<\bar{x}^TL(\gamma)\bar{x} \end{array} }

C34 偏差系のSM安定化制御


偏差系のSM安定化制御…Homework

レギュレータ問題は平衡状態の安定化、追値問題は他の平衡状態への安定化を意味します。LQI制御のところでみたように、偏差系を導入すれば、追値問題は偏差系の安定化問題となります。この観点から積分動作導入による追従制御を見直してみます。


[1] 定値外乱を受ける制御対象

\displaystyle{(1)\quad \begin{array}{l} \dot{x}(t)=Ax(t)+Bu(t)+w\\ y(t)=Cx(t)\\ (x(t)\in{\rm\bf R}^n, u(t)\in{\rm\bf R}^m, y(t)\in{\rm\bf R}^p, w\in{\rm\bf R}^n, m=p) \end{array} }

の出力を、次のコマンド(定値目標)

\displaystyle{(2)\quad r\in{\rm\bf R}^m %\begin{array}{l} %\dot{r}(t)=\Gamma(r(t)-r_c)\\ %(r(t),r_c\in{\rm\bf R}^m) %\end{array} %\Gammaは安定行列 }

に追従させることを考えます。そのために、積分動作

\displaystyle{(3)\quad \begin{array}{l} \dot{x}_I(t)=y(t)-r\\ %\dot{x}_I(t)=r(t)-y(t)\\ (x_I(t)\in{\rm\bf R}^m) \end{array} }

を考え、次の拡大系を構成します。

\displaystyle{(4)\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} }

定常状態では

\displaystyle{(5)\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_{\infty},x_{I\infty},u_{\infty}は定数ベクトル)。まず、(4)から(5)を引いて、つぎの偏差系を得ます。

偏差系E1:
\displaystyle{(6)\quad \underbrace{ \frac{d}{dt} \left[\begin{array}{c} x(t)-x_\infty \\ x_I(t)-x_{I\infty} \end{array}\right] }_{\dot{x}_{E1}(t)} = \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_{E1}(t)} + \underbrace{ \left[\begin{array}{c} B \\ 0 \end{array}\right] }_{B_{E1}} (u(t)-u_\infty) }

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

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

を得ます。さらに、(1)の状態方程式と観測方程式をまとめた

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

から、(5)すなわち

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

を引いて、つぎの関係式が成り立ちます。

\displaystyle{(10)\quad \underbrace{ \left[\begin{array}{c} {\dot x}(t) \\ y(t)-r \end{array}\right] }_{x_{E2}(t)} = \underbrace{ \left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right] }_{S_E} \underbrace{ \left[\begin{array}{c} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] }_{x_{E3}(t)} }

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

偏差系E3:
\displaystyle{(11)\quad \underbrace{ \frac{d}{dt} \left[\begin{array}{c} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] }_{\dot{x}_{E3}(t)} = \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] }_{x_{E3}(t)} + \underbrace{ \left[\begin{array}{c} 0 \\ I_m \end{array}\right] }_{B_{E3}} {\dot u}(t) }

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

\displaystyle{(12)\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_E} = \underbrace{ \left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right] }_{S_E} \underbrace{ \left[\begin{array}{cc} A & B \\ 0 & 0 \end{array}\right] }_{A_{E3}} }

\displaystyle{(13)\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_E} \underbrace{ \left[\begin{array}{c} 0 \\ I_m \end{array}\right] }_{B_{E3}} }

\displaystyle{(14)\quad \underbrace{ \left[\begin{array}{cc} 0 & I_m \end{array}\right] }_{C_{E2}} \underbrace{ \left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right] }_{S_E} = \underbrace{ \left[\begin{array}{cc} C & 0 \end{array}\right] }_{C_{E3}} }

以下では、この偏差系E3が標準形となっていることに注意して、SMCを設計します。

(11)を、改めて次のように書きます。

\displaystyle{(15)\quad \begin{array}{l} \underbrace{ \left[\begin{array}{c} \dot x_1(t)\\ \dot x_2(t) \end{array}\right] }_{\dot{x}_{E3}(t)} = \underbrace{ \left[\begin{array}{cc} A & B \\ 0 & 0 \end{array}\right] }_{A_{E3}} \underbrace{ \left[\begin{array}{c} x_1(t)\\ x_2(t) \end{array}\right] }_{x_{E3}(t)} + \underbrace{ \left[\begin{array}{c} 0\\ I_m \end{array}\right] }_{B_{E3}} {\dot u}(t)\\ (x_1(t)=x(t)-x_\infty, x_2(t)=u(t)-u_\infty) \end{array} }

スイッチング関数として、次式を考えます。

\displaystyle{(16)\quad s(t)= \underbrace{ \left[\begin{array}{cc} S_1 & S_2 \\ \end{array}\right] }_{S} \underbrace{ \left[\begin{array}{c} x_1(t)\\ x_2(t) \end{array}\right] }_{x_{E3}(t)} = \underbrace{S_2 \left[\begin{array}{cc} M & I \\ \end{array}\right] }_{S} \underbrace{ \left[\begin{array}{c} x_1(t)\\ x_2(t) \end{array}\right] }_{x_{E3}(t)} \ (M=S_2^{-1}S_1) }

(15)に対して、座標変換

\displaystyle{(17)\quad \underbrace{ \left[\begin{array}{c} x_1(t)\\ s(t) \end{array}\right] }_{x'_{E3}(t)} = \underbrace{ \left[\begin{array}{cc} I & 0 \\ S_1 & S_2 \\ \end{array}\right] }_{T_s} \underbrace{ \left[\begin{array}{c} x_1(t)\\ x_2(t) \end{array}\right] }_{x_{E3}(t)}\\ \Leftrightarrow \underbrace{ \left[\begin{array}{c} x_1(t)\\ x_2(t) \end{array}\right] }_{x_{E3}(t)} = \underbrace{ \left[\begin{array}{cc} I & 0 \\ -S_2^{-1}S_1 & S_2^{-1} \\ \end{array}\right] }_{T_s^{-1}} \underbrace{ \left[\begin{array}{c} x_1(t)\\ s(t) \end{array}\right] }_{x'_{E3}(t)} }

を行って、次式を得ます。

\displaystyle{(18)\quad \underbrace{ \left[\begin{array}{c} \dot x_1(t)\\ \dot s(t) \end{array}\right] }_{\dot{x}'_{E3}(t)} = \underbrace{ \left[\begin{array}{cc} \bar{A}_{11} & \bar{A}_{12} \\ \bar{A}_{21} & \bar{A}_{22} \\ \end{array}\right] }_{T_sA_{E3}T_s^{-1}} \underbrace{ \left[\begin{array}{c} x_1(t)\\ s(t) \end{array}\right] }_{x'_{E3}(t)} + \underbrace{ \left[\begin{array}{cc} 0\\ S_2 \end{array}\right] }_{T_sB_{E3}} {\dot u}(t) }

\displaystyle{(19)\quad \begin{array}{l} \bar{A}_{11}=A-BM\\ \bar{A}_{12}=BS_2^{-1}\\ \bar{A}_{21}=S_1(A-BM)\\ \bar{A}_{22}=S_1BS_2^{-1} \end{array} }

以下では、\bar{A}_{11}が安定行列となるようにスイッチング関数が選ばれていると仮定します。

このとき、SMC則

\displaystyle{(20)\quad { {\dot u}(t)=\underbrace{{\dot u}_\ell(t)}_{linear\ control}+\underbrace{{\dot u}_n(t)}_{switching\ component}} }

を、2次安定性

\displaystyle{(21)\quad { \begin{array}{lll} V(s)=s(t)^TP_2s(t)& \Rightarrow \dot{V}(s)\le -s^T(t)s(t)&(t\le t_s)\\ V(x_1)=x_1^T(t)P_1x_1(t)& \Rightarrow \dot{V}(x_1)\le -x_1^T(t)Q_1x_1(t)&(t> t_s) \end{array}} }

が成り立つように決定します(P_1>0, P_2>0, Q_1>0)。

[2] 可到達性の検討 (t\le t_s)

等価制御は

\displaystyle{(22)\quad \begin{array}{l} s(t)=0\Rightarrow\dot{s}(t)=0 \Rightarrow 0=\bar{A}_{21}x_1(t)+\bar{A}_{22}s(t)+S_2{\dot u}(t)\\ \Rightarrow {\dot u}_{eq}(t)=-\underbrace{S_2^{-1}}_{(SB_{E3})^{-1}} \underbrace{\left[\begin{array}{cc} \bar{A}_{21} & \bar{A}_{22} \\ \end{array}\right]x'_{E3}(t)}_{SA_{E3}x_{E3}(t)}} \end{array} }

のように得られます。(20)の第1項{\dot u}_\ellは、この等価制御をベースして

\displaystyle{(23)\quad { \begin{array}{l} {\dot u}_\ell(t)=-\underbrace{S_2^{-1}}_{(SB_{E3})^{-1}} \underbrace{(\left[\begin{array}{cc} \bar{A}_{21} & \bar{A}_{22} \\ \end{array}\right]x'_{E3}(t)-\Phi s(t))}_{(SA_{E3}-\Phi S)x_{E3}(t)}} \end{array}} }

のように構成します(\Phiは安定行列)。このとき閉ループ系は次式で与えられます。

\displaystyle{(24)\quad \begin{array}{l} \left[\begin{array}{c} \dot{x}_1(t)\\ \dot{s}(t) \end{array}\right] = \left[\begin{array}{cc} \bar{A}_{11} & \bar{A}_{12} \\ 0 & \Phi \\ \end{array}\right] \left[\begin{array}{c} x_1(t)\\ s(t) \end{array}\right] + \left[\begin{array}{c} 0\\ S_2{\dot u}_n(t) \end{array}\right] \end{array }

すなわち

\displaystyle{(25)\quad \begin{array}{l} \dot{x}_1(t)=\bar{A}_{11}x_1(t)+\bar{A}_{12}s(t)\\ \dot{s}(t)=\Phi s(t)+S_2{\dot u}_n(t) \end{array} }

ここで、\Phiは安定行列なので

\displaystyle{(26)\quad \begin{array}{l} P_2\Phi+\Phi^TP_2=-I \end{array} }

を満たすP_2>0を選ぶことができます。これを用いて

\displaystyle{(27)\quad { {\dot u}_n(t)=-\underbrace{S_2^{-1}}_{(SB_{E3})^{-1}}\rho\frac{P_2s(t)}{||P_2s(t)||}} }

と選びます(\rho>0は定数)。このとき次式が成り立ちます。

\displaystyle{(28)\quad \begin{array}{l} \dot{V}(s)=2s^T(t)P_2\dot{s}(t)\\ =2s^T(t)P_2(\Phi s(t)-\rho\frac{P_2s(t)}{||P_2s(t)||})\\ =s^T(t)(P_2\Phi+\Phi^TP_2)s(t)+2s^T(t)P_2(-\rho\frac{P_2s(t)}{||P_2s(t)||})\\ \le -||s(t)||^2-2\rho||P_2s(t)||\\ \le -||s(t)||^2 \end{array} }

[3] スライディングモードの検討 (t> t_s)

\bar{A}_{11}は安定行列なので

\displaystyle{(29)\quad \begin{array}{l} P_1\bar{A}_{11}+\bar{A}_{11}^TP_1=-Q_1<0 \end{array} }

を満たすP_1>0を選ぶことができます。

\displaystyle{(30)\quad \begin{array}{l} \dot{V}(x_1)=2x_1^T(t)P_1\dot{x}_1(t)\\ =2x_1^T(t)P_1( \bar{A}_{11}x_1(t)+\bar{A}_{12}\underbrace{s(t)}_{0})\\ =x_1^T(t(P_1\bar{A}_{11}+\bar{A}_{11}^TP_1)x_1(t)\\ =-x_1^T(t)Q_1x_1(t) \end{array} }

[4] 積分動作をもつSMC

上で求めた偏差系E3に対するSMCは次式で与えられました。

\displaystyle{(31)\quad  {\dot u}(t)={\dot u}_\ell(t)+{\dot u}_n(t) }

\displaystyle{(32)\quad  {\dot u}_\ell(t)=-(SB_{E3})^{-1}(SA_{E3}-\Phi S)x_{E3}(t) }

\displaystyle{(33)\quad  {\dot u}_n(t)=-(SB_{E3})^{-1}\rho\frac{P_2s(t)}{||P_2s(t)||}}=-(SB_{E3})^{-1}\rho\,{\rm sgn}(P_2Sx_{E3}(t)) }

これらを積分して、制御対象(1)に対する積分動作をもつSMCを導出します。

\displaystyle{(34)\quad  {u}(t)={u}_\ell(t)+{u}_n(t) }

まず(32)は(10)を用いて次式のように書けます。

\displaystyle{(35)\quad  {\dot u}_\ell(t)=- \underbrace{ (SB_{E3})^{-1}(SA_{E3}-\Phi S)S_E^{-1} }_{\left[\begin{array}{cc} F & F_I \end{array}\right]} \underbrace{ \left[\begin{array}{c} {\dot x}(t) \\ y(t)-r \end{array}\right] }_{x_{E2}(t)} }

これを積分して

\displaystyle{(36)\quad  u_\ell(t)=-Fx(t)+F_I\int_0^t(r-y(\tau))d\tau }

次に(33)は(10)を用いて次式のように書けます。

\displaystyle{(37)\quad  {\dot u}_n(t) =-S_2^{-1}\rho\, {\rm sgn}( \underbrace{ P_2SS_E^{-1} }_{\left[\begin{array}{cc} G & G_I \end{array}\right]} \underbrace{ \left[\begin{array}{c} {\dot x}(t) \\ y(t)-r \end{array}\right] }_{x_{E2}(t)}) }

これを積分すれば

\displaystyle{(38)\quad  u_n(t)=S_2^{-1}\rho\,\int_0^t\underbrace{{\rm sgn}(-G{\dot x}(\tau)+G_I(r-y(\tau)))}_{0,\pm 1}d\tau }

ここで、\dot{x}_1(t)=\frac{d}{dt}(x(t)-x_\infty)=\dot{x}(t)に注意し、(25)の第1式を用いて\dot{x}(t)

\displaystyle{(39)\quad  \begin{array}{l} \underbrace{\dot{x}_1(t)}_{\dot{x}(t)}=\underbrace{(A-BM)}_{\bar{A}_{11}}x_1(t)+\underbrace{BS_2^{-1}}_{\bar{A}_{12}}\underbrace{(S_1x_1(t)+S_2x_2(t))}_{s(t)}\\ =(A-BM)x_1(t)+BS_2^{-1}S_2(Mx_1(t)+x_2(t))\\ =Ax_1(t)+Bx_2(t)\\ =Ax(t)+Bu(t)-\left[\begin{array}{cc} A & B \end{array}\right] \left[\begin{array}{c} x_\infty \\ u_\infty \end{array}\right]\\ =Ax(t)+Bu(t)-\left[\begin{array}{cc} A & B \end{array}\right]S_E^{-1} \left[\begin{array}{c} -w \\ r \end{array}\right]\\ =Ax(t)+Bu(t)-\left[\begin{array}{cc} I & 0 \end{array}\right] \left[\begin{array}{c} -w \\ r \end{array}\right]\\ =Ax(t)+Bu(t)+w \end{array} }

となって元の状態方程式となりますが、wを無視し、uの近似値を使うことも一手段かもしれません。

[5] 数値例(1)

\displaystyle{(101)\quad \begin{array}{l} \dot{x}(t)=\underbrace{0}_{a}x(t)+\underbrace{1}_{b}u(t)+w\\ y(t)=\underbrace{1}_{c}x(t)\\ \end{array} }

\displaystyle{(109)\quad \left[\begin{array}{c} -w \\ r \end{array}\right] = \underbrace{ \left[\begin{array}{cc} 0 & 1 \\ 1 & 0 \end{array}\right] }_{S_E} \left[\begin{array}{c} x_\infty \\ u_\infty \end{array}\right] \Rightarrow \left[\begin{array}{c} x_\infty \\ u_\infty \end{array}\right]= \left[\begin{array}{c} r \\ -w \end{array}\right] }

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

\displaystyle{(116)\quad s(t)= \underbrace{ \left[\begin{array}{cc} s_1 & s_2 \\ \end{array}\right] }_{S} \underbrace{ \left[\begin{array}{c} x_1(t)\\ x_2(t) \end{array}\right] }_{x_{E3}(t)} = \underbrace{s_2 \left[\begin{array}{cc} m & 1 \\ \end{array}\right] }_{S} \underbrace{ \left[\begin{array}{c} x_1(t)\\ x_2(t) \end{array}\right] }_{x_{E3}(t)} \ (m=s_2^{-1}s_1) }

\displaystyle{(117)\quad \underbrace{ \left[\begin{array}{c} x_1(t)\\ s(t) \end{array}\right] }_{x'_{E3}(t)} = \underbrace{ \left[\begin{array}{cc} 1 & 0 \\ s_1 & s_2 \\ \end{array}\right] }_{T_s} \underbrace{ \left[\begin{array}{c} x_1(t)\\ x_2(t) \end{array}\right] }_{x_{E3}(t)}\\ \Leftrightarrow \underbrace{ \left[\begin{array}{c} x_1(t)\\ x_2(t) \end{array}\right] }_{x_{E3}(t)} = \underbrace{ \left[\begin{array}{cc} 1 & 0 \\ -s_2^{-1}s_1 & s_2^{-1} \\ \end{array}\right] }_{T_s^{-1}} \underbrace{ \left[\begin{array}{c} x_1(t)\\ s(t) \end{array}\right] }_{x'_{E3}(t)} }

\displaystyle{(118)\quad \underbrace{ \left[\begin{array}{c} \dot x_1(t)\\ \dot s(t) \end{array}\right] }_{\dot{x}'_{E3}(t)} = \underbrace{ \left[\begin{array}{cc} -m & s_2^{-1} \\ -s_1m & m \\ \end{array}\right] }_{T_sA_{E3}T_s^{-1}} \underbrace{ \left[\begin{array}{c} x_1(t)\\ s(t) \end{array}\right] }_{x'_{E3}(t)} + \underbrace{ \left[\begin{array}{cc} 0\\ s_2 \end{array}\right] }_{T_sB_{E3}} {\dot u}(t) }

\displaystyle{(123)\quad \begin{array}{l} {\dot u}_\ell(t)= -\underbrace{s_2^{-1}}_{(SB_{E3})^{-1}} \underbrace{(\left[\begin{array}{cc} -s_1m & m \\ \end{array}\right]x'_{E3}(t)-\Phi s(t))}_{(SA_{E3}-\Phi S)x_{E3}(t)}}\\ =-s_2^{-1}(\left[\begin{array}{cc} 0 & ms_2 \\ \end{array}\right]- \left[\begin{array}{cc} \Phi s_1 & \Phi s_2 \\ \end{array}\right])x_{E3}(t)\\ =-s_2^{-1}\left[\begin{array}{cc} -\Phi s_1 & ms_2-\Phi s_2 \\ \end{array}\right]x_{E3}(t)\\ =-\left[\begin{array}{cc} -\Phi m & m-\Phi \\ \end{array}\right] \underbrace{ \left[\begin{array}{cc} 0 & 1 \\ 1 & 0 \end{array}\right] }_{S_E^{-1}} \underbrace{ \left[\begin{array}{c} {\dot x}(t) \\ y(t)-r \end{array}\right] }_{x_{E2}(t)}\\ =-\underbrace{(m-\Phi)}_{f}{\dot x}(t)+\underbrace{m(-\Phi)}_{f_I}(r-y(t))\\ \Rightarrow u_\ell(t)=-fx(t)+f_I\int_0^t (r-y(\tau))d\tau \end{array} }

\displaystyle{(126)\quad \begin{array}{l} p_2\Phi+\Phi^Tp_2=-1\Rightarrow p_2=-\frac{1}{2}\Phi \end{array} }

\displaystyle{(137)\quad  \begin{array}{l} {\dot u}_n(t) =-s_2^{-1}\rho\, {\rm sgn}(\underbrace{-\frac{1}{2}\Phi}_{p_2} \underbrace{ \left[\begin{array}{cc} s_1 & s_2 \end{array}\right] }_{S} \underbrace{ \left[\begin{array}{cc} 0 & 1 \\ 1 & 0 \end{array}\right] }_{S_E^{-1}} \underbrace{ \left[\begin{array}{c} {\dot x}(t) \\ y(t)-r \end{array}\right] }_{x_{E2}(t)})\\ =\rho\, {\rm sgn}(\frac{1}{2}\Phi \left[\begin{array}{cc} 1 & m \end{array}\right] \left[\begin{array}{c} {\dot x}(t) \\ y(t)-r \end{array}\right])\\ =\rho\, {\rm sgn}(-\underbrace{\frac{1}{2}(-\Phi)}_{g}{\dot x}(t)+\underbrace{\frac{1}{2}m(-\Phi)}_{g_I} (r-y(t)))\\ \Rightarrow u_n(t)=\rho\,\int_0^t\underbrace{{\rm sgn}(-g{\dot x}(\tau)+g_I(r-y(\tau)))}_{0,\pm 1}d\tau \end{array} }

m=1, \Phi=-0.5の場合のシミュレーション結果を次に示します。

[5] 数値例(2)

\displaystyle{(201)\quad \begin{array}{l} \dot{x}(t)= \underbrace{\left[\begin{array}{cc} 0 & 1 \\ 0 & 0 \end{array}\right]}_{A}x(t) +\underbrace{\left[\begin{array}{cc} 0  \\ 1  \end{array}\right]}_{B}u(t)+w\\ y(t)=\underbrace{\left[\begin{array}{cc} 1 & 0   \end{array}\right]}_{C}x(t)\\ \end{array} }

\displaystyle{(209)\quad \left[\begin{array}{c} 0 \\ -w \\ r \end{array}\right] = \underbrace{ \left[\begin{array}{ccc} 0 & 1 & 0 \\ 0 & 0 & 1 \\ 1 & 0 & 0  \end{array}\right] }_{S_E} \left[\begin{array}{c} x_{1\infty} \\ x_{2\infty} \\ u_\infty \end{array}\right] \Rightarrow \left[\begin{array}{c} x_{1\infty} \\ x_{2\infty} \\ u_\infty \end{array}\right]= \left[\begin{array}{c} r \\ 0 \\ -w \end{array}\right] }

\displaystyle{(211)\quad \underbrace{ \frac{d}{dt} \left[\begin{array}{c} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] }_{\dot{x}_{E3}(t)} = \underbrace{ \left[\begin{array}{ccc} 0 & 1 & 0 \\ 0 & 0 & 1 \\ 0 & 0 & 0  \end{array}\right] }_{A_{E3}} \underbrace{ \left[\begin{array}{c} x(t)-x_\infty \\ u(t)-u_\infty \end{array}\right] }_{x_{E3}(t)} + \underbrace{ \left[\begin{array}{c} 0 \\ 0 \\ 1 \end{array}\right] }_{B_{E3}} {\dot u}(t) }

\displaystyle{(216)\quad \begin{array}{l} s(t)= \underbrace{ \left[\begin{array}{ccc} s_{11} & s_{12} & s_2 \\ \end{array}\right] }_{S} \underbrace{ \left[\begin{array}{c} x_1(t)\\ x_2(t) \end{array}\right] }_{x_{E3}(t)} = \underbrace{s_2 \left[\begin{array}{ccc} m_1 & m_2 & 1 \\ \end{array}\right] }_{S} \underbrace{ \left[\begin{array}{c} x_1(t)\\ x_2(t) \end{array}\right] }_{x_{E3}(t)}\\ (m_1=s_2^{-1}s_{11},m_2=s_2^{-1}s_{12}) \end{array} }

\displaystyle{(217)\quad \begin{array}{l} \underbrace{ \left[\begin{array}{c} x_1(t)\\ s(t) \end{array}\right] }_{x'_{E3}(t)} = \underbrace{ \left[\begin{array}{ccc} 1 & 0 & 0\\ 0 & 1 & 0\\ s_{11} & s_{12} & s_2 \\ \end{array}\right] }_{T_s} \underbrace{ \left[\begin{array}{c} x_1(t)\\ x_2(t) \end{array}\right] }_{x_{E3}(t)}\Leftrightarrow\\ \underbrace{ \left[\begin{array}{c} x_1(t)\\ x_2(t) \end{array}\right] }_{x_{E3}(t)} = \underbrace{ \left[\begin{array}{ccc} 1 & 0 & 0\\ 0 & 1 & 0\\ -s_2^{-1}s_{11} & -s_2^{-1}s_{12} & s_2^{-1} \\ \end{array}\right] }_{T_s^{-1}} \underbrace{ \left[\begin{array}{c} x_1(t)\\ s(t) \end{array}\right] }_{x'_{E3}(t)} \end{array} }

\displaystyle{(218)\quad \underbrace{ \left[\begin{array}{c} \dot x_1(t)\\ \dot s(t) \end{array}\right] }_{\dot{x}'_{E3}(t)} = \underbrace{ \left[\begin{array}{ccc} 0 & 1 & 0\\ -m_1 & -m_2 & s_2^{-1} \\ -s_{12}m_1 & -s_{12}m_2 & m_2 \end{array}\right] }_{T_sA_{E3}T_s^{-1}} \underbrace{ \left[\begin{array}{c} x_1(t)\\ s(t) \end{array}\right] }_{x'_{E3}(t)} + \underbrace{ \left[\begin{array}{cc} 0\\ 0\\ s_2 \end{array}\right] }_{T_sB_{E3}} {\dot u}(t) }

\displaystyle{(223)\quad \begin{array}{l} {\dot u}_\ell(t)= -\underbrace{s_2^{-1}}_{(SB_{E3})^{-1}} \underbrace{(\left[\begin{array}{ccc} -s_{12}m_1 & -s_{12}m_2 & m_2 \\ \end{array}\right]x'_{E3}(t)-\Phi s(t))}_{(SA_{E3}-\Phi S)x_{E3}(t)}}\\ =-s_2^{-1}(\left[\begin{array}{ccc} 0 & 0 & m_2s_2 \\ \end{array}\right]- \left[\begin{array}{ccc} \Phi s_{11} & \Phi s_{12} & \Phi s_2 \\ \end{array}\right])x_{E3}(t)\\ =-s_2^{-1}\left[\begin{array}{ccc} -\Phi s_{11} & -\Phi s_{12} & m_2s_2-\Phi s_2 \\ \end{array}\right]x_{E3}(t)\\ =-\left[\begin{array}{ccc} -\Phi m_1 & -\Phi m_2 & m_2-\Phi \\ \end{array}\right] \underbrace{ \left[\begin{array}{ccc} 0 & 0 & 1 \\ 1 & 0 & 0 \\ 0 & 1 & 0  \end{array}\right] }_{S_E^{-1}} \underbrace{ \left[\begin{array}{c} {\dot x}(t) \\ y(t)-r \end{array}\right] }_{x_{E2}(t)}\\ =-\underbrace{ \left[\begin{array}{cc} -\Phi m_2 & m_2-\Phi \\ \end{array}\right] }_{F}{\dot x}(t)+\underbrace{m_1(-\Phi)}_{F_I}(r-y(t))\\ \Rightarrow u_\ell(t)=-Fx(t)+F_I\int_0^t (r-y(\tau))d\tau \end{array} }

\displaystyle{(226)\quad \begin{array}{l} p_2\Phi+\Phi^Tp_2=-1\Rightarrow p_2=-\frac{1}{2}\Phi \end{array} }

\displaystyle{(237)\quad  \begin{array}{l} {\dot u}_n(t) =-s_2^{-1}\rho\, {\rm sgn}(\underbrace{-\frac{1}{2}\Phi}_{p_2} \underbrace{ \left[\begin{array}{ccc} s_{11} & s_{12} & s_2 \end{array}\right] }_{S} \underbrace{ \left[\begin{array}{ccc} 0 & 0 & 1 \\ 1 & 0 & 0 \\ 0 & 1 & 0  \end{array}\right] }_{S_E^{-1}} \underbrace{ \left[\begin{array}{c} {\dot x}(t) \\ y(t)-r \end{array}\right] }_{x_{E2}(t)})\\ =s_2^{-1}\rho\, {\rm sgn}(\frac{1}{2}\Phi \left[\begin{array}{ccc} s_{12} & s_2 & s_{11}  \end{array}\right] \left[\begin{array}{c} {\dot x}(t) \\ y(t)-r \end{array}\right])\\ =\rho\, {\rm sgn}(\frac{1}{2}\Phi \left[\begin{array}{ccc} m_2 & 1 & m_1  \end{array}\right] \left[\begin{array}{c} {\dot x}(t) \\ y(t)-r \end{array}\right])\\ =\rho\, {\rm sgn}(-\underbrace{\frac{1}{2}(-\Phi) \left[\begin{array}{cc} m_2 & 1  \end{array}\right] }_{G}{\dot x}(t)+\underbrace{\frac{1}{2}m_1(-\Phi)}_{G_I} (r-y(t)))\\ \Rightarrow u_n(t)=\rho\,\int_0^t\underbrace{{\rm sgn}(-G{\dot x}(\tau)+G_I(r-y(\tau)))}_{0,\pm 1}d\tau \end{array} }

m_1=m_2=1, \Phi=-0.5の場合のシミュレーション結果を次に示します。

演習…Flipped Classroom

MATLAB
%cCIP_smci2.m
%-----
 clear all, close all
 global mc m ell g th0
 mc=1; m=0.1; ell=0.2; g=9.8;
 ths=0 %input('ths   = <0,180> ')/180*pi;
 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); 
%-----
 Tcart=1; Mr=0.5;
 Tpend=1/4*2*pi*sqrt(4/3*ell/g); Mth=3/180*pi;
 Tamp=0.01; Mamp=10; 
 CM=eye(2,4); DM=zeros(2,1);
 CS=[1 0]; C=CS*CM; D=CS*DM;
 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)];
 SE=[A B;C D];
 CE=eye(n+m);
 QE=diag([1/Mr 1/Mth Tcart/Mr Tpend/Mth 1/Mamp].^2);
 S=swflqr(AE,BE,QE)
%-----
 Phi=-0.5;
 P2=0.5*inv(-Phi);
 Check=P2*Phi+Phi*P2
 P2S=P2*S/SE;
 G=P2S(1:4)
 GI=P2S(5) 
 Leq=inv(S*BE)*S*AE;
 LPhi=-inv(S*BE)*Phi*S;
 L=(Leq+LPhi)/SE;
 F=L(1:4)
 FI=L(5)
 rho=1;
 Ln=inv(S*BE)*rho
%-----
 x0=[0;th0*0;0;0];
 sim('CIP_smci2_2015a.mdl')
%-----
%eof
SCILAB

C33 応用例


応用例…Homework

[1] ある航空機の線形状態方程式として、次を考えます。

\displaystyle{(1)\quad \begin{array}{l} \dot{x}(t)=Ax(t)+Bu(t)\\ x(t)= \left[\begin{array}{ll} \theta & pitch\ angle\\ q & pitch\ rate\\ \alpha & angle\ of\ atack\\ \eta & elevator\ deflection\\ \delta & flap\ deflection \end{array}\right],\quad u(t)= \left[\begin{array}{ll} \eta_c & elevator\ command\\ \delta_c& flap\ comannd \end{array}\right]\\ A=\left[\begin{array}{rrrrr} 0 & 1 & 0 & 0 & 0\\ 0 &-1.99 & -13.41 & -18.95 & -3.60\\ 0 & 1 & -1.74 & -0.08 & -0.59\\ 0 & 0 & 0 & -20 & 0\\ 0 & 0 & 0 & 0 & -20 \end{array}\right],\quad B=\left[\begin{array}{rr} 0 & 0 \\ 0 & 0 \\ 0 & 0 \\ 20 & 0\\ 0 & 20 \end{array}\right] \end{array} }

A行列の固有値は次のように求められます。

\displaystyle{(2)\quad \lambda(A):\left\{\begin{array}{cl} -1.864\pm j 3.660 & short\ period\ mode\\ 0 & pitch\ attitude\ mode\\ -20 & elevator\ actuator\ mode\\ -20 & flap\ actuator\ mode \end{array}\right. }

状態変数として、pitch angle \thetaの代わりに、fligt path angle \gamma=\theta-\alphaを用いると、次式となります。

\displaystyle{(3)\quad \begin{array}{l} \dot{x}(t)=Ax(t)+Bu(t)\\ x(t)= \left[\begin{array}{ll} \gamma=\theta-\alpha & fligt\ path\ angle\\ q & pitch\ rate\\ \alpha & angle\ of\ atack\\ \eta & elevator\ deflection\\ \delta & flap\ deflection \end{array}\right],\quad u(t)= \left[\begin{array}{ll} \eta_c & elevator\ command\\ \delta_c& flap\ comannd \end{array}\right]\\ A=\left[\begin{array}{rrrrr} 0 & 0 & 1.74 & 0.08 & 0.59\\ 0 &-1.99 & -13.41 & -18.95 & -3.60\\ 0 & 1 & -1.74 & -0.08 & -0.59\\ 0 & 0 & 0 & -20 & 0\\ 0 & 0 & 0 & 0 & -20 \end{array}\right],\quad B=\left[\begin{array}{rr} 0 & 0 \\ 0 & 0 \\ 0 & 0 \\ 20 & 0\\ 0 & 20 \end{array}\right] \end{array} }

また、評価変数は、fligt path angle \gammaとpitch angle \thetaとすると、その出力方程式は次式のように与えられます。

\displaystyle{(4)\quad \begin{array}{l} y(t)=Hx(t)\\ H= \left[\begin{array}{ccccc} 1 & 0 & 1 & 0 & 0\\ 1 & 0 & 0 & 0 & 0 \end{array}\right] \end{array} }

制御目的は、次図のように、pitch angle \thetaとfligt path angle \gammaの間の非干渉化を達成し、独立して設定値に合せることです。

図1 航空機の操縦法

[2] モデル規範による追従制御

この場合の制御則は次のように構成されます。

\displaystyle{(5)\quad { \begin{array}{l} u(t)=-Fx(t)+Gr(t)+u_\ell(t)+u_n(t)\\ u_\ell(t)=-\underbrace{(SB)^{-1}(SA_m-\Phi S)}_{L=L_{eq}+L_{\Phi}}e(t)\\ u_n(t)=-\underbrace{(SB)^{-1}\rho(t,e)}_{L_n}\frac{P_2s(t)}{||P_2s(t)||}\quad(P_2\Phi+\Phi^TP_2=-I) \end{array}} }

●追従すべき規範モデルは

\displaystyle{(6)\quad \begin{array}{l} \dot{x}(t)=A_mx(t)+B_mu(t)\\ y(t)=Hx(t) \end{array}} }

ただし

\displaystyle{(7)\quad \begin{array}{l} A_m=A-BF\\ B_m=BG\\ G=-(H(A-BF)^{-1}B)^{-1} \end{array} }

いまこの規範モデルにもたせるべき望ましい固有値を

\displaystyle{(8)\quad \lambda(A_m):\left\{\begin{array}{cl} -5.6\pm j 4.2 & short\ period\ mode\\ -1 & pitch\ attitude\ mode\\ -20 & elevator\ actuator\ mode\\ -20 & flap\ actuator\ mode \end{array}\right. }

とし、また望ましいモード分布を次により指定します。

\displaystyle{(9)\quad {\tt specpos}= \left[\begin{array}{ccc|cc} 1 & 1 & 1 & 0 & 0\\ 1 & 0 & 1 & 0 & 0\\ 0 & 1 & 1 & 0 & 0\\\hline 0 & 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 0 & 1\\ \end{array}\right],\quad {\tt specent}= \left[\begin{array}{rrr|rr} 0 & 0 & 1 & -1 & -1\\ 1 & -1 & 0 & -1 & -1\\ -1 & 1 & 0 & -1 & -1\\\hline -1 & -1 & -1 & 1 & -1\\ -1 & -1 & -1 & -1 & 1\\ \end{array}\right] }

ここで、{\tt specpos}は望ましい固有ベクトルの要素を指定する場合1、指定しない場合0で表した行列、{\tt specエent}は望ましい固有ベクトルの要素が非零指定の場合1、零の場合0、非零任意の場合-1で表した行列です。

このとき以下に示すプログラムを使って、次が得られます。

\displaystyle{(10)\quad \begin{array}{l} F=WV^{-1} =\left[\begin{array}{rrrrr} -1.9338 & -0.5744 & -2.1188 & 0.4750 & 0.1066\\ 1.9571 & 0.2253 & 3.1273 & -0.0694 & -0.0515 \end{array}\right]\\ W=\left[\begin{array}{rrrrr} 0.8445 & 1.9935 & -0.2742 & 0 & 0\\ -2.7055 & 1.2780 & 0.1767 & 0 & 0 \end{array}\right]\\ V=\left[\begin{array}{rrrrr} 0 & -0.0000 & 0.6667 & 0.0030 & -0.0313\\ 1.0000 & -9.5000 & -0.3333 & 0.9964 & 0.1493\\ -0.9286 & 1.0000 & -0.3333 & -0.0528 & 0.0238\\ -1.8252 & -2.2364 & 0.2886 & 1.0000 & -0.0649\\ 2.9860 & -2.6459 & -0.1860 & -0.0825 & 1.0000 \end{array}\right] \end{array} }

●ここでは、スイッチング関数

\displaystyle{(11)\quad s(t)= \underbrace{ \left[\begin{array}{cc} M & I \end{array}\right] }_{S} x(t) }

を構成するために

\displaystyle{(12)\quad \begin{array}{l} \dot{x}_1(t)=A_{11}x_1(t)+A_{12}\hat{u}(t) \end{array} }

を2次形式評価関数

\displaystyle{(13)\quad J=\int_0^\infty(x_1^T(t)Q_{11}x_1(t)+\hat{u}^T(t)Q_{22}\hat{u}(t))dt }

を最小にする

\displaystyle{(14)\quad \hat{u}(t)=-Mx_1(t) }

を求めます。

\displaystyle{(15)\quad Q_{11}={\rm diag}\{10,5,5\},\ Q_{22}={\rm diag}\{20,20\} }

と選ぶとき、スイッチング関数を決める行列Sが、次のように得られます。

\displaystyle{(16)\quad S=\left[\begin{array}{rrr|rr} 0.7025 & 0.4209 & 0.1894 & -1 & 0\\ -0.0810 & 0.0635 & 0.0267 & 0 & -1\\ \end{array}\right] }

P_2\Phi+\Phi^TP_2=-Iを満たす\PhiP_2は、たとえば次のように定めます。

\displaystyle{(17)\quad \underbrace{ \left[\begin{array}{rr} 0.025 & 0\\ 0 & 0.025 \end{array}\right] }_{P_2} \underbrace{ \left[\begin{array}{rr} 20 & 0\\ 0 & 20 \end{array}\right] }_{\Phi} +(*)^T=-I }

このときSMC則における行列Lは次のように計算されます。

\displaystyle{(18)\quad L=\left[\begin{array}{rrrrr} -1.2285 & -0.1858 & -2.1624 & 0.0782 & 0.0460\\ 1.8631 & 0.2827 & 3.0804 & -0.1299 & -0.0060 \end{array}\right] }

図2 モデル規範法によるSMC制御系応答

MATLAB
%cAIRCRAFT_smcm.m
%-----
 clear all, close all
 A= [0 0 1.74 0.08 0.59;
     0 -1.99 -13.41 -18.95 -3.6;
     0 1 -1.74 -0.08 -0.59;
     0 0 0 -20 0;
     0 0 0 0 -20];
 B=[0 0;0 0;0 0;20 0;0 20];
%----- 
 lambda=[-5.6 4.2 -1 -20 -20];
 nocomp=1;
%固有ベクトルの非零要素の位置 (1:指定、0:指定せず)
 specpos=[ 1  1  1  0  0; 
           1  0  1  0  0; 
           0  1  1  0  0;
           0  0  0  1  0;
           0  0  0  0  1]
%固有ベクトルの要素の値 (0:零、1:非零指定、-1:非零任意)       
 specent=[ 0  0  1 -1 -1;
           1 -1  0 -1 -1;
          -1  1  0 -1 -1;
          -1 -1 -1  1 -1;
          -1 -1 -1 -1  1]
 F=vplace2(A,B,lambda,nocomp,specpos,specent)       
 pl=eig(A-B*F)
%-----
 H=[1 0 1 0 0;1 0 0 0 0];
 Am=A-B*F;
 G=-inv(H*(Am\B));
 Bm=B*G;
%-----
 Q=diag([10,5,5,20,20]);
 S=swfopt(A,B,Q)
%-----
 P2=diag([0.025,0.025]);
 Phi=-20/0.025*P2;
 Check=P2*Phi+Phi*P2
 L=inv(S*B)*(S*Am-Phi*S)
 Ln=inv(S*B)
%-----
 sim('AIRCRAFT_smcm_2015a.mdl')
%-----
%eof
SCILAB

[3] 積分動作導入による追従制御

この場合の制御則は次のように構成されます。

\displaystyle{(20)\quad { \begin{array}{l} u(t)=u_\ell(t)+u_n(t)\\ u_\ell(t)=-\underbrace{(SB)^{-1}(SA-\Phi S)x(t)}_{L=L_{eq}+L_\Phi}\\ -\underbrace{(SB)^{-1}(\Phi S_r+S_1B_r)}_{L_r} r(t) -\underbrace{(SB)^{-1}S_r}_{L_{\dot r}} \dot{r}(t)\\ u_n(t)=-\underbrace{(SB)^{-1}\rho(t,x)}_{L_n}\frac{P_2(s(t)-S_rr(t))}{||P_2(s(t)-S_rr(t))||} \end{array}} }

●ここでは、等価制御による閉ループ系のA_{eq}が望ましい固有値・固有ベクトルをもつように設計して、次式からスイッチング関数を決める行列Sを決定します。

\displaystyle{(21)\quad { S[v_1\cdots v_{n-m}]=0 % \Rightarrow  [v_1\cdots v_{n-m}]^TS^T=0} }

A_{eq}の望ましい固有値を

\displaystyle{(22)\quad \lambda(A_{eq}):\left\{\begin{array}{cl} -5.6\pm j 4.2 & short\ period\ mode\\ -1 & pitch\ attitude\ mode\\ -0.4 & elevator\ deflection\ mode\\ -0.7 & flap\ deflextion\ mode \end{array}\right. }

とし、また望ましいモード分布を次により指定します。

\displaystyle{(23)\quad {\tt specpos}= \left[\begin{array}{ccccc} 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0\\\hline 1 & 1 & 1 & 1 & 1\\ 1 & 0 & 1 & 1 & 1\\ 0 & 1 & 1 & 0 & 1\\\hline 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0\\ \end{array}\right],\quad {\tt specent}= \left[\begin{array}{rrrrr} -1 & -1 & -1 & -1 & -1\\ -1 & -1 & -1 & -1 & -1\\\hline 0 & 0 & 1 & 0 & 1\\ 1 & -1 & 0 & 1 & 0\\ -1 & 1 & 0 & -1 & 0\\\hline -1 & -1 & -1 & -1 & -1\\ -1 & -1 & -1 & -1 & -1\\ \end{array}\right] }

このとき次のような行列Sを得ます。

\displaystyle{(24)\quad S=\left[\begin{array}{rrrrr|rr} 0.0531 & 0.0137 & -0.1436 & -0.0260 & -0.1373 & 0.0500 & 0\\ -0.0072 & -0.0612 & 0.1635 & 0.0035 & 0.1661 & 0 & 0.0500 \end{array}\right] }

このときSMC則における行列Lなどは次のように計算されます。

\displaystyle{(25)\quad L=\left[\begin{array}{rrrrrrr} -1.0616 & -0.2743 & 2.9379 & 0.6060 & 2.4605 & -0.4927 & -0.0900\\ 0.1440 & 1.2236 & -3.3390 & -0.2296 & -3.2769 & 0.0671 & 0.0142 \end{array}\right] }

\displaystyle{(26)\quad L_{r}=\left[\begin{array}{rr} -2.9499 & 0.0120\\ 0.4000 & 2.9391 \end{array}\right] }

\displaystyle{(27)\quad L_{\dot{r}}=\left[\begin{array}{rr} -0.1448 & 0.0013\\ 0.0196 & 0.1439 \end{array}\right] }

図3 積分動作導によるSMC制御系応答

MATLAB
%cAITCRAFT_smci.m
%-----
 clear all, close all
 A0= [0 0 1.74 0.08 0.59;
     0 -1.99 -13.41 -18.95 -3.6;
     0 1 -1.74 -0.08 -0.59;
     0 0 0 -20 0;
     0 0 0 0 -20];
 B0=[0 0;0 0;0 0;20 0;0 20]; 
 H=[1 0 1 0 0;1 0 0 0 0];
 A=[zeros(2,2) -H ;
    zeros(5,2) A0];
 B=[zeros(2,2); B0];
 Gamma=diag([-0.9,-0.7]);
%----- 
 lambda=[-5.6 4.2 -1 -0.4 -0.7];
 nocomp=1;
 specpos=[zeros(2,5);[1 1 1 1 1;1 0 1 1 1;0 1 1 0 1];zeros(2,5)]
 specent=[-ones(2,5);[0 0 1 0 1;1 -1 0 1 0;-1 1 0 -1 0];-ones(2,5)]
 S=swfvpl(A,B,lambda,nocomp,specpos,specent);
 SS=S(:,6:7)\S*0.05
 F=(SS*B)\(SS*A)
 eig(A-B*F)
%-----
 [nn,mm]=size(B);
 S1=SS(:,1:nn-mm);
 S2=SS(:,nn-mm+1:nn);
 Br=eye(nn-mm,mm);
 B2=B(nn-mm+1:nn,:); 
 Lambda=S2*B2;
 P2=diag([0.025,0.025]);
 Phi=-20/0.025*P2;
 Check=P2*Phi+Phi*P2
 L=inv(Lambda)*(SS*A-Phi*SS)
%-----
%Sr=[-0.1448 0.0013;0.0196 0.1439]
 Sr=zeros(2)
 Lr=inv(Lambda)*(Phi*Sr+S1*Br)
 Ldr=inv(Lambda)*Sr
 Ln=0.1*inv(Lambda)
%-----
 sim('AIRCRAFT_smci_2015a.mdl')
%-----
%eof
SCILAB


制御技術セミナー(応用編)目次

C32 積分動作


積分動作導入による追従制御…Homework

[1] 制御対象

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

の出力を、次のコマンド

\displaystyle{(2)\quad \begin{array}{l} \dot{r}(t)=\Gamma(r(t)-r_c)\\ (r(t),r_c\in{\rm\bf R}^m) \end{array} }

に追従させることを考えます(\Gammaは安定行列)。そのために、積分動作

\displaystyle{(3)\quad \begin{array}{l} \dot{x}_r(t)=r(t)-y(t)\\ (x_r(t)\in{\rm\bf R}^m) \end{array} }

を考え、次の拡大系を構成します((1)は標準形であることを仮定)。

\displaystyle{(4)\quad \begin{array}{l} \left[\begin{array}{c} \dot x_r(t)\\ \dot x(t) \end{array}\right] = \left[\begin{array}{c|cc} 0 & -C_1 & -C_2\\\hline 0 & A_{11} & A_{12} \\ 0 & A_{21} & A_{22} \end{array}\right] \left[\begin{array}{c} x_r(t)\\ x(t) \end{array}\right] + \left[\begin{array}{c} 0\\\hline 0\\ B_2 \end{array}\right] u(t) + \left[\begin{array}{c} I_p \\\hline 0 \\ 0 \end{array}\right] r(t) \end{array} }

これを、次のように分割します。

\displaystyle{(5)\quad \begin{array}{l} \left[\begin{array}{c} \dot x_1(t)\\ \dot x_2(t) \end{array}\right] = \left[\begin{array}{cc|c} 0 & -C_1 & -C_2\\ 0 & A_{11} & A_{12} \\\hline 0 & A_{21} & A_{22} \end{array}\right] \left[\begin{array}{c} x_1(t)\\ x_2(t) \end{array}\right] + \left[\begin{array}{c} 0\\ 0\\\hline B_2 \end{array}\right] u(t) + %\underbrace{ \left[\begin{array}{c} I_p \\ 0 \\\hline 0 \end{array}\right] %}_{} r(t)\\ (x_1(t)\in{\rm\bf R}^n, x_2(t)\in{\rm\bf R}^m) \end{array} }

動的システムの振る舞いが閉じ込められる超平面は {\cal S}=\{x:Sx=S_rr\}としますが、r=0の場合のスイッチング関数として、次式を考えます。

\displaystyle{(6)\quad s(t)= \underbrace{ \left[\begin{array}{cc} S_1 & S_2 \\ \end{array}\right] }_{S} \underbrace{ \left[\begin{array}{c} x_1(t)\\ x_2(t) \end{array}\right] }_{x(t)} = \underbrace{S_2 \left[\begin{array}{cc} M & I \\ \end{array}\right] }_{S} \underbrace{ \left[\begin{array}{c} x_1(t)\\ x_2(t) \end{array}\right] }_{x(t)} \ (M=S_2^{-1}S_1) }

(5)に対して、座標変換

\displaystyle{(7)\quad \underbrace{ \left[\begin{array}{c} x_1(t)\\ s(t) \end{array}\right] }_{x'(t)} = \underbrace{ \left[\begin{array}{cc} I & 0 \\ S_1 & S_2 \\ \end{array}\right] }_{T_s} \underbrace{ \left[\begin{array}{c} x_1(t)\\ x_2(t) \end{array}\right] }_{x(t)}\\ \Leftrightarrow \underbrace{ \left[\begin{array}{c} x_1(t)\\ x_2(t) \end{array}\right] }_{x(t)} = \underbrace{ \left[\begin{array}{cc} I & 0 \\ -S_2^{-1}S_1 & S_2^{-1} \\ \end{array}\right] }_{T_s^{-1}} \underbrace{ \left[\begin{array}{c} x_1(t)\\ s(t) \end{array}\right] }_{x'(t)} }

を行って、次式を得ます。

\displaystyle{(8)\quad %\underbrace{ \left[\begin{array}{c} \dot x_1(t)\\ \dot s(t) \end{array}\right] %}_{\dot{x}'(t)} = %\underbrace{ \left[\begin{array}{cc} \bar{A}_{11} & \bar{A}_{12} \\ S_2\bar{A}_{21} & S_2\bar{A}_{22}S_2^{-1} \\ \end{array}\right] %}_{T_sAT_s^{-1}} %\underbrace{ \left[\begin{array}{c} x_1(t)\\ s(t) \end{array}\right] %}_{x'(t)} + \left[\begin{array}{cc} 0\\ S_2B_2 \end{array}\right] u(t) + \left[\begin{array}{cc} B_r \\ S_1B_r \end{array}\right] r(t) }

\displaystyle{(9)\quad \begin{array}{l} \bar{A}_{11}= \left[\begin{array}{cc} 0 & -C_1 \\ 0 & A_{11} \end{array}\right] -\left[\begin{array}{c} -C_2\\ A_{12} \end{array}\right]M\\ \bar{A}_{12}= \left[\begin{array}{c} -C_2\\ A_{12} \end{array}\right]S_2^{-1}\\ \bar{A}_{21}=M \left[\begin{array}{cc} 0 & -C_1 \\ 0 & A_{11} \end{array}\right] + \left[\begin{array}{cc} 0 & A_{21} \end{array}\right] -A_{22}M\\ \bar{A}_{22}=M \left[\begin{array}{c} -C_2\\ A_{12} \end{array}\right] +A_{22}\\ B_r=\left[\begin{array}{cc} I_p \\ 0 \end{array}\right] \end{array} }

以下では、\bar{A}_{11}が安定行列となるようにスイッチング関数が選ばれていると仮定します。

このとき、SMC則

\displaystyle{(10)\quad { u(t)=\underbrace{u_\ell(t)}_{linear\ control}+\underbrace{u_n(t)}_{switching\ component}} }

を、2次安定性

\displaystyle{(11)\quad { \begin{array}{lll} V(s)=(s(t)-S_rr(t))^TP_2(s(t)-S_rr(t))&\\ \Rightarrow \dot{V}(s)\le -(s(t)-S_rr(t))^T(s(t)-S_rr(t))&(t\le t_s)\\ V(x_1)=x_1^T(t)P_1x_1(t)&\\ \Rightarrow \dot{V}(x_1)\le -x_1^T(t)Q_1x_1(t)&(t> t_s) \end{array}} }

が成り立つように決定します(P_1>0, P_2>0, Q_1>0)。

[2] 可到達性の検討 (t\le t_s)

等価制御は、(8)においてr=0の場合

\displaystyle{(12)\quad \begin{array}{l} s(t)=0\Rightarrow\dot{s}(t)=0\\ \Rightarrow 0=S_2\bar{A}_{21}x_1(t)+S_2\bar{A}_{22}S_2^{-1}s(t)+S_2B_2u(t)\\ \Rightarrow u_{eq}(t)=-\underbrace{(S_2B_2)^{-1}}_{(SB)^{-1}} \underbrace{\left[\begin{array}{cc} S_2\bar{A}_{21} & S_2\bar{A}_{22}S_2^{-1} \\ \end{array}\right]x'(t)}_{SAx(t)}} \end{array} }

のように得られます。(10)の第1項u_\ellは、この等価制御をベースして、\dot{s}(t)-S_r\dot{r}(t)=0を満足させるように

\displaystyle{(13)\quad { \begin{array}{l} u_\ell(t)=-\underbrace{(S_2B_2)^{-1}}_{(SB)^{-1}} \underbrace{(\left[\begin{array}{cc} S_2\bar{A}_{21} & S_2\bar{A}_{22}S_2^{-1} \\ \end{array}\right]x'(t)-\Phi s(t))}_{(SA-\Phi S)x(t)}}\\ -(S_2B_2)^{-1}(\Phi S_r+S_1B_r)r(t)+(S_2B_2)^{-1}S_r\dot{r}(t) \end{array}} }

のように構成します(\Phiは安定行列)。このとき閉ループ系は次式で与えられます。

\displaystyle{(14)\quad \begin{array}{l} \left[\begin{array}{c} \dot{x}_1(t)\\ \dot{s}(t)-S_r\dot{r}(t) \end{array}\right] = \left[\begin{array}{cc} \bar{A}_{11} & \bar{A}_{12} \\ 0 & \Phi \\ \end{array}\right] \left[\begin{array}{c} x_1(t)\\ s(t)-S_rr(t) \end{array}\right] + \left[\begin{array}{c} B_rr(t)\\ S_2B_2u_n(t) \end{array}\right] \end{array} }

すなわち

\displaystyle{(15)\quad \begin{array}{l} \dot{x}_1(t)=\bar{A}_{11}x_1(t)+\bar{A}_{12}(s(t)-S_rr(t))+B_rr(t)\\ \dot{s}(t)-S_r\dot{r}(t)=\Phi (s(t)-S_rr(t))+S_2B_2u_n(t) \end{array} }

ここで、\Phiは安定行列なので

\displaystyle{(16)\quad \begin{array}{l} P_2\Phi+\Phi^TP_2=-I \end{array} }

を満たすP_2>0を選ぶことができます。これを用いて

\displaystyle{(17)\quad { u_n(t)=-(S_2B_2)^{-1}\rho(u_\ell,y)\frac{P_2(s(t)-S_rr(t))}{||P_2(s(t)-S_rr(t))||}} }

と選びます(\rho>0)。このとき次式が成り立ちます。

\displaystyle{(18)\quad \begin{array}{l} \dot{V}(s)=2(s(t)-S_rr(t))^TP_2(\dot{s}(t)-S_r\dot{r}(t))\\ =2(s(t)-S_rr(t))^TP_2 (\Phi (s(t)-S_rr(t))-\rho(u_\ell,y)\frac{P_2(s(t)-S_rr(t))}{||P_2(s(t)-S_rr(t))||})\\ =(s(t)-S_rr(t))^T(P_2\Phi+\Phi^TP_2)(s(t)-S_rr(t))\\ +2(s(t)-S_rr(t))^TP_2(-\rho(u_\ell,y)\frac{P_2(s(t)-S_rr(t))}{||P_2(s(t)-S_rr(t))||})\\ \le -||s(t)-S_rr(t)||^2-2\rho(u_\ell,y)||P_2(s(t)-S_rr(t))||\\ \le -||s(t)-S_rr(t)||^2 \end{array} }

[3] スライディングモードの検討 (t> t_s)

\bar{A}_{11}は安定行列なので

\displaystyle{(19)\quad \begin{array}{l} P_1\bar{A}_{11}+\bar{A}_{11}^TP_1=-Q_1<0 \end{array} }

を満たすP_1>0を選ぶことができます。

\displaystyle{(20)\quad \begin{array}{l} \dot{V}(x_1)=2x_1^T(t)P_1\dot{x}_1(t)\\ =2x_1^T(t)P_1( \bar{A}_{11}x_1(t)+\bar{A}_{12}\underbrace{(s(t)-S_rr(t))}_{0}+B_rr(t))\\ =x_1^T(t(P_1\bar{A}_{11}+\bar{A}_{11}^TP_1)x_1(t)+2x_1^T(t)P_1B_rr(t)\\ =-x_1^T(t)Q_1x_1(t)+2x_1^T(t)P_1B_rr(t)\\ \le -\sigma_n^2(Q_1)x_1^T(t)x_1(t)+2||x_1^T(t)P_1||\,||B_rr(t)||\\ \le -\sigma_n^2(Q_1)||x_1(t)||^2+2\sqrt{x_1^T(t)P_1^2x_1(t)}\,||r(t)||\\ \le -\sigma_n^2(Q_1)||x_1(t)||^2+2\sqrt{\sigma_1^2(P_1^2)x_1^T(t)x_1(t)}\,||r(t)||\\ = -\sigma_n^2(Q_1)||x_1(t)||^2+2\sigma_1^2(P_1)||x_1(t)||\,||r(t)||\\ = -\sigma_1^2(P_1)||x_1(t)||(\frac{\sigma_n^2(Q_1)}{\sigma_1^2(P_1)}||x_1(t)||-2||r(t)||) \end{array} }
したがって

\displaystyle{(21)\quad||r(t)||<\frac{1}{2}\frac{\sigma_n^2(Q_1)}{\sigma_1^2(P_1)}||x_1(t)|| \Rightarrow \dot{V}(x_1)<0 }

演習…Flipped Classroom

MATLAB
%cCIP_smci.m
%-----
 clear all, close all
 global mc m ell g th0
 mc=1; m=0.1; ell=0.2; g=9.8;
 ths=0; %input('ths   = <0,180> ')/180*pi;
 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); 
 A0=A; B0=B; 
 H=[1 0 0 0];
 A=[0 -H ;
    zeros(4,1) A0];
 B=[0; B0];
 CM=[1 0 0 0;
     0 1 0 0];
 Gamma=-5; 
%-----
 lambda=[-0.8 0.5 -1.5 1];
 nocomp=2;
 specpos=rand(4,4);
 specent=rand(4,4);
 S=swfvpl(A,B,lambda,nocomp,specpos,specent)      
 SS=S;
 F=(SS*B)\(SS*A)
 eig(A-B*F)
%-----
[nn,mm]=size(B);
 S1=SS(:,1:nn-mm);
 S2=SS(:,nn-mm+1:nn);
 Br=eye(nn-mm,mm);
 B2=B(nn-mm+1:nn,:); 
 Lambda=S2*B2;
 Phi=-0.1;
 P2=0.5*inv(-Phi);
 Check=P2*Phi+Phi*P2
 P2S=P2*S;
 Leq=inv(S*B)*S*A;
 LPhi=-inv(S*B)*Phi*S;
 L=Leq+LPhi;
 rho=2;
 Ln=inv(S*B)*rho;
%-----
 Sr=0;
 Lr=inv(Lambda)*(Phi*Sr+S1*Br)
 Ldr=inv(Lambda)*Sr
 Ln=0.1*inv(Lambda)
%-----
 x0=[0;th0*0;0;0];
 sim('CIP_smci_2015a.mdl')
%-----
%eof
SCILAB

C31 モデル規範による追従制御

モデル規範による追従制御…Homework

[1] 次の状態方程式で表される制御対象を考えます。

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

この状態が理想的なモデル

\displaystyle{(2)\quad \dot{x}_m(t)=A_mx_m(t)+B_mr(t) }

の状態を追従するように、すなわち追従誤差e(t)について

\displaystyle{(3)\quad e(t)=x(t)-x_m(t)\rightarrow 0 \quad(t\rightarrow\infty) }

となるように制御則を決定したいとします。これは状態オブザーバの設計によく似ていますが、モデル(2)の入力が新規の入力r(t)であることが違います。

追従誤差のダイナミックスは

\displaystyle{(4)\quad \begin{array}{l} %\bullet \dot{e}(t)=Ax(t)+Bu(t)-(A_mx_m(t)+B_mr(t))\\ %=Ax(t)-Ax_m(t)+Ax_m(t)-A_mx_m(t)+Bu(t)-B_mr(t)\\ %=Ae(t)+(A-A_m)x_m(t)+Bu(t)-B_mr(t)\\ %\bullet \dot{e}(t)=Ax(t)+Bu(t)-(A_mx_m(t)+B_mr(t))\\ =Ax(t)-A_mx(t)+A_mx(t)-A_mx_m(t)+Bu(t)-B_mr(t)\\ =A_me(t)+(A-A_m)x(t)+Bu(t)-B_mr(t) \end{array} }

となります。いま条件

\displaystyle{(5)\quad \begin{array}{l} (BB^\dag-I)(A-A_m)=0\Leftrightarrow B\underbrace{B^\dag(A-A_m)}_{F}=A-A_m\\ (BB^\dag-I)B_m=0\Leftrightarrow B\underbrace{B^\dag B_m}_{G}=B_m \end{array} }

を仮定すると(B^\dagBB^\dag B=Bを満たす疑似逆行列)、制御則

\displaystyle{(6)\quad u(t)=-Ke(t)-Fx(t)+Gr(t) }

の下では、誤差方程式は次式となります。

\displaystyle{(7)\quad \dot{e}(t)=(A_m-BK)e(t) }

実際、

\displaystyle{(8)\quad \begin{array}{l} \dot{e}=A_me+(A-A_m)x+Bu-B_mr\\ =A_me+(A-A_m)x+B(-Ke-Fx+Gr)-B_mr\\ =A_me+(A-A_m)x-BKe-BB^\dag(A-A_m)x+BB^\dag B_mr-B_mr\\ =(A_m-BK)e-\underbrace{(BB^\dag-I)(A-A_m)}_{0}x+\underbrace{(BB^\dag -I)B_m}_{0}r\\ \end{array} }

以上から、制御則(6)の設計の指針として、まずFGは(5)より

\displaystyle{(9)\quad \begin{array}{l} A_m=A-BF\\ B_m=BG \end{array} }

だから、望ましいモデル(2)を(1)に対する状態フィードバックと入力変換で得るものとします。特にGrから出力

\displaystyle{(10)\quad y(t)=Hx(t) }

までの定常ゲイン

\displaystyle{(11)\quad \left\{\begin{array}{l} 0=(A-BF)x(\infty)+BGr(\infty)\\ y(\infty)=Hx(\infty) \end{array}\right. \Rightarrow y(\infty)=\underbrace{-H(A-BF)^{-1}BG}_{I}r(\infty) }

が単位行列となるように、次式で決めます。

\displaystyle{(12)\quad G=-(H(A-BF)^{-1}B)^{-1} }

次にKですが、(7)が安定行列であることが前提となり、望ましいモデルより、追従誤差は速いダイナミックスをもつことが必要であることに注意します。

[2] そこでスライディングモード制御を適用することを考えます。まずスイッチング関数を

\displaystyle{(13)\quad s(t)=Se(t) }

とするとき、スライディングモード時は

\displaystyle{(14)\quad Se(t)=0\Rightarrow S\dot{e}(t)=0 }

すなわち

\displaystyle{(15)\quad S\dot{e}(t)=S(A_me(t)+(A-A_m)x(t)+Bu(t)-B_mr(t))=0 }

が成り立ち、等価制御は次式となります。

\displaystyle{(16)\quad u_{eq}(t)=-(SB)^{-1}S(A_me(t)+(A-A_m)x(t)-B_mr(t)) }

このとき追従誤差のダイナミックスは次式となります。

\displaystyle{(17)\quad \begin{array}{l} \dot{e}=A_me+(A-A_m)x+Bu-B_mr\\ =A_me+(A-A_m)x+B(-(SB)^{-1}S(A_me+(A-A_m)x-B_mr))-B_mr\\ =(I-B(SB)^{-1}S)(A_me+(A-A_m)x-B_mr)\\ =(I-B(SB)^{-1}S)(A_me+BFx-BGr)\\ =\underbrace{(I-B(SB)^{-1}S)A_m}_{A_{eq}}e \end{array} }

したがって、スライディングモード制御を適用したときの、全体の制御則は、次式で表されます。

\displaystyle{(18)\quad { \begin{array}{l} u(t)=-Fx(t)+Gr(t)+u_\ell(t)+u_n(t)\\ u_\ell(t)=-(SB)^{-1}(SA_m-\Phi S)e(t)\\ u_n(t)=-(SB)^{-1}\rho(t,e)\frac{P_2s(t)}{||P_2s(t)||} \end{array}} }

ただし

\displaystyle{(19)\quad P_2\Phi+\Phi^TP_2=-I }

演習…Flipped Classroom

MATLAB
%cCIP_smcm.m
%-----
 clear all, close all
 global mc m ell g th0
 mc=1; m=0.1; ell=0.2; g=9.8;
 ths=0; %input('ths   = <0,180> ')/180*pi;
 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); 
 H=[1 0 0 0];
%-----
 lambda=[-0.8*1.5 0.5 -1.5*1.5 1];
 nocomp=2;
 specpos=rand(4,4);
 specent=rand(4,4);
 F=vplace2(A,B,lambda,nocomp,specpos,specent)       
 pl=eig(A-B*F)
%-----
 H=[1 0 0 0];
 Am=A-B*F;
 G=-inv(H*(Am\B));
 Bm=B*G;
%-----
 Mr=0.5; Mth=3/180*pi; 
 Tc=1; Tpen=0.25*2*pi*sqrt(2*ell/g);
 Q=diag([1/Mr^2,1/Mth^2,Tc^2/Mr^2,Tpen^2/Mth^2]);
 S=swflqr(A,B,Q) 
%-----
 Phi=-0.1;
 P2=0.5*inv(-Phi);
 Check=P2*Phi+Phi*P2
 P2S=P2*S;
 Leq=inv(S*B)*S*A;
 LPhi=-inv(S*B)*Phi*S;
 L=Leq+LPhi;
 rho=1;
 Ln=inv(S*B)*rho;
%-----
 x0=[0;th0*0;0;0];
 sim('CIP_smcm_2015a.mdl')
%-----
%eof
SCILAB