平衡状態回りの線形化

平衡状態回りの線形化…Homework

[0]　制御を考えるときは、まず対象の平衡状態(equilibrium state)を念頭におきます。そして何らかの要因でこの平衡状態が乱されたとき、元の平衡状態に戻すことが制御の基本的な役割と考えられます。制御の対象としては、力学系、電気系、化学系など様々なものがあり、その振舞いはサイエンスによってすでに十分検討されています。しかし、平衡状態を保持したり、遷移させたりという対象への働きかけは、エンジニアリングの範疇となります。人が楽に操れるドローンや１輪車というモノ作りは、制御という技術がなければ達成できなかったと言えます。

●制御系設計を確実に実施・達成するための方法として次図に示すHILS アプローチが知られています。
図1 HILS アプローチ

これから、制御系設計、仮想制御実験、制御実験とステップを踏むことが分かります。２台のPCを用いて実時間での閉ループシミュレーションを行う仮想制御実験の成否が決め手となるので、HILS:Hardware In the Loop Simulationがキーワードとなったと思われます。ただ、最初のステップである制御系設計を１台のPC上で十分検討することが大切であることは述べるまでもありません。制御系設計は①非線形シミュレータの開発と②コントローラの設計からなります。以下では、非線形シミュレータの開発について、
　 $1^\circ$ 運動方程式の導出（数式処理の利用）
　 $2^\circ$ 平衡状態の検討（fsolveの利用）
　 $3^\circ$ 非線形状態方程式の導出（S-functionの作成）
　 $4^\circ$ 非線形シミュレータの開発（平衡状態の保持）
　 $5^\circ$ 線形化（linmodの利用）
　 $6^\circ$ 線形シミュレータの開発
の手順を具体的に説明しますが、その前に非線形系と線形系の振舞いは平衡状態周りではどう違うのかについて振り子を例にとって説明します。

[1]　高校の物理で出てきた次の単振り子を考えます。これは吊り荷を下げたクレーンの振止めの問題を考えるのに役立ちます。

図2 単振り子

左側に示すように鉛直線に沿って垂れ下がった状態が平衡状態です。右側に示すように重りを手で（ひもが緩まないように）持ち上げて離したときの運動方程式は

$\displaystyle{(1)\quad mL\ddot\theta(t)=-mg\sin\theta(t)\ \Rightarrow\ \boxed{\ddot\theta(t)=-\frac{g}{L}\sin\theta(t)} }$

で与えられます。ここで、 $L$ は振り子の長さ、 $m$ は重りの質量、 $g$ は重力加速度、 $\theta(t)$ は時刻 $t$ における振り子の鉛直線となす角度を表しています。初期時刻 $t=0$ では $\theta(0)\ne0, \dot{\theta}(0)=0$ とします。

これは簡単な微分方程式のように思えますが、そうではありません。実際、

$\displaystyle{(2)\quad \sin x=x-\frac{x^3}{3!}+\frac{x^5}{5!}-\frac{x^7}{7!}+\cdots }$

のように展開できますから、運動方程式は

$\displaystyle{(3)\quad \ddot\theta(t)=-\frac{g}{L}(\theta(t)-\frac{\theta^3(t)}{3!}+\frac{\theta^5(t)}{5!}-\frac{\theta^7(t)}{7!}+\cdots) }$

となるのですが、これは手計算では手の付けようもありません。しかしながら、 $\theta$ が十分小さく、高次項が無視できれば

$\displaystyle{(4)\quad \boxed{\ddot\theta(t)=-\frac{g}{L}\theta(t)}\qquad(\theta(0)\ne0, \dot{\theta}(0)=0) }$

となって、解は次式で表されます。

$\displaystyle{(5)\quad \theta(t)=\theta(0)\cos\sqrt{\frac{g}{L}}t }$

