Diseño e implementación de un sistema de navegación inercial tipo strapdown para estimar la posición de un robot móvil, aplicable a un prototipo de autopiloto de un UAV

Texto completo

(1)La versión digital de esta tesis está protegida por la Ley de Derechos de Autor del Ecuador. Los derechos de autor han sido entregados a la “ESCUELA POLITÉCNICA NACIONAL” bajo el libre consentimiento de los autores. Al consultar esta tesis deberá acatar con las disposiciones de la Ley y las siguientes condiciones de uso:. · Cualquier uso que haga de estos documentos o imágenes deben ser sólo para efectos de investigación o estudio académico, y usted no puede ponerlos a disposición de otra persona. · Usted deberá reconocer el derecho del autor a ser identificado y citado como el autor de esta tesis. · No se podrá obtener ningún beneficio comercial y las obras derivadas tienen que estar bajo los mismos términos de licencia que el trabajo original.. El Libre Acceso a la información, promueve el reconocimiento de la originalidad de las ideas de los demás, respetando las normas de presentación y de citación de autores con el fin de no incurrir en actos ilegítimos de copiar y hacer pasar como propias las creaciones de terceras personas.. Respeto hacia sí mismo y hacia los demás..

(2) ESCUELA POLITÉCNICA NACIONAL FACULTAD DE INGENIERÍA ELÉCTRICA Y ELECTRÓNICA. DISEÑO E IMPLEMENTACIÓN DE UN SISTEMA DE NAVEGACIÓN INERCIAL TIPO STRAPDOWN PARA ESTIMAR LA POSICIÓN DE UN ROBOT MÓVIL, APLICABLE A UN PROTOTIPO DE AUTOPILOTO DE UN UAV. PROYECTO PREVIO A LA OBTENCIÓN DEL TÍTULO DE INGENIERO EN ELECTRÓNICA Y CONTROL. JORGE DAVID GUALLICHICO LOACHAMÍN ([email protected]) CARLOS ADRIÁN UTRERAS CAIZA ([email protected]). DIRECTOR: MSc. PATRICIO BURBANO ([email protected]) CO-DIRECTOR: Dr. ANDRÉS ROSALES ([email protected]) Quito, Febrero 2013.

(3) ii. DECLARACIÓN. Nosotros, Jorge David Guallichico Loachamín y Carlos Adrián Utreras Caiza, declaramos bajo juramento que el trabajo aquí descrito es de nuestra autoría; que no ha sido previamente presentada para ningún grado o calificación profesional; y, que hemos consultado las referencias bibliográficas que se incluyen en este documento.. A través de la presente declaración cedemos nuestros derechos de propiedad intelectual correspondientes a este trabajo, a la Escuela Politécnica Nacional, según lo establecido por la Ley de Propiedad Intelectual, por su Reglamento y por la normatividad institucional vigente.. ______________________ Jorge David Guallichico Loachamín CI: 171922053-3. ___________________ Carlos Adrián Utreras Caiza CI: 171842139-7.

(4) iii. ESCUELA POLITÉCNICA NACIONAL “E. SCIENTIA. HOMINIS. SALUS”. La versión digital de esta tesis está protegida por la Ley de Derechos del Autor del Ecuador. Los derechos de autor han sido entregados a la “ESCUELA POLITÉCNICA NACIONAL” bajo el libre consentimiento de los autores. Al consultar esta tesis deberá acatar las disposiciones de la Ley y de las siguientes condiciones de uso:. ·. Cualquier uso que haga de estos documentos o imágenes deben ser sólo para efectos de investigación o estudio académico, y usted no puede ponerlos a disposición de otra persona.. ·. Usted deberá reconocer el derecho del autor a ser identificado y citado como el autor de esta tesis.. ·. No se podrá obtener ningún beneficio comercial y las obras derivadas tienen que estar bajo los mismos términos de licencia que el trabajo original.. El libre Acceso a la información, promueve el reconocimiento de la originalidad de las ideas de los demás, respetando las normas de presentación y de citación de autores con el fin de no incurrir en actos ilegítimos de copiar y hacer pasar como propias las creaciones de terceras personas.. Respeto hacia sí mismo y hacia los demás.

(5) iv. CERTIFICACIÓN. Certifico que el presente trabajo fue desarrollado por Jorge David Guallichico Loachamín y Carlos Adrián Utreras Caiza, bajo mi supervisión.. ________________________ MSc. Patricio Burbano Romero DIRECTOR DEL PROYECTO. ________________________ Dr. Andrés Rosales Acosta CO-DIRECTOR DEL PROYECTO.

(6) v. AGRADECIMIENTO. Agradezco a mis padres por haberme brindado el apoyo necesario a lo largo de mi vida académica, a mi madre por apoyarme emocionalmente en todo momento y a mi padre por estar conmigo siempre que lo he necesitado. A mi amigo y compañero Jorge Guallichico, por su amistad, paciencia y dedicación, sin lo cual no hubiese sido posible realizar este proyecto de titulación. A mi director, MSc. Patricio Burbano por interesarse en este proyecto de titulación, brindándonos su conocimiento, su experiencia y sus consejos para concluir el presente trabajo. A mi co-director, Dr. Andrés Rosales por su confianza brindada, su apoyo y su buena energía a lo largo del proceso de ejecución de este trabajo. Al director del proyecto UAV (Unmanned Aerial Vehicle), Dr. Eduardo Ávalos por otorgarnos todas las facilidades para la elaboración del sistema de navegación inercial. A mis amigos, por escucharme, comprenderme y darme ánimos en los bueno y malos momentos, ya que ellos siempre han sido testigos de mis triunfos en la vida.. Carlos Adrián Utreras.

(7) vi. AGRADECIMIENTO. Agradezco a todos quienes me apoyaron durante, mi vida académica especialmente a mi madre quién con su esfuerzo me ha enseñado que el trabajo duro y constante siempre da sus frutos. A mi padre y a mi hermana con quienes siempre pudo contar cuando necesito ayuda o un consejo. A los grandes amigos que encontré con el paso de los años especialmente, a los de mi vida universitaria, quienes compartieron sus experiencias conmigo y me ayudaron a ser una mejor persona. A mis profesores que en las aulas dejaron su conocimiento depositado en mi persona y que me enseñaron el valor de superarse profesionalmente con el esfuerzo y dedicación propia. Al MSc. Patricio Burbano director del proyecto, por todo el tiempo que dedicó al desarrollo del proyecto, por la ayuda y las facilidades proporcionadas para que el proyecto pudiera ser llevado a cabo. Al Dr. Andrés Rosales co-director del proyecto, por toda la ayuda brinda a lo largo del desarrollo del proyecto de titulación, por su tiempo y por ser con los estudiantes antes que un catedrático un amigo con el cual puedes contar.. Jorge David Guallichico.

(8) vii. DEDICATORIA Dedico este proyecto a mis padres Rosa y Carlos por el cariño que me han brindado y por haber sido parte de todos los logros realizados en mi vida, también al MSc. Patricio Burbano, Dr. Andrés Rosales y Dr. Eduardo Ávalos por la confianza que depositaron en mi persona, y a mis seres queridos por siempre apoyarme.. Carlos Adrián Utreras.

(9) viii. DEDICATORIA A mi madre quien fue la inspiración en mi vida para luchar día a día, a la memoria de mi abuelita quien estuvo a mi lado hasta sus últimos días y a todas las personas quienes forman parte de mi vida y de quienes sigo aprendiendo, nuevas cosas cada día de mi vida.. Jorge David Guallichico.

(10) ix. CONTENIDO RESUMEN………………………………………………………………. xiii. PRESENTACIÓN……………………………………………………... xiv. CAPÍTULO 1…………………………………………………………………….….... 1. MARCO TEÓRICO…………………………………………………………………. 1. 1.1. SISTEMA DE NAVEGACIÓN INERCIAL…………………………...………. 1. 1.1.1. CONFIGURACIÓN DEL SISTEMA INERCIAL………………….………. 1. 1.1.1.1. Gimballed (ejes flotantes)……………………………………………..... 2. 1.1.1.1.1. Referencia Inercial………………………………………………. 3. 1.1.1.1.2. Referencia terrestre……………………………………………... 3. 1.1.1.2. Strapdown (ejes fijos)…………………………………………………... 3. 1.2.2. SISTEMAS DE COORDENADAS…………………………………………. 4. 1.2.2.1. Sistema de Coordenada Inercial Verdadero…………………………….. 4. 1.2.2.2. Sistema de Coordenadas ECI (Earth Centered Inertial)……………….. 5. 1.2.2.3. Sistema de Coordenadas ECEF (Earth - Earth Centered - Fixed)…….... 5. 1.2.2.4. Sistema de Coordenadas de Navegación (n-frame)…………………….. 5. 1.2.2.5. Sistema de Coordenadas Body (b-frame)…………………………...... 6. 1.2. UNIDAD DE MEDIDAS INERCIALES (IMU)………………………………. 7. 1.2.1. ACTITUD Y ORIENTACIÓN…………………………………………….... 7. 1.2.1.1. Componentes de AHRS………………………………………………... 7. 1.2.1.2. Ángulos de Euler……………………………………………………….. 8. 1.3. SENSORES…………………………………………………………………......... 8. 1.3.1. ACELERÓMETRO……………………………………………………….... 8. 1.3.2. GIROSCOPIO…………………………………………………….………... 9. 1.3.3. MAGNETÓMETRO……………………………………………….……….. 10. 1.3.4. GLOBAL POSITIONING SYSTEM (GPS)…………………….…………... 11. 1.3.4.1. Parámetros de un GPS………………………………………………….. 11. 1.3.4.2. Funcionamiento de un GPS…………………………………………….. 11. 1.4. FILTRO DE KALMAN (FK)………………………………………………….... 12. 1.4.1. ECUACIONES DE KALMAN…...…………………………………………. 12.

