Pendubot

Universidad Autónoma de Querétaro Facultad de Ingeniería Carrera: Ingeniería en Automatización Construcción e Instrumentación de un Pendubot TESIS Que como parte de los requisitos para obtener el grado de Ingeniero en Automatización, con línea terminal en Instrumentación y Control Presenta: Brenda Santa Dublan Barragán Dirigido por: Dr. Roberto Valentín Carrillo Serrano SINODALES Dr. Roberto Valentín Carrillo Serrano Presidente Firma Dr. Víctor Manuel Hernández Guzmán Secretario Firma MC. Alfonso Noriega Ponce Vocal Firma MC. José Marcelino Gutiérrez Villalobos Suplente Firma Dr. Aurelio Domínguez González Director de la Facultad Centro Universitario Querétaro, Qro. Mayo de 2013 México RESUMEN En este trabajo es construido e instrumentado un pendubot con control de corriente como prototipo experimental para apoyo didáctico en los cursos de control de la facultad de Ingeniera de la Universidad Autónoma de Querétaro. Este sistema electromecánico consiste en dos eslabones rígidos interconectados por uniones o juntas rotacionales, teniendo así dos grados de libertad de enlaces rígidos, con actuador en el primero y el segundo de ellos es no actuado. El prototipo es probado con un controlador por retroalimentacion de estado, en la posición donde ambos eslabones apuntan hacia arriba. El controlador es programado en una computadora portátil donde la interfaz entre la computadora y el prototipo es realizada con un microcontrolador comercial usando comunicación serial, mientras que tanto la interfaz con el usuario como la programación del controlador están hechas en builder C++ versión 6. Finalmente son mostrados resultados experimentales los cuales prueban el adecuado funcionamiento del sistema. (Palabras clave: pendubot, lazo de corriente, sistemas mecánicos subactuados, retroalimentación de estado) I SUMMARY In this work it is built and instrumented a pendubot with a current controller as an experimental setup for teaching support in control courses of the Faculty of Engineering at the Universidad Autonoma de Queretaro. The electromechanical system consists of two rigid links connected by rotational joints or connections, having two degrees of freedom of rigid links with the first actuated and the second of them is not actuated. The prototype is tested with a state feedback controller in the position where both links are pointing upward. The controller is programmed into a laptop where the interface between the computer and the prototype is made with a commercial microcontroller using serial communication, while both the user interface and controller programming are made in C++ builder 6 version. Finally experimental results are shown to test the proper operation of the system. (Key words: pendubot, current loop, underactuated mechanical systems, state feedback) II A Dios, a mis padres, familia y amigos —por todo lo recibido. III AGRADECIMIENTOS A la Universidad Autónoma de Querétaro (UAQ) por brindarme la oportunidad de formarme como ingeniero. A los profesores de la Facultad de Ingeniería por todas sus enseñanzas, en especial a mi asesor Dr. Roberto Valentín Carrillo Serrano por su constante retroalimentación, apoyo a este trabajo, confianza, amistad y por todo lo aprendido a lo largo del desarrollo de la tesis. Al Dr. Edgar Alejandro Rivas Araiza por sus comentarios, recomendaciones a este trabajo, confianza y amistad. Al Dr. Víctor Manuel Hernández Guzmán por sus recomendaciones a este trabajo. Al M. en C. Alfonso Noriega Ponce por sus recomendaciones a este trabajo. Al M. en C. José Marcelino Gutiérrez Villalobos por el apoyo y retroalimentación brindada. Al Ing. Antonio Hurtado Bárcenas por todos sus consejos tanto en lo profesional como en lo personal, por creer en mi y ayudarme a descubrir que tan grande puedo ser. IV ÍNDICE GENERAL RESUMEN I SUMMARY II DEDICATORIA III AGRADECIMIENTOS IV 1. INTRODUCCIÓN 1.1. Planteamiento del problema . . . . . . . . . . . . . 1.1.1. Objetivo . . . . . . . . . . . . . . . . . . . . 1.1.2. Hipótesis . . . . . . . . . . . . . . . . . . . 1.2. Justificación . . . . . . . . . . . . . . . . . . . . . . 1.3. Nomenclatura para derivadas con respecto al tiempo . . . . . . 1 1 1 2 2 2 2. REVISIÓN DE LITERATURA 2.1. Estado del arte . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.2. Pendubot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3 3 5 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3. FUNDAMENTACIÓN TEÓRICA 7 3.1. Modelo matemático . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7 3.2. Linealización del sistema en un punto de operación . . . . . . . . . . . . . . 12 3.3. Controlador para atrapar el pendubot con ambos eslabones arriba . . . . . . . 13 4. MATERIALES Y MÉTODOS 4.1. Metrología de atributos estáticos . . . . . . 4.2. Metrología de atributos dinámicos . . . . . 4.3. Implementación del controlador . . . . . . 4.4. Interfaz entre el pendubot y la computadora . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20 20 21 26 27 5. PROTOTIPO EXPERIMENTAL 29 5.1. Subsistema mecánico . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29 5.2. Subsistema eléctrico-electrónico . . . . . . . . . . . . . . . . . . . . . . . . 31 6. RESULTADOS Y DISCUSIÓN 6.1. Parámetros del prototipo experimental . . . . . . . . . . . . . . . . . . . . . 6.2. Ganancias del controlador lineal . . . . . . . . . . . . . . . . . . . . . . . . 6.3. Resultados experimentales . . . . . . . . . . . . . . . . . . . . . . . . . . . V 33 33 35 37 7. CONCLUSIONES 40 BIBLIOGRAFÍA 42 APÉNDICES 45 A. Programas utilizados A.1. Programa para el microcontrolador PIC16F877A . . . . . . . . . . . . . . . A.2. Programa de implementación del controlador en BUILDER C++ . . . . . . . A.3. Programa en MATLAB para cálculo de ganancias del controlador . . . . . . 45 45 48 52 VI ÍNDICE DE CUADROS 4.1. Características del MAX232. . . . . . . . . . . . . . . . . . . . . . . . . . . 28 6.1. 6.2. 6.3. 6.4. Parámetros del pendubot construido. . . Parámetros simplificados del pendubot. Ganancias del controlador lineal. . . . . Ganancias del controlador PI. . . . . . . VII . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35 35 36 36 ÍNDICE DE FIGURAS 2.1. Pendubot. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6 3.1. Dibujo esquematico del Pendubot (Spong y Block, 1995). . . . . . . . . . . . 7 4.1. 4.2. 4.3. 4.4. 4.5. 4.6. 4.7. 4.8. 4.9. Báscula EX 2202. . . . . . . . . . . . . . . . . . . . . . . . . . . . Vernier 531-128. . . . . . . . . . . . . . . . . . . . . . . . . . . . Motor 7K00011 con codificador incremental incluido. . . . . . . . . Codificador incremental óptico S1-1000-250-IE-B-D. . . . . . . . . Sensor de corriente ACS712. . . . . . . . . . . . . . . . . . . . . . Respuesta del sensor de corriente ACS712. . . . . . . . . . . . . . Comportamiento de la corriente con respecto a las cuentas del ADC Computadora TOSHIBA SATELLITE. . . . . . . . . . . . . . . . . Convertidor USB-serial TU-S. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20 21 22 23 24 24 25 26 27 5.1. 5.2. 5.3. 5.4. 5.5. Dimensiones del prototipo experimental. . . . . . Vistas del pendubot. . . . . . . . . . . . . . . . . Diseño del prototipo experimental. . . . . . . . . Diagrama eléctrico del prototipo. . . . . . . . . . Fuente implementada de 5V para en el pendubot. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29 30 30 31 32 6.1. 6.2. 6.3. 6.4. 6.5. 6.6. Prototipo experimental. . . . . . . . . . . . . . . . . . . Pendubot con ambos eslabones hacia abajo. . . . . . . . Pendubot con ambos eslabones hacia arriba. . . . . . . . Resultados experimentales. . . . . . . . . . . . . . . . . Perturbación del segundo eslabón de derecha a izquierda. Prototipo funcionando. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33 34 34 37 39 39 VIII . . . . . . . . . . . . . . . I. INTRODUCCIÓN I.1. Planteamiento del problema Los péndulos invertidos han sido herramientas clásicas en los laboratorios de control desde los años 1950’s, los cuales fueron inicialmente utilizados para ilustrar ideas del control lineal tales como la estabilización de sistemas inestables y para mostrar muchas de las ideas emergentes en el campo del control de sistemas no lineales. Los péndulos subactuádos son sistemas mecánicos que tienen menor cantidad de actuadores que grados de libertad. Los péndulos subactuádos son sistemas físicos no lineales, y en consecuencia se requiere la implementación de técnicas de control no lineal, ademas de ser útiles para la enseñanza de control de sistemas no lineales. Dentro de los sistemas subactuádos más comunes están el péndulo invertido sobre un carro (López-López, 2011), el péndulo de Furuta (Furuta et al., 1992), el péndulo con rueda inercial (Spong et al., 2001) y el pendubot por mencionar algunos. El pendubot es uno de los prototipos de laboratorio más comunes, y al día de hoy, en la Universidad Autónoma de Querétaro (UAQ) se carece de un sistema de esta naturaleza. En vista de esta área de oportunidad detectada, se pretende aplicar los conocimientos obtenidos en la carrera de Ingeniería en Automatización con línea terminal en instrumentación y control en la construcción de un pendubot. I.1.1 Objetivo Construir e instrumentar un pendubot equipado con control de corriente como prototipo experimental para apoyo didáctico en los cursos de control de la Facultad de Ingeniería de la Universidad Autónoma de Querétaro. 1 I.1.2 Hipótesis Es posible construir e instrumentar un Pendubot utilizando un lazo de corriente programado en una computadora con un periodo de muestreo de 0.01[s]. I.2. Justificación Independientemente que el pendubot a construir se pruebe con el controlador lineal por retroalimentación de estado, la principal aplicación de éste prototipo es contar con la plataforma experimental para que los investigadores del área de control de la Facultad de Ingeniería prueben cualquier tipo de controlador que diseñen para dicha plataforma. La segunda aplicación del prototipo es en el área docente, ya que se puede utilizar como herramienta didáctica para explicar algunos conceptos de la teoría de control moderna como lo son la variable de estado, control de sistemas no lineales y el control de sistemas subactuádos. La tercer aplicación es contar con un prototipo de exhibición en ferias industriales y congresos, transmitiendo a la sociedad parte del trabajo universitario en la Facultad de Ingeniería. I.3. Nomenclatura para derivadas con respecto al tiempo Finalmente, la notación compacta para expresar derivadas con respecto al tiempo en este trabajo es: ẏ = dy , dt ÿ = d2 y , dt2 donde y puede ser un escalar o un vector. 2 II. REVISIÓN DE LITERATURA II.1. Estado del arte Un sistema se define como no lineal si sus parámetros y atributos varían de manera no proporcional a la entrada aplicada, por lo cual las ecuaciones que describen su comportamiento tienen carácter no lineal, además de no cumplir con el principio de superposición (McRobie y Thompson, 1990). La mayor parte de los sistemas reales son no lineales dentro de un amplio rango de escalas de tiempo y espacio. Gracias al aumento de la capacidad de procesamiento de los sistemas de computo ha resultado factible simularlos y analizarlos con modelos no lineales que representan los fenómenos físicos de una manera más precisa que los modelos lineales (Sastry, 1999). El pendubot tuvo sus primeras apariciones gracias a Spong y Block (1995). Ellos presentaron el concepto de un robot subactuado de dos eslabones rotacionales llamado pendubot para estudiar los controladores no lineales con este tipo de sistema, además de usarlo como apoyo para la docencia. Alvarez-Gallegos y Gonzalez-Hernandez (1997) trabajaron con el análisis de la dinámica del pendubot en la reconstrucción de atractores basadas en series de tiempo. Sanchez et al. (1998) presentaron un controlador proporcional derivativo (PD) con múltiples entradas y múltiples salidas (MIMO), equipado con un sistema dinámico de conmutación difuso el cual aplicaron para poner en marcha dicho pendubot. Fantoni et al. (1999, 2000) proponen un controlador con un enfoque basado en energía y en las propiedades de la pasividad del pendubot, usando la energía total del sistema como la función de Lyapunov. Tiempo después Begovich et al. (2002) presentaron un enfoque para seguimiento de trayectorias de sistemas no lineales con metodología difusa. Su principal contribución 3 consistió en la aplicación en tiempo real del algoritmo Takagi-Sugeno (T-S) al pendubot. En ese mismo año Zhang y Tarn (2002) presentaron un nuevo controlador híbrido para la estabilización por retroalimentación con resultados experimentales en el pendubot. En el 2003 se desarrollo un controlador bang-bang con un algoritmo genético modificado para aplicar periódicamente un par constante en el sistema, partiendo cuando el sistema tiene sus dos eslabones situados en la posición baja natural, el par constante es aplicado sobre la primera articulación de accionamiento para hacer que los enlaces se muevan hacia arriba por una o más veces hasta lograr finalmente la posición vertical deseada. El pendubot es atrapado por un controlador lineal LQR donde una vez que el sistema entra en la región de atracción del controlador lineal, el pendubot queda atrapado con sus dos eslabones apuntando hacia arriba (Zhao y Yi, 2003). Wang et al. (2004) Presentaron un nuevo método de control adaptivo modo deslizante(ASMC) para un manipulador subactuado llamado pendubot. El ASMC adapta un tipo de estructura jerárquica. En primer lugar el sistema pendubot está dividido en dos subsistemas. Para cada subsistema, se define la primera capa plano de deslizamiento. Usando la ley de Lyapunov, se deriva la ley de control de modo deslizante y se obtiene el derecho de adaptación de los parámetros del controlador. La ley de control puede conducir a sus valores deseados del sistema, y poner en práctica el control anti-disturbio. Hicieron un análisis estable sobre los planos sub-deslizantes en la teoría, y los resultados de la simulación muestran la validez del controlador y sus capacidades de adaptación para todas las clases de perturbaciones externas. En ese mismo mes Li et al. (2004) presentaron un controlador acrobático para un pendubot utilizando un controlador difuso para mantener el primer eslabón oscilante periódicamente mientras el segundo eslabón se mantiene en posición vertical. Para facilitar el diseño del controlador, representaron la dinámica del pendubot con un modelo Takagi-Sugeno difuso. Un trabajo posterior fue el de Buitrago et al. (2006) en el cual implementan la técnica de linealización parcial por realimentación (Slotine y Li, 1991) para levantar el pendubot, y una retroalimentación óptima del estado para atrapar el pendubot en las posiciones media y con los dos eslabones hacia arriba. Así mismo, Shibli (2006) describe la implementación de un controlador adaptativo utilizando sistemas difusos y redes neuronales. Un controlador PD 4 difuso se emplea para controlar los dos enlaces de movimiento desde la posición que cuelga libremente hasta la posición vertical del balanceo. Entonces la red neuronal se utiliza para controlar el péndulo en la posición vertical. Shoji et al. (2010) por su parte controlaron el pendubot basados en el concepto de dinámica cero inestable. En este trabajo, el pendubot se utiliza como una analogía del brazo humano incluyendo un resorte en la unión subactuada del sistema. Uno de los últimos trabajos encontrados es de López Encalada (2011), quien propone desarrollar un algoritmo de control para el pendubot utilizando la capacidad de procesamiento de un FPGA (Field Programmable Gate Array), en la cual se implementan los algoritmos de adquisición de datos, pre-procesado, algoritmo de control, regulación del punto de operación y determinación de la salida del sistema, así como también el módulo de comunicación con una computadora para registrar los valores de las variables. II.2. Pendubot Como puede verse en Fantoni y Lozano (2002), el pendubot, nombre formado por la contracción de las palabras pendulum y robot, es un sistema electromecánico que consiste de dos eslabones rígidos interconectados por uniones o juntas rotacionales. El pendubot tiene la estructura de un robot manipulador de dos grados de libertad de enlaces rígidos, pero no tiene actudor en la segunda articulación, solo esta restringido por el movimiento de la primera articulación debido al acoplamiento que existe entre ellas, tal como se muestra en la Figura 2.1. 5 Figura 2.1: Pendubot. El primer eslabón está actuado por un motor de DC y el segundo eslabón no está actuado, es decir puede moverse libremente y tiene una gran variedad de modos de comportamiento, se muestra como está el acople directamente al eje del motor DC y posee además el alojamiento para el eje del segundo eslabón, el cual se mueve libremente y es controlado por acción del primer eslabón. Para la medición de la posición y velocidad angular de las uniones se emplean codificadores incrementales ópticos (encoders). El diseño de nuevos prototipos para la didáctica de control es importante, por lo cual con este trabajo se pretende construir un Pendubot para la Facultad de Ingeniería de la Universidad Autónoma de Querétaro. El prototipo se pretende probar con la estrategia lineal presentada por Fantoni y Lozano (2002). 6 III. FUNDAMENTACIÓN TEÓRICA III.1. Modelo matemático Las variables y parámetros que describen el sistema físico pendubot (ver Fig. 2.1 ) Fantoni y Lozano (2002): Figura 3.1: Dibujo esquematico del Pendubot (Spong y Block, 1995). Donde : m1 : Masa del eslabón 1; m2 : Masa del eslabón 2; l1 : Longitud del eslabón 1; 7 l2 : Longitud del eslabón 2; lc1 : Distancia del centroide del eslabón 1; lc2 : Distancia del centroide del eslabón 2; I1 : Momento de inercia del eslabón 1; I2 : Momento de inercia del eslabón 2; g: Aceleración de la gravedad; q1 : Ángulo del eslabón 1 respecto a la horizontal X; q2 : Ángulo que forma el eslabón 2 con respecto a la prolongación del eslabón 1; τ1 : Torque aplicado al eslabón 1. Para simplificar la escritura del modelo matemático se definen las siguientes constantes: 2 θ1 = m1 lc1 + m2 l12 + I1 , 2 θ2 = m2 lc2 + I2 , θ3 = m2 l1 lc2 , θ4 = m1 lc1 + m2 l1 , θ5 = m2 lc2 . (3.1) Con el fin de determinar las ecuaciones de movimiento que rigen el sistema se emplea la metodología de Euler-Lagrange (Wells, 1972; Kelly et al., 2005), las cuales son deducidas a partir de la energía cinética y potencial del sistema (Fantoni y Lozano, 2002). La energía cinética del Eslabón 1 es: 1 2 )q̇12 , K1 = (I1 + m1 lc1 2 8 (3.2) La energía cinética del Eslabón 2 es: 1 2 + m2 l12 )q̇12 K2 = (I2 + m2 l1 lc2 cos q2 + m2 lc2 2 1 2 2 + (I2 + m2 l1 lc2 cos q2 + m2 lc2 )q̇1 q̇2 + (I2 + m2 lc2 )q̇22 . 2 (3.3) Con los cinco parámetros definidos en (3.1), la energía cinética total es K = K1 + K2 , 1 1 = (θ1 + θ2 + 2θ3 cos q2 )q̇12 + (θ2 q̇22 ) + (θ2 + θ3 cos q2 )q̇1 q̇2 . 2 2 (3.4) La energía potencial total es P = θ4 g sen q1 + θ5 g sen (q1 + q2 ). La función de Lagrange esta dada por: L = K − P, 1 1 (θ1 + θ2 + 2θ3 cos q2 )q̇12 + (θ2 q̇22 ) + (θ2 + θ3 cos q2 )q̇1 q̇2 = 2 2 − θ4 g sen q1 − θ5 g sen (q1 + q2 ). (3.5) Las correspondientes ecuaciones de movimiento se obtienen utilizando ecuaciones de Euler-Lagrange (Wells, 1972; Kelly et al., 2005).   d ∂L − dt ∂ q̇1   d ∂L − dt ∂ q̇2 ∂L = τ1 , ∂q1 ∂L = 0, ∂q2 (3.6) (3.7) Aplicando las derivadas parciales a (3.5) se tiene :  ∂L ∂ q̇  1 ∂L ∂q  1 ∂L ∂ q̇  2 ∂L ∂q2  = (θ1 + θ2 + 2θ3 cos q2 )q̇1 + (θ2 + θ3 cos q2 )q̇2 , (3.8) = −θ4 g cos q1 − θ5 g cos(q1 + q2 ), (3.9) = θ2 q̇2 + (θ2 + θ3 cos q2 )q̇1 , (3.10) = −θ3 sin q2 q̇12 − θ3 sin(q2 )q̇1 q̇2 − θ5 g cos(q1 + q2 ). (3.11) 9 Derivando (3.8) y (3.10) se tiene que :   d ∂L = (θ1 θ2 + 2θ3 cos q2 )q̈1 + (θ2 + θ3 cos q2 ) − θ3 sin(q2 )q̇22 q̈2 dt ∂ q̇1 (3.12) − 2θ3 sin q2 q̇1 q̇2 , d dt  ∂L ∂ q̇2  = θ2 q̈2 − θ3 sin q2 q̇2 q̇1 + (θ2 + θ3 cos q2 )q̈1 . De (3.6) y (3.7) finalmente se obtiene las ecuaciones de movimiento como: τ1 = (θ1 + θ2 + 2θ3 cos q2 )q̈1 + (θ2 + θ3 cos q2 )q̈2 − θ3 sin q2 q̇22 − 2θ3 sin q2 q̇1 q̇2 (3.13) + θ4 g cos q1 + θ5 g cos(q1 + q2 ), 0 = θ2 q̈2 + (θ2 + θ3 cos q2 )q̈1 + θ3 sin q2 q̇12 + θ5 g cos(q1 + q2 ). (3.14) Las ecuaciones (3.13) y (3.14) se puede reescribir de la forma matricial compacta como: D(q)q̈ + C(q, q̇)q̇ + g(q) = τ, (3.15) Donde:  q=  D(q) =   C(q, q̇) =  q1 q2   ∈ R2 , (3.16) θ1 + θ2 + 2θ3 cos q2 θ2 + θ3 cos q2 θ2 + θ3 cos q2 θ2   ∈ R2×2 , −θ3 sen (q2 )q̇2 −θ3 sen (q2 )q̇2 − θ3 sen (q2 )q̇1 θ3 sen (q2 )q̇1  g(q) =  0 θ4 g cos q1 + θ5 g cos (q1 + q2 ) θ5 g cos (q1 + q2 ) 10    ∈ R2×2 ,  ∈ R2 , (3.17) (3.18) (3.19)  τ = τ1 0   ∈ R2 . (3.20) Despejando q̈ ecuación (3.15) tenemos: q̈ = D(q)−1 [τ − g(q) − C(q, q̇)q̇] . (3.21) De (3.17) obtenemos (3.22)  D(q)−1 = Adj(D(q)T ) = |D(q)|  −θ2 − θ3 cos q2 θ2 −θ2 − θ3 cos q2 θ1 + θ2 + 2θ3 cos q2 θ1 θ2 − θ32 cos2 q2   . (3.22) Retomando la ecuación (3.21) se tiene que:       θ g cos q + θ g cos (q + q ) q̈1 τ 1 5 1 2   = [D(q)−1 ]  1  − [D(q)−1 ]  4  θ5 g cos (q1 + q2 ) q̈2 0    −θ sen (q ) q̇ −θ sen (q ) q̇ − θ sen (q ) q̇ q̈ 3 2 2 3 2 2 3 2 1   1  , (3.23) − [D(q)−1 ]  θ3 sen (q2 )q̇1 0 q̈2       q̈ τ θ g cos q1 + θ5 g cos (q1 + q2 )  1  = [D(q)−1 ]  1  − [D(q)−1 ]  4  q̈2 0 θ5 g cos (q1 + q2 )    −q̇2 −q̇2 − q̇1 q̈   1 , − [D(q)−1 ]θ3 sen(q2 )  q̇1 0 q̈2 (3.24) obteniendo asi: q̈1 = 1 [θ2 θ3 sin q2 (q̇1 + q̇2 )2 + θ3 2 cos q2 sin (q2 )q̇12 − θ2 θ4 g cos q1 + θ2 τ1 θ1 θ2 − θ32 cos2 q2 + θ3 θ5 g cos q2 cos (q1 + q2 )], (3.25) 11 q̈2 = 1 [−θ3 (θ2 + θ3 cos q2 ) sin q2 (q̇1 + q̇2 )2 − (θ1 + θ3 cos q2 )θ3 sin(q2 )q̇22 θ1 θ2 − θ32 cos2 q2 + (θ2 + θ3 cos q2 )(θ4 g cos q1 − τ1 ) − (θ1 + θ3 cos q2 )θ5 g cos(q1 + q2 )]. (3.26) III.2. Linealización del sistema en un punto de operación Se define el estado de la siguiente manera:    y1        y2   =  y=    y3      y4 q1    q̇1  ,  q2   q̇2 (3.27) así el sistema (3.15) en términos del estado (3.27) está dado como:      ẏ h  1   1         ẏ2   h2      = ẏ = h(y, τ1 ) =  =    ẏ3   h3        ẏ4 h4 q̇1    y   2     q̈1   ẏ2  = .    q̇2   y4     q̈2 ẏ4 (3.28) Los puntos de operación del sistema (3.28) se obtienen al hacer:  y2∗   0       ∗     ẏ2   0     ẏ = h(y ∗ , τ1∗ ) =   ∗  =  ,  y4   0      ∗ ẏ4 0 (3.29) reescribiendo (3.14) en términos del estado (3.27) tenemos: 0 = θ2 ẏ4 + (θ2 + θ3 cos y3 )ẏ2 + θ3 y22 sin y3 + θ5 g cos(y1 + y3 ), (3.30) sustituyendo (3.29) en (3.30) se obtiene: θ5 g cos (y1∗ + y3∗ ) = 0, 12 (3.31) cos (y1∗ + y3∗ ) = 0, (3.32) lo cual se consigue con: π y1∗ + y3∗ = (2k + 1) , 2 k ∈ Z. (3.33) Ahora si reescribimos (3.13) en términos del estado (3.27) tenemos: τ1 = (θ1 + θ2 + 2θ3 cos y3 )ẏ2 + (θ2 + θ3 cos y3 )ẏ4 − θ3 y42 sin y3 − 2θ3 y2 y4 sin y3 (3.34) + θ4 g cos y1 + θ5 g cos(y1 + y3 ), sustituyendo (3.31) y (3.29) en (3.34) tenemos: τ1∗ = θ4 g cos y1∗ . (3.35) El punto de operación del pendubot con sus dos eslabones hacia arriba:  π 2       0  ∗  y =  ,  0    0 (3.36) satisface (3.32), (3.31), (3.29), por lo que al sustituir (3.36) en (3.35) tenemos: τ1∗ = 0. (3.37) III.3. Controlador para atrapar el pendubot con ambos eslabones arriba El sistema linealizado en el punto de operación (3.36) está dado como: ż = Az + Bw, donde: 13 (3.38) z = y − y∗, (3.39) w = τ1 − τ1∗ . (3.40) h1 = y2 , (3.41) 1 [θ2 θ3 (y2 + y4 )2 sen y3 + θ32 y22 cos y3 sin y3 − θ2 θ4 g cos y1 θ1 θ2 − θ32 cos2 y3 (3.42) partiendo de (3.28): h2 = + θ3 θ5 g cos y3 cos (y1 + y3 ) + θ2 τ1 ], (3.43) h3 = y4 , h4 = 1 [−(y2 − y4 )2 θ3 (θ2 + θ3 cos y3 ) sin y3 − y22 (θ1 + θ3 cos y3 )θ3 sin y3 θ1 θ2 − θ32 cos2 y3 + (θ2 + θ3 cos y3 )(θ4 g cos y1 − τ1 ) − (θ1 + θ3 cos y3 )θ5 g cos(y1 + y3 )], (3.44) donde las matrices de la linealización (3.38) por la aproximación del primer término de la serie de Taylor esta dada como:  A= ∂h(y, τ1 ) ∂y y∗ τ1∗    =    ∂h1 ∂y1 ∂h1 ∂y2 ∂h1 ∂y3 ∂h1 ∂y4 ∂h2 ∂y1 ∂h2 ∂y2 ∂h2 ∂y3 ∂h2 ∂y4 ∂h3 ∂y1 ∂h3 ∂y2 ∂h3 ∂y3 ∂h3 ∂y4 ∂h4 ∂y1 ∂h4 ∂y2 ∂h4 ∂y3 ∂h4 ∂y4  B= ∂h(y, τ1 ) ∂τ1 y∗ τ1∗ 14    =    ∂h1 ∂τ1 ∂h2 ∂τ1 ∂h3 ∂τ1 ∂h4 ∂τ1         , (3.45) y∗ τ1∗         . y∗ τ1∗ (3.46) de (3.41) tenemos que: ∂y2 ∂h1 = = 0, ∂y1 ∂y1 (3.47) ∂h1 ∂y2 = = 1, ∂y2 ∂y2 (3.48) ∂y2 ∂h1 = = 0, ∂y3 ∂y3 (3.49) ∂y2 ∂h1 = = 0, ∂y4 ∂y4 (3.50) de (3.42) tenemos que: ∂h2 [θ2 θ4 g sin y1 − θ3 θ5 g cos y3 sin(y1 + y3 )] [θ1 θ2 − θ32 cos2 y3 ] , = 2 ∂y1 (θ1 θ2 − θ32 cos2 y3 ) [θ2 θ4 g sin y1 − θ3 θ5 g cos y3 sin(y1 + y3 )] (θ1 θ2 − θ32 cos2 y3 ) = y∗ , τ1∗ (θ2 θ4 − θ3 θ5 )g , θ1 θ2 − θ32 = (3.51) ∂h2 [θ2 θ3 (y2 + y4 )2 sin y3 + θ32 (2y2 cos y3 ) sin y3 ] [θ1 θ2 − θ32 cos2 y3 ] , = 2 ∂y2 (θ1 θ2 − θ32 cos2 y3 ) = [θ2 θ3 (y2 + y4 )2 sin y3 + θ32 (2y2 cos y3 ) sin y3 ] (θ1 θ2 − θ32 cos2 y3 ) y∗ , τ1∗ = 0, (3.52) ∂h2 = [θ2 θ3 (y2 + y4 )2 cos y3 + (θ32 y22 )(cos2 y3 − sin2 y3 ) + (2θ32 sin y3 ) ∂y3 1 +θ3 θ5 g − (cos y3 sin (y1 + y3 ) − cos (y1 + y3 ) sin y3 )] 2, (θ1 θ2 − θ32 cos2 y3 ) [(θ2 θ3 (y2 + y4 )2 ) cos y3 + (θ32 y22 )(cos2 y3 − sin2 y3 ) + (2θ32 sin y3 )] = , y∗ (θ1 θ2 − θ32 cos2 y3 ) τ∗ 1 (θ1 θ2 − θ32 )(−θ3 θ5 g) , = (θ1 θ2 − θ32 )2 (−θ3 θ5 g) = , (θ1 θ2 − θ32 ) (3.53) 15 ∂h2 ∂h2 = = 0, ∂y4 ∂y4 (3.54) ∂y4 ∂h3 = = 0, ∂y1 ∂y1 (3.55) ∂h3 ∂y4 = = 0, ∂y2 ∂y2 (3.56) ∂h3 ∂y4 = = 0, ∂y3 ∂y3 (3.57) ∂y4 ∂h3 = = 1, ∂y4 ∂y4 (3.58) de (3.43) tenemos que: de (3.44) tenemos: ∂h4 = ∂y1 = (θ2 + θ3 cos y3 )(θ4 g cos y1 − τ1 ) − (θ1 + θ3 cos y3 )θ5 g cos(y1 + y3 ) θ1 θ2 − θ32 cos2 y3 (θ1 + θ3 )θ5 g − θ4 g(θ2 + θ3 ) , θ1 θ2 − θ32 cos2 y3 ∂h4 = ∂y2 , (3.59) −2(y2 − y4 )θ3 (θ2 + θ3 cos y3 ) sin y3 − 2y2 (θ1 + θ3 cos y3 ) θ1 θ2 − θ32 cos2 y3 = 0, ∂h4 ∂y3 y∗ τ1∗ y∗ , τ1∗ (3.60) y∗ = gθ5 (θ1 + θ3 ) (θ1 θ2 − θ32 cos2 y3 ) , (θ1 θ2 − θ32 cos2 y3 ) (θ1 θ2 − θ32 cos2 y3 ) = gθ5 (θ1 + θ3 ) , (θ1 θ2 − θ32 cos2 y3 ) τ1∗ ∂h4 = 0. ∂y4 16 (3.61) (3.62) Así las matrices de la linealización (3.38) son:  0   (θ2 θ4 −θ3 θ5 )g  ∂h(y, τ1 ) θ1 θ2 −θ32 A= =  y∗ ∂y  τ1∗ 0  (θ5 g(θ1 +θ3 )−θ4 g(θ2 +θ3 ) θ1 θ2 −θ32  B= ∂h(y, τ1 ) ∂τ1 y∗ τ1∗    =    1 0 0 5g − θ1θθ32θ−θ 2 3 0 0 0 (θ5 g(θ1 +θ3 ) θ1 θ2 −θ32 0 θ2 θ1 θ2 −θ32 0 −θ2 −θ3 θ1 θ2 −θ32 0    0  ,  1   0 (3.63)     .    (3.64) Para verificar la controlabilidad del sistema linealizado (3.38), es necesario encontrar las siguientes expresiones partiendo de (3.63) y (3.64):     2 A =    4 −θ3 θ5 ) − g(θθ22θ−θ 1 θ2 0 θ5 gθ3 θ32 −θ1 θ2 0 0 4 −θ3 θ5 ) − g(θθ22θ−θ 1 θ2 3 0 θ5 gθ3 θ32 −θ1 θ2 (gθ4 (θ2 +θ3 )−gθ5 (θ1 +θ3 )) θ32 −θ1 θ2 0 −gθ5 (θ1 +θ3 ) θ32 −θ1 θ2 0 0 (gθ4 (θ2 +θ3 )−gθ5 (θ1 +θ3 )) θ32 −θ1 θ2 0 −(θ1 +θ3 )gθ5 θ32 −θ1 θ2 3  a311 a312 a313 a314   3  a21 a322 a323 a324 3 A =  3  a31 a332 a333 a334  a341 a342 a343 a344 a311 = 0, a312 = − (θ2 θ4 − θ3 θ5 ) , θ32 − θ1 θ2 a313 = 0, 17      , (3.65)        ,    (3.66) (3.67) (3.68) (3.69) a314 = a321 = gθ3 θ5 , − θ1 θ2 (3.70) θ32 g 2 (θ22 θ42 − θ2 θ3 θ4 θ5 + θ32 θ4 θ5 − θ1 θ3 θ52 ) 2 (θ32 − θ1 θ2 ) , (3.71) a322 = 0, a323 = a332 = −g 2 θ3 θ5 (θ1 θ5 + θ2 θ4 ) 2 (θ32 − θ1 θ2 ) , (3.73) a324 = 0, (3.74) a331 = 0, (3.75) (gθ4 (θ2 + θ3 ) − gθ5 (θ1 + θ3 )) , θ32 − θ1 θ2 a334 = a341 = (3.72) (3.76) a333 = 0, (3.77) −(θ1 + θ3 )gθ5 , θ32 − θ1 θ2 (3.78) g 2 (θ1 θ5 + θ2 θ4 ) (θ1 θ5 − θ2 θ4 − θ3 θ4 + θ3 θ5 ) 2 (θ32 − θ1 θ2 ) , (3.79) a342 = 0, a343 = (3.80) g 2 θ5 (θ5 θ12 + θ5 θ1 θ3 + θ4 θ32 + θ2 θ4 θ3 ) 2 (θ32 − θ1 θ2 ) , (3.81) a344 = 0,     AB =     0 1 0 (θ2 θ4 −θ3 θ5 )g θ1 θ2 −θ32 0 5g − θ1θθ32θ−θ 2 3 0 0 0 (θ5 g(θ1 +θ3 )−θ4 g(θ2 +θ3 ) θ1 θ2 −θ32 0 (θ5 g(θ1 +θ3 ) θ1 θ2 −θ32 (3.82) 0 18    0    1   0 0 θ2 θ1 θ2 −θ32 0 −θ2 −θ3 θ1 θ2 −θ32         =       θ2 − θ2 −θ 1 θ2 3 0 θ2 +θ3 θ32 −θ1 θ2 0      , (3.83)         2 A B=     0 g (θ4 θ22 +θ5 θ32 )    2  (θ32 −θ1 θ2 ) ,  0   g (θ4 θ22 +θ4 θ2 θ3 +θ5 θ32 +θ1 θ5 θ3 ) − 2 (θ32 −θ1 θ2 ) g (θ4 θ22 +θ5 θ32 )  2  (θ32 −θ1 θ2 )   0  A3 B =  g(θ θ2 +θ θ θ +θ θ2 +θ θ θ )  − 42 423 53 153 2  (θ32 −θ1 θ2 )  0 (3.84)      ,    (3.85) de lo anterior, podemos usar (3.64), (3.83), (3.84) y (3.85) como columnas para construir la matriz de controlabilidad (3.86): Pc = [B | AB | A2 B | A3 B], det(Pc ) = θ52 g 2 θ32 4 (θ32 − θ1 θ2 ) 6= 0, (3.86) (3.87) dado que la ecuación (3.87) es diferente de cero, se concluye que el sistema es controlable y es posible ubicar los valores propios de la matriz de lazo cerrado [A − BK] donde se requiera usando el controlador lineal por retroalimentacion de estado siguiente: w = −Kz, (3.88) τ1 = −K(y − y ∗ ). (3.89) 19 IV. MATERIALES Y MÉTODOS IV.1. Metrología de atributos estáticos Los atributos estáticos del prototipo experimental sólo se miden al principio ya que se consideran invariantes en el tiempo. Masa, m [kg]: La masa de los componentes de interés del prototipo experimental se obtiene con una báscula comercial modelo EX 2202 de la marca OHAUS, con un rango de 0 a 2200 gramos fuerza y una resolución de 0.1 gramos fuerza (ver Figura 4.1). Las medicio- Figura 4.1: Báscula EX 2202. nes de peso se realizan en la ciudad de Querétaro, Qro., por triplicado, calculando el promedio con el siguiente procedimiento: 1. Poner a cero la báscula; 2. Pesar el componente de interés; 20 3. Convertir los gramos fuerza a kilogramos fuerza con el factor de conversión 1 kg ( 1000 fgf ); N 4. Convertir los kilogramos fuerza a Newtons con el factor de conversión ( 9.81 ); 1 kgf 5. Encontrar la masa en kilogramos del componente de interés con: m= W , g (4.1) donde m es la masa del componente en kilogramos (kg), W es el peso del componente en Newtons (N ) y g es la aceleración de la gravedad en metros sobre segundo al cuadrado (g = 9.81 sm2 ). Distancia, d [m]: Las distancias de los componentes de interés del prototipo experimental se obtienen con un calibrador tipo vernier comercial modelo 531-128 de la marca MITUTOYO, con un rango de 0 a 150 milímetros con una resolución de 0.02 milímetros (ver Figura 4.2). Las mediciones de longitud se toman a una temperatura ambiente de 298 ◦ K, por Figura 4.2: Vernier 531-128. triplicado, calculando el promedio y aplicando el siguiente procedimiento: 1. Ajustar el calibrador para tocar el contorno del componente en cuestión. 2. Tomar la lectura con la escala de resolución de 0.02 milímetros. 1m 3. Convertir los milímetros a metros con el factor de conversión ( 1000 ). mm IV.2. Metrología de atributos dinámicos Posición angular del primer eslabón, q1 [rad]: 21 La posición angular del primer eslabón se obtiene con un codificador incremental óptico comercial incluido en el motor modelo 7K00011, marca Tohoko Ricoh, con una resolución de 400 cuentas por revolución (ver Figura 4.3). La actualización de la posición angular del primer eslabón en el controlador se realiza cada 0.01 [s]. Figura 4.3: Motor 7K00011 con codificador incremental incluido. Velocidad angular del primer eslabón, q̇1 [ rad ]: s La velocidad angular del primer eslabón se calcula de manera numérica basándose en la siguiente expresión: q˙1 (k) = q1 (k) − q1 (k − 1) , ∆t (4.2) donde q˙1 (k) es la velocidad angular del primer eslabón en el instante actual, q1 (k) es la posición angular del primer eslabón en el instante actual, q1 (k − 1) es la posición angular del primer eslabón en el instante anterior y ∆t es el periodo de tiempo entre las muestras de posición (∆t = 0.01 [s]). Posición angular del segundo eslabón, q2 [rad]: La posición angular del segundo eslabón se obtiene con un codificador incremental óptico comercial modelo S1-1000-250-IE-B-D, marca US DIGITAL, con una resolución 1000 cuentas por revolución (ver Figura 4.4). La actualización de la posición angular del segundo eslabón en el controlador se realiza cada 0.01 [s]. Velocidad angular del segundo eslabón, q˙2 [ rad ]: s 22 Figura 4.4: Codificador incremental óptico S1-1000-250-IE-B-D. La velocidad angular del segundo eslabón se calcula de manera numérica basándose en la siguiente ecuación: q˙2 (k) = q2 (k) − q2 (k − 1) , ∆t (4.3) donde q˙2 (k) es la velocidad angular del segundo eslabón en el instante actual, q2 (k) es la posición angular del segundo eslabón en el instante actual, q2 (k − 1) es la posición angular del segundo eslabón en el instante anterior y ∆t es el periodo de tiempo entre las muestras de posición (∆t = 0.01 [s]). Corriente eléctrica del motor, ic [A]: Para medir la corriente se utilizo el sensor que se muestra en la Figura 4.5 23 Figura 4.5: Sensor de corriente ACS712. El sensor de efecto Hall proporciona la medición de la corriente que circula por el motor encargado de propulsar el primer eslabón. Da una señal analógica de voltaje que varía linealmente con la corriente (ver Figura 4.6). El dispositivo requiere de una alimentación de 5 VDC para el pin marcado como VCC. Figura 4.6: Respuesta del sensor de corriente ACS712. La manera que el ADC del microcontrolador PIC18F877A interpreta la corriente es : ic2 − ic1 (Vout − Vout1 ), Vout2 − Vout1 5 − (−5) (Vout − 1.6), ic − (−5) = 3.4 − 1.6 ic − ic1 = 24 (4.4) (4.5) 10 (Vout − 1.6), 1.8 (4.6) 10 (Vout − 1.6) − 5, 1.8 (4.7) ic + 5 = ic = icmax = 10 (3.23 − 1.6) − 5, 1.8 (4.8) icmax = 4.05[A], (4.9) icmin = −4.1[A]. (4.10) mediante la siguiente gráfica se puede ver el comportamiento lineal que tiene las cuentas con respecto al corriente del motor ic . Figura 4.7: Comportamiento de la corriente con respecto a las cuentas del ADC Así un Vout de 3.23[V] corresponde a 1023 cuentas teniendo una corriente de 4.05[A], Vout de 2.48[V] corresponde a 511 cuentas teniendo una corriente de 0[A] y Vout de 1.75[V] corresponde a 0 cuentas teniendo una corriente de -4.05[A]. 25 Antes de meter la señal de corriente al microcontrolador PIC16F877A usando como tarjeta adquisidora de datos se maneja un filtro RC pasabajas de primer orden (ver Figura 5.4) para evitar que en la señal de corriente (debido a que el sensor de corriente tiene tiempos de respuestas del orden de los microsegundos) se observe la forma de la señal de modulación de ancho de pulso que pilotea el puente H. El valor de la resistencia es R = 22000[Ω] y el valor del capacitor es C = 0.22[µF ]. Lo anterior para tener una frecuencia de corte ωc = ], fc = 32.9Hz. 206.6[ rad s IV.3. Implementación del controlador La ley de control es implementada en una computadora portátil modelo A305S6858B, marca TOSHIBA, que se presenta en la Figura 4.8, la cual cuenta con las siguientes características: Figura 4.8: Computadora TOSHIBA SATELLITE. Software: • Sistema operativo W7; • MATLAB versión 7.1; • Borland C++ Builder versión 6; • MPLAB versión 7.43; • PCW C compiler versión 4.11. Hardware: • Procesador Intel Centrino 2.0 GHz; • Memoria RAM de 4 GB; 26 • Disco duro de 320 GB; • Tres puertos USB2. IV.4. Interfaz entre el pendubot y la computadora La interfaz serial a 115,200 bits por segundo entre el sistema de control y el Pendubot se realiza a través de un microcontrolador modelo PIC16F877A de la marca Microchip trabajando a 20 MHz, quien también se encarga de leer el encoder del Primer eslabón y el encoder del Segundo eslabón y proporcionar los registros de posición de ambos encoders, además de fijar el periodo de muestreo Ts = 0.01 segundos (para mayores detalles ver Figura 5.4 y los programas del Apéndice A. La etapa de potencia consta de una compuerta DM74LS08, la cual permite al microcontrolador enviar una señal del tipo de modulación de ancho de pulso (PWM) al puente H modelo L298N tal y como se muestra en la Figura 5.4. Un convertidor USB-Serial modelo TU-S, marca TRENDNET, permite tener conexión serial con la computadora (ver Figura 4.9). Figura 4.9: Convertidor USB-serial TU-S. Un circuito integrado MAX232 convierte los niveles CMOS del microcontrolador a los niveles del RS-232 de la computadora (ver Figura 5.4 y Tabla 4.1). 27 Tabla 4.1: Características del MAX232. Características: Valor: Velocidad máxima 120,000 bps Voltaje de operación 5V Corriente nominal 0.008 A 28 V. PROTOTIPO EXPERIMENTAL V.1. Subsistema mecánico En la Figura 5.1 se muestran las dimensiones del pendubot. Figura 5.1: Dimensiones del prototipo experimental. En la Figura 5.2 se muestra la vista frontal, vista lateral y la vista superior del pendubot. 29 Figura 5.2: Vistas del pendubot. En la Figura 5.3 se puede observar en perspectiva el prototipo experimental. Figura 5.3: Diseño del prototipo experimental. 30 V.2. Subsistema eléctrico-electrónico En la Figura5.4 se muestra el diagrama de conexiones de los componentes que forman la interfaz entre el pendubot y la computadora. Figura 5.4: Diagrama eléctrico del prototipo. En la Figura 5.5 se muestra la fuente que se implemento para poder proporcionar la 31 entrada de 5V al sistema eléctrico-electrónico 5.4. Figura 5.5: Fuente implementada de 5V para en el pendubot. 32 VI. RESULTADOS Y DISCUSIÓN VI.1. Parámetros del prototipo experimental En la Figura 6.1 se muestra el prototipo real construido en este trabajo mientras en la Tabla 6.1 y 6.2 se muestran los valores numéricos de los parámetros del prototipo. Cabe mencionar que se despreció el momento de inercia del eje ya que sólo se tomó en cuenta el momento de inercia del péndulo. Figura 6.1: Prototipo experimental. 33 En la Figura 6.2 se muestra el pendubot con ambos eslabones hacia bajo y la conexión entre el pendubot y la computadora. Figura 6.2: Pendubot con ambos eslabones hacia abajo. En la Figura 6.3 se muestra el pendubot con ambos eslabones hacia arriba. Figura 6.3: Pendubot con ambos eslabones hacia arriba. 34 Tabla 6.1: Parámetros del pendubot construido. Símbolo: Descripción: Valor: Unidades: masa del motor 0.7600 Kg masa del primer eslabón 0.0103 Kg l1 longitud del primer eslabón 0.1130 m lc1 distancia del centroide de la primera eslabón 0.0846 m I1 momento de inercia del primer eslabón 0.000216626 masa del segundo eslabón 0.1100 Kg l2 longitud de la segunda eslabón 0.1280 m lc2 distancia del centroide de la segunda eslabón 0.0640 m I2 momento de inercia del segundo eslabón 0.00015434 g aceleración de la gravedad 9.81 km constate de par del motor 0.069 mmotor msol1 msol2 Kg.m2 Kg.m2 m s2 N.m A Tabla 6.2: Parámetros simplificados del pendubot. Parámetro físico: Valor: Unidades: θ1 0.0018 Kg.m2 θ2 0.0006049 Kg.m2 θ3 0.00079552 Kg.m2 θ4 0.0142 Kg.m2 θ5 0.0070 Kg.m2 VI.2. Ganancias del controlador lineal En la tabla 6.3 se muestran las ganancias del controlador lineal usadas en el experimento y que permiten que los cuatro valores propios de la matriz [A − BK] tengan parte real negativa, quedando λ1 = −36.2265, λ2 = −12.6232 + 10.6192j, λ3 = −12.6232 − 10.6192j y λ4 = −3.0347. Las ganancias de la tabla 6.3 se encuentran usando la función acker de MTLAB, ver apéndice A.3. 35 Tabla 6.3: Ganancias del controlador lineal. Ganancia: Valor: k1 -1.100 k2 -0.185 k3 -1.050 k4 -0.100 Debido a que el controlador (3.89) esta en términos de par aplicado al primer eslabón, se recurrió a un lazo esclavo de corriente proporcional+integral (PI) encargado de aplicar el voltaje necesario al motor para que fluya la corriente necesaria para producir el par que pide el controlador (3.89). La corriente deseada para el lazo de corriente se calcula con la siguiente expresión: id = τ1 . km (6.1) El error del lazo de corriente se calcula como: ei = id − ic . El lazo PI de corriente esta dado de la siguiente manea: Z t uf = kpi ei (t) + kii ei (τ )dτ, (6.2) (6.3) 0 Tabla 6.4: Ganancias del controlador PI. Ganancia: Valor: kpi 0.03 kii 130.0 Finalmente, es importante resaltar que el controlador PI se introdujo de manera numérica en builder C++ usando integración numérica rectangular y el algoritmo del controlador se ejecuta al igual que el controlador por retroalimentación de estado cada ∆t = Ts = 0.01[s] (ver detalles en el apéndice A.2). 36 VI.3. Resultados experimentales En la Figura 6.4 se muestra el resultado del experimento aplicado al pendubot construido. El primer grupo de variables mostradas en la Figura 6.4 corresponden al subsistema mecánico y llevan el siguiente orden: posición del primer eslabón, velocidad del primer eslabón, posición del segundo eslabón, velocidad del segundo eslabón y el par solicitado por el controlador. Figura 6.4: Resultados experimentales. Las ultimas variables que se muestran en la Figura 6.4 pertenecen al subsistema 37 eléctrico y son la corriente deseada, la corriente aplicada al motor y el voltaje necesario aplicado al motor para alcanzar la corriente deseada. Se puede observar en las primeras cuatro gráficas de la Figura 6.4 que durante el intervalo de tiempo de t = 0[s] a t = 1.3[s] el microcontrolador y el programa se encuentran corriendo y el pendubot se encuentra en su posición natural estable (los dos eslabones apuntan hacia abajo). Posteriormente en el intervalo de tiempo t > 1.3[s] a t = 2.85[s] se mueve manualmente el pendubot a la posición donde ambos eslabones apuntan hacia arriba. Es importante observar que durante este periodo de tiempo la potencia se ha mantenido apagada, de tal manera que en la sexta gráfica de la Figura 6.4 la corriente ic se mantiene en 0[A] independientemente que el par solicitado por el controlador τ1 6= 0 (ver quinta gráfica de la Figura 6.4) y la corriente deseada id 6= 0 (ver sexta gráfica de la Figura 6.4). Observese en la séptima gráfica de la Figura 6.4 que la fuente de potencia se prende hasta el tiempo t = 4[s]. A partir de este momento, el pendubot es capaz de mantenerse por si solo en el punto de operación deseado ya que la corriente ic sigue a la corriente id (ver sexta gráfica de la Figura 6.4) proporcionando a su vez el par demandado por el controlador (3.89). Posteriormente, en los tiempos t = 12.5[s], t = 14.1[s] y t = 16[s] son aplicadas perturbaciones manuales al segundo eslabón para validar que el controlador es capaz de mantener al pendubot en el punto de operación y sus cercanías pese a las perturbaciones externas aplicadas. En la tercera gráfica de la Figura 6.4 que muestra la posición del segundo eslabón q2 [rad] se observa que las dos primeras perturbaciones son de derecha a izquierda (ver tambien la Figura 6.5) mientras que la ultima perturbación es de izquierda a derecha. Finalmente, en las Figuras 6.5 y 6.6 se muestra el prototipo completo funcionando 38 Figura 6.5: Perturbación del segundo eslabón de derecha a izquierda. Figura 6.6: Prototipo funcionando. 39 VII. CONCLUSIONES Se construyó el prototipo experimental pendubot con materiales comerciales comunes, se utilizo un microcontrolador PIC16F877A de bajo costo y una computadora portátil para implementar el controlador por retroalimentacion de estado. Se probó el adecuado funcionamiento del sistema linealizando el pendubot en la posición en la cual sus dos eslabones apuntan hacia arriba y fue posible verificar la correcta operación del lazo de corriente implementado con un sensor Efecto Hall ACS712 de la marca Allegro. Tanto el controlador maestro por retroalimentacion de estado como el controlador proporcional + integral esclavo de corriente fueron programados en una computadora portátil funcionando el sistema pendubot completo con un periodo de muestreo de 0.01[s]. El motor D06D304E de la marca Hitachi utilizado inicialmente, cuenta con codificador incremental óptico de 240 ppr, situación que no nos permitió controlar el pendubot debido al ruido de cuantización. Por tal razón fue necesario aumentar la resolución del codificador incremental óptico con un motor modelo 7k00011 de la marca Tohoko Ricoh que cuenta con un codificador incremental óptico de 400 ppr para poder controlar el pendubot. Para un buen desempeño experimental fue necesario que la mesa estuviese completamente plana y bien apoyada para que el prototipo quedara bien soportado y evitar tanto vibraciones innecesarias como descalibraciones en las posiciones angulares de los eslabones debidas a la desalineación entre la base del prototipo y la mesa. Debido al termino exitoso de este trabajo el grupo de control ya cuenta con una plataforma experimental pendubot para que los investigadores de esta área de la Facultad de Ingeniería de la Universidad Autónoma de Querétaro prueben cualquier tipo de controlador que diseñen para dicha plataforma. Por otro lado, en el aspecto docente, ya se puede utilizar el pendubot como herra40 mienta didáctica para explicar algunos conceptos de la teoría de control moderna como lo son la variable de estado, control de sistemas no lineales y el control de sistemas subactuádos. Finalmente, los profesores de la Facultad de Ingeniería tienen un prototipo experimental pendubot para exhibición en ferias industriales y congresos, transmitiendo a la sociedad parte del trabajo universitario en nuestra Facultad. 41 BIBLIOGRAFÍA Alvarez-Gallegos, J. and HG Gonzalez-Hernandez. 1997. Analysis of the dynamics of an underactuated robot: the forced pendubot. Decision and Control, 1997., Proceedings of the 36th IEEE Conference on. 2 IEEE :1494–1499. Begovich, O., E.N. Sanchez and M. Maldonado. 2002. Takagi-Sugeno fuzzy scheme for real-time trajectory tracking of an underactuated robot. Control Systems Technology, IEEE Transactions on 10(1):14–20. Buitrago, D.G., G.A. Holguín and A.E. Mejía. 2006. Técnicas híbridas de control aplicadas al pendubot. Scientia et Technica 1(30). Fantoni, I. and R. Lozano. 2002. Non-linear control for underactuated mechanical systems. Springer, Great Britain. Fantoni, I., R. Lozano and M.W. Spong. 1999. Passivity based control of the Pendubot. American Control Conference, 1999. Proceedings of the 1999. 1 IEEE :268–272. Fantoni, I., R. Lozano and M.W. Spong. 2000. Energy based control of the pendubot. Automatic Control, IEEE Transactions on 45(4):725–729. Furuta, K., M. Yamakita and S. Kobayashi. 1992. Swing-up control of inverted pendulum using pseudo-state feedback. Proceedings of the Institution of Mechanical Engineers, Part I: Journal of Systems and Control Engineering 206(4):263–269. Kelly, R., V. Santibañez and A. Loria. 2005. Control of robot manipulators in joint space. Springer, London. Li, W., K. Tanaka and H.O. Wang. 2004. Acrobatic control of a pendubot. Fuzzy Systems, IEEE Transactions on 12(4):549–554. 42 López Encalada, R.A. 2011. Control del Módulo Pendubot utilizando una FPGA PhD thesis QUITO/EPN/2011. López-López, C.U. 2011. Construcción de un prototipo experimental del péndulo invertido sobre un carro para el área de control. Tesis de licenciatura Universidad Autónoma de Querétaro. McRobie, A. and M. Thompson. 1990. Chaos, catastrophes and engineering: Applying chaos theory to engineering may seem odd. New Scientist 1720. Sanchez, E., L.A. Nuno, Y.C. Hsu and G. Chen. 1998. Fuzzy PD scheme for underactuated robot swing-up control. Fuzzy Systems Proceedings, 1998. IEEE World Congress on Computational Intelligence., The 1998 IEEE International Conference on. 1 IEEE :302– 306. Sastry, S. 1999. Nonlinear systems. Springer, New York, USA. Shibli, M. 2006. Direct adaptive control for underactuated mechatronic systems using fuzzy systems and neural networks: A pendubot case. Electrical and Computer Engineering, 2006. CCECE’06. Canadian Conference on. IEEE :1490–1493. Shoji, T., S. Nakaura and M. Sampei. 2010. Throwing motion control of the springed pendubot via unstable zero dynamics. Control Applications (CCA), 2010 IEEE International Conference on. IEEE :1602–1607. Slotine, J.-J. E. and W. Li. 1991. Applied nonlinear control. Prentice-Hall, Englewood Cliffs, New Jersey. Spong, M. W., P. Corke and R. Lozano. 2001. Nonlinear control of the reaction wheel pendulum. Automatica 37:1845–1851. Spong, M.W. and D.J. Block. 1995. The Pendubot: A mechatronic system for control research and education. Decision and Control, 1995., Proceedings of the 34th IEEE Conference on. 1 IEEE :555–556. 43 Wang, W., J.Q. Yi, D.B. Zhao and X.J. Liu. 2004. Adaptive sliding mode controller for an underactuated manipulator. Machine Learning and Cybernetics, 2004. Proceedings of 2004 International Conference on. 2 IEEE :882–887. Wells, D. A. 1972. Teoría y problemas de dinámica de Lagrange. McGraw-Hill, Colombia. Zhang, M. and T.J. Tarn. 2002. Hybrid control of the Pendubot. Mechatronics, IEEE/ASME Transactions on 7(1):79–86. Zhao, D. and J. Yi. 2003. Swing up Pendubot with a GA-tuned Bang-bang Controller. Robotics, Intelligent Systems and Signal Processing, 2003. Proceedings. 2003 IEEE International Conference on. 2 IEEE :752–757. 44 A. PROGRAMAS UTILIZADOS A.1. Programa para el microcontrolador PIC16F877A // Programa para comunicación serial entre PC y el Pendubot // Con el PIC16F877A y el compilador PCWH V3.43 #include<16f877A.h> #device adc=10 //manejar adc de 10 bits #include<stdlib.h> #include<math.h> #fuses HS,NOWDT,PUT,NOBROWNOUT,NOLVP,NOWRT,NOPROTECT,NOCPD #use delay(clock=20000000) //Base de tiempo para retardos (frecuencia del Xtal) #use rs232(baud=115200,XMIT=PIN_C6,RCV=PIN_C7,BITS=8,PARITY=N)//Config. P. Serie //direcciones de los puertos y algunos registros #byte OPTION= 0x81 #byte TMR0 = 0x01 #byte CVRCON= 0x9D #byte ADCON1= 0x9F #byte PORTA = 0x05 #byte PORTB = 0x06 #byte PORTC = 0x07 #byte PORTD = 0x08 #byte PORTE = 0x09 #bit PC0 = 0x07.0 #bit PD0 = 0x08.0 #bit PD1 = 0x08.1 #bit PD2 = 0x08.2 #bit PD3 = 0x08.3 #bit PD4 = 0x08.4 #bit PD5 = 0x08.5 //------------Declaracion de variables------------// int16 inter,cuenta1,cuenta2,conv; int8 cuentaH1,cuentaL1,cuentaH2,cuentaL2,puerto,AB,AB_1,BC,BC_1,aux; int8 convH,convL,tope,dato,pwm; //------------Rutina de interrupción------------// #int_rb void rb_isr() { 45 puerto=PORTB; AB=((puerto)&(0x30))>>4; aux=AB^AB_1; if(aux!=0) if(aux!=3) if(((AB_1<<1)^AB)&(0x02)) { cuenta1--; } else { cuenta1++; } AB_1=AB; BC=((puerto)&(0xC0))>>6; aux=BC^BC_1; if(aux!=0) if(aux!=3) if(((BC_1<<1)^BC)&(0x02)) { cuenta2--; } else { cuenta2++; } BC_1=BC; } //------------Programa principal------------// void main(void) { set_tris_a(0b11111111); set_tris_b(0b11111111); set_tris_c(0b10000000); //Configurando pines para comunicacion serial y uso de pwm set_tris_d(0b00001100); set_tris_e(0b11111111); PC0=0; OPTION=0x07; //prescaler asignado a timer0 con relación 1:256 setup_ccp1(CCP_PWM); //configurando pwm setup_timer_2(T2_DIV_BY_16,255,1);//T=(1/clock)*4*t2div*(period+1) PORTC=0; 46 TMR0=0; cuenta1=0;//conidcion inicial para el brazo cuenta2=0; set_pwm1_duty(0); AB=0; AB_1=0; BC=0; BC_1=0; if(PD2) tope=196; else tope=98; setup_adc(ADC_CLOCK_INTERNAL); //Configurando el convertidor ADC con reloj interno setup_adc_ports(A_ANALOG_RA3_RA2_REF); set_adc_channel(0); delay_us(15); enable_interrupts(global); enable_interrupts(int_rb); PC0=1; while(TRUE) { conv=read_adc(); inter=(conv)&(0xFF00); convH=inter>>8; convL=(conv)&(0x00FF); inter=(cuenta1)&(0xFF00); cuentaH1=inter>>8; cuentaL1=(cuenta1)&(0x00FF); inter=(cuenta2)&(0xFF00); cuentaH2=inter>>8; cuentaL2=(cuenta2)&(0x00FF); PD4=1; //inicia envío putc(0xAA); //mandando dato de reconocimieno al puerto serial putc(cuentaH1); //mandando cuenta al puerto serial putc(cuentaL1); putc(cuentaH2); putc(cuentaL2); if(PD3) { putc(convH); putc(convL); 47 } PD4=0; //termina envio PD5=1; //inicia espera del timer do { if(kbhit()) { dato=getc();//obteniendo el dato if(dato>=128) { PD0=0; PD1=1; dato=dato-128; pwm=dato<<1; } else { PD0=1; PD1=0; pwm=dato<<1; } set_pwm1_duty(pwm); //actualizando el valor del pwm } } while(TMR0<tope); PD5=0; //cada cuenta vale (4/FXtal)*256 seg //termina espera del timer TMR0=0; } } //cierre del while infinito //cierre del main A.2. Programa de implementación del controlador en BUILDER C++ //Programa para controlar el pendubot con los dos eslabones arriba //--------------------------------------------------------------------------#include <vcl.h> #pragma hdrstop #include "Main.h" #include <math.h> #include <stdio.h> #include <share.h> #include <conio.h> #include <dos.h> //--------------------------------------------------------------------------#pragma package(smart_init) #pragma link "CSPIN" #pragma resource "*.dfm" 48 //--------------------------------------------------------------------------#define Ts 0.01 #define pprq1 400.0 #define pprq2 1000.0 #define pi_ 3.141593 #define km 0.069 #define kpi 0.03 #define kii 130.0 #define VM 10.0 #define IM 4.05 //-------------------ganancias del controlador------------------------------#define k1 -1.10 #define k2 -0.185 #define k3 -1.05 #define k4 -0.100 FILE *ptrMonit; TMainForm *MainForm; unsigned char flagcom=0,flagfile=0,signo_sal,pwm; //de 8 bits unsigned short int pos1,pos2,corr; //de 16 bits float uf,pwmf,t=0,escq1=pi_/(2.0*pprq1),escq2=pi_/(2.0*pprq2); float id,ic,ei,propi,intei,mi=2.0*IM/1023.0,iTs=1/Ts,escs=127/VM; float q1=-pi_/2,q2=0,q1p=0,q2p=0,q1_1=-pi_/2,q2_1=0,tau1,tau1M=IM*km,q1d=pi_/2,z1; //--------------------------------------------------------------------------void ProcessByte(BYTE byte) { if(flagcom!=0) flagcom++; if((byte==0xAA)&&(flagcom==0)) { pos1=0; pos2=0; corr=0; flagcom=1; } if(flagcom==2) { pos1=byte; pos1=pos1<<8; } if(flagcom==3) { pos1=pos1+byte-pprq1; 49 } if(flagcom==4) { pos2=byte; pos2=pos2<<8; } if(flagcom==5) { pos2=pos2+byte; } if(flagcom==6) { corr=byte; corr=corr<<8; } if(flagcom==7) { q1_1=q1; q2_1=q2; corr=corr+byte; ic=mi*corr-IM; q1=(signed short int)pos1; q1=escq1*q1; //posicion eslabon 1 q2=(signed short int)pos2; q2=escq2*q2; //posicion eslabon 2 z1=q1-q1d; q1p=(q1-q1_1)*iTs; //velocidad eslabon 1 q2p=(q2-q2_1)*iTs; //velocidad eslabon 2 //--------------controlador maestro por retro de estado----------------tau1=-k1*z1 -k2*q1p -k3*q2 -k4*q2p; if(tau1>tau1M) tau1=tau1M; if(tau1<-tau1M) tau1=-tau1M; //--------------------lazo PI esclavo de corriente---------------------id=tau1/km; //corriente necesaria para alcanzar tau1 ei=id-ic; propi=kpi*ei; if((intei<VM)&&(intei>(-VM))) intei=intei+kii*Ts*ei; else { if(intei>=VM) intei=0.95*VM; if(intei<=(-VM)) 50 intei=-0.95*VM; } uf=propi+intei; if(uf>VM) uf=VM; if(uf<-VM) uf=-VM; pwmf=uf; //--------------exhibicion de valores en pantalla----------------------MainForm->Edit1->Text = FloatToStr (q1); MainForm->Edit2->Text = FloatToStr (q1p); MainForm->Edit3->Text = FloatToStr (q2); MainForm->Edit4->Text = FloatToStr (q2p); MainForm->Edit5->Text = FloatToStr (t); MainForm->Edit6->Text = FloatToStr (id); MainForm->Edit7->Text = FloatToStr (ic); MainForm->Edit8->Text = FloatToStr (uf); //---------escalamiento de salida a 7 bits más 1 bit de signo----------pwmf=escs*pwmf; pwm=(unsigned char)abs(pwmf); if(pwmf<0) pwm=pwm+128; //mandando primer byte MainForm->Acknowledge(); MainForm->send_byte(pwm); /*abrir/crear un archivo*/ if(flagfile==0) if((ptrMonit=fopen("MONIT.TXT", "w"))==NULL){} flagfile=1; /*escribir algunos datos en el archivo*/ fprintf(ptrMonit,"%3.3f\t%3.3f\t%3.3f\t%3.3f\t%3.3f\t%3.3f\t%3.3f\t%3.3f\t%3.3f\n" ,t,q1,q2,q1p,q2p,id,ic,uf,tau1); t=t+Ts; flagcom=0; }//cierre de ultimo if de falgcom }//cierre de función ProcessByte //--------------------------------------------------------------------------__fastcall TMainForm::TMainForm(TComponent* Owner) : TForm(Owner), SerialPort(1, ProcessByte), fAcknowledge(true) { if (SerialPort.IsReady() != TRUE) MessageBox(NULL, "Problemas con el puerto", "Error", MB_OK); } //--------------------------------------------------------------------------- 51 void TMainForm::send_byte(unsigned char byte_sal) { if (fAcknowledge == false) return; SerialPort.WriteByte(byte_sal); fAcknowledge = false; } //--------------------------------------------------------------------------void TMainForm::Acknowledge() { fAcknowledge = true; } void __fastcall TMainForm::Button1Click(TObject *Sender) { /* close the file */ fclose(ptrMonit); Close(); } //--------------------------------------------------------------------------- A.3. Programa en MATLAB para cálculo de ganancias del controlador clc clear g=9.81; %Calculos primer eslabon mmotor=2*0.38 mrotor=mmotor/2 rrotor=0.032 Irotor=(mrotor*rrotor^2)/2 mcople1=0.03065 rcople1=0.0127 Icople1=(mcople1*rcople1^2)/2 msol1=0.01031 l1=0.113 asol1=0.0127 menc=0.02034 mcarenc=menc/2; m1=msol1+mcarenc; lc1=(msol1*(l1/2)+mcarenc*l1)/m1 Isol1=(l1^2+asol1^2)*msol1/12 I1=Isol1+msol1*(lc1-(l1/2))^2+Irotor+Icople1 %Calculos segundo eslabon 52 mrotenc=menc/2; rrotenc=0.003; Irotenc=(mrotenc*rrotenc^2)/2 mcople2=0.03254 rcople2=0.0127 Icople2=(mcople2*rcople2^2)/2 msol2=0.110 l2=0.128 asol2=0.0127 lc2=l2/2 Isol2=(l2^2+asol2^2)*msol2/12 I2=Irotenc+Icople2+Isol2 m2=msol2; teta1=m1*lc1*lc1+m2*l1*l1+I1 teta2=m2*lc2*lc2+I2 teta3=m2*l1*lc2 teta4=m1*lc1+m2*l1 teta5=m2*lc2 den=teta1*teta2-teta3^2; a21=g*(teta2*teta4-teta3*teta5)/den; a23=-teta3*teta5*g/den; a41=g*(teta5*(teta1+teta3)-teta4*(teta2+teta3))/den; a43=g*teta5*(teta1+teta3)/den; b2=teta2/den; b4=-(teta2+teta3)/den; A=[0 1 0 0; a21 0 a23 0; 0 0 0 1; a41 0 a43 0] B=[0; b2; 0; b4] Pc=ctrb(A,B) det(Pc) rank(Pc) 53 %%{ p=[-36.2265;-12.6232+10.6192i;-12.6232-10.6192i;-3.0347] K=acker(A,B,p) %} %{ K=[-1.10 -0.185 -1.05 -0.100]; P=eig(A-B*K) %} 54