informe robotica

Análisis de la Cinemática y Dinámica de un Manipulador

Robótico de 6 GDL �Stanford Arm�

Macedo Vilca Denis Daniel

[email protected]

Figura 0.1: Stanford Arm

1

Índice

1. Introducción 3

2. Descripción de los enlaces 3

2.1. Constantes de un enlace. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

2.2. Variables de un enlace. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

3. Cinemática Directa del Manipulador. 7

3.1. Calculo de las matrices de transformación. . . . . . . . . . . . . . . . . . . . . . . 7

3.1.1. Sistema de referencia {0}. . . . . . . . . . . . . . . . . . . . . . . . . . . . 8







3.2. Visualización general de los sistemas de referencia. . . . . . . . . . . . . . . . . . 14

3.2.1. Código en Matlab para la visualización de los marcos de referencia:15

3.3. Solución al problema Cinematico Directo. . . . . . . . . . . . . . . . . . . . . . . 16

4. Cinemática Inversa del Manipulador 17

4.1. Hallando θ1. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18

4.2. Hallando θ2. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19

4.3. Hallando d3. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19

4.4. Hallando θ4. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20

4.5. Hallando θ5. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21

4.6. Hallando θ6. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21

4.7. Aclaraciones. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

5. Modelo Dinámico del Manipulador. 22

5.1. Uso del toolbox HEMERO. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

5.1.1. Asignación de parámetros según manipulador. . . . . . . . . . . . . . . . . 23

5.1.2. Script para la generación de las matrices M , G y V . . . . . . . . . . . . . 25

5.2. Aclaraciones. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27

2

1. Introducción

Este trabajo se enfoca en el análisis de la cinemática y dinámica de un manipulador robótico de6 grados de libertad, especí�camente el manipulador robótico �Stanford Arm� el cual consta de5 articulaciones rotacionales y 1 prismática, ordenadas de la forma: �RRPRRR�.

En lo que sigue del trabajo se tratara sobre los criterios de análisis de dicho manipulador, par-tiendo con el criterio de descripción de enlaces o eslabones que unen articulaciones consecutivasy que se tomará en cuenta en las secciones siguientes y que serán la notación básica para analizarel manipulador, para luego según dado marcos de referencias unidos a cada articulación, tratarsobre la asignación de parámetros Denavit-Hartemberg, los cuales podremos utilizar para en-contrar la relación entre marcos de referencia consecutivos en función de la variable de la cadaarticulación.

Ya obtenidos todos los parámetros Denavit-Hartemberg podremos utilizarlos para encontrar lamatriz de transformación homogenea general del manipulador(Problema Cinemático Directo)utilizando para ello, las matrices de transformación homogenea que relacionan cada marco dereferencia con el anterior ubicados en el manipulador. Con esto tendríamos solucionado el prob-lema cinemático directo del manipulador, el cual consiste en hallar la ubicación del efector �nalteniendo como datos conocidos las variables de cada articulación. Es decir si conocemos los án-gulos(Articulación Rotacional) y las distancias desplazadas(Articulación Prismática) de todaslas articulaciones del manipulador, entonces según la matriz de transformación total podremossaber en que lugar se ubica nuestro manipulador y su dirección.

Es una necesidad en relación a manipuladores poder decir al manipulador que ubique su efector�nal en una posición y orientación deseada, y según esto tener que averiguar cuales son los valoresque tomara cada variable, según sea esta rotacional o de desplazamiento e indicar al actuador cor-respondiente ubicarse o desplazarse lo necesario y así obtener la ubicación y orientación deseada.Aunque es fácil prever que hay distintos valores de las variables que harían que el manipulador sesitúe en la posición y orientación deseada(múltiples soluciones), no es necesariamente fácil sabertodos los conjuntos de variables que serian soluciones; Por lo tanto el problema que se pretenderesolver es el Problema Cinematico Inverso, el cual intenta saber cuales deberían ser los valoresde las variables de cada articulación del manipulador, para que este se posicione y se orientecomo uno desea.

2. Descripción de los enlaces

2.1. Constantes de un enlace.

Aquí nos enfocaremos en como se describirá un enlace, el cual en robots manipuladores realestiene muchos atributos, pero aquí para �nes de obtener la cinemática del robot, solo nos cen-traremos en considerar a un enlace como un cuerpo rígido el cual de�ne la relación existenteentre dos ejes articulares consecutivos de un manipulador. Por lo visto en la �gura 2.1 un enlaceesta de�nido por dos valores constantes.

