Universidad Politcnica de Puebla

Ingeniera Mecatrnica

Proyecto: Programa para el posicionamiento de un robot paralelo de dos grados de libertad

Alumno: David Badillo Gonzlez

Asesor del proyecto: M.C. Carlos Garca Meneses

Puebla, Pue., Abril de 2010

Abstract:

The present work has as main objective to develop a computer program to control the actuators of a parallel robot with two degrees of freedom using the software Matlab, in order to build a physical robot in coming years. It is very important to have a program which controls the position and the lengths of the actuators of the robot, since of this way will be achieved manipulate fully the robot from a computer. Even though the completion of a program of this kind requires a significant investment of time and effort the work will be divided into several stages, beginning with to know the environment of software, define the variables to monitor and control, investigate the format of input data, perform the program and finally it will be necessary to test the program finished.

ndice

I. II. III. IV. V. VI. VII. VIII.

Introduccin .......................................................................................... 1 Antecedentes ......................................................................................... 1 Planteamiento del problema ................................................................ 3 Objetivos: general y especficos.......................................................... 4 Justificacin .......................................................................................... 4 Metodologa ........................................................................................... 5 Cronograma ........................................................................................... 6 Desarrollo .............................................................................................. 7 Manipulador paralelo de 2GDL ................................................................ 7 Modelo dinmico...................................................................................... 9 Ecuaciones de energa .......................................................................... 10 Ecuacin de Lagrange .......................................................................... 11 Desarrollo del programa ....................................................................... 14

IX. X. XI. XII.

Resultados y discusin de resultados .............................................. 21 Conclusiones ....................................................................................... 21 Fuentes de referencias ....................................................................... 22 Anexos ................................................................................................. 23 A.1 MATLAB Serial port interface ......................................................... 23 A.2 Serial port object properties ............................................................ 28 A.3 Serial port I/O functions .................................................................. 30 A.4 Programa del modelo dinmico del robot ....................................... 31 A.5 Programa para solucionar el modelo dinmico ............................... 33 A.6 Programa para el posicionamiento del robot .................................. 33

I.

Introduccin

Este proyecto forma parte de un trabajo que tendr como objetivo construir un robot paralelo de dos grados de libertad. Este trabajo se ha venido desarrollando con anterioridad abarcando los estudios cinemticos y dinmicos, as como el desarrollo de un control proporcional-diferencial no lineal. La finalidad de este proyecto es la de realizar un programa de computadora para poder posicionar el robot paralelo de dos grados de libertad; este programa debe contener el modelo dinmico, la ley de control y la trayectoria de seguimiento del robot. El programa, en resumen, al inicializarlo generara los vectores de posicin, que posteriormente sern enviados elemento por elemento mediante los puertos seriales a las tarjetas electrnicas, las cuales controlaran los motores de la estructura mecnica.

II.

Antecedentes

Los primeros trabajos tericos relacionados con las estructuras mecnicas paralelas aparecieron ya hace siglos, antes de que se empezara a hablar de robots, cuando los primeros gemetras franceses e ingleses realizaron sus estudios sobre los poliedros y sus aplicaciones. Ya en la poca actual, el primer mecanismo paralelo fue patentado en el ao 1931 (US Patent N 1,789,680). Se trataba de una plataforma de movimiento destinada a la industria del entretenimiento diseada por James E. Gwinnett, (Gwinnett, 1931). Desgraciadamente el mecanismo diseado por este ilustre visionario no fue nunca construido.

