7 非線形システムの線形化

【本章のねらい】
・振子を例に，MAXIMA $^*$ を用いて，非線形運動方程式を計算し，非線形状態方程式を求める。また，平衡状態のまわりで線形化して，線形状態方程式を求め，安定性と可制御性の判定を行う。
・振子の線形状態方程式を用いて，積分動作を加えた状態フィードバックのLQI設計を行い，これを非線形状態方程式に適用して，安定化シミュレーションを行う。

* http://ja.wikipedia.org/wiki/Maximaを参照。

7.1 非線形システムのモデリングの例

次の例題を考える。

例題7.1 図7.1の振子を考える。MAXIMAを用いて，非線形運動方程式を計算し，これから非線形状態方程式を求めよ。さらに，平衡状態のまわりで線形化して，線形状態方程式を求めよ。

図7.1

解答運動エネルギー $T=\frac{1}{2}J\dot{\theta}^2$ （ $J=\frac{1}{3}m\ell^2$ ），ポテンシャルエネルギー $V=mg\cos\theta$ から，ラグランジュ関数 $L=T-V$ を得て，これをラグランジュ方程式

$\displaystyle{\frac{d}{dt}\left(\frac{\partial L}{\partial \dot\theta}\right)-\frac{\partial L}{\partial\theta}=0}$

に代入する。この計算をMAXIMAを用いて行うためには，コマンド

/*pen*/
dth:’diff(th(t),t);
ddth:’diff(th(t),t,2);
x:ell*sin(th(t));
y:ell*cos(th(t));
J:(1/3)*m*ell^2;
T:(1/2)*m*(diff(x,t)^2+diff(y,t)^2)+(1/2)*J*dth^2;
V:m*g*ell*cos(th(t));
L:T-V;
LE:diff(diff(L,dth),t)-diff(L,th(t))=0,trigreduce;
sol:solve(LE,ddth);
f:matrix([dth],[rhs(sol[1])]);

を与えればよい $^*$ 。すなわち，非線形運動方程式は次式により得られる。

$\displaystyle{\ddot{\theta}=\frac{3g}{4\ell}\sin\theta}$

これより，非線形状態方程式

$\underbrace{ \left[\begin{array}{c} \dot{\theta}\\ \ddot{\theta} \end{array}\right] }_{\dot \xi} = \underbrace{ \left[\begin{array}{c} \dot{\theta}\\ \frac{3g}{4\ell}\sin\theta \end{array}\right] }_{f(\xi)}$

が求められる。平衡状態は

$f(\xi^*)=0 \Rightarrow \xi^* =\left[\begin{array}{c} \theta^* \\ 0 \end{array}\right]$ ( $\theta^*=0,\pi$ )

のように求められる。この平衡状態 $\xi^*$ のまわりで， $f(\xi)$ の右辺を１次近似して

$\underbrace{ \left[\begin{array}{c} f_1(\xi)\\ f_2(\xi) \end{array}\right] }_{f(\xi)} \simeq \underbrace{\left. \left[\begin{array}{cc} 0 & 1 \\ \frac{\partial f_2}{\partial\theta} & \frac{\partial f_2}{\partial\dot{\theta}} \end{array}\right]\right|_{{\theta=\theta^*}\atop{\dot{\theta}=0\ }} }_{A} \underbrace{ \left[\begin{array}{c} \theta-\theta^*\\ \dot{\theta} \end{array}\right] }_{x=\xi-\xi^*}$

を得る。この計算をMAXIMAを用いて行うためには，コマンド

A:matrix([diff(f[1,1],th(t)),diff(f[1,1],dth)],
[diff(f[2,1],th(t)),diff(f[2,1],dth)]);

を与えればよい。すなわち，線形状態方程式は，次式のように求められる。

