Facultad de Ciencias de la Electrónica

Transcripción

1 Benemérita Universidad Autónoma de Puebla Facultad de Ciencias de la Electrónica Implementación de un sistema AHRS basado en un Observador No lineal Tesis presentada a la Facultad de Ciencias de la Electrónica como requisito para la obtención del grado de Licenciado en Ingeniería en Mecatrónica por Heberto Madrigal Sastré asesorado por Dr. José Fermi Guerrero Castellanos Dr. Gerardo Mino Aguilar Dr. W. Fermín Guerrero Sánchez Puebla, Pue., México. Diciembre i

2

3 Título: Implementación de un sistema AHRS basado en un Observador No lineal Estudiante: Heberto Madrigal Sastré COMITÉ M.C. Rodrigo Lucio Maya Ramírez Presidente Dr. Germán Ardul Muñoz Hernández Secretario Dra. Liliana Cortez Georgievna Vocal ASESORES Dr. José Fermi Guerrero Castellanos Dr. Gerardo Mino Aguilar Dr. W. Fermín Guerrero Sánchez

4

5 Índice general Agradecimientos Resumen Introducción IX XI XIII 1. Generalidades y Estado del Arte Sistema de Referencia de Actitud y Rumbo (AHRS) AHRS tradicionales Ventajas y desventajas Aplicaciones AHRS Strapdown Ventajas y desventajas Aplicaciones Algoritmos para la estimación de la actitud y rumbo Parametrización de la actitud y rumbo en términos de cuaterniones Estimación de la actitud y rumbo usando Filtros de Kalman Estimación de la actitud y rumbo usando Observadores No Lineal AHRS Strapdown comerciales Sensores comúnmente usados en sistemas AHRS strapdown Sensores Inerciales MEMS Acelerómetros MEMS Clasificación Acelerómetros MEMS piezoresistivos Acelerómetros MEMS capacitivos Acelerómetros MEMS piezoeléctricos Modelo de medición del acelerómetro Girómetros MEMS Clasificación Girómetros MEMS de diapasón Girómetros MEMS de anillo vibratorio Modelo de medición del girómetro iii

6 iv ÍNDICE GENERAL 2.4. Magnetómetros Clasificación Magnetómetros magnetorresistivo Magnetómetros magnetoinductivo Modelo de medición del magnetómetro Diseño e implementación del sistema AHRS Descripción del Observador No Lineal (ONL) Unidad de Mediciones Inerciales (IMU) Magnetómetros Controlador Digital de Señales Módulo bluetooth Batería de Polímero de Litio Estación de monitoreo Integración Adquisición de los datos de los sensores Computo del cuaternión de orientación Comunicación con la estación de monitoreo Interfaz gráfica de la estación de monitoreo Resultados Experimentales Toma de tiempo de la ejecución del Observador No Lineal Comparación de los datos provenientes del dspic y MATLAB Convergencia del cuaternión de la orientación estimado y del bias estimado Comparación del sistema AHRS contra un helicóptero con encoders incrementales Conclusiones 49 A. Cuaterniones 55 A.1. Álgebra de cuaterniones A.1.1. Suma de cuaterniones A.1.2. Resta de cuaterniones A.1.3. Multiplicación de cuaterniones A.1.4. Conjugación de cuaterniones A.1.5. División de cuaterniones A.1.6. Exponenciación de cuaterniones A.1.7. Cuaterniones a ángulos de Euler B. A Robust Nonlinear Observer for Rigid Body Attitude Estimation 61 C. Publicaciones generadas 69

7 Índice de figuras 1.1. Ángulos de alabeo (θ), cabeceo (φ) y guiñada (ψ) Sistema de referencia inercial NED Sistema AHRS tradicional Plataforma gimbalizada LN-3 (Imagen cortesía de Litton Industries) Aplicaciones de los sistemas AHRS tradicionales Sistema AHRS strapdown AHRS strapdown Marconi FIN3110 (Imagen cortesía de Marconi Electronic Systems) Aplicaciones de los sistemas AHRS strapdown Esquema de las componentes de un cuaternión Esquema de las componentes de la velocidad angular ω del cuerpo rígido Ácaro sobre un dispositivo MEMS (Imagen cortesía de Sandia National Laboratories) Estructura básica de un acelerómetro MEMS Sección transversal de un acelerómetro MEMS piezoresistivo Acelerómetro MEMS capacitivo Acelerómetro MEMS piezoeléctrico Fuerza de Coriolis Estructura de un girómetro MEMS de diapasón Estructura de un girómetro MEMS de anillo resonante Magnetómetro magnetorresistivo Integración del hardware y software en el sistema AHRS Diagrama de bloques del Observador No Lineal ( multiplicación de cuaterniones, multiplicación de matrices) IMU Razor Micromag dspic33fj128mc Módulo Bluetooth Batería LiPo Estación de monitoreo v

8 vi ÍNDICE DE FIGURAS 3.9. Diagrama a bloques de la estación de monitoreo Diagrama de bloques del sistema AHRS Diagrama de flujo del programa del dspic33fj128mc Diagrama de conexión maestro-esclavo usando SPI Diagrama a bloques del envío de la información de orientación Panel frontal de la interfaz gráfica de LabVIEW Diagrama a bloques de la recepción de la interfaz gráfica de LabVIEW Sistema AHRS Procedimiento de las pruebas Tiempo de ejecución del sistema AHRS Comparación entre los cuaterniones q ps y ˆq provenientes del dspic Comparación entre el cuaternión q ps de la SVD de Matlab y el del dspic Comparación entre el cuaternión ˆq del ONL de Matlab y el del dspic Convergencia del cuaternión de la orientación estimado ˆq Convergencia del ángulo β del cuaternión de error q e Evolución del bias estimado ˆ ν Evolución del bias estimado ˆ ν, con inyección de un bias constante al bias ν Evolución del bias estimado ˆ ν, con inyección de un bias en forma de rampa al bias ν Twin Rotor MIMO systems Comparación del sistema AHRS contra un helicóptero con encoders incrementales

9 Índice de tablas 1.1. Especificaciones técnicas de MTi y 3DM-GX3-25 (Ver [5] y [6] para más detalles) Especificaciones técnicas de los girómetros LPR530AL y LY530ALH (Ver [25] y [26] para más detalles) Especificaciones técnicas del acelerómetro ADXL335 (Ver [24] para más detalles) Especificaciones técnicas del MicroMag3 (Ver [27] para más detalles) Especificaciones técnicas del dspic33fj128mc802 (Ver [22] para más detalles) Características del módulo Bluetooth Mate Gold (Ver [28] y [29] para más detalles) Características de la batería LiPo Características necesarias de la computadora que conforma la estación de monitoreo (Ver [30] para más detalles) Información de orientación entregada por sistema AHRS vii

10

11 Agradecimientos Esta tesis está dedicada a mis Padres, a quienes agradezco por su cariño, comprensión y apoyo incondicional. A mis hermanos por la compañía y el apoyo que me brindan. Sé que cuento con ellos siempre. A mis amigos por su confianza y lealtad. A mis asesores, en especial al Dr. J. Fermi Guerrero Castellanos por su apoyo, paciencia y enseñanzas dadas durante el desarrollo de esta tesis. Al Dr. W. Fermín Guerrero Sánchez del Laboratorio de Sistemas Dinámicos Controlables por las facilidades prestadas durante el desarrollo de pruebas de este trabajo. Al M.C. Rodrigo Lucio Maya Ramírez, al Dr. Germán Ardul Muñoz Hernández y a la Dra. Liliana Cortez por haber aceptado ser los revisores de esta tesis. Al Cuerpo Académico: Sistemas de Potencia para Tracción, Calidad y Generación de Energía de la FCE, a la Vicerrectoría de Investigación y Estudios de Posgrado (VIEP) y al Programa de Mejoramiento del Profesorado (PROMEP) por las becas y por el apoyo económico brindado para la compra de material y equipo necesario para la realización de la presente tesis. ix

12

13 Resumen En el presente trabajo de tesis se desarrolla la implementación de un Sistema de Referencia de Actitud y Rumbo (AHRS) usando sensores inerciales y magnéticos de bajo costo. Un sistema AHRS sirve para estimar la orientación de un cuerpo rígido con respecto a un sistema de referencia inercial. Debido a que la orientación no puede ser medida directamente se debe de hacer una estimación de ésta fusionando las señales de diferentes sensores, en nuestro caso se utilizaron acelerómetros, girómetros y magnetómetros. Estos sensores pueden ser clasificados como sensores de velocidad angular (girómetros) y sensores de vectores de observación (magnetómetros y acelerómetros). El Observador No Lineal basado en cuaterniones desarrollado en [34] es implementado con el fin de fusionar todas las fuentes de información y así obtener la estimación de la orientación. El problema de la estimación de la orientación es resuelta en dos partes. En la primera parte, un cuaternión de la orientación es estimado por medio de los vectores de observación. En este primer paso, la estimación de la orientación se realiza usando una SVD (Descomposición en Valores Singulares). Después, el cuaternión obtenido en esta primera etapa es considerado como una orientación medida aunque se encuentra contaminada por los ruidos presentes en los sensores de vectores de observación. La segunda parte del método propuesto consiste en un Observador No Lineal a fin de producir la estimación del componente del ruido bias presente en las medidas de los girómetros y el cuaternión de la orientación. Este observador es conducido por una ley de retroalimentación, la cual consiste en un cuaternión del error obtenido por medio del cuaternión propagado por la ecuación del observador el cual contiene las medidas del girómetro y el cuaternión obtenido de la técnica de la SVD. La orientación es parametrizada mediante el cuaternión unitario, lo que evita las singularidades geométricas presentes en los ángulos de Euler. Se prestó especial atención al bajo consumo de energía, velocidad y peso del sistema, lo que llevó a la selección de un Controlador Digital de Señales (DSC) de 16 bits, sensores inerciales MEMS y sensores magnetoinductivos de bajo consumo energético. El conjunto de sensores está basado en un acelerómetro de tres ejes, un girómetro de dos ejes, un girómetro de eje único y un magnetómetro de tres ejes. Además, el sistema está equipado con un módulo Bluetooth, que proporciona capacidades inalámbricas. El voltaje de alimentación del circuito electrónico del sistema AHRS es de 3,3 V. La dimensión y el peso son 80x40x50 mm y 82 g, respectivamente. La metodología de estimación de la orientación es implementada en tiempo real logrando así una frecuencia de 55.5 Hz de la salida de los datos xi