3

Figura 2.1: Enlace entre ejes i− 1 e i.

ai−1 se re�ere a la distancia entre los ejes consecutivos i − 1 e i, esta distancia es medida a lolargo de la linea mutuamente perpendicular a los dos ejes.

αi−1 es el ángulo que se re�ere a la localización relativa de los dos ejes consecutivos respecto ala normal común a los ejes.

2.2. Variables de un enlace.

Los parámetros anteriores fueron para describir un solo enlace, ahora para describir la relaciónentre dos enlaces consecutivos utilizaremos otro dos parámetros los cuales se ven en la �gura 2.2.

Figura 2.2: Relación entre enlaces consecutivos

Estos nuevos parámetros son comúnmente llamados variables articulares y se describen a con-tinuación.

di se re�ere a la distancia entre las extensiones de ai−1 y ai medida a lo largo del eje i.

Θi se re�ere al ángulo medido entre las extensiones de ai−1 y ai respecto al eje i.

4

Figura 2.3: Manipulador robótico RRPRRR

Figura 2.4: Representación básica del manipulador

Ya descritos los parámetros involucrados en un enlace y por consiguiente los que se utilizaran eneste documento, se continua con la asignación de parámetros D-H ya de forma mas concisa.

Para realizar esta asignación consideraremos la �gura 2.3, en la cual se muestra el manipuladorrobótico de 6 grados de libertad. En la �gura 2.4, se muestra una presentación simple de estebasándose en el tipo de articulaciones utilizadas, asignando sistemas de referencia base.

Debemos tener en cuenta, que para la asignación de los sistemas de referencia, especí�camente losorígenes y los ejes seguiremos los pasos sistemáticos de Denavit-Hartemberg. los cuales utilizare-mos para obtener los parámetros que describen un enlace y la relación entre enlaces consecutivos.Esto lo podemos ver en la �gura 2.5.

Por lo tanto de la �gura 2.5, se desprende la matriz de transformación homogenea que relacionael sistema de referencia i con el sistema i− 1. Esta matriz esta dada por la ecuación 2.1.

i−1i T =

cθi −sθi 0 ai−1

sθicαi−1 cθicαi−1 −sαi−1 −sαi−1disθisαi−1 cθisαi−1 cαi−1 cαi−1di

0 0 0 1

(2.1)

5

Figura 2.5: Parámetros que relacionan dos sistemas consecutivos

Para poder aplicar la siguiente matriz al manipulador robótico, previamente hallaremos los sis-temas de referencias del manipulador, que según las reglas de Denavit-Hartemberg se dan de lasiguiente manera:

1. Encontrar los ejes de las articulaciones, estos serán para articulaciones rotacionales los ejesde rotación y para prismáticas, los ejes de deslizamiento.

2. Encontrar la normal común a los ejes consecutivos, esta sera generalmente única salvo paraejes paralelos en los cuales existen in�nitas normales comunes.

3. Ubicar el punto de origen del sistema de referencia i − 1 en la intersección de la normalcomún a los ejes i − 1 e i con el eje i y allí situar el vector Zi−1 del sistema de referencia{i− 1}.

4. Ubicar el vector Xi−1 del sistema de referencia {i− 1} a lo largo de la normal común a losejes i− 1 e i .

5. Ubicar ai−1 que se re�ere a la distancia entre los ejes Zi−1 y Zi medida a lo largo del ejeXi−1.

6. Hallar αi−1 que es el ángulo entre los ejes Zi−1 y Zi consecutivos respecto al eje Xi−1.

7. Hallar di que es la distancia entre los ejes Xi−1 y Xi medida a lo largo del eje Zi.

8. Ubicar Θi que es el ángulo medido entre los ejes Xi−1 y Xi respecto al eje Zi.

Si la articulación tratada es de rotación, la variable sera el ángulo Θi; si en cambio la articulaciónes prismática, la variable sera la distancia di.

Por lo tanto cada uno de estos parámetros deberán ser hallados para cada enlace del manipulador.Como consideración tomaremos un sistema de referencia base �jo al cual llamaremos marco osistema de referencia {0} consistente en los ejes X0Y0Z0 como se muestra en la �gura 2.4.Ahora aplicaremos todo lo anterior dicho para hallar cada marco de referencia del manipuladortratado, lo cual nos lleva a la �gura 2.6 en la cual se muestra todos los sistemas de referenciaasignados al manipulador considerando los pasos sistemáticos de Denavit-Hartemberg.

