YMCPBのLQI制御 | 制御系CAD

ここでは、次の４種類のLQI制御系設計について述べます。

Case	設計モデル	状態FB	状態OB	積分動作	注意点
1	５次	５次	未使用	要	「絵に描いた餅」
2	５次	３次	不要	要	ヒーブのゲインを強制的に零
3	３次	３次	不要	要	ヒーブからの連成を抑制できるか
4	５次	５次	要	要	コントローラが微分方程式

Note e12に示すように、データセットNo.1について、id=128の場合を設計用モデルとして採用したとき、Case2とCase3のどちらを用いても制御性能に大きな差異が見られません。そこでこのコントローラがどれくらいのモデル変動をカバーできるかという意味のロバスト性をシミュレーションにより調べてみます。ここで、モデル変動としては、前進速度を変える場合と、重心位置を変える場合の２通りを考えます。そのために次の３組のデータセットを考えます。

	艇	艇質量[kg]	長手方向重心位置[m]	備考
No3	SPTM247	2845	1.937	2022/1/19重量重心測定結果より
No4	SPTM247	2945	1.937	No3重量に+100kg
No5	SPTM247	2745	1.937	No3重量に-100kg

設計用モデルとして、データセットNo.3における船外機取付角-2[deg]のときのid=128の場合を採用し、Case 2とCase 3のLQIコントローラを設計します。そしてデータセットNo.3,4,5における船外機取付角-2[deg]のときのモデル（id=126～133）を安定化できるかを調べます。シミュレーション結果を次に示します。

⇒Case2:いくつかの動作点で不安定
⇒Case3:いくつかの動作点で不安定
図1　データセットNo.3の場合、LQIコントローラのロバスト性

⇒Case2:いくつかの動作点で不安定
⇒Case3:いくつかの動作点で不安定
図2　データセットNo.4の場合、LQIコントローラのロバスト性

⇒Case2:いくつかの動作点で不安定
⇒Case3:いくつかの動作点で不安定
図3　データセットNo.5の場合、LQIコントローラのロバスト性

これらのシミュレーションは本来は非線形シミュレータ上で行うべきですが、ここでは６次元線形モデルを用いて簡易的に行っています。

第１の注意点は、６次元線形モデルの第４番目の状態変数 $\dot{x}(t)-V^*$ の係数を強制的に $a_{44}=0$ としていることです。これは $a_{44}>0$ となる場合があって、 $\dot{x}(t)-V^*$ が発散してしまうからです。

第２の注意点は、上のシミュレーションでは各データセットのすべてのモデルを安定化できていませんが、設計用モデルとして、データセットNo.1における船外機取付角-2[deg]のときのid=128の場合を採用すると、各データセットのすべてのモデルは安定化されることです。

これらの事情は、線形モデルが暫定版の非線形シミュレータから得られているので正確ではないためとも考えられます。いずれにしてもできるだけ実機のダイナミックスを反映した非線形シミュレータ上での再検討が必要になります。

MATLAB

%cYMCPB5_robust_lqi.m
%-----
 clear all, close all
 dataset=[...
 "ABC_No1_20220128",...
 "ABC_No2_20220131",...
 "ABC_No3_20220128",...
 "ABC_No4_20220131",...
 "ABC_No5_20220131"];
%-----設計用データセット
 no=3
 load(dataset(no))
 ID=[7,8,9,10,11,12,13,14,...
 36,37,38,39,40,41,42,43,...
 66,67,68,69,70,71,72,73,...
 96,97,98,99,100,101,102,103,...
 126,127,128,129,130,131,132,133];
%-----６次元モデル
%(1)x,(2)z,(3)th,(4)dx-V*,(5)dz,(6)dth
 sys=ss(A0(:,:,ID),B0(:,:,ID),eye(6,6),[]);
%-----５次元モデル
%(1)z,(2)th,(3)dz,(4)dth,(5)the
 k=[2,3,5,6]; i=[2,4,5]; j=[2];
 for id=ID
   A(:,:,id)=[A0(k,k,id) B0(k,j,id);zeros(1,5)];
 end 
 omegae=0.0380*3;
 B=[zeros(4,1);omegae];
 E=eye(5,5);
 CM=E(i,:);
 C=CM(3,:);
 sys5=ss(A(:,:,ID),B,eye(5,5),[]);  