$\underbrace{ \frac{d}{dt} \left[\begin{array}{c} \theta-\theta^*\\ \dot{\theta} \end{array}\right] }_{\dot x} = \underbrace{ \left[\begin{array}{cc} 0 & 1 \\ \frac{3g}{4\ell}\cos\theta^* & 0 \end{array}\right] }_{A} \underbrace{ \left[\begin{array}{c} \theta-\theta^*\\ \dot{\theta} \end{array}\right] }_{x}$

この線形状態方程式は，平衡状態近傍では，

$\sin\theta\simeq\cos\theta^*(\theta-\theta^*)$

と近似できるので，これを非線形運動方程式に代入し

$\displaystyle{\ddot{\theta}=\frac{3g}{4\ell}\cos\theta^*(\theta-\theta^*)}$

を得て，これから直接求めることもできる。

演習7.1 例題7.1において， $\ell=0.25{\rm [m]},g=9.8{\rm [m/s^2]}$ のとき，つぎの初期値について，Simulinkを用いて，非線形状態方程式と安定な線形状態方程式による時間応答 $\theta(t)\,(0\le t\le10)$ を比較せよ。
(1) $\theta(0)=(3/180)\pi,\dot{\theta}(0)=0$
(2) $\theta(0)=(177/180)\pi,\dot{\theta}(0)=0$

例題7.2 図7.2の台車によって駆動される振子を考える。MAXIMAを用いて，非線形運動方程式を計算し，これから非線形状態方程式を求めよ。さらに，平衡状態のまわりで線形化して，線形状態方程式を求めよ。

図7.2

解答台車の運動エネルギーは $T_{cart}=\frac{1}{2}M\dot{r}^2$ ，振子の運動エネルギーは
$T_{pen}=\frac{1}{2}m(\dot{x}^2+\dot{y}^2)+\frac{1}{2}J\dot{\theta}^2$ （ $J=\frac{1}{3}m\ell^2$ ），
であり，振子のポテンシャルエネルギーは $U_{pen}=mg\cos\theta$ 。これらから，
ラグランジュ関数 $L=T_{cart}+T_{pen}-U_{pen}$ を得て，これをラグランジュ方程式

$\displaystyle{\frac{d}{dt}\left(\frac{\partial L}{\partial \dot{r}}\right)-\frac{\partial L}{\partial r}=F}$

$\displaystyle{\frac{d}{dt}\left(\frac{\partial L}{\partial \dot\theta}\right)-\frac{\partial L}{\partial\theta}=0}$

に代入する。この計算をMAXIMAを用いて行うためには，コマンド

/*pend*/
dr:’diff(r(t),t); ddr:’diff(r(t),t,2);
dth:’diff(th(t),t); ddth:’diff(th(t),t,2);
T0:(1/2)*M*diff(r(t),t)^2;
x1:r(t)+ell*sin(th(t)); y1:ell*cos(th(t));
J1:(1/3)*m*ell^2;
T1:(1/2)*m*(diff(x1,t)^2+diff(y1,t)^2)+(1/2)*J1*dth^2;
V1:m*g*y1;
L:T0+T1-V1;
LE1:diff(diff(L,dr),t)-diff(L,r(t))=F,trigreduce,ratsimp;
LE2:diff(diff(L,dth),t)-diff(L,th(t))=0,trigreduce,ratsimp;
sol:solve([LE1,LE2],[ddr,ddth]);

を与えればよい。この結果から，非線形運動方程式

$\underbrace{ \left[\begin{array}{cc} M+m & m\ell\cos\theta \\ m\ell\cos\theta & \frac{4}{3}m\ell^2 \end{array}\right] }_{M(\theta)} \underbrace{ \left[\begin{array}{c} \ddot{r}\\ \ddot{\theta} \end{array}\right] }_{\ddot{\xi}_1} + \underbrace{ \left[\begin{array}{cc} 0 & -m\ell\dot{\theta}\sin\theta \\ 0 & 0 \end{array}\right] }_{C(\theta)} \underbrace{ \left[\begin{array}{c} \dot{r}\\ \dot{\theta} \end{array}\right] }_{\dot{\xi}_1}$ \\
\hspace{5mm}
$+ \underbrace{ \left[\begin{array}{c} 0 \\ -m\ell g\sin\theta \end{array}\right] }_{G(\theta)} = \underbrace{ \left[\begin{array}{c} F\\ 0 \end{array}\right] }_{\zeta}$

