Repositorio Institucional de Documentos

Proyecto Fin de Carrera. CARACTERIZACIÓN EMPÍRICA Y DISEÑO DEL SISTEMA DE CONTROL DE TRAYECTORIA DE UN QUADROTOR MEDIANTE SIMULACIÓN. Escuela de ingeniería y Arquitectura Curso 2012/2013. Autor: Inmaculada Martín Blázquez. Director: Antonio Romeo Tello. Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 2 Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 3 Resumen del PFC. En el presente proyecto se pretende realizar una caracterización empírica de un quadrotor, a partir de una simulación en MATLAB de su dinámica realizada por terceros, y diseñar un sistema de control para la trayectoria de este. Para la caracterización del sistema, se le ha sometido a determinados experimentos para obtener los parámetros que rigen la dinámica del aparato. Estos han sido realizados para obtener la función de transferencia del drone para las diferentes entradas de éste. Tras esto, se ha planteado un sistema de control de la trayectoria en tres partes: “position control”, “attitude planner” y “attitude controller”. El primer bloque se encarga de realizar el control de las coordenadas X, Y y Z, el segundo se encarga del cálculo de los giros adecuados a realizar para seguir la trayectoria y el tercero se encarga de regular dichos ángulos de rotación. Para la implementación de los reguladores, se hará una comparativa entre varios tipos y métodos de obtención para una de las partes, y se elegirá el que mejores prestaciones presente, aplicándose dicho procedimiento para el resto de entradas. Con el fin de facilitar el comando y monitorización del aparato, se desarrollará una interfaz gráfica para uso y disfrute del usuario permitiendo realizar un seguimiento tanto de la evolución de la posición del aparato como de las acciones suministradas por los reguladores y la velocidad resultante en cada motor, además de introducir las coordenadas y la orientación en z deseadas sin necesidad de utilizar la pantalla correspondiente a la simulación del modelo. Finalmente, se procederá a la obtención de un generador de trayectoria, el cual calculará la trayectoria a seguir para alcanzar la posición y orientación deseadas por el usuario. Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 4 Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 5 Índice Memoria. Índice. Resumen del PFC. ........................................................................................................ 3 Índice Memoria. ........................................................................................................... 5 1.- Objetivo y Alcance del proyecto. ........................................................................... 7 2.- Introducción. ......................................................................................................... 11 2.1.- Introducción a los quadrotors. ....................................................................... 11 2.2.- Cinemática y dinámica del quadrotor. ........................................................... 12 3.- Caracterización Experimental del Sistema. .......................................................... 17 3.1.- Signo de la ganancia. ..................................................................................... 17 3.2.- Obtención experimental de cT. ....................................................................... 17 3.3.- Obtención experimental de cQ. ...................................................................... 20 3.4.- Identificación de los polos del sistema. ......................................................... 22 4.- Control de trayectoria. .......................................................................................... 25 4.1.- Control de altitud. .......................................................................................... 26 4.1.1.- Regulador proporcional derivativo: lugar de las raíces........................... 27 4.1.2.- Regulador con realimentación de doble lazo: “servopropulsor”. ............ 31 4.1.3.- Regulador integral derivativo: método de Ziegler-Nichols en bucle cerrado. .................................................................................................................... 33 4.1.4.- Regulador obtenido aplicando técnicas frecuenciales. ............................ 35 4.1.5.- Elección del regulador............................................................................. 39 4.2.- Control de la componente de orientación “Yaw”. ......................................... 41 4.3.- Control de las componentes de orientación “pitch”/“roll” y posición en el plano horizontal. ......................................................................................................... 42 4.3.1Control de roll/pitch. ................................................................................ 43 4.3.2.- Control de X/Y. ....................................................................................... 45 4.4. Ajuste final del control. ................................................................................ 47 5.- Generador de trayectoria. ..................................................................................... 49 Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 6 5.1.- Código completo del botón de “Generate trajectory”. ................................... 53 6.- Interfaz gráfica de usuario. ................................................................................... 57 6.1.- Inicio de la interfaz y de la simulación. ......................................................... 57 6.2.- Botones de la interfaz gráfica. ....................................................................... 60 6.2.1.- Botón de “START SIMULATION”. ...................................................... 60 6.2.2.- Botón de “STOP SIMULATION”. ......................................................... 61 6.2.3.- Botón de “CLOSE”. ................................................................................ 62 6.2.4.- Botón de “Zoom” (para la gráfica de representación de la trayectoria). . 63 6.2.5.- Botón de “Rotate” (para la gráfica de representación de la trayectoria). 64 6.2.6.- Botón de “Reset View” (para la gráfica de representación de la trayectoria). ............................................................................................................. 64 6.2.7.- Botón de “Generate Trajectory” .............................................................. 64 6.2.8.- Botón “Go home”. ................................................................................... 66 6.3.- Mostrar las gráficas. ....................................................................................... 67 6.3.1.- Visualización de las coordenadas X, Y, Z y de la componente de orientación Yaw. ..................................................................................................... 67 6.3.2.- Visualización de las acciones y velocidades angulares de los motores. . 69 6.3.3.- Visualización de la trayectoria. ............................................................... 70 7.- Conclusiones. ........................................................................................................ 73 8.- Referencias bibliográficas e Informáticas. ........................................................... 75 8.1.- Bibliografía. ................................................................................................... 75 8.2.- Programas de cálculo y software. .................................................................. 76 9.- Anexos. ................................................................................................................. 77 Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 7 1.- Objetivo y Alcance del proyecto. Partiendo de la versión 9.7 de la “Robotic ToolBox” para MATLAB de Peter Corke , el objetivo planteado para este proyecto es obtener una caracterización de su dinámica mediante el sometimiento del quadrotor a diferentes ensayos y realizar un control de trayectoria para éste (3.3). Dado que todo el proyecto se ha fundamentado en una simulación realizada por terceros, la validez de los datos obtenidos está sujeta a la precisión que hayan tenido los autores del código del programa a la hora de desarrollar las fórmulas físicas que describen el comportamiento del sistema. Algo que se ve bien reflejado con los datos obtenidos en uno de los experimentos. Las pruebas a las que se le ha sometido al drone, son semejantes a los que se le sometería a uno físico; a excepción de que en ese caso se obtendría el peso mediante una báscula. Sin embargo, al no tratarse en nuestro caso de un ente físico, este parámetro es el aportado por los diseñadores de la simulación. Por otro lado, el alcance del presente proyecto es el desarrollo tanto del sistema de control como de una interfaz gráfica para uso y disfrute del usuario así como de la creación de un generador de trayectorias adaptado a este tipo de robot. Respecto al control de trayectoria, se procederá a realizar una estrategia de control de las variables de situación de forma desacoplada: cada variable se controlará de forma independiente a las demás. La validez de dicha estrategia viene ligada al hecho de que la sustentación no se vea afectada cuando se realicen desplazamientos en el plano horizontal, es decir, que los ángulos de giro sobre los ejes x e y (“roll” y “pitch”) sean muy pequeños, de forma que su coseno sea aproximable a uno. Debido a que se trata de un control desacoplado, se procederá a su división en tres partes: “position controller”, “attitude planner” y “attitude controller”1. El primer bloque se encarga del control de las coordenadas en el espacio de la tarea, por lo que sus entradas serán las X, Y y Z deseadas por el usuario; las salidas serán dos: la correspondiente al control de altitud (el cual se encuentra englobado dentro del control 1 En este caso, la traducción de los nombres de los bloques serían el controlador de posición, el organizador de situación y el control de situación. Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 8 de posición) y las correspondientes a las acciones desarrolladas por los reguladores que controlan los movimientos en el plano horizontal. Al ser un drone que tiene seis grados de libertad, en principio, debería tener seis variables de localización, tres de posición y tres de orientación. Sin embargo, el usuario sólo podrá introducir cuatro de ellas: las tres correspondientes a su localización en el espacio cartesiano y su orientación respecto del eje z (“yaw”). Es decir, estamos tratando con un sistema subactuado2 al introducir un número de consignas de entrada menor al realmente necesario. Las otras dos variables, “roll” y “pitch” (los ángulos de giro sobre x e y respectivamente), serán proporcionados por el control de posición en dicho plano. Estas dos componentes, junto a la “yaw” deseada, actuaran como entradas del organizador de situación, en el cual se llevará a cabo el control de las orientaciones respecto a los tres ejes de coordenadas, siendo la componente de giro sobre z el valor deseado por el usuario y las otras dos componentes de orientación las salida del “position controller” para x e y, las cuales son de carácter transitorio: su valor en régimen permanente (en situación de sustentación3, en ascenso/descenso o en ausencia de perturbaciones) es nulo. Fig. 1 Diagrama de la distribución por bloques del sistema de control. El diagrama de bloques mostrado pasa a quedar de la siguiente forma en la simulación. 2 “[…] son aquellos con menos actuadores que grados de libertad”. Castro Salguero, R. (2001) Sistemas mecánicos subactuados. Universidad Nacional de Ingeniería, Facultad de Ingeniería Eléctrica y Electrónica. Perú. (Tesis doctoral). 3 Situación en la que el aparato se encuentra volando a una altura constante y sin que se den desplazamientos o giros en ninguno de los ejes. Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 9 Fig. 2 Diagrama de la simulación en el que se muestra la distribución por bloques del sistema de control. El objetivo de este bloque es obtener los ángulos de rotación “roll” y “pitch” necesarios para realizar las traslaciones en el plano XY. El control de las tres componentes de rotación se realiza, finalmente, en el control de situación. Para obtener el sistema de control, se calcularan varios reguladores obtenidos por distintos métodos para el control de altitud, de los cuales se elegirá el que mejores prestaciones tenga: no haya sobreoscilaciones y el tiempo de respuesta sea el más bajo. Una vez elegido, se aplicará el mismo procedimiento al resto de entradas. Sin embargo, hay que tener en cuenta que en el control de Z se realiza un control lineal sobre una variable que es cuadrática, w2: el empuje hacia arriba depende de forma directamente proporcional de ésta, mientras que el resto de entradas depende de w. Fig. 3 Vista de la representación del quadrotor en la simulación. La interfaz gráfica permite la visualización tanto de la trayectoria en 3D seguida como las gráficas correspondientes a las acciones de los reguladores, las velocidades de los motores y las evoluciones temporales de las coordenadas. Otra función que cumple, Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 16 Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 17 3.- Caracterización Experimental del Sistema. A continuación se describen los experimentos realizados para obtener tanto los parámetros que describen la dinámica del sistema como las características de la función de transferencia, datos necesarios para poder realizar un control de trayectoria con cierta calidad. Aunque no todos los ensayos se han realizado antes de adentrarnos en parte de control del proyecto, se ha optado por describirlos en esta sección para mayor claridad y orden de la memoria. 3.1.- Signo de la ganancia. En este experimento se trata de obtener el signo de la ganancia de la función de transferencia. Si ésta es positiva, al introducirle una velocidad angular positiva, el movimiento del quadrotor será en sentido ascendente. Para realizarlo, hay que tener en cuenta que la velocidad debe hacer que el sistema se eleve o, en caso contrario, el sistema caerá al suelo. En este caso, se le introduce una constante en la entrada T de 900. Al no interesar que se den desplazamientos en el plano XY ni se den cambios en la orientación sobre el eje Z, al resto de entradas se les introduce una constante de valor nulo. Fig. 7 Velocidad angular constante positiva por la entrada T. Este valor hace que el sistema se desplace hacia arriba, lo que indica que la ganancia del sistema es positiva. 3.2.- Obtención experimental de cT. En este ensayo se trata de hacer que el sistema esté en situación de sustentación: mantenerse en una posición de forma constante en el tiempo. En dicha situación, según Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 18 el artículo de Peter Corke y compañeros de investigación, el empuje total del sistema es, siendo las otras entradas nulas: 𝑇Σ= 4 ∗𝑐𝑡∗ 𝑤02=𝑚∗𝑔 siendo w0 la velocidad que deben tener los motores en dicha situación, m la masa del vehículo, g la aceleración de la gravedad y cT el coeficiente de empuje, cuyo valor es el que se busca conocer. Con este experimento lo que realmente se obtiene es la velocidad angular de los motores a la cual el sistema se mantiene sustentado en el aire, a partir del cual, despejando el parámetro de la expresión del empuje total, hallará el valor deseado. Para ello, realizamos un control de altura, es decir, no deben darse movimientos en X o en Y, de forma que el sistema se mantenga en un valor de Z constante. Dado que no sabemos el tipo del sistema, se ha obtado por utilizar el método Ziegler-Nichols en bucle cerrado ya que nos permite calcular un regulador sin necesidad de conocer en profundidad el sistema. Este procedimiento se basa en, utilizando regulador proporcional, buscar la ganancia que haga que la respuesta del sistema sea una oscilación sinusoidal. Con este valor y con el periodo de la onda resultante, obtendremos los coeficientes necesarios para implementar o un PI o un PID. En este caso, manteniendo la cte de 900 para que el sistema se eleve del suelo, obtenemos que para una ganancia de 4.5, el sistema oscila con un periodo crítico de 16.3s. A partir de aquí podemos calcular los coeficientes para cualquiera de los dos reguladores sin ninguna dificultad. En este caso, se ha optado por un control PID dado que su acción inicial, proporcionada por la parte derivativa, hace que el empuje en t=0 tenga un alto valor de forma que se mueva rápidamente en el eje Z. Aplicando Ziegler-Nichols, los parámetros resultantes son: 𝐾𝑝=4.5 2.22 � 𝑇𝑖= 𝑇𝑐2 �= 16.3 2 � Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 19 𝑇𝑑= 𝑇𝑐8 �=16.3 8 � Aunque se ha optado por un PID, al poder disponer de la salida dz/dt, el esquema finalmente empleado es un PI-D. Fig. 8 Bucle de control con PI-D de la altura con una consigna de 5. Una vez implementado esto, al quitar la constante prealimentada, se aprecia que la acción no es suficiente en los momentos iniciales para hacer que el sistema se eleve. Según Ziegler-Nichols, el valor de la ganancia es acomodable posteriormente a la aplicación del regulador sobre el sistema, por lo que se multiplica los coeficientes del regulador por una constante obtenida de forma experimental de valor 175. Fig. 9 Evolución de Z en función del tiempo. Finalmente, se lanza la simulación y, tras ésta, se obtiene la velocidad de sustentación una vez que el quadrotor se encuentra a una altura constante, en este caso de un valor de 5m. El valor de w0 es de 860.9857 rad/s. A partir de ahí: 𝑐𝑇=𝑚∗𝑔 4∗𝑤02=4∗9.81 4∗(860.9857)2= 1.3234 ∗10−5 Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 20 3.3.- Obtención experimental de cQ. Para la obtención de éste parámetro se obliga al quadrotor a girar sobre su eje Z al aplicar un empuje en la entrada τyaw del sistema, aprovechando el control utilizado en el anterior experimento. Fig. 10 Obtención experimental de cQ aplicando un valor constante en τyaw. Al iniciar la simulación, el aparato empieza a girar conforme empieza a elevarse hasta que el control hace que se mantenga a la altura de referencia, sin que deje de rotar sobre su eje en vertical. Sin embargo, al analizar la aceleración obtenida en yaw, se puede ver que, tras una elevación inicial de la aceleración en escalón y un posterior descenso, está aumenta de forma cuasi lineal. Este resultado no se corresponde con lo obtenido con un aparato físico, dado que en ese caso la fricción viscosa del aire haría que ésta fuera decreciendo hasta anularse, al alcanzarse una velocidad de giro constante. Fig. 11 Gráfica correspondiente a d2yaw/dt2, aplicando un valor constante en τyaw. A la luz de estos resultados, se hicieron otras pruebas introduciendo un pulso y un escalón en dicha entrada, estando el quadrotor a una altura constante, dando lugar a un comportamiento semejante. Fig. 12 Gráfica correspondiente a d2yaw/dt2, aplicando un escalón en t=15s. Fig. 13 Gráfica correspondiente a d2yaw/dt2, aplicando un pulso en t=5s de 15s de duración. Finalmente, se obtuvo el valor inicial de la aceleración, el resultante de la aplicación del escalón, para calcular el valor del parámetro cQ al ser éste el dato que realmente nos interesa con independencia del comportamiento posterior que exhiba el sistema. Sabemos que el par motor es, aplicando solamente en τyaw: 𝑀𝑚(𝑡)=−𝐼𝑧𝑧∗𝑑2𝑦𝑎𝑤 𝑑𝑡2=𝜏3=𝑐𝑄∗�𝜎𝑖 4 𝑖=1 ∗𝑤𝑖2 siendo σi=[-1,+1] para indicar el sentido de giro de las aspas de cada rotor: -1 para sentido anti horario y +1 para sentido horario; siendo Izz=0.1490 la inercia del aparato en Z. Dada la estructura interna del “control mixer”, el controlador de los motores del quadrotor, la expresión resultante del par motor queda: 𝑀𝑚=𝑐𝑄∗(𝑤22+𝑤42−𝑤12−𝑤32) Que, al aplicar una τyaw=10 en t=15s, teniendo el motor una velocidad angular de w=869’7955 rad/s en dicho instante, pasa a ser de la forma 𝑀𝑚=𝑐𝑄∗((𝑤−10)2+(𝑤−10)2−(𝑤+10)2−(𝑤+10)2) 𝑀𝑚=𝑐𝑄∗(2∗(𝑤−10)2−2∗(𝑤+10)2)=𝑐𝑄∗2∗(−40𝑤)=−𝐼𝑧𝑧∗𝑑2𝑦𝑎𝑤 𝑑𝑡2𝑡=15 siendo 𝑑2𝑦𝑎𝑤 𝑑𝑡2𝑡=15= 0.0494. Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 22 Finalmente, el valor de cQ, obtenido de esta forma, es de 1.0696*10-7. De forma teórica, utilizando para ello las expresiones dadas por el artículo [4] del que se parte para la realización del proyecto, se obtiene un valor de 1.0697*10-7. Dada la similitud entre ambos valores, se toma como válido el valor inicial de la aceleración en el eje yaw. Para dar más razones de peso a esta hipótesis, se ha consultado una de las obras de referencia citadas en el documento [4] en lo referente al modelado del quadrotor en la que dicen, respecto de las expresiones utilizadas de forma teórica para el cálculo de este parámetro, que “this is calculated on the basis of uniform inflow and no viscous losse”8. De ahí se deduce que, en base al modelado realizado, se dé esta situación en la simulación, la cual no se podría dar en un espacio físico real al no permitir las leyes de la física que un cuerpo posea una aceleración infinita. 3.4.- Identificación de los polos del sistema. En vista del resultado obtenido anteriormente (2.3), y como paso previo para el cálculo de un regulador para el control de altitud, el siguiente ensayo a realizar es determinar el tipo de sistema. El modelado de éste se realiza mediante la siguiente ecuación, en la que se cuenta la gravedad y la fricción viscosa del aire, en el campo transformado de Laplace: 4∗𝑐𝑇∗𝑤2(𝑠)−𝑚𝑔=(𝑚∗𝑠+𝑓)𝑠∗𝑥(𝑠) De dicha expresión, obtenemos la función de transferencia del sistema: 𝐺(𝑠)=𝑥(𝑠) 𝑤2(𝑠) 𝐺(𝑠) = 4∗𝑐𝑇 𝑠∗(𝑚∗𝑠+𝑓) Llegados a este punto, tenemos la que se supone que es la función de transferencia del quadrotor. Para comprobar si realmente es esa función de transferencia la que se ha 8 “Esto está calculado sobre la base de un flujo de entrada uniforme y sin pérdidas viscosas”. Traducido por Inmaculada Martín Blázquez. Leishman, J.G. (2006) Principles of helicopter Aerodynamics. Cambridge: Cambridge University Press. p67. Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 23 tenido en cuenta, se le aplica una w ligeramente mayor a la de sustentación y se espera a que se entre en un régimen permanente en el que la velocidad se mantenga constante. Fig. 14 Experimento para la determinación de los polos del sistema. Esto es así debido a que para la velocidad, 𝑑𝑥 𝑑𝑡 , el sistema es un primer orden, por lo que su respuesta ante un escalón sería: 𝑠(𝑡)=4∗𝑐𝑇 𝑚∗𝐴∗(1 −𝑒−𝑓 𝑚∗𝑡) siendo A la altura del escalón. Fig. 15 Representación gráfica de la velocidad ante una entrada de tipo escalón. Sin embargo, se ve que la respuesta tiene forma de rampa o, lo que es lo mismo, el sistema tiene un polo en el origen. Esto viene a confirmar del todo la hipótesis planteada anteriormente sobre la falta de consideración de la fricción viscosa del aire. Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 24 Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 25 4.- Control de trayectoria. En este apartado plantearemos las tres partes en que se compone en el control de trayectoria: el control de posición, el organizador y el control de situación9. En el primer bloque se encuentran encapsulados tanto el control de las coordenadas X e Y, cuya salida serán los ángulos “roll” y pitch”, como del control de altitud. El siguiente bloque, el “attitude planner”, se encarga de recoger tanto los ángulos de giro adecuados para realizar los traslados en el plano horizontal así como el valor de “yaw” establecido por el usuario. Finalmente, el “attitude controller” se encarga de realizar el control de éstos. Dichos ángulos son: - Roll: ángulo de giro sobre el eje X. - Pitch: ángulo de giro sobre el eje Y. - Yaw: ángulo de giro sobre el eje Z. Los cuales difieren de los utilizados tradicionalmente en robótica dado que en este tipo de robots se utilizan los sistemas de referencia de la navegación aérea. Fig. 16 Componentes de la orientación sobre los ejes de coordenadas cartesianas. El motivo de realizar esta división es obtener un sistema de control de trayectoria desacoplado, es decir, realizar el control de cada componente de forma individual. La ventaja de esto es que permite que dicho sistema sea más simple al no depender la acción de los reguladores de las otras variables. Sin embargo, esta división da lugar a un control más delicado en lo referente a los giros sobre los ejes horizontales: tanto “pitch” como “roll” influyen a la hora de realizar desplazamientos en el plano XY de forma que, si se da una fuerte inclinación sobre uno 9 “Position controller”, “Attitude planner” y “Attitude controller”. Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 32 amortiguadas, lo que conlleva a una ξ=1, y un tiempo de respuesta11 de 1s, para ello ωn=4.75. 𝐾1∗𝐾=𝜔𝑛2 𝐾1=𝜔𝑛2 𝐾=4.752 4∗𝑐𝑇𝑚 � siendo m=4Kg y cT=1.3234*10-5: K1=1.7049*106. Y, el cálculo de la segunda constante: 2𝜉𝜔𝑛=𝐾2∗𝐾= 2𝜔𝑛 𝐾2=2𝜔𝑛 𝐾=2𝜔𝑛 4∗𝑐𝑇𝑚 � 𝐾2=2𝜔𝑛𝑚 4𝑐𝑇= 7.1785 ∗105 Resumiendo, utilizando un servopropulsor obtenemos un sistema de segundo orden cuyo coeficiente de amortiguamiento y su frecuencia natural dependen de dos constantes; sus valores dependerán de las especificaciones a considerar. Una vez calculados, pasamos a ver su comportamiento con el drone. Fig. 24 Esquema del servopropulsor en simulink. Al aplicar este control a nuestro sistema vemos que, efectivamente, se cumple el tiempo de respuesta establecido en el diseño del regulador. La acción inicial es superior a 106. 11 Tiempo que tarda la salida en alcanzar una franja de valores entorno a un 5% del valor final. Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 33 Fig. 25 Respuesta del quadrotor con drone. Fig. 26 Acción del servopropulsor. 4.1.3.- Regulador proporcional integral derivativo (PID): método de Ziegler-Nichols en bucle cerrado. Este método es el mismo que se utilizó a la hora de realizar el experimento para obtener el valor de cT (2.2), por lo que no nos explayaremos mucho en su explicación. Fig. 27 Diagrama de bloques del regulador proporcional integral derivativo en serie, siendo Zd el valor introducido por el usuario. Dado que es un sistema con dos integradores, el sistema oscilara con una ganancia muy pequeña, sin embargo, debido a nuestro control lineal con w2 y a que las entradas al quadrotor se corresponden con w, utilizaremos un valor de ganancia relativamente elevado con vistas al regulador final. Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 34 El valor del periodo crítico es de 17.248s, por lo que los coeficientes del PID serán: 𝜏𝑖=17.248 2 𝜏𝑑=17.248 8 𝐾𝑝=10000 1.7 Siendo τi la constante de tiempo de la parte integradora, τd la constante de tiempo de la parte derivativa y Kp la parte proporcional del regulador. Con dichos valores, aplicamos el regulador al sistema obteniendo una respuesta subamortiguada. Fig. 28 Respuesta ante un escalón con un regulador PID. En aras de buscar la mejor respuesta posible, se aumenta el valor de la ganancia. A pesar de ello, la mejor respuesta posible es con cierto sobrepasamiento de la señal de consigna, para una ganancia del proporcional de 107, y conllevando acciones iniciales muy fuertes. Fig. 29 Respuesta ante un escalón con un PID con Kp=107. Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 35 Fig. 30 Acción del PID con una ganancia de 107 ante una entrada en escalón. 4.1.4.- Regulador obtenido aplicando técnicas frecuenciales. Para el cálculo del regulador utilizando métodos frecuenciales, lo primero que hay que hacer es representar el diagrama de Bode del sistema, G(s), a partir del cual conoceremos los aportes que necesita, tales como aumentos o decrementos de ganancia así como de fase para que se ajuste a las especificaciones cuyo cumplimiento se exijan. Fig. 31 Diagrama de Bode de G(s). Gráfica de arriba: diagrama de módulos; gráfica de abajo: diagrama de fases. En la gráfica se puede ver todo lo que ya sabemos sobre el sistema. El diagrama de módulos tiene forma de rampa con una pendiente de -40dB/déc, lo cual nos indica que hay dos integradores en el origen; el hecho de que no haya transiciones significa que no hay más polos o ceros en la función de transferencia. También se detecta la existencia de los integradores en el hecho de que el diagrama de fases es una línea recta de valor -180º. Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 36 Una vez analizado el diagrama de Bode de G(s), analizamos la estabilidad del sistema midiendo el margen de fase y el margen de ganancia. El primero es “el ángulo que debe girar la curva G(jω) con respecto al origen, para que el punto de cruce de la ganancia del lugar geométrico pase a través del punto (-1, j0)”12 de la traza de Nyquist mientras que el segundo parámetro es la distancia, en el diagrama de módulos, del punto de cruce de fases al punto (-1, j0). Según el signo de estos, el sistema será estable o inestable en bucle cerrado. Si son positivos ambos (ωf>ωg) el sistema será estable; en caso contrario, será inestable. Para analizar la estabilidad, en nuestro caso, no tenemos en consideración el margen de ganancia dado que este se determina de la forma 𝑀𝑔= 0𝑑𝐵−| G(jω)|Arg(G(jω))=−180º y en nuestro caso éste es positivo. Otra forma de entender el margen de fases es como el valor del argumento de G(jω), cuando su módulo vale 0dB, más 180º. 𝑀𝑓=𝐺(𝑗𝜔)|𝐺(𝑗𝜔)|=0+180º En conclusión, nuestro sistema es marginalmente estable. Necesitamos un aporte de fase para que el sistema sea estable: necesitamos un PAF. El regulador proporcional de avance de fase lo que provoca es un cambio en el diagrama de fases a la frecuencia que deseemos. 𝑅(𝑠)=1 + 𝜏𝑠 1 + 𝛼𝜏𝑠, 0 < 𝛼< 1 Como podemos ver, este regulador tiene un polo y un cero, siendo el cero más rápido que el polo. Esto se traduce, en términos frecuenciales, a que tiene dos frecuencias angulares, siendo la del polo mayor que la del cero. Es decir, éste último empieza a actuar antes que el primero. Eligiendo adecuadamente τ y α tendremos el aporte de fase que necesitamos en la banda frecuencial elegida. También provoca un aumento de la ganancia a altas frecuencias, aumentando la frecuencia de corte. 12 Piedrafita Moreno, R., Romeo Tello, A. (2009) Control Automático en los domionios frecuencial y de tiempo discretos. Zaragoza: Kronos Editorial. p57. Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 37 Concretamente, ω>>ωmáx: 20log10(1/α) ω=ωmáx: 10log(1/α) Las especificaciones de diseño establecidas para este regulador son: S.O.=0%, tr=1s y εp=0, el cual se cumple por las características del sistema. Una vez establecidas, el procedimiento de diseño es: 1. Determinar el Mf y la ωc para el régimen frecuencial transitorio que deseamos. Estos valores los obtendremos partiendo de las especifiaciones anteriormente descritas: 𝑆.𝑂. = 𝑒−𝜉𝜋 �1−𝜉2 2 �=0% 𝜉=1 � � � 𝑀𝑓𝑑=80º13 𝑡𝑟=4.75 𝜉𝜔𝑛= 1𝑠 𝜉=1 � � � 𝜔𝑛= 4.75 𝜔𝑐𝑑 𝜔𝑛=��4𝜉4+ 1 2−2𝜉2 2 𝜉=1 � � � 𝜔𝑐𝑑 𝜔𝑛= 0.4859 𝜔𝑐𝑑= 2.3079 𝑟𝑎𝑑𝑠 � 2. Parametrización del PAF. Para aprovechar de forma eficiente el máximo pico de fase que aporta el regulador, hacemos que ωcd=ωmáx, pasando a ser la frecuencia de corte deseada. 𝜙𝑚á𝑥= 𝑀𝑓𝑑−𝑀𝑓𝐴𝑟𝑔�𝐺�𝑗𝜔=𝑗𝜔𝑐𝑑��=80º−0 = 80º 𝛼=1−sin 𝜙𝑚á𝑥 1 + sin 𝜙𝑚á𝑥= 0.076543 𝜏=1 𝜔𝑚á𝑥√𝛼 2= 4.9526 3. Ajustar la ganancia en función del εp. En nuestro caso no hace falta, dado que éste es nulo al tener dos integradores en cadena directa. 13 Con un margen de fase de 75º sería suficiente pero, para asegurarnos el régimen transitorio, le sumamos un coeficiente de seguridad de ζ=5º. Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 38 Una vez hecho todo esto, comprobamos como ha cambiado el diagrama de Bode del nuevo sistema. Fig. 32 Diagrama de Bode del sistema incluyendo el PAF. Gráfica de arriba: diagrama de módulos; gráfica de abajo: diagrama de fases. Vemos que en el diagrama de fases aparece un pico cuyo valor máximo, de -100º, se da para la frecuencia de corte que hemos establecido nosotros durante su parametrización. Como podemos ver, el valor del módulo para dicha frecuencia no es nulo, sino que vale -91dB aprox, por lo que tenemos que realizar el último paso para obtener definitivamente el regulador. 4. Ajuste del diagrama de módulos. Ahora lo que tenemos que hacer es añadir una ganancia que haga que para la ωcd se dé, efectivamente, un margen de fase de 80º. En nuestro caso, lo que tenemos que hacer es subir el diagrama de módulos, es decir, añadir una ganancia positiva. |𝐺(𝑗𝜔)|𝜔=𝑤𝑐𝑑= 0𝑑𝐵 ⟺ |𝐺(𝑗𝜔)|𝜔=𝑤𝑐𝑑+20 log10𝐾𝑟= 0𝑑𝐵 |𝐺(𝑗𝜔)|𝜔=𝑤𝑐𝑑+20 log10𝐾𝑟=−91 +20 log10𝐾𝑟= 0𝑑𝐵 𝐾𝑟=109120 �=35481 Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 39 Fig. 33 Diagrama de Bode del sistema incluyendo el PAF ajustado. Gráfica de arriba: diagrama de módulos; gráfica de abajo: diagrama de fases. Finalmente, en el diagrama de módulos del sistema con el regulador proporcional de avance de fase obtenido, tras el ajuste del diagrama de módulos, se puede apreciar que realmente el margen de fase es de 80º. Tras todo esto, sólo queda estudiar el comportamiento en bucle cerrado ante un escalón. En la gráfica se ve que hay una cierta sobreoscilación, siendo el tiempo de respuesta mayor que el establecido por nosotros. Fig. 34 Respuesta ante un escalón del drone con la inclusión del PAF en bucle cerrado. 4.1.5.- Elección del regulador. Tras analizar los diferentes tipos de reguladores propuestos, se ve claramente que el regulador que mejores prestaciones da es el servopropulsor. El tiempo de respuesta del Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 40 sistema es de 1s, el que hemos impuesto nosotros, y sin que haya sobreoscilaciones ni se produzcan errores de ejecución en el desarrollo de la simulación. Del resto de reguladores, el único que se le acerca a ese tiempo de respuesta es el PAF y, aun así, lo hace con un sobrepasamiento de la señal de consigna. Si tomamos en consideración las acciones iniciales, los cuatro aportan valores muy elevados de estas, sin contar la acción que introducimos nosotros con la realimentación de w02. Sin embargo, dado que no se va a implementar de forma física, no es un parámetro importante a la hora de elegir el corrector adecuado. Esto mismo es aplicable a los valores de las ganancias. Sin embargo, para complementar el regulador elegido, vamos a implementar una estructura de control por prealimentación denominado “servomecanismo”. Lo que conseguimos con estructura es que la salida siga lo más rápidamente posible las variaciones de la entrada cuando éstas se produzcan, al adicionarse una acción dependiente de éstas a la del regulador principal. Fig. 35 Diagrama de bloques de un servomecanismo en bucle cerrado. La entrada, Zd, es el valor dado por el usuario. Al introducir la prealimentación, la salida del sistema queda de la forma 𝑆(𝑠)=𝑅(𝑠)𝑝+𝑅(𝑠) 1 + 𝑅(𝑠)𝐺(𝑠)∗𝐺(𝑠)𝐸(𝑠)+1 1 + 𝑅(𝑠)𝐺(𝑠)∗𝑃(𝑠) Esto influye al regulador presente en la prealimentación en la forma de que se debe cumplir que su función de transferencia sea la unidad. 𝑅(𝑠)𝑝+𝑅(𝑠) 1 + 𝑅(𝑠)𝐺(𝑠)∗𝐺(𝑠)= 1 ⟺ 𝑅(𝑠)𝑝=1 𝐺(𝑠) ; 𝐺(𝑠)=𝐾𝑠2 � ⇒ 1 𝐺(𝑠)=𝑠2 𝐾 Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 41 Para no ralentizar la simulación en demasía, se procederá a calcular la primera y la segunda derivada fuera de simulink y, después, se transferirán los datos resultantes a éste. El proceso a seguir se explicará en el apartado 6. 4.2.- Control de la componente de orientación “Yaw”. Lo primero que debemos hacer es obtener la función de transferencia correspondiente a la entrada τyaw. Para ello, partimos del empuje que provoca un giro en el eje Z 𝜏𝑦𝑎𝑤=𝑐𝑄∗(𝑤22+𝑤42−𝑤12−𝑤32)=𝑐𝑄∗(2𝑤22−2𝑤12) Ahora bien, si aplicamos empuje en dicha entrada cuando el quadrotor se encuentra en situación de sustentación, entonces queda de la forma 𝜏𝑦𝑎𝑤=𝑐𝑄∗�2�𝑤𝑦𝑎𝑤−𝑤0�2−2�𝑤𝑦𝑎𝑤+𝑤0�2�= 2 ∗𝑐𝑄∗�−4∗𝑤𝑦𝑎𝑤∗𝑤0� 𝜏𝑦𝑎𝑤=𝐼𝑧𝑧∗𝑑2𝑦𝑎𝑤 𝑑𝑡2 ⇒ 𝜏𝑦𝑎𝑤(𝑠)=𝐼𝑧𝑧∗𝑠2𝑦𝑎𝑤(𝑠) Igualando los términos, la función de transferencia resultante es 𝜓(𝑠) 𝑤𝑦𝑎𝑤(𝑠)=−8∗𝑤0∗𝑐𝑄 𝐼𝑧𝑧∗𝑠2=𝐾 𝑠2=−0.004945 𝑠2 En este caso, aunque al principio pueda parecer que se trata de un control sobre una variable cuadrática, como ocurre en el apartado 4.1, en realidad es un control lineal sobre una variable lineal. Es decir, sobre “w”. Tras este apunte, pasamos a obtener los coeficientes del servopropulsor, cuyo calculo está ya explicado en el subapartado correspondiente del control de altitud, con las mismas especificaciones: ξ=1 y tr=1. Fig. 36 Diagrama de bloques del regulador servopropulsor, siendo Yawd el valor deseado por el usuario. Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 48 Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 49 5.- Generador de trayectoria. Una vez transferidas las coordenadas de introducidas por el usuario a la interfaz gráfica, se procede a calcular la trayectoria a seguir para alcanzar dicha posición. En nuestro caso, el tipo de trayectoria será de tipo trapezoidal para la velocidad. Este tipo de trayectoria permite que los cambios de valor de ésta se den de forma menos brusca que si tuviera forma de escalón al darse que, en dichos tramos, la evolución temporal de la trayectoria se corresponde con un polinomio de segundo orden. Dicho de otra manera, es una trayectoria derivable tres veces, estando la aceleración compuesta por siete tramos. Fig. 44 Posición, velocidad y aceleración de entrada. Realmente, en este tipo de trayectoria, la aceleración tendría forma de escalón. Sin embargo, en la simulación no resulta así a que en el bloque de introducción de la segunda derivada en simulink también se activa la casilla correspondiente a “interpolación de datos” de forma que, cuando no disponga de datos, el programa “rellene” esos huecos. Esto hará que los escalones que realmente conforman la gráfica Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 50 de la aceleración, tengan una forma más parecida a un trapecio precisamente por la interpolación lineal que éste realiza. Como se procederá a explicar en apartados siguientes (6.2.7), los datos se recogen de los “edit text” de la interfaz cuando el usuario pulsa el botón de “Generar trayectoria”. Una vez obtenidos, se procede a realizar el cálculo de la trayectoria. T_rotacion_destino=eul2tr(yawusuario,0,0); %obtener matriz de rotación de destino % componer matriz: (R p) % (0 1) Tdestino=[T_rotacion_destino [xusuario,yusuario,zusuario]';[0,0,0,1]]; handles.Tdestino=Tdestino; Torigen=handles.Torigen; %componer trayectoria trayectoria_trf=ctrajTPZ(Torigen,Tdestino,vector_tiempos,aceleracionus uario); % obtener x, y,z trayectoria_x=trayectoria_trf(:,13); handles.trayectoria_x=trayectoria_x; % para que puedan ser utilizados por % otras funciones % añadimos dato al final para evitar problemas con la interpolacion de datos trayectoria_x=[trayectoria_x;xusuario]; assignin('base','x_trayectoria',trayectoria_x); trayectoria_y=trayectoria_trf(:,14); handles.trayectoria_y=trayectoria_y; % para que puedan ser utilizados por % otras funciones % añadimos dato al final para evitar problemas con la interpolacion de datos trayectoria_y=[trayectoria_y;yusuario]; assignin('base','y_trayectoria',trayectoria_y); trayectoria_z=trayectoria_trf(:,15); handles.trayectoria_z=trayectoria_z; % para que puedan ser utilizados por % otras funciones % añadimos dato al final para evitar problemas con la interpolacion de datos trayectoria_z=[trayectoria_z;zusuario]; assignin('base','z_trayectoria',trayectoria_z); % obtener yaw vector_yaws=[]; [filas,columnas]=size(trayectoria_trf); % para saber cuantas filas y % columnas tiene trayectoria_trf para el bucle for for i=1:filas matriz=reshape(trayectoria_trf(i,:),4,4); angulo=tr2eul(matriz); yawi=angulo(1)+angulo(3); vector_yaws=[vector_yaws,yawi]; end vector_yaws=[vector_yaws,yawusuario]; Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 51 assignin('base','yaw_trayectoria',vector_yaws'); % los vectores antes de añadir un punto mas para la interpolacion de datos % y que serán utilizados para el caculo de las velocidades y aceleraciones posicionx=handles.trayectoria_x; posiciony=handles.trayectoria_y; posicionz=handles.trayectoria_z; [filas,columnas]=size(posicionx); % para saber cuantas filas y % tiene trayectoria_trf para el bucle for % los tres tienen la misma longitud. Solo se calcula para uno. % bucle x. %calculo velocidad de x for i=2:filas; velocidadxi=(posicionx(i)-posicionx(i-1))/0.01; velocidadx(i-1)=[velocidadxi]; end % calculo aceleracion de x for i=2:filas-1; aceleracionxi=(velocidadx(i)-velocidadx(i-1))/0.01; aceleracionx(i-1)=[aceleracionxi]; end aceleracionx=[0,aceleracionx,0,0]; % añadimos un cero al principio para que % el primer valor sea nulo y dos al final para completar el vector y evitar % los problemas de interpolacion de datos assignin('base','x_aceleracion',aceleracionx'); % a workspace velocidadx=[0,velocidadx,0]; % lo mismo que con la aceleracion assignin('base','x_velocidad',velocidadx'); % a workspace % bucle y. % calculo velocidad de y for i=2:filas; velocidadyi=(posiciony(i)-posiciony(i-1))/0.01; velocidady(i-1)=[velocidadyi]; end % calculo aceleracion de y for i=2:filas-1; aceleracionyi=(velocidady(i)-velocidady(i-1))/0.01; aceleraciony(i-1)=[aceleracionyi]; end aceleraciony=[0,aceleraciony,0,0];% añadimos un cero al principio para que % el primer valor sea nulo y dos al final para completar el vector y evitar % los problemas de interpolacion de datos assignin('base','y_aceleracion',aceleraciony'); % a workspace velocidady=[0,velocidady,0]; % lo mismo que con la aceleracion assignin('base','y_velocidad',velocidady'); % a workspace % bucle z. % calculo velocidad de z for i=2:filas; velocidadzi=(posicionz(i)-posicionz(i-1))/0.01; velocidadz(i-1)=[velocidadzi]; end % calculo aceleracion de z Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 52 for i=2:filas-1; aceleracionzi=(velocidadz(i)-velocidadz(i-1))/0.01; aceleracionz(i-1)=[aceleracionzi]; end aceleracionz=[0,aceleracionz,0,0];% añadimos un cero al principio para que % el primer valor sea nulo y dos al final para completar el vector y evitar % los problemas de interpolacion de datos assignin('base','z_aceleracion',aceleracionz'); % a workspace velocidadz=[0,velocidadz,0]; % lo mismo que con la aceleracion assignin('base','z_velocidad',velocidadz'); % a workspace guidata(hObject, handles); % actualiza los valores de las variables Lo primero es, con el valor de yaw, obtener la matriz de rotación correspondiente al punto de destino y, una vez calculada, se obtiene la matriz de transformación de dicho punto componiéndola con la resultante de “eul2tr” y el vector columna correspondiente a la posición. Después se procede a guardar dichos datos en “handles.Tdestino”, para poder ser utilizados en otra función (6.2.2). Tras este paso, se establece el valor de “Torigen”, siendo su valor “handles.Torigen” (6.1 y 6.2.2), el cual tomará un valor en función de si es la primera ejecución o ya se ha realizado una previamente. Una vez obtenidas ambas transformaciones, junto con el vector de tiempos y el valor de la aceleración, se llama a la función “ctrajTPZ”. Esta función es la que compone la trayectoria cartesiana en una matriz de nx16, almacenándose dichos datos en la variable “trayectoria_trf”; siendo 16 el número correspondiente a la cantidad de elementos que hay en una matriz 4x4. Una vez obtenida ésta, vamos sacando los datos, primero, correspondientes a las tres coordenadas cartesianas, x,y,z. Tras esto, procedemos al cálculo de la componente de giro sobre z. Debido a que en este caso, respecto a los ángulos de Euler nos encontramos siempre en situación de singularidad15, el valor de “yaw” se divide entre el primer y el tercer ángulo de Euler, de ahí que realicemos un bucle “for” que va desde uno hasta el número de filas existentes en “trayectoria_trf” haciendo la suma de estos dos ángulos y almacenándolos dentro de un vector. Dichos vectores de almacenamiento de datos son enviados después al workspace de forma que simulink pueda utilizarlos a la hora de realizar la simulación. Para utilizarlos como señal de entrada, enviamos dichos datos al lugar de trabajo general en formato 15 El segundo y el tercer ángulo son de valor nulo. Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 53 columna (en el caso del vector de yaw) o, directamente, tal cual se obtienen de “trayectoria_trf”. Antes de esto, se guardan en variables de tipo “handles” para que puedan ser utilizados por otras funciones de la interfaz (6.2.7). Esta función también se encarga del cálculo de la primera y segunda derivada de las entradas de forma que no sobrecargamos simulink y provocamos, con ello, una ralentización de la ejecución de la simulación al realizar las prealimentaciones doblemente derivativas de consigna. Al igual que para el caso de “yaw”, se obtiene el número de filas que tiene uno de los vectores de posición y se utiliza ese valor para los bucles “for” donde se harán las diferencias hacía atrás entre los valores. Finalmente, se procede a guardar todas las variables “handles” utilizadas. Como curiosidad, al ser simulaciones infinitas, se ha tenido que añadir, tras el cálculo de los vectores, al final de los mismos, el último valor obtenido de forma que, al tener activada la casilla de interpolación de los datos del bloque de simulink, la trayectoria se mantenga constante para tiempos mayores a los establecidos por el usuario sin que se den incrementos lineales de ésta debido a dicha “orden”. En los casos de las velocidades y las aceleraciones, al ser vectores con una longitud menor que los de posición, se han rellenado con ceros al principio y, en el caso de las aceleraciones, también al final. Además, también se evita así la problemática de la interpolación de datos realizada por simulink que se ha mencionado anteriormente. 5.1.- Código completo del botón de “Generate trajectory”. En este apartado se muestra el código completo correspondiente al generador de trayectorias, el cual se explica tanto en 6.2.7 como 5. % --- Executes on button press in generartrayectoria. function generartrayectoria_Callback(hObject, eventdata, handles) % hObject handle to generartrayectoria (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) xusuario=get(handles.xdeseada,'String'); xusuario=str2num(xusuario); % pasar a número yusuario=get(handles.ydeseada,'String'); yusuario=str2num(yusuario);% pasar a número zusuario=get(handles.zdeseada,'String'); zusuario=str2double(zusuario); Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 54 %limitar z mín if zusuario<0.2 set(handles.MensajeError, 'String','ERROR. Zmín=0.2'); else set(handles.MensajeError, 'String',''); end yawusuario=get(handles.yawdeseada,'String'); yawusuario=str2num(yawusuario)*pi/180; %pasar a número y despues a radianes. duracionusuario=get(handles.duracion,'String'); duracionusuario=str2num(duracionusuario); %pasar a número vector_tiempos=[0:0.01:duracionusuario]; % crear vector de tiempos vector_tiempos2=[0:0.01:duracionusuario+0.01]; assignin('base','vec_tiempos',vector_tiempos2');%transferir aceleracionusuario=get(handles.aceleracion,'String'); aceleracionusuario=str2num(aceleracionusuario); %pasar a número T_rotacion_destino=eul2tr(yawusuario,0,0); %obtener matriz de rotación de destino % componer matriz: (R p) % (0 1) Tdestino=[T_rotacion_destino [xusuario,yusuario,zusuario]';[0,0,0,1]]; handles.Tdestino=Tdestino; Torigen=handles.Torigen; %componer trayectoria trayectoria_trf=ctrajTPZ(Torigen,Tdestino,vector_tiempos,aceleracionus uario); % obtener x, y,z trayectoria_x=trayectoria_trf(:,13); handles.trayectoria_x=trayectoria_x; % para que puedan ser utilizados por % otras funciones % añadimos dato al final para evitar problemas con la interpolacion de datos trayectoria_x=[trayectoria_x;xusuario]; assignin('base','x_trayectoria',trayectoria_x); trayectoria_y=trayectoria_trf(:,14); handles.trayectoria_y=trayectoria_y; % para que puedan ser utilizados por % otras funciones % añadimos dato al final para evitar problemas con la interpolacion de datos trayectoria_y=[trayectoria_y;yusuario]; assignin('base','y_trayectoria',trayectoria_y); trayectoria_z=trayectoria_trf(:,15); handles.trayectoria_z=trayectoria_z; % para que puedan ser utilizados por % otras funciones % añadimos dato al final para evitar problemas con la interpolacion de datos trayectoria_z=[trayectoria_z;zusuario]; assignin('base','z_trayectoria',trayectoria_z); % obtener yaw Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 55 vector_yaws=[]; [filas,columnas]=size(trayectoria_trf); % para saber cuantas filas y % columnas tiene trayectoria_trf para el bucle for for i=1:filas matriz=reshape(trayectoria_trf(i,:),4,4); angulo=tr2eul(matriz); yawi=angulo(1)+angulo(3); vector_yaws=[vector_yaws,yawi]; end vector_yaws=[vector_yaws,yawusuario]; assignin('base','yaw_trayectoria',vector_yaws'); % los vectores antes de añadir un punto mas para la interpolacion de datos % y que serán utilizados para el caculo de las velocidades y aceleraciones posicionx=handles.trayectoria_x; posiciony=handles.trayectoria_y; posicionz=handles.trayectoria_z; % bucle x. %calculo velocidad de x for i=2:1001; velocidadxi=(posicionx(i)-posicionx(i-1))/0.01; velocidadx(i-1)=[velocidadxi]; end % calculo aceleracion de x for i=2:1000; aceleracionxi=(velocidadx(i)-velocidadx(i-1))/0.01; aceleracionx(i-1)=[aceleracionxi]; end aceleracionx=[0,aceleracionx,0,0]; % añadimos un cero al principio para que % el primer valor sea nulo y dos al final para completar el vector y evitar % los problemas de interpolacion de datos assignin('base','x_aceleracion',aceleracionx'); % a workspace velocidadx=[0,velocidadx,0]; % lo mismo que con la aceleracion assignin('base','x_velocidad',velocidadx'); % a workspace % bucle y. % calculo velocidad de y for i=2:1001; velocidadyi=(posiciony(i)-posiciony(i-1))/0.01; velocidady(i-1)=[velocidadyi]; end % calculo aceleracion de y for i=2:1000; aceleracionyi=(velocidady(i)-velocidady(i-1))/0.01; aceleraciony(i-1)=[aceleracionyi]; end aceleraciony=[0,aceleraciony,0,0];% añadimos un cero al principio para que % el primer valor sea nulo y dos al final para completar el vector y evitar % los problemas de interpolacion de datos assignin('base','y_aceleracion',aceleraciony'); % a workspace velocidady=[0,velocidady,0]; % lo mismo que con la aceleracion assignin('base','y_velocidad',velocidady'); % a workspace Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 56 % bucle z. % calculo velocidad de z for i=2:1001; velocidadzi=(posicionz(i)-posicionz(i-1))/0.01; velocidadz(i-1)=[velocidadzi]; end % calculo aceleracion de z for i=2:1000; aceleracionzi=(velocidadz(i)-velocidadz(i-1))/0.01; aceleracionz(i-1)=[aceleracionzi]; end aceleracionz=[0,aceleracionz,0,0];% añadimos un cero al principio para que % el primer valor sea nulo y dos al final para completar el vector y evitar % los problemas de interpolacion de datos assignin('base','z_aceleracion',aceleracionz'); % a workspace velocidady=[0,velocidady,0]; % lo mismo que con la aceleracion assignin('base','z_velocidad',velocidadz'); % a workspace guidata(hObject, handles); % actualiza los valores de las variables Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 57 6.- Interfaz gráfica de usuario. Una vez terminado el diseño del sistema de control de trayectoria, pasamos a crear una interfaz de operación para un posible usuario que permita una fácil supervisión de la actuación del quadrotor, sin necesidad de tener amplios conocimientos sobre su dinámica ni del control de situación y de utilizar la pantalla de simulación. Para ello, seguimos utilizando el programa MATLAB pero la extensión GUI16 para realizar dicha interfaz. En este apartado se tratará de explicar el código del programa relevante a la hora de obtener las gráficas, dado que al colocar bloques tales como “axes” o “edit text” en el archivo “.Fig” se crean funciones en el archivo “.m” de creación de creación de dichos bloques, las cuales no tienen mayor utilidad que el hecho de poder utilizar dichos bloques para escribir textos o dibujar gráficas en ellos dependiendo, por ejemplo, de la opción elegida en un menú. Fig. 45 Interfaz gráfica. 6.1.- Inicio de la interfaz y de la simulación. Al abrir la interfaz, también se abre la simulación. Esto se consigue creando una función que haga dicha tarea y que sea llamada desde la función de inicio de la GUI. Tras esto, se carga el archivo “puma560”, el cual sólo sirve para poder utilizar las funciones empleadas en el cálculo de la trayectoria. 16 Graphical Users Interface. Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 64 6.2.5.- Botón de “Rotate” (para la gráfica de representación de la trayectoria). Al igual que en el botón anterior pero habilitando, en este caso, el poder realizar rotaciones en la gráfica en caso de que se pulse dicho botón. Sin embargo, en caso de que se hubiera hecho un zoom anteriormente, mantiene la vista sin modificar. % --- Executes on button press in Rotate. function Rotate_Callback(hObject, eventdata, handles) % hObject handle to Rotate (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) rotate3d(handles.Trayectoria3D, 'on'); zoom(handles.Trayectoria3D,'off'); Fig. 49 Visualización de “rotate” en la gráfica de trayectoria. 6.2.6.- Botón de “Reset View” (para la gráfica de representación de la trayectoria). Este botón permite volver a la apariencia inicial de la gráfica de representación de la trayectoria y desactiva la posibilidad de realizar las anteriores operaciones. % --- Executes on button press in ResetView. function ResetView_Callback(hObject, eventdata, handles) % hObject handle to ResetView (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) dibujartrayectoria(handles); zoom(handles.Trayectoria3D,'off'); rotate3d(handles.Trayectoria3D, 'off'); 6.2.7.- Botón de “Generate Trajectory” Al pulsar el botón “Generate Trajectory” lo que se hace es recoger los datos introducidos en los “edit text” por el usuario, pasarlos a formato numérico y utilizarlos para calcular la trayectoria. Dado que es dentro de esta función donde se realiza el cálculo de la trayectoria, se explicará en este apartado lo correspondiente a la adquisición de los datos. El cálculo de Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 65 la trayectoria se ha descrito en el apartado correspondiente al generador de trayectorias propiamente dicho (5). % --- Executes on button press in generartrayectoria. function generartrayectoria_Callback(hObject, eventdata, handles) % hObject handle to generartrayectoria (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) xusuario=get(handles.xdeseada,'String'); xusuario=str2num(xusuario); % pasar a número yusuario=get(handles.ydeseada,'String'); yusuario=str2num(yusuario);% pasar a número zusuario=get(handles.zdeseada,'String'); zusuario=str2double(zusuario); %limitar z mín if zusuario<0.2 set(handles.MensajeError, 'String','ERROR. Zmín=0.2'); else set(handles.MensajeError, 'String',''); end yawusuario=get(handles.yawdeseada,'String'); yawusuario=str2num(yawusuario)*pi/180; %pasar a número y despues a radianes. duracionusuario=get(handles.duracion,'String'); duracionusuario=str2num(duracionusuario); %pasar a número vector_tiempos=[0:0.01:duracionusuario]; % crear vector de tiempos vector_tiempos2=[0:0.01:duracionusuario+0.01]; assignin('base','vec_tiempos',vector_tiempos2');%transferir aceleracionusuario=get(handles.aceleracion,'String'); aceleracionusuario=str2num(aceleracionusuario); %pasar a número Lo primero que hacemos es recoger los datos de x e y y pasarlos de string a número. Después procedemos a obtener el valor de z introducido por el usuario y se hace la comprobación de que éste es mayor que 0.2m con un bucle “if”: si es menor, aparece un mensaje de error indicando la altura mínima permitida (6.2.1). Fig. 50 Mensaje de error. Tras esto, recogemos el valor de la “yaw” deseada y se pasa a radianes. Con esto terminamos la parte correspondiente a la obtención de la posición y orientación, respecto de Z elegida. Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 66 Los otros dos datos que se necesitan son el tiempo en el que se quiere hacer la traslación y la aceleración que el usuario desea. Con el valor de la duración, se crean dos vectores de tiempos, uno que se lanzará al workspace de MATLAB (el cual será utilizado por simulink) y otro que se utilizará para el cálculo de la trayectoria (5). Respecto a la aceleración, puede darse un error que no aparezca en la pantalla de la interfaz gráfica sino que aparece en la ventana de comandos de MATLAB debido a que la aceleración y el tiempo requeridos no sean acordes el uno con el otro. Es decir, que se requiera una aceleración relativamente elevada y un tiempo relativamente bajo, de forma que la forma de la gráfica de la aceleración sea un triángulo y no un trapecio: acelera con pendiente casi infinita; por lo que se recomienda al usuario que no pierda de vista dicha pantalla. 6.2.8.- Botón “Go home”. Lo que hace este botón es volver al punto inicial y poner en posición y dibujo por defecto los menús y las gráficas. % --- Executes on button press in posicion_home. function posicion_home_Callback(hObject, eventdata, handles) % hObject handle to posicion_home (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) %establecer condiciones iniciales en simulink z0 = [0 0 -0.15]; % z0 Position initial conditions 1x3 n0 = [0 0 0]; % n0 Ang. position initial conditions 1x3 v0 = [0 0 0]; % v0 Velocity Initial conditions 1x3 o0 = [0 0 0]; % o0 Ang. velocity initial conditions 1x3 init = [z0 [n0 v0 o0]]; assignin('base','xInitial',init); handles.Torigen=[eye(3,3) [0 0 0.15]';0,0,0,1]; set(handles.SelectorCoordenadas,'value',1); SelectorCoordenadas_Callback(hObject, eventdata, handles) set(handles.SelectorAcciones,'value',1); SelectorAcciones_Callback(hObject, eventdata, handles); zoom(handles.Trayectoria3D,'off'); rotate3d(handles.Trayectoria3D, 'off'); diagramaen3d(handles); guidata(hObject, handles); % actualiza los valores de las variables Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 67 6.3.- Mostrar las gráficas. Aquí se explicarán las líneas de código correspondientes al trazado de las evoluciones temporales de las coordenadas X, Y, Z y Yaw así como las de las acciones de los reguladores y las velocidades de los motores. También se procederán a explicar a las correspondientes a la visualización de la trayectoria en 3D. 6.3.1.- Visualización de las coordenadas X, Y, Z y de la componente de orientación Yaw. El mostrar una u otra gráfica se realiza en función de la opción elegida en un menú de tipo “pop-up”, la cual se realiza con la instrucción “switch… case”. Los casos enumerados son los que se corresponden con las posiciones que ocupan en el menú los nombres de las componentes de la trayectoria que pueden elegirse para su representación, mientras que el caso denominado “otherwise” está asignado a lo que se muestra en la opción “-”, que es la que resulta por defecto al abrirse o cerrarse la interfaz y que muestra un círculo cuando no se elige ninguna de las otras opciones. Al elegir una de las opciones, se recogen los datos transferidos de simulink al workspace y se utilizan para dibujar la gráfica correspondiente. Debido al código de la simulación, la componente Z de salida del bloque “quadrotor” es negativa, por lo que cogemos el valor de dicha variable de un osciloscopio, el cual guarda los datos como “structure with time” que tiene como entrada el valor absoluto de Z. El resto se recogen de la variable de almacenamiento de simulink “result”, el cual es una estructura de tipo array18. En estas gráficas también se visualiza la referencia para poder compararla con la evolución de la salida. Para que no se borre de la gráfica la representación de la referencia al dibujar la coordenada que se desea ver, se utiliza la instrucción “hold(handles.CoordenadasGraf,'on');”. Al inicio o en caso de que se elija otra coordenada, se deshace el “hold” para que se puedan trazas las siguientes sin superponerse con la elección anterior. function SelectorCoordenadas_Callback(hObject, eventdata, handles) % hObject handle to SelectorCoordenadas (see GCBO) 18 “result(1:end,1)” significa que los datos que interesan son los de la fila 1 hasta la última de la columna 1. Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 68 % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % Hints: contents = cellstr(get(hObject,'String')) returns SelectorCoordenadas contents as cell array % contents{get(hObject,'Value')} returns selected item from SelectorCoordenadas coord=get(handles.SelectorCoordenadas,'value'); hold(handles.CoordenadasGraf,'off'); switch coord case 2 tiempo=evalin('base','result(1:end,1)'); %Pasa de workspace a gui X=evalin('base','result(1:end,2)'); %Pasa de workspace a gui x_ref=evalin('base','x_referencia.signals.values'); %Pasa de workspace a gui plot(handles.CoordenadasGraf,tiempo,x_ref); %dibuja en el axes CoordenadasGraf hold(handles.CoordenadasGraf,'on'); plot(handles.CoordenadasGraf,tiempo,X,'r'); %dibuja en el axes CoordenadasGraf grid(handles.CoordenadasGraf); case 3 tiempo=evalin('base','result(1:end,1)'); %Pasa de workspace a gui Y=evalin('base','result(1:end,3)'); %Pasa de workspace a gui y_ref=evalin('base','y_referencia.signals.values'); %Pasa de workspace a gui plot(handles.CoordenadasGraf,tiempo,y_ref); %dibuja en el axes CoordenadasGraf hold(handles.CoordenadasGraf,'on'); plot(handles.CoordenadasGraf,tiempo,Y,'r'); %dibuja en el axes CoordenadasGraf grid(handles.CoordenadasGraf); case 4 tiempo=evalin('base','Zr.time'); %Pasa de workspace a gui Z=evalin('base','Zr.signals.values'); %Pasa de workspace a gui z_ref=evalin('base','z_referencia.signals.values'); %Pasa de workspace a gui plot(handles.CoordenadasGraf,tiempo,z_ref); %dibuja en el axes CoordenadasGraf hold(handles.CoordenadasGraf,'on'); plot(handles.CoordenadasGraf,tiempo,Z,'r'); %dibuja en el axes CoordenadasGraf grid(handles.CoordenadasGraf); case 5 tiempo=evalin('base','result(1:end,1)'); %Pasa de workspace a gui Yaw=evalin('base','result(1:end,5)'); %Pasa de workspace a gui yaw_ref=evalin('base','yaw_referencia.signals.values'); %Pasa de workspace a gui plot(handles.CoordenadasGraf,tiempo,yaw_ref); %dibuja en el axes CoordenadasGraf hold(handles.CoordenadasGraf,'on'); plot(handles.CoordenadasGraf,tiempo,Yaw,'r'); %dibuja en el axes CoordenadasGraf grid(handles.CoordenadasGraf); otherwise dibujarcirculoCoordenadasGraf(handles) end Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 69 6.3.2.- Visualización de las acciones y velocidades angulares de los motores. En este caso, al elegir una opción del menú, no sólo se representa la gráfica sino que aparece un mensaje de texto debajo explicando a que corresponde cada gráfica. % Función del selector de acciones para mostrar uno u otro mensaje dependiendo de la % acción del regulador elegida. % --- Executes on selection change in SelectorAcciones. function SelectorAcciones_Callback(hObject, eventdata, handles) % hObject handle to SelectorAcciones (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % Hints: contents = cellstr(get(hObject,'String')) returns SelectorAcciones contents as cell array % contents{get(hObject,'Value')} returns selected item from SelectorAcciones set(handles.s, 'String','No se ha seleccionado ninguna Acción'); v=get(handles.SelectorAcciones,'value'); switch v case 2 set(handles.s, 'String','Acción correspondiente al control de altitud'); tiempo=evalin('base','T.time'); %Pasa de workspace a gui T=evalin('base','T.signals.values'); %Pasa de workspace a gui plot(handles.AccionesGraf,tiempo,T); %dibuja en el axes AccionesGraf grid(handles.AccionesGraf); case 3 set(handles.s, 'String','Acción correspondiente al control de posición en X'); tiempo=evalin('base','TauPitch.time'); %Pasa de workspace a gui TauPitch=evalin('base','TauPitch.signals.values'); %Pasa de workspace a gui plot(handles.AccionesGraf,tiempo,TauPitch); %dibuja en el axes AccionesGraf grid(handles.AccionesGraf); case 4 set(handles.s, 'String','Acción correspondiente al control de posición en Y'); tiempo=evalin('base','TauRoll.time'); %Pasa de workspace a gui TauRoll=evalin('base','TauRoll.signals.values'); %Pasa de workspace a gui plot(handles.AccionesGraf,tiempo,TauRoll); %dibuja en el axes AccionesGraf grid(handles.AccionesGraf); case 5 set(handles.s, 'String','Acción correspondiente al control de orientación respecto Z'); tiempo=evalin('base','TauYaw.time'); %Pasa de workspace a gui TauPitch=evalin('base','TauYaw.signals.values'); %Pasa de workspace a gui Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 70 plot(handles.AccionesGraf,tiempo,TauPitch); %dibuja en el axes AccionesGraf grid(handles.AccionesGraf); case 6 set(handles.s, 'String', 'Velocidad del motor 1'); tiempo=evalin('base','result(1:end,1)'); %Pasa de workspace a gui w1=evalin('base','result(1:end,14)'); %Pasa de workspace a gui plot(handles.AccionesGraf,tiempo,w1); %dibuja en el axes AccionesGraf grid(handles.AccionesGraf); case 7 set(handles.s, 'String', 'Velocidad del motor 2'); tiempo=evalin('base','result(1:end,1)'); %Pasa de workspace a gui w2=evalin('base','result(1:end,15)'); %Pasa de workspace a gui plot(handles.AccionesGraf,tiempo,w2); %dibuja en el axes AccionesGraf grid(handles.AccionesGraf); case 8 set(handles.s, 'String', 'Velocidad del motor 3'); tiempo=evalin('base','result(1:end,1)'); %Pasa de workspace a gui w3=evalin('base','result(1:end,16)'); %Pasa de workspace a gui plot(handles.AccionesGraf,tiempo,w3); %dibuja en el axes AccionesGraf grid(handles.AccionesGraf); case 9 set(handles.s, 'String', 'Velocidad del motor 4'); tiempo=evalin('base','result(1:end,1)'); %Pasa de workspace a gui w4=evalin('base','result(1:end,17)'); %Pasa de workspace a gui plot(handles.AccionesGraf,tiempo,w4); %dibuja en el axes AccionesGraf grid(handles.AccionesGraf); otherwise set(handles.s, 'String','No se ha seleccionado ninguna Acción'); dibujarcirculoAccionesGraf(handles) end 6.3.3.- Visualización de la trayectoria. Esta función se ejecuta cuando es llamada por la función correspondiente al botón de stop tras parar la simulación. Lo que hace es recoger los datos de las coordenadas y dibujarlos en una gráfica 3D. Se recogen tanto los de la trayectoria seguida como los de la trayectoria deseada. Estos últimos proceden del generador de trayectorias (5). A pesar de que no se muestra en el código, para realizar una visualización en 3D en los objetos “axis” lo primero que hay que hacer modificar la característica “View” en el “Property Inspector”. En este caso, se ha introducido los valores [37.0 0.5]: 37.0 de azimut y 0.5 de elevación. Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 71 function dibujartrayectoria(handles) hold(handles.Trayectoria3D,'off'); X=evalin('base','result(1:end,2)'); %Pasa de workspace a gui Y=evalin('base','result(1:end,3)'); %Pasa de workspace a gui Z=evalin('base','Zr.signals.values'); %Pasa de workspace a gui plot3(handles.Trayectoria3D,X,Y,Z,'r'); hold(handles.Trayectoria3D,'on'); plot3(handles.Trayectoria3D,handles.trayectoria_x,handles.trayector ia_y,handles.trayectoria_z); grid(handles.Trayectoria3D); hold(handles.Trayectoria3D,'off'); Fig. 51 Visualización de la trayectoria tanto deseada (azul) como la realmente recorrida (roja), punto X=1, Y=1, Z=1 y ψ=90º, aunque no se visualice. Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 72 Caracterización empírica y diseño del sistema de control de trayectoria de un quadrotor mediante simulación. 73 7.- Conclusiones. Las conclusiones obtenidas del sistema de control son que tenemos un sistema con un tiempo de respuesta de 3s en cada variable y, en general, sin que se den sobreoscilaciones mayores a unos cuantos milímetros o milésimas de radian. La excepción puede ser cuando “yaw” toma un valor de 90º/-90º, donde nos encontramos en una situación de singularidad al producirse un cambio en la dependencia de los ángulos “roll” y “pitch” respecto de x/y, pudiendo darse oscilaciones un poco más acusadas. Fig. 52 Respuesta en el caso de singularidad, punto X=1, Y=1, Z=1 y ψ=90º (en la gráfica en radianes). Sin embargo, hay que tener en cuenta que la “buena” actuación de los reguladores se debe a que el drone se encuentra en condiciones ideales, y no sólo por la ausencia de fricción viscosa con el medio, sino por el hecho de está en una situación climática estable (no hay viento que afecte a su trayectoria, la temperatura podría decirse que se mantiene constante y, por lo tanto, la densidad del aire también) y no tenemos limitaciones en lo que a alimentación eléctrica se refiere. Además, de no tener en cuenta efectos aerodinámicos de segundo orden que, en una situación real, si afectarían tanto a la hora del seguimiento de la trayectoria como a la estrategia de control elegida. Respecto a la interfaz gráfica, se podrían haber añadido ciertas mejoras como la representación de las gráficas en tiempo real, el mantener los ejes al realizar un zoom sobre la gráfica de la trayectoria o que la ventana de la interfaz se maximice