２つの運動方程式(1)と(4)の違いを数値シミュレーションで見てみると、図3のようになります。初期角度が10 $^\circ$ のときは（平衡状態近傍では）両者に差異は見られませんが、90 $^\circ$ のときは周期が一致しません。また初期角度が177 $^\circ$ のときのシミュレーションを行ってみると、両者の差異に愕然することでしょう。

図3 単振り子の振動シミュレーション（モデルが線形か非線形か、初期状態を平衡状態周辺にとるかによる相違）

演習A12-1…Flipped Classroom

次のコードを用いて、図2のシミュレーションを行え。

MATLAB

%a12_1.m
 clear all, close all
 t=[0,10];
 x0=[10/180*pi;0];
 [t1,y1]=ode45(@f1,t,x0);
 [t2,y2]=ode45(@f2,t,x0);
 plot(t1,180/pi*y1(:,1),t2,180/pi*y2(:,1)), hold on
 x0=[90/180*pi;0];
 [t1,y1]=ode45(@f1,t,x0);
 [t2,y2]=ode45(@f2,t,x0);
 plot(t1,180/pi*y1(:,1),t2,180/pi*y2(:,1))
 grid
 title(' ');
 xlabel('Time[s]');
 ylabel('x1 and x2');
 legend('x_1','x_2')
%-----
 function dx=f1(t,x), L=1; g=9.8; dx=[x(2);-g/L*sin(x(1))]; end
 function dx=f2(t,x), L=1; g=9.8; dx=[0 1;-g/L 0]*x;  end
%eof

SCILAB

coming soon

＜ヒント＞　(1)と(4)は次のような連立１階微分方程式として表せることに注意。

$\displaystyle{(6)\quad \underbrace{ \left[\begin{array}{l} \dot{\theta}(t) \\ \dot{\omega}(t) \end{array}\right] }_{\dot{x}(t)} = \underbrace{ \left[\begin{array}{l} \omega(t) \\ -\frac{g}{L}\sin\theta(t) \end{array}\right] }_{f(x(t))} }$

$\displaystyle{(7)\quad \underbrace{ \left[\begin{array}{l} \dot{\theta}(t) \\ \dot{\omega}(t) \end{array}\right] }_{\dot{x}(t)}= \underbrace{ \left[\begin{array}{cc} 0 & 1 \\ -\frac{g}{L}&0 \end{array}\right] }_{A} \underbrace{ \left[\begin{array}{l} \theta(t) \\ \omega(t) \end{array}\right] }_{x(t)} }$

[2]　制御対象として次図に示す水タンクを例にとって、非線形シミュレータの開発手順を説明します。

図4 水タンクTANK

$1^\circ$ 運動方程式の導出（数式処理の利用）
時刻 $t$ における水位、流入量、流出量をそれぞれ $h(t)$ 、 $q_i(t)$ 、 $q_o(t)$ とします。タンクの断面積を $S$ とすると、運動方程式は次式となります。

$\displaystyle{(8)\quad S\dot{h}(t)=-\underbrace{k\sqrt{h(t)}}_{q_o(t)}+q_i(t)}$

$2^\circ$ 平衡状態の検討（fsolveの利用）
(8)から平衡状態 $h^*$ と平衡入力 $q_i^*$ が次式のように求まります。

$\displaystyle{(9)\quad 0=-k\sqrt{h^*}+q_i^*\Rightarrow q_i^*=k\sqrt{h^*}}$

$3^\circ$ 非線形状態方程式の導出（S-functionの作成）
(8)から次の非線形状態方程式を得ます。

$\displaystyle{(10)\quad \dot{h}(t)=f(h(t),q_i(t))=-\frac{k}{S}\sqrt{h(t)}+\frac{1}{S}q_i(t)}$

$4^\circ$ 非線形シミュレータの開発（平衡状態の保持）
(10)に基づいて制御対象TANKの平衡状態周りの挙動を模擬する非線形シミュレータを次図のように構成します。

図5 TANKの非線形シミュレータ

