Diseo de un Sistema de Navegacin Inercial para Seguimiento y Control de Vehculos en Movimiento

Hubert Chang Escalante, Jeenert Espinoza Valenzuela Profesor Asesor: Ingeniero Fermn Rafael Cabezas Soldevilla Asignatura: Control Temporal Escuela Profesional de Ingeniera Telecomunicaciones Facultad de Ingeniera Elctrica y Electrnica Universidad Nacional de Ingeniera

Resumen
Los vehculos controlados tienen muchas aplicaciones, en la industria, en el bienestar social y en la defensa nacional como en la supervisin de lneas areas de transmisin elctrica de alta tensin, de gaseoductos y oleoductos, en la seguridad pblica de mega eventos, en la observacin de reas geogrficas normalmente inaccesibles para LIT, etc. Este trabajo presenta el diseo de un Sistema de Navegacin Inercial (INS) que es un proceso muy importante para el seguimiento, control y guiado de estos vehculos y es el que proporciona la posicin, actitud y orientacin del vehculo mvil.

iniciativa es de disminuir el error de orientacin y dar una medida de desplazamiento en un mismo mdulo, as como el desarrollo de la IMU para nuestras condiciones geogrficas y climticas. Desde el 2008 hasta la actualidad se han desarrollado con asesora argentina unas avionetas, prototipos de UAVs utilizando tecnologa nacional. Adicionalmente a lo expuesto con anterioridad el proyecto buscar dar pasos para desarrollar un autopiloto con la mayora de sus componentes construidos y lo ms importante desarrollados en el Per. En el presente trabajo vamos a describir como se disea el sistema de navegacin inercial y como se acondicionan las seales de aceleracin, velocidad angular y campo magntico mediante el algoritmo de Rugen Kutta y Filtro de Kalman Extendido para obtener una buena actitud, orientacin, velocidad y posicin del vehculo mvil.

INTRODUCCIN
Para enviar un vehculo mvil, en especial un vehculo areo no tripulado (UAV) hacia un objetivo determinado es necesario, entre otros, dos importantes sistemas : Un sistema de navegacin inercial que permita la necesaria capacidad de comando sobre el vehculo y Un sistema de autopiloto que permita al vehculo poseer una adecuada estabilidad, tanto para lograr una correcta navegacin como para permitir el buen desempeo de determinado tipo de cargas tiles. El Sistema de Navegacin Inercial es importante para el desarrollo de tecnologas propias y generar incentivo para el desarrollo de prototipos UAV con tecnologa nacional. Buscar mejoras en la respuesta de la IMU respecto a las que comercialmente se encuentran en el mercado de acuerdo a varias utilidades a destinar. En vez de solo adaptar estas seales, en lo que se basa esta

PRESENTACIN DEL PROBLEMA

La necesidad de crear una cultura de tecnologa propia ya que el alquiler de satlites, comprar bienes sin transferencia tecnolgica o en nuestro caso hacer un despliegue humado para zonas de desastre, tomar fotos de cardmenes, cultivos o alquilar Drones para temas de especficos es de altsimo costo y tiempo asumido momentneamente por no tener una cultura de crear tecnologa aplicada. Fig 1

Las diversas aplicaciones entre otras, en la industria, en el bienestar social y en la defensa nacional como en la supervisin de lneas areas de transmisin elctrica de alta tensin, de gaseoductos y oleoductos, en la seguridad pblica de mega eventos, en la observacin de reas geogrficas normalmente inaccesibles, etc. Fig. 2

estabilidad alrededor de los tres ejes de movimiento del vuelo se lo conoce como "autopiloto". Lograda la estabilidad en los tres ejes, se requiere ahora lograrla en otros aspectos del vuelo, altitud, direccin de avance y velocidad. Luego un lazo un poco ms complejo permite las correcciones de desvo por viento.

