Cinematica Directa e Inversa de Un Robot de 4 Grados de Libertad PDF

2012-B ROBÓTICA – CINEMÁTICA DEL ROBOT 4DOF - RPPR
“CINEMATICA DIRECTA E INVERSA DE UN ROBOT

4DOF – RPPR”
Cuya Solari, Omar Antonio
oacs_198@hotmail.com
Flores Bustinza, Edwing Irwing
irwing_1988_@hotmail.com
Torres Chavez, Jonathan Emmanuel
jonathan260605@hotmail.com
1
En el presente informe se desarrolla el análisis de la cinemática directa e inversa

de un Robot de cuatro grados de libertad del tipo Revoluta – Prisma – Prisma –
Revoluta que podemos observar en la figura N°1.
Figura N°1.- Robot 4DOF - RPPR
El proceso del desarrollo de la cinemática del robot se basa en el problema de

hallar la posición y orientación del efector final del robot (último eslabón) para los
valores de las coordenadas articulares del robot (cinemática directa) y viceversa
(cinemática inversa).
DIRECTA
Valor de las Coordenadas articulares Posición y orientación del efector final del robot
(q1, q2,…,qn) INVERSA (x,y,z,α,β,ϒ)
Figura N°2.- Cinemática del Robot
I.- Cinemática Directa
Para comenzar el análisis, abordaremos el tema de la cinemática directa. Primero

debemos establecer los frames o ejes de referencia para cada juntura presente en
el robot. Ante tal situación emplearemos el algoritmo de Denavit-Hartenberg,
mediante el cual obtendremos matrices de transformación homogénea para cada
grado de libertad. Cada matriz homogénea tendrá la información de la posición,
orientación, perspectiva y escala de sus ejes coordenados correspondientes o
frames, respecto a ejes anteriores o de referencia.
La estructura de la matriz de transformación homogénea se presenta a

continuación:
2
Figura N°3.- Estructura de una matriz homogénea
Algoritmo Denavit - Hartenberg
Este algoritmo realiza una secuencia de traslaciones y rotaciones del frame

mundial a lo largo de los eslabones del robot, por tal motivo el algoritrmo tiene
por objetivo encontrar una tabla por eslabón que describa los cambios en
rotación y traslación de los ejes “X” y “Z”. Entre esos cambios se consignaran los
valores de articulaciones (REVOLUTAS O PRISMATICAS) que posea el robot.
Para obtener la tabla con la información de las articulaciones de nuestro robot,

utilizaremos una secuencia de pasos que se detallan seguidamente:
1. Debemos enumerar los eslabones, comenzando con 1 (primer eslabón

móvil de la cadena) y acabando con 4 (el último eslabón móvil).
Consideraremos el eslabón 0 a la base fija del robot.
Figura N°4.- Numeración de eslabones
2. El siguiente paso es enumerar cada articulación
3
Figura N°5.- Numeración de articulaciones
3. Localizamos los ejes de cada rotación. Si se trata de una articulación

rotativa, el eje será su propio eje de giro; si es prismática, será a lo largo
del eje del cual se produce el desplazamiento. A su vez indicaremos sus
dimensiones:
Figura N°6.- Sistema de coordenadas del robot RPPR
Los pasos anteriores permitirán hallar las matrices homogéneas que son
necesarias para hallar las relaciones matemáticas entre los sistemas
coordenados. Por tanto un sistema de coordenadas (Si) definido según el
4
algoritmo D-H se relaciona con el siguiente sistema de coordenadas mediante

cuatro parámetros: θi (rotación con respecto al eje z), di (traslación con
respecto al eje z), αi (traslación con respecto al eje x), ai (rotación con
respecto al eje x), los cuales serán indicados mediante la siguiente tabla:
θi di αi ai
1 q1 l1 0 0
2 0 q2 -a2 -pi/2
3 0 q3 0 0
4 q4 l4 0 0
Figura N°7.- Denavit – Hartenberg
Estos parámetros D-H nos servirán para hallar las matrices de transformaciones
homogéneas “para cada eslabón respecto al frame móvil anterior”, definidas
según la siguiente matriz:
Figura N°8.- Matriz Homogénea Denavit – Hartenberg
Es importante recalcar que esta matriz ha sido resultado de las transformaciones

de derecha a izquierda de cada matriz de transformación de coordenadas, esta
secuencia es la siguiente:
( ) ( ) ( ) ( )
Una vez realizado el análisis anterior emplearemos el software Matlab para

