第8章

ロボット制御
第８回
逆運動学のプログラム
２関節アームの運動学
手先位置
（Px , Py)
y軸
第２リンク
L2
関節角度２(th2)
L1
第1リンク
関節角度１(th1)
ｘ軸
•運動学：関節角の値から手先の位置を計算する
手先のｘ座標 Px = L1×cos(th1) + L2×cos(th1+th2)
手先のｙ座標 Py = L1×sin(th1) + L2×sin(th1+th2)
•逆運動学：手先の位置から関節角の値を計算する
（式１）
（式２）
逆運動学計算の手順（詳細は第6回）
①与えられた手先位置が実現可能かのチェックを行う
c2 = (Px2 + Py2 － L12 － L22 ) / (2*L1*L2) を計算する
-1 ≦ c2 ≦ １であるかのチェックをする
②th2を求める
s2 = sqrt(1.0-c2*c2)
（
の意味）
2
1 − c2
2
2
(s2) =1-(c2) の解は±があるので，th2の解も2個存在して
解1 th2 = atan2( s2 , c2)
解2 th2 = atan2(-s2 , c2)
③th1を求める
解1 th1 = atan2(Py , Px) － atan2( L2*s2 , L1+L2*c2)
解2 th1 = atan2(Py , Px) － atan2( －L2*s2 , L1+L2*c2)
変数名の対応
Px:pos[0] ， Py:pos[1] ， th1：th[0] ， th2:th[1]

Download Report