二輪型倒立振子のパラメータと数値シミュレーション結果

みなさん，こんにちは．
おかしょです．

このブログでは二輪型倒立振子の開発を行っています．

ロボットを作って自律させるためには制御器を設計する必要があります．

このとき，いきなりロボットに制御器を適用するのは怖いので数値シミュレーションを先に行います．

この記事では，その数値シミュレーションの準備を行います．

この記事を読むと以下のようなことがわかる・できるようになります．

二輪型倒立振子の数値シミュレーションのやり方
二輪型倒立振子の数値パラメータ

この記事を読む前に
状態変数の格納庫作成
運動方程式を解く
パラメータの設定
微分方程式の積分
数値シミュレーション結果
まとめ
続けて読む
1. 共有:
2. 関連

この記事を読む前に

この記事では二輪型倒立振子の数値シミュレーションを行います．

その際に二輪型倒立振子の運動方程式を利用します．

運動方程式の導出方法はこちらで解説していて，運動方程式を利用して数値シミュレーションを行う方法は以下の記事で解説しているので，そちらを先に読んでおくことをおすすめします．

状態変数の格納庫作成

二輪型倒立振子の運動方程式を見ると，状態変数は車輪の角度$\phi$とその角速度$\dot{\phi}=P$，振子の角度$\theta$，その角速度$\dot{\theta}=Q$，入力$\tau$です．

まずはこれらのシミュレーション結果を格納する格納庫を作成します．

phi = zeros(time/dt, 1);
theta = zeros(time/dt, 1);
P = zeros(time/dt, 1);
Q = zeros(time/dt, 1);
tau = zeros(time/dt, 1);

ここで，zeros(a, b)は要素が0のa行b列の行列を作成するコマンドです．
timeはシミュレーション時間，dtはシミュレーションの刻み幅を表しています．

例えば

time = 10;
dt = 0.0001;

とした場合は，格納庫は100,000行1列で生成されます．

運動方程式を解く

以前求めた二輪型倒立振子の運動方程式は以下のような形をしていました．（導出はこちらで解説しています）

$$ \{(m_{t}+m_{p}) r^2+m_{p}rL\cos \theta +J_{t}\}\ddot{\phi}+\{(m_{t}+m_{p})r^{2}+2m_{p}rL\cos \theta+m_{p}L^{2}+J_{t}+J_{p} \}\ddot{\theta}-m_{p}rL\sin \theta \cdot \dot{\theta}^{2}-m_{p}gL\sin \theta +\mu_{\theta} \dot{\theta} = 0$$
$$ \{(m_{t}+m_{p}) r^2 +J_{t}\}\ddot{\phi}-m_{p}rL\sin \theta \cdot \dot{\theta}^{2} +\{(m_{t}+m_{p})r^{2}+m_{p}rL\cos \theta+J_{t}\}\ddot{\theta}+\mu_{\phi} \dot{\phi }=\tau$$

この式を連立して$\ddot{\phi}$と$\ddot{\theta}$について解きます．
このままでは計算しずらいので，以下のように各状態変数の係数を簡単化します．

\begin{eqnarray}
\left\{
\begin{array}{l}
A_{1}\ddot{\phi}+A_{2}\ddot{\theta}= A_{3}\\
B_{1}\ddot{\phi}+B_{2}\ddot{\theta}=B_{3}+\tau
\end{array}
\right.
\end{eqnarray}

\begin{eqnarray}
A_{1} &=& (m_{t}+m_{p}) r^2+m_{p}rL\cos \theta +J_{t} \\
A_{2} &=& (m_{t}+m_{p})r^{2}+2m_{p}rL\cos \theta+m_{p}L^{2}+J_{t}+J_{p}\\
A_{3} &=& m_{p}rL\sin \theta \cdot \dot{\theta}^{2}+m_{p}gL\sin \theta -\mu_{\theta} \dot{\theta}\\
B_{1} &=& (m_{t}+m_{p}) r^2 +J_{t}\\
B_{2} &=& (m_{t}+m_{p})r^{2}+m_{p}rL\cos \theta+J_{t}\\
B_{3} &=& m_{p}rL\sin \theta \cdot \dot{\theta}^{2}-\mu_{\phi} \dot{\phi }\\
\end{eqnarray}

これを解くと

\begin{eqnarray}
\ddot{\phi} &=& \frac{A_{3}B_{2}-A_{2}B_{3}}{A_{1}B_{2}-A_{2}B_{1}}-\frac{A_{2}}{A_{1}B_{2}-B_{1}A_{2}}\tau\\
\ddot{\theta} &=& \frac{A_{3}B_{1}-A_{1}B_{3}}{A_{2}B_{1}-A_{1}B_{2}}-\frac{A_{1}}{A_{2}B_{1}-A_{1}B_{2}}\tau\\
\end{eqnarray}

パラメータの設定

次にパラメータの設定を行います．

このパラメータとは$A_{1~3}$や$B_{1~3}$の値を求めるために必要な定数のことです．
車輪の半径$r$や振子の長さ$L$などのことです．

これらのパラメータは実際に測定しました．慣性モーメントは計算で求めるのは難しいので，3D CADを用いて推算しました．

数値シミュレーションで用いたパラメータを以下に示します．

\begin{array}{c|c|c}
\hline
パラメータ & 単位 & 数値\\
\hline
タイヤ質量 & kg & 0.052\\
\hline
倒立振子質量 & kg & 0.396\\
\hline
タイヤ慣性モーメント& kg\cdot m^{2} & 9947.933\times 10^{-9}\\
\hline
倒立振子慣性モーメント& kg\cdot m^{2} & 4.413\times 10^{-3}\\
\hline
振子長さ& m& 0.084489\\
\hline
タイヤ半径& m & 0.0275\\
\hline
タイヤ摩擦係数& -& 0.05\\
\hline
振子粘性係数& -& 0.1\\
\hline
重力加速度& m/s^{2}& 9.80665\\
\hline
\end{array}

微分方程式の積分

先程解いた運動方程式を積分します．
今回はルンゲクッタで積分をしました．

数値シミュレーション結果

以上のような設定で数値シミュレーションを行います．
ここでは入力を一切加えない自由応答を示します．

初期値は以下のように設定しました．

\begin{array}{c|c|c}
\hline
パラメータ & 単位 & 初期値\\
\hline
\phi & rad & 0\\
\hline
\theta & rad & 0.01\\
\hline
P & rad/s & 0\\
\hline
Q & rad/s & 0\\
\hline
\end{array}

振子だけ少し傾いている状態で数値シミュレーションを開始しました．
振子を傾けた状態で手を放せば，制御をしていないので倒立振子は倒れてしまうはずです．今回数値シミュレーションでは，地面にぶつかって止まるようにはしていないので振子の角度は最初の状態から180°回転して収束すると思われます．

数値シミュレーションの結果は以下のようになりました．