Reporte Tcnico

Introduccin al diseo de un sistema de navegacin inercial

Juan Sebastin Cruz A.

Diciembre 2013

Introduccin al diseo de un sistema de Navegacin Inercial Los sistemas de navegacin inercial son sistemas que permiten aproximar la posicin, orientacin y velocidad de cualquier objeto en movimiento, haciendo uso de acelermetros, giroscopios y ms recientemente de magnetmetros. Las unidades de medida inerciales o IMUs por sus siglas en ingls (Inertial Measurement Units) contienen generalmente 3 acelermetros, 3 giroscopios y 3 magnetmetros, uno para cada eje ortogonal. IMU Giroscopio Los giroscopios son sistemas que nos permiten medir los valores de desplazamiento angular en el tiempo. Existen mltiples tipos de giroscopios, dentro de los que ms destacan los mecnicos, pticos y los MEMS. Debido al rpido avance de la electrnica, estos ltimos se han convertido en ideales para todo tipo de aplicaciones. Los MEMS utilizan el efecto de Coriollis para la medicin de la velocidad angular, como se muestra en la figura 1.

Figura 1. Efecto Coriolis en Giroscopio [2] Cuando la masa se est moviendo en la direccin de la y una el objeto se mueve con una velocidad angular , entonces la masa va a experimentar una fuerza en la direccin de la flecha naranja, como resultado de la fuerza de Coriollis. El resultado del desplazamiento fsico causado para esta fuerza es ledo por un sensor capacitivo. La mayora de los giroscopios MEMS tienen dos masas oscilantes , las cuales se mueven constantemente en direcciones opuestas (figura 2); la fuerza de Coriolis en cada masa tambin acta en direcciones opuestas dando como resultado un cambio en la capacitancia. Este valor es proporcional al cambio en la velocidad angular.

Figura 2.Giroscopio con dos masas [2] Por su naturaleza, los giroscopios electrnicos tienen a tener tanto un offset como un drift a travs del tiempo. Magnetmetros

Los magnetmetros son instrumentos que nos permiten medir la fuerza y direccin de un campo magntico. Los ms comunes son los magnetmetros o brujas digitales de estado slido, ya que permiten tamaos muy reducidos y bajos costos. Las tecnologas ms comunes miden los campos a travs de magnetoresistencia anisotrpica. Esto los hace muy sensibles aumentando su precisin y linealidad. La mayora de esto instrumentos periten medir la direccin del campo magntico terrestre con una exactitud inferior a los 2 grados.

Acelermetro Los acelermetros son sistemas que nos permiten medir la aceleracin esttica debida a la gravedad y la aceleracin dinmica producida por el movimiento. Al igual que en el caso de los giroscopios, existen varios tipo de acelermetros, pero los ms comunes son microacelermetros capacitivos los cuales sensan la aceleracin a travs del cambio en la capacitancia de un capacitor formado por su estructura. Uno de sus electrodos est colocado en la masa ssmica, mientras que el otro permanece estacionario. Una aceleracin provoca una variacin en la distancia entre ambos electrodos, alterando su capacitancia inicial en proporcin a la aceleracin experimentada. Entonces, un circuito electrnico puede medir esta variacin de capacitancia [9]. La figura 2.1 muestra la estructura de un microacelermetro capacitivo de sensado vertical, en el cual la masa ubicada en la parte superior y el electrodo en la inferior forman un capacitor de

placas paralelas. Las aceleraciones en el sentido del eje vertical genera una flexin en la masa, causando un cambio en la distancia de separacin entre el electrodo y la masa mvil; esto origina un aumento o disminucin en la capacitancia inicial en funcin de la direccin de la aceleracin aplicada.

Figura 2.1 Microacelerometro

Por la naturaleza fsica de los acelermetros, estos se ven influenciados por gran cantidad de ruido, por lo que es muy importante que su seal pase por un proceso de filtrado.

Sistema de navegacin Inercial

Un sistema de navegacin inercial requiere el uso de acelermetros, magnetmetros y giroscopios. Para esto fue usada una unidad de medida inercial comercial conocida como Razor IMU. La configuracin de la misma para los propsitos de esta investigacin se puede encontrar en el Anexo A. A continuacin se describen los algoritmos usados para la obtencin de la posicin y la orientacin.