が得られる。これより，非線形状態方程式

$\underbrace{ \left[\begin{array}{c} \dot{\xi}_1\\ \ddot{\xi}_1 \end{array}\right] }_{\dot \xi} = \underbrace{ \left[\begin{array}{c} \dot{\xi}_1\\ -M(\theta)^{-1}(\zeta-C(\theta)\dot{\xi}_1-G(\theta)) \end{array}\right] }_{f(\xi,\zeta)}$

が求められる。平衡状態は， $f(\xi^*,\zeta^*)=0$ より， $\dot{\xi}_1=0$ ，したがって， $G(\theta)=\zeta$ を満足することから

$\xi^* =\left[\begin{array}{c} 0 \\ \theta^* \\ 0 \\ 0 \end{array}\right]$ ( $\theta^*=0,\pi$ )，
$\zeta^* =\left[\begin{array}{c} F^* \\ 0 \end{array}\right]$ ( $F^*=0$ )

のように求められる。この平衡状態近傍で， $f(\xi,\zeta)$ の右辺を１次近似して

$\underbrace{ \left[\begin{array}{c} \dot{r}\\ \dot{\theta}\\ f_3(r,\theta,\dot{r},\dot{\theta})\\ f_4(r,\theta,\dot{r},\dot{\theta})\\ \end{array}\right] }_{f(\xi,\zeta)} \simeq \underbrace{\left. \left[\begin{array}{cccc} 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \frac{\partial f_3}{\partial r} & \frac{\partial f_3}{\partial\theta} & \frac{\partial f_3}{\partial\dot{r}} & \frac{\partial f_3}{\partial\dot{\theta}} \\ \frac{\partial f_4}{\partial r} & \frac{\partial f_4}{\partial\theta} & \frac{\partial f_4}{\partial\dot{r}} & \frac{\partial f_4}{\partial\dot{\theta}} \end{array}\right] \right|_{{{{{r=0}\atop{\theta=\theta^*}}\atop{\dot{r}=0}}\atop{\dot{\theta}=0}}\atop{F^*=0}} }_{A} \underbrace{ \left[\begin{array}{c} r \\ \theta-\theta^* \\ \dot{r} \\ \dot{\theta} \end{array}\right] }_{x=\xi-\xi^*}$
$+ \underbrace{\left. \left[\begin{array}{c} 0 \\ 0 \\ \frac{\partial f_3}{\partial F} \\ \frac{\partial f_4}{\partial F} \end{array}\right] \right|_{{{{{r=0}\atop{\theta=\theta^*}}\atop{\dot{r}=0}}\atop{\dot{\theta}=0}}\atop{F^*=0}} }_{B} \underbrace{ F }_{u}$

を得る。この計算をMAXIMAを用いて行うためには，コマンド

f:matrix([dr],[dth],[rhs(sol[1][1])],[rhs(sol[1][2])]);
a31:diff(f[3,1],r(t)); a32:diff(f[3,1],th(t));
a33:diff(f[3,1],dr); a34:diff(f[3,1],dth);
a41:diff(f[4,1],r(t)); a42:diff(f[4,1],th(t));
a43:diff(f[4,1],dr); a44:diff(f[4,1],dth);
b3:diff(f[3,1],F); b4:diff(f[4,1],F);
A:matrix([0,0,1,0],[0,0,0,1],[a31,a32,a33,a34],
[a41,a42,a43,a44]); B:matrix([0],[0],[b3],[b4]);
A1:A,th(t)=0,F=0,trigreduce,ratsimp;
B1:B,th(t)=0,F=0,trigreduce,ratsimp;
A2:A,th(t)=%pi,F=0,trigreduce,ratsimp;
B2:B,th(t)=%pi,F=0,trigreduce,ratsimp;

