2012-B

Robtica

Control Dinmico Robot 2DOF - RP

Control Dinmico por Torque Computado de un ROBOT 2DOF RP

Cuya Solari, Omar Antonio oacs_198@hotmail.com Flores Bustinza, Edwing Irwing irwing_1988_@hotmail.com Torres Chavez, Jonathan Emmanuel jonathan260605@hotmail.com

2012-B

Robtica

Control Dinmico Robot 2DOF - RP

I.- Obtencin del Modelo Dinmico del Robot RP mediante la formulacin Lagrange Euler La complejidad computacional de este algoritmo depende de la cantidad de grados de libertad que posea el robot a trabajar O(n4), por lo cual, ya que nuestro robot solo posee dos grados de libertad es factible realizar este procedimiento para el modelado dinmico cuyas ecuaciones finales quedarn bien estructuradas para obtener los pares y fuerzas respectivos para su movimiento. A continuacin detallaremos paso a paso el proceso con su respectivo cdigo en el programa Matlab. Para trabajar primero definimos nuestros parmetros a tratar, tanto las fijas como las variables mediante el comando syms e ingresamos nuestra tabla de parmetros D-H. Nuestros puntos sern posicin de coordenadas (q1 y q2), velocidades (dq1 y dq2) y aceleraciones (ddq1 y ddq2). Luego nuestros fijos son longitud del primer eslabn y desplazamiento (d2) para el movimiento prismtico del segundo eslabn (cabe resaltar que el segundo eslabn se desplaza desde una distancia inicial 0.5 m y seguido de un movimiento longitudinal definido por q2).
disp('DINAMICA DE UN ROBOT 2DOF USANDO EL METODO DE LAGRANGE - EULER') disp('--------------------------------------------------------------') syms q1 q2 dq1 dq2 ddq1 ddq2; syms l1 d2; %longitud de los eslabones syms g; %parametro de gravedad [m/seg^2] syms m1 m2 x1 x2 y1 y2 z1 z2 %-----------------------------------------------------------disp('1.- Tabla de Parametros DH') disp('-a---alpha---d---theta---') DH=[ 0 -pi/2 0 q1 0 0 q2 0 ]

1.1.- Paso 1 Se asigna a cada eslabn un sistema de referencia de acuerdo a las normas D-H.

Figura N1.- Prototipo a trabajar robot RP

2012-B 1.2.- Paso 2

Robtica

Control Dinmico Robot 2DOF - RP

Obtenemos las matrices de transformacin para cada elemento i. Para esto estaremos usando la funcin denavit para obtener nuestras matrices deseadas, tambin vamos a usar el comando simplify para obtener resultados de manera simblica, es decir, en funcin de nuestras variables.
disp('2.- Calculo de las matrices de transformacion: Aij') disp('-------------------------------------------------------------') %-----------------------------------------------------------%T=rotz(theta)*transl(0,0,d)*transl(a,0,0)*rotx(alpha) disp('Matriz de TH del primer eslabon movil desde la base del robot') disp('-------------------------------------------------------------') A01=simple(simplify(denavit(DH(1,1),DH(1,2),DH(1,3),DH(1,4)))) A12=simple(simplify(denavit(DH(2,1),DH(2,2),DH(2,3),DH(2,4)))); %Matriz de transformacion de la base al efector final disp('--------------------------------------------------------------') disp('Matriz de TH del efector final desde la base del robot') disp('-------------------------------------------------------------') A02=simple(simplify(A01*A12))

A01 = [ cos(q1), 0, -sin(q1), 0] [ sin(q1), 0, cos(q1), 0] [ 0, -1, 0, 0] [ 0, 0, 0, 1] -------------------------------------------------------------Matriz de TH del efector final desde la base del robot ------------------------------------------------------------A02 = [ cos(q1), 0, -sin(q1), -q2*sin(q1)] [ sin(q1), 0, cos(q1), q2*cos(q1)] [ 0, -1, 0, 0] [ 0, 0, 0, 1] 1.3.- Paso 3 Obtenemos las matrices definidas por:

2012-B

Robtica

Control Dinmico Robot 2DOF - RP

Es decir obtenemos las derivadas parciales de nuestras matrices de transformacin homognea respecto a las base solidaria con referencia a nuestras variables de posicin q1 y q2, ahora usaremos el comando diff el cual nos permite derivar elementos respecto a un punto de referencia; seguidamente de comando simplify para trabajar en modo simblico.
disp('3.- Calculo de las matrices : Uij') disp('-------------------------------------------------------------') %-----------------------------------------------------------U11=simple(simplify(diff(A01,q1))) U12=simple(simplify(diff(A01,q2))) U21=simple(simplify(diff(A02,q1))) U22=simple(simplify(diff(A02,q2))) %------------------------------------------------------------

U11 = [ -sin(q1), 0, -cos(q1), 0] [ cos(q1), 0, -sin(q1), 0] [ 0, 0, 0, 0] [ 0, 0, 0, 0] U12 = [ 0, 0, 0, 0] [ 0, 0, 0, 0] [ 0, 0, 0, 0] [ 0, 0, 0, 0] U21 = [ -sin(q1), 0, -cos(q1), -q2*cos(q1)] [ cos(q1), 0, -sin(q1), -q2*sin(q1)] [ 0, 0, 0, 0] [ 0, 0, 0, 0] U22 = [ 0, 0, 0, -sin(q1)] [ 0, 0, 0, cos(q1)] [ 0, 0, 0, 0] [ 0, 0, 0, 0] 1.4.- Paso 4 Obtenemos las matrices definidas por:

2012-B

Robtica

Control Dinmico Robot 2DOF - RP

Mediante este proceso obtenemos la derivada parcial de respecto a los parmetros de posicin q1 y q2, usamos el comando diff para las derivadas, simplify para obtener ecuaciones en modo simblico y simple para reducir a forma concreta.
disp('4.- Calculo de las matrices : Uijk') disp('-------------------------------------------------------------') %-----------------------------------------------------------U111=simple(simplify(diff(U11,q1))) U112=simple(simplify(diff(U11,q2))) U121=simple(simplify(diff(U12,q1))) U122=simple(simplify(diff(U12,q2))) U211=simple(simplify(diff(U21,q1))) U212=simple(simplify(diff(U21,q2))) U221=simple(simplify(diff(U22,q1))) U222=simple(simplify(diff(U22,q2))) %------------------------------------------------------------

