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

知能システム論１（８）
動力学(Dynamics)
力と運動方程式
２００８．６．１０
練習問題
 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
姿勢の計算 (
 x1
x3
y1
y3
1

z1   0

 0
z 3   x 2
 0 ,  2  0 ,  3   / 2 rad のとき )
1
0

0

1 
0
1
0
y2
 0

z2  0

  1
x 2
0
1
0
y2
1  0
 
0  0
 
0    1
回転速度・加速度の計算
0 
 
 1  z 11  0  1 
 
 1 
0 
 
0
 
 1 
0 
 
 2  z 11  y 22  0
 
 1 
0 
 
 3  z 11  y 22  y 33  0
 
 1 
z 2    x1
0
1
0
y1
1

z1  0

 0
0
1
0
0  1
 
0  0
 
1   0
0
1
0
0

0

1 
1

0

0 
(1  1rad / s , 2  0 , 3  0 , 1  0 , 2  0 , 3  0のとき )
 1  z 11   1  z11  z 11  0
 2  z11  y 22   2  y 22  0
 3  z11  y 22  y 33   2  y 22   3  y 33  0
腕先端Ｐの加速度の計算
P 
d P
 l 2  2  z 2  l 2  2  z 2  l 3 3  z 3  l 3 3  z 3
dt
 l 2  2  z 2  l 2  2  ( 2  z 2 )  l 3 3  z 3  l 3 3  ( 3  z 3 )
0  0  0 
0  0  1 
0  0 
  1
     
     
   
 
 l 2 0  ( 0  0 )  l3 0  ( 0  0 )  l3 0  1  l3 0
     
     
   
 
 1   1   1 
 1   1   0 
 1   0 
 0 
1 rad/sec
講義内容
１．はじめに
２．ベクトルの基礎
３．運動学(Kinematics)
４．動力学（Dynamics)
５．行列の演算と応用(Matrix)
６．軌道計算(Trajectory)
７．ロボットの制御(Control)
８．応用(Application)
力のモーメント
大きさ：
方向：
向き：
ｎ
| F || r | sin 
ｎ（ｒ と F を含む面に垂直）
右ねじ
r
|r|sinθ
M  rF
 r F sin  n
θ
F
先端部における力と力のモーメントの効果
第 i 関節にかかる力のモーメント:
M  ( P  Pi )  F
第 i 関節の回転力 Ti ：
Ti  p i  ( M  ( P  Pi )  F )
 p i  M  ( p i  ( P  Pi ))  F
F
M
P
 T1   ( p1  ( P  P1 ))
  
: 
:
  
T
 T 6   ( p 6  ( P  P6 ))
T
F 
T 
T  J  
M 
P-Pi
Zi
p 
 F 
:  
M
T 
p 6 
T
1
J
T
Pi
Yi=pi
Z0
X０
P1
Y0
ヤコビアン（係数行列）Jacobian
P 
6

p i  ( P  Pi )i
マニピュレータ先端の並進速度
i 1
6
  6 

p j j
マニピュレータ先端リンクの回転速度
j 1
上式を１つにまとめると次のようになる
 P   p 1  ( P  P1 )
  
p1
  
 1 
p 6  ( P  P6 )   
  
p6
  

 6


先端の速度
例題のヤコビアン
 P   z 1  ( P  P1 )
  
z1
  
J:ヤコビアン
y 2  ( P  P2 )
y2
 1 
y 3  ( P  P3 )   


 2
y3
  

 3
自由度変数の
変化速度
例題
0
f
0
l 
0
l 
 
 
 
 
 
 
M  0 , F  0 , p1  p 2  0 , P  l , P1  0 , P2  0
 
 
 
 
 
 
 0 
 0 
 1 
 0 
 0 
 0 
T1 , T 2 : ?
p1
p2
l
T1  ( p1  ( P  P1 ))  F
0
 
( 0 
 
 1 
l 
 
l )
 
 0 
P2
f
 
0   fl
 
 0 
P1
y
l
P
0
 
T 2  ( p 2  ( P  P2 ))  F  ( 0 
 
 1 
x
z
0
 
l )
 
 0 
f
 
0   fl
 
 0 
f
 
F  0
 
 0 
エネルギー保存の条件
を用いて先端に加わる
関節トルクの関係を導
く
力と
F x  x  F y  y  F z  z  T1   1  T 2   2  T 3   3
F
F
x
x
F
( F
x
J
T
Fy
x
 
F z  y  T1
 
  z 
Fy
 1 


F z J   2  T1


   3 
Fy
F z J  T1
x
T2


Fy
x
F

Fy

F z J )  T1
Fz
 T1 
 Fx 
 

T 
T  J Fy
 2
 
 T 3 
 F z 

T
 T1
Ｔ3，⊿θ3
 1 


T3    2


   3 
T2
T3 
T2
T
 1 


T3    2


   3 