OBJETIVOS
Diseo de un Sistema de Navegacin Inercial (INS) para Seguimiento y Control de Vehculos en Movimiento. Utilizacin de sensores inerciales y magnetmetro comerciales para lograr una Unidad de Medicin Inercial (IMU) mejor a las comercialmente existentes. Resolucin haciendo un tratamiento matemtico mediante el Algoritmo recursivo de Filtro Extendido de Kalman (FKE) para la deteccin de fallas aditivas bajo influencia de ruido gaussiano blanco. . La posicin (INS/GPS) se enva al mdulo de clculo y control de trayectorias que establece donde est el vehculo, donde tiene que ir y de acuerdo a esto le indica al mdulo de autopiloto cuanto tiene que virar, subir, bajar, etc. El primer problema que enfrentan los diseadores es lograr posicionar geogrficamente a la plataforma del UAV, con la mayor precisin en la trayectoria requerida. Para ello se requiere disponer de un sistema de navegacin, y por lo tanto de la necesaria capacidad de comando sobre el vehculo, adems se requiere que el vehculo posea una adecuada estabilidad, para lograr una correcta navegacin. Los controles de vuelo de los UAV, sean por superficies mviles o por empuje, no slo deben proveer a su guiado, sino tambin a su estabilidad de vuelo. En otras palabras el UAV puede ser guiado dentro de su trayectoria deseada slo si posee un vuelo estable, es decir, no oscila o se bambolea alrededor del eje de su curso. Los componentes claves para lograr la estabilidad del vuelo, normalmente consisten en girscopos, que no slo sirven para estabilizar la plataforma del UAV, sino tambin para aviones, misiles, torpedos y submarinos. Otra importante consideracin es el "rgimen" al cual el mvil se desva de su curso, el que puede ser medido mediante la precesin de un girscopo de rgimen (rate gyro) para ser alimentado al sistema de control. Al conjunto de girscopos que proveen la Este instrumento opera en base a una propiedad giroscpica, concretamente la de rigidez en el espacio.

DESCRIPCIN DE LA SOLUCIN
Actitud y Orientacin El indicador de actitud, tambin llamado horizonte artificial, es un instrumento que muestra la actitud del avin respecto al horizonte. La incorporacin del horizonte artificial a los aviones es fundamental para permitir el vuelo en condiciones de visibilidad reducida o nula.

Fig. 4.- Componentes de la velocidad a leer.

Existen tres ejes de vuelo sobre los que se puede rotar con movimiento de cabeceo (pitch), de balanceo o inclinacin (roll) y de guiada (yaw) en forma nica o combinada. Como muestra la Fig. 4.

vertical; otro transductor conectado a un sistema adecuado para medir la velocidad permite, mediante un lazo a travs del acelerador, lograr una velocidad constante; y un comps facilita mantener el rumbo magntico deseado. En algunos casos un lazo un poco ms complejo permite las correcciones de desvo por viento, las que pueden ser efectuadas automticamente por el autopiloto o bajo comando del operador remoto, segn el grado de sofisticacin empleado. Es necesario evitar que debido a las maniobras entre en prdida, para ello se dispone de un sensor de ngulo de ataque acoplado al autopiloto, que acta como limitador de cabreado, de forma tal que cualquiera sea el comando que inadvertidamente o no genere el operador remoto, el modelo no exceder un ngulo de ataque tal que lo lleve a la prdida. De esta forma el VNT volar recto y a nivel a una altitud preseleccionada; como el autopiloto acta en todas las fases de la misin, desde el decolaje o lanzamiento hasta su recuperacin o aterrizaje, asegura la estabilidad de la plataforma y la facilidad de su control. Cuando sea necesario variar las condiciones del vuelo, ser este autopiloto el que actuar, sea en forma automtica en funcin de un programa ya establecido en la computadora de a bordo, sea obedeciendo a seales externas transmitidas por el operador remoto desde una estacin de control. De esta forma el autopiloto establecer determinados ngulos de ascenso o descenso y altitudes a mantener, grados de viraje y ngulo de inclinacin de los mismos, recurriendo para ello a los girscopos de referencia. Otro tanto har con la velocidad, ajustando la posicin del acelerador. El hecho de que las maniobras son ejecutadas con un rgimen de variacin controlado brinda la capacidad de volar fuera del alcance visual del operador remoto. Y esto es lo que tiene que realizar nuestra INS. La IMU es un dispositivo que est compuesto por sensores bsicos inerciales (acelermetros y giroscopios), adicionalmente un magnetmetro para entregar medidas de aceleracin, velocidad angular, campo. Componentes del INS: 6DOF DIGITAL: Acelermetro y Giroscopio LSM303DLM: Magnetmetro LS20031: GPS XBEE PRO S2B: Mdulo inalmbrico MBED: Microcontrolador

