【倒立振子part29】倒立振子のシステム同定(上手くいかなかった)

part27にてシステム同定用のモデルを作成してきた。
前回でモータのモデルを1次遅れ系と仮定したのでモデルを完成させる。
今回はうまくいかなかった！次に期待！

モデル作成

前回求めたモデルは下記である。
\begin{cases} J_r\ddot{\theta_r}+(m_r{l_r}^2+mp{l_p}^2+J_r+J_p)\ddot{\theta_p}+\mu_p\dot{\theta_p}+(m_rl_r+m_pl_p)g\theta_p=0\\ J_r\ddot{\theta_r}+\mu_r\dot\theta_r+J_r\ddot{\theta_p}=\tau_\omega \end{cases}
また外力のトルクはモータの関係式より
\displaystyle{ \begin{cases} \tau_\omega=\tau_L=\tau_m-J_m\dot\omega_m-D\omega\\ \tau_m=\frac{k_t(V-k_e\omega_m)}{R} \end{cases} }
となる。これらの式をまとめると
\begin{cases} J_r\ddot{\theta_r}+(m_r{l_r}^2+mp{l_p}^2+J_r+J_p)\ddot{\theta_p}+\mu_p\dot{\theta_p}+(m_rl_r+m_pl_p)g\theta_p=0\\ (J_m+J_r)\ddot{\theta_r}+(\frac{k_e}{R}+D)\dot\theta_r+J_r\ddot{\theta_p}=\frac{k_t}{R}V \end{cases}

次に状態方程式の形式へするためにベクトル\begin{pmatrix} \ddot\theta_p & \ddot\theta_r \end{pmatrix}について整理する。
\displaystyle{ \begin{pmatrix}m_r{l_r}^2+m_p{l_p}^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\\ -(D+\frac{k_tk_e}{R})\dot\theta_r+\frac{k_t}{R}V\end{array}\right)}

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

\begin{eqnarray} \left\{\begin{array}{l} a_{11}=m_r{lr}^2+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}

また倒立振子の状態ベクトルをx=\left(\theta_p, \dot\theta_p,\theta_r,\dot\theta_r\right)'とすると状態空間モデルは
\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}(D+\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}(D+\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

以上でシステム同定用の振子のモデルができた。

同定入力の設計

モータ同様にプリ同定試験を実施したが最適な立ち上がり時間が分からなかったので試行錯誤で決めていく。
入力データのM系列の条件

サンプリング周期 シフトレジスタ係数 クロック周期 シフトレジスタ個数 データ数
T(s) p T_{m}(s) n N=2^n-1
0.04 1 0.04 7 127

システム同定試験(上手くいってない)

何をミスっていたのか？

原因は測定用のプログラムをさぼっていたこと！
システム同定のサンプリングタイムが50msだから、for文のdelayを50msにして測定すればいいやんけと思って測定していたが、これではデータが均一時間ののサンプルにならない＋カルマンフィルタのサンプリングタイムが50msになってしまい正確な計算ができていなかった。(50msごとに処理しているので当然だが気が付かなかった。。。)
そこで次からは測定にはタイマー割り込みをつかうことにする。

今後

仕事が大変だったり初めてのmatlabで上手くいかないことが続いたのでブログの更新ができていなかったが、よく考えたら自分の備忘録なので上手くいかないことも書いていいのだ。
上手くいったことだけじゃなくて、試したこととか失敗とかも書いていこうと思う。