Download - Examen-Robotica-Final (1)
-
7/25/2019 Examen-Robotica-Final (1)
1/14
UNIVERSIDAD AUTNOMA DEYUCATN
-
7/25/2019 Examen-Robotica-Final (1)
2/14
Facultad de Ingeniera
Prier !arcial"R#$%tica I&
PROFESOR'M(C( CRU)*IM+NE),RAU-IO*OS+
Entrega gru!al
Fec.a de entrega'/0 de *uni# de /123
Universidad Autnoma de
Yucatn Ingeniera en
Mecatrnica Robtica I
-
7/25/2019 Examen-Robotica-Final (1)
3/14
Nombre :
Fecha :
NOTA' Ante4 de c#nte4tar5 lee cuidad#4aente l# 6ue 4e te !ide(
1. !a con"iguracin de# $UMA %&& con #os sistemas coordenados se muestra en #a
siguiente "igura. 'eterminar si se ha seguido e# a#goritmo de 'enavit(artenberg.
'etermine #os )armetros cinemticos estructura#es* com)#ete tab#a )ara e#
mani)u#ador + ha##e su mode#o cinemtico inverso.
-
7/25/2019 Examen-Robotica-Final (1)
4/14
A )artir de# an#isis de #os sistemas de re"erencia anteriores + de #a dis)osicin de #os
mismos en sus e,es x + y * )rinci)a#mente* se determina -ue )ara e#estab#ecimiento de a#gunos de estos sistemas no se ha seguido e# a#goritmo 'enavit(artenberger
on base en #os nuevos sistemas de re"erencia )ro)uestos* es )osib#e determinar #a tab#a
'enavit (artenver
i ai i d i /i)o
1 1
& & & R
0 2
& 90 & R
3 a
2& d
3R
2 4
a3
90 d4
R
3 5
& 90 & R
% 6
& 90 & R
!a ecuacin de trans"ormacin T de# robot $UMA estar dada )or:
T=A 60=A 1
0 A 2
1 A 3
2 A4
3 A 5
4 A6
5
'onde #as matrices de cada es#abon ser
-
7/25/2019 Examen-Robotica-Final (1)
5/14
A1
0=[c
1 s
1
s 1 c 1
0 0
0 0
0 0
0 0
1 0
0 1] A 12=[
c 2
s 2
0 0
0 0
1 0
s 1
c 2
0 0
0 0
0 1] A 23=[
c 3 s 3s
3 c
3
0 a20 0
0 0
0 0
1 d30 1
]A
3
4=[ c
4 s
4
0 0
0 a3
1 d4
s 4 c 40 0
0 0
0 1] A 45=[
c 5
s 5
0 0
0 0
1 0s
5 c
5
0 0
0 0
0 1]
A5
6=[ c
6 s
6
0 0
0 0
1 0
s 6
c 6
0 0
0 0
0 1]
4ntonces tenemos -ue #a matri5 de trans"ormacin nos -ueda
T60 =A 6
0=[r
11 r
12 r
13 px
r21
r22
r23
pyr
31 r
32 r
33 pz
0 0 0 1]
'onde:
( 6)s(
4)+c (
4)(s
6)
( 5)c c
6s (2+3 ) s (5)+c (2+3)(c (4 ) c (5 )(c(6)s ( 4)s (6)))
c + c 1
r11=s (1)
r21=c (6)s(1)s ( 2+3 ) s (5 )c (1)(c ( 5 ) c ( 6 ) s ( 4 )+c ( 4 ) s ( 6 ))+c (2+3 ) s( 1)(c ( 4)c (5 )c (6r31=c (6)(c ( 4 ) c (5 ) s ( 2+3 )+c (2+3 ) s (5 ))+s ( 2+3 ) s(4)s (6)
2+
3
s ()s(5)s( 6)c (2+3)(c (6)s(4)+c (4)c (5)s( 6))r12=s(1) (c (4) c (6)c (5 ) s (4 ) s (6 ))+c (1)
r22=c (1 )(c ( 4 ) c ( 6)+c (5 ) s ( 4) s (6 ))+s ( 1)( s (2+3 ) s (5 )s( 6)c (2+3)(c (6)s(4)+c (4)cr
32=c (2+3 ) s (5 ) s ( 6 )+ s( 2+ 3)(c (6 ) s (4)+c ( 4 ) c (5 ) s (6 ))
r13
=s (1) s (4 ) s (5 )c (1) (c (5 ) s (2+ 3 )+c ( 2+ 3 ) c (4 ) s (5 )) r
23=c (1 ) s (4 ) s (5 )s (1 )(c (5 ) s (2+3 )+c (2+3 ) c (4 ) s (5 ))
r33
=c ( 2+ 3 ) c ( 5 )+c ( 4) s (2 +3 ) s (5 ) px=d 3 s (1 )+c (1 )(a2 c (2 )+a3 c (2+3 )d4 s (2+3 )) py=d3 c (1)+s (1 )(a2c (2)+a3 c (2+3 )d4s (2+3 ))
-
7/25/2019 Examen-Robotica-Final (1)
6/14
2
2+3
pz=d4 c ( 2+ 3 )a2 s
4n base a #a matri5 de trans"ormacin * )rocederemos a ca#cu#ar #a cinemtica inversa* se )one#a de)endencia 1 de# #ado i5-uierdo de #a ecuacin:
(A10)1T=A 2
1 A 3
2 A 4
3 A 5
4 A6
5
[ c (
1) s(
1) 0 0
s(1) c( 1) 0 00 0 1 0
0 0 0 1][
r11
r12
r13
pxr
21 r
22 r
23 py
r31
r32
r33
pz0 0 0 1
]= A61(A2
1
)1
(A 10
)1
T=A 32
A 43
A54
A65
(A32)1 (A 2
1)1 (A 10)1 T=A 4
3 A5
4 A 6
5
+ as sucesivamente.
'e #as ecuaciones de )osicin se uti#i5a una sim)#e t6cnica en #a -ue se mu#ti)#ica cada #ado )or
una inversa -ue es cada es#abn de# robot )ara as encontrar #as variab#es q .
'e #a )rimera mu#ti)#icacin de es#abones* )odemos obtener #os e#ementos 70*28 de ambos #ados de
#a ecaucin* as obtenemos:
s 1Px +c 1Py=d 3
$ara reso#ver estas ecuaciones usamos identidades trigonom6tricas
Px=c Py=s
=Px2+Py
2
=Atan 2(Px , Py)9ustitu+endo
c 1 ss 1 c=d 3
/rignom6tricamente
s ( 1 )=d 3
d 3, Px2+Py
2d 32
1=Atan 2 (Px , Py )Atan 2
/enemos ahora #as )osib#es so#uciones )ara 1 #o -ue nos )ermite tomar ahora e#ementos de
#a mu#ti)#icacin 71*28 + 7*28 e igua#amos ambos #ados:
C1Px+S1Py=a3 C23d4 S23+a2 C2 ,Pz=a3 S23+d 4 C23+a2 S2
-
7/25/2019 Examen-Robotica-Final (1)
7/14
9i e#evamos a# cuadrado estas ecuaciones con #a )rimera -ue obtuvimos + #as sumamos
obtenemos:
a3
C3d
4S
3=K
K=Px
2+Py2+Pz
2a22a3
2d32d4
2
2 a2
Ahora -ue hemos removido 1 * )odemos seguir reso#viendo )ara ca#cu#ar #os siguientes
angu#os* entonces obtenemos -ue:
3=Atan 2 (a3 , d4 )Atan 2 (k ,a32 +d4 2K2)Ahora vo#vamos a tener en cuenta #a matri5 de trans"ormacin* )odemos escribir de# #ado
i5-uierdo en "uncin de #o -ue conocemos + 2 :
[
c (1)c (
2+
3) s (
1)c(
2+
3) s (
2+
3) a
2c (
3)
c (1) s(
2+
3) s(
2+
3) s(
1) c(
2+
3) a
2s(
3)
s (1) c (
1) 1 d
3
0 0 0 1
] [
r 11 r 12 r13 pxr
21 r
22 r
23 py
r31
r32
r33
pz0 0 0 1
]= A
6
1
/omamos #os e#ementos 71*28 + 70*28 + #os igua#amos con sus corres)ondientes + obtenemos:
C1
C23
Px +S1 C23PyS23Pza2C3=a3C
1S
23PxS 1 S23P yC23Pz+ a2 S3=d4
4stas ecuaciones )ueden ser resue#tas simu#taneamente )ara c (2+3) + s (2+3)
a(3a2 C3)Pz+(C1Px+S1Py )(a2 S3d4)
Pz2
+(C1Px+S1Py)2
S23=
a
(2 S3d4)Pz+(a3+a2C3)(C1Px+S1Py)
Pz2+(C1Px+S1Py)
2
C23=
!os denominadores son igua#es + )ositivos* )or #o tanto reso#vemos )ara #a suma de 2y 3
23=Atan2
((a3a2C3 )Pz(C1Px+S1Py ) (d 4a2 S3 ) , (a2 S3d4 )Pz(a3+a2C3)(C1Px + S1Py ))bteniendo #os va#ores resu#tan cuatro )osib#es va#ores de 2 +3 debido a #ascombinaciones de 2y 3 )or #o tanto se obtienen:
2=
23
3
9iem)re + cuando
s ( 5) 0
)odremos reso#ver )ara 4 de #a siguiente manera:
4
=Atan2(r13
S1
+r23
C1
,r13
C1
C23
r23
S1
C23
+r33
S23
)
-
7/25/2019 Examen-Robotica-Final (1)
8/14
4 se e#ige arbitrariamente + cuando ca#cu#emos 6 * se ca#cu#a acorde con e##o.
Ahora )odemos escribir #a ecuacion
[ c (
1) s (
1) 0 0
s(1) c( 1) 0 0
0 0 1 00 0 0 1
][r
11 r
12 r
13 px
r21
r22
r23
py
r31 r32 r33 pz0 0 0 1
]= A6
1
'e modo -ue todo e# #ado i5-uierdo sea una "uncin so#amente de variab#es conocidas + 4
(A40)1 T=A 5
4 A 6
5
Igua#ando #os e#ementos 71*8 + 7*8 de esta mu#ti)#icacin obtenemos
r13 (C1 S23 )+r23(S 1 S23 )+r33(C23 )=C5
$or #o tanto )odemos reso#ver )ara 5
5=Atan 2 (S5 ,C5 )
4n donde
s ( 5)
+
c ( 5)
se obtienen mediante #a ecuacin
r13 (C1 S23 )+r23(S 1 S23 )+r33(C23 )=C5
ontinuando #as mu#ti)#icaciones de #a matri5:
(A50)1 T=A 6
5
Igua#ando #os e#ementos de #a matri5 7*18 + 71*18 de ambos #ados de #a ecuacin* obtenemos:
6=Atan 2(S6 , C6)
4n donde
S6=r
11 (C1 C23 S4S1 C4)r21(S1 C23 S4+C1 C4 )+r31 ( S23 C4 )21( ( S1 C23 C4C1 S 4 ) C5S1 S23)r31 (S23 C4 C5+C23 S5 )
C6=r11 ((C1C23 C4+S1 S4 ) C5C1 S23 S 5 )+r
$ara cada una de #as cuatro so#uciones ca#cu#adas antes* obtenemos #a so#ucin inversa mediante:
4=
4+180
5=
5
'6=
6+180
-
7/25/2019 Examen-Robotica-Final (1)
9/14
0. $ara e# robot ti)o 9ARA -ue se muestra en #a "igura:
a8 Asigne #os sistemas coordenadas basndose en #a re)resentacin o a#goritmo
'enavit(artenberg.
b8 4scriba #a ecuacin de trans"ormacin.
a8 !os sistemas de coordenadas )ro)uestos de acuerdo con e# a#goritmo 'enavit
(artenberg )ara #os sistemas de re"erencia de cada es#abn de# robot se muestran en #a
siguiente "igura:
-
7/25/2019 Examen-Robotica-Final (1)
10/14
A )artir de #os sistemas )ro)uestos* se determina #a tab#a 'enavit(artenberg de# robot
9ARA:
i ai i d i /i)o
1 1
& & d1
R
0 290 d
3& d
2R
3 +90 d4 & & R2 & & 180 d
5$
b8 !a ecuacin de trans"ormacin T de# robot 9ARA estar dada )or:
T=A 40=A1
0 A2
1 A3
2 A 4
3
9iendo #as matrices )ara cada trans"ormacin:
A1
0=
[c
1 s
1
s 1 c 1
0 0
0 0
0 0
0 0
1 d10 1 ]
A3
2=[c
3+ 90 s
3+ 90
s 3 +90 c 3+90 0 d
4
0 0
0 0
0 0
1 0
0 1]
A2
1=
[c
290 s
290
s 290 c 290
0 d3
0 0
0 0
0 0
1 d20 1 ]
A4
3=[1 0
0 c 180
0 0
s 180 00 s 180
0 0
c 180 d50 1
]$or tanto* #a T ser:
-
7/25/2019 Examen-Robotica-Final (1)
11/14
2. Un bra5o robtico con grados de #ibertad ha sido dise;ado )ara a)#icar )intura a
una )ared como se muestra en #a "igura.
a8 Asigne #os sistemas coordinados basndose en #a )resentacin '(.
b8 !#ene #a tab#a de )armetros
c8 (a##e e# mode#o cinemtico inverso.
a8 !os sistemas de coordenadas )ro)uestos de acuerdo con e# a#goritmo 'enavit(artenberg
)ara #os sistemas de re"erencia de cada es#abn de# robot se muestran en #a siguiente "igura:
b8 A )artir de #os sistemas )ro)uestos* se determina #a tab#a 'enavit(artenberg de# robot
cartesiano:
i ai i d i /i)o
1 1
& & l3
R
0 & & & l4
$
& l5
90 & $( 90 & & l
6$
-
7/25/2019 Examen-Robotica-Final (1)
12/14
c8 A )artir de #a tab#a '( anterior* es )osib#e obtener #as matrices A + T -ue
)ermitirn determinar #a cinemtica inversa de# robot cartesiano:
T=AH
0 =A 10
A21
A32
AH
3
9iendo #as matrices )ara cada trans"ormacin:
A1
0=[c
1 s
1
s 1
c 1
0 0
0 0
0 0
0 0
1 l30 1
]A
3
2=
[1 0
0 c(90 )0 l
5
s (90 ) 0
0 s (90 )0 0
c (90 ) 00 1
]
A2
1=[1 0
0 1
0 0
0 0
0 0
0 0
1 l40 1
]A H
3 =
[c 90 s 90s 90 c 90
0 0
0 0
0 0
0 0
1 l6
0 1
]
-
7/25/2019 Examen-Robotica-Final (1)
13/14
onsiderando #a matri5 T :
A 40
=
[nx xny y
ax pxay py
nz z0 0
az pz0 1 ]A)#icamos #a matri5 inversa (A10)1 sobre /
(A10)1 T=A 2
1 A
3
2 A H
3
(A10)1
T=[c1 nx+c1 ny c1 x+s1 yc
1nyc1 nx c1 ys1 x
c1 ax+s1 ay c1px+s1pyc
1ays1 ax c1pys1px
nz z0 0
az pz0 1 ]=[
0 10 0
0 l51 l
6
1 00 0
0 l 40 1 ]
/omando #os e#ementos 71*28 + 70*28 se )rocede a buscar 1 :
c1px+ s1py=l5
c1pys1px=l6
9e dividen ambas ecuaciones entre c1 )ara obtener tan1
px + tan1py =l
5
c1
pytan1px=l
6
c1
Ahora dividimos una entre otra )ara obtener:
(px+tan 1py )pytan1px
=l
5
l6
-
7/25/2019 Examen-Robotica-Final (1)
14/14
l5
l6=K
(px+tan 1py )pytan1px
=K
9i arreg#amos #a ecuacin de ta# "orma -ue des)e,emos tan1 )odemos ha##ar 1 :
tan1(pyk +px )=(py
px
K)
tan1(pypxK)(pyk+px)
1=arctan ((pypxK)
(pyk +px )) ,