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

知能システム論１（７）
運動学(Kinematics)
速度・加速度
２００７．５．２２
講義内容
１．はじめに
２．ベクトルの基礎
３．運動学(Kinematics)：速度・加速度
４．動力学（Dynamics)
５．行列の演算と応用(Matrix)
６．軌道計算(Trajectory)
７．ロボットの制御(Control)
８．応用(Application)
練習問題
z
図のマニピュレータのPに関するヤコビアンを求めよ。
θ3
(1) (xi,yi,zi)を求める(i=1~3)。
y3
z3
x3
1
1
(2) Pi、Pを求める。
P
z1,z2
(3) ヤコビアンの式に上を代入する。
y1,y2
θ2
θ1
x
x1,x2
θ1=45度
θ2=0度
θ3=90度
y
練習問題の解
図のマニピュレータのPに関するヤコビアンを求めよ。ただしθ1=45度、
θ 2=0、 θ3=90度、 l1  0 , l 2  l 3  1 とせよ。
(1)
( x1
(xi,yi,zi)を求める(i=1~3)。
y1
( x3
( x2
 ( x2
z1 )  ( x 2
y2
y3
z3 ) 
y2
 c3

z2 ) 0

  s 3
y2
 0

z2 ) 0

  1
 c1

z 2 )  s1

 0
 s1
c1
0
 1
0 
2
  1
0 
 
2
1   0

1
2
1
2
0
0

0

1

θ3
P3
リンク2
リンク３
l3
s3 

0

c 3 
0
1
0
0
1
0
1

0

0 
l2
P
 0

  0

 1

1
1
2
0
Z2

2
1

2
0 

1
2
P2
X2
(2) Pi、Pを求める。
 1

0
 
2

 

P1  P2  0 , P3  l 2 z 2  0 , P  P3  l 3 z 3   1
 
2

 1 
 1 
θ2
X０
l1
Y2
Z0,Z1 リンク1
Y1
P1
θ1
X1
Y0
問題のマニピュレータのヤコビアンは次のとおり。
ヤコビアン J
P   z 1  ( P  P1 )
y 2  ( P  P2 )
 1
  1

0
  
2 
2
 
  1

z1  ( P  P1 )  0   1
  
2 
2 
 1   1   0 

 

 1
  1
  1

2 
2 
2

 1
  1

y 2  ( P  P2 )   1
2  
2 
2

 0   1   1 

 
 

 1
  1

2 
2  0 

 1
  0 
y 3  ( P  P3 )   1
2  
2  

 0   0    1

 

 1 
 
y 3  ( P  P3 )  2 
3 
 
 1
2

J   1
2

 0

1
2
1
2
1
0 

0 

 1

