PROYECTO

Cinemática directa

https://www.youtube.com/watch?v=GGMJxgJoypA

θ1 θ2

Z1 Y1 Z2 Y2

X1 X2

L2

L3 L4

L5

X3

d1

Y3 Z3

Z0

Y0

X0

θi αi di ai
1 π /2 0° d1 L2
2 θ1 0° 0 L3
3 θ2 π L5 L4
 [ ]
cθi −s θi . cαi sθi. sαi ai. cθi
sθi cθi. cαi −cθi . sαi ai . sθi
Ai=
0 s αi c αi di
0 0 0 1

[ ] [ ] [ ]
0 −1 0 0 cθ 1 −s θ 1 0 L 3.cθ 1 cθ 2 s θ 2 0 L 4.cθ 2
1 0 0 L2 sθ 1 cθ 1 0 L 3. sθ 1 sθ 2 −cθ 2 0 L 4. sθ 2
A1= A2= A3=
0 0 1 d1 0 0 1 0 0 0 −1 L5
0 0 0 1 0 0 0 1 0 0 0 1
0
T3 =0A3= A1. A2. A3
0
A2= A1. A2

[ ][ ]
0 −1 0 0 cθ 1 −s θ1 0 L 3. cθ 1
1 0 0 L2 sθ 1 cθ 1 0 L3. sθ 1
=
0 0 1 L1 0 0 1 0
0 0 0 1 0 0 0 1

[ ]
−sθ 1 −cθ 1 0 −L3. sθ 1
cθ 1 −sθ 1 0 L 3. cθ 1+ L2
=
0 0 1 d1
0 0 0 1
0
T3 =0A3=0A2. A3= A1. A2. A3

[ ][ ]
−sθ 1 −cθ 1 0 −L3. sθ 1 cθ 2 s θ 2 0 L 4. cθ 2
cθ 1 −sθ 1 0 L 3. cθ 1+ L2 sθ 2 −cθ 2 0 L 4. sθ 2
=
0 0 1 d1 0 0 −1 L5
0 0 0 1 0 0 0 1

[ ]
−s (θ 1+θ 2) c (θ 1+θ 2) 0 −L 4 s (θ 1+θ 2 ) −L3. sθ 1
c(θ 1+θ 2) s(θ 1+θ 2) 0 L 4 c ( θ 1+ θ 2 )+ L 3. sθ 1+ L 2
=
0 0 −1 d 1+ L5
0 0 0 1

Cinemática inversa

Cinemática diferencial (jacobiano )

Para el robot PRR

J6x3= [ Z o Z 1 x (P−P 1) Z 2 x( P−P 2)

0 Z1 Z2 ]

[ ]
−s (θ 1+θ 2) c (θ 1+θ 2) 0 −L 4. s ( θ 1+ θ 2 )−L 3. sθ 1
0 c(θ 1+θ 2) s(θ 1+θ 2) 0 L 4. c ( θ 1+θ 2 ) + L3. sθ 1+ L2
A3 =
0 0 −1 d 1+ L5
0 0 0 1
 [] [ ]
0 −L 4. s ( θ1+θ 2 ) −L3. sθ 1
Zo= 0 P= L 4. c ( θ 1+θ 2 )+ L3. sθ 1+ L 2
1 d 1+ L 5
0
A1=A1

[ ]
0 −1 0 0
1 0 0 L2
=
0 0 1 d1
0 0 0 1

[] []
0 0
Z1= 0 P1= L2
1 d1

Z1x(P-P1)

[ ][ ][ ]
0 −L 4. s ( θ 1+θ 2 )−L 3. sθ 1 −L 4. c ( θ 1+θ 2 )−L 3. sθ 1
0 x L 4. c ( θ 1+ θ2 )+ L 3. sθ 1 = −L 4. s ( θ1+θ 2 ) −L3. sθ 1
1 L5 0
0
A2= A1. A2

[ ]
−sθ 1 −cθ 1 0 −L3. sθ 1
cθ 1 −sθ 1 0 L 3. cθ 1+ L2
=
0 0 1 d1
0 0 0 1

[] [ ]
0 −L 3. sθ 1
Z2= 0 P2= L3. cθ 1+ L 2
1 d1

Z2x(P-P2)

[ ][ ][ ]
0 −L 4. s ( θ 1+θ 2 ) −L 4. c ( θ 1+θ 2 )
0 x L 4. c (θ 1+θ 2 ) = −L 4. s ( θ1+θ 2 )
1 L5 0

[ ]
0 −L 4. s ( θ 1+ θ 2 )−L3. sθ 1 −L 4. c (θ 1+θ 2 )
0 −L 4. s ( θ 1+ θ 2 )−L3. sθ 1 −L 4. s ( θ 1+θ 2 )
J6x3=
1 0 0
0 0 0
0 0 0
0 1 1