obtener las matrices de transformación homogénea de cada eslabón del robot.
Describiremos la programación realizada:
5
a) Antes de comenzar nuestro programa debemos entonces crear una función que
realice el algoritmo D-H y que luego será utilizada en nuestro programa
principal.
Esta función debe contener los parámetros: θi (giro con respecto al eje ), di
(traslación con respecto al eje ), αi (traslación con respecto al eje ), ai
(rotación con respecto al eje ), y su resultado debe ser devuelto en una matriz.
% Función Denavit-Hartenberg
function dh=denavit(a,alpha,d,theta)
dh=[cos(theta) -cos(alpha)*sin(theta) sin(alpha)*sin(theta) a*cos(theta)

sin(theta) cos(alpha)*cos(theta) -sin(alpha)*cos(theta) a*sin(theta)
0 sin(alpha) cos(alpha) d
0 0 0 1];
b) En primer lugar declararemos los parámetros físicos del robot, las coordenadas
articulares y parámetros Denavit-Hartenberg
clear all; close all; clc

% CINEMATICA DIRECTA Robot RPPR
a2=0.1; l4=0.1; l1=0.2; g1=0.05; g2=0.05; g3=0.05; g4=0.05;
% Coordenadas Articulares Iniciales
q = [0 0.1 0.2 pi/4];
%------------------------------------------------------------------------
q1 = q(1); % Revoluta
q2 = q(2); % Prismática
q3 = q(3); % Prismática
q4 = q(4); % Revoluta
%------------------------------------------------------------------------
% Parámetros DH del robot
% % Parámetros Denavit-Hartenberg del robot
% theta = [q1 0 0 q4 ];
% d = [l1 q2 q3 l4];
% a = [0 -a2 0 0 ];
% alpha = [0 -pi/2 0 0 ];
%------------------------------------------------------------------------
% Parámetros Denavit-Hartenberg del robot
% a alpha d theta
PD=[ 0 0 l1 q1
-a2 -pi/2 q2 0
0 0 q3 0
0 0 l4 q4
-g1 0 0 0
0 0 g2 0
g3 0 0 0
0 0 g4 0];
%------------------------------------------------------------------------
De lo anterior debemos observar que en la etapa de los parámetros Denavit-

Hartenberg existen cuatro líneas auxiliares. El motivo de la creación de éstas se
6
debe a que el efector final únicamente es un punto, para fines de simulación y

correcta visualización se ha creado un Gripper que sólo contemplara rotación
respecto al efector final:
Figura N° 9.- Efector final Gripper
Este gripper posee las dimensiones mencionadas en la figura anterior. Para su

creación se ha empleado el concepto de Frames Fantasmas (Ghost Frames). La
razón por la cual se emplean estos tipos de sistemas de referencia fantasma se
debe básicamente a que para pasar de un sistema de referencia al de la siguiente
articulación, en algunos casos (dependiendo del tipo de robot) no podemos
hacerlo de manera directa. Entonces necesitamos generar sistemas de referencia
fantasmas (no forman parte directa del robot) para mediante el uso del algoritmo
Denavit-Hartenberg, poder llegar a la siguiente articulación del robot.
En esta ocasión no se está empleando este concepto en su totalidad, sino con

fines ilustrativos. Sucede que si nos planteamos el movimiento del cuarto
eslabón notaremos que realiza un movimiento rotacional alrededor de su eje z, el
cual no podrá ser visualizado de manera clara mediante un simulador. Por tal
razón elaborar un efector final mediante frames fantasmas o adicionales
permitirá visualizar el movimiento producto de la articulación q4. Ya que el
gripper no incrementará los grados de libertad del robot pero si nos permitirá
visualizar la evolución de la ultima juntura.
Básicamente podemos describir el comportamiento de los frames fantasmas

mediante la figura N° 10 que mostramos a continuación:
7
Figura N°10.- Frames fantasmas S5, S6, S7, S8
Claramente podemos encontrar este análisis mediante Denavit – Hartenberg en la

programación realizada en Matlab:
c) El siguiente paso es conocer las matrices de transformación homogénea para

cada articulación
% Matrices de transformación homogénea para cada artiuclación

A01 = denavit(PD(1,1),PD(1,2),PD(1,3),PD(1,4));% eslabon 1 movil
8

A34 = denavit(PD(4,1),PD(4,2),PD(4,3),PD(4,4));% efector final
Ag1 = denavit(PD(5,1),PD(5,2),PD(5,3),PD(5,4));
d) Una vez declaradas las matrices que describen la posición y orientación de los
frames de los eslabones del robot, pasaremos a postmultiplicar cada notación
para obtener el frame actual respecto del anterior (tener en cuenta que el anterior
esta en base al frame solidario). Luego en el caso del efector final se tendrá su
posición y orientación respecto al frame solidario OXYZ.
S0=eye(4); %frame fijo (matriz identidad 4x4)

S1=A01*S0; % posición y orientación frame 1 (respect. al fijo xyz)
S2=A01*A12; % posición y orientación frame 2 (respct. al movil 1)
S3=A01*A12*A23; % posición y orientación frame 3 (respect. al movil 2)
S4=A01*A12*A23*A34; % posición y orientación frame 4(efector final)
S5=S4*Ag1; % PARA EL GRIPPER
e) Luego extraeremos los datos de posición de cada eslabón para el ploteo del
robot.
%Extrayendo datos de eslabones para ploteo

