Transcript ロボティクスーその来し方行く末

知能システム論１（４）
運動学(Kinematics)
２００８．５．１３
講義内容
１．はじめに
２．ベクトルの基礎、応用（回転）
３．運動学(Kinematics)
４．動力学（Dynamics)
５．行列の演算と応用(Matrix)
６．軌道計算(Trajectory)
７．ロボットの制御(Control)
８．応用(Application)
回転変換行列の性質
軸まわりに 回転したときの
j
回転変換行列を表す記号：
j
E
a)交換則成り立たない
E k ( E j v )  E j ( E k v )
b)同一軸周りの回転：交換則
E k ( E k v)  E k ( E k v )  E k (   )v
c)行列式
| E k | det( E k )  1
d)逆行列
k 1
(E )  E
e)内積
k (  )
k T
 (E )
Ev  Ew  v  w
f)外積
Ev  Ew  Ev  w


1 
 0





v  0 , w  1 ,n  
 
 

0
0



練習問題
z
n
ｗ
x
v
Θ=６０度、
としたとき
E n v  E n w  E n (v  w)
y
v
1 
3
1 
,
3
1 
3 
であることを示せ。
1 0 0
v  w  0  1  0
     
0 0 1
 2/3 
E n v   2 / 3 


  1 / 3
  1 / 3
E n   2 / 3 


 2 / 3 
E n
 2 / 3  1/ 3 2 / 3 
  2 / 3 2 / 3  1 / 3


 1 / 3 2 / 3 2 / 3 
 2/3 
E n v  w    1 / 3


 2 / 3 
4  2
 9   2/3 
1 4  
n
n
E vE w  
    1 / 3
 9   2/3 

4  2 
 9 
ロボットマニピュレータの運動学(Kinematics)
ロボットの自由度（関節）の動きを表す変数と、ロボットを
構成するリンクの位置姿勢、またはそれらの変化速度、
加速度間の関係式を求める。
自由度変数ー＞リンクの位置姿勢
順運動学 （Forward Kinematics)
リンクの位置姿勢ー＞自由度変数
逆運動学 (Inverse Kinematics)
講義では基準座標系からみた３次元ベクトルによる
表現を基本とする。
リンクの姿勢を表す座標系（ベクトルの組）
各リンクに固定した直交する単位ベクトル：Xi,Yi,Ziを
導入する。
 xix 
xi   xiy ,
 
 xiz 
 yix 
yi   yiy ,
 
 yiz 
 zix 
zi   ziy 
 
 ziz 
θ3
リンク３
Y3
Z3
リンク2
X3
θ2
Z2
Y0
Y1
sin 1
X1
θ１
cos1
X2
X0
X０
Y2
Z0,Z1 リンク1
Y1
θ1
図1
X1
Y0
ベクトルの回転
n
v3
Vをn軸まわりに回転
v  v1l  v2m  v3n
v  v1l   v2m  v3n
l
v
v2
v1
m
l,m,nのn軸まわりの回転
l  cos  l  sin   m
m   sin   l  cos  m
n
n  n
cos
l  m n  l m n sin 
 0
 sin 
cos
0
0
0

1
l
cos
l
n

m
sin 
m
l x
l  m n  l y
l z
mx
my
mz
l   cos   l  sin   m
nx  cos 
n y   sin 
nz   0
 sin 
cos 
0
0 l x cos   mx sin  ・ ・
0  l y cos   m y sin  ・ ・
1  l z cos   mz sin  ・ ・
 cos   nx2 (1  cos  )

l   nx n y (1  cos  )  nz sin 
nz nx (1  cos  )  n y sin 

nx n y (1  cos  )  nz sin 
cos   n y2 (1  cos  )
nx n y (1  cos  )  nx sin 
nz nx (1  cos  )  n y sin   l x 

nx n y (1  cos  )  nx sin   l y 
cos   nz2 (1  cos  )  l z 
l x cos   l x nx2 (1  cos  )  l y nx n y (1  cos  )  l y nz sin   l z nz nx (1  cos  )  l z n y sin  










(1  cos  )nx (l x nx  l y n y  l z nz )  sin  (l y nz  l z n y )  cos   l x 

 





l n  0
sin  (l z n y  l y nz )  cos   l x  sin   mx  cos   l x 


 sin  (l x nz  l z nx )  cos   l y   sin   m y  cos   l y   cos   l  sin   m
sin  (l y nx  l x n y )  cos   l z   sin   mz  cos   l z 


