manipulator dynamicsw3.uch.edu.tw/control/download/5運動學.pdf · manipulator dynamics...
TRANSCRIPT
1
Manipulator Dynamics
Introduction to ROBOTICS
2
Outline
• Homework Highlight• Review
– Kinematics Model– Jacobian Matrix– Trajectory Planning
• Dynamic Model– Langrange-Euler Equation– Examples
3
Homework highlight
• Composite Homogeneous Transformation Matrix Rules: – Transformation (rotation/translation) w.r.t.
(X,Y,Z) (OLD FRAME), using pre-multiplication
– Transformation (rotation/translation) w.r.t. (U,V,W) (NEW FRAME), using post-multiplication
4
Homework Highlight• Homogeneous Representation
– A frame in space3R
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
=⎥⎦
⎤⎢⎣
⎡=
10001000 zzzz
yyyy
xxxx
pasnpasnpasn
PasnF
x
y
z),,( zyx pppP
ns
a
(X’)
(y’)(z’)
5
Homework Highlight– Assign to complete the right-
handed coordinate system.– The hand coordinate frame is specified by the
geometry of tool. Normally, establish Zn along the direction of Zn-1 axis and pointing away from the robot; establish Xn so that it is normal to both Zn-1and Zn. Assign Yn to complete the right-handed system.
iiiii XZXZY ××+= /)(
nO
a0 a1
Z0
X0
Y0
Z3
X2
Y1
X1
Y2
d2
Z1
X33O
2O1O0O
Z2
Joint 1
Joint 2
Joint 3
6
Review
• Steps to derive kinematics model:– Assign D-H coordinates frames– Find link parameters– Transformation matrices of adjacent joints– Calculate kinematics matrix– When necessary, Euler angle representation
7
Review• D-H transformation matrix for adjacent coordinate
frames, i and i-1.– The position and orientation of the i-th frame coordinate
can be expressed in the (i-1)th frame by the following 4 successive elementary transformations:
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡−
−
=
= −−−
10000
),(),(),(),( 111
iii
iiiiiii
iiiiiii
iiiiiiiii
i
dCSSaCSCCSCaSSSCC
xRaxTzRdzTT
ααθθαθαθθθαθαθ
αθ
8
Review • Kinematics Equations
– chain product of successive coordinate transformation matrices of
– specifies the location of the n-th coordinate frame w.r.t. the base coordinate system
⎥⎦
⎤⎢⎣
⎡=⎥
⎦
⎤⎢⎣
⎡=
= −
100010000
12
11
00
nnn
nn
n
PasnPR
TTTT K
iiT 1−
nT0
Orientation matrix
Position vector
9
Jacobian Matrix
⎥⎥⎥⎥⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢⎢⎢⎢⎢
⎣
⎡
6
5
4
3
2
1
θθθθθθ
⎥⎥⎥⎥⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢⎢⎢⎢⎢
⎣
⎡
6
5
4
3
2
1
θθθθθθ
&
&
&
&
&
&
⎥⎥⎥⎥⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢⎢⎢⎢⎢
⎣
⎡
ϕθφzyx
Joint Space Task Space
Forward
Inverse
Kinematics
Jaconian Matrix: Relationship between joint space velocity with task space velocity
⎥⎥⎥⎥⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢⎢⎢⎢⎢
⎣
⎡
z
y
x
zyx
ω
ωω&
&
&
JacobianMatrix
10
Jacobian Matrix
⎥⎥⎥⎥⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢⎢⎢⎢⎢
⎣
⎡
z
y
x
zyx
ω
ωω&
&
&
1
2
1
6
)(
×
× ⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
⎥⎦
⎤⎢⎣
⎡=
nn
n
q
dqqdh
&
M
&
&
nn
n
n
n
qh
qh
qh
qh
qh
qh
qh
qh
qh
dqqdhJ
×
×
⎥⎥⎥⎥⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢⎢⎢⎢⎢
⎣
⎡
∂∂
∂∂
∂∂
∂∂
∂∂
∂∂
∂∂
∂∂
∂∂
=⎟⎟⎠
⎞⎜⎜⎝
⎛=
6
6
2
6
1
6
2
2
2
1
2
1
2
1
1
1
6
)(
L
MMMM
L
L
Jacobian is a function of q, it is not a constant!
11
Jacobian Matrix• Inverse Jacobian
• Singularity– rank(J)<min{6,n}, – Jacobian Matrix is less than full rank– Jacobian is non-invertable– Occurs when two or more of the axes of the robot
form a straight line, i.e., collinear– Avoid it
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
==
666261
262221
161211
JJJ
JJJJJJ
qJY
L
MMMM
L
L
&&
⎥⎥⎥⎥⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢⎢⎢⎢⎢
⎣
⎡
6
5
4
3
2
1
qqqqqq
&
&
&
&
&
&
YJq && 1−=
5q
1q
12
Trajectory Planning• Trajectory planning,
– “interpolate” or “approximate” the desired path by a class of polynomial functions and generates a sequence of time-based “control set points” for the control of manipulator from the initial configuration to its destination.
– Requirements: Smoothness, continuity
– Piece-wise polynomial interpolate– 4-3-4 trajectory
30312
323
334
343
20212
223
232
10112
123
134
141
)(
)(
)(
atatatatath
atatatath
atatatatath
++++=
+++=
++++=
13
Manipulator Dynamics• Mathematical equations describing the
dynamic behavior of the manipulator– For computer simulation– Design of suitable controller– Evaluation of robot structure
– Joint torques Robot motion, i.e. acceleration, velocity, position
14
Manipulator Dynamics• Lagrange-Euler Formulation
– Lagrange function is defined
• K: Total kinetic energy of robot• P: Total potential energy of robot• : Joint variable of i-th joint• : first time derivative of • : Generalized force (torque) at i-th joint
iq
PKL −=
iii q
LqL
dtd τ=
∂∂
−∂∂ )(&
iq& iqiτ
15
Manipulator Dynamics• Kinetic energy
– Single particle: – Rigid body in 3-D space with linear velocity (V) , and
angular velocity ( ) about the center of mass
– I : Inertia Tensor: • Diagonal terms: moments of inertia• Off-diagonal terms: products of inertia
2
21 mvk =
ωωTT IVmVk21
21
+=
ω
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
+−−
−+−
−−+
≡
∫∫∫∫∫∫∫∫∫
dmyxyzdmxzdm
yzdmdmzxxydm
xzdmxydmdmzy
I
)(
)(
)(
22
22
22
∫ += dmzyI xx )( 22
∫= dmxyI xy )(
16
Velocity of a link
iir
ix
iyiz
0x
0z
0yir0
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
=
1i
i
i
ii z
yx
r
ii
ii
ii
ii rTTTrTr )( 12
11
000 −== L
A point fixed in link i and expressed w.r.t. the i-th frame
Same point w.r.t the base frame
17
Velocity of a link
ii
i
jj
j
ii
iii
ii
i
ii
ii
ii
ii
ii
ii
iii
rqqTrTrTTT
rTTTrTTT
rTTTdtdr
dtdVV
)(
)(
1
001
21
10
12
11
012
11
0
12
11
000
∑=
−
−−
−
∂∂
=++
++=
==≡
&&&LL
L&L&
L
0=iir&
Velocity of point expressed w.r.t. i-th frame is zeroiir
Velocity of point expressed w.r.t. base frame is:i
ir
18
Velocity of a link
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡−
−
=−
100001
iii
iiiiiii
iiiiiii
ii dCS
SaCSCCSCaSSSCC
Tαα
θθαθαθθθαθαθ
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡−
−−−
=∂∂ −
10000000
1 iiiiiii
iiiiiii
i
ii CaSSSCC
SaCSCCS
qT θθαθαθ
θθαθαθ
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡−
−
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡ −
==∂∂
−−
10000
0000000000010010
11
iii
iiiiiii
iiiiiii
iii
i
ii
dCSSaCSCCSCaSSSCC
TQq
Tαα
θθαθαθθθαθαθ
• Rotary joints, iiq θ=
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡ −
=
0000000000010010
iQ
19
Velocity of a link
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡−
−
=−
100001
iii
iiiiiii
iiiiiii
ii dCS
SaCSCCSCaSSSCC
Tαα
θθαθαθθθαθαθ
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
=∂∂ −
0000100000000000
1
i
ii
qT
iii
i
ii TQq
T1
1−
− =∂∂
• Prismatic joint, ii dq =
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
=
0000100000000000
iQ
20
Velocity of a link
⎩⎨⎧
>≤
=∂∂ −−
−−
ijforijforTTQTTT
qT i
ij
jjj
j
j
i
011
12
21
100 LL
⎩⎨⎧
>≤
=∂∂
≡ −−
ijforijforTQT
qTU
ijj
j
j
i
ij 01
100
The effect of the motion of joint j on all the points on link i
∑∑==
− =∂∂
===≡i
j
iijij
ii
i
jj
j
ii
ii
iii
i rqUrqqTrTTT
dtdr
dtdVV
11
01
21
1000 )()()( &&L
21
Kinetic energy of link i
⎥⎦
⎤⎢⎣
⎡=
⎥⎦
⎤⎢⎣
⎡=
⎥⎦
⎤⎢⎣
⎡=
=++=
∑∑
∑∑
∑ ∑
= =
= =
= =
i
prp
Tir
Tii
iiip
i
r
i
prp
Tir
Tii
iiip
i
r
i
p
i
r
Tiirir
iipip
Tiiiiii
qqUdmrrUTr
dmqqUrrUTr
dmrqUrqUTr
dmVVtracedmzyxdK
1 1
1 1
1 1
222
)(21
21
)(21
)(21)(
21
&&
&&
&&
&&&
∑=
≡n
iiiaATr
1)(
• Kinetic energy of a particle with differential mass dm in link i
22
Kinetic energy of link i⎥⎦
⎤⎢⎣
⎡== ∑ ∫∑∫
= =
i
prp
Tir
Tii
iiip
i
rii qqUdmrrUTrdKK
1 1)(
21
&&
⎥⎥⎥⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢⎢⎢⎢
⎣
⎡
−+
+−
++−
=
⎥⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢⎢
⎣
⎡
== ∫
∫∫∫∫∫∫∫∫∫∫∫∫∫∫∫∫
iiiiiii
iizzyyxx
yzxz
iiyzzzyyxx
xy
iixzxyzzyyxx
iii
iiiiii
iiiiii
iiiiii
Tii
iii
mzmymxm
zmIII
II
ymIIII
I
xmIIIII
dmdmzdmydmx
dmzdmzdmzydmzx
dmydmzydmydmyx
dmxdmzxdmyxdmx
dmrrJ
2
2
2
2
2
2
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
=
1i
i
i
ii z
yx
r
Center of mass
∫= dmxm
x ii
i1
Pseudo-inertia matrix of link i
23
Manipulator Dynamics
[ ]∑∑∑
∑ ∫∑∑∑
= = =
= ===
=
⎥⎦
⎤⎢⎣
⎡==
n
i
i
p
i
rrp
Tiriip
i
prp
Tir
Tii
iiip
i
r
n
i
n
ii
qqUJUTr
qqUdmrrUTrKK
1 1 1
1 111
)(21
)(21
&&
&&
iJ
iq
• Total kinetic energy of a robot arm
: Pseudo-inertia matrix of link i, dependent on the mass distribution of link i and are expressed w.r.t. the i-th frame,
Need to be computed once for evaluating the kinetic energy
Scalar quantity, function of and ,iq& ni L,2,1=
24
Manipulator Dynamics
)( 00i
ii
ii
ii rTgmrgmP −=−=
∑ ∑= =
−==n
i
n
i
ii
iii rTgmPP
1 10 ])([
)0,,,( zyx gggg =
• Potential energy of link i
: gravity row vector expressed in base frame2sec/8.9 mg =
ir0
iir : Center of mass w.r.t. i-th frame
: Center of mass w.r.t. base frame
• Potential energy of a robot arm
g
Function of iq
25
Manipulator Dynamics• Lagrangian function
[ ]∑∑∑ ∑= = = =
+=−=n
i
i
j
i
k
n
i
ii
iikj
Tikiij rTgmqqUJUTrPKL
1 1 1 10 )()(
21
&&
jjji
n
ijj
n
ij
j
k
j
mmk
Tjij
m
jkn
ij
j
kk
Tjijjk
iii
rUgm
qqUJqU
TrqUJUTr
qL
qL
dtd
∑
∑∑∑∑∑
=
= = == =
−
∂
∂+=
∂∂
−∂∂
=
1 11
)()(
)(
&&&&
&τ
26
Manipulator Dynamics
⎪⎩
⎪⎨
⎧
<<≥≥≥≥
=≡∂
∂−
−−
−−
−−
−
kiorjikjiTQTQTjkiTQTQT
UqU i
jjj
kkk
ikk
kjj
j
ijkk
ij
01
11
10
11
11
0
The interaction effects of the motion of joint j and joint k on all the points on link i
⎩⎨⎧
>≤
=∂∂
≡ −−
ijforijforTQT
qTU
ijj
j
j
i
ij 01
100
The effect of the motion of joint j on all the points on link i
27
Manipulator Dynamics
im
n
k
n
mkikm
n
kkiki CqqhqD ++= ∑∑∑
= ==
&&&&1 11
τ
∑=
=n
kij
Tjijjkik UJUTrD
),max(
)(
∑=
=n
mkij
Tjijjkmikm UJUTrh
),,max()(
• Dynamics Model
jjji
n
ijji rUgmC ∑
=
−=
28
Manipulator Dynamics
)(),()( qCqqhqqD ++= &&&τ
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡=
nnn
n
DD
DDD
L
M
L
1
111
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡=
nh
hqqh M&
1
),(
• Dynamics Model of n-link Arm
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡=
nC
CqC M
1
)(
The Acceleration-related Inertia matrix term, Symmetric
The Coriolis and Centrifugal terms
The Gravity terms⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡=
nτ
ττ M
1Driving torque applied on each link
29
Example
X0
Y0
X1Y1
1θ
L
m
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
=
1001
1
l
r
Example: One joint arm with point mass (m) concentrated at the end of the arm, link length is l , find the dynamic model of the robot using L-E method.
Set up coordinate frame as in the figure
]0,0,8.9,0[ −=g
11
11
11
11
10
10
100001000000
rCSSC
rTr
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡ −
==θθθθ
30
Example
X0
Y0
X1Y1
1θ
L
m
θθθ
&&
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡⋅⋅−
====
00
1
1
11
101
11
10
101
ClSl
rTQrTdtdrV
11
11
11
11
10
10
100001000000
rCSSC
rTr
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡ −
==θθθθ
31
Example
[ ] dmClSlClSl
TrK 2111
1
1
)00
00
(21 θθθ
θθ
&∫ ⋅⋅−
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡⋅⋅−
=
dmVVTrdK T )(21
11=
mClCSl
CSlSl
Tr 21
21
211
211
221
2
)
0000000000)(00)(
(21 θ
θθθθθθ
&
⎥⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢⎢
⎣
⎡
⋅⋅⋅−⋅⋅−⋅
=
21
2221
221
2
21])()([
21 θθθθ && mlmClSl =+=
Kinetic energy
32
Example
)( 11
0 rTmgP −= [ ]⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡ −
−−=
100
100001000000
008.90 11
11 lCSSC
mθθθθ• Potential energy
18.9 θSlm ⋅⋅=
12
12 8.9
21 θθ SlmmlPKL ⋅⋅−=−= &
112
112
111
8.98.9)(
)(
θθθθ
θθτ
ClmmlClmmldtd
LLdtd
⋅⋅−=⋅⋅−=
∂∂
−∂∂
=
&&&
&
• Lagrange function
• Equation of Motion
33
Example: Puma 560• Derive dynamic equations for the first 4 links of PUMA 560 robot
34
Example: Puma 560
Joint i θi αi ai(mm) di(mm)1 θ1 -90 0 0 2 θ2 0 431.8 -149.09 3 θ3 90 -20.32 0 4 θ4 -90 0 433.07 5 θ5 90 0 0 6 θ6 0 0 56.25
• Get robot link parameters• Set up D-H Coordinate frame
• Get transformation matrices iiT 1−
• Get D, H, C terms
35
Example: Puma 560
)()()( 31331212211111111TTT UJUTrUJUTrUJUTrD ++=
)()( 31332212222112TT UJUTrUJUTrDD +==
)( 313333113TUJUTrDD ==
• Get D, H, C terms
∑=
=n
kij
Tjijjkik UJUTrD
),max(
)( 3,2,1;3 == in
)()( 323322222222TT UJUTrUJUTrD +=
)( 323333223TUJUTrDD ==
)( 3333333TUJUTrD =
36
Example: Puma 560
)()()( 313311212211111111111TTT UJUTrUJUTrUJUTrh ++=
∑=
=n
mkij
Tjijjkmikm UJUTrh
),,max()(
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡=
nh
hqqh M&
1
),(
• Get D, H, C terms ∑∑= =
=n
k
n
mmkikmi qqhh
1 1
&&
)()( 313312212212112TT UJUTrUJUTrh +=
)( 313323123TUJUTrh =
)()( 313321212221121TT UJUTrUJUTrh +=
)()( 313322212222122TT UJUTrUJUTrh += ……
23133231321313132123
22122211213111321112
211111
qhqqhqqhqqh
qhqqhqqhqqhqhh
&&&&&&&
&&&&&&&&
++++
++++=
)( 313313113TUJUTrh =
37
Example: Puma 560
20
21211 )( TQU =
⎪⎩
⎪⎨
⎧
<<≥≥≥≥
=≡∂
∂−
−−
−−
−−
−
kiorjikjiTQTQTjkiTQTQT
UqU i
jjkjk
k
ikk
kjj
j
ijkk
ij
01
11
10
11
11
0
30
21311 )( TQU =
• Get D, H, C terms
10
21111 )( TQU =
21
22
10222 )( TQTU =
312
101321312 TQTQUU ==
323
201313 TQTQU =
212
101221212 TQTQUU ==
31
22
10322 )( TQTU =
323
212
10332323 TQTQTUU == 3
232
01331 TQTQU = 3223
20333 TQQTU =
38
Example: Puma 560
jjji
n
ijji rUgmC ∑
=
−=
33313
22212
111111 rgUmrgUmrgUmC −−−=
33323
222222 rgUmrgUmC −−=
• Get D, H, C terms
333333 rgUmC −=