%-----設計ポイント 
 index=35 %[3,11,19,27,35]
 id=ID(index); 
 Vs=dx_eq_Series(id);    %*3.6
 zs=z_eq_Series(id);     %*100
 ths=th_eq_Series(id);   %/pi*180
 thes=the_eq_Series(id); %/pi*180
 [Asys,Bsys,Csys,Dsys]=ssdata(sys(:,:,index));
 [A,B]=ssdata(sys5(:,:,index));
%-----
 [n,m]=size(B);
 AE=[A B;zeros(m,n+m)];
 BE=[zeros(n,m);eye(m)];
 S=[A B;C 0];
 Mz=0.05;
 Mth=3/180*pi;
 Mthe=10/180*pi; 
 Mue=1;
 Tue=0.05;
 CE=eye(n+m,n+m); 
 QE=diag([1/Mz^2,1/Mth^2,0,0,1/Mthe^2,1/Mue^2]);
 RE=(Tue/Mue)^2;
 [KE,PE]=opt(AE,BE,CE,QE,RE);
 poles=eig(AE-BE*KE);
 FE=KE/S;
%----- 
 F=FE(:,1:n)
 FI=FE(:,n+1:n+m)
 F(1)=0; F(3)=0;
%-----全モデルに対するシミュレーション
for no=[3,4,5] 
for iSIM=index-2:index+5  
 id=ID(iSIM);
 Vs=dx_eq_Series(id);    %*3.6
 zs=z_eq_Series(id);     %*100
 ths=th_eq_Series(id);   %/pi*180
 thes=the_eq_Series(id); %/pi*180
 [Asys,Bsys,Csys,Dsys]=ssdata(sys(:,:,iSIM));
 Asys(4,4)=0;
 [A,B]=ssdata(sys5(:,:,iSIM));  
 us=thes;
 xs=[0;zs;ths;Vs;0;0];
 x0=[0;0;-1/180*pi;0;0;0];
 yc=0; 
 E=eye(6,6);
 CM=E([2,3,5,6],:);
 CM1=100*E([2],:);
 CM2=180/pi*E([3],:);
 ns=4;
 simout=sim('nYMCPB_lqi.slx');
%[no,iSIM,id,180/pi*us]
 figure(no)
 subplot(221),plot(simout.x.Time,100*simout.x.Data(:,2)),hold on
 subplot(223),plot(simout.y.Time,180/pi*simout.y.Data(:,1)),hold on
 subplot(222),plot(simout.y.Time,180/pi*simout.y.Data(:,3)),hold on
 subplot(224),plot(simout.u.Time,simout.u.Data),hold on
end %iSIM
 figure(no)
 subplot(221),title("z[cm]"),grid
 subplot(223),title("th[deg]"),grid
 subplot(222),title("the[deg]"),grid
 subplot(224),title("ue"),grid
end %no 
%-----
%eof

MATLAB

%cYMCPB3_robust_lqi.m
%-----
 clear all, close all
 dataset=[...
 "ABC_No1_20220128",...
 "ABC_No2_20220131",...
 "ABC_No3_20220128",...
 "ABC_No4_20220131",...
 "ABC_No5_20220131"];
%-----設計用データセット
 no=3
 load(dataset(no))
 ID=[7,8,9,10,11,12,13,14,...
 36,37,38,39,40,41,42,43,...
 66,67,68,69,70,71,72,73,...
 96,97,98,99,100,101,102,103,...
 126,127,128,129,130,131,132,133];
%-----６次元モデル
%(1)x,(2)z,(3)th,(4)dx-V*,(5)dz,(6)dth
 sys=ss(A0(:,:,ID),B0(:,:,ID),eye(6,6),[]);
%-----３次元モデル
%(1)th,(2)dth,(3)the
 k=[3,6]; j=[2];
 for id=ID
   AA(:,:,id)=[A0(k,k,id) B0(k,j,id);zeros(1,3)];
 end 
 omegae=0.0380*3;
 B=[zeros(2,1);omegae];
 E=eye(3,3);
 C=E(3,:);
 sys3=ss(AA(:,:,ID),B,eye(3,3),[]);  
%-----設計ポイント 
 index=35 %[3,11,19,27,35]
 id=ID(index); 
 Vs=dx_eq_Series(id);    %*3.6
 zs=z_eq_Series(id);     %*100
 ths=th_eq_Series(id);   %/pi*180
 thes=the_eq_Series(id); %/pi*180
 [Asys,Bsys,Csys,Dsys]=ssdata(sys(:,:,index));
 [A,B]=ssdata(sys3(:,:,index));