Cinemática inversa

Hd= A1. A2. A3

[ ][ ]
nx ox ax Px −s (θ 1+θ 2) c (θ 1+θ 2) 0 −L 4 s (θ 1+θ 2 ) −L3. sθ 1
ny oy ax Py c(θ 1+θ 2) s(θ 1+θ 2) 0 L 4 c ( θ 1+ θ 2 )+ L 3. sθ 1+ L 2
=
nz oz ax Pz 0 0 −1 d 1+ L5
0 0 0 1 0 0 0 1

PX=−L 4 s ( θ 1+θ 2 )−L 3. sθ 1

PY= L 4 c ( θ 1+ θ 2 )+ L 3. sθ 1+ L 2

PZ=d 1+ L5

A1-1Hd= A2. A3

A-1= RT -RT.P

0 1

[ ] [ ] [ ] []
0 −1 0 0
0 −1 0 0 1 0 0
1 0 0 L2
A1= R= 1 0 0 R = −1 0 0
T
P= L2
0 0 1 d1
0 0 1 0 0 1 L3
0 0 0 1

[ ][ ] [ ]
0 1 0 0 −L2
-RT.P= - −1 0 0 L2 = 0
0 0 1 L 3 −d 1

[ ]
0 1 0 −L2
−1 0 0 0
A1-1=
0 0 1 −d 1
0 0 0 1

A2. A3=

[ ][ ]
cθ 1 −s θ 1 0 L 3.cθ 1 cθ 2 s θ 2 0 L 4. cθ 2
sθ 1 cθ 1 0 L 3. sθ 1 sθ 2 −cθ 2 0 L 4. sθ 2
=
0 0 1 0 0 0 −1 L5
0 0 0 1 0 0 0 1

[ ]
c (θ 1+ θ 2) s(θ 1+θ 2) 0 L 4. c ( θ 1+θ 2 ) + L3. cθ 1
s(θ 1+θ 2) −c (θ 1+ θ 2) 0 L 4. s ( θ 1+θ 2 )+ L3. sθ 1
=
0 0 −1 L5
0 0 0 1

A1-1Hd= A2.A3

[ ][ ][ ]
0 1 0 −L2 nx ox ax Px c (θ 1+ θ 2) s(θ 1+θ 2) 0 L 4. c ( θ 1+θ 2 ) + L3. cθ 1
−1 0 0 0 ny oy ax Py s(θ 1+θ 2) −c (θ 1+ θ 2) 0 L 4 s ( θ 1+θ 2 ) + L3. sθ 1
=
0 0 1 −d 1 nz oz ax Pz 0 0 −1 L5
0 0 0 1 0 0 0 1 0 0 0 1
A2-1A1-1Hd= A3

A-1= RT -RT.P

0 1

[ [ ]
] [ ][ ]
cθ 1 −s θ 1 0 L 3.cθ 1
cθ 1 −sθ 1 0 cθ 1 sθ 1 0 L3. cθ 1
sθ 1 cθ 1 0 L 3. sθ 1
A2= R= sθ 1 cθ 1 0 RT= −sθ 1 cθ 1 0 P= L3. sθ 1
0 0 1 0
0 0 1 0 0 1 0
0 0 0 1

[ ][ ] [ ]
cθ 1 sθ 1 0 L3. cθ 1 −L3
-R .P= - −sθ 1 cθ 1 0 L 3. sθ 1 = 0
T

0 0 1 0 0

[ ]
cθ 1 sθ 1 0 −L 3
−sθ 1 cθ 1 0 0
A2-1=
0 0 1 0
0 0 0 1

A2-1A1-1

[ ][ ]
cθ 1 sθ 1 0 −L 3 0 1 0 −L 2
−sθ 1 cθ 1 0 0 −1 0 0 0
=
0 0 1 0 0 0 1 −d 1
0 0 0 1 0 0 0 1

[ ]
−sθ 1 cθ 1 0 −L 2. cθ 1−L 3
−cθ 1 −sθ 1 0 L 2. sθ 1
=
0 0 1 −d 1
0 0 0 1

[ ]
cθ 2 s θ 2 0 L 4.cθ 2
sθ 2 −cθ 2 0 L 4. sθ 2
A3=
0 0 −1 L5
0 0 0 1

A2-1A1-1Hd= A3