Y no hubo que esperar mucho mas para que el primer diseo de robot industrial paralelo apareciera (US Patent N 2,286,571). En 1940, tan solo diecinueve aos despus de que Karel Kapek acuara el trmino robot, Willard L.V. Pollard presentaba un ingenioso robot de 5 grados de libertad destinado a operaciones de pintura con spray. El robot consista en tres brazos de dos eslabones cada uno. Los eslabones estaban unidos mediante juntas universales. Los tres actuadores de la base comandaban la posicin de la herramienta, mientras que la orientacin era proporcionada por otros dos actuadores situados en la base y que transmitan el movimiento a la herramienta mediante la rotacin proporcionada a unos cables flexibles. Sin embargo, han sido otros mecanismos paralelos los que han logrado un mayor reconocimiento general y han contribuido a la aparicin de un mayor nmero de publicaciones sobre la robtica paralela. Por estricto orden temporal, fue el Dr. Eric Gough quien en 1947 invento la plataforma paralela ms popular de todas. El Dr. Gough diseo un octaedro hexpodo con lados de longitud variable como plataforma para la comprobacin del comportamiento de los neumticos de la casa Dunlop (para la cual trabajaba) bajo cargas aplicadas en diferentes ejes. De esta forma, intentaba simular el proceso de aterrizaje de un avin. En la actualidad existen multitud de plataformas basadas en este diseo en numerosas empresas, y se conocen bajo el nombre de MAST (Multi-Axis Simulation Table). Ya en 1965, Mr. Stewart, (Stewart, 1965) presento un articulo en el que describa una plata forma de movimiento de 6 grados de libertad destinada a trabajar como simulador de vuelo. Contrariamente a la creencia general, el mecanismo de Stewart es diferente al presentado por Gough. El artculo de Stewart tuvo y tiene una gran influencia en el mundo acadmico y se considera como uno de los primeros trabajos de anlisis de plataformas paralelas. Paralelamente, el ingeniero Klaus Cappel realizaba en el Franklin Institute Research Laboratory numerosas investigaciones con plataformas paralelas de 6 grados de libertad. En 1967 Mr. Cappel (Cappel, 1967) patentaba un simulador de movimiento basado en un hexpodo, y construyo varias de sus invenciones.
2

Mas tarde en 1978, Hunt (Hunt, 1978) sugiri que se usaran los mecanismos actuados de forma paralela de los simuladores de vuelo, como robots manipuladores y destaco que los manipuladores paralelos requeran de un estudio mas detallado en el contexto de las aplicaciones robticas a la vista de las ventajas en cuanto rigidez y precisin respecto a los robots serie convencionales. Adems de la excelente relacin carga admisible/peso propio, los robots paralelos presentan otras caractersticas interesantes. En 1979, McCallion y Pham (McCallion and Pham, 1979) fueron los primeros que propusieron usar la plataforma de Stewart como un manipulador paralelo para una clula de ensambla je robotizada, fundamentalmente porque la posicin del efector final es mucho menos sensible a los errores que los sensores articulares de los robots en serie. La gran rigidez asegura que las deformaciones de los actuadores sean mnimas y esta caracterstica contribuye en gran manera a la precisin en el posicionamiento del manipulador [6].

III.

Planteamiento del problema

Actualmente la carrera de Ingeniera Mecatrnica no cuenta con laboratorios del rea de robtica tanto para fines didcticos como de investigacin, por lo que se opto por solucionar esta problemtica mediante proyectos de investigacin tanto para profesores y alumnos de la carrera. Con esto se pretende ir subsanando esta deficiencia ya con bases previamente fundamentadas y solidas. La primera accin que se ha tomado es la de construir un robot paralelo de dos grados de libertad, para esto el trabajo se dividi en varias etapas segn el origen tecnolgico del mismo, es decir, el proyecto se secciona de la siguiente manera: 1. Programa para el posicionamiento de un robot paralelo de 2 grados de libertad
3

2. Diseo de la estructura mecnica para el robot paralelo de 2 grados de libertad 3. Diseo y construccin de una tarjeta electrnica para controlar un motor de CD a 24V (Driver para robot paralelo de 2 grados de libertad) 4. Control de motor de CD por tarjeta PIC servo 5. Diseo y construccin de un encoder para servomotor Hoy en da ya se ha comenzado a trabajar con los puntos 1 y 3 del proyecto, con lo que se pretende que un futuro no mayor a dos aos se tenga el trabajo terminado.

IV.

Objetivos: general y especficos