Fig. 5.- Sistema de estabilizacin de balanceo de la nave On-Off Los girscopos, colocados en funcin de estos ejes de vuelo, permiten disponer de seales, para mantener al VNT en un vuelo estable sobre una direccin de avance dada. Dos caractersticas bsicas de los girscopos los convierten en una herramienta muy importante para la estabilizacin del mvil y su guiado. Fig 5. Una de ellas es su rigidez axial en el espacio; la otra es su tendencia a la precesin cuando est sujeto a fuerzas externas. Cuando un rotor giroscpico adquiere velocidad y est alineado en una direccin dada en el espacio, puede ser usado como una referencia de eje de vuelo para cualquier mvil. Tomando las seales producidas por los balancines en los cuales el rotor est montado, amplificndolas y encaminndolas a travs de apropiados servos y actuadores, movern las superficies de control de vuelo para hacer que el mvil retome el curso deseado. Otra importante consideracin es el "rgimen" al cual el mvil se desva de su curso, el que puede ser medido mediante la precesin de un girscopo de rgimen (rate gyro) para ser alimentado al sistema de control. Al conjunto de girscopos que proveen la estabilidad alrededor de los tres ejes de movimiento del vuelo se lo conoce como "autopiloto". Lograda la estabilidad en los tres ejes, se requiere ahora lograrla en otros aspectos del vuelo, altitud, direccin de avance y velocidad. Una altitud constante se obtiene fcilmente al recurrir a un transductor conectado a un sensor baromtrico, normalmente complementado por un acelermetro

En la Fig 6 Se observa los componentes del INS

Fig. 6.- Componentes de INS. Sistema de medicin de actitud Determinar la actitud de la plataforma, implica calcular los ngulos de pitch, roll y yaw, para ello es necesario resolver la ecuacin mostrada en (1).

Y por lo tanto:

Donde p,q y r es el vector de velocidades angulares medidas por los giroscopios en el eje x y z respectivamente.

De esta forma se obtiene los ngulos de Euler a partir de las varias interacciones en un tiempo de muestreo determinado, pero como el RK calcula la actitud actual a partir de la anterior, los ngulos resultantes presentan errores acumulativos, por esta razn se ha decido utilizar un Filtro de Kalman con el objetivo de reducir el error de los ngulos resultantes. Filtro de Kalman (FK)

Es el vector de las derivadas de los angulos roll, pich y yaw respectivamente. Mtodo Rugen-Kutta (RK) La expresin mostrada en 1 es una ecuacin diferencial discreta, la cual debe resolverse para determinar los ngulos de Euler, para ello se a empleado el mtodo de Rugen-Kutta, el cual que est dado por la expresin (2)

El FK tiene como objetivo estimar los estados de un sistema dinmico lineal ruidoso, utilizando un algoritmo ptimo y recursivo. En este caso el sistema dinmico es no lineal por lo que se utilizara un Filtro de Kalman Extendido (FKE).

Fig. 7.- Algoritmo de FKE Los trminos xi y yi se ajustan a la expresin mostrada en 1 de la siguiente forma: Las funciones f(x, u,w) y h(x, v) pueden ser utilizadas directamente para realizar prediccin, pero para calcular la covarianza Pk a estas funciones es necesario obtener su jacobiano.

Los jacobianos utilizados para la implementacin del FKE son utilizados por la naturaleza no lineal de las funciones y estn dados por las siguientes expresiones:

Mx: Es la medida original del campo magntico sin

compensacin en el eje x. My: Es la medida original del campo magntico sin compensacin en el eje y. Mz: Es la medida original del campo magntico sin compensacin en el eje z. : ngulo de inclinacin Pitch y : ngulo de inclinacin roll.

Con las nuevas medidas obtenidas se calcula el heading de la siguiente forma:

La Correccin del FKE se basa en los ngulos roll y pitch calculados a partir del acelermetro (observacin), por esta razn solo estos dos ngulos son corregidos por el FKE, quedando la orientacin (ngulo yaw) para ser calculada mediante un magnetmetro. Sistema de medicin de orientacin Para obtener la orientacin de la plataforma se ha utilizado las medidas del campo magntico en el plano XY, estas medidas con una calibracin previa describen un comportamiento de un circulo unitario, al momento de rotar a la plataforma sobre un eje fijo, debido a este comportamiento la ecuacin que se emplea para calcular el ngulo de orientacin o Headinges [4]: Fig. 8.- Diagrama del proceso de orientacin Sistema de medicin de desplazamiento Dnde: Hed: ngulo de orientacin con respecto al norte magntico o Heading. My: Medida calibrada del campo magntico en el eje y. Mx: Medida calibrada del campo magntico en el eje x. La ecuacin (6) es aplicable solo cuando el movimiento de la plataforma se realiza en el plano horizontal, sin embargo ya que el sistema tiene que ser aplicable a un UAV, se requiere de una compensacin de las medidas del campo magntico, previo a la realizacin del clculo del ngulo orientacin. Las ecuaciones (7) y (8) se emplean para realizarla compensacin de las medidas del magnetmetro [4]: Para estimar la posicin de la plataforma mvil, se emplea la medida de los acelermetros, suprimiendo los efectos de la gravedad mediante las siguientes ecuaciones [6].