%-----
 [n,m]=size(B);
 AE=[A B;zeros(m,n+m)];
 BE=[zeros(n,m);eye(m)];
 S=[A B;C 0];
 Mth=3/180*pi;
 Tth=0.5;
 Mthe=5/180*pi; 
 Mue=1;
 Tue=0.05;
 CE=eye(n+m,n+m); 
 QE=diag([1/Mth^2,(Tth/Mth)^2,1/Mthe^2,1/Mue^2]);
 RE=(Tue/Mue)^2;
 [KE,PE]=opt(AE,BE,CE,QE,RE);
 poles=eig(AE-BE*KE);
 FE=KE/S;
%-----  
 F=FE(:,1:n)
 FI=FE(:,n+1:n+m) 
%-----全モデルに対するシミュレーション
for no=[3,4,5] 
for iSIM=index-2:index+5 
 id=ID(iSIM);
 Vs=dx_eq_Series(id);    %*3.6
 zs=z_eq_Series(id);     %*100
 ths=th_eq_Series(id);   %/pi*180
 thes=the_eq_Series(id); %/pi*180
 [Asys,Bsys,Csys,Dsys]=ssdata(sys(:,:,iSIM));
 Asys(4,4)=0;
 [A,B]=ssdata(sys3(:,:,iSIM));    
 us=thes;
 xs=[0;zs;ths;Vs;0;0];
 x0=[0;0;-1/180*pi;0;0;0];
 yc=0; 
 E=eye(6,6);
 CM=E([3,6],:);
 CM1=100*E([2],:);
 CM2=180/pi*E([3],:);
 ns=2;
 simout=sim('nYMCPB_lqi.slx');
%[no,iSIM,id,180/pi*us]
 figure(no)
 subplot(221),plot(simout.x.Time,100*simout.x.Data(:,2)),hold on
 subplot(223),plot(simout.y.Time,180/pi*simout.y.Data(:,1)),hold on
 subplot(222),plot(simout.y.Time,180/pi*simout.y.Data(:,3)),hold on
 subplot(224),plot(simout.u.Time,simout.u.Data),hold on
end %iSIM
 figure(no)
 subplot(221),title("z[cm]"),grid
 subplot(223),title("th[deg]"),grid
 subplot(222),title("the[deg]"),grid
 subplot(224),title("ue"),grid
end %no 
%-----
%eof

Note e12　LQI制御系の設計
Case 1：５次元モデルに基づくLQI制御

●LQI制御系を設計するために、次の５次元モデルを考えます。

$\displaystyle{(1.1)\quad \begin{array}{l} \underbrace{ \frac{d}{dt} \left[\begin{array}{c} z(t)-z^*\\ \theta(t)-\theta^*\\ \dot{z}(t)\\ \dot{\theta}(t)\\ \theta_e(t)-\theta_e^* \end{array}\right] }_{\dot{x}(t)} = \underbrace{ \left[\begin{array}{ccccc} 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 1 & 0\\ a_{52} & a_{53} & a_{55} & a_{56} & b_{52}\\ a_{62} & a_{63} & a_{65} & a_{66} & b_{62}\\ 0 & 0 & 0 & 0 & 0 \end{array}\right] }_{A} \underbrace{ \left[\begin{array}{c} z(t)-z^*\\ \theta(t)-\theta^*\\ \dot{z}(t)\\ \dot{\theta}(t)\\ \theta_e(t)-\theta_e^* \end{array}\right] }_{x(t)}\\ + \underbrace{ \left[\begin{array}{c} 0 \\ 0 \\ 0 \\ 0 \\ \omega_e \end{array}\right] }_{B} u(t) \end{array}}$

$\displaystyle{(1.2)\quad \begin{array}{l} \underbrace{ \left[\begin{array}{c} \theta(t)-\theta^*\\ \dot{\theta}(t)\\ \theta_e(t)-\theta_e^* \end{array}\right] }_{y(t)} = \underbrace{ \left[\begin{array}{ccccc} 0 & 1 & 0 & 0 & 0\\ 0 & 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 0 & 1 \end{array}\right] }_{C_M} \underbrace{ \left[\begin{array}{c} z(t)-z^*\\ \theta(t)-\theta^*\\ \dot{z}(t)\\ \dot{\theta}(t)\\ \theta_e(t)-\theta_e^* \end{array}\right] }_{x(t)} \end{array}}$

