船舶のロバスト制御

船舶のロバスト制御…Homework

図1 制御対象（船舶の回頭運動制御）

●船舶の針路制御のために、次のNomotoモデルを考えます。

$\displaystyle{(1)\quad \dot{r}(t)=-\frac{1}{T(t)}r(t)+\frac{K(t)}{T(t)}\delta(t-L)+w(t)} }$

ただし、 $r(t)=\dot{\psi}(t)$ かつ

$\displaystyle{(2)\quad T(t)=\frac{L}{U(t)}T',\ K(t)=\frac{U(t)}{L}K' \quad (U_1\le U(t)\le U_2) }$

ここで、舵の効果が出るまでの無駄時間 $L$ を想定しています。混同の恐れはないので船長 $L$ と同じ表記を用います。また前進速度 $U$ の変動により、時定数 $T$ と定常ゲイン $K$ が変動しますが、次式から舵は前進速度の２乗で効いてくることがわかります。

$\displaystyle{(3)\quad \dot{r}(t)=-\underbrace{\left(\frac{U(t)}{L}\right)\frac{1}{T'}}_{a(t)=\frac{1}{T(t)}}r(t) +\underbrace{\left(\frac{U(t)}{L}\right)^2\frac{K'}{T'}}_{b(t)=\frac{K(t)}{T(t)}}\delta(t-L)+w(t) }$

いま、ある前進速度 $U^*$ に注目しますと、次の表現もできます。

$\displaystyle{(4a)\quad \dot{r}(t)=-\underbrace{\left(\frac{U(t)}{U^*}\right)\frac{1}{T^*}}_{a(t)=\frac{1}{T(t)}}r(t) +\underbrace{\left(\frac{U(t)}{U^*}\right)^2\frac{K^*}{T^*}}_{b(t)=\frac{K(t)}{T(t)}}\delta(t-L)+w(t) }$

ただし

$\displaystyle{(4b)\quad T^*=\frac{L}{U^*}T',\ K^*=\frac{U^*}{L}K' \quad (U_1\le U^*\le U_2) }$

いま、次のような前進速度の変動を考えます。このとき、単位フィードバックによる閉ループ系は大きく変動します。

図2 前進速度が変動する場合の回頭運動

●ある前進速度 $U^*$ を基準にしたNOMOTOモデルを考えます。

$\displaystyle{(5)\quad \left\{\begin{array}{l} \dot{\psi}(t)=r(t) \\ \displaystyle{\dot{r}(t)=-\left(\frac{U}{U^*}\right)\frac{1}{T^*}r(t) +\left(\frac{U}{U^*}\right)^2\frac{K^*}{T^*}\delta(t)} \end{array}\right. }$

これに、舵のダイナミクス

$\displaystyle{(6)\quad \dot{\delta}(t)=-\frac{1}{T_a}\delta(t)+\frac{K_a}{T_a}u(t) }$

を接続した状態空間表現を次のように表します。

$\displaystyle{(7a)\quad \underbrace{ \left[\begin{array}{c} \dot{\psi}(t) \\ \dot{r}(t) \\ \dot{\delta}(t) \end{array}\right] }_{\dot{x}(t)} = \underbrace{ \left[\begin{array}{ccc} 0 & 1 & 0\\ 0 & -\left(\frac{U}{U^*}\right)\frac{1}{T^*} & \left(\frac{U}{U^*}\right)^2\frac{K^*}{T^*} \\ 0 & 0 & -\frac{1}{T_a} \end{array}\right] }_{A(U,U^2)} \underbrace{ \left[\begin{array}{c} \psi(t) \\ r(t) \\ \delta(t) \end{array}\right] }_{x(t)} + \underbrace{ \left[\begin{array}{c} 0 \\ 0 \\ \frac{K_a}{T_a} \end{array}\right] }_{B} u(t) }$
$\displaystyle{(7b)\quad \underbrace{ \psi(t) }_{y(t)} = \underbrace{ \left[\begin{array}{ccc} 1 & 0 & 0 \end{array}\right] }_{C} \underbrace{ \left[\begin{array}{c} \psi(t) \\ r(t) \\ \delta(t) \end{array}\right] }_{x(t)} }$

ここで、次のようなポリトピック表現ができることに注目します（本式は未発表です）。

$\displaystyle{(8a)\quad A(U,U^2)=p_1(U,U^2)A_1+p_2(U,U^2)A_2+p_3(U,U^2)A_3 }$

ただし

$\displaystyle{(8b)\quad \begin{array}{l} A_1=A(U_1,U_1^2)\\ A_2=A(U_2,U_2^2)\\ A_3=A(\frac{U_1+U_2}{2},U_1U_2) \end{array} }$

また、 $U_3=\frac{U_1+U_2}{2}$ を定義して

$\displaystyle{(8c)\quad \begin{array}{l} p_1(U,U^2)=\frac{1}{p_0}\det \left[\begin{array}{cc} U-U_3 & U_2-U_3 \\ U^2-U_1U_2 & U_2^2-U_1U_2 \\ \end{array}\right]\\ p_2(U,U^2)=\frac{1}{p_0}\det \left[\begin{array}{cc} U_1-U_3 & U-U_3 \\ U_1^2-U_1U_2 & U^2-U_1U_2 \\ \end{array}\right]\\ p_3(U,U^2)=\frac{1}{p_0}\det \left[\begin{array}{cc} U_1-U_2 & U_2-U \\ U_1^2-U_2^2 & U_2^2-U^2 \\ \end{array}\right]\\ p_0=\det \left[\begin{array}{cc} U_1-U_2 & U_2-U_3 \\ U_1^2-U_2^2 & U_2^2-U_1U_2 \\ \end{array}\right]\\ p_1(U,U^2)+p_2(U,U^2)+p_3(U,U^2)=1 \end{array} }$

これは端点 $(U_1,U_1^2)$ 、 $(U_2,U_2^2)$ と仮想的な端点 $(U_3,U_1U_2)$ が作る３角形領域における変動をカバーすることができます。

図3 船舶のパラメータ変動凸領域

●これに基づいて、次のようなLPV制御の仕組みを考えます。

図4 船舶のLPV制御の仕組み

まず次の２ポートシステムを構成します。

$\displaystyle{(9a)\quad P: \left\{\begin{array}{l} \left[\begin{array}{c} \dot{x} \\ \dot{x}_I \end{array}\right]= \underbrace{ \left[\begin{array}{cc} A(U,U^2)& 0 \\ -C & 0 \end{array}\right] }_{A(U,U^2)} \left[\begin{array}{c} x \\ x_I \end{array}\right] + \underbrace{ \left[\begin{array}{c} 0 \\ 1 \end{array}\right] }_{B_1} r + \underbrace{\left[\begin{array}{c} B \\ 0 \end{array}\right] }_{B_2} u\\ \underbrace{ \left[\begin{array}{c} y_{11} \\ y_{12} \end{array}\right] }_{y_1} = \underbrace{ \left[\begin{array}{cc} 0 &\omega_I\\ \omega_DCA(U,U^2) & 0 \end{array}\right] }_{C_1} \left[\begin{array}{c} x \\ x_I \end{array}\right] + \underbrace{ \left[\begin{array}{c} 0 \\ 0 \end{array}\right] }_{D_{11}} r + \underbrace{ \left[\begin{array}{c} 0 \\ \omega_DCB \end{array}\right] }_{D_{12}} u\\ \underbrace{ \left[\begin{array}{c} y \\ x_I \end{array}\right] }_{y_2} = \underbrace{ \left[\begin{array}{cc} C & 0\\ 0 & 1 \end{array}\right] }_{C_2} \left[\begin{array}{c} x \\ x_I \end{array}\right] + \underbrace{ \left[\begin{array}{c} 0 \\ 0 \end{array}\right] }_{D_{21}} r \end{array}\right.}$

$\displaystyle{(9b)\quad A(U,U^2)=p_1(U,U^2)A_1+p_2(U,U^2)A_2+p_3(U,U^2)A_3 }$

次に、これに対して、次のコントローラを設計します。

$\displaystyle{(10a)\quad K_0: \left\{\begin{array}{l} \dot{x}_K=A_K(U,U^2)x_K+ \underbrace{ \left[\begin{array}{cc} B_K^{(1)}(U,U^2) & B_K^{(2)}(U,U^2) \end{array}\right] }_{B_K(U,U^2)} \left[\begin{array}{c} y \\ x_I \end{array}\right] \\ u=C_K(U,U^2)x_K + \underbrace{ \left[\begin{array}{cc} D_K^{(1)}(U,U^2) & D_K^{(2)}(U,U^2) \end{array}\right] }_{D_K(U,U^2)} \left[\begin{array}{c} y \\ x_I \end{array}\right] \end{array}\right.}$

$\displaystyle{(10b)\quad \begin{array}{l} A_K(U,U^2)=p_1(U,U^2)A_{K1}+p_2(U,U^2)A_{K2}+p_3(U,U^2)A_{K3}\\ B_K(U,U^2)=p_1(U,U^2)B_{K1}+p_2(U,U^2)B_{K2}+p_3(U,U^2)B_{K3}\\ C_K(U,U^2)=p_1(U,U^2)C_{K1}+p_2(U,U^2)C_{K2}+p_3(U,U^2)C_{K3}\\ D_K(U,U^2)=p_1(U,U^2)D_{K1}+p_2(U,U^2)D_{K2}+p_3(U,U^2)D_{K3} \end{array} }$

これから、次の出力フィードバックを得ます。

$\displaystyle{(11)\quad K: \left\{\begin{array}{l} \left[\begin{array}{c} \dot{x}_K \\ \dot{x}_I \end{array}\right]= \underbrace{ \left[\begin{array}{cc} A_K(U,U^2) & B_K^{(2)}(U,U^2) \\ 0 & 0 \end{array}\right] }_{A_K(U,U^2)} \left[\begin{array}{c} x_K \\ x_I \end{array}\right]\\ + \underbrace{ \left[\begin{array}{cc} B_K^{(1)}(U,U^2) & 0\\ -1& 1 \end{array}\right] }_{B_K(U,U^2)} \left[\begin{array}{c} y \\ r \end{array}\right] \\ u= \underbrace{ \left[\begin{array}{cc} C_K(U,U^2) & D_K^{(2)}(U,U^2) \end{array}\right] }_{C_K(U,U^2)} \left[\begin{array}{c} x_K \\ x_I \end{array}\right]\\ + \underbrace{ \left[\begin{array}{cc} D_K^{(1)}(U,U^2) & 0 \end{array}\right] }_{D_K(U,U^2)} \left[\begin{array}{c} y \\ r \end{array}\right] \end{array}\right. }$

●いま、 $U=U^*$ として設計した $H_\infty$ 制御による閉ループ系のシミュレーションを次に示します。３種類の速度変動に応じてばらつきか見られます。

図5 船舶のLTI制御のシミュレーションと性能評価

●これに対して、速度変動に応じたLPV制御による閉ループ系のシミュレーションを次に示します。 $H_\infty$ 制御の場合のばらつきが抑制されていることが分かります。

図6 船舶のLOP制御のシミュレーションと性能評価

演習B63…Flipped Classroom
$1^\circ$ 図2は次のプログラムを用いて得られている。これを参考にして、MATLAB用プログラムを作成しなさい。

MATLAB

SCILAB

//ship7.sce
//-----
T1=118; T2=7.8; T3=18.5; Tship=T1+T2-T3; Kship=0.185; tL=6;
Tdelta=1; Kdelta=1;
//
Us=7.7; U1=Us*0.5; U2=Us*1.5; U3=(U1+U2)/2; U32=U1*U2;
a0=[ 0  1          0                 ;
     0  0          0                 ;
     0  0         -1/Tdelta          ];
a1=[ 0  0          0                 ;
     0 -1/Us/Tship 0                 ;
     0  0          0                 ];
a2=[ 0  0          0                 ;
     0  0          1/Us^2*Kship/Tship;
     0  0          0                 ];
A1=a0+U1*a1+U1^2*a2; 
A2=a0+U2*a1+U2^2*a2;  
A3=a0+U3*a1+U32*a2;
B0=[0;0;Kdelta/Tdelta];
//-----
function Q=interp3(P1,P2,P3,P)
 x1=P1(1); x2=P2(1); x3=P3(1); x=P(1);
 y1=P1(2); y2=P2(2); y3=P3(2); y=P(2);
 alpha=((x1-x2)*(y2-y3)-(x2-x3)*(y1-y2));
 Q(1) =((x -x2)*(y2-y3)-(x2-x3)*(y -y2))/alpha;
 Q(2) =((x1-x3)*(y -y3)-(x -x3)*(y1-y3))/alpha;
 Q(3) =((x1-x2)*(y2-y )-(x2-x )*(y1-y2))/alpha;
endfunction
//
function Ut=U(t,ID)
 if ID==0, Ut=Us,
 else if t<=ID*100, Ut=Us-(Us-U1)/(ID*100)*t, else Ut=U1, end, end
endfunction
//
function dxG=fG(t,xG1), dxG=AG(t)*xG1+B0*ut, endfunction
//
function AGt=AG(t), 
 P1=[U1;U1^2]; P2=[U2;U2^2]; P3=[U3;U3^2]; Ut=U(t,ID); P=[Ut;Ut^2]; 
 Q=interp3(P1,P2,P3,P); p1=Q(1); p2=Q(2); p3=Q(3); 
 AGt=p1*A1+p2*A2+p3*A3;
endfunction
//-----
t0=0; t1=300; nt=1500; td=(t1-t0)/nt; t=t0:td:t1; iL=tL/td;
v0=0; v1=0; v2=0; v3=0; 
for i=1:nt
 v0=[v0 U(t(i),0)]; //constant velocity
 v1=[v1 U(t(i),1)]; //rapid velocity variation
 v2=[v2 U(t(i),2)]; //medium velocity variation
 v3=[v3 U(t(i),3)]; //slow velocity variation
end
clf(0)
scf(0);subplot(211),plot(t,v0,'b',t,v1,'r',t,v2,'m',t,v3,'k'),mtlb_grid,mtlb_axis([t0 t1 0 10]);
legend(['const.vel.';'rapid vel.var.';'medium vel.var.';'slow vel.var.'])
//
clf(1)
xG=zeros(3,1); y=0; u=0; ID=0
for i=1:nt
 if i<=iL, yt=0;         ut=0;
 else      yt=y(:,i-iL); ut=1-yt; end
 xG=[xG ode(xG(:,i),t(i),t(i+1),fG)];
 y=[y xG(1,i+1)]; u=[u ut];
end
scf(0);subplot(212),plot(t,y,'b');
//
xG=zeros(3,1); y=0; u=0; ID=1
for i=1:nt
 if i<=iL, yt=0;         ut=0;
 else      yt=y(:,i-iL); ut=1-yt; end
 xG=[xG ode(xG(:,i),t(i),t(i+1),fG)];
 y=[y xG(1,i+1)]; u=[u ut];
end
scf(0);subplot(212),plot(t,y,'r')
//
xG=zeros(3,1); y=0; u=0; ID=2
for i=1:nt
 if i<=iL, yt=0;         ut=0;
 else      yt=y(:,i-iL); ut=1-yt; end
 xG=[xG ode(xG(:,i),t(i),t(i+1),fG)];
 y=[y xG(1,i+1)]; u=[u ut];
end
scf(0);subplot(212),plot(t,y,'m')
//
xG=zeros(3,1); y=0; u=0; ID=3
for i=1:nt
 if i<=iL, yt=0;         ut=0;
 else      yt=y(:,i-iL); ut=1-yt; end
 xG=[xG ode(xG(:,i),t(i),t(i+1),fG)];
 y=[y xG(1,i+1)]; u=[u ut];
end
scf(0);subplot(212),plot(t,y,'k'),mtlb_grid,mtlb_axis([t0 t1 -1 3]);
legend(['const.vel.';'rapid vel.var.';'medium vel.var.';'slow vel.var.'])
//-----
//eof

$2^\circ$ 図5は次のプログラムを用いて得られている。これを参考にして、MATLAB用プログラムを作成しなさい。

MATLAB

SCILAB

//ship8.sce
//-----
function [LME,LMI,OBJ]=synlmiof5(YLIST)
 [gam,R,S,AK,BK,CK,DK]=YLIST(:);
 LME1=R-R';
 LME2=S-S';
 LME=list(LME1,LME2);
 LMI0=[R eye(A);eye(A) S];
 AW=[A*R+B2*CK A+B2*DK*C2
     AK S*A+BK*C2];
 LMI1=-(AW+AW'+2*alpha*LMI0);
 LMI2=-[-r*LMI0  AW;
        AW'      -r*LMI0];
 LMI3=-[ sin(th)*AW   cos(th)*AW;
        -cos(th)*AW   sin(th)*AW];
 LMI3=LMI3+LMI3';
 BW=[B1+B2*DK*D21
     S*B1+BK*D21];
 CW=[C1*R+D12*CK C1+D12*DK*C2];
 [p,m]=size(D11);
 LMI4=-[AW+AW' BW CW'
        BW' -gam*eye(m,m) (D11+D12*DK*D21)'
        CW D11+D12*DK*D21 -gam*eye(p,p)];
 [p,m]=size(DK);
 LMI5=-[-1e0*eye(m,m) DK'
        DK -1e0*eye(p,p)];
 LMI=list(LMI0,LMI1,LMI2,LMI3,LMI4,LMI5);
 OBJ=gam;
endfunction
//-----
function [AK,BK,CK,DK]=syncont(R,S,ak,bk,ck,dk)
 [u,sd,v]=svd(eye()-S*R); Ni=sqrt(sd)\u'; Mti=v/sqrt(sd); 
 AK=Ni*(ak-S*(A-B2*dk*C2)*R-bk*C2*R-S*B2*ck)*Mti; 
 BK=Ni*(bk-S*B2*dk); 
 CK=(ck-dk*C2*R)*Mti;
 DK=dk; 
endfunction
//=====
T1=118; T2=7.8; T3=18.5; Tship=T1+T2-T3; Kship=0.185;
Tdelta=1; Kdelta=1; tL=9; wD=tL; wI=0.01;
//
A0=[0 1 0;0 -1/Tship Kship/Tship;0 0 -1/Tdelta]; B0=[0;0;Kdelta/Tdelta]; 
C0=[1 0 0];
A=[A0 zeros(3,1);-C0 0]; B1=[zeros(3,1);1]; B2=[B0;0];
C1=[zeros(1,3) wI;wD*C0*A0 0;zeros(1,4)]; D11=zeros(3,1); D12=[0;wD*C0*B0;1];
C2=[C0 0;zeros(1,3) 1]; D21=[0;0]; D22=[0;0];
//-----
alpha=0.01; r=10; th=%pi/16;
gam0=100; R0=eye(4,4); S0=eye(4,4);
AK0=-eye(4,4); BK0=ones(4,2); CK0=ones(1,4); DK0=ones(1,2);
YLIST0=list(gam0,R0,S0,AK0,BK0,CK0,DK0);
YLIST=lmisolver(YLIST0,synlmiof5);
[gam,R,S,ak,bk,ck,dk]=YLIST(:); 
//
[AK,BK,CK,DK]=syncont(R,S,ak,bk,ck,dk)
plK=spec(AK)
ACL=[A+B2*DK*C2 B2*CK;BK*C2 AK];
BCL=[B1+B2*DK*D21;
     BK*D21];
CCL=[C2(1,:) zeros(1,4)];
plCL=spec(ACL)
//-----
AK=[AK BK(:,2);zeros(1,5)];
BK=[BK(:,1) zeros(4,1); -1 1];
CK=[CK DK(:,2)];
DK=[DK(:,1) 0];
AL=[A B2*CK;zeros(5,4) AK];
BL=[B2*DK; BK];
CL=[C2 D22*CK];
DL=D22*DK;
//-----
clf(0),clf(1)
w=logspace(-3,0,100); nw=length(w);
g=freq(A0,B0,C0,%i*w);
for i=1:nw, ga(i)=20*log10(norm(g(:,i))); end
scf(0);plot2d(w,ga,logflag='ln')
gws=20*log10(abs(wI./(%i*w))); gwt=20*log10(abs(wD^(-1)./(%i*w)));
scf(0);plot2d(w,gws,logflag='ln')
scf(0);plot2d(w,gwt,logflag='ln')
g=freq(AL,BL(:,1),CL(1,:),DL(1,1),%i*w); 
for i=1:nw, ga(i)=20*log10(abs(g(:,i))); end
scf(0);plot2d(w,ga,logflag='ln'),mtlb_grid
g=freq(ACL,BCL,CCL,%i*w); 
for i=1:nw, ga(i)=20*log10(abs(g(:,i))); end
scf(1);plot2d(w,ga,logflag='ln')
for i=1:nw, ga(i)=20*log10(abs(1-g(:,i))); end
scf(1);plot2d(w,ga,logflag='ln')
scf(1);plot2d(w,-gws,logflag='ln')
scf(1);plot2d(w,gwt,logflag='ln'),mtlb_grid
//-----
clf(2)
AG=[0 1;0 -1/Tship]; BG=[0 0;Kship/Tship 1]; CG=[1 0];
function dxG=fG(t,xG),dxG=AG*xG+BG*ut, endfunction
function dxK=fK(t,xK),dxK=AK*xK+BK*yt, endfunction
t0=0; t1=300; nt=1500; td=(t1-t0)/nt; t=t0:td:t1; iL=tL/td;
//
xG=zeros(2,1); y=0; xK=zeros(5,1); u=0; w=0
for i=1:nt
 if i<=iL, yt=[0;0];         ut=[0;w];
 else      yt=[y(:,i-iL);1]; ut=[CK*xK(:,i)+DK*yt;w]; end
 xG=[xG ode(xG(:,i),t(i),t(i+1),fG)]; 
 xK=[xK ode(xK(:,i),t(i),t(i+1),fK)];
 y=[y CG*xG(:,i+1)]; u=[u ut(1)];
end
scf(2);subplot(211),plot(t,y,'r')
scf(2);subplot(212),plot(t,u,'r')
//
xG=zeros(2,1); y=0; xK=zeros(5,1); u=0; w=0.5*1e-3
for i=1:nt
 if i<=iL, yt=[0;0];         ut=[0;w];
 else      yt=[y(:,i-iL);1]; ut=[CK*xK(:,i)+DK*yt;w]; end
 xG=[xG ode(xG(:,i),t(i),t(i+1),fG)]; 
 xK=[xK ode(xK(:,i),t(i),t(i+1),fK)];
 y=[y CG*xG(:,i+1)]; u=[u ut(1)];
end
scf(2);subplot(211),plot(t,y,'r--'),mtlb_grid,mtlb_axis([t0 t1 -0.5 1.5]);
scf(2);subplot(212),plot(t,u,'r--'),mtlb_grid,mtlb_axis([t0 t1 -1 1]);
//=====
Us=7.7; U1=Us*0.5; U2=Us*1.5; U3=(U1+U2)/2; U32=U1*U2;
a0=[ 0  1          0                 ;
     0  0          0                 ;
     0  0         -1/Tdelta          ];
a1=[ 0  0          0                 ;
     0 -1/Us/Tship 0                 ;
     0  0          0                 ];
a2=[ 0  0          0                 ;
     0  0          1/Us^2*Kship/Tship;
     0  0          0                 ];
A1=a0+U1*a1+U1^2*a2; 
A2=a0+U2*a1+U2^2*a2;  
A3=a0+U3*a1+U32*a2;
//-----
function Q=interp3(P1,P2,P3,P)
 x1=P1(1); x2=P2(1); x3=P3(1); x=P(1);
 y1=P1(2); y2=P2(2); y3=P3(2); y=P(2);
 alpha=((x1-x2)*(y2-y3)-(x2-x3)*(y1-y2));
 Q(1) =((x -x2)*(y2-y3)-(x2-x3)*(y -y2))/alpha;
 Q(2) =((x1-x3)*(y -y3)-(x -x3)*(y1-y3))/alpha;
 Q(3) =((x1-x2)*(y2-y )-(x2-x )*(y1-y2))/alpha;
endfunction
//
function Ut=U(t,ID)
 if ID==0, Ut=Us,
 else if t<=ID*100, Ut=Us-(Us-U1)/(ID*100)*t, else Ut=U1, end, end
endfunction
//
function dxG=fG(t,xG), dxG=AG(t)*xG+B0*ut, endfunction
//
function AGt=AG(t), 
 P1=[U1;U1^2]; P2=[U2;U2^2]; P3=[U3;U3^2]; Ut=U(t,ID); P=[Ut;Ut^2]; 
 Q=interp3(P1,P2,P3,P); p1=Q(1); p2=Q(2); p3=Q(3); 
 AGt=p1*A1+p2*A2+p3*A3;
endfunction
//-----
clf(3)
t0=0; t1=300; nt=1500; td=(t1-t0)/nt; t=t0:td:t1; iL=tL/td;
xG=zeros(3,1); y=0; xK=zeros(5,1); u=0; ID=0
for i=1:nt
 if i<=iL, yt=[0;1];         ut=0;
 else      yt=[y(:,i-iL);1]; ut=CK*xK(:,i)+DK*yt; end
 xG=[xG ode(xG(:,i),t(i),t(i+1),fG)];
 xK=[xK ode(xK(:,i),t(i),t(i+1),fK)];
 y=[y xG(1,i+1)]; u=[u ut];
end
scf(3);subplot(211),plot(t,y,'b');
scf(3);subplot(212),plot(t,u,'b');
//
xG=zeros(3,1); y=0; xK=zeros(5,1); u=0; ID=1
for i=1:nt
 if i<=iL, yt=[0;1];         ut=0;
 else      yt=[y(:,i-iL);1]; ut=CK*xK(:,i)+DK*yt; end
 xG=[xG ode(xG(:,i),t(i),t(i+1),fG)];
 xK=[xK ode(xK(:,i),t(i),t(i+1),fK)];
 y=[y xG(1,i+1)]; u=[u ut];
end
scf(3);subplot(211),plot(t,y,'r')
scf(3);subplot(212),plot(t,u,'r')
//
xG=zeros(3,1); y=0; xK=zeros(5,1); u=0; ID=2
for i=1:nt
 if i<=iL, yt=[0;1];         ut=0;
 else      yt=[y(:,i-iL);1]; ut=CK*xK(:,i)+DK*yt; end
 xG=[xG ode(xG(:,i),t(i),t(i+1),fG)];
 xK=[xK ode(xK(:,i),t(i),t(i+1),fK)];
 y=[y xG(1,i+1)]; u=[u ut];
end
scf(3);subplot(211),plot(t,y,'m')
scf(3);subplot(212),plot(t,u,'m')
//
xG=zeros(3,1); y=0; xK=zeros(5,1); u=0; ID=3
for i=1:nt
 if i<=iL, yt=[0;1];         ut=0;
 else      yt=[y(:,i-iL);1]; ut=CK*xK(:,i)+DK*yt; end
 xG=[xG ode(xG(:,i),t(i),t(i+1),fG)];
 xK=[xK ode(xK(:,i),t(i),t(i+1),fK)];
 y=[y xG(1,i+1)]; u=[u ut];
end
scf(3);subplot(211),plot(t,y,'k'),mtlb_grid,mtlb_axis([t0 t1 -0.5 1.5]);
//legend(['const.vel.';'rapid vel.var.';'medium vel.var.';'slow vel.var.'])
scf(3);subplot(212),plot(t,u,'k'),mtlb_grid,mtlb_axis([t0 t1 -1 1]);
legend(['const.vel.';'rapid vel.var.';'medium vel.var.';'slow vel.var.'])
//-----
//eof

$3^\circ$ 図6は次のプログラムを用いて得られている。これを参考にして、MATLAB用プログラムを作成しなさい。

MATLAB

SCILAB

//ship9.sce
//-----
function [LME,LMI,OBJ]=synlmi(YLIST)
 [gam,R,S,AK1,BK1,CK1,DK1,AK2,BK2,CK2,DK2,AK3,BK3,CK3,DK3]=YLIST(:);
 LME1=R-R'; 
 LME2=S-S'; 
 LME=list(LME1,LME2);
 LMI0=[R eye(A1);eye(A1) S];
 AW1=[A1*R+B2*CK1 A1+B2*DK1*C2;
      AK1 S*A1+BK1*C2];
 LMI11=-(AW1+AW1'+2*alpha*LMI0);
 LMI21=-[-r*LMI0 AW1;AW1 -r*LMI0];
 LMI31=-[sin(th)*AW1 cos(th)*AW1;-cos(th)*AW1 sin(th)*AW1];
 LMI31=LMI31+LMI31';
 BW=[B1+B2*DK1*D21;
     S*B1+BK1*D21];
 CW1=[C11*R+D121*CK1 C11+D121*DK1*C2];
 [p,m]=size(D11);
 LMI41=-[AW1+AW1' BW CW1'
         BW' -gam*eye(m,m) (D11+D121*DK1*D21)'
         CW1  D11+D121*DK1*D21 -gam*eye(p,p)];
 [p,m]=size(DK1);
 LMI51=-[-1e2*eye(m,m) DK1';DK1 -1e2*eye(p,p)];
//
 AW2=[A2*R+B2*CK2 A2+B2*DK2*C2;
      AK2 S*A2+BK2*C2];
 LMI12=-(AW2+AW2'+2*alpha*LMI0);
 LMI22=-[-r*LMI0 AW2;AW2 -r*LMI0];
 LMI32=-[sin(th)*AW2 cos(th)*AW2;-cos(th)*AW2 sin(th)*AW2];
 LMI32=LMI32+LMI32';
 BW=[B1+B2*DK2*D21;
     S*B1+BK2*D21];
 CW2=[C12*R+D122*CK2 C12+D122*DK2*C2];
 [p,m]=size(D11);
 LMI42=-[AW2+AW2' BW CW2'
        BW' -gam*eye(m,m) (D11+D122*DK2*D21)'
        CW2  D11+D122*DK2*D21 -gam*eye(p,p)];
 [p,m]=size(DK2);
 LMI52=-[-1e2*eye(m,m) DK2';DK2 -1e2*eye(p,p)];
//
 AW3=[A3*R+B2*CK3 A3+B2*DK3*C2;
      AK3 S*A3+BK3*C2];
 LMI13=-(AW3+AW3'+2*alpha*LMI0);
 LMI23=-[-r*LMI0 AW3;AW3 -r*LMI0];
 LMI33=-[sin(th)*AW3 cos(th)*AW3;-cos(th)*AW3 sin(th)*AW3];
 LMI33=LMI33+LMI33';
 BW=[B1+B2*DK3*D21;
     S*B1+BK3*D21];
 CW3=[C13*R+D123*CK3 C13+D123*DK3*C2];
 [p,m]=size(D11);
 LMI43=-[AW3+AW3' BW CW3'
        BW' -gam*eye(m,m) (D11+D123*DK3*D21)'
        CW3  D11+D123*DK3*D21 -gam*eye(p,p)];
 [p,m]=size(DK3);
 LMI53=-[-1e2*eye(m,m) DK3';DK3 -1e2*eye(p,p)];
 LMI=list(LMI0,LMI11,LMI21,LMI31,LMI41,LMI51,LMI12,LMI22,LMI32,LMI42,LMI52,LMI13,LMI23,LMI33,LMI43,LMI53);
 OBJ=gam;
endfunction
//-----
function [AK,BK,CK,DK]=syncont(R,S,ak,bk,ck,dk)
 [u,sd,v]=svd(eye()-S*R); Ni=sqrt(sd)\u'; Mti=v/sqrt(sd); 
 AK=Ni*(ak-S*(A-B2*dk*C2)*R-bk*C2*R-S*B2*ck)*Mti; 
 BK=Ni*(bk-S*B2*dk); 
 CK=(ck-dk*C2*R)*Mti;
 DK=dk; 
endfunction
//=====
T1=118; T2=7.8; T3=18.5; Tship=T1+T2-T3; Kship=0.185;
Tdelta=10; Kdelta=1; tL=9; 
wD1=tL; wD2=tL; wD3=tL; wI1=0.01; wI2=0.01; wI3=0.01;
//
A0=[0 1 0;0 -1/Tship Kship/Tship;0 0 -1/Tdelta]; B0=[0;0;Kdelta/Tdelta]; 
C0=[1 0 0];
//-----
Us=7.7; U1=Us*0.5; U2=Us*1.5; U3=(U1+U2)/2; U32=U1*U2;
a0=[ 0  1          0                  0;
     0  0          0                  0;
     0  0         -1/Tdelta           0;
    -1  0          0                  0];
a1=[ 0  0          0                  0;
     0 -1/Us/Tship 0                  0;
     0  0          0                  0;
     0  0          0                  0];
a2=[ 0  0          0                  0;
     0  0          1/Us^2*Kship/Tship 0;
     0  0          0                  0;
     0  0          0                  0];
A1=a0+U1*a1+U1^2*a2; 
A2=a0+U2*a1+U2^2*a2;  
A3=a0+U3*a1+U32*a2;
B1=[zeros(3,1);1]; 
B2=[B0;0];
C11=[zeros(1,3) wI1;wD1*C0*A0 0;zeros(1,4)]; 
C12=[zeros(1,3) wI2;wD2*C0*A0 0;zeros(1,4)]; 
C13=[zeros(1,3) wI3;wD3*C0*A0 0;zeros(1,4)]; 
D11=zeros(3,1); 
D121=[0;wD1*C0*B0;1];
D122=[0;wD2*C0*B0;1];
D123=[0;wD3*C0*B0;1];
C2=[C0 0;zeros(1,3) 1]; 
D21=[0;0]; 
D22=[0;0];
//-----
alpha=0.01; r=15; th=%pi/4;
gam0=100; R0=eye(4,4); S0=eye(4,4);
AK0=-eye(4,4); BK0=ones(4,2); CK0=ones(1,4); DK0=ones(1,2);
YLIST0=list(gam0,R0,S0,AK0,BK0,CK0,DK0,AK0,BK0,CK0,DK0,AK0,BK0,CK0,DK0);
YLIST=lmisolver(YLIST0,synlmi);
[gam,R,S,ak1,bk1,ck1,dk1,ak2,bk2,ck2,dk2,ak3,bk3,ck3,dk3]=YLIST(:); 
//
A=A1; [AK1,BK1,CK1,DK1]=syncont(R,S,ak1,bk1,ck1,dk1);
plK1=spec(AK1),
ACL1=[A1+B2*DK1*C2 B2*CK1;BK1*C2 AK1];
BCL1=[B1+B2*DK1*D21;BK1*D21];
CCL1=[C2(1,:) zeros(1,4)];
plCL1=spec(ACL1)
//
A=A2; [AK2,BK2,CK2,DK2]=syncont(R,S,ak2,bk2,ck2,dk2);
plK2=spec(AK2),
ACL2=[A2+B2*DK2*C2 B2*CK2;BK2*C2 AK2];
BCL2=[B2+B2*DK2*D21;BK2*D21];
CCL2=[C2(1,:) zeros(1,4)];
plCL2=spec(ACL2)
//
A=A3; [AK3,BK3,CK3,DK3]=syncont(R,S,ak3,bk3,ck3,dk3);
plK3=spec(AK3),
ACL3=[A3+B2*DK3*C2 B2*CK3;BK3*C2 AK3];
BCL3=[B1+B2*DK3*D21;BK3*D21];
CCL3=[C2(1,:) zeros(1,4)];
plCL3=spec(ACL3)
//-----
AK1=[AK1 BK1(:,2);zeros(1,5)];
BK1=[BK1(:,1) zeros(4,1); -1 1];
CK1=[CK1 DK1(:,2)];
DK1=[DK1(:,1) 0];
AL1=[A1 B2*CK1;zeros(5,4) AK1];
BL1=[B2*DK1; BK1];
CL1=[C2 D22*CK1];
DL1=D22*DK1;
clf(0),clf(1)
w=logspace(-3,0,100); nw=length(w);
g=freq(A0,B0,C0,%i*w);
for i=1:nw, ga(i)=20*log10(norm(g(:,i))); end
scf(0);plot2d(w,ga,logflag='ln')
gws=20*log10(abs(wI1./(%i*w))); gwt=20*log10(abs(wD1^(-1)./(%i*w)));
scf(0);plot2d(w,gws,logflag='ln')
scf(0);plot2d(w,gwt,logflag='ln')
g=freq(AL1,BL1(:,1),CL1(1,:),DL1(1,1),%i*w); 
for i=1:nw, ga(i)=20*log10(abs(g(:,i))); end
scf(0);plot2d(w,ga,logflag='ln'),mtlb_grid,mtlb_axis([10^(-3) 10^0 -80 60])
g=freq(ACL1,BCL1,CCL1,%i*w); 
for i=1:nw, ga(i)=20*log10(abs(g(:,i))); end
scf(1);plot2d(w,ga,logflag='ln')
for i=1:nw, ga(i)=20*log10(abs(1-g(:,i))); end
scf(1);plot2d(w,ga,logflag='ln'),mtlb_grid,mtlb_axis([10^(-3) 10^0 -80 60])
scf(1);plot2d(w,-gws,logflag='ln')
scf(1);plot2d(w,gwt,logflag='ln')
//
clf(2),clf(3)
AK2=[AK2 BK2(:,2);zeros(1,5)];
BK2=[BK2(:,1) zeros(4,1); -1 1];
CK2=[CK2 DK2(:,2)];
DK2=[DK2(:,1) 0];
AL2=[A2 B2*CK1;zeros(5,4) AK1];
BL2=[B2*DK1; BK1];
CL2=[C2 D22*CK1];
DL2=D22*DK1;
w=logspace(-3,0,100); nw=length(w);
g=freq(A0,B0,C0,%i*w);
for i=1:nw, ga(i)=20*log10(norm(g(:,i))); end
scf(2);plot2d(w,ga,logflag='ln')
gws=20*log10(abs(wI2./(%i*w))); gwt=20*log10(abs(wD2^(-1)./(%i*w)));
scf(2);plot2d(w,gws,logflag='ln')
scf(2);plot2d(w,gwt,logflag='ln')
g=freq(AL2,BL2(:,1),CL2(1,:),DL2(1,1),%i*w); 
for i=1:nw, ga(i)=20*log10(abs(g(:,i))); end
scf(2);plot2d(w,ga,logflag='ln'),mtlb_grid,mtlb_axis([10^(-3) 10^0 -80 60])
g=freq(ACL2,BCL2,CCL2,%i*w); 
for i=1:nw, ga(i)=20*log10(abs(g(:,i))); end
scf(3);plot2d(w,ga,logflag='ln')
for i=1:nw, ga(i)=20*log10(abs(1-g(:,i))); end
scf(3);plot2d(w,ga,logflag='ln'),mtlb_grid,mtlb_axis([10^(-3) 10^0 -80 60])
scf(3);plot2d(w,-gws,logflag='ln')
scf(3);plot2d(w,gwt,logflag='ln')
//
clf(4),clf(5)
AK3=[AK3 BK3(:,2);zeros(1,5)];
BK3=[BK3(:,1) zeros(4,1); -1 1];
CK3=[CK3 DK3(:,2)];
DK3=[DK3(:,1) 0];
AL3=[A3 B2*CK1;zeros(5,4) AK1];
BL3=[B2*DK1; BK1];
CL3=[C2 D22*CK1];
DL3=D22*DK1;
w=logspace(-3,0,100); nw=length(w);
g=freq(A0,B0,C0,%i*w);
for i=1:nw, ga(i)=20*log10(norm(g(:,i))); end
scf(4);plot2d(w,ga,logflag='ln')
gws=20*log10(abs(wI3./(%i*w))); gwt=20*log10(abs(wD3^(-1)./(%i*w)));
scf(4);plot2d(w,gws,logflag='ln')
scf(4);plot2d(w,gwt,logflag='ln')
g=freq(AL3,BL3(:,1),CL3(1,:),DL3(1,1),%i*w); 
for i=1:nw, ga(i)=20*log10(abs(g(:,i))); end
scf(4);plot2d(w,ga,logflag='ln'),mtlb_grid,mtlb_axis([10^(-3) 10^0 -80 60])
g=freq(ACL3,BCL3,CCL3,%i*w); 
for i=1:nw, ga(i)=20*log10(abs(g(:,i))); end
scf(5);plot2d(w,ga,logflag='ln')
for i=1:nw, ga(i)=20*log10(abs(1-g(:,i))); end
scf(5);plot2d(w,ga,logflag='ln'),mtlb_grid,mtlb_axis([10^(-3) 10^0 -80 60])
scf(5);plot2d(w,-gws,logflag='ln')
scf(5);plot2d(w,gwt,logflag='ln')
//return
//=====
a0=[ 0  1          0                 ;
     0  0          0                 ;
     0  0         -1/Tdelta          ];
a1=[ 0  0          0                 ;
     0 -1/Us/Tship 0                 ;
     0  0          0                 ];
a2=[ 0  0          0                 ;
     0  0          1/Us^2*Kship/Tship;
     0  0          0                 ];
A1=a0+U1*a1+U1^2*a2; 
A2=a0+U2*a1+U2^2*a2;  
A3=a0+U3*a1+U32*a2;
//-----
function Q=interp3(P1,P2,P3,P)
 x1=P1(1); x2=P2(1); x3=P3(1); x=P(1);
 y1=P1(2); y2=P2(2); y3=P3(2); y=P(2);
 alpha=((x1-x2)*(y2-y3)-(x2-x3)*(y1-y2));
 Q(1) =((x -x2)*(y2-y3)-(x2-x3)*(y -y2))/alpha;
 Q(2) =((x1-x3)*(y -y3)-(x -x3)*(y1-y3))/alpha;
 Q(3) =((x1-x2)*(y2-y )-(x2-x )*(y1-y2))/alpha;
endfunction
//
function Ut=U(t,ID)
 if ID==0, Ut=Us,
 else
  if t<=ID*100, Ut=Us-(Us-U1)/(ID*100)*t, else Ut=U1, end
 end
endfunction
//
function dxG=fG(t,xG), dxG=AG(t)*xG+B0*ut, endfunction
//
function AGt=AG(t), 
 P1=[U1;U1^2]; P2=[U2;U2^2]; P3=[U3;U3^2]; Ut=U(t,ID); P=[Ut;Ut^2]; 
 Q=interp3(P1,P2,P3,P); p1=Q(1); p2=Q(2); p3=Q(3); 
 AGt=p1*A1+p2*A2+p3*A3;
endfunction
//
function dxK=fK(t,xK), 
 dxK=AK(t)*xK+BK(t)*yt, 
endfunction
//
function AKt=AK(t), 
 P1=[U1;U1^2]; P2=[U2;U2^2]; P3=[U3;U3^2]; Ut=U(t,ID); P=[Ut;Ut^2]; 
 Q=interp3(P1,P2,P3,P); p1=Q(1); p2=Q(2); p3=Q(3); 
 AKt=p1*AK1+p2*AK2+p3*AK3;
endfunction
//
function BKt=BK(t), 
 P1=[U1;U1^2]; P2=[U2;U2^2]; P3=[U3;U3^2]; Ut=U(t,ID); P=[Ut;Ut^2]; 
 Q=interp3(P1,P2,P3,P); p1=Q(1); p2=Q(2); p3=Q(3); 
 BKt=p1*BK1+p2*BK2+p3*BK3;
endfunction
//
function CKt=CK(t), 
 P1=[U1;U1^2]; P2=[U2;U2^2]; P3=[U3;U3^2]; Ut=U(t,ID); P=[Ut;Ut^2]; 
 Q=interp3(P1,P2,P3,P); p1=Q(1); p2=Q(2); p3=Q(3); 
 CKt=p1*CK1+p2*CK2+p3*CK3;
endfunction
//
function DKt=DK(t), 
 P1=[U1;U1^2]; P2=[U2;U2^2]; P3=[U3;U3^2]; Ut=U(t,ID); P=[Ut;Ut^2]; 
 Q=interp3(P1,P2,P3,P); p1=Q(1); p2=Q(2); p3=Q(3); 
 DKt=p1*DK1+p2*DK2+p3*DK3;
endfunction
//-----
clf(6)
t0=0; t1=300; nt=1500; td=(t1-t0)/nt; t=t0:td:t1; iL=tL/td;
xG=zeros(3,1); y=0; xK=zeros(5,1); u=0; ID=0;
for i=1:nt
 if i<=iL, yt=[0;1];         ut=0;
 else      yt=[y(:,i-iL);1]; ut=CK(t(i))*xK(:,i)+DK(t(i))*yt; end
 xG=[xG ode(xG(:,i),t(i),t(i+1),fG)];
 xK=[xK ode(xK(:,i),t(i),t(i+1),fK)];
 y=[y xG(1,i+1)]; u=[u ut];
end
scf(6);subplot(211),plot(t,y,'b');
scf(6);subplot(212),plot(t,u,'b');
//
xG=zeros(3,1); y=0; xK=zeros(5,1); u=0; ID=1;
for i=1:nt
 if i<=iL, yt=[0;1];         ut=0;
 else      yt=[y(:,i-iL);1]; ut=CK(t(i))*xK(:,i)+DK(t(i))*yt; end
 xG=[xG ode(xG(:,i),t(i),t(i+1),fG)];
 xK=[xK ode(xK(:,i),t(i),t(i+1),fK)];
 y=[y xG(1,i+1)]; u=[u ut];
end
scf(6);subplot(211),plot(t,y,'r');
scf(6);subplot(212),plot(t,u,'r');
//
xG=zeros(3,1); y=0; xK=zeros(5,1); u=0; ID=2;
for i=1:nt
 if i<=iL, yt=[0;1];         ut=0;
 else      yt=[y(:,i-iL);1]; ut=CK(t(i))*xK(:,i)+DK(t(i))*yt; end
 xG=[xG ode(xG(:,i),t(i),t(i+1),fG)];
 xK=[xK ode(xK(:,i),t(i),t(i+1),fK)];
 y=[y xG(1,i+1)]; u=[u ut];
end
scf(6);subplot(211),plot(t,y,'m');
scf(6);subplot(212),plot(t,u,'m');
//
xG=zeros(3,1); y=0; xK=zeros(5,1); u=0; ID=3;
for i=1:nt
 if i<=iL, yt=[0;1];         ut=0;
 else      yt=[y(:,i-iL);1]; ut=CK(t(i))*xK(:,i)+DK(t(i))*yt; end
 xG=[xG ode(xG(:,i),t(i),t(i+1),fG)];
 xK=[xK ode(xK(:,i),t(i),t(i+1),fK)];
 y=[y xG(1,i+1)]; u=[u ut];
end
scf(6);subplot(211),plot(t,y,'k'),mtlb_grid,mtlb_axis([t0 t1 -0.5 1.5]);
//legend(['const.vel.';'rapid vel.var.';'medium vel.var.';'slow vel.var.'])
scf(6);subplot(212),plot(t,u,'k'),mtlb_grid,mtlb_axis([t0 t1 -1 1]);
legend(['const.vel.';'rapid vel.var.';'medium vel.var.';'slow vel.var.'])
//-----
//eof

制御系CAD

制御を意識したモノつくりを目指して

船舶のロバスト制御