Figura 3. Sistema de navegacin Inercial Orientacin El sistema de navegacin propuesto, se muestra en la figura 3. Existen varias alterativas para obtener la orientacin del objeto a travs de un sistema inercial. Las ms conocidas son los filtros de Kalman y Filtros Complentarios, los cuales usan los datos de los giroscopios para aproximar los ngulos de inclinacin, corrigiendo estas medidas con las de los acelermetros. Otra alternativa es a travs del uso de una matriz de cosenos directores (DCM por sus siglas en ingles). Esta ltima funciona de la siguiente manera: 1. La orientacin del objeto se obtiene a partir de la integracin de la informacin proveniente de los giroscopios.

2. Como se mencion en la primera parte, los datos de los giroscopios se ven influencias por el ruido (gyro drift y gyro offset). La integracin acumula estos errores en los elementos de la matriz de cosenos directores. Sabiendo que la matriz debe ser siempre ortogonal, se realizan pequeos ajustes para que las ortogonalidad se mantenga.

3. De igual forma, para corregir estos errores derivados de la integracin, se usa un controlador PI, el cual reduce el error de los ngulos obtenidos a travs de los giroscopios y los que se pueden obtener con los acelermetros y los magnetmetros.

El proceso se muestra de manera esquemtica en la figura 4, el cual es propuesto por Premerlani [3].

Figura 4. DCM

El Razor IMU hace uso de este algoritmo y nos entrega directamente los ngulos de Euler referentes a la orientacin en cada uno de los ejes (Pitch, Yaw y Roll). El procesamiento de los datos se realiz a travs de Matlab, por lo que las modificaciones a la programacin del IMU se pueden encontrar en el Anexo B.

Posicin Como se observa en el esquema de la figura 3, la posicin de obtiene a travs de una doble integracin de los valores de aceleracin dados por los acelermetros, junto con los datos de orientacin.

Los acelermetros, no solo miden la aceleracin dinmica, sino que tambin estn midiendo la aceleracin debida a la gravedad. Por esta razn debemos eliminar el valor de gravedad de nuestras mediciones. Para esto, lo que haremos ser transformar la gravedad medida en nuestro sistema inercial, a nuestro sistema de movimiento. Tendremos entonces que la matriz de rotacin estar definida como:

Donde ,

corresponde a los ngulos de rotacin en Z, Y y X respectivamente.

Definimos el vector gravedad como: g=[ ]

Por tanto la gravedad en nuestro cuerpo en movimiento estar definida como:

Con esto podemos compensamos el valor de la gravedad a los valores obtenidos directamente de la unidad inercial: at=

Como se puede observar, el valor de la gravedad en el sistema en movimiento no depende del ngulo de guiada o Yaw. Al hacer esto entonces, suponemos que despus de eliminar la gravedad, la direccin de los vectores de ax, ay y az estn alineados con nuestro eje inercial en todo momento. El anlisis de los datos se realiz a travs de Matlab. La programacin se puede encontrar en el Anexo C. Los valores de aceleracin del dispositivo en una posicin esttica se pueden observar en la figura 5.

Raw Accs Data at Inertial Frame (Static) 300

250

200
Raw Data Acc

150

AccX AccY AccZ

100

50

-50

20

40

60

80

100 120 Sample

140

160

180

200

Figura 5. Valores de Aceleracin esttico Como se puede observar en la calibracin del IMU, del anexo A, los valores de aceleracin varan entre -255 y 255, siendo este ltimo el valor mximo para la gravedad. Se caracterizaron cada uno de los ejes de aceleracin La posicin estara dada entonces por: X(t)= Y(t)= Z(t)=

Sin embargo como se puede observar en la figura 6 la seal de los acelermetros presenta bastante ruido, lo que hace que los errores en la posicin sean bastante amplios, debido a la doble integracin.

Raw AccX and AccY Data at Body Frame (Static) 3

1
Raw Data Acc

-1

-2

