ロボティクスーその来し方行く末
Download
Report
Transcript ロボティクスーその来し方行く末
知能システム論1(8)
動力学(Dynamics)
力と運動方程式
2008.6.10
練習問題
1 0, 2 0, 3 / 2,
1 1 rad / sec, 2 0 , 3 0
θ3
P3
のとき、マニピュレータ先端部の
加速度ベクトルをもとめよ。 リンク3
リンク2
l3
l2
P
θ2
Z2
P2
X2
X0
l1
Y2
Z0,Z1 リンク1
Y1
P1
θ1
X1
Y0
姿勢の算出
A1
x 1 cos 1 x 0 sin 1 y 0
y 1 sin 1 x 0 cos 1 y o
x1
y1
z1 x 0
z1 z 0
y0
単位行列
x 2 c 2 x1 s 2 z1
A2
y 2 y1
x 2
y2
z 2 x1
y1
z 2 s 2 x1 c 2 z1
x 3 c3 x 2 s3 z 2
y3 y2
x 3
y3
z 3 x 2
y2
z3 s3 x 2 c3 z 2
x 3
c1
z 0 s1
0
y3
z 3 x 0
y0
z 0 A1 A2 A3 A1 A2 A3
xi
s1
c1
0
c2
z1 0
s 2
1
A3 c 3
0
z2 0
s 3
yi
0
0
1
0
0
0
1
s2
0
c 2
s3
0
c 3
z i A1 Ai
姿勢の計算 (
x1
x3
y1
y3
1
z1 0
0
z 3 x 2
0 , 2 0 , 3 / 2 rad のとき )
1
0
0
1
0
1
0
y2
0
z2 0
1
x 2
0
1
0
y2
1 0
0 0
0 1
回転速度・加速度の計算
0
1 z 11 0 1
1
0
0
1
0
2 z 11 y 22 0
1
0
3 z 11 y 22 y 33 0
1
z 2 x1
0
1
0
y1
1
z1 0
0
0
1
0
0 1
0 0
1 0
0
1
0
0
0
1
1
0
0
(1 1rad / s , 2 0 , 3 0 , 1 0 , 2 0 , 3 0のとき )
1 z 11 1 z11 z 11 0
2 z11 y 22 2 y 22 0
3 z11 y 22 y 33 2 y 22 3 y 33 0
腕先端Pの加速度の計算
P
d P
l 2 2 z 2 l 2 2 z 2 l 3 3 z 3 l 3 3 z 3
dt
l 2 2 z 2 l 2 2 ( 2 z 2 ) l 3 3 z 3 l 3 3 ( 3 z 3 )
0 0 0
0 0 1
0 0
1
l 2 0 ( 0 0 ) l3 0 ( 0 0 ) l3 0 1 l3 0
1 1 1
1 1 0
1 0
0
1 rad/sec
講義内容
1.はじめに
2.ベクトルの基礎
3.運動学(Kinematics)
4.動力学(Dynamics)
5.行列の演算と応用(Matrix)
6.軌道計算(Trajectory)
7.ロボットの制御(Control)
8.応用(Application)
力のモーメント
大きさ:
方向:
向き:
n
| F || r | sin
n(r と F を含む面に垂直)
右ねじ
r
|r|sinθ
M rF
r F sin n
θ
F
先端部における力と力のモーメントの効果
第 i 関節にかかる力のモーメント:
M ( P Pi ) F
第 i 関節の回転力 Ti :
Ti p i ( M ( P Pi ) F )
p i M ( p i ( P Pi )) F
F
M
P
T1 ( p1 ( P P1 ))
:
:
T
T 6 ( p 6 ( P P6 ))
T
F
T
T J
M
P-Pi
Zi
p
F
:
M
T
p 6
T
1
J
T
Pi
Yi=pi
Z0
X0
P1
Y0
ヤコビアン(係数行列)Jacobian
P
6
p i ( P Pi )i
マニピュレータ先端の並進速度
i 1
6
6
p j j
マニピュレータ先端リンクの回転速度
j 1
上式を1つにまとめると次のようになる
P p 1 ( P P1 )
p1
1
p 6 ( P P6 )
p6
6
先端の速度
例題のヤコビアン
P z 1 ( P P1 )
z1
J:ヤコビアン
y 2 ( P P2 )
y2
1
y 3 ( P P3 )
2
y3
3
自由度変数の
変化速度
例題
0
f
0
l
0
l
M 0 , F 0 , p1 p 2 0 , P l , P1 0 , P2 0
0
0
1
0
0
0
T1 , T 2 : ?
p1
p2
l
T1 ( p1 ( P P1 )) F
0
( 0
1
l
l )
0
P2
f
0 fl
0
P1
y
l
P
0
T 2 ( p 2 ( P P2 )) F ( 0
1
x
z
0
l )
0
f
0 fl
0
f
F 0
0
エネルギー保存の条件
を用いて先端に加わる
関節トルクの関係を導
く
力と
F x x F y y F z z T1 1 T 2 2 T 3 3
F
F
x
x
F
( F
x
J
T
Fy
x
F z y T1
z
Fy
1
F z J 2 T1
3
Fy
F z J T1
x
T2
Fy
x
F
Fy
F z J ) T1
Fz
T1
Fx
T
T J Fy
2
T 3
F z
T
T1
T3,⊿θ3
1
T3 2
3
T2
T3
T2
T
1
T3 2
3
T2
T3
T2
T3
P
T
Fx, Fx, Fx ⊿x, ⊿y, ⊿z
T2,⊿θ2
T
T1,⊿θ1
X0
Z0
P1
Y0
動力学(Dynamics)
逆動力学: リンクの位置、速度、加速度から関節トルクを求める。
順動力学: 関節トルクからリンクの加速度を求める。
・逆動力学はロボットの制御に用い、順動力学はロボットの
シミュレーションに用いる。
・ここでは、主として逆動力学について述べる。順動力学は
逆動力学に基づき展開することができる。
はじめに例題の3関節(自由度)ロボットを取り上げ、
各リンクの質量が重心に集中している場合を扱い、
一般への展開はその後考える。
運動方程式(Equation of Motion)
F3
P3 g
F
M3
Fi , M i :
F3
z3
M3
l3 g
第 i-1 リンクが
第 i リンクに与える
力とモーメント
ベクトル
P2 g
M
l2 g
リンク重心位置
l2
z2
M
2
F2
P1 g P1 l1 g z1 l1 g z1
P1 g
P2 g P2 l 2 g z 2 l1 z1 l 2 g z 2
P3 g P3 l 3 g z 3 l1 z1 l 2 z 2 l 3 g z 3
x0
z0
M2
F2
l1
l1 g
y0
F1
M1
各リンク重心の速度と加速度
P1 g 0
P1 g P1 l1 g z1 l1 g z
P2 g l 2 g 2 z 2
P2 g P2 l 2 g z 2 l1 z1 l 2 g z 2
P3 g l 2 2 z 2 l 3 g 3 z 3
P3 g P3 l 3 g z 3 l1 z1 l 2 z 2 l 3 g z 3
P1 g 0
P2 g l 2 g ( 2 z 2 2 ( 2 z 2 ))
P3 g l 2 ( 2 z 2 2 ( 2 z 2 )) l 3 g ( 3 z 3 3 ( 3 z 3 ))
回転速度ベクトル
回転加速度ベクトル
1 z11
1 z11
2 z11 y 22
2 z11 y 22 ( 2 y 2 )2
3 z11 y 22 y 33
3 z11 y 22 y 33 ( 2 y 2 )2 ( 3 y 3 )3
各リンクの力とモーメントの釣合い
F M 0
リンク3
0
g 0
g
F3 m 3 ( P3 g g ) 0
F3 m 3 ( P3 g g )
M 3 ( P3 g P3 ) ( m 3 ( P3 g g )) 0
M 3 ( P3 g P3 ) F3
リンク2
F2 F3 m 2 ( P2 g g ) 0
M 2 M 3 ( P3 P2 ) F3
( P2 g P2 ) ( m 2 ( P2 g g )) 0
F2 F3 m 2 ( P2 g g )
M 2 M 3 ( P3 P2 ) F3
( P2 g P2 ) ( F2 F3 ))
リンク1
F1 F2 0
F1 F2
M 1 M 2 ( P2 P1 ) F2 0
M 1 M 2 ( P2 P1 ) F2
関節トルク:
T1 z1 M 1 , T2 y 2 M 2 , T3 y 3 M 3
各リンクの運動方程式の一般形
Fi Fi 1 m i ( Pig g )
M i M i 1 ( Pi 1 Pi ) Fi 1 ( Pig Pi ) ( Fi Fi 1 ) I i i i ( I i i )
I i :慣性テンソル
I i X i
I ix
Zi 0
0
Yi
I ix
(y
2
I iy
(z
2
I iz
( x y ) dm
z ) dm
x ) dm
2
0
I iy
0
0
0 X i
I iz
Yi
Zi
T
慣性主軸
Zi
X軸周りの慣性モーメント
xydm
zxdm
i
yzdm
0
となる軸
r
2
dm
z x
i
2
2
y
i
Xi
Yi
重心周りの角運動量
N
dr
r ( dt ) dm
( xX
yY zZ ) ( ( xX yY zZ )) dm
X ( X ) x dm X ( Y ) xydm X ( Z ) xzdm
2
Y ( X ) xydm Y ( Y ) y dm Y ( Z ) yzdm
2
Z ( X ) xzdm Z ( Y ) yzdm Z ( Z ) z dm
2
慣性乗積がゼロのX,Y,Z軸を選ぶと、
N X ( X ) x dm Y ( Y ) y dm Z ( Z ) z dm
2
2
2
(( Y )Y ( Z ) Z ) x dm
( X ) X
(( X ) X ( Z ) Z ) y dm
( Y )Y
( x z ) dm
Iy
(( X ) X ( Y )Y ) z dm
( Z ) Z ( x y ) dm
Iz
2
2
2
( y z ) dm
2
2
2
2
2
2
Ix
オイラー(Euler)方程式(ベクトル表現)
角運動量の時間微分が力のモーメント
N I x ( X ) X I y ( Y )Y I z ( Z ) Z
M
dN
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
X Y Z 0
I x ( X ) X I y ( Y )Y I z ( Z ) Z
X X
I x ( X ) X I y ( Y )Y I z ( Z ) Z
Y Y
( I x ( X ) X I y ( Y )Y I z ( Z ) Z )
Z Z
X
Y
I x ( X )
Z I y ( Y ) X
I z ( Z )
Y
I x ( X )
Z I y ( Y )
I z ( Z )
X
X
Y
Y
I X
Ix
Z 0
0
Ix
Z 0
0
Y
Ix
Z 0
0
0 X
0 Y
I z Z
0
Iy
0
0 X
T
0 Y
T
I z Z
T
0
Iy
0
0
Iy
0
0
0 X
I z
X
X
Y
M I ( I )
Z
T
Y
Y
Ix
Z 0
0
Ix
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 ( b c ) / 12
2
Z
I y m ( a c ) / 12
2
Y
X
dz
a
dy
c
2
b
( y z ) dm
2
y
c / 2
3
( y z ) adydz
2
2
c / 2 b / 2
c/2
dz a
z y
2
3
2
b/2
b/2
c/2
a
2
2
I z m ( a b ) / 12
c/2
Ix
2
(b
c / 2
b / 2
3
3
bz ) dz a b z
2
12
a ( b c bc ) / 12 abc ( b c ) / 12 m ( b c ) / 12
3
3
2
2
2
2
12
bz
c/2
3
3
c / 2
慣性モーメントの計算
x
rd dr
l / 2 R 2
r sin
r
Ix
( y r sin ) rd drdy
2
2
2
l / 2 0 0
l / 2 R 2
z
R
Z
( ry r sin ) d drdy
2
3
2
l / 2 0 0
X
l / 2 R 2
Y
( ry r
2
3
1 cos 2
2
l / 2 0 0
R
) d drdy
2
l/2 R
l
( ry r
2
3
2 ) r ( sin 2 ) 4
3
l / 2 0
drdy
0
l/2 R
2 ( ry r
2
3
2 )drdy
l / 2 0
R
l/2
2 ( y
2
l/2
r / 2 r / 8 ) dy
2
4
l / 2
( y
2
R R / 4 ) dy R y / 3 R y / 4
2
0
2
2
2
l / 2
R l ( l / 12 R / 4 ) m ( l / 12 R / 4 )
2
4
2
2
Iz Ix
I y mR / 2
2
3
4
l/2
l / 2
逆動力学の計算手順
1)
2)
3)
4)
5)
6)
各リンクの姿勢と重心位置(θ:given)
各リンク重心の速度・回転速度( :given)
各リンク重心の加速度・回転加速度( :given)
各リンクの運動方程式
各関節に加わる力・モーメントベクトル(Fi,Mi)
各関節のトルク(Ti)
1)~3)はベースから手先へ、
4)~6)は手先からベースの向きに漸化式を立てる
オイラー(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
基準座標系でのオイラー方程式
M dN
dt
I ( I )
x
X
Mx
z
y
Y
M
y
オイラー(Euler)方程式
M dN
I X
X
Y
dt
Y
I ( I )
I x
Z 0
0
I x
Z 0
0
0
Iy
0
0
Iy
0
0
0 X
I z
Y
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
T
0 x
0 y I x x X I y y Y I z z Z
I z z
( I ) ( I x x X I y y Y I z z Z )
( x X y Y z Z ) ( I x x X I y y Y 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 z Y ( I y I x ) x y Z
M I x x X I y y Y I z z Z
( I z I y ) y z X ( I x I z ) x z Y ( 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