Trabajo Fin de Grado Grado en Ingeniería...

Trabajo Fin de Grado Grado en Ingeniería Aeroespacial

Verificación y validación de sistemas de control de vuelo para MAV-VTOL basadas en MATLAB Stateflow

Autor: Pablo Rodríguez Díaz

Tutores: Jesús Mª González Villagómez Manuel Vargas Villanueva

Dep. Ingeniería de Sistemas y Automática Escuela Técnica Superior de Ingeniería

Universidad de Sevilla Sevilla, 2014

Trabajo Fin de Grado Grado en Ingeniería Aeroespacial

Verificación y validación de sistemas de control de vuelo para MAV-VTOL basadas en MATLAB

Stateflow

Autor:

Pablo Rodríguez Díaz

Tutor:

Jesús Mª González Villagómez

Manuel Vargas Villanueva

Dep. de Ingeniería de Sistemas y Automática

Escuela Técnica Superior de Ingeniería

Universidad de Sevilla Sevilla, 2014

Trabajo Fin de Grado: Verificación y validación de sistemas de control de vuelo para MAV-VTOL basadas en MATLAB Stateflow

Autor: Pablo Rodríguez Díaz

Tutor: Jesús Mª González Villagómez

Manuel Vargas Villanueva

El tribunal nombrado para juzgar el Proyecto arriba indicado, compuesto por los siguientes miembros:

Presidente:

Vocales:

Secretario:

Acuerdan otorgarle la calificación de:

Sevilla, 2014

El Secretario del Tribunal

1

AgradecimientosEl mayor agradecimiento a mi tutor, Jesús, por su labor y dedicación en este proyecto,por enseñarme e involucrarse.

Gracias al Prof. Manuel Vargas por todo su apoyo ofrecido, y al Prof. Francisco Rodrí-guez Rubio por la oportunidad de poder participar en este trabajo.

También agradecer a Antonio Ventosa Cutillas por su ayuda en este trabajo.

Me gustaría agradecer a mi novia, Loreto, por apoyarme en cada momento, y a mimadre y a mi hermana, por no dejar de confiar en mi. Mi gratitud también a mi padrepor todo lo que me inculcó y que nunca olvidaré.

Agradecer finalmente a los amigos que siempre están.

3

ResumenSe presenta el desarrollo de una solución para la verificación y validación de sistemas degobierno y control de vuelo para pequeñas plataformas de tipo VTOL (Vertical Take-Offand Landing). Para la simulación del vehículo se dispone del correspondiente modelodinámico, que será interconectado con diferentes subsistemas de control de estabilizacióny posición, gobernados por una máquina de estados definida en MATLAB Stateflow. Elconjunto de simulaciones para la verificación del diseño de la solución propuesta sonllevadas a cabo mediante Simulink. El objetivo es disponer de mecanismos que permitanevaluar el correcto funcionamiento de prototipos antes de ser evaluados en condicionesreales.

Índice general

1. Introducción 1

1.1. Motivación . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1

1.2. Objetivos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

1.3. Organización del trabajo . . . . . . . . . . . . . . . . . . . . . . . . . . 5

2. Estado del arte 7

3. Quadrotor (MAV-VTOL) 15

3.1. Descripción del MAV . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

3.2. Modelado . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

3.3. Estimación del estado . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18

3.4. Modelo dinámico . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

4. Descripción de la solución 27

4.1. Introducción . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27

4.2. Modelo dinámico . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28

4.3. Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30

4.3.1. Control de de estabilización . . . . . . . . . . . . . . . . . . . . 30

4.3.2. Control de posición . . . . . . . . . . . . . . . . . . . . . . . . . 32

4.3.3. Lógica de gobierno . . . . . . . . . . . . . . . . . . . . . . . . . 32

4.4. Stateflow . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33

5

ÍNDICE GENERAL 6

4.4.1. Diseño . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33

4.4.2. Estados de navegación . . . . . . . . . . . . . . . . . . . . . . . 36

4.4.2.1. Tierra . . . . . . . . . . . . . . . . . . . . . . . . . . . 36

4.4.2.2. Aire . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37

4.4.2.3. Emergencia . . . . . . . . . . . . . . . . . . . . . . . . 42

4.4.3. Estados de control . . . . . . . . . . . . . . . . . . . . . . . . . 43

4.4.3.1. Niveles de control . . . . . . . . . . . . . . . . . . . . . 43

4.4.3.2. Tipos de control . . . . . . . . . . . . . . . . . . . . . 46

4.4.3.3. Estados . . . . . . . . . . . . . . . . . . . . . . . . . . 47

4.4.4. Transiciones . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48

4.4.4.1. Transición Tierra - Aire . . . . . . . . . . . . . . . . . 48

4.4.4.2. Transiciones en el Modo Aire . . . . . . . . . . . . . . 49

4.4.4.3. Transiciones en el modo Vuelo . . . . . . . . . . . . . . 49

4.4.4.4. Transición Aire - Emergencia . . . . . . . . . . . . . . 50

4.4.4.5. Transiciones a Tierra . . . . . . . . . . . . . . . . . . . 50

4.4.4.6. Transición desde ToHome . . . . . . . . . . . . . . . . 50

4.4.4.7. Transiciones en el Modo de Control . . . . . . . . . . . 50

4.5. Generador de trayectorias . . . . . . . . . . . . . . . . . . . . . . . . . 51

5. Experimentos y validación 57

5.1. Introducción . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57

5.2. Instrucciones para la simulación . . . . . . . . . . . . . . . . . . . . . . 57

5.3. Simulaciones . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59

5.3.1. Despegue, estabilización y estabilización en altura . . . . . . . . 60

5.3.2. Despegue, estabilización y movimiento en el plano . . . . . . . . 61

5.3.3. Despegue, estabilización y movimiento en el espacio . . . . . . . 63

5.3.4. Despegue, movimiento en el plano, aterrizaje, despegue y estabi-lización . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65

ÍNDICE GENERAL 7

5.3.5. Despegue, estabilización, movimiento en el espacio, ToHome yaterrizaje . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66

5.3.6. Despegue, estabilización, movimiento en el plano y emergencia . 68

5.3.7. Despegue, estabilización, movimiento en el espacio y emergencia 69

5.3.8. Despegue, estabilización, movimiento en el espacio, movimientoen el plano, movimiento en el espacio y “ToHome”. . . . . . . . 71

5.3.9. Todos los modos . . . . . . . . . . . . . . . . . . . . . . . . . . 72

5.3.10. Simulación en Flightgear . . . . . . . . . . . . . . . . . . . . . . 74

6. Conclusiones y trabajo futuro 77

6.1. Conclusiones . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 77

6.2. Trabajo futuro . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 78

6.2.1. Modelo dinámico . . . . . . . . . . . . . . . . . . . . . . . . . . 78

6.2.2. Bloque de control . . . . . . . . . . . . . . . . . . . . . . . . . . 78

6.2.3. Máquina de estados . . . . . . . . . . . . . . . . . . . . . . . . . 78

6.2.4. Implementación hardware . . . . . . . . . . . . . . . . . . . . . 79

6.2.4.1. Generación de código . . . . . . . . . . . . . . . . . . . 79

6.2.4.2. Hardware-in-the-loop . . . . . . . . . . . . . . . . . . . 80

A. Elipsoide de referencia 87

B. Códigos de programación 90

B.1. dinamica_quadrotor.m . . . . . . . . . . . . . . . . . . . . . . . . . . . 90

B.2. Secuenciador de Waypoints . . . . . . . . . . . . . . . . . . . . . . . . . 97

B.3. modos.m . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 99

B.4. wp_interp.m . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103

B.5. googleEarth.m . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 104

B.6. trayectoria.m . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 104

B.7. representar.m . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 105

Índice de figuras

2.1. Imagen del AR.Drone 2.0 . . . . . . . . . . . . . . . . . . . . . . . . . . 8

2.2. Posibles configuraciones de motores en la plataforma Arducopter . . . . 9

2.3. Imagen de un quadcopter con la tecnología Arducopter . . . . . . . . . 10

2.4. Imagen del software Mission Planner . . . . . . . . . . . . . . . . . . . 11

2.5. Imagen de la aplicación TrackDrone Lite . . . . . . . . . . . . . . . . . 11

2.6. Imagen del simulador Flightgear . . . . . . . . . . . . . . . . . . . . . . 12

3.1. Ejemplo de una plataforma tipo quadrotor . . . . . . . . . . . . . . . . 15

3.2. Esquema de las fuerzas y pares que actúan sobre un vehículo tipo quadrotor 16

3.3. Rotación de los ángulos de Tait-Bryan del sistema de coordenadas iner-cial al sistema de coordenadas fijado al helicóptero. . . . . . . . . . . . 19

4.1. Modelo completo desarrollado en MATLAB Simulink . . . . . . . . . . 28

4.2. Ejes representativos de las coordenadas locales del quadrotor . . . . . . 29

4.3. Imagen del bloque Modelo Dinámico . . . . . . . . . . . . . . . . . . . 29

4.4. Contenido del bloque “Control Mixer” . . . . . . . . . . . . . . . . . . 30

4.5. Diagrama del modelo donde se incluyen las referencias de entrada y salidade cada bloque de control . . . . . . . . . . . . . . . . . . . . . . . . . 31

4.6. Diagrama Simulink del Control de estabilización, incluido en el bloque“Bajo Nivel” . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31

4.7. Diagrama Simulink del Control Traslacional, incluído en el bloque “AltoNivel” . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32

8

ÍNDICE DE FIGURAS 9

4.8. Elemento de Stateflow para incorporar scripts de Matlab . . . . . . . . 35

4.9. Esquema explicativo de los estados que componen el bloque Stateflow ysu jerarquía . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35

4.10. Modos de funcionamiento correspondientes al bloque “Navegación” . . 36

4.11. Diagrama Stateflow del estado principal “Tierra” . . . . . . . . . . . . 37

4.12. Diagrama donde se presentan los modos de funcionamiento incluidos enel estado principal “Aire” . . . . . . . . . . . . . . . . . . . . . . . . . 37

4.13. Diagrama Stateflow del modo “Despegue” . . . . . . . . . . . . . . . . . 38

4.14. Diagrama Stateflow del modo “Vuelo” . . . . . . . . . . . . . . . . . . 40

4.15. Diagrama Stateflow del modo “Aterrizaje” . . . . . . . . . . . . . . . . 41

4.16. Diagrama Stateflow del modo “To Home” . . . . . . . . . . . . . . . . 42

4.17. Diagrama Stateflow del estado principal “Emergencia” . . . . . . . . . 44

4.18. Esquema de los estados y jerarquía de el estado Modo de control . . . . 45

4.19. Modos de Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47

4.20. Modo de control implementado en Stateflow . . . . . . . . . . . . . . . 48

4.21. Vista del contenido del bloque «Generador de trayectorias» . . . . . . . 53

4.22. Ruta circular en Google Earth® situada en el edificio de laboratorios dela Escuela Superior de Ingenieros de Sevilla . . . . . . . . . . . . . . . . 54

5.1. Captura del menú “Modo de vuelo” basado en la herramienta GUI . . . 58

5.2. Pantallas de selección de los tipos de control . . . . . . . . . . . . . . . 58

5.3. Gráficas de modo y altura con respecto al tiempo en la simulación 1. . 61

5.4. Gráfica del plano XY para la simulación 2 . . . . . . . . . . . . . . . . 62

5.5. Valores de posición lineal a lo largo del tiempo para la simulación 2. . . 62

5.6. Posición angular respecto al tiempo para la simulación 2 . . . . . . . . 63

5.7. Altura respecto al tiempo en la simulación 3 . . . . . . . . . . . . . . . 64

5.8. Trayectoria 3D de la simulación 3. . . . . . . . . . . . . . . . . . . . . . 64

5.9. Posición respecto al tiempo en la simulación 4 . . . . . . . . . . . . . . 65

ÍNDICE DE FIGURAS 10

5.10. Trayectoria vista en 3D de la simulación 4. . . . . . . . . . . . . . . . . 66

5.11. Posición respecto al tiempo en la simulación 5. . . . . . . . . . . . . . . 67

5.12. Trayectoria 3D de la simulación 5 . . . . . . . . . . . . . . . . . . . . . 67

5.13. Posición respecto al tiempo para la simulación 6 . . . . . . . . . . . . . 68

5.14. Trayectoria 3D para la simulación 6 . . . . . . . . . . . . . . . . . . . . 69

5.15. Posición respecto al tiempo en la simulación 7 . . . . . . . . . . . . . . 70

5.16. Trayectoria 3D en forma de hélice para la simulación 7 . . . . . . . . . 70

5.17. Posición respecto al tiempo en la simulación 8. . . . . . . . . . . . . . . 71

5.18. Trayectoria 3D obtenida en la simulación 8. . . . . . . . . . . . . . . . 72

5.19. Valores en posición y modo de vuelo respecto al tiempo en la simulación 9 73

5.20. Representación en Google Maps de la trayectoria generada por el qua-drotor en la simulación 9. . . . . . . . . . . . . . . . . . . . . . . . . . 74

6.1. Imagen de placa Ardupilot, basada en la plataforma Arduino . . . . . . 79

6.2. Imagen de una captura de pantalla de Simulink con la librería Ardupilot2 Target . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 80

A.1. Sistema de referencia WGS84 . . . . . . . . . . . . . . . . . . . . . . . 88

Índice de cuadros

3.1. Principales efectos físicos actuantes sobre un helicóptero . . . . . . . . 17

5.1. Simulación 1: referencias para la variable z. . . . . . . . . . . . . . . . . 60

5.2. Simulación 2: referencias para el conjunto de variables [x, y]. . . . . . . 63

5.3. Simulación 3: referencias para el conjunto de variables [x, y, z ]. . . . . . 64

11

ÍNDICE DE CUADROS 12

Capítulo 1

Introducción

1.1. Motivación

El uso de plataformas aéreas no tripuladas está experimentando un notable crecimien-to en los últimos tiempos, tanto en el campo de la investigación, como en solucionesaportadas por empresas privadas, e incluso en el ámbito doméstico. Ésto es debido prin-cipalmente a su bajo coste y las posibilidades que ofrece para el desarrollo de numerosasaplicaciones, tanto civiles como militares, tales como:

Transporte de carga

Observación meteorológica

Cartografía

Fotografía y filmación

Supervisión de cultivos

Vigilancia y seguridad ciudadana

Espionaje

Maniobras en territorio hostil

etc.

1

CAPÍTULO 1. INTRODUCCIÓN 2

El desarrollo de este tipo de vehículos ha sido posible gracias al gran avance tecnológicoque ha supuesto la miniaturización de actuadores y sensores ocurrido en los últimosaños, así como la evolución de sistemas de almacenamiendo de energía y procesamientode datos.

Su uso está cada vez más extendido, incluso en España, donde ya existe normativavigente que establece el marco legal para su utilización [1]. Las plataformas de ala fijahan sido objeto de estudio principal de los sistemas de control de vuelo, sin embargo, laaparición de UAV de ala rotatoria con despegue vertical, el denominado sistema VTOL(Vertical Take-Off and Landing), son hoy día una de las principales referencias en sis-temas autónomos de vuelo de pequeño tamaño, en concreto los sistemas multirotor, yde forma particular los quadrotor.

Según [2], el interés que despiertan este tipo de plataformas reside en su versatilidad ymaniobrabilidad, permitiéndoles la ejecución de un gran número de tareas. Sin embargosu control se hace más complejo por la inestabilidad de su dinámica, entre otros motivos.

Este tipo de helicópteros consigue un vuelo estacionario, estable y preciso a través delbalance de fuerzas de propulsión ejercidas por las cuatro hélices accionadas por los cua-tro actuadores.

Las ventajas que presentan este tipo de plataformas con respecto a un helicópteroconvencional son:

Aumenta la capacidad de carga gracias a la suma de empuje proporcionado porsus cuatro rotores

Alta maniobrabilidad, idóneo para el despegue, aterrizaje y vuelo en entornoscomplejos

La sencillez mecánica, debido a que el movimiento es proporcionado únicamentepor la variación de velocidad de giro de los rotores. En un helicóptero convencional,la velocidad de giro suele ser constante, y el movimiento se produce variando losángulos de ataque de las hélices (cíclico y colector), lo que precisa de elementosmecánicos más complejos.


Sin embargo, las plataformas tipo quadrotor presentan como desventaja su aumento depeso y el aumento en el consumo de energía debido al mayor número de rotores.

La incorporación de sensores cada vez mas sofisticados ha provocado la evolución delos sistemas tripulados de forma remota hacia una generación de vehículos capacesde navegar de forma autónoma. Los elementos inerciales permiten la estabilización ycontrol de la plataforma, la incorporación de sistemas de posicionamiento por satéliteestablecen su localización y facilitan el guiado del vehículo en exteriores, y los sensoresde flujo óptico, cámaras o sistemas láser añaden autonomía al guiado al no depender desistemas externos e incluyen la posibilidad de navegación en interiores. Estos sistemasdescritos sólo son una parte de los numerosas opciones que se pueden incluir en unaplataforma de este tipo para su control y guiado, sin embargo gracias a trabajos re-cientes es posible ampliar su funcionalidad añadiendo pequeños brazos manipuladores,actuadores de cámara giroestabilizados, o por último sensores de presión y contacto,que permiten la interacción de la plataforma con el entorno.