14 xii Resumen (cuaternión estimado de la orientación y las medidas de la aceleración, velocidad angular y campo magnético). Aunado a esto, se creó una estación de monitoreo. Tal estación de monitoreo es una mini-notebook, que corre un programa hecho en LabVIEW encargado de realizar la recepción, visualización y almacenamiento del cuaternión estimado de la orientación y las mediciones de los sensores.

15 Introducción La navegación es una habilidad o un arte muy antigua que se ha convertido en una ciencia compleja. La navegación puede definirse como el conjunto de técnicas utilizadas para desplazarse entre un par de puntos conocidos, llamados origen y destino, siguiendo una trayectoria también conocida. Si bien durante mucho tiempo el término navegación estuvo asociado esencialmente a barcos, el desarrollo de la aviación le agregó una nueva dimensión: además de la posición horizontal (latitud y longitud), se necesita también la altura de la aeronave para garantizar que no se acerque peligrosamente a algún obstáculo. Se habla entonces de navegación 3D. En este tipo de navegación aparte de conocer bien la posición también es necesario conocer la orientación. La estimación de la orientación es un tema que ha atraído un fuerte interés en los últimos años. Esto se debe a las múltiples aplicaciones, es decir, naves espaciales, satélites, misiles tácticos, donde la orientación es esencial para el control o la vigilancia. En la última década la aplicación de los Sistemas Micro-electromecánicos (MEMS) ha ganado un fuerte interés. El desarrollo de los sensores MEMS inerciales ha sido impulsado por la necesidad de soluciones de sensado de bajo costo en aplicaciones militares y comerciales. Además de la estimación de la orientación en la industria aeroespacial y en la industria automovilística, el reducido tamaño de los sensores MEMS inerciales ha impulsado nuevas aplicaciones en la robótica, en la realidad virtual y la en biomecánica. Por otra parte, el interés en vehículos aéreos no tripulados (UAVs) ha incentivado el desarrollo de sistemas AHRS de bajo costo, ligeros y de bajo consumo de energía. El trabajo desarrollado en la presente tesis consiste en la implementación de un sistema de la estimación de la orientación basado en un Observador No Lineal. Para la descripción del sistema implementado, el presente documento es organizado como se indica a continuación. En el capítulo 1 se hace una revisión del estado del arte de los Sistemas de Referencia de Actitud y Rumbo, que es el marco donde se clasifica este trabajo. En el capítulo 2 se describen los sensores de bajo consumo de energía y un tamaño reducido que comúnmente son utilizados en los sistemas AHRS strapdown. El capítulo 3 presenta a detalle la implementación del sistema, así como los componentes empleados y su interacción. En el capítulo 4 se muestran los resultados de las pruebas a las que se sometió el sistema AHRS y por último en el capítulo 5 se dan las conclusiones. xiii

16 xiv Objetivos Introducción Para el trabajo de tesis se fijaron los siguientes objetivos: Objetivo General: Implementar un sistema de referencia de actitud y rumbo AHRS (Attitude Heading Reference System) utilizando un dspic-dsc y sensores de tecnología MEMS. Objetivos Específicos: Desarrollar el sistema mínimo basado en el dspic33fj128mc802 y el conjunto de sensores MEMS. Implementar en el dspic33fj128mc802 el algoritmo de fusión de datos basado en un observador no lineal, para la determinación de la actitud y rumbo (orientación). Diseñar la tarjeta de comunicación de datos que envíe las variables adquiridas a través del observador no lineal a una computadora por medio de una interfaz vía bluetooth. Diseñar la interfaz de usuario que permita visualizar, analizar y almacenar las variables del sistema. Implementar un prototipo conteniendo los sensores y la tarjeta de comunicación de datos.

17 Capítulo 1 Generalidades y Estado del Arte 1.1. Sistema de Referencia de Actitud y Rumbo (AHRS) En la navegación aérea la medida precisa de la orientación (actitud y rumbo) con respecto a un sistema de referencia inercial es una necesidad primordial, ya que ésta auxilia al piloto para mantener el vuelo, así como a su navegación, sistema conocido como horizonte artificial; en el caso de los vehículos aéreos tripulados. Para el caso de los vehículos aéreos autónomos se debe de conocer la orientación a cada instante, para que el control pueda computar las señales que se enviarán a los actuadores con el fin de mantener una orientación deseada. En la figura 1.1 se muestran los ángulos de Euler usados para describir la orientación de un objeto en tres dimensiones. Figura 1.1: Ángulos de alabeo (θ), cabeceo (φ) y guiñada (ψ) 1

18 CAPÍTULO 1. GENERALIDADES Y ESTADO DEL ARTE 1.2. AHRS TRADICIONALES Un sistema de Referencia de Actitud y Rumbo, AHRS por sus siglas en inglés (Attitude and Heading Reference System), es un sistema cuya función principal es hacer la estimación de la orientación de un avión con respecto a un sistema de referencia inercial. En la figura 1.2 se muestra el sistema de referencia NED (Norte, Este, Abajo), que es el más utilizado en los sistemas AHRS. Figura 1.2: Sistema de referencia inercial NED Los AHRS están compuestos por dos clases de sensores: sensores inerciales (acelerómetros y girómetros) y sensores magnéticos (magnetómetros), midiendo aceleración lineal, velocidad angular e intensidad del campo magnético respectivamente. Los sensores son de 3 ejes cada uno y se encuentran dispuestos de manera ortogonal, de tal forma que los ejes sensibles de los sensores coinciden con los ejes de rotación del vehículo donde es colocado [1]. Al conjunto de girómetros y acelerómetros se le conoce como Unidad de Mediciones Inerciales ó IMU(del inglés Inertial Measurement Unit). Los sistemas AHRS no sólo limitan su uso a vehículos aéreos, sino también pueden ser empleados en vehículos terrestres, acuáticos, submarinos y espaciales; de hecho pueden ser usados sobre cualquier cuerpo rígido en el que se desee conocer su orientación, por esta razón se comienza a trabajar con estos sistemas en otras áreas como la robótica, biomecánica, realidad virtual, videojuegos, etc AHRS tradicionales Los primeros AHRS hicieron posible la navegación autónoma y su aparición se remonta a la segunda guerra mundial en las famosas bombas alemanas V1 y V2 [7]. Estos son complejos sistemas mecánicos, formados principalmente por: motores, gimbales y pasados acelerómetros y girómetros mecánicos. En los sistemas AHRS tradicionales los acelerómetros se encuentran dispuestos ortogonalmente y sus ejes de medición alineados con el del sistema de referencia NED [8]. Para mantener está orientación los sensores inerciales son aislados de cualquier movimiento de rotación externo mediante un conjunto de tres gimbales que son giro-estabilizados (ver figura 1.3). Los gimbales son estructuras en forma de anillo, anidadas entre si y en sus extremos tienen rodamientos que están conectados a un servomotor en un extremo y en el otro a un encoder. 2

19 CAPÍTULO 1. GENERALIDADES Y ESTADO DEL ARTE 1.2. AHRS TRADICIONALES Figura 1.3: Sistema AHRS tradicional El giro de los gimbales es controlado por los servomotores, los cuales están conectados a las salidas de los girómetros; y sin importar como se mueva el vehículo, el gimbal interno siempre mantendrá su respectiva orientación. El gimbal interno puede considerarse como una plataforma estable, sobre la cual están montadas los acelerómetros y girómetros. Generalmente a todo el arreglo se le conoce como plataforma gimbalizada, en la figura 1.4 se muestra un ejemplo de una plataforma gimbalizada. Por medio de los encoders se puede conocer el rumbo y la actitud del vehículo, cada encoder mide un ángulo diferente: el encoder en el gimbal interno mide el ángulo de guiñada, el encoder del gimbal intermedio mide el ángulo de cabeceo y el encoder del gimbal externo mide el ángulo de alabeo. Figura 1.4: Plataforma gimbalizada LN-3 (Imagen cortesía de Litton Industries) 3

20 CAPÍTULO 1. GENERALIDADES Y ESTADO DEL ARTE 1.2. AHRS TRADICIONALES Ventajas y desventajas A continuación se muestran las ventajas y las desventajas que presentan los AHRS tradicionales. Ventajas: El aislamiento de los sensores inerciales de las altas velocidades angulares, lo que elimina muchos errores de los sensores dependientes de la velocidad y generalmente permiten a los sensores una mayor exactitud. Desventajas: Son sistemas mecánicos complejos Son costosos Son pesados Tienen un alto consumo de energía Deben de ser calibrados constantemente Propensos a sufrir desgaste mecánico Aplicaciones Las aplicaciones que tienen los AHRS tradicionales es su uso principalmente en aeronaves, pero también se les puede encontrar en barcos, submarinos, misiles y naves espaciales. En la figura 1.5 se muestran algunos ejemplos de las aplicaciones de los AHRS tradicionales. (a) Submarinos (Imagen cortesía de Royal Navy). (b) Cohetes (Imagen cortesía de la NASA). 4

21 CAPÍTULO 1. GENERALIDADES Y ESTADO DEL ARTE 1.3. AHRS STRAPDOWN (c) Aeronaves (Imagen cortesía de U.S. Air Force). Figura 1.5: Aplicaciones de los sistemas AHRS tradicionales AHRS Strapdown Los sistemas AHRS strapdown están formados principalmente por: un microprocesador o un microcontrolador más los sensores inerciales como se logra apreciar en la figura 1.7. En los sistemas strapdown los acelerómetros y girómetros no son aislados de las rotaciones del vehículo como sucede en los sistemas AHRS tradicionales [9]. Por lo tanto, los sensores son expuestos a altas velocidades de giro, que pueden provocar errores en los sensores dependientes de la orientación y la velocidad. Esto se debe principalmente a que la IMU es montada rígidamente dentro del vehículo, con una orientación alineada a los ejes de giro de éste. Figura 1.6: Sistema AHRS strapdown 5

