Download - Capítulo_2 dinámica
-
7/23/2019 Captulo_2 dinmica
1/30
Borrador del libro: ROBOTICA: MODELADO,SIMULACIN Y DISEO CON MATLAB.R.Saltarn, M.Almonacid, J.M.Azorn, J.M.Sabater 19/04/06
1
Captulo2
Cinemtica de Robots Clsicos
La nica emergencia para la que se est siemprepreparado es aquella que nunca sucede.
(Ley de Pfeiffer)
2.1.- Introduccin
Los robots clsicos presentan una arquitectura antropomrfica serial, semejante albrazo humano. Consisten de una serie de barras rgidas unidas entre s a travs dearticulaciones de un grado de libertad del tipo rotacional o prismtica. En general cadaarticulacin logra su movimiento a travs de un accionamiento de potencia e incluyeotros dispositivos como reductores de velocidad, frenos y sensores de posicin ovelocidad.
Aunque al definir las relaciones cinemticas de un robot no se suelen consideran losaspectos dinmicos, nada ms alejado de la realidad cuando se quiere disear un robot
ya que existe una inevitable relacin causa-efecto entre la cinemtica y la dinmica.Nada ms claro resulta que al pensar en las dimensiones de un robot, la longitud de unbrazo afecta al cuadrado la inercia de los eslabones y por lo tanto el peso del robot y lapotencia requerida en los actuadores.
Las arquitecturas de los robots clsicos presentan una serie de propiedadesdinmicas y estructurales caracterizadas por una gran rigidez estructural, repetibilidad yelevado peso propio. El elevado peso propio de los robots clsicos limita la capacidadcarga til y las velocidades de trabajo, las cuales usualmente estn en torno a los 60grados/seg. para las primeras tres articulaciones de los robots industriales (robots desoldadura) y 250 grados/seg. para los robots pequeos de altas prestaciones como el
STUBLI RX90.
-
7/23/2019 Captulo_2 dinmica
2/30
Borrador del libro: ROBOTICA: MODELADO,SIMULACIN Y DISEO CON MATLAB.R.Saltarn, M.Almonacid, J.M.Azorn, J.M.Sabater 19/04/06
2
Un aspecto importante que refleja las relaciones dinmicas del robot respecto de lacarga til que pueden manipular, puede estudiarse en la siguiente tabla.
Robot Peso Carga til Repetibilidad Carga til/peso
ABB IRB 2000 370 10 0.100 0.0270ABB IRB 4400 940 45 0.100 0.0047ABB IRB 6400/3.0 1450 75 0.100 0.0510STUBLI RX 90 120 6 0.002 0.0500GMF S 10 200 10 0.200 0.0417Hitachi M6100 410 10 0.100 0.0243Puma 550 63 4 0.100 0.0063SCARA Adept 3 205 25 0.025 0.1220SCARA GMF A-600 120 6 0.013 0.0500
Al calcular la cinemtica de los robots clsicos debe considerarse que dependiendode las dimensiones de sus primeras articulaciones, el peso de los robots de tipoindustrial oscila en torno a valores que tienen una relacin en el mejor de los casos de0.150 (Carga til/peso). Por lo cual, por ejemplo un robot industrial con un alcance de3.0 metros con capacidad para mover cargas de 75 kg puede tener un peso de 1450 kg(ABB IRB 6400).
Las siguientes son algunas recomendaciones que deben tenerse en cuenta al definirla cinemtica de un robot, la cual debe hacerse en consideracin de la dinmica queimponen las dimensiones de las barras que lo forman:
El espacio de trabajo del robot debe ser cuidadosamente estudiado para definir elvolumen justo de trabajo del robot
En un robot de seis grados de libertad rotacional, las primeras tres barras son las queaportan la mayor dinmica debido a su peso. A menudo es posible localizar los
primeros tres accionamientos de potencia en la base del robot, pero para lograr estose debe ser cuidadoso en el uso de mecanismos de cuatro barras que mueven el
brazo mas alejado (robot ABB IRB2400). En un robot de seis grados de libertad, las tres primeras articulaciones del robot
deben dar las condiciones de posicin y las tres ltimas articulaciones del extremodel robot deben concentrar en un punto de la mano, los tres grados de libertad de
orientacin.
En este captulo se van a presentar las herramientas necesarias para resolver los dosproblemas fundamentales en el estudio de la cinemtica del robot. El primero de ellos,consiste en determinar la posicin y orientacin del extremo final de la cadenacinemtica conocidos los valores de las coordenadas articulares y las caractersticasgeomtricas del robot, y es conocido como problema cinemtico directo. La solucindel problema inverso permite hallar las variables articulares conocida la posicin yorientacin del extremo de la cadena cinemtica. Para la resolucin de estos problemasse utiliza la representacin de Denavit-Hartenberg y las matrices de transformacinhomognea. Por ltimo se plantean las relaciones entre las velocidades de las
articulaciones y las del extremo del robot, que vienen expresadas por la matrizJacobiana.
-
7/23/2019 Captulo_2 dinmica
3/30
Borrador del libro: ROBOTICA: MODELADO,SIMULACIN Y DISEO CON MATLAB.R.Saltarn, M.Almonacid, J.M.Azorn, J.M.Sabater 19/04/06
3
De acuerdo con la estructura del libro, todos los apartados presentanherramientas desarrolladas en MatLab para la solucin de los problemas planteados. Alfinal del captulo se plantea un ejemplo global donde se soluciona la cinemtica de unrobot con arquitectura clsica.
2.2- Cinemtica directa del brazo de un robot manipulador
Las tcnicas que se estudian aqu, se aplican a un manipulador mecnico decadena abierta y tratan el estudio analtico y el modelado en MatLab de la geometra delmovimiento de un robot con respecto a un sistema de referencia fijo como una funcindel tiempo sin considerar la dinmica.
2.2.1 El problema cinemtico directo
El problema cinemtico directo se plantea en trminos de encontrar una matrizde transformacin que relaciona el sistema de coordenadas ligado al cuerpo enmovimiento respecto a un sistema de coordenadas que se toma como referencia. Paralograr esta representacin se usan las matrices de transformacin homognea 4x4, lacual incluye las operaciones de traslacin y la orientacin.
La matriz de transformacin homognea es una matriz de 4x4 que transforma unvector expresado en coordenadas homogneas desde un sistema de coordenadas hastaotro sistema de coordenadas. Para una descripcin ms amplia acerca de las basesalgebraicas de las transformaciones homogneas se recomienda estudiar las referencias:[1] y [2].
La matriz de transformacin homognea tiene la siguiente estructura:
=
=
100031 zzzz
yyyy
xxxx
x pasn
pasn
pasn
escaladof
posicindevectorrotacindematrizT
=
1000
pasnT
donde los vectores n, s, a, son vectores ortogonales unitarios y p es un vector quedescribe la posicin x, y, z del origen del sistema actual respecto del sistema dereferencia.
Para entender las propiedades de la matriz de transformacin homognea nos fijamos enel siguiente grfico.
-
7/23/2019 Captulo_2 dinmica
4/30
Borrador del libro: ROBOTICA: MODELADO,SIMULACIN Y DISEO CON MATLAB.R.Saltarn, M.Almonacid, J.M.Azorn, J.M.Sabater 19/04/06
4
Figura-2.1. Interpretacin geomtrica de la matriz de transformacin homognea
=
1000
3001
4010
10100
j
i T
Al analizar las columnas de la submatriz de rotacin de la matriz detransformacin homogneaiTj, un observador localizado en el origen de sitema-i, puedever como estn orientados los ejes x, y, zdel sistema-j, adems tambin observa comose ha desplazado en coordenadas cartesianas el origen del sistema-jrespecto del origendel sistema de referencia con la informacin del vector de posicin.
2.2.2 La representacin de Denavit-Hartenberg
La representacin de D-H, se aplica a robots de cadena cinemtica abierta y consisteen una serie de reglas para colocar los sistemas de referencia de cada eslabn del robot.Antes de aplicar el mtodo de D-H es importante tener en cuenta los siguientescomentarios:
Se parte de una configuracin cualesquiera del robot, si bien es aconsejablecolocarlo en una posicin sencilla de analizar. Se numeran los eslabones, asignando el 0para la base y n-1para el ltimo eslabn,
siendo nel nmero de grados de libertad del robot. El sistema de coordenadas ortonormal dextrgiro de la base (x0, y0, z0)se establece
con el eje z0 localizado a lo largo del eje de movimiento de la articulacin 1 yapuntando hacia fuera del hombro del brazo del robot.
El sistema de referencia de cada eslabn se coloca al final del mismo, en el extremode la articulacin a la cual esta conectado el eslabn siguiente.
El ngulo desplazamiento de cada eslabn siempre se mide tomando como base elsistema de referencia del eslabn anterior.
x
y
z
z
x
y
iTj
Sistema-i
Sistema-j
p=(10, 4, 3)
-
7/23/2019 Captulo_2 dinmica
5/30
Borrador del libro: ROBOTICA: MODELADO,SIMULACIN Y DISEO CON MATLAB.R.Saltarn, M.Almonacid, J.M.Azorn, J.M.Sabater 19/04/06
5
Al colocar el sistema de referencia del eslabn-i, se deben seguir las siguientesreglas:- El eje zi del sistema de referencia debe quedar alineado a lo largo de la
articulacin- El eje xidebe colocarse con orientacin normal al plano formado por los ejes zi-1
y zi Al establecer los sistemas de coordenadas de la mano se debe tener en cuenta elprincipio de Piepers en el cual se establece que los tres ltimos sistemas dereferencia se intercepten en un punto, lo cual permite obtener una solucin para el
problema cinemtico inverso de forma cerrada para estas articulaciones.
Adems de las reglas anteriores la convencin de D-H establece las siguientescondiciones para los dems parmetros geomtricos, de acuerdo a la figura-2.2.
Figura-2.2. Sistemas de coordenadas para la convencin de D-H.
Cada sistema de coordenadas se establece sobre las siguientes reglas.
i: Es el ngulo de la articulacin desde el eje xi-1 hasta el eje xi, medidorespecto del eje zi-1, usando la regla de la mano derecha.di: Es la distancia medida desde el origen del sistema i-1, a lo largo del eje zi-1hasta la interseccin del eje zi-1con el eje xi .ai: Es la distancia de separacin entre los orgenes de los sistemas de referenciai-1e i, medida a lo largo del eje xi hasta la interseccin con el eje zi-1. (o ladistancia ms corta entre los ejes zi-1y zi, cuando estos no se interceptan)i: Es el ngulo que separa los ejes ziy zi-1, medido respecto del eje xi
di
ai
i
i
xi-1
zi-1
xi
zi
Eslabn i-1
Eslabn i
Eslabn i+1
Articulacin i Articulacin i+1
ii+1
-
7/23/2019 Captulo_2 dinmica
6/30
Borrador del libro: ROBOTICA: MODELADO,SIMULACIN Y DISEO CON MATLAB.R.Saltarn, M.Almonacid, J.M.Azorn, J.M.Sabater 19/04/06
6
Con base en la figura-2.2 y de acuerdo a las reglas de D-H, se determina la siguientematriz de transformacin homognea:
=
0000
00
00
0001
1000
0100
0010
001
1000
0100
00
00
1000
100
0010
0001
1
ii
ii
i
ii
ii
i
i
i
cs
sc
a
cs
sc
dA
=
1000
01
iii
iiiiiii
iiiiiii
i
i
dcs
sacsccs
cassscc
A
Cdigo en M atlab. La funcin DENAVIT realiza los clculos anteriores devolviendo lamatriz de transformacin homognea
% DENAVIT Matriz de transformacin homognea.% DH = DENAVIT(TETA, D, A, ALFA) devuelve la matriz de transformacion% homognea 4 x 4 a partir de los parametros de Denavit-Hartenberg% D, ALFA, A y TETA.%% See also DIRECTKINEMATIC.
functiondh=denavit(teta, d, a, alfa)dh=[cos(teta) -cos(alfa)*sin(teta) sin(alfa)*sin(teta) a*cos(teta);
sin(teta) cos(alfa)*cos(teta) -sin(alfa)*cos(teta) a*sin(teta);0 sin(alfa) cos(alfa) d;0 0 0 1];
NOTA: todas las funciones de este captulo se
encuentran disponibles en el directorio capitulo 2 del cdadjunto.
-
7/23/2019 Captulo_2 dinmica
7/30
Borrador del libro: ROBOTICA: MODELADO,SIMULACIN Y DISEO CON MATLAB.R.Saltarn, M.Almonacid, J.M.Azorn, J.M.Sabater 19/04/06
7
2.2.3 Representacin de la cinemtica directa de robots manipuladores
En esta seccin se explican algunas arquitecturas de robots y como construir latabla de los parmetros de D-H. Para una informacin ms detallada sobre este tema, serecomienda estudiar las referencias [1] y [2].
Ejemplo 2.1
Figura-2.3 Parmetros de D-H para un robot cilndrico
Tabla 2.1 Parmetros de D-H para el robot cilndrico de la figura-2.3
Eslabn i di ai i1 1 l1 0 02 0 d2 - a2 -/23 0 d3 0 04 4 l4 0 0
Note el lector el signo negativo del parmetro a2as como la localizacin del origendel sistema de coordenadas (x2, y2, z2)
Las variables articulares son en este caso 1,d2, d3,4
z1
1
x0
y0
z0
1 y1
z2
y2x2
3
y3
l1
d2
d3
a2l4
0
z4
1
23
4 4
x4
y4
z3
-
7/23/2019 Captulo_2 dinmica
8/30
Borrador del libro: ROBOTICA: MODELADO,SIMULACIN Y DISEO CON MATLAB.R.Saltarn, M.Almonacid, J.M.Azorn, J.M.Sabater 19/04/06
8
Ejemplo 2.2
Figura-2.4 Parmetros de D-H para un robot rotacional
Tabla 2.2 Parmetros de D-H para el robot cilndrico de la figura-2.4
Eslabn i di ai i
1 1+/2 l1 0 -/22 2-/2 0 a2 03 3+ 0 0 /24 4 l4 0 /25 5 0 0 /26 6 l6 0 0
Note el lector que en este caso todas las variables articulares corresponden a losgiros en las articulaciones.
Obsrvese la rotacin en 4entre los eslabones 3 y 4. Obsrvese que el sistema de referencia (x3, y3, z3) se ha trasladado al principio del
eslabn 3 cumpliendo con la representacin de D-H.
x0
z0
y0
1
z2
y2
x2
3
z4y4
x4
s
a
n
6
5
y5
z5
x5
y3
z3
x3
4
z1
y1x1
l1
a2
l4
l6
0
1
2
3
45
6
-
7/23/2019 Captulo_2 dinmica
9/30
Borrador del libro: ROBOTICA: MODELADO,SIMULACIN Y DISEO CON MATLAB.R.Saltarn, M.Almonacid, J.M.Azorn, J.M.Sabater 19/04/06
9
Cdigo en M atlab.La funcin DIRECTKINEMATIC resuelve la cinemtica directa derobots de cadena abierta. Para utilizarla debe invocarse junto con el vector decoordenadas articulares Qde ncomponentes siendo nel nmero de eslabones del robot.
A continuacin se muestra el cdigo fuente utilizado para cada uno de los robots
que se estn utilizando como ejemplos. El lector debe notar las modificacionesrealizadas en el cdigo, debido al diferente nmero de eslabones de cada robot y a losparmetros D-H de cada robot. Se recomienda que se realicen estos ejemplos comoprctica de los conocimientos adquiridos.
Ejemplo 2.3
Cdigo fuente de la funcin DIRECTKINEMATIC para el robot cilndrico de 4grados de libertad del ejemplo 2.1
Debe notarse que el vector de coordenadas articulares Q representa los 4 gradosde libertad del robot, 2 rotacionales y 2 prismticas y se introduce en los parmetros D-H como variables.
Las dimensiones del robot, introducidas en los parmetros de D-H, son lasindicadas en el captulo 1.
% DIRECTKINEMATIC4 Direct Kinematic.
% A04 = DIRECTKINEMATIC4(Q) devuelve la matriz de transformacin del% primer sistema de coordenadas al ltimo en funcin del vector Q% de variables articulares.%% See also DENAVIT.
functionA04 = directkinematic4(q)
% Parmetros Denavit-Hartenberg del robotteta = [q(1) 0 0 q(4)];d = [0.4 q(2) q(3) 0.2 ];a = [0 -0.1 0 0 ];alfa = [0 -pi/2 0 0 ];
% Matrices de transformacin homognea entre sistemas de coordenadasconsecutivosA01 = denavit(teta(1), d(1), a(1), alfa(1));
A12 = denavit(teta(2), d(2), a(2), alfa(2));A23 = denavit(teta(3), d(3), a(3), alfa(3));A34 = denavit(teta(4), d(4), a(4), alfa(4));
% Matriz de transformacin del primer al ltimo sistema decoordenadasA04 = A01 * A12 * A23 * A34;
-
7/23/2019 Captulo_2 dinmica
10/30
Borrador del libro: ROBOTICA: MODELADO,SIMULACIN Y DISEO CON MATLAB.R.Saltarn, M.Almonacid, J.M.Azorn, J.M.Sabater 19/04/06
10
A continuacin se presenta la matriz obtenida para una configuracin en la quetodas las coordenadas articulares tienen valor nulo:
q=zeros(4,1)
q =
0000
T=directkinematic4(q)
T =
1.0000 0 0 -0.10000 0.0000 1.0000 0.20000 -1.0000 0.0000 0.4000
0 0 0 1.0000
Se recomienda analizar la matriz homognea obtenida tal y como se indic enel apartado 2.2.1, comprobando que tanto la posicin como la orientacin del efectorfinal coinciden con lo esperado cuando se introducen las coordenadas articulares delejemplo.
Ejemplo 2.4
Cdigo fuente de la funcin DIRECTKINEMATIC para el robot rotacional de 6grados de libertad del ejemplo 2.2
Debe notarse que en el caso particular de un robot enteramente rotacional, lasvariables articulares representan los parmetros teta de D-H, como se estudi en elapartado 2.2.3
Las dimensiones del robot, introducidas en los parmetros de D-H, son las
indicadas en el captulo 1.
-
7/23/2019 Captulo_2 dinmica
11/30
Borrador del libro: ROBOTICA: MODELADO,SIMULACIN Y DISEO CON MATLAB.R.Saltarn, M.Almonacid, J.M.Azorn, J.M.Sabater 19/04/06
11
% DIRECTKINEMATIC6 Direct Kinematic.% A06 = DIRECTKINEMATIC6(Q) devuelve la matriz de transformacin del% primer sistema de coordenadas al ltimo en funcin del vector Q% de variables articulares.
%% See also DENAVIT.
functionA06 = directkinematic6(q)
% Parmetros Denavit-Hartenberg del robotteta = q;d = [0.315 0 0 0.5 0 0.08];a = [0 0.45 0 0 0 0];alfa = [-pi/2 0 pi/2 -pi/2 pi/2 0];
% Matrices de transformacin homognea entre sistemas de coordenadasconsecutivos
A01 = denavit(teta(1), d(1), a(1), alfa(1));A12 = denavit(teta(2), d(2), a(2), alfa(2));A23 = denavit(teta(3), d(3), a(3), alfa(3));A34 = denavit(teta(4), d(4), a(4), alfa(4));A45 = denavit(teta(5), d(5), a(5), alfa(5));A56 = denavit(teta(6), d(6), a(6), alfa(6));
% Matriz de transformacin del primer al ltimo sistema de
coordenadasA06 = A01 * A12 * A23 * A34 * A45 * A56;
A continuacin se presenta la matriz obtenida para una configuracin en la que todas lascoordenadas articulares tienen valor nulo:
q=zeros(6,1)
q =
00000
0
T=directkinematic6(q)
T =
1.0000 0 0 0.45000 1.0000 0 00 0 1.0000 0.89500 0 0 1.0000
-
7/23/2019 Captulo_2 dinmica
12/30
Borrador del libro: ROBOTICA: MODELADO,SIMULACIN Y DISEO CON MATLAB.R.Saltarn, M.Almonacid, J.M.Azorn, J.M.Sabater 19/04/06
12
NOTA: todas las funciones de este captulo seencuentran disponibles en el directorio capitulo 2 del cdadjunto.
2.3.- Cinemtica inversa del brazo de un robot manipulador
La cinemtica inversa consiste en hallar los valores de las coordenadas
articulares del robot [ ]Tnqqqq ,....,, 21= conocida la posicin y orientacin del extremodel robot.
A pesar de que en la literatura [1] y [2] se pueden encontrar diversos mtodosgenricos para la resolucin de la cinemtica inversa que pueden ser implementados encomputadora, suele ser habitual la resolucin por medio de mtodos geomtricos. Lamayor parte de los robots suelen tener cadenas cinemticas relativamente sencillas, quefacilitan la utilizacin de los mtodos geomtricos. Para muchos robots, si se consideranslo los tres primeros grados de libertad, se tiene una estructura planar. Este hechofacilita la resolucin del problema. Asimismo los ltimos tres grados de libertad suelenusarse para la orientacin de la herramienta, lo cual permite una resolucin geomtricadesacoplada de la posicin de la mueca del robot y de la orientacin de la herramienta.
En esta seccin se va a resolver el problema cinemtico inverso para los dosrobots anteriores, utilizando el mtodo geomtrico e implementndolo en Matlab.
Ejemplo 2.5
Solucin del robot cilndrico de 4 grados de libertad.
En este caso particular, la solucin geomtrica es inmediata. Se parte de que la posicin
del extremo del robot es conocida (px, py, pz) y se va a calcular los valores de lascoordenadas articulares.
Articulacin 1
Para obtener el valor de 1 (TETA1 en el cdigo de Matlab.) se proyecta elpunto del extremo del robot (px, py, pz) sobre el plano (x0, y0, z0) obteniendo unasencilla relacin angular. Sabiendo que 1es el ngulo entre x0y x1, se obtienen lassiguientes grficas.
-
7/23/2019 Captulo_2 dinmica
13/30
Borrador del libro: ROBOTICA: MODELADO,SIMULACIN Y DISEO CON MATLAB.R.Saltarn, M.Almonacid, J.M.Azorn, J.M.Sabater 19/04/06
13
Figura-2.5 Cinemtica inversa del robot de 4 gdl.
De las que se deducen las siguientes relaciones:
22yx ppR +=
4322
2 ldaRr +==
R
psin
y= R
px=cos
R
asin 2=
R
r=cos
utilizando la funcin atan2 de Matlabse calculan los valores de los ngulos:
)cos,(2 sinatan= )cos,(2 sinatan=
que permiten el clculo de 1como -.
Articulacin 2
De la figura 2.3 se obtiene la siguiente frmula:
1221 lpdpdl zz ==+ Articulacin 3
De la figura 2.5:
43222 ldaRr +==
px,py,pzorientacin
y0x0
z0
x0
y0
px, py
a2
d3+l4
1
Rr
-
7/23/2019 Captulo_2 dinmica
14/30
Borrador del libro: ROBOTICA: MODELADO,SIMULACIN Y DISEO CON MATLAB.R.Saltarn, M.Almonacid, J.M.Azorn, J.M.Sabater 19/04/06
14
Articulacin 4
Para calcular la ltima articulacin se necesita el clculo previo del sistema dereferencia (x3, y3, z3), que se resolver mediante la cinemtica directa explicada en elejemplo 2.3. Dado que el vector ade aproximacin es necesariamente paralelo a z4 se
deben cumplir las siguientes relaciones:
34 ynsin = )cos,(2 444 sinatan=
34 cos ys=
Cdigo en M atlab. La funcin INVERSEKINEMATIC4 resuelve la cinemtica inversadel robot cilndrico de 4 gdl. Para ello toma como parmetros la matriz homognea T,que representa la orientacin y posicin del extremo del robot y devuelve el vector decoordenadas articulares.
% Q = INVERSEKINEMATIC4(T) devuelve el vector de coordenadas% articulares correspondiente a la solucin cinemtica inversa de% la mano del manipulador en la posicin y orientacin expresadas% en la matriz T.
%% See also DIRECTKINEMATIC4, DENAVIT.
functionq = inversekinematic4(T)
p = T(1:3,4); % Posicin de la mano del manipulador
% Inicializacin de las variables articulares a calcularq1 = 0;q2 = 0;
q3 = 0;q4 = 0;
% Parmetros Denavit-Hartenberg del robotteta = [q1 0 0 q4 ];d = [0.4 q2 q3 0.2];a = [0 -0.1 0 0 ];alfa = [0 -pi/2 0 0 ];
% Solucin de la primera articulacin: q1
R = sqrt(p(1)^2+p(2)^2);r = sqrt(R^2-a(2)^2);
sphi = -p(1)/R;cphi = p(2)/R;phi = atan2(sphi, cphi);
y3
x4
y4
x3
4
donde ny sson vectores de orientacin del extremodel robot.
-
7/23/2019 Captulo_2 dinmica
15/30
Borrador del libro: ROBOTICA: MODELADO,SIMULACIN Y DISEO CON MATLAB.R.Saltarn, M.Almonacid, J.M.Azorn, J.M.Sabater 19/04/06
15
sbeta = -a(2)/R;
cbeta = r/R;beta = atan2(sbeta, cbeta);
q1 = phi - beta;
% Solucin de la segunda articulacin: q2q2 = p(3) - d(1);
% Solucin de la tercera articulacin: q3
q3 = r - d(4);
% Solucin de la cuarta articulacin: q4
% Clculo de la matriz de transformacin A03A01 = denavit(q1, d(1), a(1), alfa(1));A12 = denavit(teta(2), q2, a(2), alfa(2));A23 = denavit(teta(3), q3, a(3), alfa(3));A03 = A01 * A12 * A23;
y3 = A03(1:3,2);
sq4 = dot(T(1:3,1), y3); % Vector orientacin n: T(1:3,1)cq4 = dot(T(1:3,2), y3); % Vector orientacin s: T(1:3,2)q4 = atan2(sq4, cq4);
% Vector de variables articularesq = [q1 q2 q3 q4]';
Se observa como la cinemtica directa est incluida en los clculos necesariospara obtener la matriz 0A3.
En el ejemplo mostrado a continuacin se puede comprobar como despus deasignar un vector de coordenadas articulares aleatorio, y obtener la matriz homogneadel extremo de robot correspondiente a este vector, si sobre esta matriz se aplica lafuncin INVERSEKINEMATIC4 se obtiene el vector q original.
-
7/23/2019 Captulo_2 dinmica
16/30
Borrador del libro: ROBOTICA: MODELADO,SIMULACIN Y DISEO CON MATLAB.R.Saltarn, M.Almonacid, J.M.Azorn, J.M.Sabater 19/04/06
16
q=rand(4,1)
q =
0.8913
0.7621
0.4565
0.0185
T=directkinematic4(q)
T =
0.6283 -0.0116 -0.7779 -0.57350.7778 -0.0144 0.6284 0.3347
-0.0185 -0.9998 0.0000 1.16210 0 0 1.0000
inversekinematic4(T)
ans =
0.8913
0.7621
0.4565
0.0185
Ejemplo 2.6
Solucin del robot rotacional de 6 grados de libertad.
En contraste con el ejemplo anterior, la solucin de la cinemtica inversa de un robot de6 grados de libertad requiere del lector un mayor esfuerzo de comprensin. En esteejemplo se va a resolver mediante el mtodo geomtrico. Para ello, en virtud del
principio de Piepers, es necesario separar el clculo de la posicin y orientacin delextremo del robot.
En primer lugar se calcula la posicin del punto de interseccin de los 3 ltimos gradosde libertad, que se conoce como MUECA del robot. Con las coordenadas (px, py, pz)de la mueca se resuelve el problema de la posicin, de manera similar al ejemploanterior.
Los 3 ltimos grados de libertad se utilizan para orientar la herramienta colocada en elextremo del robot, y se resuelven en un paso posterior.
Como se observa en la figura 2.6, la articulacin 3 (3), en adelante CODO del robot,puede tener dos valores distintos, conocidos como configuracin CODO ARRIBA yCODO ABAJO, y que representan los casos en que la articulacin 3 est situada por
debajo o por encima de la articulacin 2, para lograr una misma posicin de la mueca.
-
7/23/2019 Captulo_2 dinmica
17/30
Borrador del libro: ROBOTICA: MODELADO,SIMULACIN Y DISEO CON MATLAB.R.Saltarn, M.Almonacid, J.M.Azorn, J.M.Sabater 19/04/06
17
Asimismo, la MUECA puede tomar tambin dos configuraciones distintas, MUECAARRIBA y MUECA ABAJO, para obtener una misma orientacin del efector final.
Debido a las anteriores consideraciones, es necesario introducir en la resolucin delproblema cinemtico inverso dos parmetros, llamados por similitud CODO y
MUECA, que representen las cuatro posibles configuraciones que puede adoptar elrobot cuando se le solicita alcanzar una posicin y orientacin determinadas.
Para una descripcin ms amplia acerca de la resolucin del problema cinemticoinverso se recomienda estudiar las referencias: [1] y [2].
Figura-2.6 Cinemtica inversa del robot de 6 gdl.
A continuacin se muestra la solucin geomtrica del robot estudiado y suimplementacin en Matlab.
Se recomienda seguir las explicaciones con el cdigo fuente de la funcinINVERSEKINEMATIC6.
Problema de posicin. Clculo de las tres primeras articulaciones.
Articulacin 1
22yx ppR +=
R
psin
y=1 R
px=1cos
)cos,(2 111 sinatan=
MUECApx, py, pzCODO
a
s
n
y0x0
z0
R
px,py
1
x0
-
7/23/2019 Captulo_2 dinmica
18/30
Borrador del libro: ROBOTICA: MODELADO,SIMULACIN Y DISEO CON MATLAB.R.Saltarn, M.Almonacid, J.M.Azorn, J.M.Sabater 19/04/06
18
Articulacin 2
Para la resolucin de la segunda articulacin, se ha de tener en cuenta la posibilidad deCODO ARRIBA y CODO ABAJO. En cada caso el ngulo 2 se calcular como lasuma o la resta de los ngulos y calculados.
Configuracin CODO ARRIBA Configuracin CODO ABAJO
Identificando las relaciones geomtricas presentes en los anteriores esquemas:
21
22 )( lpppr zyx ++=
cos2 222
223 rllrl +=
cos2 2
22
223 =
+
rl
lrl
2cos1=sin
r
plsin z
= 1
r
R=cos
+=2 =2
Configuracin CODO ARRIBA Configuracin CODO ABAJO
l1
l2
l3
r
px ,py, pz
2
x2
x1 l1
l2
l3r
px ,py, pz
2
x2
x1
-
7/23/2019 Captulo_2 dinmica
19/30
Borrador del libro: ROBOTICA: MODELADO,SIMULACIN Y DISEO CON MATLAB.R.Saltarn, M.Almonacid, J.M.Azorn, J.M.Sabater 19/04/06
19
Articulacin 3
Al igual que en la articulacin 2, se debe tener en cuenta las dos posiblesconfiguraciones del codo.
=2
33
23
=
Problema de orientacin. Clculo de las tres ltimas articulaciones.
Conocidos los tres primeros ngulos 1, 2y 3se resuelve la cinemtica directa paralos tres primeros eslabones, obtenindose la matriz 0A3necesaria para la resolucin delas ltimas tres articulaciones. Siguiendo la referencia Fu [3], para resolver el problemade la orientacin se han de conseguir que las tres ltimas articulaciones cumplan loscriterios siguientes:
1. Establecer la articulacin 4 de forma tal que una rotacin respecto de larotacin 5 alinear el eje de movimiento de la articulacin 6 con el vector de
aproximacin dado (a).2.
La articulacin 5 alinear el eje de movimiento de la articulacin 6 con elvector de aproximacin.
3. Fijar la articulacin 6 para alinear el vector de orientacin dado (s) (o el dedeslizamiento (n)) y el vector normal.
Matemticamente, estos criterios significan:
1.( )
xaz
xazz
3
34
=
2. a= z53.
s = y6
Ambas orientaciones de la mueca (ARRIBA y ABAJO) se definen observando laorientacin del sistema de coordenadas de la mano (n,s,a) con respecto al sistema decoordenadas (x5, y5, z5). Para analizar las configuraciones de MUECA ARRIBA yMUECA ABAJO, se utiliza un parmetro de orientacin (omega en el cdigo deMatlab) que hace referencia a la orientacin del vector unitario n(o s) con respecto alvector unitario x5(o y5) y que viene definido en la referencia Fu [4] como:
0 si se est en el caso degenerado
= sy5 si sy50ny5 si sy5=0
l2
l1
l3x2
x3
3l2
l1
l3
x2
x3
3
-
7/23/2019 Captulo_2 dinmica
20/30
Borrador del libro: ROBOTICA: MODELADO,SIMULACIN Y DISEO CON MATLAB.R.Saltarn, M.Almonacid, J.M.Azorn, J.M.Sabater 19/04/06
20
Articulacin 4
Para conocer el signo de la ecuacin( )
xaz
xazz
3
34
= se determina conociendo la
orientacin y la configuracin de MUECA. se calcula segn su definicin, yMUECA es un parmetro del robot. Se parte de la suposicin de que el signo es
positivo, y esta hiptesis se corrige con el parmetro M que se calcula comocombinacin de la configuracin de la mueca y la orientacin requeridas.
Utilizando los parmetros y MUECA se construye la siguiente tabla:
Configuracin MUECA M=MUECA*sign()ABAJO +1 0 +1ABAJO +1
-
7/23/2019 Captulo_2 dinmica
21/30
Borrador del libro: ROBOTICA: MODELADO,SIMULACIN Y DISEO CON MATLAB.R.Saltarn, M.Almonacid, J.M.Azorn, J.M.Sabater 19/04/06
21
Cdigo en M atlab. La funcin INVERSEKINEMATIC6 resuelve la cinemtica inversadel robot rotacional de 6 gdl. Para ello toma como parmetros la matriz homognea T,y los parmetros CODO y MUECA para definir las posibles configuraciones. Ha detenerse en consideracin que determinados puntos y orientaciones que pertenezcan alespacio de trabajo del robot, no podrn alcanzarse con algunas configuraciones. Por
ello se recomienda al lector que trate de experimentar con estos puntos extremos con laherramienta que a continuacin se presenta, y analice los resultados.
% INVERSEKINEMATIC6 Inverse Kinematic% Q = INVERSEKINEMATIC6(T, CODO, MUNECA) devuelve el vector de coordenadas% articulares correspondiente a la solucin cinemtica inversa de la mano% del manipulador en la posicin y orientacin expresadas en la matriz T.% CODO = 1 indica codo del robot arriba, es decir, que la articulacin 3 se% sita por encima de la articulacin 2, mientras que CODO = -1 indica codo% abajo, es decir que la articulacin 2 se sita por encima de la 3.% MUNECA = 1 indica que la mueca del robot se sita por debajo de lacoordenada% expresada en T, mientras que MUNECA = -1 significa que la mueca se sita% por arriba.
%% See also DIRECTKINEMATIC6, DENAVIT.
functionq = inversekinematic6(T,codo,muneca)
% Parmetros Denavit-Hartenberg del robotd = [0.315 0 0 0.5 0 0.08];a = [0 0.45 0 0 0 0];
alfa = [-pi/2 0 pi/2 -pi/2 pi/2 0];
% Posicin de la mano del manipuladorp = T(1:3,4)-d(6)*T(1:3,3);
% Solucin de la primera articulacin: q1R = sqrt(p(1)^2+p(2)^2);sq1=p(2)/R;cq1=p(1)/R;q1 = atan2(sq1,cq1);
% Solucin de la segunda articulacin: q2r = sqrt(R^2+(p(3)-d(1))^2);salfa = (d(1)-p(3))/r;calfa = R/r;
cbeta = (r^2+a(2)^2-d(4)^2)/(2*r*a(2));sbeta = sqrt(1-cbeta^2);
ifcodo == -1 % Codo abajo
sq2 = salfa*cbeta+sbeta*calfa;cq2 = calfa*cbeta-salfa*sbeta;
else % Codo arribasq2 = salfa*cbeta-sbeta*calfa;cq2 = calfa*cbeta+salfa*sbeta;
end
q2 = atan2(sq2,cq2);
% Solucin de la tercera articulacin: q3cbeta=(a(2)^2+d(4)^2-r^2)/(2*a(2)*d(4));
sbeta=sqrt(1-cbeta^2);beta=atan2(sbeta,cbeta);
-
7/23/2019 Captulo_2 dinmica
22/30
Borrador del libro: ROBOTICA: MODELADO,SIMULACIN Y DISEO CON MATLAB.R.Saltarn, M.Almonacid, J.M.Azorn, J.M.Sabater 19/04/06
22
ifcodo == 1 % Codo arribaq3 = 3*pi/2-beta;
else % Codo abajoq3 = beta - pi/2;
end
% Solucin de la cuarta articulacin: q4% Clculo de la matriz de transformacin A03A01 = denavit(q1, d(1), a(1), alfa(1));
A12 = denavit(q2, d(2), a(2), alfa(2));A23 = denavit(q3, d(3), a(3), alfa(3));A03 = A01 * A12 * A23;x3 = A03(1:3,1);y3 = A03(1:3,2);z3 = A03(1:3,3);z4 = cross(z3,T(1:3,3)); % Vector orientacin a: T(1:3,3)
% Determinacin del indicador de orientacin omegaaux = dot(T(1:3,2),z4); % Vector orientacin s: T(1:3,2)
ifaux ~= 0omega = aux;
elseaux=dot(T(1:3,1),z4); % Vector orientacin n: T(1:3,1)ifaux ~=0
omega=aux;else
omega=0;end
end
M = muneca*sign(omega);
sq4 = -M*dot(z4,x3);cq4 = M*dot(z4,y3);q4 = atan2(sq4,cq4);
% Solucin de la quinta articulacin: q5z5 = T(1:3,3); % Vector de orientacin a: T(1:3,3)
A34 = denavit(q4, d(4), a(4), alfa(4));A04 = A03 * A34;x4 = A04(1:3,1);y4 = A04(1:3,2);
sq5 = dot(T(1:3,3),x4); % Vector de orientacin a: T(1:3,3)cq5 = -dot(T(1:3,3),y4); % Vector de orientacin a: T(1:3,3)
q5 = atan2(sq5,cq5);
% Solucin de la sexta articulacin: q6
y6 = T(1:3,2); % Vector de orientacin s: T(1:3,2)
A45 = denavit(q5, d(5), a(5), alfa(5));A05 = A04 * A45;y5 = A05(1:3,2);
sq6 = dot(T(1:3,1),y5); % Vector de orientacin n: T(1:3,1)cq6 = dot(T(1:3,2),y5); % Vector de orientacin s: T(1:3,2)q6 = atan2(sq6,cq6);
% Vector de coordenadas articularesq = [q1 q2 q3 q4 q5 q6]';
-
7/23/2019 Captulo_2 dinmica
23/30
Borrador del libro: ROBOTICA: MODELADO,SIMULACIN Y DISEO CON MATLAB.R.Saltarn, M.Almonacid, J.M.Azorn, J.M.Sabater 19/04/06
23
En el ejemplo mostrado a continuacin se puede comprobar como despus deasignar un vector de coordenadas articulares aleatorio, lo cual ya incluye unadeterminada configuracin de CODO y MUECA y obtener la matriz homognea delextremo de robot correspondiente a este vector, si sobre esta matriz se aplica la funcinINVERSEKINEMATIC6 con los valores correctos de CODO y MUECA para el
vector de coordenadas articulares, se obtiene el vector q original.
Se recomienda experimentar la funcin INVERSEKINEMATIC6 convectores de coordenadas articulares sencillos de analizar.
q=rand(6,1)
q =
0.67210.83810.01960.68130.37950.8318
T=directkinematic6(q)
T =
-0.7400 -0.3846 0.5518 0.57560.6484 -0.1900 0.7372 0.4819
-0.1787 0.9033 0.3900 0.33870 0 0 1.0000
inversekinematic6(T,-1,1)
ans =
0.67210.83810.01960.68130.37950.8318
2.4.- Representacin grfica en MatLab usando alambres.
En este apartado se van a presentar una sencilla herramienta grfica vlida paracomprobar los resultados anteriores utilizando las funciones ya estudiadas. Serecomienda al lector que realice estos ejemplos modificando el cdigo de Matlab. Paracomprobar los resultados se ha utilizado una nueva funcin nombrada comoDRAWROBOT3D, que permite una sencilla representacin 3D utilizando lneas de laconfiguracin del manipulador. Ha de considerarse que esta funcin realiza el trazadodel robot conforme a su representacin D-H, por lo que los sistemas de referencia quehan sido desplazados dan lugar a eslabones de longitud nula, dando la impresin pticade que han desaparecido. Para comprobar que el efecto de estas rotaciones si es tomado
en consideracin se sugiere al lector que coloque un pequeo sistema de referencia en elextremo del robot.
-
7/23/2019 Captulo_2 dinmica
24/30
Borrador del libro: ROBOTICA: MODELADO,SIMULACIN Y DISEO CON MATLAB.R.Saltarn, M.Almonacid, J.M.Azorn, J.M.Sabater 19/04/06
24
Ejemplo 2.7
Cdigo en M atlab. La funcin DRAWROBOT3D realiza una representacin 3D de unrobot en funcin del vector de variables articulares Q
Particularizando DRAWROBOT3D para los ejemplos anteriores se obtiene lasfunciones que se han llamado DRAWROBOT3D4 y DRAWROBOT3D6.
% DRAWROBOT3D4 Representacin 3D de un robot.% DRAWROBOT3D4(Q) realiza una representacin 3D de un robot% en funcin del vector de variables articulares Q.%% See also DENAVIT, DIRECTKINEMATIC4.
functiondrawrobot3d4(q)
% Parmetros Denavit-Hartenberg del robot
teta = [q(1) 0 0 q(4)];d = [0.4 q(2) q(3) 0.2 ];a = [0 -0.1 0 0 ];alfa = [0 -pi/2 0 0 ];
% Matrices de transformacin homognea entre sistemas de coordenadas
consecutivosA01 = denavit(teta(1), d(1), a(1), alfa(1));A12 = denavit(teta(2), d(2), a(2), alfa(2));A23 = denavit(teta(3), d(3), a(3), alfa(3));A34 = denavit(teta(4), d(4), a(4), alfa(4));
% Matrices de transformacin del primer sistema al correspondiente
A02 = A01 * A12;A03 = A02 * A23;A04 = A03 * A34;
% Vector de posicion (x, y, z) de cada sistema de coordenadasx0 = 0; y0 = 0; z0 = 0;x1 = A01(1,4); y1 = A01(2,4); z1 = A01(3,4);xi = x1; yi = y1; zi = z1 + d(2);x2 = A02(1,4); y2 = A02(2,4); z2 = A02(3,4);x3 = A03(1,4); y3 = A03(2,4); z3 = A03(3,4);x4 = A04(1,4); y4 = A04(2,4); z4 = A04(3,4);
% Se dibuja el robot
x = [x0 x1 xi x2 x3 x4];y = [y0 y1 yi y2 y3 y4];z = [z0 z1 zi z2 z3 z4];plot3(x,y,z);% Se coloca una rejilla a los ejesgrid;% Se establecen los lmites de los ejesaxis([-1.5 1.5 -1.5 1.5 0 1.5]);
Si se representa la configuracin en la que todas las coordenadas articulares sonnulas, se obtiene el siguiente dibujo:
-
7/23/2019 Captulo_2 dinmica
25/30
Borrador del libro: ROBOTICA: MODELADO,SIMULACIN Y DISEO CON MATLAB.R.Saltarn, M.Almonacid, J.M.Azorn, J.M.Sabater 19/04/06
25
Ejemplo 2.8
Se presenta ahora el cdigo y la representacin grfica del robot rotacional de 6grados de libertad.
% DRAWROBOT3D6 Representacin 3D de un robot.% DRAWROBOT3D6(Q) realiza una representacin 3D de un robot% en funcin del vector de variables articulares Q.%% See also DENAVIT, DIRECTKINEMATIC6.
functiondrawrobot3d6(q)
% Parmetros Denavit-Hartenberg del robotteta = q;d = [0.315 0 0 0.5 0 0.08];a = [0 0.45 0 0 0 0 ];alfa = [-pi/2 0 pi/2 -pi/2 pi/2 0 ];
% Matrices de transformacin homognea entre sistemas de coordenadas
consecutivosA01 = denavit(teta(1), d(1), a(1), alfa(1));A12 = denavit(teta(2), d(2), a(2), alfa(2));A23 = denavit(teta(3), d(3), a(3), alfa(3));A34 = denavit(teta(4), d(4), a(4), alfa(4));A45 = denavit(teta(5), d(5), a(5), alfa(5));
A56 = denavit(teta(6), d(6), a(6), alfa(6));
-
7/23/2019 Captulo_2 dinmica
26/30
Borrador del libro: ROBOTICA: MODELADO,SIMULACIN Y DISEO CON MATLAB.R.Saltarn, M.Almonacid, J.M.Azorn, J.M.Sabater 19/04/06
26
% Matrices de transformacin del primer sistema al correspondiente
A02 = A01 * A12;A03 = A02 * A23;A04 = A03 * A34;A05 = A04 * A45;A06 = A05 * A56;
% Vector de posicion (x, y, z) de cada sistema de coordenadasx0 = 0; y0 = 0; z0 = 0;x1 = A01(1,4); y1 = A01(2,4); z1 = A01(3,4);
x2 = A02(1,4); y2 = A02(2,4); z2 = A02(3,4);x3 = A03(1,4); y3 = A03(2,4); z3 = A03(3,4);x4 = A04(1,4); y4 = A04(2,4); z4 = A04(3,4);x5 = A05(1,4); y5 = A05(2,4); z5 = A05(3,4);x6 = A06(1,4); y6 = A06(2,4); z6 = A06(3,4);
% Se dibuja el robotx = [x0 x1 x2 x3 x4 x5 x6];y = [y0 y1 y2 y3 y4 y5 y6];z = [z0 z1 z2 z3 z4 z5 z6];
plot3(x,y,z);% Se coloca una rejilla a los ejesgrid;% Se establecen los lmites de los ejesaxis([-1 1 -1 1 0 1.5]);
Si se representa la configuracin en la que todas las coordenadas articulares sonnulas, se obtiene el siguiente dibujo:
-
7/23/2019 Captulo_2 dinmica
27/30
Borrador del libro: ROBOTICA: MODELADO,SIMULACIN Y DISEO CON MATLAB.R.Saltarn, M.Almonacid, J.M.Azorn, J.M.Sabater 19/04/06
27
2.5.- Matriz Jacobiana.
La matriz Jacobiana expresa una relacin entre las velocidades de lasarticulaciones y las velocidades del extremo del robot.
Hasta el momento las herramientas desarrolladas representaban relaciones entrelas variables articulares y la posicin y orientacin del extremo del robot, sin incluir lasfuerzas que originan el movimiento del robot. Sin embargo un estudio cinemtico de unrobot debe proporcionar informacin sobre las velocidades y aceleraciones del robot. Elsistema de control del robot precisa conocer qu velocidades debe imprimir a cadaarticulacin para conseguir que el extremo del manipulador se mueva siguiendo unadeterminada trayectoria. Estas relaciones las proporciona la matriz Jacobiana.
Figura-2.6 Matriz Jacobiana directa e inversa.
La forma ms directa de obtener la matriz Jacobiana es diferenciar lasecuaciones correspondientes del modelo cinemtico directo;
),...,( 1 nx qqfx= ),...,( 1 ny qqfy= ),...,( 1 nz qqfz= ),...,( 1 nqqf= ),...,( 1 nqqf= ),...,( 1 nqqf=
obteniendo:
=
nq
q
Jz
y
x
&
&
&
&
&
&
&
&
.
.
.
.1
con
=
n
n
xx
q
f
q
f
q
f
q
f
J
...
...
...
...
...
1
1
Velocidad de lasArticulaciones
),...,,( 111 qqq &&&
Velocidad delextremo del robot
),,,,,( &&&&&& zyx
Jacobiana Directa
Jacobiana Inversa
-
7/23/2019 Captulo_2 dinmica
28/30
Borrador del libro: ROBOTICA: MODELADO,SIMULACIN Y DISEO CON MATLAB.R.Saltarn, M.Almonacid, J.M.Azorn, J.M.Sabater 19/04/06
28
y la jacobiana inversa se expresa mediante:
=
nn f
x
f
f
x
f
J
...
...
...
...
... 11
1
Se recomienda al lector que quiera profundizar en el conocimiento de la matrizJacobiana que estudie las referencias [1] y [2] de robtica general donde se explicanvarios mtodos de obtencin de las matrices Jacobiana directa e inversa, y se tratan las
situaciones en las que el determinante de la matriz Jacobiana (Jacobiano) se anula,dando lugar a configuraciones singulares que deben ser evitadas en una correctaplanificacin de trayectorias.
2.6.- PRACTICA. Animacin de los robots.
En este apartado se va a realizar un sencillo ejemplo de utilizacin de las funcionesestudiadas hasta ahora. Se trata de colocar al robot en dos posiciones distintas y animaruna trayectoria recta entre esas dos configuraciones. Para ello se debern generar varias
posiciones intermedias.
En el cd adjunto se presenta la funcin planifica(p1,p2,ni) en la que se introducen lascoordenadas cartesianas de los puntos inicial y final y el nmero de puntos intermedios.Se sugiere al lector que utilice esta funcin para tratar de realizar un ejemplo con elrobot prismtico de 4 gdl.
Ejemplo 2.9
En este ejemplo se utiliza la cinemtica inversa del robot rotacional de 6 gdl para trazaruna lnea recta entre un punto p1 inicial y un punto p2 final. El nmero de puntosintermedios es variable.
Se ha utilizado la funcin PLANIFICA6(P1,P2,N,S,A,CODO,MUECA,NPUNTOS)en el que se introduce las coordenadas cartesianas de los puntos inicial y final, laorientacin (n,s,a) del punto final, los parmetros CODO y MUECA para seleccionarla configuracin del robot y el nmero de puntos intermedio. Esta funcin proporcionauna matriz de (npuntos+2) columnas por 6 filas que se utilizar por la funcinANIMACION6(MAT_Q) para dibujar la trayectoria entre los dos puntos.
-
7/23/2019 Captulo_2 dinmica
29/30
Borrador del libro: ROBOTICA: MODELADO,SIMULACIN Y DISEO CON MATLAB.R.Saltarn, M.Almonacid, J.M.Azorn, J.M.Sabater 19/04/06
29
Cdigo en Matlab. La funcin PLANIFICA6(P1,P2,N,S,A,CODO,MUECA,NPUNTOS) calcula la matriz de coordenadas articulares utilizada para graficar elmovimiento del robot.
% PLANIFICA6 Planificacin de trayectorias% MAT_Q = PLANIFICA6(P1, P2, N, S, A, CODO, MUNECA, NPUNTOS) realiza
% una planificacin de trayectoria en lnea recta desde la coordenada% cartesiana P1 hasta la P2, de manera que la mano del manipulador% posee la orientacin expresada por [N S A]. CODO = 1 indica codo del% robot arriba, es decir, que la articulacin 3 se sita por encima de% la articulacin 2, mientras que CODO = -1 indica codo abajo, es decir% que la articulacin 2 se sita por encima de la 3. MUNECA = 1 indica que% la mueca del robot se sita por debajo de la coordenada cartesiana,% mientras que MUNECA = -1 significa que la mueca se sita por arriba.% NPUNTOS indica el nmero de puntos en los que se divide la trayectoria.% En MAT_Q se devuelve las coordenadas articulares, almacenadas por% columnas, correspondientes a cada uno de los puntos cartesianos en los% que se divide la trayectoria. MAT_Q es una matriz de NPUNTOS + 2 columnas% y 6 filas.%% See also INVERSEKINEMATIC6.
functionmat_q = planifica6(p1, p2, n, s, a, codo, muneca, npuntos)
% Clculo del vector unitariou = p2-p1;mu = sqrt(u(1)^2+u(2)^2+u(3)^2);u = (1/mu)*u;% Clculo de la distancia entre puntosd = mu/(npuntos+1);
fori=0:(npuntos+1)% Clculo de la posicin cartesiana actual de la mano del
manipulador
p = p1+(i*d)*u;T = [n s a p];% Clculo de las coordenadas articularesq = inversekinematic6(T,codo,muneca);mat_q(:,i+1) = q;
end
Utilizando ahora la funcin ANIMACION6(MAT_Q) se presenta el movimiento delrobot entre los 2 puntos especificados.
-
7/23/2019 Captulo_2 dinmica
30/30
Borrador del libro: ROBOTICA: MODELADO,SIMULACIN Y DISEO CON MATLAB.R.Saltarn, M.Almonacid, J.M.Azorn, J.M.Sabater 19/04/06
Notar que la funcin plot3 ha permitido dibujar sobre la animacin la trayectoriaseguida por el extremo del robot.
REFERENCIAS
[1] Robtica, Control y Visin. Fu, Gonzlez, Lee[2] Fundamentos de robtica. Ed. McGraw-Hill. A.Barrientos, L.F.Pen, C.Balaguer, R.Aracil.