6

Figura 2.6: Asignación completa de sistemas de referencia

i αi−1 ai−1 di θi

1 0 0 0 θ12 −90º 0 d2 θ23 90º 0 d3 04 0 0 0 θ45 −90º 0 0 θ56 90º 0 0 θ6

Cuadro 1: Parámetros D-H del manipulador de 6 GDL

Teniendo ya todos los sistemas de referencia �jados con el �n de reducir parámetros obtenemosla siguiente tabla de parámetros Denavit-Hartemberg.

3. Cinemática Directa del Manipulador.

Para la resolución del problema cinemático directo del manipulador robótico, procederemos a uti-lizar los parámetros Denavit-Hartemberg, que relacionan dos sistemas de referencia consecutivos,solidarios a los ejes de las articulaciones del manipulador.

La forma de expresar esta relación de dos sistemas de referencia consecutivos se consigue medianteuna matriz de transformación homogenea, pero que en este caso, sus elementos estarán expresadoscomo función de las constantes y variables articulares del manipulador.

3.1. Calculo de las matrices de transformación.

Para el calculo de las matrices de transformación que relacionaran un sistema de referencia icon otro i − 1 en función de las constantes y variables articulares utilizaremos los parámetrosD-H hallados anteriormente para las 6 articulaciones del manipulador basándonos en la ecuaciónmatricial 2.1 que es llamada matriz de transformación.

7

Por lo tanto a continuación reemplazaremos los parámetros D-H en tal matriz para cada sistemade referencia, además para poder visualizar de mejor manera los sistemas de referencia se añadenlos grá�cos hallados con el toolbox hemero, una herramienta para Matlab, para análisis de ma-nipuladores y robots móviles, en los cuales se asignan valores arbitrarios de las distancias con el�n de obtener una visualización mas descriptiva.

3.1.1. Sistema de referencia {0}.

El sistema de referencia {0} se considera como sistema de referencia base y es arbitraria suelección, pero una buena elección simpli�caría los siguientes parámetros involucrados, pudiendoconsiderarla como la matriz identidad siguiente:

00T =

1 0 0 00 1 0 00 0 1 00 0 0 1

(3.1)

Esta se puede representar como la �gura 3.1.

Figura 3.1: Marco de referencia {0} de color blanco


El sistema de referencia {1} se considera como sistema de referencia de la primera articulacióndel manipulador, calculada en base a los parámetros D-H para la articulación i = 1. Por lo tanto

8

la matriz de transformación que relaciona el sistema {1} con el sistema de referencia {0} es lasiguiente:

01T =

cos θ1 − sin θ1 0 0sin θ1 cos θ1 0 0

0 0 1 00 0 0 1

(3.2)

Esta se puede representar en la �gura 3.2. En la cual aparece igual que el marco de referen-cia {0} ya que esta girara alrededor de su eje Z1

Figura 3.2: Marco de referencia {1} de color celeste


El sistema de referencia {2} se considera como sistema de referencia de la segunda articulacióndel manipulador, calculada en base a los parámetros D-H para la articulación i = 2. Por lo tantola matriz de transformación que relaciona el sistema {2} con el sistema de referencia {1} es lasiguiente:

9

12T =

cos θ2 − sin θ2 0 0

0 0 0 d2− sin θ2 − cos θ2 1 0

0 0 0 1

(3.3)

Esta se puede representar en la �gura 3.3, de color magenta, en la cual aparece junto al marcode referencia {1}. Cabe aclarar que en la matriz, d2 representa una constante, ya que la variablees θ2.

Figura 3.3: Marco de referencia {2} de color magenta


El sistema de referencia {3} se considera como sistema de referencia de la tercera articulacióndel manipulador, calculada en base a los parámetros D-H para la articulación i = 3. Por lo tantola matriz de transformación que relaciona el sistema {3} con el sistema de {2} es la siguiente:

23T =

1 0 0 00 0 −1 −d30 1 0 00 0 0 1

(3.4)

Esta se puede representar en la �gura 3.4, de color amarillo.

10

Figura 3.4: Marco de referencia {3} de color amarillo


El sistema de referencia {4} se considera como sistema de referencia de la cuarta articulación delmanipulador, calculada en base a los parámetros D-H para la articulación i = 4. Por lo tanto lamatriz de transformación que relaciona el sistema {4} con el sistema de {3} es la siguiente:

34T =

cos θ4 − sin θ4 0 0sin θ4 cos θ4 0 0