Sin embargo, la integración de todos estos sensores en la plataforma y su configuracióny adaptación no es trivial, ya que se necesita conseguir un nivel de seguridad adecua-do para una plataforma aérea robótica, lo que requiere una combinación óptima entresoftware y hardware embarcado y ser robusto a fallos sensoriales o de pérdida de datoscon el enlace de tierra. Cualquier otra situación no prevista podría conllevar daños enla plataforma, con la consiguiente pérdida económica y de horas de trabajo que ellosupondría, o incluso ser peligroso con la seguridad de las personas. Es por ello que sehace imprescindible la evaluación del software embarcado para poder detectar posiblesfallos que deriven en situaciones imprevistas y/o peligrosas.

Las soluciones actuales, económicamente viables, que combinan hardware y softwaremás extendidas están basadas en entornos de microprogramación en lenguage C/C++y hardware basado en Arduino. Periódicamente la Comunidad aporta nuevas actua-lizaciones del firmware del dispositivo e incluye nuevas mejoras y funcionalidades. Anivel de investigación, lo que se pretende es poder reutilizar parte de estas aportacionespara poder avanzar en campos como el control o la fusión sensorial, pudiendo accedera cualquier elemento hardware o software del sistema y poder reconfigurarlo, si fueranecesario. Si bien es una plataforma muy extendida, no es posible una comprobación


por simulación del sistema, de tal manera que ante cualquier modificación del códigofuente, o bien una reformulación de la lógica de aplicación, la única depuración y ve-rificación posibles, incluyendo el comportamiento de los controladores, es mediante eldespegue de la plataforma y comprobando que la evolución de la misma no deriva ensituaciones de inestabilidad. Para entornos como investigación básica y docencia, sonsituaciones no deseables.

Se hace por tanto imprescindible el desarrollo de entornos que permitan una simula-ción completa del sistema para su verificación, control de supervisión, planificación detareas, gestión de fallos y posterior validación. Es lo que se conoce típicamente como“Software-in-the-loop”: como máquina de estados que permite probar el comportamien-to del sistema ante cualquier entrada o evento externo, comprobando que la salida secorresponde con lo previamente diseñado.

1.2. Objetivos

El presente proyecto tiene como objetivo el desarrollo de una solución para la verifica-ción y validación de sistemas de gobierno y control de pequeñas plataformas de tipoMAV - VTOL (Micro Air Vehicle - Vertical Take Off Landing). Para la simulación delvehículo se dispone del correspondiente modelo dinámico, que será interconectado condiferentes sistemas de control y estabilización, gobernados por una máquina de estadosdefinida en MATLAB Stateflow®. El conjunto de simulaciones para la verificación deldiseño de la solución propuesta son llevadas a cabo mediante Simulink. El objetivo esdisponer de mecanismos que permitan evaluar el correcto funcionamiento de prototiposantes de ser evaluados en condiciones reales.

La solución propuesta en este trabajo, para diseñar e implementar una máquina decontrol de estados e integrarla en una plataforma de tipo quadrotor, está basada en lacombinación de MATLAB Stateflow® y el software de simulación MATLAB Simulink®,muy extendido a nivel académico. Stateflow es un entorno para el modelado y simula-ción de lógica de decisión combinatoria y secuencial basado en máquinas de estado ydiagramas de flujo. Permite la representación de diagramas de transición de estado conel fin de modelar la evolución del sistema ante eventos internos, externos o condiciones


basadas en tiempo.

1.3. Organización del trabajo

Este trabajo está dividido de la siguiente forma:

En el capítulo 2 se define el Estado del Arte. Se describen plataformas quadrotorsusceptibles de ser simuladas y algunos ejemplos de software de simulación yplanificación. Se presentan algunas ideas sobre la utilización de software comercialen el sector educativo.

En el capítulo 3 se describe el helicóptero quadrotor, comentando algunos detallesde su funcionamiento y a continuación se presentan las ecuaciones que definen elmodelo dinámico de este tipo de plataformas, mediante la matriz de rotación ylas ecuaciones cinemáticas.

En el capítulo 4 se detalla la descripción de la solución adoptada, donde se realizauna breve descripción del diagrama principal de Simulink y a continuación pasana desarrollarse cada uno de los bloques por separado. En primer lugar se habla delbloque Quadrotor, que contiene la implementación de las ecuaciones presentadasen el capítulo 3. Después, se desarrolla el bloque de Control, separado en controlde Alto Nivel y Bajo Nivel. El siguiente punto es una introducción a MATLABStateflow y el desarrollo de la máquina de estados, donde se encuentra el gruesodel trabajo. Se incorpora descripción de todos los estados añadidos y de las tran-siciones utilizadas. Por último, se añade la descripción de un bloque Generadorde trayectorias.

En el capítulo 5 se presentan diferentes simulaciones realizadas con objeto deverificar que el diseño se ajusta al comportamiento esperado y se presentan lasgráficas con los datos más relevantes para esta comprobación.

En el capítulo 6 finaliza la memoria de este trabajo añadiendo las conclusiones ylas posibles líneas de trabajo futuro que se presentan para la continuidad de estetrabajo.

Capítulo 2

Estado del arte

Hasta hace unos años, el desarrollo de vehículos aéreos no tripulados (UAV, por sussiglas en inglés) estaba asociado a pequeñas o medianas aeronaves militares cuya fun-ción principal era servir de aeronaves espía o incluso realizar misiones de ataque. Lasnoticias sobre este tipo de tecnología estaban asociadas en su mayoría al término droney sus connotaciones bélicas.

Sin embargo, durante los últimos años la investigación sobre el uso civil de estas plata-formas ha evolucionado de tal forma que sus posibilidades actuales se multiplican pormomentos. Cada vez es más habitual leer noticias sobre nuevas aplicaciones civiles endesarrollo (reparto de mercancía, uso en la construcción, detección de incendios, etc.).El coste relativamente asequible y las múltiples posibilidades que ofrecen este tipo deplataformas son claves en este crecimiento.

Actualmente, muchas empresas y centros de investigación se encargan de desarrollarnuevos vehículos y componentes, pero también de desarrollar el software necesario parasu uso eficiente y seguro. Se precisa de software de control, de planificación y de gestiónde datos, y además debe ser robusto y extremadamente fiable, por tratarse de platafor-mas aéreas.

7

CAPÍTULO 2. ESTADO DEL ARTE 8

Plataformas

Existe gran variedad de plataformas que permiten el desarrollo de aplicaciones y me-joras en el entorno académico, por ser accesibles y disponer de amplia información encomunidades educativas o en Internet. Algunos de los ejemplos más conocidos son:

AR.Drone

AR.Drone es un quadrotor comercial de la compañía francesa Parrot®. Fue lanzadopara uso recreativo, y destaca por su pilotaje ya que se controla desde un dispositivomóvil tipo smartphone o tablet, o desde un ordenador. Una de sus características prin-cipales es la presencia de una cámara que transmite imágenes del vuelo en tiempo real

Figura 2.1: Imagen del AR.Drone 2.0

El AR.Drone dispone de una diferencia fundamental con la mayoría de drones comer-ciales de la competencia, el software AR.Drone Open API Platform [11], que permiteel desarrollo de aplicaciones y juegos para esta plataforma. Gracias a este software pue-den realizarse proyectos que van mucho más allá del uso recreativo para el que en unprincipio fue diseñado. Dispone de multitud de documentación en la red, donde exis-ten comunidades de usuarios que comparten sus desarrollos, por lo que resulta muyaccesible.

Ardupilot

Ardupilot es una plataforma de vehículos aéreos no tripulados de código abierto. Permi-te controlar UAVs tipo aeronaves de ala fija, helicópteros convencionales, o plataformas


multirotor. Esta plataforma fue creada en 2007 por la comunidad DIY Drones [12],basándose en la plataforma de prototipos electrónicos de código abierto Arduino [13].Los vehículos tipo quadrotor se tratan en la sección Arducopter [14], donde se permitennumerosas configuraciones: Tricopter, Quadcopter, Hexacopter, Octacopter, etc. comose observa en la figura 2.2.

Figura 2.2: Posibles configuraciones de motores en la plataforma Arducopter

La ventaja de esta plataforma es que resulta muy sencillo cargar cualquier código ge-nerado en el IDE genérico de Arduino, e incluso, cualquier otro tipo código que puedaadaptarse a esta plataforma. Si bien es una plataforma muy extendida, no es posibleuna comprobación por simulacion del sistema, de tal manera que ante cualquier modi-cación del código fuente, o bien una reformulación de la lógica de aplicación, la unicadepuración y vericación posibles, incluyendo el comportamiento de los controladores,es mediante el despegue de la plataforma y comprobando que la evolución de la mismano deriva en situaciones de inestabilidad. Para entornos como investigación y docencia,son situaciones no admisibles.

Software

Si bien existe gran cantidad de software de simulación aeronáutica de forma gráfica,y las plataformas cuyo código es accesible tienen programas para poder modificar sucódigo, existe un vacío en términos de validación y verificación de estos UAV, tal y


Figura 2.3: Imagen de un quadcopter con la tecnología Arducopter

como se hace de forma constante en la industria aeronáutica de las grandes aeronaves.Es por eso que surge en gran parte este proyecto, ya que aunque algunas de sus funcio-nes pueden realizarlas alguno de las siguientes aplicaciones, el hecho de poder simularel comportamiento de un UAV sin necesidad de embarcar el código y testear su com-portamiento frente a diferentes modos de funcionamiento resulta bastante interesantedesde el punto de vista académico e investigador.

A continuación se detallan algunas aplicaciones que realizan funciones de simulación,planificación o desarrollo de plataformas tipo quadrotor:

Mission Planner

Software de configuración y planificación para plataformas Ardupilot.Este software permite definir ajustes iniciales, configurar parámetros, testear el dispo-sitivo, planificar misiones, obtener datos de vuelo y ejecutar análisis posterior mediantealmacenamiento de registro de datos o logs.

TrackDrone Lite

TrackDrone Lite es una aplicación desarrollada por el Grupo de Control Predictivo yOptimización Heurística de la Universidad Politécnica de Valencia. Permite implemen-tar, simular y testear cualquier tipo de algoritmos de control, destinados al seguimientode trayectorias de manera autónoma en la plataforma AR.Drone. Mediante este soft-ware es posible controlar el drone tanto en modo manual como en modo automático


Figura 2.4: Imagen del software Mission Planner

desde un PC con Windows mediante conexión WiFi.

Esta aplicación está desarrollada en Microsoft Visual C++ Express 2010. Para másinformación seguir la referencia [15]

Figura 2.5: Imagen de la aplicación TrackDrone Lite


AR.Drone Simulink Development-Kit

Este software implementado en MATLAB Simulink permite la simulación y controlmediante Wi-Fi del dispositivo ARDrone 2.0 [16]

Flightgear

FlightGear es un simulador de vuelo de código abierto multiplataforma de gran acepta-ción entre usuarios de este tipo de simuladores. Entre otras muchas funciones, destacala posibilidad de conexión mediante puertos internos o vía Internet a otros programaspara poder escribir y leer datos del juego.

Resulta interesante la opción de conectarlo con MATLAB Simulink gracias al bloque“FlightGear Preconfigured 6DoF Animation” del Aerospace Toolbox que incorpora esteprograma. Mediante este bloque es posible visualizar de forma gráfica la evolución delvector de estado de un modelo de UAV diseñado en MATLAB/Simulink, tan sóloconectando al bloque las entradas longitud, latitud, altitud, y posición angular (pitch,roll y yaw) desde el modelo propio.

Figura 2.6: Imagen del simulador Flightgear


AerosimRC

Software de simulación para entrenamiento. Permite iniciarse en el control de vehículosaéreos por RC, aprender nuevas técnicas de vuelo, entrenar modos de vuelo concretos opracticar el uso de elementos externos como camaras a bordo y cámaras sobre gimbal.

Sector educativo

Debido a la velocidad con la que evoluciona la tecnología, los estudiantes actuales estánsometidos cada vez más a todo tipo de medios audiovisuales. Este hecho plantea undesafío para los profesores, ya que resulta complicado atraer su atención, especialmenteen temas técnicos y con tiempo limitado. Además, las asignaturas de automática hande ser prácticas y actuales. Por tanto, atraer la atención de los estudiantes y mantenerel contenido actualizado y práctico son objetivos primordiales, que pueden alcanzarsegracias a los experimentos en clase, prácticas de laboratorio y contenido sobre tecnolo-gía de reciente aparición.

Como ya se ha comentado, los quadrotor presentan algunas grandes ventajas que loshacen útiles y accesibles para investigadores y comunidades educativas. Es por esto queresultan una opción perfecta para realización de prácticas y proyectos de asignaturasrelacionadas con el Control Automático.

Por eso otro motivo de desarrollo para este proyecto es presentar una forma de aprendi-zaje de algunos puntos importantes de la Automática mediante prácticas y experimen-tos, para aumentar el interés de los alumnos y sus conocimientos sobre esta materia.

Capítulo 3

Quadrotor (MAV-VTOL)

En este capítulo se describen las ecuaciones que rigen la dinámica de los sistemasquadrotor. La información que se presenta en este capítulo ha sido obtenida de lareferencia [17].

3.1. Descripción del MAV

El vehículo en que se basa este trabajo es un helicóptero tipo quadrotor, con cuatrorotores coplanarios, similar al de la figura 3.1.

Figura 3.1: Ejemplo de una plataforma tipo quadrotor

El movimiento de este tipo de vehículos se produce por los cambios de velocidad ensus rotores. Estos rotores constan de un motor eléctrico, mecanismo de engranaje y unrotor de palas.

15

CAPÍTULO 3. QUADROTOR (MAV-VTOL) 16

Para conseguir el movimiento en la dirección de uno de los ejes del vehículo, se debeaumentar la velocidad de giro del rotor trasero y disminuir la del rotor delantero. Éstoprovoca un par que origina el giro de la plataforma según el eje perpendicular a la direc-ción del movimiento. Para conseguir el desplazamiento en la dirección perpendicular,se realiza la misma operación pero con los otros dos rotores.

El movimiento en la dirección vertical se produce aumentando o disminuyendo la velo-cidad de giro de los cuatro rotores de forma simultánea.

El quadrotor dispone de dos rotores que giran en sentido horario y dos en sentido anti-horario. Esto anula el giro de la plataforma por el giro de sus rotores y permite eldenominado movimiento de guiñada, mediante diferencia en la velocidad de giro entrelos rotores con sentido horario y los de sentido anti-horario.

Figura 3.2: Esquema de las fuerzas y pares que actúan sobre un vehículo tipo quadrotor

3.2. Modelado

Para obtener el modelo dinámico basado en ecuaciones que describan la posición yorientación, se supone el quadrotor como un cuerpo rígido en el espacio, sujeto a unafuerza principal (empuje) y tres momentos (pares). En la figura 3.1 se muestra unesquema de las fuerzas que ejercen las distintas hélices para generar el movimiento delvehículo.

A continuación, de una forma más detallada, se describen las fuerzas y pares actuantessobre la plataforma.


El par para generar un movimiento de balanceo o de roll (ángulo φ) se realiza medianteun desequilibrio entre las fuerzas f2 y f4 . Para el cabeceo o pitch (ángulo θ), el des-equilibrio se realizará entre las fuerzas f1 y f3. El movimiento en el ángulo de guiñadao de yaw (ángulo ψ) se realizará por el desequilibrio entre los conjuntos de fuerzas (f1 ,f3) y (f2 , f4). Se debe a que los rotores 1 y 3 giran en sentido contrario a los rotores 2y 4. El empuje total produce que el quadrotor se desplace perpendicularmente al planode los rotores, y se obtiene como suma de las cuatro fuerzas que ejercen los rotores.

Los quadrotor suelen ser sistemas de vuelo de estructura ligera, por lo que el modelodinámico debe incluir los efectos giroscópicos resultantes tanto del cuerpo rígido rotandoen el espacio, como de la rotación de las cuatro hélices. En la Tabla 3.1 se describentales efectos, donde C representan términos constantes, Ω es la velocidad del rotor, JRes el momento de inercia rotacional del rotor alrededor de su eje, l es la distancia delcentro de masa a los rotores, J es el momento de inercia del cuerpo rígido y φ, θ y ψson los denominados ángulos de Tait-Bryan.

Efectos Fuentes Formulación

Efectos Aerodinámicos- Rotación de los rotores

CΩ2- Giro de hélices

Pares Inerciales Opuestos - Cambio en la velocidad de rotación de los rotores JRΩEfectos de la gravedad - Posición del centro de masa l