U111 = [ -cos(q1), 0, sin(q1), 0] [ -sin(q1), 0, -cos(q1), 0] [ 0, 0, 0, 0] [ 0, 0, 0, 0] U112 = [ 0, 0, 0, 0] [ 0, 0, 0, 0] [ 0, 0, 0, 0] [ 0, 0, 0, 0] U121 = [ 0, 0, 0, 0] [ 0, 0, 0, 0] [ 0, 0, 0, 0] [ 0, 0, 0, 0] U122 = [ 0, 0, 0, 0] [ 0, 0, 0, 0] [ 0, 0, 0, 0] [ 0, 0, 0, 0] U211 = [ -cos(q1), 0, sin(q1), q2*sin(q1)] [ -sin(q1), 0, -cos(q1), -q2*cos(q1)] [ 0, 0, 0, 0] [ 0, 0, 0, 0] 5

2012-B U212 = [ 0, 0, 0, -cos(q1)] [ 0, 0, 0, -sin(q1)] [ 0, 0, 0, 0] [ 0, 0, 0, 0] U221 = [ 0, 0, 0, -cos(q1)] [ 0, 0, 0, -sin(q1)] [ 0, 0, 0, 0] [ 0, 0, 0, 0] U222 = [ 0, 0, 0, 0] [ 0, 0, 0, 0] [ 0, 0, 0, 0] [ 0, 0, 0, 0] 1.5.- Paso 5

Robtica

Control Dinmico Robot 2DOF - RP