ここで操作入力は $u_e(t)$ ではなく $u(t)$ としていますが、制御系の評価の際に

$\displaystyle{(2)\quad u_e(t)={\rm sign}(u(t)) }$

とします。

●制御対象YMCPBに対する制御目的は、あるスラスト $T^*$ と取付角 $\theta_e^*$ の下で、巡航速度 $V^*$ で走行するとき、一定の姿勢 $z^*,\theta^*$ を保つこととします。制御目的を達成する制御則として次式を考えます。

$\displaystyle{(3)\quad u(t)=-Fx(t)+\int_0^t(\theta_e^*-\theta_e(t))dt }$

ここで、右辺第１項は状態フィードバック、第２項は船外機取付角をある収納状態 $\theta_e(0)\ne0$ から所定の平衡入力値 $\theta_e^*$ に設定するための積分動作です。いま

$\displaystyle{(4)\quad \begin{array}{l} \theta_e(t)-\theta_e^* = \underbrace{ \left[\begin{array}{ccccc} 0 & 0 & 0 & 0 & 1 \end{array}\right] }_{C} \underbrace{ \left[\begin{array}{c} z(t)-z^*\\ \theta(t)-\theta^*\\ \dot{z}(t)\\ \dot{\theta}(t)\\ \theta_e(t)-\theta_e^* \end{array}\right] }_{x(t)} \rightarrow \theta_e^c=0\quad(t\rightarrow\infty) \end{array}}$

となって、船外機取付角を設定値 $\theta_e^*$ に維持できたとすると、次式が成り立つ必要があります。

$\displaystyle{(4)\quad \left\{\begin{array}{l} \underbrace{\dot{x}(\infty)}_{0}=A\underbrace{x(\infty)}_{x_\infty}+B\underbrace{u(\infty)}_{u_\infty}\\ \underbrace{ \theta_e(t)-\theta_e^* }_{\theta_e^c=0}=C\underbrace{x(\infty)}_{x_\infty} \end{array}\right.}$

すなわち

$\displaystyle{(4')\quad \left[\begin{array}{c} 0 \\ 0 \\ 0 \\ 0 \\ 0 \\\hline \theta_e^c \\ \end{array}\right] = \underbrace{ \left[\begin{array}{ccccc|c} 0 & 0 & 1 & 0 & 0 & 0\\ 0 & 0 & 0 & 1 & 0 & 0\\ a_{52} & a_{53} & a_{55} & a_{56} & b_{52}& 0\\ a_{62} & a_{63} & a_{65} & a_{66} & b_{62}& 0\\ 0 & 0 & 0 & 0 & 0 & \omega_e\\\hline 0 & 0 & 0 & 0 & 1 & 0 \end{array}\right] }_{S= \left[\begin{array}{cc} A & B\\ C & 0 \end{array}\right] } \left[\begin{array}{c} z_\infty\\ \theta_\infty\\ 0\\ 0\\ \theta_e^c \\\hline 0 \end{array}\right] }$

ここで、システム行列 $S$ は正則ですから、任意の平衡入力値 $\theta_e^*$ に対して、 $\theta_e(\infty)=\theta_e^*$ をもたらす状態変数 $z_\infty$ と $\theta_\infty$ が定まることが分かります。そこで次の偏差系E3を考えます。

$\displaystyle{(5)\quad \begin{array}{l} \underbrace{ \frac{d}{dt} \left[\begin{array}{c} (z(t)-z^*)-z_\infty\\ (\theta(t)-\theta^*)-\theta_\infty\\ \dot{z}(t)\\ \dot{\theta}(t)\\ (\theta_e(t)-\theta_e^*)-\theta_e^c \\\hline u(t) \end{array}\right] }_{\dot{x}_E(t)} = \underbrace{ \left[\begin{array}{ccccc|c} 0 & 0 & 1 & 0 & 0 & 0\\ 0 & 0 & 0 & 1 & 0 & 0\\ a_{52} & a_{53} & a_{55} & a_{56} & b_{52}& 0\\ a_{62} & a_{63} & a_{65} & a_{66} & b_{62}& 0\\ 0 & 0 & 0 & 0 & 0 & \omega_e\\\hline 0 & 0 & 0 & 0 & 0 & 0 \end{array}\right] }_{A_E}\\ \times\underbrace{ \left[\begin{array}{c} (z(t)-z^*)-z_\infty\\ (\theta(t)-\theta^*)-\theta_\infty\\ \dot{z}(t)\\ \dot{\theta}(t)\\ (\theta_e(t)-\theta_e^*)-\theta_e^c \\\hline u(t) \end{array}\right] }_{x_E(t)} + \underbrace{ \left[\begin{array}{cccc} 0 \\ 0 \\ 0 \\ 0 \\ 0 \\\hline 1 \\ \end{array}\right] }_{B_E} \dot{u}(t) \end{array} }$

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

$\displaystyle{(6)\quad \dot{u}(t)=- \underbrace{ \left[\begin{array}{ccccc|c} k_{1} & k_{2} & k_{3} & k_{4} & k_{5} & k_{6} \end{array}\right] }_{K_E} \underbrace{ \left[\begin{array}{c} (z(t)-z^*)-z_\infty\\ (\theta(t)-\theta^*)-\theta_\infty\\ \dot{z}(t)\\ \dot{\theta}(t)\\ (\theta_e(t)-\theta_e^*)-\theta_e^c \\\hline u(t) \end{array}\right] }_{x_E(t)} }$

を、二次形式評価関数

$(7.1)\quad \begin{array}{l} \displaystyle{J=\int_0^\infty\left\{q_1^2(z(t)-z^*-z_\infty)^2+q_2^2(\theta(t)-\theta^*-\theta_\infty)^2+q_5^2(\theta_e(t)-\theta_e^*-\theta_e^c)^2\right.}\\ \left.+q_6^2u^2(t)+r^2\dot{u}^2(t)\right\}\,dt}}\\ \end{array} }$

