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

知能システム論１（９）
動力学(Dynamics)
運動方程式のまとめ
２００８．６．１７
講義内容
１．はじめに
２．ベクトルの基礎
３．運動学(Kinematics)
４．動力学（Dynamics)まとめ
５．行列の演算と応用(Matrix)
６．軌道計算(Trajectory)
７．ロボットの制御(Control)
８．応用(Application)
力のモーメント（回転力）
大きさ：
方向：
向き：
ｎ
| F || r | sin
ｎ（ｒ と F を含む面に垂直）
右ねじ
r
|r|sinθ
θ
F
M  rF
 r F sin  n
先端部における力と力のモーメントの効果
第 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:ヤコビアン
例題のヤコビアン
1 
 P   z1  ( P  P1 ) y2  ( P  P2 ) y3  ( P  P3 )  
2 
 


z1
y2
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


F J )  T
1
T2 T3 T
  T
1
T2 T3 T
Fz J  T1 T2 T3 
Fy
Fy
T
z
Fz
T
P
Fx, Fx, Fx ⊿x, ⊿y, ⊿z
Ｔ2，⊿θ2
Ｔ１，⊿θ1
T1 
 Fx 
T   J T F 
 2
 y
T3 
 Fz 
X０
Z0
P1
Y0
動力学(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 z1
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
：慣性テンソル
 Iix
Zi  0
 0
Ii  X i Yi
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  x )dm
2
2
i
Iiz   ( x2  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 
I  X Y Z  0 I y 0 X Y Z T 
 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
z
 I x (  X )
Z  I y (  Y )     X Y


 I z (  Z ) 
 I x (  X )
Z  I y (  Y ) 


 I z (  Z ) 
 X Y
 X Y
I x
Z  0

 0
I x
Z  0
 0
I  X Y
I x
Z  0
 0
0
Iy
0
0   X 
0    Y     X Y


I z   Z 
0
Iy
0
0  X T 
 
0  Y T     X Y
I z  Z T 
0
Iy
0
0
0 X Y
I z 
M  I    ( I )
Z T
I x
Z  0

 0
I x
Z  0
 0
とすると。
0
Iy
0
0   X 
0   Y 


I z   Z 
0
Iy
0
0  X T 
 
0  Y T 
I z  Z T 
重心周りの角運動量
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
慣性モーメントの計算
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
z
x
r sin 
Ix 
r 
l / 2 R 2

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

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
逆動力学の計算手順
１)
２）
３）
４）
５）
６）
各リンクの姿勢と重心位置(θ:given）
各リンク重心の速度・回転速度（:given）
各リンク重心の加速度・回転加速度（:given）
各リンクの運動方程式
各関節に加わる力・モーメントベクトル(Fi,Mi）
各関節のトルク（Ti）
１）～３）はベースから手先へ、
４）～６）は手先からベースの向きに漸化式を立てる
演習問題
z
2  , ,
右の２リンクからなる
2 2 2
水平多関節ロボット
について問い
l2 g
l1z
に答えよ。
2
各リンクの質量
ｍ１、ｍ２は
P2
l
1y
重心に集中して
2 F2 , M 2
いるものとする。
1 0 1
l1g
レポート提出期限
0
７月１日（火）
F M 0
y
x
zzy
P0 , P1
F1 , M1
y
１．ロボットの位置姿勢xi,yi,zi,Piを求めよ。
ただし、 1  2  0
のときxi,yi,ziは
1



1,1,1 基準座標系x0,y0.z0と一致するものとする。
0
２．各リンクの回転速度ベクトルと関節位置
速度を求めよ。
３．各リンクの重心の加速度ベクトルを求めよ。
４．各リンクの運動方程式を求めよ。
５．各関節トルクを求めよ。
x
x
１．
x1  cos1 x0  sin 1 y0
y1   sin 1  cos1 y0
z1  z0
２．
1  1z0
2  (1  2 ) z0
x2  cos2 x1  sin 2 y1
x2  cos2 x1  sin 2 y1
P1  P0  0
P2  l1g y1  l1z z2
z2  z0
1  1z0
 2  (1  2 ) z0
P1  0
P2  l1y y1  l1z z2  l1y1  y1  l1y1 z0  y1  l1y1 x1
３．
P1g  P1  l1g y1  l1g1 x1
P2 g  P2  l2 g y 2  l1y1 x1  l2 g2  y2  l1y1 x1  l2 g (1  2 ) z0  y2  l1y1x1  l2 g (1  2 ) x2
1g  l1g (1 x1  1 x1 )  l1g (1 x1  11  x1 )  l1g (1 x1  12 y1 )
P
2 g  l1y (1 x1  1 x1 )  l2 g ((1  2 ) x2  (1  2 ) x2 )  l1y (1 x1  12 y1 )  l2 g ((1  2 ) x2  (1  2 )2 y2 )
P
４．
順動力学(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jg   pk  ( Pjg  Pk )k
j i
k 1
j
jg   pk  ( Pjg  Pk )k
P
1
k 1
Pjg
j
n
M i  m j ( Pjg  Pi )  ( pk  ( Pjg  Pk )k )
j i
n

n
m ( P
j
jg
n
Ti  pi  M i  
n
j i

jg  Pi )  ( pk  ( Pjg  Pk ) k ))
 m j (( pk  (Pjg  Pk ))  ( pi  (Pjg  Pi ))k
k 1 j max(i ,k )
Mi
 m p  ((P
k 1 j max(i ,k )

Pgi
 Pi )  ( pk  ( Pjg  Pk )k )
n
k
Pjg  Pi
k 1
k 1 j max(i ,k )
n
1
x0
pi
y0
上の式展開での
n
j

 記号の順序入換
n  6, i  4
のとき
j i k 1
k 1 j  4 ~ 6
k 2 j4~6
k 3 j 4~6
k 4 j4~6
k 5 j 5~6
k 6 j 6
j  4 k 1~ 4
j  5 k 1~ 5
j  6 k 1~ 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  
1
T1  
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 ))