ロボットソフトウェア特論...

ロボットソフトウェア特論 (7-3)

2018.5.30

電気通信大学

大学院情報理工学研究科

末廣尚士

電気通信大学大学院情報理工学研究科情報学専攻ロボットソフトウェア特論2018年度前学期

10. ロボットアームの順運動学

何をしたいか関節角度から手首、手先、ツールの位置や姿勢を計算する．

何のためにアームの位置や姿勢の現状を知り，それを制御するため

順運動学が不要な制御もあるが，，，

2


- 座標系の関係の決まり方

3


- アームの構造の座標系による表現

座標系による表現リンクに貼り付けられた座標系でその位置・姿勢などの状態を表現する

順運動学などが簡単に求められる

座標系の連鎖シリアルリンクの場合，手先のリンクは根元のリンクによってまとめて動かされる

4


- 座標系の置き方(いろいろあるが)

動かされるリンクの原点と関節の位置とを一致させる。

関節軸を座標軸のどれかに一致させる。

”可能なら”ジョイントパラメタ（関節角）がゼロのとき座標系の姿勢がすべて一致する。

可能でない場合は、、、、工夫が必要

他に，有名（数学的にきれい）なものにはDH(Denavit-Hartenberg，デナビット・ハーテンバーグ)法などがある

5


- ロボットアームの座標系表現

2𝑇3 𝜃3 =

cos 𝜃3 0 sin 𝜃3 00 1 0 0

−sin 𝜃3 0 cos 𝜃3 𝑙2ℎ0 0 0 1

1𝑇2 𝜃2 =

cos 𝜃2 0 sin 𝜃2 00 1 0 0

−sin 𝜃2 0 cos 𝜃2 𝑙1ℎ0 0 0 1

0𝑇1 𝜃1 =

cos 𝜃1 −sin 𝜃1 0 0sin 𝜃1 cos 𝜃1 0 00 0 1 𝑏ℎ0 0 0 1

6


- 手首位置の計算

3𝑇wrist =

1 0 00 1 00 0 1

00𝑙3ℎ

0 0 0 1

ベースから見た手首の座標系は，0𝑇wrist 𝜃1, 𝜃2, 𝜃3

=0𝐴wrist

0𝒅wrist

0 0 0 1= 0𝑇1 𝜃1

1𝑇2 𝜃22𝑇3 𝜃3

3𝑇𝑤𝑟𝑖𝑠𝑡

基本的に順運動学はこれで終わり

7


- 座標系を使ってpythonで計算する

8

>>> bh=0.1>>> l1h=0.05>>> l2h=0.4>>> l3h=0.4>>> T1=FRAME(xyzabc=[0,0,bh,0,0,pi/6])>>> T2=FRAME(xyzabc=[0,0,l1h,0,pi/4,0])>>> T3=FRAME(xyzabc=[0,0,l2h,0,pi/2,0])>>> Tw=FRAME(xyzabc=[0,0,l3h,0,0,0])>>> A=T1*T2*T3*Tw>>> A.xyzabc()[0.48989794855663565, 0.28284271247461901, 0.15000000000000008, -2.677945044588987, 0.65905803582640898, 2.4568734505875103]


- 関数に仕立てる(2)

9

from geo import *

bh=0.1l1h=0.05l2h=0.4l3h=0.4

Tw=FRAME(xyzabc=[0,0,l3h,0,0,0])

def fk2(th):T1=FRAME(xyzabc=[0,0,bh,0,0,th[0]])T2=FRAME(xyzabc=[0,0,l1h,0,th[1],0])T3=FRAME(xyzabc=[0,0,l3h,0,th[2],0])return T1*T2*T3*Tw


- 確認する

10

>>> fk1([pi/6, pi/4, pi/2])(0.48989794855663565, 0.28284271247461895, 0.15000000000000008)

