UNIVERSIDAD CATÓLICA DE SANTA MARÍA
FACULT ACULTAD DE D E CIENCIAS CI ENCIAS E INGENIERÍAS IN GENIERÍAS FÍSICAS Y FORMALES ESCUELA PROFESIONAL DE INGENIERÍA MECÁNICA, MECÁNICA ELECTRICA Y MECATRÓNICA CURSO: ROBOTICA I TEMA: CINEMATICA DEL ROBOT – ALGORITMO DENAVITHARTENBERG INTEGRANTES: BEJARANO CARBAJAL, CARLOS TENORIO MARIN, EDGAR
AREQUIPA - PERÚ
!"#
CINEMÁTICA INVERSA Y MODELO DIFERENCIAL I. OBJETIVOS I.1. Aplicar y resolver problemas de cinemática inversa del robot y de modelo diferencial, empleando en la solución herramientas matemáticas y de software.
II. MARCO TEÓRICO
MODELO CINEMÁTICO INVERSO La Cinemática inversa (IK) es la técnica que permite determinar el movimiento de una cadena de articulaciones para lorar que un actuador final se ubique en una posición concreta. !l cálculo de la cinemática inversa es un problema comple"o que consiste en la resolución de una serie de ecuaciones cuya solución normalmente no es #nica. !l ob"etivo de la cinemática inversa es encontrar los valores que deben tomar las coordenadas articulares del robot para que su e$tremo se posicione y oriente se#n una determinada locali%ación espacial. &epende de la confiuración del robot 'e$isten soluciones m#ltiples(.
)iempre que se especifica una posición de destino y una orientación en términos cartesianos, debe calcularse la cinemática inversa del dispositivo para poder despe"ar los ánulos de articulación requeridos. Los sistemas que permiten describir destinos términos cartesianos son capaces de mover el manipulador a puntos que nunca fueron capaces de mover el espacio de traba"o a los cuales tal ve% nunca haya ido antes. A estos puntos los llamaremos puntos calculados. !l movimiento de una cadena cinemática ya sea si es un robot o un persona"e animado es modelado por ecuaciones cinemáticas propias de la misma cadena. !stas ecuaciones definen la confiuración de la cadena en términos de sus parámetros. *or e"emplo las fórmulas de la cinemática inversa permiten el cálculo de los parámetros de unión del bra%o de un robot para levantar un ob"eto. +a que una de las órmulas similares es determinar las posiciones del esqueleto de un persona"e animado que se va a mover de una manera en particular.
MÉTODOS !ntre los diversos métodos de solución mediante la cinemática inversa, se pueden distinuir los siuientes-
Mt!"!s #e!mtric!s )e suele utili%ar las primeras variables articulares. so de relaciones eométricas y trionométricas 'resolución de triánulos(.
Matrices "e Trans$!rmaci%n &!m!'nea )e despe"an la n variables en función de las componentes de los vectores.
Desac!amient! Cinemátic! •
!n robots de / 0&L.
•
)eparación y posicionamiento.
Otr!s Mt!"!s "e s!*ci%n •
lebra de tornillo.
•
2uaternios duales.
•
3étodos iterativos.
MODE+O DI,ERE-CIA+ !l ob"etivo de esta cinemática consiste en e$presar el modelo diferencial de un 4obot 3anipulador a través de la 3atri% 5acobiana. 2omo se ha estudiado en los puntos anteriores, la cinemática directa e inversa establece la relación entre las variables de las articulaciones y el efector final, ahora, en este punto la cinemática diferencial se presenta para dar la relación entre las velocidades, lineal y anular 'rotacional( del efector final, esta dependencia se calcula a partir de una matri%, conocida como 63atri% 5acobiana7.
MATRI JACOBIA-A !$presar la velocidad es sencillo porque ella depende de la variación temporal de la posición 'vector p(, pero la situación se complica si se pretende obtener la velocidad anular del efector final como una función de los vectores que definen su orientación 'n, s, a(. *ara lorar una representación m8nima de la orientación m8nima de la orientación es me"or utili%ar los ánulos de !uler. )e tienen entonces seis variables independiente, conocidas como espacio operacional del robot, que facilitan el estudio de trayectorias 'variación de la posición y orientación en el tiempo( del efector final a través de sus derivadas y la relación que estas uardan con los parámetros articulares.
&erivando, se obtiene-
!ntonces, se tiene que-
JACOBIA-A I-VERSA &el mismo modo que se ha obtenido la relación directa que permite obtener las velocidades del e$tremo a partir de las velocidades articulares, puede obtenerse la relación inversa que permite calcular las velocidades articulares partiendo de las del e$tremo. !n la obtención de la relación inversa pueden emplearse diferentes procedimientos, debido a que puede ser bastante complicada su obtención.
)e tiene entonces,
2omo se mencionó anteriormente, e$isten diversos métodos que permiten llear a la obtención de la matri% 5acobiana inversa, el procedimiento más laro consiste en derivar parcialmente respecto a las variables que definen la posición y la orientación, cada una de las funciones obtenidas mediante el estudio cinemático inverso, tal como se muestra a continuación-
4eali%ado todos estos procedimientos, se llea a la conclusión de que es aconse"able buscar una solución más óptima como lo es reali%ar los cálculos a partir del estudio cinemático directo y de la matri% "acobiana, para lueo invertir dicha matri%, el #nico problema que se presenta es que no siempre la matri% 5acobiana es cuadrada, y en caso de no serlo su determinante será nulo y no poseerá inversa. 2uando 5 no es cuadrada se debe a la pretensión de conocer las seis variables que caracteri%an el espacio operacional completo
y no tener seis
rados de libertad 'más o menos de seis rados( en el robot manipulador estudiado. 3as de seis rados de libertad sinifica que cualquier posición y orientación se puede alcan%ar sin mover una de las articulaciones, por lo que la velocidad articular puede asumirse como cero. 3enos de / rados de libertad implica que es imposible que el efector final adquiera alunas orientaciones de manera que uno o varios de los ánulos de !uler puedan obviarse del estudio cinemático y traba"ar nuevamente con una matri% cuadrada. !n casos de que se traba"e de forma numérica y la matri% "acobiana inversa no e$ista, es conveniente el uso de una pseudo inversa (J/JT)01.
23-TOS SI-#3+ARES !ntre una de las aplicaciones de la 3atri% 5acobiana es com#n usarse para calcular los valores de los ánulos y despla%amientos para los cuales la matri% se sinulari%a, es decir, valores para los cuales no tiene inversa. !stos valores se conocen como sinularidades y tienen la particularidad de lorar definir los l8mites del volumen de traba"o de los manipuladores. *ara definir el volumen de traba"o del robot en estudio, los cálculos se resumen a hacer la matri% 5acobiana iual a cero.
IV. 2ROCEDIMIE-TO
I9.1.1 4esolver el *roblema 2inemático &irecto para este robot, es decir, obtener $:f1'q1,q;(, y:f;'q1,q;(, por métodos eométricos y el método de &enavit<=artenber.
Mt!"!s #e!mtric!s *osición-
x =q 1 + L . cos ( 30 ) + L. cos ( 30 + q 2)
y = L . sen ( 30 ) + L . sen ( 30 + q 2 ) z =0
>rientacion-
[ n o a ]= Rotz ( 30 + q 2 )
Denavit &arten4er' &espués de reali%ar el procedimiento de &enavit =artenber, se obtienen los siuientes datos-
1 2
[
Θ
D1
A
α
0
Q1+Lcos(3 0)
Lsen(30)
90
60-Q2
0
L
0
]
( ) −cos ( 90 ) sen ( 0 ) sen ( 90 ) sen( 0 ) Lsen ( 30 ) cos ( 0) sen ( 0 ) cos ( 90 ) cos ( 0 ) − sen ( 90 ) cos ( 0) Lsen ( 30 ) sen ( 0 ) 5 A1 6 0 sen ( 90) cos ( 90 ) q 1 + Lcos ( 30) cos 0
0
0
0
1
A7 6
1
[
( −q 2 ) −cos ( 0 ) sen ( 60−q 2) sen ( 0 ) sen (60 −q 2 ) sen ( 60− q 2) cos ( 0 ) cos ( 60−q 2) − sen ( 0 ) cos ( 60−q 2) 0 sen ( 0 ) cos ( 0 ) 0 cos 60
0
0
0
1
Lcos ( 60 −q 2 ) Lsen ( 60 −q 2 )
]
IV.1.7. Res!ver e 2r!4ema Cinemátic! Invers! ara este r!4!t8 es "ecir8 !4tener 916'1(:8;)< 976'7(:8;)8 9=6'=(:8;). −1
q1 = x − L∗cos (30 )− L∗cos (
sen
( y − Lsen ( 30) ) L
)¿
−1
q 2=
sen
( y − Lsen ( 30) ) L
−30
IV.1.=. Ea4!rar *n r!'rama en MAT+AB 9*e ermita !4tener a cinemática inversa "e R!4!t "e 7#D+ "a"!s !s va!res "e as varia4es "e !cai>aci%n esacia. clear all close all clc L=input('L='); x=input('X='); y=input('Y='); q1=x-L*cos(pi/6)-L*cos((asin(y-L*sin(pi/6)))/L); q2=asin((y-L*sin(pi/6))/L)-pi/6;
I9.1.?. >btener la e$presión de la matri% 5acobiana anal8tica de este robot. J =
[
1 0
]
− Lsen( 30 + q 2 ) Lcos ( 30 + q 2 )
IV.1.5. Obtener l e!"res#$n %e l &tr#' cob#n %e este robot. Ja =¿
I9.1./. @uscar e interpretar las confiuraciones sinulares si es que e$isten.
I9.1.. 2alcular la posición y velocidad del e$tremo para los siuientes valores- L : 1mB 'q1,q;( : '1 m,CD/ rad(B 'qE1,qE;( : 'F.; mDse, CD1F radDse(. x =q 1 + L . cos ( 30 ) + L. cos ( 30 + q 2)
y = L . sen ( 30 ) + L . sen ( 30 + q 2 ) x =1 + cos ( 30 ) + cos ( 30 + π / 6)
x =1 + 0.866 + 0.5 =2.366
y = sen ( 30 ) + sen ( 30 + π / 6 ) y =0.5 + 0.866=1.366
[][ ´ [ ´ ]= [ [ ´´ ]=[ ´ x ´ y
=
1 0
][ ]
− Lsen ( 30 + q 2) q 1 Lcos ( 30 + q 2 ) q 2
x y
1
−0.866
0
0.5
x y
0.54656 0.2618
][ / ] 1
π 6
]
I9.1.G. !laborar un prorama en 3AHLA@ que permita obtener el modelo diferencial del 4obot de ;0&L dados los valores de las velocidades a rticulares o las velocidades de la locali%ación espacial.
clear all clc disp('li!a una opcion"' ); disp('1# $n%resar &elocidades articulares"' ) disp('2# $n%resar &elocidades de localiacion espacial"' ) o=input(''); i o==1 L1=input('') L2=input('') q1=input('') q2=input('') x=L1*cosd(q1)L2*cosd(q1q2); y=L1*sind(q1)L2*sind(q1q2); =+-L1*sind(q1)-L2*sind(q1q2),L2*sind(q1q2);L1*cosd(q1)L2*cosd(q1q2),L2*cosd(q1q2) .=+q1;q2 =*. &x=(1,1) &y=(2,1) else i o==2 L1=input('') L2=input('') x=input('') y=input('') q2=acosd(((x02)(y02)-(L102)-(L202))/(2*L1*L2)); q1=atan(-x/y)-atan((L2*sind(q2))/(L1(L2*cosd(q2)))); sys q1; sys q2; $=+di(q1,x),di(q1,y);di(q2,x),di(q2,y) =+x;y .=$* &q1=.(1,1) &q2=.(2,1) end end
I9.;. )ea el robot )2A4A de la fiura ;, se pide-
,i'*ra 7
I9.;.1. 4esolver el problema cinemático inverso para este robot. '4!)>L9!4 @A)A&>)! ! LA )>L2IJ &!L *2& )!0K LA@>4AH>4I> AH!4I>4( '9!4II2A4 LA 2>44!2HA A)I0A2IJ &! L>) )I)H!3A)(
[ [
Θ
D1
A
α
1
Q1
L1
L2
0
2
Q2
0
L3
0
3
0
-Q3
0
0
Q
0
0
0
( q 1 ) −cos ( 0 ) sen ( q 1 ) sen ( 0 ) sen ( q 1 ) −sen ( 0 ) cos ( q 1 ) cos ( 0 ) cos ( q 1 ) sen ( q 1 ) 5 A1 6 0 sen ( 0 ) cos ( 0 ) L 1 cos
0
0
0
1
( q 1 ) −cos ( 0 ) sen ( q 2 ) sen ( 0 ) sen ( q 2 ) −sen ( 0 ) cos ( q 2 ) cos ( 0 ) cos ( q 2 ) sen ( q 1 ) 1 A7 6 0 sen ( 0 ) cos ( 0 ) L 1 cos
0
0
0
L 2 cos ( q 1 ) L 2 sen ( q 1 )
1
L 2cos ( q 1 ) L 2 sen ( q 1 )
] ]
[ [
( ) −cos ( 0 ) sen ( 0 ) sen ( 0 ) sen ( 0 ) −sen ( 0 ) cos ( 0 ) cos ( 0 ) cos ( 0 ) sen ( 0 ) 7 A= 6 0 sen ( 0 ) cos ( 0 ) − q 3 cos 0
0
0
0
1
0 0
]
( q 4 ) −cos ( 0 ) sen ( q 2 ) sen ( 0 ) sen ( q 4 ) sen ( q 4 ) cos ( 0 ) cos ( q 2 ) −sen ( 0 ) cos ( q 4 ) = A? 6 0 sen ( 0 ) cos ( 0 ) 0 cos
0
0
0
1
0 0
]
T6
q1 =tan
−1
( x ) y
−1
q 2=cos
(
y − L 2∗sin q 1 ) L 3
q3 = L 1 − z
−1
q 4=cos
− 2∗cos q 1 ( x L ) L 3
I9.;.;. &eterminar el modelo diferencial del robot. 'H>&A) LA) 5A2>@IAA)(
I9.;.. &eterminar las confiuraciones sinulare s del robot )2A4A, si es que e$isten I9.;.?. !laborar los proramas en 3AHLA@ que permitan obtener los modelos cinemático inverso y el modelo diferencial dados los datos adecuados. clear all close all clc l1=input('L1='); l2=input('L2='); l3=input('L3='); x=input('X='); y=input('Y='); =input('4='); q1=atan(x/y); q2=acos((y-l2*sin(atan(x/y)))/l3);
q3=l1-; q5=acos((x-l2*cos(atan(x/y)))/l3); sys sys sys sys
q1; q2; q3; q5;
$=+di(q1,x),di(q1,y),di(q1,);di(q2,x),di(q2,y),di(q2,); di(q3,x),di(q3,y),di(q3,);di(q5,x),di(q5,y),di(q5,) =+x;y; .=$* &q1=.(1,1) &q2=.(2,1) &q3=.(3,1) &q5=.(5,1)
I9.;.M. 2alcular la posición y velocidad del e$tremo del robot estableciendo alunos valores de prueba.