Objetivo general: Desarrollar un programa para posicionar un robot paralelo de dos grados de libertad. Objetivos especficos. El programa debe hacer uso de los puertos seriales debido a que las tarjetas que controlan a los motores utilizan este tipo de interfaz. El programa debe resolver el modelo dinmico del robot. Generar los vectores de posicin deseados tanto en x como en y. Enviar los datos de las posiciones deseadas mediante los puertos seriales. Comparar la posicin real con la deseada para que de esta forma exista una realimentacin si as se requiere.

V.

Justificacin

Es muy importante tener un programa con el cual posicionar un robot, debido a que este contendr instrucciones que lograran que las dems partes que lo integran lleguen a un fin comn, por ejemplo que el efector final se desplace de un punto a otro o que el efector cumpla con una trayectoria previamente establecida. Sin el programa las dems partes del robot solo serian funcionales para objetivos dispersos o inclusive no cumpliran alguna funcin en especfico, como por ejemplo la estructura mecnica.

VI.

Metodologa

Para poder llegar al objetivo de disear el programa para el modelo cinemtico de un robot paralelo de dos grados de libertad se comienza por realizar un estudio cinemtico y dinmico del manipulador, utilizando el mtodo de Euler-Lagrange. Investigar la forma en que sern introducidos los datos al programa en base a las variables a controlar. Conociendo todo esto se procede a realizar el programa con la ayuda de Matlab, que es un programa matemtico que ofrece un entorno de desarrollo integrado con un lenguaje de programacin propio. Debido a que su lenguaje de programacin es ms manejable y simple a comparacin con otros. En la ltima etapa de la construccin del programa, es necesario realizar las pruebas convenientes para corregir aquellos errores que pudieran surgir, y de esta forma, llegar al objetivo de manera satisfactoria. En la Figura 1 se muestra el mapa conceptual de la metodologa utilizada.

Figura 1. Mapa conceptual de la metodologa

VII.

Cronograma

Tabla 1. Cronograma de actividades

VIII.

Desarrollo

Manipulador paralelo de 2 GDL La estructura esta constituida por dos pistones elctricos de masas Mv y dos longitudes totales de extremo a extremo l1(t) y l2(t), unidos en un extremo por una carga mvil de masa Mp que es afectada por la fuerza gravitacional g, ver figura 2. Los otros dos extremos de los pistones se encuentran fijos a dos ejes paralelos cuya distancia de separacin fija es D (longitud de la base de la plataforma).El pistn izquierdo puede moverse alrededor de un eje fijo P1 y el pistn derecho puede moverse alrededor del eje fijo P2. Para analizar el modelo se va a definir el siguiente sistema de coordenadas, el eje x yace sobre la base de la plataforma, y el eje y apunta perpendicularmente a la base de la plataforma, en la figura 2 se muestra la estructura geomtrica de un manipulador paralelo de dos grados de libertad. Para obtener el modelo dinmico del manipulador descrito en la figura 2, se definen las coordenadas cartesianas asignando dos variables independientes x(t) y y(t), 1(t) y 2(t) son los ngulos formados entre los pistones y la barra fija de longitud D. A partir de estas consideraciones se hace uso de las ecuaciones de EulerLagrange [1] con el objeto de encontrar propiedades relacionadas con la funcin de energa del sistema, para esto primero se procede a encontrar las ecuaciones de energa cintica y potencial asociados con el manipulador paralelo de dos grados de libertad.

Mpg

Figura 2. Manipulador paralelo de 2 grados de libertad

Se considera que: = = () = () = () (1)

= = sin () = = () donde i =1, 2

(2)

Sabemos que por trigonometra: 1 =

1 ()

1 =

1 ()

2 =

2 ()

2 =

2 ()

(3)

donde: 1 = 2 + 2 2 = ( )2 + 2 (4)

Modelo dinmico Primeramente, se calcula la posicin y la velocidad de cada actuador de masa Mv, as como de la carga [1] mecnica de masa Mp. Sea () el vector que va del origen de coordenadas al centro de gravedad de cada i-actuador y sea () el vector que va del origen de coordenadas a la carga mvil de masa Mp. Expresando cada vector () (i = 1,2) y () en coordenadas rectangulares, se obtiene: 1 = , 2 1

2 1