-3

-4 0

20

40

60

80

100 120 Sample

140

160

180

200

Figura 6.Ruido en los acelermetros. En 9 segundos, estando esttico el movimiento, el error en la posicin es cercano a los 0.35m, como se puede ver en la figura 7.

Figura 7. Posicin en X, con el objeto esttico.

Estando esttico, es posible implementar un filtro de medias mviles el cual pareciera ser una solucin viable para reducir el ruido. El filtro de medias mviles obtiene el promedio de los n datos siguientes al dato que se pretende predecir. Al estar el elemento esttico, los valores de la aceleracin en los 3 ejes deben ser 0. La figura 7 muestra el valor de la aceleracin sin filtrar y con el filtro de medias mviles.
AccX 2 1.5 1 0.5
Acc Raw

AccX Filtered AccX

0 -0.5 -1 -1.5 -2

0.5

1.5

2 time(s)

2.5

3.5

Figura 7. Implementacin de filtro de media mviles. El error en la posicin despus de 9 segundos es cercano a los 12 cm. Sin embargo, el sistema inercial debe reducir al mximo el error de posicin, no solo cuando est esttico, sino tambin cuando est en movimiento, por lo que el filtro de medias mviles no es funcional. Partiendo de esto, se aplic un filtro de Kalman a los valores del acelermetro.

Filtro de Kalman
El filtro de Kalman es una serie de ecuaciones matemticas que de manera recursiva permiten estimar el estado del proceso, de tal forma que se minimice la media del error cuadrtico. Este filtro permite hacer estimaciones de los estados pasados, presentes y futuros del proceso. El filtro de Kalman estima el proceso usando la siguiente retroalimentacin: Estima el proceso en un tiempo determinado y luego obtiene la retroalimentacin de las medidas (ruido).Las ecuaciones del filtro se dividen en dos grupos: time update y measurement update. Las ecuaciones de time update, o etapa predictiva, son responsables de proyectar (en tiempo) el estado actual y la estimacin de la covarianza del error para obtener los estimados a priori para el siguiente paso. Las ecuaciones de measurement update, o etapa correctiva, es la responsable de la retroalimentacin para incorporar una nueva medida en el estimado a priori y obtener un mejorado estimado a posterior [5].

Las ecuaciones se presentan a continuacin: Time Update (Prediccin)

Predicted (a priori) state estimate Predicted (a priori) estimate covariance Donde:

A es el modelo de transicin de estado, aplicado al estado anterior B es el modelo de control de entrada aplicado al vector de control Q es la matriz de covarianzas del error del proceso.

Measurement Update (Correcin)

Ganancia de Kalman Updated (a posteriori) state estimate

Updated (a posteriori) estimate covariance

Donde: H es el modelo de observacin el cual mapea el espacio de estados real en el espacio de estado observable. R es la matriz de covarianzas del error medido Zk es la medida del proceso en el instante K.

Para el propsito de nuestra investigacin, se aplicar un filtro de Kalman particular a cada uno de los tres acelermetros presentes en el IMU. El proceso quedara entonces como se muestra en la figura 8.

Figura 8. Sistema Inercial

Para el caso del acelermetro: El estado no cambia de un paso al otro, por lo que el modelo de transicin de estado A ser igual a 1. No existe un vector de control de entrada por lo que el valor de uk-1 ser igual a 0. La medida del ruido es directamente del estado del sistema, por lo que el modelo de observacin H ser igual a 1. El filtro de Kalman quedar definido entonces como: Time Update (Prediccin)

Measurement Update (Correcin)

La programacin del filtro en Matlab se puede encontrar en el Anexo D. Para obtener un valor cercano de la covarianza del error medido R y de Q, existen varios mtodos entre ellos Auto-covarianza de mnimos cuadrados, ecuacin de Riccatis y Allan Variances[4]. Este ltimo nos permite medir la estabilidad de la seal y aproximar un valor de varianza al error. Aplicamos esta ltima metodologa [7] [8] para treinta mil datos de los acelermetros y obtuvimos el resultado mostrado en la figura 8.

