LQ制御の応用 | 制御系CAD

LQ制御の応用…Homework

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

図1 様々な倒立振子

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

[2] CIP

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

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

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

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

を、評価関数

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

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

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

MATLAB

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

[3] CIP2

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

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

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

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

を、評価関数

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

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

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

MATLAB

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

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

[4] AIP

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

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

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

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

を、評価関数

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

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

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

MATLAB

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

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

[5] PIP

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

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

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

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

を、評価関数

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

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

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

MATLAB

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

[6] DIP

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

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

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

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

を、評価関数

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

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

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

MATLAB

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