2 = 2 2 , = ,

2 2

(5)

Despus, se derivan las expresiones anteriores para obtener las velocidades de cada elemento, se obtiene:
1 () 2

1 =

1 , 1 2 , 2 (6)

2 =

2 () 2

= , Ahora, se expresan las velocidades [2] angulares de cada actuador (i = 1, 2), en coordenadas rectangulares. Para esto, se toma en cuenta la geometra de la Figura 1, tenemos: = 1 ()1 = 1 1 1 =
1 1 1 ()

= 2 2 = 2 ()2 2 =
2 2 2 ()

(7) (8) (9)

Sustituyendo (3) y (4) en (9), se tiene que: 1 =

2 + 2

2 =

() 2 + 2

(10)

Ecuaciones de Energa Ya obtenidas las expresiones para la posicin y la velocidad, se procede a obtener las ecuaciones de energa [3]. La energa cintica para cada actuador y la carga Mp se define como: 1 = 1 () 2 1
1

2 = 2 2 () = 2 ( 2 + 2 ) donde, =
2 2 1

(11)

es el momento de inercia de los actuadores.

La energa cintica total [2] del sistema es: = 1 + 2 + expresndolo en trminos de las coordenadas rectangulares(x, y) tenemos: =
2 2 2 + 2

(12)

() 2 ()2 + 2

2 + 2

(13)

Despus, la energa [2] potencial de los actuadores y de la MP se definen como: 1 = 1 1 2

1

2 = 2 2 = La energa potencial total del sistema es: = 1 + 2 + ()

(14)

(15)
10

en trminos de las coordenadas (x, y) se tiene: =

2 2 + 2

2 ()2 + 2

(16)

Ecuacin de Lagrange Se considera un sistema no conservativo, en donde se aplican dos fuerzas que actan en la direccin de las coordenadas generalizadas q=(x, y). La ecuacin de [1] Euler-Lagrange se escribe:

= 0

(17)

donde, F= (fx, fy) es el vector de fuerzas que acta sobre el punto OP, de la Figura 3 se obtiene: = 1 1 2 2 = 1 1 + 2 2 (18)

Figura 3. Diagrama de fuerzas

Sabiendo que el [1] Lagrangiano es: = () (19)

11

sustituyendo (13) y (16) en (19) se obtiene: = 2

2 2 + 2

+2

() 2 ()2 + 2

2 + 2

2 2 + 2

2 ()2 + 2

(20)

considerando que q=(x, y), la ecuacin de Lagrange (17) se puede escribir para cada una de las coordenadas x y y de la siguiente forma:

= =

(21) (22)

Derivando la expresin de L de la ecuacin (12) segn lo solicitado en las dos ecuaciones anteriores y para simplificar las expresiones resultantes se hacen las siguientes igualdades: 1 = 2 =
2 2 + 2 2

2 2 2 + 2 2

1 2

(23) (24) (25) 2 (26)

2
2 + 2 2

2 () 2 2 + 2 2

1 = 2 = con

2 2 + 2 2

2 2 2 + 2 2

()2 + () 2 + 2 2

2 ()2 +() 2 + 2 2

1 = 2 = se obtiene, para la fuerza fx: = + +

2 2 2 2 2 1 2 1

+ 2 + 2

(27) (28)

() + ()2 + 2

2 2 2 2

+
2 2
2 2

2 2
2 2

1 1
2 1

(29)

2 +
2

2 1 2 1

1 +
2 + 2 3/2

1 1
2 1

2 + 2 3/2

12

y para la fuerza fy: = +

2 2

2 2
2 2

1 1
2 1

+ +
2 1 2 1

2 1 2 1

+
2 2 2 2

2 2 2 2

2 2

1 1
2 1

1 +
2

1 +

2 (30)

+ +

2 + 2

+ 3/2

2 ()2 + 2 3/2

Escribiendo los resultados anteriores en forma matricial, se obtiene:

1 2 + 2 + 2 1 2 = 22 2 12 1 2 1 2 2 2 1 2 1

2 2
2 2

1 1
2 1