Efectos giroscópicos-Cambio en la orientación del cuerpo rígido Jθψ

- Cambio en la orientación del plano de los rotores JRΩθ, φ

Fricción - Todos los movimientos del helicóptero Cφ,θ, ψ

Cuadro 3.1: Principales efectos físicos actuantes sobre un helicóptero

El quadrotor es un sistema mecánico subactuado de 6 grados de libertad y 4 entradasde control. Debido a las complejidad de esta situación, se deben realizar algunas supo-siciones para el modelado: Se desprecian los efectos de los momentos causados por elcuerpo rígido sobre las dinámicas traslacionales, así como el efecto suelo. El centro demasa y origen del sistema de coordenadas fijo se consideran coincidentes, y se suponeque la estructura del helicóptero es simétrica, lo que implica que la matriz de inerciasea diagonal.


3.3. Estimación del estado

En esta sección se presenta la estimación de la orientación y posición inercial del vehícu-lo. El quadrotor, como sólido rígido, está caracterizado por un sistema de coordenadasligado a él y con origen en su centro de masas. El sistema de ejes ligado a él está de-terminado por B = −→xL,−→yL,−→zL, donde el eje −→xL es la dirección normal de ataque delhelicóptero, −→yL es ortogonal a −→xL y es positivo hacia la derecha mirando en el sentidopositivo de éste, mientras que −→zL está orientado en sentido ascendente y ortogonal alplano −→xLO−→yL. El sistema de coordenadas inercial I = −→x ,−→y ,−→z se considera fijo conrespecto a la Tierra. Se determina ξ = x, y, z como la posición del centro de masa delhelicóptero con respecto al sistema inercial I, y la orientación del vehículo se supondrádada por una matriz de rotación RI : B→I , donde RI ∈ SO(3) es una matriz derotación ortonormal.

La rotación de un cuerpo rígido puede ser obtenida mediante ángulos de Euler, o cuater-niones, por ejemplo. Existen varias definiciones de los ángulos de Euler para representarla orientación relativa de dos sistemas de coordenadas. La más popular en ingeniería ae-roespacial es la convención-xyz (giro alrededor de x, y’, z”) y es conocida como ángulosde Tait-Bryan, también conocidos por ángulos “Cardano”.

Los ángulos de Tait-Bryan se utilizan para describir una rotación general en el espacioEuclideo tridimensional a través de tres rotaciones sucesivas en torno de ejes del sistemamóvil en el cual están definidos. Así, en este trabajo se usarán los ángulos de Tait-Bryanpara describir la orientación del quadrotor.

La rotación de un cuerpo rígido en el espacio se realiza mediante tres rotaciones suce-sivas:

1. Rotación según −→x de φ: el primer giro es el correspondiente al ángulo de roll ode balanceo, φ, y se realiza alrededor del eje −→x .

x1

y1

z1

=

1 0 00 cosφ − senφ0 senφ cosφ

xL

yL

zL

(3.1)

2. Rotación según −→y de θ: el segundo giro se realiza alrededor del eje −→y a partirdel nuevo eje −→yL con el ángulo ángulo pitch o de cabeceo, θ, para dejar el eje −→zL


en su posición final. x2

y2

z2

=

cos θ 0 sen θ

0 1 0− sen θ 0 cos θ

x1

y1

z1

(3.2)

3. Rotación según −→z de ψ: el tercer giro y la última rotación corresponde al ánguloyaw o de guiñada, ψ, y se realiza alrededor del eje −→z a partir del nuevo eje −→zLpara dejar el quadrotor en su posición final.

x3

y3

z3

=

cosψ − senψ 0senψ cosψ 0

0 0 1

x2

y2

z2

(3.3)

En esta representación surge una singularidad en θ = ±π/2, en cambio en los ángulosφ y ψ el giro completo no produce singularidades. La figura 3.3 representa las tresrotaciones:

Figura 3.3: Rotación de los ángulos de Tait-Bryan del sistema de coordenadas inercialal sistema de coordenadas fijado al helicóptero.

Las matrices de rotación que representan la orientación del cuerpo rígido rotando alre-dedor de cada eje queda como sigue:


R(x, φ) =


, R(y, θ) =

cos θ 0 sen θ

0 1 0− sen θ 0 cos θ

, (3.4)

R(z, ψ) =


0 0 1

La matriz de rotación completa de B respecto a I, llamada matriz de cosenos directores,viene dada por:

RI = R(z, ψ)·R(y, θ)·R(x, φ)

RI =


0 0 1

·

cos θ 0 sen θ0 1 0

− sen θ 0 cos θ

·


(3.5)

RI =

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φ

La matriz de rotación expresada en el sistema de coordenadas B es la traspuesta deRI , por su propiedad ortonormal, y viene dada por:

RB =

cosψ cos θ senψ cos θ − sen θ

cosψ sen θ senφ− senψ cosφ senψ sen θ senφ+ cosψ cosφ cos θ senφcosψ sen θ cosφ+ senψ senφ senψ sen θ cosφ− cosψ senφ cos θ cosφ

(3.6)

A partir de la matriz de rotación 3.5, se pueden obtener las ecuaciones cinemáticas derotación del quadrotor que establecen las relaciones entre las velocidades angulares.

Sea una matriz ortonormal R, donde:


RTR = In (3.7)

y su derivada respecto al tiempo es:

RTR + RTR = 0n (3.8)

Definiendo:

S = RTR (3.9)

se obtiene a partir de 3.8 que:

ST+S =0n (3.10)

donde S es anti-simétrica. La relación entre la derivada de la matriz ortonormal y lamatriz anti-simétrica es la siguiente:

S = R-1R (3.11)

Por lo tanto, las ecuaciones cinemáticas para determinar la orientación del helicópterosuponiendo la matriz de rotación 3.5, vienen dadas por:

RI = RI ,·S(ω) (3.12)

donde ω = [p q r]T son las velocidades angulares en el sistema de coordenadas ligadoal cuerpo rígido y S(ω) (S(ω)(·) = ω ×·) es la siguiente matriz anti-simétrica:

S(ω) =

0 −r q

r 0 −p−q p 0

(3.13)

Manipulando matemáticamente 3.12 se obtiene:


φ

θ

ψ

=

1 senφ tan θ cosφ tan θ0 cosφ − senφ0 senφ sec θ cosφ sec θ

p

q

r

(3.14)

La variación de los ángulos de Tait-Bryan (φ, θ, ψ) es una función discontinua. Hay quedistinguirla de las velocidades angulares en el sistema de referencia asociado al cuerporígido (p, q, r) las cuales pueden medirse con giróscopos. Normalmente, se utiliza laIMU, del inglés Inertial Measurement Unit o Unidad de Medida Inercial, para medirlas rotaciones y calcular los ángulos de Tait-Bryan.

La relación entre las velocidades angulares en el sistema fijado al cuerpo y la variación enel tiempo de los ángulos de Tait-Bryan se obtiene a través de la inversión del Jacobianode 3.14, y viene dada por:

p

q

r

=

1 0 − senφ0 cosφ senφ cos θ0 − senφ cosφ cos θ

φ

θ

ψ

(3.15)

El movimiento rotacional del quadrotor viene dado por las componentes de las veloci-dades angulares: velocidad angular de balanceo (p), velocidad angular de cabeceo (q),y velocidad angular de guiñada (r), sobre los ejes −→xL,−→yL y −→zL respectivamente. Las ve-locidades angulares son debidas a los pares ligados al cuerpo rígido producidos por lasfuerzas externas, las cuales definen momentos en los tres ejes: momento de balanceo(L), momento de cabeceo (M), y momento de guiñada (N ) sobre los ejes −→xL,−→yL y −→zLrespectivamente

El movimiento de traslación viene dado por la velocidad v = [u0 v0 w0]T en ejesinerciales, y relacionada con la velocidad absoluta del helicóptero expresada en B, V =[uL vL wL]T mediante la expresión:

v = RI·V (3.16)

3.4. Modelo dinámico

En esta sección se obtendrán las ecuaciones dinámicas del quadrotor. A través de laformulación de Newton-Euler, es posible obtener las ecuaciones dinámicas de un cuerpo


rígido sujeto a fuerzas externas aplicadas en el centro de masas, que expresadas en losejes cuerpo quedan de la siguiente forma:

mI3x3 00 J

Vω

+ ω ×mVω × Jω

= F + Fd

τ + τd

(3.17)

donde J∈R3x3 es la matriz de inercia, I3x3∈R3x3 es la matriz identidad, V es el vectorvelocidad traslacional en ejes cuerpo, ω es el vector velocidad angular, también en ejescuerpo, y m es la masa total del quadrotor.

Con las suposiciones iniales, puede considerarse que la matriz de inercia es diagonal:

J =

Ixx 0 00 Iyy 00 0 Izz

(3.18)

Siendo el vector de estado [ξ v η ω]T , donde ξ y v ∈ R3 representan la posición yvelocidad lineales expresadas en los ejes inerciales, η la posición angular y ω la velocidadangular, ambas en ejes cuerpo, las ecuaciones de movimiento de un cuerpo rígido quedande la siguiente forma:

ξ = v

mv = RIFb

RI = RIS(ω)

Jω = −ω × Jω + τb

(3.19)

donde ξ = v = RIV y S(ω) = RTI RI .

Queda expuesto por tanto que el quadrotor, como se indicó en la introducción de estecapítulo, es un sistema mecánico subactuado con 6 grados de libertad y 4 actuaciones(los tres pares producidos por las hélices y la fuerza resultante ejercida por éstas).

También se deben tener en cuenta las fuerzas y pares externos que actúan sobre elquadrotor, representados por Fb y τb. Estas fuerzas y pares son el peso, fuerzas aero-dinámicas, y los pares y el empuje desarrollado por los motores, que se representanpor:


RIFb = −mg·E3 + RIE3

( 4∑i=1

bΩ2i

)+ AT

τb = −4∑i=1

JR(ω × E3)·Ωi + τa + AR

(3.20)

Estas ecuaciones junto con las del modelo dinámico 3.19 pueden reescribirse de la si-guiente forma:

ξ = v

v = −g·E3+RIE3bm

( 4∑i=1

bΩ2i

)+ AT

m

RI = RIS(ω)

Jω = −ω × Jω−4∑i=1

JR(ω × E3)·Ωi + τa + AR

(3.21)

donde:

AT y AR son las fuerzas y pares aerodinámicos que actúan sobre el quadrotorcalculados a partir de los respectivos coeficientes, Ci, mediante la expresión Ai =12ρaireCiW

2, donde ρaire es la densidad del aire y W es la velocidad respecto alaire.

g es la constante gravitacional

JR es el momento de inercia rotacional del rotor alrededor de su eje

b es el coeficiente de empuje aplicado a los rotores

Ωi es la velocidad angular del motor i

El empuje total T , considerado como la entrada de control al modelo U4, viene deter-minado por la suma de las fuerzas de empuje generadas por cada motor fi, como sepresenta en la siguiente ecuación:

T = U4 =( 4∑i=1

fi

)=( 4∑i=1

bΩ2i

)(3.22)

El vector de pares aplicados sobre el quadrotor está representado por τa y viene determi-nado por el esfuerzo de torsión τM generado por cada motor, considerando desacoplado


en la variable Ωi, velocidad de giro del motor, el sistema dinámico del disco del motor.El motor aplica un esfuerzo de tensión que se opone a la fricción aerodinámica, deno-tada por τdrag = krΩi, siendo kr > 0 una constante. Así, aplicando la segunda ley deNewton:

JRΩi = −τdrag + τMi(3.23)

Las otra entrada de control es el vector de pares, determinado por:

τ a =

l(f2 − f4)l(f3 − f1)

4∑i=1

τMi

=

lb(Ω2

2 − Ω24)

lb(Ω23 − Ω2

1)kr(Ω2

1 + Ω23 − Ω2

2 − Ω24)

l·U1

l·U2

U3

(3.24)

donde l es la distancia del centro de masas a los motores.

Capítulo 4

Descripción de la solución

4.1. Introducción

La solución adoptada en este proyecto permite simular y validar el diseño de una má-quina de control para pequeñas plataformas tipo MAV - VTOL mediante un diagramade bloques desarrollado en MATLAB Simulink.

El bloque que contiene la lógica de decisión consiste en una máquina de estados basadaen Stateflow. Esta máquina de estados conecta con un sistema de control que propor-ciona las referencias necesarias para el movimiento de la plataforma, que es calculadomediante el bloque “Quadrotor”, y proporciona como resultado el vector de estado.

El diagrama de bloques se compone de los siguientes elementos:

Bloque Quadrotor, donde quedan implementadas las ecuaciones del modelo diná-mico de la plataforma, presentadas en la sección 2.

Bloque de Control, encargado de proporcionar las señales de referencia a la pla-taforma. Se descompone a su vez en dos bloques, “Alto Nivel” y “Bajo Nivel”,donde se sitúan respectivamente los controladores de posición y los controladoresde estabilización.

Stateflow o máquina de estados, bloque principal del proyecto, donde queda reco-gida la lógica de gobierno de la plataforma, y se establece el tipo de referenciasque se proporcionan al Bloque de Control.

27

CAPÍTULO 4. DESCRIPCIÓN DE LA SOLUCIÓN 28

Generador de trayectorias, el bloque que genera una secuencia de puntos de pasoy los envía a la máquina de estado como valores de referencia en posición.

Bloque Plot, donde se generan gráficas útiles para la visualización e interpretaciónde los resultados.

En la figura 4.1 se puede observar una vista completa del modelo.

Figura 4.1: Modelo completo desarrollado en MATLAB Simulink

4.2. Modelo dinámico

El bloque Quadrotor recoge la implementación del modelo dinámico de la plataformabajo estudio, presentado en el capítulo 2. Las entradas del bloque se corresponden conlos pares de actuación [τφ, τθ, τψ] y empuje total T calculados por el bloque Control,que provocarán la evolución del vector de estados, representado como

x = [ x y z x y z φ θ ψ φ θ ψ ] (4.1)

Estas variables representan, respectivamente, las tres componentes de posición lineal,las tres de velocidad lineal, las tres de posición angular y por último las tres correspon-dientes a las derivadas de los ángulos de Tait-Bryan.


Figura 4.2: Ejes representativos de las coordenadas locales del quadrotor

Dentro de este bloque, se pueden encontrar otros dos bloques adicionales, tal y comose ve en la figura 4.3, y cuya función está explicada a continuación:

Figura 4.3: Imagen del bloque Modelo Dinámico

ControlMixer Transforma los pares solicitados por el controlador y el empuje totalen velocidad de giro los rotores al cuadrado, aplicando las saturaciones y paráme-tros correspondientes, configurados por el autor [3]. Se puede ver el diagrama debloques en la figura 4.4

Quadrotor Contiene las ecuaciones de la dinámica del quadrotor que obtienen el vectorde estado x a partir de la salida del Control Mixer. Esta función queda implemen-tada en el archivo dinamica_quadrotor.m, que puede verse de forma completaen el apéndice B.1. Se ha obtenido del modelo de Peter Corke incluido en lasreferencias [3].

Adicionalmente, pueden añadirse modelos de perturbaciones externas, tales como vientoo efecto suelo, mediante bloques propios que incluyan el modelado de estos efectos, o


Figura 4.4: Contenido del bloque “Control Mixer”

los propios del Aeroespace Toolbox.Este modelo puede ser sustituido por otro modelo dinámico de cualquier plataformamultirotor a condición de que tenga como entrada los pares de actuación y el empujetotal.

4.3. Control

El conjunto de bloques relativos al control están gobernados por la máquina de estadosy proporcionan las señales de actuación a la plataforma para lograr las referenciasdeseadas. Si bien es un elemento clave en el diseño de la solución global, para un primerdesarrollo se ha optado por reutilizar controladores para plataformas de tipo VTOLdisponibles en [3].

Consisten básicamente en controladores tipo PD para el control de estabilización yposición. Para un mayor detalle sobre la implementación de las leyes de control, seguirla referencia indicada [3]

Típicamente, debido a la dinámica de los subsistemas que conforman el modelo de estetipo de plataformas (modelado visto en el capítulo 2), la tarea del control se descomponeen control rotacional y control traslacional [2] [8] [9](alternativamente de estabilizacióny traslación). El modelo presentado en la figura 4.5 presenta esta descomposición.

4.3.1. Control de de estabilización

También denominado control rotacional, está incluido en el bloque “Bajo Nivel”.


Figura 4.5: Diagrama del modelo donde se incluyen las referencias de entrada y salidade cada bloque de control

Se tiene como entrada la posición angular de referencia,[φref , θref , ψref ], proporcionadapor el bloque de control de posición o como referencia directamente en posición angular.A pesar de que el movimiento según el eje z se considera de traslación, se ha incluidoen el modo de Bajo Nivel por ser un control directo entre la referencia en altura y elempuje total suministrado, T .