$\displaystyle{(7.2)\quad \begin{array}{l} q_1=\frac{1}{M_z}, q_2=\frac{1}{M_\theta}, q_5=\frac{1}{M_{\theta_e}}, q_6=\frac{1}{M_{u}}, r=\frac{1}{M_{u}/T_{u}} \end{array} }$

$\displaystyle{(7.3)\quad \begin{array}{l} |z(t)-z^*-z_\infty|<{M_z}, |\theta(t)-\theta^*-\theta_\infty|<{M_\theta}, |\theta_e(t)-\theta_e^*-\theta_e^c|<M_{\theta_e}},\\ |u(t)|<M_{u}, |\dot{u}(t)|<\frac{M_{u}}{T_{u}} \end{array} }$

を最小化して求めます。(6)は

$\displaystyle{(8)\quad \left[\begin{array}{c} \frac{d}{dt} \left[\begin{array}{c} z(t)-z^*\\ \theta(t)-\theta^*\\ \dot{z}(t)\\ \dot{\theta}(t)\\ \theta_e(t)-\theta_e^* \\\hline \end{array}\right]\\\hline (\theta_e(t)-\theta_e^*)-\theta_e^c \end{array}\right]=S \left[\begin{array}{c} (z(t)-z^*)-z_\infty\\ (\theta(t)-\theta^*)-\theta_\infty\\ \dot{z}(t)\\ \dot{\theta}(t)\\ (\theta_e(t)-\theta_e^*)-\theta_e^c \\\hline u(t) \end{array}\right] }$

に注意して

$\displaystyle{(9)\quad \dot{u}(t)=- \underbrace{ \left[\begin{array}{ccccc|c} k_{1} & k_{2} & k_{3} & k_{4} & k_{5} & k_{6} \end{array}\right]S^{-1} }_{F_E} \left[\begin{array}{c} \frac{d}{dt} \left[\begin{array}{c} z(t)-z^*\\ \theta(t)-\theta^*\\ \dot{z}(t)\\ \dot{\theta}(t)\\ \theta_e(t)-\theta_e^* \\\hline \end{array}\right]\\\hline (\theta_e(t)-\theta_e^*)-\theta_e^c \end{array}\right] }$

と書けるので、これを積分して次のLQI制御則を得ます。

$\displaystyle{(10)\quad u(t) =- \underbrace{ \left[\begin{array}{ccccc} f_{1} & f_{2} & f_{3} & f_{4} & f_{5} \end{array}\right] }_{F} \underbrace{ \left[\begin{array}{c} z(t)-z^*\\ \theta(t)-\theta^*\\ \dot{z}(t)\\ \dot{\theta}(t)\\ \theta_e(t)-\theta_e^* \end{array}\right] }_{x(t)} +\underbrace{f_6}_{F_I}\int_0^t(\theta_e^c-(\theta_e(t)-\theta_e^*))dt} }$

