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

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

Download Report