[ ][ ][ ]
−sθ 1 cθ 1 0 −L 2. cθ 1−L 3 nx ox ax Px cθ 2 s θ 2 0 L 4.cθ 2
−cθ 1 −sθ 1 0 L 2. sθ 1 ny oy ax Py sθ 2 −cθ 2 0 L 4. sθ 2
=
0 0 1 −d 1 nz oz ax Pz 0 0 −1 L5
0 0 0 1 0 0 0 1 0 0 0 1

-sθ1.PX+cθ1.PY-L2.cθ1-L3=L4.cθ2
-cθ1.PX-sθ1.PY+L2.sθ1=L4.sθ2

PZ-L1=L5

PZ=L5+L1

-sθ1.PX+(PY-L2).cθ1= L4.cθ2+L3

s2θ1.PX2-2.sθ1.PX.(PY-L2).cθ1+(PY-L2)2.c2θ1= L42.c2θ2+2.L4.cθ2.L3+L32

-cθ1.PX-(PY-L2).sθ1= L4.sθ2

c2θ1.PX2+2.cθ1.PX.(PY-L2).sθ1+(PY-L2)2.s2θ1= L42.s2θ2

PX2+(PY-L2)2= L42+L32+2.L4.cθ2.L3

cθ2= PX2+(PY-L2)2- L42-L32

2.L4.L3

θ2=arccos PX2+(PY-L2)2- L42-L32

2.L 4.L3

cθ1= L4.(PY-L2).cθ2+(PY-L2).L3+L4. sθ2

PX2+(PY-L2)2

θ1=arccos L4.(PY-L2).cθ2+(PY-L2).L3+L4. sθ2

P X2+(PY-L2)2

Dinámica directa

Energía cinética

Energía cinética para el primer estabón:

θi αi di ai
1 0° 0° d1 0
0
A1=A1

[ ]
1 0 0 0
0 1 0 0
= x=0 y=0 z=d1
0 0 1 d1
0 0 0 1
 [ ][
∂x ∂x ∂x

]
∂d 1 ∂θ 1 ∂ θ2
0 0 0
∂y ∂y ∂y
Jv1 = = 0 0 0
∂d 1 ∂θ 1 ∂ θ2
1 0 0
∂z ∂z ∂z
∂d 1 ∂θ 1 ∂ θ2

[ ][ ] [ ]
0 0 0 d˙1 0
V1= Jv1.(ḋ 1 ,θ̇ 1 ,θ̇ 2)= 0 0 0 0 = 0
1 0 0 0 d˙1

[]
0
T
1 1
˙
v v =[ 0 0 d 1 ] 0 =ḋ 12
d˙1

1 1 T
K1= m1v1Tv1+ w 1 I1w1 donde w1=0
2 2
1
K1= m1ḋ 12
2
Energía cinética para el segundo estabón:

θi αi di ai
1 π /2 0° d1 L2
2 θ1 0° 0 Lc2

[ ] [ ]
0 −1 0 0 cθ 1 −s θ 1 0 Lc 2. cθ 1
1 0 0 L2 sθ 1 cθ 1 0 Lc 2. sθ 1
A1= A2=
0 0 1 d1 0 0 1 0
0 0 0 1 0 0 0 1
0
A2= A1. A2

[ ][ ]
0 −1 0 0 cθ 1 −s θ1 0 Lc 2. cθ 1
1 0 0 L2 sθ 1 cθ 1 0 Lc 2. sθ 1
=
0 0 1 d1 0 0 1 0
0 0 0 1 0 0 0 1

[ ]
−sθ 1 −cθ 1 0 −Lc 2. sθ 1
cθ 1 −sθ 1 0 Lc 2. cθ 1+ L 2
=
0 0 1 d1
0 0 0 1

X= -Lc2.sθ1.θ̇ 1 y= Lc2.cθ1+L2 z=d1

 [ ][
∂x ∂x ∂x

]
∂d1 ∂θ 1 ∂ θ2
0 −Lc 2. c θ 1 0
∂y ∂y ∂y
Jv2 = = 0 −Lc 2. s θ 1 0
∂d1 ∂θ 1 ∂ θ2
1 0 0
∂z ∂z ∂z
∂d1 ∂θ 1 ∂ θ2

[ ][ ] [ ]
0 −Lc 2. c θ 1 0 d˙1 −L c 2 . c θ 1 . θ̇ 1
V2= Jv2.(ḋ 1 ,θ̇ 1 ,θ̇ 2)= 0 −Lc 2. s θ 1 0 θ˙1 = −L c 2 . s θ 1 . θ̇ 1
1 0 0 0 d˙1

[ ]
−L c 2 . c θ 1 . θ̇ 1
V V T
2 2 =[ −L c 2 . c θ 1 . θ̇ 1 −L c 2 . s θ 1 . θ̇ 1 d˙1 ] −L c 2. s θ 1. θ̇ 1 =Lc22.θ̇ 12+ḋ 12
d˙1