(11) x. 1.4.2. FILTRO DE KALMAN EXTENDIDO (FKE)…..…………………………. 14. 1.5 UNMANNED AERIAL VEHICLE (UAV)…………………………………….. 15. 1.5.1 AUTOPILOTO……………………………………………………………….. 16. CAPÍTULO 2……………………………………………………………………......... 17. DISEÑO Y CONSTRUCCIÓN DEL HARDWARE………………………………. 17. 2.1.. DESCRIPCIÓN. Y. CARACTERÍSTICAS. DE. LOS. SENSORES. EMPLEADOS................................................................................................................ 17. 2.1.1 ESPECIFICACIONES TÉCNICAS DEL SENSOR LSM303DLM…………. 17. 2.1.2 ESPECIFICACIONES TÉCNICAS DEL SENSOR 6DOF DIGITAL……….. 18. 2.2. PROTOCOLOS DE COMUNICACIÓN EMPLEADOS……………….……. 18. 2.2.1 COMUNICACIÓN ENTRE SENSORES Y MICROCONTROLADOR……. 19. 2.2.2 COMUNICACIÓN ENTRE EL MICRONTROLADOR Y LA PC…………. 20. 2.2.3 PROTOCOLO DE COMUNICACIÓN DEL GPS………..…………………. 21. 2.3 DISTRIBUCIÓN DE LOS SENSORES EN LA PLATAFORMA MÓVIL….. 21. 2.3.1 CAMBIO DE COORDENADAS DE BODY A NAVEGACIÓN……..…….. 23. 2.4. SISTEMA DE MEDICIÓN DE ACTITUD……………………………………. 25. 2.5. SISTEMA DE MEDICIÓN DE ORIENTACIÓN……………..……………... 25. 2.5.1. CALCULO. DE. ORIENTACIÓN. SIN. COMPENSACIÓN. DE. INCLINACIÓN……………………………………………………………………. 2.5.2.. CÁLCULO. DE. ORIENTACIÓN. CON. COMPENSACIÓN. 26. DE. INCLINACIÓN…………………………………………………………………….... 27. 2.6. SISTEMA DE MEDICIÓN DE POSICIÓN….……………………………….. 28. 2.7 ENSAMBLAJE DE LA PLATAFORMA…………………………………….... 30. 2.8 INSTALACIÓN DEL INS EN UN UAV……………………………………....... 31. CAPÍTULO 3……………………………………………………..…………………... 36. DESARROLLO DEL SISTEMA DE NAVEGACIÓN INERCIAL……...………. 36. 3.1. CÁLCULO DE LA ACTITUD…………………………………………………. 37. 3.1.1. LINEAR WEIGHTED MOVING AVERAGE (LWMA)……………………. 39. 3.1.2. MÉTODO DE RUGEN-KUTTA (RK)……………………………...……….. 41. 3.1.3. FILTRO DE KALMAN EXTENDIDO………...…………………………….. 43. 3.2. CÁLCULO DE LA ORIENTACIÓN..…………………………………………. 46.

(12) xi. 3.3. CÁLCULO DE LA POSICIÓN……..…………………………………………. 47. CAPÍTULO 4…………………………………………………………………………. 49. DESARROLLO DEL HMI DEL SISTEMA DE NAVEGACIÓN INERCIAL….. 49. 4.1. SOFTWARE HMI…………………………………………….…………………. 49. 4.1.1. ENTORNO DE PROGRAMACIÓN DE LABVIEW………………..………. 50. 4.1.2. PRINCIPALES CARACTERÍSTICAS DE LABVIEW…….……………….. 50. 4.2 DESARROLLO DEL HMI PARA EL SISTEMA DE NAVEGACIÓN INERCIAL UTILIZANDO LABVIEW…………………………………………….. 52. 4.2.1 VI PARA LA ADQUISICIÓN DE DATOS………………..…………………. 52. 4.2.2. VI PARA LA VISUALIZACIÓN DE LOS ÁNGULOS DE EULER CON UN OBJETO EN 3D……………………………………………………………….... 54. 4.2.3 VI PARA CALIBRACIÓN DEL MAGNETÓMETRO Y UBICACIÓN DEL NORTE MAGNÉTICO……………………………………………………………... 56. 4.2.4 VI PARA LA ESTIMACIÓN DE LA POSICIÓN DE LA PLATAFORMA.... 58. 4.2.5 VI PARA LA VISUALIZACIÓN DEL DESPLAZAMIENTO EN EL PLANO XY………………………………………………………………………...... 60. 4.2.6 VI PARA EL POSICIONAMIENTO POR GPS…………………….………... 60. 4.2.7 PANEL FRONTAL DEL HMI………………………………………………... 61. 4.2.7.1 Adquisición de datos……………………………………………………….. 62. 4.2.7.2 Sistema de Actitud…………………………………………………………. 63. 4.2.7.3 Sistema de Posición………………………………………………………... 64. 4.2.7.4 Sistema de Orientación…………………………………………………….. 65. 4.2.7.5 Posicionamiento por GPS………………………………………………….. 66. 4.2.7.6 Modo de Pruebas…………………………………………………..………. 67. CAPÍTULO 5…………………………………………………………………………. 69. PRUEBAS Y RESULTADOS EN UN VEHÍCULO MÓVIL……………………... 69. 5.1. PRUEBAS DE SENSORES……………………………………………………... 69. 5.1.1. MEDIDAS DEL ACELERÓMETRO…….………………………………….. 69. 5.1.2. MEDIDAS DEL GIROSCOPIO……………...………………………………. 72. 5.1.3. MEDIDAS DEL MAGNETÓMETRO …………………..……………..……. 74. 5.2. PRUEBAS DE ACTITUD APLICANDO FKE……………………………….. 77.

(13) xii. 5.3. PRUEBAS DE ORIENTACIÓN……………………………………………….. 79. 5.4. PRUEBAS DEL SISTEMA DE POSICIONAMIENTO…………………….... 79. 5.5. COMPARACIÓN REALIZADA DE LA IMU DESARROLLADA Y LA IMU COMERCIAL…………………………………………..…………………….... 83. CAPÍTULO 6…………………………………………………………………………. 85. CONCLUSIONES Y RECOMENDACIONES…………………………………….. 85. 6.1 CONCLUSIONES………………………………………………………………... 85. 6.2 RECOMEDACIONES............................................................................................ 87. REFERENCIAS BIBLIOGRÁFICAS..……………………………………………. 89. ANEXOS.

(14) xiii. RESUMEN La facultad de ciencias perteneciente a la Escuela Politécnica Nacional (EPN) desarrolla un proyecto de investigación acerca de UAV (Unmanned Aerial Vehicle), dentro del cual se encuentra el Sistema de Navegación Inercial (INS) que es el tema en el que se enfoca este proyecto de titulación. El INS es desarrollado desde los sensores básicos: sensores inerciales (acelerómetro & giroscopio) y magnetómetro. Este sistema debe ser capaz de medir la actitud y determinar el desplazamiento de un robot móvil. A partir de los sensores inerciales se comienza desarrollando una Unidad de Medida Inercial (IMU) que incluye la implementación de un filtro de Kalman extendido, cuyo objetivo es medir la actitud. La orientación es medida por una brújula desarrollada a partir del magnetómetro. El desplazamiento del robot móvil es obtenido a partir de las medidas del acelerómetro, por medio de la integración de estas se obtiene la velocidad y con una nueva integración se obtiene la posición. Todos los resultados del INS se visualizan en tiempo real a través de una Interfaz Hombre – Máquina (HMI), del cual se podrá adquirir los datos para acoplar el INS al proyecto UAV..