P0=[S0(1,4) S0(2,4) S0(3,4)];
P1=[S1(1,4) S1(2,4) S1(3,4)];
P2=[S2(1,4) S2(2,4) S2(3,4)];
P3=[S3(1,4) S3(2,4) S3(3,4)];
P4=[S4(1,4) S4(2,4) S4(3,4)];
P5=[S5(1,4) S5(2,4) S5(3,4)];
P6=[S6(1,4) S6(2,4) S6(3,4)];
P7=[S7(1,4) S7(2,4) S7(3,4)];
P8=[S8(1,4) S8(2,4) S8(3,4)];
f) Finalmente realizaremos los ploteos de los sistemas de referencia de cada

articulación (incluyendo los frames fantasmas) y un prototipo del robot de 4DOF
del tipo RPPR que está siendo sujeto a análisis.
%-----------------------------ploteos------------------------------------
figure
plot3([P0(1) P1(1)],[P0(2) P1(2)],[P0(3) P1(3)],'c','LineWidth',4)
hold
plot3([P1(1) P2(1)],[P1(2) P2(2)],[P1(3) P2(3)],'r','LineWidth',4)
plot3([P2(1) P3(1)],[P2(2) P3(2)],[P2(3) P3(3)],'g','LineWidth',4)
plot3([P3(1) P4(1)],[P3(2) P4(2)],[P3(3) P4(3)],'m','Linewidth',4)
plot3([P4(1) P5(1)],[P4(2) P5(2)],[P4(3) P5(3)],'b','Linewidth',4)
hold
frame(S0,'b',0.05),hold,frame(S1,'b',0.05),hold,frame(S2,'b',0.05)
9
hold,frame(S3,'b',0.05),hold,frame(S4,'b',0.05)
axis([-0.5 0.5 -0.5 0.5 0 0.5])
rotate3d,grid,view(149,54)
g) Por último mostraremos en pantalla los resultados obtenidos
%----------------------------resultados----------------------------------
disp('-----------------------------------------------------------------')
disp('------------------MATRIZ DE TH (ESLABON 4)-----------------------')
disp(S4)
disp('-----------------------------------------------------------------')
disp('------------------MATRIZ DE TH (ESLABON 3)-----------------------')
disp(S3)
disp('-----------------------------------------------------------------')
disp('-------------------MATRIZ DE TH (ESLABON 2)----------------------')
disp(S2)
disp('-----------------------------------------------------------------')
disp('-------------------MATRIZ DE TH (ESLABON 1)----------------------')
disp(S1)
disp('-----------------------------------------------------------------')
disp('---------------------MATRIZ DE TH (Fijo)-------------------------')
disp(S0)
disp('-----------------------------------------------------------------')
Para conocer los resultados ejecutamos el programa y obtenemos las siguientes

matrices de transformación homogéneas:
10
Figura N°11.- Matrices d Transformación Homogénea del Robot
RPPR de 4 grados de libertad
El gráfico obtenido a partir de los comandos de ploteo es el siguiente:
11
0.5
0.45
0.4
0.35 Z
X
0.3 Y
Z X Z
YZ
Eje Z
0.25 Y
X
0.2 X Y
0.15
0.1
Z
0.05
-0.5
0 X Y -0.4
-0.5 -0.3
-0.2
-0.1
0
0 0.1
0.2
0.3
0.4
0.5 0.5 Eje X
Figura N°12.- Simulación del Robot RPPR de 4 grados de libertad
Eje Y
Claramente podemos observar en la figura anterior la estructura del robot, los

sistemas de referencia por cada articulación incluyendo el Frame de la base fija
del robo. Debemos mencionar que el efector se encuentra con esa posición final y
orientación debido a las coordenadas articulares iniciales dadas al robot:

q = [0 0.1 0.2 pi/4];
Así, nuestro robot posee un revoluta que no ha sido girada con respecto a su eje z
(color celeste), un primer prisma cuya longitud inicial es de 0.1 unidades (color
rojo), un segundo prisma cuya longitud inicial es de 0.2 unidades (color verde) y
una revoluta (color rosado) unida a un efector (color azul) que han sido rotadas
45° (pi/4).
Comprobación con el Toolbox “Corke” de Matlab
12
Primero insertaremos la tabla D-H para este robot siguiendo el formato del
toolbox Corke para cada eslabón o “link”, además de crear el objeto con titulo y
con parámetros D-H ya declarados:
Toolbox Corke con FKINE

