ロボティクスーその来し方行く末
Download
Report
Transcript ロボティクスーその来し方行く末
知能システム論1(9)
動力学(Dynamics)
運動方程式のまとめ
2008.6.17
講義内容
1.はじめに
2.ベクトルの基礎
3.運動学(Kinematics)
4.動力学(Dynamics)まとめ
5.行列の演算と応用(Matrix)
6.軌道計算(Trajectory)
7.ロボットの制御(Control)
8.応用(Application)
力のモーメント(回転力)
大きさ:
方向:
向き:
n
| F || r | sin
n(r と F を含む面に垂直)
右ねじ
r
|r|sinθ
θ
F
M rF
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
TF
T J
M
p
F
:
T M
p6
P-Pi
Zi
T
1
J
T
Pi
Yi=pi
Z0
X0
P1
Y0
ヤコビアン(係数行列)Jacobian
6
P pi ( P Pi )i
マニピュレータ先端の並進速度
i 1
6
6 p jj
j 1
マニピュレータ先端リンクの回転速度
上式を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 T11 T22 T33
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
T3,⊿θ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
T2,⊿θ2
T1,⊿θ1
T1
Fx
T J T F
2
y
T3
Fz
X0
Z0
P1
Y0
動力学(Dynamics)
逆動力学: リンクの位置、速度、加速度から関節トルクを求める。
順動力学: 関節トルクからリンクの加速度を求める。
・逆動力学はロボットの制御に用い、順動力学はロボットの
シミュレーションに用いる。
・ここでは、主として逆動力学について述べる。順動力学は
逆動力学に基づき展開することができる。
はじめに例題の3関節(自由度)ロボットを取り上げ、
各リンクの質量が重心に集中している場合を扱い、
一般への展開はその後考える。
運動方程式(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
各リンク重心の速度と加速度
P1g 0
P2 g l2 g2 z2
P1g P1 l1g z1 l1g z1
P3g l22 z2 l3g3 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 z11
1 z11
2 z11 y22
2 z11 y22 (2 y2 )2
3 z11 y22 y33 3 z11 y22 y33 (2 y2 )2 (3 y3 )3
各リンクの力とモーメントの釣合い
F M 0
リンク3
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
関節3での力モーメントの釣合い
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 ( Pi1 Pi ) Fi1 ( Pig Pi ) ( Fi Fi 1 ) Iii i ( Iii )
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 X軸周りの慣性モーメント
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 ) yz
M y I y y ( I x I z )zx
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 ) yz
M y I y y ( I x I z )zx
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 xx X I y yY I zz Z )
(x X yY z Z ) ( I xx X I y yY I zz Z )
I yx y Z I zxzY I xx y Z I z yz X I xxzY I y yz X
( I z I y ) yz X ( I x I z )xzY ( I y I x )x y Z
M I x x X I y yY I z z Z
( I z I y )yz X ( I x I z )xzY ( I y I x )xy Z
( I x x ( I z I y )yz ) X ( I y y ( I x I z )xz )Y ( I z z ( I y I x )xy )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
慣性乗積がゼロのX,Y,Z軸を選ぶと、
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
Z
I y m(a 2 c2 ) / 12
Y
a
dz
dy
c
X
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
) rddrdy
l / 2 0 0
R
Z
l / 2 R 2
2
3
2
(
ry
r
sin
)ddrdy
l / 2 0 0
X
Y
l / 2 R 2
2
3
(
ry
r
l / 2 0 0
R
l
1 cos 2
)ddrdy
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
逆動力学の計算手順
1)
2)
3)
4)
5)
6)
各リンクの姿勢と重心位置(θ:given)
各リンク重心の速度・回転速度(:given)
各リンク重心の加速度・回転加速度(:given)
各リンクの運動方程式
各関節に加わる力・モーメントベクトル(Fi,Mi)
各関節のトルク(Ti)
1)~3)はベースから手先へ、
4)~6)は手先からベースの向きに漸化式を立てる
演習問題
z
2 , ,
右の2リンクからなる
2 2 2
水平多関節ロボット
について問い
l2 g
l1z
に答えよ。
2
各リンクの質量
m1、m2は
P2
l
1y
重心に集中して
2 F2 , M 2
いるものとする。
1 0 1
l1g
レポート提出期限
0
7月1日(火)
F M 0
y
x
zzy
P0 , P1
F1 , M1
y
1.ロボットの位置姿勢xi,yi,zi,Piを求めよ。
ただし、 1 2 0
のときxi,yi,ziは
1
1,1,1 基準座標系x0,y0.z0と一致するものとする。
0
2.各リンクの回転速度ベクトルと関節位置
速度を求めよ。
3.各リンクの重心の加速度ベクトルを求めよ。
4.各リンクの運動方程式を求めよ。
5.各関節トルクを求めよ。
x
x
1.
x1 cos1 x0 sin 1 y0
y1 sin 1 cos1 y0
z1 z0
2.
1 1z0
2 (1 2 ) z0
x2 cos2 x1 sin 2 y1
x2 cos2 x1 sin 2 y1
P1 P0 0
P2 l1g y1 l1z z2
z2 z0
1 1z0
2 (1 2 ) z0
P1 0
P2 l1y y1 l1z z2 l1y1 y1 l1y1 z0 y1 l1y1 x1
3.
P1g P1 l1g y1 l1g1 x1
P2 g P2 l2 g y 2 l1y1 x1 l2 g2 y2 l1y1 x1 l2 g (1 2 ) z0 y2 l1y1x1 l2 g (1 2 ) x2
1g l1g (1 x1 1 x1 ) l1g (1 x1 11 x1 ) l1g (1 x1 12 y1 )
P
2 g l1y (1 x1 1 x1 ) l2 g ((1 2 ) x2 (1 2 ) x2 ) l1y (1 x1 12 y1 ) l2 g ((1 2 ) x2 (1 2 )2 y2 )
P
4.
順動力学(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)
として逆動力
学で求める
問題はAの定式化!
i 0 (i 1 ~ n)
j k
の時の逆動力学
j n
j
j
Pjg 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 j4~6
k 3 j 4~6
k 4 j4~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 ))