+
2 2
2 2 2 1 2 1

2 1 2 1

+ 1

2 2 2 2

2 2

2 + 2
2

1 1

2 1 +

1 1
2 1 2 2 2 2

2 2
2 2

1 1
2 1

2 + 2 3/2 2 2 + 2 2

2 + 2 3/2 2 ()2 + 2 3/2

3/2 +

(31)

La ecuacin matricial (31) tiene la forma generalizada para el modelo dinmico de un robot, a saber: + , + = (32)

en donde, M(q) es la matriz de inercia simtrica, para nuestro es igual a: +

2 2
2 2 2 1 2 1

+
2 1

2 2 2 2

2 2
2 2

1 1
2 1

1 1

2 1 2 1

2 2 2 2

(33)

13

 , es la matriz de Coriolis, y es igual a:

2 2

, =

2 2

2 + 2

2 1 2 1

1 1

2 2
2 2 2 1 2 1

2 1 +

1 1
2 1 2 2 2 2

1 (34)

2 2
2 2

1 1
2 1

y G(q) es el vector de gravedad, igual a:

2 + 2 3/2 2 2 + 2 2 2

2 + 2 3/2 2 ()2 + 2 3/2

+ 3/2

(35)

La ecuacin (31) representa a un sistema compacto de ecuaciones de movimiento que describe en forma detallada el comportamiento del manipulador paralelo de dos grados de libertad.

Desarrollo del programa Habiendo establecido previamente el programa con el que se va a trabajar (MATLAB) y el tipo de interfaz a manejar, se procedi a investigar el

funcionamiento de los puertos seriales (ver anexo A.1). Otro punto que se debe de incluir en el desarrollo del programa, es el hacer uso de los programas previamente realizados, programa del modelo dinmico del robot y el programa para solucionar el modelo dinmico (ver anexos A.4 y A.5). Al correr el programa para solucionar el modelo dinmico (anexo A.5) se genera una grafica que muestra la trayectoria a seguir y el desplazamiento del robot al tratar de seguirla, para finalmente cumplirla (figura 4).

14

Figura 4. Seguimiento de la trayectoria Una vez conocido el funcionamiento de estos programas se procede a realizar las modificaciones requeridas para la solucin de este problema. Se comienza por crear y configurar el puerto serial a utilizar (Communications properties, anexo A.2), pero debido a que en este caso se requiere manipular tanto las coordenadas en x como en y es necesario hacer uso de dos puertos seriales (nombrados SerPIC y SerPIC1) y tomando en cuenta que en la comunicacin serial solo se puede enviar un dato a la vez. Ya creados y configurados los puertos se crea un ciclo con el comando while, el cual tiene la funcin de enviar cada determinado tiempo la posicin que debe de adoptar el robot para as seguir la trayectoria definida mediante la funcin fprintf. Pero antes de entrar al ciclo se mandan a abrir los puertos por los cuales se enviaran los datos con el comando fopen; cabe mencionar que los datos se pueden enviar en diferentes formatos, como se muestra en la tabla 2.

15

Tabla 2. Tipos de formatos de los datos salida

Una vez que se finaliza el ciclo en el programa se mandan a deshabilitar, a remover y a limpiar los puertos que se han utilizado para enviar los datos. Debido a que es necesario realizar pruebas como se estableci en el cronograma de actividades, se realizan unas modificaciones al programa que se ha desarrollado; para esto se opta por generar un banco de datos con un cierto margen de error, para que de esta manera al momento de enviar la posicin deseada que debe de adoptar el robot se realice una comparacin con una segunda posicin que en este caso seria la posicin real y de esta forma realizar una realimentacin para tratar de minimizar el error. Para realizar las pruebas se hace uso del programa Virtual Serial Port Driver, ver figura 5, el cual tiene la funcin de crear pares de puertos seriales virtuales, y de esta forma trabajar con ellos sin necesidad de que la computadora con la que se este trabajando cuente con ellos de forma fsica.

16