p
速度の関係式(幾何学的導出）
P 
6

6
p i  ( P  Pi )i
  6 

p ii
i 1
i 1
 P   p 1  ( P  P1 )
  
p1
  



 1 
p 6  ( P  P6 )   
 : 
p6
  

 6


r
| r | sin 
V  p  r 
P
P
P-Pi
Pi
同じ関係式は微小変位に関しても成り立つ
 1 
 P 



J
:







   6 
pi
p
n

V  ( r sin  ) n

r
| r | sin 
V  p  r 
 p r sin   n
 r sin   n
先端の速度から関節角速度を求める
 1 
 P 
 
  J : 
 
6 
 
 p 1  ( P  P1 )
J  
p1

 1 
 
 :  J
6 
 


1
p 6  ( P  P6 ) 

p6

 P 
 
 
加速度の算出
θ3
マニピュレータ先端部の加速度
P  P0  l1 z 1  l 2 z 2  l 3 z 3
リンク３
P  P0  l1 z1  l 2 z 2  l 3 z 3
 l 2 2  z 2  l 3 3  z 3
P 
d P
dt
P3
リンク2
l3
l2
P
Z2
 l 2 2  z 2  l 2 2  z 2
P2
 l 3 3  z 3  l 3 3  z 3
 l 2 2  z 2  l 2 2  ( 2  z 2 )
 l 3 3  z 3  l 3 3  (  3  z 3 )
θ2
X2
X０
l1
Y2
Z0,Z1 リンク1
Y1
P1
θ1
X1
Y0
回転加速度ベクトル
i
i 

i
 i 
 ( p 
j
j
 p j j )
j 1
p j j
i
j 1


( p jj   j  p j j )
j 1
前図マニピュレータの回転加速度ベクトル
 1  z11   1  z11  z11
 2  z11  y 22   2  y 22
 3  z11  y 22  y 33   2  y 22   3  y 33
 1  z11
 2  z11  y 22
 3  z11  y 22  y 33
６自由度マニピュレータの加速度
P  P6   6  ( P  P6 )
P6  P5   5  ( P6  P5 )

P2  P1   1  ( P2  P1 )
時間微分
P  P6   6  ( P  P6 )   6  ( P  P6 )
P6  P5   5  ( P6  P5 )   5  ( P6  P5 )

P2  P1   1  ( P2  P1 )   1  ( P2  P1 )
加速度のヤコビアンを用いた表現
 1 
 1 
 P 
   
  J :  J : 
 
6 
6 
 
 
 1 
 P 
 
  J : 
 
6 
 
 p 1  ( P  P1 )
J  
p1



p 6  ( P  P6 ) 

p6

 p 1  ( P  P1 )  p 1  ( P  P1 )
J  
p 1



p 6  ( P  P6 )  p 6  ( P  P6 ) 

p 6

応用
直動自由度を含む場合
長さをあらわすパラメータを時間の関数として扱う。
P  sx 1
P  s x1  s x1
P  sx1  s x1  s x1  s x1
 1  1 z1
 1  1 z1
 sx1  2 s ( 1  x1 )  s  1  ( 1  x1 )  s  1  x1
x1 
d
dt
( x1 ) 
d
dt
z1
( 1  x1 )   1  x1   1  ( 1  x1 )
1
s
x1
y1
Ｐ
P  sx  2 s ( z  x )  s  2 z  ( z  x )  s  z  x
1
1
1
1
1
1
1
1
1 1
1
2
 sx1  2 s1 y 1  s 1 z1  ( y 1 )  s 1 y 1
2
2










 s x1  2 s  1 y 1  s  1 (  x1 )  s  1 y 1  ( s  s  1 ) x1  ( 2 s1  s 1 ) y 1
練習問題
 1  0,  2  0,  3   / 2,
1  1 rad / sec, 2  0 , 3  0
θ3
P3
のとき、マニピュレータ先端部の
加速度ベクトルをもとめよ。 リンク３
リンク2
l3
l2
P
θ2
Z2
P2
X2
X０
l1
Y2
Z0,Z1 リンク1
Y1
P1
θ1
X1
Y0
姿勢の算出
A1
x 1  cos  1  x 0  sin  1  y 0
y 1   sin  1  x 0  cos  1  y o
 x1
y1
z1    x 0
z1  z 0
y0
単位行列
x 2  c 2 x1  s 2 z1
A2
y 2  y1
x 2
y2
z 2    x1
y1
z 2  s 2 x1  c 2 z1
x 3  c3 x 2  s3 z 2
y3  y2
x 3
y3
z 3   x 2
y2
z3  s3 x 2  c3 z 2
x 3
 c1

z 0  s1

 0
y3
z 3   x 0
y0
z 0  A1 A2 A3  A1 A2 A3
xi
 s1
c1
0
 c2

z1  0

  s 2
1
A3  c 3
0

z2  0

  s 3
yi
0
0
1
0
0

0

1 
s2 

0

c 2 
s3 

0

c 3 
z i   A1    Ai
ここまで
演習問題
６月５日(火）まで提出のこと
図のマニピュレータのPに関するヤコビアンをθ1,θ2,θ3の関数として求めよ。
５月８日出題の演習問題の結果を利用せよ。
θ3
P3
リンク３
リンク2
l3
l2
P
θ2
Z2
P2
X2
X０
l1
Y2
Z0,Z1 リンク1
Y1
P1
θ1
X1
Y0