Los bloques principales son los tipos de control implementados para “Bajo Nivel”, y elinterruptor se encarga de conmutar entre estos tipos.

La salida resultante son los pares correspondientes a los ángulos de roll, pitch y yaw,[τφ, τθ, τψ], y el empuje total T .

Figura 4.6: Diagrama Simulink del Control de estabilización, incluido en el bloque “BajoNivel”


4.3.2. Control de posición

También denominado control traslacional, se incluye en el bloque “Alto Nivel” . Tomacomo referencia la posición [xref , yref ] y proporciona la posición angular de referen-cia al control de estabilización, mediante los valores [φref , θref ], correspondientes a losdenominados ángulos de roll y pitch, respectivamente. La señal correspondiente a lavariable zref se traslada directamente al control de estabilización.

Una de las entradas a los bloques de control de traslación es el bus que incluye lasreferencias de posición angular. Esta entrada se realiza para incorporar el valor dereferencia del ángulo yaw, ψref , al vector de salida, [φref , θref , ψref ], ya que este valores independiente de la posición lineal, se obtiene en su caso de la máquina de estados,y su par correspondiente se calcula en el control de Bajo Nivel.

En la figura 4.7 se observan dos bloques principales, correspondientes a los tipos decontrol implementados para Alto Nivel, un switch o interruptor que conmuta entre estostipos de control, y otro interruptor más denominado “Switch1” que conmuta entre dosseñales: las referencias en posición angular proporcionadas por el control traslacional olas proporcionadas directamente por la máquina de estados. La decisión de activar unade estas dos señales corresponde al hilo HI de la entrada EN, que se encarga de activaro desactivar el control de posición.

Figura 4.7: Diagrama Simulink del Control Traslacional, incluído en el bloque “AltoNivel”

4.3.3. Lógica de gobierno

La lógica de gobierno de las leyes de control se basa en las siguientes ideas:


La interfaz que presentan los bloques de control, tanto al diagrama de estadoscomo al modelo dinámico, es similar, independientemente de las leyes que se es-tén ejecutando, sean del tipo lineal o no lineal, en su implementación en tiempocontinuo o discreto. Esto permite la adición de nuevos bloques de control, sinmodificar el diseño global (tan sólo el subsistema al que afecte), para ser evalua-dos y verificados. Pueden añadirse tantos modos de control como se quiera. Porejemplo, se podría insertar un bloque de control adaptativo o de control robusto.La única condición es tenerlos en cuenta en la lógica de Stateflow. Este detalle sepuede observar en las figuras 4.7 y 4.6.

El bus “enabled” proviene de la máquina de estados y contiene las señales quedeterminan qué bloques del control son activados o desactivados: en función delmodo de vuelo para elegir entre control de posición o control de estabilización, ypor solicitud del usuario al decidir utilizar control Lineal, No Lineal, o cualquierotro que se haya podido añadir. La lógica de gobierno de control incluída en lamáquina de estados está explicada en la sección 4.4.3

Uno de los objetivos del diseño inicial era poder permitir una conmutación en“caliente” entre las diferentes leyes de control. Tanto el usuario como el sistema,pueden, en cualquier momento y si el sistema lo cree conveniente, poder conmutarentre leyes de estabilización o traslación. Se observa el detalle de la señal “enable”para poder simular la habilitación de diferentes leyes de control en “caliente”.

4.4. Stateflow

4.4.1. Diseño

La solución propuesta en este trabajo, para diseñar e implementar una máquina decontrol de estados e integrarla en una plataforma de tipo quadrotor, está basada enla combinación de MATLAB Stateflow® y el software de simulación MATLAB Simu-link®, muy extendido a nivel académico[4]. Stateflow es un entorno para el modeladoy simulación de lógica de decisión combinatoria y secuencial basado en máquinas deestado y diagramas de flujo.


El diagrama Stateflow permite definir lógica de decisión compleja en el control de laplataforma de forma gráfica y estructurada, con el fin de simular la respuesta del siste-ma a entradas externas, eventos y condiciones temporales. En esta propuesta, existendos elementos principales para el diseño: estados y transiciones.

Estado: Los estados representan modos de funcionamiento del sistema. En éstosse establecen valores para variables y se incluyen funciones relativas a la semánticade Stateflow.Los estados se utilizan para el diseño secuencial de diagramas de transición deestados. Pueden estar activos o inactivos, dependiendo de las condiciones que seapliquen.

Los estados se organizan de forma jerárquica y pueden relacionarse entre sí me-diante transiciones, que pueden incluir algún tipo de restricción o condición ypermiten gestionar el paso de un estado a otro. Dos o más estados en el mismonivel pueden trabajar de forma exclusiva (OR) o de forma paralela (AND). Losestados contienen etiquetas, que pueden ser, entre otras:

• Nombre: Nombre del estado.

• Entry (en): Para acciones al comienzo del estado

• During (du): Para acciones a ejecutar mientras permanece activo el estado

• Exit (ex): Acciones a la salida del estado.

Transición: Gráficamente, son líneas acabadas en flecha, indicando el sentido dela transición, que unen unos estados con otros. Representan el paso de un estadoorigen a un estado objetivo, y pueden llevar adjuntos una etiqueta en forma deevento o condición que se encarga de gestionar la conmutación entre dos estados.

Además de los componentes de Stateflow, en los estados y transiciones se pueden in-tegrar otro tipo de componentes para complementar la máquina de estado, tales comofunciones de MATLAB, Simulink, código C o tablas de verdad. En este trabajo se hanincluido funciones de MATLAB para las transiciones entre modos de vuelo, como severá a continuación en la descripción de los estados y transiciones.


Figura 4.8: Elemento de Stateflow para incorporar scripts de Matlab

Otra de las ventajas que presenta Stateflow es la visualización de la evolución de losestados durante la simulación, y la posibilidad de observar los resultados mediante grá-ficas de MATLAB o Scopes de Simulink.La integración de Stateflow en Simulink se realiza mediante el bloque “Stateflow Chart”,donde conectan las entradas y salidas de la máquina de estados.

Para recoger toda la lógica de la plataforma, se han definido los estados y la jerarquíaestablecidos en la figura 4.9. Las líneas discontínuas significan que los estados se ejecutande forma paralela.

El diagrama de Stateflow incluye como estados principales “Navegación” y “Modo deControl”. Ambos se ejecutan de forma paralela. En “Navegación” se encuentran todoslos modos de funcionamiento, y en “Modos de Control” se incluyen los estados queregulan el tipo de control que gobierna la plataforma.

Figura 4.9: Esquema explicativo de los estados que componen el bloque Stateflow y sujerarquía


4.4.2. Estados de navegación

En el bloque “Navegación” se gestiona la lógica del modo de funcionamiento solicitadopara que se ejecute de forma correcta y con la máxima seguridad. Los subestadosprincipales de este estado son los modos “Tierra”, “Aire” y “Emergencia”, descritos acontinuación. En la figura 4.10 se puede ver un esquema de este estado principal.

Figura 4.10: Modos de funcionamiento correspondientes al bloque “Navegación”

4.4.2.1. Tierra

El modo “Tierra” transcurre desde que la plataforma es iniciada (se conecta a la alimen-tación eléctrica) hasta que se produce el armado de motores y comienza el movimiento.Mientras este estado está activo, se ejecuta la función ok_tierra=check(modo) dondedeben incluirse las comprobaciones sobre el estado de sensores y demás elementos de laplataforma para garantizar la seguridad, tales como GPS, enlace RC, sónar, barómetro,IMU, sensor de flujo óptico, etc. Además, comprueba si el modo de vuelo solicitadopertenece al estado “Aire”, con el fin de evitar bucles internos innecesarios en caso deencontrarse la plataforma en tierra y solicitar los modos “Aterrizaje” o “Emergencia”.

El modo “Tierra” tiene como predeterminado el subestado “Check”, donde se verificaque la variable ok_tierra devuelva el valor lógico “1” , y que el usuario esté solicitandoel inicio del modo “Aire”, a través de la entrada al bloque STATEFLOW aire_tierra. Sise cumplen estas dos condiciones, se pasa al estado “Armado Motores”, donde se iniciael giro de los rotores y se pasa al modo “Aire”.


Figura 4.11: Diagrama Stateflow del estado principal “Tierra”

4.4.2.2. Aire

Este estado es donde transcurre principalmente la navegación de la plataforma. Constade los modos “Despegue”, “Vuelo”, “Aterrizaje” y “To Home”.

Figura 4.12: Diagrama donde se presentan los modos de funcionamiento incluidos en elestado principal “Aire”

4.4.2.2.1. DespegueEl modo despegue es el modo por defecto del estado “Aire”, ya que es donde se inicia

el vuelo. Se trata de control de bajo nivel (nivel 0) y consiste en proporcionar únicamentereferencias en altura, manteniendo en el origen las referencias para los ángulos pitch yroll. El ángulo yaw se mantiene en el valor actual de ese instante para anular el par, yaque no se considera relevante para esta maniobra. Para realizar el despegue de formaestable se han incluido tres subestados detallados a continuación:

1. D_0: Obtiene el valor de las variables tD, zD , correspondientes al valor del tiempo


y la altura en el instante en que comienza el modo “Despegue”

2. D_1: A partir de estos valores, se proporciona la referencia en altura en formade rampa mediante la ecuación zref = zD − 0,8 ∗ (t− tD). Esta es la ecuación deuna rampa con una pendiente de valor 0.8. El signo negativo se debe a que setrabaja en ejes horizonte local, donde la altura desde el suelo es negativa.

3. D_2: La transición a este estado se efectúa cuando |zref | > |hdespegue|. En eseinstante, se determina zref = hdespegue y se pasa automáticamente al modo “Vuelo”.

Figura 4.13: Diagrama Stateflow del modo “Despegue”

4.4.2.2.2. VueloEl estado “Vuelo” se inicia una vez acabado el despegue y es donde se efectúan las

operaciones para las que se ha iniciado el vuelo de la plataforma. Consta de los cuatroestados de vuelo “Estabilización”, “Estabilización en altura”, “Movimiento en el plano”y “Movimiento en el espacio”, además del modo utilizado como transición “Stop”.

Estabilización

Es el modo de vuelo por defecto, y su objetivo es mantener estabilizada la plataforma.Es por tanto un modo de bajo nivel. Se proporciona como referencia un valor constantepara el empuje T . Los valores de los ángulos pitch y roll están definidos en la funcionauxiliar de MATLAB eulerRef0 = est(yaw0), en la que se adjudican los valores dereferencia de posición angular.


Estabilización en altura

Este modo es similar al anterior pero añadiendo como grado de libertad adicional con-trolado la altura respecto al suelo. Permite el movimiento en dirección vertical pro-porcionando un valor de referencia zref que puede ser una constante o una variableproporcionada por el Gestor de Trayectorias. El ángulo ψref se iguala al valor de ψpara eliminar el giro residual. Los valores de referencia de los ángulos se dan continua-mente en la función [eulerRef1,z_ref1]=alt(ref1,yaw1).

Movimiento en el plano

Este es el modo de movimiento en el plano. En él se mantiene constante la referencia dealtura a la que se encuentra en ese instante y se proporciona una referencia en posiciónen el plano XY, bien constante o proporcionada por el generador de trayectorias. Eneste caso dicho sistema definiría una serie de waypoints que conformarían la trayectoriadeseada. Este bloque permite incluir algoritmos de planificación de caminos que expan-den las utilidades de la plataforma al aumentar su nivel de autonomía. En este modose activa el control de alto nivel. La función [xyz2,yaw2]=plano(ref2,estado2,yawRef2)proporciona las referencias en posición, adjudicando a [xref , yref ] sus valores correspon-dientes con que provienen del generador de trayectorias. A zref se le asigna el valor dez del vector de estado para mantenerla constante. El ángulo ψref toma el valor pro-porcionado por el gestor de trayectorias, por considerarse interesante la posibilidad deproporcionar referencias en este caso, por ejemplo, en caso de tener una cámara fijainstalada y querer que apunte según la dirección del movimiento.

Movimiento en el espacio

En este último modo de vuelo son proporcionadas referencias para los 3 grados de li-bertad traslacionales de la plataforma, permitiendo un movimiento libre en el espaciotridimensional. El movimiento se produce al proporcionar como referencia puntos delespacio, que tras pasar por el control de alto nivel, se convierten en referencias de posi-ción angular. Conserva las características del modo 2 pero añade la posibilidad de variarreferencias en altura. La función que ejecuta es [xyz3,yaw3] =sixDoF(ref3,yawRef3), queadjudica a [xref , yref , zref ] los valores correspondientes que provienen del generador detrayectorias. En caso del ángulo ψref , al igual que en el modo de movimiento plano, setoma la referencia externa.


Stop

Este modo permite una transición segura entre los demás modos de vuelo, ya queobliga al vehículo a detenerse antes de cambiar de un movimiento a otro. El motivo derealizar las transiciones de esta forma es que existen dos modos de control de posicióny dos modos de control de estabilización; cuando se pasa de rotacional a traslacionalsin detener el vehículo, se anulan las referencias en posición angular pero la inercia quelleva el vehículo provoca que la transición sea inestable, y por tanto se hace necesariodetenerlo. Para ello se hace uso de las funciones stop y ok_stop, explicadas en el siguienteapartado.

Figura 4.14: Diagrama Stateflow del modo “Vuelo”

4.4.2.2.3. AterrizajeEste modo permite aterrizar la plataforma de forma estable y segura en 4 pasos:

1. A_1: Parar la plataforma. Mediante la función xyz=stop(estado) se toma comoreferencia en posición lineal la posición de la plataforma en ese instante. Se ejecutaal comienzo del estado. Durante el periodo que permanece activo este estado seejecuta la función ok_A=ok_stop(estado,xyz), que se encarga de verificar que elerror entre la referencia y la posición actual es un número menor que una ciertatolerancia establecida. Si esto es correcto, ok_A toma el valor lógico “1” y permitepasar al siguiente estado.


2. A_2: Referencia en altura. Se proporciona una referencia en rampa para la va-riable z de tal forma que el aterrizaje sea progresivo, suave y seguro. A la entradadel estado, se toman los valores [tA, zA]correspondientes al tiempo y la altura endicho instante. A partir de éstos, se elabora la referencia en rampa mediante laecuación zref = zA + 0,8·(t− tA), correspondiente a una rampa con pendiente demagnitud 0.8 positiva, por encontrarnos en el sistema de referencia de ejes locales.

3. A_3: Se inicia cuando la referencia supera el valor zref > 0. En ese instantese determina zref = 0, para no proporcionar valores de altura bajo el suelo.Lo deseable en la implementación sería proporcionar una referencia descendenteen altura mediante la utilización de sensores de tipo sónar que proporcionan laaltura actual de la plataforma. Esta condición podría considerarse para un trabajofuturo.

4. A_4: Cuando el valor de z pasa a ser 0, se entiende que ha tocado el suelo, einicia este estado que sirve de transición al estado “Tierra”.

Figura 4.15: Diagrama Stateflow del modo “Aterrizaje”

4.4.2.2.4. ToHomeEste modo permite volver al punto inicial desde donde despegó la plataforma, con

la altura de referencia de despegue, hdesp. Consiste en proporcionar como referencia elvalor inicial de las posiciones x e y, que por tratarse de ejes horizonte local son valoresnulos. La presencia de este modo se justifica para casos en los que el quadrotor seha desplazado más allá de la línea de visión y se pretende que vuelva o simplementepara aterrizar en el punto de partida, para lo cual sería necesario ejecutar el modo


Aterrizaje cuando el quadrotor se encuentre sobre el origen. Se podría haber incluidoel aterrizaje de la plataforma dentro de este modo, pero se ha considerado innecesariopor redundancia con el modo Aterrizaje. La referencia en yaw se establece como la quetiene en el instante en que se inicia el modo por no considerarse relevante.

Figura 4.16: Diagrama Stateflow del modo “To Home”

4.4.2.3. Emergencia

El modo “Emergencia” aporta seguridad a la plataforma ya que permite aterrizar elvehículo en lugar en que se encuentre siempre que el usuario lo solicite o el sistemade forma autónoma detecte un error que, previamente programado, considere comoemergencia. Estos casos podrían ser, por ejemplo:

Nivel de carga de la batería por debajo de un cierto límite.

Lectura de sensores errónea, posible descalibración.

Pérdida de conexión con la estación base o con el control remoto.

Referencia proporcionada por el generador de trayectorias no realizable.

También sería posible implementar una función de ley de control que detecte fallos en lavelocidad de los rotores. Un ejemplo de este caso puede ser la rotura de una o varias delas hélices, caso en el cual para un aterrizaje estable debería implementarse un controladaptativo que permitiese el control de la plataforma mientras rota sobre sí misma. [10]

El estado “Emergencia” se encuentra fuera del estado principal “Aire” para permitiraterrizar en cualquiera de los modos de vuelo descritos con anterioridad si se encuentraalgun fallo.