22 CAPÍTULO 1. GENERALIDADES Y ESTADO DEL ARTE 1.3. AHRS STRAPDOWN Los sistemas de plataforma estable y los sistemas strapdown están basados en los mismos principios subyacentes. Los sistemas strapdown han reducido la complejidad mecánica y tienden a ser físicamente más pequeños que los sistemas de plataforma estable. Estos beneficios se deben a que los gimbales han sido reemplazados por software. Esto implica un mayor costo computacional al hacer la estimación de la orientación, pero es más barato que la implementación de gimbales. Como el costo de la computación ha disminuido los sistemas strapdown se han convertido en los AHRS dominantes. Figura 1.7: AHRS strapdown Marconi FIN3110 (Imagen cortesía de Marconi Electronic Systems) Ventajas y desventajas A continuación se muestran las ventajas y las desventajas que presentan los AHRS strapdown. Ventajas: Son más baratos que los sistemas de gimbales En aplicaciones en las que se requiere el control de la actitud del vehículo, los girómetros strapdown proveen una mayor precisión que los de los sistemas gimbales. Son pequeños No son pesados Tienen un bajo consumo de energía No presentan desgastes mecánicos 6

23 CAPÍTULO 1. GENERALIDADES Y ESTADO DEL ARTE 1.3. AHRS STRAPDOWN Desventajas: Los sensores deben de operar a velocidades de rotación más grandes en comparación con los usados en sistemas gimbales. La entrada dinámica de los girómetros strapdown debe ser de magnitudes más grandes que las de los girómetros de los sistemas gimbales. Son más económicos que los sistemas gimbales, pero siguen teniendo un elevado costo para el público en general Aplicaciones Los AHRS strapdown tienen las mismas aplicaciones que los AHRS tradicionales, pueden ser empleados en cualquier tipo de vehículo. Sin embargo, los AHRS strapdown basados en tecnología MEMS tienen un campo mucho más amplio de aplicaciones. Dentro de estas aplicaciones podemos mencionar: UAV 1, plataformas de estabilización, robótica asistida, juegos de video, captura de movimientos, biomecánica, etc. En la figura 1.8 se muestran algunas aplicaciones de los AHRS strapdown. (a) Captura de movimiento (Imagen cortesía de Xsens). Figura 1.8: Aplicaciones de los sistemas AHRS strapdown. 1 Un vehículo aéreo no tripulado UAV por siglas en inglés (Unmanned Aerial Vehicle), es una aeronave que vuela sin tripulación humana a bordo. 7

24 CAPÍTULO 1. GENERALIDADES Y ESTADO DEL ARTE 1.4. ALGORITMOS PARA LA ESTIMACIÓN DE LA ACTITUD Y RUMBO (b) Biomecánica (Imagen cortesía de FATRONIK). (c) Robótica móvil (Imagen cortesía de irobot). Figura 1.8: Aplicaciones de los sistemas AHRS strapdown 1.4. Algoritmos para la estimación de la actitud y rumbo Los AHRS strapdown son en realidad estimadores de la orientación, ya que esta no puede ser medida directamente y debe de ser estimada usando diferentes tipos de sensores tales como acelerómetros, girómetros y magnetómetros. La fusión de las señales de los sensores para obtener la orientación se hace por medio de algoritmos matemáticos de estimación implementados en microprocesadores ó microcontroladores Parametrización de la actitud y rumbo en términos de cuaterniones La orientación de cualquier cuerpo rígido puede ser representada por medio de cuaterniones. Esta representación consiste en un vector unitario e y un ángulo de rotación β alrededor de dicho eje, tal como se muestra en la figura 1.9. El cuaternión q está entonces definido como sigue donde q = [ cos β 2 e sin β 2 ] = [ q0 q ] H (1.1) H = {q q q T q = 1, q = [q 0 q] T, q 0 R, q R 3 } (1.2) q = [q 1 q 2 q 3 ] T y q 0 son las partes vectorial y escalar del cuaternión respectivamente. 8

25 CAPÍTULO 1. GENERALIDADES Y ESTADO DEL ARTE 1.4. ALGORITMOS PARA LA ESTIMACIÓN DE LA ACTITUD Y RUMBO Figura 1.9: Esquema de las componentes de un cuaternión. En aplicaciones de control de orientación, el cuaternión unitario representa la rotación de un sistema de coordenadas inercial N(x n, y n, z n ) localizado en algún punto del espacio, al sistema de coordenadas B(x b, y b, z b ) localizadas en el centro de masa del cuerpo rígido. Donde B es un sistema de referencia móvil. El producto de cuaterniones se define como: [ ] [ T q10 q 1 q20 q 1 q 2 = q 1 I 3 q 10 + [ q 1 ] q 2 ] (1.3) donde denota la multiplicación de cuaterniones, I 3 es la matriz identidad y [ ξ ] es el tensor antisimétrico asociado con el vector axial ξ y está dado por [ ξ ] = ξ 1 ξ 2 ξ 3 0 ξ 3 ξ 2 = ξ 3 0 ξ 1 (1.4) ξ 2 ξ 1 0 El error de orientación es utilizado para cuantificar el desface entre dos orientaciones. Si q define la orientación verdadera y ˆq es el cuaternión estimado, entonces el cuaternión de error que representa el error de orientación entre la orientación verdadera y la estimada viene dado por q e = ˆq 1 q = [q e0 q e T ] T (1.5) donde ˆq 1 es la rotación complementaria del cuaternión ˆq, el cual es el cuaternión conjugado [31]. 9

26 CAPÍTULO 1. GENERALIDADES Y ESTADO DEL ARTE 1.4. ALGORITMOS PARA LA ESTIMACIÓN DE LA ACTITUD Y RUMBO En el caso de que el cuaternión de verdadero y el cuaternión estimado coincidan, el cuaternión de error se convierte en q e = [±1 0 T ] T (1.6) Esto se debe al hecho de que q y q representan la misma orientación física. En realidad, uno se encuentra girado 2π relativamente al otro alrededor del vector unitario e. Figura 1.10: Esquema de las componentes de la velocidad angular ω del cuerpo rígido. Denotando con ω = [ω 1 ω 2 ω 3 ] T el vector de velocidad angular del marco de referencia en el cuerpo B relativo al marco inercial N, se muestra en la figura 1.10 la ecuación cinemática está dada por [ ] = q0 q 1 [ 2 q 1 T I 3 q 0 + [ q 1 ] ] ω = 1 Ξ(q) ω (1.7) Estimación de la actitud y rumbo usando Filtros de Kalman Uno de los principales usos de los observadores es la estimación de variables de estado de un sistema en el cual las medidas están contaminadas con ruido. Considere un sistema lineal discreto con la siguiente dinámica: x k+1 = Ax k + Bu k + w k (1.8) y k = Cx k + v k (1.9) 10

27 CAPÍTULO 1. GENERALIDADES Y ESTADO DEL ARTE 1.4. ALGORITMOS PARA LA ESTIMACIÓN DE LA ACTITUD Y RUMBO En las ecuaciones anteriores A, B y C son matrices, k es el índice de tiempo, x k es llamado el estado del sistema, u k es una entrada conocida del sistema, y k es la salida medida, w k y z k son los ruidos blancos Gaussianos de media cero. La variable w k se llama el ruido del proceso y la variable z k se llama ruido de la medición. El vector x k contiene toda la información sobre el estado actual del sistema, pero en general no puede ser medido directamente. En su lugar, medimos y k que es una función de x k y que está dañada por el ruido z k y por medio de ésta se estima x k. Por lo anterior, w k y z k satisfacen: E(w k ) = 0 E(z k ) = 0 (1.10) E(w j w T k ) = S w para j k y cero en otro caso (1.11) E(z j z T k ) = S z para j k y cero en otro caso (1.12) S w y S z son la matrices de covarianza para la perturbación w en el proceso y el ruido z en la media. El observador óptimo que permitirá estimar el estado x está dado por las siguientes ecuaciones y es conocido como el Filtro de Kalman (KF): ˆx k+1 = (Aˆx k + Bu k ) + K k (y k+1 C ˆx k ) (1.13) K k = AP k C T (CP k C T + S z ) 1 (1.14) P k+1 = AP k A T + S w AP k C T S 1 z CP k A T (1.15) Donde ˆx es el vector de estados estimado, K es la matriz de ganancias de Kalman y P es la matriz de covarianza del error de estimación (ˆx x). El KF provee un eficiente metodo computacional para combinar señales de salida de sensores contaminados por ruido para estimar el estado de un proceso lineal estocástico, bajo la suposición que el ruido que afecta las medidas es blanco y presenta una distribución Gaussiana. Debido a que en la naturaleza la mayoría de los sistemas son no lineales, la condición de linealidad no puede ser satisfecha todo el tiempo y ni para todas las aplicaciones. La estimación de la actitud y rumbo es un ejemplo de cuando el sistema es no lineal debido a las ecuaciones involucradas. Para el caso de los sistemas no lineales se puede usar el Filtro de Kalman Extendido (EKF), el cual es una ampliación del Filtro de Kalman ordinario [10]. En el sistema en el que se diseña un EKF se asume que todos los componentes del ruido tienen media cero y siguen una función de probabilidad de distribución Gaussiana. Sin embargo, ésta no es una suposición válida, especialmente cuando el ruido es parte de una función no lineal. Por ejemplo, el problema de la estimación de la actitud y rumbo usando EKF consiste en la linealización de ecuaciones dinámicas altamente no lineales. El filtro de Kalman Extendido trabaja de manera eficiente en las zonas en las que se realizó la linealización, fuera de ellas presenta problemas de divergencia. Además que no se garantiza la convergencia global del sistema. 11