0 1 1 00 0 0 1

(3.5)

Esta se puede representar en la �gura 3.5, de color rojo.

11

Figura 3.5: Marco de referencia {4} de color rojo


El sistema de referencia {5} se considera como sistema de referencia de la quinta articulación delmanipulador, calculada en base a los parámetros D-H para la articulación i = 5. Por lo tanto lamatriz de transformación que relaciona el sistema {5} con el sistema de {4} es la siguiente:

45T =


0 0 1 0− sin θ5 − cos θ5 0 0

0 0 0 1

(3.6)

Esta se puede representar en la �gura 3.6, de color azul.

12

Figura 3.6: Marco de referencia {5} de color azul


El sistema de referencia {6} se considera como sistema de referencia de la sexta articulación delmanipulador, calculada en base a los parámetros D-H para la articulación i = 6. Por lo tanto lamatriz de transformación que relaciona el sistema {6} con el sistema de {5} es la siguiente:

56T =


0 0 −1 0sin θ6 cos θ6 0 0

0 0 0 1

(3.7)

Esta se puede representar en la �gura 3.7, de color verde.

13

Figura 3.7: Marco de referencia {6} de color verde

3.2. Visualización general de los sistemas de referencia.

Lo anterior mostraba como obtener los marcos de referencia y las matrices de transformaciónhomogenea, que relacionaban sistemas de referencia consecutivos. Por lo tanto aquí mostraremostodos los marcos de referencia utilizando el toolbox HEMERO para Matlab, para la visualizaciónde los marcos de referencia en Matlab.

14

3.2.1. Código en Matlab para la visualización de los marcos de referencia:

%Variables articulares 5 rotacionales tx y 1 prismática d3

syms t1 t2 d3 t4 t5 t6 real;

syms d2 real;

d2=2; %d2 es constante, y suponemos un valor para visualización

t1=0; %Según la figura 2.6 damos

t2=pi/2; %valores a las variables

d3=4; %articulares para obtener los

t4=0; %marcos de referencia

t5=pi/2;

t6=0;

%Ingresamos la matriz de parámetros D-H

%(alpha_i-1, a_i-1, theta-i, d_i-1, rho)

%Para rho=0 la articulación es de rotación

%Para rho=1 la articulación es prismática

dh=[0 0 t1 0 0;

-pi/2 0 t2 d2 0;

pi/2 0 0 d3 1;

0 0 t4 0 0;

-pi/2 0 t5 0 0;

pi/2 0 t6 0 0];

%Utilizamos el vector q

%de variables articulares

q=[t1 t2 d3 t4 t5 t6];

%Matrices de transformación que relacionan.

%articulaciones consecutivas

%Ti(relaciona marco i con marco i-1)

T1=linktrans(dh(1,:),q(1,1));






%Muestra los marcos de referencia

frame(T1,'c',1);

frame(T1*T2,'m',1);

frame(T1*T2*T3,'y',1.2);

frame(T1*T2*T3*T4,'r',0.8);

frame(T1*T2*T3*T4*T5,'b',0.5);

frame(T1*T2*T3*T4*T5*T6,'g',0.3);

%limitación de los ejes

axis([-1 5 -1 5 -1 2]);

grid on

15

El código anterior nos muestra los marcos de referencia como en la �gura. Notemos que en cuantoa la descripción de sistemas de referencia las �guras son iguales o describen lo mismo.

Figura 3.8: Marcos de referencia del manipulador en Matlab

Figura 3.9: Marcos de referencia asignados según D-H

3.3. Solución al problema Cinematico Directo.

Ya determinadas las matrices que relacionan sistemas de referencia en función de sus variablesarticulares, ya podemos determinar la matriz de transformación general, que relaciona, en estecaso la muñeca del manipulador con el sistema de referencia base en función de las variablesarticulares. Esto es:

16

06T = f(θ1, θ2, d3, θ4, θ5, θ6) (3.8)

06T = 0

1T12T

23T

34T

45T

56T (3.9)

Al reemplazar las respectivas matrices de transformación en la ecuación 3.9 obtenemos la matriz06T que viene a ser la solución al problema cinemático directo. Cabe resaltar que la naturalezade la matriz es complicada para sus términos que describen la orientación de el ultimo sistemade referencia referido al sistema base, por lo que aquí no se mostrara todos los elementos de talmatriz salvo cuando sean necesarios.