%----------------------------------------------
% alpha a theta d sigma offset
%-----------------------------------------------
L{1}=link([0 0 q1 l1 0 0]);
L{2}=link([-pi/2 -a2 0 q2 1 0]);
L{3}=link([ 0 0 0 q3 1 0]);
L{4}=link([ 0 0 q4 l4 0 0]);
R4=robot(L,'R4','LD954','Robot 4DOF (RPPR)');
R4.name='\bf Robot RPPR';
R4.manuf='LD954';
Luego declararemos valores de color para las líneas, sombra y ajustes de escala
para el ploteo:
R4.plotopt={'workspace',[-0.5 0.5 -0.5 0.5 -1 0.5]};

R4.lineopt={'color','blue','LineWidth',4};
R4.shadowopt={'color','black','LineWidth',2};
Ahora escribiremos en pantalla la función FKINE que nos la posición y

orientación del efector final del robot configurado en el toolbox (R4) relativo a la
matriz de articulaciones declaradas al inicio del código. Esta matriz FKINE
deberá comprobarse con la matriz hallada en el código puro anterior “S4”:
T=fkine(R4,q);
drivebot(R4,q);
disp('--------------------------------------------')
disp('Matriz de Transformacion Homogenea FKINE')
disp(T)
disp('--------------------------------------------')
view(149,54)
13
0.5
0
Z
Robot RPPR
-0.5 y
x z
-1
-0.5
-0.5
0
Y 0.5 0.5 X
Figura N°13.- Simulación del robot RPPR con el toolbox de Corke – Matlab
II.- Cinemática Inversa
Continuando con el análisis del robot, ahora abordaremos la parte de la

cinemática inversa. Básicamente podemos hallarle dos utilidades:
 Permite verificar si las variables correspondientes a los grados de libertad

ingresados son las correctas de acuerdo a la posición del efector final
resultante.
 Para poder ingresar alguna posición final en la cual se colocará el efector

del robot. Debemos indicar al respecto que esta aplicación tiene lugar en
robots que no poseen gran número de revolutas.
Para llevar a cabo la cinemática inversa realizamos un análisis geométrico:
a) Planteamos una vista superior del robot. Para estos fines rotamos la primera
revoluta 45° y la cuarta revoluta 60°. Los primas siguen teniendo las mismas
longitudes:
14
Figura N°14.- Análisis Geométrico del Robot 4DOF – RPPR (Plano X0, Y0)
b) Del gráfico anterior podemos deducir las siguientes ecuaciones:
Primera Articulación:
√( ) ( )
√( ) ( )
( ) ( )
( ) ( )
( ) ( )
15
Segunda Articulación:
Tercera Articulación:
Cuarta Articulación:
Para llevar a cabo este análisis tomamos otra vista del robot. La vista que
tendremos en la figura 14 será la del efector y la comparación entre los frames 3 y
4. En este caso aparecerá la última juntura q4, cuyo valor lo encontraremos
vectorialmente:
Figura N°15.- Análisis Geométrico del Robot 4DOF – RPPR (Plano X3, Y3)
Sabemos que el producto escalar de dos vectores se define como:
⃗ ⃗⃗ | || |
16
Entonces utilizaremos esta herramienta matemática para describir los siguientes

vectores:
⃗⃗⃗⃗⃗ ⃗⃗⃗⃗⃗ | || | ( ) ( )
⃗⃗⃗⃗⃗ ⃗⃗⃗⃗⃗ | || | ( ) ( )
De aquí nos damos cuenta que es posible obtener el valor de la juntura q4

fácilmente. Pero resulta importante detallar que los vectores ⃗⃗⃗⃗ ⃗⃗⃗⃗ ⃗⃗⃗⃗ son
parte de la matriz de transformación homogénea hallada anteriormente para cada
eslabón:
⃗⃗⃗⃗⃗ ⃗⃗⃗⃗⃗ ⃗⃗⃗⃗⃗
[ ]
Por lo tanto de nuestro código fuente obtendremos:
⃗⃗⃗⃗⃗ ( )
⃗⃗⃗⃗⃗ ( )
⃗⃗⃗⃗⃗ ( )
⃗⃗⃗⃗⃗ ( )
Para finalmente:
( ) ( ) ( )
( ) ( ) ( )
( ( ) ( ))
c) Una vez obtenida la cinemática inversa empleamos el software Matlab para

simular y verificar resultados. Entonces a la programación desarrollada para la
cinemática directa le adicionamos lo siguiente:
En un nuevo editor crear la función cinversa que implementará las ecuaciones