(15) xiv. PRESENTACIÓN En este proyecto se desarrolla un INS aplicable en un autopiloto de un UAV, su desarrollo. ha. sido. secuencial. para culminar. los. objetivos. y alcances. satisfactoriamente. La descripción del proyecto se ha dividido en los siguientes 6 capítulos: El primer capítulo consiste en explicar todos los conceptos necesarios para la compresión del proyecto: INS, ángulos de Euler, sistemas de coordenadas, sensores inerciales, filtro de Kalman y UAV En el capítulo 2 se detallan los sensores y las expresiones básicas para el desarrollo del INS así como la construcción de la plataforma y la distribución de los diferentes elementos del sistema sobre la misma. El capítulo 3 trata sobre el desarrollo del INS, el cual consiste en mostrar la programación realizada en el microcontrolador y en LABVIEW para obtener la actitud, orientación y posición de un robot móvil, todos los algoritmos se han representado mediante diagramas de flujo. En el capítulo 4 se indica el proceso de desarrollo del HMI para el INS utilizando LABVIEW, se muestran los bloques en los cuales se divide el sistema y como se realizó cada uno de ellos, para posteriormente mostrar formas de onda y valores relevantes de actitud, orientación y posición. El capítulo 5 indica las pruebas realizadas a cada sensor para su debida calibración y acondicionamiento, se visualiza las respuestas de las medidas, explicando sus ventajas, desventajas y soluciones planteadas. El capítulo 6 abarca las conclusiones obtenidas al finalizar el proyecto, enfocándose en los problemas encontrados y soluciones planteadas, también se sugiere algunas ideas para realizar futuros proyectos con un INS..

(16) 1. CAPÍTULO 1 MARCO TEÓRICO 1.1.. SISTEMA DE NAVEGACIÓN INERCIAL. El presente proyecto está dirigido a desarrollar un Sistema de Navegación Inercial o INS (Inertial Navegation System) cuyo objetivo es estimar la posición, actitud y orientación de un vehículo móvil manual o autónomo, como es el caso del UAV (Unmanned Aerial Vehicles). Un INS se basa en una Unidad de Medida Inercial o IMU (Inertial Measurement Unit) sin la necesidad de una referencia externa (sólo inicialmente).. Figura 1.1 Sistema de Navegación Inercial El INS se fundamenta en las medidas de los vectores de aceleración (α) y velocidad angular (ω), obtenidas por los sensores inerciales que forman parte de la IMU, con estas medidas se logra cumplir su objetivo. 1.1.1. CONFIGURACIÓN DEL SISTEMA INERCIAL Existen dos configuraciones en un INS, la diferencia entre estas dos configuraciones se encuentra en el marco de referencia que cada una utiliza para.

(17) 2. medir las señales de operación de los sensores inerciales, los marcos de referencias se explican con mayor detalle en la sección 1.1.2. 1.1.1.1.. Gimballed (ejes flotantes). En esta configuración los sensores inerciales van montados sobre una plataforma que está sujeta a una estructura gimbal como se puede observar en la figura 1.2, y esta plataforma se encuentra alineada. al sistema de coordenadas de. navegación, el cual es explicado en el literal 1.1.2.2.. Figura 1.2 Sistema Gimballed [1] Si los giroscopios montados en la plataforma detectan una rotación, esas señales se verán reflejadas en el movimiento de los motores de la estructura gimbal con el propósito de mantenerla estable la plataforma en su sistema de coordenadas. Debido a este acontecimiento en la estructura gimbal a esta configuración también se la conoce como “ejes flotantes”. El sistema de ejes flotantes se divide en:.

(18) 3. 1.1.1.1.1. Referencia Inercial.- se la aplica en los satélites artificiales cuyo movimiento se desarrolla respecto al centro de masas de la tierra. 1.1.1.1.2. Referencia Terrestre.- se la aplica en vehículos que se desea conocer su desplazamiento relativo a la tierra. 1.1.1.2. Strapdown (ejes fijos) En esta configuración los sensores inerciales van montados rígidamente sobre el vehículo móvil como se observa en la figura 1.3, por lo cual las medidas obtenidas serán reflejadas en un sistema de coordenadas body. En el sistema los ejes de referencia se encuentran fijos al vehículo móvil y se debe partir de una posición y orientación conocida de los ejes fijos. Figura 1.3 Sistema Strapdown [2] Este sistema al no utilizar una estructura gimbal reduce costos y espacio físico, por esta razón el INS de este proyecto se realizará mediante un sistema strapdown..

(19) 4. Tabla 1.1 Ventaja y desventaja de cada clasificación Gimballed. Strapdown. Ventaja. Desventaja. Ventaja. Cálculos simples Complejo en el Simple porque. solo. Desventaja en. se aspecto mecánico aspecto. necesita tener las debido. a. el Complejo en los cálculos ya que. la mecánico, ya que en este caso se. tres componentes regulación de la solo se necesita necesita de la aceleración.. plataforma. no. de los sensores solamente de las de aceleración y tres componentes velocidad angular de la aceleración, sobre los ejes del sino también de objeto móvil.. las. tres. componentes de velocidad angular 1.1.2. SISTEMAS DE COORDENADAS Un sistema de coordenadas es un sistema de referencia que permite localizar inequívocamente una posición cualquiera en un espacio dimensional. Para poder desarrollar un INS se necesita hacer cambios de coordenadas la cuales se describen a continuación. 1.1.2.1.. Sistema de Coordenada Inercial Verdadero. En el sistema de coordenada inercial las leyes de Newton son válidas, debido a que Newton asumía que su sistema de coordenadas no tenía movimiento, Newton consideraba que el campo inercial era fijo con respecto a las estrellas, de esta manera sus leyes de movimiento solo son válidas en este sistema de coordenadas. El sistema de coordenada inercial no es un sistema de coordenadas práctico. [3].

(20) 5. 1.1.2.2.. Sistema de Coordenadas ECI (Earth Centered Inertial). Este sistema de coordenadas tiene origen en el centro de masa de la Tierra y se mueve con el planeta por lo que idealmente no rota respecto al espacio inercial (fijo con respecto a las estrellas), sin embargo si se acelera con respecto al espacio inercial ya que se mueve con la Tierra, como la Tierra rota y se mueve alrededor del sol, el sistema de referencia inercial es como un observador fijo en la Tierra que rota a una velocidad que es la combinación de la velocidad rotacional de la Tierra (Ω) y la posición de la Tierra alrededor del sol. El eje ‫ ݖ‬coincide con el eje de rotación de la Tierra y los ejes ‫ ݔ‬y‫ ݕ‬se encuentran. en el plano ecuatorial de la Tierra donde el eje ‫ ݔ‬apunta a una estrella llamada. equinoccio Vernal. 1.1.2.3.. Sistema de Coordenadas ECEF (Earth - Earth Centered - Fixed). Este sistema de coordenada tiene su origen en el centro de masa de la Tierra y rota con la misma, El eje ‫ ݖ‬coincide con el eje de rotación de la Tierra y los ejes ‫ݔ‬. y‫ ݕ‬se encuentran en el plano ecuatorial de la Tierra donde el eje ‫ ݔ‬apunta al Meridiano de Greenwich (0° latitud, 0° longitud), esto se puede observar en la figura 1.4. 1.1.2.4.. Sistema de Coordenadas de Navegación (n-frame). Este sistema de coordenadas también se lo conoce como NED (North, East, Down) ya que sus ejes apuntan a estas direcciones, se denominará a los ejes de navegación como: xn al que apunta al norte, yn al que apunta al este y zn al que apunta abajo, teniendo como origen la localización del sistema de navegación, esto se puede observar en la figura 1.4..

(21) 6. Figura 1.4 Sistema de Coordenadas ECEF y de Navegación [4] 1.1.2.5.. Sistema de Coordenadas Body (b-frame). Este sistema de coordenadas tiene origen en el centro de masa del vehículo y se encuentran alineadas a los ejes de los ángulos roll, pitch y yaw (estos ángulos se definen en el apartado 1.2.1.2), esto se puede observar en la figura 1.5 donde los ejes del sistema de coordenadas body son representados por 1 b, 2b, y 3b. Es el sistema de coordenadas básico para los sensores inerciales.. Figura 1.5 Relación entre coordenadas body, plataforma e IMU [5].

(22) 7. 1.2.. UNIDAD DE MEDIDAS INERCIALES (IMU). Es un dispositivo que está compuesto por sensores inerciales (acelerómetros y giroscopios), circuitos electrónicos y una CPU, para entregar medidas de aceleración y velocidad angular. Todos los sensores se encuentran debidamente calibrados y compensados. La IMU en general es un componente de un Sistema de Navegación por lo cual se suele ocupar otros componentes para corregir las limitaciones de una IMU, por ejemplo se utiliza magnetómetro para corregir la orientación. Para obtener una mejor precisión la IMU debe estar recubierta por una caja diseñada para que la temperatura se mantenga constante y donde las paredes de la caja disminuyan las interferencias electromagnéticas, si las medidas son dadas de forma analógica se debe minimizar el ruido eléctrico en los cables y en el conversor análogo-digital y si las medidas son dadas de forma digital se debe tomar en cuenta que existirá un retraso temporal. 1.2.1. ACTITUD Y ORIENTACIÓN En este proyecto se llamara actitud a la inclinación del vehículo respecto al plano horizontal y orientación al rumbo del vehículo respecto al norte magnético, a este sistema se lo conoce también como AHRS (Attitude and Heading Reference System). 1.2.1.1.. Componentes de AHRS. ·. Pitch: es la relación vertical que existe entre la nariz y el horizonte.. ·. Roll: es la cantidad que la nariz se inclina hacia la izquierda o la derecha.. ·. Yaw: es la dirección que la nariz está apuntando.