Pero si creo necesario mostrar como es que la posición del sistema de referencia {6} referenciadoal sistema {0}, tan solo depende de las tres primeras variables articulares. Esto se puede notarviendo que el manipulador se sitúa en un punto del espacio gracias a las tres primeras articula-ciones, y las tres siguientes son dedicadas a orientar el efector �nal de la herramienta. Lo dichoanteriormente se ve de manera mas clara al ver los términos de posición que presenta la matrizsolución.

06T =

r11 r12 r13 pxr21 r22 r23 pyr31 r32 r33 pz0 0 0 1

(3.10)

06T =

X X X c1s2d3 − s1d2X X X s1s2d3 + c1d2X X X c2d30 0 0 1

(3.11)

4. Cinemática Inversa del Manipulador

Ya teniendo conocida la cinemática directa del manipulador robótico estudiado, ahora buscare-mos las relaciones que describen sus cinemática inversa y que por lo tanto darán solución alproblema cinemático inverso del manipulador.

El problema cinemático inverso asume en un principio que son conocidos los valores de posicióny orientación del efector �nal(en este caso, de el ultimo sistema de referencia), en relación conel sistema de referencia base del manipulador. Es decir la matriz dada en 3.10 es conocida contodos sus parámetros.

Sin embargo obtuvimos otra descripción de la matriz 06T en el problema cinemático directo

mediante 3.11, como vimos esta dada como una función de las variables articulares. Mediantela relación de estas dos representaciones de las matrices generales, sera que podremos hallar lasvariables articulares como función de la posición y orientación dada.

Analizando el manipulador y observando como están asignados los marcos de referencia podemosver que el marco {6} esta asignado en la muñeca del manipulador, al igual que los marcos {5}{4} y {3} , además de observar los términos de posición de la matriz dada en 3.11. Se puede

17

concluir que este manipulador, como dijimos antes, utiliza solo las tres primeras articulacionespara posicionar su muñeca y las otras tres ultimas para orientar su efector �nal.

Por lo tanto podemos a�rmar que la las componentes de posición de la matriz 03T nos bastarían

para poder hallar las tres primeras variables articulares θ1 θ2 y d3.

Las siguientes ecuaciones describirían lo anteriormente dicho:

03T = 0

1T12T

23T (4.1)

4.1. Hallando θ1.

Dado que el lado izquierdo de la ecuación es totalmente conocido, y que el lado izquierdo esta enfunción de las tres variables, podríamos pasar una de estas tres variables al lado izquierdo parapoder hallar relaciones que solucionen la primera variable.

01T

−103T = 1

2T23T (4.2)

X X X c1px + s1pyX X X −s1px + c1pyX X X pz0 0 0 1

=

X X X s2d3X X X d2X X X c2d30 0 0 1

(4.3)

Por lo tanto:−s1px + c1py = d2 (4.4)

Dado que px y py son las componentes en X6 y Y6 del punto de posición deseado, podríamosde�nir la proyección escalar del vector que de�ne el punto deseado sobre el plano XY como:

rxy =

√(px)

2+ (py)

2(4.5)

Existiendo un ángulo φ que de�ne esta proyección en el plano XY , entonces:

px = rxy cosφ (4.6)

py = rxy sinφ (4.7)

Reemplazando las ecuaciones anteriores en 4.4 tendríamos que:

sin(φ− θ1) =d2rxy

18

entonces:

cos(φ− θ1) =±√r2xy − d22rxy

sin(φ− θ1)

cos(φ− θ1)=

d2

±√r2xy − d22

φ− θ1 = Atan2(d2,±√r2xy − d22)

θ1 = Atan2(px, py)−Atan2(d2,±√r2xy − d22) (4.8)

La ecuación 4.8 da la solución de la variable articular θ1. Es de notar que depende absolutamentede parámetros conocidos, y que puede tener múltiples soluciones en diferentes cuadrantes.

Ahora ya conocida la variable articular 1 podemos continuar hallando las demás en función deesta.

4.2. Hallando θ2.

Ya conocida la primera variable articular podemos deducir la siguiente ecuaciones de la relación4.3.

s2d3c2d3

=c1px + s1py

pz

θ2 = Atan2(c1px + s1py,pz) (4.9)

La ecuación 4.9 da la solución para la variable articular θ2 basándose en la variable θ1 comoaclaramos antes.

4.3. Hallando d3.

De la relación 4.3 obtenemos:

s2d3 = c1px + s1py (4.10)

c2d3 = pz (4.11)

19