Obtenemos las matrices de pseudoinercia definidas por: {

para cada elemento, que viene

Donde las integrales estn extendidas al elemento i considerado, y ( ) son las coordenadas del diferencial de masa dm respecto al sistema de coordenadas del elemento.

Figura 2

2012-B

Robtica

Control Dinmico Robot 2DOF - RP

Para este paso vamos a utilizar los valores que nos da nuestro diagrama respecto a los frames, por lo cual para el primer eslabn y respetando su centro de masa y gravedad hacia el primer frame tendremos: x1=0 y1=0 z1=L1=0.5 m1=1.4 Kg Anlogamente para el segundo eslabn: x2=0 y2=0; z2=0; m2=1 Kg Ahora estos valores los remplazamos en la matriz de pseudoinercias definido de manera correcta en el matlab.
disp('5.- Matrices de Pseudoinercia : Ji') disp('-------------------------------------------------------------') %-----------------------------------------------------------%Para el primer eslabon: J1[Kg.m^2] %m1=1.4Kg %Coordenadas del centro de gravedad 1 respecto al frame 1 %x1=0;y1=0;z1=l1=0.5m; disp('Para el Primer Eslabon') disp('-------------------------------------------------------------') J1=[ 0 0 0 0 0 0 0 0 0 0 (m1*l1^2) (m1*l1) 0 0 (m1*l1) m1 ] %-----------------------------------------------------------%Para el primer eslabon: J1[Kg.m^2] %m2=1Kg %Coordenadas del centro de gravedad 2 respecto al frame 2 %x2=0;y2=0;z2=0; disp('-------------------------------------------------------------') disp('Para el segundo eslabon') disp('-------------------------------------------------------------') J2=[ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 m2] %------------------------------------------------------------------------

J1 = [ 0, 0, 0, 0] [ 0, 0, 0, 0] [ 0, 0, l1^2*m1, l1*m1] [ 0, 0, l1*m1, m1] ------------------------------------------------------------7

2012-B

Robtica

Control Dinmico Robot 2DOF - RP

Para el segundo eslabon ------------------------------------------------------------J2 = [ 0, 0, 0, 0] [ 0, 0, 0, 0] [ 0, 0, 0, 0] [ 0, 0, 0, m2] 1.6.- Paso 6 Obtenemos la matriz de inercias D=[ Con: i,j= 1,2 n=2 (dos grados de libertad) ] cuyos elementos vienen definidos por:

Para esta etapa haremos uso del comando trace el cual realizara la traza de la matriz correspondiente, lo manipularemos en modo simbolico con simplify y haremos la respectiva suma de las matrices que cumplen con cada posicin de D.
disp('6.- Calculo de las matrices de inercia : D = [dij]') disp('-------------------------------------------------------------') %------------------------------------------------------------------------%MATRIZ D11 d11=simple(simplify(trace(U11*J1*U11.')))+simple(simplify(trace(U21*J2*U2 1.'))) %MATRIZ D12 d12=simple(simplify(trace(U22*J2*U21.'))) %MATRIZ D21 d21=simple(simplify(trace(U21*J2*U22.'))) %MATRIZ D22 d22=simple(simplify(trace(U22*J2*U22.'))) %Matriz general D = [d11 d12;d21 d22]

d11 = m1*l1^2 + m2*q2^2 d12 =0 d21 =0 d22 =m2 D= [ m1*l1^2 + m2*q2^2, 0] [ 0, m2]

2012-B 1.7.- Paso 7 Obtener los trminos

Robtica

Control Dinmico Robot 2DOF - RP

definidos por:

Con i,k,m=1,2 De esta etapa obtendremos ocho valores pues plantearemos a la matriz previa definida puntos respecto a una nueva referencia k m. Usando los mismos comandos trace, simplify la adicin.
disp('7.- Calculo del vector hikm') disp('-------------------------------------------------------------') %------------------------------------------------------------------------h111=simplify(trace(U111*J1*U11.'))+simplify(trace(U211*J2*U21.')) h112=simplify(trace(U212*J2*U21.')) h121=simplify(trace(U221*J2*U21.')) h122=simplify(trace(U222*J2*U21.')) h211=simplify(trace(U211*J2*U22.')) h212=simplify(trace(U212*J2*U22.')) h221=simplify(trace(U221*J2*U22.')) h222=simplify(trace(U222*J2*U22.')) %------------------------------------------------------------------------

h111 =0 h112 =m2*q2 h121 =m2*q2 h122 =0 h211 =-m2*q2 h212 =0 h221 =0 h222 =0 1.8.- Paso 8 Obtener la matriz columna de fuerza de Coriolis y centrpeta H=[ ] elementos vienen definidos por: cuyos

2012-B

Robtica

Control Dinmico Robot 2DOF - RP

Para esta fase definimos por clculo las operaciones que nos conlleva a obtener nuestra matriz de coriolis, utilizando la ultima matriz obtenida con nuestros valores de posicin derivadas.
disp('8.-Calculo del vector columna de fuerza centrifuga y de coriolis:H=[hi]') disp('----------------------------------------------------------------------') %------------------------------------------------------------------------h1=h111*dq1^2+h112*dq1*dq2+h121*dq2*dq1+h122*dq2^2; h2=h211*dq1^2+h212*dq1*dq2+h221*dq2*dq1+h222*dq2^2; %------------------------------------------------------------------------%Matriz de Coriolis H=[h1;h2] %--------------------------------------------------------------------

H= 2*dq1*dq2*m2*q2 -dq1^2*m2*q2 1.9.- Paso 9 Obtener la matriz columna de fuerzas de gravedad C=[ ] cuyos elementos estn definidos por: Con: i=1,2 g = gravedad

La estructura horizontal de nuestro robot permiten que las fuerzas de gravedad se proyecten sobre los apoyos, por lo cual la matriz de gravedad C se ve afectada debido a que el vector gravedad g expresado en el sistema de referencia de la base {S0} es g = {g 0 0 0}

10

2012-B

Robtica

Control Dinmico Robot 2DOF - RP

Figura 3
disp('9.-Calculo de la matriz columna de fuerzas de gravedad: C=[ci]') disp('--------------------------------------------------------------') %------------------------------------------------------------------------g1=[g 0 0 0]; % Segun el frame 0 , gravedad actuando en el eje X %------------------------------------------------------------------------% Vector de compuesto por coordenadas de posicion del centro de masas % para cada eslabon r11=[0;0;l1;1]; %CG para el eslabon 1 r22=[0;0;0;1]; %CG para el eslabon 2 c1=-m1*g1*U11*r11 - m2*g1*U21*r22; c2=-m1*g1*U12*r11 - m2*g1*U22*r22; %------------------------------------------------------------------------% Matriz de gravedad C=[c1;c2] %---------------------------------------------------------------------

C= g*l1*m1*cos(q1) + g*m2*q2*cos(q1) g*m2*sin(q1) 1.10.- Paso 10 Finalmente tenemos la ecuacin dinmica del sistema Donde es el vector de fuerzas y pares motores efectivos aplicados sobre cada coordenada qi.

11

2012-B

Robtica

Control Dinmico Robot 2DOF - RP

Donde el vector gravedad sera: [ ]

disp('10.- Ecuacion dinamica del robot') disp('-------------------------------------------------------------') %------------------------------------------------------------------------tau=simplify(D*[ddq1;ddq2]+H+C); disp('El torque o par para la primera articulacion revoluta es: ') disp('-------------------------------------------------------------') T1 = tau(1) % Juntura Revoluta disp('-------------------------------------------------------------') disp('La fuerza para al articulacion prismatica es:') disp('-------------------------------------------------------------') F2 = tau(2) % Juntura Prismatica disp('-------------------------------------------------------------') disp('La notacion general de la ecuacion dinamica es:') disp('-------------------------------------------------------------') tau=[tau(1); tau(2)]

T1 = ddq1*(m1*l1^2 + m2*q2^2) + 2*dq1*dq2*m2*q2 + g*l1*m1*cos(q1) + g*m2*q2*cos(q1) ------------------------------------------------------------La fuerza para al articulacin prismtica es: ------------------------------------------------------------F2 = m2*(- q2*dq1^2 + ddq2 + g*sin(q1)) -------------------------------------------------------------

La notacin general de la ecuacin dinmica es: ------------------------------------------------------------tau = [ddq1*(m1*l1^2 + m2*q2^2) + 2*dq1*dq2*m2*q2 + g*l1*m1*cos(q1) + g*m2*q2*cos(q1) m2*(- q2*dq1^2 + ddq2 + g*sin(q1))]

12

2012-B

Robtica

Control Dinmico Robot 2DOF - RP

II.- Prueba de generacin de trayectoria Para elaborar la trayectoria del robot RP necesitamos de las variables de posicin tanto inicial como final (q0 y qf), adems de ello vamos a trabajar con dos funciones adjuntas al cdigo principal que son cinversa y plot RP, adems utilizaremos el comando jtraj para que nos revuelva la velocidad y aceleracin de la trayectoria con dq y ddq respectivamente. 2.1.- Funcin cinversa Toma tus coordenadas de posicin q y tu espacio de trayectoria ok, se inicializan estos parmetros para nuestro robot y se defines las constantes de medida L1 y d2. Como nuestro robot es un RP puede crear diversas dimensiones de radio en su espacio de desplazamiento y rotacin, es as que le involucramos parmetros de radio R para los ejes x e y. Ahora como tenemos al eslabn 2 que toma un inicio de 0.5m creamos una condicional que si este eslabn es menor a esa medida retorne y corrija la falla. Nuestros valores de posicin q van a actualizarse a cada paso de nuestra trayectoria usando el comando atan2 entre x e y para la rotacin y radio total con la dimensin del eslabn 1, cuando la trayectoria es correcta ok=1 queda definido como proceso correcto y te arrojan los puntos q siguientes hasta fin del trayecto.
function [q ok]=cinversa(x,y) ok=0; q=[0 0]'; L1=0.5;d2=0.5; R=sqrt(x^2+y^2); if R<0.5 return end q(1)=atan2(y,x); q(2)=R - L1; ok=1; q=[q(1) q(2)]; end

2.2.- Funcin plotRP Trabaja con los valores de posicin q y los valores fijos de dimensin de los eslabones, en esta funcin tambin se realizan actualizacin de q, fijando puntos iniciales a cero, ahora en el ploteo cada eslabn quedar definido con una funcin especfica en funcin a la rotacin q1, L1 y el enlongamiento q2.
function plotRP(q) L1=0.5; d2=0.5; q1=q(1); q2=q(2); x(1)=0;

13

2012-B

Robtica

Control Dinmico Robot 2DOF - RP

y(1)=0; % Eslabon 1 x(2)=L1*cos(q1); y(2)=L1*sin(q1); % Eslabon 2 x(3)=(L1+q2)*cos(q1); y(3)=(L1+q2)*sin(q1); hold on; plot(x,y,'m','linewidth',1); plot(x,y,'k.','MarkerSize',20); grid on; axis([-1 4 -1 4]); hold off; end

2.3.- Algoritmo Principal Aqu ingresando puntos iniciales y finales nos arroja una trayectoria guiada por un nmero de puntos N, utilizando por defecto un polinomio de grado 7 con condiciones limite cero para velocidad y aceleracin. Tomamos puntos iniciales y finales, aplicamos funcin cinversa a tales puntos y creamos una condicional de espacio de trayectoria para no salir del rango fijado, si las condiciones son favorables el trayecto se da sin mayor problema y comienza a plotear cada uno de los N pares de puntos establecidos.
clear all; close all; clc % Coordenadas cartesiana inicial x0=1; y0=0; % Coordenadas cartesiana final xf=0; yf=3; [q0 ok0]=cinversa(x0,y0); % Calculo de Coordenadas articulares iniciales [qf okf]=cinversa(xf,yf); % Calculo de Coordenada articulares finales if ok0*okf==1 clf; hold on; plot([x0 xf],[y0 yf],'b','LineWidth',2); %Plotea la recta que debe seguir la trayectoria a interpolar con el jtraj plot([x0 xf],[y0 yf],'r.','MarkerSize',20); title('\bf Prueba de Trayectoria 1 para Manipulador RP') xlabel('\bf x') ylabel('\bf y') grid on; axis([-1 4 -1 4]) hold off;

Con N=50 nmero de puntos a seguir en la trayectoria invocamos el comando jtraj cuyo papel es devolvernos las evoluciones de velocidad y aceleracin para este proceso en el tiempo as como tambin la trayectoria en q para cada 14

2012-B

Robtica

Control Dinmico Robot 2DOF - RP

articulacin del robot, creamos un bucle de iteraciones de 1 a N en el cual se va plotear nuestra actualizacin de puntos.
N=50; % Numero de puntos [q dq ddq]=jtraj(q0,qf,N);% nos devuelve los valores de la posicin, velocidad y aceleracin respectivamente. for k=1:N plotRP(q(k,:)); pause(0.1); end figure, subplot(311), plot(q),title('\bf Posicion Angular (rad)') legend('q_1(R)','q_2(P)'),grid subplot(312), plot(dq),title('\bf Velocidad Angular (rad/seg)') legend('dq_1(R)','dq_2(P)'),grid subplot(313), plot(ddq),title('\bf Aceleracion Angular (rad/seg^2)') legend('ddq_1(R)','ddq_2(P)'),grid else display('Fuera del campo de accin'); end

A continuacin se mostrar los diagramas obtenidos mediante el uso de Matlab para nuestro robot RP:

Figura N2.- Patrn de trayectoria estimado por el jtraj de HEMERO

Del diagrama de trayectoria, la revoluta es constante para q1 que barre un ngulo de 90 grados y el desplazamiento de d2 es variable en el espacio de trabajo hasta llegar al punto final.

15

2012-B

Robtica

Control Dinmico Robot 2DOF - RP

Figura N3.- Diagrama donde podemos ver las actualizaciones de posicin (q), velocidad (dq) y aceleracin (ddq) para la trayectoria definida en el algoritmo principal.

Tenemos tambin otro algoritmo para obtener la trayectoria del robot, pero en este caso usamos el linspace para el ploteo de inicio a final. El comando jtraj arrojara trayectoria para los "q, q' y q''" cada dos puntos consecutivos, ya que se realizara interpolacin con N1 puntos entre dos puntos contenidos consecutivamente en la recta general. Ya no con el inicio y fin de sta como en la prueba anterior
close all; clear all; clc; % Coordenadas cartesiana inicial x0=1; y0=-0.5; % Coordenadas cartesiana final xf=0; yf=3; [q0 ok0]=cinversa(x0,y0); % Calculo de Coordenadas articulares iniciales [qf okf]=cinversa(xf,yf); % Calculo de Coordenada articulares finales if ok0*okf==1 clf; hold on; plot([x0 xf],[y0 yf],'b','LineWidth',2); plot([x0 xf],[y0 yf],'r.','MarkerSize',20); title('\bf Prueba de Trayectoria 2 para Manipulador RP') xlabel('\bf x') ylabel('\bf y') axis square axis([-1 4 -1 4]); hold off; N=10; % Cantidad de parejas de puntos a intepolar x=linspace(x0,xf,N); y=linspace(y0,yf,N); % Para interpolacion con N1 puntos entre dos puntos N1=3;

16

2012-B

Robtica

Control Dinmico Robot 2DOF - RP

%Creando almacenadores de datos Q=zeros(N1,N); dQ=zeros(N1,N); ddQ=zeros(N1,N); for k=2:N [q dq ddq]=jtraj(cinversa(x(k-1),y(k1)),cinversa(x(k),y(k)),N1); Q(:,k-1:k)=[q(:,1),q(:,2)]; dQ(:,k-1:k)=[dq(:,1),dq(:,2)]; ddQ(:,k-1:k)=[ddq(:,1),ddq(:,2)]; for j=1:length(q) plotRP(q(j,:)); % Dibuja la evolucion de los eslabones en XY pause(0.1); end end end

Figura N4.- Diagrama de trayectoria para dos puntos consecutivos definidos en la lnea recta de inicio a final.

17

2012-B

Robtica

Control Dinmico Robot 2DOF - RP

III.- Modelo Dinmica Directa ROBOT RP El modelo dinmico de un robot tiene por objetivo conocer la relacin entre el movimiento del robot y las fuerzas implicadas en el mismo. El modelo dinmico directo o dinmica directa expresa la evolucin temporal de las coordenadas articulares del robot en funcin de las fuerzas y pares que intervienen. La parte inicial del informe muestra el modelamiento de nuestro Robot RP de 2 grados de libertad en funcin de Lagrange Euler. Una vez obtenido este modelamiento vamos a realizar una simulacin de la dinmica. Para ello emplearemos el software Matlab junto a las herramientas de Simulink que nos ofrece. Es de nuestro conocimiento que el robot al ser del tipo Revoluta Prisma expresar la evolucin de sus coordenadas articulares en funcin de Torque y Fuerza. Por tal motivo simularemos el comportamiento de ambos mediantes seales sinusoidales. Estas seales ingresarn a la dinmica directa del robot y permitirn observar como ya se dijo la evolucin temporal de las coordenadas articulares del Robot. La idea fundamental de esta simulacin es observar esta evolucin. El esquema general a implementar es el siguiente:

Figura N 5.- Esquema General de Dinmica Directa del Robot RP

El torque y la fuerza tendrn comportamiento de seales sinusoidales, las configuraciones respectivas son las siguientes:

18

2012-B

Robtica

Control Dinmico Robot 2DOF - RP

Figura N 6.- Configuracin de seales sinusoidales para torque y fuerza

El torque se comportar como una seal sinusoidal de amplitud 8 y de frecuencia 2: . La fuerza se comportar como una seal sinusoidal de amplitud 5 y de frecuencia 1:

Figura N 7.- Comportamiento del torque y fuerza del robot RP

19

2012-B

Robtica

Control Dinmico Robot 2DOF - RP

3.1.- La dinmica directa de nuestro robot ser el reflejo de las ecuaciones de fuerza y torque obtenidas en la etapa de modelamiento: ..(1)

Donde la matriz de Masas es: [ La matriz de Coriolis es: [ ]..(3) ] .(2)

Y la matriz de Gravedad es: [ Resultando finalmente: ( [ ) ] ]..(4)

(5)

..(6)

3.2.- Recordando el concepto de la dinmica directa donde obtendremos la evolucin de las coordenadas articulares a partir de la fuerza o torque, despejamos la ecuacin general y tenemos: .(7)

Obtenemos entonces la aceleracin del motor respectivo. Si empleamos integradores podemos obtener el comportamiento de la velocidad y posicin de cada articulacin.

20

2012-B

Robtica

Control Dinmico Robot 2DOF - RP

De acuerdo a esto elaboraremos el esquema general de la dinmica directa:

Figura N 8.- Dinmica directa del Robot RP

Segn lo anterior: u es la seal de torque o fuerza. invD es una funcin matemtica que describe la matriz inversa de la matriz de masas del robot. Coriolis y gravedad son las matrices de respectivas del robot. invD*R es funcin de multiplicacin para obtener Se emplean integradores para obtener no solo la aceleracin del sistema, sino tambin de la velocidad y posicin.

3.3.- Describiremos cada funcin matemtica empleada: a) Coriolis: la matriz de coriolis se debe al movimiento relativo existente entre los diversos elementos, as como las fuerzas centrpetas que dependen de la configuracin instantnea del manipulador. De acuerdo al anlisis realizado mediante Lagrange-Euler: [ ]

21

2012-B

Robtica

Control Dinmico Robot 2DOF - RP

A partir de ello creamos una funcin matemtica en Matlab para emplearla en nuestro Simulink.
function Cdq=coriolis(v) m2=1; d2=0.5; dq=[v(1);v(2)]; q=[v(3);v(4)]; c11=2*m2*q(2)*dq(2); c12=0; c21=-q(2)*m2*dq(1); c22=0; C=[c11 c12;c21 c22]; Cdq=C*dq;

b) Gravedad: esta matriz es la que se origina por la fuerza de gravedad. Crearemos tambin una funcin para emplearla en el Simulink [ ]

function g=gravedad(q) m1=1.4; m2=1; L1=0.5; g=9.81; g1=m1*g*L1*cos(q(1)) + m2*g*q(2)*cos(q(1)); g2=m2*g*sin(q(1)); g=[g1;g2];

c) invD: esta es la matriz inversa de la matriz de Masas del sistema. Creamos tambin esta funcin de masas en Matlab y luego hallamos su matriz inversa: [
function iD=invD(q) m1=1.4; m2=1; L1=0.5; D11=(L1^2)*m1 + m2*(q(2))^2; D12=0; D21=0; D22=m2; D=[D11 D12;D21 D22]; iD=inv(D);

d) invD*R: este bloque representa la multiplicacin de matrices para obtener la aceleracin de las articulaciones: 22

2012-B

Robtica

Control Dinmico Robot 2DOF - RP

Una vez implementadas las funciones y el esquema en Simulink obtenemos los siguientes resultados:

Figura N 9.- Evolucin temporal de las coordenadas articulares de cada articulacin empleando simulink

La imagen anterior representa la evolucin temporal de las posiciones, velocidades y aceleraciones de cada articulacin. Las grficas de color rojo representan las coordenadas articulares de la primera articulacin, es decir provenientes del torque de la revoluta. Las grficas de color azul representan las provenientes de la fuerza del prisma. Si observamos el Simulink en la parte inferior existe un bloque llamado salidas. Este bloque permite almacenar los valores obtenidos y poder emplearlos en todo Matlab. Para realizar una contrastacin de los resultados obtenidos realizamos una pequea programacin en el Editor.
clear all; close all; clc tf=10; ang1=30; q1=pi*ang1/180; q2=0.5; t=sim('Din_robot_2dof',tf); % Tiempo de simulacin al cdigo Simulink Q=zeros(length(t),6); % Arreglo de 10x6, almacena los datos q, dq y ddq for i=1:6 Q(:,i)=salidas(:,i);%almacenamos q1 y d2 en fila 1 y 2, dq1 y dd2 en fila 3 y 4 ... end

23

2012-B
%Ploteos

Robtica

Control Dinmico Robot 2DOF - RP

subplot(311),plot(t,Q(:,1),t,Q(:,2)),xlabel('tiempo'),ylabel('q_1 & d_2'),legend('q_1','d_2'),grid subplot(312),plot(t,Q(:,3),t,Q(:,4)),xlabel('tiempo'),ylabel('dq_1 & dd d_2'),legend('dq_1','d d_2'),grid subplot(313),plot(t,Q(:,5),t,Q(:,6)),xlabel('tiempo'),ylabel('ddq_1 & dd d_2'),legend('ddq_1','dd d_2'),grid

Los parmetros iniciales son q1 = 30, es decir la revoluta con un ngulo inicial, y q2= 0.5, que es la posicin inicial del prisma. El arreglo Q de 6 columnas y tantas filas como muestras se tomen en los 10 segundos de simulacin. Las filas representan: q1, q2, dq1, dq2, ddq1, ddq2. Los resultados obtenidos son los siguientes:

1000

q &d

q1 0 -1000 d2 0 1 2 3 4 5 tiempo 6 7 8 9 10

1 2

200 dq1 0 -200 d d2 0 1 2 3 4 5 tiempo 6 7 8 9 10

dq & dd d

ddq & dd d

50 ddq1 0 -50 dd d2 0 1 2 3 4 5 tiempo 6 7 8 9 10

Figura N 10.- Evolucin temporal de las coordenadas articulares de cada articulacin empleando el Editor de Matlab

24

2012-B

Robtica

Control Dinmico Robot 2DOF - RP

3.4.- Alternativa en cdigo puro Matlab para Dinmica Directa As como empleamos Simulink para realizar la dinmica directa, podemos realizar este proceso implementando un cdigo en el editor de Matlab. 3.4.a.- En la primera parte del cdigo mostramos las condiciones iniciales del sistema:
clear all; close all; clc % Condiciones iniciales T=0.01; % Tiempo de muestreo q=[pi/6 0]'; % ngulo inicial y posicin inicial dq=[0 0]'; % velocidades iniciales ddq=[0 0]'; % aceleraciones iniciales plotRP(q); % dibujo del robot en su posicin inicial

3.4.b.- Podemos evaluar varias situaciones de nuestro robot. Las cuales se muestran a continuacin:
% Casos del torque Torq=[0 0]'; %Sin torque de entrada y solo interactuando con la gravedad %Torq=[8 0]'; %Torque < gravedad y sin fuerza al motor 2 %Torq=[0 5]'; %Sin torque al primer motor , solo fuerza al prisma %Torq=[10 5]';%Insertamos torque mayor que al fuerza de gravedad N=100; Q=zeros(2,N); dQ=zeros(2,N); ddQ=zeros(2,N); % % % % nmero de muestras arreglo para almacenar posiciones arreglo para almacenar velocidades arreglo para almacenar aceleraciones

3.4.c.- Realizamos la implementacin de la dinmica directa recordando el mismo criterio que las frmulas empleadas en el simulink: Debemos indicar antes que recordaremos lo siguiente: para realizar las integraciones respectivas

25

2012-B

Robtica

Control Dinmico Robot 2DOF - RP

for k=1:N ddq=Minv(q)*(Torq - G(q(1),q(2))- C(dq(1),dq(2),q(2))); dq=dq+ddq*T; q=q+dq*T; Q(:,k)=[q(1);q(2)]; dQ(:,k)=[dq(1);dq(2)]; ddQ(:,k)=[ddq(1);ddq(2)]; plotRP(q); axis([-5 2 -1 2]) pause(0.01); end

3.4.d.- Finalmente realizaremos los ploteos de la evolucin de las coordenadas articulares en el tiempo:
figure subplot(311),plot(Q(1,:),'r'),hold,plot(Q(2,:),'b'),grid legend('\bf q_1(R)','\bf q_2(P)'),xlabel('\bf t'),ylabel('\bf q(rad)') title('\bf Evolucion Temporal de las Articulaciones') subplot(312),plot(dQ(1,:),'r'),hold,plot(dQ(2,:),'b'),grid legend('\bf dq_1','\bf dq_2'),xlabel('\bf t'),ylabel('\bf dq(rad/seg)') subplot(313),plot(ddQ(1,:),'r'),hold,plot(ddQ(2,:),'b'),grid legend('\bf ddq_1','\bf ddq_2'),xlabel('\bf t'),ylabel('\bf ddq(rad/seg^2)')

Una vez implementada la programacin. Obtenemos los siguientes resultados:

2
q(rad)

Evolucion Temporal de las Articulaciones 5 0 -5 0 10 20 30 40 50 t 60 70 80 q1(R) q2(P) 90 100

1.5

1
dq(rad/seg)

5 0 -5 0 10 20 30 40 50 t 60 70 80 90 dq1 dq2 100

0.5

0
ddq(rad/seg 2)

20 0 -20 0 10 20 30 40 50 t 60 70 80 ddq1 ddq2 90 100

-0.5

-1 -5

-4

-3

-2

-1

Figura N 11.- Comportamiento del Robot con Torque: T= [0 0]

26

2012-B

Robtica

Control Dinmico Robot 2DOF - RP

Evolucion Temporal de las Articulaciones 10
q(rad)

0 -10 0 10 20 30 40 50 t 60 70 80

q1(R) q2(P) 90 100

1.5

1
dq(rad/seg)

20 0 -20 0 10 20 30 40 50 t 60 70 80 90 dq1 dq2 100

0.5

0
ddq(rad/seg 2)

20 0 -20 0 10 20 30 40 50 t 60 70 80 ddq1 ddq2 90 100

-0.5

-1 -5

-4

-3

-2

-1

Figura N 12.- Comportamiento del Robot con Torque: T= [8 0]

2
10
q(rad)

Evolucion Temporal de las Articulaciones q1(R) q2(P) 0 10 20 30 40 50 t 60 70 80 90 100

0 -10

1.5

1
dq(rad/seg)

20 0 -20 0 10 20 30 40 50 t 60 70 80 90 dq1 dq2 100

0.5

0
ddq(rad/seg 2)

50 0 -50 0 10 20 30 40 50 t 60 70 80 ddq1 ddq2 90 100

-0.5

-1 -5

-4

-3

-2

-1

Figura N13.- Comportamiento del Robot con Torque: T= [0 5]

27

2012-B

Robtica

Control Dinmico Robot 2DOF - RP

2
q(rad)

Evolucion Temporal de las Articulaciones 5 0 -5 0 10 20 30 40 50 t 60 70 80 q1(R) q2(P) 90 100

1.5

1
dq(rad/seg)

20 0 -20 0 10 20 30 40 50 t 60 70 80 90 dq1 dq2 100

0.5

0
ddq(rad/seg 2)

100 0 -100 0 10 20 30 40 50 t 60 70 80 ddq1 ddq2 90 100

-0.5

-1 -5

-4

-3

-2

-1

Figura N 14.- Comportamiento del Robot con Torque: T= [10 5]

Debemos recordar que simplemente se trata del comportamiento de nuestro robot frente a determinados torques o fuerzas a las que se le est sometiendo. No est siguiendo ninguna trayectoria en particular. IV.- Modelo Dinmica Inversa del robot RP La dinmica inversa expresa las fuerzas y pares que intervienen en funcin de la evolucin de las coordenadas articulares y sus derivadas. Tal y como lo dice su nombre realiza el proceso inverso de la dinmica directa, es decir a partir de las coordenadas articulares halla las fuerzas o pares de nuestras articulaciones. La aplicacin que tiene es que permite seleccionar actuadores, lo que implica la eleccin de los motores ms adecuados de acuerdo a la exigencia de nuestro proyecto. Por tal motivo aplicando la dinmica inversa podremos conocer que motores debemos elegir para implementar nuestro robot. En nuestro Robot RP, hallaremos la dinmica inversa de empleando nuevamente simulink. Para esto describiremos el comportamiento de las posiciones, velocidades y aceleraciones de acuerdo a las siguientes frmulas: q1: 2 Sen(u) q2: 0.5 Cos(u) dq1: 2 Cos(u) dq2: -0.5 Sen(u) ddq1: -2 Sen(u) ddq2: -0.5 Cos(u) 28

2012-B

Robtica

Control Dinmico Robot 2DOF - RP

Estos comportamientos ingresarn a nuestra dinmica inversa que arrojaran las curvas de torque y fuerza deseado para el robot, teniendo en cuenta las perdidas por fricciones por coriolis y gravedad. La estructura general es la siguiente:

Figura N 15.- Esquema general de la dinmica inversa

El proceso de dinmica inversa involucra conocer las matrices de Masa, Coriolis y gravedad. Para ello nuevamente creamos funciones en Matlab para luego emplearlas en simulink: a) Masas:
function M=Masas(q) m1=1.4; m2=1; L1=0.5; M11=(L1^2)*m1 + m2*(q(2))^2; M12=0; M21=0; M22=m2; M=[M11 M12;M21 M22];

b) Coriolis:
function Cdq=coriolis(v) m2=1; d2=0.5; dq=[v(1);v(2)]; q=[v(3);v(4)]; c11=2*m2*q(2)*dq(2); c12=0; c21=-q(2)*m2*dq(1); c22=0; C=[c11 c12;c21 c22];

29

2012-B
Cdq=C*dq;

Robtica

Control Dinmico Robot 2DOF - RP

c) Gravedad:
function g=gravedad(q) m1=1.4; m2=1; L1=0.5; g=9.81; g1=m1*g*L1*cos(q(1)) + m2*g*q(2)*cos(q(1)); g2=m2*g*sin(q(1)); g=[g1;g2];

