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

Robótico de 6 GDL Stanford Arm

Macedo Vilca Denis Daniel

jdmacedo@ieee.org

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.1.2. Sistema de referencia {1}. . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
3.1.3. Sistema de referencia {2}. . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
3.1.4. Sistema de referencia {3}. . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
3.1.5. Sistema de referencia {4}. . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
3.1.6. Sistema de referencia {5}. . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
3.1.7. Sistema de referencia {6}. . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
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 de

6 grados de libertad, especícamente el manipulador robótico Stanford Arm el cual consta de
5 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 consecutivas
y que se tomará en cuenta en las secciones siguientes y que serán la notación básica para analizar
el manipulador, para luego según dado marcos de referencias unidos a cada articulación, tratar
sobre 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 cada
articulación.

Ya obtenidos todos los parámetros Denavit-Hartemberg podremos utilizarlos para encontrar la

matriz de transformación homogenea general del manipulador(Problema Cinemático Directo)
utilizando para ello, las matrices de transformación homogenea que relacionan cada marco de
referencia 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 nal
teniendo 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 todas
las articulaciones del manipulador, entonces según la matriz de transformación total podremos
saber 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 valores
que 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 se
sitúe en la posición y orientación deseada(múltiples soluciones), no es necesariamente fácil saber
todos los conjuntos de variables que serian soluciones; Por lo tanto el problema que se pretende
resolver es el Problema Cinematico Inverso, el cual intenta saber cuales deberían ser los valores
de las variables de cada articulación del manipulador, para que este se posicione y se oriente
como 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 reales
tiene 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 dene la relación existente
entre dos ejes articulares consecutivos de un manipulador. Por lo visto en la gura 2.1 un enlace
esta denido por dos valores constantes.

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

ai−1 se reere a la distancia entre los ejes consecutivos i − 1 e i, esta distancia es medida a lo
largo de la linea mutuamente perpendicular a los dos ejes.
αi−1 es el ángulo que se reere a la localización relativa de los dos ejes consecutivos respecto a
la 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ón
entre 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 reere a la distancia entre las extensiones de ai−1 y ai medida a lo largo del eje i.
Θi se reere 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 en
este 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 manipulador
robótico de 6 grados de libertad. En la gura 2.4, se muestra una presentación simple de este
basá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 los
orí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 relaciona
el sistema de referencia i con el sistema i − 1. Esta matriz esta dada por la ecuación 2.1.

 
cθi −sθi 0 ai−1
sθ i cαi−1 cθi cαi−1 −sαi−1 −sαi−1 di 
i−1
(2.1)

i T =
 sθi sαi−1

cθi sαi−1 cαi−1 cαi−1 di 
0 0 0 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 la
siguiente manera:

1. Encontrar los ejes de las articulaciones, estos serán para articulaciones rotacionales los ejes
de 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 para
ejes paralelos en los cuales existen innitas normales comunes.
3. Ubicar el punto de origen del sistema de referencia i − 1 en la intersección de la normal
común a los ejes i − 1 e i con el eje i y allí situar el vector Ẑi−1 del sistema de referencia
{i − 1}.

4. Ubicar el vector X̂i−1 del sistema de referencia {i − 1} a lo largo de la normal común a los
ejes i − 1 e i .
5. Ubicar ai−1 que se reere a la distancia entre los ejes Ẑi−1 y Ẑi medida a lo largo del eje
X̂i−1 .

6. Hallar αi−1 que es el ángulo entre los ejes Ẑi−1 y Ẑi consecutivos respecto al eje X̂i−1 .
7. Hallar di que es la distancia entre los ejes X̂i−1 y X̂i medida a lo largo del eje Ẑi .
8. Ubicar Θi que es el ángulo medido entre los ejes X̂i−1 y X̂i respecto al eje Ẑi .

Si la articulación tratada es de rotación, la variable sera el ángulo Θi ; si en cambio la articulación

es 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 o
sistema de referencia {0} consistente en los ejes X̂0 Ŷ0 Ẑ0 como se muestra en la gura 2.4.
Ahora aplicaremos todo lo anterior dicho para hallar cada marco de referencia del manipulador
tratado, lo cual nos lleva a la gura 2.6 en la cual se muestra todos los sistemas de referencia
asignados 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 θ1
2 −90º 0 d2 θ2
3 90º 0 d3 0
4 0 0 0 θ4
5 −90º 0 0 θ5
6 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 obtenemos
la 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 mediante
una matriz de transformación homogenea, pero que en este caso, sus elementos estarán expresados
como 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 i