T2
T3 
T2
T3 
P
T
Fx, Fx, Fx ⊿x, ⊿y, ⊿z
Ｔ2，⊿θ2
T
Ｔ１，⊿θ1
X０
Z0
P1
Y0
動力学(Dynamics)
逆動力学： リンクの位置、速度、加速度から関節トルクを求める。
順動力学： 関節トルクからリンクの加速度を求める。
・逆動力学はロボットの制御に用い、順動力学はロボットの
シミュレーションに用いる。
・ここでは、主として逆動力学について述べる。順動力学は
逆動力学に基づき展開することができる。
はじめに例題の３関節(自由度）ロボットを取り上げ、
各リンクの質量が重心に集中している場合を扱い、
一般への展開はその後考える。
運動方程式(Equation of Motion)
F3
P3 g
F
M3
Fi , M i :
 F3
z3
 M3
l3 g
第 i-1 リンクが
第 i リンクに与える
力とモーメント
ベクトル
P2 g
M
l2 g
リンク重心位置
l2
z2
M
2
F2
P1 g  P1  l1 g z1  l1 g z1
P1 g
P2 g  P2  l 2 g z 2  l1 z1  l 2 g z 2
P3 g  P3  l 3 g z 3  l1 z1  l 2 z 2  l 3 g z 3
x0
z0
 M2
 F2
l1
l1 g
y0
F1
M1
各リンク重心の速度と加速度
P1 g  0
P1 g  P1  l1 g z1  l1 g z
P2 g  l 2 g  2  z 2
P2 g  P2  l 2 g z 2  l1 z1  l 2 g z 2
P3 g  l 2 2  z 2  l 3 g  3  z 3
P3 g  P3  l 3 g z 3  l1 z1  l 2 z 2  l 3 g z 3
P1 g  0
P2 g  l 2 g ( 2  z 2   2  ( 2  z 2 ))
P3 g  l 2 ( 2  z 2   2  ( 2  z 2 ))  l 3 g ( 3  z 3   3  ( 3  z 3 ))
回転速度ベクトル
回転加速度ベクトル
 1  z11
 1  z11
 2  z11  y 22
 2  z11  y 22  ( 2  y 2 )2
 3  z11  y 22  y 33
 3  z11  y 22  y 33  ( 2  y 2 )2  (  3  y 3 )3
各リンクの力とモーメントの釣合い
F  M 0
リンク３
 0 


g  0


  g 
F3  m 3 (  P3 g  g )  0
F3  m 3 ( P3 g  g )
M 3  ( P3 g  P3 )  ( m 3 (  P3 g  g ))  0
M 3  ( P3 g  P3 )  F3
リンク２
F2  F3  m 2 (  P2 g  g )  0
M 2  M 3  ( P3  P2 )  F3
 ( P2 g  P2 )  ( m 2 (  P2 g  g ))  0
F2  F3  m 2 ( P2 g  g )
M 2  M 3  ( P3  P2 )  F3
 ( P2 g  P2 )  ( F2  F3 ))
リンク1
F1  F2  0
F1  F2
M 1  M 2  ( P2  P1 )  F2  0
M 1  M 2  ( P2  P1 )  F2
関節トルク：
T1  z1  M 1 , T2  y 2  M 2 , T3  y 3  M 3
各リンクの運動方程式の一般形
Fi  Fi  1  m i ( Pig  g )
M i  M i  1  ( Pi  1  Pi )  Fi  1  ( Pig  Pi )  ( Fi  Fi  1 )  I i i   i  ( I i i )
I i ：慣性テンソル
I i  X i
 I ix

Zi 0

 0
Yi
I ix 
(y
2
I iy 
 (z
2
I iz 
 ( x  y ) dm
 z ) dm
 x ) dm
2
0
I iy
0
0

0 X i

I iz 
Yi
Zi
T
慣性主軸
Zi
Ｘ軸周りの慣性モーメント
 xydm


 zxdm
i
 yzdm
0
となる軸
r
2
dm
z x
i
2
2
y
i
Xi
Yi
重心周りの角運動量
N 

dr
 r  ( dt ) dm
 ( xX
 yY  zZ )  (  ( xX  yY  zZ )) dm
 X  (  X )  x dm  X  (  Y )  xydm  X  (  Z )  xzdm
2
 Y  (   X )  xydm  Y  (  Y )  y dm  Y  (  Z )  yzdm
2
 Z  (  X )  xzdm  Z  (  Y )  yzdm  Z  (  Z )  z dm
2
慣性乗積がゼロのＸ，Ｙ，Ｚ軸を選ぶと、
N  X  (  X )  x dm  Y  (  Y )  y dm  Z  (  Z )  z dm
2
2
2
 ((   Y )Y  (  Z ) Z )  x dm
 (  X ) X
 ((   X ) X  (   Z ) Z )  y dm
 (   Y )Y
 ( x  z ) dm
Iy
 ((   X ) X  (   Y )Y )  z dm
 (   Z ) Z  ( x  y ) dm
Iz
2
2
2
 ( y  z ) dm
2
2
2
2
2
2
Ix
オイラー(Euler)方程式（ベクトル表現）
角運動量の時間微分が力のモーメント
N  I x (   X ) X  I y (   Y )Y  I z (   Z ) Z
M 
dN
dt
 I x (   X ) X  I y (   Y )Y  I z (   Z ) Z
 I x (   X ) X  I y (   Y )Y  I z (   Z ) Z
  X    Y    Z  0
 I x (   X ) X  I y (   Y )Y  I z (   Z ) Z
