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

生産情報システム学（９）
動力学(Dynamics)
運動方程式の補足・演習
２００４．６．１４
講義内容
１．はじめに
２．ベクトルの基礎
３．運動学(Kinematics)
４．動力学（Dynamics)
５．行列の演算と応用(Matrix)
６．軌道計算(Trajectory)
７．ロボットの制御(Control)
８．応用(Application)
動力学(Dynamics)
逆動力学： リンクの位置、速度、加速度から関節トルクを求める。
順動力学： 関節トルクからリンクの加速度を求める。
・逆動力学はロボットの制御に用い、順動力学はロボットの
シミュレーションに用いる。
・ここでは、まず逆動力学について述べ、その結果に基づき
順動力学を展開する方法を述べる。
はじめに例題の３関節(自由度）ロボットを取り上げ、
各リンクの質量が重心に集中している場合を扱い、
一般への展開はその後考える。
運動方程式(Equation of Motion)
F3
F
P3g
Fi , Mi :
M3
 F3
z3
 M3
l3g
M
P2g
l2 g
リンク重心位置
P1g  P1  l1g z1  l1g z1
P2 g  P2  l2 g z2  l1z1  l2 g z2
P3g  P3  l3g z3  l1z1  l2 z2  l3g z3
第 i-1 リンクが
第 i リンクに与える
力とモーメント
ベクトル
P1g
x0
l2
z2
z0
M2
l1
l1g
F2
 M2
 F2
y0
F1
M1
各リンク重心の速度と加速度
P1g  0
P2 g  l2 g2  z2
P1g  P1  l1g z1  l1g z
P3g  l22  z2  l3g3  z3
P3g  P3  l3g z3  l1z1  l2 z2  l3g z3
P2 g  P2  l2 g z2  l1z1  l2 g z2
1g  0
P
2 g  l2 g ( 2  z2  2  (2  z2 ))
P
3g  l2 ( 2  z2  2  (2  z2 ))  l3g ( 3  z3  3  (3  z3 ))
P
回転速度ベクトル
回転加速度ベクトル
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 
3g  g )  0
F3  m3 (P
3g  g )
F3  m3 ( P
3g  g ))  0
M 3  ( P3g  P3 )  (m3 (P
M 3  ( P3g  P3 )  F3
リンク２
2 g  g )  0
F2  F3  m2 ( P
M 2  M 3  ( P3  P2 )  F3
2 g  g ))  0
 ( P2 g  P2 )  (m2 ( P
2 g  g )
F2  F3  m2 ( P
M 2  M 3  ( P3  P2 )  F3
 ( P2 g  P2 )  ( F2  F3 ))
リンク1
F1  F2  0
M1  M 2  ( P2  P1 )  F2  0
関節トルク：
F1  F2
M1  M 2  ( P2  P1 )  F2
T1  z1  M1, T2  y2  M2 , T3  y3  M3
各リンクの運動方程式の一般形
ig  g )
Fi  Fi 1  mi ( P
Mi  Mi 1  ( Pi1  Pi )  Fi1  ( Pig  Pi )  ( Fi  Fi 1 )  Iii  i  ( Iii )
Ii
：慣性テンソル
Ii  X i Yi
 Iix
Zi  0
 0
0
Iiy
0
0
0 X i Yi
Iiz 
Zi T
慣性主軸
Zi
Iix   ( y 2  z 2 )dm Ｘ軸周りの慣性モーメント
 xydm   yzdm
  zxdm  0
となる軸
i
Iiy   ( z 2  x2 )dm
r
i
Iiz   ( x2  y 2 )dm
i
Xi
y
dm
z x
Yi
重心周りの角運動量
X  (  X )  X  (  (Y  Z ))
 X  ((  Z )Y  (  Y )Z )
 (  Z )Z  (  Y )Y
dr
N   r  ( )dm
dt
  ( xX  yY  zZ )  (  ( xX  yY  zZ ))dm
 X  (  X ) x2dm  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 ) x2dm  Y  (  Y ) y 2dm  Z  (  Z ) z 2dm
 ((  Y )Y  (  Z )Z ) x2dm
 (  X ) X  ( y 2  z 2 )dm
 ((  X ) X  (  Z )Z ) y 2dm
 (  Y )Y  ( x2  z 2 )dm
 ((  X ) X  (  Y )Y ) z 2dm
 (  Z )Z  ( x2  y 2 )dm
