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

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

Download Report