Figura 5. Virtual Serial Port Driver Habiendo creado los puertos virtuales es posible trabajar con distintos programas al mismo tiempo, con los cuales uno de ellos puede enviar los datos y el otro recibirlos. En este caso el programa a utilizar para visualizar el envi de datos que realiza MATLAB, es el programa ISIS de Proteus, en el cual son colocados en la pantalla de trabajo dos puertos COM y dos terminales virtuales, ver figura 6, los cuales servirn para visualizar el envi de datos.

Figura 6. Diagrama del circuito para visualizacin de datos

17

Antes de correr los programas es necesario configurar las mismas propiedades de comunicacin en los puertos tanto en MATLAB (figura 7) como en ISIS (figura 8) de la misma forma, recordando que uno es el que enva y el otro el que recibe.

Figura 7. Configuracin de los puertos que envan (MATLAB)

Figura 8. Configuracin de los puertos que reciben (ISIS)

Realizadas las previas configuraciones de los puertos, ahora es posible realizar la simulacin ejecutando primeramente ISIS y consecuentemente MATLAB.

18

Al ejecutar MATLAB se generara una ventana de dialogo, ver figura 9, la cual confirmara que la transmisin de los datos ha comenzado junto con una grafica, figura 10, la cual contendr la trayectoria deseada, la comparacin entre esta misma y el seguimiento de la trayectoria que realiza el robot, y la comparacin entre la posicin deseada a la posicin real que adoptara el robot.

Figura 9. Ventana de dialogo

Figura 9. Graficas de trayectorias

19

Al ejecutar ISIS se desplegaran dos pantallas en donde se visualizaran los datos, al inicio se visualizar el dato de la posicin deseada, figura 10, y despus de determinado tiempo se visualizar el error, figura 11.

Figura 10. Posicin deseada

Figura 11. Posicin deseada y compensacin

20

IX.

Resultados y discusin de resultados.

Los resultados obtenidos en la realizacin de este proyecto han cubierto de manera satisfactoria todos los objetivos planteados. Ya que al ejecutar el programa al inicio resuelve el modelo dinmico, para as generar los vectores de las posiciones deseadas a partir de una trayectoria a seguir. Pero debido a la problemtica de no contar con la estructura mecnica del robot y su parte sensrica se opt por generar un banco de datos, que en este caso representaba la posicin real con determinado margen de error, para as comprobar la funcionalidad del programa. Cabe mencionar que los resultados cumplieron con las expectativas.

X.

Conclusiones

Con este proyecto se contribuir a la causa de poder construir un robot en la Universidad Politcnica de Puebla en prximos aos, ya que actualmente no se cuenta con la infraestructura para su implementacin. Tambin se piensa que este tipo de proyectos fomentan un mayor inters por la ciencia, pues ayudan a conocer y a descubrir las nuevas tecnologas que actualmente estn siendo usadas y desarrolladas en torno a la Mecatrnica. Este proyecto servir como base de proyectos a futuro, como son el poder construir robots paralelos de seis grados de libertad.

21

XI.

Fuentes de referencia

[1]

Carlos Fernando Aguilar Ibez. Control implcito para una cadena cinemtica cerrada de dos grados de libertad

[2]

Dare A. Wells. Teora y problemas de Dinmica de Lagrange. McGraw-Hill. Colombia (1972).

[3]

Marcelo Alonso y Edward J. Finn. Fsica Volumen I: Mecnica. Fondo educativo interamericano. Mxico (1970).

[4]

Ferdinand P. Beer, E. Russell Johnston Jr. Mecnica vectorial para ingenieros: Esttica. McGraw-Hill. Mxico (1998)

[5]

Ferdinand P. Beer, E. Russell Johnston Jr. Mecnica vectorial para ingenieros: Dinmica. McGraw-Hill. Mxico (1998)

[6]

Aracil, R., R. Saltarn, J. Ma. Sabater Maquinas con un pasado para una

y O. Reinoso. Robots paralelos: robtica del futuro. Revista