28 CAPÍTULO 1. GENERALIDADES Y ESTADO DEL ARTE 1.5. AHRS STRAPDOWN COMERCIALES Estimación de la actitud y rumbo usando Observadores No Lineal En los últimos años, importantes esfuerzos en la investigación se han dirigido hacia la síntesis de observadores no lineales de la orientación, con el fin de abordar los problemas de divergencia, la distribución no Gaussiana del ruido y alto costo computacional que presenta el Filtro de Kalman Extendido. El primer trabajo en este tópico fue presentado en [11]. Posteriormente, en [12] y [13] observadores no lineales basados en cuaterniones y observadores del bias de los girómetros son propuestos en el marco de vehículos de navegación de la marina y en la calibración de sensores de satélites, respectivamente. Además, la formulación de observadores no lineales de la orientación basados en el grupo ortogonal de matrices SO(3) 2 se han propuesto recientemente en [14], [15], [16]. Estos observadores no lineales (a excepción de [16]) no toman en cuenta el ruido presente en las mediciones de girómetros ni el término del bias variante en el tiempo. Además, la complejidad computacional de estos enfoques es una desventaja, ya que estos algoritmos trabajan con matrices de 3 3 que tienen que conservar su ortogonalidad AHRS Strapdown comerciales En el mercado existen diversos sistemas que permiten la estimación de la posición y la orientación de un cuerpo en el espacio [2][3][4]. Sin embargo, estos sistemas son caros y además el algoritmo de fusión de datos diverge cuando son experimentados movimientos de alta frecuencia. Debido a estos inconvenientes los AHRS comerciales no han tenido una gran penetración en el mercado. En la Tabla 1.1 se hace la comparación de dos AHRS comerciales: MTi de Xsens y 3DM-GX3-25 de MicroStrain. Tabla 1.1: Especificaciones técnicas de MTi y 3DM-GX3-25 (Ver [5] y [6] para más detalles). Características MTi 3DM-GX3-25 Rango de orientación 360 o 360 o Rango de vel. angular ± 360 /s ± 360 /s Resolución de la orientación(c. estáticas) ± 0.5 ± 0.5 Resolución de la orientación(c. dinámicas) ± 2.0 ± 2.0 Frecuencia de la salida de datos 512Hz 1000Hz Voltaje de alimentación 4.4V - 6.0V 4.5V V Dimensiones mm mm Interfaz digital USB 2.0 y/o RS-232 USB 2.0 y/o RS-232 Peso 50g 18g Precio 2334 USD 2295 USD 2 El grupo ortogonal de matrices SO(3) tienen las siguientes características: son matrices de 3 3, su matriz inversa es igual a la matriz transpuesta y su determinante es igual a 1. 12

29 Capítulo 2 Sensores comúnmente usados en sistemas AHRS strapdown Este capítulo trata sobre los sensores inerciales y sensores magnéticos que comúnmente se utilizan en los sistemas AHRS strapdown. De esta forma dentro de este capítulo primero se describe el funcionamiento de cada sensor, después se muestra como se clasifican y por último se presentan los modelos de mediciones de los sensores. Debido a la gran cantidad de sensores existentes que pueden ser utilizados en los sistemas AHRS strapdown, en este capítulo solo se expondrán aquellos que tengan un tamaño reducido y un bajo consumo de energía. Es decir, se exhibirán aquellos basados principalmente en la tecnología MEMS para el caso de los acelerómetros y girómetros; en el caso de los magnetómetros sólo se expondrán aquellos que presenten las mismas condiciones anteriores más la restricción de que su rango de medición incluya el campo magnético terrestre Sensores Inerciales MEMS Los Sistemas Microelectromecánicos (Microelectromechanical Systems, MEMS) son dispositivos de escala micrométrica que integran elementos mecánicos y electrónicos. El tamaño de estos dispositivos puede variar desde un micrómetro hasta un milímetro (ver figura 2.1). Los dispositivos MEMS superan muchas de las características que habían impedido la adopción de sistemas inerciales en muchas aplicaciones potenciales, especialmente donde el costo, el tamaño y el consumo de energía se encuentran parametrizados. Las fuerzas externas que actúan sobre un cuerpo causan una aceleración y/ó un cambio de su orientación (posición angular). La rapidez del cambio de la posición angular es la velocidad angular. Un sensor inercial que mide la aceleración lineal de un cuerpo sobre su eje sensible se llama acelerómetro. Del mismo modo, un sensor que mide la velocidad angular en un sistema inercial sobre su eje sensible se llama girómetro. Los sensores inerciales MEMS ofrecen una alta resolución y un bajo consumo de energía, en un tamaño extremadamente pequeño. Debido a esto, se han convertido en los sensores más populares para medir movimiento, aceleración, inclinación y vibración. 13

30 CAPÍTULO 2. SENSORES COMÚNMENTE USADOS EN SISTEMAS AHRS STRAPDOWN 2.2. ACELERÓMETROS MEMS Figura 2.1: Ácaro sobre un dispositivo MEMS (Imagen cortesía de Sandia National Laboratories) Acelerómetros MEMS Un acelerómetro es un sensor que mide aceleraciones o movimientos de traslación. Los acelerómetros MEMS están formados principalmente por tres elementos: una masa de prueba que genera una fuerza inertial debido a una aceleración o desaceleración en el acelerómetro, una suspensión(resorte) que soporta la masa de prueba y la regresa a su posición inicial una vez que la aceleración termina y un sistema de medición que relaciona la señal de salida con la aceleración inducida. La posición de equilibrio de la masa de prueba es a una aceleración cero. Cuando el acelerómetro está sujeto a una aceleración, como se muestra en la figura 2.2, la masa de prueba tiende a resistirse al cambio de movimiento debido a su propia inercia. Como resultado, la masa es desplazada en sentido contrario con respecto al cuerpo. La extensión neta del resorte provee una medida de la fuerza aplicada, que es proporcional a la aceleración. Figura 2.2: Estructura básica de un acelerómetro MEMS. 14

31 CAPÍTULO 2. SENSORES COMÚNMENTE USADOS EN SISTEMAS AHRS STRAPDOWN 2.2. ACELERÓMETROS MEMS Una aceleración en dirección positiva causará que la masa de prueba se mueva hacía abajo con respecto a la posición de equilibrio. Cuando el acelerómetro se encuentra estático y bajo un campo gravitacional. La masa de prueba se desplazada hacia abajo debido a la fuerza gravitacional. Como el acelerómetro se encuentra estático, una aceleración en dirección contraria debe de ser ejercida por éste. Por lo tanto, éste ejerce una menos aceleración gravitacional. La fuerza total (F ) que actúa en la masa de prueba (m) en el espacio puede ser representada por la siguiente ecuación: F = m a = m f m g (2.1) donde f es la aceleración producida por fuerzas distintas a la del campo gravitacional. En el caso de arriba el acelerómetro se encuentra estático (i.e., a es 0). Por lo tanto la salida del acelerómetro para este ejemplo es a = g. Otra forma de visualizar esto es imaginando que el acelerómetro se encuentra en caída libre. Para este caso a = 0 debido a que f = g. Para orientarse con respecto a sistema de referencia inercial, se requiere conocer las aceleraciones del cuerpo a a lo largo de tres ejes. Comúnmente se utilizan tres acelerómetros de un eje montados ortogonalmente para proveer mediciones independientes de una fuerza específica Clasificación Una forma común de clasificar los acelerómetros se basa en los tipos de transducción utilizado para convertir el desplazamiento mecánico de la masa de prueba en una señal eléctrica. Algunos de los principios comunes son los sensores piezoresistivos, capacitivos y piezoeléctricos Acelerómetros MEMS piezoresistivos El funcionamiento de estos sensores se basa en el efecto piezoresistivo. Este efecto puede resumirse como en el cambio de la resistencia de un material al deformarse. Los acelerómetros piezoresistivos están compuestos por una masa de prueba, una viga y un elemento piezoresistivo. El material piezoresistivo se encuentra al inicio de la viga y en el otro extremo está la masa de prueba. Cuando el acelerómetro es encuentra sujeto bajo a una aceleración, la masa de prueba es desplazada de su posición de equilibrio ocasionando que la viga se alargue o se acorte, ocasionando cambios de tensión y resistencia en el material piezoresistivo. Esta variación de la resistencia es detectada por un circuito puente Wheatstone que emite una señal eléctrica proporcional a la aceleración. Los acelerómetros piezoresistivos cuentan con elementos de protección en caso de que los esfuerzos en la viga superen el esfuerzo de ruptura de su 15

32 CAPÍTULO 2. SENSORES COMÚNMENTE USADOS EN SISTEMAS AHRS STRAPDOWN 2.2. ACELERÓMETROS MEMS material. Estos son colocados por debajo y por encima de la viga limitando el desplazamiento de la masa de prueba, cuando el acelerómetro se encuentra expuesto a altas aceleraciones. Figura 2.3: Sección transversal de un acelerómetro MEMS piezoresistivo. La principal ventaja de estos sensores es su fácil proceso de fabricación. Sin embargo, estos sensores presentan una alta sensibilidad indeseada a la temperatura y una baja sensibilidad a la salida. Los niveles típicos de rendimiento de estos dispositivos son una sensibilidad de 1.3 mv/g, 5-50 G rango dinámica, y el coeficiente de temperatura compensada de 0,2 %/ C Acelerómetros MEMS capacitivos Los acelerómetros capacitivos miden la aceleración utilizando el cambio en la capacitancia de un capacitor formado por su estructura. La estructura básica de estos sensores se muestra en la figura 2.4. En ésta, el capacitor se forma entre los electrodos conectados a la masa de prueba y los electrodos unidos al anclaje estacionario. Las aceleraciones hacen que la distancia entre los electrodos fijos y los electrodos móviles se incremente de un lado y disminuya en el otro. Este fenómeno altera la capacitancia inicial de manera proporcional a la aceleración experimentada. Figura 2.4: Acelerómetro MEMS capacitivo. 16

33 CAPÍTULO 2. SENSORES COMÚNMENTE USADOS EN SISTEMAS AHRS STRAPDOWN 2.2. ACELERÓMETROS MEMS La ventajas que presentan los acelerómetros capacitivos son: una alta sensibilidad, un nivel bajo de ruido, una baja sensibilidad a la temperatura, una salida lineal, baja disipación de energía y una estructura simple. Sin embargo, estos dispositivos son propensos a interferencias electromagnéticas, por lo que necesitan un empaquetado especial para proteger al acelerómetro y a su circuito electrónico. Además, la sensibilidad y la resolución de los acelerómetros capacitivos dependen del área de las placas que forman los capacitores Acelerómetros MEMS piezoeléctricos En los acelerómetros piezoeléctricos la fuerza o aceleración aplicada sobre la masa de prueba es convertida a una señal eléctrica por un material piezoeléctrico. Los materiales piezoeléctricos al ser sometidos a tensiones mecánicas adquieren una polarización eléctrica, lo que provoca una deferencia de potencial. En la figura 2.5 se muestra la estructura básica de un acelerómetro piezoeléctrico, la cual utiliza una película delgada de un material piezoeléctrico. La masa de prueba cuando experimenta una aceleración ejerce una fuerza sobre el material piezoeléctrico. La fuerza sobre el material piezoeléctrico genera una carga eléctrica que puede ser detectada por un amplificador. Figura 2.5: Acelerómetro MEMS piezoeléctrico. La ventaja de estos sensores es que tienen un bajo consumo de potencia y una integración monolítica con el circuito electrónico. Sin embargo, al igual que los acelerómetros piezoresistivos también presentan el inconveniente de ser sensibles a la temperatura Modelo de medición del acelerómetro El acelerómetro de tres ejes sensa las fuerzas inerciales y la gravedad en el sistema de coordenadas móvil B. El vector de medidas formado por las salidas de los tres acelerómetros se escribe de la forma: ba = R T E( a g) + η A (2.2) donde g = [0 0 g] T, a R 3 y g = 9,81 m/s 2. RE T R3 3 es la matriz de rotación que expresa las coordenadas de cualquier vector en el sistema de referencia móvil en el sistema de referencia inercial. Utilizando a los ángulos de Euler como parametrización de la orientación, la matriz R E esta dada por: 17