(23) 8. Figura 1.6 Componentes de AHRS [6] 1.2.1.2.. Ángulos de Euler. Los ángulos de Euler son un conjunto de 3 coordenadas angulares (θ, Φ, Ψ) que sirven para especificar la orientación de un objeto móvil respecto a un eje de referencia de ejes ortogonales fijos [7], por este motivo para la representación matemática de la actitud y orientación del vehículo se utilizaran los ángulos de Euler, siendo así: ·. Θ = pitch. ·. Φ = roll. ·. Ψ = yaw. 1.3.. SENSORES. El INS aparte de utilizar los sensores inerciales como acelerómetro y giroscopio también utiliza magnetómetro y GPS, a continuación se describirá el funcionamiento de cada uno estos sensores. 1.3.1. ACELERÓMETRO El principio de un acelerómetro depende de un sistema de masas y resortes donde la tensión y la fuerza de los resortes esta descrita por la ley de Hooke: “La.

(24) 9. fuerza de resistencia o la fuerza para establecer la posición de equilibrio en un resorte, es proporcional a la cantidad de fuerza al estirarlo o comprimirlo.” En este caso se ha utilizado un acelerómetro lineal en 3 ejes ADXL345 que utiliza el principio de capacitancia para medir el desplazamiento de un sistema elástico que consiste en una barra de silicio sujeta por 4 hilos. A esta configuración se adicionan tres placas metálicas que forman dos condensadores. La placa de la mitad cambia la distancia entre las placas de los extremos [8]. Figura 1.7 Modelo de un acelerómetro con capacitancia [8] Un acelerómetro mide la componente estática de la aceleración provocada por la gravedad que actúa sobre el vehículo. y la componente dinámica de la. aceleración que se produce por el movimiento del vehículo. La unidad de medida del acelerómetro se da en [g] (1[g]=9.8 [݉Ȁ‫ ݏ‬ଶ ሿ). 1.3.2. GIROSCOPIO El principio de funcionamiento de un giroscopio está basado en la conservación del momento angular. Figura 1.8 Funcionamiento de un giroscopio [9].

(25) 10. Se ha utilizado un giroscopio de 3 ejes (ITG3200), en este giroscopio cuando la masa se mueve en dirección ‫ ݒ‬como se indica en la figura 1.8, experimentara una. fuerza de coriolis (Fcoriolis) en dirección a la flecha amarilla ‫ ݏ݈݅݋݅ݎ݋ܿܨ‬ൌ െʹ ‫ݒ כ ߗ כ ݉ כ‬. (1.1). Donde m es la masa, ߗ es la velocidad angular y ‫ ݒ‬es la velocidad lineal, esta. fuerza es transformada en una señal eléctrica proporcional a ella. Un giroscopio mide la velocidad angular de un vehículo en [dps] (degrees per second). [9] 1.3.3. MAGNETÓMETRO Un magnetómetro detecta el cambio del campo magnético aplicado a una muestra. determinada, la medida depende mucho de la ubicación en la que se encuentre así como de otros factores externos entre ellos el más importante la radiación electromagnética, emitida por antenas y equipos electrónicos. Se ha utilizado el LSM303DLM, un sensor integrado que posee tres ejes de medición para el campo magnético, y gracias a los múltiples factores de escala, se pueden identificar campos desde ±1,3 gauss hasta ±8,1 gauss.. Figura 1.9 Magnetómetro [10] El principio de funcionamiento de este magnetómetro está dado por la ley de Faraday, ya que se tiene una muestra que frente a un campo magnético variable.

(26) 11. produce un campo eléctrico el cual puede ser medido y procesado por un microcontrolador o por un ordenador, previa una conversión análoga/digital. 1.3.4. GLOBAL POSITIONING SYSTEM (GPS) El Sistema de Posicionamiento Global permite determinar la posición de un objeto en el planeta Tierra, este sistema está compuesta por una red de 24 satélites con trayectorias sincronizadas aproximadamente a unos 20.000 Km de distancia del planeta Tierra, esta red de satélites es conocida con el nombre de NAVSTAR (Navegation. Satellite. Timing. and. Ranging). y fue. desarrolladlo. por. el. Departamento de Defensa de los Estados Unidos con fines puramente militares. 1.3.4.1.. Parámetros de un GPS. El GPS calculan los siguientes parámetros: ·. Posición 3D (Coordenadas X, Y, Z). ·. Tiempo (Los satélites emiten información temporal en Universal Time Coordinated). ·. Velocidad del móvil.. 1.3.4.2.. Funcionamiento de un GPS. El receptor que se ha utilizado es un LS20031 el cual recibe señales de los satélites indicando la identificación y la hora de cada uno de ellos, en base a estas señales se sincroniza el reloj del GPS y calcula el tiempo que tardan en llegar las señales al equipo y de tal modo mide la distancia a los satélites. De esta forma el GPS recibe dos tipos de datos: los del Almanaque (datos de ubicación y operación de cada satélite en relación al resto de satélites) y Efemérides (datos únicamente del satélite que está siendo captado cuyos datos se utilizan para calcular la distancia del receptor al satélite)..

(27) 12. Figura 1.10 Medición de distancias desde los satélites [11] El receptor utiliza como mínimo 3 satélites para realizar una trilateración satelital para calcular la posición del mismo, con un elevado número de satélites captados (8 satélites) por el receptor y si éstos tienen una geometría adecuada (dispersos) puede obtenerse precisiones de 2,5 metros y si se utiliza un DPGS (GPS Diferencial) se puede obtener precisiones de 1 metro, sin embargo cuando solo son captados el mínimo de satélites para realizar el cálculo de posición el error puede ser de 15 metros. [11] El número de satélites captados dependerá de la ubicación geográfica del receptor, por ejemplo en sitios urbanos se capta menos satélites debido a los obstáculos como casas y grandes edificios.. 1.4.. FILTRO DE KALMAN (FK). El FK tiene como objetivo estimar los estados de un sistema dinámico lineal ruidoso, utilizando un algoritmo óptimo y recursivo, óptimo porque toma en cuenta toda la información que se le proporcione para minimizar el error al cuadrado y recursivo porque no requiere almacenar todos los datos previos y reprocesarlos cada vez que llega una nueva información. El FK para estimar los estados accede a las medidas del sistema. [12] 1.4.1. ECUACIONES DE KALMAN Todas las variables que el FK utiliza para estimar una solución, se agrupan dentro de un vector llamado vector de estados. El vector incluye los parámetros deseados: ‫ ݔ‬ൌ ሾ‫ݔ‬ଵ ‫ݔ‬ଶ ‫ݔ‬ଷ ǥ‫ݔ‬௡ ሿ.

(28) 13. El vector de medidas es una agrupación de todas las variables cuya medida están disponibles: ‫ ݖ‬ൌ ሾ‫ݖ‬ଵ ‫ݖ‬ଶ ‫ݖ‬ଷ ǥ‫ݖ‬௡ ሿ Por lo tanto el FK estima el estado x de un proceso en el tiempo discreto que es gobernado por la ecuación diferencial del tipo: ‫ݔ‬௞ାଵ ൌ ‫ܣ‬௞ ‫ݔ‬௞ ൅ ‫ݑܤ‬௞ ൅ ߱௞ (1.2) Con una medida ‫ ݖ‬correspondiente a la observación y que es: ‫ݖ‬௞ ൌ ‫ܪ‬௞ ‫ݔ‬௞ ൅ ‫ݒ‬௞ . (1.3). La matriz A da una relación entre el estado anterior y el estado presente del FK. La matriz B relaciona la entrada de control ‫ ݑ‬con el estado ‫ݔ‬.. La matriz H relaciona el vector de estados con el vector de medidas. Las variable aleatorias ߱௞ y ‫ݒ‬௞ representan el ruido del proceso y de la medida respectivamente y se asume que son independientes y blancos.. El FK proporciona una ecuación que computa un estimador del estado a posteriori ‫ݔ‬ො௞ como combinación lineal del estimador a priori ‫ݔ‬ො௞ି y la diferencia entre la observación actual ‫ݖ‬௞ y la predicción de medida ‫ܪ‬௞ ‫ݔ‬ො௞ି : ‫ݔ‬ො௞ ൌ ‫ݔ‬ො௞ି ൅ ‫ܭ‬ሺ‫ݖ‬௞ െ ‫ܪ‬௞ ‫ݔ‬ො௞ି ሻ. (1.4). La diferencia ሺ‫ݖ‬௞ െ ‫ܪ‬௞ ‫ݔ‬ො௞ି ሻ se llama innovación de la medida y muestra la. diferencia entre la predicción de la medida ‫ܪ‬௞ ‫ݔ‬ො௞ି y la observación actual ‫ݖ‬௞ . La matriz K es llamada Ganancia de Kalman que establece la cantidad de influencia del error entre la estimación y la medida: ‫ܭ‬௞ ൌ ܲ௞ି ‫ܪ‬௞் ሺ‫ܪ‬௞ ܲ௞ି ‫ܪ‬௞் ൅ ܴ௞ ሻିଵ. (1.5).

(29) 14. Siendo ܲ௞ି el estimador de la covarianza del error a priori y ܴ௞ la covarianza del. error de la medida. Cuando la covarianza del error de medida ܴ௞ se aproxime a 0, se tendrá más confianza en la observación actual ‫ݖ‬௞ mientras que la medida. predicha ‫ܪ‬௞ ‫ݔ‬ො௞ି perderá confianza en la misma medida. Por otra parte, cuando el. estimador de la covarianza del error a priori ܲ௞ି se aproxime a 0 se perderá. confianza en la medida ‫ݖ‬௞ y de la medida predicha ‫ܪ‬௞ ‫ݔ‬ො௞ି se incrementará. ܳ es la covarianza del error del sistema [13].. De tal forma el algoritmo queda dividido en dos pasos que se ejecutan de manera iterativa: la predicción y la corrección como se muestra en la figura 1.11.. Figura 1.11 Algoritmo del FK [13]. 1.4.2. FILTRO DE KALMAN EXTENDIDO (FKE) Cuando el sistema sea no lineal se puede utilizar un FKE, en el cual se linealiza el sistema, en este caso en vez de existir las matrices A, B y H, existen dos funciones ሺǡ ǡ ሻy ݄ሺ‫ݔ‬ǡ ‫ݒ‬ሻ del proceso no lineal y de la observación no lineal respectivamente..

(30) 15. Las funciones ሺǡ ǡ ሻy ݄ሺ‫ݔ‬ǡ ‫ݒ‬ሻ pueden ser utilizadas directamente para realizar. predicción, pero para calcular la covarianza ܲ௞ a estas funciones es necesario obtener su jacobiano.. 1.5 UNMANNED AERIAL VEHICLE (UAV) Un UAV también es conocido como Drone, este vehículo no tiene piloto a bordo, el vuelo puede ser autónomo o controlado desde tierra. Una ventaja de utilizar un UAV es el ahorro de combustible en comparación al utilizar aviones tripulados para realizar reconocimiento o vigilancia de determinado territorio.. Figura 1.12 UAV tomado de [14] Este proyecto está enfocado a aplicarse en el prototipo cero del autopiloto de un UAV, éste puede ser avión, helicóptero, cuadricóptero, etc..

(31) 16. 1.5.1 AUTOPILOTO El autopiloto es un sistema usado para guiar un vehículo sin la asistencia de una persona, su prioridad es la estabilidad del vehículo, para cumplir con este objetivo, las medidas de vuelo deben ser en tiempo real. Los autopilotos son capaces de permitir realizar acciones específicas al UAV, como desplazarse a un lugar determinado por medio de un software incluido con el autopiloto..

(32) 17. CAPÍTULO 2 DISEÑO Y CONSTRUCCIÓN DEL HARDWARE 2.1. DESCRIPCIÓN Y CARACTERÍSTICAS DE LOS SENSORES EMPLEADOS Los sensores que se han escogido para el presente proyecto de titulación, se seleccionaron en base a varios parámetros, los cuales se detallan a lo largo del desarrollo de los siguientes literales. 2.1.1 ESPECIFICACIONES TÉCNICAS DEL SENSOR LSM303DLM El sensor LSM303DLM es un módulo compuesto de 6 sensores, proporcionando un grado de libertad por cada sensor y sus principales características son:. Figura 2.1 Sensor LSM303DLM [15] Ø 3 ejes para medición del campo magnético (eje x, eje y, eje z). Ø 3 ejes para medición de la aceleración (eje x, eje y, eje z). Ø Escalas de medición del campo magnético de ±1.3 / ±1.9 /±2.5. /±4.0/±4.7/±5.6/ ±8.1gauss. Ø Escalas de medición de la aceleración ±2g/±4g/±8g. Ø Comunicación I2C en modo estándar (100kHz) o fast mode (400kHz). Ø Temperatura de operación de -40 a +85ºC..

(33) 18. 2.1.2 ESPECIFICACIONES TÉCNICAS DEL SENSOR 6DOF DIGITAL. El sensor 6DOF DIGITAL es un módulo en el que están embebidos 3 giroscopios y 3 acelerómetros proporcionando un grado de libertad adicional por cada sensor y sus principales características son:. Figura 2.2 Sensor 6DOF DIGITAL [16]. Ø 3 ejes para la medición de la velocidad angular Ø 3 ejes para la medición de aceleración Ø Rango de medición de ±2000°/sec Ø Comunicación I2C fast mode (400kHz) Ø Filtro pasa bajos digital programable Ø Temperatura de operación de -40 a +85ºC.. 2.2. PROTOCOLOS DE COMUNICACIÓN EMPLEADOS Cada uno de los diferentes dispositivos que conforman el sistema INS utiliza un protocolo de comunicación específico, cada uno de estos se muestra en la figura 2.3 y se detallan en los siguientes literales..

(34) 19. LSM303DLM. XBEE PRO S2B. 6DOF DIGITAL. LS20031. MBED. Figura 2.3 Protocolo de comunicación entre cada elemento del INS 2.2.1 COMUNICACIÓN ENTRE SENSORES Y MICROCONTROLADOR El chip en el cual se encuentran embebidos el acelerómetro y giroscopio, utilizan el protocolo de comunicación I2C, este protocolo cuenta con dos líneas de comunicación que son SDA y SCL, la ventaja de utilizar I2C es que por un mismo bus de datos se pueden comunicar varios dispositivos, simplemente con indicar la dirección del dispositivo que se desee leer.. Figura 2.4 Estructura del protocolo I2C [17].

(35) 20. La velocidad de transmisión es de 100 kbaudios y se sigue la siguiente trama: 1. START condition (Master) 2. 7 Bits de dirección de esclavo (Master) 3. 1 Bit de RW, 0 es Leer y 1 Escribir. (Master) 4. 1 Bit de Acknowledge (Slave) 5. Byte de dirección de memoria (Master) 6. 1 Bit de Acknowledge (Slave) 7. Byte de datos (Master/Slave (Escritura/Lectura)) 8. 1 Bit de Acknowledge (Slave/Master (Escritura/Lectura)) 9. STOP condition (Master) 2.2.2 COMUNICACIÓN ENTRE EL MICRONTROLADOR Y LA PC. Figura 2.5 Software para la configuración de los módulos Xbee-PRO S2B [19].

(36) 21. La comunicación entre el microcontrolador y la PC se realiza mediante un par transmisor - receptor Xbee Pro 2, los cuales se comunican entre sí utilizando el protocolo Zigbee el cual está basado en el estándar de comunicaciones para redes inalámbricas IEEE 802.15.4 [18], la comunicación de Xbee-PC y Xbeemicrocontrolador se la realiza mediante RS232 asincrónica. La configuración de los parámetros de estos módulos se la realizó mediante el software proporcionado por el fabricante, esto permite que los dos módulos se enlacen entre si y no con otro que se encuentre a su alrededor, y al mismo tiempo configura la trama con la cual se enviaran los datos [19]. 2.2.3 PROTOCOLO DE COMUNICACIÓN DEL GPS. El GPS trabaja con el protocolo NMEA 0183 en el cual se encuentran encriptados los datos que este recibe de los satélites, los parámetros. básicos de este. protocolo son, Baud Rate de 4800, 8 bits de datos, 1 bit de parada, sin paridad, sin handshake [20].. Adicionalmente para adquirir los datos del GPS ya sea por un microcontrolador o un Xbee se emplea comunicación RS232 asincrónica.. 2.3 DISTRIBUCIÓN DE LOS SENSORES EN LA PLATAFORMA MÓVIL Debido a que un sistema de medición inercial debe adaptarse a cualquier plataforma, la placa donde se encuentran los diferentes sistemas de medida, fue diseñada para montarse en cualquier superficie siempre y cuando se tomen en cuenta varias consideraciones, principalmente las del entorno de funcionamiento. Dado que la medición realizada depende de varios aspectos como son, la temperatura, niveles de ruido y principalmente la vibración, la ubicación de los sensores en la plataforma se realizó buscando el centro de masa del cuerpo y.

(37) 22. evitando en lo posible que los sensores sean afectados por las interferencias indicadas anteriormente. Para lograr que los ejes de los sensores concuerden con los ejes de la plataforma, y asumiendo que la distribución de fábrica de los sensores es aproximadamente ortogonal, se realiza la transformación de los ejes de fábrica de los sensores a coordenadas body, con la finalidad de obtener mediciones correctas según el comportamiento de la plataforma. Para esta transformación solo se toma en cuenta la distribución física de los sensores sobre la plataforma, esto debido que en lo posible se ha alineado los tres ejes x, y y z de los sensores con los de la plataforma, pero no se ha conseguido una coincidencia en los signos.. Figura 2.6 Ubicación de los sensores en la plataforma móvil Los ejes de los sensores se muestran en la figura 2.6 por lo cual el cambio de ejes a coordenadas body se realiza mediante las siguientes matrices. [21] ͳ Ͳ Ͳ ௕ ‫ܥ‬௔௖௖ ൭Ͳ െͳ Ͳ൱ሺʹǤͳሻ Ͳ Ͳ ͳ.

(38) 23. ͳ Ͳ ൭Ͳ െͳ Ͳ Ͳ ͳ Ͳ ௕ ൭Ͳ െͳ ‫ܥ‬௠௔௚ Ͳ Ͳ. ௕ ‫ܥ‬௚௜௥௢. Ͳ Ͳ൱ሺʹǤʹሻ ͳ Ͳ Ͳ൱ሺʹǤ͵ሻ ͳ. Con esta transformación se logra que los ejes del sensor y de la plataforma coincidan, los coeficientes de la matriz dependerán del sensor que se utilice, ya que no todos tienen la misma distribución de fábrica. 2.3.1 CAMBIO DE COORDENADAS DE BODY A NAVEGACIÓN La ecuación para el cambio de coordenadas de un sistema de ejes fijos (X, Y, Z) a un sistema de ejes solidarios con el cuerpo (x, y, z) se obtiene a partir de tres rotaciones sucesivas alrededor de cada uno de los ejes del sistema fijo siguiendo la regla de la mano derecha, estos ángulos formados entre los dos sistemas de coordenadas se conocen como ángulos de Euler. La primera rotación se realiza alrededor del eje Z pasando de las coordenadas X, Y, Z a las coordenadas ܺ ′ , ܻ ′ , ܼ ′ , formando un ángulo conocido como ψ (yaw) que da lugar a la siguiente ecuación [22] :. ܺ ܺᇱ ൥ܻ ᇱ ൩ ൌ ൣ‫ܥ‬ట ൧ ൥ܻ ൩ሺʹǤͶሻ ܼ ܼᇱ. El giro en yaw se puede expresar mediante la siguiente matriz de dirección de cosenos ߰ ‫ܥ‬ట ൌ ൭െ ߰ Ͳ. ߰ ߰ Ͳ. Ͳ Ͳ൱ሺʹǤͷሻ ͳ.

(39) 24. La segunda rotación se realiza alrededor del eje Y pasando de las coordenadas ܺ ′ ,ܻ ′ ,ܼ ′ a las coordenadas ܺ ′′ ,ܻ ′′ ,ܼ ′′ , formando un ángulo conocido como ߠ (pitch) que da lugar a la siguiente ecuación:. ܺ ᇱᇱ ܺᇱ ൥ܻ ᇱᇱ ൩ ൌ ሾ‫ܥ‬ఏ ሿ ൥ܻ ᇱ ൩ሺʹǤ͸ሻ ܼ ᇱᇱ ܼᇱ. El giro en pitch se puede expresar mediante la siguiente matriz de dirección de cosenos ߠ ‫ܥ‬ఏ ൌ ൭ Ͳ ߠ. Ͳ െ ߠ ͳ Ͳ ൱ሺʹǤ͹ሻ Ͳ ߠ. La tercera rotación se realiza alrededor del eje X pasando de las coordenadas ܺ ′′ ,ܻ ′′ ,ܼ ′′ a las coordenadas ‫ݔ‬,‫ݕ‬,‫ ݖ‬formando un ángulo conocido como ߶ (roll) que. da lugar a la siguiente ecuación:. ‫ݔ‬ ܺ ᇱᇱ ቈ‫ݕ‬቉ ൌ ൣ‫ܥ‬ఝ ൧ ൥ܻ ᇱᇱ ൩ሺʹǤͺሻ ‫ݖ‬ ܼ ᇱᇱ. El giro en roll se puede expresar mediante la siguiente matriz de dirección de cosenos ͳ ‫ܥ‬థ ൌ ൭Ͳ Ͳ. Ͳ ߶ െ ߶. Ͳ ߶ ൱ሺʹǤͻሻ ߶. Con las tres rotaciones se puede definir la matriz de cambio de coordenadas de un eje fijo a las de un cuerpo móvil de la siguiente forma: ‫ݔ‬ ܺ ௕ ቈ‫ݕ‬቉ ൌ ‫ܥ‬௡ ൥ܻ ൩ሺʹǤͳͲሻ ‫ݖ‬ ܼ. ‫ܥ‬௡௕ ൌ ‫ܥ‬ట ‫ܥ‬ఏ ‫ܥ‬థ ሺʹǤͳͳሻ.

(40) 25. ߠ ߰ ‫ܥ‬௡௕ ൌ ൭ ߰ ߠ ߶ െ ߰ ߶ ߮ ߠ ߶ ൅ ߰ ߶. ߠ ߰ ߰ ߶ െ ߰ ߠ ߶ ߰ ߠ ߶ െ ߰ ߶. െ ߠ ߠ ߶ ൱ሺʹǤͳʹሻ ߠ ߶. Como se desea pasar de los ejes del cuerpo a un sistema de ejes fijos se utilizará la siguiente ecuación: ‫ݔ‬ ܺ ௡ ‫ݕ‬ ൥ܻ ൩ ൌ ‫ܥ‬௕ ቈ ቉ሺʹǤͳ͵ሻ ‫ݖ‬ ܼ. Donde ‫ܥ‬௕௡ ൌ ሾ‫ܥ‬௡௕ ሿ் , con lo cual se puede expresar el cambio de coordenadas del. cuerpo a ejes de navegación.. 2.4. SISTEMA DE MEDICIÓN DE ACTITUD Determinar la actitud de la plataforma implica, calcular los ángulos de pitch y roll, para ello se ha empleado el método de Euler el cual utiliza la matriz mostrada en 2.14, definiendo de esta forma los tres ángulos buscados roll, pitch y yaw. [23] ‫׎‬ሶ ͳ ‫ܧ‬ሶ ൌ ቎ ߠሶ ቏ ൌ ൥Ͳ Ͳ ߮ሶ. ‫ ׎‬ή ߠ ‫׎‬ ‫ ׎‬ή ߠ. ‫ ׎‬ή ߠ ‫݌‬ െ ‫ ׎‬൩ ቈ‫ ݍ‬቉ሺʹǤͳͶሻ ‫ ׎‬ή ߠ ‫ݎ‬. ‫݌‬ Donde ቈ‫ ݍ‬቉ es el vector de velocidades medidas por los giroscopios en el eje x, y y ‫ݎ‬. z respectivamente.. 2.5. SISTEMA DE MEDICIÓN DE ORIENTACIÓN Para la estimación de la orientación se emplea la medida del magnetómetro en conjunto con la del giroscopio,. el magnetómetro permite además tener una. referencia para la orientación, ya que este se calibra de tal forma que siempre el ángulo medido sea con respecto al norte magnético..

(41) 26. 2.5.1 CÁLCULO DE ORIENTACIÓN SIN COMPENSACIÓN DE INCLINACIÓN Para obtener la orientación de la plataforma se ha utilizado las medidas del campo magnético en el plano XY, estas medidas con una calibración previa describen un comportamiento de un circulo unitario, al momento de rotar a la plataforma sobre un eje fijo, debido a este comportamiento la ecuación que se emplea para calcular el ángulo de orientación o Heading es: ‫ܯ‬௬ ‫ ݀݁ܪ‬ൌ ିଵ ൬ ൰ሺʹǤͳͷሻ ‫ܯ‬௫ Donde ‫݀݁ܪ‬ǣ Ángulo de orientación con respecto al norte magnético o Heading ‫ݕܯ‬ǣ Medida calibrada del campo magnético en el eje y. ‫ݔܯ‬ǣ Medida calibrada del campo magnético en el eje x [23] Las medidas del magnetómetro son mucho más propensas a errores, debido a la presencia de campos magnéticos diferentes a los de la tierra, con la finalidad de mermar estos efectos y lograr al mismo tiempo que el frente de la plataforma coincida con el norte magnético, por lo que el sensor se ha montado de tal forma que cuando el My=0 y el Mx=1 se obtenga el Hed=0º y al rotar en sentido horario el ángulo incremente hasta 360º luego de girar una vuelta completa.. Figura 2.7 Vectores de campo magnético en el plano XY y vector orientación resultante [23].

(42) 27. Esta forma de calcular la orientación es aplicable solo si la plataforma permanece en el plano, de lo contrario se debe realizar una compensación de los ángulos de inclinación (roll y pitch) que presente la plataforma. 2.5.2. CÁLCULO DE ORIENTACIÓN CON COMPENSACIÓN DE INCLINACIÓN Debido a que la plataforma puede presentar ángulos de inclinación, los ángulos de roll y pitch deben emplearse conjuntamente con las mediadas del magnetómetro para obtener una orientación sin errores considerable.. Figura 2.8 Representación de la orientación cuando se presenta inclinación del cuerpo [23] Por esta razón se emplean las siguientes ecuaciones de compensación de las medidas [23]: ‫ܯ‬௫௖ ൌ ‫ܯ‬௫ ή ߠ ൅ ‫ܯ‬௭ ή ߠ ሺʹǤͳ͸ሻ ‫ܯ‬௬௖ ൌ ‫ܯ‬௫ ή ‫ ׎‬ή ߠ ൅ ‫ܯ‬௬ ή ‫ ׎‬െ ‫ܯ‬௭ ή ‫ ׎‬ή ߠ ሺʹǤͳ͹ሻ Dónde: ‫ܯ‬௫௖ ǣ Es la medida del campo magnético en el eje x compensado con los ángulos. de inclinación.. ‫ܯ‬௬௖ ǣ Es la medida del campo magnético en el eje y compensado con los ángulos. de inclinación.. ‫ܯ‬௫ ǣ Es la medida original del campo magnético sin compensación en el eje x.

(43) 28. ‫ܯ‬௬ ǣ Es la medida original del campo magnético sin compensación en el eje y ‫ܯ‬௭ ǣ Es la medida original del campo magnético sin compensación en el eje z ߠǣ Ángulo de inclinación Pitch ‫׎‬ǣ Ángulo de inclinación roll. 2.6. SISTEMA DE MEDICIÓN DE POSICIÓN Para estimar la posición de la plataforma móvil, se emplea la medida de los acelerómetros, sin embargo no se puede tomar una lectura directa, ya que este mide tanto las fuerzas externas al cuerpo como la fuerza de gravedad, la cual siempre está presente en el ambiente. El campo gravitacional se refleja en todos los ejes de la plataforma debido a que el movimiento de. la plataforma no siempre se realiza en una superficie. completamente plana, por lo cual se lo puede expresar mediante las siguientes ecuaciones [21]. ݃௫ െ݃ ή ߠ Ͳ ௡ ݃௕ ൌ ൥݃௬ ൩ ൌ ‫ܥ‬௕ ൥Ͳ൩ ൌ ቎ ݃ ή ߠ ή ‫ ׎‬቏ሺʹǤͳͺሻ ݃௭ ݃ ή ߠ ή ‫׎‬ ܾ Debido a esto se debe ajustar la medida realizada por los acelerómetros mediante un conjunto de ecuaciones presentadas a continuación las cuales reflejen los efectos de la gravedad en cada uno de los ejes de la plataforma así como los efectos de la rotación sobre la aceleración. ߲ ଶ‫ݎ‬ ܽ ൌ ଶ ൅ ݃ሺʹǤͳͻሻ ߲‫ݐ‬. ܽூ௕ ൌ ܽ௕ െ ሺ߱ ൅ ȳሻ‫ݒ‬௕ െ ݃௕ ሺʹǤʹͲሻ Donde ܽூ௕ es la aceleración considerando a la plataforma un cuerpo idealmente. inercial.

(44) 29. ܽ௕ Es la aceleración medida por el acelerómetro. Ω Cantidad de rotación del cuerpo debido a la rotación de la tierra ߱ Cantidad de rotación del cuerpo debido al propio cuerpo. Dado que los giroscopios miden siempre Ω y ߱ entonces se tiene: ‫݌‬ ሺ߱ ൅ ȳሻ ൌ ቈ‫ ݍ‬቉ሺʹǤʹͳሻ ‫ݎ‬. Con todo el desarrollo presentado desde (2.4) hasta (2.21) se puede escribir el conjunto de aceleraciones, una vez que se ha suprimido los efectos de gravedad y de rotación del cuerpo y viene dado por las siguientes expresiones: ܽூ௕௫ ൌ ܽ௕௫ ൅ ‫ݒ‬௕௬ ‫ ݎ‬൅ ‫ݒ‬௕௭ ‫ ݍ‬൅ ݃ ߠ. ܽூ௕௬ ൌ ܽ௕௬ െ ‫ݒ‬௕௫ ‫ ݎ‬൅ ‫ݒ‬௕௭ ‫ ݌‬െ ݃ ߠ ‫׎‬ሺʹǤʹʹሻ ܽூ௕௭ ൌ ܽ௕௭ ൅ ‫ݒ‬௕௫ ‫ ݍ‬െ ‫ݒ‬௕௭ ‫ ݌‬െ ݃ ߠ ‫׎‬. El conjunto de ecuaciones presentadas en 2.22 permiten calcular la aceleración de la plataforma una vez que se han suprimido los efectos de rotación y de la gravedad que están presentes en la plataforma. Dónde: ܽூ௕௫ ൌ Aceleración sin los efectos de rotación y de gravedad en el eje x. ܽூ௕௬ ൌ Aceleración sin los efectos de rotación y de gravedad en el eje y ܽூ௕௭ ൌ Aceleración sin los efectos de rotación y de gravedad en el eje z. ܽ௕௫ ൌ Aceleración medida directamente en el eje x. ܽ௕௬ ൌ Aceleración medida directamente en el eje y. ܽ௕௭ ൌ Aceleración medida directamente en el eje z ‫ݒ‬௕௫ ൌ Velocidad del cuerpo en el eje x. ‫ݒ‬௕௬ ൌ Velocidad del cuerpo en el eje y ‫ݒ‬௕௭ ൌ Velocidad del cuerpo en el eje z.

(45) 30. ‫ ݌‬ൌ Velocidad angular del cuerpo en el eje x ‫ ݍ‬ൌ Velocidad angular del cuerpo en el eje y ‫ ݎ‬ൌ Velocidad angular del cuerpo en el eje z ߠ ൌ Ángulo pitch ‫ ׎‬ൌ Ángulo roll ݃ ൌGravedad. 2.7 ENSAMBLAJE DE LA PLATAFORMA Para realizar las pruebas de funcionamiento del INS se utiliza una plataforma robótica, la cual se ha ensamblado a partir de un chasis metálico, que cuenta con seis motores de corriente continua cada uno con su respectiva suspensión, las cuales permiten que la plataforma pueda ubicarse a diferentes ángulos de inclinación con mucha facilidad, en la figura 2.9 se muestra el chasis de la plataforma con sus respectivo motores.. Figura 2.9 Chasis de la plataforma móvil Para la alimentar los motores de la plataforma se utiliza una batería de lipo de tres celdas, las cuales proporcionan el nivel de corriente apropiado para el funcionamiento de los mismos. El control del movimiento se realiza mediante dos drivers que varían el voltaje de tres ruedas de forma simultánea, proporcionando.

(46) 31. un control por tracción diferencial que es guiado a través de un radiocontrol de cinco canales a 2.4 GHz. La figura 2.10 muestra el driver en conjunto con el receptor de Radio Control.. Figura 2.10 Driver de control [24]. 2.8. INSTALACIÓN DEL INS EN UN UAV Después de realizar las pruebas necesarias en una plataforma terrestre para comprobar el correcto funcionamiento del INS, el hardware de este sistema que se puede observar en la figura 2.11 se lo instala en un UAV (Helicóptero RC).. Figura 2.11 Hardware del INS.

(47) 32. El hardware del INS tiene los siguiente componentes: a) Placa Principal: contiene sensores inerciales, el microcontrolador y el Xbee. b) Magnetómetro: Sensor Verde c) Xbee - GPS: transmite la infiormación del GPS d) GPS: dispositivo blanco con verde El procedimiento de instalación se detalla acontinuación. 1. Escoger un UAV con la capacidad de carga necesaria para ubicar el hardware del INS (aproximadamente 120 [gr]), en este caso se ha escogido un helicóptero conla capacidad de carga de 300 [gr]. También se debe seleccionar un lugar en el UAV que sea paralelo al plano horizontal del UAV, en el caso del helicóptero se ha seleccionado el trineo del mismo, lugar donde adicionalmente se sujeta el set de entrenamiento (4 esferas blancas) que servirá de soporte para la placa principal del INS.. Figura 2.12 Helicóptero y set de entrenamiento 2. Se necesita escoger el método de sujetar el hardware del INS, se recomienda utilizar amarras delgadas debido al tamaño de los orifios de cada componente para sujetarlos al helicóptero. De esta forma se ubica la placa principal en el trineo apoyandola en el set de entrenamiento, la placa debe ser sujetada con las amarras en cada unos de sus extremos como se puede observar en la figura 2.13.