El ángulo yaw se mantiene el valor que tenga en ese instante, ya que no se consideraimportante para este modo. Los subestados que forman el modo “Emergencia” sonsimilares al modo de vuelo “Aterrizaje”:

1. E_1: Se detiene al vehículo utilizando la función xyz=stop(estado) , que pro-porciona como referencia en posición lineal la posición en ese instante. Durante lapermanencia en el estado se ejecuta la función ok_EM = ok_stop(estado,xyz) ,que comprueba que el error entre la referencia y la posición actual sea menor queuna cierto valor establecido. En ese caso se pasa al siguiente estado.

2. E_2: Se proporciona una referencia en rampa para la variable z, de tal formaque se consiga un aterrizaje suave. Inicialmente se toman los valores tE, zE co-rrespondientes al tiempo y la altura en ese instante. Con estos valores se calculala referencia en altura mediante la expresión zref = zE + 0,8·(t− tE), correspon-diente a una rampa cuya pendiente toma el valor 0.8 positivo. Este signo se debeal sistema de referencia de ejes locales.

3. E_3: Cuando la referencia supera el valor zref > 0 se inicia este estado, y sedetermina zref = 0.

4. E_4: Cuando z toma el valor 0, se inicia este estado que sirve de transición alestado “Tierra”.

4.4.3. Estados de control

El estado Modo de Control gobierna el nivel control que se utiliza, y dentro de cadanivel, el tipo de control implementado. Este estado recibe información de la señal alto-nivel y de la estructura tipo. Se comunica con el bloque control mediante la estructuraenable. El esquema de los estados que lo forman puede verse en la figura 4.18

4.4.3.1. Niveles de control

Dentro del estado Modo de Control, se encuentran dos estados principales, distinguidoscomo niveles de control:


Figura 4.17: Diagrama Stateflow del estado principal “Emergencia”

[BAJO NIVEL] Control rotacional o de estabilización.

[ALTO NIVEL] Control traslacional o de posición.

Por la jerarquía del control de este tipo de plataformas, el control de estabilizaciónsiempre permanece activo, ya que al proporcionar referencias en posición, el control deposición calcula las referencias de posición angular que deben ser interpretadas por elcontrol de estabilización.

La selección se realiza mediante la variable altonivel, establecida en el modo de vuelo.Indica si la plataforma es gobernada únicamente por el control de estabilización o seactiva además el control de posición. Los valores posibles y su significado son:

[0] BAJO NIVEL. Modo de vuelo gobernado únicamente por control de esta-bilización.

[1] ALTO NIVEL. Se activa el control de posición, que envía referencias enposición angular al control de estabilización.


Figura 4.18: Esquema de los estados y jerarquía de el estado Modo de control

Los modos de vuelo gobernados únicamente por el control de estabilización son:

Despegue

Estabilización

Estabilización en altura

Los modos de vuelo donde se activa el control de posición incluyen ciertos modos comoel Aterrizaje. Ésto se debe a la necesidad de parar la plataforma antes de continuarel aterrizaje para mantener la estabilidad. El despegue no se incluye en el control deposición por tener únicamente controlada la variable de altura, que como se ha visto,pertenece al control de estabilización. Los modos de vuelo que disponen del control deposición son:

Movimiento en el plano

Movimiento en el espacio 3D

Aterrizaje

To Home

Emergencia

Stop


4.4.3.2. Tipos de control

Dentro de cada uno de los estados BAJONIVEL y ALTONIVEL, se incluyen los respec-tivos tipos de control que se hayan implementado. Cabe destacar que debido al carácterindependiente de los bloques de control utilizados, se permite añadir tantos tipos decontrol como se quiera, tanto para Alto Nivel, como para Bajo Nivel, siempre tenién-dolos en cuenta en la lógica Stateflow, tal y como se ha comentado en la subsección4.3.

En este trabajo sólo se ha implementado el bloque de control “Lineal”, tanto para con-trol de estabilización como para control de posición, con el objetivo de comprobar lalógica implementada, y además, se ha añadido otro bloque denominado “No Lineal” amodo de ejemplo de utilizar varios tipos de control, aunque su contenido es el mismoque el del bloque “Lineal”.

tipo: Es una estructura de MATLAB donde quedan almacenados las peticionesdel usuario sobre el tipo de control que debe utilizarse.

• tipo.HI: Recoge la petición del usuario sobre el control de posición, in-cluido en el bloque de Alto Nivel.

[1] Lineal [2] No Lineal [...] Otros posibles modos de control a implementar

• tipo.LO: Recoge la petición del usuario sobre el control de estabilización,incluido en el bloque de Bajo Nivel.

[1] Lineal [2] No Lineal [...] Otros posibles modos de control a implementar

Para implementar la lógica del modo de control en Stateflow, se planteó la posibilidadde hacerlo de dos formas: Mediante un estado, con transiciones, o mediante una tablade verdad. Se eligió la primera por la visualización constante durante la simulación delmodo de control elegido.

La variable de salida que comunica con el bloque de control es el bus enable (e), cuyascomponentes son:


Figura 4.19: Modos de Control

e.hi_lin: Modo lineal para el control de posición.

e.hi_nlin: Modo no lineal para el control de posición.

e.HI: Señal que activa el control de posición.

e.lo_lin: Modo lineal para el control de estabilización.

e.lo_nlin: Modo no lineal para el control de estabilización.

Estas componentes actúan como señales de habilitación (del inglés, enable). Puedentomar los valores lógicos [0] Desactivado o [1] Activado.

4.4.3.3. Estados

La solución implementada puede verse en la figura 4.4.3.3. A continuación se describenlos estados que componen el estado Modo de Control:

BAJONIVEL Control de estabilización o de Bajo Nivel. Si este estado estáactivo, se proporcionan únicamente referencias en posición angular.

• BAJO_LINEAL Control de estabilización lineal

• BAJO_NOLINEAL Control de estabilización no lineal

ALTONIVEL Control de posición o de Alto Nivel. Cuando este estado está acti-vo, se proporcionan referencias en posición, que el control de alto nivel transformaen referencias en posición angular. Por tanto, como se ha indicado al principio


de este apartado, el control de bajo nivel o control de estabilización permaneceactivo siempre. Es por ello que debe ser contemplado en los estados del controlde alto nivel.

• ALTO_LINEAL Control de posición lineal

BAJOLINEAL1 Control de estabilización lineal cuando está activo elcontrol de posición BAJONOLINEAL1 Control de estabilización no lineal cuando está

activo el control de posición

• ALTO_NOLINEAL Control de posición no lineal

BAJOLINEAL2 Control de estabilización lineal cuando está activo elcontrol de posición BAJONOLINEAL2 Control de estabilización no lineal cuando está

activo el control de posición

Figura 4.20: Modo de control implementado en Stateflow

4.4.4. Transiciones

Las transiciones son, como se ha indicado anteriormente, el procedimiento para pasar deun estado a otro de una forma ordenada, y sujeto a condiciones, restricciones o eventos.En este trabajo se han utilizado transiciones directas o restringidas por una condiciónlógica, tal y como se describen a continuación:

4.4.4.1. Transición Tierra - Aire

Esta transición se realiza de forma directa una vez se ha realizado el armado de moto-res. Para llegar hasta este último punto es necesario obtener un valor ’1’ en la función


ok_tierra=check(modo). La función check es un script de MATLAB integrada en elbloque Stateflow donde deben incluirse una serie de condiciones imprescindibles para elarranque de un quadrotor, tales como la comprobación de sensores, voltaje de baterías,comunicación con la estación en tierra, enlace RC con la emisora, etc. Estas compro-baciones no se incluyen en este trabajo por no haberse implantado aún en un modeloconcreto, por lo que se deja como trabajo futuro.

La transición interna entre “Check” y “Armado Motores” queda descrita en el estado“Tierra”4.4.2.1.

4.4.4.2. Transiciones en el Modo Aire

Estas transiciones están gobernadas por la señal externa modo con la que el usuariosolicita el modo de vuelo. Las transiciones son:

Despegue - Vuelo: Se realiza de forma directa una vez que el quadrotor alcanza laaltura de despegue definida.

Vuelo - Aterrizaje: Cuando se solicita de forma externa el modo Aterrizaje seinicia este estado descrito en 4.4.2.2.3.

Vuelo - To Home: El modo To Home se inicia de la misma forma que el estadoAterrizaje. Las transiciones internas de este modo están descritas en 4.4.2.2.4.

4.4.4.3. Transiciones en el modo Vuelo

El modo de vuelo es una variable externa introducida por el usuario y puede tomar va-lores consecutivos del 1 al 7, aunque los tres últimos se corresponden respectivamentecon Aterrizaje, To Home y Emergencia. En este apartado se tienen en cuenta los cuatroprimeros: Estabilización (1), Estabilización en Altura (2), Movimiento en el plano (3)y Movimiento en el espacio (4).

Debido a que cada uno de estos modos puede estar gobernado por control de posición ocontrol de estabilización, podrían encontrarse problemas de estabilidad en la transiciónentre ellos. Por tanto la conmutación debe hacerse de forma cauta y con garantías, espor ello que se ha añadido el modo adicional Stop con el fin de establecer una transición


segura en tiempo finito. Una vez verificada la estabilidad, se procede a comprobar encada caso los siguientes parámetros mediante una función denominada ok_stop, quedepende de la posición angular y de las referencias en posición, ya que su objetivo escomprobar que el quadrotor está parado.

4.4.4.4. Transición Aire - Emergencia

El modo aire ejecuta de forma continua la función emergencia, la cual debe compro-bar el estado de todos los dispositivos críticos de la plataforma y enviar la señal deemergencia si alguno de ellos presenta algún fallo (en este trabajo, emergencia se esta-blece designando el valor ’7’ a la variable modo). También puede ser solicitado de formaexterna por el usuario, en caso de que este considerara que el vuelo no es seguro.

Las transiciones internas del estado Emergencia están descritas en 4.4.2.3.

4.4.4.5. Transiciones a Tierra

Los modos Aterrizaje y Emergencia se dan por finalizados una vez la plataforma toqueel suelo. Esta condición se comprueba de forma interna en cada uno de ellos y, unavez aceptada, se conmuta al estado Tierra para las posibles comprobaciones por si laplataforma tuviera que despegar de nuevo.

4.4.4.6. Transición desde ToHome

Desde el modo “To Home” se puede conmutar al estado “Aterrizaje” o a cualquiera delos estados contenidos en “Vuelo” solicitándolo de forma externa.

4.4.4.7. Transiciones en el Modo de Control

Por defecto, el estado Modo de Control entra en BAJONIVEL, pero es la variable alto-nivel, definida en cada modo de vuelo, la que determina si nos encontramos únicamentecon control de estabilización o se activa el control de posición.

Dentro de los estados ALTONIVEL o BAJONIVEL, es la estructura tipo la que deter-mina si debe ser activado el control lineal, el control no lineal, o cualquier otro que se


haya implementado. Esta estructura tiene dos variables, HI, que toma los valores [0,1]para determinar el tipo de control de posición (1 para lineal, 0 para no lineal), y LO,definida de igual forma pero para el control de estabilización. Si se implementaran otrostipos de control, habría que incorporarlos a la estructura lin.

4.5. Generador de trayectorias

Se ha considerado en este trabajo incluir un generador de trayectorias para probar losdistintos modos de vuelo y para verificar la respuesta de la máquina de estado antereferencias externas que varían con el tiempo. Actualmente el generador de trayectoriases una parte fundamental de una plataforma tipo MAV, ya que le proporciona mayorautonomía y versatilidad.

Un detalle a tener en cuenta a la hora de generar trayectorias es que las plataformasde ala rotatoria tipo VTOL (Despegue Vertical) resultan mucho más precisas a la horade seguir trayectorias determinadas por puntos de paso o waypoints que las de ala fija,aunque generalmente no tan rápidas.

El generador de trayectorias diseñado para este trabajo es un bloque denominado «Ge-nerador de trayectorias» incluido en el diagrama principal de Simulink, el cual tienecomo entradas:

Modo de vuelo solicitado

Matriz P constante de waypoints

Vector de estado x

El bloque tiene como salida la referencia en posición [xref , yref , zref ] en cada instanteen forma de vector dado en ejes horizonte local, que servirá de entrada a la máquinade estados para proporcionar referencias al controlador de posición, al controlador deestabilización o ambos a la vez, según el modo de vuelo solicitado.

El contenido de este bloque es principalmente una función de MATLAB denominadaSecuenciador de waypoints que toma la matriz Pnx3, con n waypoints de 3 coordenadas,