Donde las ecuaciones (12) y (13) reflejan el efecto de la rotacin y la gravedad.

Siendo: aIb La aceleracin considerando a la plataforma un cuerpo

idealmente inercial. ab Es la aceleracin medida por el acelermetro. Cantidad de rotacin del cuerpo debido a la rotacin de la tierra. Cantidad de rotacin del cuerpo debido al propio cuerpo. gb Vector gravedad del cuerpo. n C b Matriz de cambio de coordenadas de body a inercial.

Donde Mxc: Es la medida del campo magntico en el eje x compensado con los ngulos de inclinacin. Myc: Es la medida del campo magntico en el eje y compensado con los ngulos de inclinacin.

Dado que los giroscopios miden siempre y entonces se tiene:

Reemplazando las ecuaciones 12 y 13 en la ecuacin 14 se obtienen las aceleraciones, suprimido las rotaciones y la gravedad y se representa por la ecuaciones (15):

Dnde: aIbx, aIby, aIbz = Aceleracin sin los efectos de rotacin y de gravedad en el eje x, y y z respectivamente . abx, aby, abz = Aceleracin medida directamente en el eje x,
y y z respectivamente. vbx, vby, vbz = Velocidad del cuerpo en el eje x, y y z respectivamente. p, q, r = Velocidad angular del cuerpo en el eje x y z respectivamente. = ngulo pitch = ngulo roll g = Gravedad.

Fig. 9.- Diagrama del clculo de la posicin.

Una vez que se han corregido completamente las medidas de las aceleraciones, se procede a realizar la integracin de la seal, con lo cual se obtiene la velocidad, la misma que ser integrada nuevamente para obtener el desplazamiento de la plataforma. Fig. 9. En todos los procesos de filtrado se debe utilizar un filtro pasa altos, el filtro en la aceleracin se emplea para suprimir los efectos de las vibraciones de la plataforma mvil, de igual forma este mismo filtro se encarga de suprimir las aceleraciones estticas que se producen al inclinar el vehculo, esto debido a que para el clculo del desplazamiento solo se requiere la aceleracin dinmica

Adicionalmente se realiza una correccin de la seal de velocidad mediante el muestreo de la misma, ya que por efecto de los filtros se produjo un error en la integracin final, esto debido a que la velocidad cambia de signo de forma aleatoria, produciendo un dato errneo de desplazamiento.

Fig. 10.- Integracin de GPS para establecer referencias. Finalmente para obtener un punto de referencia inicial que indique la posicin se la plataforma se emplearia un GPS, esta medida es referencial ya que el desplazamiento obtenido a partir de sensores inerciales es sumado a este punto inicial

RESULTADOS
El acelermetro y el giroscopio entregan medidas con las que se puede calcular la actitud de un robot mvil, la diferencia es que en base a las medidas del giroscopio se puede calcular los ngulos roll, pitch y yaw, mientras que en base a las medias del acelermetro solo se puede calcular los ngulos roll y pitch. La decisin de elegir el sensor inercial para calcular la actitud, se basa en pruebas realizadas, donde

tambin se compara con el valor entregado por un FKE que fusiona los dos sensores inerciales. Al final el FKE entrega un valor satisfactorio. La ventaja ms destacable de utilizar un FKE es que corrige el error acumulable que se produce al utilizar un mtodo iterativo, ejemplo Rugen-Kutta, para calcular la actitud de un vehculo mvil.

pronto posibles doctorados en reas tan importantes como automatizacin e instrumentacin en las universidades de mayor prestigio del pas.

