２リンクアームの動力学シミュレーション(モデリング編)

ロボティクス 復習

学生卒業から1年半、だんだん体も学生のときみたいに動かなくなり、そして少しずつおっさん体型になったかも(・△・;)

そして、卒業してから、まったく研究内容に見向きもしなかったので、結構忘れてしまってます。
これから、少しずつ思い出して、趣味につながり、仕事につながりなんてなればいいかな。

まず、「２リンクアームの動力学シミュレーション」を復習してみようかしら。動力学シミュレーションはロボティクスでは基礎の基礎だからね。

説明するまでもないと思うけど、シミュレーションはロボットを実際に動かさなくても作成したコントローラがうまく動くか確かめられるので、とても重要。また動力学シミュレーションは、実際のロボットと同等の環境でテストできるので最終的な確認には特に役立つ。

このシミュレーションを実現するのにいくつかステップを踏んでいこうと思います。
①２リンクロボットアームの運動方程式を立てる。
②シミュレーションを行うソースを作成(C言語で記述・注目はどのように微分方程式を解いていくかかな）
③最終的にはアニメーションでシミュレーションした状況を確認
(OpenGLを使ってみようかな）

今回は①の運動方程式を立ててみようと思います。
基本、ロボットのモデリングについての話ですが、モデリングする方法はニュートン・オイラー方程式とラグランジュ方程式がある。

ニュートンオイラー方程式は、直線運動の力のつりあい（ニュートン方程式）・回転運動のモーメントのつりあい（オイラー方程式）という見解から動力学を考えたもの。
ラグランジュ方程式は、エネルギーに基づいて考えたもの。
という話みたい。

ってか、学生のとき、ラグランジュ方程式を利用して、運動方程式を立てたんだけど、２リンクならまだしも、３リンクマニピュレータになると１日かかる(・△・;)。

はっきり言って、変数計算できるソフトウェアを使って運動方程式を導き出すのをおススメします。MathmaticaやMapleを利用できる環境なら、簡単に運動方程式を導いてくれます。

今回も大変なことはしたくないので、ネット上に転がっていた運動方程式を紹介。

図に必要なパラメータを書いたんだけど、足りない部分もあるので、パラメータ一覧を下に書き出しました。
質力　　　 m1, m2
リンク長　　 L1, L2
リンク中心長 Lc1, Lc2　
イナーシャ 　 J1, J2
重力加速度 　 g
関節トルク τ1,τ2

運動方程式は、以下のようになります。
(モデルはリンク中心に質点があることをことを前提としいます。)

\mathbb{M}(\mathbb{q})\ddot{\mathbb{q}} + \mathbb{C}(\mathbb{q},\dot{\mathbb{q}}) + \mathbb{G}(\mathbb{q}) = \mathbb{\tau}

\mathbb{M}(\mathbb{q}) = \(\array{\\{M_{11}}{M_{12}}\\{M_{21}}{M_{22}}}\), \mathbb{C}(\mathbb{q}, \dot{\mathbb{q}}) = \(\array{\\{C_{1}}\\{C_{2}}}\)
\mathbb{G}(\mathbb{q}) = \(\array{\\{G_{1}}\\{G_{2}}}\), \mathbb{q} = \(\array{\\{\theta_{1}}\\{\theta_{2}}}\), \mathbb{\tau} = \(\array{\\{\tau_{1}}\\{\tau_{2}}}\)

M_{11} = J_{1}+J_{2}+m_{1}Lc_{1}^{2}+m_{2}L_{1}^{2}+m_{2}Lc_{2}^{2}+2m_{2}L_{1}Lc_{2}\cos(\theta_{2})
M_{12} = J_{2}+m_{2}Lc_{2}^{2}+m_{2}L_{1}Lc_{2}\cos(\theta_{2})
M_{21} = M_{12}
M_{22} = J_{2}+m_{2}Lc_{2}^{2}

C_{1} = -m_{2}L_{1}Lc_{2}\sin(\theta_{2})(2\dot{\theta_{1}}\dot{\theta_{2}}+\dot{\theta_{2}}^2)
C_{2} = m_{2}\dot{\theta_{1}}^{2}L_{1}Lc_{2}\sin(\theta_{2})

G_{1} = -m_{1}gLc_{1}\sin(\theta_{1})-m_{2}g(L_{1}\sin(\theta_{1})+Lc_{2}\sin(\theta_{1}+\theta_{2}))
G_{2} = -m_{2}gLc_{2}\sin(\theta_{1}+\theta_{2})

コントローラは目標角度に追従するようなPDコントローラにすると、
\tau_{1} = cp_{1}(dv_{1}-\theta_{1})-cd_{1}\dot{\theta_{1}}
\tau_{2} = cp_{2}(dv_{2}-\theta_{2})-cd_{2}\dot{\theta_{2}}
cp_{1}, cp_{2}, cd_{1}, cd_{2} : コントローラ係数

dv_{1}, dv_{2} : 目標角度

次回は、実際にシミュレーション紹介できたらいいなー
今、頑張って思い出して、ソース書いてます・・・。