X    X
 I x (  X ) X  I y (  Y )Y  I z (   Z ) Z
Y    Y
   ( I x (   X ) X  I y (   Y )Y  I z (   Z ) Z )
Z    Z
 X
Y
 I x (   X ) 


Z  I y (   Y )     X


 I z (  Z ) 
Y
 I x (  X ) 


Z  I y (  Y )


 I z (   Z ) 
 X
 X
Y
Y
I  X
Ix

Z 0

 0
Ix

Z 0

 0
Y
Ix

Z 0

 0
0    X

0   Y

I z     Z
0
Iy
0
0  X
 T
0 Y

T
I z   Z
T
0
Iy
0
0
Iy
0
0

0 X

I z 


   X





     X


Y
M  I     ( I  )
Z
T
Y
Y
Ix

Z 0

 0
Ix

Z 0

 0
とすると。
0
Iy
0
0
Iy
0
0    X

0  Y

I z     Z
0  X
 T
0 Y

T
I z   Z
T









慣性モーメントの計算
I x  m ( b  c ) / 12
2
Ｚ
I y  m ( a  c ) / 12
2
Ｙ
Ｘ
dz
a
dy
c
2
b
 ( y  z ) dm 
2
y

c / 2

3
 ( y  z )  adydz
2
2
c / 2 b / 2
c/2
dz   a
 z y
2
3
2
b/2
b/2
c/2
 a
2
2
I z  m ( a  b ) / 12
c/2
Ix 
2

(b
c / 2
b / 2
3
3
 bz ) dz   a b z
2
12
  a ( b c  bc ) / 12   abc ( b  c ) / 12  m ( b  c ) / 12
3
3
2
2
2
2
12
 bz
c/2
3
3
c / 2
慣性モーメントの計算
x
rd  dr
l / 2 R 2
r sin 

r
 
Ix 
 ( y  r sin  )  rd  drdy
2
2
2
l / 2 0 0
l / 2 R 2
z
 
 
Ｒ
Ｚ
 ( ry  r sin  ) d  drdy
2
3
2
l / 2 0 0
Ｘ
l / 2 R 2
 
 
Ｙ
 ( ry  r
2
3
1  cos 2
2
l / 2 0 0
R
) d  drdy
2
l/2 R
 
 
l
( ry  r
2
3
2 )  r ( sin 2 ) 4
3
l / 2 0
drdy
0
l/2 R

 
 2 ( ry  r
2
3
2 )drdy
l / 2 0
R
l/2
 
 2 ( y
2
l/2
r / 2  r / 8 ) dy  
2
4
l / 2
 ( y
2
R   R / 4 ) dy    R y / 3   R y / 4
2
0
2
2
2
l / 2
  R l ( l / 12  R / 4 )  m ( l / 12  R / 4 )
2
4
2
2
Iz  Ix
I y  mR / 2
2
3
4
l/2
l / 2
逆動力学の計算手順
１)
２）
３）
４）
５）
６）
各リンクの姿勢と重心位置(θ:given）
各リンク重心の速度・回転速度（ :given）
各リンク重心の加速度・回転加速度（ :given）
各リンクの運動方程式
各関節に加わる力・モーメントベクトル(Fi,Mi）
各関節のトルク（Ti）
１）～３）はベースから手先へ、
４）～６）は手先からベースの向きに漸化式を立てる
オイラー(Euler)方程式
慣性主軸座標系でのオイラー方程式
M
x
 I x  x  ( I z  I y ) y  z
M
y
 I y  y  ( I x  I z ) z x
M z  I z z  ( I y  I x ) x  y
慣性主軸
Z
z M
基準座標系でのオイラー方程式
M  dN
dt
 I     ( I  )
x
X
Mx
z
y
Y
M
y
オイラー(Euler)方程式
M  dN
I    X
 X
Y
dt
Y
 I     ( I  )
I x

Z 0

 0
I x

Z 0

 0
0
Iy
0
0
Iy
0
0

0 X

I z 
Y
M
x
 I x  x  ( I z  I y ) y  z
M
y
 I y  y  ( I x  I z ) z x
M z  I z z  ( I y  I x ) x  y
Z  
T
0   x 
 
0  y  I x  x X  I y  y Y  I z  z Z
 
I z    z 
  ( I  )    ( I x x X  I y  y Y  I z z Z )
 ( x X   y Y   z Z )  ( I x x X  I y  y Y  I z z Z )
 I y  x  y Z  I z x  zY  I x x  y Z  I z y  z X  I x  x  zY  I y  y  z X
 ( I z  I y ) y  z X  ( I x  I z ) x  z Y  ( I y  I x ) x  y Z
M  I x  x X  I y  y Y  I z  z Z
 ( I z  I y ) y  z X  ( I x  I z ) x  z Y  ( I y  I x ) x  y Z
 ( I x  x  ( I z  I y ) y  z ) X  ( I y  y  ( I x  I z ) x  z )Y  ( I z  z  ( I y  I x ) x  y ) Z
 M x X  M yY  M z Z