BIBLIOGRAFA
[1] Cabezas Soldevilla Fermn, El estimador lneal ptimo para sistemas de control estocsticos, Tesis de Bachiller, FIEE UNI, Lima-Per 1974 [2] Melsa James, Sage Andrew, Estimation theory with applications to communications and control, McGraw Hill U.S.A. 1971. [3] Cabezas Soldevilla Fermn, Cabezas Huerta Franklin, Diseo de autopiloto para vehculo areo no tripulado, Premio ejrcito peruano 2009, Lima-Per 2009 [4] Kirk Donald, Optimal Control Theory, Prentice Hall, New Jersey, U.S.A. 1970 [5] Cabezas Soldevilla Fermn, Simulador digital para diseo de sistemas de control ptimo, Tesis de Grado, FIEE UNI Lima-Per 1974 [6] Jorge David Guallchico Loachimin, Diseo de INS tipo strapdown aplicable a una UAV, Pre-Tesis, EPN Ecuador 2013.

CONCLUSIONES
Para conseguir el desarrollo tecnolgico necesario que nos permita tener la capacidad para crear, disear y construir equipos y sistemas con tecnologa moderna para la industria en general se recomienda lo siguiente: El gobierno tiene que apoyar decididamente a las pequeas y medianas empresas que produzcan productos tecnolgicos como herramientas, accesorios, equipos industriales, educativos, mdicos, comunicaciones, militares, etc, donde la garanta tecnolgica debera ser otorgada por el Concytec previa confirmacin que los productos tengan aporte tecnolgico importante. Se debe capacitar a los cientficos de las reas tecnolgicas al ms alto nivel, crendose lo ms

Esquema Operacional de los vehculos no tripulados

SENSORES Y ACTUADORES
IMU (Sensores Inerciales) 3 acelermetros Integracin de los sensores inerciales. Fusin de sensores por filtrado Kalman: Seguimiento de los errores de posicin, velocidad y actitud.

ALGORTMICA
Mdulo navegacin inercial (INS/GPS) Mdulo de clculo y control de trayectorias

Posicin (INS/GPS)
Variables de entrada

GPS Posicin y velocidad Altura baromtrica Sensor de presin absoluta Velocidad Anemomtrica (Air Speed) Velocidad del avin con

Posicin (INS/GPS) Waypoint

Actitud AHRS (INS/GPS)

Dinmica de vuelo

Virado Trepado-Picado

Variables de entrada

Siguiente WayPoint

Virado Actuadores - Empuje - Profundidad - Alerones Trepado-Picado

Control misin

Variables controladas (PIDs)

Lista de WayPoint determinacin de waypoint alcanzado

Para la implementacin fsica del esquema mostrado hay que tener en cuenta lo siguiente: a) Existe hoy en da una industria mundial de alta tecnologa y muy probada que provee las partes necesarias para la implementacin de los VNT a precios muy accesibles. Por lo que para un desarrollo inicial y para no sobrepasar presupuestos bsicos se pueden adquirir (si no los hubieran) la estructura del VNT que es fundamentalmente la de una nave a control remoto y el autopiloto con los sensores y actuadores. El tema caliente para la implementacin exitosa del esquema total mostrado de la operatividad de los VNT es la implementacin del mdulo: Clculo y Control de Trayectorias debido a que las diversas aplicaciones finales de los VNT se definen y programan en este mdulo y donde participan diversos agentes con decisiones distribuidas y con un alto nivel de autonoma. Para implementar este mdulo es necesario conocer cabalmente los componentes de todos los mdulos, para hacer el modelo matemtico del comportamiento de todo el sistema (aerodinmica del UAV y autopiloto), realizar las simulaciones de las aplicaciones iniciales y futuras con buena exactitud para proceder a la implementacin fsica del proyecto. La precisin y validez del modelo de simulacin est determinada en gran medida por la exactitud de los valores numricos utilizados para los parmetros fsicos del autopiloto y para la representacin de la inercia y la aerodinmica del VNT. Una vez que el modelo correcto se ha conseguido se puede utilizar para simular diferentes condiciones de vuelo y las maniobras para ajustar los parmetros del piloto automtico. En consecuencia el gran reto del ingeniero peruano es definir las aplicaciones reales, simularlas e implementarlas. El objetivo del presente trabajo es mostrar que esto es perfectamente viable y para esto se ha desarrollado el modelo matemtico de un VNT y se ha diseado las caractersticas de su piloto automtico, evidentemente este es un trabajo inicialmente terico por que no se dispone todava de la estructura de la nave y el autopiloto reales, pero justamente la parte de modelado y simulacin de todo el sistema con componentes reales es la base del xito de cualquier proyecto de operatividad de un VNT.

b)

c)

d)