一般に、制御目的が平衡状態の安定化であれば、非線形シミュレータの入力側では平衡入力を加え、出力側では平衡状態を差し引いておきます。このとき、
「零入力 $u=0$ に対して零状態 $x=0$ が出力される」
はずですから、非線形シミュレータが正しく開発されているかのチェックに役立ちます。

$5^\circ$ 線形化（linmodの利用）

$\displaystyle{(11)\quad \begin{array}{l} \dot{h}(t)=f(h(t),q_i(t))=\\ f(h^*,q_i^*)+\frac{\partial f(h^*,q_i^*)}{\partial h}(h(t)-h^*)+\frac{\partial f(h^*,q_i^*)}{\partial q_i}(q_i(t)-q_i^*) \end{array} }$

から、次の線形状態方程式を得ます。

$\displaystyle{(12)\quad \underbrace{\frac{d}{dt}(h(t)-h^*)}_{\dot{x}(t)}=\underbrace{-\frac{k}{2S\sqrt{h^*}}}_{a}\underbrace{(h(t)-h^*)}_{x(t)}+\underbrace{\frac{1}{S}}_{b}\underbrace{(q_i(t)-q_i^*)}_{u(t)}}$

$6^\circ$ 線形シミュレータの開発
(12)に基づいて制御対象TANKの線形シミュレータを次図のように構成します。

図6 TANKの線形シミュレータ

● $k=0.1,S=1$ のとき、水位 $h^*=1$ の平衡状態にあるとします。今、バケツで水を入れたため、水位が $h(0)=1.2$ になったとします。このとき、平衡状態に戻る過程を非線形シミュレータと線形シミュレータで比較した結果を図7に示します。

図7 TANKの非線形応答と線形応答の比較

演習A12-2…Flipped Classroom

Simulinkを用いて図8を開発し、図7のグラフを描け。

図8 TANKの非線形シミュレータと線形シミュレータ

＜ヒント＞

Simulink

%sTANK.m
%-----
 function [sys,x0]=sTANK(t,state,input,flag)
%-----
 global h0
%-----
 if abs(flag)==1
  h=state(1);  
  qi=input(1);
  k=0.1; S=1;
  dh=-k/S*sqrt(h)+1/S*qi;
  sys=dh;
%-----  
 elseif flag==3
  sys=state(1);
 elseif flag==0
  sys=[1;0;1;1;0;0];
  x0=h0;
 else
  sys=[];
 end
%-----
%eof

MATLAB

%lTANK.m
 clear all, close all
 global h0
 k=0.1; S=1;
 h0=1;
%----- 平衡状態の計算
 x0=[1;1];
 x=fsolve(@TANK,x0)
 xs=x(1), us=x(2)
%----- 非線形シミュレータのチェック
 xs=1; us=k*sqrt(xs)
 sim("nTANK_obj")
%----- 線形化
 [a,b,c,d]=linmod('eTANK_linmod',xs,us)
 x0=0.2;
 sim("eTANK_linsys")  
%----- 線形応答と非線形応答の比較
 h0=1.2
 sim("nTANK_comp")  
%------
 function err=TANK(x)
   h=x(1); u=x(2); k=0.1;
   err=-k*sqrt(h)+u;
 end
%eof

[3] 一般には、制御対象の運動方程式から、非線形状態方程式を求め、これを平衡状態と平衡入力まわりで線形化（１次近似）し、線形状態方程式を求めます。

いま、 $\xi_1,\cdots,\xi_n$ を状態変数、 $\zeta_1,\cdots,\zeta_m$ を入力変数として、運動方程式から、次のような連立１階微分方程式で表される非線形状態方程式を得ます。

