Control y simulación del Péndulo de Furuta

Equation Chapter 1 Section 1 Trabajo Fin de Grado Grado en Ingeniería de las Tecnologías Industriales Control y Simulación del Péndulo de Furuta Autor: Carlos Regalo Núñez Tutor: David Muñoz De La Peña Sequedo Dep. Ingeniería de Sistemas y Automática Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2016 iii Trabajo Fin de Grado Grado en Ingeniería de las Tecnologías Industriales Control y Simulación del Péndulo de Furuta Autor: Carlos Regalo Núñez Tutor: David Muñoz De La Peña Sequedo Profesor titular Dep. de Ingeniería de Sistemas y Automática Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2016 v Trabajo Fin de Grado: Control y Simulación del Péndulo de Furuta Autor: Carlos Regalo Núñez Tutor: David Muñoz De La Peña Sequedo El tribunal nombrado para juzgar el Trabajo arriba indicado, compuesto por los siguientes miembros: Presidente: Vocales: Secretario: Acuerdan otorgarle la calificación de: Sevilla, 2016 El Secretario del Tribunal vii A mi familia A mis maestros A mis amigos ix Agradecimientos La realización del trabajo final de carrera es algo muy importante para todos los alumnos, ya que es la primera vez en la que nos encontramos ante un trabajo completamente individual y al que tenemos que enfrentarnos prácticamente solos, usando los conocimientos que hemos ido adquiriendo a lo largo de tantos años en nuestra escuela de ingenieros. Aunque, tal y como he dicho, tenemos que enfrentarnos solos, sin la ayuda, las indicaciones y las ideas de David, tutor de este trabajo, nunca hubiera sido posible realizar dicha labor. Por otro lado el apoyo de colegas y familiares también se hace imprescindible para continuar con la ardua tarea en los momentos de dificultad. Por todo ello escribo estas líneas para mostrarles toda mi gratitud. Carlos Regalo Núñez Sevilla, 2016 xvii Índice de Tablas Tabla 2-1. Parámetros de simulación. 14 Tabla 3-1. Parámetros identificados. 68 Tabla 5-1.Identificación a un paso. 81 Tabla 5-2. Identificación a N pasos. 83 xix Índice de Figuras Figura 1-1. Aplicaciones de la ingeniería de control. 1 Figura 1-2. La ingeniería de control en industrias tradicionales. 2 Figura 1-3. Péndulo de Furuta. 2 Figura 1-5. Paseo en segway. 3 Figura 1-4. Péndulo de QUBE-Servo. 3 Figura 2-1. Sistemas de coordenadas definido. 6 Figura 2-2. Modelo de masa-muelle-amortiguador. 8 Figura 2-3. Modelo en Simulink®. 9 Figura 2-4. Contenido del subsistema. 9 Figura 2-5. Simulación del modelo masa-muelle-amortiguador. 9 Figura 2-6. Modelo de Furuta en Simulink. 10 Figura 2-7. Subsistema modelo de Furuta en Simulink (I). 11 Figura 2-8. Subsistema modelo de Furuta en Simulink (II). 12 Figura 2-9. Cálculo del parámetro Alfa. 12 Figura 2-10. Cálculo del parámetro Beta. 12 Figura 2-11. Cálculo del parámetro Gamma. 13 Figura 2-12. Cálculo del parámetro Jp. 13 Figura 2-13. Parámetros del ModeloA21 enmascarado. 13 Figura 2-14. Pestaña Parmeters & Dialog. 14 Figura 2-15. Ángulo Theta. 15 Figura 2-16. Ángulo Phy. 15 Figura 2-17. V-Realm Builder 2.0 16 Figura 2-18. Péndulo de Furuta en VRealm. 17 Figura 2-19. Diagrama de árbol de un modelo en VRealm. 17 Figura 2-20. Campo rotación en VRealm. 18 Figura 2-21. Bloques VR Sink y VR Signal Expander conectados. 18 Figura 2-22. Ventana VR Sink 19 Figura 2-23. Entorno virtual del péndulo en 3D. 20 Figura 2-24. Bloque VR Signal Expander. 20 Figura 2-25. Balance de Control PD. 23 Figura 2-26. Diagrama de bode del filtro HP del balance de control. 24 Figura 2-27. Error filtrado y sin filtrar. 24 Figura 2-28. Tensión y ángulos de salida en balance de control PD (I). 25 Figura 2-29. Tensión y ángulos de salida en balance de control PD (II). 26 Figura 2-30. Tensión de entrada θ límite. 26 Figura 2-31. Ángulos theta y phy cuando θ límite. 27 Figura 2-32. Linealización de función algebraica ( I ). 28 Figura 2-33. Linealización de función algebraica (II). 29 Figura 2-34. Control LQR. 30 Figura 2-35. Control LQR modelo en Simulink® 32 Figura 2-36. Parámetros de la ganacia primera del control LQR. 33 Figura 2-37. Parámetros de la ganancia segunda del control LQR. 33 Figura 2-38. Contenido del bloque Estado X. 34 Figura 2-39. Ángulo theta en control LQR. 35 Figura 2-40. Ángulo phy en el control LQR. 35 Figura 2-41. Entrada al sistema proporcionada por el control LQR. 36 Figura 2-42. Ángulo phy en control LQR para 𝜃0 = 0.2. 36 Figura 2-43. Ángulo Theta en control LQR para 𝜃0 = 0.2. 37 Figura 2-44. Ángulo theta para distintos valores de los parámetros de ponderación. 38 Figura 2-45. Ángulo phy para distintos valores de los parámetros de ponderación. 38 Figura 2-46. Control en energía y control en posición. 39 Figura 2-47. Cálculo de la energía mecánica del péndulo. 40 Figura 2-48. Subsistema de cálculo de la energía. 41 Figura 2-49. Modelo swing-up en Simulink. 42 Figura 2-50. Control LQR con interruptor. 43 Figura 2-51. Bloque interruptor. 44 Figura 2-52. Ángulo phy control swing-up. 44 Figura 2-53. Ángulo theta control swing-up. 45 Figura 2-54. Energía del péndulo. 45 Figura 2-55. Ángulo phy en swing-up con 𝜃0 = 𝜋/2. 46 Figura 2-56. Ángulo theta en swing-up con 𝜃0 = 𝜋/2. 46 Figura 2-57. Energía en swing-up con 𝜃0 = 𝜋/2. 47 Figura 3-1. Péndulo Qube-Servo. 50 Figura 3-2. Bloque del encoder del péndulo QUBE-Servo. 50 Figura 3-3. Bloque de actuación del péndulo QUBE-Servo. 50 Figura 3-4. Bloque imprescindible para usar dispositivos Quarc. 51 Figura 3-5. Parámetros del aparato de Quarc. 51 Figura 3-6. Compilación del péndulo de QUBE-Servo (I). 52 Figura 3-7. Compilación del péndulo de QUBE-Servo (II). 52 Figura 3-8. Tensión de entrada prueba 1. 53 xxi Figura 3-9. Ángulo del brazo del péndulo ante la entrada aleatoria. 54 Figura 3-10. Ángulo del péndulo ante la entrada aleatoria. 54 Figura 3-11. Tensión de entrada prueba 2. 55 Figura 3-12. Ángulo del brazo del péndulo ante la entrada sinusoidal. 55 Figura 3-13. Ángulo del péndulo ante la entrada sinusoidal. 56 Figura 3-14. Diagrama de flujo (I). 57 Figura 3-15. Diagrama de flujo (II). 58 Figura 3-16. Diagrama de flujo (III). 59 Figura 3-17. Diagrama de flujo (IV). 59 Figura 3-18. Diagrama de flujo (V). 60 Figura 3-19. Uso de splines (I). 60 Figura 3-20. Uso de splines (II). 61 Figura 3-21. Comparación entre QUBE-Servo y modelo A21 frente a entrada sinusoidal. 62 Figura 3-22. Comparación entre QUBE-Servo y modelo A21 frente a entrada aleatoria. 62 Figura 3-23. Modelo en Simulink del péndulo que se cargará desde el programa (I). 63 Figura 3-24. Modelo en Simulink del péndulo que se cargará desde el programa (II). 64 Figura 3-25. Tensión cargada VS deseada. 64 Figura 3-26. Creación de la entrada aleatoria. 65 Figura 3-27. Diagrama de flujo (VI). 66 Figura 3-28. Interpolación de las variables del espacio de estados cada „paso‟ segundos. 66 Figura 3-29. Diagrama de flujo (VII). 67 Figura 3-30. Diagrama de flujo (VIII). 68 Figura 3-31. Respuesta a entrada aleatoria con paso 1 segundo (I). 69 Figura 3-32. Respuesta a entrada aleatoria con paso 1 segundo (II). 69 Figura 3-33. Respuesta a entrada aleatoria con paso 0.2 segundos (I). 70 Figura 3-34. Respuesta a entrada aleatoria con paso de 0.2 segundos (II). 70 Figura 3-35. Respuesta a entrada sinusoidal con paso de 1 segundo (I). 71 Figura 3-36. Respuesta a entrada sinusoidal con paso de 1 segundo (II). 71 Figura 3-37. Respuesta a entrada sinusoidal con paso de 0.2 segundos (I). 72 Figura 3-38. Respuesta a entrada sinusoidal con paso 0.2 segundos (II). 72 Figura 3-39. Matrices A y B proporcionadas por el fabricante. 73 Figura 3-40. Ganancia óptima para control LQR. 75 Figura 3-41. Ángulo theta con control LQR en modelo matemático con los parámetros identificados. 76 Figura 3-42. Ángulo phy con control LQR en modelo matemático con los parámetros identificados. 76 Figura 3-43. Control LQR en el QUBE-Servo. 77 Figura 3-44. Prueba en péndulo real. 77 xxiii Notación A* Conjugado c.t.p. En casi todos los puntos c.q.d. Como queríamos demostrar ∎ Como queríamos demostrar e.o.c. En cualquier otro caso e número e IRe Parte real IIm Parte imaginaria sen Función seno tg Función tangente arctg Función arco tangente sen Función seno sinxy Función seno de x elevado a y cosxy Función coseno de x elevado a y Sa Función sampling sgn Función signo rect Función rectángulo Sinc Función sinc ∂y ∂x x◦ Derivada parcial de y respecto Notación de grado, x grados. Pr(A) Probabilidad del suceso A SNR Signal-to-noise ratio MSE Minimum square error : Tal que < Menor o igual > Mayor o igual \ Backslash ⇔ Si y sólo si 1 INTRODUCCIÓN l avance de la tecnología se muestra imparable ante nuestro ojos en los últimos tiempos, podemos ver impresoras que crean objetos tridimensionales a la vez que podemos ver robots humanoides cada vez más perfeccionados o los famosos drones constantemente en los telediarios, entre otras muchas creaciones fascinantes que eran impensables años atrás. Dentro de toda esta tecnología, se encuentra una disciplina de la ingeniería que ha tenido mucho que ver en la realización de estas técnicas, se trata de la ingeniería de control. Es a través de la ingeniería de control que somos capaces de dar el par adecuado en las articulaciones de los robots para que estos realicen los movimientos que esperamos, es a través de esta disciplina relativamente joven que controlamos el empuje de las aspas de los drones para obtener la posición adecuada o a gracias a ella somos capaces de controlar la temperatura del material en una impresora 3D. Figura 1-1. Aplicaciones de la ingeniería de control. Además de las aplicaciones anteriormente mencionadas, el control automático se encuentra presente en elementos aparentemente menos sofisticados, quizás porque estamos más acostumbrados a ellos, como los climatizadores de los vehículos o industrias como la química o petroquímica. E La mente que se abre a una nueva idea nunca vuelve a su tamaño original. - Albert Einstein - Diseño, Simulación y Control del Péndulo de Furuta 8 ∆𝜃 =−𝛼2𝜃 2sin 𝜃cos 𝜃+ 𝛽+𝑠𝑖𝑛2𝜃 𝜑 2sin 𝜃cos 𝜃+ 2𝛼𝜃 𝜑 sin 𝜃𝑐𝑜𝑠2𝜃+𝛽sin 𝜃+𝑠𝑖𝑛3𝜃−𝛾𝑢𝛼cos 𝜃−𝑐𝑝𝜃 ∆𝜑 =𝛼𝜃 2sin 𝜃−𝛼𝜑 2sin 𝜃𝑐𝑜𝑠2𝜃−2𝜃 𝜑 sin 𝜃cos 𝜃−𝛼sin 𝜃cos 𝜃+𝛾𝑢−𝑐𝑎𝜑 Donde 𝑐𝑝 y 𝑐𝑎 son los parámetros correspondientes a los términos de fricción viscosa del péndulo del brazo y del motor respectivamente. Estos dos últimos sistemas de ecuaciones diferenciales son las ecuaciones de movimientos del péndulo sin y considerando la fricción, y son los modelos que se implementarán en Simulink en el próximo punto. 2.1.2 Creación del modelo en Simulink Simulink es una herramienta muy potente y sencilla para resolver ecuaciones diferenciales, a continuación se va a explicar brevemente como introducir una ecuación diferencial en ella. Imaginemos que tenemos la siguiente ecuación diferencial 𝑚𝑥 +𝑐𝑥 +𝑘𝑥=𝑓(𝑡) Que corresponde a un modelo de masa-muelle-amortiguador donde se aplica una fuerza externa. Figura 2-2. Modelo de masa-muelle-amortiguador. El primer paso es normalizar la ecuación, es decir, hacer que el coeficiente de la derivada de mayor orden sea uno. 𝑥 +𝑐𝑚𝑥 +𝑘 𝑚𝑥=1 𝑚𝑓(𝑡) Luego despejamos la derivada de mayor orden 𝑥 =1 𝑚𝑓 𝑡 −𝑐𝑚𝑥 −𝑘 𝑚𝑥 Como la ecuación diferencial es de orden dos, necesitamos dos bloques integradores en los cuales se introducen las condiciones iniciales 𝑥0=𝑥(0) y 𝑥 0=𝑥 (0). En la figura 2-3 se puede ver el modelo en Simulink, en la figura 2-4 se puede ver el contenido que hay dentro del subsistema creado, crear un subsistema es útil principalmente para ahorrar espacio y que se pueda ver más claramente el modelo que realizamos, para ello solo tenemos que seleccionar la parte del modelo que queremos meter dentro del subsistema y (en el caso de MatlabR2015a) seleccionar Diagram > Subsystem & Model Reference > Create Subsystem from Selection. 9 Control y Simulación del Péndulo de Furuta Figura 2-3. Modelo en Simulink®. Figura 2-4. Contenido del subsistema. Si damos valores para los parámetros m, c y k desde el Command Window, por ejemplo, >> m=2; k=1; c=1; Y con 𝑥0=𝑥 0 = 0 y 𝑥 0=𝑥 0 = 0, simulando obtenemos la respuesta de la figura 2-5. Figura 2-5. Simulación del modelo masa-muelle-amortiguador. Diseño, Simulación y Control del Péndulo de Furuta 10 Procediendo de la misma forma, a continuación se va a crear en Simulink el modelo del péndulo de Furuta que se obtuvo en el apartado 2.1.1., considerando los parámetros de fricción. Este modelo es, ∆𝜃 =−𝛼2𝜃 2sin 𝜃cos 𝜃+ 𝛽+𝑠𝑖𝑛2𝜃 𝜑 2sin 𝜃cos 𝜃+ 2𝛼𝜃 𝜑 sin 𝜃𝑐𝑜𝑠2𝜃+𝛽sin 𝜃+𝑠𝑖𝑛3𝜃−𝛾𝑢𝛼cos 𝜃−𝑐𝑝𝜃 ∆𝜑 =𝛼𝜃 2sin 𝜃−𝛼𝜑 2sin 𝜃𝑐𝑜𝑠2𝜃−2𝜃 𝜑 sin 𝜃cos 𝜃−𝛼sin 𝜃cos 𝜃+𝛾𝑢−𝑐𝑎𝜑 Con, ∆=𝛽+𝑠𝑖𝑛2𝜃−𝛼2𝑐𝑜𝑠2𝜃 Como se explicó anteriormente, el primer paso para introducir las ecuaciones en Simulink es normalizarlas, es decir, dividir lo que tenemos a la derecha de ambas expresiones entre Δ. El modelo, tras crear un subsistema que además enmascaramos, queda como se puede ver en la figura 2-6. Figura 2-6. Modelo de Furuta en Simulink. Lo que encontramos dentro del subsistema, para acceder a ello tenemos que pulsar botón derecho sobre el modelo > Mask > Look Under Mask ya que el subsistema está enmascarado, es lo que se puede apreciar desde la figura 2-7 hasta la figura 2-12. Aunque pueda parecer complejo, lo que se ha hecho ha sido exactamente lo mismo que para el caso de la masa-muelle-amortiguador pero aplicándolo a dos ecuaciones diferenciales, que no son lineales como se puede ver y además dependen la una de la otra. Es interesante mencionar lo siguiente del subsistema:  En la entrada del modelo se ha colocado un bloque de saturación, ya que se tiene en cuenta que la tensión que podemos introducir al QUBE-Servo es de entre -5 y 5 voltios.  Las condiciones iniciales, en velocidad y posición, tanto de φ como de θ se introducen en los integradores, las condiciones iniciales que se han puesto son 𝜃0= 0, 𝜑0= 0 , 𝜃 0= 0 𝑦 𝜑 0= 0.  En el modelo en Simulink se usan etiquetas Goto y From para una mayor claridad en el modelo.  Por otro lado, los parámetros del sistema, r, l, m, Km, Cp, Ca, g y Ja, se introducen desde el exterior, para tener una mayor facilidad a la hora de cambiar sus valores, algo que nos será muy útil posteriormente para realizar la identificación de los parámetros del sistema. 11 Control y Simulación del Péndulo de Furuta Figura 2-7. Subsistema modelo de Furuta en Simulink (I). Diseño, Simulación y Control del Péndulo de Furuta 12 Figura 2-8. Subsistema modelo de Furuta en Simulink (II). Donde los subsistemas que calculan Alfa, Beta, Gamma y Jp son los que se pueden ver en las figuras 2-9, 2- 10, 2-11 y 2-12. Figura 2-9. Cálculo del parámetro Alfa. Figura 2-10. Cálculo del parámetro Beta. 13 Control y Simulación del Péndulo de Furuta Figura 2-11. Cálculo del parámetro Gamma. Figura 2-12. Cálculo del parámetro Jp. Antes se ha comentado que el subsistema está enmascarado, ¿en qué consiste dicho enmascaramiento? Crear una máscara permite que el subsistema se convierta en un bloque como cualquier otro de Simulink permitiéndonos introducir los valores como si fuera un bloque cualquiera, también permite que se haga una descripción del bloque e incluso que en el bloque pueda aparecer un dibujo o esquema. En la Figura 2-13 podemos ver la ventana que se obtiene si se pulsa con el botón izquierdo sobre el subsistema enmascarado. Figura 2-13. Parámetros del ModeloA21 enmascarado. Diseño, Simulación y Control del Péndulo de Furuta 14 Para crear el enmascaramiento del subsistema hay que hacer lo siguiente, una vez que tenemos el modelo y creamos el subsistema, pulsamos con el botón derecho sobre el modelo y seleccionamos Mask > Create Mask, entonces aparece una ventana con distintas pestañas. Las pestañas más importantes son la pestaña Documentation donde podemos dar una descripción del bloque y la pestaña Parameters & Dialog donde se pueden introducir los parámetros que después nos pide el subsistema al clicar dos veces sobre él con el botón izquierdo. Figura 2-14. Pestaña Parmeters & Dialog. Si queremos introducir algún valor nuevo solo tenemos que pulsar edit. Los parámetros que se aprecian en la tabla 2-1 serán los valores que usaremos para todas las simulaciones que se realizarán hasta el apartado 3 del proyecto. Tabla 2-1. Parámetros de simulación. PARÁMETRO VALOR Masa, m 0.024 kg Longitud del brazo, r 0.085 m Mitad de la longitud del péndulo, l 0.0645m Ganancia del motor, Km 0.036 V/(rad/s) Gravedad, g 9.8 m/𝑠2 Momento de inercia brazo y motor, Ja 0 Kg 𝑚2 Fricción viscosa del péndulo, cp 0.5 Fricción viscosa del brazo del motor, ca 0.5 15 Control y Simulación del Péndulo de Furuta Si además de los parámetros anteriores, aplicamos una tensión de 0 voltios al motor y cambiamos la condición inicial del ángulo del péndulo para que este no parta desde una posición completamente vertical, sino 𝜃0= 0.1, obtenemos una respuesta del péndulo como se puede ver en las figuras 2-15 y 2-16. Figura 2-15. Ángulo Theta. Como se puede apreciar el ángulo del péndulo va desde 0,1 radianes, que es la condición inicial hasta aproximadamente π radianes, que sería el péndulo colgando hacia abajo. Figura 2-16. Ángulo Phy. En este caso he puesto que 𝜑0= 6𝜋, aunque podría haber sido cualquiera. Las gráficas anteriores, como se puede apreciar, se han representado en Matlab y no en Simulink, para ello tenemos que conectar el bloque simout en la señal que queremos representar, con Save format: Array, y luego en el Workspace escribir lo siguiente, Diseño, Simulación y Control del Péndulo de Furuta 16 >> plot(tout,simout);grid; >> xlabel('Tiempo');ylabel('Ángulo del brazo, Phy'); Como se puede apreciar, visualizar el movimiento del péndulo a partir de las gráficas anteriores puede ser complicado, por ello en el siguiente apartado se va a explicar cómo construir un modelo en 3D del péndulo de Furuta que nos permita observar con gran detalle los movimientos del péndulo. 2.1.3 Creación del péndulo de Furuta en 3D con V Realm Matlab cuenta con un toolbox que permite diseñar modelos en 3D que se pueden colocar en Simulink y moverse en función de las señales que le llegan, esta herramienta se llama VRealm. Para acceder a VRealm hemos de irnos donde tenemos instalado nuestro Matlab, meternos en la carpeta de los toolbox y buscar sl3d a continuación nos metemos en la carpeta vrealm y dentro de esta hay que abrir la carpeta program, y una vez dentro abrimos el programa vrbuild2. Una vez abierto el programa debemos ver una ventana como la que se muestra en la figura 2-17. Figura 2-17. V-Realm Builder 2.0 Podemos ir realizando nuestro modelo añadiendo bloques en forma de cubos, cilindros o esferas entre otros. Cuando un nuevo bloque va solidario o pertenece a un bloque anterior este se introduce en el children del anterior en un diagrama de árbol que se va creando. El péndulo que se ha diseñado es el que se presenta en la figura 2-18, está constituido por una base en forma de cubo de color rojo, del que sale un enganche negro que conecta la base al brazo, al final del brazo podemos encontrar otro enganche, negro también, que conecta al brazo con el péndulo. 17 Control y Simulación del Péndulo de Furuta Figura 2-18. Péndulo de Furuta en VRealm. El diagrama de árbol que se ha ido creando para realizar el péndulo es el que se puede apreciar en la figura 2- 19, de cada elemento o bloque hay dos partes diferentes que se pueden observar en el árbol. Figura 2-19. Diagrama de árbol de un modelo en VRealm. Diseño, Simulación y Control del Péndulo de Furuta 24 Figura 2-26. Diagrama de bode del filtro HP del balance de control. Figura 2-27. Error filtrado y sin filtrar. 2.2.2. Resultados del balance de control y rango de validez En este subapartado vamos a ver los resultados que proporciona el balance de control PD ante diversas 25 Control y Simulación del Péndulo de Furuta situaciones. Simulación del balance de control con condiciones iniciales 𝜽𝟎=𝟎.𝟏,𝜽 𝟎=𝟎,𝝋𝟎=𝟎 𝒚 𝝋 𝟎=𝟎 con referencia de φ=0 Tras poner tres To Workspace, uno a la entrada llamado „entrada‟y los otros dos en phy y theta llamados „physalida‟ y „thetasalida‟ respectivamente y ejecutando el código siguiente, obtenemos el resultado que se puede apreciar en la figura 2-28. >>subplot(3,1,1); >>plot(entrada.time,entrada.signals.values,'b'); >>grid >>xlabel('tiempo');ylabel('tension'); >>subplot(3,1,2); >>plot(thetasalida.time,thetasalida.signals.values,'b'); >>grid >>xlabel('tiempo');ylabel('theta'); >>subplot(3,1,3); >>plot(physalida.time,physalida.signals.values,'b'); >>grid >>xlabel('tiempo');ylabel('phy'); Figura 2-28. Tensión y ángulos de salida en balance de control PD (I). Diseño, Simulación y Control del Péndulo de Furuta 26 Simulación del balanace de control con condiciones iniciales 𝜽𝟎=𝟎,𝜽 𝟎=𝟎,𝝋𝟎=𝟎 𝒚 𝝋 𝟎=𝟎 con referencia de φ=pi/2 En este caso, ejecutando el mismo código que en caso anterior tras simular, obtenemos el resultado que se puede apreciar en la figura 2-29. Figura 2-29. Tensión y ángulos de salida en balance de control PD (II). Rango de validez entorno al punto de funcionamiento Como hemos podido ver en los apartados anteriores, el balance de control parece responder bien cuando estamos próximos a θ=0, ahora vamos a ver a partir de que valores deja de funcionar el balance de control. Experimentalmente se puede comprobar cómo es capaz de controlar el péndulo cuando θ pertenece a [-0.57, 0.57] radianes, es decir, θ pertenece a [-32.6, 32,6] grados. En este caso límite obtenemos el resultado que se aprecia en la figura 2-30 y 2-31. Figura 2-30. Tensión de entrada θ límite. 27 Control y Simulación del Péndulo de Furuta Figura 2-31. Ángulos theta y phy cuando θ límite. Podemos concluir que los resultados son aceptables para un amplio rango de validez entorno al punto de funcionamiento. 2.3. Control LQR En el apartado anterior se controló el sistema entorno al punto de funcionamiento a través de un balance de control PD, en este apartado se va a emplear un tipo de control óptimo, el control LQR, que nos proporcionará un término proporcional que nos permitirá controlar el péndulo. Para usar esta estrategia de control previamente tendremos que linealizar y rescribir en espacio de estados las ecuaciones de movimiento del sistema. 2.3.1. Linealización de ecuaciones diferenciales Para poder realizar el control LQR previamente tenemos que linealizar nuestro sistema para pasarlo al espacio de estados, en este apartado se intentará explicar detalladamente la linealización de ecuaciones diferenciales. Antes de linealizar ecuaciones diferenciales veremos como linealizar una ecuación algebraica y se comentará su significado. Sea la función no lineal 𝑦 𝑡 =𝑡2, suponiendo que queremos tener una aproximación lineal a dicha función en 𝑡𝑜= 2, la ecuación que rige dicha función lineal es la siguiente, 𝑦(𝑡)𝐿𝐼𝑁=𝑦 𝑡𝑜 + 𝜕 𝜕𝑡𝑦 𝑡 𝑡=𝑡𝑜(𝑡−𝑡𝑜) 𝑦(𝑡)𝐿𝐼𝑁= 4 + 4(𝑡−2) Diseño, Simulación y Control del Péndulo de Furuta 28 Figura 2-32. Linealización de función algebraica ( I ). Se introduce el término de variable incremental, 𝛼 𝑡 =𝑡−𝑡𝑜. Como se puede ver en la figura 2-32 para que la aproximación lineal sea válida hemos de estar entorno a 𝑡𝑜, es decir, la varible incremental debe estar próxima a cero. Además de modelo de variables incrementales a veces también se le puede llamar modelo de pequeña señal. Si tenemos más variables entorno a las que linealizar el procedimiento es idéntico, sea la función 𝑓 𝑥,𝑦 = 𝑥𝑦 se quiere linealizar entorno a 𝑥𝑜= 0e 𝑦𝑜= 0. Se definen las variables incrementales 𝛼 𝑡 =𝑥 𝑡 −𝑥𝑜 𝛽 𝑡 =𝑦 𝑡 −𝑦𝑜 La linealización responde a la expresión de Taylor, 𝑓(𝑥,𝑦)𝐿𝐼𝑁=𝑓 𝑥𝑜,𝑦𝑜 + 𝜕 𝜕𝑥𝑓 𝑥,𝑦 𝑥𝑜,𝑦𝑜𝛼 𝑡 + 𝜕 𝜕𝑦𝑓 𝑥,𝑦 𝑥𝑜,𝑦𝑜𝛽 𝑡 Que en nuestro caso daría lugar a, 𝑓(𝑥,𝑦)𝐿𝐼𝑁= 0 En la figura 2-33 se puede apreciar gráficamente dicha linealización, que no es más que un plano tangente en x=0 e y=0 a la función 𝑓 𝑥,𝑦 =𝑥𝑦. 29 Control y Simulación del Péndulo de Furuta Figura 2-33. Linealización de función algebraica (II). Para linealizar ecuaciones diferenciales básicamente hacemos lo mismo que para ecuaciones algebraicas pero teniendo en cuenta que: 1) Si hay una parte de la ecuación diferencial que se deriva respecto al tiempo, el primer paso es realizar dicha derivación total, véase el siguiente ejemplo. 𝐹 𝑡 =𝑑 𝑑𝑡 𝑎 ∙𝐻2(𝑡) →𝐹 𝑡 = 2 ∙𝑎∙𝐻(𝑡)∙𝐻 (𝑡) 2) Hay que tomar la expresión de Taylor, derivar parcialmente, respecto de todas las variables y sus derivadas, ya que todas tendrán un valor concreto en el punto de funcionamiento. A continuación se van a linealizar las ecuaciones de movimiento de péndulo de Furuta entorno al punto de funcionamiento que no es otro que 𝜃𝑜= 0, 𝜃 𝑜= 0, 𝜃 0= 0, 𝜑𝑜= 0, 𝜑 𝑜= 0, 𝜑 𝑜= 0 𝑦 𝑢𝑜= 0, que sería con el péndulo en posición vertical hacia arriba con el brazo en un ángulo de cero grados, donde no debe de haber movimiento, de ahí que las derivadas temporales sean cero, y con tensión aplicada igual a cero voltios. Las ecuaciones de movimiento, obtenidas en el apartado 2.1.1., son, ∆𝜃 =−𝛼2𝜃 2sin 𝜃cos 𝜃+ 𝛽+𝑠𝑖𝑛2𝜃 𝜑 2sin 𝜃cos 𝜃+ 2𝛼𝜃 𝜑 sin 𝜃𝑐𝑜𝑠2𝜃+𝛽sin 𝜃+𝑠𝑖𝑛3𝜃−𝛾𝑢𝛼cos 𝜃−𝑐𝑝𝜃 ∆𝜑 =𝛼𝜃 2sin 𝜃−𝛼𝜑 2sin 𝜃𝑐𝑜𝑠2𝜃−2𝜃 𝜑 sin 𝜃cos 𝜃−𝛼sin 𝜃cos 𝜃+𝛾𝑢−𝑐𝑎𝜑 Con, ∆=𝛽+𝑠𝑖𝑛2𝜃−𝛼2𝑐𝑜𝑠2𝜃 Linealizando la primera ecuación término a término queda que el término de la izquierda de la igualdad se reduce a (𝛽−𝛼2)𝜃 , el primer, segundo y tercer sumando de la derecha se reducen a cero, al igual que el quinto, mientras que el cuarto queda como 𝛽𝜃, el sexto queda –𝛾𝛼𝑢 y el último queda igual al ser ya lineal. Por otro lado, linealizando la segunda ecuación diferencial queda que ∆𝜑 →(𝛽−𝛼2)𝜑 , 𝛼𝜃 2sin 𝜃→0, −𝛼𝜑 2sin 𝜃𝑐𝑜𝑠2𝜃→0 , −2𝜃 𝜑 sin 𝜃cos 𝜃→0, −𝛼sin 𝜃cos 𝜃→ −𝛼𝜃 y los dos términos restantes ya son lineales. Diseño, Simulación y Control del Péndulo de Furuta 30 Por tanto, obtenemos la siguiente ecuación de movimiento linealizada entorno al punto de funcionamiento: 𝛽−𝛼2 𝜃 =𝛽 𝜃− 𝛾 𝛼 𝑢−𝑐𝑝 𝜃 𝛽−𝛼2 𝜑 =−𝛼 𝜃+ 𝛾 𝑢−𝑐𝑎 𝜑 2.3.2. Obtención del modelo en espacio de estados Una vez que tenemos las ecuaciones linealizadas de movimiento vamos a tratar de describirlas en espacio de estados, esto es, describirlas de la forma siguiente, 𝑥 =𝐴 𝑥+𝐵 𝑢 El parámetro x es el espacio de estados, que en nuestro caso es, 𝑥= 𝜑𝜃 𝜑 𝜃 La ecuación de movimiento, considerando que α=0.56, β=0.74, γ=2.37, cp=0.5 y ca=0.5, queda como, 𝜃 = 1.737 𝜃−3.115 𝑢−1.174 𝜃 𝜑 =−1.315 𝜃+ 5.563 𝑢−1.174 𝜑 Que escrito en espacio de estados es, 𝜑 𝜃 𝜑 𝜃 = 0 0 0 0 1 0 0 1 0−1.315 0 1.737 −1.174 0 0 −1.174 𝜑𝜃𝜑 𝜃 + 0 0 5.563 −3.115 𝑢 2.3.3. Uso de Matlab para la obtención del parámetro K en control LQR El control LQR proporciona una ganancia óptima k, de forma que se minimiza un cierto criterio. El control es de la forma que se puede apreciar en la siguiente figura. Figura 2-34. Control LQR. La instrucción lqr de Matlab tiene la siguiente forma, [K, S, E]=lqr(A, B, Q, R, N); Y resuelve el control minimizando el siguiente criterio, 𝐽= 𝑥′𝑄 𝑥+𝑢′𝑅 𝑢+ 2 𝑥′𝑁 𝑢 Para nuestro caso, 𝐴= 0 0 0 0 1 0 0 1 0−1.315 0 1.737 −1.174 0 0 −1.174 𝐵= 0 0 5.563 −3.115 31 Control y Simulación del Péndulo de Furuta Las matrices Q y R son de ponderación, en nuestro caso le asignaremos los siguientes valores, aunque se les podrían haber dado otros, 𝑄= 1 0 0 1 0 0 0 0 0 0 0 0 1 0 0 1 𝑅= 1 Por otro lado, N es un término que omitiremos dándolo el valor cero. Las salidas que nos genera la función son K (la gancia óptima), S (la solución de la ecuación asociada de Riccati) y E (los polos en bucle cerrado). Introduciendo pues estos valores, obtenemos la siguiente solución. >> [K,S,E]=lqr(A,B,Q,R,N) K = -1.0000 -6.6752 -0.8122 -3.8068 S = 4.8311 11.3609 3.4233 6.4346 11.3609 36.7543 10.1043 20.1879 3.4233 10.1043 3.0609 5.7271 6.4346 20.1879 5.7271 11.4499 E = -6.5615 -1.5968 -0.5331 -0.9966 Por tanto la ganancia óptima es, 𝐾= [ −1−6.6752 −0.8122 −3.8068] En el siguiente apartado probaremos los resultados de este controlador en nuestro modelo en Simulink. 2.3.4. Implementación del control LQR en el modelo en Simulink del péndulo de Furuta En este apartado se verá como implementar el control LQR en nuestro modelo de Simulink® sobre el cual hemos de realizar algunas modificaciones al trabajar ahora en espacio de estados. Diseño, Simulación y Control del Péndulo de Furuta 32 Figura 2-35. Control LQR modelo en Simulink® 33 Control y Simulación del Péndulo de Furuta En la figura 2-35 se puede ver como la referencia es el ángulo φ, un escalón. También se probará con la referencia del ángulo φ constante igual a cero. Las ganancias que se han introducido no son como las habituales, son de tipo Matrix (k*u). La primera ganancia contiene lo que se puede apreciar en la figura 2-36, en ella se puede ver como la entrada u se multiplica por un vector con un uno en la primera componente y cero en las demás de forma que tras multiplicarse por dicha ganancia la entrada, el resultado es un vector de cuatro filas y una columna donde el primer componente es el ángulo φ de referencia y los demás cero. Figura 2-36. Parámetros de la ganacia primera del control LQR. La segunda ganancia contiene lo que se puede apreciar en la figura 2-37, esto es, la K óptima obtenida previamente. Dicha ganancia óptima de dimensiones 1x4 se multiplica por un vector de entrada 4x1, el error de los estados, dando como resultado la tensión de entrada. Figura 2-37. Parámetros de la ganancia segunda del control LQR. Diseño, Simulación y Control del Péndulo de Furuta 40 En la figura 2-46 se representa, en rojo, la zona donde se realiza un control en energía y la zona, en verde, donde se realiza un control en posición LQR o balance de control PD. Es necesario un tipo de interruptor que cambie de un tipo de control a otro en función de la posición del péndulo. 2.4.1. Control en energía en posiciones alejadas del punto de funcionamiento Para realizar un control en energía debemos calcular en todo momento la energía del péndulo, cómo hacerlo junto a qué ley de control implementar son los temas que se tratan en este apartado. La energía del péndulo es, 𝑉=𝑚𝑝 𝑔 𝑙𝑝 2 (1 + cos 𝜃 ) 𝑇=1 2 𝐽𝑝𝜃 2 𝐸=𝑇+𝑉 Donde 𝑚𝑝 es la masa, 𝑙𝑝 es la longitud y 𝐽𝑝 es el momento de inercia del péndulo supuesto con densidad uniforme. Luego ya podemos hacer un modelo en Simulink que nos proporcione la energía total a partir de la posición del péndulo y de su velocidad. Considerando que la masa del péndulo es 0.024 kg y su longitud es 0.129m y considerando que la gravedad es 9,8 m/s2 podemos calcular que, 𝑚𝑝 𝑔 𝑙𝑝 2= 0.024 ∙9.8 ∙0.129 2= 0.0151704 𝑘𝑔 𝑚2 𝑠2 𝐽𝑝=4 3 𝑚𝑝 𝑙𝑝2= 0.0005325 𝑘𝑔 𝑚2 El modelo creado para obtener la energía mecánica total es el siguiente, Figura 2-47. Cálculo de la energía mecánica del péndulo. Este modelo se encuentra dentro de un subsistema, 41 Control y Simulación del Péndulo de Furuta Figura 2-48. Subsistema de cálculo de la energía. La energía de referencia para el control en energía será aquella que tiene el péndulo cuando este se encuentra en la posición deseada, esto es, en posición vertical hacia arriba, 𝜃= 0, y sin velocidad, 𝜃 = 0. 𝐸𝑟𝑒𝑓=𝑚𝑝 𝑔 𝑙𝑝= 0.0303408 𝑘𝑔 𝑚2 𝑠2 La ley de control que se va a usar en el swing-up es la siguiente, 𝑢=𝐾 𝐸𝑟𝑒𝑓−𝐸 𝑠𝑖𝑔𝑛(cos 𝜃 𝜃 ) Donde sign es la función signo, que vale uno cuando cos 𝜃 𝜃 es positivo, menos uno cuando es negativo y cero cuando es cero. Por tanto se podría decir que la ley de control es un controlador proporcional pero con algo más, la función signo. La ganancia K= -50 y se ha obtenido experimentalmente. Grosso modo, con la función signo lo que conseguimos es que cambie el sentido de la actuación cada vez que el péndulo pasa por la posición horizontal. 2.4.2. Implementación del swing-up en Simulink Ahora vamos a implementar el control completo del péndulo, que va a constar de un swing-up para que cuando estemos lejos del punto de funcionamiento nos lleve a una zona próxima y de un control LQR para cuando estemos en una zona controlable cerca del punto de funcionamiento. Para pasar de un control al otro se va a implementar un interruptor que pase de uno a otro tipo de control en función de la energía, el valor de energía a partir del cual vamos de uno a otro tipo de control es 0.029 𝑘𝑔 𝑚2 𝑠2 que correspondería a un valor de 𝜃= ±24.27º si el péndulo estuviera quieto, es decir, sin energía cinética. Este valor se ha obtenido experimentalmente ya que sabemos del apartado 2.2.2.3. que para 𝜃∈ −30º, 30º el control LQR es capaz de controlar correctamente (ya que el balance de control PD es capaz de controlar), pero como la energía del péndulo también incluye energía cinética podría ser que tuviéramos una energía mayor que la que corresponde a la energía potencial del péndulo en theta igual a 30º (ó -30º), pero no porque estuviéramos en ese ángulo ni cerca, sino porque el movimiento del péndulo le añade energía cinética. De este modo con unos ángulos menores que treinta grados, tendremos mayor energía potencial (ya que estamos a mayor altura) colocándonos del lado de la seguridad si tenemos también energía cinética debido al movimiento. Una vez que entre el control LQR, ya estamos en un ángulo theta próximo al valor de funcionamiento pero el ángulo phy podría encontrarse en cualquier posición alejada de la del punto de funcionamiento para la cual se obtuvo la ganancia óptima, esto podría provocar que el controlador LQR fuera incapaz de controlar el péndulo al partir de posiciones de todas las variables de estado alejadas del punto de funcionamiento. Para evitar esto hemos colocado un interruptor de forma que una vez que se cambia del swing-up al control LQR se guarde el valor del ángulo phy en ese momento y ese sea la referencia del ángulo del brazo. A continuación se muestran los diagramas en Simulink del control LQR y swing-up. Diseño, Simulación y Control del Péndulo de Furuta 42 Figura 2-49. Modelo swing-up en Simulink. 43 Control y Simulación del Péndulo de Furuta Los subsistemas del modelo A21 y de la energía ya se han presentado previamente y son exactamente iguales, en cambio el subsistema del control LQR se ha modificado al colocarle el interruptor que se comentó anteriormente. Figura 2-50. Control LQR con interruptor. Diseño, Simulación y Control del Péndulo de Furuta 44 Como se puede ver, en el momento que llegamos a la energía a partir de la cual se entra en funcionamiento el LQR se guarda ese ángulo phy y es el que se usa como referencia, anteriormente también se está calculando la acción de control del LQR, pero esta no se aplica debido al otro interruptor. Los contenidos de los bloques interruptores son los siguientes, Figura 2-51. Bloque interruptor. 2.4.3. Resultados obtenidos con el swing-up Si partimos con el péndulo parado en posición vertical hacia abajo obtenemos la siguiente respuesta al controlarlo. Figura 2-52. Ángulo phy control swing-up. 45 Control y Simulación del Péndulo de Furuta Figura 2-53. Ángulo theta control swing-up. La energía del péndulo es la que se muestra a continuación, donde se puede ver como se alcanza la referencia de energía. Figura 2-54. Energía del péndulo. Si partimos de otras condiciones, por ejemplo 𝜃0=𝜋/2 obtenemos la siguiente respuesta, Diseño, Simulación y Control del Péndulo de Furuta 46 Figura 2-55. Ángulo phy en swing-up con 𝜃0=𝜋/2. Figura 2-56. Ángulo theta en swing-up con 𝜃0=𝜋/2. 47 Control y Simulación del Péndulo de Furuta Figura 2-57. Energía en swing-up con 𝜃0=𝜋/2. En conclusión se puede decir que la respuesta del swing-up es ciertamente aceptable siendo capaz de controlar el péndulo partiendo desde cualquier condición inicial con unos buenos resultados. Diseño, Simulación y Control del Péndulo de Furuta 48 3 IDENTIFICACIÓN Y CONTROL DEL PÉNDULO REAL DE QUBE-SERVO n este apartado se comienza a trabajar con el péndulo Qube-Servo, en primer lugar se va a presentar el péndulo con sus componentes, se va a ver el modelo en Simulink que usaremos y también como poner en marcha el péndulo, posteriormente se tratará de identificar los parámetros del péndulo a través de distintas técnicas como la identificación a un paso, identificación a N pasos o incluso técnicas de ingeniería inversa a partir de valores proporcionados por el fabricante, finalmente probaremos un controlador obtenido previa identificación del sistema y veremos su resultado. 2.1. El péndulo de Furuta Qube-Servo El péndulo de Qube-Servo que trataremos de controlar es el que se ve en la figura 3-1, como se puede apreciar consta de un péndulo conectado a un encoder a través de un brazo, a diferencia del modelo con el que se ha venido trabajando, este péndulo tiene la posición 𝜃= 0 radianes cuando se encuentra vertical hacia abajo y 𝜃=𝜋 radianes cuando está hacia arriba, esto deberá tenerse en cuenta a la hora de identificar y controlar el sistema. Otro detalle importante de este péndulo es que debido a que el encoder está conectado a la base a través de un cable, el brazo del péndulo no podrá realizar giros completos sobre sí mismo, algo que el modelo con el que hemos venido trabajando puede hacer sin problemas ya que no se le ha impuesto esa limitación. Los datos del estado del péndulo así como la actuación pasan a través de una conexión USB desde nuestro sistema al ordenador y viceversa, estos datos son tratados directamente en Simulink a través de unos bloques proporcionados por el fabricante del péndulo. E En cada rama del conocimiento, el progreso es proporcional a la cantidad de hechos sobre los que se construye, y por tanto a la facilidad de obtención de datos. - James Clerk Maxwell - Identificación y Control del Péndulo Real de Qube-Servo 56 Por su parte el péndulo presenta la siguiente respuesta, Figura 3-13. Ángulo del péndulo ante la entrada sinusoidal. Se puede ver la señal del ángulo theta como una serie de escalones debido a que realmente el ángulo del péndulo apenas varía y está muy cercano a cero, lo que provoca que podamos apreciar la resolución del encoder. Esto se debe a que el brazo se mueve muy despacio, ya que la entrada está comprendida entre -0.5 y 0.5 voltios, y es suave debido a que es sinusoidal. 3.1.2 Identificación a un paso o en bucle abierto Este método de identificación consiste en ir probando con todas las combinaciones posibles de los parámetros a identificar del péndulo, estos son, la longitud del brazo [r], la mitad de la longitud del péndulo [l], la masa del péndulo [m], la ganancia del motor [Km], el parámetro de fricción del péndulo [cp] y la fricción viscosa del brazo [ca], hasta que la respuesta del modelo matemático se parezca a la real. Ciertamente se trata de un método de fuerza bruta. El fabricante del péndulo nos proporciona valores de los parámetros a identificar, lo que hacemos es tomar variaciones de ± X% de estos valores y calcular el 𝑒2, siendo e el error entre la respuesta real del ángulo theta (o phy) y el mismo ángulo del modelo matemático, para todas las combinaciones de los parámetros tomando como válidos aquellos que dan un menor sumatorio del error cuadrático. Este es de hecho el método de los mínimos cuadrados (nombre dado por el matemático Adrien-Marie Legendre) creado por Gauss. Se ha desarrollado un programa en Matlab que realiza este conjunto de operaciones, a continuación se explica su funcionamiento. 57 Control y Simulación del Péndulo de Furuta INICIO Carga entrada y salidas real de QUBE-Servo Crea vector de 5 componentes de cada parámetro Realiza Spline con mucha resolución de las salidas cargadas i=1 i<=5 j=1 j<=5 k=1 k<=1 m=1 m<=1 p=1 p<=5 q=1 q<=5 Calcula y guarda error para todos los parámetros en ‘matrizerror’ q=q+1 p=p+1 m=m+1 k=k+1 j=j+1 i=i+1 Buscamo el error mínimo con la función min n2=nº de columnas de la matriz donde se guardan los errores i=1 i<=n2 matrizerror(7,i) =errormin Flag=1 i=i+1 r=matrizerror(1,flag) l=matrizerror(2,flag) m=matrizerror(3,flag) ... Representar resultados FIN SI NO SI NO SI NO SI NO SI NO NO SI SI NO Figura 3-14. Diagrama de flujo (I). Identificación y Control del Péndulo Real de Qube-Servo 58 INICIO ‘Crea vector de 5 componentes de cada parámetro’ rval=linspace(0.9*r,1.1*r,5) lval=linspace(0.9*l,1.1*l,5) mval=linspace(0.9*m,1.1*m,5) kmval=linspace(0.9*km,1.1*km,5) cpval=linspace(0.9*cp,1.1*cp,5) caval=linspace(0.9*ca,1.1*ca,5) FIN ‘Crea vector de 5 componentes de cada parámetro’ Figura 3-15. Diagrama de flujo (II). 59 Control y Simulación del Péndulo de Furuta INICIO ‘módulo Spline’ Calcular Interpolación theta real Calcular interpolación phy real FIN ‘módulo spline’ Figura 3-16. Diagrama de flujo (III). INICIO ‘módulo calcula y guarda error’ r=rval(i) l=lval(j) m=mval(k) ... SIMULACIÓN DEL MODELO Error=(spline real-spline modelo)^2 Matriz error=[ i j k m p q error; i2 j2 k2 m2 p2 q2 error2;…] FIN ‘módulo calcula y guarda error’ Figura 3-17. Diagrama de flujo (IV). Identificación y Control del Péndulo Real de Qube-Servo 60 INICIO ‘módulo representa’ Ploteamos theta real frente a theta del modelo con los parámetros que minimizan el error Ploteamos phy real frente a phy del modelo con los parámetros que minimizan el error FIN ‘módulo representa’ Figura 3-18. Diagrama de flujo (V). Para cargar los parámetros reales, la entrada de tensión y las salidas theta y phy, usamos el comando load. En el primer módulo se hace uso de la función linspace, que proporciona un vector de cinco componentes para cada parámetro entre un menos diez por ciento y más diez por ciento del valor teórico propuesto por el fabricante. En el segundo módulo se usa el comando spline, que permite una interpolación cuadrática entre puntos consecutivos de los valores de los ángulos del péndulo y del brazo reales, pero, ¿por qué hacemos esto?, el motivo es los ángulos reales que cargamos y las que se generan en el modelo matemático no tienen la misma resolución, es decir, la enésima componente del vector donde se guarda el ángulo real y la misma componente del vector del ángulo generado por el modelo matemático no tienen lugar al mismo tiempo, luego no se puede calcular el error como simplemente la diferencia entre ambos vectores si no hacemos antes una interpolación. A continuación se intenta clarificar esta idea, supongamos que tenemos la siguientes respuestas real y obtenida por el modelo matemático, Figura 3-19. Uso de splines (I). 61 Control y Simulación del Péndulo de Furuta Como se puede apreciar tenemos más muestras, más resolución, en la respuesta del modelo matemático que en la respuesta real obtenida, una solución para poder calcular el error, es decir, la diferencia entre la señal real y la del modelo, de la que solo tenemos las muestras discretas, es calcular a través de una interpolación puntos intermedios del modelo real, esto es lo que se aprecia a continuación, Figura 3-20. Uso de splines (II). Como se puede ver, ahora se dispone de muestras intermedias en la señal con menor resolución para poder calcular el error como la diferencia entre unos valores y otros. Grosso modo esto es lo que se hace en el programa, pero teniendo en cuenta que realmente tenemos muchas muestras por segundo tanto en el modelo real como en el modelo matemático, así al interpolar las señales con aún mayor resolución, el error introducido por la interpolación, ante el posible valor real, es mínimo. Luego entramos en una serie de bucles que lo que hacen es calcular el error cuadrático entre ambas señales para todos los valores de cada parámetro, como son 5 valores por cada parámetro y 6 parámetro en total, se realizarán las operaciones de dentro de los bucles un total de 56=15625 veces, este es el principal motivo por el que el programa tarda un tiempo considerable en ejecutarse, ya que debe ejecutar Simulink dicho número de veces. Las operaciones que se realizan dentro de los bucles no son otras más que ejecutar el modelo matemático de Simulink con los parámetros que correspondan a través de la instrucción sim y luego se calcula el error cuadrático en theta y phy. Para cada iteración guardamos los valores de los parámetros y el error obtenido en una matriz. Tras salir de los bucles, vamos a la matriz de error que se creó y se busca a través de la función min cual es el error mínimo cometido y el valor de los parámetros que corresponden a ese error mínimo. En último lugar, el programa simula de nuevo el modelo para los parámetros que producen un error mínimo y representa los resultados comparándolos con los resultados reales. Tras ejecutar el programa iterativamente con los parámetros que van proporcionando menor error en cada ocasión llegamos a los siguientes parámetros: r=0.0619 m, l=0.0859 m, m=0.0238 kg, Km=0.0320 V/(rad/s), Cp=0.7895 y Ca=1.0526 Con estos parámetros obtenemos los resultados que se muestran a continuación. Identificación y Control del Péndulo Real de Qube-Servo 62 En primer lugar, para la entrada sinusoidal hemos obtenido la siguiente identificación. Figura 3-21. Comparación entre QUBE-Servo y modelo A21 frente a entrada sinusoidal. Con estos parámetros, tenemos la siguiente respuesta entre el modelo matemático y el real para el ángulo phy ante la entrada aleatoria. Figura 3-22. Comparación entre QUBE-Servo y modelo A21 frente a entrada aleatoria. 63 Control y Simulación del Péndulo de Furuta Como se puede apreciar la identificación no es buena, esto se debe principalmente a que identificamos los parámetros en bucle abierto, es decir, si la identificación es mala y la respuesta del sistema real y del modelo divergen, dentro de un tiempo no estás considerando únicamente el error entre un modelo y otro sino que también estas considerando un error extra debido a que ambas respuestas parten de puntos distintos debido a que vienen divergiendo desde atrás en el tiempo. En el caso del ángulo phy ante la entrada aleatoria la identificación es igualmente mala, aunque se consigue que ambas señales no estén desfasadas, lo cual es lógico. El programa se encuentra en el anexo al final del documento. 3.1.3 Identificación a N pasos o en bucle cerrado Como dijimos en el apartado anterior, al identificar en bucle abierto estamos incluyendo un error extra debido a que las señales vienen divergiendo desde atrás en el tiempo y aunque, de repente, el sistema real se comportara igual que el modelo matemático, seguiríamos teniendo un error (que no debería de existir) que vamos sumando debido a que ambos modelos parten desde puntos distintos a causa de los errores anteriores. Para evitar esto se ha creado un nuevo programa que trata de identificar el sistema a N pasos, es decir, corrigiendo su estado respecto al estado real del QUBE-Servo cada un paso de N segundos. De esta forma vamos viendo durante cuánto tiempo el modelo matemático tiene validez y tiene un comportamiento similar al modelo real. Como se verá a continuación, para ello debemos coger las cuatro variables del espacio de estados y tomar el valor adecuado de las mismas como condiciones iniciales cada periodo de tiempo N para que cada dicho periodo de tiempo, el sistema real y el modelo matemático partan desde el mismo punto. Para poder introducir desde fuera las condiciones iniciales de posición y velocidad de phy y theta se han añadido estas variables al sistema de Simulink. Figura 3-23. Modelo en Simulink del péndulo que se cargará desde el programa (I). En este modelo se pueden introducir desde el exterior, el workspace, las condiciones iniciales de posición y Identificación y Control del Péndulo Real de Qube-Servo 64 velocidad. Figura 3-24. Modelo en Simulink del péndulo que se cargará desde el programa (II). A continuación se explicará el programa de identificación a N pasos, pero previamente hay que explicar cómo se crea la entrada que se usará en el sistema, en concreto la entrada aleatoria. Se creó un vector aleatorio de tensiones que iban desde -2 hasta 2 voltios, cada uno de estos valores se mantenían durante medio segundo, ya que la entrada al sistema real era una Repeating Sequence Stair con un Sample Time de medio segundo. Pero al guardar la entrada y cargarla lo que tenemos son solo los puntos de tensión sin ser mantenidos en el tiempo, esto es lo que se muestra a continuación. Figura 3-25. Tensión cargada VS deseada. 65 Control y Simulación del Péndulo de Furuta Hay que manipular la entrada para que sea la que realmente queremos, para ello hacemos un breve programa que crea un vector que a cada medio segundo la asigna 250 valores cuyo valor es el que corresponde de la entrada cargada. Así como la identificación es durante cinco segundos, tendremos un vector de 5x2x250 = 2500 valores que si representan la entrada deseada, llamamos b a este vector. a=load('entrada_pendulo_aleatoria.mat') b=[]; for i=1:30 unos=ones(1,250); b=[b,a(i)*unos]; end Para acabar de completar la entrada ejecutamos el siguiente modelo en Simulink. Figura 3-26. Creación de la entrada aleatoria. En la secuencia repetitiva introducimos el vector b creado anteriormente, con un Sample Time de 0.002 segundos, ya que tenemos 2500 valores para 5 segundos (5/2500=0.002, un valor cada 0.002 segundos). To File permite guardar un archivo con un nombre dado y en un formato determinado, el archivo se ha llamado entradaaleatoria_ts y se ha guardado en formato Timeserie. Una vez que se ejecuta el modelo ya tenemos la entrada correcta creada y además en formato Timeserie que nos permitirá manipular fácilmente dicha entrada, ya que se debe ir introduciendo „a pasos‟. A continuación se explica a través de diagramas de flujo el funcionamiento del programa. En primer lugar pedimos el número de pasos del sistema a través del comando input, luego se calculan las velocidades de los ángulos theta y phy (esto se explica en un módulo distinto) y se interpolan todas las variables del espacio de estado para obtener su valor en cada momento de tiempo t= k*paso con k=0, 1, 2, 3, 4, 5 ya que tendremos que tomar esos valores como condiciones iniciales para la simulación de cada tramo de „paso‟ segundos. Luego cargamos los parámetros del modelo y finalmente simulamos paso a paso, como realizar dicha simulación se explica en un módulo diferente. En la figura 3-28 se puede apreciar lo que hacemos al interpolar, que no es otra cosa más que coger los valores de theta, phy y sus derivadas en ciertos puntos y que serán consideradas las condiciones iniciales de la simulación en cada intervalo de N segundos. Identificación y Control del Péndulo Real de Qube-Servo 72 Figura 3-37. Respuesta a entrada sinusoidal con paso de 0.2 segundos (I). Figura 3-38. Respuesta a entrada sinusoidal con paso 0.2 segundos (II). Incluso en un tiempo de 0.2 segundos, aunque no diverjan demasiado ambas respuestas, hay un error apreciable. Se puede concluir que el modelo matemático quizás no contiene toda la dinámica del péndulo real, ya que dicho modelo, incluso el que se ha encontrado que comete menor error cuadrático, solo es válido para periodos de tiempo muy pequeños tales como 0.1 o 0.2 segundos, cuando aumentamos mas el tiempo de identificación ambas respuestas se muestran bastante distintas. A pesar de que la identificación del péndulo solo es válida y no comete demasiado error en intervalos de tiempo pequeñito, en apartados posteriores se hallará un 73 Control y Simulación del Péndulo de Furuta controlador LQR para estos parámetros y se verá si es capaz de controlar el péndulo. El código del programa de identificación en bucle cerrado o a N pasos se adjunta en los anexos de este documento. 3.1.4 Identificación mediante ingeniería inversa En la documentación del péndulo QUBE-Servo se proporciona información sobre las matrices A y B del control LQR, estas matrices dependen de los parámetros del sistema, se puede tratar de identificar a los parámetros a partir de dichas matrices. Las matrices proporcionadas se pueden apreciar en la figura 3-39. Figura 3-39. Matrices A y B proporcionadas por el fabricante. En el apartado 2.3.2 llegamos a que las matrices A y B de nuestro modelo matemático son, 𝐴= 0 0 1 0 0 0 0 1 0 0 −𝛼 𝛽−𝛼2 𝛽 𝛽−𝛼2 −𝐶𝑎 𝛽−𝛼2 0 0 −𝐶𝑝 𝛽−𝛼2 𝐵= 0 0 𝛾 𝛽−𝛼2 −𝛾𝛼 𝛽−𝛼2 Donde, 𝛼= 3𝑟/7𝑙 𝛽=𝑚 𝑟2+ 𝐽𝑎 𝐽𝑝+ 𝑚 𝑙2 𝛾=𝐾𝑚 𝑚 𝑔 𝑙 Identificación y Control del Péndulo Real de Qube-Servo 74 Donde Jp es la inercia del péndulo, 𝐽𝑝=4 3 𝑚 𝑙2 Como se puede apreciar, las componentes A (4,3) y A (4,4) están aparentemente intercambiadas, esto puede deberse a que el modelo matemático que se ha usado para este trabajo no es el mismo que ha usado el fabricante del péndulo, quizás nuestro modelo matemático es demasiado simplificado para representar la compleja dinámica del péndulo, por ello a la hora de identificar el péndulo, dicha identificación solo es válida durante breves periodos de tiempo. Aunque los modelos fueran algo distintos se podrían tratar de identificar con fsolve, una función de Matlab que nos permite resolver sistemas de ecuaciones no lineales, pero la diferencia entre los modelos es tal que se puede apreciar a simple vista que no existe solución posible, ya que alpha y beta deben ser positivos, y si comparamos A (3,2) y A (4,2) de ambas matrices A, nos damos cuenta que dichos valores de la matriz A de nuestro modelo matemático deberían de ser forzosamente de signo contrario, algo que vemos que no ocurre en la A usada por el fabricante, luego no existe forma posible de identificar los parámetros a partir del modelo matemático que se ha usado en este trabajo. En trabajos futuros se podría tratar de hallar otros modelos matemáticos para nuestro sistema. 3.2 Pruebas de control en el péndulo de QUBE-Servo En los apartados 3.2.2 y 3.2.3 se han obtenido unos parámetros del sistema a partir de la identificación que surge de comparar la respuesta real del sistema con nuestro modelo matemático ante diversas entradas. Estos parámetros son aquellos que han proporcionado un mínimo error cuadrático entre ambas respuestas. En este apartado se va a obtener un controlador LQR a partir de esos parámetros, se va a comprobar cómo es capaz de controlar en nuestro modelo matemático y posteriormente se comprobará si es capaz de controlar en el péndulo real, lo que dependerá de que tan buena sea la identificación que hemos obtenido. 3.2.1 Obtención de un controlador LQR Para obtener la ganancia óptima del controlador LQR, previamente hemos de linealizar las ecuaciones de movimiento del mismo. Como ya se mencionó anteriormente, el péndulo real y el péndulo del modelo matemático no tienen el mismo sistema de referencia para la coordenada del péndulo y esto es algo que debe ser considerado, pero como el control LQR funciona solo cerca del punto de funcionamiento, obtenemos lo mismo en ambos sistemas de referencia. Considerando las variables con un subíndice 2 como aquellas del péndulo real, con el ángulo 𝜃2= 0 cuando el péndulo está vertical hacia abajo, y sin subíndice para el modelo matemático, con θ=0 en la posición vertical hacia arriba del péndulo, ambas definidas en el sentido de la regla de la mano derecha, tenemos, 𝜃2=𝜃+𝜋 𝜃 2=𝜃 𝜃 2=𝜃 𝜑2=𝜑 𝜑 2=𝜑 𝜑 2=𝜑 Usando el cambio anterior y linealizando llegamos a las siguientes ecuaciones, 𝛽−𝛼2 𝜃 2=𝛽 𝜃2−𝜋 −𝛾𝛼𝑢 − 𝐶𝑝𝜃 2 𝛽−𝛼2 𝜑 2=−𝛼 𝜃2−𝜋 −𝛾𝑢 − 𝐶𝑎𝜑 2 Como queremos controlar entorno a 𝜃2=𝜋, es como si θ fuera la variable incremental, expresándolo en función de θ llegamos a la misma expresión que en el apartado 2.3.3, 𝛽−𝛼2 𝜃 =𝛽𝜃−𝛾𝛼𝑢 − 𝐶𝑝𝜃 𝛽−𝛼2 𝜑 =−𝛼𝜃−𝛾𝑢 − 𝐶𝑎𝜑 75 Control y Simulación del Péndulo de Furuta Escribiendo la ecuación en espacio de estados y usando los parámetros de la identificación se obtienen las matrices A y B. 𝐴= 0 0 0 0 1 0 0 1 0−2.4288 0 1.75 −8.2789 0 0−6.2069 𝐵= 0 0 12.5623 −3.8792 A continuación hay que seleccionar una matriz Q y otra R, en este caso se han seleccionado las siguientes, Q= 5 0 0 1 0 0 0 0 0 0 0 0 1 0 020 R=1 Usando esos parámetros se obtiene la siguiente ganancia óptima tras usar el comando lqr, 𝐾= −2.2361 −12.5120 0.0261 −4.2876 Si usamos esta ganancia, figura 3-40, sobre el modelo matemático se obtiene la respuesta que se puede apreciar en la figura 3-41 y 3-42. Figura 3-40. Ganancia óptima para control LQR. Identificación y Control del Péndulo Real de Qube-Servo 76 Figura 3-41. Ángulo theta con control LQR en modelo matemático con los parámetros identificados. Figura 3-42. Ángulo phy con control LQR en modelo matemático con los parámetros identificados. 3.2.2 Resultados del control Se usa ahora el controlador obtenido en el apartado anterior sobre el péndulo real de QUBE-Servo para ver si se consigue controlar correctamente. 77 Control y Simulación del Péndulo de Furuta Figura 3-43. Control LQR en el QUBE-Servo. Se consigue que el péndulo esté controlado durante un segundo aproximadamente, aunque termina cayendo. Esto es coherente con la identificación obtenida, ya que como se pudo observar la identificación era buena durante un corto periodo de tiempo a partir del cual el comportamiento del modelo real y el modelo matemático identificado divergían considerablemente, debido principalmente a la complejidad del sistema a identificar, cuya dinámica no parece estar completamente recogida en el modelo matemático usado. A continuación se muestra el resultado obtenido, se puede ver que controla aproximadamente bien durante un breve periodo de tiempo pero termina cayendo, debido a que la identificación no es completamente satisfactoria debido al modelo del sistema. Figura 3-44. Prueba en péndulo real. Identificación y Control del Péndulo Real de Qube-Servo 78 79 Control y Simulación del Péndulo de Furuta 4 CONCLUSIONES Y TRABAJOS FUTUROS n este trabajo se propone en primer lugar un modelo matemático que representa de una manera coherente el comportamiento del péndulo de Furuta. Dicho modelo se ha desarrollado en Simulink, permitiéndose que cualquier persona que quiera hacer prácticas de control con un sistema complejo y con dinámicas muy rápidas pueda usar directamente el modelo para poner a prueba sus conocimientos. Además se ha creado un modelo en tres dimensiones del péndulo, que una vez conectado a Simulink proporcionará una importante ayuda visual a la hora de ver el comportamiento del sistema. Se han desarrollado diversas técnicas de control para poder controlar el sistema y también una técnica de control híbrido que lleva el sistema cerca de la zona de funcionamiento interviniendo luego otra técnica de control distinta. Se puede decir que los controladores proporcionan un resultado francamente bueno, esto añadido al modelo tridimensional nos permite ver de una forma amigable el comportamiento del péndulo. La identificación de sistemas es una parte esencial, que puede llegar a ser muy compleja, en la ingeniería de control. En este trabajo se ha intentado identificar el péndulo de Furuta, un sistema fuertemente no lineal y que además contiene dos parámetros de fricción viscosa que son realmente muy complejos de identificar. Para identificar el sistema se ha realizado una comparación entre el sistema real y el modelo matemático para una misma entrada, intentando obtener qué parámetros del modelo matemático proporcionan menor diferencia cuadrática entre la respuesta de ambos sistemas. Tras diversas técnicas de identificación se han obtenido unos parámetros que permiten modelar el comportamiento del sistema con cierta validez pero durante tiempos 0,1 ó 0,2 segundos. Una validez durante tan breve periodo de tiempo puede deberse principalmente a que el modelo matemático que se contempla no contiene toda la dinámica del péndulo. Con los parámetros identificados se ha determinado una ganancia óptima y se ha probado sobre el modelo matemático, con un excelente resultado, y sobre el modelo real consiguiéndose que controle como se pudo apreciar. En trabajos futuros se podría tratar de obtener un modelo matemático más complejo que describiera mejor la dinámica del sistema, de esta forma al tratar de identificar el péndulo real quizás se podrían obtener unos resultados más satisfactorios. También podría ser interesante tratar de construir un péndulo que podríamos identificar y tratar de controlar a través de una placa tipo arduino, ya que no se necesitarían demasiados materiales y el péndulo de QUBE-Servo tiene un precio relativamente alto. E No es el conocimiento, sino el acto de aprendizaje, y no la posesión, sino el acto de llegar allí, lo que concede el mayor disfrute. - Carl Friedrich Gauss - Conclusiones y Trabajos Futuros 80 81 Control y Simulación del Péndulo de Furuta 5 ANEXOS En el presente anexo se incluyen los códigos de los dos programas de identificación que se usaron en los apartados 3.2.2 y 3.2.3. Tabla 5-1.Identificación a un paso. Programa de identificación a un paso. % Identificador de parámetros : % Identificación de parámetros del péndulo educacional QUBE-Servo. Trabajo % Final de Grado: Control y Simulación del Péndulo de Furuta. Autor: Carlos % Regalo Núñez. %Se van a identificar los parámetros r~0.085, l~0.129/2, m~0.024, Km~0.0365, %Cp~0.5 y Ca~0.5 del péndulo de Furuta. Probaremos valores del +-10% del %valor supuesto para los 4 primeros parámetros y del +300% para los dos %últimos. % Cargamos los parámetros REALES de los archivos correspondientes que % dependerá de si probamos con la entrada seno o aleatoria: load('entrada_pendulo_seno.mat'); load('phy_angulo_brazo_seno.mat'); load('theta_angulo_pendulo_seno.mat'); %Valores entorno a los que probamos: r=0.0688;l=0.0781;m=0.0251;Km=0.0356;Cp=8;Ca=1.7188; % Valores de los parámetros que vamos a probar: r_val=linspace(0.9*r,1.1*r,5); l_val=linspace(0.9*l,1.1*l,5); m_val=linspace(0.9*m,1.1*m,5); Km_val=linspace(0.9*Km,1.1*Km,5); Cp_val=linspace(0.5*Cp,1.5*Cp,5); Ca_val=linspace(0.5*Ca,1.5*Ca,5); g=9.8; Ja=0; %Cálculo del error para cada conjunto de valores: vectorvalores=[];pasada=0; resolucion=0.005; % Resolucion para sacar el error. intervalodeseado=[0:resolucion:5]; phyrealinterpolado=spline(phy_angulo_brazo.time,phy_angulo_brazo.signals.valu es,intervalodeseado); thetarealinterpolado=spline(theta_angulo_pendulo.time,theta_angulo_pendulo.si gnals.values,intervalodeseado); for i=1:5 for j=1:5 for k=1:5 for n=1:5 for p=1:5 for q=1:5 r=r_val(i); l=l_val(j); m=m_val(k); Km=Km_val(n); Cp=Cp_val(p); Ca=Ca_val(q); %Simulamos con esos valores: [t,x]=sim('TFG_14PENDULOENMASCARADO.slx',5); %Interpolacion de las salidas: Referencias 88