Estas funciones emplean las mismas matrices obtenidas en el modelamiento dinmico realizado mediante el mtodo de Lagrange-Euler. En este caso el torque y fuerza se basan en la siguiente frmula: ...(8)

Entonces la dinmica inversa sigue el siguiente esquema:

Figura N16.- Dinmica inversa del robot

Tal y como se mencion la evolucin de nuestras articulaciones ingresan a nuestra dinmica inversa y de acuerdo a la frmula antes mencionada, ingresan a nuestras funciones de Masas, Coriolis y Gravedad para finalmente obtener el comportamiento del torque y fuerza de nuestros motores. A su vez en nuestro esquema general observamos que existen dos ganancias, las cuales provienen de los reductores de ambos motores. 30

2012-B

Robtica

Control Dinmico Robot 2DOF - RP

Finalmente observamos el comportamiento del torque y fuerza requeridos por nuestros motores a elegir para nuestro robot:

Figura N17.- Comportamiento en el tiempo del torque de la revoluta del robot

Figura N18.- Comportamiento en el tiempo de la fuerza aplicada al prisma del robot

Debemos recordar nuevamente que estos comportamientos se deben a la evolucin que hemos dado de las posiciones, velocidades y aceleraciones de nuestro robot de acuerdo a funciones sinusoidales. Adems debemos aclarar nuevamente que an no estamos dando una trayectoria especfica a nuestro robot.