34 CAPÍTULO 2. SENSORES COMÚNMENTE USADOS EN SISTEMAS AHRS STRAPDOWN 2.3. GIRÓMETROS MEMS donde c x = cos x y s x = sin x. c θ c ψ c ψ s θ s φ s ψ c θ c ψ s θ c φ + s ψ s φ R E = c θ s ψ s ψ s θ s φ + c ψ c φ s ψ s θ c φ c ψ s φ (2.3) s θ c θ s φ c θ c φ Note que cuando la aceleración absoluta del vehículo es pequeña ( a g ) las medidas de los acelerómetros proporcionan los componentes del vector de gravedad. η A R 3 es considerado un vector de ruido blanco Gaussiano Girómetros MEMS Un girómetro es un sensor de velocidad angular que mide la velocidad angular de rotación. Los girómetros MEMS usan elementos mecánicos vibrantes, llamados masas de prueba, para monitorear el movimiento rotacional. Estos sensores no tienen ninguna parte móvil que requiera de rodamientos y por lo tanto pueden ser miniaturizados para reducir el costo del sensor. Los girómetros MEMS basan su funcionamiento en la detección de una fuerza. Dicha fuerza actúa sobre la masa de prueba que está sujeta a un movimiento lineal vibratorio dentro de un sistema de referencia inercial; el cual está girando alrededor de un eje perpendicular al eje del movimiento lineal. La fuerza resultante se conoce como fuerza de Coriolis y actúa en una dirección perpendicular al eje de rotación y al eje de vibración. Este fenómeno se muestra en la figura 2.6. Figura 2.6: Fuerza de Coriolis. La fuerza de Coriolis muchas veces es llamada una fuerza ficticia, por que no aparece cuando el movimiento es expresado en un sistema de referencia inercial [17]. La magnitud de la fuerza de Coriolis está dada por (2.4). F Coriolis = 2m ω v (2.4) donde v y ω son la velocidad lineal y la velocidad angular respectivamente, a las que es sometida la masa de prueba. 18

35 CAPÍTULO 2. SENSORES COMÚNMENTE USADOS EN SISTEMAS AHRS STRAPDOWN 2.3. GIRÓMETROS MEMS Todos los girómetros vibratorios hacen uso del efecto Coriolis para medir la velocidad angular. Cuando un girómetro es acelerado linealmente, la masa de prueba se mueve en la misma dirección. Por lo tanto no se genera la fuerza de Coriolis y la salida del girómetro será el valor correspondiente a una velocidad angular cero, esto muestra que los girómetros MEMS no son sensibles a aceleraciones lineales, tales como inclinaciones, golpes o vibraciones Clasificación Generalmente los girómetros pueden clasificarse de acuerdo a su diseño de fabricación. Estos diseños son de diapasón y de anillo vibratorio, los cuales serán descritos en las siguientes secciones Girómetros MEMS de diapasón Los girómetros de diapasón son un ejemplo común de los girómetros vibratorios. Estos girómetros consisten en dos diapasones que son conectados por un bloque central unido a una estructura de soporte, como se muestra en la figura 2.7. El diapasón superior se encuentran sujeto a una vibración de amplitud fija en el plano del dispositivo. Esto representa el movimiento primario del girómetro. Los dientes del diapasón vibran en una condición de antifase por lo que ninguna fuerza resulta en la estructura de soporte desde el movimiento primario. El mecanismo usado para producir la resonancia en los dientes puede ser eléctrico, electromagnético o piezoeléctrico. Cuando el girómetro se gira alrededor del eje central de la bifurcación, se generan dos fuerzas de Coriolis opuestas, que son transmitidas al bloque central. A su vez, los dientes inferiores experimentan una vibración que es perpendicular a la dirección del movimiento primario. Para detectar las vibraciones inducidas por las fuerzas de Coriolis se usan mecanismos de detección capacitivos, piezoresistivos o piezoeléctricos. Figura 2.7: Estructura de un girómetro MEMS de diapasón. 19

36 CAPÍTULO 2. SENSORES COMÚNMENTE USADOS EN SISTEMAS AHRS STRAPDOWN 2.4. MAGNETÓMETROS Girómetros MEMS de anillo vibratorio En la figura 2.8 se muestra un girómetro de anillo vibratorio. El principio de estos girómetros está basado en la existencia de dos modos fundamentales (más preciso, un modo par) [18]. Los modos están girados 45 uno con respecto al otro. Un modo primario de vibración es excitado y un modo secundario es inducido por la fuerza de Coriolis bajo la aplicación de una velocidad angular. Cuando el girómetro gira alrededor del eje z (normal al plano del anillo) se genera una fuerza de Coriolis que ocasiona que la energía del modo de primario sea transferida al modo secundario. Este cambio puede ser detectado por un gran número de maneras, tales como circuitos electrostáticos, electromagnéticos, piezoeléctricos, piezoresistivos, o capacitivos de la ejecución. Los girómetros de anillos vibratorio son muy sensibles y tienen muy poca sensibilidad a la temperatura. Figura 2.8: Estructura de un girómetro MEMS de anillo resonante Modelo de medición del girómetro La velocidad angular ω G es medida por tres girómetros montados ortogonalmente. La señal de salida del girómetro se ve influenciada por un ruido de baja frecuencia conocido como deriva, esto es: ω G = ω + ν + η G (2.5) ν = T 1 ν + η ν (2.6) donde η G y η ν R 3 se suponen como ruidos blancos y Gaussianos; y T = τi 3 es una matriz diagonal de constantes de tiempos Magnetómetros Los magnetómetros son sensores que generalmente miden el vector del campo magnético. La medición puede incluir la magnitud del vector del campo magnético, su dirección o ambos. 20

37 CAPÍTULO Clasificación SENSORES COMÚNMENTE USADOS EN SISTEMAS AHRS STRAPDOWN 2.4. MAGNETÓMETROS Los magnetómetros pueden ser clasificados de acuerdo a su rango de medición del campo magnético [20]. Los magnetómetros que detectan campos magnéticos menores a 1 µg se consideran sensores de bajo campo. Aquellos que tienen un rango de medición de 1 µg a 10 G son sensores de campo terrestre. Y los sensores que miden un campo magnético mayor a 10 G se conocen como sensores de deriva del campo magnético. En las siguientes secciones se describirán solo los sensores dentro de la categoría de campo magnético terrestre, debido a que en los sistemas de orientación el rango del campo magnético de interés es en el que se encuentra el campo magnético terrestre Magnetómetros magnetorresistivo Los magnetómetros magnetorresistivos son sensores que se basan en el cambio de la resistencia que experimenta un material cuando es expuesto a un campo magnético. Si bien, este efecto está presente en toda la materia, sólo alcanza valores apreciables en los materiales ferromagnéticos [19]. En los materiales conductores este fenómeno es muy débil y es conocido como magnetorresistencia ordinaria. Para los materiales magnéticos dicho fenómeno se llama magnetorresistencia anisotrópica. La magnetorresistencia anisotrópica consiste en el cambio de la resistencia del material dependiendo del ángulo entre la corriente que circula por dicho material y la dirección de magnetización. Los sensores magnetorresistivos pueden medir la magnitud y la dirección del campo magnético. Este tipo de sensor está hecho de una delgada película de una aleación de níquel e hierro (Permalloy). En presencia de un campo magnético el permalloy cambia su resistencia en un 2-3 %. Generalmente, estas resistencias se usan en un arreglo de puente de Wheatstone como se muestra en la figura 2.9; para formar un sensor magnetorresistivo de un eje de medición. Figura 2.9: Magnetómetro magnetorresistivo. Los sensores magnetorresistivos tienen un gran sensibilidad y un amplio rango de temperatura de operación. Su alta sensibilidad se presta para aplicaciones de medición del campo magnético terrestre, brújulas electrónicas y sistemas de navegación. 21

38 CAPÍTULO 2. SENSORES COMÚNMENTE USADOS EN SISTEMAS AHRS STRAPDOWN 2.4. MAGNETÓMETROS Magnetómetros magnetoinductivo Los magnetómetros magnetoinductivo son sensores relativamente de reciente creación, la primera patente de este tipo de sensor fue registrada en el año de Este sensor consiste en una bobina con un núcleo ferromagnético, que cambia de permutabilidad magnética dentro de un campo magnético. La bobina es el elemento de inductancia dentro de un oscilador R/L. La frecuencia del oscilador es proporcional al campo magnético que se está midiendo. Cuando el sensor se gira 90 con respecto al campo magnético, la variación en la frecuencia puede ser de hasta un 100 %. Dicha frecuencia puede medirse por medio de un microprocesador o un microcontrolador para determinar la magnitud del campo magnético medido. Estos magnetómetros son sensores tienen un sencillo diseño, son de bajo costo y de bajo consumo de energía Modelo de medición del magnetómetro El magnetómetro de tres ejes mide el vector del campo magnético B en el sistema de coordenadas móvil B. El vector de medidas formado por las salidas de los tres magnetómetros se escribe de la siguiente forma: bm = R T E B + η M (2.7) donde B y η M R 3. η M se supone como un ruido blanco y Gaussiano; que está presente en las medidas del magnetómetro. 22

39 Capítulo 3 Diseño e implementación del sistema AHRS En este capítulo se describe la integración entre los componentes del hardware y software que forman el sistema AHRS. En la figura 3.1 se muestra el diagrama de esta integración. Cada componente de la presente aplicación será descrito a lo largo del capítulo. De esta manera, primero se describe el Observador No Lineal que es el algoritmo de estimación de la orientación (las pruebas de estabilidad del ONL se muestran en el apéndice B, debido a que no se encuentran dentro del alcance de esta tesis). Después se describen los componentes electrónicos, de alimentación y de visualización de datos: los sensores inerciales, los sensores magnéticos, el controlador digital de señales, el módulo bluetooth, la batería LiPo y la estación de monitoreo. Y por último se describe la integración de todos los elementos del hardware y del software. Figura 3.1: Integración del hardware y software en el sistema AHRS. 23

