cinematica directa e inversa de un robot de 4 grados de libertad
TRANSCRIPT
-
2012-B ROBTICA CINEMTICA DEL ROBOT 4DOF - RPPR
1
CINEMATICA DIRECTA E INVERSA DE UN ROBOT
4DOF RPPR
Cuya Solari, Omar Antonio
Flores Bustinza, Edwing Irwing
Torres Chavez, Jonathan Emmanuel
-
2012-B ROBTICA CINEMTICA DEL ROBOT 4DOF - RPPR
2
En el presente informe se desarrolla el anlisis de la cinemtica directa e inversa
de un Robot de cuatro grados de libertad del tipo Revoluta Prisma Prisma
Revoluta que podemos observar en la figura N1.
Figura N1.- Robot 4DOF - RPPR
El proceso del desarrollo de la cinemtica del robot se basa en el problema de
hallar la posicin y orientacin del efector final del robot (ltimo eslabn) para los
valores de las coordenadas articulares del robot (cinemtica directa) y viceversa
(cinemtica inversa).
Figura N2.- Cinemtica del Robot
I.- Cinemtica Directa
Para comenzar el anlisis, abordaremos el tema de la cinemtica directa. Primero
debemos establecer los frames o ejes de referencia para cada juntura presente en
el robot. Ante tal situacin emplearemos el algoritmo de Denavit-Hartenberg,
mediante el cual obtendremos matrices de transformacin homognea para cada
grado de libertad. Cada matriz homognea tendr la informacin de la posicin,
orientacin, perspectiva y escala de sus ejes coordenados correspondientes o
frames, respecto a ejes anteriores o de referencia.
La estructura de la matriz de transformacin homognea se presenta a
continuacin:
Posicin y orientacin del efector final del robot
(x,y,z,,,)
Valor de las Coordenadas articulares
(q1, q2,,qn)
DIRECTA
INVERSA
-
2012-B ROBTICA CINEMTICA DEL ROBOT 4DOF - RPPR
3
Figura N3.- Estructura de una matriz homognea
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 eslabn que describa los cambios en
rotacin y traslacin 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 informacin de las articulaciones de nuestro robot,
utilizaremos una secuencia de pasos que se detallan seguidamente:
1. Debemos enumerar los eslabones, comenzando con 1 (primer eslabn
mvil de la cadena) y acabando con 4 (el ltimo eslabn mvil).
Consideraremos el eslabn 0 a la base fija del robot.
Figura N4.- Numeracin de eslabones
2. El siguiente paso es enumerar cada articulacin
-
2012-B ROBTICA CINEMTICA DEL ROBOT 4DOF - RPPR
4
Figura N5.- Numeracin de articulaciones
3. Localizamos los ejes de cada rotacin. Si se trata de una articulacin
rotativa, el eje ser su propio eje de giro; si es prismtica, ser a lo largo
del eje del cual se produce el desplazamiento. A su vez indicaremos sus
dimensiones:
Figura N6.- Sistema de coordenadas del robot RPPR
Los pasos anteriores permitirn hallar las matrices homogneas que son
necesarias para hallar las relaciones matemticas entre los sistemas
coordenados. Por tanto un sistema de coordenadas (Si) definido segn el
-
2012-B ROBTICA CINEMTICA DEL ROBOT 4DOF - RPPR
5
algoritmo D-H se relaciona con el siguiente sistema de coordenadas mediante
cuatro parmetros: i (rotacin con respecto al eje z), di (traslacin con
respecto al eje z), i (traslacin con respecto al eje x), ai (rotacin con
respecto al eje x), los cuales sern 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 N7.- Denavit Hartenberg
Estos parmetros D-H nos servirn para hallar las matrices de transformaciones
homogneas para cada eslabn respecto al frame mvil anterior, definidas
segn la siguiente matriz:
Figura N8.- Matriz Homognea Denavit Hartenberg
Es importante recalcar que esta matriz ha sido resultado de las transformaciones
de derecha a izquierda de cada matriz de transformacin de coordenadas, esta
secuencia es la siguiente:
( ) ( ) ( ) ( )
Una vez realizado el anlisis anterior emplearemos el software Matlab para
obtener las matrices de transformacin homognea de cada eslabn del robot.
Describiremos la programacin realizada:
-
2012-B ROBTICA CINEMTICA DEL ROBOT 4DOF - RPPR
6
a) Antes de comenzar nuestro programa debemos entonces crear una funcin que
realice el algoritmo D-H y que luego ser utilizada en nuestro programa
principal.
Esta funcin debe contener los parmetros: i (giro con respecto al eje ), di (traslacin con respecto al eje ), i (traslacin con respecto al eje ), ai (rotacin con respecto al eje ), y su resultado debe ser devuelto en una matriz.
% Funcin 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 parmetros fsicos del robot, las coordenadas
articulares y parmetros 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); % Prismtica q3 = q(3); % Prismtica q4 = q(4); % Revoluta
%------------------------------------------------------------------------ % Parmetros DH del robot % % Parmetros 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 ]; %------------------------------------------------------------------------ % Parmetros 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 parmetros Denavit-
Hartenberg existen cuatro lneas auxiliares. El motivo de la creacin de stas se
-
2012-B ROBTICA CINEMTICA DEL ROBOT 4DOF - RPPR
7
debe a que el efector final nicamente es un punto, para fines de simulacin y
correcta visualizacin se ha creado un Gripper que slo contemplara rotacin
respecto al efector final:
Figura N 9.- Efector final Gripper
Este gripper posee las dimensiones mencionadas en la figura anterior. Para su
creacin se ha empleado el concepto de Frames Fantasmas (Ghost Frames). La
razn por la cual se emplean estos tipos de sistemas de referencia fantasma se
debe bsicamente a que para pasar de un sistema de referencia al de la siguiente
articulacin, 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 articulacin del robot.
En esta ocasin no se est empleando este concepto en su totalidad, sino con
fines ilustrativos. Sucede que si nos planteamos el movimiento del cuarto
eslabn 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
razn elaborar un efector final mediante frames fantasmas o adicionales
permitir visualizar el movimiento producto de la articulacin q4. Ya que el
gripper no incrementar los grados de libertad del robot pero si nos permitir
visualizar la evolucin de la ultima juntura.
Bsicamente podemos describir el comportamiento de los frames fantasmas
mediante la figura N 10 que mostramos a continuacin:
-
2012-B ROBTICA CINEMTICA DEL ROBOT 4DOF - RPPR
8
Figura N10.- Frames fantasmas S5, S6, S7, S8
Claramente podemos encontrar este anlisis mediante Denavit Hartenberg en la
programacin realizada en Matlab:
c) El siguiente paso es conocer las matrices de transformacin homognea para
cada articulacin
% Matrices de transformacin homognea para cada artiuclacin A01 = denavit(PD(1,1),PD(1,2),PD(1,3),PD(1,4));% eslabon 1 movil
-
2012-B ROBTICA CINEMTICA DEL ROBOT 4DOF - RPPR
9
A12 = denavit(PD(2,1),PD(2,2),PD(2,3),PD(2,4));% eslabon 2 movil A23 = denavit(PD(3,1),PD(3,2),PD(3,3),PD(3,4));% eslabon 3 movil 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)); Ag2 = denavit(PD(6,1),PD(6,2),PD(6,3),PD(6,4)); Ag3 = denavit(PD(7,1),PD(7,2),PD(7,3),PD(7,4)); Ag4 = denavit(PD(8,1),PD(8,2),PD(8,3),PD(8,4));
d) Una vez declaradas las matrices que describen la posicin y orientacin de los frames de los eslabones del robot, pasaremos a postmultiplicar cada notacin 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 posicin y orientacin respecto al frame solidario OXYZ.
S0=eye(4); %frame fijo (matriz identidad 4x4) S1=A01*S0; % posicin y orientacin frame 1 (respect. al fijo xyz) S2=A01*A12; % posicin y orientacin frame 2 (respct. al movil 1) S3=A01*A12*A23; % posicin y orientacin frame 3 (respect. al movil 2) S4=A01*A12*A23*A34; % posicin y orientacin frame 4(efector final) S5=S4*Ag1; % PARA EL GRIPPER S6=S5*Ag2; % PARA EL GRIPPER S7=S4*Ag3; % PARA EL GRIPPER S8=S7*Ag4; % PARA EL GRIPPER
e) Luego extraeremos los datos de posicin de cada eslabn 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
articulacin (incluyendo los frames fantasmas) y un prototipo del robot de 4DOF
del tipo RPPR que est siendo sujeto a anlisis.
%-----------------------------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) plot3([P5(1) P6(1)],[P5(2) P6(2)],[P5(3) P6(3)],'b','Linewidth',4) plot3([P4(1) P7(1)],[P4(2) P7(2)],[P4(3) P7(3)],'b','Linewidth',4) plot3([P7(1) P8(1)],[P7(2) P8(2)],[P7(3) P8(3)],'b','Linewidth',4) hold frame(S0,'b',0.05),hold,frame(S1,'b',0.05),hold,frame(S2,'b',0.05)
-
2012-B ROBTICA CINEMTICA DEL ROBOT 4DOF - RPPR
10
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 transformacin homogneas:
-
2012-B ROBTICA CINEMTICA DEL ROBOT 4DOF - RPPR
11
Figura N11.- Matrices d Transformacin Homognea del Robot
RPPR de 4 grados de libertad
El grfico obtenido a partir de los comandos de ploteo es el siguiente:
-
2012-B ROBTICA CINEMTICA DEL ROBOT 4DOF - RPPR
12
Figura N12.- Simulacin del Robot RPPR de 4 grados de libertad
Claramente podemos observar en la figura anterior la estructura del robot, los
sistemas de referencia por cada articulacin incluyendo el Frame de la base fija
del robo. Debemos mencionar que el efector se encuentra con esa posicin final y
orientacin debido a las coordenadas articulares iniciales dadas al robot:
% Coordenadas Articulares Iniciales 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).
Comprobacin con el Toolbox Corke de Matlab
-0.5
-0.4
-0.3
-0.2
-0.1
0
0.1
0.2
0.3
0.4
0.5
-0.5
0
0.5
0
0.05
0.1
0.15
0.2
0.25
0.3
0.35
0.4
0.45
0.5
Eje X
ZY
X
Z
Y
X
Z
Y
Y
Y
X
Z
Z
X
X
Eje Y
Eje
Z
-
2012-B ROBTICA CINEMTICA DEL ROBOT 4DOF - RPPR
13
Primero insertaremos la tabla D-H para este robot siguiendo el formato del toolbox Corke para cada eslabn o link, adems de crear el objeto con titulo y con parmetros 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 lneas, 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 funcin FKINE que nos la posicin y orientacin del efector final del robot configurado en el toolbox (R4) relativo a la matriz de articulaciones declaradas al inicio del cdigo. Esta matriz FKINE deber comprobarse con la matriz hallada en el cdigo puro anterior S4: T=fkine(R4,q);
drivebot(R4,q); disp('--------------------------------------------') disp('Matriz de Transformacion Homogenea FKINE') disp(T) disp('--------------------------------------------') view(149,54)
-
2012-B ROBTICA CINEMTICA DEL ROBOT 4DOF - RPPR
14
Figura N13.- Simulacin del robot RPPR con el toolbox de Corke Matlab
II.- Cinemtica Inversa
Continuando con el anlisis del robot, ahora abordaremos la parte de la
cinemtica inversa. Bsicamente podemos hallarle dos utilidades:
Permite verificar si las variables correspondientes a los grados de libertad
ingresados son las correctas de acuerdo a la posicin del efector final
resultante.
Para poder ingresar alguna posicin final en la cual se colocar el efector
del robot. Debemos indicar al respecto que esta aplicacin tiene lugar en
robots que no poseen gran nmero de revolutas.
Para llevar a cabo la cinemtica inversa realizamos un anlisis geomtrico:
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:
-0.5
0
0.5
-0.5
0
0.5
-1
-0.5
0
0.5
XY
Z Robot RPPR
xyz
-
2012-B ROBTICA CINEMTICA DEL ROBOT 4DOF - RPPR
15
Figura N14.- Anlisis Geomtrico del Robot 4DOF RPPR (Plano X0, Y0)
b) Del grfico anterior podemos deducir las siguientes ecuaciones:
Primera Articulacin:
( ) ( )
( ) ( )
( )
( )
( )
( )
(
) (
)
-
2012-B ROBTICA CINEMTICA DEL ROBOT 4DOF - RPPR
16
Segunda Articulacin:
Tercera Articulacin:
Cuarta Articulacin:
Para llevar a cabo este anlisis tomamos otra vista del robot. La vista que
tendremos en la figura 14 ser la del efector y la comparacin entre los frames 3 y
4. En este caso aparecer la ltima juntura q4, cuyo valor lo encontraremos
vectorialmente:
Figura N15.- Anlisis Geomtrico del Robot 4DOF RPPR (Plano X3, Y3)
Sabemos que el producto escalar de dos vectores se define como:
| || |
-
2012-B ROBTICA CINEMTICA DEL ROBOT 4DOF - RPPR
17
Entonces utilizaremos esta herramienta matemtica para describir los siguientes
vectores:
| || | ( ) ( )
| || | ( ) ( )
De aqu nos damos cuenta que es posible obtener el valor de la juntura q4
fcilmente. Pero resulta importante detallar que los vectores son
parte de la matriz de transformacin homognea hallada anteriormente para cada
eslabn:
[
]
Por lo tanto de nuestro cdigo fuente obtendremos:
( )
( )
( )
( )
Para finalmente:
( ) ( ) ( )
( ) ( ) ( )
( ( ) ( ))
c) Una vez obtenida la cinemtica inversa empleamos el software Matlab para
simular y verificar resultados. Entonces a la programacin desarrollada para la
cinemtica directa le adicionamos lo siguiente:
En un nuevo editor crear la funcin cinversa que implementar las ecuaciones
halladas de forma geomtrica explicada en los tems a y b y que permitir
verificar los valores de las coordenadas articulares iniciales.
-
2012-B ROBTICA CINEMTICA DEL ROBOT 4DOF - RPPR
18
function qi = cinversa1(T,q1,q2,q3,q4,l1,l4,a2)
%------------------------------------------------------------------------
%DEVUELVE EL VERCTOR DE CORRDENADAS ARTICULARES QUE CORRESPONDE %------------------------------------------------------------------------
%Posicin 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]; %------------------------------------------------------------------------
%Articulacin 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];
-
2012-B ROBTICA CINEMTICA DEL ROBOT 4DOF - RPPR
19
Recordar que debemos cambiar las coordenadas articulares iniciales en la cinemtica directa, con la finalidad de verificar junturas revolutas y prismticas del robot:
% Coordenadas Articulares Iniciales 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 posicin robot se observa grficamente:
Figura N16.- Simulacin del Robot RPPR de 4 grados de libertad
-0.5
0
0.5
-0.5
0
0.5
0
0.1
0.2
0.3
0.4
ZY
X
ZYX
Eje X
Z
Y
Y
YX
Z
Z
X
X
Eje Y
Eje
Z
-
2012-B ROBTICA CINEMTICA DEL ROBOT 4DOF - RPPR
20
III.- Implementacin de la cinemtica directa e inversa en LABVIEW CODIGO
PURO.
Teniendo en cuenta lo trabajado en el cdigo Matlab para la cinemtica directa e
inversa de este robot, nos disponemos a pasar cada lnea de cdigo al software
Labview en programacin grafica y utilizando SubVIs por cuestin de orden y
sntesis para el cdigo.
Figura N17.- Panel Frontal
Contenido por SubVi
Para implementar la cinemtica directa e inversa del robot en Labview se tendrn
en cuenta los siguientes SubVIs que han sido creados:
SubVI D-H.
Tratamiento de parmetros D-H y generacin de matrices TH para los
eslabones.
Extraccin de coordenadas para el ploteo.
Cinemtica inversa (Para comprobacin).
Cinemtica inversa (Para insertar los datos de la posicin del efector final).
Como se observa nuestro panel frontal presenta dos modos de operacin en un
men ring cuyo men se describe a continuacin:
-
2012-B ROBTICA CINEMTICA DEL ROBOT 4DOF - RPPR
21
a) El primero, recibe datos en TEACH PENDANT de las articulaciones
individualmente, esos valores se procesarn de acuerdo al diagrama de bloques
del caso 0 que se detallar ms adelante, del cual se obtendrn las posiciones
del efector final a travs de las matrices de transformacin homognea que
tambin sern calculadas para finalmente extraer los datos de posicin del
efector final y eslabones para graficarlos con la herramienta CURVE 3D y as
visualizar de forma interactiva el movimiento de nuestro robot. Esta opcin
permite comprobar los datos ingresados en las articulaciones a travs de un
SubVi con la cinemtica inversa.
Figura N18.- Resumen panel frontal
b) La segunda opcin la hemos llamado Modo Coordenada P(x,y,z), aqu el
cdigo habilita un men como el siguiente:
Mediante esta opcin el cdigo procesar un punto insertado cualesquiera para el
efector final, es decir podr dirigir al robot hacia un punto. Con este dato el
cdigo calcular los valores de las junturas mediante un Vi de cinemtica
inversa. ste SubVi arrojar un arreglo con las junturas calculadas para dicha
accin, luego las ingresar a la cinemtica directa que generar las matrices para
cada eslabn y posicionar al efector final del robot en el punto insertado.
-
2012-B ROBTICA CINEMTICA DEL ROBOT 4DOF - RPPR
22
Con estas pautas generales se presentar los diagramas de bloques para el cdigo
en general para cada caso expuesto:
Caso 0:
Caso 1:
-
2012-B ROBTICA CINEMTICA DEL ROBOT 4DOF - RPPR
23
1.- SubVI DENAVIT - HARTENBERG
El SubVI tiene la siguiente estructura en el panel frontal:
Figura N19.- 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:
Figura N 20.- Panel de Bloques SubVi D-H.
Usaremos esta notacin para asignar las matrices D-H respectivas para cada
eslabn.
Coeficientes de
la matriz D-H
Matriz D-H
-
2012-B ROBTICA CINEMTICA DEL ROBOT 4DOF - RPPR
24
2.- Tratamiento de parmetros D-H y generacin de matrices TH para los
eslabones.
En este SubVi se toman en cuenta las siguientes las entradas: parmetros D-H
fijos, articulaciones insertadas (en modo Teach Pendant o Modo coordenada).
Estos valores servirn para obtener en la salida del SubVI las matrices de TH de
cada eslabn mvil (S1, S2, S3, S4) respecto al mundial, adems de los
eslabones fijos para el gripper (S5, S6, S7 y S8) y el eslabn solidario que refiere
al frame solidario S0. La tabla de parmetros D-H general tambin 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 cdigo en dos estructuras Flat
Sequence:
a) La primera contiene un cdigo que procesa los nmeros 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 parmetros D-H del robot, usando la herramienta
Replace Array Subset, la cual nos permite reemplazar el valor en una posicin
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
extrados uno a uno por cada fila (es decir por eslabn) para ingresar a la
siguiente parte de la estructura.
-
2012-B ROBTICA CINEMTICA DEL ROBOT 4DOF - RPPR
25
Figura N 22.- Panel de bloques
b) Aqu se procesarn estos valores por eslabn con el propsito 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 transformacin homognea
para cada eslabn, pero estas matrices describirn su posicin y orientacin
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 cdigo,
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 clculo de
los D-H extras que se han agregado para su generacin, ver figura 23. Se observa
que la multiplicacin de las matrices es estratgica, esto debido a que necesito
que el gripper refleje el movimiento de la ltima articulacin q4, por tanto las
matrices S5, S6, S7 y S8 deben estar respecto al frame del efector final S4.
Tabla D-H
Matrices de TH por
eslabn mvil
-
2012-B ROBTICA CINEMTICA DEL ROBOT 4DOF - RPPR
26
Con estos resultados podemos pasar a plotear la cinemtica 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.- Extraccin de coordenadas para el ploteo
En este SubVI se llevara a cabo la extraccin de las posiciones x, y, z de cada
eslabn 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 travs 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
-
2012-B ROBTICA CINEMTICA DEL ROBOT 4DOF - RPPR
27
a) La primera parte de la estructura Flat Sequence Structure usada extraemos
los datos de traslacin 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 alteracin en los clculos pues solo es con fines grficos.
Figura N 25.- Cdigo fuente para extraccin de datos para el ploteo
-
2012-B ROBTICA CINEMTICA DEL ROBOT 4DOF - RPPR
28
4.- Cinemtica Inversa (para comprobacin)
Este SubVI fue creado en base al cdigo de la funcin cinversa1 del cdigo Matlab
expuesto y explicado anteriormente. El programa recibe los parmetros fijos D-H
junto con la matriz de transformacin homognea del efector final S4 como
entradas, y las salidas sern las cuatro articulaciones q1, q2, q3 y q4 que sern
comprobadas con referencia a la cinemtica directa. Cabe resaltar que esta SubVi
se encuentra en el modo de operacin Teach Pendant del cdigo principal.
Figura N 26.- Panel Frontal del SubVI cinversa1
Cada parte de la estructura Flat Sequence usada corresponde al clculo de cada
articulacin segn el mtodo geomtrico planteado anteriormente en anlisis
para este manipulador.
a) En el primer sector se extraen los datos de la posicin del efector final adems
de los vectores de orientacin en x4 y y4 que nos darn informacin para el
clculo de la articulacin q4 posteriormente en este cdigo.
b) El segundo y tercer sector corresponden al clculo geomtrico de las
articulaciones q1 (revoluta), q2 (prismtica) y q3 (prismtica) segn el anlisis
geomtrico. Para el caso de las junturas revolutas se ha utilizado la herramienta
matemtica atan2 de Labview la cual nos da un rango ms amplio para el
argumento a calcular ( ).
c) En el cuarto sector hace un clculo rpido de las matrices necesarias para
encontrar la matriz del eslabn 3 (A03), con el objetivo de extraer los datos de
orientacin del vector y3 para ser operado, mediante el producto escalar de
vectores, con el vector de orientacin x4 y y4 extrados antes y obtener as la
juntura q4.
-
2012-B ROBTICA CINEMTICA DEL ROBOT 4DOF - RPPR
29
Figura N 27.- Bloques segn mtodo geomtrico para la cinemtica Inversa
(comprobacin)
5.- Cinemtica Inversa (insertar posicin deseada para el efector final)
Este programa para el modo de operacin Coordenada P(x,y,z) sigue la lnea de
clculo del SubVI anterior con las siguientes diferencias:
a) Este cdigo recibe un punto deseado P(x,y,z) para el efector final del robot.
b) Se realiza el clculo rpido de las matrices de los eslabones adicionando la del
efector final A04 con el objetivo de extraer los datos de orientacin para los
vectores x4 y y4 que luego se utilizarn para el clculo de q4.
Figura N 28.- Panel Frontal
-
2012-B ROBTICA CINEMTICA DEL ROBOT 4DOF - RPPR
30
Figura N 29.- Panel de bloques con sealizaciones para este SubVI
IV.- Conclusiones
1. La cinemtica 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 generacin del robot en el espacio, adems de
entregar informacin de posicin y orientacin respecto a un sistema de
referencia.
2. La informacin de posicin y orientacin de las matrices de transformacin
homognea resulta vital para el desarrollo del modelamiento de un manipulador
robtico. Ellas nos informan si el sistema en anlisis rot o se traslad respecto a
un eje de referencia.
3. La adicin de los frames fantasmas resulta con el nico fin de describir
rotaciones o traslaciones extras que si bien es cierto, no contienen articulacin,
nos ayudan a tener detalle acerca de la estructura del robot a analizar para casos
particulares. Es importante mencionar que no hay alteracin en los grados de
libertad al utilizar este criterio.
4. Las caractersticas de este manipulador nos permiti interactuar con dos
modos de operacin para visualizar el comportamiento de la cinemtica inversa.
Es importante decir que la cinemtica inversa es netamente una herramienta de
comprobacin de articulaciones.
5. El clculo de la cuarta articulacin para el modo de operacin 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 cdigo de la cinemtica
inversa , para este caso, se le ha colocado el valor de cero a esta variable q4.
y4
x4
A04
P(x,y,z)
-
2012-B ROBTICA CINEMTICA DEL ROBOT 4DOF - RPPR
31
6. Al calcular un valor de juntura revoluta entre dos eslabones mviles para la
cinemtica inversa, resulta conveniente y seguro hacerlo vectorialmente con las
orientaciones que nos dan las matrices de TH cada vez que se produce la
rotacin.
7. Una vez ms se comprob la efectividad del software Labview para comprender
y comprobar aspectos de los algoritmos matemticos para esta experiencia.