基礎から学ぶ制御工学-20-3.7.5
基礎から学ぶ制御工学
amazon kindle版を出版しました。
3.7.5 例題4（倒立振子）
センサで剛体の傾きをフィードバックして、台車を動かし、剛体が倒れないようにします。
$M $：台車の質量[kg]
$m $：剛体の質量[kg]
$2L $：剛体の長さ（重心は中心の$L$）[m]
$\theta $：傾き[rad]
$\mu_1$：台車の粘性抵抗
$\mu_2$：振子の粘性抵抗
とします。
台車の質量$ M $より、台車の運動方程式は次式となります。
$\dfrac{1}{2}M \dot{x}^2$
図から、剛体の重心の位置は次のようになります。
$\begin{eqnarray} \left \lbrace \begin{array}{l} x_1=x+L \sin \theta \\ y=L \cos \theta \end{array} \right. \end{eqnarray} $ ・・・(3.7.37)
ここで、$x,\theta $は時間関数なので，$x_1,y_1$を微分すると
$\begin{eqnarray} \left \lbrace \begin{array}{l} \dot{x_1}=\dot{x}+L \dot{\theta} \cos \theta \\ \dot{y_1}=- L \dot{\theta} \sin \theta \end{array} \right. \end{eqnarray} $ ・・・(3.7.38)
よって、これらの式から、$J$を振子の慣性モーメントと$J=\dfrac{mL^2}{3}$すると、次式のようになります。
$T=\dfrac{1}{2}M \dot{x}^2+\dfrac{1}{2} m(\dot{x}^2+\dot{y}^2)+\dfrac{1}{2} J \dot{\theta}^2$ ・・・(3.7.39)
$=\dfrac{1}{2}M \dot{x}^2+\dfrac{1}{2}m((\dot{x}+L \dot{\theta} \cos \theta)^2+(- L \dot{\theta} \sin \theta)^2)+\dfrac{1}{2} J \dot{\theta}^2$
$=\dfrac{1}{2}M \dot{x}^2+\dfrac{1}{2}m(\dot{x}^2+L^2 \dot{\theta}^2 \cos^2 \theta+2 \dot{x} L \dot{\theta} \cos \theta+ L^2 \dot{\theta}^2 \sin^2 \theta)+\dfrac{1}{2} J \dot{\theta}^2$
$=\dfrac{1}{2}M \dot{x}^2+\dfrac{1}{2}m(\dot{x}^2+2 \dot{x} L \dot{\theta} \cos \theta+ L^2 \dot{\theta}^2 )+\dfrac{1}{2} J \dot{\theta}^2$ ・・・(3.7.40)
台車の重心が高さ$0 $の位置から変わらないとすると、台車の位置エネルギーは$0 $となる。剛体の角度$\theta $のときの剛体の位置エネルギーは次式で表されます。
$U=mgL \cos \theta $ ・・・(3.7.41)
台車と剛体の粘性抵抗は次式で表されます。
$D=\dfrac{1}{2}\mu_1 \dot{x}^2+\dfrac{1}{2}\mu_2 \dot{\theta}^2$ ・・・(3.7.42)
よって、ラグラジアンは
$L=T-U=\dfrac{1}{2}M \dot{x}^2+\dfrac{1}{2}m(\dot{x}^2+2 \dot{x} L \dot{\theta} \cos \theta+ L^2 \dot{\theta}^2 )+\dfrac{1}{2} J \dot{\theta}^2-mgL \cos \theta $ ・・・(3.7.43)
式(3.7.43)を見ても分かるように，台車と振子，つまり台車位置$x $と振子の角度$\theta $の２つのラグランジュの運動方程式を求める必要があります。
まず台車の位置$x $に関して運動方程式を求めます。式(3.7.2）の第1項を計算すると、
$\dfrac{d}{dt} \left \lbrace \dfrac{\partial }{\partial \dot{x}} \left( \dfrac{1}{2}M \dot{x}^2+\dfrac{1}{2}m (\dot{x}^2+2 \dot{x} L \dot{\theta} \cos \theta+ L^2 \dot{\theta}^2 )+\dfrac{1}{2} J \dot{\theta}^2-mgL \cos \theta \right) \right \rbrace$
$=\dfrac{d}{dt} \left( m L^2 \dot{\theta} +J \dot{\theta} \right) $
$=\dfrac{d}{dt}(M \dot{x}+m \dot{x}+L m \dot{\theta} \cos \theta)$
$=(M+m) \ddot{x}+L m \ddot{\theta} \cos \theta-L m \dot{\theta}^2 \sin \theta$ ・・・(3.7.44)
式(3.7.2）の第2項を計算すると、
$\dfrac{\partial }{\partial \dot{x}} \left( \dfrac{1}{2}M \dot{x}^2+\dfrac{1}{2}m(\dot{x}^2+2 \dot{x} L \dot{\theta} \cos \theta+ L^2 \dot{\theta}^2 )+\dfrac{1}{2} J \dot{\theta}^2-mgL \cos \theta \right)=0 $ ・・・(3.7.45)
式(3.7.2）の第3項を計算すると、
$\dfrac{\partial }{\partial \dot{x}} \left( \dfrac{1}{2}\mu_1 \dot{x}^2+\dfrac{1}{2}\mu_2 \dot{\theta}^2 \right)=\mu_1 \dot{x} $ ・・・(3.7.46)
式(3.7.44)、(3.7.45)、(3.7.46)より、台車位置$x $に関するラグランジュの方程式は次式となります。
$(M+m) \ddot{x}+L m \ddot{\theta} \cos \theta-L m \dot{\theta}^2 \sin \theta+\mu_1 \dot{x}=f $ ・・・(3.7.47)
次に振子の角度$\theta$に関して運動方程式を求めます。式(3.7.2）の第1項を計算すると、
$\dfrac{d}{dt} \left \lbrace \dfrac{\partial }{\partial \dot{\theta}} \left( \dfrac{1}{2}M \dot{x}^2+\dfrac{1}{2}m (\dot{x}^2+2 \dot{x} L \dot{\theta} \cos \theta+ L^2 \dot{\theta}^2 )+\dfrac{1}{2} J \dot{\theta}^2-mgL \cos \theta \right) \right \rbrace$
$=\dfrac{d}{dt} \left( L m \dot{x} \cos \theta+ L^2 m \dot{\theta} +J \dot{\theta} \right) $
$ L m \ddot{x} \cos \theta-L m \dot{x} \dot{\theta} \sin \theta+ (L^2 m+J) \ddot{\theta} $ ・・・(3.7.48)
式(3.7.2）の第2項を計算すると、
$\dfrac{\partial }{\partial {\theta}} \left( \dfrac{1}{2}M \dot{x}^2+\dfrac{1}{2}m (\dot{x}^2+2 \dot{x} L \dot{\theta} \cos \theta+ L^2 \dot{\theta}^2 )+\dfrac{1}{2} J \dot{\theta}^2-mgL \cos \theta \right)$
$=-m L\dot{x} \dot{\theta} \sin \theta+mgL \sin \theta$ ・・・(3.7.49)
式(3.7.2）の第3項を計算すると、
$\dfrac{\partial }{\partial \dot{\theta}} \left( \dfrac{1}{2}\mu_1 \dot{x}^2+\dfrac{1}{2}\mu_2 \dot{\theta}^2 \right)=\mu_2 \dot{\theta} $ ・・・(3.7.50)
剛体角度$\theta $に関するラグランジュの方程式は次式となる。
$ L m \ddot{x} \cos \theta- m L \dot{x} \dot{\theta} \sin \theta+ (L^2 m+J) \ddot{\theta} +m L\dot{x} \dot{\theta} \sin \theta-mgL \sin \theta+\mu_2 \dot{\theta} =0$
$ L m \ddot{x} \cos \theta+ (L^2 m+J) \ddot{\theta} -mgL \sin \theta+\mu_2 \dot{\theta} =0$
・・・(3.7.51)
一般的に、剛体の傾き角$ \theta $が小さいとき$\theta \rightarrow 0$のとき、$\cos \theta=1、\sin \theta=\theta $となります。
$\dot{\theta}$も微小変化であると考えると，$\dot{\theta}^2 \rightarrow 0$と考えて良い。よって，式(3.7.47)，(3.7.51)は次式のように書き換えられます。
$(M+m)\ddot{x}+Lm\ddot{\theta}+\mu_1 \dot{x}=f $ ・・・(3.7.52)
$Lm \ddot{x}+(L^2 m +J)\ddot{\theta}-mgL \theta + \mu_2 \dot{\theta}$ ・・・(3.7.53)
パソコンなどでシミュレータを作成する場合、このままでは扱わずに、行列を使って表現した方が扱いやすいことがあります。このように、行列ベクトル形式で表現したシステムの方程式を，状態方程式$\boldsymbol{\dot{z}=Az+Bf}$と呼びます。状態変数を次のようにおくと
$\ddot{x}=\dfrac{f-Lm\ddot{\theta}-\mu_1 \dot{x} }{m+M}$
$Lm \dfrac{f-Lm\ddot{\theta}-\mu_1 \dot{x} }{m+M}+(L^2 m +J)\ddot{\theta}-mgL \theta + \mu_2 \dot{\theta}=0$
$Lm (f-Lm\ddot{\theta}-\mu_1 \dot{x} )+(m+M)(L^2 m +J)\ddot{\theta}-(m+M) mgL \theta + \mu_2 \dot{\theta}=0$
$(m+M)(L^2 m +J)-m^2 L^2=C$とおくと
$\ddot{\theta}=\dfrac{Lm \mu_1}{C} \dot{x}+\dfrac{(m+M)mgL}{C}\theta-\dfrac{(m+M)\mu_2}{C} \dot{\theta}-\dfrac{mL}{C}f$ ・・・(3.7.54)
この式を(3.7.52)に代入すると
$(M+m)\ddot{x}+Lm \left \lbrace \dfrac{Lm \mu_1}{C} \dot{x}+\dfrac{(m+M)mgL}{C}\theta-\dfrac{(m+M)\mu_2}{C} \dot{\theta}-\dfrac{mL}{C}f \right \rbrace+\mu_1 \dot{x}=f $
$(M+m)\ddot{x}+\mu_1 \dot{x}+ \dfrac{m^2 L^2 \mu_1}{C} \dot{x}+\dfrac{(m+M)m^2 gL^2 }{C}\theta-\dfrac{(m+M)mL\mu_2}{C} \dot{\theta}-\dfrac{m^2 L^2 }{C}f -f=0 $
$(M+m)\ddot{x}+ \dfrac{(m+M)(m L^2+J) \mu_1}{C} \dot{x}+\dfrac{(m+M)m^2 gL^2 }{C}\theta-\dfrac{(m+M)mL\mu_2}{C} \dot{\theta}-\dfrac{(m+M)(m L^2+J) }{C}f=0 $
$\ddot{x}=- \dfrac{(m L^2+J) \mu_1}{C} \dot{x}-\dfrac{m^2 gL^2 }{C}\theta+\dfrac{mL\mu_2}{C} \dot{\theta}+\dfrac{(m L^2+J) }{C}f=0 $
・・・(3.7.55)
$\dot{z}=\begin{bmatrix} \dot{x} \\ \dot{\theta} \\ \ddot{x} \\ \ddot{\theta} \end{bmatrix} =\begin{bmatrix} 0 & 0 & 1& 0 \\ 0 & 0 & 0 & 1 \\ 0 &-\dfrac{m^2 gL^2 }{C} &- \dfrac{(m L^2+J) \mu_1}{C} & \dfrac{mL\mu_2}{C} \\ 0 & \dfrac{(m+M)mgL}{C}&\dfrac{Lm \mu_1}{C} & -\dfrac{(m+M)\mu_2}{C} \end{bmatrix} \begin{bmatrix} {x} \\ {\theta} \\ \dot{x} \\ \dot{\theta} \end{bmatrix} +\begin{bmatrix} 0 \\ 0 \\ \dfrac{m L^2+J }{C} \\ -\dfrac{m L }{C} \end{bmatrix} f$
・・・(3.7.56)