40 CAPÍTULO 3. DISEÑO E IMPLEMENTACIÓN DEL SISTEMA AHRS 3.1. DESCRIPCIÓN DEL OBSERVADOR NO LINEAL (ONL) 3.1. Descripción del Observador No Lineal (ONL) En el desarrollo del sistema AHRS se fusionan las señales de tres diferentes sensores: girómetros, acelerómetros y magnetómetros. Estos sensores pueden clasificarse en dos categorías: 1. Sensores de velocidad angular: la velocidad angular ω G es medida por tres girómetros montados ortogonalmente. 2. Sensores de vectores de referencia: Consideremos la representación de un vector x i con respecto al marco de referencia inercial N y al marco de referencia en el cuerpo B denotado por r i y b i respectivamente. Los vectores r i son conocidos como vectores de referencia y se conocen perfectamente. Los vectores b i se conocen como vectores de observación y se obtienen de los sensores colocados en el vehículo (para nuestro caso magnetómetros, acelerómetros). A estos vectores podemos representarlos por medio de cuaterniones como se muestra a continuación: b iq = [0 bi T ] T r iq = [0 r i T ] T (3.1) Estos cuaterniones están relacionados por el cuaternión de rotación q de tal manera que b iq = q 1 r iq q (3.2) De (1.3) y usando algebra de cuaterniones podemos encontrar el siguiente modelo de medidas: [ 0 ( b ] i r i ) T b i r i [( b i + r i ) q = Hq = 0 (3.3) ] con H R 4 4. Note que (3.3) es un sistema lineal bien estructurado. La matriz de observación en función de los n pares de vectores (b i, r i ) con i (1, 2, 3,..., n) es escrita como: H = H 1. H n = 0 ( b 1 r 1 ) T b 1 r 1 [( b 1 + r 1 ) ]. 0 ( b n r n ) T b n r n [( b n + r n ) ] (3.4) Con H R 4n 4. En caso de que las mediciones fueran perfectas la ecuación de medidas Hq = 0 siempre se satisface. Sin embargo, en la práctica las mediciones son contaminadas con perturbaciones originadas por ruido, vibraciones, etc. Por lo tanto el problema consiste 24

41 CAPÍTULO 3. DISEÑO E IMPLEMENTACIÓN DEL SISTEMA AHRS 3.1. DESCRIPCIÓN DEL OBSERVADOR NO LINEAL (ONL) en estimar el cuaternión de la orientación a partir de la matriz de observación perturbada H, esto es: { 1 q ps = min q 2 Hq } 2 2 (3.5) sujeto a q ps 2 = 1 (3.6) Por otro lado, si el valor inicial de q(t 0 ) es conocido y si la medida de la velocidad angular fuera perfecta ω = ω G sería posible obtener q(t) integrando la ecuación (1.7). Sin embargo, por lo general q(t 0 ) se desconoce y las velocidades angulares son contaminadas por las derivas (ver ecuación (2.5)) lo que nos da como resultado divergencia lenta de la orientación conforme pasa el tiempo. Por lo tanto la idea de observación puede ser usada para corregir en línea la integración ˆq(t) de algunos erróneos ˆq(t 0 ) y errores de orientación causados por la deriva presente en las mediciones de los girómetros. Esta corrección puede llevarse a cabo de acuerdo al error medible q e = ˆq 1 q ps, donde q ps es considerado como una medida de la orientación obtenida por medio de los vectores de observación. Con el fin de resolver esta problemática y fusionar todas las medidas de los sensores, se usó un Observador No Lineal 1, cuyo diagrama a bloques se presenta en la figura 3.2. Figura 3.2: Diagrama de bloques del Observador No Lineal ( multiplicación de cuaterniones, multiplicación de matrices). En el diagrama del Observador No Lineal la estimación de la orientación se realiza en dos partes: 1 En el Apéndice B se muestra el paper del Observador No Lineal para la estimación de la orientación 25

