Beruflich Dokumente
Kultur Dokumente
L ( q , q& ) T ( q , q& ) U ( q )
h ere
T ( q , q& ) K in etic en erg y o f th e m a n ip u la to r
U ( q ) P o ten tia l en erg y o f th e m a n ip u la to r
Generalized Equation of Motion
Universiti Kuala Lumpur Malaysia France Institute
of the Manipulator:
d§ w · w
Fi ¨
dt © wq&i
L q, q& ¸ L q, q& 1didn
¹ wqi
i is a link of manipulator
Starting Generalized Equation
Solution
Universiti Kuala Lumpur Malaysia France Institute
vk as Linear Velocity
Zk as Angular Velocity
Rewriting the Kinetic Energy Term:
Universiti Kuala Lumpur Malaysia France Institute
n ª
«
vk mK vk Zk DKZk
T T
º
»
T q, q& ¦ « 2 »
«¬ »¼
K 1
T q, q& ¦
n &
qT
« K
ª AK T
m AK
&
q &
q
T
K »
BK T
DBK
&
q º
« 2 »
K 1
«¬ »¼
Simplifies to:
¦ A
mK A B
q&
n
K T K K T
DK BK
T q, q& q&T
K 1
2
Building an Equation for Potential
Energy:
Universiti Kuala Lumpur Malaysia France Institute
n Generalized
U (q ) ¦ mK g T c k (q ) coordinate of centers
K 1 of mass (from earlier)
g is acceleration due to gravity (a vector)
Introducing a new term:
n This is a weighted
c (q ) { ¦ mK c k (q ) sum of the centers of
K 1
mass of the links of
the manipulator
?U (q ) g T c (q )
Finally, The Manipulator
Lagrangian Equation :
Universiti Kuala Lumpur Malaysia France Institute
¦ A B
q& g c (q)
n
K T K K T
mK A DK BK
L q, q&
K
&
q T 1
2
T
Building a General L-E Dynamic
Model
Universiti Kuala Lumpur Malaysia France Institute
• But Remembering:
d§ w · w
Fi ¨ L q, q& ¸ L q, q& 1didn
dt © wq&i ¹ wqi
i is a link of manipulator
Starting with
this term
The Lagrange-Euler (Torque)
Universiti Kuala Lumpur Malaysia France Institute
n n n
7i ¦ D q q&& ¦¦C q q& q&
j 1
ij j
k 1 j 1
i
kj k j hi q bi q&i
Inertial Gravitational
Coriolis & Forces Frictional
Forces Centrifugal Forces
Forces
Lets look at 2 examples
Universiti Kuala Lumpur Malaysia France Institute
parameters.
D-H Parameters for the robot ( i )
Center of Mass for each links rc
Inertia Tensor Matrix for each links I%i Must be given !!!
i =1,2,3
R-R-P Robot
Variable 1: D-H Parameter
Link Ti di ai Di
1 T1 0 a1 00
2 T2 0 a2 1800
3 00 l3 0 00
Variable 2: Center of Mass
ª 1 º ª 1 º ª º
« 2 a1 » « 2 a1 » « 0 »
« » « » « »
rc(1) « 0, » rc(2) « 0 » rc(3) « 0 »
« 0 » « 0 » « l »
« » « » « 3 »
¬ ¼ ¬ ¼ ¬ 2¼
Note the negative sign. The center of mass is w.r.t. to the end of the link frame.
Variable 3: Inertia Tensor Matrix
The inertia tensor matrix of the links 1, 2 and 3 about its
respective center of masses in the frame xc(1) yc(1) zc(1)
xc(2) yc(2) zc(2) and xc(3) yc(3) zc(3) with its origin at r (1) , r (2) , r (3)
Universiti Kuala Lumpur Malaysia France Institute
c c c
is given as follows:
ª0 0 0º
« » m1a12
I%1 «0 I 1 0 » with I1
«0 0 12
¬ I 1 »¼
ª0 0 0º
« » m2 a22
I%2 «0 I 2 0» with I2
«0 0 I 2 »¼ 12
¬
ªI 3 0 0º
« » m3l32
I%3 «0 I3 0 » with I3
«0 0 0 »» 12
«¬ ¼
Universiti Kuala Lumpur Malaysia France Institute
ª b2 c2 º
« 12 0 0 »
« »
« a c
2 2
»
I%K mK « 0 0 »
12
« »
« 0 a b »
2 2
« 0
¬ 12 »¼
is given by:
ª x1(1) º
« (1) »
1
P « y1 » T 1r (1)
« z1(1) » 0 c
0
Universiti Kuala Lumpur Malaysia France Institute
« »
«¬ 1 »¼
ª x2(2) º
« (2) »
P
2
« y2 » T 2 r (2)
« z2(2) » 0 c
0
« »
«¬ 1 »¼
ª x3(3) º
« (3) »
P
3
« y3 » T 3r (3)
« z3(3) » 0 c
0
« »
«¬ 1 »¼
The linear velocities of masses m1 , m2 , m3 of the
links 1,2 and 3, with respect to the base frame x0 , y0 , z0
is given by
Universiti Kuala Lumpur Malaysia France Institute
ª x&1(1) º
« (1) »
v1 « y&1 » J l(1) rc(1)
« z&1(1) »
¬ ¼
ª x&2(2) º
« (2) »
v2 « y& 2 » J l(2) rc(2)
« z&2(2) »
¬ ¼
ª x&3(3) º
« (3) »
v3 « y&3 » J l(3) rc(3)
« z&3(3) »
¬ ¼
The angular velocities of the center of masses m1 , m2 , m3
of the links 1,2 and 3 with respect to the base frame
Universiti Kuala Lumpur Malaysia France Institute
x0 , y0 , z0 is given by
ª0º ª 0 0 0 º ªT&1 º
Z1 « 0 » T& « 0 0 0 » «T& » J a(1) q1
« » 1 « »« 2»
«¬1 »¼ «¬1 0 0 »¼ «¬ d&3 »¼
¦ m J J J I J
3 T T
(i ) (i ) (i ) (i )
D(q) i l l a i a
i 1
robot :
J J I J
T T
(1) (1) (1) (1)
D(q ) m1 J l l a 1 a
m J J J I J
T T
(2) (2) (2) (2)
2 l l a 2 a
m J J J I J
T T
(3) (3) (3) (3)
3 l l a 3 a
Universiti Kuala Lumpur Malaysia France Institute
1 1
K1 m1V12 m1r12T& 2
2 2
1 1 1
K2 m2V22 m2 r2 m2 (r1 r2 ) 2 T& 2
2
2 2 2
2 2 2
1
2
2
2 &2 1
m1r1 m2 (r1 r2 ) T m2 r&22
2
The Lagrangian function of the T r manipulator
L K P
1
2
2
2 &2 1
m1r1 m2 ( r1 r2 ) T m2 r&22
2
(m1 m 2 ) gr1 sin T m2 gr2 sin T
It is noted that
Similarly, for joint 2 ( r2 )
L K P
Universiti Kuala Lumpur Malaysia France Institute
1
2
2
2 &2 1
m1r1 m2 ( r1 r2 ) T m2 r&22
2
(m1 m 2 ) gr1 sin T m2 gr2 sin T
wL
m2 r&2
wr&2
d wL
m2 &&
r2 Homework !!
dt wr&2
wL
m2 (r1 r2 )T& 2 m2 g sin T
wr2
We have:
d wL wL
Universiti Kuala Lumpur Malaysia France Institute
Fr
dt wr2 wr2
m2 r2 m2 ( r1 r2 )T 2 m2 g sin T
TT (Torque due to T ) and Fr
Universiti Kuala Lumpur Malaysia France Institute
ª (m1 m2 )r1 m2 r2 gc os T º
« »
¬ m 2 g sin T ¼
It can be seen that the Torque and Force component is made up of the
acceleration component, velocity component and position component.