$\displaystyle{(13)\quad \left\{\begin{array}{l} \dot{\xi}_1=f_1(\xi_1,\cdots,\xi_n,\zeta_1,\cdots,\zeta_m) \\ \quad\vdots \\ \dot{\xi}_n=f_n(\xi_1,\cdots,\xi_n,\zeta_1,\cdots,\zeta_m) \end{array}\right. }$

これを次のようにベクトル表示します。 $\xi$ が状態変数ベクトル、 $\zeta$ が入力変数ベクトルです。

$\displaystyle{(14)\quad \boxed{\dot{\xi}=f(\xi,\zeta)}\ \Leftrightarrow\ \underbrace{ \left[\begin{array}{l} \dot{\xi}_1 \\ \vdots \\ \dot{\xi}_n \end{array}\right] }_{\dot{\xi}} = \underbrace{ \left[\begin{array}{l} f_1(\xi,\zeta) \\ \quad\vdots \\ f_n(\xi,\zeta) \end{array}\right] }_{f(\xi,\zeta)} }$

平衡状態 $\xi^*$ と平衡入力 $\zeta^*$ は

$\displaystyle{(15)\quad \dot{\xi}^*=f(\xi^*,\zeta^*)=const.}$

を満足するベクトルとして規定されます。これらの周りで $f(\xi,\zeta)$ を１次近似すると次式を得ます。

$\displaystyle{(16)\quad \dot{\xi}=f(\xi,\zeta)\simeq \underbrace{f(\xi^*,\zeta^*)}_{\dot{\xi}^*} +\frac{\partial f(\xi^*,\zeta^*)}{\partial \xi}(\xi-\xi^*) +\frac{\partial f(\xi^*,\zeta^*)}{\partial \zeta}(\zeta-\zeta^*) }$

ただし

$\displaystyle{(17)\quad A=\frac{\partial\,f(\xi^*,\zeta^*)}{\partial\,\xi}= \left[\begin{array}{ccc} \frac{\partial f_1(\xi^*,\zeta^*)}{\partial \xi_1}&\cdots& \frac{\partial f_1(\xi^*,\zeta^*)}{\partial \xi_n}\\ \vdots&\ddots&\vdots\\ \frac{\partial f_n(\xi^*,\zeta^*)}{\partial \xi_1}&\cdots& \frac{\partial f_n(\xi^*,\zeta^*)}{\partial \xi_n} \end{array}\right] }$

$\displaystyle{(18)\quad B=\frac{\partial\,f(\xi^*,\zeta^*)}{\partial\,\zeta}= \left[\begin{array}{ccc} \frac{\partial f_1(\xi^*,\zeta^*)}{\partial \zeta_1}&\cdots& \frac{\partial f_1(\xi^*,\zeta^*)}{\partial \zeta_m}\\ \vdots&\ddots&\vdots\\ \frac{\partial f_n(\xi^*,\zeta^*)}{\partial \zeta_1}&\cdots& \frac{\partial f_n(\xi^*,\zeta^*)}{\partial \zeta_m} \end{array}\right] }$

これから次の線形状態方程式を得ます。

$\displaystyle{(19)\quad \boxed{\underbrace{\dot{\xi}-\dot{\xi}^*}_{\dot{x}}= \underbrace{\frac{\partial f(\xi^*,\zeta^*)}{\partial \xi}}_A\underbrace{(\xi-\xi^*)}_x +\underbrace{\frac{\partial f(\xi^*,\zeta^*)}{\partial \zeta}}_B\underbrace{(\zeta-\zeta^*)}_u} }$

したがって、次に留意します。

線形状態方程式の「零状態 $x=0$ 」は平衡状態 $\xi=\xi^*$ にあることを意味し、これは平衡入力 $\zeta=\zeta^*$ によってもたらされるので「零入力 $u=0$ 」を前提とする。

●図9は非線形シミュレータ（非線形状態方程式）を用いて線形制御（状態フィードバック）の評価を行う場合の構成を、線形シミュレータ（線形状態方程式）を用いる場合と対比させています。

図9 非線形シミュレータと線形シミュレータの比較

演習A12-3…Flipped Classroom

非線形状態方程式(6)から線形状態方程式(7)を、次式を用いて示せ。

$\displaystyle{(20)\quad \underbrace{ \frac{d}{dt} \left[\begin{array}{cc} \xi_1-\xi_1^* \\ \xi_2-\xi_2^* \end{array}\right] }_{\dot{x}=\dot{\xi}-\dot{\xi}^*}= \underbrace{ \left[\begin{array}{cc} \frac{\partial f_1(\xi^*,\tau^*)}{\partial \xi_1} & \frac{\partial f_1(\xi^*,\tau^*)}{\partial \xi_2} \\ \frac{\partial f_2(\xi^*,\tau^*)}{\partial \xi_1} & \frac{\partial f_2(\xi^*,\tau^*)}{\partial \xi_2} \end{array}\right] }_{A=\frac{\partial f(\xi^*,\tau^*)}{\partial \xi}} \underbrace{ \left[\begin{array}{l} \xi_1-\xi_1^* \\ \xi_2-\xi_2^* \end{array}\right] }_{x=\xi-\xi^*} }$

●制御対象が $n$ 自由度をもつ力学系の場合、運動方程式の一般式は

$\displaystyle{(21)\quad M(\xi_1)\dot{\xi}_2+C(\xi_1,\xi_2)\xi_2+G(\xi_1)=\zeta\quad (\xi_2=\dot{\xi}_1) }$

で与えられます（ $\xi_1$ は $n$ 個の位置変数からなるベクトル、 $\xi_2$ は $n$ 個の速度変数からなるベクトル）。このとき非線形状態方程式は次式となります。

$\displaystyle{(22)\quad \underbrace{\frac{d}{dt} \left[\begin{array}{l} \xi_1 \\ \xi_2 \end{array}\right]}_{\dot{\xi}} =\underbrace{ \left[\begin{array}{l} \xi_2 \\ M^{-1}(\xi_1)(\zeta-C(\xi_1,\xi_2)\xi_2-G(\xi_1)) \end{array}\right]}_{f(\xi,\zeta)}} }$