31

2012-B

Robtica

Control Dinmico Robot 2DOF - RP

V.- Implementacin del Control Dinmico por Torque Computado en LABVIEW CODIGO PURO. Teniendo en cuenta las pautas trabajadas en los cdigos Matlab y Simulink para la dinmica directa, inversa y generacin de trayectorias para cada articulacin de este robot, nos disponemos a corroborar y concluir la experiencia en el software Labview en programacin grafica y utilizando SubVIs por cuestin de orden y sntesis para el cdigo. Este cdigo nos traer la ventaja de poder interactuar con la performance del control propuesto para nuestro robot, as como verificar el control de trayectoria en q deseada en el Plano XY.

Figura N19.- Panel Frontal del cdigo principal

5.1.- Contenido de cada Sub Instrumento Virtual (SubVi) Para implementar el diagrama de bloques del sistema de control por torque computado del robot RP en Labview se tendrn en cuenta los siguientes SubVIs y sectores que representan: Trayectorias Deseadas. Algoritmo de control Torque Computado. Dinmica Inversa. Dinmica Directa. Condiciones Iniciales. Ecuaciones recursivas. 32

2012-B

Robtica

Control Dinmico Robot 2DOF - RP

Ploteo de resultados. Ploteo RP.

Seguidamente se tiene el panel de Bloques donde se han consignado cada punto a explicar en el cdigo:

Figura N 20.- Diagrama de Bloques del cdigo principal

5.1.a.- Trayectorias Deseadas En este primer sector del cdigo, se ha creado un Subvi llamado generador el cual tiene la tarea de entregar seis funciones que describan trayectorias deseadas, es decir, espacio en q o evolucin temporal de q. Esto lo hemos logrado con el uso de una estructura Case que tiene en cada caso las funciones (que para esta ocasin hemos colocado senoidales y cosenoidales) para cada articulacin as como para las velocidades y aceleraciones deseadas:

33

2012-B

Robtica

Control Dinmico Robot 2DOF - RP

Figura N21.- Panel Frontal y de bloques

Detallando el SubVi generador, contiene una lgica para entregar el valor del tiempo segn un paso determinado a la funcin trigonomtrica. Multiplicamos el valor del paso con la iteracin del bucle de control para este propsito, la funcin como ya la dijimos es una funcin trigonomtrica que es invocada desde un Case que contiene los seis casos para trayectorias deseadas:

Figura N 22.- Panel Frontal SubVi Generador

q1= 2*sin(t) q2=2*cos(t) dq1=2*cos(t) dq2=-2*sin(t) ddq1=-2*sin(t) ddq2=-2*cos(t)

34

2012-B

Robtica

Control Dinmico Robot 2DOF - RP