42 CAPÍTULO 3. DISEÑO E IMPLEMENTACIÓN DEL SISTEMA AHRS 3.1. DESCRIPCIÓN DEL OBSERVADOR NO LINEAL (ONL) 1. Estimación de la orientación por medio de vectores de observación. En la primera parte el par de vectores de referencia y de observación (b i, r i ) se usan para calcular la matriz de medidas H, una vez que ya se tiene esta matriz aplicamos una Descomposición en Valores Singulares (SVD) de H utilizando el Lema 1 para obtener el cuaternión de orientación q ps. Suposición 1: Hay al menos dos vectores linealmente independientes b i y b j con i j, tal que, rank( H) 3. Lema 1: Sea H R 4n 4 la matriz de observación formada por los n pares de vectores ( b i, r i ) con i (1, 2, 3,..., n). Entonces, hay una matriz ortonormal U = [U 1 U 2... U 4n ] R 4n 4n, una matriz ortonormal V = [V 1 V 2 V 3 V 4 ] R 4 4 y S R 4n 4 con los elementos σ k a lo largo de la diagonal y ceros en todos los demás lugares, donde k = 1 : 4, tal que H = USV T (3.7) entonces, el cuaternión de la orientación está dado por la última columna de la matriz V, esto es q ps = V 4 (3.8) Además, la condición de normalidad q ps = 1 se satisface. Consultar [31] para ver la prueba. 2. Observador no lineal de la orientación con estimación del bias. En la segunda parte consiste en el observador no lineal el cual produce una estimación del bias que varía en el tiempo y una estimación del cuaternión de orientación. El Observador No Lineal propuesto para la estimación de la orientación está dado por: O A = { ˆq = 1 2 Ξ(ˆq) [ ω G ˆ ν + sign(q e0 )K 1 q e ] ˆ ν = T 1ˆ ν sign(q e0 )K 2 q e (3.9) donde T R 3 3 fue definido en (2.6) y K 1, K 2 son constantes positivas. ˆq es la predicción de la orientación en el tiempo t. Este se obtiene a través de la integración de la ecuación (3.9), usando la velocidad angular medida ω G, el bias estimado ˆν y q e el cual es el vector del cuaternión de error q e que mide la diferencia entre ˆq y la orientación medida q ps obtenido por medio de la SVD de la matriz de medición H, esto es: 26

43 CAPÍTULO 3. DISEÑO E IMPLEMENTACIÓN DEL SISTEMA AHRS 3.2. UNIDAD DE MEDICIONES INERCIALES (IMU) q e = ˆq 1 q ps = [q e0 q e T ] T (3.10) Ver el apéndice B para el análisis de estabilidad del ONL Unidad de Mediciones Inerciales (IMU) Para realizar las mediciones de la aceleración lineal y la velocidad angular en el sistema AHRS se utilizó una IMU. La unidad de mediciones inerciales empleada para este propósito es la IMU Razor mostrada en la figura 3.3, la cual hace uso de dos girómetros LPR530AL(ejes de alabeo y cabeceo) y LY530ALH (eje de guiñada) y un acelerómetro de 3 ejes ADXL335 ; las principales características de estos sensores se muestran en las tablas 3.1 y 3.2 respectivamente. Se escogió la IMU Razor en la presente aplicación debido a que los acelerómetros y girómetros son de tecnología MEMS y están montados en un mismo PCB, formando así un sistema de 6 grados de libertad. Además, que los sensores tienen un amplio rango de medición, los girómetros tienen una escala de medición de 300 /s, mientras que las salidas de los acelerómetros tienen un rango de medición de ± 3G. Los sensores inerciales dentro de la IMU Razor presentan un bajo consumo de energía y la salida de sus mediciones son señales analógicas. Figura 3.3: IMU Razor. Tabla 3.1: Especificaciones técnicas de los girómetros LPR530AL y LY530ALH (Ver [25] y [26] para más detalles). Características LPR530AL LY530ALH Salida Analógica Analógica Ejes sensibles 2 (Pitch & Roll) 1 (Yaw) Rango de medición ± 300 /s ± 300 /s Sensitividad 3.3 mv/ /s 3.3 mv/ /s Consumo de energía 6.8 ma 5.5 ma Voltaje de alimentación 2.7 V V 2.7 V V 27

44 CAPÍTULO 3. DISEÑO E IMPLEMENTACIÓN DEL SISTEMA AHRS 3.3. MAGNETÓMETROS Tabla 3.2: Especificaciones técnicas del acelerómetro ADXL335 (Ver [24] para más detalles). Características Salida Analógica Ejes sensibles 3 Rango de medición ± 3.0 G Sensitividad 300 mv/g Consumo de energía 350 µa Voltaje de alimentación 1.8 V V 3.3. Magnetómetros En la presente aplicación para medir la intensidad del campo magnético terrestre se usó un sensor magnético-inductivo digital, el MicroMag3 que se muestra en la figura 3.4 y sus principales características se visualizan en la tabla 3.3. Las ventajas del MicroMag3 incluyen un voltaje de operación de 3 volts para la compatibilidad con los nuevos sistemas de bajo consumo de energía, una gran inmunidad ante señales de ruido en todas las condiciones, y un amplio rango de medición de ± 1100 µt. Además, al ser un sensor digital se puede acceder fácilmente a las datos de las mediciones por medio de una interfaz serial SPI, también se pueden configurar la resolución y el rango de medición del campo magnético por medio de software para ampliar la variedad de aplicaciones. La medición es muy estable sobre la temperatura y inherentemente libre del error de la deriva. Figura 3.4: Micromag3. Tabla 3.3: Especificaciones técnicas del MicroMag3 (Ver [27] para más detalles). Características Salida Digital Protocolo de comunicación SPI Ejes sensibles 3 Rango de medición ± 1100 µt Consumo de energía 500 µa Voltaje de alimentación 3.0 V V 28

45 CAPÍTULO 3. DISEÑO E IMPLEMENTACIÓN DEL SISTEMA AHRS 3.4. CONTROLADOR DIGITAL DE SEÑALES 3.4. Controlador Digital de Señales Las aplicaciones modernas mezclan las funciones típicas de un microcontrolador (MCU ) con las de un procesador digital de señales (DSP). Esta situación ha impulsado a compañías fabricantes de semiconductores a crear un circuito integrado híbrido MCU/DSP, dando origen al llamado Controlador Digital de Señales, DSC por sus siglas en inglés (Digital Signal Controller). Un DSC integra a la perfección los atributos de control de un microcontrolador con las capacidades de cómputo y el rendimiento de un procesador digital de señales en un solo núcleo. Los DSCs están diseñados para satisfacer las rigurosas demandas de sistemas de tiempo real. No sólo es su rendimiento en tiempo real superior a otros microcontroladores de 16 y 32 bits [23], sino que también ofrecen una serie de características que permiten mejorar la fiabilidad y robustez del sistema, y reducir el costo del sistema mediante la eliminación de componentes externos. Para el desarrollo del sistema AHRS se usó un controlador digital de señales de la compañía Microchip, el dspic33fj128mc que se muestra en la figura 3.5, cuyas características principales se muestran en la tabla 3.4. La programación del dspic se realizó en el ambiente de lenguaje C, utilizando el compilador C30 proporcionado por el fabricante. Las características más destacadas de los dspic se puede mencionar que: son DSCs de 16 bits, presentan una arquitectura harvard modificada la cuál separa el bus de datos del bus de instrucciones y pueden operar a una velocidad de hasta 60 MIPS. Figura 3.5: dspic33fj128mc802. Tabla 3.4: Especificaciones técnicas del dspic33fj128mc802 (Ver [22] para más detalles). Características Arquitectura 16 bits Velocidad del CPU 40 MIPS RAM 16, 384 Bytes Memoria del Programa 128 KB Periféricos de comunicación digital 2-UART, 2-SPI, 1-I2C CAN 1-ECAN Periféricos analógicos 1-A/D 6 12-bits Voltaje de alimentación 3.0V - 3.6V Rango de temperatura -40 C a +150 C Temporizadores 5 16-bits, 2 32-bits 29

46 CAPÍTULO 3. DISEÑO E IMPLEMENTACIÓN DEL SISTEMA AHRS 3.5. MÓDULO BLUETOOTH 3.5. Módulo bluetooth El envió de los datos provenientes del sistema AHRS a la estación terrestre se hace a través de una conexión inalámbrica por medio de un módulo bluetooth. El módulo que se usó para hacer esta conexión es el Bluetooth Mate Gold mostrado en la figura 3.6, con características dadas en la tabla 3.5. Este módulo presenta un bajo consumo de potencia y soporta diversos protocolos de comunicación como: UART y USB. Este módulo bluetooth es muy fácil de usar, ya que todas las capas del protocolo bluetooth son transparentes para el usuario, quedando a éste las señales de comunicación serial T x y Rx. Además, se puede usar este módulo como maestro o esclavo. En la presente aplicación se empleó a tal módulo bluetooth en configuración esclavo, ya que es la estación terrestre quién solicita a éste establecer un enlace de comunicación. Figura 3.6: Módulo Bluetooth. Tabla 3.5: Características del módulo Bluetooth Mate Gold (Ver [28] y [29] para más detalles). Características Potencia Voltaje de alimentación Interfaces de comunicación Velocidades de transferencia Alcance Dimensiones 30mA conectado y < 10mA en modo de espera 3.0 V a 3.6 V UART (SPP ó HCI) y USB (solo HCI) bps 100 metros 45 16,6 3,9 mm 3.6. Batería de Polímero de Litio Las baterías de polímero de litio, también denominadas baterías LiPo son más livianas, almacenan más energía y retienen la carga por más tiempo que las baterías de níquel de un tamaño similar, lo que da lugar a una mayor autonomía de batería en un diseño más ligero. Además, las baterías LiPo pueden cargarse en cualquier momento sin tener que esperar a que descarguen por completo y sin que se reduzca su vida útil, debido a que no sufren de efecto de memoria de carga. Las baterías LiPo pueden entregar entre 3.7 y 22.2 volts (dependiendo del número de celdas que esté compuesta la batería) con una corriente de descarga de dos a treinta veces 30

47 CAPÍTULO 3. DISEÑO E IMPLEMENTACIÓN DEL SISTEMA AHRS 3.7. ESTACIÓN DE MONITOREO la capacidad de sus celdas (dadas la tasa de descarga en amperes y la capacidad nominal en amperio/hora), cualidades que les permiten entregar altas dosis de potencia en regímenes de gran consumo. La corriente de descarga suministrada por una batería LiPo puede ser determinada empleando la relación dada por la ecuación (3.11) I = M C n (3.11) donde I es la corriente descargada en (A), M es un múltiplo ó fracción de C, C es un valor numérico de la capacidad nominal de la batería en (Ah) y n es el tiempo en horas al cual C está declarado (ver [33] para más detalles en administración de energía). El peso y tamaño de las baterías de polímero de litio, las hace muy útiles para equipos pequeños que requieran potencia y duración; debido a estas cualidades se optó por usar este tipo de baterías en la presente aplicación. En la figura 3.7 se muestra la batería LiPo de una celda utilizada para energizar al sistema AHRS y en la Tabla 3.6 sus respectivas características. Figura 3.7: Batería LiPo. Tabla 3.6: Características de la batería LiPo. Características Capacidad nominal (C) 1000 mah Tasa de descarga 2C Dimensiones ,7 mm 3.7. Estación de monitoreo En la figura 3.8 se muestra la estación de monitoreo, la cual es una Mini-Notebook SONY VAIO VPC-YB15AL con módem bluetooth. En ella se corre una interfaz gráfica desarrollada en LabVIEW versión 2009, cuya función es visualizar la información de orientación del sistema AHRS (cuaternión de orientación estimado, ángulos de Euler, vectores de velocidad angular, de aceleración y de campo magnético) y enviar un comando de habilitación del envío de datos al sistema AHRS. En la figura 3.9 se muestra el diagrama a bloques de 31

48 CAPÍTULO 3. DISEÑO E IMPLEMENTACIÓN DEL SISTEMA AHRS 3.8. INTEGRACIÓN la estación de monitoreo y en la tabla se muestran los requisitos mínimos necesarios para correr LabVIEW Figura 3.8: Estación de monitoreo. Figura 3.9: Diagrama a bloques de la estación de monitoreo. Tabla 3.7: Características necesarias de la computadora que conforma la estación de monitoreo (Ver [30] para más detalles). Características Procesador Memoria RAM Espacio en disco Sistema operativo Resolución de pantalla Conectividad Pentium 4/M ó su equivalente 256 MB (mínimo) - 1 GB (recomendable) 1.6 GB Windows Vista/XP/ pixeles Bluetooth (dada la aplicación del cuadrirotor) 3.8. Integración En esta sección se describe la integración entre los componentes de software y hardware, es decir, la integración entre los distintos dispositivos utilizados en la implementación del sistema AHRS. El sistema AHRS está formado por la batería LiPo, el cargador de la batería, 32

49 CAPÍTULO 3. DISEÑO E IMPLEMENTACIÓN DEL SISTEMA AHRS 3.8. INTEGRACIÓN acelerómetros (ADXL335), girómetros (LPR530AL y LY530ALH), magnetómetros (Micro- Mag3), el módulo bluetooth y el controlador digital de señales (dspic33fj128mc802). En la figura 3.10 se muestra dicha integración, así como la dirección del flujo de información y energía entre estos. Figura 3.10: Diagrama de bloques del sistema AHRS. En el sistema AHRS el dspic DSC se encarga de recibir la información de los sensores. Una vez cuando los datos de los sensores son adquiridos, se obtiene el cuaternión de orientación estimado mediante el computo del Observador No Lineal. Para visualizar como se encuentra orientado el sistema AHRS con respecto al sistema NED, la estación de monitoreo manda un comando de habilitación de envío de información. Este comando se transmite de forma inalámbrica utilizando un modem bluetooth. Cuando el comando es recibido por el sistema AHRS se transmite el cuaternión de la orientación estimado y los vectores medidos por los sensores a la estación de monitoreo, empleando el protocolo de comunicaciones RS-232 que es el protocolo que maneja el módulo bluetooth. En la figura 3.11 se muestra el diagrama de flujo de los procesos mencionados anteriormente que realiza el dspic33fj128mc

50 CAPÍTULO 3. DISEÑO E IMPLEMENTACIÓN DEL SISTEMA AHRS 3.8. INTEGRACIÓN Figura 3.11: Diagrama de flujo del programa del dspic33fj128mc Adquisición de los datos de los sensores En la presente aplicación se emplean dos tipos de sensores: sensores analógicos (acelerómetros y girómetros) y sensores digitales (magnetómetros). Debido a la distinta naturaleza de las señales de los sensores se emplean diferentes módulos periféricos del ds- PIC33FJ128MC802 para la obtención de los datos provenientes de los sensores. 34

51 CAPÍTULO 3. DISEÑO E IMPLEMENTACIÓN DEL SISTEMA AHRS 3.8. INTEGRACIÓN Para los girómetros(lpr530al, LY530ALH) y el acelerómetros (ADXL335) se usa el módulo del convertidor analógico-digital (ADC). Este módulo puede ser configurado por la aplicación del usuario para que funcione como un ADC de 10-bits o como un ADC de 12-bits. En el sistema AHRS se optó por trabajar el módulo ADC en la configuración de 12-bits, lo que nos permite operar con una resolución 2 de 805 µv. Para la obtención de la velocidad angular y la aceleración del sistema AHRS se ocupan 6 canales analógicos (3 canales para el acelerómetro y 3 canales para girómetro) del dspic. Las señales de estos sensores son muestreadas y digitalizadas cada 18 ms. En el caso del magnetómetro (MicroMag3) es un sensor digital se necesita una interfaz de comunicación serial para la obtención de la intensidad del campo magnético. Esta interfaz se realiza en base al protocolo SPI (Serial Peripheral Interface), que es el módulo de comunicación que trae este sensor. La conexión entre 2 dispositivos mediante el uso del protocolo SPI está basada en una relación maestro-esclavo, como se logra apreciar en la figura Para realizar esta conexión se requiere un bus de datos de 4 líneas: señal de reloj (SCLK), datos de entrada (MISO), datos de salida (MOSI) y habilitación de dispositivo (SS). Para obtener la intensidad del campo magnético el dspic le manda un byte al MicroMag3 indicándole el eje de medición, después el sensor manda la medición del campo magnético digitalizada correspondiente a dicho eje en 2 bytes. Figura 3.12: Diagrama de conexión maestro-esclavo usando SPI. Una vez que se obtienen los datos de de los acelerómetros, magnetómetros y girómetros son guardados dentro del dspic en arreglos del tipo flotantes. De esta manera las medidas de los sensores son almacenados de la siguiente forma: ba = [a x a y a z ] bm = [m x m y m z ] ω G = [ω x ω y ω z ] (3.12) donde b a, b m y ω G son arreglos de 3 elementos que nos representan las mediciones del vector de aceleración, del vector de campo magnético y del vector de velocidad angular respectivamente. 2 La resolución se entiende como el voltaje necesario (señal analógica) para lograr que en la salida (señal digital) haya un cambio del bit menos significativo(lsb). 35

52 CAPÍTULO 3. DISEÑO E IMPLEMENTACIÓN DEL SISTEMA AHRS 3.8. INTEGRACIÓN Computo del cuaternión de orientación Una vez cuando se obtienen todos los datos de los sensores guardados en sus respectivas variables. Lo primero que se hace es obtener la matriz de observación H utilizando los vectores de referencia referencia (r a y r m ) y los vectores de observación (b a y b m ) provenientes de las mediciones de los acelerómetros y magnetómetros, quedando de la siguiente forma: H = H a H m = 0 ( b a r a ) T ba r a [( b a + r a ) ] 0 ( b m r m ) T bm r m [( b m + r m ) ] (3.13) Después encontramos las matrices U, S y V aplicando la descomposición en valores singulares a la matriz H para obtener el cuaternión de orientación q ps que es la cuarta columna de la matriz V. Después encontramos el cuaternión estimado ˆq y el bias estimado ˆv integrando las ecuaciones del ONL mediante el método de Runge-Kutta 4 [17]: O A = { ˆq = 1 2 Ξ(ˆq) [ ω G ˆ ν + sign(q e0 )K 1 q e ] ˆ ν = T 1ˆ ν sign(q e0 )K 2 q e (3.14) Y por último se calcula el cuaternión de error q e multiplicando el conjugado del cuaternión estimado ˆq 1 con el cuaternión de orientación q ps. Todas las cantidades de las ecuaciones de arriba son definidas en la Sección Comunicación con la estación de monitoreo Debido a que no siempre se pueda tener contacto visual del sistema AHRS, necesitamos poder monitorearlo de forma remota; para esto necesitamos enviar la información de como se encuentra orientado el sistema AHRS. La tarea encargada de enviar la información de la orientación toma el cuaternión de orientación estimado, el vector de velocidad angular, el vector de aceleración y el vector de campo magnético y los envía por medio del protocolo RS-232 al módulo bluetooth, el cual se encarga de transmitirlos a la estación de monitoreo. La información transmitida concerniente a cada elemento del cuaternión estimado y a cada vector, está representada en un entero con signo de 16 bits conformado por los bytes MSB y LSB. Por lo tanto se debe concatenar dicha información y multiplicarla por un determinado factor para dimensionarla a las unidades requeridas, tal proceso se describe en la figura El resultado de lo mencionado anteriormente es una cadena de 26 bytes que contiene la información de orientación del sistema AHRS, cuya estructura se muestra en la Tabla 3.8. La información de orientación se envía solamente cuando se ha establecido una conexión bluetooth y se ha recibido el comando de habilitación de envío de datos. 36

53 CAPÍTULO 3. DISEÑO E IMPLEMENTACIÓN DEL SISTEMA AHRS 3.8. INTEGRACIÓN Figura 3.13: Diagrama a bloques del envío de la información de orientación. Tabla 3.8: Información de orientación entregada por sistema AHRS Número de Byte Información representada 1 CuatESt q 0 MSB 2 CuatESt q 0 LSB 3 CuatESt q 1 MSB 4 CuatESt q 1 LSB 5 CuatESt q 2 MSB 6 CuatESt q 2 LSB 7 CuatESt q 3 MSB 8 CuatESt q 3 LSB 9 VelAng X MSB 10 VelAng X LSB 11 VelAng Y MSB 12 VelAng Y LSB 13 VelAng Z MSB 14 VelAng Z LSB 15 Acel X MSB 16 Acel X LSB 17 Acel Y MSB 18 Acel Y LSB 19 Acel Z MSB 20 Acel Z LSB 21 CampMag X MSB 22 CampMag X LSB 23 CampMag Y MSB 24 CampMag Y LSB 25 CampMag Z MSB 26 CampMag Z LSB Interfaz gráfica de la estación de monitoreo Cuando la información de orientación (cuaternión de orientación estimado y los vectores de velocidad angular, de campo magnético y de aceleración) llega a la estación de monitoreo 37

54 CAPÍTULO 3. DISEÑO E IMPLEMENTACIÓN DEL SISTEMA AHRS 3.8. INTEGRACIÓN es desconcatenada y dimensionada a las unidades requeridas. Después de haber realizado esto el cuaternión de la orientación es convertido a ángulos de Euler. Posteriormente la información de la orientación incluyendo los ángulos de Euler calculados son mostrados en los indicadores gráficos correspondientes y además son almacenados en un archivo con extensión lvm en caso de que el usuario final los necesitara para desarrollar otra tarea. Y por último la interfaz en LabVIEW manda un byte de habilitación o deshabilitación de la comunicación entre la estación terrestre y el sistema AHRS. En la figura 3.14 se muestra la interfaz visual de la estación terrestre y en la figura 3.15 se muestra el diagrama a bloques del proceso antes mencionado. Figura 3.14: Panel frontal de la interfaz gráfica de LabVIEW. Figura 3.15: Diagrama a bloques de la recepción de la interfaz gráfica de LabVIEW. 38

55 CAPI TULO 3. DISEN O E IMPLEMENTACIO N DEL SISTEMA AHRS 3.8. INTEGRACIO N Finalmente, en la figura 3.16 se muestran los PCBs y su respectivo ensamble de la integracio n de todos los componentes anteriormente descritos, dando como resultado el sistema AHRS. (a) PCBs de los sensores y del mo dulo bluetooth. (b) PCBs del dspic DSC. Figura 3.16: Sistema AHRS 39

56

57 Capítulo 4 Resultados Experimentales En el presente capítulo se muestran los resultados obtenidos experimentalmente de las pruebas a las que fue sometido el sistema AHRS. Las pruebas realizadas fueron encaminadas a comprobar la calidad del desempeño del sistema AHRS. Las pruebas que se realizaron al sistema AHRS son las siguientes: toma de tiempo de ejecución del Observador No Lineal, comparación de los resultados obtenidos del controlador digital de señales dspic33fj120mc802 con los resultados provenientes de Matlab, convergencia del cuaternión de orientación estimado y bias estimado desde condiciones iniciales fijadas de manera arbitraria y comparación de la exactitud del sistema AHRS con respecto a un sistema académico interactivo (Twin Rotor) con encoders incrementales. Los parámetros utilizados en el ONL son: K 1 = 6,9, K 2 = 0,9. Para asegurar la estabilidad asintótica, los parámetros deben de cumplir las siguientes condiciones K 1 > 0 y 0 < K 2 < 1; como es el caso del control de sistemas no lineales, no existe una metodología definida para proponer sus valores. Estos fueron determinados al observar el desempeño del sistema en tiempo real. En la figura 4.1 se muestra el procedimiento seguido para hacer las pruebas al sistemas AHRS. Figura 4.1: Procedimiento de las pruebas 4.1. Toma de tiempo de la ejecución del Observador No Lineal En esta primera prueba mostrada en la figura 4.2, se obtuvo el tiempo de ejecución de cada tarea que forma parte del programa del dspic33fj128mc802. Para medir el tiempo que dura cada tarea al programa principal se le insertaron banderas que eran activadas al 41

58 CAPÍTULO 4. RESULTADOS EXPERIMENTALES 4.1. TOMA DE TIEMPO DE LA EJECUCIÓN DEL OBSERVADOR NO LINEAL momento que comenzaba dicha tarea y se desactivaban al finalizar ésta. Estas banderas fueron medidas con la ayuda de un osciloscopio. El objetivo de esta prueba fue medir el tiempo que tardaba en realizarse todo el programa incluyendo el envío del cuaternión de orientación estimado y los vectores de aceleración, velocidad angular y campo magnético a la estación terrestre para así poder fijar el periodo de muestreo y la frecuencia de la salida de datos del sistema AHRS. El periodo de muestreo se fijó en 18 ms, ya que es un tiempo mayor al computo del computo del ONL junto con el envío de datos para garantizar que el sistema AHRS siempre se ejecute. Con un periodo de muestreo de 18 ms, el sistema AHRS presenta una frecuencia de salida de datos de 55 Hz. (a) Tiempo de adquisición de datos de los sensores (b) Tiempo del computo del cuaternión proveniente de la SVD (c) Tiempo del computo del cuaternión proveniente del ONL (d) Tiempo de ejecución del programa completo sin envío de datos Para todas las pruebas a excepción de ésta el sistema AHRS trabajó a una frecuencia de 10 Hz. Esto se hizo por que la interfaz gráfica en LabVIEW no mandaba el comando de activación cada 18 ms, debido principalmente a que la actualización de los indicadores gráficos y el almacenamiento de los datos toman más tiempo. Por consecuencia el sistema AHRS no enviaba la información de la orientación en cada periodo de muestreo y se perdían campañas completas de datos. 42

59 4.2. CAPÍTULO 4. RESULTADOS EXPERIMENTALES COMPARACIÓN DE LOS DATOS PROVENIENTES DEL DSPIC Y MATLAB (e) Tiempo de ejecución del programa completo con envío de datos (f) Periodo de muestreo Figura 4.2: Tiempo de ejecución del sistema AHRS 4.2. Comparación de los datos provenientes del dspic y MATLAB En una segunda prueba se compararon el cuaternión de la orientación estimado ˆq y el cuaternión de la orientación de la SVD q ps, provenientes del computo del Observador No Lineal dentro del dspic33fj128mc802 contra sus cuaterniones análogos calculados en Matlab. En la figura 4.3 se muestran las gráficas comparativas de los cuaterniones enviados por el dspic a la estación de monitoreo. Figura 4.3: Comparación entre los cuaterniones q ps y ˆq provenientes del dspic En la figura 4.4 se muestran las gráficas comparativas de los cuaterniones q ps de la descomposición en valores singulares (SVD) de la matriz H provenientes del dspic y de Matlab. En esta figura se aprecia que los cuaterniones son casi idénticos. 43