Embed Size (px)
Citation preview
1
Dynamics
• Differential equation relating input torques and forces to the positions (angles) and their derivatives.
• Like force = mass times acceleration.
)(),()( qgqqqcqqD
anglesjoint of vector theis
uesinput torq of vector theis
q
2
Euler-Lagrange Equations
• Equations of motion for unconstrained system of particles is straightforward (F = m x a).
• For a constrained system, in addition to external forces, there exist constrained forces which need to be considered for writing dynamic equations of motion.
• To obtain dynamic equations of motion using Euler-Lagrange procedure we don’t have to find the constrained forces explicitly.
3
Holonomic ConstraintsF
(x,y)
m
l222 lyx
mjtqq nj ,,1,0),,,(
sconstraint holonomicfor expression General
1
4
Nonholonomic Constraints
)(sin)(
)(cos)(
ttry
ttrx
mjtqqqqf nnj ,,1,0),,,,,,( 11
x
y
General expression for nonholonomic constraints is:
Nonholonomic constraints contain velocity terms which cannot be integrated out.
A rear powered front steering vehicle
5
krr ,,1
Consider a system of k particles, with corresponding coordinates,
kiqqrr
nii
n
,,1),,,(
and ,,
1
1
Often due to constraints or otherwise the position of k particles can be written in terms of n generalised coordinates (n < k),
In this course we consider only holonomic constraints and for those constraints one can always find in principle n (n < k) independent generalised coordinates.
6
Virtual Displacements
ir
kidtt
rdq
q
rdr i
n
jj
j
ii ,,2,1,
1
Define virtual displacements from above by setting dt=0.
kiqq
rr
n
jj
j
ii ,,2,1,
1
In our case dqj are independent and satisfy all the constraints. If additional constraints have to be added to dqj to finally arrive at a statement of virtual displacements which have only independent dqj, we can replace dqj with jq
7
Virtual Work
forces ngconstraini theare
forces external theare )(a
j
j
f
f
k
jj
Ta
jj
k
jj
T
j rffrFW1
)(
1
)(
Let Fi be total force on every particle, then virtual work is defined as:
Constraining forces do no work when a virtual displacement, i.e., displacement satisfying all the constraints, takes place (as is the case with holonomic constraints), so in equilibrium
0)(1
k
jj
T
j rfW
8
D’Alembert’s Principle
D’Alembert’s principle states that, if one introduces a fictitious additional force, the negative of the rate of change of the momentum of particle i, then each particle will be in equilibrium.
dt
drmprpfW j
jj
k
jj
T
jj
,0)(1
are) but t independennot are ( ij δqr
9
Generalised Forces
is called the i-th generalised force. The equations of motion become:
.,,2,1,
where
1i
11 11
niq
rf
qqq
rfrf
k
j i
jT
j
i
n
ii
k
j
n
ii
i
jT
j
k
jj
T
j
.0))((11
i
k
j i
jT
jj
n
ii q
q
rrm
10
F
(x,y)
m
l
222 lyx
cos
sin
coordinate dGeneralise
;sin
cos
1,1
1
1
1
F
F
F
FF
q
l
l
y
xr
nk
y
x0))((11
i
k
j i
jT
jj
n
ii q
q
rrm
Write the equation of motion.
11
F
(x,y)
m
l
222 lyx
cos
sin
coordinate dGeneralise
;sin
cos
1,1
1
1
1
F
F
F
FF
q
l
l
y
xr
nk
y
x
0)(
0))((
1
11
yym
xxm
rrm i
k
j i
jT
ji
n
ii
FlFlFl
l
lF
q
rf T
j i
jT
j
22
1
2
1
sincos
sin
cos
2222
2222
))(cossin()cos(
))(cossin()sin(
llx
y
llx
x
Fml
:ismotion ofequation The
12
Euler-Lagrange Equations of Motion
k
j i
jT
jj
i
jT
jj
k
j i
jT
jj
q
r
dtd
rmq
rrm
dtd
q
rrm
1
1
)()(
)(
i
j
l
n
l li
j
i
j
i
j
i
j
i
n
i
jT
jj
q
vq
r
q
r
dtd
q
r
q
v
rrv
1
2
11
and
Then
)(
13
k
j i
jT
jj
i
jT
jj
k
j i
jT
jj q
vvm
q
vvm
dtd
q
rrm
11
)(
ii
k
j i
jT
jj
k
jj
T
jj qK
q
Kdtd
q
rrmvvm
11
)(21
K
be K toenergy kinetic theDefine
.,2,1,0
0))((111
niqK
q
Kdtd
qqK
q
Kdtd
rrm
i
ii
n
iii
ii
i
k
j i
jT
jj
n
ii
14
K-VL
niqL
q
Ldtd
qV
i
ii
i
i
i
Where
.,2,1,
Then
such that
forces),or torques(external tau and energy) (potential V
exist theresuppose Finally,
15
F
(x,y)
m
l
y
xyxmK21
.,2,1, niqL
q
Ldtd
i
ii
Write the dynamic equations of motions for this system.
16
F
(x,y)
m
l
Fl
mly
xyxmK
2
2
21
21
.,2,1, niqL
q
Ldtd
i
ii
Flml
2
:ismotion of
equation dynamic The
17
Expression for Kinetic Energy
B
T
B
T
B
dmzyxvzyxv
dxdydzzyxzyxvzyxvK
mdxdydzzyx
),,(),,(21
),,(),,(),,(21
),,(
BBody
0)(or 1
1,
1,
1
:asgiven is ),,( mass of Centre
B
c
B
c
B
c
B
c
B
c
ccc
dmrrrdmm
r
ydmm
zydmm
yxdmm
x
zyx
18
Attach a coordinate frame to the body at its centre of mass, then velocity of a point r is given by:
mass of centre theofocity linear vel theis
mass of centre sbody' the toattached
frame theoflocity angular ve theis
where
)(
c
c
v
rSvv
4321
)()(21
KKKK
dmrSvrSvK c
T
B
c
19
c
T
cc
T
B
c vvmdmvvK21
21
1
)0 (since 0)(21
)(21
2 c
B
T
c
T
B
c rdmrSvdmrSvK
0)(21
3 dmvrSK c
T
B
)()(21
)()(21
)()(21
4
T
TT
B
T
B
JSSTr
dmSrrSTr
dmrSrSK
BBB
BBB
BBB
T
B
dmzyzdmxzdm
yzdmdmyxydm
xzdmxydmdmx
dmrrJ
2
2
2
Where
20
BBB
BBB
BBB
dmyxyzdmxzdm
yzdmdmzxxydm
xzdmxydmdmzy
I
)(
)(
)(
Where
22
22
22
0
0
0
)(
xy
xz
yz
S
IJSSTrK TT
21
)()(21
4
21
00000
4
0
21
21
21
IRIR
IK
R
TTT
T
T
Frame for I and Omega
The expression for the kinetic energy is the same whether we write it in body reference frame or the inertial frame but it is much easier to write I in body reference frame since it doesn’t change as the body rotates but its value in the inertial frame is always changing.
So we write the angular velocity and the inertia matrix in the body reference frame.
22
Jacobian and velocity
frame) referencebody in is ( )()(
,)(
i
T
ii
vci
qqJqR
qqJv
i
ci
qqJqRIqRqJqJqJmqKn
i
T
iii
T
v
T
vi
T
iicici1
)()()()()()()(21
qqDqK T )()(21
D(q) is a symmetric positive definite matrix and is known as the inertia matrix.
23
Potential Energy V
B B
c
TTT mrgrdmgrdmgV
24
Two Link Manipulator
0z
1
2
11, lm
22 , lm
p
1z
2z
0x
1x
2x
Links are symmetric, centre of mass at half the length.
21,,, Find21
cc vv
21
2211
T
2
T
1
22211121
ggV21
21
)(21
)(21
K
Find
cc
TT
c
T
cc
T
c
rmrm
IIvvmvvm
25
0z
1
2
11, lm
22 , lm
p
1z
2z
0x
1x
2x
Links are symmetric, centre of mass at half the length.
1000
100
0
0
1
1
1
1111
111
l
slcs
clsc
A
1000
0100
0
0
122111212
122111212
21
2
0
slslcs
clclsc
AAT
1000
100
0
0
1
2
2
2222
222
l
slcs
clsc
A
26
0z
1
2
11, lm
22 , lm
p
1z
2z
0x
1x
2x21
2211
T
2
T
1
22211121
ggV21
21
)(21
)(21
K
cc
TT
c
T
cc
T
c
rmrm
IIvvmvvm
Revision Questions:
1. What are m1 and m1?
2. In which frame are vc1 and vc2 specified?
3. In which frame are I1 and I2 specified?
4. In which frame are 1 and 2 specified?
5. What is g?
6. What are rc1 and rc2?
27
K-VL niqL
q
Ldtd
i
ii
Where.,2,1,
qqDqK T )()(21
mrgV c
T
.,2,1
,21
)(,
,,
,,
nk
qV
qqq
d
q
dqqd k
k
jiji j
ji
i
jk
jijjk
Euler-Lagrange Equations – The general form.
and
28
k
k
k
ji
j
ik
i
jk
ijk qV
q
d
q
d
q
dc
and 21
Let
,,,
)(),()( qgqqqCqqD
],,[ ,],,[)(
21
)(
where
11
1
,,,
1
n
T
n
i
n
i k
ji
j
ik
i
jkn
iiijkkj
qg
d
q
d
q
dqqcC
29
01
0
1
1
0
1
0
0
11
0
0
2/
)( cc r
l
RSr
0z
1
2
11, lm
22 , lm
p
1z
2z
0x
1x
2x
0
1cr
0
2cr
1
2cr
Revision
The linear velocity of a rotating vector is the cross-product of its angular velocity and the vector itself.
Proof: Let be the vector rotating with an angular velocity then its derivative with time can be written as follows.
0
1cr1
0
2
1
11
0
01
2
0
1
1
0
0 )()( 22
22
2
cc
cc
c rzrzrr
r
1
0
0
01
0
0 )(z 111
ccc rrr
30
0z
1
2
11, lm
22 , lm
p
1z
2z
0x
1x
2x
0
1cr
0
2cr
1
2cr
1000
100
0
0
1
1
1
1111
111
l
slcs
clsc
A
1000
0100
0
0
122111212
122111212
21
2
0
slslcs
clclsc
AAT
0
0 ;
0
0
2/ 1
1
0
10
2
2
0
1
222
l
Rrr
l
Rr ccc
;
0
0
2/1
1
0
0
1
l
Rrc
31
r.manipulatolink - twofor the )( and ),(),( Evaluate 1
1
0
0
0
0 221 ccc rzrzrz
32
qJl
l
rvcvcc 1,12
2
111
11
0
00
0cos)2/(
0sin)2/(
qJlll
lll
rvcvcc 2,22
2
121221211
21221211
00
)cos()cos()2/(cos
)sin()sin()2/(sin
2
1211
11
00
00
,
1
0
0
21
2211
T
2
T
1
22211121
ggV21
21
)(21
)(21
K
cc
TT
c
T
cc
T
c
rmrm
IIvvmvvm
33
Write the expressions for K and V. From these obtain:
).( and ),,(),( qgqqCqD
34
2
2
21
2
2222
221
2
2
2
122112
21
2
1
2
12
2
1111
)2/(
)cos)2/()2/((
)2/(2)2/(()2/(
zz
zz
zzzz
Ilmd
Iqllllmdd
IIllllmlmd
0;0;;
:sin)2/( ;0
222122112221
2212121111
cchchc
hqllmcc
)cos()2/(
)cos()2/(cos))2/((
))sin()2/(sin(;sin)2/(
21222
2122112111
21
21211221111
qqglm
qqglmqglmlm
VVV
qqlqlgmVqlgmV
35
Newton-Euler Formulation
ii gm
if 1
1
i
i
i fR
i1
1
i
i
iR icir , icir ,1
iciiii
i
ii amgmfRf ,1
1
)()( ,11
1
,1
1
iiiiiicii
i
iciii
i
ii IIrfRrfRii
36
Newtonian Mechanics
1. Every action has an equal and opposite reaction. Thus if body 1 applies a force f and torque tau to body 2, then body 2 applies a force –f and torque of –tau to body 1.
2. The rate of change of linear momentum equals the total force applied to the body.
3. The rate of change of the angular momentum equals the total torque applied to the body.
37
Basic Relationships
000 )(
dtId
fdtmvd )(
ωIIωII
ωIIRS
ωRIRRISRhR
ωRIRISωRIIωRh
RIωRωRIRh
h
RR
T
TTT
T
T
)()(
)(
)(
)(
bygiven is momentumAngular
frame) inertial in the is and frame referencebody in the is (
,
0
0
0
0
00
38
Recursion-angular velocity
1011
1
1
)0(
1
)0(
)(;)(
frame-1)th-(iin is and frame-ith in the is i.e.,
link, the toattached framein vectorsall express We
i
Ti
iiii
Ti
ii
ii
iiii
zRbqbR
qz
iiiiii
Ti
ii
iiiiiii
i
Ti
i
qbqbR
qzqz
R
11
1
)0(
1
)0(
1
)0(
)0(
0
)(
)()(
)()(
39
Recursion-Linear velocity
)()(
)()(
)()()(remember ;)(
)(
1,1,1,1,
,,1,1,
)0(
,0,
)0(
,
)0()0()0(
,
)0()0(
1,
)0(
,
)0(
,
)0()0(
1,
)0(
,
iiiiiiiie
Ti
iie
ciiiciiie
Ti
iic
ic
Ti
ic
ciiiciiieic
ciiieic
rraRa
rraRa
RbRabaRaRa
rraa
rvv
ii
ii
i
40
The algorithm
ii
n
icieii
ec
f
i
f
aa
i
aa
and
1) n to from decreasing (for solve and
0 and 0
conditions terminal theStart with 2.
and ,,,
n) to1 from increasing (for solve and
0,0,0,0
conditions initial theStart with 1.
1n1
,1,
0,0,00
iciiii
i
ii amgmfRf ,1
1
)()( ,11
1
,1
1
iiiiiicii
i
iciii
i
ii IIrfRrfRii
41
xy
z
0y
1z
1y
1x0x
0z
0y1z1y1x0x
0z0o
1o
i is the angular velocity of frame i with respect to the inertial frame expressed in frame i.