halladas de forma geométrica explicada en los ítems a y b y que permitirá
verificar los valores de las coordenadas articulares iniciales.
17
function qi = cinversa1(T,q1,q2,q3,q4,l1,l4,a2)
%------------------------------------------------------------------------
%DEVUELVE EL VERCTOR DE CORRDENADAS ARTICULARES QUE CORRESPONDE
%------------------------------------------------------------------------
%Posición del Efector Final
%------------------------------------------------------------------------
p=T(1:3,4); %Px = p(1), Py = p(2), Pz = p(3)
%------------------------------------------------------------------------
%Parametros DH
%------------------------------------------------------------------------
teta= [q1 0 0 q4];
d=[l1 q2 q3 l4];
a=[0 -a2 0 0];
alpha=[0 -pi/2 0 0];
%------------------------------------------------------------------------
% a alpha d theta
%------------------------------------------------------------------------
PD=[ 0 0 l1 q1
-a2 -pi/2 q2 0
0 0 q3 0
0 0 l4 q4];
%------------------------------------------------------------------------
%Articulación 1
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);
sbeta=-a(2)/R;
cbeta=r/R;
beta=atan2(sbeta,cbeta);
q1i=phi-beta;
%------------------------------------------------------------------------
%Articulacion 2
q2i=p(3)-d(1); %Pz-L1
%------------------------------------------------------------------------
%Articulacion 3
q3i=r-d(4); %r-L4
%------------------------------------------------------------------------
%Articulacion 4
A01 = denavit(PD(1,1),PD(1,2),PD(1,3),q1i);
A12 = denavit(PD(2,1),PD(2,2),PD(2,3),PD(2,4));
A23 = denavit(PD(3,1),PD(3,2),q2i,PD(3,4));
A34 = denavit(PD(4,1),PD(4,2),q3i,PD(4,4));
A03=A01*A12*A23;
y3 =A03(1:3,2);
sq4=dot(T(1:3,1),y3); %Definimos el producto escalar x4.y3
cq4=dot(T(1:3,2),y3); %Definimos el producto escalar y4.y3
q4i=atan2(sq4,cq4);
%------------------------------------------------------------------------
%Declaramos la matriz final qi
qi=[q1i*180/pi q2i q3i q4i*180/pi];
18
Recordar que debemos cambiar las coordenadas articulares iniciales en la

cinemática directa, con la finalidad de verificar junturas revolutas y prismáticas
del robot:
q = [pi/4 0.1 0.2 pi/3];
En el programa principal incluimos lo siguiente:
%------------------------------------------------------------------------
%Coordenadas insertadas al inicio
q = [(q1*180/pi) q2 q3 (q4*180/pi)];
disp('--------------Coordenadas Iniciales---------------')
disp(q);
%------------------------------------------------------------------------
%Cinematica Inversa Robot RPR
qi = cinversa1(S4,q1,q2,q3,q4,l1,l4,a2);
disp('--------------Coordenadas Finales-----------------')
disp(qi);
%------------------------------------------------------------------------
Para conocer los resultados ejecutamos el programa:
Finalmente la posición robot se observa gráficamente:
0.4
0.3
Eje Z
0.2 Z
XY
XY Z Y Z
0.1 Z X
0 X Y
-0.5 Z
X Y
-0.5
0
Eje Y 0.5 0.5 Eje X
Figura N°16.- Simulación del Robot RPPR de 4 grados de libertad
19
III.- Implementación de la cinemática directa e inversa en LABVIEW CODIGO

PURO.
Teniendo en cuenta lo trabajado en el código Matlab para la cinemática directa e

inversa de este robot, nos disponemos a pasar cada línea de código al software
Labview en programación grafica y utilizando SubVI’s por cuestión de orden y
síntesis para el código.
Figura N°17.- Panel Frontal
Contenido por SubVi
Para implementar la cinemática directa e inversa del robot en Labview se tendrán

en cuenta los siguientes SubVI’s que han sido creados:
 SubVI D-H.
 Tratamiento de parámetros D-H y generación de matrices TH para los
eslabones.
 Extracción de coordenadas para el ploteo.
 Cinemática inversa (Para comprobación).
 Cinemática inversa (Para insertar los datos de la posición del efector final).
Como se observa nuestro panel frontal presenta dos modos de operación en un

menú ring cuyo menú se describe a continuación:
20
a) El primero, recibe datos en “TEACH PENDANT” de las articulaciones

individualmente, esos valores se procesarán de acuerdo al diagrama de bloques
del caso 0 que se detallará más adelante, del cual se obtendrán las posiciones
del efector final a través de las matrices de transformación homogénea que
también serán calculadas para finalmente extraer los datos de posición del
efector final y eslabones para graficarlos con la herramienta CURVE 3D y así
visualizar de forma interactiva el movimiento de nuestro robot. Esta opción
permite comprobar los datos ingresados en las articulaciones a través de un
SubVi con la cinemática inversa.
Figura N°18.- Resumen panel frontal
b) La segunda opción la hemos llamado “Modo Coordenada P(x,y,z)”, aquí el

código habilita un menú como el siguiente:
Mediante esta opción el código procesará un punto insertado cualesquiera para el