また平衡状態 $\xi^*= \left[\begin{array}{l} \xi^*_1 \\ \xi^*_2 \end{array}\right]$ と平衡入力 $\zeta^*$ は非線形連立方程式

$\displaystyle{(23)\quad M(\xi_1)\underbrace{\dot{\xi}_2}_{0}+C(\xi_1^*,\xi_2^*)\xi^*_2+G(\xi_1^*)=\zeta^* }$

を解いて求めます（fsolveの利用）。さらに線形化を行うと

$\displaystyle{(24)\quad \underbrace{ \frac{d}{dt} \left[\begin{array}{cc} \xi_1-\xi_1^* \\ \xi_2-\xi_2^* \end{array}\right] }_{\dot{x}}= \underbrace{ \left[\begin{array}{cc} 0 & I_n \\ A_{21} & A_{22} \end{array}\right] }_{A} \underbrace{ \left[\begin{array}{l} \xi_1-\xi_1^* \\ \xi_2-\xi_2^* \end{array}\right] }_{x}+ \underbrace{ \left[\begin{array}{cc} 0 \\ B_2 \end{array}\right] }_{B} \underbrace{ (\zeta-\zeta^*) }_{u} }$

の形式となります。

Note A12　剛体振り子…Homework

一様な剛体の棒の一端を軸支した剛体振り子を考えます。この平衡状態はもちろん棒が鉛直線に沿っている状態ですが、回転軸が上端にある場合と下端にある場合とで２種類の平衡状態( $\alpha$ )と( $\beta$ )をもつところが単振り子と違います。（ $\beta$ )の方は、人が１輪車に乗った状況を模擬しているといえます。

図1 剛体振り子

いま回転軸回りの棒の運動を考えます。剛体の回転運動は「慣性モーメント×角加速度＝トルク」で表されますので、棒の回転角を $\theta$ とすると、運動方程式は次式となります。

$\displaystyle{(1)\quad J\ddot{\theta}(t)=-mg\ell\sin\theta(t) }$