Por ltimo cada caso se invocara con una variable tipo String o cadena de texto desde el panel principal del cdigo, aqu el panel de bloques del SubVi:

Figura N 23.- Cdigo planteado

5.1.b.- Algoritmo de Control Torque Computado En este sector del cdigo, explicaremos el algoritmo Torque Computado empleado para el control dinmico de este robot. Este SubVi recibe varias seales, entre ellas:

Figura N24.- Panel Frontal

35

2012-B

Robtica

Control Dinmico Robot 2DOF - RP

1. Primero las trayectorias deseadas de q1d, q2d, dq1d, dq2d, ddq1d y ddq2d, adems de las trayectorias obtenidas por el robot q, q y q las cuales las utiliza para generar las seales de error respectivas segn el diagrama de bloques de la figura. 2. Segundo, recibe el diseo de las matrices de ganancias Kp y Kd, es decir recibe el valor del coeficiente de amortiguamiento y la frecuencia natural, que para este caso comprenden criterios importantes segn la teora de control: El control en Robtica se basa en bsicamente un movimiento crticamente amortiguado, es decir una respuesta deseada que reduzca el error lo ms rpido posible a cero. Por ello el valor del coeficiente de amortiguamiento es 1. Luego la frecuencia natural, siendo un valor netamente experimental, es recomendable que sea baja, a partir de los 10 rad/seg. Por ello se ha colocado un control desde este valor mnimo. Estos criterios nos darn como resultados dos matrices diagonales correspondientes a Kp y Kd, decimos matrices porque estas ganancias se multiplicaran a cada seal de error para cada articulacin haciendo que el control sea ms especfico y con un tracking del error ms robusto para cada eslabn:

Por ltimo, los valores de los elementos de cada matriz diagonal no tienen porque ser iguales necesariamente, esto tiene que ver con que los motores empleados para cada eslabn sean homogneos entre s, es decir iguales en parmetros y hardware. Para nuestro caso estamos considerando motores homogneos.

36

2012-B

Robtica

Control Dinmico Robot 2DOF - RP

Figura N25.- Algoritmo de control en cdigo puro Labview

Todos los criterios explicados se han plasmado en este SubVi. Con estas matrices de ganancias, el algoritmo pasara a multiplicar cada una a su respectiva matriz de error (e y e), a su vez el error producido en las trayectorias de aceleracin se suma a la seal compensatoria para generar la ley de control a emplear en este algoritmo:

Siendo el torque necesario para compensar los torques por perdidas de friccin de coriolis y gravedad segn topologa dinmica del robot ( ) que probablemente le impedan al robot seguir un patrn de trayectoria deseado para sus articulaciones desacopladas. El resultado de esta ley de control ingresara como una aceleracin mejorada al modelo dinmico inverso del robot.