Iberoamericana de Automtica e Informtica Industrial. (2006). [7] Isidro Zabalza, Javier Ros. Aplicaciones actuales de los robots paralelos. 8 Congreso Iberoamericano de ingeniera mecnica. [8] [9] Carlos A. Jara. Anlisis del espacio de trabajo de un robot paralelo 3RRR. R. Saltarn. Control visual de un robot paralelo: anlisis y diseo de la plataforma robotenis. [10] Rafael Aracil, Roque Saltaren, Mara A. Scarano R., Ral Bermejo S. Estudio del comportamiento de un robot paralelo trepador para labores de supervisin. [11] Mauricio C. Mndez. Desarrollo de un robot paralelo para manufactura gil. The Anhuac Journal.

22

XII.

Anexos

A.1 MATLAB Serial port interface

23

24

25

26

27

A.2 Serial port object properties

28

29

A.3 Serial port I/O functions

30

A.4 Programa del modelo dinmico del robot

%ROBOT PARALELO DE DOS GRADOS DE LIBERTAD %Programa del modelo dinmico del robot %Seguimiento de trayectoria %Ley de control P-D no lineal axd=-radio*cos(t); ayd=-radio*sin(t); %-----------------------------------------------%Geometria del robot l1=sqrt(x1^2+y1^2); function dx=trayectoria(t,x) l2=sqrt((x1-L)^2+y1^2); t1=atan(y1/x1); %variables de estado x1=x(1); %coordenada x% y1=x(2); %coordenada y% x2=x(3); %derivada de x1=velocidad en x% y2=x(4); %derivada de y1=velocidad en y% t2=atan(y1/(L-x1)); S1=y1/l1; S2=y1/l2; C1=x1/l1; C2=(L-x1)/l2;

%Parametros del robot Mp=5; Mv=1.25; r=0.14; L=0.2; J=0.01225; g=9.81;

%Matriz de inercia global m11 m12 m22 M m11=Mp+J*(S1^2/l1^2+S2^2/l2^2);

%Constantes de la ley de control k0=10; k1=10; alfa=0.1; beta=6;

m12=J*(C2*S2/l2^2-C1*S1/l1^2); m22=Mp+J*(C1^2/l1^2+C2^2/l2^2); M=[m11 m12;m12 m22];

%------------------------------------------------%SEGUIMIENTO DE TRAYECTORIA %Trayectoria deseada: circunferencia con centro %en el punto (h,k) y radio (radio) h=0.1; k=0.18; radio=0.02; xd=h+radio*cos(t); yd=k+radio*sin(t); qd=[xd;yd]; vxd=-radio*sin(t); vyd=radio*cos(t); vqd=[vxd;vyd];

%Matriz de Coriolis d1=(x2*x1+y2*y1)/(x1^2+y1^2); d2=(-x2*(L-x1)+y2*y1)/((L-x1)^2+y1^2); O11=d2*S2^2/l2^2+d1*S1^2/l1^2; O12=d2*C2*S2/l2^2-d1*C1*S1/l1^2; O22=d2*C2^2/l2^2+d1*C1^2/l1^2; O=[O11 O12;O12 O22];

%Vector de gravedad g11=(Mv*g*r/2)*(-x1*y1/l1^3+(L-x1)*y1/l2^3);

31

g22=Mp*g+(Mv*g*r/2)*(x1^2/l1^3+(L-x1)^2/l2^3); G=[g11;g22];

Kfo=0; Kf1=0; Kc=(Kfo+KG)/u;

l1d=sqrt(xd^2+yd^2); l2d=sqrt((L-xd)^2+yd^2);

norv1=sqrt(xd^2+yd^2); norv2=sqrt(vxd^2+vyd^2); norv3=sqrt((k0*ex+k1*evx)^2+(k0*ey+k1*evy)^2);

gx=(Mv*g*r/2)*(-xd*yd/l1d^3+(L-xd)*yd/l2d^3); gy=Mp*g+(Mv*g*r/2)*(xd^2/l1d^3+(L-xd)^2/l2d^3);

norv4=sqrt(ex^2+ey^2); norv5=sqrt(evx^2+evy^2); norv6=sqrt(axd^2+ayd^2);

%------------------------------------------------%Matriz de control con seguimiento de trayectoria %Definicion del error ex=x1-xd; ey=y1-yd; e=[ex;ey];