ここで、 $2\ell$ は棒の長さ、 $m$ は棒の質量、 $g$ は重力加速度です。回転軸回りの慣性モーメント $J$ は、まず重心回りの慣性モーメント

$\displaystyle{(2)\quad J_0=\int_{-\ell}^{\ell}r^2\,dm=\int_{-\ell}^{\ell}r^2\,\frac{m}{2\ell}dr=\frac{m}{2\ell}\left[\frac{r^3}{3}\right]_{-\ell}^{\ell}=\frac{1}{3}m\ell^2 }$

を求めて、平行軸の定理を使って、次のように計算されます（積分範囲を $0$ から $2\ell$ としてもよい）。

$\displaystyle{(3)\quad J=J_0+m\ell^2=\frac{4}{3}m\ell^2 }$

結局、剛体振り子の運動を調べるためには、次の微分方程式を解くことになります。

$\displaystyle{(4)\quad \boxed{\ddot{\theta}(t)=-\frac{3g}{4\ell}\sin\theta(t) }}$

一般に平衡状態が何らかの原因で乱されたとき、元の平衡状態に戻すことが制御動作と考えられます。したがって、剛体振り子の平衡状態近傍の運動の理解が大切となります。単振り子の場合に調べたように、平衡状態回りでは右辺を１次近似しても、元の解の振舞いをよく表していました。でも剛体振り子の場合は２種類の平衡状態があるので、少し注意が必要です。 $\sin x$ の $x=a$ における展開式

$\displaystyle{(5)\quad \sin x=\sin a+\cos a\,(x-a)-\frac{\sin a}{2!}(x-a)^2+\cdots }$

を用いて、平衡状態( $\alpha$ )の近傍では（ $a=0$ ）、 $\sin\theta\simeq\theta$ として

$\displaystyle{(6)\quad \ddot\theta(t)=-\frac{3g}{4\ell}\theta(t) }$

また平衡状態( $\beta$ )の近傍では（ $a=\pi$ ）、 $\sin\theta\simeq\,\theta-\pi$ として

$\displaystyle{(7)\quad \ddot\theta(t)=\frac{3g}{4\ell}(\theta(t)-\pi) }$

のような１次近似を行います。これらに基づいてシミュレーションを行う場合は、それぞれ次式を用います。

$\displaystyle{(8)\quad \underbrace{ \left[\begin{array}{l} \dot{\theta}(t) \\ \dot{\omega}(t) \end{array}\right] }_{\dot{x}(t)}= \underbrace{ \left[\begin{array}{cc} 0 & 1 \\ -\frac{3g}{4\ell}&0 \end{array}\right] }_{A_\alpha} \underbrace{ \left[\begin{array}{l} \theta(t) \\ \omega(t) \end{array}\right] }_{x(t)} }$

$\displaystyle{(9)\quad \underbrace{ \frac{d}{dt} \left[\begin{array}{l} \theta(t)-\pi \\ \omega(t)-0 \end{array}\right] }_{\dot{x}(t)}= \underbrace{ \left[\begin{array}{cc} 0 & 1 \\ \frac{3g}{4\ell}&0 \end{array}\right] }_{A_\beta} \underbrace{ \left[\begin{array}{l} \theta(t)-\pi \\ \omega(t)-0 \end{array}\right] }_{x(t)} }$

これらの線形状態方程式(8),(9)は、非線形状態方程式(4)を、線形化して得られます。

最後に、長さ $2\ell$ をもつ剛体振り子と同じ周期をもつ単振り子の長さ $L$ を求めておきます。これは撃力中心の位置を表しています。これは振れ止めの制御を行う場合、重要な役割を果たします。

$\displaystyle{(10)\quad \frac{g}{L}=\frac{3g}{4\ell}\Rightarrow \boxed{L=\frac{2}{3}\times 2\ell} }$

図2 剛体振り子と同じ周期をもつ単振り子の長さは？

制御系CAD

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

平衡状態回りの線形化