con otro i − 1 en función de las constantes y variables articulares utilizaremos los parámetros
D-H hallados anteriormente para las 6 articulaciones del manipulador basándonos en la ecuación
matricial 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 sistema
de referencia, además para poder visualizar de mejor manera los sistemas de referencia se añaden
los 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 su

elección, pero una buena elección simplicaría los siguientes parámetros involucrados, pudiendo
considerarla como la matriz identidad siguiente:

 
1 0 0 0
0  0 1 0 0 
(3.1)

0T =  0

0 1 0 
0 0 0 1

Esta se puede representar como la gura 3.1.

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

3.1.2. Sistema de referencia {1}.

El sistema de referencia {1} se considera como sistema de referencia de la primera articulación

del 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 la
siguiente:

 
cos θ1 − sin θ1 0 0
0  sin θ1 cos θ1 0 0 
(3.2)

1T = 

0 0 1 0 
0 0 0 1

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 Ẑ1

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

3.1.3. Sistema de referencia {2}.

El sistema de referencia {2} se considera como sistema de referencia de la segunda articulación

del manipulador, calculada en base a los parámetros D-H para la articulación i = 2. Por lo tanto
la matriz de transformación que relaciona el sistema {2} con el sistema de referencia {1} es la
siguiente:

9
  
cos θ2 − sin θ2 0 0
0 0 0 d2 
1
(3.3)

2 T =  − sin θ
 
2 − cos θ2 1 0 
0 0 0 1

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

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

3.1.4. Sistema de referencia {3}.

El sistema de referencia {3} se considera como sistema de referencia de la tercera articulación

del manipulador, calculada en base a los parámetros D-H para la articulación i = 3. Por lo tanto
la matriz de transformación que relaciona el sistema {3} con el sistema de {2} es la siguiente:

 
1 0 0 0
 0 0 −1 −d3 
2
(3.4)

3T =  0

1 0 0 
0 0 0 1

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

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

3.1.5. Sistema de referencia {4}.

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

 
cos θ4 − sin θ4 0 0
3  sin θ4 cos θ4 0 0 
(3.5)

4T = 

0 1 1 0 
0 0 0 1

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

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

3.1.6. Sistema de referencia {5}.

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

 
cos θ5 − sin θ5 0 0
0 0 1 0 
4
(3.6)

5 T =  − sin θ
 
5 − cos θ5 0 0 
0 0 0 1

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

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

3.1.7. Sistema de referencia {6}.

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

 
cos θ6 − sin θ6 0 0
 0 0 −1 0 
5
(3.7)

6 T =  sin θ

6 cos θ6 0 0 
0 0 0 1

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ón
homogenea, que relacionaban sistemas de referencia consecutivos. Por lo tanto aquí mostraremos
todos los marcos de referencia utilizando el toolbox HEMERO para Matlab, para la visualización
de 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));
T2=linktrans(dh(2,:),q(1,2));
T3=linktrans(dh(3,:),q(1,3));
T4=linktrans(dh(4,:),q(1,4));
T5=linktrans(dh(5,:),q(1,5));
T6=linktrans(dh(6,:),q(1,6));
%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 cuanto
a 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 variables
articulares, ya podemos determinar la matriz de transformación general, que relaciona, en este
caso la muñeca del manipulador con el sistema de referencia base en función de las variables
articulares. Esto es:

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

0
6T = 01 T 12 T 23 T 34 T 45 T 56 T (3.9)

Al reemplazar las respectivas matrices de transformación en la ecuación 3.9 obtenemos la matriz

6 T que viene a ser la solución al problema cinemático directo. Cabe resaltar que la naturaleza
0

de la matriz es complicada para sus términos que describen la orientación de el ultimo sistema
de referencia referido al sistema base, por lo que aquí no se mostrara todos los elementos de tal
matriz salvo cuando sean necesarios.
Pero si creo necesario mostrar como es que la posición del sistema de referencia {6} referenciado
al sistema {0}, tan solo depende de las tres primeras variables articulares. Esto se puede notar
viendo 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 dicho
anteriormente se ve de manera mas clara al ver los términos de posición que presenta la matriz
solución.
 
r11 r12 r13 px
 r21 r22 r23 py 
0
6T =  r
  (3.10)
31 r32 r33 pz 
0 0 0 1
 
X X X c1 s2 d3 − s1 d2
0  X X X s1 s2 d3 + c1 d2 
(3.11)

6T =  X

X X c 2 d3 
0 0 0 1

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 al
problema cinemático inverso del manipulador.

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

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