l n  m
姿勢の算出
x1  cos1  x0  sin 1  y0
y1   sin 1  x0  cos  yo
A1
x1
y1
z1   x0
y0
z1  z0
x2  c2 x1  s2 z1
y2  y1
x2
z2  s2 x1  c2 z1
y2
z2   x1
y1
x3  c3 x2  s3 z2
y3  y 2
x3
z3  s3 x2  c3 z2
x3
y3
z3   x0
y0
z0 A1 A2 A3
y3
xi
z3   x2
yi
y2
zi   x0
c1  s1 0
z0  s1 c1 0


0 1
 0
A2
 c2
z1  0

 s2
0 s2 
1 0

0 c2 
A3  c3
0 s3 
1 0

0 c3 
z2  0

 s3
y0
z0 A1    Ai
練習問題
先のロボットマニピュレータにおいて
Θ１=９０度、θ２＝４５度、θ３=４５度としたときの
Xi,Yi,Zi (i=1,2,3)を求めよ。

0  1 0
x1
x2
x3
y3
y1
y2
z1   x0
z2   x1
z3   x2
y2
y1
y0
0  1 0
z0 1 0 0


0 0 1
 1
2

z1  0
 1
 
2
 1
2

z2  0
 1
 
2
0
1
0
0
1
0

2
0 
1 
2 
1

2
0 
1 
2 
1
x2
y2
z2   1

0

 0
  1
2
 1

2
1
0
0

 0
x3 y3 z3    1
2
 1

2
 0  1 0
0
0 1


 1 0 0
1
2


0  0

1  1
2

0
0

2
0 
1 
2 
1
0
1
0

0 
1 
2
1 
2 
1
0
0

0  1 2

1  0
2 
1   1 2
2  
0
1
0

2
0 
1 
2 
1
リンクの位置を表すベクトル
各リンクに固定した座標系の原点の位置を表すベク
トル：Piを導入する。
θ3
P3
P1  0
P 2  P1  l1 z1
P3  P2  l2 z2
リンク３
リンク2
l3
l2
P
θ2
Z2
P2
P  P3  l3 z3
X2
図2
X０
l1
Y2
Z0,Z1 リンク1
Y1
P1
θ1
X1
Y0
練習問題
先のロボットマニピュレータにおいて
Θ１=９０度、θ２＝４５度、θ３=４５度としたときの
Pi(i=1,2,3),Pを求めよ。


P1  0

0
0 


l
0


2
P  P3  l3 z3  P3  l3  l3 





2
P2  l1 z1  0

 
 0  
l
 l1  2

l1 
 0  

2

 l2
 

0


2   l2
P3  P2  l2 z2  P2  


2
l
2
 

l
2  l1  2


2

 
演習問題
A4の用紙に書いて５月２７日までに提出のこと
先のロボットマニピュレータにおいて
Pi(i=1,2,3),Pを与える漸化式を展開し、それらを
θ１、θ２、θ３の関数として表せ。
リンクの姿勢の一般形
各リンクへの座標系の設定法：
・各自由度の回転軸が座標系の
X,Y,Z軸のどれかに一致し、
θ3
θi=0 (i=1~n)のときすべての
座標系が一致するように設定
X3
・各自由度の回転軸を
リンク３
座標系のZ軸に一致する
Y3
ように設定
・各自由度の回転軸が
座標系のX,Y,Z軸のどれかに一致するよ
う設定
第iリンクに固定した座標系Xi,Yi,Ziの
θi=0のときの値を、Xi’,Yi’,Zi’とすると、
[Xi’,Yi’,Zi’] = [Xi-1,Yi-1,Zi-1]
とは必ずしもならない。
Z3
リンク2
θ2
X2
Y2
X０
θ1
Z2
Z0,Z1 リンク1
Y1
Y0
xi1
yi1
zi1   xi 1
yi 1
bxx

zi 1 bxy
bxz
byx
byy
byz
xi 1
Bi 1
xi yi
 xi 1
xn
zi   xi1
yi 1
yn
yi1
zi 1 Bi 1 Ai
bzx 

bzy 
bzz 
zi 1
bxz
'
i 1
x
bxy
zi1 Ai
bxx
xi1  bxx xi 1  bxy yi 1  bxz zi 1
zn   B0 A1    Bi 1 Ai    Bn1 An
前頁の例の場合、
yi 1
x3
y3
0 1 0
z3   A1 0 0 1 A2 A3


1 0 0
リンクの位置の一般形
リンクiから見て、座標系i+1の原点が lix , liy , liz
ずれているとすると各位置ベクトルは次の
θ3
漸化式で求められる。
P3
リンク３
リンク2
l3
P1  0
l2
P
Z2
P 2  P1  l1 x x1  l1 y y1  l1z z1
P3  P2  l2 x x2  l2 y y2  l2 z z2
P  P3  l3 x x3  l3 y y3  l3 z z3
θ2
P2
l1
Y2
Z0,Z1 リンク1
X2
X０
θ1
P1
X1
Y1
Y0