概要

の方に運動方程式の導出はありました。
今回は、レールにのっけた台車に振り子を取付けて倒立させることを目標にやります。

奥の歯車のようなものはエンコーダーで角度をとり
手前の真鍮パイプが振り子に相当します。
振り子の中ほどにジャイロセンサーがついています。
左端にモーターがあり、台車をベルトで動かします。
本体の質量が445gでかなり重くなってしまったので、入力する力に本体の回転による力は入れないことにします。
また、レール粘性よりもレール摩擦が卓越しているので、粘性項は無視して式を立てます。

完成しました．

(2024/3/28)Qiitaの仕様変更で数式が壊れていたため修正しました

運動方程式

上のリンクでやっていることを再度やることになりますが、やっていきます。

まず、振り子自体の力・トルクのつり合い

\begin{eqnarray}
m(a_x,a_y)&=& m(0,-g)+(f_x,f_y)\\
J(d^2\theta/dt^2) + c(d\theta/dt) &=& \vec{R} \times \vec{f}\\
\vec{f} &=& (f_x,f_y) \\
\vec{R} &=& R(\sin \theta,- \cos \theta)\\
\end{eqnarray}

次に、台車の運動方程式、上の紙で入力の力Fを入れ忘れてますが、Fとします。
摩擦力$F_r$は$f_y$の影響をうけると思いますが、本体が$445g$で振り子が$56g$なので今回は大きさは一定と仮定して式を立てます。発生方向は移動方向の反対方向なので角速度$\omega$を見て判断させます。

Ma_{台} = -f_x-F_r+F\\

振り子の支店の速度は$v_{台}$と一致するので

\begin{eqnarray}
(v_{台},0) &=& (v_x,v_y)+(0,0,\frac{d\theta}{dt} ) \times  \vec{R} \\
\frac{d}{dt} (v_x,v_y)&=& (a_x,a_y) \\
\end{eqnarray}

よって、$a_x,a_y$が計算できます。

\begin{eqnarray}
v_x &=& v-R\omega \cos \theta\\
v_y &=& - \omega R \sin \theta\\
( \omega &=&d\theta/dtとしました。) \\
\therefore
a_x&=& \frac{dv}{dt} -R \frac{d\omega}{dt} \cos \theta +R \omega^2 \sin \theta\\
a_y &=& -R \frac{d \omega }{dt} \sin \theta-R \omega^2 \cos \theta
\end{eqnarray}

これを振り子の力の方程式に入れると、fx、fyが求まります。

\begin{eqnarray}
m(a_x,a_y) &=& m(0,-g)+(f_x,f_y)\\
\therefore f_x &=& m(dv/dt - R \frac{d\omega }{dt} \cos\theta +Rw^2S\theta)\\
fy &=& m(-Rdw/dtS\theta-Rw^2C\theta + g)\\
\end{eqnarray}

ここから、 $\vec{R} \times \vec{f}　$ が求まります。

 \vec{R} \times \vec{f}\ = mR(dv/dtC\theta-Rdw/dt+gS\theta)\\
\therefore J(d^2\theta/dt^2) + c(d\theta/dt) =mR(dv/dt*C\theta-Rdw/dt+gS\theta)\\
\therefore (J+mR^2)(d^2\theta/dt^2) + c(d\theta/dt) =mR(dv/dt*C\theta+gS\theta)\\

がまとまります。
そして、台車の力のつり合いの式

Ma_{台} = -1*fx-Fr+F\\
\therefore M(dv/dt) = -m(dv/dt - R*dw/dt*C\theta +Rw^2S\theta)-Fr+F\\

がまとまり、二式立ちました。

#電気回路方程式
入力Fとしましたが、今回はギアモータを使っているので実際の入力は電圧です。
モーターの電気・機械方程式を立てます。
ギア比が入ってくると結構頭がこんがらがってあまり自信がないですが、

仕事が一致するように考えました。この後の状態方程式表現では状態変数として$\theta_M , \theta$を使うことにします。
印加電圧と電流・逆起電圧の関係より

V=R_e*i+K*(d\theta_M)/dt

モーターの機械特性より、ギアボックスの慣性モーメントJM、粘性係数CM、摩擦トルク損失：τ損より

J_M(d^2\theta_M/dt^2)+c_M*(d\theta_M/dt)=K*i-\tau_損-FR_m n

$-FR_m n$は台車を動かすのに使ったトルクです。最初、Kiがあるのでこれを忘れて悩みました。

#式の整理：状態空間表現へ:近似
このままだとかなり式が混雑して難易度が高いので、次の近似を入れます

c_M を無視
fxを無視

J_M(d^2\theta_M/dt^2)=K*i-\tau_損-FR_m n\\
M(dv/dt) = -Fr+F\\
(J+mR^2)(d^2\theta/dt^2) + c(d\theta/dt) =mR(dv/dt*C\theta+gS\theta)\\

を整理し、を$\theta_M , \theta$の二階微分を分離します。