●制御対象YMCPBに対するLQI制御系を設計するプログラムを次に示します。

MATLAB

%cYMCPB5_lqi.m
%-----
 clear all, close all
 dataset=[...
 "ABC_No1_20220128",...
 "ABC_No2_20220131",...
 "ABC_No3_20220128",...
 "ABC_No4_20220131",...
 "ABC_No5_20220131"];
 no=1
 load(dataset(no))
 ID=[8,9,10,11,12,13,14,...
 36,37,38,39,40,41,42,43,...
 66,67,68,69,70,71,72,73,...
 97,98,99,100,101,102,103,...
 126,127,128,129,130,131,132,133];
%-----６次元モデル
%(1)x,(2)z,(3)th,(4)dx-V*,(5)dz,(6)dth
 sys=ss(A0(:,:,ID),B0(:,:,ID),eye(6,6),[]);
%-----５次元モデル
%(1)z,(2)th,(3)dz,(4)dth,(5)the
 k=[2,3,5,6]; i=[2,4,5]; j=[2];
 for id=ID
   A(:,:,id)=[A0(k,k,id) B0(k,j,id);zeros(1,5)];
 end 
 omegae=0.0380*3;
 B=[zeros(4,1);omegae];
 E=eye(5,5);
 CM=E(i,:);
 C=CM(3,:);
 sys5=ss(A(:,:,ID),B,eye(5,5),[]);  
%-----設計ポイント id=128=ID(33)
 Vs=dx_eq_Series(128);    %*3.6
 zs=z_eq_Series(128);     %*100
 ths=th_eq_Series(128);   %/pi*180
 thes=the_eq_Series(128); %/pi*180
 [Asys,Bsys,Csys,Dsys]=ssdata(sys(:,:,33));
 [A,B]=ssdata(sys5(:,:,33));
%-----
 [n,m]=size(B);
 AE=[A B;zeros(m,n+m)];
 BE=[zeros(n,m);eye(m)];
 S=[A B;C 0];
 Mz=0.05;
 Mth=3/180*pi;
 Mthe=10/180*pi; 
 Mue=1;
 Tue=0.05;
 CE=eye(n+m,n+m); 
 QE=diag([1/Mz^2,1/Mth^2,0,0,1/Mthe^2,1/Mue^2]);
 RE=(Tue/Mue)^2;
 [KE,PE]=opt(AE,BE,CE,QE,RE);
 poles=eig(AE-BE*KE);
 FE=KE/S;
%----- 
 F=FE(:,1:n)
 FI=FE(:,n+1:n+m)
%F(1)=0; F(3)=0;
%-----  
 us=thes;
 xs=[0;zs;ths;Vs;0;0];
 x0=[0;0;-1/180*pi;0;0;0];
 yc=0; 
 E=eye(6,6);
 CM=E([2,3,5,6],:);
 CM1=100*E([2],:);
 CM2=180/pi*E([3],:);
 ns=4;
%-----
%eof
% F = 0.0728   20.4471    0.4015  -39.3709  186.9394
% FI = 127.8489

図1　LQI制御系シミュレーション

これを実行して次のシミュレーション結果を得ます。

図1　５次元モデルに基づくLQI制御系と符号関数を通したLQI制御

Case 2：Case1でヒーブゲインを零とする場合

図2　ヒーブゲインを零とした場合のLQI制御系と符号関数を通したLQI制御

Case 3：３次元モデルに基づくLQI制御

●制御対象YMCPBに対するLQI制御系を設計するプログラムを次に示します。

MATLAB

%cYMCPB3_lqi.m
%-----
 clear all, close all
 dataset=[...
 "ABC_No1_20220128",...
 "ABC_No2_20220131",...
 "ABC_No3_20220128",...
 "ABC_No4_20220131",...
 "ABC_No5_20220131"];
 no=1
 load(dataset(no))
 ID=[8,9,10,11,12,13,14,...
 36,37,38,39,40,41,42,43,...
 66,67,68,69,70,71,72,73,...
 97,98,99,100,101,102,103,...
 126,127,128,129,130,131,132,133];
%-----６次元モデル
%(1)x,(2)z,(3)th,(4)dx-V*,(5)dz,(6)dth
 sys=ss(A0(:,:,ID),B0(:,:,ID),eye(6,6),[]);