(48) 33. Figura 2.13 Placa principal del hardware del INS 3. Para escoger la ubicación del magnetómetro se debe conciderar un lugar distanciado de elementos como motores o imanes que puedan perjudicar el funcionamiento del sensor. Por este motivo se ha ubicado el magnetómetro en el alerón horizontal trasero del helicóptero, sujetado mediante una amarra como se puede observar en la figura 2.14. Figura 2.14 Ubicación del magnetómetro en el helicóptero 4. El Xbee – GPS se ha ubicado en el lado posterior del helicóptero sujetandolo con una amarra. La ubicación del GPS debe estar en el exterior del UAV, por este motivo se lo coloca en la parte superior del helicóptero como se puede observar en la figura 2.15..

(49) 34. Figura 2.15 Ubicación del GPS y su Xbee 5. Se necesita colocar la fuente de energía para polarizar el INS, se recomienda utilizar una bateria de 5 [V]. Se ha colocado la bateria en en centro del helicóptero de tal forma que no interrumpa el correcto funcionamientod el mismo. Por ultimmo se polariza el INS con la bateria, como se puede observar en la figura 2.16.. Figura 2.16 Ubicación del GPS y su Xbee Antes de poner en funcionanmiento el helicópetro se debe verificar que el hardware del INS no perjudique el correcto funcionamiento mecánico del helicóptero, por la misma razón todos los cables del INS deben ser colocados de una manera ordenada y bien adegurados. En la figura 2.17 se puede observar el INS instalado en el UAV..