Ix
Iy
Iz
オイラー(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
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
 I x (  X ) X  I y (  Y )Y  I z (  Z )Z
慣性主軸
Z
z M
z
x
Mx
y
Y
My
X
 I x  X 
 I x  X 
 X Y Z  I y  Y     X   Y   Z  I y  Y 
 I z  Z 
 I z  Z 
I  X Y
I x
Z  0
 0
0
Iy
0
0
0 X Y
I z 
Z T とすると、
M  I    ( I )
慣性モーメントの計算
I x  m(b2  c2 ) / 12
Ｚ
I y  m(a 2  c2 ) / 12
Ｙ
a
dz
dy
c
Ｘ
b
I x   ( y 2  z 2 )dm 
 a
c/2

c / 2
y
3
I z  m(a 2  b2 ) / 12
c/2 b/2

2
2
(
y

z
) adydz

c / 2 b / 2
b/2
3 z y
dz  a
2
b / 2
c/2

3
3
3
c/2
(b 12  bz )dz  a b z 12  bz 3
c / 2
2
c / 2
 a(b3c  bc3 ) / 12  abc(b2  c2 ) / 12  m(b2  c2 ) / 12
慣性モーメントの計算
rd dr x
r 
z
Ｒ
Ix 
l / 2 R 2
  (y
2
 r 2 cos2  ) rddrdy
l / 2 0 0

l / 2 R 2

2
3
2
(
ry

r
cos
 )ddrdy

l / 2 0 0
Ｚ

Ｙ
l / 2 R 2

2
3
(
ry

r

l / 2 0 0
R
Ｘ

l

cos2  1
)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

l /2
 2 ( y r
2 2
l / 2
R
l /2
/ 2  r / 8) dy    ( y R  R / 4)dy   R y / 3  R y / 4 l / 2
4
2
0
2
4
2
l / 2
 R2l (l 2 / 12  R2 / 4)  m(l 2 / 12  R2 / 4)
Iz  Ix
I y  mR2 / 2
3
4
l /2
練習問題
Z
Iy
R
dr
X
を求めよ。
r
R
R
R
I y   r dm   r  2 rldr   2 l  r dr  2 l r / 4 0
2
0
2
0
  r 2l (r 2 / 2)  m(r 2 / 2)
3
0
4
R
逆動力学の計算手順
１)
２）
３）
４）
５）
６）
各リンクの姿勢と重心位置(θ:measured）
各リンク重心の速度・回転速度（:measured）
各リンク重心の加速度・回転加速度（:given）
各リンクの運動方程式
各リンクに加わる力・モーメントベクトル(Fi,Mi）
各関節のトルク（Ti）
１）～３）はベースから手先へ、
４）～５）は手先からベースの向きに漸化式を立てる
順動力学(Forward Dynamics)
順動力学： 関節トルクから自由度の加速度を求める。
ロボットのシミュレーションなどに用いる。
1 
T1 
 :   A :   N ( , , g )
i i
 
 
n 
Tn 
 
1 
T1 
 
1  

A ( :  N (i ,i , g ))   : 
 
n 
Tn 
 
A:
慣性行列
N : 1  0 (i  1 ~ n)
として逆動力
学で求める
問題はＡの定式化！
i  0 (i  1 ~ n)
j k
の時の逆動力学
j n
j
j
Pgj   pk  ( Pgj  Pk )k
j i
k 1
j
gj   pk  ( Pgj  Pk )k
P
1
k 1
Pgj
j
n
M i  m j ( Pgj  Pi )  ( pk  ( Pgj  Pk )k )
j i
n

n
m ( P
j
gj
n
Ti  pi  Mi  
n
j i
 ))

P
)

(
p

(
P

P
)

gj
i
k
gj
i
k
m j (( pk  (Pgj  Pi ))  ( pi  (Pgj  Pi ))k
k 1 j max(i ,k )
Mi
m p  ((P
k 1 j max(i ,k )

Pgi
 Pi )  ( pk  ( Pgj  Pi )k )
n
k
Pgj  Pi
k 1
k 1 j max(i ,k )
n
1
x0
pi
y0
n

 
m
(
p

(
P

P
))

(
p

(
P

P
))
:::
m
(
p

(
P

P
))

(
p

(
P

P
))
j
1
gj
1
1
gj
1
n
n
gn
n
1
gn
1  
1
T1  
j 1
  
: 
:::
:::
:::
  : 
  
Tn   mn ( p1  ( Pgn  P1 ))  ( pn  ( Pgn  Pn )) ::: mn ( pn  ( Pgn  Pn ))  ( pn  ( Pgn  Pn ))  

  n


A
Aik 
慣性行列
n
m ( p
j max(i ,k )
j
k
 ( Pgj  Pk ))  ( pi  ( Pgj  Pi ))
演習問題
z
2  , ,
右の３リンクからなる
2 2 2
水平多関節ロボット
について問い
l2 g
l1z
に答えよ。
2
各リンクの質量
ｍ１、ｍ２、ｍ３は
P2
l
1y
重心に集中して
2
いるものとする。
1 0 1
l1g
提出期限
0
６月２８日（月）
P3
3
3
l3g
y3
x3
x
P0 , P1
3
l2 y
y
zzy
z3  , ,
l2 z
y
x1
 , ,
x0
1
1
1
１．ロボットの位置姿勢xi,yi,zi,Piを求めよ。
ただし、1  2  3  0 のときxi,yi,ziは
基準座標系と一致するものとする。
２．各リンクの回転速度ベクトルと関節位置速度を
求めよ。
３．各リンクの重心の加速度ベクトルを求めよ。
４．各リンクの運動方程式を求めよ。
５．各関節トルクを求めよ。
1
0
 
0
姿勢の算出
x1  cos1  x0  sin 1  y0
y1   sin 1  x0  cos  yo
z1  z0
x1
x2  c2 x1  s2 z1
y2  y1
z2  s2 x1  c2 z1
y3 z3   x0
y1 z1   x0
y0
0
0
 
1 A1
c1  s1 0
z0 s1 c1 0
 0 0 1
A2
x2
x3  c3 x2  s3z2
y3  y2
z3  s3 x2  c3z2
x3
0
1
 
0
x3
y0 z0 A1 A2 A3
xi
yi
y2 z2   x1
y3 z3   x2
zi   x0
y1
 c2 0 s2 
z1  0 1 0 
 s2 0 c2 
A3  c3
y2 z2  0
 s3
0 s3 
1 0 
0 c3 
y0 z0 A1    Ai  A1    Ai