1. INTRODUCCIÓN
El mecanismo de cinco barras es un mecanismo am pliamente estudiado en la literatura de mecanismos y máquinas 1-4, en parte por su simplicidad y facilidad de construcción, utilizándose ampliamente en la Aca demia como ejemplo del estudio cinemático y dinámi co de mecanismos en el plano. Algunas de las investi gaciones realizadas con este mecanismo comprenden su uso como generador de funciones 5, generador de trayectorias 6, diseño cinemático de los eslabones acorde con la tarea 7 y diversas aplicaciones robóticas 8-10, entre otras. Una aplicación robótica de especial interés para este mecanismo es su uso para la recuperación de pacientes con impedimento en los miembros superiores 11.
Desde el punto de vista cinemático, la topología del mecanismo de cinco barras se caracteriza por tener dos cadenas cinemáticas compuestas por jutas rotacio nales siendo la primera de ella la junta actuada, de allí la denominación 2-RR 12,13. El mecanismo de cinco barras, es un sistema mecánico ideal para estudiar los conceptos básicos teóricos de robots paralelos: cinemá tica, dinámica y control. Ejemplo de ello es el banco de enseñanza distribuido por Quanser, denominado por la empresa como "2-DOF Robot" 14. Sin embargo, a pesar de la existencia de un producto comercial, mu chas veces es necesario construir un sistema ajustado a las necesidades específicas del problema a investigar. En este sentido en 15,16 se muestra el desarrollo de DEXTAR en los laboratorios de la École de Technolo gie Supérieure (ÉTS), Canadá; el cual se utilizó para la calibración cinemática 13, y más adelante en el con trol de trayectoria por tiempo mínimo 17.
En cuanto al control del sistema, en 8 se ilustra un método de control por actuación híbrida, es decir un motor DC con control de velocidad en una junta y un servo motor con control de posición en el otro grado de libertad del sistema. En 18-20 se muestran di ferentes técnicas de control no lineal aplicadas en el mecanismo de cinco barras. Otros ejemplos de control por actuación híbrida son presentados en 21,22. Los avances en control para el mecanismo plano de cinco barras también han permitido utilizarlo en aplicacio nes de teleoperación con Robótica Paralela como en el laboratorio PAROLA 23, que cuenta con un mecanis mo de cinco barras, un robot Delta y un robot 3-RRR. Otros retos en cuanto al control de este mecanismo se adelantan para el caso de posicionamiento del efector para eslabones flexibles, logrado con investigaciones en control QFT realizado por 24, y el control basado en modelo para supresión de las vibraciones 25.
En este trabajo se presenta la primera parte de una investigación del mecanismo de cinco barras que está siendo desarrollada en la Universidad Tecnológica de
Bolívar, Cartagena, Colombia. Este reporte compren de el diseño, construcción, modelamiento y control. La primera sección explica el diseño realizado y el proto tipo construido. Posteriormente, se introducen los mo delos cinemático y dinámico, este último se realiza con el enfoque empleado en 26,27 para un robot espacial Stewart-Gough en el espacio de la tarea, pero en este trabajo se desarrolla en el espacio articular. Se conti núa con simulaciones de control por par calculado y experimentos de control dinámico por par calculado.
2. METODOLOGÍA
2.1 Descripción del prototipo
El mecanismo fue construido en la Universidad Tecnológica de Bolívar utilizando fresadora CNC, torno convencional y taladro. El sistema utiliza dos actuadores motor Maxon Motors RE30 de 60W con reducción 66:1 y encoder de 500 pulsos por revolución. Los eslabones son de aluminio, mecanizados en fresadora HAAS TM-1R a partir de platinas de 2 in de ancho y 1/2 in de espesor. La figura 1 ilustra el modelo CAD del conjunto desarrollado en SolidEdge, mientras que la figura 2 muestra el prototipo final ensamblado. La tabla 1 resume las dimensiones de los eslabones del mecanismo.
2.2 Cinemática de posición
La figura 3 presenta la nomenclatura utilizada para el análisis cinemático del mecanismo de cinco eslabones. El sistema de referencia inercial se ubica en el punto A, los puntos B, C, D y P se emplean para definir la ubicación de las juntas rotacionales. El punto P define además la ubicación deseada del mecanismo, es decir la posición del efector final. Los vectores, donde i va de 1 a 5, son los utilizados para determinar la posición y orientación de cada eslabón. El ángulo Θi define el giro del eslabón respecto al eje x. La posición del centro de gravedad de cada eslabón viene dada por el vector y el ángulo de giro de dicho vector con respecto a cada vector se denota por βi. El análisis de posición comprende la cinemática inversa, comúnmente empleada en la planificación de movimiento en el efector final, y la cinemática directa, empleada para cerrar el lazo de control de posición en el efector final. La nomenclatura definida permite establecer la ecuación vectorial del lazo en la ecuación 1, a partir de la figura 3.
2.2.1 Cinemática inversa de posición
El mecanismo de cinco barras tiene 2 -DOF, por lo que se necesitan controlar dos actuadores para definir la posición del efector. La cinemática inversa se encarga de calcular los valores de las juntas actuadas, Θ2 y Θ5, en función de la posición deseada del punto P y de los demás parámetros del mecanismo. El lado izquierdo del lazo en ec. (1) puede ser escrito en sus componentes para generar las ecuaciones escalares dadas en ecs. (2) y (3), sabiendo que las componentes del punto P son P x y Py:
donde c i = cosΘ i y s i = senΘ i . Despejando para Θ2 de las ecs. (2) y (3) después de cancelar los términos del ángulo Θ3 elevando al cuadrado en ambos lados de las ecs. (2) y (3), sumar término a término, y sustituir las identidades de la tangente del ángulo medio se obtiene la ec. (4),
donde 𝐴 2 =2 𝐿 1 − 𝑃 𝑥 𝐿 2 , 𝐵 2 =−2 𝑃 𝑦 𝐿 2 , 𝐶 2 = 𝐿 3 2 − 𝐿 1 2 − 𝐿 2 2 − 𝑃 𝑥 2 − 𝑃 𝑦 2 +2 𝐿 1 𝑃 𝑥
De igual forma, para obtener las expresiones de Θ5, se parte del origen del marco inercial hasta el efector, punto P, pasando por los elementos 4 y 5 para obtener inicialmente las ecs. (5) y (6).
Despejando para Θ5 de las ecs. (5) y (6) se obtiene la ec. (7),
donde 𝐴 5 =2 𝐿 5 𝑃 𝑥 , 𝐵 5 =2 𝐿 5 𝑃 𝑦 , 𝐶 5 = 𝐿 5 2 − 𝐿 4 2 + 𝑃 𝑥 2 + 𝑃 𝑦 2
Los restantes ángulos de posición se obtienen mediante el rearreglo en la ecuación vectorial (1) y repitiendo el procedimiento de solución trigonométrica, se obtiene
donde 𝐴 3 =2 𝐿 3 𝑟 𝑥 , 𝐵 3 =2 𝐿 3 𝑟 𝑦 , 𝐶 3 = 𝐿 4 2 − 𝐿 3 2 − 𝑟 𝑥 2 − 𝑟 𝑦 2 , y
Donde 𝐴 4 =2 𝐿 4 𝑟 𝑥 , 𝐵 4 =2 𝐿 4 𝑟 𝑦 , 𝐶 4 = 𝐿 4 2 − 𝐿 3 2 − 𝑟 𝑥 2 − 𝑟 𝑦 2
2.2.2 Cinemática directa de posición
Para la cinemática directa de posición, se calculan las coordenadas del punto P conociendo la información que entregan los encoders ubicados en los actuadores, ángulos Θ 2 y Θ 5, y se verifica el cumplimiento de las ecs. (2) y (5) para P x , y las ecs. (3) y (6) para P y .
2.3 Cinemática de velocidad
El análisis de velocidad se emplea para relacionar las velocidades de las juntas actuadas, 2 y 5, con la velocidad del efector final, V p . Al igual que en el análisis de posición, está el análisis cinemático directo y el análisis cinemático inverso.
2.3.1 Cinemática inversa de velocidad
Al derivar la ecs. (2) y (3) se obtienen las ecs. (10) y (11), respectivamente, que relacionan la velocidad del efector final con las velocidades angulares de los eslabones 2 y 3.
Eliminando w 3 del sistema de ecs. (10) y (11) resulta la relación de velocidades entre el efector final y la junta actuada 2, presentada en la ec. (12), con S i-j = sen(Θi - Θj).
Similarmente, se elimina Θ2 de (10) y (11) para relacionar la velocidad angular del eslabón 3 con la velocidad del efector final, dada en la ec. (13).
Para relacionar las velocidades angulares ω4 y ω 5 con la velocidad del efector final Vp se derivan las ecs. (5) y (6) para generar la ecs. (14) y (15).
Se despeja para las velocidades angulares ω5 y ω4 a partir del sistema de ecs. (14) y (15) en función de la velocidad del efector, obteniendo las ecs. (16) y (17), respectivamente.
2.3.2 Jacobiano inverso
Las ecuaciones (12) y (16) que contienen las razones de cambio de las juntas actuadas 2 y 5, respectivamente, se agrupan para encontrar el jacobiano inverso de velocidades, J -1 , mediante la ec. (18), donde la velocidad del punto P es Vp, es presentada en la ec. (19), y el vector de velocidad de las juntas actuadas es ω, presentado en la ec. (20). El jacobiano inverso relaciona las velocidades angulares de los actuadores con la velocidad del efector final, se deduce a partir de la ec. (18) y se presenta explícitamente en la ec. (21).
2.3.3 Cinemática directa de velocidad
Ahora se obtiene una expresión para relacionar las velocidades angulares ω3 y ω4 con la velocidad del efector final V p , en función de las velocidades angulares de los actuadores ω2 y ω 5 , ya conocidos. Se inicia tomando derivada de las ecs. (2), (3), (5) y (6) con respecto al tiempo, y son igualadas por componentes para obtener las ecs. (22) y (23).
Las velocidades conocidas son ω2 y ω5, por lo que se puede eliminar la velocidad angular ω4 del sistema de ecs. (22) y (23) para obtener la expresión de ω3 presentada en la ec. (24).
De igual forma, se elimina ω 3 de las ecs. (22) y (23) para hallar la expresión de ω4 en la ec. (25).
La relación entre las velocidades del efector final y las velocidades angulares de entrada se obtienen al reemplazar la ecs. (24) y (25) en las ecs. (10) y (11); y (14) y (15). Reorganizando términos y teniendo en cuenta que el resultado debe ser el mismo, ya que es la misma variable de salida con las mismas variables de entrada, se obtiene la expresión para las velocidades del efector final en las coordenadas x y y, presentadas en las ecs. (26) y (27), respectivamente.
2.3.4 Jacobiano
La ecuación (18) se puede reescribir como se presenta en la ec. (28), y es posible identificar la expresión del jacobiano del sistema, J, mostrada en la ec. (29), es decir la matriz que relaciona la velocidad de la herramienta con la velocidad de las juntas actuadas. La ecuación (29) concuerda con la inversa de la ec. (21), como era de esperarse.
2.3.5 Jacobiano de los eslabones
El jacobiano de los eslabones, denotado por JΘ,i, se utiliza en el modelo dinámico, y sirve para relacionar la velocidad del centro de gravedad de cada eslabón, V cgi , con las velocidades angulares de los actuadores representadas en el vector de velocidad de las juntas actuadas w presentado en la ec. (20). La expresión genérica para un eslabón i se muestra en la ec. (30).
Para el eslabón 2 la velocidad del centro de gravedad se calcula por
con las velocidades de las componentes dadas por
donde 𝑠 𝑖+𝛽𝑖 =𝑠𝑒𝑛 𝜃 𝑖 + 𝛽 𝑖 𝑦 𝑐 𝑖+𝛽𝑖 =cos( 𝜃 𝑖 + 𝛽 𝑖 )
A partir de las ecs. (32) y (33) se deduce el jacobiano del segundo eslabón en la ec. (34).
Similarmente, para la velocidad del centro de masa del quinto eslabón se establecen las ecs. en (35) y (36), a partir de la cual se puede deducir el jacobiano del quinto eslabón, presentado en la ec. (37).
Para el caso del tercer eslabón, la velocidad del centro de gravedad se calcula por las ecs. (38) y (39).
Al reemplazar la ec. (24) en las ecs. (38) y (39), se expresan las componentes de la velocidad del centro de masa del tercer eslabón en términos de las juntas actuadas para obtener el jacobiano de tercer eslabón, dado por
Por último, para el eslabón 4 se tiene la expresión de velocidad del centro de gravedad expresada en las ecs. (41) y (42).
Reemplazando la ec. (25) en las ecs. (41) y (42) se obtiene, después de reorganizar términos, el jacobiano del cuarto eslabón dado por
2.4 Cinemática inversa de aceleración
Puesto que la formulación dinámica se basará en el principio del trabajo virtual, el análisis dinámico se transforma en la obtención de la derivada de los jacobianos de los eslabones. Sin embargo, para el control por par calculado se requiere conocer la aceleración deseada de las juntas actuadas, siendo una de las desventajas de esta estrategia de control. Es necesario entonces presentar el análisis inverso de aceleración para determinar la aceleración deseada de las juntas a partir de la aceleración deseada del efector final. Derivando la ec. (18) se obtiene la expresión que relaciona la aceleración de las juntas actuadas con la aceleración del efector final, presentada en (44).
El término puede obtenerse ya sea derivando explícitamente la expresión del jacobiano inverso de la ec. (21) y multiplicando este resultado por la velocidad del efector final; o despejando el término correspondiente a partir de la derivada de las ecs. (10) y (11); y (14) y (15), en cuyo caso se obtiene la expresión presentada en la ec. (45).
2.5 Análisis dinámico
El análisis dinámico del robot paralelo plano de cinco barras se realizó utilizando la misma metodología aplicada para un robot paralelo espacial Stewart- Gough en los trabajos (26, 27) en el espacio de la tarea; mientras que en el presente trabajo el modelo dinámico se calcula en el espacio articular o de las juntas. La ecuación dinámica se expresa mediante el modelo:
donde M, C y g son la matriz de masa generalizada, la matriz de Coriollis y el vector gravitacional en el espacio articular, respectivamente, y es el vector de fuerza (y/o momento) generalizada o externa. El cálculo de cada uno de esos términos debe tener en consideración las contribuciones de cada eslabón, mediante las expresiones:
donde los términos M , C y g representan las matrices de masas, de Coriollis y el vector gravitacional, respectivamente, de cada eslabón expresado en el sistema de referencia de su centro de gravedad. La matriz de Coriollis expresada en el centro de gravedad para el mecanismo cinco barras es nula, C. = (0), por cuanto es un mecanismo planar, sin embargo, si existen la matriz de masa y el vector gravitacional. La matriz de masa de cada eslabón tiene el formato:
donde m. es la masa del eslabón e I es el momento de inercia del eslabón alrededor del eje perpendicular al plano del robot bidimensional. El vector gravitacional de cada eslabón, g , estará determinado por la expresión del vector gravitacional en el sistema de referencia del mecanismo, tal como se define en la Figura 3. Se define el vector gravitacional g = (gx; gy; 0)T, y ayudará a definir el de cada eslabón como se presenta en la ecuación 49.
Por último, las matrices J Θ,i representan los jacobianos de los eslabones -presentados previamente en las ecs. (34), (37), (40) y (43), y las matrices representan sus derivadas. La obtención de estos términos puede ser lo más tedioso en el modelado dinámico, especialmente para las matrices de los eslabones 3 y 4. Sin embargo, el algoritmo de la dinámica es relativamente fácil de programar si se cuenta con un software de manipulación simbólica como Mathematica o Maple, que genere las expresiones de las matrices de masas, Coriollis y el vector gravitacional. El resultado se puede exportar a Matlab para realizar las simulaciones de los algoritmos de control, o directamente a C para programar el sistema de control embebido.
2.6 Control por par calculado
El control por par calculado es un tipo de control no lineal ideal para controlar robots basándose en el modelo dinámico. Un resumen del método, ver 28,29, se presenta a continuación. A partir del modelo dinámico, ec. (50), se busca la manera de cancelar los términos no lineales de Coriollis y gravedad, para ello se crea una ley de control, ec. (51), donde es el vector de aceleración deseada en los eslabones, y u es la variable de error de aceleración definida por la ec. (52), luego expresada conspicuamente mediante la ec. (53) en términos del vector error e, su razón de cambio, y las matrices definidas positivas K p y K v , por lo cual se reduce la expresión a la ec. (54).
La sustitución de la ec. (53) en la ec. (51) permite expresar la ley de control:
sin embargo, en este artículo se emplea el formato:
donde α d es el vector de aceleración de juntas deseado, con y definidos los vectores de error de ángulo y velocidad de juntas, expresados respectivamente mediante las ecs. (57) y (58).
2.7 Simulación computacional
Se realizan simulaciones en Simulink con el objeto de validar tanto el modelo dinámico desarrollado, como la ley de control a implementar. La figura 4 muestra el esquema de control empleado. La tarea a realizar es un movimiento circular del efector final del robot. El planificador de trayectoria arroja el punto deseado, su velocidad y aceleración lineal según el tiempo de simulación. Esos datos pasan a la cinemática inversa, la cual calcula la posición Θi, la velocidad ωi y la aceleración angular αi de cada eslabón, y este resultado se transfiere al controlador. Este bloque calcula el error de posición Θ d -Θ, y el error de velocidad ωd-ω, las matrices de masas M0, las matrices de Coriollis C0, y el vector gravitacional g 0 , para posteriormente evaluar la ley de control en (56).
El controlador calcula el par de cada motor y el resultado se pasa a un bloque en SimMechanics, figura 5, el cual realiza la simulación dinámica directa del sistema, devolviendo la posición articular de los eslabones de entrada. Por último, un bloque realiza la cinemática directa para evaluar la posición y la velocidad actuales del sistema a partir de la información suministrada por los actuadores. Para una fácil visualización de la tarea, se cuenta con el modelo de la figura 6. Los valores de las masas, inercias y la posición de los centros de gravedad de los eslabones utilizados en Simmechanics fueron obtenidos a partir del modelo CAD del mecanismo, el cual se construyó con Siemens NX 10. Las dimensiones de los eslabones se encuentran en la tabla 1, mientras que en la tabla 2 se encuentran las propiedades dinámicas empleadas en la simulación. Como se puede apreciar en la figura 3 del diseño CAD del sistema, se tiene que los ángulos p. utilizados para la localización de los centros de gravedad para cada eslabón tienen un valor de cero. La simulación se realizó para valores de ganancias establecidos en K p = 20I 2x2 y K v = 10 I 2x2 . La gravedad se encuentra sobre el eje y, y tiene un valor de -9.8 m/s2. La constante Kv se requiere en simulación por cuanto no se simula la fricción en las juntas. Los resultados obtenidos se ilustran en las figuras 7, 8, 9 y 10.
Eslabón | Masa (kg) | Inercia (kg- mm 2 ) | r. cg (mm) |
2 | 0.079 | 186.95 | 80.43 |
3 | 0.196 | 1589.29 | 124.92 |
4 | 0.194 | 1517.91 | 122.84 |
5 | 0.079 | 186.95 | 80.43 |
Se aprecia en la figura 7 que la trayectoria deseada tomada como un círculo de diámetro 100 mm con centro en el punto (77.5; 350; 0) mm, difiere levemente de la trayectoria generada por el robot en la simulación. La figura 8 ilustra los errores angulares de posicionamiento de los actuadores. Se aprecia que el máximo error de seguimiento es de 0.014 rad. En la figura 9 se presenta el error de velocidad angular de los actuadores del mecanismo, obteniendo un error máximo de seguimiento en velocidad de 0.008 rad/s. El par requerido por los motores para realizar la maniobra se muestra en la figura 10. En el caso del motor 1 que actúa sobre la junta 2, el par máximo requerido es de 0.4 N-m, y para el motor 2 que actúa sobre la junta 5, el par es de -0.4 N-m.
3. RESULTADOS Y DISCUSIÓN
El control del sistema se montó sobre una tarjeta embebida tipo PC-104 referencia Advantech PCM- 3362N-S6A1E. La tarjeta se conecta al bus PC-104 con una tarjeta Sensoray 526, con cuatro salidas analógicas y cuatro entradas de encoders. El sistema operativo fue Linux, Debían Jessie de 64 bits, al cual se le agregó el kernel de tiempo real distribuido por Pengutronix 30. El sistema de control se conecta con amplificadores Maxon EPOS 2 24/2. Los motores Maxon Motors tienen una constante de par, Kt de 25.9 mN-m/A, lo cual significa que usando los reductores GP-32 con reducción 66:1 y eficiencia promedio del 50%, el conjunto motorreductor podrá entregar un par continuo máximo de 1.7 N-m.
El algoritmo de control se diseñó en Matlab y se exportó a C con la herramienta codegen de Matlab. El programa principal en Linux está basado en el ejemplo de aplicación de tiempo real colocado de referencia en la página de los parches RT PREEMPT 31. El control se ejecuta a una prioridad de 49 y a una frecuencia de 1000 Hz.
Debido a la fricción del sistema y a pequeños desali neamientos mecánicos respecto al modelo teórico, fue necesario agregar la parte integral al controlador de par calculado. Los valores finales de las constantes emplea das fueron, K = 100I2x2 K = 500I2x2 y K = 10I2x2. Como se puede apreciar respecto a la simulación teórica, la fricción entre las juntas juega un papel importante en la parte experimental, ello explica los valores mucho más altos en las constantes K p y K v .
La figura 11 ilustra la trayectoria real realizada por el efector final del robot. En ella se observa que inicialmente hay un apreciable error de seguimiento el cual se reduce después de 3 s (en el intervalo (5, 8) s), que se puede apreciar mejor en la figura 12. Esta figura ilustra el error real de posición angular del robot, partiendo de una posición inicial de 0° en la junta 2 y 180° en la junta 5, en ella se aprecia que el error máximo de posición durante el seguimiento de la trayectoria circular es de 5°.
La figura 13 ilustra el error de velocidad angular en las juntas 2 y 5 durante el experimento, se observa similitud en la forma del error de velocidad angular de las simulaciones, mostrado en la figura 9. El error máximo registrado en la etapa de seguimiento es de 5°/s. Por último, la figura 14 muestra los torques en las juntas 2 y 5, se aprecia ruido debido a la fricción.
4. CONCLUSIONES
Se presentó la aplicación de la técnica de control por par calculado a un robot paralelo plano de dos grados de libertad. El control por par calculado es adecuado para sistemas no lineales, y se requirió del modelo dinámico explícito del robot. El modelo calculado es ideal para el control de par calculado por cuanto es un modelo completo que contiene las contribuciones dinámicas de cada eslabón presente en la cadena cinemática. Las simulaciones computacionales permitieron validar el algoritmo de control propuesto y arrojaron un pequeño error de seguimiento, característico del control por par calculado. La experimentación confirmó el error de seguimiento, con la necesidad de incrementar las constantes proporcionales y de velocidad usadas en la simulación, dado que el controlador no tiene en cuenta los términos no modelados de la ecuación dinámica, como es, entre otros, la fricción. Desde el punto de vista de la concordancia entre las pruebas y los resultados experimentales, el proyecto es satisfactorio, sin embargo, como trabajo futuro se propone la aplicación de otras estrategias de control no lineal para la misma trayectoria y comparar sus resultados.