(50) 35. Figura 2.17 INS instalado de un UAV.

(51) 36. CAPÍTULO 3 DESARROLLO DEL SISTEMA DE NAVEGACIÓN INERCIAL En el presente capítulo se detalla la programación del microcontrolador Mbed y LABVIEW para desarrollar el sistema de navegación inercial (INS), lo que implica la programación para: obtener la actitud, uso del Filtro de Kalman y la posición.. Calculo de la Actitud. Declaración e inicialización de variables. Configuración de acelerómetro y giroscopio. Calibrar acelerómetro y giroscopio. Lectura de datos del acelerómetro y giroscopio. Linear Weighted Moving Average (LWMA) de los datos del acelerómetro. Salida de Datos de Pitch y Roll en base al acelerómetro. Rugen Kutta (RK) de los datos del girsocopio. Salida de datos de Pitch, Roll y Yaw en base al giroscopio FKE de datos de LWMA y RK. Salida de datos filtrados de Pitch, Roll. Figura 3.1 Algoritmo para calcular la actitud de un robot móvil.

(52) 37. 3.1. CÁLCULO DE LA ACTITUD El algoritmo para calcular la actitud del robot móvil se ha representado por el diagrama de bloques señalado en la figura 3.1. La configuración del acelerómetro y del giroscopio se ha realizado en base a la hoja de especificaciones de cada sensor, se puede observar las configuraciones en la tabla 3.1: Tabla 3.1 Configuración de Sensores Configuración de Sensores Acelerómetro (ADLX345). Giroscopio (ITG3200). Comunicación: I2C. Comunicación: I2C. Rango: ±2 g. Rango: ±2000 °/s. Frecuencia Salida de Datos: 1.6 KHz. Frecuencia Salida de Datos: 8 KHz. Se trabaja en rango de ±2 g para obtener una mayor resolución en la medida debido a que idealmente 3.9 mg por bit menos significativo (LSB). La calibración de cada sensor es necesaria debido a que todos los sensores tienen un desvió de la medida y esto perjudica la fiabilidad de la respuesta obtenida de cada sensor, por lo cual es indispensable conocer el desvió de cada eje de los sensores para ser posteriormente removido, para obtener en la posición inicial un valor inicial de la medida igual a cero en cada eje. La figura 3.2 muestra el subproceso que permite conocer el desvió de la medida en cada eje del sensor, para lograr esto, se adquiere los datos de los sensores, se los convierte a cada dato en un dato de 16 bits, para de esta forma sumar una muestra de 1000 datos obtenidos y a este resultado se divide para su número de muestras..