Figura 8.Allan Variances El punto ms bajo corresponde a la desviacin estndar del error. Sin embargo, con estos valores la estabilidad y asertividad del filtro de Kalman se reducan por lo que se realizaron varias pruebas experimentales de prueba y error para llegar a valores de Q y R que mejoraran el desempeo del filtro. La aceleracin de X filtrada a travs de Kalman se muestra en la figura 9.

Figura 9.Posicin Obtenida por el sistema inercial filtrado.

Figura 10.Aceleracin filtrada con el filtro de Kalman

Como se puede observar, la aplicacin del filtro nos brinda un error de menos de 4 cm en 10 segundos, estando en posicin esttica, lo que mejora significativamente el desempeo del sistema de navegacin.

CONCLUSIN Este reporte nos brinda una introduccin a los elementos necesarios para el diseo de sistemas de navegacin inercial. Como parte de la reduccin del error en el proceso de obtencin de posicin se propuso lo combinacin de dos esquemas que no se usan comnmente juntos: Matriz de cosenos directores y filtros de Kalman independientes para los acelermetros. Los resultados de la combinacin de estos dos algoritmos, mejoran significativamente el desempeo de la obtencin de la posicin. Es importante resaltar que los errores se acumulan durante cada parte del proceso: Errores en la obtencin de los ngulos de orientacin generan que la componente gravitatoria de la aceleracin no se elimine por completo; adicionalmente el ruido presente en las seales del acelermetro, una calibracin imprecisa y posteriormente la doble integracin de este valor, impide que se pueda llegar a un valor de posicin preciso en largos periodos de tiempo. A travs de la combinacin de algoritmos, se logr reducir el error de posicin en cerca de un 85%, con respecto a los valores obtenidos sin el filtrado. La puesta a punto del Filtro de Kalman es fundamental, por lo que sera importante usar otras alternativas que nos permitieran obtener de manera ms cercana los valores de las covarianzas de los errores (Q y R). Las limitaciones estn actualmente en la precisin de los datos obtenidos por parte de los MEMS (giroscopios y acelermetros), los cuales por su naturaleza tienden a entregar error de offset, drift y ruido. La obtencin de valores de posicin y orientacin en periodos de tiempo extensos, no se puede realizar de manera precisa haciendo uso de sistemas de navegacin inercial simples.

Referencias [1] Building an AHRS using the SparkFun "9DOF Razor IMU" or "9DOF Sensor Stick".GitHub. https://github.com/ptrbrtz/razor-9dof-ahrs/wiki/Tutorial [2]Esfandyrai, J., De Nuccio, R., Xu, G. Introduction to MEMS gyroscopes.SolidState Technology. http://electroiq.com/blog/2010/11/introduction-to-mems-gyroscopes/ [3] Premerlani, W. , Bizard. P. Direction Cosine Matrix IMU: Theory. [4] Singhal, T., Harit, A., Vishwakarma D. Kalman Filter Implementation on an Accelerometer sensor data for three state estimation of a dynamic system. International Journal of Research in Engineering and Technology (IJRET) Vol1, No.6 2012 ISSN 2277-4378.

[5]Welch, G., Bishop, G. An Introduction to the Kalman Filter. Department of Computer Science. University of North Carolina at Chapel Hill. July 24, 2006. [6] Woodman, O. Technical Report: An introduction to inertial navigation. University of Cambridge. UCAM-CL-TR-696, ISSN 1476-2986. [7]Vukmirica, V., Trajkovki, I., Asanovic, N. Two Methods for the Determination of Inertial Sensor Parameters. Scientiic Technical Review, 2010, Vol 60., No 3-4, pp. 27-33. [8]Hopcroft,M. Allan Variances in Matlab. http://www.mathworks.com/matlabcentral/fileexchange/13246-allan [9]Herrera, A. Microacelermetros para la industria automotriz. http://www.uv.mx/cienciahombre/revistae/vol22num2/articulos/microace/index.html

ANEXO A.
Razor IMU 9DOF
La unidad de medicin inercial Razor, incorpora 3 sensores: 3 giroscopios ITG-3200 (uno por cada eje). 3 acelermetros ADXL345 (uno por cada eje). 3 magnetmetros HMC5883L (uno por cada eje).