y proporciona la primera fila como posición de referencia. El bloque Stateflow lo tomaráy según el modo de vuelo, tendrá en cuenta ninguna, una, dos o las tres coordenadas(para los casos de Estabilización, Estabilización en altura, Movimiento en el plano yMovimiento en el espacio, respectivamente..

Esta función calcula constantemente el error de distancia ε entre el waypoint deseadoy el valor de posición del vector estado según el modo de vuelo, con las siguientesecuaciones:

Estabilización en altura: ε = |z − zref |

Movimiento en el plano: ε =√

(x− xref )2 + (y − yref )2

Movimiento en el espacio: ε =√

(x− xref )2 + (y − yref )2 + (z − zref )2

ε = 1 si el modo no es ninguno de los anteriores. En ese caso el “Generador detrayectorias” no es utilizado.

Cuando ε es menor que un cierto valor (según la precisión que se requiera, a menor valorlas trayectorias serán más precisas y lentas, a mayor valor tendrán mayor continuidady serán más rápidas) se pasa al siguiente waypoint, que se corresponde con la siguientefila de la matriz P. Así, el «Secuenciador de waypoints» pasará por todos los puntoshasta llegar al último, momento en el que volverá al primero.

La adición de una entrada y una salida para ψref tiene sentido por si se quiere imple-mentar algún tipo de función o bloque que dé valores al ángulo yaw. En este trabajo,se toma como entrada externa.

En la figura 4.21 se puede ver el contenido del bloque “Generador de trayectorias”.

Como se ha comentado previamente, la entrada de waypoints se realiza mediante puntosligados a ejes horizonte local. Sin embargo, las plataformas aéreas autónomas disponengeneralmente de receptores de posicionamiento global, como GPS. Estos dispositivostrabajan con coordenadas geográficas (latitud y longitud), por tanto se precisa de unafunción que adapte este tipo de coordenadas a los ejes con los que se trabaja en elmodelo que aquí se presenta.


Figura 4.21: Vista del contenido del bloque «Generador de trayectorias»

Para ello, MATLAB cuenta con la función

P = lla2flat(LLA,LL0, PSI0, HREF,MODEL)

que transforma una matriz LLA de puntos en ejes geográficos en una matriz P en ejeshorizonte local, donde:

LLA es una matriz de dimensión n x 3 con n waypoints y 3 columnas correspon-dientes a la latitud, longitud y altura.

LL0 es la posición inicial en coordenadas geográficas.

PSI0 es el ángulo que forma el eje x de las coordenadas locales con el nortegeográfico, que en este caso se toma como 0, aunque más tarde se deben permutarlas coordenadas x e y, para adaptarlo a los ejes utilizados en este trabajo.

HREF es la altura de referencia y

MODEL es el elipsoide de referencia, en este caso ’WGS84’. Para más informaciónsobre el modelo ’WGS84’ consultar el apéndice A.


También puede hacerse directamente a partir de un software de información geográficaque permita seleccionar puntos y exportarlos a un archivo. En este trabajo se ha utili-zado el software Google Earth® [5]. Éste permite seleccionar una ruta mediante puntosy guardarlos en un archivo de extensión ’*.kml’. Con este archivo, se puede exportar auna matriz de MATLAB mediante el script de MATLAB read_kml [7] B.5.

Estas dos funciones se incorporan en este trabajo en el script ’wp.m’, el cual se ejecutaen MATLAB previamente a la simulación mediante el comando P = wp(archivo), don-de ’archivo’ es el nombre del fichero de extensión ’*.kml’. El contenido de este ficherose aporta en el apéndice B.4

Figura 4.22: Ruta circular en Google Earth® situada en el edificio de laboratorios de laEscuela Superior de Ingenieros de Sevilla

Capítulo 5

Experimentos y validación

5.1. Introducción

En este capítulo se mostrarán resultados de simulaciones realizadas en distintos casossupuestos, para verificar la validez de todos los modos de vuelo y la correcta configura-ción de los modos de control.

5.2. Instrucciones para la simulación

Para ejecutar estas simulaciones, se ha diseñado la función de MATLAB modos.m quedetermina los valores de entrada para poder realizar la simulación. El contenido delscript está disponible en el apéndice B. Su única entrada es el nombre del archivo .kmlgenerado en Google Earth con la ruta deseada. Sus salidas son:

P: Matriz n x 3 con los waypoints generados.

mode: Estructura de MATLAB que contiene modo de vuelo seleccionado y elinstante al que va asociado. Se crea mediante la herramienta gráfica GUI deMATLAB de forma que la entrada de datos sea mas intuitiva. Al ejecutarse,pregunta al usuario qué modo de vuelo desea, y una vez seleccionado, pregunta siquiere elegir otro. En este caso se otorgan 10 segundos de simulación a cada modoseleccionado, como ejemplo de simulación. Se pueden seleccionar tantos como sequieran.

57

CAPÍTULO 5. EXPERIMENTOS Y VALIDACIÓN 58

Figura 5.1: Captura del menú “Modo de vuelo” basado en la herramienta GUI

tipoControl: Estructura de MATLAB que contiene las variables HI y LO, dondese indica el tipo de control que gobierna en el control de alto nivel y en el de bajonivel, respectivamente. También está creado con GUI.

Figura 5.2: Pantallas de selección de los tipos de control

airetierra: Esta variable simula una entrada externa para decidir si se quiere llevarel quadrotor al aire. Se pregunta mediante GUI, aunque para visualizar todo lodesarrollado en este trabajo debe estar en la posición “Aire”.


T: tiempo de simulación. Se calcula según el número de modos de vuelo seleccio-nados, otorgando 10 segundos a cada uno.

El procedimiento a seguir para realizar una simulación consiste en:

1. Crear en MATLAB una matriz P nx3 de waypoints donde las filas correspon-dan a las coordenadas x y z y las columnas a cada waypoint. Se puede hacer deforma manual u obtenerla mediante la función waypoints.m, mediante el coman-do P=wp(’nombrearchivokml’) o P=wp_interp(’nombrearchivokml’) si sedesea además interpolar los puntos. Para obtenerlo de esta segunda forma, esnecesario haber generado el archivo *.kml en Google Earth, generando una se-cuencia de puntos con el botón “Generar Ruta” de la barra de herramientas. Unavez creado, guardarlo.

2. Ejecutar el archivo modos.m y seleccionar los modos de vuelo, el tipo de controlpara control de estabilización y para control de posición, la opción Aire para podervolar.

3. Abrir el archivo modelo.slx con Simulink y ejecutar la simulación

4. Para representar las gráficas, ejecutar en MATLAB el archivo representar.m

5.3. Simulaciones

Las gráficas de las diferentes variables mostradas frente al tiempo de simulación incluyenuna gráfica superior donde están representadas las variables modo y modoR. Estasvariables representan, respectivamente, el modo de vuelo solicitado por el usuario y elmodo real que se encuentra activo en cada momento. Para entender estas variables, semuestra a continuación del valor que lleva asociado cada modo de vuelo, idéntico paralas dos.

-1 Tierra

0 Despegue

1 Estabilización


2 Estabilización en altura

3 Movimiento en el plano

4 Movimiento en el espacio

5 Aterrizaje

6 ToHome

7 Emergencia

Las gráficas mostradas a continuación se obtienen del fichero representar.m, detalladoen el apéndice B. Con él, se obtienen las gráficas necesarias para el análisis de los datosproporcionados por la plataforma.

Para interpretar las gráficas, se aclara que las unidades de medida son segundos, para eltiempo, metros, para las coordenadas de posición lineal, y radianes, para las coordenadasangulares.

5.3.1. Despegue, estabilización y estabilización en altura

En esta simulación se ejecutan únicamente los modos de “Estabilización” y “Estabili-zación en altura”, por lo que la plataforma únicamente toma referencias en altura parael segundo caso. Los valores de las referencias son:

Waypoint 1 2 3 4 5 6 7 8 9 10zref -5 -3 -7 -3 -5 -3 -8 -3 -7 -3

Cuadro 5.1: Simulación 1: referencias para la variable z.


Figura 5.3: Gráficas de modo y altura con respecto al tiempo en la simulación 1.

En la figura 5.3 se observa la evolución de z a lo largo del tiempo de simulación. En losprimeros instantes se introduce la referencia en rampa para el despegue.

5.3.2. Despegue, estabilización y movimiento en el plano

En este caso se introducen una serie de waypoints en las coordenas X e Y para recorreruna trayectoria en forma de estrella. Al tratarse de movimiento en el plano, la referenciaen altura es constante. La trayectoria no llega a tocar a los puntos de paso por el radiode tolerancia permitido, establecido en 0.1m en este caso.


Figura 5.4: Gráfica del plano XY para la simulación 2

En las siguientes gráficas se observan los valores de posición lineal y posición angulara lo largo del tiempo. En ambas se compara con el valor de la variable relacionadacon el modo de vuelo, que nos indica que parte de tierra, inicia el despegue y una vezfinalizado comienza el modo de movimiento en el plano. La línea roja de esta gráficaes el estado real que está ejecutando la plataforma y la azul es el modo solicitado deforma externa.

Figura 5.5: Valores de posición lineal a lo largo del tiempo para la simulación 2.


El valor de z real no permanece constante debido a las variaciones de altura que sufreel quadrotor al realizar el desplazamiento, pero se compensa con la referencia fija paraesta variable.

Figura 5.6: Posición angular respecto al tiempo para la simulación 2

En la gráfica 5.6 se pueden observar los giros que realiza el quadrotor para su des-plazamiento en los ángulos pitch y roll, y la respuesta ante una referencia en escalónpara el ángulo yaw, permitida en este modo de vuelo. El controlador PD implementadoprovoca una respuesta subamortiguada.

Las referencias proporcionadas aparecen en la tabla 5.2.

Waypoint 1 2 3 4 5 6 7 8 9 10xref 4 1 0 -1 -4 -2 -3 0 3 2yref 0 1 4 1 0 -1 -5 -2 -5 -1

Cuadro 5.2: Simulación 2: referencias para el conjunto de variables [x, y].

5.3.3. Despegue, estabilización y movimiento en el espacio

En esta simulación el quadrotor sigue un camino marcado por waypoints en las tresdimensiones espaciales.


Figura 5.7: Altura respecto al tiempo en la simulación 3

La tabla con las referencias es la unión de los valores de los apartados anteriores:

Waypoint 1 2 3 4 5 6 7 8 9 10xref 4 1 0 -1 -4 -2 -3 0 3 2yref 0 1 4 1 0 -1 -5 -2 -5 -1zref -5 -3 -7 -3 -5 -3 -8 -3 -7 -3

Cuadro 5.3: Simulación 3: referencias para el conjunto de variables [x, y, z ].

Figura 5.8: Trayectoria 3D de la simulación 3.

En la gráfica 5.8 se observa la trayectoria en 3D del quadrotor . En primer lugar, laplatarforma realiza el despegue vertical, seguido de la estabilización. A continuacióninicia el movimiento siguiendo los puntos de paso numerados en la tabla 5.3.


5.3.4. Despegue, movimiento en el plano, aterrizaje, despeguey estabilización

En este caso se realiza una simulación para comprobar el funcionamiento del modo deaterrizaje y posterior despegue de la plataforma. Para ello, usando los mismos puntosde paso de las simulaciones anteriores, recogidos en la tabla 5.3, se solicita como primermodo el “movimiento en el plano”, para después aterrizar la plataforma. El siguientemodo es “estabilización”, lo que implica el despegue.

Figura 5.9: Posición respecto al tiempo en la simulación 4

En la figura 5.9 se observa la variación de la posición lineal y el modo de vuelo conrespecto al tiempo. Se puede observar que sobre el instante correspondiente a t=26sel modo real desciende al valor -1, lo que implica el aterrizaje. No es hasta el instantet=30s, cuando se solicita el modo “estabilización” cuando la plataforma comienza adespegar.


Figura 5.10: Trayectoria vista en 3D de la simulación 4.

En la figura 5.10 se observa la trayectoria 3D. El movimiento se inicia con el despeguedesde el origen, punto verde, hasta la altura de despegue, 5 m. Ahí comienza el vueloplano hasta el instante del aterrizaje, que coincide cerca del waypoint 3. A partir deaquí el movimiento es únicamente vertical, ya que después del aterrizaje la plataformavuelve a despegar y se mantiene estabilizada.

5.3.5. Despegue, estabilización, movimiento en el espacio, ToHo-me y aterrizaje

El objetivo de esta simulación es comprobar el funcionamiento del modo To Home, quetal como se vió en el capítulo 4, se corresponde con un modo de vuelo que permitevolver al orígen pero se decidió que fuese sin aterrizar, por redundancia con el modo“Aterrizaje” existente. Así, tras despegar, la plataforma iniciará el movimiento en lastres dimensiones espaciales y cuando se solicite el modo “To Home” volverá al puntoinicial, con la altura determinada para el despegue.


Figura 5.11: Posición respecto al tiempo en la simulación 5.

Las gráficas presentadas se corresponden con la posición lineal y el modo de vuelo,además de otra gráfica donde se observa la trayectoria en tres dimensiones.

Figura 5.12: Trayectoria 3D de la simulación 5

En esta gráfica tridimensional se observa como el quadrotor despega y comienza su vuelosiguiendo los puntos de paso, hasta llegar al cuarto, instante en el que se ha solicitadoel modo “To Home”. La plataforma acude al origen de coordenadas y se mantiene hasta


que se inicia el aterrizaje.

5.3.6. Despegue, estabilización, movimiento en el plano y emer-gencia

Esta simulación permite comprobar el funcionamiento del modo “Emergencia”.

Figura 5.13: Posición respecto al tiempo para la simulación 6

En la gráfica 5.13 se observa la variación de la posición respecto al tiempo, y los modos devuelo. Se observa que en el instante t=30s se solicita el inicio del estado “Emergencia”,y en torno al instante t=41s la plataforma toma tierra.


Figura 5.14: Trayectoria 3D para la simulación 6

La gráfica tridimensional permite observar como en el instante en que se solicita elmodo “Emergencia” la plataforma comienza a detenerse y retrocede para cumplir conla referencia en posición que se da al principio de este modo. Este retroceso se debe aque la frenada no es instantánea, requiere de en torno a unos 2 o 3 segundos, segúnse aprecia en la gráfica de la coordenada x, a partir del instante t=30. En cuanto elquadrotor se encuentra parado, comienza el aterrizaje y llega a tierra.

5.3.7. Despegue, estabilización, movimiento en el espacio yemergencia

En esta ocasión, se pretende comprobar el comportamiento del modo emergencia. Seha cambiado la trayectoria de referencia por una helicoidal, determinada por el archivotrayectoriaHelicoidal.m, incluido en el apéndice B.


Figura 5.15: Posición respecto al tiempo en la simulación 7

En la figura 5.15 se observa la variación de la posición respecto al tiempo, y cómo eneste caso las referencias no son escalones sino valores contínuos, lineales para z y nolineales para x e y, durante el modo de “movimiento en el espacio”.

Figura 5.16: Trayectoria 3D en forma de hélice para la simulación 7

En la gráfica tridimensional se observa el despegue desde el punto de origen (en ver-de) hasta la altura de despegue, z=5m, donde inicia la estabilización. En el instante


t=10s se solicita el modo “Movimiento en el espacio”, a partir del cual la plataformainicia el recorrido helicoidal. En el instante t=50s se solicia el modo “Emergencia” y laplataforma desciende a tierra.

5.3.8. Despegue, estabilización, movimiento en el espacio, mo-vimiento en el plano, movimiento en el espacio y “ToHo-me”.

Esta simulación aumenta la duración con respecto a las anteriores, con el objetivo depoder simular los movimientos en el plano y el espacio de forma alterna para probar suconmutación. Finalmente se solicita el modo “To Home” para el regreso de la plataformaal origen. En este caso también se ha hecho uso de la trayectoria helicoidal.

Figura 5.17: Posición respecto al tiempo en la simulación 8.

En este caso, el comportamiento de x e y es igual para los modos plano y espacial. Elcambio se produce en la variable z, donde se observa que en el modo plano permanececonstante.


Figura 5.18: Trayectoria 3D obtenida en la simulación 8.

En este caso, por clarificar, se ha hecho una representación tridimensional por colores,para distinguir el modo de vuelo en cada caso. El azul corresponde a los instantes deldespegue, el verde al movimiento espacial, y el cyan al movimiento plano. El rojo siguesiendo la referencia helicoidal creada.

5.3.9. Todos los modos

Para una simulación completa, se creará una trayectoria mediante puntos seleccionadosen mapa, para lo cual se va utilizar la herramienta programada para tomar puntos enGoogle Earth. Se recuerda que para ello es imprescindible la función wp mediante elcódigo P=wp(’archivo’), donde ’archivo’ es el nombre del archivo de extensión *.kmlgenerado por Google Earth. Así, se obtendrá una matriz de puntos de paso denominadaP. Si se desea obtener una trayectoria mucho más definida, con mayor cantidad depuntos, se realizará con el código P=wp_interp(’archivo’). En este caso se utilizaráeste último código.

Un detalle a destacar es que generar puntos de paso con esta técnica proporciona lascoordenadas x e y de estos puntos. Sin embargo, es necesario crear las referencias enaltura z. En este caso, este archivo tiene una parte donde se genera esta referencia, arazón de añadir 1 metro de altura por cada waypoint que pase, partiendo de 3 metrospara el primero.


Figura 5.19: Valores en posición y modo de vuelo respecto al tiempo en la simulación 9

A continuación se describe el vuelo seguido numerando sus modos y el tiempo quepermanecen vigentes:

0-10s Estabilización

10-60s Movimiento en el espacio

60-70s To Home

70-80s Aterrizaje

80-90s Estabilización en altura

90-120s Movimiento en el plano

120-140s Emergencia

El quadrotor sigue las trayectorias tal y como se ha comprobado en las simulacionesanteriores.

La ruta seleccionada en Google Earth se ha obtenido ejecutando el comando googleEarthel cual llama al archivo del mismo nombre y toma los valores x,y y z y los convierte acoordenadas geográficas mediante la función flat2lla, que convierte coordenadas en el


plano tangente local a geográficas, aportando un punto de origen, en este caso O. Latrayectoria final puede verse en la imagen 5.20.

Figura 5.20: Representación en Google Maps de la trayectoria generada por el quadrotoren la simulación 9.

5.3.10. Simulación en Flightgear

Otra opción que permite visualizar el vector de estados es conectar el modelo con unsimulador de vuelo. FlightGear es un simulador de código abierto, que permite visua-lizar el vector de estado de la plataforma de una forma gráfica gracias a sus múltiplesopciones de conexión. En este caso, se dispone del bloque “FlightGear Preconfigured6DoF Animation” del Toolbox Aerospace Simulink, cuyas entradas son longitud, lati-tud, altitud, y posición angular (pitch, roll y yaw).

Capítulo 6

Conclusiones y trabajo futuro

6.1. Conclusiones

Este trabajo ha tenido como objeto el diseño de un sistema para la verificación yvalidación de sistemas de control de vuelo tipo MAV-VTOL, y más concretamente parahelicópteros de tipo quadrotor. Se ha hecho un estudio de la dinámica y las ecuacionesque modelan su comportamiento físico, aunque la implementación de este apartado nose ha desarrollado por no ser objeto principal, al igual que el diseño de sistemas decontrol, los cuales se han reutilizado de trabajos ya desarrollados.

Sin embargo, sí se ha hecho especial hincapié en el diseño desde cero de una máquinade estados que gobierne los sistemas de control, permita realizar simulaciones y obtenerresultados analizables con objeto de validar dichos sistemas. Para ello, se ha estimadooportuno llevar a cabo el desarrollo de esta máquina de estados en el software MATLABStateflow, muy indicado para este tipo de diseños, por su sencillez de trabajo gracias alentorno gráfico y su potencia, lo que le aporta infinidad de posibilidades de desarrollo.

En cuanto a los resultados obtenidos con este trabajo permiten confirmar la utilidad deesta herramienta como sistema de validación. Las pruebas realizadas ofrecen resultadosde todas las variables que componen este sistema, y gracias a MATLAB, la gestiónde estos datos y su tratamiento es relativamente simple. Por eso, se considera que estediseño puede ser válido tanto para la validación de soluciones desarrolladas en el ámbitode la investigación, como para ofrecer soluciones académicas para prácticas y trabajosrelacionados con la tecnología que aquí se trata.

77

CAPÍTULO 6. CONCLUSIONES Y TRABAJO FUTURO 78

Dentro de los resultados, se quiere mencionar también que gracias al desarrollo de estetrabajo se ha incluído parte de su contenido en un artículo publicado en las XXXVJornadas de Automática celebradas en Valencia entre los días 3 y 5 de Septiembre de2014 [18].

6.2. Trabajo futuro

6.2.1. Modelo dinámico

Como trabajo futuro podría implementarse un modelo dinámico algo más fiel a larealidad. Para ello se pueden añadir otros bloques que implementen perturbacionesexternas como el viento o efectos propios de este tipo de plataformas, como el efectosuelo.

6.2.2. Bloque de control

El diseño de esta solución tiene como característica principal en el bloque control laposibilidad de añadir tantos tipos de control como se quiera para poder conmutar entreellos, incluso durante la simulación. Sin embargo, por no ser objeto fundamental deeste trabajo, no se ha implementado más que un control lineal a efectos de realizarlas pruebas pertinentes. Cabe la posibilidad de expandir su utilidad agregando nuevostipos de control, asegurándose de la compatibilidad con las señales de entrada y salida,y teniéndolo en cuenta en la lógica de Stateflow.

6.2.3. Máquina de estados

El desarrollo de la máquina de estados incorpora ciertos modos de vuelo que resul-tan imprescindibles para este tipo de plataformas, sin embargo, queda como trabajo adesarrollar la implementación de nuevos estados que amplíen la funcionalidad de estediseño.


6.2.4. Implementación hardware

El objetivo principal del diseño planteado en este trabajo consiste en la validación deesta máquina de estados antes de ser implantada en hardware real. El hecho de quese haya probado en simulación previa a su implantación permite ahorrar en costes,ya sea en tiempo de desarrollo como en pruebas y validación. Del mismo modo, unaventaja significativa es el hecho de que no es necesaria la adquisición previa de ningunaplataforma real, pudiendo así evitar una compra anticipada del hardware que tras eltiempo de desarrollo, en muchas ocasiones, queda relativamente obsoleto o anticuado.

6.2.4.1. Generación de código

La primera alternativa de poder validar la máquina de estados diseñada medianteMATLAB, es hacer uso de las herramientas de compilación cruzada para platafor-mas distintas al PC. En este caso, es posible generar código para hardware embebido,y posteriormente ser programada la placa de control. Haciendo uso de la herramientaMatlab Embedded Coder y el toolbox Real-Time Workshop, es posible generar códigoC/C++ optimizado para hardware con recursos limitados. En este caso, una de laslíneas abiertas del presente trabajo consiste en la programación de placas de controlArdupilot, las cuales integran el conjunto de sensores necesarios para la estimación delestado, y poder así cerrar el ciclo completo, desde diseño hasta validación experimental.La generación del código diseñado y su implantación y validación en hardware real eslo que se conoce como software-in-the-loop.

Figura 6.1: Imagen de placa Ardupilot, basada en la plataforma Arduino

Del mismo modo, es posible generar código optimizado para hardware de control másavanzado y potente, como son los procesadores ARM, de bajo consumo y gran potencia.


Dichos procesadores proporcionan gran capacidad de cálculo gracias a las unidades depunto flotante disponibles.

6.2.4.2. Hardware-in-the-loop

Para la verificación y validación previa de la plataforma, sería recomendable incluir lasseñales correspondientes a la medida de los sensores, así como bloques que verifiquenla validez de las medidas proporcionadas por sensores como la IMU o el GPS.

Si bien es posible simular el software de control, y la máquina de estados para sufutura implantación en hardware real, también es posible incluir en las simulaciones unprototipo de la plataforma real, mediante software compatible. Existen unas libreríasdesarrolladas para la placa de control Arduino [13], las cuales proporcionan el conjuntode ficheros y librerías necesarias para ser ejecutadas en simulación, sin necesidad algunade hardware objetivo. Esto permite, adicionalmente, la ejecución en tiempo real delsoftware que gestiona el conjunto de sensores junto con la máquina de estados diseñada,incluyendo parámetros tan importantes como el bias de los sensores o caracterizacióndel ruído de los mismos, muy presentes en sensores MEMS1 disponibles en el mercado.

Figura 6.2: Imagen de una captura de pantalla de Simulink con la librería Ardupilot 2Target

Utilizando este tipo de software, es posible además el cierre del lazo de verificacióny validación incluyendo, además el hardware de control. En este caso, el conjunto de

1Microelectromechanical systems, Sistema Microelectromecánico.


pruebas a realizar estarían enmarcadas dentro del conjunto de pruebas denominadashardware-in-the-loop.

Bibliografía

[1] Real Decreto-ley 8/2014, de 4 de julio, de aprobación de medidas urgentes para elcrecimiento, la competitividad y la eficiencia.«BOE» núm. 163, de 5 de julio de2014. Páginas 52553 a 52555

[2] Raffo, G.V., (2011) Robust Control Strategies for a quadrotor helicopter. An unde-ractuated mechanical system. Tesis Doctoral. Escuela de Ingenieros. Universidadde Sevilla.

[3] Corke, P., (2011) Robotics, Vision and Control. Fundamental Algorithms inMATLAB®, Springer.http://petercorke.com/Robotics_Toolbox.html

[4] Mathworks Matlab Stateflow ®.http://www.mathworks.es/products/stateflow/index.html

[5] Google Earth ®.https://earth.google.es

[6] Sistemas de referencia. Instituto Geográfico Nacional.http://www.ign.es/ign/layoutIn/actividadesGeodesiaStmagd.do

[7] Farris, A., (2006) read_kml MATLAB Central File Exchange.http://www.mathworks.com/matlabcentral/fileexchange/13026-read-kml

[8] Bouabdallah, S., Murrieri, P., y Siegwart, R., (2004) Design and Control of anIndoor Micro Quadrotor. En Proc. IEEE Int. Conf. on Rob. and Automat., volume5, pp 4393–4398, New Orleans, USA.

[9] Castillo, P., Lozano R., y Dzul, A.E., (2005) Modelling and Control of Mini-FlyingMachines, Springer.

83

http://petercorke.com/Robotics_Toolbox.html

http://www.mathworks.es/products/stateflow/index.html

https://earth.google.es

http://www.ign.es/ign/layoutIn/actividadesGeodesiaStmagd.do

http://www.mathworks.com/matlabcentral/fileexchange/13026-read-kml

BIBLIOGRAFÍA 84

[10] D’Andrea, R., The astounding athletic power of quadcopters.https://www.youtube.com/watch?v=w2itwFJCgFQ

[11] AR.Drone Open API Platformhttps://projects.ardrone.org/

[12] DIY Droneshttp://diydrones.com/

[13] Plataforma Arduinohttp://arduino.cc/

[14] Plataforma Arducopterhttp://copter.ardupilot.com/

[15] TrackDrone Litehttp://cpoh.upv.es/es/investigacion/software/item/236-trackdrone-lite.html

[16] AR.Drone Simulink Development-Kithttp://www.mathworks.es/matlabcentral/fileexchange/43719-ar-drone-simulink-development-kit-v1

[17] Raffo, G.V., (2007) Modelado y control de un helicóptero quadrotor. Tesis deMáster.

[18] Actas de las XXXV Jornadas de Automática, 3-5 de septiembre de 2014, ValenciaISBN-13: 978-84-697-0589-6 © 2014 Comité Español de Automática de la IFAC(CEA-IFAC)

https://www.youtube.com/watch?v=w2itwFJCgFQ

https://projects.ardrone.org/

http://diydrones.com/

http://arduino.cc/

http://copter.ardupilot.com/

http://cpoh.upv.es/es/investigacion/software/item/236-trackdrone-lite.html

http://cpoh.upv.es/es/investigacion/software/item/236-trackdrone-lite.html

http://www.mathworks.es/matlabcentral/fileexchange/43719-ar-drone-simulink-development-kit-v1

http://www.mathworks.es/matlabcentral/fileexchange/43719-ar-drone-simulink-development-kit-v1

BIBLIOGRAFÍA 85

BIBLIOGRAFÍA 86

Apéndice A

Elipsoide de referencia

Un instrumento imprescindible para la navegación autónoma de vehículos aéreos es sulocalización. Para determinar su posición, precisa establecer un sistema de referencia.Dependiendo del objetivo que se pretenda alcanzar, se pueden utilizar distintos modelosde la Tierra. Desde algunos muy simplificados, como por ejemplo modelos de Tierraplana para Mecánica del Vuelo si se consideran distancias relativamente cortas, a otrosmucho mas sofisticados, que intentan representar la superficie topográfica de la Tierra deforma real. Esta representación resulta imposible, y es por eso que se utilizan superficiesideales, matemáticas, admitiendo que la Tierra se parece a ellas de forma más o menosaproximada. Por la forma de la Tierra, existen varios tipos de superficies:

Esfera: sencilla pero poco precisa

Elipsoide de revolución achatado en los polos, mas realista.

Geoide: una superficie compleja que aproxima bien la topografía definida en baseal modelo geopotencial de la Tierra (modelo gravitatorio y de rotación terrestre),y el que más se aproxima, aunque resulta bastante más complejo.

Elipsoides de referencia

El elipsoide presenta la ventaja de ser suficientemente simple como para ser manejabley suficientemente preciso para ser útil. Para definir un elipsoide son necesarios dosparámetros:

87

APÉNDICE A. ELIPSOIDE DE REFERENCIA 88

re: semieje ecuatorial. (a)

rp: semieje polar. (b)

Normalmente se utiliza el factor de achatamiento, definido por:

f = 1− rpre

aunque en las tablas se suele dar 1f

Otra alternativa es la excentricidad:

e =

√√√√1−r2p

r2e

WGS84

El WGS84 o Sistema Geodésico Mundial 1984 es un elipsoide de referencia aceptadocomo estándar en todo el mundo. Es el que se usa en el sistema GPS, por eso aunque losUAV normalmente tienen radios de acción pequeños, si se desea utilizar este sistemade localización se debe admitir este modelo terrestre.

Sus valores característicos son:

a = 6378137,0 metros1f= 298.257223563

Figura A.1: Sistema de referencia WGS84

Para más información sobre modelos cartográficos consultar la referencia [6].

APÉNDICE A. ELIPSOIDE DE REFERENCIA 89

Apéndice B

Códigos de programación

B.1. dinamica_quadrotor.m

Esta funcion se incluye en el bloque modelo dinámico y contiene todas las ecuacionespresentadas en el capítulo 2.

function [sys,x0,str,ts] = dinamica_quadrotor(t,x,u,flag, quad)

% Flyer2dynamics lovingly coded by Paul Pounds, first coded 12/4/04

% A simulation of idealised X-4 Flyer II flight dynamics.

% version 2.0 2005 modified to be compatible with latest version of Matlab

% version 3.0 2006 fixed rotation matrix problem

% version 4.0 4/2/10, fixed rotor flapping rotation matrix bug, mirroring

warning off MATLAB:divideByZero

% New in version 2:

% - Generalised rotor thrust model

% - Rotor flapping model

% - Frame aerodynamic drag model

% - Frame aerodynamic surfaces model

% - Internal motor model

% - Much coolage

% Version 1.3

90

APÉNDICE B. CÓDIGOS DE PROGRAMACIÓN 91

% - Rigid body dynamic model

% - Rotor gyroscopic model

% - External motor model

%ARGUMENTS

% u Reference inputs 1x4

% tele Enable telemetry (1 or 0) 1x1

% crash Enable crash detection (1 or 0) 1x1

% init Initial conditions 1x12

%INPUTS

% u = [N S E W]

% NSEW motor commands 1x4

%CONTINUOUS STATES

% z Position 3x1 (x,y,z)

% v Velocity 3x1 (xd,yd,zd)

% n Attitude 3x1 (Y,P,R)

% o Angular velocity 3x1 (wx,wy,wz)

% w Rotor angular velocity 4x1

%CONTINUOUS STATE MATRIX MAPPING

% x = [z1 z2 z3 n1 n2 n3 z1 z2 z3 o1 o2 o3 w1 w2 w3 w4]

%INITIAL CONDITIONS

z0 = [0 0 0]; % 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];

