UPGto Campus Cortazar Departamento de Robtica

Robot 3 GDL Seguimiento de Trayectorias basado en el Modelo

Juan Francisco Salomn Rodrguez Tpicos de Robtica 2 | Departamento de Robtica | UPGto Campus Cortazar

Resumen
Se presenta el diseo de un controlador para seguimiento de trayectoria basado en el modelo matemtico dado.

Desarrollo y anlisis de resultados

Instrucciones: Considere un modelo matemtico simplificado dado por las ecs. (1), (2) y (3), de un robot manipulador del tipo escara de 3 GDL. Obtenga lo siguiente: (a) proponga controladores para seguimiento de trayectoria; (b) la dinmica en lazo cerrado y la dinmica del error de los controladores; (c) la ecuacin caracterstica de la dinmica del error; (d) una simulacin numrica en Simulink para analizar el desempeo del controlador, aplicado a las ecs. (1) y (2), con un tiempo de 3 s, utilizando el mtodo numrico Runge-Kutta a un paso fijo de integracin de 100ms. Considere una trayectoria con x=R*cos(wt), y=R*sin(wt) y z=w*t, con w = 5, R=15 y condiciones iniciales para teta de 200 [1].
[dx]= [ a1*cos(teta1)+a2*cos(teta1+teta2)]; % ec. 1 [dy]= [ a1*sin(teta1)+a2*sin(teta1+teta2)]; % ec. 2 [dy]= [ (d3)]; % ec. 3

Modelo matemtico de la velocidad dx=J*dteta; %donde dx representa al vector x en su primera derivada y dteta es la entrada de control. a) Controlador cinemtico de la velocidad propuesto U=J^(-1)*[dxd-k*(x-xd)]; donde k representa a los elementos (1,1), (2,2) y (3,3) de la matriz inverza de J.
[U1] [dxd-k11(x-xd)] [U2]= J^(-1)* [dyd-k22(y-yd)] [U3] [dzd-k33(z-zd)]

b) Dinmica en lazo cerrado

dx=J*dteta dx=J*J^(-1)*[dxd-k*(x-xd)];

Dinmica del error

dx=[dxd-k*(x-xd)]; 0 =[(dx-dxd)+k*(x-xd)];

c) Ec. Caracterstica de la dinmica del error.

de + k*e = 0; s + k = 0; s = -k; con valores de k > 0
Prof. Esteban Chvez Conde | Tpicos De Robtica II | Ingeniera Robtica

1/1

UPGto Campus Cortazar Departamento de Robtica

[ de1 + k11*e1 ] [0]; [ de2 + k22*e2 ]=[0]; [ de3 + k33*e3 ] [0];

Donde el error e = x - xd. Representado en forma de vector al error en su primera derivada tenemos:
[de1] [k11 0 0 ][e1] [de2]+[0 k22 0 ][e2]=0 [de3] [0 0 k33][e3]

Desarrollo de Simulacin Numrica Primero se realiz la simulacin de cada una de las ecuaciones ya mencionadas, en un archivo m-file, y mediante un bloque de simulink de Matlab Function para obtener como respuesta la derivada de x,y y z. Parmetros utilizados:
a1=200; que es la longitud del eslabn 1; a2=150; que es la longitud del eslabn 2;

Programa Principal:
function salida=R3GDLfuntion(j) %constantes k=20; % k debe de tener valor mayor a 0; %entradas teta1=j(1); a1=j(2); x=j(3); dxd=j(4); xd=j(5); yd=j(6); dyd=j(7); y=j(8); a2=j(9); teta2=j(10); d3=j(11); z=j(12); zd=j(13); dzd=j(14); %Elementos de la matriz J j11=-(a1*sin(teta1)+a2*sin(teta1+teta2)); j12=-(a2*sin(teta1+teta2)); j13= 0; j21= (a1*cos(teta1)+a2*cos(teta1+teta2)); j22= (a2*cos(teta1+teta2)); j23= 0; j31= 0; j32= 0; j33= 1; %Matriz J J=[j11 j12 j13; j21 j22 j23; j31 j32 j33]; n=[(dxd-k*(x-xd));(dyd-k*(y-yd));(dzd-k*(z-zd))];

Prof. Esteban Chvez Conde | Tpicos De Robtica II | Ingeniera Robtica

2/1

UPGto Campus Cortazar Departamento de Robtica

dteta=J\n; dteta1=dteta(1,1); dteta2=dteta(2,1); dd3=dteta(3,1); %modelo matematico dx=-(a1*sin(teta1)*dteta1+a2*sin(teta1+teta2)*(dteta1+dteta2)); dy= (a1*cos(teta1)*dteta1+a2*cos(teta1+teta2)*(dteta1+dteta2)); dz= dd3; %vector de salida salida=[dx;dteta1;dteta2;dy;dd3;dz];

En la figura 1 se observa el diagrama de bloques representativo para cada una de las ecuaciones.

Figura 1 Diagrama de bloques para las ecuaciones Una vez realizado esto, se desarrollo de igual manera un diagrama de bloques con el controlador propuesto para el seguimiento de trayectoria. En las figuras 2 y 3 se observa el diagrama de bloques del sistema dinmico para las ecuaciones y la trayectoria deseada respectivamente.

Prof. Esteban Chvez Conde | Tpicos De Robtica II | Ingeniera Robtica

3/1

UPGto Campus Cortazar Departamento de Robtica

Figura 2 Sistema representativo para las ecuaciones.

Figura 3 Sistema representativo para la trayectoria deseada (x). Primeramente, se observa la comparacin del comportamiento de cada una de las variables de salida, haciendo las comparaciones para verificar que la trayectoria deseada se acerca a la que se obtiene realmente, utilizando valor de la ganancia k=1, como se muestra en la figura 4.

Prof. Esteban Chvez Conde | Tpicos De Robtica II | Ingeniera Robtica

4/1

UPGto Campus Cortazar Departamento de Robtica

Figura 4 Comparacin del comportamiento de la seal deseada y la seal real. En la figura 5 se observa la representacin de resultados para la comparacin de la variable x deseada y la x real mientras que en la figura 6 esta la representacin para y.

Figura 5 Representacin de resultados para la variable x deseada.

Figura 6 Representacin de resultados para la variable x real.

Prof. Esteban Chvez Conde | Tpicos De Robtica II | Ingeniera Robtica

5/1

UPGto Campus Cortazar Departamento de Robtica

5. Conclusiones generales
En el seguimiento de trayectoria, se observa que el comportamiento de la salida real inicialmente comienza a estabilizarse despus de cierto tiempo, esto debido a las ganancias del controlador propuesto y el desempeo del mismo.

6. Referencias
[1] E. Chvez-Conde, Examen de Regularizacin Parte 1 Seguimiento de Trayectorias Mediante PID basado en el Modelo.

Prof. Esteban Chvez Conde | Tpicos De Robtica II | Ingeniera Robtica

6/1