>>> fk2([pi/6, pi/4, pi/2])f:(m:[[-0.61237243569579447, -0.49999999999999994, 0.61237243569579458], [-0.35355339059327368, 0.86602540378443871, 0.35355339059327373],[-0.70710678118654757, 0.0,

-0.70710678118654746]],v:[0.48989794855663565, 0.28284271247461901, 0.15000000000000008])>>> _.xyzabc()[0.48989794855663565, 0.28284271247461901, 0.15000000000000008, -2.677945044588987, 0.65905803582640898, 2.4568734505875103]>>>


課題3-2 (再掲)(1)前出の3自由度アーム(arm3dof.py)の関節角度

𝜃1，𝜃2，𝜃3が与えられたとき手先の方向はどのようになっているか，アームのベース座標系での手首から手先への単位ベクトルで示せ．

11


課題3-2の解答例答えは，手首座標系のz軸方向

12

>>> fk2([pi/6, pi/4, pi/2])f:(m:[[-0.61237243569579447, -0.49999999999999994, 0.61237243569579458], [-0.35355339059327368, 0.86602540378443871, 0.35355339059327373],[-0.70710678118654757, 0.0, -0.70710678118654746]],v:[0.48989794855663565, 0.28284271247461901, 0.15000000000000008])

>>> _.mat.col(2)v:[0.61237243569579458, 0.35355339059327373, -0.70710678118654746]>>>


課題3-3 (再掲+修正)

(1) 6自由度アーム(arm6dof.py)に関して，3自由度アームの時と同様に，関節角度が与えられたときのアームのベース座標系での手首ハンドの位置，および，手先の方向を求めよ

13


- ハンドについて

決まりはないが，ここでは，物をつかむときに掴みやすいように座標系を設定する．

14


- ハンドと手首の関係

アームの手首部に装着したときに，その座標系がどのような状態になるかを考えて座標変換行列を設定する．

15

wrist𝑇hand =

1 0 00 −1 00 0 −1

00

𝑙_hand0 0 0 1

ベースから見たハンドの座標系は3自由度アームだと，0𝑇hand 𝜃1, 𝜃2, 𝜃3= 0𝑇1 𝜃1

1𝑇2 𝜃22𝑇3 𝜃3

3𝑇wristwrist𝑇hand


6自由度アームの順運動学

16

2𝑇3 𝜃3 =

cos 𝜃3 0 sin 𝜃3 00 1 0 0

−sin 𝜃3 0 cos 𝜃3 𝑙2ℎ0 0 0 1

1𝑇2 𝜃2 =

cos 𝜃2 0 sin 𝜃2 00 1 0 0

−sin 𝜃2 0 cos 𝜃2 𝑙1ℎ0 0 0 1

0𝑇1 𝜃1 =

cos 𝜃1 −sin 𝜃1 0 0sin 𝜃1 cos 𝜃1 0 00 0 1 𝑏ℎ0 0 0 1



17

5𝑇6 𝜃6 =

cos 𝜃6 −sin 𝜃6 0 0sin 𝜃6 cos 𝜃6 0 00 0 1 𝑙5ℎ0 0 0 1

4𝑇5 𝜃5 =

cos 𝜃5 0 sin 𝜃5 00 1 0 0

−sin 𝜃5 0 cos 𝜃5 𝑙4ℎ0 0 0 1

3𝑇4 𝜃4 =

cos 𝜃4 −sin 𝜃4 0 0sin 𝜃4 cos 𝜃4 0 00 0 1 𝑙3ℎ0 0 0 1



18

wrist𝑇hand =

1 0 00 −1 00 0 −1

00

𝑙_hand0 0 0 1

6𝑇wrist =

1 0 00 1 00 0 1

00𝑙6ℎ

0 0 0 1

0𝑇hand 𝜃1, 𝜃2, 𝜃3, 𝜃4, 𝜃5, 𝜃6= 0𝑇1 𝜃1

1𝑇2 𝜃22𝑇3 𝜃3

3𝑇4 𝜃44𝑇5 𝜃5

5𝑇6 𝜃66𝑇wrist

wrist𝑇hand


- 関数に仕立てる

19