deltad=(Kfi*(norv2)^2+Kf1*norv2+u*norv6)/mup; deltae=(u*(k0*norv4+k1*norv5)+Kfi*(norv5)^2+Kf1*no rv5)/mup; delta=Kc+deltad+deltae; div=delta*norv3+beta*exp(-alfa*t);

fx=-(u*(k0*ex+k1*evx)*delta^2)/div+gx; evx=x2-vxd; fy=-(u*(k0*ey+k1*evy)*delta^2)/div+gy; evy=y2-vyd; ev=[evx;evy]; %aceleracion en x y %Propiedades mecanicas global mup u KG Kfi fuerza=F-G-O*[x2;y2]; mup=Mp; u=2*Mp+4*J/r^2; Kfi=2*(sqrt(2))*J/r^2; ay=Ac(2); KG=(Mp+2*Mv)*g; %Archivo de derivadas %Terminos no lineales dx=[x2;y2;ax;ay]; Ac=inv(M)*fuerza; ax=Ac(1); F=[fx;fy];

32

A.5 Programa para solucionar el modelo dinmico

%ROBOT PARALELO DE DOS GRADOS DE LIBERTAD %Solucin del modelo dinmico %El archivo con el modelo dinmico se llama "trayectoria"

[T,X]=ode23(@trayectoria,[0 15],[0.1;0.18;0;0], 0.1); global h k radio; h=0.1;k=0.18; radio=0.02;

txd=h+radio*cos(T); tyd=k+radio*sin(T); plot(X(:,1),X(:,2),txd,tyd); axis('equal') grid

A.6 Programa para el posicionamiento del robot

clc %ROBOT PARALELO DE DOS GRADOS DE LIBERTAD %Programa para el posicionamiento del robot %posiciones iniciales (0.1,0.18) subplot(2,2,2);plot(X(:,1),X(:,2),txd,tyd); legend('Seguimiento de la trayectoria','Trayectoria deseada') axis('equal') [T,X]=ode23(@trayectoria,[0 15],[0.1;0.18;0;0], 0.1); global h k radio; h=0.1;k=0.18; radio=0.02; %Posiciones deseadas xd=X(:,1); txd=h+radio*cos(T); yd=X(:,2); tyd=k+radio*sin(T); %Posiciones reales subplot(2,2,[1 3]); plot(txd,tyd,'r') title('TRAYECTORIA DESEADA','FontSize',12) xr=[xr xd-0.001]; axis('equal') yr=[]; xr=[]; grid grid

33

yr=[yr yd-0.001];

msgbox('Enviando...') while n<=m

%Grafica de la posicion real con la deseada subplot(2,2,4);plot(xd,yd,'m',xr,yr,'k') legend('Posicin deseada','Posicin real') axis('equal') grid fprintf(SerPIC,'%d',XD); %pause(0.001) %Configuracion de los puertos SerPIC=serial('COM2'); set(SerPIC,'BaudRate',2400); set(SerPIC,'DataBits',8); set(SerPIC,'Parity','none'); set(SerPIC,'StopBits',1); set(SerPIC,'FlowControl','none'); ey=YD-yr(n,1); SerPIC1=serial('COM5'); set(SerPIC1,'BaudRate',2400); set(SerPIC1,'DataBits',8); set(SerPIC1,'Parity','none'); set(SerPIC1,'StopBits',1); set(SerPIC1,'FlowControl','none'); %Cierre de los puertos seriales %Apertura de los puertos seriales fopen(SerPIC); fopen(SerPIC1); fclose(SerPIC); fclose(SerPIC1); delete(SerPIC); delete(SerPIC1); %Envio de datos n=1; m=length(xd); disp('BEGIN') clear SerPIC clear SerPIC1 disp('STOP') n=n+1; pause(0.001) end fprintf(SerPIC1,'%d',ey); fprintf(SerPIC1,'%d',YD); %pause(0.001) ex=XD-xr(n,1); fprintf(SerPIC,'%d',ex); XD=xd(n,1); YD=yd(n,1);

34