mediante 3.11, como vimos esta dada como una función de las variables articulares. Mediante
la relación de estas dos representaciones de las matrices generales, sera que podremos hallar las
variables 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 podemos
ver 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 articulaciones
para posicionar su muñeca y las otras tres ultimas para orientar su efector nal.
0
Por lo tanto podemos armar que la las componentes de posición de la matriz 3 T nos bastarían
para poder hallar las tres primeras variables articulares θ1 θ2 y d3 .

Las siguientes ecuaciones describirían lo anteriormente dicho:

0
3T = 01 T 12 T 23 T (4.1)

4.1. Hallando θ1 .
Dado que el lado izquierdo de la ecuación es totalmente conocido, y que el lado izquierdo esta en
función de las tres variables, podríamos pasar una de estas tres variables al lado izquierdo para
poder hallar relaciones que solucionen la primera variable.
0 −10
1T 3T = 12 T 23 T (4.2)

   
X X X c1 px + s1 py X X X s2 d3
 X X X −s1 px + c1 py 
= X X X d2 
(4.3)

 
 X X X pz   X X X c2 d3 
0 0 0 1 0 0 0 1

Por lo tanto:
−s1 px + c1 py = d2 (4.4)

Dado que px y py son las componentes en X̂6 y Yˆ6 del punto de posición deseado, podríamos
denir la proyección escalar del vector que dene el punto deseado sobre el plano XY como:
q
rxy
2
= (px ) + (py )
2
(4.5)

Existiendo un ángulo φ que dene 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:

d2
sin(φ − θ1 ) =
rxy

18
entonces: q
2 − d2
± rxy 2
cos(φ − θ1 ) =
rxy

sin(φ − θ1 ) d2
= q
cos(φ − θ1 ) 2 − d2
± rxy 2

q
2 − d2 )
φ − θ1 = Atan2(d2 , ± rxy 2

q
θ1 = Atan2(px , py ) − Atan2(d2 , ± 2 − d2 )
rxy 2 (4.8)

La ecuación 4.8 da la solución de la variable articular θ1 . Es de notar que depende absolutamente

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

4.2. Hallando θ2 .

Ya conocida la primera variable articular podemos deducir la siguiente ecuaciones de la relación

4.3.
s2 d3 c1 px + s1 py
=
c2 d3 pz

θ2 = Atan2(c1 px + s1 py, pz ) (4.9)

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

aclaramos antes.

4.3. Hallando d3 .

De la relación 4.3 obtenemos:

s2 d3 = c1 px + s1 py (4.10)
c2 d3 = pz (4.11)

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

d3 (s22 + c22 ) = s1 (c1 px + s1 py ) + c2 pz

d3 = s1 (c1 px + s1 py ) + c2 pz (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 a
hallar variables relacionales, ahora necesitamos denir otra relación para poder hallar las demás
variables articulares, que por lo general dependerán de todas las variables, excepto de la variable
prismática, ya que esta no actúa como elemento dotador de orientación, solo de translación.
Entonces podríamos denir la siguiente relaciones:
0
6T = 03 T 34 T 45 T 56 T

0 −10
3T 6T =34 T54 T 56 T

 
X X c1 c2 r13 + s1 c2 r23 − s2 r33 X

 X X −s1 r13 + c1 r23 X 

 c2 r31 + c1 s2 r11 + s1 s2 r21 c2 r32 + c1 s2 r12 + s1 s2 r22 c2 r33 + c1 s2 r13 + s1 s2 r23 X 
0 0 0 1
 
X X c4 s5 X
 X X s 4 s5 X 
(4.13)

= −s5 c6 s5 s6 c5 X 
0 0 0 1

La ecuación matricial 4.13 nos ayudara ahora a encontrar las demás variables articulares, debido
a 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:

s4 −s1 r13 + c1 r23
=
c4 c1 c2 r13 + s1 c2 r23 − s2 r33

θ4 = Atan2(−s1 r13 + c1 r23 , c1 c2 r13 + s1 c2 r23 − s2 r33 ) (4.14)

La ecuación 4.14 nos da la solución para la variable articular θ4 , vale recordar que dijimos que
esta 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:

s4 s5 = −s1 r13 + c1 r23

c5 = c2 r33 + c1 s2 r13 + s1 s2 r23

entonces:
s5 −s1 r13 +c1 r23/s4
=
c5 c2 r33 + c1 s2 r13 + s1 s2 r23

θ5 = Atan2(−s1 r13 +c1 r23/s4 , c2 r33 + c1 s2 r13 + s1 s2 r23 ) (4.15)

La ecuación 4.15 nos da la solución para la variable articular θ5 , de nuevo se nota la dependencia
de 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) de
4.13 para obtener:

s5 s6 = c2 r32 + c1 s2 r12 + s1 s2 r22

−s5 c6 = c2 r31 + c1 s2 r11 + s1 s2 r21

entonces:
s6 c2 r32 +c1 s2 r12 +s1 s2 r22/s5
=
c6 c2 r31 + c1 s2 r11 + s1 s2 r21

θ6 = Atan2(c2 r32 +c1 s2 r12 +s1 s2 r22/s5 , c2 r31 + c1 s2 r11 + s1 s2 r21 ) (4.16)

La ecuación 4.16 nos da la solución para la variable articular θ6 , que tampoco es dependiente de
d3 .
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 la
localización y orientación de la muñeca del manipulador(marco de referencia {6}), por lo tanto
todos 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 manipulador
físico en si; si no que por motivos de reducir parámetros complicados se situó en el mismo punto
que {1}.
Por lo dicho anteriormente, y además ver el manipulador físico en si, con su efector nal de un
largo dado, uno se preguntaría... ¾De que me sirve posicionar la muñeca y no el efector nal en
si?¾Y porque no considerar el largo del efector nal?. Pues la respuesta es sencilla; dado que el
efector nal esta unido al sistema {6} y además esta a lo largo de su eje Zˆ6 , bastaría con hallar
las relaciones para la muñeca, y luego tan solo adicionar la distancia del efector al eje Zˆ0 de la
matriz de transformación general.

5. Modelo Dinámico del Manipulador.

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

Estas variables involucradas y relacionadas constituyen el modelo dinámico del manipulador,

del cual, su identicación es en general difícil, debido a que se necesita conocer la distribución
de masas de cada vinculo, que se describen por un tensor de inercia, en el cual también esta
involucrado 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, se
pueden considerar ciertos parámetros ideales, con lo cual la obtención del modelo dinámico
podría lograrse sin demasiado esfuerzo, con el consiguiente error introducido, pero aceptable
para 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 para
obtener el modelo en forma iterativa.

Primero se debe describir correctamente todos los parámetros necesarios para calcular el modelo
dinámico y tener en cuenta todos los factores que se están idealizando. Para tal efecto se incluye
en 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 por
cuestiones 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 de
masas del enlace, los consideraremos nulos ya que estos describen la distribución de masas
de un vinculo referidos a su centro de masa. Como para este estudio consideraremos que
nuestros 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 consideraremos
parámetros de los actuadores. Este valor sera 0
G : Velocidad de la articulación/Velocidad del vinculo, este parámetro lo consideraremos
como 1, debido a que no habrá relación en la transmisión.
B , Tc+ , Tc− : 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ón
de 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), aunque
claramente esto volverá mas compleja la solución.

