読者です 読者をやめる 読者になる 読者になる

統失ニートの再受験日記

大体は海水魚について書きます。統失ですが、医学部再受験しようと思います。

倒立振り子3(運動方程式)

Arduino

毎日更新を目指していたのですが、早速昨日はお休みしてしまいました。頑張らなきゃ。

前回の記事の(2)極配置で必要なので、倒立振り子の運動方程式を追っていきます。図のようなモデルを考えます。

f:id:cmsan:20150327205611j:plain

振り子の運動方程式は、回転方行、水平方向、垂直方向それぞれ、

   {J \ddot θ = Vl sin θ - Hlcosθ - C \dotθ }

   {m \frac{ d^2 }{ dt^2}  (x + lsinθ) = H}

   {m \frac{ d^2 }{ dt^2}  (lcosθ) = V-mg}

   {H=u-B \dot x}

となります。ここで、{V、H}は振り子棒部分の受ける力、{B、C}は水平、回転方向の摩擦係数です。{J}は慣性モーメント{J={ml^2}/3}です。

まず、普通に解こうとしたのですが、私には式が複雑になりすぎ面倒だったので、近似しまくります。{θ}が十分小さいとして、{sinθ \simeq θ、cosθ \simeq 1 }と置きます。さらに、摩擦もないことにして、{B=0、C =0}とします。進化系運動方程式は、

   {J \ddot θ = Vl θ - Hl }

   {m  (\ddot x + l\ddotθ) = H}

   {V=mg}

   {H=u}

大丈夫かなこれ?

とりあえず解きます。これらより{H、V}を消去して、{\ddot x、\ddotθ}について解きます。

   { \ddot x = \frac{1}{Jm} ((J+ml^2)u -m^2 gl^2 θ)}

   { \ddotθ = \frac{1}{Jm} (m^2 glθ-mlu)}

これを下のように置きます。{a,b,c,d}の値は、それぞれ係数を比較してください。

   { \ddot θ = aθ+cu  }

   { \ddot x = bθ+du  } 

ここで、{X} ={ \bigl[ θ    \dot θ    x    \dot x \bigr]  }^Tと置きます。すると行列形式下のような式が出来上がります。ただ、{x、θ}の関係式を行列で書いただけです。

   {   \begin{equation}   \begin{bmatrix} \dot{θ} \\ \ddot{θ} \\ \dot{x} \\ \ddot{x} \end{bmatrix} \end{equation}     }{   \begin{equation}   \begin{bmatrix} 0   1   0   0 \\ a    0    0    0 \\ 0    0    0    1 \\ b    0    0    0 \end{bmatrix}   \begin{bmatrix} {θ} \\ \dot{θ} \\ {x} \\ \dot{x} \end{bmatrix} \end{equation}  }     + {   \begin{equation}   \begin{bmatrix} 0 \\ c \\ 0 \\ d \end{bmatrix} \end{equation}     } u

これを{\dot X = AX + Bu}と置くと、振り子の状態方程式の出来上がりです。{A、B}は係数の行列ですね。さらに力{u}

   {u = k_1 θ + k_2 \dot θ + k_3 x +  k_4 \dot x}

と定義します。行列で書くと{u} ={ \bigl[ k_1   k_2   k_3   k_4 \bigr]  } X = KXです。すると、状態方程式

   {\dot X = (A+BK)X}

と書き直せます。

続く...