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.


2/30


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.


3/30


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.


4/30


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)


5/30


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


6/30


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/30


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


8/30


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


9/30


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;


10/30


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.


11/30


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 =

1.0000 0 0 0.45000 1.0000 0 00 0 1.0000 0.89500 0 0 1.0000


12/30


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.


13/30


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


14/30


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.


15/30


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.


16/30


16

q=rand(4,1)

q =

0.8913

0.7621

0.4565

0.0185


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.


17/30


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


18/30


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


19/30


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


20/30


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


21/30


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);


22/30


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]';


23/30


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 =

-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.


24/30


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:


25/30


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));


26/30


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:


27/30


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


28/30


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.


29/30


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.


30/30


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.

capítulo_2 dinámica

Documents