efector final, es decir podrá dirigir al robot hacia un punto. Con este dato el
código calculará los valores de las junturas mediante un Vi de cinemática
inversa. Éste SubVi arrojará un arreglo con las junturas calculadas para dicha
acción, luego las ingresará a la cinemática directa que generar las matrices para
cada eslabón y posicionará al efector final del robot en el punto insertado.
21
Con estas pautas generales se presentará los diagramas de bloques para el código
en general para cada caso expuesto:
Caso 0:
Caso 1:
22
1.- SubVI DENAVIT - HARTENBERG
El SubVI tiene la siguiente estructura en el panel frontal:
Figura N°19.- Panel frontal SubVi D-H
Luego en el panel de bloques programaremos la estructura de la matriz TH de

Denavit - Hartenberg de la figura 8:
Matriz D-H
Coeficientes de
la matriz D-H
Figura N° 20.- Panel de Bloques SubVi D-H.
Usaremos esta notación para asignar las matrices D-H respectivas para cada
eslabón.
23
2.- Tratamiento de parámetros D-H y generación de matrices TH para los

eslabones.
En este SubVi se toman en cuenta las siguientes las entradas: parámetros D-H
fijos, articulaciones insertadas (en modo Teach Pendant o Modo coordenada).
Estos valores servirán para obtener en la salida del SubVI las matrices de TH de
cada eslabón móvil (S1, S2, S3, S4) respecto al mundial, además de los
eslabones fijos para el gripper (S5, S6, S7 y S8) y el eslabón solidario que refiere
al frame solidario “S0”. La tabla de parámetros D-H general también es
construida y mostrada en la salida.
Figura N° 21.- Panel Frontal del SubVI “DH+Si”
Para lograr lo explicado anteriormente, tenemos el diagrama de bloques que se

observa en la Figura 22. Aquí sectorizamos el código en dos estructuras Flat
Sequence:
a) La primera contiene un código que procesa los números escalares L1, L4, a2 y
las articulaciones q1, q2 , q3, q4 para ser ingresados a los arreglos d, theta, a y
alpha que representan a los parámetros D-H del robot, usando la herramienta
“Replace Array Subset”, la cual nos permite reemplazar el valor en una posición
de un arreglo o índice por otro valor, de esta forma construiré la tabla D-H
general con un “Reshape Array” para convertir el arreglo fila en una columna de
4 elementos luego a un “Build Array” para apilar todas las columnas y generar la
tabla solo aplicando transpuesta a todo el arreglo.
A su vez se observa que los valores completos de la tabla general D-H son
extraídos uno a uno por cada fila (es decir por eslabón) para ingresar a la
siguiente parte de la estructura.
24
Tabla D-H
Matrices de TH por
eslabón móvil
Figura N° 22.- Panel de bloques
b) Aquí se procesarán estos valores por eslabón con el propósito de encontrar las
matrices de D-H (A01, A12, A23 y A34) de cada uno utilizando el SubVI anterior
de D-H. Luego pasamos a calcular las matrices de transformación homogénea
para cada eslabón, pero estas matrices describirán su posición y orientación
respecto al frame mundial (S1, S2, S3, S4), para ello hacemos producto matricial
siguiendo el algoritmo ya explicado. Finalmente obtendremos en el último
producto la matriz del efector final “S4”, la cual es importante para el código,
pues con esto hemos logrado pasar el algoritmo aprendido en clase y comprobado
antes en Matlab, a Labview con éxito.
c) Como parte extra hemos hecho un VI para el gripper que contiene el cálculo de
los D-H extras que se han agregado para su generación, ver figura 23. Se observa
que la multiplicación de las matrices es estratégica, esto debido a que necesito
que el gripper refleje el movimiento de la última articulación q4, por tanto las
matrices S5, S6, S7 y S8 deben estar respecto al frame del efector final S4.
25
Con estos resultados podemos pasar a plotear la cinemática directa del robot
modelado 4DOF RPPR con ayuda del algoritmo D-H desarrollado hasta aquí.
Figura N° 23.- Diagrama de bloques para el SubVI para el gripper
3.- Extracción de coordenadas para el ploteo
En este SubVI se llevara a cabo la extracción de las posiciones x, y, z de cada

eslabón del robot y gripper a graficar, para ello haremos uso de las matrices de
TH calculadas del algoritmo D-H finales del SubVi anterior. En este proceso
usaremos las propiedades que nos da Labview a través del manejo de arreglos
para lograr agrupar los vectores x, y, z y entregarlos al Toolkit de ploteo CURVE
3D de Labview, el cual requiere en arreglo de 1D los datos para x, y, z.
Figura N° 24.- Panel Frontal
26
a) La primera parte de la estructura Flat Sequence Structure usada extraemos