from geo import *

bh=0.1l1h=0.05l2h=0.4l3h=0.3l4h=0.1l5h=0.1l6h=0.05l_hand=0.1

Tw=FRAME(xyzabc=[0,0,l6h,0,0,0])Th=FRAME(xyzabc=[0,0,l_hand,pi,0,0])


- 関数に仕立てる

20

def fk_6dof(th):T1=FRAME(xyzabc=[0,0,bh,0,0,th[0]])T2=FRAME(xyzabc=[0,0,l1h,0,th[1],0])T3=FRAME(xyzabc=[0,0,l2h,0,th[2],0])T4=FRAME(xyzabc=[0,0,l3h,0,0,th[3]])T5=FRAME(xyzabc=[0,0,l4h,0,th[4],0])T6=FRAME(xyzabc=[0,0,l5h,0,0,th[5]])

return T1*T2*T3*T4*T5*T6*Tw*Th

例題の答えは，ハンドの原点とz軸方向（の反対）

fk_6dof(th).vec-fk_6dof(th).mat.col(2)


- 手先位置の軌道をグラフにする

21

ff=open('test.dat')aa=ff.read()ff.close()

bb=eval(aa)

まずは動作ログを読み込む


- 手先位置の軌道をグラフにする

22

dd = map(fk_6dof, bb)

手先座標系軌道

手先位置をプロット

ff=map(lambda x: x.vec, dd)plt.plot(range(len(ff)),ff)plt.legend(["x","y","z"])plt.show()


- 手先位置の軌道を描く

23


- 手先位置の3D軌跡を描く

手首位置を三次元プロットする参考：http://matplotlib.org/api/figure_api.html

24

from mpl_toolkits.mplot3d import Axes3Dfig = plt.figure()ax = fig.gca(projection='3d')xx=[p.vec[0] for p in dd]yy=[p.vec[1] for p in dd]zz=[p.vec[2] for p in dd]ax.plot(xx,yy,zz)ax.legend(["xyz"])plt.show


- 手先位置の3D軌跡を描く

25


- 汎用順運動学関数

アームの構造表現自由度数

関節の位置・姿勢，関節軸

手首への座標変換

手首からツールへの座標変換

関節角度を与えるとベースからツールの座標変換を返す

26


-汎用順運動学関数：calc_fk()

27

def calc_fk(joints,arm_conf) :# arm_conf={'dof':dof,# 'j_conf':[[[xyzabc of joint],joint_type],,],# 'wrist':[xyzabc of wrist],# 'tool':[xyzabc of tool]]dof=arm_conf['dof']j_conf=arm_conf['j_conf']wrist = FRAME(xyzabc=arm_conf['wrist'])tool = FRAME(xyzabc=arm_conf['tool'])rslt=FRAME()for i in range(dof) :

j_para = [0.0,0.0,0.0,0.0,0.0,0.0]j_para[j_conf[i][1]]=joints[i]rslt=rslt*FRAME(xyzabc=j_conf[i][0])*FRAME(xyzabc=j_para)

return rslt*wrist*tool


- calc_fkの使い方

アームの構造を辞書形式で表現する

28

arm_conf={'dof':6,'j_conf':[[[0,0,bh,0,0,0],5],[[0,0,l1h,0,0,0],4],

[[0,0,l2h,0,0,0],4],[[0,0,l3h,0,0,0],5],[[0,0,l4h,0,0,0],4],[[0,0,l5h,0,0,0],5]],

'wrist':[0,0,l6h,0,0,0],'tool':[0,0,l_hand,pi,0,0]}


- calc_fkとfk_6dof

29

j1=[pi/3,pi/4,pi/2,pi/4,0,pi/6]

fk_6dof(j1).xyzabc()

calc_fk(j1,arm_conf).xyzabc()

[0.3712310601229375, 0.6429910574805843,

-0.026776695296636774, 0.7137243789447659,

-0.3613671239067079, 3.015795354733762]

答えは同じ

ロボットソフトウェア特論...

Documents