%-----３次元モデル
%(1)th,(2)dth,(3)the
 k=[3,6]; j=[2];
 for id=ID
   AA(:,:,id)=[A0(k,k,id) B0(k,j,id);zeros(1,3)];
 end 
 omegae=0.0380*3;
 B=[zeros(2,1);omegae];
 E=eye(3,3);
 C=E(3,:);
 sys3=ss(AA(:,:,ID),B,eye(3,3),[]);  
%-----設計ポイント id=128=ID(33)
 Vs=dx_eq_Series(128);    %*3.6
 zs=z_eq_Series(128);     %*100
 ths=th_eq_Series(128);   %/pi*180
 thes=the_eq_Series(128); %/pi*180
 [Asys,Bsys,Csys,Dsys]=ssdata(sys(:,:,33));
 [A,B]=ssdata(sys3(:,:,33));
%-----
 [n,m]=size(B);
 AE=[A B;zeros(m,n+m)];
 BE=[zeros(n,m);eye(m)];
 S=[A B;C 0];
 Mth=3/180*pi;
 Tth=0.5
 Mthe=5/180*pi; 
 Mue=1;
 Tue=0.05;
 CE=eye(n+m,n+m); 
 QE=diag([1/Mth^2,(Tth/Mth)^2,1/Mthe^2,1/Mue^2]);
 RE=(Tue/Mue)^2;
 [KE,PE]=opt(AE,BE,CE,QE,RE);
 poles=eig(AE-BE*KE);
 FE=KE/S;
%-----  
 F=FE(:,1:n)
 FI=FE(:,n+1:n+m)
%-----  
 us=thes;
 xs=[0;zs;ths;Vs;0;0];
 x0=[0;0;-1/180*pi;0;0;0];
 yc=0; 
 E=eye(6,6);
 CM=E([3,6],:);
 CM1=100*E([2],:);
 CM2=180/pi*E([3],:);
 ns=2;
%-----
%eof
% F =
%    -2.8411  -45.8237  192.5801
% FI =
%   231.8536

図3　３次元モデルに基づくLQI制御系と符号関数を通したLQI制御

Case 4：Case 1で状態オブザーバを用いる場合

いま、観測変数が状態変数の最初にくるように、状態方程式と観測方程式における状態変数の順番を変えておきます。

$\displaystyle{(11)\quad \begin{array}{l} \underbrace{ \frac{d}{dt} \left[\begin{array}{c} \theta(t)-\theta^*\\ \dot{\theta}(t)\\ \theta_e(t)\\\hline z(t)-z^*\\ \dot{z}(t) \end{array}\right] }_{\dot{x}'(t)} = \underbrace{ \left[\begin{array}{ccc|cc} 0 & 1 & 0 & 0 & 0 \\ a_{63} & 0 & b_{62} & a_{62} & 0 \\ 0 & 0 & 0 & 0 & 0 \\\hline 0 & 0 & 0 & 0 & 1 \\ a_{53} & 0 & b_{52} & a_{52} & 0 \end{array}\right] }_{A'=\left[\begin{array}{cc} A'_{11} & A'_{12}\\ A'_{21} & A'_{22} \end{array}\right] } \underbrace{ \left[\begin{array}{c} \theta(t)-\theta^*\\ \dot{\theta}(t)\\ \theta_e(t)\\\hline z(t)-z^*\\ \dot{z}(t) \end{array}\right] }_{x'(t)}\\ + \underbrace{ \left[\begin{array}{c} 0 \\ 0 \\ \omega_e\\\hline 0 \\ 0 \end{array}\right] }_{B'=\left[\begin{array}{cc} B'_1 \\ B'_2 \end{array}\right] } u(t) \end{array}}$

$\displaystyle{(12)\quad \begin{array}{l} \underbrace{ \left[\begin{array}{c} \theta(t)-\theta^*\\ \dot{\theta}(t)\\ \theta_e(t) \end{array}\right] }_{y(t)} = \underbrace{ \left[\begin{array}{ccc|cc} 1 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 \end{array}\right] }_{C'=[I_p\ 0]} \underbrace{ \left[\begin{array}{c} \theta(t)-\theta^*\\ \dot{\theta}(t)\\ \theta_e(t)\\\hline z(t)-z^*\\ \dot{z}(t) \end{array}\right] }_{x'(t)} \end{array}}$

このとき、 $U$ を