los datos de traslación de las matrices TH Si (i=0:8), los cuales representan a las
filas 0, 1 y 2 de la columna 3 de cada matriz, a éstos los que hemos llamado Pi
(i=0:8). Para esto usamos los “Index Array” de Labview.
b) En la segunda parte del Flat procesaremos los datos de los Pi(x,y,z) para
graficarlos, para ello apilaremos los datos de las coordenadas “x” en un arreglo
1D llamado Px, lo mismo para Py y Pz. Tener en cuenta que para el gripper se
han agrupado los datos de forma tal que se pueda visualizar sin problemas los
cambios en el grafico 3D final, esto debido a que le ploteador CURVE 3D de
Labview une los puntos en secuencia y no independientemente, por lo que se
tendrá que repetir un punto en el gripper (P5) para obtener los resultados que se
pretenden. No existe alteración en los cálculos pues solo es con fines gráficos.
Figura N° 25.- Código fuente para extracción de datos para el ploteo
27
4.- Cinemática Inversa (para comprobación)
Este SubVI fue creado en base al código de la función cinversa1 del código Matlab
expuesto y explicado anteriormente. El programa recibe los parámetros fijos D-H
junto con la matriz de transformación homogénea del efector final “S4” como
entradas, y las salidas serán las cuatro articulaciones q1, q2, q3 y q4 que serán
comprobadas con referencia a la cinemática directa. Cabe resaltar que esta SubVi
se encuentra en el modo de operación “Teach Pendant” del código principal.
Figura N° 26.- Panel Frontal del SubVI cinversa1
Cada parte de la estructura Flat Sequence usada corresponde al cálculo de cada

articulación según el método geométrico planteado anteriormente en análisis
para este manipulador.
a) En el primer sector se extraen los datos de la posición del efector final además
de los vectores de orientación en “x4” y “y4” que nos darán información para el
cálculo de la articulación q4 posteriormente en este código.
b) El segundo y tercer sector corresponden al cálculo geométrico de las

articulaciones q1 (revoluta), q2 (prismática) y q3 (prismática) según el análisis
geométrico. Para el caso de las junturas revolutas se ha utilizado la herramienta
matemática “atan2” de Labview la cual nos da un rango más amplio para el
argumento a calcular ( ).
c) En el cuarto sector hace un cálculo rápido de las matrices necesarias para

encontrar la matriz del eslabón 3 (A03), con el objetivo de extraer los datos de
orientación del vector y3 para ser operado, mediante el producto escalar de
vectores, con el vector de orientación x4 y y4 extraídos antes y obtener así la
juntura q4.
28
Figura N° 27.- Bloques según método geométrico para la cinemática Inversa

(comprobación)
5.- Cinemática Inversa (insertar posición deseada para el efector final)
Este programa para el modo de operación “Coordenada P(x,y,z)” sigue la línea de

cálculo del SubVI anterior con las siguientes diferencias:
a) Este código recibe un punto deseado P(x,y,z) para el efector final del robot.
b) Se realiza el cálculo rápido de las matrices de los eslabones adicionando la del

efector final A04 con el objetivo de extraer los datos de orientación para los
vectores x4 y y4 que luego se utilizarán para el cálculo de q4.
Figura N° 28.- Panel Frontal
29
x4
A04
y4
P(x,y,z)
Figura N° 29.- Panel de bloques con señalizaciones para este SubVI
IV.- Conclusiones
1. La cinemática directa y el algoritmo DENAVIT – HARTENBERG resultan una

pieza importante para el Modelamiento de un robot, pues este algoritmo alberga
todas las condiciones para la generación del robot en el espacio, además de
entregar información de posición y orientación respecto a un sistema de
referencia.
2. La información de posición y orientación de las matrices de transformación

homogénea resulta vital para el desarrollo del modelamiento de un manipulador
robótico. Ellas nos informan si el sistema en análisis rotó o se trasladó respecto a
un eje de referencia.
3. La adición de los frames fantasmas resulta con el único fin de describir

rotaciones o traslaciones extras que si bien es cierto, no contienen articulación,
nos ayudan a tener detalle acerca de la estructura del robot a analizar para casos
particulares. Es importante mencionar que no hay alteración en los grados de
libertad al utilizar este criterio.
4. Las características de este manipulador nos permitió interactuar con dos

modos de operación para visualizar el comportamiento de la cinemática inversa.
Es importante decir que la cinemática inversa es netamente una herramienta de
comprobación de articulaciones.
5. El cálculo de la cuarta articulación para el modo de operación “Coordenada

P(x,y,z)” resulta innecesario pues el efector llegará hacia el punto deseado
rotando o no rotando el efector final, por ende en el código de la cinemática
inversa , para este caso, se le ha colocado el valor de cero a esta variable q4.
30
6. Al calcular un valor de juntura revoluta entre dos eslabones móviles para la

cinemática inversa, resulta conveniente y seguro hacerlo vectorialmente con las
orientaciones que nos dan las matrices de TH cada vez que se produce la
rotación.
7. Una vez más se comprobó la efectividad del software Labview para comprender
y comprobar aspectos de los algoritmos matemáticos para esta experiencia.
31