Debería estar claro el hecho de que las masas consideradas en este análisis son concentradas, lo
que signica que las fuerzas aplicables se realizarían sobre los centros de masas. La armación
anterior 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 es
aquí que nos centraremos en los siguientes parámetros:

mi : Son las masas totales(en nuestro caso concentradas y actúan sobre un solo punto) de
los enlaces i del manipulador.
rx , ry , rz : Describen la posición del centro de masa de un enlace respecto al cuadro de
referencia del enlace. Para nuestro caso solo actúan en su componente rz , ya que nuestros
centros de masas están todos sobre el eje Ẑ .
g : inuencia de la gravedad. Se toma referenciada al sistema de referencia base {0}, por lo
tanto aquí se modela por −g Zˆ0 .

De lo aclarado anteriormente se desprende el siguiente cuadro basado en la gura 5.2 para el

manipulador robótico de 6 GDL.

i mi rx ry rz
1 m1 0 0 −l1
2 m2 0 0 −l2
3 m3 0 0 −l3
4 m4 0 0 −l4
5 m5 0 0 0
6 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 que
solo 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ón
del modelo completo como un vector la τ que represente la relación entre los pares torsionales
en 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 matrices
necesarias 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ámico
completo.
Al ser este un manipulador de 6 grados de libertad la dinámica del mismo, aun reduciendo
pará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 al
obtener por Matlab las matrices involucradas.

5.2. Aclaraciones.

En los scripts entregados, es necesario notar que se pueden cambiar los parámetros de la matriz
dyn, con el n de reducir aun mas las expresiones resultantes, aunque aun así las expresiones
seguirán siendo inmensas. solo se podrá notar una considerable variación en las matrices simples
y no en el modelo completo.
Por ejemplo se podría ubicar las masas concentradas en el mismo sistema de referencia al cual
pertenece, con ello se anularían las longitudes que denen el centro de masas.
Con esto se da por concluido este análisis.

27