Home Made Garbage

逆運動学 で制御 ーロボットアーム自作への道2ー

ホーム
電子工作
Raspberry Pi
IOT
WEB

さて今回は前回作ったロボットアームを本格的に動かしていきたいと思います。

以下の書籍で逆運動学なるものを知りましたので、その逆運動学でロボットアームを制御します。

ここではアームにペンをつけて文字を書かせてみます。

 

 

逆運動学とは

前回は各サーボモータの角度を指定して、ロボットアームの姿勢を制御していました。つまり各サーボの角度でアーム先端の座標(x, y, z)が決まります。これを順運動学(Forward Kinematics)といいます。

逆運動学(Inverse Kinematics)とは、まさに逆でロボットアーム先端の座標(x, y, z)を指定して、各サーボの角度を算出する手法です。IKなどと言われているようです。

逆運動学によるサーボ角度算出

前回作ったロボットアームにマジックペンをつけて、ペン先の座標を指定して文字を書くことを目指します。

各アームの長さは以下の通りです。
 $L_1 = 92$ mm
 $L_2 = 137$ mm
 $L_3 = 165$ mm

 
それでは算出していきましょう!
座標は以下のように平面上を(x, y)、高さをzとします。

首振りサーボの角度$θ_1$は平面座標(x, y)ですぐ導出できます。

$$θ_1 =\tan^{-1} \left(\frac{y}{x} \right)$$

 

2個目のサーボの角度$θ_2$は余弦定理を用いて導出します。

${L_3}^2 = {L_2}^2 + {l_d}^2 – 2{L_2}{l_d}\cos{θ_2}’$

${θ_2}’ = \cos^{-1} \left(\frac{{L_2}^2+{l_d}^2 – {L_3}^2}{2{L_2}{l_d}} \right)$

$$θ_2 = {θ_2}’ + φ = \cos^{-1} \left(\frac{{L_2}^2+{l_d}^2 – {L_3}^2}{2{L_2}{l_d}} \right) + φ$$

$$      但し、φ = \tan^{-1} \left(\frac{z – L_1}{l} \right)$$

$$       l_d = \sqrt{l^2 + \left(z – L_1\right)^2}$$

$$       l = \sqrt{x^2 + y^2}$$

 

3個目のサーボの角度$θ_3$も余弦定理を用いて導出します。

${l_d}^2 = {L_2}^2 + {L_3}^2 – 2{L_2}{L_3}\cos \left(90°+ θ_3\right)= {L_2}^2 + {L_3}^2 + 2{L_2}{L_3}\sin {θ_3}$

$$θ_3 = \sin^{-1} \left(\frac{{l_d}^2-{L_2}^2 – {L_3}^2}{2{L_2}{L_3}} \right)$$

 

以上のとおり、所望の座標(x, y, z)とロボットアームの長さ$L_1$、$L_2$、$L_3$からサーボの角度を導出することができました。

参考

 

動作

逆運動学によって座標の指定のみでロボットアームの制御ができるようになったので以下のように線がひけます。

 

 

Arduinoコード

冒頭に紹介した書籍の第5章サンプルコードを参考にプログラミングしました。
以下でArduinoサンプルコードをダウンロードできます。ありがたい!
 https://www.ohmsha.co.jp/book/9784274222115/

求めた逆運動学の式を用いてロボットアーム先端の座標を制御します。
シリアルモニタで打ち込んだ文字によってロボットアームの動作を選択しています。

おわりに

逆運動学によって座標の指定でロボットアームを自由に動かせるようになりました。

 

 

さて、次は現状ちょっと可動範囲が狭いのでアームの構成の調整と
もっと手軽にアームを制御できるように工夫したいと思います。

それではまた

Related Posts

コメントはこちらから

メールアドレスが公開されることはありません。コメントのみでもOKです。

このサイトはスパムを低減するために Akismet を使っています。コメントデータの処理方法の詳細はこちらをご覧ください