Dinamica del robot Langrage

Parámetros de Denavit-Hartenberg
Articulaciones θ d a α
1 θ1 L1 0 90°
2 θ2 0 L2 0
3 θ3 0 L3 0

Dimensiones
Las matrices de transformación son:

0
=
1

1
=
2
 2
=
3

Matrices Uij

−S 1 −C 1 0 0 −C 1 −S 1 0 L 3Cϴ
U11=
[
C1
0
0
S1
0
0
0 Cϴ
0 0
0 1
] [
−C 1 −S 1
U21=
S1
0
0
0 Cϴ
C1
0
0
0
0
0
Cϴ
0
1
] U22=

[ S1
0
0
C1
0
0
0 Sϴ
0 0
0 1
]
C 1 −S 1 0 0 S 1 −S 1 0 CϴSϴ
U31=
0
0
[
−S 1 −C 1 0 0
0
0
0 0
0 1
] [U32=

S 1 −S 1
Cs1 C 1
0
0
0
0
0 Sϴ
0
0
0
Sϴ
0
1
] U33=

[ C1 C1
0
0
0
0
0 0
0 0
0 1
]
Matrices Uijk

S 1 −C 1 0 0 C 1 −S 1 0 SϴCϴ
U111=
[
C1 S1
0
0
0
0
−C 1 −S 1 0 CϴSϴ
0
0
0
0
0
1
] [
U121=
S 1 −C 1 0
0
0
0
0
0
0
Cϴ
0
1
U111=
]
[ −S 1 C 1
0
0
0
0
0
0
0
Sϴ
0
1
]
 −S 1 −C 1 0 0
U211=
0
0
[
−C 1 −S 1
0
0
−S 1 −C 1 0 0
0
0
0
0
0
1
] U221=[ 0 ] U222=

[ C1
0
0
S1
0
0
0
0
0
0
0
1
]
−S 1 −C 1 0 0 −S 1 −C 1 0 0
U311=
C1
0
0
[ S1
0
0
−S 1 −C 1 0 0
0
0
0
0
0
1
] [ U221=
C1
0
0
S1
0
0
0 SϴCϴ
0
0
0
1
]U222=

[ C1
0
0
S1
0
0
0
0
0
0
0
1
]
−S 1 −C 1 0 0 −S 1 −C 1 0 0
U311=
C1
0
0
[ S1
0
0
0
0
0
0
0
1
] U321=[ 0 ] U333=
[C1
0
0
S1
0
0
0
0
0
0
0
1
]
J1

Elemento 1

0 m1 L 1 0 0
J1=
0
[ 0 0 0
0 m1 L 1 m 1l 1 L 1 0
0 m1 L 1 m1 L 1 m 1
]
Elemento 2
 0 0 0 0
J2=
[0 0
0 m1 L 1 L1
0 m1 L 1
0
0
0
0
m1 L 1 m 1
]
Elemento 3

0 01 0 0
J2=
[0 0 0 0
0 0 0 0
0 0 0 m1
]
Matrices de inercia

D 11=m1 L˙1 L1 ˙ C 1+m2 L 2 S 1+ M 3 L 3 +m3C2 L˙2 L˙3 L˙2 L 3˙ L1

˙ L1 ˙

D 12=m 2 L˙1C 1 S 1 S 1 C 1 L˙1 L˙3 L2+

˙ m1 L˙3 C 1 S 1 S 1 C 1 S 1+ m3 L1
˙ L2
˙ +m3C3S3
2 L2
˙ L1
˙

D 13=m 1C 1+ m2 L1
˙ S 1 S 1C 1 S 1C 1 S 1+m2 L3 L 3 L1 L1 +m1 L3L3L3 C1 S1 S1 C1

D 21=m 2 L 1 L 1 L 3−m1 L1 L1 S 1 C 1 S 2 S 2 S 3 C 1 C 2 C 2 C 3 C 3+ S1C1 S1 C1 C2 C2 C2

C3 C3 S2 S2 S3

D 22=m 1 L 1 L 1C 1−S 1 S 1 S 1 C 1−L 1C 2 S 1 S 2 C 3 C 1−m3 L1 L1 L3 L 3 L1 L1 LS 1C 1 S 3

D 23=m3 L1 L1 L1 L3 L 3−S 1 M 3+C 1 S 1 S 3 m2

D 31=m 2 L 1C 1 C 1 S 1 S 1 S 3−m3 L 1 L 2 L 3

D 32=¿m1L3L3L2L2LC1S1S2S3C3C2C1- S1S3S2M1M2S1C2C3

D 32=m 1 L 1 L 1C 1+ m2 L2−S 1 S 2 S 3 m2 m3 S 1 S 2 S 3 L 12 L 3 L−S 1C 1C 2 C 3 S 3 S 2 S 1

Obtenoms D

d 11 d 12 d 13
D= [ d 21 d 22 d 23
d 31 d 32 d 33 ]
+

Terminos de D=[ hi ] ¨

h 1=m1 ω˙ 2 ω1 ˙ ω˙ 1 M 3 L˙1 +m3ω˙ 1 ω˙ 2C2L2 L˙3 L˙1 L 1˙L˙3 –

˙ L˙1 L˙2 L˙3 C 1+m2 L1+
m1L L˙1 ω˙2C1 L˙1 L 2˙L˙33C3C2+
ω˙3 m 3 S 1 L1
˙ L3−
˙ ω˙ 3 m3 L˙1 L˙2 LS 1C 1 S 3−m 1 L L1
˙ ω˙ 3 S 2 L˙2 S 3 C 1 C 2 C 2 ω˙ 2C 3C 3
h 2= ω˙ 3+m3 L 1 L˙3 ω˙ 3 ω˙2L2 L2+ω˙3 m 1 ω˙1 L1
˙ L˙2C 1+ ω˙ 2m 2 L˙1 S 1-
m 3 L˙1 L˙2 L3−S
˙ 1 S 1 S 1 C 1−m 1 L 3 L3 C 1 S 1 S 1 ω1
˙ C 1 S 1+ ω˙ 2 m2 L 1 C 1C 1 S 1 S 1 S 3−ω˙2 m2 ω˙3

h 3=¿ ω˙3 m1 L˙3 L˙1 C+ω˙ 1 ω˙ 2 m1 ω˙ 3 ω˙ 2 L3 L˙3 L˙1 L˙2 S 1 C 1 S 1+ m3 ω˙ 1 L 1˙L˙2+ ω˙3
L˙1c 1 ω˙ 3+ ω˙ 3m3ω˙ 1 L1
˙ L2
˙ -m1 C 1 ω˙ 3

H1
H= [ ]
H2
H3

Para la matriz de C

0
C= []
0
0

d 11 d 12 d 13 H1
T= D= [ d 21 d 22 d 23
d 31 d 32 d 33 ] [ ]
+ H= H2
H3