[ ][ ]
1 0 0 0
T
2 1
˙
w I w2=[ 0 0 θ 1 ] 0 1 0 0 = I1θ̇ 12
˙
0 0 I 1 θ1

1 1 T
K2= m2v2Tv2+ w 2 I2w2
2 2
1 1
K2= m2(Lc22.θ̇ 12+ḋ 12)+ (I1θ̇ 12)
2 2
Energía cinética para el tercer estabón:

θi αi di ai
1 π /2 0° d1 L2
2 θ1 0° 0 L3
3 θ2 0° 0 Lc3

[ ] [ ] [ ]
0 −1 0 0 cθ 1 −s θ 1 0 L 3.cθ 1 cθ 2 −s θ 2 0 Lc 3. cθ 2
1 0 0 L2 sθ 1 cθ 1 0 L 3. sθ 1 sθ 2 cθ 2 0 Lc 3. sθ 2
A1= A2= A3=
0 0 1 d1 0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1 0 0 0 1
0
A2= A1. A2

[ ][ ]
0 −1 0 0 cθ 1 −s θ1 0 L 3. cθ 1
1 0 0 L2 sθ 1 cθ 1 0 L3. sθ 1
=
0 0 1 L1 0 0 1 0
0 0 0 1 0 0 0 1

[ ]
−sθ 1 −cθ 1 0 −L3. sθ 1
cθ 1 −sθ 1 0 L 3. cθ 1+ L2
=
0 0 1 d1
0 0 0 1
0
A3=0A2. A3= A1. A2. A3
 [ ][ ]
−sθ 1 −cθ 1 0 −L3. sθ 1 cθ 2 −s θ 2 0 Lc 3. cθ 2
cθ 1 −sθ 1 0 L 3. cθ 1+ L2 sθ 2 cθ 2 0 Lc 3. sθ 2
=
0 0 1 d1 0 0 1 0
0 0 0 1 0 0 0 1

[ ]
−s (θ 1+θ 2) −c (θ 1+θ 2) 0 −Lc 3. s ( θ 1+ θ 2 )−L3. sθ 1
c(θ 1+θ 2) s(θ 1+θ 2) 0 Lc 3.c ( θ 1+ θ 2 )+ L 3. cθ 1+ L2
=
0 0 1 d1
0 0 0 1

x= -Lc3.s(θ1+ θ2)-L3.sθ1 y= Lc3.c(θ1+ θ2)-L3.cθ1+L2 z=d1

[ ][
∂x ∂x ∂x

]
∂d1 ∂θ 1 ∂ θ2
0 −Lc 3. c ( θ 1+θ 2 )−L 3.cθ 1 −Lc 3. c ( θ 1+θ 2 )
∂y ∂y ∂y
Jv3 = = 0 −Lc 3. s ( θ 1+ θ 2 )−L3. sθ 1 −Lc 3. s ( θ 1+ θ 2 )
∂d1 ∂θ 1 ∂ θ2
1 0 0
∂z ∂z ∂z
∂d1 ∂θ 1 ∂ θ2

[ ][ ]
0 −Lc 3. c ( θ 1+θ 2 )−L 3.cθ 1 −Lc 3. c ( θ 1+θ 2 ) d˙1
V3= Jv3.(ḋ 1 ,θ̇ 1 ,θ̇ 2)= 0 −Lc 3. s ( θ 1+ θ 2 )−L3. sθ 1 −Lc 3. s ( θ 1+ θ 2 ) θ˙1
1 0 0 θ˙2

[ ]
(−Lc 3. c ( θ 1+θ 2 )−L3. cθ 1) θ˙1−Lc 3. c ( θ 1+θ 2 ) θ˙2
= (−Lc 3. s ( θ 1+θ 2 )−L 3. sθ 1) θ˙1−Lc 3. s ( θ 1+ θ 2 ) θ˙2
d˙1

V3TV3=
[(−Lc 3. c ( θ 1+θ 2 )−L 3. cθ 1) θ˙1−Lc 3. c ( θ 1+θ 2 ) θ˙2 (−Lc 3. s ( θ 1+θ 2 )−L 3. sθ 1) θ˙1−Lc 3. s ( θ 1+θ 2 ) θ˙2 d˙1 ]

[ ]
(−Lc 3. c ( θ 1+θ 2 )−L3. cθ 1) θ˙1−Lc 3. c ( θ 1+θ 2 ) θ˙2
* (−Lc 3. s ( θ 1+θ 2 )−L 3. sθ 1) θ˙1−Lc 3. s ( θ 1+ θ 2 ) θ˙2
d˙1