multiplicando por s2 a la primera expresión, y por c2 a la segunda expresión, y luego sumandolas dos ecuaciones tenemos:

d3(s22 + c22) = s1(c1px + s1py) + c2pz

d3 = s1(c1px + s1py) + c2pz (4.12)

Por lo tanto la ecuación 4.12 da la solución para la variable prismática d3 .

4.4. Hallando θ4.

Ya que es obvio que agotamos las posibilidades que nos brindaba la relación 4.3, en cuanto ahallar variables relacionales, ahora necesitamos de�nir otra relación para poder hallar las demásvariables articulares, que por lo general dependerán de todas las variables, excepto de la variableprismática, ya que esta no actúa como elemento dotador de orientación, solo de translación.

Entonces podríamos de�nir la siguiente relaciones:

06T = 0

3T34T

45T

56T

03T

−106T =3

4 T45 T

56T

X X c1c2r13 + s1c2r23 − s2r33 XX X −s1r13 + c1r23 X

c2r31 + c1s2r11 + s1s2r21 c2r32 + c1s2r12 + s1s2r22 c2r33 + c1s2r13 + s1s2r23 X0 0 0 1

=

X X c4s5 XX X s4s5 X−s5c6 s5s6 c5 X

0 0 0 1

(4.13)

La ecuación matricial 4.13 nos ayudara ahora a encontrar las demás variables articulares, debidoa que los términos mostrados de la ecuación, son los que contienen la información útil para nue-stros �nes.

De los términos (2, 3) y (1, 3) de la relación 4.13 podemos dividirlos y obtener:

s4c4

=−s1r13 + c1r23

c1c2r13 + s1c2r23 − s2r33

θ4 = Atan2(−s1r13 + c1r23, c1c2r13 + s1c2r23 − s2r33) (4.14)

La ecuación 4.14 nos da la solución para la variable articular θ4, vale recordar que dijimos queesta dependería de las variables anteriores, pero menos de la variable prismática d3.

20

4.5. Hallando θ5.

Otra vez referenciandonos en la relación 4.13, podemos utilizar las ecuaciones que posee. Si es-pecí�camente utilizamos los términos (2, 3) y (3, 3) de 4.13 obtenemos:

s4s5 = −s1r13 + c1r23

c5 = c2r33 + c1s2r13 + s1s2r23

entonces:s5c5

=−s1r13+c1r23/s4

c2r33 + c1s2r13 + s1s2r23

θ5 = Atan2(−s1r13+c1r23/s4, c2r33 + c1s2r13 + s1s2r23) (4.15)

La ecuación 4.15 nos da la solución para la variable articular θ5, de nuevo se nota la dependenciade todas las variables rotacionales anteriores, excepto de la prismática d3.

4.6. Hallando θ6.

Para terminar con la ultima variable rotacional, nos referimos a los términos (3, 2) y (3,1) de4.13 para obtener:

s5s6 = c2r32 + c1s2r12 + s1s2r22

−s5c6 = c2r31 + c1s2r11 + s1s2r21

entonces:s6c6

=c2r32+c1s2r12+s1s2r22/s5

c2r31 + c1s2r11 + s1s2r21

θ6 = Atan2(c2r32+c1s2r12+s1s2r22/s5, c2r31 + c1s2r11 + s1s2r21) (4.16)

La ecuación 4.16 nos da la solución para la variable articular θ6, que tampoco es dependiente ded3.

Con haber hallado todas las variables articulares en función de la posición y orientación deseadas,el problema cinemático inverso, esta resuelto.

21

4.7. Aclaraciones.

Luego de haber solucionado el problema cinemático inverso, cabe aclarar, que este supone lalocalización y orientación de la muñeca del manipulador(marco de referencia {6}), por lo tantotodos los cálculos realizados hasta aquí son para localizar y dar dirección a la muñeca del ma-nipulador, referenciado al sistema base {0}, que además no esta �jado a la base del manipuladorfísico en si; si no que por motivos de reducir parámetros complicados se situó en el mismo puntoque {1}.

Por lo dicho anteriormente, y además ver el manipulador físico en si, con su efector �nal de unlargo dado, uno se preguntaría... ¾De que me sirve posicionar la muñeca y no el efector �nal ensi?¾Y porque no considerar el largo del efector �nal?. Pues la respuesta es sencilla; dado que elefector �nal esta unido al sistema {6} y además esta a lo largo de su eje Z6, bastaría con hallarlas relaciones para la muñeca, y luego tan solo adicionar la distancia del efector al eje Z0 de lamatriz de transformación general.