(53) 38. Calibración de sensores. i=0 Calib=0. i<=1000. No. Calib=calib/1000. Si Fin de la subproceso Leer datos de los sensores. Conversión de datos leídos a datos de 16 bits (d16b). calib= calib + d16b. i++ Figura 3.2 Algoritmo para calibración de sensores Después de adquirir algún dato de los sensores, como se había mencionado antes, se debe remover el desvío de la medida de cada eje de los sensores, y a estas transformarlas en unidades de gravedad [g] para el acelerómetro, esto se consigue dividendo el dato adquirido para el valor de 256 que es el número ideal por cada [g] indicado en la hoja de especificaciones del sensor y grados por segundo [°/s] para el giroscopio, de la misma forma basado por la hoja de especificaciones del sensor se divide el valor adquirido por 14.357 ideal por cada [°/s] . Este procedimiento se puede observar en la figura 3.3..

(54) 39. Lectura de los datos de los sensores. Leer datos de sensores (dato) Dato_calib = dato - calib Transformación a unidades [g] acelerómetro y [°/s] giroscopio. Fin de subproceso Figura 3.3 Algoritmo de lectura de sensores 3.1.1. LINEAR WEIGHTED MOVING AVERAGE (LWMA): El indicador Moving Avarage (Media Móvil) muestra el valor medio de la medida tomada de un sensor durante un determinado periodo de tiempo, de esta forma se obtiene una estimación más precisa de la actitud del robot móvil. Existen varios tipos de la media móvil [25]: simple, exponencial, suavizada y ponderada. Lo único que diferencia a las medias móviles de los diferentes tipos son los coeficientes que se asignan a los últimos datos, por lo cual en este caso se utiliza la Media Móvil Ponderada (LWMA) ya que asignan más peso a los últimos datos como se puede observar en la fórmula 3.1.. ‫ ܣܯܹܮݖ‬ൌ. σಿ ೔సబ ௭೔ ‫כ‬ሺேି௜ሻ σಿ ೔ ேି௜. (3.1). Donde N es el número de datos que son usados para realizar la LWMA y z es el valor medido por el sensor..