%CONTINUOUS STATE EQUATIONS

% z‘ = v

% v‘ = g*e3 - (1/m)*T*R*e3

% I*o‘ = -o X I*o + G + torq

% R = f(n)

% n‘ = inv(W)*o


% Dispatch the flag.

%

switch flag

case 0

[sys,x0,str,ts]=mdlInitializeSizes(init, quad); % Initialization

case 1

sys = mdlDerivatives(t,x,u, quad); % Calculate derivatives

case 3

sys = mdlOutputs(t,x, quad); % Calculate outputs

case 2, 4, 9 % Unused flags

sys = [];

otherwise

error([’Unhandled flag = ’,num2str(flag)]); % Error handling

end

end % End of flyer2dynamics

%==============================================================

% mdlInitializeSizes

% Return the sizes, initial conditions, and sample times for the

% S-function.

%==============================================================

%

function [sys,x0,str,ts] = mdlInitializeSizes(init, quad)

%

% Call simsizes for a sizes structure, fill it in and convert it

% to a sizes array.

%

sizes = simsizes;

sizes.NumContStates = 12;

sizes.NumDiscStates = 0;

sizes.NumOutputs = 12;

sizes.NumInputs = 4;

sizes.DirFeedthrough = 0;

sizes.NumSampleTimes = 1;

sys = simsizes(sizes);


%

% Initialize the initial conditions.

x0 = init;

%

% str is an empty matrix.

str = [];

%

% Generic timesample

ts = [0 0];

if quad.verbose

disp(sprintf(’t\t\tz1\t\tz2\t\tz3\t\tn1\t\tn2\t\tn3\t\tv1\t\tv2\t\tv3\t\to1\t\to2\t\to3\t\tw1\t\tw2\t\tw3\t\tw4\t\tu1\t\tu2\t\tu3\t\tu4’))end

end % End of mdlInitializeSizes.

%==============================================================

% mdlDerivatives

% Calculate the state derivatives for the next timestep

%==============================================================

%

function sys = mdlDerivatives(t,x,u, quad)

global a1s b1s

%CONSTANTS

%Cardinal Direction Indicies

N = 1; % N ’North’ 1x1

E = 2; % S ’South’ 1x1

S = 3; % E ’East’ 1x1

W = 4; % W ’West’ 1x1

D(:,1) = [quad.d;0;quad.h]; % Di Rotor hub displacements 1x3

D(:,2) = [0;quad.d;quad.h];

D(:,3) = [-quad.d;0;quad.h];


D(:,4) = [0;-quad.d;quad.h];

%Body-fixed frame references

e1 = [1;0;0]; % ei Body fixed frame references 3x1

e2 = [0;1;0];

e3 = [0;0;1];

%EXTRACT STATES FROM U

w = u(1:4);

%EXTRACT STATES FROM X

z = x(1:3); % position in W

n = x(4:6); % RPY angles W

v = x(7:9); % velocity in W

o = x(10:12); % angular velocity in W

%PREPROCESS ROTATION AND WRONSKIAN MATRICIES

phi = n(1); % yaw

the = n(2); % pitch

psi = n(3); % roll

% rotz(phi)*roty(the)*rotx(psi)

R = [cos(the)*cos(phi) sin(psi)*sin(the)*cos(phi)-cos(psi)*sin(phi)

cos(psi)*sin(the)*cos(phi)+sin(psi)*sin(phi); %BBF > Inertial rotation matrix

cos(the)*sin(phi) sin(psi)*sin(the)*sin(phi)+cos(psi)*cos(phi)

cos(psi)*sin(the)*sin(phi)-sin(psi)*cos(phi);

-sin(the) sin(psi)*cos(the) cos(psi)*cos(the)];

%Manual Construction

% Q3 = [cos(phi) -sin(phi) 0;sin(phi) cos(phi) 0;0 0 1]; % RZ %Rotation mappings

% Q2 = [cos(the) 0 sin(the);0 1 0;-sin(the) 0 cos(the)]; % RY

% Q1 = [1 0 0;0 cos(psi) -sin(psi);0 sin(psi) cos(psi)]; % RX


% R = Q3*Q2*Q1 %Rotation matrix

%

% RZ * RY * RX

iW = [0 sin(psi) cos(psi); %inverted Wronskian

0 cos(psi)*cos(the) -sin(psi)*cos(the);

cos(the) sin(psi)*sin(the) cos(psi)*sin(the)] / cos(the);

%ROTOR MODEL

for i=[N E S W] %for each rotor

%Relative motion

Vr = cross(o,D(:,i)) + v;

mu = sqrt(sum(Vr(1:2).ˆ2)) / (abs(w(i))*quad.r); %Magnitude of mu, planar components

lc = Vr(3) / (abs(w(i))*quad.r); %Non-dimensionalised normal inflow

li = mu; %Non-dimensionalised induced velocity approximation

alphas = atan2(lc,mu);

j = atan2(Vr(2),Vr(1)); %Sideslip azimuth relative to e1 (zero over nose)

J = [cos(j) -sin(j);

sin(j) cos(j)]; %BBF > mu sideslip rotation matrix

%Flapping

beta = [((8/3*quad.theta0 + 2*quad.theta1)*mu - 2*(lc)*mu)/(1-muˆ2/2); %Longitudinal

flapping

0;]; %sign(w) * (4/3)*((Ct/sigma)*(2*mu*gamma/3/a)/(1+3*e/2/r) + li)/(1+muˆ2/2)];

%Lattitudinal flapping (note sign)

beta = J’*beta; %Rotate the beta flapping angles to longitudinal and lateral coordinates.

a1s(i) = beta(1) - 16/quad.gamma/abs(w(i)) * o(2);

b1s(i) = beta(2) - 16/quad.gamma/abs(w(i)) * o(1);

%Forces and torques