Esto nos permite 9 grados de medicin inercial. Las salidas de todos los sensores son procesadas por el microcontrolador ATmega328. Un sistema de AHRS (attitude heading reference system) ya ha sido diseado para esta tarjeta y ser usado para los propsitos de esta investigacin.
Instalacin del Software 1. Descargar el AHRS firmware de la siguiente direccin: http://dlnmh9ip6v2uc.cloudfront.net/datasheets/Sensors/IMU/SF9DOF_AHRS.zip 2. Desde el IDE de Arduino, abrir el archivo Rrazor_AHRS.pde (Arduino/Razor_AHRS/ Rrazor_AHRS.pde). 3. En la seccin USER SETUP AREA descomentar #define HW__VERSION_CODE 10736, la cual corresponde a nuestra versin del IMU.

4. Ir a ToolsBoard y seleccionar la tarjeta Arduino Pro or Pro Mini (3.3V, 8mhz) w/ATmega328. Con esto ser suficiente para poder cargar el firmware a la tarjeta.

Calibracin de los sensores

Calibrar los sensores es fundamental para poder reducir los errores que por naturaleza tienen estos. El firmware que se descarg anteriormente, nos permite calibrar los valores de los 9 sensores con los que contamos. Calibracin del Acelermetro 1. Conectar el IMU a la computadora y descargar el firmware. Poner atencin al seleccionar el puerto serial correcto. 2. Abrir cualquier monitor serial, puede ser el del Arduino. Enviar #oc el cual nos permite entrar al modo de calibracin. La salida que se deber mostrar ser de la siguiente forma:
accel x,y,z (min/max) = -5.00/-1.00 25.00/29.00 225.00/232.00

3. Aqu lo que se intenta es encontrar los valores mximo y mnimos de la lectura que puede tener el acelermetro en cada uno de los ejes. 4. Mueva lentamente el IMU en cada uno de los ejes y observe el valor mximo o mnimo. Debido a la sensiblidad de los acelermetros, es muy importante que lo realice muy lentamente y varias veces; para volver a realizar una medicin debe simplemente enviar #oc.

5. Escribir los valores en la pestaa de programacin RAZOR_AHRS_ar, en la zona de SENSOR CALIBRATION.

Calibracin del Giroscpio 1. Dejar el IMU sobre una superficie estable. 2. Abrir cualquier monitor serial, puede ser el del Arduino. Activar el modo de calibracin #oc y enviar #on el cual lo lleva al siguiente sensor 3. Esperar 10 segundos sin mover el IMU. El programa calcula el promedio de ruido de cada uno de los giroscopios. 4. La salida se debe ver como:
gyro x,y,z (current/average) = -29.00/-27.98 102.00/100.51 -5.00/-5.85

5. Escribir los valores en la pestaa de programacin RAZOR_AHRS_ar, en la zona de SENSOR CALIBRATION.

Calibracin del magnetmetro 1. Abrir y correr en Processing el archivo Margnetometer_calibration. Es necesario tener instalada la librera EJML. 2. Rotar el sensor de tal forma que se cubran todas las orientaciones, produciendo puntos que cubran en su mayora a la esfera.

3. Presionar la barra espaciadora y en la consola de Processing aparecern una serie de lneas de cdigo, las cuales se debern colocar en la seccin de calibracin extendida del magnetmetro.

La calibracin de los sensores nos permite principalmente: Corregir escalas errneas que puedan llegar a tener. De esta manera todos los ejes pueden deben ser consistentes. Corregir zero offsets cuando la tarjeta est esttica.

ANEXO B.
Para los propsitos de la manipulacin de datos, es necesario que la unidad inercial nos entregue la informacin de los ngulos de Yaw, Pitch y Roll, adems de la aceleracin medida con cada uno de los acelermetros. En la programacin, en la pestaa de OutPut, debemos realizar las modificaciones que se observan en la siguiente imagen.

La obtencin de los datos por serial a travs de MatLab se muestra a continuacin:

ANEXO C.

ANEXO D.