Story Transcript
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
Agradecimientos El 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 mi madre y a mi hermana, por no dejar de confiar en mi. Mi gratitud también a mi padre por todo lo que me inculcó y que nunca olvidaré. Agradecer finalmente a los amigos que siempre están.
2
3
Resumen Se presenta el desarrollo de una solución para la verificación y validación de sistemas de gobierno y control de vuelo para pequeñas plataformas de tipo VTOL (Vertical Take-Off and Landing). Para la simulación del vehículo se dispone del correspondiente modelo dinámico, que será interconectado con diferentes subsistemas de control de estabilización y posición, gobernados por una máquina de estados definida en MATLAB Stateflow. El conjunto de simulaciones para la verificación del diseño de la solución propuesta son llevadas a cabo mediante Simulink. El objetivo es disponer de mecanismos que permitan evaluar el correcto funcionamiento de prototipos antes de ser evaluados en condiciones reales.
4
Í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 estabilización . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
65
7
ÍNDICE GENERAL 5.3.5. Despegue, estabilización, movimiento en el espacio, ToHome y aterrizaje . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
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, movimiento en 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 inercial 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 salida de 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 “Alto Nivel” . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
32
8
9
ÍNDICE DE FIGURAS 4.8. Elemento de Stateflow para incorporar scripts de Matlab . . . . . . . .
35
4.9. Esquema explicativo de los estados que componen el bloque Stateflow y su 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 en el 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 de la 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 quadrotor 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 Ardupilot 2 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 crecimiento en los últimos tiempos, tanto en el campo de la investigación, como en soluciones aportadas por empresas privadas, e incluso en el ámbito doméstico. Ésto es debido principalmente a su bajo coste y las posibilidades que ofrece para el desarrollo de numerosas aplicaciones, 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ógico que ha supuesto la miniaturización de actuadores y sensores ocurrido en los últimos años, así como la evolución de sistemas de almacenamiendo de energía y procesamiento de datos.
Su uso está cada vez más extendido, incluso en España, donde ya existe normativa vigente que establece el marco legal para su utilización [1]. Las plataformas de ala fija han sido objeto de estudio principal de los sistemas de control de vuelo, sin embargo, la aparició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 sistemas autónomos de vuelo de pequeño tamaño, en concreto los sistemas multirotor, y de forma particular los quadrotor.
Según [2], el interés que despiertan este tipo de plataformas reside en su versatilidad y maniobrabilidad, permitiéndoles la ejecución de un gran número de tareas. Sin embargo su 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 del balance de fuerzas de propulsión ejercidas por las cuatro hélices accionadas por los cuatro actuadores.
Las ventajas que presentan este tipo de plataformas con respecto a un helicóptero convencional son: Aumenta la capacidad de carga gracias a la suma de empuje proporcionado por sus cuatro rotores Alta maniobrabilidad, idóneo para el despegue, aterrizaje y vuelo en entornos complejos La sencillez mecánica, debido a que el movimiento es proporcionado únicamente por 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 elementos mecánicos más complejos.
CAPÍTULO 1. INTRODUCCIÓN
3
Sin embargo, las plataformas tipo quadrotor presentan como desventaja su aumento de peso 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 de los sistemas tripulados de forma remota hacia una generación de vehículos capaces de navegar de forma autónoma. Los elementos inerciales permiten la estabilización y control de la plataforma, la incorporación de sistemas de posicionamiento por satélite establecen su localización y facilitan el guiado del vehículo en exteriores, y los sensores de flujo óptico, cámaras o sistemas láser añaden autonomía al guiado al no depender de sistemas externos e incluyen la posibilidad de navegación en interiores. Estos sistemas descritos sólo son una parte de los numerosas opciones que se pueden incluir en una plataforma de este tipo para su control y guiado, sin embargo gracias a trabajos recientes 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ón y adaptación no es trivial, ya que se necesita conseguir un nivel de seguridad adecuado para una plataforma aérea robótica, lo que requiere una combinación óptima entre software y hardware embarcado y ser robusto a fallos sensoriales o de pérdida de datos con el enlace de tierra. Cualquier otra situación no prevista podría conllevar daños en la plataforma, con la consiguiente pérdida económica y de horas de trabajo que ello supondría, o incluso ser peligroso con la seguridad de las personas. Es por ello que se hace imprescindible la evaluación del software embarcado para poder detectar posibles fallos que deriven en situaciones imprevistas y/o peligrosas.
Las soluciones actuales, económicamente viables, que combinan hardware y software má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 actualizaciones del firmware del dispositivo e incluye nuevas mejoras y funcionalidades. A nivel de investigación, lo que se pretende es poder reutilizar parte de estas aportaciones para poder avanzar en campos como el control o la fusión sensorial, pudiendo acceder a cualquier elemento hardware o software del sistema y poder reconfigurarlo, si fuera necesario. Si bien es una plataforma muy extendida, no es posible una comprobación
CAPÍTULO 1. INTRODUCCIÓN
4
por simulación del sistema, de tal manera que ante cualquier modificación del código fuente, o bien una reformulación de la lógica de aplicación, la única depuración y verificación posibles, incluyendo el comportamiento de los controladores, es mediante el despegue de la plataforma y comprobando que la evolución de la misma no deriva en situaciones de inestabilidad. Para entornos como investigación básica y docencia, son situaciones no deseables.
Se hace por tanto imprescindible el desarrollo de entornos que permitan una simulación completa del sistema para su verificación, control de supervisión, planificación de tareas, 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 comportamiento del sistema ante cualquier entrada o evento externo, comprobando que la salida se corresponde con lo previamente diseñado.
1.2.
Objetivos
El presente proyecto tiene como objetivo el desarrollo de una solución para la verificación y validación de sistemas de gobierno y control de pequeñas plataformas de tipo MAV - VTOL (Micro Air Vehicle - Vertical Take Off Landing). Para la simulación del vehículo se dispone del correspondiente modelo dinámico, que será interconectado con diferentes sistemas de control y estabilización, gobernados por una máquina de estados definida en MATLAB Stateflow®. El conjunto de simulaciones para la verificación del diseño de la solución propuesta son llevadas a cabo mediante Simulink. El objetivo es disponer de mecanismos que permitan evaluar el correcto funcionamiento de prototipos antes de ser evaluados en condiciones reales.
La solución propuesta en este trabajo, para diseñar e implementar una máquina de control de estados e integrarla en una plataforma de tipo quadrotor, está basada en la combinació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 simulación de lógica de decisión combinatoria y secuencial basado en máquinas de estado y diagramas de flujo. Permite la representación de diagramas de transición de estado con el fin de modelar la evolución del sistema ante eventos internos, externos o condiciones
CAPÍTULO 1. INTRODUCCIÓN
5
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 quadrotor susceptibles de ser simuladas y algunos ejemplos de software de simulación y planificación. Se presentan algunas ideas sobre la utilización de software comercial en el sector educativo. En el capítulo 3 se describe el helicóptero quadrotor, comentando algunos detalles de su funcionamiento y a continuación se presentan las ecuaciones que definen el modelo dinámico de este tipo de plataformas, mediante la matriz de rotación y las ecuaciones cinemáticas. En el capítulo 4 se detalla la descripción de la solución adoptada, donde se realiza una breve descripción del diagrama principal de Simulink y a continuación pasan a desarrollarse cada uno de los bloques por separado. En primer lugar se habla del bloque Quadrotor, que contiene la implementación de las ecuaciones presentadas en el capítulo 3. Después, se desarrolla el bloque de Control, separado en control de Alto Nivel y Bajo Nivel. El siguiente punto es una introducción a MATLAB Stateflow y el desarrollo de la máquina de estados, donde se encuentra el grueso del trabajo. Se incorpora descripción de todos los estados añadidos y de las transiciones utilizadas. Por último, se añade la descripción de un bloque Generador de trayectorias. En el capítulo 5 se presentan diferentes simulaciones realizadas con objeto de verificar que el diseño se ajusta al comportamiento esperado y se presentan las grá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 y las posibles líneas de trabajo futuro que se presentan para la continuidad de este trabajo.
CAPÍTULO 1. INTRODUCCIÓN
6
Capítulo 2 Estado del arte Hasta hace unos años, el desarrollo de vehículos aéreos no tripulados (UAV, por sus siglas en inglés) estaba asociado a pequeñas o medianas aeronaves militares cuya función principal era servir de aeronaves espía o incluso realizar misiones de ataque. Las noticias sobre este tipo de tecnología estaban asociadas en su mayoría al término drone y sus connotaciones bélicas.
Sin embargo, durante los últimos años la investigación sobre el uso civil de estas plataformas ha evolucionado de tal forma que sus posibilidades actuales se multiplican por momentos. Cada vez es más habitual leer noticias sobre nuevas aplicaciones civiles en desarrollo (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 de plataformas son claves en este crecimiento.
Actualmente, muchas empresas y centros de investigación se encargan de desarrollar nuevos vehículos y componentes, pero también de desarrollar el software necesario para su uso eficiente y seguro. Se precisa de software de control, de planificación y de gestión de datos, y además debe ser robusto y extremadamente fiable, por tratarse de plataformas aéreas.
7
CAPÍTULO 2. ESTADO DEL ARTE
8
Plataformas Existe gran variedad de plataformas que permiten el desarrollo de aplicaciones y mejoras en el entorno académico, por ser accesibles y disponer de amplia información en comunidades 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 lanzado para uso recreativo, y destaca por su pilotaje ya que se controla desde un dispositivo móvil tipo smartphone o tablet, o desde un ordenador. Una de sus características principales 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 comerciales de la competencia, el software AR.Drone Open API Platform [11], que permite el desarrollo de aplicaciones y juegos para esta plataforma. Gracias a este software pueden realizarse proyectos que van mucho más allá del uso recreativo para el que en un principio fue diseñado. Dispone de multitud de documentación en la red, donde existen comunidades de usuarios que comparten sus desarrollos, por lo que resulta muy accesible.
Ardupilot Ardupilot es una plataforma de vehículos aéreos no tripulados de código abierto. Permite controlar UAVs tipo aeronaves de ala fija, helicópteros convencionales, o plataformas
CAPÍTULO 2. ESTADO DEL ARTE
9
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 permiten numerosas configuraciones: Tricopter, Quadcopter, Hexacopter, Octacopter, etc. como se 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 generado en el IDE genérico de Arduino, e incluso, cualquier otro tipo código que pueda adaptarse a esta plataforma. Si bien es una plataforma muy extendida, no es posible una comprobación por simulacion del sistema, de tal manera que ante cualquier modicación del código fuente, o bien una reformulación de la lógica de aplicación, la unica depuració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 misma no 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 su código, existe un vacío en términos de validación y verificación de estos UAV, tal y
CAPÍTULO 2. ESTADO DEL ARTE
10
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 funciones pueden realizarlas alguno de las siguientes aplicaciones, el hecho de poder simular el comportamiento de un UAV sin necesidad de embarcar el código y testear su comportamiento frente a diferentes modos de funcionamiento resulta bastante interesante desde 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 dispositivo, planificar misiones, obtener datos de vuelo y ejecutar análisis posterior mediante almacenamiento de registro de datos o logs.
TrackDrone Lite TrackDrone Lite es una aplicación desarrollada por el Grupo de Control Predictivo y Optimización Heurística de la Universidad Politécnica de Valencia. Permite implementar, simular y testear cualquier tipo de algoritmos de control, destinados al seguimiento de trayectorias de manera autónoma en la plataforma AR.Drone. Mediante este software es posible controlar el drone tanto en modo manual como en modo automático
CAPÍTULO 2. ESTADO DEL ARTE
11
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ás información seguir la referencia [15]
Figura 2.5: Imagen de la aplicación TrackDrone Lite
CAPÍTULO 2. ESTADO DEL ARTE
12
AR.Drone Simulink Development-Kit Este software implementado en MATLAB Simulink permite la simulación y control mediante Wi-Fi del dispositivo ARDrone 2.0 [16]
Flightgear FlightGear es un simulador de vuelo de código abierto multiplataforma de gran aceptación entre usuarios de este tipo de simuladores. Entre otras muchas funciones, destaca la posibilidad de conexión mediante puertos internos o vía Internet a otros programas para 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 este programa. Mediante este bloque es posible visualizar de forma gráfica la evolución del vector de estado de un modelo de UAV diseñado en MATLAB/Simulink, tan sólo conectando 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
CAPÍTULO 2. ESTADO DEL ARTE
13
AerosimRC Software de simulación para entrenamiento. Permite iniciarse en el control de vehículos aéreos por RC, aprender nuevas técnicas de vuelo, entrenar modos de vuelo concretos o practicar 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án sometidos cada vez más a todo tipo de medios audiovisuales. Este hecho plantea un desafío para los profesores, ya que resulta complicado atraer su atención, especialmente en temas técnicos y con tiempo limitado. Además, las asignaturas de automática han de ser prácticas y actuales. Por tanto, atraer la atención de los estudiantes y mantener el contenido actualizado y práctico son objetivos primordiales, que pueden alcanzarse gracias a los experimentos en clase, prácticas de laboratorio y contenido sobre tecnología de reciente aparición.
Como ya se ha comentado, los quadrotor presentan algunas grandes ventajas que los hacen útiles y accesibles para investigadores y comunidades educativas. Es por esto que resultan una opción perfecta para realización de prácticas y proyectos de asignaturas relacionadas con el Control Automático. Por eso otro motivo de desarrollo para este proyecto es presentar una forma de aprendizaje de algunos puntos importantes de la Automática mediante prácticas y experimentos, para aumentar el interés de los alumnos y sus conocimientos sobre esta materia.
CAPÍTULO 2. ESTADO DEL ARTE
14
Capítulo 3 Quadrotor (MAV-VTOL) En este capítulo se describen las ecuaciones que rigen la dinámica de los sistemas quadrotor. La información que se presenta en este capítulo ha sido obtenida de la referencia [17].
3.1.
Descripción del MAV
El vehículo en que se basa este trabajo es un helicóptero tipo quadrotor, con cuatro rotores 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 en sus rotores. Estos rotores constan de un motor eléctrico, mecanismo de engranaje y un rotor 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 debe aumentar la velocidad de giro del rotor trasero y disminuir la del rotor delantero. Ésto provoca un par que origina el giro de la plataforma según el eje perpendicular a la direcció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 velocidad 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 antihorario. Esto anula el giro de la plataforma por el giro de sus rotores y permite el denominado movimiento de guiñada, mediante diferencia en la velocidad de giro entre los 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 y orientación, se supone el quadrotor como un cuerpo rígido en el espacio, sujeto a una fuerza principal (empuje) y tres momentos (pares). En la figura 3.1 se muestra un esquema de las fuerzas que ejercen las distintas hélices para generar el movimiento del vehículo. A continuación, de una forma más detallada, se describen las fuerzas y pares actuantes sobre la plataforma.
17
CAPÍTULO 3. QUADROTOR (MAV-VTOL)
El par para generar un movimiento de balanceo o de roll (ángulo φ) se realiza mediante un desequilibrio entre las fuerzas f2 y f4 . Para el cabeceo o pitch (ángulo θ), el desequilibrio se realizará entre las fuerzas f1 y f3 . El movimiento en el ángulo de guiñada o 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 2 y 4. El empuje total produce que el quadrotor se desplace perpendicularmente al plano de 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 modelo dinámico debe incluir los efectos giroscópicos resultantes tanto del cuerpo rígido rotando en el espacio, como de la rotación de las cuatro hélices. En la Tabla 3.1 se describen tales efectos, donde C representan términos constantes, Ω es la velocidad del rotor, JR es el momento de inercia rotacional del rotor alrededor de su eje, l es la distancia del centro 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
- Rotación de los rotores
CΩ2
Efectos Aerodinámicos - Giro de hélices Pares Inerciales Opuestos Efectos de la gravedad
- Cambio en la velocidad de rotación de los rotores - Posición del centro de masa -Cambio en la orientación del cuerpo rígido
Efectos giroscópicos - Cambio en la orientación del plano de los rotores Fricción
- Todos los movimientos del helicóptero
JR Ω˙ l Jθψ JR Ωθ, φ ˙ θ, ˙ ψ˙ 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 entradas de control. Debido a las complejidad de esta situación, se deben realizar algunas suposiciones para el modelado: Se desprecian los efectos de los momentos causados por el cuerpo rígido sobre las dinámicas traslacionales, así como el efecto suelo. El centro de masa y origen del sistema de coordenadas fijo se consideran coincidentes, y se supone que la estructura del helicóptero es simétrica, lo que implica que la matriz de inercia sea diagonal.
18
CAPÍTULO 3. QUADROTOR (MAV-VTOL)
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ículo. El quadrotor, como sólido rígido, está caracterizado por un sistema de coordenadas ligado a él y con origen en su centro de masas. El sistema de ejes ligado a él está de− → → − − → terminado por B = {− x→ L , yL , zL }, donde el eje xL es la dirección normal de ataque del − → helicóptero, − y→ L es ortogonal a xL y es positivo hacia la derecha mirando en el sentido − positivo de éste, mientras que → zL está orientado en sentido ascendente y ortogonal al − → − → − − − plano xL OyL . El sistema de coordenadas inercial I = {→ x ,→ y ,→ z } se considera fijo con respecto a la Tierra. Se determina ξ = {x, y, z} como la posición del centro de masa del helicó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 de rotación ortonormal. La rotación de un cuerpo rígido puede ser obtenida mediante ángulos de Euler, o cuaterniones, por ejemplo. Existen varias definiciones de los ángulos de Euler para representar la orientación relativa de dos sistemas de coordenadas. La más popular en ingeniería aeroespacial es la convención-xyz (giro alrededor de x, y’, z”) y es conocida como ángulos de Tait-Bryan, también conocidos por ángulos “Cardano”. Los ángulos de Tait-Bryan se utilizan para describir una rotación general en el espacio Euclideo tridimensional a través de tres rotaciones sucesivas en torno de ejes del sistema móvil en el cual están definidos. Así, en este trabajo se usarán los ángulos de Tait-Bryan para describir la orientación del quadrotor. La rotación de un cuerpo rígido en el espacio se realiza mediante tres rotaciones sucesivas: − 1. Rotación según → x de φ: el primer giro es el correspondiente al ángulo de roll o − de balanceo, φ, y se realiza alrededor del eje → x .
x1 y1 z1
=
1 0 0 0 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 partir → − del nuevo eje − y→ L con el ángulo ángulo pitch o de cabeceo, θ, para dejar el eje zL
19
CAPÍTULO 3. QUADROTOR (MAV-VTOL) 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 ángulo − − yaw o de guiñada, ψ, y se realiza alrededor del eje → z a partir del nuevo eje → zL para dejar el quadrotor en su posición final.
x3 y3 z3
=
cos ψ − sen ψ 0 x2 y sen ψ cos ψ 0 2 0 0 1 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 tres rotaciones:
Figura 3.3: Rotación de los ángulos de Tait-Bryan del sistema de coordenadas inercial al sistema de coordenadas fijado al helicóptero. Las matrices de rotación que representan la orientación del cuerpo rígido rotando alrededor de cada eje queda como sigue:
20
CAPÍTULO 3. QUADROTOR (MAV-VTOL)
R(x, φ) =
1 0 0 0 cos φ − sen φ 0 sen φ cos φ
, R(y, θ) =
cos ψ − sen ψ 0 sen ψ cos ψ 0 0 0 1
R(z, ψ) =
cos θ 0 sen θ 0 1 0 − sen θ 0 cos θ
(3.4)
,
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 =
RI =
cos ψ − sen ψ 0 cos θ 0 sen θ · sen ψ cos ψ 0 0 1 0 0 0 1 − sen θ 0 cos θ
·
1 0 0 0 cos φ − sen φ 0 sen φ cos φ
(3.5)
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 de RI , 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 de rotación del quadrotor que establecen las relaciones entre las velocidades angulares. Sea una matriz ortonormal R, donde:
21
CAPÍTULO 3. QUADROTOR (MAV-VTOL)
RT R = In
(3.7)
˙ T R + RT R ˙ = 0n R
(3.8)
˙ S = RT R
(3.9)
ST +S =0n
(3.10)
y su derivada respecto al tiempo es:
Definiendo:
se obtiene a partir de 3.8 que:
donde S es anti-simétrica. La relación entre la derivada de la matriz ortonormal y la matriz anti-simétrica es la siguiente: ˙ S = R-1 R
(3.11)
Por lo tanto, las ecuaciones cinemáticas para determinar la orientación del helicóptero suponiendo la matriz de rotación 3.5, vienen dadas por: ˙ I = RI , ·S(ω) R
(3.12)
donde ω = [p q r]T son las velocidades angulares en el sistema de coordenadas ligado al cuerpo rígido y S(ω) (S(ω)(·) = ω × ·) es la siguiente matriz anti-simétrica:
S(ω) =
0 −r q r 0 −p −q p 0
Manipulando matemáticamente 3.12 se obtiene:
(3.13)
22
CAPÍTULO 3. QUADROTOR (MAV-VTOL)
φ˙ θ˙ ψ˙
=
1 sen φ tan θ cos φ tan θ 0 cos φ − sen φ 0 sen φ sec θ cos φ sec θ
p q r
(3.14)
˙ θ, ˙ ψ) ˙ es una función discontinua. Hay que La variación de los ángulos de Tait-Bryan (φ, distinguirla de las velocidades angulares en el sistema de referencia asociado al cuerpo rígido (p, q, r) las cuales pueden medirse con giróscopos. Normalmente, se utiliza la IMU, del inglés Inertial Measurement Unit o Unidad de Medida Inercial, para medir las 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 en el tiempo de los ángulos de Tait-Bryan se obtiene a través de la inversión del Jacobiano de 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 velocidades angulares: velocidad angular de balanceo (p), velocidad angular de cabeceo (q), − → → − y velocidad angular de guiñada (r), sobre los ejes − x→ L ,yL y zL respectivamente. Las velocidades angulares son debidas a los pares ligados al cuerpo rígido producidos por las fuerzas 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 − x→ L ,yL y zL respectivamente El movimiento de traslación viene dado por la velocidad v = [u0 v0 w0 ]T en ejes inerciales, 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.4.
(3.16)
Modelo dinámico
En esta sección se obtendrán las ecuaciones dinámicas del quadrotor. A través de la formulación de Newton-Euler, es posible obtener las ecuaciones dinámicas de un cuerpo
23
CAPÍTULO 3. QUADROTOR (MAV-VTOL)
rígido sujeto a fuerzas externas aplicadas en el centro de masas, que expresadas en los ejes cuerpo quedan de la siguiente forma:
˙ mI3x3 0 V ω × mV F + Fd + = 0 J ω˙ ω × Jω τ + τd
(3.17)
donde J∈R3x3 es la matriz de inercia, I3x3 ∈R3x3 es la matriz identidad, V es el vector velocidad traslacional en ejes cuerpo, ω es el vector velocidad angular, también en ejes cuerpo, y m es la masa total del quadrotor. Con las suposiciones iniales, puede considerarse que la matriz de inercia es diagonal:
J=
Ixx 0 0 0 Iyy 0 0 0 Izz
(3.18)
Siendo el vector de estado [ξ v η ω]T , donde ξ y v ∈ R3 representan la posición y velocidad lineales expresadas en los ejes inerciales, η la posición angular y ω la velocidad angular, ambas en ejes cuerpo, las ecuaciones de movimiento de un cuerpo rígido quedan de la siguiente forma:
ξ˙ = v mv˙ = RI Fb ˙ I = RI S(ω) R
(3.19)
Jω˙ = −ω × Jω + τb
˙ I. donde ξ˙ = v = RI V y S(ω) = RIT R Queda expuesto por tanto que el quadrotor, como se indicó en la introducción de este capí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 el quadrotor, representados por Fb y τb . Estas fuerzas y pares son el peso, fuerzas aerodinámicas, y los pares y el empuje desarrollado por los motores, que se representan por:
24
CAPÍTULO 3. QUADROTOR (MAV-VTOL)
RI Fb = −mg·E3 + RIE3 τb = −
4 P
4 P i=1
bΩ2i
+ AT (3.20)
JR (ω × E3 )·Ωi + τa + AR
i=1
Estas ecuaciones junto con las del modelo dinámico 3.19 pueden reescribirse de la siguiente forma:
ξ˙ = v v˙ = −g·E3 +RIE3 mb
4 P i=1
bΩ2i +
AT m
˙ I = RI S(ω) R 4 P
Jω˙ = −ω × Jω−
(3.21)
JR (ω × E3 )·Ωi + τa + AR
i=1
donde: AT y AR son las fuerzas y pares aerodinámicos que actúan sobre el quadrotor calculados a partir de los respectivos coeficientes, Ci , mediante la expresión Ai = 1 ρ C W 2 , donde ρaire es la densidad del aire y W es la velocidad respecto al 2 aire i aire. 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 determinado por la suma de las fuerzas de empuje generadas por cada motor fi , como se presenta en la siguiente ecuación:
T = U4 =
4 X i=1
!
fi =
4 X
!
bΩ2i
(3.22)
i=1
El vector de pares aplicados sobre el quadrotor está representado por τa y viene determinado por el esfuerzo de torsión τM generado por cada motor, considerando desacoplado
25
CAPÍTULO 3. QUADROTOR (MAV-VTOL)
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, denotada por τdrag = kr Ωi , siendo kr > 0 una constante. Así, aplicando la segunda ley de Newton: JR Ω˙ i = −τdrag + τMi
(3.23)
Las otra entrada de control es el vector de pares, determinado por:
l(f − f4 ) 2 l(f − f ) = 3 1 τa = 4 P
i=1
τMi
lb(Ω22 − Ω24 ) lb(Ω23 − Ω21 ) kr (Ω21 + Ω23 − Ω22 −
Ω24 )
donde l es la distancia del centro de masas a los motores.
l·U1 l·U2 U3
(3.24)
CAPÍTULO 3. QUADROTOR (MAV-VTOL)
26
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 diagrama de bloques desarrollado en MATLAB Simulink. El bloque que contiene la lógica de decisión consiste en una máquina de estados basada en Stateflow. Esta máquina de estados conecta con un sistema de control que proporciona las referencias necesarias para el movimiento de la plataforma, que es calculado mediante 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 plataforma. 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 controladores de estabilización. Stateflow o máquina de estados, bloque principal del proyecto, donde queda recogida la lógica de gobierno de la plataforma, y se establece el tipo de referencias que 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 paso y 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ón de 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 plataforma bajo estudio, presentado en el capítulo 2. Las entradas del bloque se corresponden con los 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 correspondientes a las derivadas de los ángulos de Tait-Bryan.
CAPÍTULO 4. DESCRIPCIÓN DE LA SOLUCIÓN
29
Figura 4.2: Ejes representativos de las coordenadas locales del quadrotor Dentro de este bloque, se pueden encontrar otros dos bloques adicionales, tal y como se 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 total en velocidad de giro los rotores al cuadrado, aplicando las saturaciones y parámetros correspondientes, configurados por el autor [3]. Se puede ver el diagrama de bloques en la figura 4.4 Quadrotor Contiene las ecuaciones de la dinámica del quadrotor que obtienen el vector de estado x a partir de la salida del Control Mixer. Esta función queda implementada en el archivo dinamica_quadrotor.m, que puede verse de forma completa en el apéndice B.1. Se ha obtenido del modelo de Peter Corke incluido en las referencias [3]. Adicionalmente, pueden añadirse modelos de perturbaciones externas, tales como viento o efecto suelo, mediante bloques propios que incluyan el modelado de estos efectos, o
CAPÍTULO 4. DESCRIPCIÓN DE LA SOLUCIÓN
30
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 plataforma multirotor a condición de que tenga como entrada los pares de actuación y el empuje total.
4.3.
Control
El conjunto de bloques relativos al control están gobernados por la máquina de estados y proporcionan las señales de actuación a la plataforma para lograr las referencias deseadas. Si bien es un elemento clave en el diseño de la solución global, para un primer desarrollo se ha optado por reutilizar controladores para plataformas de tipo VTOL disponibles en [3]. Consisten básicamente en controladores tipo PD para el control de estabilización y posición. Para un mayor detalle sobre la implementación de las leyes de control, seguir la referencia indicada [3] Típicamente, debido a la dinámica de los subsistemas que conforman el modelo de este tipo de plataformas (modelado visto en el capítulo 2), la tarea del control se descompone en control rotacional y control traslacional [2] [8] [9](alternativamente de estabilización y 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”.
CAPÍTULO 4. DESCRIPCIÓN DE LA SOLUCIÓN
31
Figura 4.5: Diagrama del modelo donde se incluyen las referencias de entrada y salida de cada bloque de control Se tiene como entrada la posición angular de referencia,[φref , θref , ψref ], proporcionada por 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 incluido en el modo de Bajo Nivel por ser un control directo entre la referencia en altura y el empuje total suministrado, T . Los bloques principales son los tipos de control implementados para “Bajo Nivel”, y el interruptor 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 “Bajo Nivel”
CAPÍTULO 4. DESCRIPCIÓN DE LA SOLUCIÓN
4.3.2.
32
Control de posición
También denominado control traslacional, se incluye en el bloque “Alto Nivel” . Toma como referencia la posición [xref , yref ] y proporciona la posición angular de referencia al control de estabilización, mediante los valores [φref , θref ], correspondientes a los denominados ángulos de roll y pitch, respectivamente. La señal correspondiente a la variable 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 las referencias de posición angular. Esta entrada se realiza para incorporar el valor de referencia del ángulo yaw, ψref , al vector de salida, [φref , θref , ψref ], ya que este valor es 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 de control implementados para Alto Nivel, un switch o interruptor que conmuta entre estos tipos de control, y otro interruptor más denominado “Switch1 ” que conmuta entre dos señales: las referencias en posición angular proporcionadas por el control traslacional o las proporcionadas directamente por la máquina de estados. La decisión de activar una de estas dos señales corresponde al hilo HI de la entrada EN, que se encarga de activar o desactivar el control de posición.
Figura 4.7: Diagrama Simulink del Control Traslacional, incluído en el bloque “Alto Nivel”
4.3.3.
Lógica de gobierno
La lógica de gobierno de las leyes de control se basa en las siguientes ideas:
CAPÍTULO 4. DESCRIPCIÓN DE LA SOLUCIÓN
33
La interfaz que presentan los bloques de control, tanto al diagrama de estados como al modelo dinámico, es similar, independientemente de las leyes que se estén ejecutando, sean del tipo lineal o no lineal, en su implementación en tiempo continuo o discreto. Esto permite la adición de nuevos bloques de control, sin modificar el diseño global (tan sólo el subsistema al que afecte), para ser evaluados y verificados. Pueden añadirse tantos modos de control como se quiera. Por ejemplo, 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 se puede observar en las figuras 4.7 y 4.6. El bus “enabled” proviene de la máquina de estados y contiene las señales que determinan qué bloques del control son activados o desactivados: en función del modo de vuelo para elegir entre control de posición o control de estabilización, y por solicitud del usuario al decidir utilizar control Lineal, No Lineal, o cualquier otro que se haya podido añadir. La lógica de gobierno de control incluída en la má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 conmutar entre 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. 4.4.1.
Stateflow Diseño
La solución propuesta en este trabajo, para diseñar e implementar una máquina de control de estados e integrarla en una plataforma de tipo quadrotor, está basada en la combinación de MATLAB Stateflow® y el software de simulación MATLAB Simulink®, muy extendido a nivel académico[4]. Stateflow es un entorno para el modelado y simulación de lógica de decisión combinatoria y secuencial basado en máquinas de estado y diagramas de flujo.
CAPÍTULO 4. DESCRIPCIÓN DE LA SOLUCIÓN
34
El diagrama Stateflow permite definir lógica de decisión compleja en el control de la plataforma de forma gráfica y estructurada, con el fin de simular la respuesta del sistema a entradas externas, eventos y condiciones temporales. En esta propuesta, existen dos elementos principales para el diseño: estados y transiciones.
Estado: Los estados representan modos de funcionamiento del sistema. En éstos se establecen valores para variables y se incluyen funciones relativas a la semántica de Stateflow. Los estados se utilizan para el diseño secuencial de diagramas de transición de estados. Pueden estar activos o inactivos, dependiendo de las condiciones que se apliquen. Los estados se organizan de forma jerárquica y pueden relacionarse entre sí mediante transiciones, que pueden incluir algún tipo de restricción o condición y permiten gestionar el paso de un estado a otro. Dos o más estados en el mismo nivel pueden trabajar de forma exclusiva (OR) o de forma paralela (AND). Los estados 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 de la transición, que unen unos estados con otros. Representan el paso de un estado origen a un estado objetivo, y pueden llevar adjuntos una etiqueta en forma de evento 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 integrar otro tipo de componentes para complementar la máquina de estado, tales como funciones de MATLAB, Simulink, código C o tablas de verdad. En este trabajo se han incluido funciones de MATLAB para las transiciones entre modos de vuelo, como se verá a continuación en la descripción de los estados y transiciones.
CAPÍTULO 4. DESCRIPCIÓN DE LA SOLUCIÓN
35
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 los estados 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ía establecidos en la figura 4.9. Las líneas discontínuas significan que los estados se ejecutan de forma paralela. El diagrama de Stateflow incluye como estados principales “Navegación” y “Modo de Control”. Ambos se ejecutan de forma paralela. En “Navegación” se encuentran todos los modos de funcionamiento, y en “Modos de Control” se incluyen los estados que regulan el tipo de control que gobierna la plataforma.
Figura 4.9: Esquema explicativo de los estados que componen el bloque Stateflow y su jerarquía
CAPÍTULO 4. DESCRIPCIÓN DE LA SOLUCIÓN
4.4.2.
36
Estados de navegación
En el bloque “Navegación” se gestiona la lógica del modo de funcionamiento solicitado para que se ejecute de forma correcta y con la máxima seguridad. Los subestados principales de este estado son los modos “Tierra”, “Aire” y “Emergencia”, descritos a continuació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 alimentació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) donde deben incluirse las comprobaciones sobre el estado de sensores y demás elementos de la plataforma 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 solicitado pertenece al estado “Aire”, con el fin de evitar bucles internos innecesarios en caso de encontrarse la plataforma en tierra y solicitar los modos “Aterrizaje” o “Emergencia”.
El modo “Tierra” tiene como predeterminado el subestado “Check”, donde se verifica que la variable ok_tierra devuelva el valor lógico “1” , y que el usuario esté solicitando el inicio del modo “Aire”, a través de la entrada al bloque STATEFLOW aire_tierra. Si se cumplen estas dos condiciones, se pasa al estado “Armado Motores”, donde se inicia el giro de los rotores y se pasa al modo “Aire”.
CAPÍTULO 4. DESCRIPCIÓN DE LA SOLUCIÓN
37
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. Consta de los modos “Despegue”, “Vuelo”, “Aterrizaje” y “To Home”.
Figura 4.12: Diagrama donde se presentan los modos de funcionamiento incluidos en el estado principal “Aire”
4.4.2.2.1. Despegue El 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 únicamente referencias en altura, manteniendo en el origen las referencias para los ángulos pitch y roll. El ángulo yaw se mantiene en el valor actual de ese instante para anular el par, ya que no se considera relevante para esta maniobra. Para realizar el despegue de forma estable 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
CAPÍTULO 4. DESCRIPCIÓN DE LA SOLUCIÓN
38
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 forma de rampa mediante la ecuación zref = zD − 0,8 ∗ (t − tD ). Esta es la ecuación de una rampa con una pendiente de valor 0.8. El signo negativo se debe a que se trabaja 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 ese instante, 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. Vuelo El 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 cuatro estados 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 constante para el empuje T . Los valores de los ángulos pitch y roll están definidos en la funcion auxiliar de MATLAB eulerRef0 = est(yaw0), en la que se adjudican los valores de referencia de posición angular.
CAPÍTULO 4. DESCRIPCIÓN DE LA SOLUCIÓN
39
Estabilización en altura Este modo es similar al anterior pero añadiendo como grado de libertad adicional controlado la altura respecto al suelo. Permite el movimiento en dirección vertical proporcionando un valor de referencia zref que puede ser una constante o una variable proporcionada 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 continuamente 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 de altura a la que se encuentra en ese instante y se proporciona una referencia en posición en el plano XY, bien constante o proporcionada por el generador de trayectorias. En este caso dicho sistema definiría una serie de waypoints que conformarían la trayectoria deseada. Este bloque permite incluir algoritmos de planificación de caminos que expanden las utilidades de la plataforma al aumentar su nivel de autonomía. En este modo se 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 correspondientes con que provienen del generador de trayectorias. A zref se le asigna el valor de z del vector de estado para mantenerla constante. El ángulo ψref toma el valor proporcionado por el gestor de trayectorias, por considerarse interesante la posibilidad de proporcionar referencias en este caso, por ejemplo, en caso de tener una cámara fija instalada 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 libertad traslacionales de la plataforma, permitiendo un movimiento libre en el espacio tridimensional. El movimiento se produce al proporcionar como referencia puntos del espacio, que tras pasar por el control de alto nivel, se convierten en referencias de posición angular. Conserva las características del modo 2 pero añade la posibilidad de variar referencias en altura. La función que ejecuta es [xyz3,yaw3] =sixDoF(ref3,yawRef3), que adjudica a [xref , yref , zref ] los valores correspondientes que provienen del generador de trayectorias. En caso del ángulo ψref , al igual que en el modo de movimiento plano, se toma la referencia externa.
CAPÍTULO 4. DESCRIPCIÓN DE LA SOLUCIÓN
40
Stop Este modo permite una transición segura entre los demás modos de vuelo, ya que obliga al vehículo a detenerse antes de cambiar de un movimiento a otro. El motivo de realizar las transiciones de esta forma es que existen dos modos de control de posición y dos modos de control de estabilización; cuando se pasa de rotacional a traslacional sin detener el vehículo, se anulan las referencias en posición angular pero la inercia que lleva el vehículo provoca que la transición sea inestable, y por tanto se hace necesario detenerlo. Para ello se hace uso de las funciones stop y ok_stop, explicadas en el siguiente apartado.
Figura 4.14: Diagrama Stateflow del modo “Vuelo”
4.4.2.2.3. Aterrizaje Este 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 como referencia en posición lineal la posición de la plataforma en ese instante. Se ejecuta al comienzo del estado. Durante el periodo que permanece activo este estado se ejecuta la función ok_A=ok_stop(estado,xyz), que se encarga de verificar que el error entre la referencia y la posición actual es un número menor que una cierta tolerancia establecida. Si esto es correcto, ok_A toma el valor lógico “1” y permite pasar al siguiente estado.
CAPÍTULO 4. DESCRIPCIÓN DE LA SOLUCIÓN
41
2. A_2: Referencia en altura. Se proporciona una referencia en rampa para la variable z de tal forma que el aterrizaje sea progresivo, suave y seguro. A la entrada del estado, se toman los valores [tA , zA ]correspondientes al tiempo y la altura en dicho instante. A partir de éstos, se elabora la referencia en rampa mediante la ecuación zref = zA + 0,8·(t − tA ), correspondiente a una rampa con pendiente de magnitud 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 instante se determina zref = 0, para no proporcionar valores de altura bajo el suelo. Lo deseable en la implementación sería proporcionar una referencia descendente en altura mediante la utilización de sensores de tipo sónar que proporcionan la altura actual de la plataforma. Esta condición podría considerarse para un trabajo futuro. 4. A_4: Cuando el valor de z pasa a ser 0, se entiende que ha tocado el suelo, e inicia este estado que sirve de transición al estado “Tierra”.
Figura 4.15: Diagrama Stateflow del modo “Aterrizaje”
4.4.2.2.4. ToHome Este modo permite volver al punto inicial desde donde despegó la plataforma, con la altura de referencia de despegue, hdesp . Consiste en proporcionar como referencia el valor inicial de las posiciones x e y, que por tratarse de ejes horizonte local son valores nulos. La presencia de este modo se justifica para casos en los que el quadrotor se ha desplazado más allá de la línea de visión y se pretende que vuelva o simplemente para aterrizar en el punto de partida, para lo cual sería necesario ejecutar el modo
CAPÍTULO 4. DESCRIPCIÓN DE LA SOLUCIÓN
42
Aterrizaje cuando el quadrotor se encuentre sobre el origen. Se podría haber incluido el aterrizaje de la plataforma dentro de este modo, pero se ha considerado innecesario por redundancia con el modo Aterrizaje. La referencia en yaw se establece como la que tiene 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 el vehículo en lugar en que se encuentre siempre que el usuario lo solicite o el sistema de forma autónoma detecte un error que, previamente programado, considere como emergencia. 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 la velocidad de los rotores. Un ejemplo de este caso puede ser la rotura de una o varias de las hélices, caso en el cual para un aterrizaje estable debería implementarse un control adaptativo que permitiese el control de la plataforma mientras rota sobre sí misma. [10] El estado “Emergencia” se encuentra fuera del estado principal “Aire” para permitir aterrizar en cualquiera de los modos de vuelo descritos con anterioridad si se encuentra algun fallo.
CAPÍTULO 4. DESCRIPCIÓN DE LA SOLUCIÓN
43
El ángulo yaw se mantiene el valor que tenga en ese instante, ya que no se considera importante para este modo. Los subestados que forman el modo “Emergencia” son similares al modo de vuelo “Aterrizaje”: 1. E_1: Se detiene al vehículo utilizando la función xyz=stop(estado) , que proporciona como referencia en posición lineal la posición en ese instante. Durante la permanencia 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 que una 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 forma que se consiga un aterrizaje suave. Inicialmente se toman los valores tE , zE correspondientes al tiempo y la altura en ese instante. Con estos valores se calcula la referencia en altura mediante la expresión zref = zE + 0,8·(t − tE ), correspondiente a una rampa cuya pendiente toma el valor 0.8 positivo. Este signo se debe al sistema de referencia de ejes locales. 3. E_3: Cuando la referencia supera el valor zref > 0 se inicia este estado, y se determina zref = 0. 4. E_4: Cuando z toma el valor 0, se inicia este estado que sirve de transición al estado “Tierra”.
4.4.3.
Estados de control
El estado Modo de Control gobierna el nivel control que se utiliza, y dentro de cada nivel, el tipo de control implementado. Este estado recibe información de la señal altonivel y de la estructura tipo. Se comunica con el bloque control mediante la estructura enable. 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, distinguidos como niveles de control:
CAPÍTULO 4. DESCRIPCIÓN DE LA SOLUCIÓN
44
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ón siempre permanece activo, ya que al proporcionar referencias en posición, el control de posición calcula las referencias de posición angular que deben ser interpretadas por el control 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 se activa 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 estabilización. [1] ALTO NIVEL. Se activa el control de posición, que envía referencias en posición angular al control de estabilización.
CAPÍTULO 4. DESCRIPCIÓN DE LA SOLUCIÓN
45
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 como el Aterrizaje. Ésto se debe a la necesidad de parar la plataforma antes de continuar el aterrizaje para mantener la estabilidad. El despegue no se incluye en el control de posició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 de posición son: Movimiento en el plano Movimiento en el espacio 3D Aterrizaje To Home Emergencia Stop
CAPÍTULO 4. DESCRIPCIÓN DE LA SOLUCIÓN 4.4.3.2.
46
Tipos de control
Dentro de cada uno de los estados BAJONIVEL y ALTONIVEL, se incluyen los respectivos tipos de control que se hayan implementado. Cabe destacar que debido al carácter independiente de los bloques de control utilizados, se permite añadir tantos tipos de control como se quiera, tanto para Alto Nivel, como para Bajo Nivel, siempre teniéndolos en cuenta en la lógica Stateflow, tal y como se ha comentado en la subsección 4.3. En este trabajo sólo se ha implementado el bloque de control “Lineal”, tanto para control de estabilización como para control de posición, con el objetivo de comprobar la lógica implementada, y además, se ha añadido otro bloque denominado “No Lineal” a modo de ejemplo de utilizar varios tipos de control, aunque su contenido es el mismo que el del bloque “Lineal”.
tipo: Es una estructura de MATLAB donde quedan almacenados las peticiones del usuario sobre el tipo de control que debe utilizarse. • tipo.HI: Recoge la petición del usuario sobre el control de posición, incluido 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 posibilidad de hacerlo de dos formas: Mediante un estado, con transiciones, o mediante una tabla de verdad. Se eligió la primera por la visualización constante durante la simulación del modo de control elegido. La variable de salida que comunica con el bloque de control es el bus enable (e), cuyas componentes son:
CAPÍTULO 4. DESCRIPCIÓN DE LA SOLUCIÓN
47
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). Pueden tomar 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 describen los 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á activo, se proporcionan referencias en posición, que el control de alto nivel transforma en referencias en posición angular. Por tanto, como se ha indicado al principio
CAPÍTULO 4. DESCRIPCIÓN DE LA SOLUCIÓN
48
de este apartado, el control de bajo nivel o control de estabilización permanece activo siempre. Es por ello que debe ser contemplado en los estados del control de alto nivel. • ALTO_LINEAL Control de posición lineal ◦ BAJOLINEAL1 Control de estabilización lineal cuando está activo el control 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 el control 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 de un 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ón ló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 motores. Para llegar hasta este último punto es necesario obtener un valor ’1’ en la función
CAPÍTULO 4. DESCRIPCIÓN DE LA SOLUCIÓN
49
ok_tierra=check(modo). La función check es un script de MATLAB integrada en el bloque Stateflow donde deben incluirse una serie de condiciones imprescindibles para el arranque 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 comprobaciones no se incluyen en este trabajo por no haberse implantado aún en un modelo concreto, 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 usuario solicita el modo de vuelo. Las transiciones son: Despegue - Vuelo: Se realiza de forma directa una vez que el quadrotor alcanza la altura de despegue definida. Vuelo - Aterrizaje: Cuando se solicita de forma externa el modo Aterrizaje se inicia este estado descrito en 4.4.2.2.3. Vuelo - To Home: El modo To Home se inicia de la misma forma que el estado Aterrizaje. 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 valores consecutivos del 1 al 7, aunque los tres últimos se corresponden respectivamente con Aterrizaje, To Home y Emergencia. En este apartado se tienen en cuenta los cuatro primeros: 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 o control de estabilización, podrían encontrarse problemas de estabilidad en la transición entre ellos. Por tanto la conmutación debe hacerse de forma cauta y con garantías, es por ello que se ha añadido el modo adicional Stop con el fin de establecer una transición
CAPÍTULO 4. DESCRIPCIÓN DE LA SOLUCIÓN
50
segura en tiempo finito. Una vez verificada la estabilidad, se procede a comprobar en cada caso los siguientes parámetros mediante una función denominada ok_stop, que depende de la posición angular y de las referencias en posición, ya que su objetivo es comprobar 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 comprobar el estado de todos los dispositivos críticos de la plataforma y enviar la señal de emergencia si alguno de ellos presenta algún fallo (en este trabajo, emergencia se establece designando el valor ’7’ a la variable modo). También puede ser solicitado de forma externa 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 toque el suelo. Esta condición se comprueba de forma interna en cada uno de ellos y, una vez aceptada, se conmuta al estado Tierra para las posibles comprobaciones por si la plataforma 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 de los 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 altonivel, definida en cada modo de vuelo, la que determina si nos encontramos únicamente con 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 determina si debe ser activado el control lineal, el control no lineal, o cualquier otro que se
CAPÍTULO 4. DESCRIPCIÓN DE LA SOLUCIÓN
51
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 otros tipos 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 los distintos modos de vuelo y para verificar la respuesta de la máquina de estado ante referencias externas que varían con el tiempo. Actualmente el generador de trayectorias es una parte fundamental de una plataforma tipo MAV, ya que le proporciona mayor autonomía y versatilidad.
Un detalle a tener en cuenta a la hora de generar trayectorias es que las plataformas de ala rotatoria tipo VTOL (Despegue Vertical) resultan mucho más precisas a la hora de 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 «Generador de trayectorias» incluido en el diagrama principal de Simulink, el cual tiene como 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 instante en forma de vector dado en ejes horizonte local, que servirá de entrada a la máquina de estados para proporcionar referencias al controlador de posición, al controlador de estabilizació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 denominada Secuenciador de waypoints que toma la matriz Pnx3 , con n waypoints de 3 coordenadas,
CAPÍTULO 4. DESCRIPCIÓN DE LA SOLUCIÓN
52
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 y Movimiento en el espacio, respectivamente..
Esta función calcula constantemente el error de distancia ε entre el waypoint deseado y el valor de posición del vector estado según el modo de vuelo, con las siguientes ecuaciones: Estabilización en altura: ε = |z − zref | Movimiento en el plano: ε =
q
Movimiento en el espacio: ε =
(x − xref )2 + (y − yref )2
q
(x − xref )2 + (y − yref )2 + (z − zref )2
ε = 1 si el modo no es ninguno de los anteriores. En ese caso el “Generador de trayectorias” no es utilizado.
Cuando ε es menor que un cierto valor (según la precisión que se requiera, a menor valor las trayectorias serán más precisas y lentas, a mayor valor tendrán mayor continuidad y serán más rápidas) se pasa al siguiente waypoint, que se corresponde con la siguiente fila de la matriz P. Así, el «Secuenciador de waypoints» pasará por todos los puntos hasta 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 implementar 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 puntos ligados a ejes horizonte local. Sin embargo, las plataformas aéreas autónomas disponen generalmente de receptores de posicionamiento global, como GPS. Estos dispositivos trabajan con coordenadas geográficas (latitud y longitud), por tanto se precisa de una función que adapte este tipo de coordenadas a los ejes con los que se trabaja en el modelo que aquí se presenta.
CAPÍTULO 4. DESCRIPCIÓN DE LA SOLUCIÓN
53
Figura 4.21: Vista del contenido del bloque «Generador de trayectorias» Para ello, MATLAB cuenta con la función P = lla2f lat(LLA, LL0, P SI0, HREF, M ODEL) que transforma una matriz LLA de puntos en ejes geográficos en una matriz P en ejes horizonte local, donde: LLA es una matriz de dimensión n x 3 con n waypoints y 3 columnas correspondientes 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 norte geográfico, que en este caso se toma como 0, aunque más tarde se deben permutar las 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ón sobre el modelo ’WGS84’ consultar el apéndice A.
CAPÍTULO 4. DESCRIPCIÓN DE LA SOLUCIÓN
54
También puede hacerse directamente a partir de un software de información geográfica que permita seleccionar puntos y exportarlos a un archivo. En este trabajo se ha utilizado el software Google Earth® [5]. Éste permite seleccionar una ruta mediante puntos y guardarlos en un archivo de extensión ’*.kml’. Con este archivo, se puede exportar a una 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 ejecuta en MATLAB previamente a la simulación mediante el comando P = wp(archivo), donde ’archivo’ es el nombre del fichero de extensión ’*.kml’. El contenido de este fichero se aporta en el apéndice B.4
Figura 4.22: Ruta circular en Google Earth® situada en el edificio de laboratorios de la Escuela Superior de Ingenieros de Sevilla
CAPÍTULO 4. DESCRIPCIÓN DE LA SOLUCIÓN
55
CAPÍTULO 4. DESCRIPCIÓN DE LA SOLUCIÓN
56
Capítulo 5 Experimentos y validación 5.1.
Introducción
En este capítulo se mostrarán resultados de simulaciones realizadas en distintos casos supuestos, para verificar la validez de todos los modos de vuelo y la correcta configuració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 que determina los valores de entrada para poder realizar la simulación. El contenido del script está disponible en el apéndice B. Su única entrada es el nombre del archivo .kml generado 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 el instante al que va asociado. Se crea mediante la herramienta gráfica GUI de MATLAB 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 si quiere elegir otro. En este caso se otorgan 10 segundos de simulación a cada modo seleccionado, como ejemplo de simulación. Se pueden seleccionar tantos como se quieran. 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, donde se indica el tipo de control que gobierna en el control de alto nivel y en el de bajo nivel, 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 llevar el quadrotor al aire. Se pregunta mediante GUI, aunque para visualizar todo lo desarrollado en este trabajo debe estar en la posición “Aire”.
CAPÍTULO 5. EXPERIMENTOS Y VALIDACIÓN
59
T: tiempo de simulación. Se calcula según el número de modos de vuelo seleccionados, 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 correspondan a las coordenadas x y z y las columnas a cada waypoint. Se puede hacer de forma manual u obtenerla mediante la función waypoints.m, mediante el comando P=wp(’nombrearchivokml’) o P=wp_interp(’nombrearchivokml’) si se desea además interpolar los puntos. Para obtenerlo de esta segunda forma, es necesario haber generado el archivo *.kml en Google Earth, generando una secuencia de puntos con el botón “Generar Ruta” de la barra de herramientas. Una vez creado, guardarlo. 2. Ejecutar el archivo modos.m y seleccionar los modos de vuelo, el tipo de control para control de estabilización y para control de posición, la opción Aire para poder volar. 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 incluyen una gráfica superior donde están representadas las variables modo y modoR. Estas variables representan, respectivamente, el modo de vuelo solicitado por el usuario y el modo real que se encuentra activo en cada momento. Para entender estas variables, se muestra a continuación del valor que lleva asociado cada modo de vuelo, idéntico para las dos. -1 Tierra 0 Despegue 1 Estabilización
60
CAPÍTULO 5. EXPERIMENTOS Y VALIDACIÓ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, detallado en el apéndice B. Con él, se obtienen las gráficas necesarias para el análisis de los datos proporcionados por la plataforma. Para interpretar las gráficas, se aclara que las unidades de medida son segundos, para el tiempo, metros, para las coordenadas de posición lineal, y radianes, para las coordenadas angulares.
5.3.1.
Despegue, estabilización y estabilización en altura
En esta simulación se ejecutan únicamente los modos de “Estabilización” y “Estabilización en altura”, por lo que la plataforma únicamente toma referencias en altura para el segundo caso. Los valores de las referencias son: Waypoint zref
1 -5
2 -3
3 -7
4 -3
5 -5
6 -3
7 -8
8 -3
9 -7
10 -3
Cuadro 5.1: Simulación 1: referencias para la variable z.
CAPÍTULO 5. EXPERIMENTOS Y VALIDACIÓN
61
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 los primeros 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 recorrer una trayectoria en forma de estrella. Al tratarse de movimiento en el plano, la referencia en altura es constante. La trayectoria no llega a tocar a los puntos de paso por el radio de tolerancia permitido, establecido en 0.1m en este caso.
CAPÍTULO 5. EXPERIMENTOS Y VALIDACIÓN
62
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 angular a lo largo del tiempo. En ambas se compara con el valor de la variable relacionada con el modo de vuelo, que nos indica que parte de tierra, inicia el despegue y una vez finalizado comienza el modo de movimiento en el plano. La línea roja de esta gráfica es el estado real que está ejecutando la plataforma y la azul es el modo solicitado de forma externa.
Figura 5.5: Valores de posición lineal a lo largo del tiempo para la simulación 2.
63
CAPÍTULO 5. EXPERIMENTOS Y VALIDACIÓN
El valor de z real no permanece constante debido a las variaciones de altura que sufre el quadrotor al realizar el desplazamiento, pero se compensa con la referencia fija para esta 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 desplazamiento en los ángulos pitch y roll, y la respuesta ante una referencia en escalón para el ángulo yaw, permitida en este modo de vuelo. El controlador PD implementado provoca una respuesta subamortiguada. Las referencias proporcionadas aparecen en la tabla 5.2. Waypoint xref yref
1 4 0
2 1 1
3 0 4
4 -1 1
5 -4 0
6 -2 -1
7 -3 -5
8 0 -2
9 3 -5
10 2 -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 tres dimensiones espaciales.
64
CAPÍTULO 5. EXPERIMENTOS Y VALIDACIÓN
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 xref yref zref
1 4 0 -5
2 1 1 -3
3 0 4 -7
4 -1 1 -3
5 -4 0 -5
6 -2 -1 -3
7 -3 -5 -8
8 0 -2 -3
9 3 -5 -7
10 2 -1 -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, la platarforma realiza el despegue vertical, seguido de la estabilización. A continuación inicia el movimiento siguiendo los puntos de paso numerados en la tabla 5.3.
CAPÍTULO 5. EXPERIMENTOS Y VALIDACIÓN
5.3.4.
65
Despegue, movimiento en el plano, aterrizaje, despegue y estabilización
En este caso se realiza una simulación para comprobar el funcionamiento del modo de aterrizaje y posterior despegue de la plataforma. Para ello, usando los mismos puntos de paso de las simulaciones anteriores, recogidos en la tabla 5.3, se solicita como primer modo el “movimiento en el plano”, para después aterrizar la plataforma. El siguiente modo 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 con respecto al tiempo. Se puede observar que sobre el instante correspondiente a t=26s el modo real desciende al valor -1, lo que implica el aterrizaje. No es hasta el instante t=30s, cuando se solicita el modo “estabilización” cuando la plataforma comienza a despegar.
CAPÍTULO 5. EXPERIMENTOS Y VALIDACIÓN
66
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 despegue desde el origen, punto verde, hasta la altura de despegue, 5 m. Ahí comienza el vuelo plano hasta el instante del aterrizaje, que coincide cerca del waypoint 3. A partir de aquí el movimiento es únicamente vertical, ya que después del aterrizaje la plataforma vuelve a despegar y se mantiene estabilizada.
5.3.5.
Despegue, estabilización, movimiento en el espacio, ToHome y aterrizaje
El objetivo de esta simulación es comprobar el funcionamiento del modo To Home, que tal como se vió en el capítulo 4, se corresponde con un modo de vuelo que permite volver 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 las tres dimensiones espaciales y cuando se solicite el modo “To Home” volverá al punto inicial, con la altura determinada para el despegue.
CAPÍTULO 5. EXPERIMENTOS Y VALIDACIÓN
67
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 vuelo siguiendo los puntos de paso, hasta llegar al cuarto, instante en el que se ha solicitado el modo “To Home”. La plataforma acude al origen de coordenadas y se mantiene hasta
CAPÍTULO 5. EXPERIMENTOS Y VALIDACIÓN
68
que se inicia el aterrizaje.
5.3.6.
Despegue, estabilización, movimiento en el plano y emergencia
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 de vuelo. 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.
CAPÍTULO 5. EXPERIMENTOS Y VALIDACIÓN
69
Figura 5.14: Trayectoria 3D para la simulación 6 La gráfica tridimensional permite observar como en el instante en que se solicita el modo “Emergencia” la plataforma comienza a detenerse y retrocede para cumplir con la referencia en posición que se da al principio de este modo. Este retroceso se debe a que la frenada no es instantánea, requiere de en torno a unos 2 o 3 segundos, según se aprecia en la gráfica de la coordenada x, a partir del instante t=30. En cuanto el quadrotor se encuentra parado, comienza el aterrizaje y llega a tierra.
5.3.7.
Despegue, estabilización, movimiento en el espacio y emergencia
En esta ocasión, se pretende comprobar el comportamiento del modo emergencia. Se ha cambiado la trayectoria de referencia por una helicoidal, determinada por el archivo trayectoriaHelicoidal.m, incluido en el apéndice B.
CAPÍTULO 5. EXPERIMENTOS Y VALIDACIÓN
70
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 en este caso las referencias no son escalones sino valores contínuos, lineales para z y no lineales 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 verde) hasta la altura de despegue, z=5m, donde inicia la estabilización. En el instante
CAPÍTULO 5. EXPERIMENTOS Y VALIDACIÓN
71
t=10s se solicita el modo “Movimiento en el espacio”, a partir del cual la plataforma inicia el recorrido helicoidal. En el instante t=50s se solicia el modo “Emergencia” y la plataforma desciende a tierra.
5.3.8.
Despegue, estabilización, movimiento en el espacio, movimiento en el plano, movimiento en el espacio y “ToHome”.
Esta simulación aumenta la duración con respecto a las anteriores, con el objetivo de poder simular los movimientos en el plano y el espacio de forma alterna para probar su conmutación. Finalmente se solicita el modo “To Home” para el regreso de la plataforma al 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. El cambio se produce en la variable z, donde se observa que en el modo plano permanece constante.
CAPÍTULO 5. EXPERIMENTOS Y VALIDACIÓN
72
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 del despegue, el verde al movimiento espacial, y el cyan al movimiento plano. El rojo sigue siendo la referencia helicoidal creada.
5.3.9.
Todos los modos
Para una simulación completa, se creará una trayectoria mediante puntos seleccionados en mapa, para lo cual se va utilizar la herramienta programada para tomar puntos en Google Earth. Se recuerda que para ello es imprescindible la función wp mediante el código P=wp(’archivo’), donde ’archivo’ es el nombre del archivo de extensión *.kml generado por Google Earth. Así, se obtendrá una matriz de puntos de paso denominada P. Si se desea obtener una trayectoria mucho más definida, con mayor cantidad de puntos, 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 las coordenadas x e y de estos puntos. Sin embargo, es necesario crear las referencias en altura z. En este caso, este archivo tiene una parte donde se genera esta referencia, a razón de añadir 1 metro de altura por cada waypoint que pase, partiendo de 3 metros para el primero.
CAPÍTULO 5. EXPERIMENTOS Y VALIDACIÓN
73
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 que permanecen 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 simulaciones anteriores. La ruta seleccionada en Google Earth se ha obtenido ejecutando el comando googleEarth el cual llama al archivo del mismo nombre y toma los valores x,y y z y los convierte a coordenadas geográficas mediante la función flat2lla, que convierte coordenadas en el
CAPÍTULO 5. EXPERIMENTOS Y VALIDACIÓN
74
plano tangente local a geográficas, aportando un punto de origen, en este caso O. La trayectoria final puede verse en la imagen 5.20.
Figura 5.20: Representación en Google Maps de la trayectoria generada por el quadrotor en 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 un simulador de vuelo. FlightGear es un simulador de código abierto, que permite visualizar el vector de estado de la plataforma de una forma gráfica gracias a sus múltiples opciones de conexión. En este caso, se dispone del bloque “FlightGear Preconfigured 6DoF Animation” del Toolbox Aerospace Simulink, cuyas entradas son longitud, latitud, altitud, y posición angular (pitch, roll y yaw).
CAPÍTULO 5. EXPERIMENTOS Y VALIDACIÓN
75
CAPÍTULO 5. EXPERIMENTOS Y VALIDACIÓN
76
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 y validación de sistemas de control de vuelo tipo MAV-VTOL, y más concretamente para helicópteros de tipo quadrotor. Se ha hecho un estudio de la dinámica y las ecuaciones que modelan su comportamiento físico, aunque la implementación de este apartado no se ha desarrollado por no ser objeto principal, al igual que el diseño de sistemas de control, 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áquina de estados que gobierne los sistemas de control, permita realizar simulaciones y obtener resultados analizables con objeto de validar dichos sistemas. Para ello, se ha estimado oportuno llevar a cabo el desarrollo de esta máquina de estados en el software MATLAB Stateflow, muy indicado para este tipo de diseños, por su sencillez de trabajo gracias al entorno 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 de esta herramienta como sistema de validación. Las pruebas realizadas ofrecen resultados de todas las variables que componen este sistema, y gracias a MATLAB, la gestión de estos datos y su tratamiento es relativamente simple. Por eso, se considera que este diseño puede ser válido tanto para la validación de soluciones desarrolladas en el ámbito de la investigación, como para ofrecer soluciones académicas para prácticas y trabajos relacionados 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 este trabajo se ha incluído parte de su contenido en un artículo publicado en las XXXV Jornadas de Automática celebradas en Valencia entre los días 3 y 5 de Septiembre de 2014 [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 la realidad. Para ello se pueden añadir otros bloques que implementen perturbaciones externas como el viento o efectos propios de este tipo de plataformas, como el efecto suelo.
6.2.2.
Bloque de control
El diseño de esta solución tiene como característica principal en el bloque control la posibilidad de añadir tantos tipos de control como se quiera para poder conmutar entre ellos, incluso durante la simulación. Sin embargo, por no ser objeto fundamental de este trabajo, no se ha implementado más que un control lineal a efectos de realizar las pruebas pertinentes. Cabe la posibilidad de expandir su utilidad agregando nuevos tipos 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 resultan imprescindibles para este tipo de plataformas, sin embargo, queda como trabajo a desarrollar la implementación de nuevos estados que amplíen la funcionalidad de este diseño.
CAPÍTULO 6. CONCLUSIONES Y TRABAJO FUTURO
6.2.4.
79
Implementación hardware
El objetivo principal del diseño planteado en este trabajo consiste en la validación de esta máquina de estados antes de ser implantada en hardware real. El hecho de que se 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, una ventaja significativa es el hecho de que no es necesaria la adquisición previa de ninguna plataforma real, pudiendo así evitar una compra anticipada del hardware que tras el tiempo 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 mediante MATLAB, es hacer uso de las herramientas de compilación cruzada para plataformas 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 herramienta Matlab Embedded Coder y el toolbox Real-Time Workshop, es posible generar código C/C++ optimizado para hardware con recursos limitados. En este caso, una de las líneas abiertas del presente trabajo consiste en la programación de placas de control Ardupilot, las cuales integran el conjunto de sensores necesarios para la estimación del estado, 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 es lo 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ás avanzado y potente, como son los procesadores ARM, de bajo consumo y gran potencia.
CAPÍTULO 6. CONCLUSIONES Y TRABAJO FUTURO
80
Dichos procesadores proporcionan gran capacidad de cálculo gracias a las unidades de punto 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 las señales correspondientes a la medida de los sensores, así como bloques que verifiquen la 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 su futura implantación en hardware real, también es posible incluir en las simulaciones un prototipo de la plataforma real, mediante software compatible. Existen unas librerías desarrolladas para la placa de control Arduino [13], las cuales proporcionan el conjunto de ficheros y librerías necesarias para ser ejecutadas en simulación, sin necesidad alguna de hardware objetivo. Esto permite, adicionalmente, la ejecución en tiempo real del software 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ón del 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 2 Target Utilizando este tipo de software, es posible además el cierre del lazo de verificación y validación incluyendo, además el hardware de control. En este caso, el conjunto de 1
Microelectromechanical systems, Sistema Microelectromecánico.
CAPÍTULO 6. CONCLUSIONES Y TRABAJO FUTURO
81
pruebas a realizar estarían enmarcadas dentro del conjunto de pruebas denominadas hardware-in-the-loop.
CAPÍTULO 6. CONCLUSIONES Y TRABAJO FUTURO
82
Bibliografía [1] Real Decreto-ley 8/2014, de 4 de julio, de aprobación de medidas urgentes para el crecimiento, la competitividad y la eficiencia.«BOE» núm. 163, de 5 de julio de 2014. Páginas 52553 a 52555 [2] Raffo, G.V., (2011) Robust Control Strategies for a quadrotor helicopter. An underactuated mechanical system. Tesis Doctoral. Escuela de Ingenieros. Universidad de Sevilla. [3] Corke, P., (2011) Robotics, Vision and Control. Fundamental Algorithms in MATLAB®, 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 an Indoor Micro Quadrotor. En Proc. IEEE Int. Conf. on Rob. and Automat., volume 5, pp 4393–4398, New Orleans, USA. [9] Castillo, P., Lozano R., y Dzul, A.E., (2005) Modelling and Control of Mini-Flying Machines, Springer. 83
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 Platform https://projects.ardrone.org/ [12] DIY Drones http://diydrones.com/ [13] Plataforma Arduino http://arduino.cc/ [14] Plataforma Arducopter http://copter.ardupilot.com/ [15] TrackDrone Lite http://cpoh.upv.es/es/investigacion/software/item/ 236-trackdrone-lite.html [16] AR.Drone Simulink Development-Kit http://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 de Máster. [18] Actas de las XXXV Jornadas de Automática, 3-5 de septiembre de 2014, Valencia ISBN-13: 978-84-697-0589-6 © 2014 Comité Español de Automática de la IFAC (CEA-IFAC)
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 su localización. Para determinar su posición, precisa establecer un sistema de referencia. Dependiendo del objetivo que se pretenda alcanzar, se pueden utilizar distintos modelos de la Tierra. Desde algunos muy simplificados, como por ejemplo modelos de Tierra plana para Mecánica del Vuelo si se consideran distancias relativamente cortas, a otros mucho mas sofisticados, que intentan representar la superficie topográfica de la Tierra de forma real. Esta representación resulta imposible, y es por eso que se utilizan superficies ideales, matemáticas, admitiendo que la Tierra se parece a ellas de forma más o menos aproximada. 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 base al 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 manejable y suficientemente preciso para ser útil. Para definir un elipsoide son necesarios dos pará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− aunque en las tablas se suele dar
rp re
1 f
Otra alternativa es la excentricidad:
e=
v u 2 u t1 − rp
re2
WGS84 El WGS84 o Sistema Geodésico Mundial 1984 es un elipsoide de referencia aceptado como estándar en todo el mundo. Es el que se usa en el sistema GPS, por eso aunque los UAV normalmente tienen radios de acción pequeños, si se desea utilizar este sistema de localización se debe admitir este modelo terrestre.
Sus valores característicos son: a = 6378137,0 metros 1 = f
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 ecuaciones presentadas 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 % - 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
91
APÉNDICE B. CÓDIGOS DE PROGRAMACIÓN
% 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);
92
APÉNDICE B. CÓDIGOS DE PROGRAMACIÓN % % 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];
93
APÉNDICE B. CÓDIGOS DE PROGRAMACIÓN
94
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
APÉNDICE B. CÓDIGOS DE PROGRAMACIÓN
95
% 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
APÉNDICE B. CÓDIGOS DE PROGRAMACIÓN
96
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
APÉNDICE B. CÓDIGOS DE PROGRAMACIÓN
97
% 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 %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 -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.
APÉNDICE B. CÓDIGOS DE PROGRAMACIÓN 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