Simulación y control de un quadrotor

Equation Chapter 1 Section 1 Trabajo de Fin de Grado Grado en Ingeniería de las Tecnologías Industriales Simulación y control de un quadrotor Autor: Rafael Luque Berraquero Tutor: Francisco Rodríguez Rubio Dep. Ingeniería de Sistemas y Automática Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2017 iii Trabajo de Fin de Grado Grado en Ingeniería de las Tecnologías Industriales Simulación y control de un quadrotor Autor: Rafael Luque Berraquero Tutor: Francisco Rodríguez Rubio Catedrático de Universidad Dep. Ingeniería de Sistemas y Automática Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2017 v Trabajo de Fin de Grado: Simulación y control de un quadrotor Autor: Rafael Luque Berraquero Tutor: Francisco Rodríguez Rubio El tribunal nombrado para juzgar el Proyecto arriba indicado, compuesto por los siguientes miembros: Presidente: Vocales: Secretario: Acuerdan otorgarle la calificación de: Sevilla, 2017 El Secretario del Tribunal vii A mi tutor, Francisco Rodríguez, por su tiempo y dedicación. A mi familia, por su apoyo incondicional. A mis compañeros. ix Agradecimientos En primer lugar, dar las gracias a mi familia, ellos me han animado y alentado en los momentos complicados a lo largo de toda mi vida y siempre confiaron en mí. Dar las gracias al profesor Francisco Rodríguez que me dio la oportunidad de realizar este trabajo y que me ha ayudado en todo lo que he necesitado. A mis profesores, porque de una manera u otra todos tienen una parte en este documento. Con todo ello mencionar y dar las gracias a mis compañeros y amigos por formar parte de este camino en el que hemos demostrado que con esfuerzo y dedicación nos podemos superar. Rafael Luque Berraquero Sevilla, 2017 6.1.10 Results 52 6.1.11 Reset 53 6.2 Simulador 54 6.2.1 Collision 54 6.2.2 Quadrotor plot 54 6.2.3 Scope 54 Referencias 55 xvii ÍNDICE DE TABLAS Tabla 3–1. Propiedades físicas X4-Flyer. 8 Tabla 4–1 Esquematización algoritmo A*. 27 xix ÍNDICE DE FIGURAS Figura 2-1. Esquiema completo del simulador. 3 Figura 2-2. Interfaz de usuario. 4 Figura 2-3. Esquema completo del simulador representado en Simulink. 4 Figura 2-4. Fotograma de simulación. 5 Figura 3-1. Ejemplo quadrotor real. 7 Figura 3-2. Notación rotores. 9 Figura 3-3. Sentido y velocidad de giro de los rotores necesario para realizar movimientos. 9 Figura 3-4. Notación sistemas de referencia inerciales y no inerciales. 10 Figura 3-5. Esquema modelo del quadrotor. 11 Figura 3-6. Representación de los ángulos de Tait-Brain. 11 Figura 3-7. Representación de la variación del ángulo de ataque. 13 Figura 3-8. Representación de los ángulos de inclinación del plano del rotor debido al efecto del “rotor flapping”. 13 Figura 3-9. Diagrama simulink correspondiente al bloque “Control mixer” donde se realiza la 15 operación matricial. Figura 3-10. El bloque “Quadrotor” engloba las ecuaciones que modelan al quadrotor. 16 Figura 4-1. Botón de la interfaz de usuario correspondiente a la ejecución del cálculo de trayectoria. 17 Figura 4-2. Botón de selección del entorno. 18 Figura 4-3. Ejemplo de representación de entorno, obstáculos y puntos final e inicial de la trayectoria. 19 Figura 4-4. Otro ejemplo de entorno con obstáculos, más complejo. 20 Figura 4-5. Selección de resolución de matriz de nodos en la interfaz de usuario. 20 Figura 4-6. Matriz de nodos. 21 Figura 4-7. Matriz de nodos tras eliminar los que no respetan la distancia de seguridad. 21 Figura 4-8. Selección de distancia de seguridad en la interfaz de usuario. 22 Figura 4-9. Evolución en grafo mediante algoritmo BFS. 22 Figura 4-10. Medida de longitud mediante el método Manhattan y el método euclídeo. 23 Figura 4-11. Exploración de los nodos mediante algoritmo BFS. 23 Figura 4-12. El avance en la exploración mediante el método BFS se asemeja a una ola. 24 Figura 4-13. a) Resultado final exploración con método BFS, mapa 1. b) Resultado final exploración con método BFS, mapa 2. 24 Figura 4-14. Evolución en grafo mediante algoritmo Dijkstra. 25 Figura 4-15. a) Avance en la exploración mediante el método Dijkstra. b) Resultado final exploración con método Dijkstra. 25 Figura 4-16. a) Resultado final exploración con método Dijkstra, mapa 1. b) Resultado final exploración con método Dijkstra, mapa 2. 26 Figura 4-17. Trayectoria más corta obtenida con el método BFS. 26 Figura 4-18. Evolución en grafo mediante algoritmo A*. 27 Figura 4-19. a) Resultado final exploración con método A*, mapa 1. b) Resultado final exploración con método A*, mapa 2. 28 Figura 4-20. Reconstrucción de la trayectoria más corta. 29 Figura 4-21. Ejemplo de trayectoria final sin suavizar. 29 Figura 4-22. Representación de los escalones presentes en una trayectoria sin suavizar. 31 Figura 4-23. Creación de malla y primer intento de unión. 31 Figura 4-24. Nueva creación de malla y segundo intento de unión. 32 Figura 4-25. Nueva creación de malla e intento de unión. 32 Figura 4-26. Representación del vector de unión entre dos nodos. 32 Figura 4-27. Repetición del proceso (creación de malla e intento de unión) desde nuevo nodo. 33 Figura 4-28. Representación del vector de unión desde el nuevo nodo con otro. 33 Figura 4-29. Representación del vector de unión con el nodo final. 33 Figura 4-30. Trayectoria final tras el segundo paso del suavizado. 34 Figura 4-31. Ejemplo de suavizado de trayectorias en otros entornos con obstáculos. 34 Figura 4-32. Se representa la fuerza de empuje (rojo) y los momentos resultantes del control en una trayectoria formada por 4 puntos (3 tramos rectos) para demostrar el problema que surge debido a la discontinuidad en las derivadas. 35 Figura 4-33. Notación para una trayectoria multi-segmentos. Los segmentos de color azul indican tramos de velocidad constante mientras que los rojos indican tramos de aceleración. 36 Figura 4-34. Se representa la fuerza de empuje (rojo) y los momentos resultantes del control en una trayectoria formada por 4 puntos (3 tramos rectos) para demostrar la mejora como consecuencia de la continuidad en derivadas. 36 Figura 4-35. Representación de la trayectoria tras suavizado e interpolación. 36 Figura 4-36. Representación de las variables de posición de referencia (x*- magenta, y*- rojo, z*- azul) y las realmente seguidas (x - amarillo, y - cian, z - verde), para una trayectoria con 4 puntos mínimos necesarios. 37 Figura 4-37. Selección del tiempo de simulación en la interfaz de usuario. 37 Figura 4-38. a) Se representa la fuerza de empuje (rojo) y los momentos resultantes del control en una trayectoria formada por 4 puntos (3 tramos rectos) para demostrar el problema que surge con la segunda forma de dar la referencia. b) Representación de las variables de posición de referencia (x*- magenta, y*- rojo, z*- azul) y las realmente seguidas (x - amarillo, y - cian, z - verde), para la misma trayectoria 38 Figura 5-1. Botón de la interfaz de usuario correspondiente a la ejecución de la simulación. 41 Figura 5-2. Detalle lazos de control interior y exterior. 42 Figura 5-3. Diagrama de bloques del regulador con doble lazo de realimentación, siendo la k1 la ganancia de la parte proporcional, la k2 la ganancia de la parte derivativa, G(s) la función de xxi transferencia del sistema para z y z* el valor de consigna. 44 Figura 5-4. Diagrama de bloques de un controlador con prealimentación. 45 Figura 5-5. Diagrama de bloques de un controlador anticipativo. 45 Figura 6-1 Representación trayectoria tridimensional real (roja) y deseada (azul). 52 Figura 6-2. Representación las fuerzas (empuje y momentos) ejercidos por el quadrotor. 53 Figura 6-3. Representación por separado de las variables de posición (x, y, z) reales y deseadas (con un *). 53 Figura 6-4. Funciones adicionales del simulador. 54 xxiii Notación AT Matriz traspuesta ℝ Todos los números reales sen Función seno cos Función coseno ∂y/∂x Derivada parcial de y respecto x A→B Del sistema de referencia A al sistema B 1 1 INTRODUCCIÓN os vehículos aéreos no tripulados (del inglés UAV’s – Unmanned Aerial Vehicles) son cada vez más comunes y presentan una enorme gama de tamaños y formas. Las aplicaciones de estos vehículos incluyen desde operaciones militares, vigilancia, espionaje, búsqueda y rescate, hasta investigaciones meteorológicas, filmación cinematográfica e investigación robótica (Corke P., 2011). Hasta hace poco tiempo, controlar un vehículo aéreo de manera autónoma era una tarea complicada debido a las limitaciones impuestas por el hardware hasta entonces existente. Lo que hizo posible la construcción de robots aéreos autónomos fue los recientes avances tecnológicos en actuadores y sensores en escala reducida, así como en el almacenamiento de energía y en el procesamiento de datos. Además, el diseño de controladores para este tipo de vehículos es complejo, debido principalmente a la dinámica presente en los sistemas aerodinámicos, los cuales son multivariables, subactuados y además presentan diversas características no lineales. Esto significa que las leyes clásicas de control lineales y monovariables pueden ver reducida su utilidad cuando se opera en condiciones no muy lejanas a las de equilibrio, ya que pueden aparecer inestabilidades. Por otra parte, las técnicas desarrolladas para robots totalmente actuados tampoco se aplican directamente al caso de sistemas mecánicos no lineales subactuados (Vianna G., 2007). Dentro de los UAV se pueden encontrar: - UAV’s de ala fija, que se basan en un principio similar al de los aviones de pasajeros compuestos por un propulsor o “jet” que proporciona el empuje delantero y unas alas, que permiten la sustentación, con una superficie controlable, para maniobrar. - UAV’s de alas rotativas, con una gran variedad de configuraciones entre las que se incluyen el helicóptero convencional con un rotor principal y otro en la cola; el “coax”, con rotores coaxiales contrarrotativos; y los “quadrotors” o helicópteros cuatrirrotor. En este documento se van a tratar los vehículos aéreos quadrotor, los cuales están ampliamente extendidos. Estos vehículos, en comparación con los helicópteros convencionales, presentan una serie de ventajas: son capaces de despegar verticalmente; son más fáciles de pilotar, modelar y maniobrar, pues el movimiento se consigue a través del accionamiento directo de los rotores, variando sus velocidades, mientras que, en un helicóptero convencional, la velocidad de giro de las hélices suele ser constante, controlando el movimiento mediante la variación de los ángulos de ataque de las palas, lo que los hace adecuados para el uso en laboratorio o como hobby; y poseen motores eléctricos en lugar de motores de combustión. Por contrapartida, presentan un aumento de peso y un aumento en el consumo de energía debido a los motores extras. Desde el punto de vista del control, la construcción de este tipo de helicóptero miniatura está lejos de simplificar el problema: más bien sucede lo contrario. Esto se debe a que los pares y fuerzas necesarios para controlar el sistema son aplicados no solo a través de efectos aerodinámicos, sino también a través del efecto de acoplamiento que aparece entre la dinámica de los rotores y la del cuerpo de la maqueta, como consecuencia del principio de acción-reacción originado en la aceleración y desaceleración de los grupos motor-hélice (efecto que no sucede en el control con velocidad de hélices constantes) (Vianna G., 2007). L Modelado 8 Tabla 3–1. Propiedades físicas X4-Flyer. Símbolo Descripción Valor g Gravedad 9.81 m/s2 ρ Densidad del aire 1.184 μ Viscosidad del aire 1.5*10-5 M Masa del quadrotor 4 kg J Matriz de inercia [0.082 0 0 0 0.082 0 0 0 0.149] kgm2 h Altura de los rotores respecto al CdG 0.007 m d Longitud de los brazos del quadrotor 0.315 m nb Número de aspas en cada rotor 2 r Radio del rotor 0.165 m c Longitud de cuerda del aspa 0.018 m e Offset del eje de giro 0 Mb Masa del aspa 0.005 kg Mc Masa estimada de la abrazadera del eje 0.01 kg ec Desplazamiento de la abrazadera del inicio del aspa 0.004 m Ib Momento de inercia del aspa Mb(r−ec)24 Ic Momento de inercia estimado de la abrazadera del eje Mc(ec)24 mb Momento estático del aspa g(Mcec 2+Mbr2) Ir Momento de inercia total del rotor nb(Ib+Ic) Ct Coeficiente adimensional de empuje 0.0048 Cq Coeficiente adimensional del torque Ct√(Ct/2) σ Relación de solidez del rotor 0.0694 θt Ángulo de la punta del aspa 6.8π 180 θ0 Ángulo del inicio del aspa 14.6π 180 θ1 Ángulo de torsión del aspa θt−θ0 θ75 ¾ del ángulo del aspa θ0+0.75θ1 θi Aproximación ideal el ángulo de inicio del aspa θt(re) a Pendiente del gradiente de la fuerza de sustentación 5.5 A Área del disco del rotor πr2 ɣ Número de bloqueo ρacr4 (Ib+Ic) b Constante de elevación CtρAr2 k Constante de resistencia aerodinámica CqρAr3 9 Simulación y control de un quadrotor El movimiento de un quadrotor se origina a partir de los cambios de velocidad de los rotores, los cuales giran en el mismo sentido dos a dos: los motores 1 y 3 giran en sentido horario mientras que el 2 y el 4 lo hacen en sentido contrario. Es decir, los que están en el mismo brazo giran en el mismo sentido. En función de la velocidad que estos posean, se pueden realizar diferentes desplazamientos a lo largo de los ejes, para algunos de los cuales se requerirán unas leves rotaciones respecto del sistema de referencia tomado desde el quadrotor, así como también, mantenerlo en sustentación. Figura 3-2. Notación rotores. Si lo que se requiere es hacer un movimiento en el eje Z, es decir, suba o baje, la velocidad de los motores deberá ser mayor o menor que la de sustentación (velocidad mínima necesaria para que el quadrotor se mantenga en equilibrio), pero de igual magnitud para todos ellos. El giro sobre dicho eje se obtiene a partir de la diferencia en el par de torsión entre cada par de rotores, o sea, se acelera los dos rotores con sentido horario mientras se desacelera los rotores con sentido anti-horario, y viceversa. Para realizar traslaciones respecto a los ejes X e Y, hay que hacer que uno de los motores de la rama correspondiente a la dirección que se pretende seguir gire a menos velocidad que el que esté enfrente: si se quiere mover sobre el eje X, el motor uno deberá girar más despacio que el motor 3. Esto provoca que se dé una rotación sobre el eje Y, modificando el empuje que hay en Z. Si el ángulo es muy pronunciado, éste no será suficiente para mantener en el aire el aparato, de forma que perderá altitud, pudiendo llegar a caer al suelo. Figura 3-3. Sentido y velocidad de giro de los rotores necesario para realizar movimientos. Realmente, la causa de esta pérdida son las limitaciones existentes respecto a las baterías que utilizan esta clase de robot como fuente de alimentación. En la situación descrita anteriormente, el sistema de control requiere una acción tan elevada que la alimentación no puede entregar la energía necesaria para que se dé dicha acción, de ahí la perdida de empuje y de altitud. Sumando a esto el hecho de que es muy difícil de implementar un control como el que podría llevar a cabo un piloto de forma manual, hace que sea complicado que el drone pueda realizar Modelado 10 maniobras arriesgadas como pueden ser giros de 360º sobre alguno de los ejes horizontales sin que acabe en el suelo (Martín I., 2012). A pesar de ello, este tipo de robots ofrecen la ventaja de tener una dinámica más simple que los de un solo rotor, además de mantenerse mejor en situación de sustentación al ejercerse el empuje con los cuatro rotores. 3.2 Descripción En esta sección se desarrollará el modelado basado en leyes físicas que describan la posición y orientación del helicóptero quadrotor. El modelo dinámico del helicóptero se presenta bajo la formulación matemática de Newton-Euler, siendo otra posible, la de Lagrange-Euler. Para obtener tal modelo dinámico se supone el vehículo como un cuerpo rígido en el espacio, sujeto a una fuerza principal (empuje) y tres momentos (pares), generados por los rotores, responsables del movimiento del aparato, como el de la siguiente figura. Figura 3-4. Notación sistemas de referencia inerciales y no inerciales. El par para generar un movimiento de balanceo (eje Y) o “roll” (ángulo ϕ) se realiza mediante un desequilibrio entre las fuerzas f2 y f4. Para el movimiento de cabeceo (eje X) o “pitch” (ángulo θ), el desequilibrio se realizará entre las fuerzas f1 y f3. El movimiento en el ángulo de guiñada o “yaw” (ángulo ψ) se realizará por el desequilibrio entre los conjuntos de fuerzas (f1, f3) y (f2, f4). Este movimiento será posible ya que los rotores 1 y 3 giran en sentido contrario a los rotores 2 y 4. Finalmente, el empuje total, que hará que el quadrotor se desplace perpendicularmente al plano de los rotores (eje Z), se obtendrá como suma de las cuatro fuerzas que ejercen los rotores (Vianna G., 2007). Estos tipos de vehículos son sistemas de vuelo de estructura ligera, por lo que el modelo dinámico deberá incluir los efectos físicos que actúan sobre el aparato: efectos aerodinámicos y pares inerciales opuestos (debidos a la rotación de las hélices), efectos de la gravedad, efectos giroscópicos (debidos a la rotación del cuerpo rígido en el espacio) y fricción del aire. Sin embargo, debido a las diversas complejidades presentadas, se realizarán algunas simplificaciones para desarrollar el modelo como que se despreciarán el efecto suelo, se asumirá el centro de masa coincidente con el origen del sistema de coordenadas fijo al helicóptero, y que la estructura del helicóptero sea simétrica, lo que resulta en la matriz de inercia diagonal. Para continuar con el modelado dinámico del quadrotor, es necesario conocer cómo se estima la posición y 11 Simulación y control de un quadrotor orientación del vehículo con respecto a un sistema de coordenadas de referencia inercial. El aparato, como sólido rígido, está caracterizado por un sistema de coordenadas ligado a él y con origen en su centro de masa. Este sistema se define considerando A={E1a,E2a,E31} como el sistema de coordenadas fijo al helicóptero, y I= {Ex,Ey,Ez} como el sistema de coordenadas inercial, que se considerará fijo con respecto a la tierra. Figura 3-5. Esquema modelo del quadrotor. Como se puede ver en la imagen, se designará el vector ξ={x,y,z} como el vector de posición del centro de masa del helicóptero con respecto al sistema inercial I. Así mismo, la orientación del vehículo se supondrá dada por una matriz de rotación R IA:A→I, donde R IA es una matriz de rotación ortonormal. La rotación del UAV puede ser obtenida utilizando diversos métodos, entre ellos, uno muy conocido es la convención-xyz (giro alrededor de x, y, z) para los ángulos de Euler, denominado, ángulos de Tait-Bryan. Este consiste en describir una rotación general en el espacio euclídeo tridimensional a través de tres rotaciones sucesivas en torno a los ejes del sistema móvil en el cual están definidos (Vianna G., 2007). Figura 3-6. Representación de los ángulos de Tait-Brain. Rotación según 𝑥 de ϕ: 𝑅(𝑥,𝜙)=[1 0 0 0 cos𝜙 −sen𝜙 0 sen𝜙 cos𝜙] Rotación según 𝑦 de θ: 𝑅(𝑦,𝜃)=[cos𝜃 0 sen𝜃 0 1 0 −sen𝜃 0 cos𝜃] Modelado 12 Rotación según 𝑧 de ψ: 𝑅(𝑧,𝜓)=[cos𝜓 −sen𝜓 0 sen𝜓 cos𝜓 0 0 0 1] La matriz de rotación completa de {A} respecto a {I}, viene dada por: 𝑅 𝐼𝐴=𝑅(𝑧,𝜓)𝑅(𝑦,𝜃)𝑅(𝑥,𝜙) (3-1) 𝑅 𝐼𝐴=[cos𝜓cos𝜃 cos𝜓sen𝜃sen𝜙−sen𝜓cos𝜙 cos𝜓sen𝜃cos𝜙+sen𝜓sen𝜙 sen𝜓cos𝜃 sen𝜓sen𝜃sen𝜙+cos𝜓cos𝜙 sen𝜓sen𝜃cos𝜙−cos𝜓sen𝜙 −sen𝜃 cos𝜓sen𝜙 cos𝜃cos𝜙 ] (3-2) y donde 𝑅 𝐴𝐼= 𝑅 𝐼𝐴𝑇. A partir de la matriz de rotación generada por las tres rotaciones sucesivas y su propiedad de ortonormalidad, relacionando la derivada de la matriz ortonormal con una cierta matriz anti-simétrica, se puede obtener las ecuaciones cinemáticas de rotación del vehículo que establecen las relaciones entre las velocidades angulares (Vianna G., 2007). 3.3 Cinemática y dinámica Antes de proceder con el modelo, es necesario realizar un balance (recuento) de todas las fuerzas (FA= R ABFB) y momentos (τB), expresadas respecto al sistema de referencia inercial, que actúan sobre el quadrotor: el peso, el empuje, los momentos de inercia, fuerzas y momentos sobre el fuselaje debido a efectos aerodinámicos y fricción del aire, etc. (Pounds P. et al., 2010). Balance de fuerzas: 𝐹𝐴= 𝑅 𝐴𝐵𝐹𝐵=𝑃+∑𝑡𝑖 4𝑖=1 +𝐴𝑇 (3-3) Balance de momentos: τ𝐵=: ∑𝑚𝑖 4𝑖=1 +∑𝑞𝑖 4𝑖=1 +𝐴𝑅 (3-4) - Peso: P=−mgEz - Empujes debidos a movimiento de cada rotor: Dado que el sistema de coordenadas fijo {I} tiene su eje z hacia abajo, siguiendo la convención aeroespacial, el empuje de cada rotor es un vector ascendente 𝑡𝑖=𝑏𝜔𝑖2, 𝑖=1,2,3,4 (3-5) en la dirección z negativa del vehículo, donde b, como ya se ha comentado en la tabla de propiedades del quadrotor, es la constante de elevación, que depende de la geometría y del perfil del rotor, y 𝜔𝑖 la velocidad angular de cada rotor. En situación de sustentación, el empuje total que se aplica al vehículo es la suma de todos los empujes generados por cada uno de los rotores de forma individual. 13 Simulación y control de un quadrotor T=∑|𝑡𝑖| 4 𝑖=1 =𝑏∑𝜔𝑖2 4 𝑖=1 =4𝑏𝜔𝑖2 (3-6) Estas expresiones son válidas siempre que el empuje generado por cada rotor esté orientado en el sentido del eje Z del quadrotor. Sin embargo, esto no siempre se cumple: cuando se producen desplazamientos en horizontal se produce un efecto aerodinámico denominado “rotor flapping”, que, debido a una diferencia entre el empuje que generan las aspas que avanzan y las que retroceden (disimetría en el empuje), como consecuencia de diferentes ángulos de ataque y flujos de aire, provoca una inclinación en el plano en el que se encuentra el área del disco del rotor, y, con ello, una disminución del empuje vertical (Pounds P. et al., 2010). Figura 3-7. Representación de la variación del ángulo de ataque. De modo que el empuje se debe redefinir 𝑡𝑖=𝑏𝜔𝑖2(−sin𝑎1𝑠𝑖 cos𝑎1𝑠𝑖sin𝑏1𝑠𝑖 −cos𝑏1𝑠𝑖cos𝑎1𝑠𝑖) (3-7) donde a1si y b1si corresponden a los primeros armónicos de los ángulos de inclinación longitudinal y lateral de cada rotor. Figura 3-8. Representación de los ángulos de inclinación del plano del rotor debido al efecto del “rotor flapping” Debido a la complejidad y los conocimientos en aerodinámica que se requieren para desarrollar los conceptos anteriores, no se profundiza más en el tema, permitiendo posibilidad de investigar más en el documento original (Pounds P. et al., 2010). Modelado 14 - Momentos debidos a los empujes de los rotores: Las diferencias entre los empujes de cada rotor generan los pares que hacen que el vehículo gire. El par de torsión en el eje x del vehículo, par de alabeo, es la diferencia entre el momento producido por el empuje del rotor 4 y el empuje producido por el rotor 2, calculados como el producto vectorial del vector empuje y la distancia al centro de gravedad del quadrotor. 𝜏𝑥=𝜏𝜃=𝑚𝑥=𝑚4−𝑚2=𝑡4×𝑑−𝑡2×𝑑 (3-8) Al ser vectores perpendiculares se puede reescribir como 𝑚𝑥=𝑑𝑡4−𝑑𝑡2 Escribiéndolo ahora en términos de las velocidades angulares de los rotores 𝑚𝑥=𝑑𝑏(𝜔42−𝜔22) (3-9) y de manera similar para el eje y, el par de cabeceo es 𝜏𝑦=𝜏𝜙=𝑚𝑦=𝑑𝑏(𝜔12−𝜔32) (3-10) - Pares de reacción debidos al movimiento de cada rotor: El par de reacción que aparece debido a la resistencia aerodinámica como consecuencia del giro del rotor se puede describir de forma semejante al empuje 𝑞𝑖=𝑘𝜔𝑖2 (3-11) donde k depende de los mismos factores que b. Este par ejerce un par de reacción en el fuselaje que actúa para hacerlo girar en sentido opuesto a su dirección de rotación. El par de reacción total alrededor del eje z es 𝜏𝑧=𝜏𝜓=𝑞1−𝑞2+𝑞3−𝑞4=𝑘(𝜔12+𝜔32−𝜔22−𝜔42) (3-12) donde los diferentes signos se deben a las diferentes direcciones de rotación de los rotores. Expresando en forma matricial las fuerzas y momentos desarrollados hasta ahora, (𝑇Г)=(𝑇 𝜏𝜃 𝜏𝜙 𝜏𝜓)=(−𝑏 −𝑏 −𝑏 0 −𝑑𝑏 0 𝑑𝑏 𝑘0 −𝑘 −𝑑𝑏 𝑘 −𝑏 𝑑𝑏 0 −𝑘) ( 𝜔12 𝜔22 𝜔32 𝜔42 ) =𝐴 ( 𝜔12 𝜔22 𝜔32 𝜔42 ) (3-13) donde la matriz A es de rango completo si b, k, d>0 y por tanto puede ser invertida, se puede deducir la expresión que relaciona las velocidades de los rotores requeridas para aplicar un empuje especificado T y un momento Г al fuselaje (Corke P., 2011). ( 𝜔12 𝜔22 𝜔32 𝜔42 ) =𝐴−1(𝑇 𝜏𝜃 𝜏𝜙 𝜏𝜓) (3-14) 15 Simulación y control de un quadrotor Esta expresión es la que se usa en el bloque simulink “Control mixer” para Figura 3-9. Diagrama simulink correspondiente al bloque “Control mixer” donde se realiza la operación matricial. - Resistencia aerodinámica debida al empuje: 𝐴𝑇=12𝜌𝐶𝑖𝑊2 donde 𝜌 es la densidad del aire, 𝐶𝑖 son coeficientes aerodinámicos y 𝑊 es la velocidad del quadrotor con respecto al aire. - Par de resistencia aerodinámica debida a los pares y momentos: Se calcula de forma similar a 𝐴𝑇 pero con otros coeficientes aerodinámicos, 𝐴𝑅=12𝜌𝐶𝑖𝑊2. Sin embargo tanto 𝐴𝑇 como 𝐴𝑅 se desprecian a la hora de desarrollar las ecuaciones. Una vez calculadas las fuerzas, las ecuaciones de movimiento en coordenadas globales de un cuerpo rígido vienen determinadas por las leyes de Newton y Euler (Vianna G., 2007). 𝜉󰇗=𝑣= 𝑅 𝐴𝐵𝑉 (3-15) 𝑚𝑣󰇗= 𝑅 𝐴𝐵𝐹𝐵←2ª Ley de Newton,dinámica de traslación (3-16) 𝑅 𝐴𝐵 󰇗= 𝑅 𝐴𝐵𝛺𝑥 (3-17) 𝐽𝛺󰇗=−𝛺×𝐽𝛺+𝜏𝐵 ←Ecuación del movimiento de Euler,aceleración rotacional (3-18) donde g es la aceleración gravitatoria, m es la masa total del vehículo, V es el vector velocidad traslacional (en {A}), ξ y ν representan respectivamente la posición y velocidad lineal expresadas en el sistema de referencia inercial {I}, Ω la velocidad angular de {A} respecto {I}, Ω× la matriz anti-simétrica de Ω de forma que se cumpla que Ω𝑥×ν=Ω×ν para el vector resultante del producto vectorial y cualquier vector ν perteneciente a ℝ3 y J es la matriz de inercia 3 × 3 del vehículo 𝐽=[𝐼𝑥𝑥 0 0 0 𝐼𝑦𝑦 0 0 0 𝐼𝑧𝑧] donde 𝐼𝑥𝑥=𝐼𝑦𝑦 debido a la simetría del aparato. Modelado 16 El movimiento del quadrotor se obtiene integrando estas ecuaciones dinámicas. Tanto el modelo como la integración de las ecuaciones está englobado en el bloque simulink “Quadrotor”. Figura 3-10. El bloque “Quadrotor” engloba las ecuaciones que modelan al quadrotor. 17 4 GENERACIÓN DE TRAYECTORIAS Figura 4-1. Botón de la interfaz de usuario correspondiente a la ejecución del cálculo de trayectoria. a planificación de trayectorias (bloque del simulink: “Generador de trayectorias”) constituye la primera aplicación del simulador, la cual, elegido un entorno con obstáculos y rellenados los argumentos iniciales (margen, resolución, puntos inicial y final, tiempo), consiste en la búsqueda de una secuencia de posiciones para un robot de forma que este se mueva desde el estado inicial al estado final, entendiendo por estado la descripción de la ubicación del robot referida a un marco de referencia absoluto. La configuración que adquiere una determinada trayectoria depende de la distribución de los obstáculos a lo largo de todo el espacio de trabajo y, por supuesto, de la geometría del robot y sus capacidades de movimiento. De esta manera, la topología del ambiente de trabajo restringirá el espacio libre de obstáculos en el cual se pueden expresar las posibles trayectorias para alcanzar el estado final deseado. Generalmente, se recurre a una representación realizada a partir de la discretización del espacio del ambiente de trabajo, con lo que se extrae una representación segura, es decir, se tendrá la garantía de que el espacio libre podrá ser ocupado por el robot (sin riesgo de colisión), por lo tanto, es necesario que tal discretización se haga en base a las características geométricas, tanto del robot como de los obstáculos. Existen dos formas básicas para especificar el movimiento (Ollero A., 2005): L Generación de trayectorias 24 Figura 4-12. El avance en la exploración mediante el método BFS se asemeja a una ola. Las siguientes imágenes son un ejemplo del problema que presenta este método. Se puede observar cómo, aunque se haya descubierto el camino más corto, ha sido necesario explorar prácticamente todos los nodos del mapa, lo que supone un gran coste y tiempo de ejecución. a) b) Figura 4-13. a) Resultado final exploración con método BFS, mapa 1. b) Resultado final exploración con método BFS, mapa 2. 4.2.2 Dijkstra También llamado Greedy Best-First Search (búsqueda voraz), presenta un funcionamiento muy parecido al anterior con la gran diferencia de que, para resolver el problema, sí sigue una heurística consistente en elegir la opción óptima en cada paso local con la esperanza de llegar a una solución general óptima. Es decir, en vez de explorar todos los caminos posibles en todas direcciones, se favorecen los caminos de menor coste en base a una estrategia de optimización. En nuestro caso, la heurística empleada es la distancia euclídea (módulo del vector) entre el nodo que se está explorando y el nodo final, de forma que cuando se va a elegir el siguiente nodo o vecino a explorar, se elige aquel cuya distancia sea menor. 25 Simulación y control de un quadrotor Figura 4-14. Evolución en grafo mediante algoritmo Dijkstra. Esto supone una focalización de la exploración hacia el punto de destino a) b) Figura 4-15. a) Avance en la exploración mediante el método Dijkstra. b) Resultado final exploración con método Dijkstra. que puede provocar que, aunque la búsqueda del punto final sea mucho más rápida, se olvide el objetivo de obtener la trayectoria más corta, pues no se tiene en cuenta el coste real de desplazamiento al elegir esa trayectoria. a) b) Generación de trayectorias 26 Figura 4-16. a) Resultado final exploración con método Dijkstra, mapa 1. b) Resultado final exploración con método Dijkstra, mapa 2. A diferencia del algoritmo BFS, no es necesario explorar casi todos los nodos del mapa, sin embargo, este sí que obtenía la trayectoria más corta. Figura 4-17. Trayectoria más corta obtenida con el método BFS. 4.2.3 A* El problema de los algoritmos anteriores de búsqueda en grafos informados, es que se guían en exclusiva por la función heurística, como puede ser el algoritmo voraz (dijkstra), la cual puede no indicar la trayectoria más corta, o en exclusiva por el coste real de desplazarse de un nodo a otro (algoritmos de escalada, BFS), pudiéndose dar el caso de que sea necesario realizar un movimiento de coste mayor para alcanzar la solución. Es por ello que un buen algoritmo de búsqueda informada debería tener en cuenta ambos factores, el valor heurístico de los nodos y el coste real del recorrido. Así, el algoritmo A* utiliza una función de evaluación f(n)=g(n)+h(n), donde h(n) representa el valor heurístico del nodo a evaluar desde el actual, n, hasta el final, y g(n), el coste real del camino recorrido para llegar a dicho nodo, n, desde el nodo inicial. A* mantiene dos estructuras de datos auxiliares, que podemos denominar abiertos, implementado como una cola de prioridad (ordenada por el valor f(n) de cada nodo), y cerrados, donde se guarda la información de los nodos que ya han sido visitados. En cada paso del algoritmo, se expande el nodo que esté primero en abiertos, y en caso de que no sea un nodo objetivo, calcula la f(n) de todos sus vecinos, los inserta en abiertos, y pasa el nodo evaluado a cerrados (López C. 2014). El algoritmo es una combinación entre búsquedas del tipo anchura con tipo profundidad: mientras que h(n) tiende primero a profundidad, g(n) tiende primero a anchura. De este modo, se cambia de camino de búsqueda cada vez que existen nodos más prometedores. Como todo algoritmo de búsqueda en amplitud, A* es un algoritmo completo: en caso de existir una solución, siempre dará con ella. Para garantizar la optimización del algoritmo, la función h(n) debe ser una heurística admisible, esto es, que no sobrestime el coste real de alcanzar el nodo objetivo. 27 Simulación y control de un quadrotor Tabla 4–1. Esquematización algoritmo A*. f(n) = g(n) + h(n) coste real del recorrido BFS algoritmo en anchura algoritmo de escalada + + + + valor heurístico del nodo Dijkstra algoritmo en profundidad algoritmo voraz Si para todo nodo n del grafo se cumple g(n)=0, nos encontramos ante una búsqueda voraz. Si para todo nodo n del grafo se cumple h(n)=0, A* pasa a ser una búsqueda no informada. Figura 4-18. Evolución en grafo mediante algoritmo A*. El siguiente pseudocódigo representa el código empleado para desarrollar este algoritmo: Crear cuadrícula (malla) de nodos y comprobar si los nodos no se encuentran en un obstáculo y respetan distancia de seguridad Inicializar estructuras de datos auxiliares necesarias para ejecución del algoritmo Inicializar Valor de evaluación del NodoInicial = 0 (0 + 0) Mientras haya nodos a evaluar Obtener índice NodoActual (Nodo de la lista con Valor de evaluación menor) Generación de trayectorias 28 Si NodoActual está explorado, Ir a Mientras Si NodoActual es el NodoFinal, Ir a Fin Mientras Actualizar NodoActual a nodo explorado dejando de estar como nodo a explorar Obtener Vecinos del NodoActual y eliminar los que no sirvan Si no hay Vecinos posibles, Ir a Mientras Para cada Vecinos(i) Si Vecinos(i) ya está explorado, ir a Para Si Vecinos(i) no es nodo a explorar Actualizar valor de evaluación del NodoActual = Coste real de desplazamiento + Heurística Actualizar vector de nodos anteriores para reconstruir trayectoria Actualizar Vecinos(i) a nodo a explorar Eliminar NodoActual de la lista de nodos (Valor de evaluación = NaN) Donde se puede comprobar que, en cada iteración, se elige como NodoActual a aquel nodo cuyo valor de evaluación, suma entre el coste del camino recorrido (módulo del vector entre el NodoInicial y el NodoActual) y la heurística del nodo (módulo del vector entre el NodoActual y el NodoFinal), sea menor. Este mismo código se podría transformar fácilmente en uno válido para el algoritmo BFS, simplemente obviando el término de heurística de forma que, como ya se dijo anteriormente, el valor de evaluación va adquiriendo valores que no aportan ninguna información adicional que permita la optimización. Así como también se podría transformar en uno válido para Dijkstra, eliminando el término del coste real del desplazamiento. Como se puede observar en las siguientes soluciones, empleando este último algoritmo no ha sido necesario explorar todos los nodos y así como también se ha encontrado la trayectoria más corta. a) b) Figura 4-19. a) Resultado final exploración con método A*, mapa 1. b) Resultado final exploración con método A*, mapa 2. 29 Simulación y control de un quadrotor A continuación, una vez explorados los nodos necesarios hasta que se ha alcanzado el nodo final, solo queda reconstruir la trayectoria, la cual se obtiene deshaciendo los pasos seguidos, de modo que esta resulta ser la más corta. Para ello se apoya en uno de los vectores de datos auxiliares en el que, en cada elemento (cada elemento representa un nodo), se ha ido almacenando el nodo anterior desde el que se alcanzó el actual, de forma que saltando de un nodo a otro se llega de nuevo el nodo inicial. Figura 4-20. Reconstrucción de la trayectoria más corta. 4.3 Suavizado El último paso e igual de importante es el suavizado de la trayectoria. Tras obtener la solución dada por el algoritmo se puede comprobar que dicha trayectoria presenta escalones, más cuanto mayor sea la resolución que se haya empleado en la matriz de nodos. Figura 4-21. Ejemplo de trayectoria final sin suavizar. Como es normal, estos escalones no interesan, pues sería muy poco eficiente que un vehículo siguiese esta trayectoria, por lo que es necesario un suavizado de esta. La trayectoria obtenida estará constituida por una cantidad de puntos, cuya magnitud dependerá, entre otras cosas como las dimensiones del entorno, etc., de la resolución. El método de suavizado que aquí se desarrolla se realiza en varias etapas. Generación de trayectorias 30 La primera fase tiene como objetivo reducir el número de puntos que constituyen el vector que conforma la trayectoria, eliminando los puntos intermedios de los tramos rectos, es decir, solo permanecen el punto inicial y final de cada escalón, excluyendo los puntos intermedios entre estos dos. Se realiza esta primera fase para simplificar significativamente el proceso de cálculo de la segunda. Una vez explicado esto se comenta ahora la técnica que se emplea mediante su pseudocódigo. Errorx_anterior=0 Errory_anterior=0 Errorz_anterior=0 Para cada Punto de la trayectoria inicial Errorx=0 Errory=0 Errorz=0 Si la coordenada x del Punto(i+1) sufre un incremento respecto de la del Punto(i) Errorx=1 Si la coordenada y del Punto(i+1) sufre un incremento respecto de la del Punto(i) Errory=1 Si la coordenada z del Punto(i+1) sufre un incremento respecto de la del Punto(i) Errorz=1 Si (Errorx ha variado respecto del Errorx_anterior) ó (Errory ha variado respecto del Errory_anterior) ó (Errorz ha variado respecto del errorz_anterior) Se guarda el punto(i) Errorx_anterior= Errorx Errory_anterior= Errory Errorz_anterior= Errory En la siguiente imagen se pueden observar los puntos descartados (azules) y los puntos seleccionados (verdes) de la trayectoria ejemplo. 31 Simulación y control de un quadrotor Figura 4-22. Representación de los escalones presentes en una trayectoria sin suavizar. A continuación, se realiza una segunda fase que consiste en reducir la nueva trayectoria al menor número de nodos posibles que puedan ser unidos por una recta, sin que dicha recta toque a ningún obstáculo. Es decir, se empieza por el primer nodo y el último, y se comprueba si estos pueden ser unidos sin que se atraviese a un obstáculo. Si es así, ya se tiene la trayectoria final, una recta formada por únicamente dos puntos, el inicial y el final, si no, se prueba a unir de nuevo el nodo inicial con el penúltimo y así sucesivamente hasta que se pueda unir el punto inicial con alguno de los otros puntos del vector obtenido en la fase anterior. Una vez unido el primer nodo, se sigue con el que se ha unido, repitiendo el mismo proceso comenzando con el nodo final, hasta que finalmente alguno se pueda unir con el último. De esta manera queda por detallar cómo se asegura que la recta que une dos nodos no toca ningún obstáculo. Para ello lo que se hace es crear una malla de puntos (en las 3 direcciones) entre los dos nodos, y se comprueba que ninguno de ellos pertenece al obstáculo. La siguiente batería de imágenes permite observar el proceso que sigue el algoritmo con mayor claridad: 1. Se prueba a unir los nodos inicial y final, comprobando que ningún punto de la malla generada toca el obstáculo. Figura 4-23. Creación de malla y primer intento de unión. 2. Como parte de estos puntos se encuentran en el obstáculo, estos nodos no se pueden unir y se prueba Generación de trayectorias 32 con el penúltimo punto de la trayectoria obtenida en la fase 1. Figura 4-24. Nueva creación de malla y segundo intento de unión. 3. Se sigue probando con los siguientes nodos. Figura 4-25. Nueva creación de malla e intento de unión. 4. Hasta que se alcanza uno con el que sí se puede unir sin que ningún punto de la malla esté en una zona no permitida. Figura 4-26. Representación del vector de unión entre dos nodos. 33 Simulación y control de un quadrotor 5. Se vuelve a repetir el proceso, pero ahora con el nodo recién unido. Figura 4-27. Repetición del proceso (creación de malla e intento de unión) desde nuevo nodo. Figura 4-28. Representación del vector de unión desde el nuevo nodo con otro. 6. Hasta que se consigue unir alguno con el final. Figura 4-29. Representación del vector de unión con el nodo final. Generación de trayectorias 40 41 5 CONTROL Figura 5-1. Botón de la interfaz de usuario correspondiente a la ejecución de la simulación. A la vista de lo explicado en el apartado anterior, la trayectoria que debe seguir el robot se especifica en el espacio cartesiano. En este caso existen dos alternativas para su ejecución: - Definir los lazos de control directamente en el espacio cartesiano y controlar el robot para que se anule el error de seguimiento de trayectoria en este espacio. - Transformar la trayectoria del espacio cartesiano al espacio de las variables articulares y controlar la evolución de cada una de las variables articulares definiendo los lazos de control en este espacio. Como es lógico la opción que aquí nos concierne es la primera, caso el cual es comúnmente empleado en robots móviles, como el que aquí se trata, donde la curvatura del camino generado en el espacio cartesiano está directamente relacionada con la variable de control que se emplea para el seguimiento de trayectorias. El robot móvil, quadrotor, explicado en este documento, presenta seis grados de libertad (seis variables de localización), tres de posición y tres de orientación, que se procederán a controlar independientemente, es decir, de forma desacoplada. 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. Como consecuencia a dicho control desacoplado se procederá a su división en dos partes: “position controller” y “attitude controller”. 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. Control 42 El primer bloque se encarga del control de las coordenadas (x,y,z) del centro de gravedad del robot en el entorno, a partir de las cuales, una vez controladas, se obtienen: - Los ángulos de rotación, “roll” (ϕ*) y “pitch” (θ*), de referencia necesarios para realizar las traslaciones en el plano X-Y (mediante un cambio de sistema de coordenadas) - El empuje necesario para la traslación en el eje Z. De este modo, las entradas a este bloque serán las variables X*, Y* y Z* de referencia, y las salidas serán los ángulos roll y pitch citados anteriormente y la acción de control resultante, correspondiente al control en altitud. Las variables “roll” y “pitch” (ángulos de giro sobre X e Y respectivamente), proporcionados por el anterior control de posición, junto a la “yaw” (ángulo de giro sobre eje Z), actuarán como entradas del “attitude controller”, en el cual se llevará a cabo el control de las orientaciones respecto a los tres ejes de coordenadas. Las componentes de orientación sobre X e Y son de carácter transitorio: su valor en régimen permanente (en situación de sustentación, en ascenso/descenso o en ausencia de perturbaciones) es nulo. Estos dos bloques representan los dos lazos que componen el controlador, uno interior, más rápido, y uno exterior, más lento, cuyos parámetros se diseñarán en función de los tiempos de respuesta del lazo interior. Esta división da lugar a un control más delicado en lo que se refiere a los giros sobre los ejes horizontales, ya que como tanto “pitch” como “roll” influyen a la hora de realizar desplazamientos en el plano X-Y, si se da una fuerte inclinación sobre uno de estos ejes, se reduce el valor del empuje en Z y el quadrotor cae al suelo al no poder compensar dicha pérdida, debido a que la acción del control de altitud sólo depende de la altura (Martín I. 2012). Según los párrafos anteriores y los parámetros de entrada del simulador, aunque se trata de un robot con seis grados de libertad, se está tratando de controlar un sistema subactuado pues se introducen un número de consignas de entrada menor al realmente necesario: las tres correspondientes a su localización en el espacio cartesiano, que irán variando conforme al cálculo de la trayectoria realizado en el apartado anterior, y la orientación respecto del eje z, “yaw”, que se considerará de valor nulo para todo el documento (otro valor de referencia para esta variable sería útil si se quisiese conseguir una orientación final concreta). Para obtener el sistema de control, se calcularán los parámetros de un controlador con doble lazo de realimentación para cada variable, con el fin de conseguir que no haya sobreoscilaciones y que el tiempo de respuesta sea el más bajo posible sin que se produzcan inestabilidades. Figura 5-2. Detalle lazos de control interior y exterior. 43 Simulación y control de un quadrotor 5.1 Position Controller 5.1.1 Control en Z Previamente al cálculo del controlador, el primer paso a realizar es determinar el tipo de sistema. El modelado de éste se realiza aplicando la 2ª Ley de Newton, en la que se cuenta la gravedad y la fricción viscosa del aire, en el campo transformado de Laplace. 𝑚𝑧󰇘=∑𝐹=>𝑚𝑧󰇘+𝑓𝑧󰇗=𝑇 󰇍  −𝑃 󰇍  donde 𝑇 󰇍  es el empuje generado por los rotores y 𝑃 󰇍  es el peso del quadrotor. 𝑚𝑧󰇘+𝑓𝑧󰇗=4𝑏𝑤2−𝑚𝑔 (5-19) Despreciando el término debido a la fricción viscosa 𝑚𝑧󰇘=4𝑏𝑤2−𝑚𝑔 y aplicando la transformada de Laplace 𝑚𝑠2𝑍(𝑠)=4𝑏𝑤2(𝑠)−𝑚𝑔 (5-20) se puede observar que aparece un término que no es controlable pues no depende de la velocidad de los rotores. Por ello, se considera que la función de transferencia del sistema es: 𝐺(𝑠)=𝑍(𝑠) 𝑤2(𝑠)=4𝑏 𝑠(𝑚𝑠)=4𝑏 𝑚𝑠2=𝐾 𝑠2 (5-3) y que el término será compensando bien sea, por ejemplo, con un controlador anticipativo para un sistema con perturbación constante o con un controlador con prealimentación, que anulen las perturbaciones debidas a la gravedad. A la vista de los resultados, se puede comprobar que, en el caso del control de Z, se va a realizar un control lineal respecto a una variable la cual no es lineal, sino que tiene forma cuadrática, ω2. Esto se debe a que el empuje que produce movimientos en dicho eje es dependiente de forma lineal del cuadrado de la velocidad angular de los distintos rotores (Martín I. (2012). Una vez identificado el sistema, se puede comenzar ahora a diseñar los parámetros del controlador. Según la función de transferencia, el sistema posee dos integradores, lo que quiere decir que los errores de posición y velocidad serán nulos, pero hacen que el sistema sea marginalmente estable. En este caso, al aplicar un proporcional derivativo lo que hacemos es incluir un cero en la función de transferencia para dotar al sistema de cierta estabilidad en bucle cerrado. Este tipo de control está compuesto dos lazos de realimentación: uno de posición y otro de velocidad. Control 44 Figura 5-3. Diagrama de bloques del regulador con doble lazo de realimentación, siendo la k1 la ganancia de la parte proporcional, la k2 la ganancia de la parte derivativa, G(s) la función de transferencia del sistema para z y z* el valor de consigna. Simplificando el esquema 𝐻(𝑠)=𝐺(𝑠)𝑘1 1+𝑘2𝐺(𝑠)𝑠 1+ 𝐺(𝑠)𝑘1 1+𝑘2𝐺(𝑠)𝑠=𝐺(𝑠)𝑘1 1+𝑘2𝐺(𝑠)𝑠+𝐺(𝑠)𝑘1 y sustituyendo la función de transferencia, 𝐺(𝑠) 𝐻(𝑠)=𝑘1𝐾 𝑠2 1+𝑘2𝐾 𝑠2𝑠+𝑘1𝐾 𝑠2=𝑘1𝐾 𝑠2+𝑘2𝐾𝑠+𝑘1𝐾 (5-4) resulta un sistema de segundo orden 𝐻(𝑠)=𝑘1𝐾 𝑠2+𝑘2𝐾𝑠+𝑘1𝐾=𝜔𝑛2 𝑠2+2𝜉𝜔𝑛𝑠+𝜔𝑛2 (5-5) cuya respuesta depende de los valores de los parámetros, los cuales se obtienen a partir de las especificaciones que elijamos para el sistema. En este caso: para que no haya sobreoscilaciones amortiguadas, ξ=1, y para un tiempo de respuesta (𝑡𝑒) de 10s, 𝜔𝑛=4.75 𝑡𝑒𝜉=0.475. Igualando ahora los términos, se obtienen los parámetros del controlador. 𝑘1𝐾=𝜔𝑛2=>𝑘1=𝜔𝑛2 𝐾=𝜔𝑛2𝑚 4𝑏 =0.4752∗4 4∗1.3234∗10−5=1.7∗104 2𝜉𝜔𝑛=𝑘2𝐾=>𝑘2=2𝜉𝜔𝑛 𝐾=2𝜉𝜔𝑛𝑚 4𝑏 =2∗1∗0.475∗4 4∗1.3234∗10−5=7.2∗104 Sin embargo, tras implementar todos los controladores, tanto este como los que se explicarán más adelante, se modifican los valores buscando una mejor respuesta. Estos valores se fijan para un te=30𝑠, por lo que k1= 1.9∗103 y k2=2.3∗104, además, también hay que tener en cuenta el signo de estas ganancias pues hay que recordar que la referencia en Z es negativa. Para complementar el controlador, como ya se dijo anteriormente, se procede a implementar una estructura de control con prealimentación para anular las perturbaciones debidas a la gravedad. Lo que se consigue con esta estructura es que la salida siga lo más rápidamente posible las variaciones de la entrada cuando éstas se produzcan. 45 Simulación y control de un quadrotor Figura 5-4. Diagrama de bloques de un controlador con prealimentación. Al introducir la prealimentación, la salida del sistema queda de la forma 𝑆(𝑠)=𝑅𝑝(𝑠)𝐶(𝑠)𝐺(𝑠) 1+𝐶(𝑠)𝐺(𝑠)𝐸(𝑠)+𝑅𝑝(𝑠)𝐺(𝑠) 1+𝐶(𝑠)𝐺(𝑠)𝐸(𝑠)=𝑅𝑝(𝑠)+𝐶(𝑠) 1+𝐶(𝑠)𝐺(𝑠)𝐺(𝑠)𝐸(𝑠) Donde se debe cumplir que la función de transferencia sea la unidad 𝑅𝑝(𝑠)+𝐶(𝑠) 1+𝐶(𝑠)𝐺(𝑠)𝐺(𝑠)=1⇔𝑅𝑝(𝑠)=1 𝐺(𝑠) Y sustituyendo finalmente 𝐺(𝑠) 𝑅𝑝(𝑠)=𝑠2 𝐾=𝑠2𝑚 4𝑏 =7.55∗104∗𝑠2 (5-6) Se observa que se trata de una prealimentación doblemente derivativa, por lo que también es necesario calcular las velocidades y aceleraciones. Recordando lo que se dijo en el apartado anterior sobre las dos formas de proporcionar la referencia, en el primer caso se calcularían aceleraciones fuera del simulink (debido a que la función “mstraj” no devuelve el vector de velocidades y aceleraciones, estas se calculan mediante la fórmula derivada a partir del vector de trayectoria), mientras que en el segundo caso se calcularía durante la propia simulación. Por último, como se dijo anteriormente, la otra posibilidad para eliminar las perturbaciones debidas a la gravedad es utilizando un controlador anticipativo para un sistema con perturbación constante, en el que el término adicional sería la velocidad media del rotor necesaria para generar un empuje igual al peso del vehículo que contrarrestaría dicho efecto. Figura 5-5. Diagrama de bloques de un controlador anticipativo. Control 46 Otras alternativas para el control anticipativo podrían ser, aplicar una ganancia muy alta para el bucle de altitud, pero esto podría conducir a saturación e inestabilidad del actuador, o utilizar un controlador proporcional-integral (PI), pero este, por otro lado, podría requerir mucho tiempo hasta que el término integral alcanzase el valor deseado, conduciendo a un sobretiempo. 5.1.2 Control en X/Y A diferencia del control de Z, en este caso no se realiza un control lineal sobre una variable cuadrática, sino sobre una lineal, es decir, los empujes que provocan cambios en los ejes X e Y no dependen de ω2, aunque a simple vista pueda parecerlo, por lo que el control de estas variables sí que se tratará de un control lineal propiamente dicho. Como en el caso anterior, el controlador utilizado será un proporcional derivativo al que opcionalmente se le podría añadir una prealimentación de consigna, la cual sería también, como en el control de altitud, doblemente derivativa, aunque, sin embargo, este control no estrará tan bien ajustado debido al efecto aerodinámico de “rotor flapping”, anteriormente comentado En este caso, se deben distinguir dos sistemas de referencia: el sistema de referencia absoluto y el sistema de referencia ligado al cuerpo del quadrotor. Esto se debe a que las rotaciones que se producen en “yaw” hacen que las coordenadas absolutas no sean las mismas respecto a las del aparato. Sin embargo, los datos que proporciona el modelo (hipotéticos sensores) sí que proporcionan la posición absoluta, por lo que lo que se tendría que hacer es obtener el error en el primer sistema de referencia y, mediante rotación en Z, pasarlo al sistema que maneja el quadrotor. 𝜉𝐸 󰇍 󰇍 󰇍 󰇍  = 𝑅𝐸𝑇 0 𝜉𝑂 󰇍 󰇍 󰇍 󰇍  =>[𝜃𝜙𝑧]=[cos𝜓 −sin𝜓 0 sin𝜓 cos𝜓 0 0 0 1]−1[𝑥𝑦𝑧] (5-7) De la relación entre los ángulos “roll” y “pitch” con las coordenadas en el plano horizontal, respecto a la referencia absoluta, 𝑋󰇘=𝑔(cos(𝜓)𝜃+sin(𝜓)𝜙) (5-8) 𝑌󰇘=𝑔(−cos(𝜓)𝜙+sin(𝜓)𝜃) (5-9) se pueden obtener las funciones de transferencia entre dichas coordenadas y ángulos, suponiendo que “yaw”=0. 𝐺(𝑠)=𝑋(𝑠) 𝜃(𝑠)=𝑔 𝑠2 (5-10) 𝐺(𝑠)=𝑌(𝑠) 𝜙(𝑠)=−𝑔 𝑠2 (5-11) De la misma forma que en el control para Z, se calculan ahora los parámetros para cada controlador, con ξ=1 (sobreoscilaciones amortiguadas), y 𝜔𝑛=1.5833 (tiempo de respuesta de 3s) 𝐻(𝑠)=𝐺(𝑠)𝑘1 1+𝑘2𝐺(𝑠)𝑠+𝐺(𝑠)𝑘1=𝑘1𝑔 𝑠2 1+𝑘2𝑔 𝑠2𝑠+𝑘1𝑔 𝑠2=𝑘1𝑔 𝑠2+𝑘2𝑔𝑠+𝑘1𝑔 (5-12) 𝑘1𝑔=𝜔𝑛2=>𝑘1=𝜔𝑛2 𝑔=1.58332 9.81 =0.2555 47 Simulación y control de un quadrotor 2𝜉𝜔𝑛=𝑘2𝑔=>𝑘2=2𝜉𝜔𝑛 𝑔=2∗1∗1.5833 9.81 =0.322 siendo estos mismos valores válidos tanto como para el controlador en X como en Y, pero con signos diferentes. De forma similar al caso anterior, se calcula la prealimentación para que la función de transferencia sea de valor unidad. 𝑅𝑝(𝑠)=1 𝐺(𝑠)=𝑠2 𝑔 (5-13) 5.2 Attitude Controller 5.2.1 Control en yaw Repitiendo los pasos de los apartados anteriores, lo primero es obtener la función de transferencia correspondiente a la entrada 𝜏𝑦𝑎𝑤. Para ello, se parte de la condición que se debe dar en las las velocidades de los rotores para que haya un empuje que provoque un giro en el eje Z. 𝜏𝑦𝑎𝑤=𝑘(𝜔22+𝜔42−𝜔12−𝜔32)=𝑘(2𝜔22−2𝜔12) Si se aplica dicho empuje en dicha entrada cuando el quadrotor se encuentra en situación de sustentación, entonces 𝜏𝑦𝑎𝑤=𝑘[2(𝜔𝑦𝑎𝑤−𝜔𝑜)2−2(𝜔𝑦𝑎𝑤+𝜔𝑜)2]=2𝑘(−4𝜔𝑦𝑎𝑤𝜔𝑜) donde 𝜔𝑜=√𝑚𝑔 4𝑏=√4∗9.81 4∗1.3234∗10−5=860.97 es la velocidad que deben tener los cuatro rotores para mantener al quadrotor en sustentación. Igualando la expresión anterior junto con la definición de momento angular de un sólido rígido 𝜏𝑦𝑎𝑤=−𝐼𝑧𝑧𝑑2𝜓 𝑑𝑡2=> 𝜏𝑦𝑎𝑤(𝑠)=𝐼𝑧𝑧𝑠2𝜓(𝑠) se puede despejar la función de transferencia 𝐺(𝑠)=𝜓(𝑠) 𝑤𝑦𝑎𝑤(𝑠)=−8𝜔𝑜𝑘 𝐼𝑧𝑧𝑠2=𝐾1 𝑠2=−8∗860.97∗1.0697∗10−7 0.149∗𝑠2=−0.004945 𝑠2=𝐾1 𝑠2 (5-14) En este caso, aunque al principio pueda parecer que se trata de un control sobre una variable cuadrática, como ocurre en los apartados anteriores, en realidad es un control lineal sobre una variable lineal, es decir, sobre 𝜔. Por último, se calculan los parámetros del controlador, exactamente de la misma forma que en los casos anteriores, pero ahora variando las especificaciones: ξ=1 (sobreoscilaciones amortiguadas), pero 𝜔𝑛 ahora se calcula para un tiempo de respuesta de 1s, pues, como se dijo anteriormente, el lazo interior debe ser más rápido que el lazo exterior, por lo que 𝜔𝑛=4.75. Control 48 𝐻(𝑠)=𝐺(𝑠)𝑘1 1+𝑘2𝐺(𝑠)𝑠+𝐺(𝑠)𝑘1=𝑘1𝐾1 𝑠2 1+𝑘2𝐾1 𝑠2𝑠+𝑘1𝐾1 𝑠2=𝑘1𝐾1 𝑠2+𝑘2𝐾1𝑠+𝑘1𝐾1 (5-15) 𝑘1𝐾1=𝜔𝑛2=>𝑘1=𝜔𝑛2 𝐾1=4.752 −0.004945=−4.56∗103 2𝜉𝜔𝑛=𝑘2𝐾1=>𝑘2=2𝜉𝜔𝑛 𝐾1=2∗1∗4.75 −0.004945=−1.92∗103 Tanto como para este controlador, como para los siguientes, no se añade ninguna prealimentación. 5.2.2 Control en pitch/roll Finalmente, solo queda diseñar los controladores para las entradas que se encargan de los desplazamientos en horizontal, los ángulos “pitch” y “roll”, los cuales se verán modificados durante dichas traslaciones. Dichas modificaciones en los valores de estos provocaran una disminución del empuje en vertical al cambiar el plano horizontal del quadrotor, de forma que este será Tz=Tcos(θ) si se produce un movimiento en el eje X o Tz= Tcos(ϕ) si es en el eje Y. Para que el valor del empuje en Z se mantenga lo más constante posible, estos ángulos deben ser muy pequeños de manera que el coseno de éstos sea prácticamente la unidad (Martín I. 2012). Las expresiones de los empujes responsables de los movimientos en el plano X-Y, como ya se explicó en apartados anteriores, son: 𝜏𝑟𝑜𝑙𝑙=𝑑𝑏(𝜔22−𝜔42) (5-16) que corresponde con el necesario para provocar un giro sobre el eje Y, y 𝜏𝑝𝑖𝑡𝑐ℎ=𝑑𝑏(𝜔32−𝜔12) (5-17) el necesario para provocar un giro sobre el eje X, donde d es la distancia de cada rotor al centro de gravedad. Como también se dijo anteriormente, las coordenadas X e Y no se manejan en el mismo sistema de referencia que en el que se manejan estos ángulos. Para obtener las funciones de transferencia se procederá de la misma forma que para el ángulo “yaw”, igualando las definiciones de los momentos angulares 𝜏𝑟𝑜𝑙𝑙=−𝐼𝑥𝑥𝑑2𝑟𝑜𝑙𝑙 𝑑𝑡2 (5-18) 𝜏𝑝𝑖𝑡𝑐ℎ=−𝐼𝑦𝑦𝑑2𝑝𝑖𝑡𝑐ℎ 𝑑𝑡2 (5-19) con las expresiones de estos, en situación de sustentación y suponiendo que no se produce giro alguno sobre el eje Z (variación de “yaw”). 𝜏𝑟𝑜𝑙𝑙=−4𝑑𝑏𝜔𝑜𝑤𝑟 𝜏𝑝𝑖𝑡𝑐ℎ=−4𝑑𝑏𝜔𝑜𝑤𝑝 49 Simulación y control de un quadrotor De modo que se obtiene 𝐺(𝑠)=𝜙(𝑠) 𝑤𝑟(𝑠)=−4𝑑𝑏𝜔𝑜 𝐼𝑥𝑥𝑠2=−4∗0.315∗1.3234∗10−5∗860.97 0.082𝑠2=−0.1751 𝑠2=𝐾2 𝑠2 (5-20) 𝐺(𝑠)=𝜃(𝑠) 𝑤𝑝(𝑠)=−4𝑑𝑏𝜔𝑜 𝐼𝑦𝑦𝑠2=−0.1751 𝑠2=𝐾2 𝑠2 (5-21) Como se cumple que Ixx=Iyy, la función de transferencia es la misma para ambos, y, por tanto, solo se realiza el diseño del controlador una vez, pues serán los mismos valores como para uno como para otro. 𝐻(𝑠)=𝐺(𝑠)𝑘1 1+𝑘2𝐺(𝑠)𝑠+𝐺(𝑠)𝑘1=𝑘1𝐾2 𝑠2 1+𝑘2𝐾2 𝑠2𝑠+𝑘1𝐾2 𝑠2=𝑘1𝐾2 𝑠2+𝑘2𝐾2𝑠+𝑘1𝐾2 (5-22) Las especificaciones, como es lógico, serán ξ=1 y ωn=4.75 (tiempo de respuesta de 1s). 𝑘1𝐾2=𝜔𝑛2=>𝑘1=𝜔𝑛2 𝐾2=4.752 −0.1751=−128.86 2𝜉𝜔𝑛=𝑘2𝐾2=>𝑘2=2𝜉𝜔𝑛 𝐾2=2∗1∗4.75 −0.1751 =−54.26 La mayor diferencia que presenta este control respecto de los demás, es que la entrada no viene dada por una referencia externa, sino que es resultado de un bucle anterior. Cabría pensar que en el control de la variable “yaw” ocurre lo mismo pues no se aprecia en ningún sitio que se le pueda indicar un valor deseado, sin embargo, esto no es cierto, lo que ocurre es que en este documento no se han considerado cambios en esta variable, por lo que la referencia, de forma interna, se mantiene constante de valor nulo. Referencias 56 [15] Vianna Raffo, G. (2007). Modelado y control de un helicóptero quadrotor (Trabajo de Fin de Máster). Recuperado de: http://bibing.us.es/proyectos/abreproy/70017/fichero/Tesis_Master_GuilhermeRaffo.pdf [16] Vianna Raffo, G., Ortega, M. G. & Rubio, F. (2010). An integral predictive/nonlinear H∞ control structure for a quadrotor helicopter. Automatica (Journal of IFAC), 46(1), pp. 29-39. Recuperado de: http://dl.acm.org/citation.cfm?id=1688046 [17] Vora, A. (2016). Ankit Vora: Motion planning for quadrotors. Recuperado el 31 de agosto de 2017, de http://ankitvora.tk/ 57 Simulación y control de un quadrotor