CAPÍTULO 5: DISEÑO DE CONTROLADORES

Transcripción

1 CAPÍTULO 5: DISEÑO DE CONTROLADORES Una vez obtenido el modelo del quadrotor pasamos a la fase de diseño del control en posición. Pero el modelo que tenemos es un sistema no lineal, así pues para aplicar las conocidas técnicas de control en primer lugar hay que linealizar el modelo [5]. Aplicando la aproximación de ángulos pequeños a la matriz de rotación que aparece en el modelo: x 0 sen( x) x con( x) 0 && x = m && y = m && z = m [( θ U ] [( φ ) U ] [ U ] g Finalmente suponiendo que la potencia total aproximadamente contrarresta la fuerza gravitatoria, U U = m g, excepto en el eje z. && x = m && y = m && z = m [( θ U ] [( φ ) U ] [ U ] g (5) Ya tenemos el modelo lineal de la aceleración que necesitábamos para poder diseñar los controladores en posición. El modelo lineal resultante es una aproximación a la realidad y por lo tanto en los experimentos comprobaremos cuanto se aproxima al modelo real de nuestro quadrotor. 5.. Diseño del controlador por el lugar de las raíces Para diseñar los controladores nos basaremos en la herramienta del lugar de las raíces, útil para analizar sistemas lineales dinámicos tipo SISO (single input single output) y su estabilidad (BIBO) [6]. 49

2 El lugar de las raíces permite determinar la posición de los polos de la función de transferencia en lazo cerrado para un determinado valor de ganancia a partir de la función de transferencia en lazo abierto G(s). Para facilitar los cálculos usamos la función de matlab sisotool (figura 3), una herramienta para el diseño de sistemas lineales mediante el lugar de las raíces. A esta función debemos pasarle sólo la función de transferencia en lazo abierto. Una vez ejecutada la aplicación podemos añadir polos y ceros para formar el controlador y comprobar como sería la respuesta del sistema a un escalón como entrada. Figura 3 : Sisotool 5... Controlador en X-Y El control en X-Y será igual debido a la simetría del quadrotor y que sale a relucir en el modelo. Por lo tanto diseñaremos un controlador para X y este mismo controlador valdrá para el control en Y. La función de transferencia en lazo abierto para el control de X será: Integrando g & x = θ g x( s) = θ s ( s) G( s) = g s Como se puede ver la G(s) no es más que dos integradores en el origen por una constante con lo cual, si esta función de transferencia es correcta no debemos de tener errores en régimen permanente al tener ya integradores en la G(s). 50

3 Como primera aproximación decidimos usar un controlador proporcionalderivativo (PD) debido a que deseamos que el movimiento del quadrotor sea suave y sin sobreoscilaciones. Cx PD =.4 s s + Situamos un cero real en y ajustamos la ganancia para que los polos sean reales. Además añadimos al controlador un polo de alta frecuencia para que sea una función propia. En el lugar de las raíces (figura 33) comprobamos que los polos en lazo cerrado se situaran en las posiciones: p p p 3 = = 6.54 = 9.64 Figura 33: Lugar de las raíces PD En los resultados experimentales con el quadrotor comprobamos que el controlador PD no es válido porque presenta un error en régimen permanente. Esto nos indica que el modelo, como ya esperábamos, sólo es una aproximación. 5

4 Para solventar este error diseñamos un control proporcional-integral-derivativo, PID. Cx PID =.365 s +.85 s s +.85 s Situamos un cero complejo en ±0.446 y ajustamos la ganancia para que la respuesta ante un escalón sea parecida a un sistema de primer orden y añadimos al controlador un polo de alta frecuencia para que se una función propia. En el lugar de las raíces (figura 34) vemos que los polos en lazo cerrado se situaran en las posiciones: p = ± 0.06 p p 3 = 9.47 = Figura 34: Lugar de las raíces PID Al final de este apartado se muestran los resultados obtenidos en Simulink con los distintos controladores y valoraremos estos resultados Controlador en Z El control en Z será totalmente distinto al control X-Y. La función de transferencia en lazo abierto para el control de Z será: 5

5 & z = T m g Integrando z( s) = m s T ( s) G( s) = m s Como se puede ver la G(s) no es más que dos integradores en el origen por una constante, la masa del quadrotor. Deseamos tener una respuesta rápida en Z para evitar que el quadrotor caiga al suelo cuando hay un cambio brusco en X-Y, ya que están acoplados. Para ello diseñamos un control proporcional-integral-derivativo, PID. Cx PID = 0.5 s + s s + s Situamos un cero complejo en -± y ajustamos la ganancia para que la respuesta ante un escalón tenga un tiempo de subida pequeño y también sobreoscile lo menos posible. Además añadimos al controlador un polo de alta frecuencia para que se una función propia. En el lugar de las raíces (figura 35) comprobamos que los polos en lazo cerrado se situaran en las posiciones: p = ±.8008 p p 3 =.5054 = 97.6 Figura 35: Lugar de las raíces PID Simulink. Al final de este apartado se muestran y valoran los resultados obtenidos en 53

6 5.. Integración en Simulink Una vez diseñado el controlador para cada eje de coordenadas pasamos a la simulación para ver el comportamiento de dichos controladores. Para esta labor usamos la herramienta de matlab, Simulink, muy buena para la simulación de todo tipo de sistemas de control continuos. El esquema general de nuestro bucle de control será el que se muestra en la siguiente figura 36: Figura 36: Esquema general del bucle de control Como ya hemos comentado, Simulink es un simulador para sistemas de control continuos pero en nuestro caso requerimos que el sistema de control sea discreto porque cuando tengamos que programar en C++ todo el control será como es lógico en discreto. Por lo tanto nos vemos obligado a transformar cada uno de los bloques a su homólogo discreto. La frecuencia de muestreo deseada será de 00 Hz (T = 0.0) ya que el tiempo de ejecución del bucle de control es de unos 0 ms Modelo En el subsistema Modelo No Lineal se encuentra el modelo calculado en el apartado anterior. En la figura 37 que se muestra a continuación podemos ver los bloques que forman el subsistema. 54

7 Figura 37: Modelo Este conjunto de bloques representa el sistema formado por las ecuaciones (3). En primer lugar tenemos las entradas al sistema que son los ángulos de navegación y la potencia dada a los rotores. En el bloque Modelo se encuentra la función matlab que realiza el cálculo del sistema de ecuaciones (3) y da como salida las aceleraciones angulares. Para tener la posición a partir de la aceleración angular debemos integrar dos veces que es lo que hace el bloque /S^ discreto : S Z S = T U = k T EK U k + U k 5... Prefiltro Para suavizar la respuesta del sistema a cambios de referencia se introduce un prefiltro en la referencia. Como consecuencia se introduce un retraso en el seguimiento de la referencia. El prefiltro tendrá una función de transferencia de primer orden, donde el polo de la función de transferencia debe ser suficiente para suavizar la respuesta del sistema modificando lo menos posible las propiedades del control. a a + S Z S = T U k = a T E k + ( a T ) U k En la figura 38 que se muestra a continuación podemos ver los bloques que forman el subsistema. 55

8 Figura 38: Prefiltro de la referencia Rotación El bloque de rotación es de vital importancia para poder controlar el quadrotor, pues es el encargado de relacionar el sistema de referencia tierra con el sistema de referencia cuerpo. En este cambio de sistemas de referencia se ha hecho la simplificación de que tanto el plano X-Y de tierra y como el de cuerpo son planos paralelos. Por lo tanto ambos sistemas de referencia podrán diferir en una rotación respecto al eje Z que viene dada por el valor de yaw. cos( x) R ( x) = sen( x) sen( x) cos( x) Con esta rotación el quadrotor se moverá siempre en su sistema de referencia pero en función a medidas del sistema de referencia de tierra. En la figura 39 podemos ver los bloques que forman el subsistema. Figura 39: Rotación en Yaw 56

9 5..4. Cálculo del error El error entre la medida y la referencia deseada se calcula de una forma tan sencilla como muestra la figura 40, haciendo la resta entre la referencia y el valor medido. Figura 40: Calculo del error Controladores En el subsistema Controladores se encuentra el controlador para cada eje de coordenada calculado anteriormente. En la figura 4 que se muestra a continuación podemos ver los bloques que forman el subsistema. Figura 4: Controladores Los controladores han sido discretizados mediante la transformación de Tustin para obtener las constantes que forman un controlador discreto. 57

10 T Td q0 = Kp ( + + ) Ti T T Td q = Kp ( + ) Ti T Td q = Kp T El controlador discreto tendrá una forma tal y como se muestra a continuación (figura 4): U k = U k + q0 ek + q ek+ + q ek+ Figura 4: Controlador discreto Saturación El bloque de saturación es algo necesario para evitar valores de cabeceo y alabeo demasiado grandes que provoquen inestabilidad en el quadrotor. En un primer lugar se optó por una saturación de 30º pero tras diferentes pruebas se decidió por un valor más conservador como es 0º. Figura 43: Saturación En estos dos bloques (figura 43) realizan la saturación para valores de pitch y roll mayores de ±0º. Y como es lógico si no se satura la señal se deja pasar con el mismo valor de entrada. 58

11 5..7. Resultados de la simulación En este apartado mostraremos los resultados obtenidos en Simulink para cada uno de los controladores Controlador X-Y Como ya comentamos antes, el control para el eje Y es igual que para X y por lo tanto sólo mostraremos los resultados para uno de ellos. La respuesta cuando hay un cambio de referencia de metro en X y el controlador es un PD es la que se muestra en la figura 44. Figura 44: Resultado controlador PD La respuesta obtenida es la esperada con un tiempo de subida de,4 segundos, sin embargo cuando la implementamos en el control del quadrotor nos aparece un error en régimen permanente que hace que no sea útil para el control de la posición. La solución a este problema es utilizar un controlador PID, con el cual obtenemos la respuesta que se muestra en la figura 45. Comprobamos que ahora tenemos una pequeña sobreoscilación y un tiempo de subida de 0,9 segundos. El resultado no es muy recomendable para el control de un quadrotor porque no es aconsejable que sobreoscile. 59

12 Figura 45: Resultado controlador PID Para intentar suavizar la respuesta ante un cambio de referencia se introduce el uso de un prefiltro. Se coge la constante de tiempo del prefiltro para que la respuesta sea la deseada. Si observamos el resultado obtenido (figura 46) vemos que hemos eliminado la sobreoscilación a costa de aumentar el tiempo de subida a, segundos. Figura 46: Resultado controlador PID con prefiltro 60

13 5..7. Controlador Z El controlador en Z también es un PID con una respuesta como la que se muestra en la figura 47. En este caso también tenemos una sobreoscilación de un % y un tiempo de subida de 0.8 segundos. Figura 47: Resultado controlador PID Al igual que en el control en X-Y también introducimos un prefiltro en la referencia para suavizar la respuesta (figura 48). Figura 48: Resultado controlador PID con prefiltro 6

14 5.3. Integración software de los controladores La integración software del control es un paso importante puesto que el buen funcionamiento de toda la integración se basa principalmente en él. En esta integración se trasladará a C++ los mismos bloques usados en Simulink por lo tanto no plantea demasiada dificultad. A continuación se muestra el diagrama de flujo de control. Figura 49: Diagrama de flujo del controlador 6

15 Ahora mostramos como se ha implementado cada uno de los bloques de Simulink en C Prefiltro El prefiltro discretizado es U a T E + a T ) U k = k ( k y por lo tanto la implementación en C++ no es más que expresar la ecuación con las variables adecuadas. // referencia X double Rx; Rx = 0.004*Xd *Rx; Rx = Rx; Xd = Xd; // referencia Y double Ry; Ry = 0.004*Yd *Ry; Ry = Ry; Yd = Yd; // referencia Z double Rz; Rz = 0.004*Zd *Rz; Rz = Rz; Zd = Zd; Rotación La rotación respecto al eje Z se integra como se muestra a continuación: //Referencia 63

16 if (Play == 0) { //Con prefiltro rx = Rx*cos(Ya) + sin(ya)*ry; ry = Rx*(-sin(Ya)) + cos(ya)*ry; rz = Rz; } else { //Sin prefiltro rx = Xd*cos(Ya) + sin(ya)*yd; ry = Xd*(-sin(Ya)) + cos(ya)*yd; rz = Zd; } //Posición x = Xmed*cos(Ya) + sin(ya)*ymed; y = Xmed*(-sin(Ya)) + cos(ya)*ymed; z = Zmed; Controladores // CONTROL EN EL EJE X control.x_uk = X_Uk + q0x*ekx + qx*x_ek + qx*x_ek; //Actualización de las variables de control X_Uk = control.x_uk; X_Ek = X_Ek; X_Ek = Ekx; // CONTROL EN EL EJE Y control.y_uk = Y_Uk + q0y*eky + qy*y_ek + qy*y_ek; //Actualización de las variables de control Y_Uk = control.y_uk; Y_Ek = Y_Ek; Y_Ek = Eky; // CONTROL EN EL EJE Z 64

17 control.z_uk = Z_Uk + q0z*ekz + qz*z_ek + qz*z_ek; //Actualización de las variables de control Z_Uk = control.z_uk; Z_Ek = Z_Ek; Z_Ek = Ekz; Saturación Para la saturación de 0º debemos saturar la salida del controlador en el caso que supere dicho valor. //Saturacion X if(control.x_uk > ) control.x_uk = ; if(control.x_uk < ) control.x_uk = ; //Saturacion Y if(control.y_uk > ) control.y_uk = ; if(control.y_uk < ) control.y_uk = ; Adaptación de lo valores La adaptación es necesaria para convertir la salida del controlador (un ángulo) a los valores de entrada al quadrotor con un rango de [0,4095]. // Pitch if (control.x_uk >= 0) { control.x_uk = floor(control.x_uk* ); } else { control.x_uk = floor(048 + control.x_uk*( )); } // Roll 65

18 if (control.y_uk >= 0) { control.y_uk = floor(048 + control.y_uk* ); } else { control.y_uk = floor(048 + control.y_uk*( )); } // Thrust control.z_uk = floor(control.z_uk* ); if(control.z_uk < 0) { control.z_uk = 0; } Resultados En este apartado se muestran los resultados obtenidos en vuelos realizados con el quadrotor Controladores X-Y En primer lugar realizamos pruebas con el controlador PD con el cual vemos (figura 50) que el movimiento del quadrotor es suave y adecuado para nuestros experimentos. Figura 50: Resultado controlador PD eje X 66

19 A pesar de parecer un control bueno, no es válido porque presenta un error en régimen permanente en el eje Y, lo que indica que el quadrotor no es totalmente simétrico. Como comprobamos en la siguiente figura 5, el quadrotor no alcanza la referencia debido a este error. Figura 5: Resultado controlador PD eje Y En la siguiente figura 5 se muestra el error en el eje Y donde podemos ver como no alcanzar el valor cero sino que se mantiene por encima. Figura 5: Resultado controlador PD error Como ya hemos comentado este error nos lleva a usar un controlador PID con un prefiltro para suaviza su respuesta ante cambios de referencias. En la figura 53 comprobamos el resultado experimental con el controlador PID, donde vemos cual es el 67

20 comportamiento real. A pesar de tener una pequeña sobreoscilación antes de alcanzar la referencia los resultados obtenidos experimentalmente son bastantes buenos. Figura 53: Resultado controlador PID Controlador Z El control en Z se puede considerar el control que mejores resultados hemos obtenido (figura 54). Ante un cambio en referencia se desplaza con suavidad hasta alcanzar la referencia y una vez alcanzada tiene una respuesta rápida debida a la dinámica del PID. Figura 54: Resultado controlador PID 68

21 5.4. Control de yaw El objetivo de este proyecto es controlar el quadrotor en posición pero nos vemos obligados a controlar también la guiñada para evitar que el quadrotor se desplace girando sobre sí mismo. El controlador de yaw será un simple proporcional, puesto que con este tipo de control obtenemos buenos resultados. En la figura 55 comprobamos el resultado de cambiar la referencia de yaw de cero grados a noventa. Se ha introducido también un prefiltro en la referencia para que se alcance la referencia suavemente. Figura 55: Resultado controlador yaw 69

22 70