(d^2\theta_M/dt^2)=1/(J_M+M(R_Mn)^2)*(K*i-\tau_損-FR_m n)\\
(J+mR^2)(d^2\theta/dt^2) =1/(J+mR^2) [-c(d\theta/dt) +mRgS\theta + mRR_MnC\theta/(J_M+M(R_Mn)^2)(K*i-\tau_損-FR_m n)]\\

これを、θ＝０近傍で線形化します。mgRの項が変わるだけですが・・・

(d^2\theta_M/dt^2)=1/(J_M+M(R_Mn)^2)*(K*i-\tau_損-FR_m n)\\
(J+mR^2)(d^2\theta/dt^2) =1/(J+mR^2) [-c(d\theta/dt) +mRg\theta + mRR_MnC\theta/(J_M+M(R_Mn)^2)(K*i-\tau_損-FR_m n)]\\

状態空間表現は

d/dt
\begin{pmatrix}
d\theta/dt \\
\theta\\
d\theta_M/dt\\
\theta_M
\end{pmatrix}
=
\begin{pmatrix}
-c/(J+mR^2) & mgR/(J+mR^2) & 0&0\\
1 & 0& 0 & 0\\
0 & 0& 0 & 0\\
0 & 0& 1 & 0\\
\end{pmatrix}

\begin{pmatrix}
d\theta/dt \\
\theta\\
d\theta_M/dt\\
\theta_M
\end{pmatrix}
+
\begin{pmatrix}
mRR_Mn/[(J_M+M(R_Mn)^2)(J+mR^2)] \\
0\\
1/(J_M+M(R_Mn)^2)\\
0
\end{pmatrix}
*(K*i-\tau_損-FR_m n)

入力をVにするため$i =1/R_e(V-K*d\theta_M/dt)$を代入し整理すると

d/dt
\begin{pmatrix}
d\theta/dt \\
\theta\\
d\theta_M/dt\\
\theta_M
\end{pmatrix}
=
\begin{pmatrix}
-c/(J+mR^2) & mgR/(J+mR^2) & -mRR_Mn/[(J_M+M(R_Mn)^2)(J+mR^2)]*(K/R_e)^2&0\\
1 & 0& 0 & 0\\
0 & 0& -(K/R_e)^2/(J_M+M(R_Mn)^2) & 0\\
0 & 0& 1 & 0\\
\end{pmatrix}

\begin{pmatrix}
d\theta/dt \\
\theta\\
d\theta_M/dt\\
\theta_M
\end{pmatrix}
+
\begin{pmatrix}
mRR_Mn/[(J_M+M(R_Mn)^2)(J+mR^2)] \\
0\\
1/(J_M+M(R_Mn)^2)\\
0
\end{pmatrix}
*(K/R_e)[V-(R_e/K)(\tau_損+FR_m n)]

$V-(R_e/K)(\tau_損+FR_m n)$ここは、θMの正負に合わせて、$(R_e/K)(\tau_損+FR_m n)$を足せばよい。
以上で状態空間表現の完成。

#式の整理：状態空間表現へ:近似ほぼなし
次に、近似をせずにやる方法を考えます。ただ、とてもめんどくさく不安になるのでまだできていません。最後はMaximaで整理させることにしました。
台車の速度が$\theta_M$であらわせるので、これを台車の力のつり合いに代入すると$\theta_M , \theta$だけで機械系の式が一本立ちます。

\begin{eqnarray}
M(dv/dt) = -m(dv/dt - R*dw/dt*C\theta +Rw^2S\theta)-Fr+F\\
(上の式は台車の力のつり合い、Fにモータの式を代入)\\
M(dv/dt) = -m(dv/dt - R*dw/dt*C\theta +Rw^2S\theta)-Fr+(1/R_mn)[K*i-\tau_損-(J_M(d^2\theta_M/dt^2)+c_M*(d\theta_M/dt))]\\
v=(d\theta_M/dt)R_mnより\\
(M+m)(d^2\theta_M/dt^2)R_mn=-m(- R*dw/dt*C\theta +Rw^2S\theta)-Fr+(1/R_mn)[K*i-\tau_損-(J_M(d^2\theta_M/dt^2)+c_M*(d\theta_M/dt))]\\
\end{eqnarray}

電流入力になりますが、電流指令の方程式でないので$i=(V-K\theta_M)/R$で電圧指令に直して使用します。
そして、振り子のトルクのつり合いの式

(J+mR^2)(d^2\theta/dt^2) + c(d\theta/dt) =mR(dv/dt*C\theta+gS\theta)\\

\therefore (J+mR^2)(d^2\theta/dt^2) + c(d\theta/dt) =mR((d^2\theta_M/dt^2)*C\theta+gS\theta)\\

で2本方程式が立ちます、

次に2本の式を$\theta_M , \theta$の二階微分を分離します。

(d^2\theta/dt^2) = -c/(J+mR^2)(d\theta/dt) +mR/(J+mR^2)(dv/dt*C\theta+gS\theta)\\