(55) 40. Linear Weighted Moving Average (LWMA) de los datos del acelerómetro. Declaración e inicialización de variables i=1 N_LWMA=10 z=medida del acelerómetro. i<N_LWMA. Si. z_buf[N_LWMA-i]=z_buf[N_LWMA-i-1]. No. i++. z_buf[0]=z i=0. i<N_LWMA. Si. zLWMA_num += z_buf[i]*(N_LWMA-i). No zLWMA_den += N_LWMA-i zLWMA = zLWMA_num / zLWMA_den i++ ang_acc = asin(zLWMA)*180/pi. Fin del Subproceso. Figura 3.4 Algoritmo LWMA Como se puede observar en la figura 3.4 se han tomado 10 datos (N=10) para realizar el proceso, estos datos se van guardando en z_buf[i] siendo dato0 el último valor obtenido, para después aplicar la fórmula 3.1, de esta forma se obtiene el ángulo correspondiente de acuerdo a la medida de aceleración estática del eje del acelerómetro utilizada..

(56) 41. Este algoritmo se debe realizar para la medida del eje ‫ ݔ‬y ‫ ݕ‬del acelerómetro, de. esta forma con el eje ‫ ݔ‬se obtendrá el ángulo pitch de la actitud, y con el eje ‫ ݕ‬se. obtendrá el ángulo roll de actitud. Se utiliza la función seno inversa para realizar la conversión de [g] a grados. 3.1.2. MÉTODO DE RUGEN-KUTTA (RK). Es un método iterativo de aproximación de soluciones de ecuaciones diferenciales ordinarias [26]. Se ha utilizado RK para resolver la conversión de sistema de coordenada body a un eje fijo en la Tierra mediante el método de Euler, cuya conversión está dada por la fórmula 2.14. RK tiene el siguiente algoritmo:. ‫ݕ‬௜ାଵ ൌ ‫ݕ‬௜ ൅. Donde: ݇ͳ ൌ ݂ሺ‫ݔ‬௜ ǡ ‫ݕ‬௜ ሻ. ሺ௞ଵାଶ‫כ‬௞ଶାଶ‫כ‬௞ଷା௞ସሻ ଺. (3.2). (3.4). ଵ. ଵ. (3.5). ଵ. ଵ. (3.6). ݇ʹ ൌ ݂ሺ‫ݔ‬௜ ൅ ‫݄ כ‬ǡ ‫ݕ‬௜ ൅ ‫݄ כ ͳ݇ כ‬ሻ ଶ ଶ ݇͵ ൌ ݂ሺ‫ݔ‬௜ ൅ ‫݄ כ‬ǡ ‫ݕ‬௜ ൅ ‫݄ כ ʹ݇ כ‬ሻ ଶ. ‫݄כ‬. ଶ. ݇Ͷ ൌ ݂ሺ‫ݔ‬௜ ൅ ݄ǡ ‫ݕ‬௜ ൅ ݇͵ ‫݄ כ‬ሻ. (3.7). ݄ ൌ ‫݋݁ݎݐݏ݁ݑ݉݁݀݋݌݉݁݅ݐ‬. De esta forma el algoritmo de RK que se implantado en el microprocesador está representado en la figura 3.5..

(57) 42. Rugen Kutta (RK) de los datos del giroscopio. Declaración e inicialización de variables. ybuf=y K1=func(ybuf,x). ybuf=y+0.5*dt*k1 K2=func(ybuf,x) ybuf=y+0.5*dt*k2 K3=func(ybuf,x) ybuf=y+0.5*dt*k3 K4=func(ybuf,x) y=y+dt*(k1+2*k2+2*k3+k4)/6. Fin de subproceso. Figura 3.5. Algoritmo de RK En este algoritmo de RK‫ ݔ‬y‫ ݕ‬son vectores que cada uno consta de 3 parámetros, donde ‫ ݔ‬proporciona las medidas de los 3 ejes tomadas del giroscopio (p, q, r) y. ‫ ݕ‬almacena los ángulos de Euler: roll (‫)׎‬, pitch (ߠ) y yaw (ߖ), los cuales son la solución del algoritmo de RK. Este algoritmo tiene una subproceso que se muestra en la figura 3.6..