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

制御工学

みなさん,こんにちは.
おかしょです.

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

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

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

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

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

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

 

この記事を読む前に

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

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

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

 

状態変数の格納庫作成

二輪型倒立振子の運動方程式を見ると,状態変数は車輪の角度\(\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°回転して収束すると思われます.

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

数値シミュレーションでも振子の角度が180°回転した\(\pi=-3.14\)で収束していることがわかります.
また,タイヤの角度も少し回転していますが,最終的には0に収束しています.

 

まとめ

この記事では以前書いた運動方程式から数値シミュレーションを行う方法の具体例として,数値シミュレーションの結果を示しました.
数値シミュレーションで用いた積分法のルンゲクッタについては解説していませんが,簡単な長方形則でも同様の結果が得られると思うので,皆さんもやってみてください.

 

続けて読む

以下の記事では倒立振子を数値シミュレーション上でP制御をしてみました.

興味のある方は読んでみてください.

Twitterでは記事の更新情報や活動の進捗などをつぶやいているので気が向いたらフォローしてください.

それでは最後まで読んでいただきありがとうございました.

コメント

タイトルとURLをコピーしました