をモーター・台車の力のつり合いの式に代入し$d^2\theta_M/dt^2$のみの式にします。

(M+m)(d^2\theta_M/dt^2)R_mn=-m(- R*[-c/(J+mR^2)(d\theta/dt) +mR/(J+mR^2)(dv/dt*C\theta+gS\theta)]*C\theta +Rw^2S\theta)-Fr+(1/R_mn)[K*i-\tau_損-(J_M(d^2\theta_M/dt^2)+c_M*(d\theta_M/dt))]\\

整理すると

\begin{eqnarray}
[(M+m)R_mn-mR^2/(J+mR^2)*C^2\theta+J_M/R_mn](d^2\theta_M/dt^2) =\\
mRC\theta(-c/(J+mR^2)*d\theta/dt+mgR/(J+mR^2)S\theta)-mRw^2S\theta\\
-Fr+(1/R_mn)[K*i-\tau_損]\\
-c_M/(R_mn)*(d\theta_M/dt)\\
\end{eqnarray}

非線形方程式になりますね。w^2項は無視することにします。$d^2\theta/dt^2$は

\begin{eqnarray}
(d^2\theta/dt^2) = -c/(J+mR^2)(d\theta/dt) +mR/(J+mR^2)(R_Mn*(d^2\theta_M/dt^2)*C\theta-Rdw/dt+gS\theta)\\
\therefore
(d^2\theta/dt^2) = 
-c/(J+mR^2)(d\theta/dt) +mR/(J+mR^2)(-Rdw/dt+gS\theta)
+mR/(J+mR^2)(R_Mn*(1/[(M+m)R_mn-mR^2/(J+mR^2)*C^2\theta+J_M/R_mn])*[mRC\theta(-c/(J+mR^2)*d\theta/dt+mgR/(J+mR^2)S\theta)-mRw^2S\theta
-Fr+(1/R_mn)[K*i-\tau_損]
-c_M/(R_mn)*(d\theta_M/dt)]*C\theta)\\
\end{eqnarray}

となります。かなり汚い式ですね（笑）
これをMaximaで整理するとして、この後の数値を代入して使いました。

パラメータ同定

まず振り子、単純に揺らすと

GZ：角速度が一致するようにパラメータを合わせると

c/J=0.694483\\
mgR/(J+mR^2) = 76.9361

別に振り子自体の重さを計るとm = 0.056kgとなりました。

次に、モーター同定、以前書いた記事をもとに推定します。

V=R*i+Kd\theta_M/dt\\
R=2.002Ω\\
K = 0.00194\\

機械特性は、モータ間電流＝０となるときの角速度の減衰より

指数近似することで

c_M/J_M = 1.517\\
c_M/\tau_{損} = 0.01234

一定電流になるまで待つことでさらにパラメータを出すと

c_M/K = 9.712*10^-5\\

\tau_{損}/K = 0.1011

最後に、台車から振り子を外し、ベルトをつけてモーターを回し、その応答よりレールと台車の摩擦力Frを推定します。

後半は一定速度で移動していました。

(d^2\theta_M/dt^2)=1/(J_M+M(R_Mn)^2)*(K*i-\tau_損-FR_m n)\\

より、台車が一定速になるときの電流値より

\tau_損+FR_m n = K*i=0.00194*0.3302 = 6.4488*10^{-4}

となりました。
台車の重さMは実際に計り0.445kgとなりました。

近似した状態方程式に代入していくと

d/dt
\begin{pmatrix}
d\theta/dt \\
\theta\\
d\theta_M/dt\\
\theta_M
\end{pmatrix}
=
\begin{pmatrix}
-0.694 & 76.93 & -0.009371&0\\
1 & 0& 0 & 0\\
0 & 0& -5.593 & 0\\
0 & 0& 1 & 0\\
\end{pmatrix}

\begin{pmatrix}
d\theta/dt \\
\theta\\
d\theta_M/dt\\
\theta_M
\end{pmatrix}
+
\begin{pmatrix}
9.696 \\
0\\
5788\\
0
\end{pmatrix}
*[V-0.66114]

制御系設計

まず、重力項を正負逆にし、振り子状態での減衰の具合を見ます。
現在のモーター電源はeneloop8本ですが、実質印加電圧は3Ｖあるかないかです。K,Reより推定した電圧で見たところですが。
V推定=Ki＋Rw　と入力してほしい電圧でPI制御を入れて電圧自体のコントローラも入れています。
振り子状態での減衰の具合ですが、まったく減衰できず逆に定常振動が発生してしまいました。系としては発散ということになります。

状態空間表現の近似がよくなかったか、うまくいかなかった。逆に振動を発生させるレベル。
複雑なことはすなわち罪
まず、台車の速度制御から始めるべき
速度制御の性能が十分なら台車速度を入力としてコントローラを構築すればいいはず

ということで、先に台車の制御を考えます。うまくいったら、更新します。