【倒立振子part4】状態空間モデルを作る

下記のpart2,3でモデルの運動方程式が作成が完了した。
今回は導出した運動方程式を状態方程式と出力方程式に変形してシステムの状態空間表現を行う。

現代制御について
運動方程式
運動方程式から状態方程式へ
次回

現代制御について

式を変更する前になぜ現代制御(状態方程式)で制御するのかを考える。
詳しく書いてある記事があったので引用させていただく。

controlabo.com

記事にもあるが現代制御はモデルの運動方程式に強く時間領域で制御できるため今回使用するのである。
簡単な制御するとき、例えばモータの角速度は測定せずに現在の振子の角度と0度(直立状態)の偏差がなくなるようにPID制御する際には古典制御で事足りるが
今回のようにモータの角速度なども考慮するため場合は現代制御を使用する。
またできそうな事も多そうなので、今後いろいろ試していくため現代制御を使用する。

運動方程式

part2とpart3によって導出できた運動方程式をまとめると

$\begin{eqnarray} \left\{\begin{array}{ll} J_r\ddot{\theta_r}+(\frac{1}{4}m_r{lr}^2+\frac{1}{4}mp{lp}^2+J_r+J_p)\ddot{\theta_p}+\mu_p\dot{\theta_p}-(m_rl_r+m_pl_p)g\theta_p=0 &{(1)} \\ J_r\ddot{\theta_r}+\mu_r\dot\theta_r+J_r\ddot{\theta_p}=\tau_\omega&{(2)} \\ \tau_\omega=-J_m\ddot\theta_r-\bar{\mu}_m\dot\theta_r+\frac{k_t}{R}v_a　,　\bar{\mu}_m=\mu_m+\frac{k_tk_e}{R}&{(3)} \end{array} \right. \end{eqnarray}$

(式番号はわかりやすいように再定義しなおした)

ここで(2)と(3)をまとめて項ごとに綺麗にすると、

$\begin{eqnarray} \left\{\begin{array}{l} J_r\ddot{\theta_r}+(\frac{1}{4}m_r{lr}^2+\frac{1}{4}mp{lp}^2+J_r+J_p)\ddot{\theta_p}+\mu_p\dot{\theta_p}-(m_rl_r+m_pl_p)g\theta_p=0&{(4)} \\ (J_r+J_m)\ddot{\theta_r}+(\mu_r+\mu_m+\frac{k_tk_e}{R})\dot\theta_r+J_r\ddot{\theta_p}=\frac{k_t}{R}v_a&{(5)} \end{array} \right. \end{eqnarray}$

運動方程式から状態方程式へ

前提として状態方程式の基本形は

$\begin{eqnarray} \left\{\begin{array}{l} \dot{x}=Ax+Bu\\ y=Cx \end{array} \right. \end{eqnarray}\tag{6}$

で表され、 $x$ は状態ベクトル、 $u$ は入力ベクトル、 $y$ は出力ベクトルである。

式(4)(5)を状態方程式の形式へするためにベクトル $\begin{pmatrix} \ddot\theta_p & \ddot\theta_r \end{pmatrix}$ について整理する。

$\displaystyle{ \begin{pmatrix}\frac{1}{4}m_r{lr}^2+\frac{1}{4}mp{lp}^2+J_r+J_p & J_r \\J_r & J_r+J_m \end{pmatrix} \left(\begin{array}{c}\ddot\theta_p\\ \ddot\theta_r\end{array}\right)= \left(\begin{array}{c}-\mu_p\dot{\theta_p}+(m_rl_r+m_pl_p)g\theta_p\\ -(\mu_r+\mu_m+\frac{k_tk_e}{R})\dot\theta_r+\frac{k_t}{R}v_a\end{array}\right)}$

上式の定数部分を計算しやすいように下記のように定義する。

$\begin{eqnarray} \left\{\begin{array}{l} a_{11}=\frac{1}{4}m_r{lr}^2+\frac{1}{4}mp{lp}^2+J_r+J_p\\ a_{12}=J_r\\ a_{21}=J_r\\ a_{12}=J_r+J_m\\ \end{array} \right. \end{eqnarray}$

この定義を適用すると元の状態方程式は下記のようになる。

$\begin{pmatrix}a_{11} & a_{12} \\a_{21} & a_{22} \end{pmatrix} \left(\begin{array}{c}\ddot\theta_p\\ \ddot\theta_r\end{array}\right)= \left(\begin{array}{c}-\mu_p\dot{\theta_p}+(m_rl_r+m_pl_p)g\theta_p\\ -(\mu_r+\mu_m+\frac{k_tk_e}{R})\dot\theta_r+\frac{k_t}{R}v_a\end{array}\right)$

この式をベクトル $\begin{pmatrix} \ddot\theta_p & \ddot\theta_r \end{pmatrix}$ について解く。
ただし行列式 $\Delta=a_{11}a_{22}-a_{12}a_{21}$ とする。

$\displaystyle{ \begin{eqnarray} \left(\begin{array}{c} \ddot\theta_p\\ \ddot\theta_r \end{array} \right) &=& \frac{1}{\Delta} \begin{pmatrix} a_{22} & -a_{12} \\-a_{21} & a_{11} \end{pmatrix} \left(\begin{array}{rr}-\mu_p\dot{\theta_p}+(m_rl_r+m_pl_p)g\theta_p\\-(\mu_r+\mu_m+\frac{k_tk_e}{R})\dot\theta_r+\frac{k_t}{R}v_a\end{array}\right)\\ &=& \frac{1}{\Delta} \left(\begin{array}{c}-a_{22}\mu_p\dot{\theta_p}+a_{22}(m_rl_r+m_pl_p)g\theta_p+a_{12}(\mu_r+\mu_m+\frac{k_tk_e}{R})\dot\theta_r-a_{12}\frac{k_t}{R}v_a\\ -a_{11}(\mu_r+\mu_m+\frac{k_tk_e}{R})\dot\theta_r+a_{11}\frac{k_t}{R}v_a+a_{21}\mu_p\dot{\theta_p}-a_{21}(m_rl_r+m_pl_p)g\theta_p \end{array}\right)\\ &=& \frac{1}{\Delta} \left(\begin{array}{c}a_{22}(m_rl_r+m_pl_p)g & -a_{22}\mu_p & a_{12}(\mu_r+\mu_m+\frac{k_tk_e}{R})\\ -a_{21}(m_rl_r+m_pl_p)g & a_{21}\mu_p & -a_{11}(\mu_r+\mu_m+\frac{k_tk_e}{R})\end{array}\right) \left(\begin{array}{rrr}\theta_p\\ \dot\theta_p\\\dot\theta_r\end{array}\right)+\frac{1}{\Delta}\left(\begin{array}{c}-a_{12}\frac{k_t}{R}v_a\\ a_{11}\frac{k_t}{R}v_a\end{array}\right) \end{eqnarray} }$

ここで倒立振子の状態ベクトルをあらゆる状態を表現できるために次のように決める。

$x=\left(\begin{array}{c}\theta_p\\ \dot\theta_p\\\theta_r\\\dot\theta_r\end{array}\right)$

上記を用いて状態ベクトル $x$ に従う行列微分方程式を求めると状態方程式になる。

$\left(\begin{array}{c}\dot\theta_p\\ \ddot\theta_p\\\dot\theta_r\\\ddot\theta_r\end{array}\right)= \begin{pmatrix}0 & 1 & 0 & 0\\\frac{a_{22}(m_rl_r+m_pl_p)g}{\Delta} & \frac{-a_{22}\mu_p}{\Delta} & 0 & \frac{a_{12}(\mu_r+\mu_m+\frac{k_tk_e}{R})}{\Delta}\\0 & 0 &0 & 1\\\frac{-a_{21}(m_rl_r+m_pl_p)g}{\Delta} & \frac{a_{21}\mu_p}{\Delta} & 0 &\frac{-a_{11}(\mu_r+\mu_m+\frac{k_tk_e}{R})}{\Delta}\end{pmatrix} \left(\begin{array}{c}\theta_p\\ \dot\theta_p\\\theta_r\\\dot\theta_r\end{array}\right)+\left(\begin{array}{c}0\\\frac{-a_{12}\frac{k_t}{R}}{\Delta}\\0\\\frac{a_{11}\frac{k_t}{R}}{\Delta}\end{array}\right)v_a$

状態ベクトルの項目は角度センサとエンコーダで測定できるため倒立振子の出力方程式は

$y=\left(\begin{array}{c}\theta_p\\ \dot\theta_p\\\theta_r\\\dot\theta_r\end{array}\right)=\left(\begin{array}{c}1&0&0&0\\0&1&0&0\\0&0&1&0\\0&0&0&1\\\end{array}\right)x$

以上より式(6)の行列項目は

$A=\begin{pmatrix}0 & 1 & 0 & 0\\\frac{a_{22}(m_rl_r+m_pl_p)g}{\Delta} & \frac{-a_{22}\mu_p}{\Delta} & 0 & \frac{a_{12}(\mu_r+\mu_m+\frac{k_tk_e}{R})}{\Delta}\\0 & 0 &0 & 1\\\frac{-a_{21}(m_rl_r+m_pl_p)g}{\Delta} & \frac{a_{21}\mu_p}{\Delta} & 0 &\frac{-a_{11}(\mu_r+\mu_m+\frac{k_tk_e}{R})}{\Delta}\end{pmatrix}　B=\left(\begin{array}{c}0\\\frac{-a_{12}\frac{k_t}{R}}{\Delta}\\0\\\frac{a_{11}\frac{k_t}{R}}{\Delta}\end{array}\right)　C=I_{4\times4}$

以上でシステムの状態空間表現ができた。

今回は強引に計算で導出したけど運動方程式から状態空間モデルに状態ベクトル入れたら勝手に計算してくれるソフト欲しい。
こんだけ現代制御理論が進んでるならありそうだけど見つけられなかった。。。
もしMatlabでできるなら、値段も個人使用なら15000円くらいなので今後のシュミレーションとかのことも考えて購入も検討に入れたい。