37

2012-B

Robtica

Control Dinmico Robot 2DOF - RP

5.1.c.- Modelo Dinmico Inverso El propsito de este sector del cdigo es procesar el tau_r proveniente del control e insertarlo como aceleracin mejorada al modelo, luego recalcula dos torques: el primero correspondiente a las friciones (tau_c) que siempre estn y se oponen al movimiento (coriolis y gravedad), el segundo es el torque de control completo (tau_m). Esta ltima seal ser la que ingrese a nuestro brazo robtico del cual se obtendrn datos importantes del siguiente sector del cdigo a explicar. Esta dinmica Inversa, como ya se explico anteriormente, calcula las fuerzas y pares que necesita el robot para determinadas evoluciones temporales o trayectorias deseadas. Se rige bajo la siguiente notacin:

Figura N26.- Cdigo para la ecuacin de la dinmica inversa

Esta dems decir que para que este cdigo se defina necesita los SubVis de las matrices de inercias, Coriolis y Gravedad. Cada una de ellas contiene los parmetros iniciales de las coordenadas de los centros de gravedad segn el enunciado del problema (segn las ecuaciones 2, 3 y 4): [ ]

38

2012-B

Robtica

Control Dinmico Robot 2DOF - RP

Figura N27.- Cdigos para cada matriz del modelo dinmico

5.1.d.- Modelo Dinmico Directo En este sector del cdigo se ingresara el torque de control al robot para obtener las evoluciones temporales de las articulaciones que deben seguir a la trayectoria deseada puesto que aqu ingresan valores de torque que ya vienen compensados de los producidos por las prdidas de coriolis y gravedad por el algoritmo de control. La ecuacin representada en este cdigo es:

39

2012-B

Robtica

Control Dinmico Robot 2DOF - RP

Figura N28.- Dinmica Directa Los resultados de esta dinmica se realimentan al control y a la dinmica Inversa para ser recalculados segn los cambios en la trayectoria deseada.

5.1.e.- Ecuaciones recursivas para q, q, y q Realizamos la implementacin de las ecuaciones recursivas recordando el mismo criterio que las frmulas empleadas en el cdigo Matlab de la dinmica directa:

Figura N29.- Ecuaciones recursivas implementadas en Labview

40

2012-B

Robtica

Control Dinmico Robot 2DOF - RP

Para ello nos basamos en la definicin matemtica de la derivada: Esta definicin nos hace deducir que emplearemos la herramienta de Labview llamada Shift Register que muestra el comportamiento actual y pasado de los datos. Teniendo en cuenta ello, implementaremos las siguientes ecuaciones: 5.1.f.- Condiciones iniciales Las condiciones iniciales consideradas para este programa son: Para la articulacin revoluta q1: 30 Para la articulacin prismtica q2: 0.5 Para las velocidades: dq1= 0 , dq2=0

Estos datos ingresaran como condicin inicial a los registros de desplazamiento de cada derivada y q respectivamente, produciendo una evaluacin instantnea en la primera iteracin del bucle. 5.1.g.- Ploteo de resultados En esta parte mostramos los resultados de la perfomance del control: visualizaremos el seguimiento de las trayectorias deseadas versus las trayectorias obtenidas del control y la dinmica del robot para las articulaciones en posiciones, velocidad y aceleracin, las cuales llegan a ser iguales. Adems se visualizar el tracking o seguimiento del error cuya convergencia a cero es rpida, y finalmente el ploteo RP donde nuestro robot traducir los valores para cada articulacin a puntos o coordenadas en el plano XY.

Figura N30.- Agrupando y ploteando los datos finales

41

2012-B 5.1.h.- Plot RP

Robtica

Control Dinmico Robot 2DOF - RP

Este SubVi trabaja con las posiciones de nuestro robot obtenidas para q y las plotea en el plano XY. Para esto insertamos los parmetros iniciales fijos L1 = d2 = 0.5m que representan las distancias de los centros de gravedad de los eslabones respecto de su frame actual y anterior respectivamente. Con esto veremos las posiciones en nuestro robot en el plano XY ante el control y trayectoria deseada. Se inicializa con parmetros de posicin 0 para x(1) y y(1) que representa el origen de coordenadas que en 3D seria la base del robot que se extiende en el eje Z, la cual, coincide con el frame solidario y el frame del primer eslabn. Seguidamente para el primer eslabn que va realizar movimiento de rotacin trabajamos en funcin a L1 (distancia de origen del frame base hacia el centro de masa del primer eslabn) y el ngulo girado q1 que definirn el punto x(2) y y(2). Para el segundo eslabn tomaremos en cuenta L1, q2 (distancias y juntura del segundo eslabn) y el ngulo barrido por q1 que definirn el punto x(3) y y(3). Todo esto se ha realizado basndonos en el frame solidario.

Figura N31.- Diagrama de bloques plot RP

42

2012-B

Robtica

Control Dinmico Robot 2DOF - RP

Podemos apreciar los procesos para obtener x(2),y(2) y x(3),y(3), comprobados en Matlab: q1=q(1); q2=q(2); x(1)=0; y(1)=0; % Eslabon 1 x(2)=L1*cos(q1); y(2)=L1*sin(q1); % Eslabon 2 x(3)=(L1+q2)*cos(q1); y(3)=(L1+q2)*sin(q1);

Ahora unimos cada coordenada mediante un bloque Merge Signal y obtenemos los puntos de ploteo para x e y; para el labview le anexamos puntos finales xf e yf, la grafica del eje de coordenadas y finalmente lo visualizamos haciendo uso del Build XY Graph. A su vez colocamos un retardo de 5ms en mostrar los cambios, con fines visuales y unos puntos fijos para insertar un eje de referencia o eje solidario en el origen de coordenadas. A continuacin algunas pruebas previas:

Figura N32.- Posicin A

43

2012-B

Robtica

Control Dinmico Robot 2DOF - RP

Figura N33.- Posicin B

5.2.- Simulaciones y performance de control Al simular el sistema implementado para este control dinmico por Torque computado, verificamos que las trayectorias deseadas son seguidas de forma adecuada y con la rapidez que demanda el diseo del controlador. A su vez podemos visualizar el dibujo en el plano XY del Robot que en los primeros instantes a condiciones iniciales se encuentra en una posicin base que luego es adecuada a la trayectoria insertada. Tambin se puede revisar el tracking o seguimiento del error, ste resulta ser instantneo de acuerdo al diseo establecido.

Figura N34.- Simulacin al inicio

44

2012-B

Robtica

Control Dinmico Robot 2DOF - RP

Figura N35 .- Simulacin del seguimiento de trayectorias y el error = 0

45