$\displaystyle{(13)\quad U= \underbrace{\left[\begin{array}{ccc|cc} -\ell_{11} & -\ell_{12} & -\ell_{13} & 1 & 0 \\ -\ell_{21} & -\ell_{22} & -\ell_{23} & 0 & 1 \end{array}\right]}_{[-L\ I_{n-p}]} }$

のように選んで、オブザーバのパラメータを次のように求めることができます。

$\displaystyle{(14)\quad \left[\begin{array}{cc} \hat{C} & \hat{D} \end{array}\right]= \left[\begin{array}{cc} U\\ C' \end{array}\right]^{-1}= \left[\begin{array}{cc} -L & I_{n-p}\\ I_p& 0 \end{array}\right]^{-1} = \left[\begin{array}{cc} 0 & I_p\\ I_{n-p}& L \end{array}\right] }$

$\displaystyle{(15)\quad \begin{array}{l} \left[\begin{array}{cc} \hat{A} & \hat{B} \end{array}\right]=UA' \left[\begin{array}{cc} U\\ C' \end{array}\right]^{-1}= \left[\begin{array}{cc} -L & I_{n-p} \end{array}\right] \left[\begin{array}{cc} A'_{11} & A'_{12}\\ A'_{21} & A'_{22} \end{array}\right] \left[\begin{array}{cc} 0 & I_p\\ I_{n-p}& L \end{array}\right]\\ =\left[\begin{array}{cc} A'_{22}-LA'_{12} & A'_{21}-LA'_{11}+\hat{A}L \end{array}\right] \end{array} }$

$\displaystyle{(16)\quad \hat{J}=UB'= \left[\begin{array}{cc} -L & I_{n-p} \end{array}\right] \left[\begin{array}{c} B'_1\\ B'_2 \end{array}\right] =B'_2-LB'_1 }$

ここで、設計パラメータはサイズ $n-p\times p$ の行列 $L$ で

$\displaystyle{(17)\quad \hat{A} = \underbrace{\left[\begin{array}{ccc} 0 & 1 \\ a_{52} & 0 \end{array}\right]}_{A'_{22}}- \underbrace{\left[\begin{array}{ccc} \ell_{11} & \ell_{12} & \ell_{13} \\ \ell_{21} & \ell_{22} & \ell_{23} \end{array}\right]}_{L} \underbrace{\left[\begin{array}{ccc} 0 & 0 \\ a_{62} & 0 \\ 0 & 0 \end{array}\right]}_{A'_{12}} }$

を安定行列とするように選びます。そのためには、仮想システムに対する状態フィードバックによる安定化問題

$\displaystyle{ \begin{array}{cl} (18.1) &\left[\begin{array}{c} \dot{w}_1 \\ \dot{w}_2 \end{array}\right] = \underbrace{\left[\begin{array}{ccc} 0 & a_{52} \\ 1 & 0 \end{array}\right]}_{A'_{22}^T} \left[\begin{array}{c} w_1 \\ w_2 \end{array}\right]+ \underbrace{\left[\begin{array}{ccc} 0 & a_{62} & 0 \\ 0 & 0 & 0 \end{array}\right]}_{A'_{12}^T} \left[\begin{array}{c} v_1 \\ v_2 \\ v_3 \end{array}\right]\\ (18.2) &\left[\begin{array}{c} v_1 \\ v_2 \\ v_3 \end{array}\right]=- \underbrace{\left[\begin{array}{ccc} \ell_{11} & \ell_{21} \\ \ell_{12} & \ell_{22} \\ \ell_{13} & \ell_{23} \end{array}\right]}_{L^T} \left[\begin{array}{c} w_1 \\ w_2 \end{array}\right] \end{array} }$

すなわち

$\displaystyle{ \begin{array}{cl} (19.1) &\left[\begin{array}{c} \dot{w}_1 \\ \dot{w}_2 \end{array}\right] = \left[\begin{array}{ccc} 0 & a_{52} \\ 1 & 0 \end{array}\right] \left[\begin{array}{c} w_1 \\ w_2 \end{array}\right]+ \left[\begin{array}{c} a_{62} \\ 0 \end{array}\right]v_2\\ (19.2) &v_2=- \left[\begin{array}{ccc} \ell_{12} & \ell_{22} \end{array}\right] \left[\begin{array}{c} w_1 \\ w_2 \end{array}\right] \end{array} }$

を解けばよいと言えます（ $\ell_{11}=\ell_{21}=0$ 、 $\ell_{13}=\ell_{23}=0$ ）。