T(:,i) = quad.Ct*quad.rho*quad.A*quad.rˆ2*w(i)ˆ2 * [-cos(b1s(i))*sin(a1s(i));

sin(b1s(i));-cos(a1s(i))*cos(b1s(i))]; %Rotor thrust, linearised angle approximations

Q(:,i) = -quad.Cq*quad.rho*quad.A*quad.rˆ3*w(i)*abs(w(i)) * e3; %Rotor drag torque

- note that this preserves w(i) direction sign


tau(:,i) = cross(T(:,i),D(:,i)); %Torque due to rotor thrust

end

%RIGID BODY DYNAMIC MODEL

dz = v;

dn = iW*o;

dv = quad.g*e3 + R*(1/quad.M)*sum(T,2);

do = inv(quad.J)*(cross(-o,quad.J*o) + sum(tau,2) + sum(Q,2)); %row sum of torques

sys = [dz;dn;dv;do]; %This is the state derivative vector

end % End of mdlDerivatives.

%==============================================================

% mdlOutputs

% Calculate the output vector for this timestep

%==============================================================

%

function sys = mdlOutputs(t,x, quad)

%CRASH DETECTION

if x(3)>0

x(3) = 0;

if x(6) > 0

x(6) = 0;

end

end

%if (x(3)>0)&(crash)

% error(’CRASH!’)

%end

%TELEMETRY

if quad.verbose

disp(sprintf(’ %0.3f\t’,t,x))end

% compute output vector as a function of state vector


% z Position 3x1 (x,y,z)

% v Velocity 3x1 (xd,yd,zd)

% n Attitude 3x1 (Y,P,R)

% o Angular velocity 3x1 (Yd,Pd,Rd)

n = x(4:6); % RPY angles

phi = n(1); % yaw

the = n(2); % pitch

psi = n(3); % roll

% rotz(phi)*roty(the)*rotx(psi)

R = [cos(the)*cos(phi) sin(psi)*sin(the)*cos(phi)-cos(psi)*sin(phi) cos(psi)*sin(the)*cos(phi)+sin(psi)*sin(phi);

%BBF > Inertial rotation matrix

cos(the)*sin(phi) sin(psi)*sin(the)*sin(phi)+cos(psi)*cos(phi) cos(psi)*sin(the)*sin(phi)-sin(psi)*cos(phi);

-sin(the) sin(psi)*cos(the) cos(psi)*cos(the)];

iW = [0 sin(psi) cos(psi); %inverted Wronskian

0 cos(psi)*cos(the) -sin(psi)*cos(the);

cos(the) sin(psi)*sin(the) cos(psi)*sin(the)] / cos(the);

% return velocity in the body frame

sys = [ x(1:6);

inv(R)*x(7:9); % translational velocity mapped to body frame

iW*x(10:12)]; % RPY rates mapped to body frame

%sys = [x(1:6); iW*x(7:9); iW*x(10:12)];

%sys = x;

end

% End of mdlOutputs.

B.2. Secuenciador de Waypoints

Esta función incluida en el diagrama Simulink, en el bloque de Generador de trayectorias,

gestiona los puntos de paso y crea una secuencia para que cuando se llegue a uno

de ellos se pase al siguiente.


function [WP_deseado,k,dist]=fcn(reset,modo,WP,x,y,z)

persistent i

if isempty(i),

i=1;

end

[n,m]=size(WP);

if reset==1

i=1;

end

%Calcular error en posición

ex = WP(i,1)- x;

ey = WP(i,2)- y;

ez = WP(i,3)- z;

k=i;

if modo==1

dist=1;

else

if modo==2

dist=abs(ez);

else

if modo==3

dist=sqrt(exˆ2+eyˆ2);

else

if modo==4

dist=sqrt(exˆ2+eyˆ2+ezˆ2);

else

dist=1;

end

end

end

end

%Distancia mínima para cambiar de waypoint

r=0.5;

if (dist<r)


i=i+1;

if(i==n+1);

i=1;

end

end

WP_deseado = WP(i,:);

end

B.3. modos.m

Función de configuración inicial del diseño para seleccionar modos de vuelo, tipos

de control y duración de la simulación, según los modos que se añadan.

%Limpiar rastro de ejecuciones anteriores

clear P mode tipoControl airetierra T

% % % % % % % % % % % % % % % % % % % % % % % % % %QUADROTOR % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % %

% % %Cargar Buses % % %

load(’BUS.ESTADO.mat’)

% % % %MODOS DE VUELO % % % % %

%Estructura con los modos de vuelo y sus tiempos

i=1;

preg=1;

while preg =0

modovuelo = menu(’Elija un modo de vuelo’, ...

’Estabilizacion’,’Estab. en altura’,’Mov. en el plano’,’Movimiento en XYZ’,’Aterrizaje’,

’ReturnToHome’,’Emergencia’);

m(i)=modovuelo;

choice2 = questdlg(’¿Desea añadir otro modo de vuelo?’, ...

’¿Desea añadir otro modo de vuelo?’, ...

’Sí’,’No’,’Sí’);


switch choice2

case ’Sí’

%disp([choice ’Sí’])

preg = 1;

case ’No’

%disp([choice ’No’])

preg = 0;

end

i=i+1;

end

n=length(m);

T=n*10; %Tiempo de simulacion

tstop=T;

A=zeros(n*1000,1);

for k=1:n;

for j=1:1000,

A(j+(k-1)*1000)=m(k);

end

end

mode.signals.values=A;

mode.time=0:0.01:(T-0.01);

% % %MODOS DE CONTROL % % %

%Este script permite elegir previamente al vuelo los tipos de control que

%van a estar activos en el vuelo del quadrotor. Debe ser ejecutado antes de

%la simulación

%CONTROL DE ALTO NIVEL

choice1 = questdlg(’Modo de control de ALTO NIVEL: Control de posición’, ...

’Control de Alto Nivel’, ...

’Lineal’,’No Lineal’,’Lineal’);

switch choice1


case ’Lineal’

%disp([choice ’Control de Alto Nivel Lineal’])

HI = 1;

case ’No Lineal’

%disp([choice ’Control de Alto Nivel No Lineal’])

HI = 2;

end

if isempty(HI)

HI = 1;

end

tipoHI=zeros(n*1000,1);

for i=1:n*1000,

tipoHI(i)=HI;

end

%CONTROL DE BAJO NIVEL

choice = questdlg(’Modo de control de Bajo NIVEL: Control de estabilización’, ...

’Control de Bajo Nivel’, ...

’Lineal’,’No Lineal’,’Lineal’);

switch choice

case ’Lineal’

%disp([choice ’: Control de Bajo Nivel Lineal’])

LO = 1;

case ’No Lineal’

%disp([choice ’: Control de Bajo Nivel No Lineal’])

LO = 2;

end

if isempty(LO)

LO = 1;

end

tipoLO=zeros(n*1000,1);

for i=1:n*1000,

tipoLO(i)=LO;

end

t1=0:0.01:T-0.01;


tipoControl.HI=timeseries(tipoHI,t1);

tipoControl.LO=timeseries(tipoLO,t1);

clear HI LO tipoHI tipoLO i choice choice1 tipoHI

% % % %MODO AIRE-TIERRA % % % %

%Alternar entre modo tierra y modo aire

%El modo tierra realiza las comprobaciones de conexiones.

%Al arrancar el modo aire,el quadcopter despega

atV=zeros(n*1000,1);

%choice = questdlg(’Modo Aire / Modo Tierra’, ...

% ’Modo Aire / Modo Tierra’, ...

% ’Aire’,’Tierra’,’Aire’);

%switch choice

case ’Aire’

%disp([choice ’Aire’])

at = 1;

case ’Tierra’

% %disp([choice ’Tierra’])

% at = 0;

%end

%if isempty(at)

% at = 0;

%end

for i=1:n*1000,

atV(i)=at;

end

%airetierra.signals.values=atV;

%airetierra.time=t1;

airetierra=timeseries(at,t1);

clear atV at n


B.4. wp_interp.m

Este código es utilizado para generar la matriz P de waypoints a partir de una

ruta creada en Google Earth. Es una variante del archivo wp.m que no se incluye

pues su única diferencia es la línea final, que permite interpolar entre los waypoints

utilizando la funcion trayectoria.m, que sí se incluye en este apéndice.

function P=wp_interp(archivo)

%WAYPOINTS

[lat,lon,z] = read_kml(archivo);

nw=length(lat);

wps=zeros(nw,3);

wps(:,1)=lat;

wps(:,2)=lon;

%Se crea un vector para la altitud, que empiece en 3m y sume 1 por cada

%waypoint

z1=zeros(nw,1);

for i=1:nw

z1(i)=3+i;

end

wps(:,3)=z1;

% P = lla2flat( LLA, LL0, PSI0, HREF, MODEL )

P = lla2flat( wps, [37.411957 -6.002696], 0, 0, ’WGS84’ );

%Cambiar ejes de coordenadas. x por y

a=P(:,1);

b=P(:,2);

P(:,1)=b;

P(:,2)=a;

clear i z z1 wps lat lon nw a b

%Una vez creados los puntos de paso, se interpola para crear mas puntos

%intermedios

P=trayectoria(P);


B.5. googleEarth.m

Este código permite convertir a formato de Google Earth (*.kml) la evolución de

x e y a lo largo del tiempo, gracias a que están incluidos en el archivo de telemetría

result.mat

%Representar en Google Earth. Es necesario tener el bloque de Google Earth.

x=result(:,3);

y=result(:,2);

n=length(x);

z=zeros(n,1);

P1 = flat2lla( [x y z], [37.411957 -6.002696], 0, 0, ’WGS84’ );

lat0=P1(:,1);

lon0=P1(:,2); %Manual

kmlStr01 = ge_plot(lon0,lat0,...

’lineWidth’,2.0, ...

’lineColor’,’ff32a4ff’,...

’msgToScreen’,true,...

’extrude’,0);

kmlFileName = ’ge_plot1.kml’; %Saca un fichero .kml que puede leer Google Earth

con el que representamos la trayectoria

ge_output(kmlFileName,kmlStr01,’name’,kmlFileName,’msgToScreen’,true);

B.6. trayectoria.m

A partir de una matriz con nx3 coordenadas, genera otra matriz con 10 puntos intermedios

entre cada waypoint,

function WP1=trayectoria(WP)

x=WP(:,1);

y=WP(:,2);

z=WP(:,3);

[n,m]=size(WP);


x(n+1)=x(1);

y(n+1)=y(1);

z(n+1)=z(1);

np=10;

x1=zeros(n,np);

y1=zeros(n,np);

z1=zeros(n,np);

for i=1:n

x1(i,:)=linspace(x(i),x(i+1),np);

y1(i,:)=linspace(y(i),y(i+1),np);

z1(i,:)=linspace(z(i),z(i+1),np);

end

clear WPX

clear WPY

clear WPZ

WPX=x1(1,1:np-1)’;

WPY=y1(1,1:np-1)’;

WPZ=z1(1,1:np-1)’;

for j=2:n

WPX=[WPX;x1(j,1:np-1)’];

WPY=[WPY;y1(j,1:np-1)’];

WPZ=[WPZ;z1(j,1:np-1)’];

end

WP1=[WPX,WPY,WPZ];

B.7. representar.m

Esta función permite representar variables del vector de estado una vez finalizada

la simulación.

%Gráficas


%estructura result

close all

w1 = result(:,1);

w2 = result(:,2);

w3 = result(:,3);

w4 = result(:,4);

modoR = result(:,5);

modo = result(:,6);

pitchRef = result(:,7);

rollRef = result(:,8);

yawRef = result(:,9);

xRef = result(:,10);

yRef = result(:,11);

zRef = result(:,12);

x = result(:,13);

y = result(:,14);

z = result(:,15);

yaw = result(:,16);

pitch = result(:,17);

roll = result(:,18);

dx = result(:,19);

dy = result(:,20);

dz = result(:,21);

dyaw = result(:,22);

dpitch = result(:,23);

droll = result(:,24);

tiempo = result(:,25);

n=length(x);

%xyzREF, xyz frente al tiempo

figure

subplot(4,1,1)

plot(tiempo,modoR,’r’,tiempo,modo,’b’,’LineWidth’,2),title(’\fontsize18Modo solicitado

y Modo real’),legend(’\fontsize16Modo Real’,’\fontsize16Modo’,’Location’, ’EastOutside’

),yaxis([-1 8])


subplot(4,1,2)

plot(tiempo,xRef,’r’,tiempo,x,’b’,’LineWidth’,2),title(’\fontsize18xRef, x’),

legend(’\fontsize16Xref’,’\fontsize16X’,’Location’, ’EastOutside’ )

subplot(4,1,3)

plot(tiempo,yRef,’r’,tiempo,y,’b’,’LineWidth’,2),title(’\fontsize18yRef, y’),

legend(’\fontsize16Yref’,’\fontsize16Y’,’Location’, ’EastOutside’ )

subplot(4,1,4)

plot(tiempo,-zRef,’r’,tiempo,-z,’b’,’LineWidth’,2),title(’\fontsize18zRef, z’),

legend(’\fontsize16-Zref’,’\fontsize16-Z’,’Location’, ’EastOutside’ )

%Altura y AlturaRef respecto al tiempo

figure

subplot(2,1,1)



),yaxis([-1 8])

subplot(2,1,2)

plot(tiempo,-zRef,’r’,tiempo,-z,’b’,’LineWidth’,2),title(’\fontsize18zRef, z’),

legend(’\fontsize16-Zref’,’\fontsize16-Z’,’Location’, ’EastOutside’ )

%eulerRef, euler frente al tiempo

figure

subplot(4,1,1)



),yaxis([-1 8])

subplot(4,1,2)

plot(tiempo,pitchRef,’r’,tiempo,pitch,’b’,’LineWidth’,2),title(’\fontsize18pitchRef,pitch’),legend(’\fontsize16pitchref’,’\fontsize16pitch’,’Location’, ’EastOutside’

)

subplot(4,1,3)

plot(tiempo,rollRef,’r’,tiempo,roll,’b’,’LineWidth’,2),title(’\fontsize18rollRef,roll’),legend(’\fontsize16rollref’,’\fontsize16roll’,’Location’, ’EastOutside’

)

subplot(4,1,4)


plot(tiempo,yawRef,’r’,tiempo,yaw,’b’,’LineWidth’,2),title(’\fontsize18yawRef,yaw’),legend(’\fontsize16yawref’,’\fontsize16yaw’,’Location’, ’EastOutside’

)

%Posicion en el plano XY

figure

plot(P(:,1),P(:,2),’*r’,0,0,’og’,’LineWidth’,2),hold on

plot(x,y,’b’,’LineWidth’,2),grid,legend(’\fontsize16Waypoints’,’\fontsize16PuntoInicial’,’\fontsize16Trayectoria en el plano’),title(’\fontsize16Gráfica 2D’)

%Trayectoria3D

figure,hold on

grid,title(’\fontsize18Trayectoria 3D’)

%Representar la trayectoria

plot3(x,y,-z,’b’,’LineWidth’,2)

%Representar los waypoints

nw=length(P(:,3));

plot3(P(:,1),P(:,2),-P(:,3),’*r’,0,0,0,’og’)

%Representar la proyección 2D

v0=zeros(n,1);

plot3(x,y,v0,’-k’) %Proyección de la trayectoria

P0=zeros(nw,1);

plot3(P(:,1),P(:,2),P0,’xk’) %Proyección de los waypoints

%M=zeros(5,nw);

%for i=1:nw %Línea vertical entre waypoint y su proyección

% h=-P(i,3)/4;

% M(:,i)=0:h:-P(i,3);

% plot3(P(i,1)*ones(5,1),P(i,2)*ones(5,1),M(:,i),’–k’),hold on

%end

xlabel(’\fontsize16x’),ylabel(’\fontsize16y’),zlabel(’\fontsize16-z’),legend(’\fontsize16Trayectoria’,’\fontsize16Waypoints’)

%Representar en Google Earth. Es necesario tener el toolbox de Google Earth.

%P1 = flat2lla( [y x z], [37.411957 -6.002696], 0, 0, ’WGS84’ );

%lat0=P1(:,1);


%lon0=P1(:,2); %Manual

%kmlStr01 = ge_plot(lon0,lat0,...

% ’lineWidth’,2.0, ...

% ’lineColor’,’ff32a4ff’,...

% ’msgToScreen’,true,...

% ’extrude’,0);

%kmlFileName = ’ge_plot1.kml’; %Saca un fichero .kml que puede leer Google Earth

con el que representamos la trayectoria

%ge_output(kmlFileName,kmlStr01,’name’,kmlFileName,’msgToScreen’,true);

clear modo modoR tiempo x y z pitch roll yaw dx dy dz dyaw dpitch droll v0 nw M

P0 h pitchRef rollRef yawRef xRef yRef zRef w1 w2 w3 w4

Trabajo Fin de Grado Grado en Ingeniería...

Documents

Transcript of Trabajo Fin de Grado Grado en Ingeniería...