Professional Documents
Culture Documents
Dynamics: - Newton-Euler Formulation - Articulated Multi-Body Dynamics - Rigid Body Dynamics
Dynamics: - Newton-Euler Formulation - Articulated Multi-Body Dynamics - Rigid Body Dynamics
Newton-Euler Formulation
Articulated Multi-Body Dynamics
Recursive Algorithm
Dynamics Lagrange Formulation
Explicit Form
MA23
Formulations
Joint Space Dynamics
Newton-Euler Lagrange
Fi -ni+1
A(q )q + b(q, q ) + g ( x) = Ni
I Ci
q: Joint Coordinates -fi+1
-ni mi
Link i Kinetic Energy: Ki
A( q) : Kinetic Energy Matrix -fi Potential Energy i
b(q, q ) : Centrifugal and Coriolis forces Newton: m v C = F Generalized Coordinates
Euler: N i = I C i + i I C i 1 T
g (q ) : Gravity forces i i
K = q M q
Eliminate Internal Forces 2
: Generalized forces
i =
RS n . Zi
T
i revolute M q + b + g =
Tf .Z
i
T
i prismatic
1
f n f
n -fi+1
-ni+1
-n Link i
-f link 3
n3 Pi+1
3 f3
-f3 -n3 ni fi
n2
2 -f2 link 2
f1 f2
1 -n1 -n2
-f1 n1 link 1
Newton-Euler Algorithm
Newtons Law
F a particle
m
l erat
dI
ions an nertialfo
rce
F = ma
cce s
a
,a
ies
lo cit
Ve inertial
es
forc
Frame
d
( p mv ) = p mv + v mv = p m v
d dt
( mv ) = F rate of change of the d
dt linear momentum is equal (p m v ) = N
to the applied force dt
Linear Momentum applied moment
angular momentum
2
Rigid Body vi = pi
Rotational Motion
mi = p ( p ) dv
V
pi p ( p ) = p ( p )
Angular Momentum = p i mi v i
i
z
= [ pp
dv ]
V
Inertia Tensor
= mi p i ( p i )
i = I
mi dv ( : density )
= p ( p ) dv
V
Inertia Tensor
Linear Momentum Angular Momentum
z
dv
I = pp ) = ( p T p ) I 3 pp T
( pp
= mv = I
LM OP
x
V
z
I = [( p T p) I3 pp T ]dv
V
Newton Equation Euler Equation
MM PP ;p p = x + y + z
p= y T 2 2 2
F1 0 0 I
NQ (p p) I = ( x + y + z )G 0 JJ
d d
( mv ) = F ( I ) = N GH 0
T 2 2 2
z 1 0
1K
3
dt dt
LM x OP LM x xy xzOP 2 0
pp = y a x y z f = M xy y yz P
= F = N
T
MM z PP MN xz yz z PQ
2
NQ 2
I + I = N L y + z xy xz OP
2 2
) = MM xy z + x
MN xz yz x + y PPQ
( pp yz 2 2
2 2
Inertia Tensor
LM I xx I xy I xz OP Parallel Axis theorem
I = M I I yy I yz PP
MN I
xy
I yz I zz Q {A}
p
m
zzz
xz
I xx = ( y 2 + z 2 )dxdydz
pc R| x c U|
S| y V|
Moments of I yy = zzz ( z 2 + x 2 )dxdydz Tz
c
c W
zzz
Inertia
Izz = ( x 2 + y 2 )dxdydz I A = I C + m [(p C T p C ) I 3 p C p C T ]
I xy = zzz xydxdydz
I A zz = I C zz + m ( x C2 + y C2 )
Products of I xz = zzz xzdxdydz
I A xy = I C xy + m x C y C
zzz
Inertia
I yz = yzdxdydz
3
a
Example 2 Newton-Euler Algorithm
I C zz = a ( x + y 2 ) dxdydz
2
{C}
and Inertial
tions
2
f or
I Czz =
1
a5; m = a 3 l era ces
cce
,a
6 ies
cit
a
ma 2 Ve
lo
I Cxx = I Cyy = I Czz =
es
6 forc
a
A
x c = yc = zc =
A A
{A} 2
ma2 2 2
I Axx = I Ayy = I Azz = ICzz + = ma
4 32
ma
I A xy = I A xz = I A yz =
4
4
Dynamic forces on Link i fi +1
fi +1 ni +1
ICi i + i ICii ni +1
p i +1
mi vCi fi Fi
ni Link i
Ni
mi v Ci = forces fi pCi
I C i i + i I C i i = moments / ci Fi = fi fi +1
ni
Inertial forces/moments N i = ni ni +1 + ( p Ci ) fi + ( p i +1 p Ci ) ( fi +1 )
Fi = m i v Ci
N i = I C i i + i I C i i
lo cit
ies T
fi . Zi prismatic
Ve Fi = mi v Ci
es with
N i = I C i i + i IC ii
forc
where i +1 = i + i +1 = i + i +1 Zi +1
i +1 = i + i Zi +1 i +1 + i +1 Zi +1
v i +1 = v i + i pi +1 + i ( i pi +1 ) + 2di +1 i Zi +1 + di +1 Zi +1
v Ci+1 = v i +1 + i +1 pCi+1 + i +1 ( i +1 pCi+1 )
5
Lagrange Equations Inertial forces
d K K 1
d K K U ( ) = G K = qT M (q) q
( ) = G; G = dt q q 2
dt q q q K 1 T
= [ q M ( q ) q ] = M (q) q
Inertial forces q q 2
d K d
( )= ( M q ) = M q + M q
dt q dt
M (q)q + V (q, q ) = G(q) T
q
M
q
q1
d K K 1
( ) =
M q + Mq # + V ( q , q )
= M q
dt q q 2
M
q T q
qn
K 1 Equations of Motion
= M q K = mx 2 ; ( mx 2 ) = mx
1
q 2 x 2 T M
q q
q1
1 T d K K
K= q M (q)q 1
2 ( ) =
M q + Mq # + V ( q , q )
= M q
1 dt q q 2
M
v = M 1/ 2 q K = v T v q T q
2 qn
K K v
= = M 1/ 2 v = Mq
q v q M (q)q + V (q, q ) + G(q) =
1
1 T
( v v) = v M 1/ 2 M (q): K = qT M q M (q) V (q, q)
v 2 2
6
Equations of Motion Explicit Form Equations of Motion Explicit Form
i vci i vci
Link i Link i
Pci Pci
Generalized Coordinates q
1 Generalized Velocities q
K i = ( mi vCT i vCi + iT I C i i )
2 Kinetic Energy 1
n Quadratic Form of K = qT M q
Total Kinetic Energy K = Ki Generalized Velocities 2
1 T 1 n
q M q ( mi vCTi vCi + iT I C i i )
i=1
2 2 i =1
Link i Link i
Pci Pci
vCi = J vi q
C = J q
1 n
i i
1 T
1 T 1 n
q M q = q T ( mi J vT J v + J T I C i J ) q
q M q = ( mi vCTi vCi + iT C I i i ) 2 2 i =1 i i i i
2 2 i n=1
1 n
= (mi q T J vTi J vi q + q T JTi I C i Ji q )
2 i =1
M = ( mi J vTi J vi + J Ti I C i J i )
i =1
m m22 " m2 n
Pci
vCi = J vi q
M ( q ) = 21 C = J q
( nn )
# # # # i i
pC pC pC
mn1 mn 2 " mnn Jv = [ " 0 0 " 0]
i i i
i
q1 q2 qi
J = [1 z1 2 z2 " i zi 0 0 " 0]
i
7
m11
Vector V ( q, q ) Centrifugal & Coriolis Forces Vector V ( q, q ) M
q2
q1
LMq FG m m121 IJ q OP
LM OP FG
111
IJ MM FH mm KP
T
1 q Mq1 q m 11 m 12
T
1 m221
V = M q = q
Q H K IJ q P
121
T
N
2 q Mq2 q m 12 m 22
MNq GH m
m122
K PQ
2 T 112
122 m222
m 12 = m111 q1 + m112 q 2
m22
m11 m12 q1 v1 g1 1 LM q1
OP L O
+ + =
1 1
( m111 + m111 m111 ) ( m122 + m122 m221 ) q 2
12
m m22 q2 v2 g2 2 MM
V (q, q ) = 2
1
( m211 + m221 m112 )
2
1
1
mij
Christoffel Symbols qk Potential Energy
1
bijk = ( mijk + mikj m jki )
Ci
Link i mi
2
V=
LMb 111 b122 q OP LM OP LM
2
1
+
2 b112 OP
q q
pCi
hi
g
Nb 211 b222 q QN Q N
2
2 Q
2 b212 1 2
U i = mi g0 hi + U 0
C (q ) B(q )
LM b 1,11 b1,22 " b1,nn q12 OP LM OP U i = mi ( g T p Ci ); U = Ui
C (q )[q ] = M
MM #
b 2 2 ,11 b2,22 " b2,nn q 22
PP MM PP Gravity Vector
U n p C i
# # # #
PQ MN PQ Gj = = ( mi g T ) i
F I
( n n ) ( n 1 )
LM2b OPLM q q OP
iGG m# gJJ
2 b1,13 " 2 b1,( n 1) n
] = M
2b
1,12
2 b2,13 " 2 b2 ,( n 1) n
1 2
GH m gJK
2 ,12 1 3
B(q) [qq
( n
( n1 ) n
MM #
( n 1 ) n
1 )
# # #
PQMNq q PQ
N2 b
) (
2 2
c3
c2
cn
m3g
c1
m2g mng
m1g
8
Matrix M
M = m1 J vT1 J v1 + JT1 I C1 J1 + m2 J vT2 J v2 + JT2 I C2 J2
g
m2 Jv1 and Jv : direct differentiation of the vectors:
LM OP
LM OP
C 2
I2 l1c1 d 2 c1
l1 0
MM 0 PP
MM 0 PP
p C1 = l1s1 ; and 0 p C2 = d2 s1
y0 m1 N Q
N Q
d2 In frame {0}, these matrices are:
C
I1 Ll c 0 O L d s c O
J = M l c 0 P; and J = M d c s P
1 1 1 2 1 1
x0 MM 0 0PP
0
v1 MM 0 0 PP
1 1
0
v2 2 1 1
This yields
N Q N Q
m ( J J )=M
0 T 0 L m l 0O
P
2
; and m ( J J ) = M
11 Lm d 0 T 0 2
2
2 0 OP
1 v1
N v1
0 0 Q N 0 2 v2 v2
m2 Q
0 0 0 0 Matrix C L
C=M
0 b O L 0 0 OP
Nb 0 PQ = MNm d
122
LMm l OP Q
.
Finally, 0
2
+ I zz1 + m2 d + I zz 2 2
0 211 2 2
M=
N
11
0
2
m2 Q Matrix V L2m d OP d + LM 0
V=M
2 2 OP LM OP
0 12
N 0 Q N m d 1 2
2 2 0 d 2QN Q
.
2
Vector V
LM2m d OP d + LM 0 0OP LM OP. 2
Equations of Motion
V=
LMm l OP LM OP
2 2 1
N 0 Q N m d 0Q Nd Q
1 2
2 2
2 2
11 + I zz1 + m2 d22 + I zz 2 0 1
N m Q Nd Q
2
In frame {0}, g
0
= 0 a g 0 f T
and the gravity vector is
N 0 Q 1 2
N m d 0Q Nd Q
2 2
2
2
LMbm l + m d ggc OP = L O.
LMl s OP LM m0 gOP LMd s 0
OP LM OP
N m gs Q MN PQ
1
l1c1 0 d2 c1 0 + 11 2 2 1
0Q M
MN 0 PPQ N c Q MMN PP
G= m2 g
0 1 1 2 1
N0 0 1
1 s1 0
0 Q
2 1 2
and
0
G=
LMbm l + m d ggc OP
11 2 2 1
N m gs Q 2 1