Cinematica Directa e Inversa de Un Robot de 4 Grados de Libertad PDF

Cargado por

Información del documento

Título original

Derechos de autor

Formatos disponibles

Compartir este documento

Compartir o incrustar documentos

Opciones para compartir

¿Le pareció útil este documento?

¿Este contenido es inapropiado?

Copyright:

Formatos disponibles

Cinematica Directa e Inversa de Un Robot de 4 Grados de Libertad PDF

Cargado por

Copyright:

Formatos disponibles

2012-B ROBÓTICA – CINEMÁTICA DEL ROBOT 4DOF - RPPR

“CINEMATICA DIRECTA E INVERSA DE UN ROBOT

Cuya Solari, Omar Antonio

Flores Bustinza, Edwing Irwing

Torres Chavez, Jonathan Emmanuel

En el presente informe se desarrolla el análisis de la cinemática directa e inversa

Figura N°1.- Robot 4DOF - RPPR

El proceso del desarrollo de la cinemática del robot se basa en el problema de

Figura N°2.- Cinemática del Robot

I.- Cinemática Directa

Para comenzar el análisis, abordaremos el tema de la cinemática directa. Primero

La estructura de la matriz de transformación homogénea se presenta a

Figura N°3.- Estructura de una matriz homogénea

Algoritmo Denavit - Hartenberg

Este algoritmo realiza una secuencia de traslaciones y rotaciones del frame

Para obtener la tabla con la información de las articulaciones de nuestro robot,

1. Debemos enumerar los eslabones, comenzando con 1 (primer eslabón

Figura N°4.- Numeración de eslabones

2. El siguiente paso es enumerar cada articulación

Figura N°5.- Numeración de articulaciones

3. Localizamos los ejes de cada rotación. Si se trata de una articulación

Figura N°6.- Sistema de coordenadas del robot RPPR

algoritmo D-H se relaciona con el siguiente sistema de coordenadas mediante

Figura N°8.- Matriz Homogénea Denavit – Hartenberg

Es importante recalcar que esta matriz ha sido resultado de las transformaciones

Una vez realizado el análisis anterior emplearemos el software Matlab para

Describiremos la programación realizada:

dh=[cos(theta) -cos(alpha)*sin(theta) sin(alpha)*sin(theta) a*cos(theta)

clear all; close all; clc

De lo anterior debemos observar que en la etapa de los parámetros Denavit-

debe a que el efector final únicamente es un punto, para fines de simulación y

Figura N° 9.- Efector final Gripper

Este gripper posee las dimensiones mencionadas en la figura anterior. Para su

En esta ocasión no se está empleando este concepto en su totalidad, sino con

Básicamente podemos describir el comportamiento de los frames fantasmas

Figura N°10.- Frames fantasmas S5, S6, S7, S8

Claramente podemos encontrar este análisis mediante Denavit – Hartenberg en la

c) El siguiente paso es conocer las matrices de transformación homogénea para

% Matrices de transformación homogénea para cada artiuclación

A12 = denavit(PD(2,1),PD(2,2),PD(2,3),PD(2,4));% eslabon 2 movil

S0=eye(4); %frame fijo (matriz identidad 4x4)

%Extrayendo datos de eslabones para ploteo

f) Finalmente realizaremos los ploteos de los sistemas de referencia de cada

g) Por último mostraremos en pantalla los resultados obtenidos

Para conocer los resultados ejecutamos el programa y obtenemos las siguientes

Figura N°11.- Matrices d Transformación Homogénea del Robot

RPPR de 4 grados de libertad

El gráfico obtenido a partir de los comandos de ploteo es el siguiente:

Claramente podemos observar en la figura anterior la estructura del robot, los

% Coordenadas Articulares Iniciales

Comprobación con el Toolbox “Corke” de Matlab

Toolbox Corke con FKINE

R4.plotopt={'workspace',[-0.5 0.5 -0.5 0.5 -1 0.5]};

Ahora escribiremos en pantalla la función FKINE que nos la posición y

II.- Cinemática Inversa

Continuando con el análisis del robot, ahora abordaremos la parte de la

 Permite verificar si las variables correspondientes a los grados de libertad

 Para poder ingresar alguna posición final en la cual se colocará el efector

Para llevar a cabo la cinemática inversa realizamos un análisis geométrico:

b) Del gráfico anterior podemos deducir las siguientes ecuaciones:

Sabemos que el producto escalar de dos vectores se define como:

Entonces utilizaremos esta herramienta matemática para describir los siguientes

De aquí nos damos cuenta que es posible obtener el valor de la juntura q4

dh=[cos(theta) -cos(alpha)sin(theta) sin(alpha)sin(theta) a*cos(theta)