N° tesis:
PROYECTO FIN DE CARRERA
Presentado a
LA UNIVERSIDAD DE LOS ANDES
FACULTAD DE INGENIERÍA
DEPARTAMENTO DE INGENIERÍA ELÉCTRICA Y ELECTRÓNICA
Para obtener el título de
INGENIERO ELECTRÓNICO
por
Andrei Alberto Cuenca Demidova
Diseño e implementación de un controlador de estabilidad para un
Cuadricoptero
Sustentado el día 9 del mes de Diciembre del año 2015 frente al jurado:
Composición del jurado
- Asesor: José Fernando Jiménez Vargas, Profesor Asociado, Universidad de Los Andes.
Diseño e implementación de un controlador de estabilidad para un Cuadricoptero
2
Contenido
1. INTRODUCCION ... 5
2. OBJETIVOS ... 5
2.1 Objetivo General ... 5
2.2 Objetivos Específicos ... 5
3. DESCRIPCIÓN DE LA PROBLEMÁTICA Y JUSTIFICACIÓN DEL TRABAJO ... 5
4. MARCO TEORICO, CONCEPTUAL E HISTORICO ... 5
4.1 Marco Teórico y conceptual ... 6
4.2 Marco Histórico ... 10
5. REQUERIMIENTOS, ESPECIFICACIONES Y RESTRICCIONES DEL TRABAJO ... 11
6. METODOLOGIA DE TRABAJO ... 11
6.1 Plan de trabajo ... 11
6.2 Búsqueda de información ... 12
6.3 Alternativas de desarrollo ... 12
7. TRABAJO REALIZADO ... 12
7.1 Modelo Matemático del Cuadricoptero ... 12
7.2 Diseño del sistema de Control ... 17
7.3 Adquisición y procesamiento de datos de los sensores ... 22
7.4 Modulo de comunicación I2C para adquisición de datos del MinIMU9 V-3 en FPGA ... 27
8. VALIDACION DEL TRABAJO ... 31
8.1 Metodología de prueba ... 31
8.2 Validación de los resultados del trabajo ... 31
9. DISCUSIÓN ... 40
10. CONCLUSIONES ... 41
11. AGRADECIMIENTOS ... 41
12. REFERENCIAS ... 41
13. APENDICES ... 44
Diseño e implementación de un controlador de estabilidad para un Cuadricoptero
3
Índice de figuras
Figura 1. Ángulos de Tiat-Bryan sobre el marco de referencia del cuerpo ... 6
Figura 2. Esquema General de un controlador PID con lazo cerrado ... 7
Figura 3. Algoritmo del Filtro de Kalman ... 9
Figura 4. Esquema del filtro complementario ... 9
Figura 5 Diagrama de fuerzas del cuadricoptero. [1] ... 13
Figura 6. Modelo del cuadricoptero para el cálculo de inercias y valores estimados. ... 19
Figura 7 Esquema del sistema de control. ... 20
Figura 8 Esquema del controlador PD ... 21
Figura 9 Esquema del modelo matemático del cuadricoptero ... 21
Figura 10 Esquema del controlador I-PD ... 22
Figura 11. Modelo en Simulink del giroscopio ... 24
Figura 12. Modelo en Simulink del Acelerómetro. ... 24
Figura 13 Esquema de integración multisensorial para el cálculo de Pitch, Roll y Yaw ... 24
Figura 14 Modulo en simulink de toma y procesamiento de datos ... 27
Figura 15. Máquina de estados para inicialización de Sensores ... 30
Figura 16. Máquina de estados para toma de datos por sensor ... 30
Figura 17. Respuestas de los controladores PD e I-PD en el Roll, Pitch y Yaw. ... 32
Figura 18. Respuestas de controladores PD, Inverse Dynamics y Backstepping presentados en el documento [8] ... 33
Figura 19. Comparación entre los Controladores PD, I-PD y un controlador PID presentado en el documento [9], en una acción en tiempo Real. ... 33
Figura 20. Señales de entrada generadas para los modelos del Acelerómetro eje X, acelerómetro eje Y, y giroscopio eje X respectivaente. ... 34
Figura 21. Datos obtenidos y procesados a partir de los generadores de señales presentados en la Figura 20. ... 35
Figura 22. Datos recolectados No calibrados mediante Matlab y Arduino. ... 36
Figura 23. Datos recolectados y calibrados mediante Matlab y Arduino. ... 36
Figura 24. Comparación entre las diferentes estimaciones del pitch y roll entre todos los métodos propuestos... 37
Figura 25. Cálculo del Yaw para comprobacion de funcionamiento ... 38
Figura 26. Montaje de Calibración y verificación de mediciones ... 38
Figura 27. Datos de verificación en tiempo real sobre variaciones controladas del Roll, Pitch y Yaw. ... 39
Figura 28. Simulación en GTKwave del módulo de comunicación I2C y adquisición de datos realizados en Verilog. ... 40
Diseño e implementación de un controlador de estabilidad para un Cuadricoptero
4
Índice de Tablas
Tabla 1. Estructura para escribir un byte desde maestro ... 28
Tabla 2. Estructura para escribir múltiples bytes desde maestro ... 28
Tabla 3. Estructura para leer un byte de un esclavo ... 28
Tabla 4. Estructura para leer múltiples bytes de un esclavo ... 28
Tabla 5. Direcciones de los registros internos de mayor importancia del L3GD20H [12] ... 28
Tabla 6. Direcciones de los registros internos de mayor importancia del LSM303D [11] ... 29
Diseño e implementación de un controlador de estabilidad para un Cuadricoptero
5
1. INTRODUCCION
Los vehículos aéreos no tripulados son una herramienta para el ser humano que han presentado múltiples utilidades en diferentes áreas. Desde el sector de ingeniería civil, aeroespacial e industrial, hasta el campo de la medicina y sector militar, los vehículos aéreos no tripulados han demostrado ser elementos versátiles que facilitan la realización de diversas actividades.
Debido a que el desarrollo de vehículos no tripulados es costoso en la actualidad, donde las tecnologías autónomas y robóticas están en auge, se ha visto la necesidad de diseñar nuevos modelos eficientes a menores costos. Además, al observar las capacidades de este tipo de dispositivos, se ha generado un incentivo en el área de investigación para generar nuevos métodos, procesos, algoritmos o aplicaciones que incluyan estos vehículos.
2. OBJETIVOS
2.1 Objetivo General
Diseñar e implementar un controlador de estabilidad para un cuadricoptero con énfasis en desarrollo sobre FPGA.
2.2 Objetivos Específicos
Desarrollar el modelo matemático del cuadricoptero
Desarrollar e implementar un controlador PD
Implementar una unidad inercial sobre la tarjeta de desarrollo
Implementar un módulo de comunicación I2C en FPGA
Realizar pruebas tanto computacionales como físicas de los algoritmos desarrollados en espacios abiertos
3. DESCRIPCIÓN DE LA PROBLEMÁTICA Y JUSTIFICACIÓN DEL TRABAJO
Como primera medida, las empresas actuales que comercializan cuadricopteros generalmente tienen reservados todos los detalles de diseño que impide realizar desarrollos estos dispositivos. Ahora, aquellos que se encuentras abiertos al público puede llegar a costar altas sumas de dinero por lo cual el desarrollo también se puede dificultar. La idea principal de este trabajo se basa en poder crear las herramientas básicas necesarias para el diseño e implementación de cuadricopteros propios de bajo costo, explorando nuevas herramientas de implementación poco comunes como lo es el uso de FPGAs como motor de control.
4. MARCO TEORICO, CONCEPTUAL E HISTORICO
Para el diseño y desarrollo de algoritmos enfocados en la solución de problemas, se requiere de un conocimiento previo que especifique todas las características del sistema y los eventos involucrados y afectados dentro del diseño. Partiendo de esta idea, para el diseño de un sistema de control de un cuadricoptero se requieren de conocimientos en áreas como física, matemática, control e instrumentación entre otros, que permiten entender las dinámicas y los funcionamientos presentes en el desarrollo.
Diseño e implementación de un controlador de estabilidad para un Cuadricoptero
6
4.1 Marco Teórico y conceptual
4.1.1 Cinética y dinámica de cuerpos rígidos
Cinemática: La cinemática es una rama de la física encargada de estudiar las leyes del movimiento de un cuerpo sin considerar las fuentes que lo provocan. Su enfoque principal está basado en el estudio de la trayectoria en función del tiempo.
Dinámica: La dinámica, en diferencia a la cinética, estudia la evolución respecto al tiempo de un sistema teniendo en cuenta las causas que provocan cambios en el estado físico o en el movimiento. A partir de este estudio se pueden determinar ecuaciones de movimiento.
Las ecuaciones que determinan el estado actual de un cuadricoptero en función de las fuerzas que actúan sobre el sistema, están definidas por un modelo matemático resultante de un análisis cinemático y dinámico sobre este.
4.1.2 Ángulos de Tait-Bryan
Los ángulos de Tait-Bryan son una derivación de los ángulos de Euler, usados muy comúnmente en sistemas de navegación. Estos ángulos describen la posición de un sistema con respecto a un marco de referencia fijo. Comúnmente estos ángulos se denominan Roll, pitch y Yaw, y describen la rotación del sistema sobre los ejes de referencia del marco. [20]
Figura 1. Ángulos de Tiat-Bryan sobre el marco de referencia del cuerpo
4.1.3 Mecánica Lagrangiana
La mecánica Lagrangiana es una reformulación de la mecánica clásica donde la trayectoria de un objeto es obtenida encontrando la trayectoria que minimiza la acción. Este método simplifica los análisis matemáticos sobre el estudio de ciertos casos. Por ejemplo, cuando se refiere al estudio de movimientos referidos a un sistema que gira, como es el caso del cuadricoptero, un análisis newtoniano requiere introducir todas las fuerzas ficticias o fuerzas de inercia como la fuerza centrífuga o la fuerza de coriolis, mientras que en la formulación Lagrangiana estas fuerzas aparecen de modo natural [21].
Las ecuaciones de movimiento en al mecaniza Lagrangiana son las ecuaciones de Lagrange, o ecuaciones de Euler-Lagrange, las cuales se derivan de las leyes de newton. Estas están definidas de la siguiente forma:
Diseño e implementación de un controlador de estabilidad para un Cuadricoptero
7
Г𝑖= 𝑑 𝑑𝑡(
𝛿𝐿 𝛿𝑞̇𝑖
) −𝛿𝐿 𝛿𝑞𝑖
(9)
Donde:
𝑞𝑖 son las coordinadas generalizadas Г𝑖 son las fuerzas generalizadas
T es la energía cinética V es la energía potencial
4.1.4 Aproximación de ángulo pequeño
La aproximación de ángulo pequeño es una simplificación de las funciones trigonométricas básicas que es verdadera cuando el ángulo tiende a cero. Esta aproximación es útil en bastantes áreas de la física, y se define de la siguiente forma [22]:
𝑆𝑖𝑛𝜃 ≈ 𝜃, 𝐶𝑜𝑠𝜃 ≈ 1 −𝜃 2 2
4.1.5 Controlador PID
El controlador PID es uno de los controladores más usados dentro de los procesos industriales. Este controlador se caracteriza por su sencillez y su versatilidad.
Figura 2. Esquema General de un controlador PID con lazo cerrado
El controlador PID es mecanismo de control basado en la retroalimentación. Esto quiere decir que para ejecutar una orden de control se requiere conocer el estado actual del sistema. Básicamente, el control PID calcula la desviación de un valor medido respecto al valor deseado. El controlador posee tres términos en orden de calcular la desviación que son, el término integral, el derivativo y el proporcional, y afectan la acción de control de la siguiente forma [23] [25]:
La acción de control proporcional aumenta la velocidad de respuesta del sistema. Un aumento en la ganancia proporcional 𝐾𝑝 disminuye el rise time, aumenta levemente el setting time, aumenta el overshoot y disminuye el error en estado estacionario, aunque si su valor es muy grande puede degradar la estabilidad del sistema.
La acción de control integral trabaja mientras exista error y depende de la magnitud de éste. Tiene la capacidad de reducir el error de la planta a cero, suponiendo esto un aumento en el orden la planta. Es decir mejora la precisión a costa de una disminución en la estabilidad. Un aumento de la ganancia integral 𝐾𝑖 =𝐾𝑝
𝑇𝑖 disminuye levemente el rise time, aumenta el
overshoot, aumenta el setting time y disminuye altamente el error en estado estacionario a costa de degradar estabilidad.
La acción de control derivativa no depende de la magnitud del error, amortigua el sistema y no interviene en el estado estable. Un aumento en la ganancia derivativa 𝐾𝑑 = 𝐾𝑝 ∙ 𝑇𝑑 disminuye levemente el rise time, el overshoot y el setting time, además de mejorar la estabilidad.
Diseño e implementación de un controlador de estabilidad para un Cuadricoptero
8
4.1.6 Unidad de medición Inercial
Una unidad de medición inercial (IMU) consiste en un arreglo de giroscopios, acelerómetros y magnetómetros configurados de tal manera que permiten obtener información sobre la posición, la velocidad, las fuerzas gravitaciones y los campos magnéticos alrededor del sistema. Dentro de un sistema de navegación, los datos entregados por las IMU con procesados para calcular la posición actual. Es posible obtener información de los ángulos de tiat-bryan con la información entregada por una unidad de medición Inercial.
4.1.7 Fusión e integración multisensorial
La integración hace referencia al uso sinérgico de información entregada por múltiples sensores para que un sistema cumpla una tarea, mientras que la fusión corresponde a cualquier etapa durante el proceso de integración donde se realiza la combinación de diferentes fuentes de información sensorial en un solo formato de representación. La fusión multisensorial puede tomar lugar en diferentes niveles de representación, como lo son el nivel de señal, de pixel, de feature, y el nivel de símbolo.
Los algoritmos usados para hacer fusión multisensorial se clasifican en métodos estimativos, métodos de clasificación, métodos por inferencia y métodos basados en inteligencia artificial. Por lo general, los métodos estimativos son efectivos cuando se realiza fusión en el nivel de señal. Los métodos de clasificación se usan para extraer características y realizar fusión en el nivel de pixel y de feature. Los métodos por inferencia suelen ser usados en el nivel de símbolo, mientras que los métodos de inteligencia artificial se usan en cualquiera de los niveles de representación.
En los métodos estimativos se encuentran los Filtros de Kalman, los Filtros de Kalman Extendidos, y métodos no recursivos como el uso del promedio ponderado. Como métodos de clasificación existen Cluster Analysis, Learning Vector Quantization, K-means clustering, y ARTMAP entre otros. Los métodos por inferencia más comunes son la inferencia Bayesiana, el método de Dempster-Shafer y el procesamiento por evidencia generalizada. Entre los métodos basados en inteligencia artificial se destacan las Redes Neuronales Adaptativas y Fuzzy Logic [13] [25].
4.1.8 Filtro de Kalman
El filtro de Kalman es un algoritmo matemático que trata de estimar estadísticamente el estado del sistema a partir de una serie de mediciones. Básicamente, el filtro de Kalman produce estadísticamente, un valor optimo estimado del sistema. Para ello, es necesario conocer el ruido de la medición, que para el caso del acelerómetro y giroscopio es ruido aleatorio con una distribución gausiana y media cero.
Diseño e implementación de un controlador de estabilidad para un Cuadricoptero
9
Figura 3. Algoritmo del Filtro de Kalman
Los parámetros se definen de la siguiente forma:
- 𝑥𝑘 Corresponde al estado del sistema y se evalúa mediante una ecuación lineal estocástica. Este valor resulta de una combinación lineal de sus estados previos más una señal de control 𝑢𝑘 y el ruido del proceso.
- 𝑧𝑘 Corresponde al valor medido y se define como una combinación lineal del estado y el ruido de la
medición.
- Las matrices A, B, H son matrices generales que varían dependiendo del caso de aplicación del filtro. La matriz A es el modelo de transición del estado el cual se aplica al estado previo. B se denomina modelo de entrada de control. La matriz H se conoce como el modelo de observación, el cual se usa para mapear el espacio verdadero del estado en el espacio observado.
- La matriz Q es la matriz de covarianza del ruido del proceso. Este es un parámetro que debe ser conocido antes de aplicar el filtro.
- La matriz P corresponde a la matriz de la covarianza del error. - K se define como la ganancia del filtro.
4.1.9 Filtro Complementario
Un filtro complementario es un modelo de fusión sensorial basado en el uso de un filtro pasa altas y un filtro pasa bajos como el mostrado en la siguiente figura:
Diseño e implementación de un controlador de estabilidad para un Cuadricoptero
10
El filtro complementario presentado en este documento está basado en el descrito en la referencia [19] y [28]. Se especifica como un filtro de balance que usa dos ganancias estáticas para unir las medidas observadas por el giroscopio con las medidas del acelerómetro.
4.1.10 Comunicación I2C
La comunicación I2C es un protocolo de comunicación definido para comunicar múltiples dispositivos con direcciones asignadas con Maestro.
La característica principal que define una comunicación I2C radica en que se usan solo dos líneas denominadas SCL y SDA. La línea SCL corresponde al clock del sistema y su objetivo principal es el de sincronizar todos los dispositivos conectados en la red I2C. Por otro lado, la línea SDA es una línea bidireccional encargada de enviar y recibir la información entre los dispositivos.
Continuamente existe una comunicación entre el maestro y los dispositivos esclavos para identificar qué operación se está realizando y quienes están involucrados en esto.
Generalmente existe un protocolo entre el maestro y los esclavos, que define cuando se reciben y cuando se transmiten datos, para quienes están dirigidos y para donde van dirigidos. El protocolo básico en una red 𝐼2𝐶
se define de la siguiente manera:
Figura 4. Protocolo básico de comunicación en una red 𝐼2𝐶 [29]
4.1.11 FPGA (Field Programmable Gate Array)
Una FPGA es un dispositivo que basa su funcionamiento en la lógica binaria, que puede ser programada en un lenguaje de descripción Hadware. Una FPGA es capaz de realizar diferentes operaciones de forma simultánea y tienen la ventaja de ser reprogramables lo cual las convierte en un elemento versátil aunque no son capaces de manejar procesos demasiado complicados. Por otro lado, el desarrollo de un dispositivo FPGA es de bajo costo, y son de fácil adquisición en comparación a otros sistemas de procesamiento.
4.2 Marco Histórico
Múltiples documentos referenciados en este documento como [6], [7], [8], [9] y [14] se enfocan en temas desarrollados en este documento. Durante el proceso de investigación y comparación se pudo encontrar que la información respecto a temas específicos tiende a variar entre un documento y otro generando incertidumbre respecto a la veracidad de la información. Es por esta razón recurrieron a diversas fuentes de información tales como textos y documentos más específicos con el fin de concatenar y verificar toda la información referenciada.
Actualmente, aunque se han desarrollado varios trabaros respecto al tema, el área de control de cuadricopteros aún sigue siendo un área con amplios campos para la futura investigación dado que el tema de robótica y sistemas autónomos recién está surgiendo.
Diseño e implementación de un controlador de estabilidad para un Cuadricoptero
11
5. REQUERIMIENTOS, ESPECIFICACIONES Y RESTRICCIONES DEL TRABAJO
Principalmente se requiere diseñar e implementar un sistema de control de lazo cerrado ya que el sistema actual opera en lazo abierto. Para ello es necesario implementar un sistema de sensores ya que el sistema actual no cuenta con ellos. Simultáneamente se requiere realizar el diseño de los modos necesarios para crear el sistema de control funcional sobre el dispositivo
Respecto a las restricciones es necesario mencionar que:
Inicialmente se cuenta con un cuadricoptero que posee un sistema de radio control diferente al de fábrica. Además, se conoce que el dispositivo requiere de una secuencia de encendido de fábrica y que cuenta con una implementación VHDL para el control a lazo abierto. Estos puntos mencionados corresponden a un trabajo realizado anteriormente sobre el dispositivo, del cual no hay registros, referencias o información ya que su creador decidió no publicarlas. Esto supone una gran restricción inicial para la implementación de los algoritmos deseados.
Por otro lado, las FPGA son dispositivos sobre los cuales no se pueden implementar cierto tipos de procesos directamente tales como divisiones u operaciones de punto flotante, que son requeridas para el desarrollo de este proyecto.
6. METODOLOGIA DE TRABAJO
6.1 Plan de trabajo
Una vez realizada la propuesta de trabajo, se estableció un plan de trabajo con las tareas a cumplir la cual fue modificada con el paso del tiempo acorde a necesidades actuales del trabajo y en respuesta a contratiempo.
A continuación se presenta el plan de trabajo realizado:
1. Semana 1-3: Se realizó una investigación relacionada a dispositivos FPGA y lenguajes de descripción
Hardware. En especial se hizo un estudio teórico de la funcionalidad de una de las FPGA desarrolladas dentro de la Universidad de los Andes. Además se realizó la instalación de los softwares necesarios para la programación de VHDL y verilog en Linux
2. Semana 3-4: Se recibieron los elementos de trabajo tales como la unidad inercial y un cuadricoptero
phantom 1 como referencia para el trabajo. A partir de esto se realizó la revisión de los documentos técnicos de los sensores incluidos en la IMU y se comprendió su funcionamiento. Por otra parte se comenzó el estudio del desarrollo de controladores para cuadricopteros.
3. Semana 5: Se encontró la necesidad de realizar un protocolo de comunicación I2C en FPGA para
adquirir los datos del sensor, dado que generalmente estos dispositivos no poseen los pines SDA y SCL necesarios para ello.
4. Semana 7-8: Durante este periodo se continuo la programación del módulo I2C y se comenzó a
realizar el desarrollo de los algoritmos de procesamiento de datos
5. Semana 9-11: Se realizó el desarrollo del modelo matemático y se hicieron algunas mediciones de
los valores incluidos en el modelo, tales como momentos de inercia y características intrínsecas de los motores y el cuadricoptero. Una vez determinados los valores, se realizaron modelos en Matlab y Simulink para realizar las verificaciones del modelo matemático, u se dio comienzo al desarrollo del controlador a partir del modelo.
6. Semana 12: Se realizaron los desarrollos de dos modelos de control son sus respectivas
Diseño e implementación de un controlador de estabilidad para un Cuadricoptero
12
7. Semana 13: Se realizó un estudio sobre el funcionamiento físico de los acelerómetros y giroscopios
con el fin de generar simulaciones de algún caso real sin recurrir a datos reales
8. Semana 14: Se comenzó con el estudio de algunos algoritmos de integración sensorial como el filtro
de Kalman y el filtro complementario. Se revisó la teoría detrás de estos conceptos y se desarrollaron los algoritmos en Matlab y Simulink para pruebas sobre datos reales.
9. Semana 15: Se comenzó el desarrollo en Matlab de todos los algoritmos diseñados, con el fin de usar
la herramienta HDL coder para traducir estos algoritmos a VHDL y Verilog y entender qué tipo de procesamientos deben ser reducidos en nivel de implementación.
10. Semana 16: Se realizó un montaje físico para calibrar y corroborar el funcionamiento de los
algoritmos de integración sensorial. De esta manera se pudo comprobar la veracidad de los datos de los ángulos obtenidos a partir de la IMU
11. Semana 17-19: Se procedieron a documentar todos los procesos realizados y todos los resultados
obtenidos del trabajo hecho.
6.2 Búsqueda de información
Una parte importante de los procesos de desarrollo de este trabajo, y especialmente de los algoritmos propuestos fue la investigación y búsqueda de información. Se utilizaron múltiples documentos de referencia provenientes de bases de datos e instituciones académicas las cuales se encuentran indicadas en la sección de referencias.
Por otro lado, la preparación académica impartida por la Universidad de los Andes fue un factor útil en el proceso de aprendizaje y desarrollo. Factores como la disciplina, la organización y la perseverancia enseñadas en la institución fueron piezas claves para el desarrollo de este trabajo.
Por último, el acompañamiento por parte del asesor del proyecto fue muy importante a la hora de resolver la infinidad de inquietudes provenientes del proceso de aprendizaje, y superar obstáculos tanto laborales como emocionales.
6.3 Alternativas de desarrollo
Para el desarrollo de algoritmos y diseño de procesos como los presentados en este documento, existen infinidad de programas y alternativas de implementación. Este tipo de trabajo no está limitado en cuestión de software y plataformas ya que se cuenta en la actualidad con un mercado amplio que ha surgido en respuesta a necesidades del ser humano. Es por esta razón que se cuentan con diferentes plataformas de desarrollo, softwares matemáticos y simuladores versátiles
7. TRABAJO REALIZADO
El trabajo realizado se dividió en varias etapas de diseño dentro de la estructura de diseño completa de control de cuadricoptero. Lo anterior se debe a que un sistema como lo es un cuadricoptero posee varias secciones que se complementan entre sí, y que requieren un desarrollo especifico y único.
7.1 Modelo Matemático del Cuadricoptero
El cuadricoptero es un dispositivo tridimensional que está sometido a varias fuerzas y momentos, que generan el ascenso o descenso del cuerpo debido a las diferencias en el instante.
Idealmente su centro de masa se encuentra ubicado en el centro, y el diagrama de fuerzas que actúan sobre el dispositivo es el siguiente:
Diseño e implementación de un controlador de estabilidad para un Cuadricoptero
13
Figura 5 Diagrama de fuerzas del cuadricoptero. [1]
La posición del vehículo en el espacio está definida por el marco inercial (x,y,z) , mientras que la posición angular está definida por los 3 ángulos de Tait-Bryan (ϕ, 𝜃, ψ) dentro del marco inercial [1][3]. El Pitch (𝜃), determina la rotación del cuerpo alrededor del eje y. El Roll (ϕ), determina la rotación del cuerpo alrededor del eje x. Por último, el Yaw (ψ), determina la posición del cuerpo alrededor del eje z. Una vez definidas estas 6 variables, se genera un vector Q que contiene las posicione lineares y angulares del cuerpo.
𝑄 = [ 𝑥 𝑦 𝑧 ϕ 𝜃 ψ]
, η = [ ϕ 𝜃 ψ
] , ξ = [ x 𝑦 z
] (1)
Además, el marco de referencia del cuerpo tiene como origen su centro de masa. Acorde a este marco, las velocidades lineales del cuerpo están definidas como 𝑉𝐵,𝑥𝑉𝐵,𝑦 𝑦 𝑉𝑏,𝑧, y las velocidades angulares como p,q y
r,es decir:
𝑉𝐵 = [ 𝑉𝐵,𝑥 𝑉𝐵,𝑦 𝑉𝑏,𝑧
] 𝑣 = [ 𝑝 𝑞 𝑟
] (2)
Una vez definidos estos vectores, es necesario tener en cuenta que, por ejemplo, la gravedad dentro del marco inercial siempre tiene dirección en el eje z, mientras que dentro del marco del cuerpo, esta tiene componentes en los ejes que dependen de la posición del cuerpo en el marco inercial. Es por esta razón que en necesario establecer una relación entre ambos marcos ya que el marco del cuerpo varía respecto al marco inercial.
La matriz de transformación que relaciona el marco de referencia del cuerpo con el marco inercial es la siguiente: [2]
[
𝑐𝑜𝑠𝜃𝑐𝑜𝑠ψ 𝑠𝑖𝑛ϕ𝑠𝑖𝑛𝜃𝑐𝑜𝑠ψ − cosϕsinψ 𝑐𝑜𝑠ϕ𝑠𝑖𝑛𝜃𝑐𝑜𝑠ψ − sinϕsinψ 𝑐𝑜𝑠𝜃𝑠𝑖𝑛ψ 𝑠𝑖𝑛ϕ𝑠𝑖𝑛𝜃𝑠𝑖𝑛ψ + cosϕcosψ 𝑐𝑜𝑠ϕ𝑠𝑖𝑛𝜃𝑠𝑖𝑛ψ − sinϕcosψ
−𝑠𝑖𝑛𝜃 𝑠𝑖𝑛ϕ𝑐𝑜𝑠𝜃 𝑐𝑜𝑠ϕcos𝜃
]
Esta matriz, simplificando Sin por S, y Cos por C, para mayor comodidad en la representación, seria:
𝑅 = [
𝐶𝜃𝐶ψ 𝑆ϕ𝑆𝜃𝐶ψ − CϕSψ 𝐶ϕ𝑆𝜃𝐶ψ − SϕSψ 𝐶𝜃𝑆ψ 𝑆ϕ𝑆𝜃𝑆ψ + CϕCψ 𝐶ϕ𝑆𝜃𝑆ψ − SϕCψ
−𝑆𝜃 𝑆ϕ𝐶𝜃 𝐶ϕC𝜃
]
Diseño e implementación de un controlador de estabilidad para un Cuadricoptero
14
Para el caso contrario, dado que la matriz R es ortogonal, es decir 𝑅𝑇 = 𝑅−1, la matriz de conversión del marco
inercial al marco del cuerpo es 𝑅𝑇 = 𝑅−1.
La matriz de transformación para conseguir las velocidades angulares a partir de las velocidades del marco inercial es 𝑊η, y para el caso contrario, es decir, para obtener las velocidades del marco del cuerpo al marco
inercial es 𝑊η−1.
𝑊η−1=
[
1 𝑆ϕ𝑇𝜃 𝐶ϕ𝑇𝜃 0 𝐶ϕ −𝑆ϕ
0 𝑆ϕ 𝐶𝜃
𝐶ϕ 𝐶𝜃 ]
(4) 𝑊η = [
1 0 −𝑆𝜃 0 𝐶ϕ 𝐶𝜃𝑆ϕ 0 −𝑆ϕ 𝐶𝜃𝐶ϕ
] (5)
Con esto, es posible establecer las siguientes relaciones [1]:
η̇ = 𝑊η−1𝑣 = [ ϕ 𝜃̇ ψ̇ ̇ ] = [
1 𝑆ϕ𝑇𝜃 𝐶ϕ𝑇𝜃 0 𝐶ϕ −𝑆ϕ
0 𝑆ϕ 𝐶𝜃
𝐶ϕ 𝐶𝜃 ]
[ 𝑝 𝑞 𝑟 ]
𝑣 = 𝑊η η̇ = [ 𝑝 𝑞 𝑟
] = [
1 0 −𝑆𝜃 0 𝐶ϕ 𝐶𝜃𝑆ϕ 0 −𝑆ϕ 𝐶𝜃𝐶ϕ
] [ ϕ 𝜃̇ ψ̇
̇
] (6)
7.1.1 Formalismo de Euler-Lagrange
El modelo dinámico se deriva usando el formalismo de Euler-Lagrange teniendo en cuenta las siguientes suposiciones:
El cuerpo y las hélices son rígidos
El cuerpo es simétrico
El empuje y el arrastre son proporcionales al cuadrado de la velocidad de la hélice
Debido a que se asume que el cuadricoptero es simétrico, es decir que los motores están todos a la misma distancia del centro de referencia y que están alineados con los ejes, entonces la matriz de inercia del cuerpo es la siguiente, donde 𝐼𝑥𝑥 = 𝐼𝑦𝑦. [1] [4]
𝐼 = [
𝐼𝑥𝑥 0 0 0 𝐼𝑦𝑦 0 0 0 𝐼𝑧𝑧
] (7)
Establecido lo anterior, Para comenzar a realizar un análisis dinámico del sistema, se parte del lagraniano L=T-V (8), el cual relaciona la energía cinética y energía potencial del sistema en un sistema de coordenadas concreto, y la ecuación general de movimiento del método de Lagrange [6], [7], [21].
Г𝑖= 𝑑 𝑑𝑡(
𝛿𝐿 𝛿𝑞̇𝑖
) −𝛿𝐿 𝛿𝑞𝑖
(9)
Cinemática: Partiendo de la matriz de transformación R (2), cualquier punto del marco del cuerpo se puede expresar dentro del marco inercial o de la tierra como [7]:
Diseño e implementación de un controlador de estabilidad para un Cuadricoptero
15
𝑟𝑋, 𝑌, 𝑍(𝑥, 𝑦, 𝑧) = 𝑅(ϕ, 𝜃, ψ) [ x 𝑦 z
], es decir:
𝑟𝑋 = (𝑐𝑜𝑠𝜃𝑐𝑜𝑠ψ)x + ( 𝑠𝑖𝑛ϕ𝑠𝑖𝑛𝜃𝑐𝑜𝑠ψ − cosϕsinψ)y + (𝑐𝑜𝑠ϕ𝑠𝑖𝑛𝜃𝑐𝑜𝑠ψ − sinϕsinψ)z (10)
𝑟𝑌 = (𝑐𝑜𝑠𝜃𝑠𝑖𝑛ψ)𝑥 + (𝑠𝑖𝑛ϕ𝑠𝑖𝑛𝜃𝑠𝑖𝑛ψ + cosϕcosψ)𝑦 + (𝑐𝑜𝑠ϕ𝑠𝑖𝑛𝜃𝑠𝑖𝑛ψ − sinϕcosψ)𝑧 (11)
𝑟𝑍 = (−𝑠𝑖𝑛𝜃)𝑥 + (𝑠𝑖𝑛ϕ𝑐𝑜𝑠𝜃)𝑦 + (𝑐𝑜𝑠ϕcos𝜃)𝑧 (12)
Derivando las ecuaciones (10) (11) y (12), se obtienen los valores de velocidad:
𝑣𝑋 = (−𝑆𝜃𝐶𝜓𝜃̇ − 𝐶𝜃𝑆𝜓𝜓̇)𝑥 + (−𝐶𝜓𝐶𝜙𝜓̇ + 𝑆𝜓𝑆𝜙𝜙̇ − 𝑆𝜓𝑆𝜙𝑆𝜃𝜓̇ + 𝐶𝜓𝐶𝜙𝑆𝜃𝜙̇ + 𝐶𝜓𝑆𝜙𝐶𝜃𝜃̇)𝑦 + (𝐶𝜓𝑆𝜙𝜓̇ + 𝑆𝜓𝐶𝜙𝜙̇ − 𝑆𝜓𝐶𝜙𝑆𝜃𝜓̇ − 𝐶𝜓𝑆𝜙𝑆𝜃𝜙̇ + 𝐶𝜓𝐶𝜙𝐶𝜃𝜃̇)𝑧 (13)
𝑣𝑌 = (−𝑆𝜃𝑆𝜓𝜃̇ + 𝐶𝜃𝐶𝜓𝜓̇)𝑥 + (−𝑆𝜓𝐶𝜙𝜓̇ − 𝐶𝜓𝑆𝜙𝜙̇ + 𝐶𝜓𝑆𝜙𝑆𝜃𝜓̇ + 𝑆𝜓𝐶𝜙𝑆𝜃𝜙̇ + 𝑆𝜓𝑆𝜙𝐶𝜃𝜃̇)𝑦 + (𝑆𝜓𝑆𝜙𝜓̇ − 𝐶𝜓𝐶𝜙𝜙̇ + 𝐶𝜓𝐶𝜙𝑆𝜃𝜓̇ − 𝑆𝜓𝑆𝜙𝑆𝜃𝜙̇ + 𝑆𝜓𝐶𝜙𝐶𝜃𝜃̇)𝑧 (14)
𝑣𝑍 = (−𝐶𝜃𝜃̇)𝑥 + (𝐶𝜙𝐶𝜃𝜙̇ − 𝑆𝜙𝑆𝜃𝜃̇)𝑦 + (−𝑆𝜙𝐶𝜃𝜙̇ − 𝐶𝜙𝑆𝜃𝜃̇)𝑧 (15)
𝑣2= 𝑣𝑋2+ 𝑣𝑌2+ 𝑣𝑍2 (16)
Resolviendo la ecuación (16) reemplazando los términos con de las ecuaciones (13), (14) y (15), se obtiene la siguiente expresión:
𝑣2(𝑥, 𝑦, 𝑧) = 𝑥2(𝑐𝑜𝑠2𝜃ψ̇2+θ̇2)
+ 𝑦2( ψ̇2( cos2ϕ+sin2ϕ𝑠𝑖𝑛2𝜃) +ψ̇ (−2sin ϕ cos ϕ cos θ𝜃̇ − 2ϕ̇ 𝑠𝑖𝑛𝜃) +sin2ϕ θ̇2
+ 𝜙̇2)
+ 𝑧2( ψ̇2( sin2ϕ+cos2ϕ𝑠𝑖𝑛2𝜃) +ψ̇ (2sin ϕ cos ϕ cos θ𝜃̇ − 2ϕ̇ 𝑠𝑖𝑛𝜃) +cos2ϕ θ̇2+ 𝜙̇2)
+ 2𝑥𝑦 (ψ̇2𝑠𝑖𝑛 ϕ sin θ cos θ+ψ̇ (𝑐𝑜𝑠ϕsinθ𝜃̇ − 𝑠𝑖𝑛ϕcosθ𝜙̇) − 𝑐𝑜𝑠ϕ𝜙̇𝜃̇)
+ 2𝑥𝑧 (ψ̇2𝑐𝑜𝑠 ϕ sin θ cos θ+ψ̇ (−𝑐𝑜𝑠ϕcosθ𝜙̇ − 𝑠𝑖𝑛ϕsinθ𝜃̇) + 𝑠𝑖𝑛ϕ𝜙̇𝜃̇)
+ 2𝑦𝑧 (−ψ̇2𝑠𝑖𝑛 ϕ𝑐𝑜𝑠ϕ𝑐𝑜𝑠2𝜃 +ψ̇ (sin2ϕ cosθ𝜃̇ −cos2ϕ cosθ𝜃̇) + 𝑠𝑖𝑛ϕ𝑐𝑜𝑠ϕθ̇2) (17)
Energía:a partir de la matriz diagonal del momento de inercia definida anteriormente, las ecuaciones de energía cinética rotacional y potencial del cuadricoptero se representan de la siguiente manera:
Teniendo en cuenta que 𝑇 =1 2𝐼𝜔
2 𝑦 𝑉 = 𝑚𝑔ℎ, entonces [4]:
𝑇 =1
2𝐼𝑥𝑥(ϕ̇ − ψ̇𝑆𝑖𝑛𝜃) 2
+1
2𝐼𝑦𝑦(𝜃̇𝐶𝑜𝑠ϕ − ψ̇𝑆𝑖𝑛ϕ𝐶𝑜𝑠𝜃) 2
+1
2𝐼𝑧𝑧(𝜃̇𝑆𝑖𝑛ϕ − ψ̇𝐶𝑜𝑠ϕ𝐶𝑜𝑠𝜃) 2
(18)
𝑉 = 𝑔 (∫ 𝑥𝑑𝑚(𝑥)(−𝑠𝑖𝑛𝜃) + ∫ 𝑦𝑑𝑚(𝑦)(𝑆𝑖𝑛ϕ𝐶𝑜𝑠𝜃) + ∫ 𝑧𝑑𝑚(𝑧)(𝐶𝑜𝑠ϕ𝐶𝑜𝑠𝜃) ) (19)
Retomando las ecuaciones (8) y (9), definiendo L a partir de las ecuaciones (18) y (19), y definiendo las ecuaciones de movimiento del método de Lagrange, se pueden determinar las ecuaciones diferenciales características para el pitch, roll y yaw [7].
𝑑 𝑑𝑡(
∂L ∂ϕ̇) −
∂L
∂ϕ= 𝜏ϕ (20), 𝑑 𝑑𝑡(
∂L ∂𝜃̇) −
∂L
∂𝜃= 𝜏𝜃 (21), 𝑑 𝑑𝑡(
∂L ∂ψ̇) −
∂L
Diseño e implementación de un controlador de estabilidad para un Cuadricoptero
16
Las ecuaciones de movimiento del método de LaGrange son las presentadas como (20), (21) y (22). Para resolver estas ecuaciones, es necesario reemplazar T y V en (8), y realizar las derivadas parciales correspondientes. El resultado de este proceso algebraico es el siguiente [7]:
𝑑 𝑑𝑡(
∂L ∂ϕ̇) −
∂L
∂ϕ= ϕ̈𝐼𝑥𝑥− ψ̈𝑆𝑖𝑛𝜃𝐼𝑥𝑥− ψ̇𝜃̇𝐶𝑜𝑠𝜃(𝐼𝑥𝑥+ (𝐼𝑦𝑦− 𝐼𝑧𝑧)(2𝐶𝑜𝑠ϕ
2− 1)) + 𝜃̇21
2𝑆𝑖𝑛2ϕ(𝐼𝑦𝑦− 𝐼𝑧𝑧)
− ψ̇21
2𝑆𝑖𝑛2ϕCos𝜃 2(𝐼
𝑦𝑦− 𝐼𝑧𝑧) + ∫ 𝑦𝑑𝑚(𝑦)(−𝑔𝐶𝑜𝑠ϕ𝐶𝑜𝑠𝜃)
+ ∫ 𝑧𝑑𝑚(𝑧)(𝑔𝑆𝑖𝑛ϕ𝐶𝑜𝑠𝜃) (23)
𝑑 𝑑𝑡(
∂L ∂𝜃̇) −
∂L
∂𝜃= 𝜃̈(𝐼𝑦𝑦𝐶𝑜𝑠
2ϕ + 𝐼𝑧𝑧𝑆𝑖𝑛2ϕ) + ψ̈1
2𝑆𝑖𝑛2ϕCos𝜃(𝐼𝑦𝑦− 𝐼𝑧𝑧) + ψ̇21
2𝑆𝑖𝑛2𝜃(−𝐼𝑥𝑥+ 𝐼𝑦𝑦𝑆𝑖𝑛 2ϕ + 𝐼
𝑧𝑧𝐶𝑜𝑠2ϕ) + 𝜃̇ϕ̇𝑆𝑖𝑛2ϕ(𝐼𝑧𝑧− 𝐼𝑦𝑦) + ψ̇𝜃̇𝐶𝑜𝑠𝜃(𝐶𝑜𝑠2𝜃 ((𝐼𝑦𝑦− 𝐼𝑧𝑧) + 𝐼𝑥𝑥) + ∫ 𝑥𝑑𝑚(𝑥)(−𝑔𝐶𝑜𝑠𝜃)
− ∫ 𝑦𝑑𝑚(𝑦)(𝑔𝑆𝑖𝑛ϕ𝑆𝑖𝑛𝜃) − ∫ 𝑧𝑑𝑚(𝑧)(𝑔𝐶𝑜𝑠ϕ𝑆𝑖𝑛𝜃) (24)
𝑑 𝑑𝑡(
∂L ∂ψ̇) −
∂L
∂ψ= ψ̈(𝐶𝑜𝑠
2𝜃(𝐼𝑧𝑧𝐶𝑜𝑠2ϕ + 𝐼𝑦𝑦𝑆𝑖𝑛2ϕ) + 𝑆𝑖𝑛2𝜃𝐼𝑥𝑥) − ϕ̈𝑆𝑖𝑛𝜃𝐼𝑥𝑥+ 𝜃̈1
2𝑆𝑖𝑛2ϕCos𝜃(𝐼𝑦𝑦− 𝐼𝑧𝑧) + 𝜃̇ψ̇𝑆𝑖𝑛2𝜃(𝐼𝑥𝑥− 𝐼𝑧𝑧𝐶𝑜𝑠2ϕ + 𝐼𝑦𝑦𝑆𝑖𝑛2ϕ) − ψ̇ϕ̇𝑆𝑖𝑛2ϕ𝐶𝑜𝑠2𝜃(𝐼𝑦𝑦− 𝐼𝑧𝑧)
+ 𝜃̇ϕ̇𝐶𝑜𝑠𝜃 (𝐼𝑥𝑥− (2𝐶𝑜𝑠2ϕ − 1)(𝐼𝑦𝑦− 𝐼𝑧𝑧)) − 𝜃̇21
2𝑆𝑖𝑛2ϕSin𝜃(𝐼𝑦𝑦− 𝐼𝑧𝑧) (25)
Una vez definidos los términos de la izquierda en las ecuaciones de movimiento de Lagrange, restan establecer los comportamientos de las torques 𝜏ϕ, 𝜏𝜃 𝑦 𝜏ψ.
Las velocidades angulares de cada rotor crean fuerzas (𝑓𝑖) en la dirección del eje del rotor. Además, la
velocidad angular y la aceleración del rotor también generar torques (𝜏𝑀𝑖) alrededor del eje del rotor, tal como se presenta en la Figura 5. Estos términos se definen como:
𝑓𝑖= 𝑘𝜔2𝑖, 𝜏𝑀𝑖= 𝑏𝜔𝑖2+ 𝐼𝑀𝜔̇𝑖 (26)
Donde k es la constante de empuje (Lift), y b es la constante de arrastre (drag).
La sumatoria total de las fuerzas producidas por los 4 motores se denomina Thrust , orientado sobre el eje Z en el plano del cuerpo. Los torques presentes sobre el cuerpo son aquellos correspondientes a los ángulos, es decir 𝜏ϕ, 𝜏𝜃 𝑦 𝜏ψ. [1]
𝑇ℎ𝑟𝑢𝑠𝑡 = ∑ 𝑓𝑖 4
𝑖=1
𝜏𝐵= [ 𝜏ϕ
𝜏𝜃 𝜏ψ
] (27)
En otros términos, conociendo que 𝜏 = 𝐿𝑓, siendo l la distancia entre el motor y el centro del cuerpo, se obtiene que: [1] [5] [7]
Diseño e implementación de un controlador de estabilidad para un Cuadricoptero
17
[ 𝜏ϕ 𝜏𝜃 𝜏ψ ] = [𝐾𝑙(𝜔42− 𝜔22) 𝐾𝑙(𝜔32− 𝜔12 𝑏(𝜔12− 𝜔22+ 𝜔32− 𝜔42)
] (29)
La ecuación (29) desprecia los efectos giroscópicos resultantes de la rotación de las hélices. Estos se definen de la siguiente manera:
𝜏ϕ′ = 𝐽𝑟𝜔𝑦(Ω1+ Ω3− Ω2− Ω4) = −𝐽𝑟𝜔𝑦 Ω𝑟, 𝜏𝜃′ = 𝐽𝑟𝜔𝑥(−Ω1− Ω3+ Ω2+ Ω4) = 𝐽𝑟𝜔𝑥Ω𝑟 (30)
Retomando las ecuaciones (23), (24) y (25), es posible usar la matriz de transformación (6) para expresar los términos en el marco de referencia del cuerpo.
𝑑 𝑑𝑡(
𝜕𝐿 𝜕𝜙̇) −
𝜕𝐿
𝜕𝜙 = 𝐼𝑥𝑥𝜔̇𝑥− 𝜔𝑦𝜔𝑧(𝐼𝑦𝑦− 𝐼𝑧𝑧) − 𝑔 ∫(𝑦𝑐𝑜𝑠 𝜙𝑐𝑜𝑠𝜃)𝑑𝑚 + 𝑔 ∫(𝑧𝑠𝑖𝑛𝜙𝑐𝑜𝑠𝜃)𝑑𝑚 (31)
𝑑 𝑑𝑡(
𝜕𝐿 𝜕𝜃̇) −
𝜕𝐿
𝜕𝜃= −𝑠𝑖𝑛𝜙 (𝜔̇𝑧𝐼𝑧𝑧− 𝜔𝑥𝜔𝑧(𝐼𝑥𝑥− 𝐼𝑦𝑦)) + 𝑐𝑜𝑠𝜙(𝜔̇𝑦𝐼𝑦𝑦− 𝜔𝑥𝜔𝑧(𝐼𝑧𝑧− 𝐼𝑥𝑥) − 𝑔 ∫(𝑥𝑐𝑜𝑠𝜃)𝑑𝑚 − 𝑔 ∫(𝑦𝑠𝑖𝑛𝜙𝑠𝑖𝑛𝜃)𝑑𝑚 − 𝑔 ∫(𝑧𝑐𝑜𝑠𝜙𝑠𝑖𝑛𝜃)𝑑𝑚 (32)
𝑑 𝑑𝑡(
𝜕𝐿 𝜕𝜓̇) −
𝜕𝐿
𝜕𝜓= −𝑠𝑖𝑛𝜃 (𝜔̇𝑥𝐼𝑥𝑥− 𝜔𝑦𝜔𝑧(𝐼𝑦𝑦− 𝐼𝑧𝑧)) + 𝑠𝑖𝑛𝜙𝑐𝑜𝑠𝜃 (𝜔̇𝑦𝐼𝑦𝑦− 𝜔𝑥𝜔𝑧(𝐼𝑧𝑧− 𝐼𝑥𝑥)) + 𝑐𝑜𝑠𝜙𝑐𝑜𝑠𝜃 (𝜔̇𝑧𝐼𝑧𝑧− 𝜔𝑥𝜔𝑦(𝐼𝑥𝑥− 𝐼𝑦𝑦)) (33)
Una vez transformadas las ecuaciones y definidos los torques (29), (30), si se aplica la aproximación de ángulo pequeño (𝜃, 𝜙, ψ ≈ 0), se pueden obtener las ecuaciones diferenciales características de los ángulos [1] [7] [8].
𝜙̈ =𝐽𝑟𝜃̇(Ω1+ Ω3− Ω2− Ω4) 𝐼𝑥𝑥
+ 𝜓 ̇𝜃̇𝐼𝑦𝑦− 𝐼𝑧𝑧 𝐼𝑥𝑥
+𝑘𝑙(Ω4 2− Ω22) 𝐼𝑥𝑥
(34)
𝜃̈ =𝐽𝑟𝜙̇(−Ω1− Ω3+ Ω2+ Ω4) 𝐼𝑦𝑦
+ 𝜓 ̇𝜙̇𝐼𝑧𝑧− 𝐼𝑥𝑥 𝐼𝑦𝑦
+𝑘𝑙(Ω3 2− Ω12) 𝐼𝑦𝑦
(35)
𝜓̈ =𝑏(Ω1
2− Ω22+ Ω32− Ω42) 𝐼𝑧𝑧
+ 𝜃 ̇𝜙̇ 𝐼𝑥𝑥− 𝐼𝑦𝑦 𝐼𝑧𝑧
(36)
7.2 Diseño del sistema de Control
7.2.1 Controlador PD
En un cuadricoptero existen 6 variables de estado que son η yξ, pero solo se cuentan con cuatro entradas de control que son las velocidades de rotación de cada motor respectivamente. Existen relaciones entre los cambios físicos que ocurren en el sistema por la variación de las velocidades de los motores, y que son perceptibles tal como el empuje, o los torques que se generan por las diferencias de aceleraciones de cada ángulo. Un controlador PID o PD es de un solo canal, por lo cual se requieren de tres controladores PD para los respectivos ángulos, y uno adicional para el control de altura, completando así los cuatro canales de control para el cuadricoptero (Roll, pitch, yaw y altura).
Diseño e implementación de un controlador de estabilidad para un Cuadricoptero
18
𝑈1= 𝐾𝑑𝑟𝑜𝑙𝑙(𝜙̇𝑟𝑒𝑓− 𝜙̇) + 𝐾𝑝𝑟𝑜𝑙𝑙(𝜙𝑟𝑒𝑓− 𝜙) = 𝜏𝜙 𝑈2= 𝐾𝑑𝑝𝑖𝑡𝑐ℎ(𝜃̇𝑟𝑒𝑓− 𝜃̇) + 𝐾𝑝𝑝𝑖𝑡𝑐ℎ(𝜃𝑟𝑒𝑓− 𝜃) = 𝜏𝜃 𝑈3= 𝐾𝑑𝑦𝑎𝑤(𝜓̇𝑟𝑒𝑓− 𝜓̇) + 𝐾𝑝𝑦𝑎𝑤(𝜓𝑟𝑒𝑓− 𝜓) = 𝜏𝜓 𝑈4= 𝐾𝑑𝑧(𝑧̇𝑟𝑒𝑓− 𝑧̇) + 𝐾𝑝𝑧(𝑧𝑟𝑒𝑓− 𝑧)
𝑚
𝐶𝑜𝑠𝜃𝐶𝑜𝑠𝜙= 𝑇 (37)
A partir de las ecuaciones presentadas en (29), es posible despejar los valores de las velocidades angulares una vez definidos los torques según (37) [1]:
Ω12= 𝑇 4𝑘−
𝜏𝜃 2𝑘𝑙−
𝜏𝜓 4𝑏 Ω22=
𝑇 4𝑘− 𝜏𝜙 2𝑘𝑙+ 𝜏𝜓 4𝑏 Ω32=
𝑇 4𝑘+ 𝜏𝜃 2𝑘𝑙− 𝜏𝜓 4𝑏 Ω42=
𝑇 4𝑘+ 𝜏𝜙 2𝑘𝑙+ 𝜏𝜓 4𝑏 (38)
A partir de estas nuevas ecuaciones, es posible definir de las ecuaciones diferenciales (34), (35) y (36) ya que como se puede observar, estas contienen los términos Ω1, Ω2, Ω3, Ω4. Para ellos se definirán unos nuevos
términos que representan las señales de control de las ecuaciones diferenciales [1].
U11= 𝑘(Ω12+ Ω22+ Ω32+ Ω42) U22= 𝑘(−Ω22+ Ω42)
U33= 𝑘(Ω12− Ω32) U44 = 𝑏(Ω12− Ω22+ Ω32− Ω42)
Ω𝑟= (Ω1− Ω2+ Ω3− Ω4) (39)
Utilizando estas señales de control, se pueden recrear las ecuaciones diferenciales de los ángulos de la siguiente forma:
𝜙̈ =𝐽𝑟𝜃̇Ω𝑟 𝐼𝑥𝑥
+ 𝜓 ̇𝜃̇𝐼𝑦𝑦− 𝐼𝑧𝑧 𝐼𝑥𝑥
+𝑙U22 𝐼𝑥𝑥
𝜃̈ = −𝐽𝑟𝜙̇Ω𝑟 𝐼𝑦𝑦
+ 𝜓 ̇𝜙̇𝐼𝑧𝑧− 𝐼𝑥𝑥 𝐼𝑦𝑦 +
𝑙U33 𝐼𝑦𝑦
𝜓̈ =U44 𝐼𝑧𝑧
+ 𝜃 ̇𝜙̇ 𝐼𝑥𝑥− 𝐼𝑦𝑦 𝐼𝑧𝑧 (40)
En este punto, se cuentan con todas las herramientas para generar los lazos de control para cada Angulo. Para generar el esquema de control se usó la herramienta de Simulink y Matlab, con el fin de simular el funcionamiento del controlador PD. Como punto inicial, ese necesario establecer las constantes que se requieren en las ecuaciones, como los momentos de inercia y valores de arrastre y empuje, que dependen exclusivamente de los elementos del sistema.
Para calcula los momentos de Inercia del Sistema, es necesario asumir que las partes de cuadricoptero se asemejan a objetos geométricos de los cuales se conocen las ecuaciones se sus momentos. Se asume que el
Diseño e implementación de un controlador de estabilidad para un Cuadricoptero
19
cuadricoptero se puede modelar como se presenta en la siguiente figura, y además se presentan los valores estimados a partir del análisis.
Figura 6. Modelo del cuadricoptero para el cálculo de inercias y valores estimados.
Los motores se modelaron como un cilindro sólido, con inercias
𝐼𝑥 = 𝐼𝑦 = 𝑚 (𝑟 2 4 +
ℎ2 12)
Donde m es la masa de cada motor, r correspondería al radio del motor y h a su altura.
Los rotores o hélices se modelaron como un disco plano, cuyas inercias corresponden a
𝐼𝑥 = 𝐼𝑦 =1 4𝑚𝑟
2, 𝐼𝑧 =1 2𝑚𝑟
2
Donde m corresponde a la masa de cada hélice, y r a su longitud.
Finalmente se decidió modelar el cuerpo del vehículo como una esfera. Teniendo en cuenta que la distancia de motor a motor es de 350mm, se fijó que el radio de la esfera para modelar el resto del cuerpo del vehículo sería 116mm.
𝐼𝑥 = 𝐼𝑦 = 𝐼𝑧 =2 5𝑚𝑟
2
Donde m corresponde a la masa del vehículo con la batería pero sin tomar en cuenta la masa de rotores y motores.
Para el cálculo de inercias, igualmente se debe tener en cuenta el teorema de ejes paralelos, el cual enuncia que cuando el eje de rotación de un objeto no corresponde al origen (en este caso el origen es el eje de rotación del cuadricóptero), se deben corregir los momentos de inercia de acuerdo a la siguiente relación:
𝐼𝑑𝑖𝑠𝑝𝑙𝑎𝑐𝑒𝑑= 𝐼𝑑𝑖𝑠𝑝𝑙𝑎𝑐𝑒𝑑 𝑚 ∙ 𝐷2
Donde m corresponde a la masa del objeto que está alejado del origen y D la distancia del cuerpo al origen. Este cálculo debe ser tenido en cuenta para las inercias de los motores y de los propulsores, pues estos no están en el origen.
Teniendo en cuenta estas consideraciones se puede hallar la matriz de inercia de los motores, de los propulsores y del cuerpo. Finalmente la suma de estas tres da como resultado la inercia total del vehículo.
Constantes Valor
Gravedad(m/s^2) 9,81
Masa Rotor (Kg) 0,011
Masa Motor (Kg) 0,056
Masa Bateria (Kg) 0,0185
Masa Carcasa (Kg) 0,367
Masa Total (Kg) 0,6535
Distancia l (m) 0,175
Radio Esfera Centro (m) 0,1167
Tamaño Rotor (m) 0,2023
Radio Rotor (m) 0,014
Altura Motor (m) 0,038
k 3,13E-05 b 7,5E-07 Ixx 0,0053 Iyy 0,0053 Izz 0,0076 Jr 2,71E-04
Diseño e implementación de un controlador de estabilidad para un Cuadricoptero
20
Una vez calculadas las constantes, es posible realizar los cálculos dentro del sistema de control y observar su funcionamiento. Para ello, se propone el esquema presentado en la Figura 7. Este esquema se divide en varios módulos los cuales corresponde al módulo de generación de referencias, el modulo PD, el modelo del cuadricoptero y por último la visualización.
Figura 7 Esquema del sistema de control.
Generación de referencias:para efectos prácticos, se optó por usar el bloque de pilot Joystick en Simulink que se encarga de convertir los datos obtenidos de un control de XBOX 360, a los ángulos del sistema de navegación del cuadricoptero. Se estableció que el análogo solo puede entregar una referencia máxima de 20° o -20° tanto para el roll como para el pitch, ya que en el mundo real, un ángulo mayor podría descontrolar muy fácil es sistema. Cabe mencionar que las referencias están dadas en grados, y es necesario hacer la conversión a radiantes antes de hacer el control.
De igual manera también se realizaron simulaciones usando entradas como escalones y sinusoides, con el fin de observar parámetros como el tiempo de establecimiento y overshoot. Los resultados para todos los casos evaluados se presentan más adelante.
Controlador:como se ha mencionado, el control corresponde a un PD. El esquema correspondiente al control de cada ángulo se presenta en la Figura 8. Para la determinación de los parámetros del PD se utilizaron las herramientas de sintonización de Matlab, y se revisaron varios documentos como referencia [1] [7] [8] [9].
Diseño e implementación de un controlador de estabilidad para un Cuadricoptero
21
Figura 8 Esquema del controlador PD
Como se puede observar, es necesario un lazo de control para cada canal, es decir, para el roll, pitch, yaw y la altura.
Modelo del cuadricoptero:El esquema para el modelo matemático del cuadricoptero se presenta en la Figura 9[1], [7]. Dentro del modelo del cuadricoptero se implementan las ecuaciones (38), (39), (40) tal cual como están descritas. Primero se obtienen los valores de Ω1, Ω2, Ω3, Ω4 a partir de las salidas del bloque del controlador. Luego, se calculan U11, U22, U33, U45 y Ω𝑟, y se generan las ecuaciones diferenciales para cada
ángulo. Para obtener los valores de los ángulos basta con integrar el valor de la aceleración angular dos veces. Respecto a la altura, es necesario realizar la transformación del plano del cuerpo al plano inercial o de la tierra usando la matriz (3).
Figura 9 Esquema del modelo matemático del cuadricoptero
7.2.2 Controlador I-PD
Adicionalmente al control PD propuesto, se realizó una modificación al esquema presentado en la Figura 7, especialmente dentro del esquema de control de la Figura 8, creando un lazo I-PD. El Control I-PD es una
Diseño e implementación de un controlador de estabilidad para un Cuadricoptero
22
variación del PID donde cambia un poco la estructura de retroalimentación. La diferencia radica en que la retroalimentación entra directamente al proporcional y al derivativo y estos se restan al integral en vez de ser sumados. El esquema del control I-PD se presenta en la Figura 10.
Figura 10 Esquema del controlador I-PD
7.3 Adquisición y procesamiento de datos de los sensores
Un factor importante dentro de un sistema de control retroalimentado, es la obtención de la información de los cambios dinámicos del sistema en el ambiente. Para el caso específico del cuadricoptero, es esencial el medir el estado actual del sistema en el ambiente y los cambios dinámicos que ocurren en el en cada momento. Para ello, el sensor adecuado para medir los ángulos del sistema sobre los cuales se ha venido trabajando, es una IMU (inertial measurement unit), o Unidad de medición Inercial.
7.3.1 Adquisición de datos
Con una IMU es posible determinar el roll, el pitch, y el yaw en tiempo real para fines de control. Para poder obtener los valores del roll, el pitch, y el yaw útiles para el control, es necesario entender el funcionamiento de cada uno de los sensores, y la forma en como estos entregan los datos en bruto, para poder realizar un procesamiento correcto.
La IMU usada para la realización de este trabajo fue la MinIMU-9 V3 [10] de pololu.
Acelerómetro:El acelerómetro es un sensor que mide las fuerzas gravitaciones que sufre el sistema. Para el cuadricoptero se cuenta con un acelerómetro 3-D, que informa de las aceleraciones que ocurren sobre cada eje.
Aunque sobre las componentes del acelerómetro es posible determinar el pitch y el roll, existen dos problemas que hacen que este no sirva completamente para determinar estos ángulos. La primera se debe a que la componente de la gravedad siempre estará presente y se sumara con las aceleraciones del movimiento propias del sistema, por lo cual las medidas siempre incluirán estos valores sumados. La segunda razón es
Diseño e implementación de un controlador de estabilidad para un Cuadricoptero
23
debida al ruido intrínseco de la medición del sensor. Es por esta razón que en necesaria una fusión multisensorial que se explicara más adelante.
Los datos de la IMU se obtienen por comunicación I2C, el cual entrega paquetes de 2 bytes por eje para cada sensor, es decir, 6 bytes por sensor y 18 bytes en total. Ahora, el acelerómetro que se incluye dentro de la
IMU MinIMU-9 V3 es el LSM303D [11] cuyo rango de medida por defecto es de ±2𝑔, y por ende 0.061 mg/LSB
para los 16 bits.
Giroscopio: El giroscopio es un sensor que mide las velocidades angulares del sistema sobre los tres ejes de referencia.
De igual manera es posible obtener los datos del pitch y el roll directamente del giroscopio por simple integración de los datos, pero similar al acelerómetro, no son útiles para el control debido a que el giroscopio cuenta con un BIAS (offset) por defecto, que al ser integrado se va acumulando. El resultado entonces será el valor de ángulo muy limpio (Sin ruido) sumado al error acumulado por la integración del BIAS.
El giroscopio incluido en la IMU MinIMU-9 V3 es el L3GD20H [12], cuyo rango por defecto es ±245dps, y por ende 8.75mdps/LSB para los 16 bits.
Magnetómetro: El magnetómetro es un sensor que mide los campos magnéticos alrededor del sistema y representa los datos sobre los 3 ejes de referencia.
A partir del magnetómetro, para los casos donde el sistema es estacionario, es posible determinar en ángulo yaw del cuerpo respecto a su sistema de referencia. Para el caso del cuadricoptero, ya que el marco de referencia del cuerpo se mueve con el sistema dentro del marco de referencia inercial, el valor del Yaw requiere una compensación de inclinación a partir de los datos del pitch y el roll.
El magnetómetro incluido en la IMU MinIMU-9 V3 también se incluye en el LSM303D [11], y su rango por defecto es de ±2 𝑔𝑎𝑢𝑠𝑠, y por ende 0.080 mgauss/LSB para los 16 bits.
7.3.1.1 Modelo Matemático del giroscopio y acelerómetro
Como trabajo adicional con el objetivo de entender los conceptos físicos involucrados, y buscar métodos adicionales de pruebas, se desarrollaron en Simulink los modelos matemáticos del acelerómetro y del giroscopio.
Los modelos matemáticos de un acelerómetro y giroscopio siguen la misma teoría. Consisten de un sistema resorte amortiguador que corresponden a un modelo de segundo orden. Adicionalmente, estos sistemas de medición poseen dos tipos de ruido en sus salidas, los cuales se especifican en los datasheets de los fabricantes como potencia del ruido y bias.
Diseño e implementación de un controlador de estabilidad para un Cuadricoptero
24
Figura 11. Modelo en Simulink del giroscopio
Figura 12. Modelo en Simulink del Acelerómetro.
A partir de estos modelos, se creó un caso de estudio teórico, donde un sistema presentaba una rotación perfecta de 0°-45°-0° sucesivamente, con una duración de 1 segundo en cada valor. Sobre este sistema de medida matemático, se realizó la primera prueba de los algoritmos de procesamiento y filtro de Kalman.
7.3.2 Procesamiento de Datos
Una vez entendido como son entregados los datos en bruto por parte de los sensores, se debe de generar un algoritmo para obtener información que realmente puede ser utilizada por el sistema de control. Para ello se propone el esquema de integración multisensorial presentado en la Figura 13, que se encarga de calcular el
pitch, el roll y el yaw a partir de todos los datos que se obtienen por las mediciones.
Diseño e implementación de un controlador de estabilidad para un Cuadricoptero
25
Procesamiento del Acelerómetro: Para obtener la estimación del Pich (Pitch*) y el Roll (Roll*) a partir del acelerómetro se deben de utilizar las siguientes ecuaciones, siendo AccelX, AccelY, AccelZ los datos obtenidos por el acelerómetro [14] [15]:
𝑃𝑖𝑡𝑐ℎ = 𝐴𝑟𝑐𝑡𝑎𝑛 ( 𝐴𝑐𝑐𝑒𝑙𝑋
√𝐴𝑐𝑐𝑒𝑙𝑌2+ 𝐴𝑐𝑐𝑒𝑙𝑍2) 𝑅𝑜𝑙𝑙 = 𝐴𝑟𝑐𝑡𝑎𝑛 ( 𝐴𝑐𝑐𝑒𝑙𝑌
√𝐴𝑐𝑐𝑒𝑙𝑋2+ 𝐴𝑐𝑐𝑒𝑙𝑍2) (41)
Estas ecuaciones se encuentran en radianes y pueden ser convertidas en grados en cualquier momento posterior
Procesamiento del Giroscopio: Como se mencionó anteriormente, es posible determinar el Pitch (Pitch**) y el Roll (Roll**) a partir del giroscopio, pero con errores considerables a causa de la integración del Bias. Básicamente lo que ocurre es que el valor calculado de los ángulos comienza a derivar (Drift) conforme el tiempo pasa, es decir, se genera un offset cada vez mucho mayor a medida que transcurre el tiempo.
Para obtener los ángulos, es posible realizar una integración trapezoidal conociendo la medida actual del giroscopio, y la medida inmediatamente anterior, además de conocer el tiempo de muestreo. La ecuación de la regla del trapecio simple es:
∫ 𝑓(𝑥)𝑑𝑥 ≈ b
a
(𝑏 − 𝑎)𝑓(𝑎) + 𝑓(𝑏) 2 (42)
Ya que no es posible determinar un valor preciso para el pitch y el roll con los sensores por separado, es necesario aplicar algún método de integración sensorial como un filtro de Kalman o un filtro complementario. Estos métodos se explican más adelante
Fusión por filtro de Kalman: el filtro de Kalman es un algoritmo matemático que sirve para predecir el estado de un sistema a partir de las mediciones realizas. Básicamente el proceso de divide en dos etapas, la primera etapa es la de predicción y la segunda es la de corrección.
Para implementar el filtro se deben de seguir los siguientes pasos [14] [16] [17]:
1. 𝑥̂𝑘|𝑘−1 = 𝐴𝑥̂𝑘−1|𝑘−1+ 𝐵𝜃̇𝑘 - Se proyecta el estado a priori.
2. 𝑃𝑘|𝑘−1= 𝐴𝑃𝑘−1|𝑘−1𝐴𝑇+ 𝑄𝑘 - Se proyecta la covarianza del error
3. 𝑦̌𝑘= 𝑧𝑘− 𝐻𝑥̂𝑘|𝑘−1
4. 𝑆𝑘= 𝐻𝑃𝑘|𝑘−1𝐻𝑇+ 𝑅
5. 𝐾𝑘= 𝑃𝑘|𝑘−1𝐻𝑇𝑆𝑘−1 - Se computa la ganancia de Kalman
6. 𝑥̂𝑘|𝑘=𝑥̂𝑘|𝑘−1+ 𝐾𝑘𝑦̌𝑘 - Se actualiza el estimado de la medida 7. 𝑃𝑘|𝑘 = (𝐼 − 𝐾̌𝑘𝐻)𝑃𝑘|𝑘−1 - Se actualiza la covarianza del error
(Para mayores detalles de la terminología. referirse a la sección 4.1.8 del marco teórico sobre el Filtro de Kalman)
Para el caso del cuadricoptero, siendo el enfoque la integración entre el giroscopio y el acelerómetro, entonces se definen las siguientes matrices [17]:
Diseño e implementación de un controlador de estabilidad para un Cuadricoptero
26
𝐴 = [1 −∆𝑡
0 1 ] , 𝐵 = [ ∆𝑡
0] , 𝐻 = [1 0], 𝑃0= [ 0 0
0 0] , 𝑄 = [
0.001 0
0 0.003] , 𝑅 = 0.03 𝜃̇𝑘− 𝐷𝑎𝑡𝑜 𝑑𝑒𝑙 𝐺𝑖𝑟𝑜𝑠𝑐𝑜𝑝𝑖𝑜
𝑧𝑘− 𝐴𝑛𝑔𝑢𝑙𝑜 𝑐𝑎𝑙𝑐𝑢𝑙𝑎𝑑𝑜 𝑎 𝑝𝑎𝑟𝑡𝑖𝑟 𝑑𝑒𝑙 𝐴𝑐𝑒𝑙𝑒𝑟𝑜𝑚𝑒𝑡𝑟𝑜
Ya que el filtro tal y como está especificado solo procesa una variable, se requiere de un filtro de Kalman para el pitch, y de otro filtro de Kalman para el roll. Para el caso del Yaw no es totalmente necesario aplicar un filtro ya que su adquisición es diferente a la del pitch y el roll
Fusión por filtro complementario: el filtro complementario es un algoritmo que fusiona la información de los sensores aplicando unas ganancias a cada dato. Se denomina complementario por que la sumatoria de las ganancias es igual a 1 [13] [19].
Para aplicar el filtro es necesario calcular de antemano los ángulos 𝜃𝑔𝑖𝑟𝑜, 𝜃𝑎𝑐𝑒𝑙, ϕ𝑔𝑖𝑟𝑜, ϕ𝑎𝑐𝑒𝑙, y se deben de
tener en cuenta las siguientes consideraciones que explican los valores de ganancia:
- La diferencia de una muestra y otra en un intervalo de tiempo es más confiable en el giroscopio que en el acelerómetro debido a que hay mucho menos ruido
- Debido al bias del giroscopio, es necesario usar los datos del acelerómetro para lograr la convergencia del resultado
Para usar los datos del giroscopio se hace uso del siguiente algoritmo iterativo:
( 𝛿ϕ/dt
𝛿θ/dt 𝛿ψ/dt
) = 1 cos ϕ [
0 sin ϕ 𝑐𝑜𝑠ϕ 0 cosϕcosθ −𝑠𝑖𝑛ϕcosθ cos θ 𝑠𝑖𝑛ϕsinθ 𝑐𝑜𝑠ϕcosθ
] [ 𝜔𝑥 𝜔𝑦 𝜔𝑧 ]
Donde los ángulos se obtienen como:
ɸ
⃗⃗ 𝑔𝑦𝑟𝑜(𝑡) = ( ϕ(t)
θ(t) ψ(t)
) = (
ϕ(t − 1) θ(t − 1) ψ(t − 1)
) + ( 𝛿ϕ/dt
𝛿θ/dt 𝛿ψ/dt
) ∆𝑡
Finalmente la ecuación resultante para el filtro complementario es [13] [19]:
𝜃 (𝑡) = 0.02 ∙ 𝜃 𝑎𝑐𝑒𝑙(𝑡) + 0.98 ∙ 𝜃 𝑔𝑦𝑟𝑜(𝑡) (43)
Procesamiento del Magnetómetro: Para obtener el valor del Yaw en estado estacionario basta con aplicar la siguiente formula:
𝑌𝑎𝑤 = 𝑎𝑡𝑎𝑛2 (−𝑚𝑦 𝑚𝑥 ) (43)
Como se mencionó, para el caso preciso de un cuadricoptero es necesario hacer una corrección de inclinación usando los ángulos del pitch y el roll resultantes de los filtros de Kalman o filtros complementarios, de la siguiente forma [14]:
𝑋𝐻 = 𝑚𝑥𝐶𝑜𝑠(𝜃) + 𝑚𝑦𝑆𝑖𝑛(𝜃)𝑆𝑖𝑛(ϕ) + 𝑚𝑧𝑆𝑖𝑛(𝜃)𝐶𝑜𝑠(ϕ) 𝑌𝐻 = 𝑚𝑦𝐶𝑜𝑠(ϕ) − 𝑚𝑧𝑆𝑖𝑛(ϕ)
𝜓 = 𝑎𝑡𝑎𝑛2 (−𝑌𝐻 𝑋𝐻 ) (44)
Diseño e implementación de un controlador de estabilidad para un Cuadricoptero
27
7.3.3 Implementación
Descritas las formas de adquisición y procesamiento de los datos, se procedió a realizar un código en Matlab y un esquema en Simulink, encargados de comprobar el funcionamiento de los algoritmos con datos reales en tiempo real. El bloque de Simulink que se presenta en la Figura 14 tiene implementando los mismos algoritmos escritos en el código de Matlab y que se describieron anteriormente. La idea de usar Simulink en esta situación se debe a que es posible ver los algoritmos en acción en tiempo real, mientras que en Matlab solo se pueden observar los resultados al final de la toma de datos.
Figura 14 Modulo en simulink de toma y procesamiento de datos
Para usar este bloque, se usó una tarjeta Arduino Duemilanove para la adquisición de datos de la MinIMU, que se comunicó mediante el puerto serial con Matlab y Simulink para el procesamiento de la información. El bloque consta de un módulo de separación de datos que provienen del puerto serial, un módulo de calibración de los datos, uno de conversión de los datos en bruto en datos útiles, y los módulos de procesamiento como los de los cálculos de los ángulos y los filtros de Kalman.
7.4 Modulo de comunicación I2C para adquisición de datos del MinIMU9 V-3 en FPGA
Partiendo de la información presentada en el marco teórico, para empezar es necesario definir las direcciones de esclavo de los sensores de la IMU. Estas direcciones están detalladas en la página principal del fabricante [10], y también dentro de los respectivos datasheets de los sensores con una mayor explicación [11] [12].
Inicialización del giroscopio: El sensor L3GD20H, cuya dirección de esclavo es 1101011b, contiene un set de registros internos que contienen la información de funcionamiento y configuración. Estos registros de igual manera son direcciones de 7 bits, y la información que contienen son de 1 byte (8 bits). Dentro de estos registros internos se almacenan también los datos de las velocidades angulares recolectadas por el giroscopio sobre los 3 ejes del marco del cuerpo, es decir p,q y r, en paquetes de 2 bytes por eje, es decir, 16 bits para cada eje de referencia.
Las estructuras de la comunicación para interactuar con los registros internos se especifican en el datasheet del sensor [11] [12], en cuatro maneras diferentes:
Diseño e implementación de un controlador de estabilidad para un Cuadricoptero
28
Tabla 1. Estructura para escribir un byte desde maestro
Tabla 2. Estructura para escribir múltiples bytes desde maestro
Tabla 3. Estructura para leer un byte de un esclavo
Tabla 4.Estructura para leer múltiples bytes de un esclavo
Tabla 5. Direcciones de los registros internos de mayor importancia del L3GD20H [12]
Antes de poder acceder a esta información, es necesario configurar y calibrar el sensor. Para ello es necesario acceder primero a los registros de configuración y escribir la información que permite modificar el funcionamiento.
Para empezar, se debe modificar el registro CTRL1 para cambiar el funcionamiento del sensor a Modo normal y habilitar las mediciones en los tres ejes, es decir, acceder al registro 0100000 y escribir 00001111. Para mayor información, ver tablas 19, 20 y 21 del datasheet del L3GD20H [12].
Luego, se debe modificar el registro CTRL4 cuya dirección es 0100011, y se debe de escribir 00110000 para establecer el rango a 245 dps.
Inicialización del Acelerómetro y magnetómetro: Una vez inicializado el giroscopio, se inicializa el acelerómetro y magnetómetro. La dirección de esclavo del LSM303D acorde a la página del fabricante [10] es 0011101b. Revisando el respectivo datasheet [11], se obtiene la siguiente tabla con las subdirecciones importantes del sensor:
Diseño e implementación de un controlador de estabilidad para un Cuadricoptero
29
Tabla 6. Direcciones de los registros internos de mayor importancia del LSM303D [11]
De la misma forma que el giroscopio, es necesario inicializar el acelerómetro y el magnetómetro. Primero se debe modificar el registro CTRL1 con subdirección 0100000 para activar los ejes del acelerómetro y seleccionar el ODR. Para esto se debe de escribir 10100111. Para mayor información, ver tablas 19, 20 y 21 del datasheet del L3GD20H [12].
Luego, se debe modificar el registro CTRL5 cuya dirección es 0100100, y se debe de escribir 01110100 para habilitar el magnetómetro y la frecuencia de muestreo en 100Hz. Por defecto el rango del acelerómetro es de ±2g y el del magnetómetro es de ±4gauss. Para modificar estos valores de rango de debe de acceder al registro CTRL2 para el acelerómetro, y al registro CTRL6 para el magnetómetro.
Adquisición de datos del giroscopio, acelerómetro y magnetómetro: Para poder obtener los datos de cada uno de los sensores, se debe de seguir el protocolo mostrado en la tabla X4 después de que se hayan
Diseño e implementación de un controlador de estabilidad para un Cuadricoptero
30
inicializado los sensores. Se deben de identificar las subdirecciones correspondientes a los registros de datos para poder solicitarlos desde el maestro, el cual es la FPGA.
Incluyendo la inicialización, se creó un código en Verilog el cual cuenta con una máquina de estados aproximada a la presentada en las siguientes figuras:
Figura 15. Máquina de estados para inicialización de Sensores