5. Modelo Dinámico del Manipulador.

En un manipulador robótico, todas las velocidades lineales y angulares que intervienen, estándadas por fuerzas y pares que se aplican a los vínculos o enlaces. Pero no solo dependen de lasfuerzas aplicadas, ya que también dependen de la masa, y mas especí�camente de la distribuciónde la masa de cada enlace.

Estas variables involucradas y relacionadas constituyen el modelo dinámico del manipulador,del cual, su identi�cación es en general difícil, debido a que se necesita conocer la distribuciónde masas de cada vinculo, que se describen por un tensor de inercia, en el cual también estainvolucrado la densidad de masa, que puede ser o no constante.

Todas estas restricciones hacen que sea muy difícil la obtención de un modelo dinámico realista.

Pero para �nes de comprensión en como se relacionan tales variables en un manipulador, sepueden considerar ciertos parámetros ideales, con lo cual la obtención del modelo dinámicopodría lograrse sin demasiado esfuerzo, con el consiguiente error introducido, pero aceptablepara ciertas aplicaciones que no necesiten demasiada precisión.

5.1. Uso del toolbox HEMERO.

El planteamiento que utiliza el toolbox Hemero involucra a las ecuaciones de Newton-Euler paraobtener el modelo en forma iterativa.

Primero se debe describir correctamente todos los parámetros necesarios para calcular el modelodinámico y tener en cuenta todos los factores que se están idealizando. Para tal efecto se incluyeen la imagen 5.1 la descripción de la matriz de parámetros dyn que utiliza hemero.

Aquí es necesario comentar los parámetros que para este estudio son desconocidos, y que porcuestiones de solubilidad obviaremos:

22

Figura 5.1: Parámetros de la matriz dyn que utiliza Hemero.

Ixx, Iyy, Izz, Ixy, Iyz, Izx: Son los elementos del tensor de inercia, referidos al centro demasas del enlace, los consideraremos nulos ya que estos describen la distribución de masasde un vinculo referidos a su centro de masa. Como para este estudio consideraremos quenuestros vínculos tienen masas concentradas, todos estos parámetros serán 0.

Jm: Inercia de la armadura, la cual también obviaremos, debido a que no consideraremosparámetros de los actuadores. Este valor sera 0

G : Velocidad de la articulación/Velocidad del vinculo, este parámetro lo consideraremoscomo 1, debido a que no habrá relación en la transmisión.

B, T+c , T−

c : Estos parámetros serán 0, debido a que no consideraremos fricciones, ni vis-cosas ni electrostáticas.

Por lo tanto la matriz de parámetros dyn se hace obvia, y se mostrara mas adelante en la secciónde código.

5.1.1. Asignación de parámetros según manipulador.

Aquí describiremos una parte de los parámetros que se deberían asignar de la forma adecuada,para que el toolbox Hemero, realice de manera correcta su algoritmo.

Nos centraremos en el caso de asignación de masas a los enlaces del manipulador de 6 GDL.En la �gura 5.2 se muestra la asignación de masas con puntos de colores sobre cada vinculo,que pertenecen al centro de masa de cada enlace. Estos centros de masas de manera lógica se

23

Figura 5.2: Descripción de centros de masas y distancias a los orígenes de los marcos de referencia

situaran según la geometría del enlace(considerando una densidad de masa ρ constante), aunqueclaramente esto volverá mas compleja la solución.

Debería estar claro el hecho de que las masas consideradas en este análisis son concentradas, loque signi�ca que las fuerzas aplicables se realizarían sobre los centros de masas. La a�rmaciónanterior genera que los parámetro de inercia sean 0.

Para el correcto funcionamiento del algoritmo, se le debe de pasar los parámetros correctos, y esaquí que nos centraremos en los siguientes parámetros:

mi: Son las masas totales(en nuestro caso concentradas y actúan sobre un solo punto) delos enlaces i del manipulador.

rx, ry, rz: Describen la posición del centro de masa de un enlace respecto al cuadro dereferencia del enlace. Para nuestro caso solo actúan en su componente rz, ya que nuestroscentros de masas están todos sobre el eje Z.

g: in�uencia de la gravedad. Se toma referenciada al sistema de referencia base {0}, por lotanto aquí se modela por −gZ0.

De lo aclarado anteriormente se desprende el siguiente cuadro basado en la �gura 5.2 para elmanipulador robótico de 6 GDL.

i mi rx ry rz

