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

知能システム論１（９）
動力学(Dynamics)
運動方程式のまとめ
２００８．６．１７
講義内容
１．はじめに
２．ベクトルの基礎
３．運動学(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
自由度変数の
変化速度
エネルギー保存の条件を用いて先端に加わる力と
関節トルクの関係を導く
Fx x  Fy y  Fz z  T11  T2  2  T3  3
F
 x 
 1 
Fz y   T1 T2 T3  2 
 z 
  3 
 1 
 1 
Fz J  2   T1 T2 T3  2 
  3 
  3 

Fy
x
F
Fy
F
(F
Fy
x
x
x

J T Fx
Ｔ3，⊿θ3


Fz J  T1 T2 T3 
Fy
Fy

Fz J )T  T1 T2 T3 
Fz
P
T
  T
T
1
Fx, Fx, Fx ⊿x, ⊿y, ⊿z
Ｔ2，⊿θ2
T2 T3 
T
Ｔ１，⊿θ1
T1 
 Fx 
T   J T  F 
 2
 y
T3 
 Fz 
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 z1
P2 g  P2  l2 g z 2  l1 z1  l2 g z 2
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
2
i
I iz   ( x 2  y 2 )dm
リンク
i
i
Xi
となる軸
r
y
dm
z x
Yi
オイラー(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  I    ( I)
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
オイラー(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 
重心周りの角運動量
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
慣性モーメントの計算
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）
１）～３）はベースから手先へ、
４）～６）は手先からベースの向きに漸化式を立てる
演習問題
右の２リンクからなる
水平多関節ロボット
について問い
l1z
に答えよ。
各リンクの質量
ｍ１、ｍ２は
重心に集中して
いるものとする。
レポート提出期限
７月１日（火）
z2
F1 , M 1
F M 0
l2 g
y2
l1 y
z1 z0 y1
P0 , P1
2 ,2 ,2
P2
x2
F2 , M 2
l1g
y0
１．ロボットの位置姿勢xi,yi,zi,Piを求めよ。
1
ただし、 1   2  0
のときxi,yi,ziは



1,1,1 基準座標系x0,y0.z0と一致するものとする。
0
２．各リンクの回転速度ベクトルと関節位置
速度を求めよ。
３．各リンクの重心の加速度ベクトルを求めよ。
４．各リンクの運動方程式を求めよ。
５．各関節トルクを求めよ。
x
x
１．
x1  cos1 x0  sin 1 y0
x2  cos 2 x1  sin  2 y1
P1  P0  0
y1   sin 1  cos1 y0
x2  cos 2 x1  sin  2 y1
P2  l1g y1  l1z z 2
z1  z0
z 2  z0
２．
1  1 z0
2  (1  2 ) z0
1  1 z0
 2  (1  2 ) z0
P1  0
P2  l1 y y1  l1z z2  l1 y1  y1  l1 y1 z0  y1  l1 y1 x1
３．
P1g  P1  l1g y1  l1g1 x1
P2 g  P2  l2 g y 2  l1 y1 x1  l2 g 2  y2  l1 y1 x1  l2 g (1  2 ) z0  y2  l1 y1 x1  l2 g (1  2 ) x2
  l ( x   x )  l ( x     x )  l ( x   2 y )
P
1g
1g
1 1
1 1
1g
1 1
1 1
1
1g
1 1
1 1
  l ( x   x )  l ((   ) x  (   ) x )  l ( x   2 y )  l ((   ) x  (   ) 2 y )
P
2g
1y
1 1
1 1
2g
1
2
2
1
2
2
1y
1 1
1 1
2g
1
2
2
1
2
2
４．
順動力学(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jg   pk  ( Pjg  Pk )k
ji
k 1
j
   p  ( P  P )
P
jg
k
jg
k
k
1
k 1
Pjg
j
n
M i   m j ( Pjg  Pi )  (  pk  ( Pjg  Pk )k )
j i
n

1
Pjg  Pi
Pgi
k 1
n
 m (P
k 1 j  max(i ,k )
j
jg
n
Ti  pi  M i  
k
 Pi )  ( pk  ( Pjg  Pk )k )
Mi
n
 m j pi  ((Pjg  Pi )  ( pk  ( Pjg  Pk )k ))
pi
k 1 j  max(i , k )
n

n
 m j (( pk  ( Pjg  Pk ))  ( pi  ( Pjg  Pi ))k
k 1 j  max(i , k )
x0
y0
上の式展開での
n
j

 記号の順序入換
n  6, i  4
k 1 j  4 ~ 6
k 2 j4~6
のとき j  4
j i k 1
k 1~ 4
j  5 k 1~ 5
j  6 k 1~ 6
k 3
k 4
j4~6
j4~6
k 5
k 6
j 5~6
j6
n
j
 
k 1 j  max(i ,k )
スカラー三重積の性質を用いた式変形
m j pi  ((Pjg  Pi )  ( pk  ( Pjg  Pk )k )) m j (( pk  ( Pjg  Pk ))  ( pi  ( Pjg  Pi ))k
A
B
C
A  ( B  C )  C  ( A  B)
C
A
B
n

 
m
(
p

(
P

P
))

(
p

(
P

P
))
:::
m
(
p

(
P

P
))

(
p

(
P

P
))
j
1
jg
1
1
jg
1
n
n
ng
n
1
ng
1  
 T1  
1
j 1
  
:
:::
:::
:::
  : 
  
Tn   mn ( p1  ( Png  P1 ))  ( pn  ( Png  Pn )) ::: mn ( pn  ( Png  Pn ))  ( pn  ( Png  Pn ))  

  n


A
Aik 
慣性行列
n
m ( p
j  max(i ,k )
j
k
 ( Pjg  Pk ))  ( pi  ( Pjg  Pi ))

Download Report