を与えればよい。この結果から， $\theta^*=0$ のときの線形状態方程式は

$\underbrace{ \frac{d}{dt} \left[\begin{array}{c} r \\ \theta \\ \dot{r} \\ \dot{\theta} \end{array}\right] }_{\dot x} = \underbrace{ \left[\begin{array}{cccc} 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ 0 & -\frac{3mg}{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 \\ \theta \\ \dot{r} \\ \dot{\theta} \end{array}\right] }_{x} + \underbrace{ \left[\begin{array}{cccc} 0 \\ 0 \\ \frac{4}{4M+m} \\ -\frac{3}{(4M+m)\ell} \\ \end{array}\right] }_{B} \underbrace{ F }_{u}$

となる。また， $\theta^*=\pi$ のときの線形状態方程式は，{\tt th(t)=\%pi}
% \hspace{5mm}{\tt A2:A,th(t)=\%pi,F=0,trigreduce,ratsimp; }\\
% \hspace{5mm}{\tt B2:B,th(t)=\%pi,F=0,trigreduce,ratsimp;}\\
として

$\underbrace{ \frac{d}{dt} \left[\begin{array}{c} r \\ \theta-\pi \\ \dot{r} \\ \dot{\theta} \end{array}\right] }_{\dot x} = \underbrace{ \left[\begin{array}{cccc} 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ 0 & -\frac{3mg}{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 \\ \theta-\pi \\ \dot{r} \\ \dot{\theta} \end{array}\right] }_{x} + \underbrace{ \left[\begin{array}{cccc} 0 \\ 0 \\ \frac{4}{4M+m} \\ \frac{3}{(4M+m)\ell} \\ \end{array}\right] }_{B} \underbrace{ F }_{u}$ \\
のように求められる。これらは，平衡状態近傍での近似式
$\sin\theta\simeq\cos\theta^*(\theta-\theta^*),\dot{\theta}^2=0$
を非線形運動方程式に代入して得られる

$\left[\begin{array}{cc} M+m & m\ell\cos\theta^* \\ m\ell\cos\theta^* & \frac{4}{3}m\ell^2 \end{array}\right] \left[\begin{array}{c} \ddot{r}\\ \ddot{\theta} \end{array}\right] + \left[\begin{array}{cc} 0 & 0 \\ 0 & 0 \end{array}\right] \left[\begin{array}{c} \dot{r}\\ \dot{\theta} \end{array}\right]$
$+ \left[\begin{array}{c} 0 \\ -m\ell g\cos\theta^*(\theta-\theta^*) \end{array}\right] = \left[\begin{array}{c} F\\ 0 \end{array}\right]$ \\
から直接求めたものと一致する。

演習7.2 例題7.2の振子を軸支した台車を，図7.3のように傾斜角 $\alpha$ の台上に置いた。MAXIMAを用いて，非線形運動方程式を計算し，これから非線形状態方程式を求めよ。さらに，平衡状態のまわりで線形化して，線形状態方程式を求めよ。

図7.3

例題7.3 例題7.2の台車によって駆動される振子について，そこで求めた状態方程式に基づいて，MAXIMAを用いて，漸近安定性と可制御性について調べよ。

解答まず，安定性を調べるために， $A$ 行列の固有値を計算する。これは，MAXIMAに，のコマンドを与えればよい。

lambda:eigenvalues(A1);

その結果， $\theta^*=0$ の場合， $\pm \sqrt{\frac{3(M+m)g}{(4M+m)\ell}}$ となり，また， $\theta^*=\pi$ の場合， $\pm j\sqrt{\frac{3(M+m)g}{(4M+m)\ell}}$ となる。どちらとも，漸近安定とはいえない。

つぎに，可制御性を， $A$ 行列の固有値に基づいて調べるためには，各固有値 $\lambda_i\,(i=1,\cdots,4)$ について，行列 $\left[\begin{array}{cc} B & \lambda I_n -A \end{array}\right]$ の階数が次数４に等しいことを確認すればよい。これは， $\theta^*=0$ の場合，MAXIMAに

rank(addcol(A1-lambda[1][1]*ident(4),B1));
rank(addcol(A1-lambda[1][2]*ident(4),B1));
rank(addcol(A1-lambda[2][1]*ident(4),B1));
rank(addcol(A1-lambda[2][2]*ident(4),B1));

% For[i = 1, i $\le$ 4,
% W=MapThread[Append,{A-lambda[[i]] IdentityMatrix[4],B}];
% Print[MatrixRank[W]]; i++]
のコマンドを与えればよい。その結果，階数はすべて４と計算され，可制御性が成り立つ。 $\theta^*=\pi$ の場合も同様である。

演習7.3 例題7.3を， $M=1{\rm [kg]},m=0.1{\rm [kg]},\ell=0.25{\rm [m]},g=9.8{\rm [m/s^2]}$ のとき，MATLABにより検討せよ。

7.2 非線形系に対する線形制御の適用

つぎの例題を考える。

例題7.4 例題7.2の台車によって駆動される倒立振子について，台車の位置 $r$ と振子の角度 $\theta$ が計測できるものとする。このとき，振子を倒立状態に保ちながら，台車を指定位置 $r^*=0.5{\rm [m]}$ まで移動させるための，積分動作を加えた状態フィードバックを求めよ。ただし， $M=1{\rm [kg]},m=0.1{\rm [kg]},\ell=0.25{\rm [m]},g=9.8{\rm [m/s^2]}$ とする。

解答まず，出力方程式として

$\underbrace{ \left[\begin{array}{c} r \\ \theta \end{array}\right] }_{y_M} = \underbrace{ \left[\begin{array}{cccc} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \end{array}\right] }_{C_M} \underbrace{ \left[\begin{array}{c} r \\ \theta \\ \dot{r} \\ \dot{\theta} \end{array}\right] }_{x}$

を得る。また，台車の位置 $r$ は

$\underbrace{ r }_{z} = \underbrace{ \left[\begin{array}{cccc} 1 & 0 \end{array}\right] }_{C_S} \underbrace{ \left[\begin{array}{c} r \\ \theta \end{array}\right] }_{y_M} = \underbrace{ \left[\begin{array}{cccc} 1 & 0 & 0 & 0 \end{array}\right] }_{C=C_SC_M} \underbrace{ \left[\begin{array}{c} r \\ \theta \\ \dot{r} \\ \dot{\theta} \end{array}\right] }_{x}$

のように表される。このとき，正方行列

$S=\left[\begin{array}{cc} A & B \\ C & 0 \end{array}\right] = \left[\begin{array}{cccc:c} 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 1 & 0\\ 0 & -\frac{3mg}{4M+m} & 0 & 0 & \frac{3}{4M+m}\\ 0 & \frac{3(M+m)g}{(4M+m)\ell} & 0 & 0 & -\frac{3}{(4M+m)\ell} \\ \hdashline 1 & 0 & 0 & 0 & 0 \end{array}\right]$

は正則である。したがって，次式が定まる。

$\left[\begin{array}{c} x_\infty \\ u_\infty \end{array}\right] = \left[\begin{array}{c} r_\infty \\ \theta_\infty \\ \dot{r}_\infty \\ \dot{\theta}_\infty \\\hdashline u_\infty \end{array}\right] =S^{-1} \left[\begin{array}{c} 0 \\ 0 \\ 0 \\ 0 \\\hdashline r^* \end{array}\right] = \left[\begin{array}{c} r^* \\ 0 \\ 0 \\ 0 \\\hdashline 0 \end{array}\right]$

つぎに，偏差系

$\frac{d}{dt} \underbrace{ \left[\begin{array}{c} x-x_\infty \\ u-u_\infty \end{array}\right] }_{x_{E3}} = \underbrace{ \left[\begin{array}{cccc} A & B \\ 0 & 0 \end{array}\right] }_{A_{E3}} \underbrace{ \left[\begin{array}{c} x-x_\infty \\ u-u_\infty \end{array}\right] }_{x_{E3}} + \underbrace{ \left[\begin{array}{cccc} 0 \\ 1 \\ \end{array}\right] }_{B_{E3}} \dot{u}$

に対する評価関数を，たとえば

$\displaystyle{J=\int_0^\infty\{\frac{1}{K_r^2}(r-r^*)^2+\frac{1}{K_\theta^2}\theta^2+\frac{T_r^2}{K_r^2}\dot{r}^2+\frac{T_\theta^2}{K_\theta^2}\theta^2}$ $\displaystyle{+\frac{1}{K_u^2}u^2+\frac{T_u^2}{K_u^2}\dot{u}^2\}\,dt}$

のように設定する。ここで， $T_r,K_r$ と $T_\theta,K_\theta$ は，それぞれ閉ループ系における台車位置と角度の時間応答の速さ（どれくらいの時間である値に到達するか）を表している。たとえば，台車を5秒間に50cm移動させるとき，周期 $T_\ell$ の振子は3度以内の振れに抑えることが妥当と考えられるときは， $T_r=5,K_r=0.5$ と $T_\theta=(1/4)T_\ell,K_\theta=(5/180)\pi$ と与える。つぎに， $T_u,K_u$ は駆動系の時間応答の速さを考慮して選ぶ。

制御目的を達成する積分動作を加えた状態フィードバック

$u=-Fx-F_Ix_I$ 　ただし， $x_I=\int_0^t(r(\tau)-r^*)\,d\tau$

は，上の評価関数を最小にするように，状態フィードバック

$\dot{u}=- \left[\begin{array}{cc} K & K_I \end{array}\right]x_{E3}$

を求め，次式から $F$ と $F_I$ を定めればよい。

$\left[\begin{array}{cc} F & F_I \end{array}\right] = \left[\begin{array}{cc} K & K_I \end{array}\right]S^{-1}$

以上の計算を，MATLABで行うためには

%pend.m
global M m ell g th0
M=1; m=0.1; ell=0.25; g=9.8; th0=3/180*pi;
E=[M+m m*ell;m*ell (4/3)*m*ell^2];
A21=-E\[0 0;0 -m*ell*g]; A=[zeros(2) eye(2);A21 zeros(2)]
B2=E\[1;0]; B=[zeros(2,1);B2]
CM=[1 0 0 0;0 1 0 0]; CS=[1 0]; C=CS*CM;
AE=[A B;zeros(1,5)]; BE=[zeros(4,1); 1]; CE=eye(5);
Tcart=0.1; Kr=0.5;
Tpend=(1/4)*(2*pi*sqrt(4/3*ell/g)); Kth=5/180*pi;
Tamp=0.1; Kamp=5;
q1=1/Kr; q2=1/Kth; q3=Tcart/Kr; q4=Tpend/Kth; q5=1/Kamp;
QE=diag([q1 q2 q3 q4 q5].^2); RE=(Tamp/Kamp)^2;
[FE,pAE]=opt(AE,BE,CE,QE,RE)
FE=FE/[A B;C 0]; F=FE(:,1:4); FI=FE(:,5);
ACL=[A-B*F -B*FI;C 0];
BCL=[zeros(4,1);-0.5];
CCL=[CM zeros(2,1);-F -FI];
DCL=[zeros(2,1);0];
sys=ss(ACL,BCL,CCL,DCL);
step(sys)

を与えればよい。

もちろん，これらは重み係数の初期値であり，実際には調整を伴うことはいうまでもない。

演習7.4 演習7.3のように，振子を軸支した台車を傾斜角 $\alpha=(5/180)\pi{\rm [rad]}$ の台上に置く場合，例題7.4と同様の設計を行え。

例題7.5 例題7.4で設計した積分動作を加えた状態フィードバックを，非線形ダイナミクスを表す非線形状態方程式と結合して，閉ループ系の時間応答をシミュレーションせよ。

解答まず，非線形状態方程式を，つぎのS-functionで記述する。

%spend.m
function [sys,x0]=spend(t,state,input,flag)
global M m ell g th0
if abs(flag)==1
u =input(1);
r =state(1); th =state(2); dr =state(3); dth=state(4);
Mp=[M+m m*ell*cos(th); m*ell*cos(th) (4/3)*m*ell^2];
Cp=[0 -m*ell*dth*sin(th);0 0];
Gp=[0; -m*ell*g*sin(th)];
sys=[dr;dth;Mp\(-Cp*[dr;dth]-Gp+[u;0])];
elseif flag==3,
sys=state(1:2);
elseif flag==0
sys=[4;0;2;1;0;0];
x0=[0;th0;0;0];
else
sys=[];
end

つぎに，Simulink上に

図7.4

のように，このS-functionブロックと例題??で設計した積分動作を加えた状態フィードバックを接続して，シミュレーションを行う。

演習7.5 演習7.4で設計した積分動作を加えた状態フィードバックを，非線形ダイナミクスを表す非線形状態方程式と結合して，閉ループ系の時間応答をシミュレーションせよ。

演習問題の解答

【演習7.1】Simulinkを用いて，次図のブロック線図を作成する。

上半分が非線形運動方程式 $\ddot{\theta}=\frac{3g}{4\ell}\sin\theta$ を，下半分が平衡状態 $\theta^*=\pi$ まわりの線形運動方程式 $\ddot{\theta}=-\frac{3g}{4\ell}(\theta-\theta^*)$ を表している。角度を得る積分器の初期値は，非線形運動方程式の場合は $\theta(0)$ を，線形運動方程式の場合は $\theta(0)-\pi$ を与えることに注意する。すなわち，(1)では，非線形運動方程式の場合は $\theta(0)=(3/180)\pi$ を，線形運動方程式の場合は $\theta(0)-\pi=(-177/180)\pi$ を与える。また，(2)では，非線形運動方程式の場合は $\theta(0)=(177/180)\pi$ を，線形運動方程式の場合は $\theta(0)-\pi=(-3/180)\pi$ を与える。

【演習7.2】非線形運動方程式を求める計算をMAXIMAを用いて行うためには

/*pend2*/
dr:’diff(r(t),t); ddr:’diff(r(t),t,2);
dth:’diff(th(t),t); ddth:’diff(th(t),t,2);
x0:r(t)*cos(alpha); y0:r(t)*sin(alpha);
T0:(1/2)*M*(diff(x0,t)^2+diff(y0,t)^2);
V0:M*g*y0;
x1:x0+ell*sin(th(t)); y1:y0+ell*cos(th(t));
J1:(1/3)*m*ell^2;
T1:(1/2)*m*(diff(x1,t)^2+diff(y1,t)^2)+(1/2)*J1*dth^2;
V1:m*g*y1;
L:T0+T1-V0-V1;
LE1:diff(diff(L,dr),t)-diff(L,r(t))=F,trigreduce,ratsimp;
LE2:diff(diff(L,dth),t)-diff(L,th(t))=0,trigreduce,ratsimp;
sol:solve([LE1,LE2],[ddr,ddth]);

のコマンドを与えればよい。この結果から，非線形運動方程式
$\underbrace{ \left[\begin{array}{cc} M+m & m\ell\cos(\theta+\alpha) \\ m\ell\cos(\theta+\alpha) & \frac{4}{3}m\ell^2 \end{array}\right] }_{M(\theta)} \underbrace{ \left[\begin{array}{c} \ddot{r}\\ \ddot{\theta} \end{array}\right] }_{\ddot{\xi}_1} + \underbrace{ \left[\begin{array}{cc} 0 & -m\ell\dot{\theta}\sin(\theta+\alpha) \\ 0 & 0 \end{array}\right] }_{C(\theta)} \underbrace{ \left[\begin{array}{c} \dot{r}\\ \dot{\theta} \end{array}\right] }_{\dot{\xi}_1}$
$+ \underbrace{ \left[\begin{array}{c} 0 \\ -m\ell g\sin\theta \end{array}\right] }_{G(\theta)} = \underbrace{ \left[\begin{array}{c} F-(M+m)g\sin\alpha\\ 0 \end{array}\right] }_{\zeta}$
が得られる。これより，平衡状態は
$\xi^* =\left[\begin{array}{c} 0 \\ \theta^* \\ 0 \\ 0 \end{array}\right]$ ( $\theta^*=0,\pi$ )，
$\zeta^* =\left[\begin{array}{c} F^* \\ 0 \end{array}\right]$ ( $F^*=(M+m)g\sin\alpha$ )
のように求まる。 $\theta^*=0$ のときの線形状態方程式は
$\underbrace{ \frac{d}{dt} \left[\begin{array}{c} r \\ \theta \\ \dot{r} \\ \dot{\theta} \end{array}\right] }_{\dot x} = \underbrace{ \left[\begin{array}{cccc} 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ 0 & -\frac{6mg\cos\alpha}{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 \\ \theta \\ \dot{r} \\ \dot{\theta} \end{array}\right] }_{x}$ \\
$+ \underbrace{ \left[\begin{array}{cccc} 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-(M+m)g\sin\alpha) }_{u}$

【演習7.3】

%pend.m
M=1; m=0.1; ell=0.25; g=9.8;
E=[M+m m*ell;m*ell (4/3)*m*ell^2];
A21=-E\[0 0;0 -m*ell*g]; A=[zeros(2) eye(2);A21 zeros]
B2=E\[1;0]; B=[zeros(2,1);B2]
r=eig(A)
tol=1e-6;
for i=1:4, k=rank([B A-r(i)*eye(4)],tol), end

【演習7.4】次を実行し，あとは例題7.4と同様に行えばよい。

%pend2.m
M=1; m=0.1; ell=0.25; g=9.8; alpha=5/180*pi;
a32=-(3*m*g*cos(alpha))/(7*M+(11-3*cos(2*alpha))/2*m);
a42=(3*(M+m)*g)/(7*M+(11-3*cos(2*alpha))/2*m)/ell;
b3=7/(7*M+(11-3*cos(2*alpha))/2*m);
b4=-3/(7*M+(11-3*cos(2*alpha))/2*m)/ell;
A21=[0 a32;0 a42]; A=[zeros(2) eye(2);A21 zeros(2)]
B2=[b3;b4]; B=[zeros(2,1);B2]

【演習7.5】まず，非線形ダイナミクスを，つぎのS-functionで記述する。

%spend2.m
function [sys,x0]=spend2(t,state,input,flag)
M=1; m=0.1; ell=0.25; g=9.8; th0=0; alpha=5/180*pi;
if abs(flag)==1
u=input(1);
r=state(1); th=state(2); dr=state(3); dth=state(4);
Mp=[M+m m*ell*cos(th+alpha); m*ell*cos(th+alpha) (7/3)*m*ell^2];
Cp=[0 -m*ell*dth*sin(th+alpha);0 0];
Gp=[0; -m*ell*g*sin(th)];
sys=[dr;dth;Mp $\backslash$ (-Cp*[dr;dth]-Gp+[u-(M+m)*g*sin(alpha);0])];
elseif flag==3
sys=state(1:2);
elseif flag==0
sys=[4;0;2;1;0;0]; x0=[0;th0;0;0];
else
sys=[];
end

Simulink上に，このS-functionブロックと，演習7.4で設計した積分動作を加えた状態フィードバックを記述して，シミュレーションを行う。詳細は割愛する。

制御系CAD

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

7 非線形システムの線形化