1 m1 0 0 −l12 m2 0 0 −l23 m3 0 0 −l34 m4 0 0 −l45 m5 0 0 06 m6 0 0 l6

Cuadro 2: Parámetros de ubicación y magnitud de masas del manipulador, para sus 6 enlaces.

24

5.1.2. Script para la generación de las matrices M , G y V .

La generalización para un robot manipulador de n articulaciones, suponiendose las masas con-centradas se da mediante la siguiente relación:

τ = M(θ)θ +G(θ) + V (θ, θ) + F (θ) (5.1)

En al cual se podría desacoplar la expresión F (θ) que representa al vector de fricciones, ya quesolo depende de los enlaces propios.

Se propone aquí el script para la generación de las matrices M , G, y V , además de la generacióndel modelo completo como un vector �la τ que represente la relación entre los pares torsionalesen cada articulación como función de la posición, velocidades, y aceleraciones del sistema.

25

% %Programa realizado para hallar las matrices involucradas

% %en el modelo dinámico del manipulador de 6 GDL

% %¨Stanford Arm¨

% %Realizado por: Macedo Vilca Denis Daniel

%Variables articulares 5 rotacionales "tx" y 1 prismática "d3"

syms t1 t2 d3 t4 t5 t6 real;

%Velocidades articulares 5 angulares "tdx" y 1 lineal "dd3"

syms td1 td2 dd3 td4 td5 td6 real;

%Aceleraciones articulares

syms tdd1 tdd2 ddd3 tdd4 tdd5 tdd6 real;

%Longitudes de los marcos de referencia al centro de masa concentrado

syms d2 l1 l2 l3 l4 l5 l6 real;

%Masas totales concentradas de los vínculos

syms m1 m2 m3 m4 m5 m6 real;

%Aceleración de la gravedad

syms g real;

%La matriz dyn con los parámetros dinámicos del manipulador

dyn=[0 0 t1 0 0 m1 0 0 -l1 0 0 0 0 0 0 0 1 0 0 0;

-pi/2 0 t2 d2 0 m2 0 0 -l2 0 0 0 0 0 0 0 1 0 0 0;

pi/2 0 0 d3 1 m3 0 0 -l3 0 0 0 0 0 0 0 1 0 0 0;

0 0 t4 0 0 m4 0 0 -l4 0 0 0 0 0 0 0 1 0 0 0;

-pi/2 0 t5 0 0 m5 0 0 0 0 0 0 0 0 0 0 1 0 0 0;

pi/2 0 t6 0 0 m6 0 0 l6 0 0 0 0 0 0 0 1 0 0 0];

%vector de variables articulares

q=[t1 t2 d3 t4 t5 t6];

%vector de velocidades articulares

qd=[td1 td2 dd3 td4 td5 td6];

%vector de aceleraciones articulares

qdd=[tdd1 tdd2 ddd3 tdd4 tdd5 tdd6];

%vector aceleración de la gravedad

grav=[0 0 -g];

%Calcular la matriz de masas

M=simple(inertia(dyn,q))

%Calcular la matriz de gravedad

G=simple(gravity(dyn,q,grav))

%Calcular la matriz de coriolis y centrifuga

V=simple(coriolis(dyn,q,qd))

%Calculo del modelo dinámico completo

%tau=rne(dyn,q,qd,qdd,grav)

%simple(tau)

Con el script presentado y el toolbox Hemero, por lo tanto se pueden calcular las matricesnecesarias para la descripción del modelo dinámico general.

Estas matrices serán reemplazadas en la ecuación 5.1 para tener solucionado el modelo dinámicocompleto.

Al ser este un manipulador de 6 grados de libertad la dinámica del mismo, aun reduciendoparámetros, es muy complicada y no amerita colocar toda la expresión resultante para el modelo

26

dinámico general. Por lo tanto se considera resuelto el modelo dinámico del manipulador alobtener por Matlab las matrices involucradas.

5.2. Aclaraciones.

En los scripts entregados, es necesario notar que se pueden cambiar los parámetros de la matrizdyn, con el �n de reducir aun mas las expresiones resultantes, aunque aun así las expresionesseguirán siendo inmensas. solo se podrá notar una considerable variación en las matrices simplesy no en el modelo completo.

Por ejemplo se podría ubicar las masas concentradas en el mismo sistema de referencia al cualpertenece, con ello se anularían las longitudes que de�nen el centro de masas.

Con esto se da por concluido este análisis.

27

informe robotica

Documents