Abstract: This paper presents the design and construction of an INS (Inertial Navigation
System) which gives position, attitude and orientation of a robotic platform. It has an IMU
(Inertial Measurement Unit) developed from inertial sensors and a magnetometer which gives
attitude and orientation throw an Extended Kalman Filter (EKF). Experimental results are
included, which validate the research and project.
Keywords: Include Control, Inertial Navigation, Inertial Measurement, Extended Kalman
Filter, GPS.
112 Revista Politcnica - Julio 2013, Vol. 32, No. 2, Pginas: 112119
Diseo e Implementacin de un Sistema de Navegacin Inercial Tipo Strapdown para estimar la Posicin de un Robot Mvil.
Es el vector de las derivadas de los ngulos roll,
pitch y yaw respectivamente.
(k1 + 2 k2 + 2 k3 + k4)
yi+1 = yi + h (2)
entregar medidas de aceleracin, velocidad angular, campo 6
magntico, actitud y orientacin. Donde:
La actitud refleja la inclinacin del vehculo respecto eje k1 = f (xi , yi )
longitudinal y lateral que se muestra en la figura 2,la 1 1
k2 = f (xi + h, yi + k1 h)
orientacin permite conocer el rumbo de navegacin del 2 2
vehculo respecto al norte magntico, a este sistema se 1 1
k3 = f (xi + h, yi + k2 h)
lo conoce tambin como AHRS (Attitude and Heading 2 2
Reference System). Sus componentes son los ngulos de k4 = f (xi + hyi + k3 h)
Euler: h = tiempo de muestreo
Roll. Los trminos xi y yi se ajustan a la expresin
Pitch. mostrada en 1 de la siguiente forma:
Yaw.
xo p yo
xi = x1 = q y yi = y1 = (3)
x2 r y2
h = dt = 0.01
Y por lo tanto:
x0 x1 sin y0 tan y1 x2 cos y0 tan y1
f0
fi = f1 = 0 x1 cos y0 x2 sin y0
f2 0 x1 sin y0 sec y1 x2 cos y0 sec y1
(4)
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
Figura 2: Componentes de AHRS [1] actual a partir de la anterior, los ngulos resultantes
(1) Sistema de medicin de actitud presentan errores acumulativos, por esta razn se ha
decido utilizar un Filtro de Kalman con el objetivo
Determinar la actitud de la plataforma, implica de reducir el error de los ngulos resultantes.
calcular los ngulos de pitch, roll y yaw, para ello
es necesario resolver la ecuacin mostrada en (1),[6]. (3) Filtro de Kalman (FK)
El FK tiene como objetivo estimar los estados de
1 sin tan cos tan
p un sistema dinmico lineal ruidoso, utilizando un
E = = 0 cos sin q (1) algoritmo ptimo y recursivo. En este caso el sistema
0 sin sec cos sec r dinmico es no lineal por lo que se utilizara un Filtro
de Kalman Extendido (FKE).
Donde:
p
q Es el vector de velocidades angulares medidas
r
por los giroscopios en el eje x, y z respectivamente.
Revista Politcnica - Julio 2013, Vol. 32, No. 2, Pginas: 113119 113
Utreras A.; Guallichico J.; Rosales A.; Burbano P.; valos E.
H= 0 0 0 M
yc
Hed = tan1 (9)
sin cos cos sin 0 Mxc
La Correccin del FKE se basa en los ngulos En la Fig.4, se muestra el cdigo grfico utilizado
roll y pitch calculados a partir del acelermetro (ob- para el clculo del ngulo de orientacin, lo primero
servacin), por esta razn solo estos dos ngulos que se hizo es graficar la respuesta original de los
son corregidos por el FKE, quedando la orientacin magnetmetros cuando la plataforma gira 360 en
(ngulo yaw) para ser calculada mediante un mag- el plano XY, sobre esta misma grfica se superpone
netmetro. una elipse de parmetros configurables, es decir se
(4) Sistema de medicin de orientacin pueden variar sus ejes, el ngulo de rotacin y sus
Para obtener la orientacin de la plataforma se desplazamientos con respecto al origen, lo que se
ha utilizado las medidas del campo magntico en el busca es determinar estos mismos parmetros pero
plano XY, estas medidas con una calibracin previa para la respuesta descrita por los magnetmetros.
describen un comportamiento de un circulo unitario, Una vez que se conoce los parmetros de la curva
al momento de rotar a la plataforma sobre un eje descrita por los magnetmetros se procede a realizar
114 Revista Politcnica - Julio 2013, Vol. 32, No. 2, Pginas: 114119
Diseo e Implementacin de un Sistema de Navegacin Inercial Tipo Strapdown para estimar la Posicin de un Robot Mvil.
Revista Politcnica - Julio 2013, Vol. 32, No. 2, Pginas: 115119 115
Utreras A.; Guallichico J.; Rosales A.; Burbano P.; valos E.
La seal de velocidad se filtra nuevamente, debido a que Figura 6: Datos del GPS sirven para tener un punto inicial de
por efectos del filtro utilizado en la aceleracin, aparece medida.
116 Revista Politcnica - Julio 2013, Vol. 32, No. 2, Pginas: 116119
Diseo e Implementacin de un Sistema de Navegacin Inercial Tipo Strapdown para estimar la Posicin de un Robot Mvil.
Se comprob que las medidas de los ngulos roll y pitch En la Fig. 12 se muestra la respuesta de desplazamiento
de la IMU desarrollada tienen pequeas desviaciones (5) una vez que la velocidad ha sido corregida, como se puede
en comparacin con la IMU comercial, pero se comprob apreciar la seal de color azul crece desde un valor inicial
que la IMU desarrollada siempre tiene una mejor medida de 0m y llega a un valor de aproximadamente 1m, esta
de orientacin (ngulo yaw) respecto al norte magntico. seal fue tomada cuando la plataforma se desplaz 1m,
partiendo inicialmente del reposo.
Revista Politcnica - Julio 2013, Vol. 32, No. 2, Pginas: 117119 117
Utreras A.; Guallichico J.; Rosales A.; Burbano P.; valos E.
Una calibracin adecuada de los magnetmetros permite [7] Mtodos de Runge-Kutta. [Online]. Disponible:
obtener un ngulo de orientacin (yaw) que es confiable http://en.wikipedia.org/wiki/Runge%E2%80%
incluso cuando en el ambiente hay la presencia de inter- 93Kutta_methods
ferencias electromagnticas de baja intensidad. Dentro del
proceso de calibracin se deben considerar las inclinaciones ACERCA DE LOS AUTORES:
de la plataforma las mismas que ocasionan una distorsin
del ngulo de orientacin, estos efectos se suprimen con JORGE GUALLICHICO (Autor)
ayuda de los ngulos de inclinacin roll y pitch.
Naci el 11 de Marzo de 1990 en la Ciudad de Quito. Rea-
La aceleracin esttica que se produce por inclinaciones
liz sus estudios secundarios en el Instituto Tecnolgico
de la plataforma, no refleja desplazamiento de la misma,
Superior SUCRE de la ciudad de Quito donde fue Aban-
debido a esto solo se utiliza la aceleracin dinmica para
derado del pabelln de la institucin. En 2007 inicia sus
el clculo de desplazamiento, la separacin de estas dos
estudios de ingeniera en la Escuela Politcnica Nacional.
aceleraciones se realiza mediante filtros pasa altos que
eliminan la componente continua de una seal. reas de inters: microcontroladores, Desarrollo de aplica-
ciones JAVA, FPGA, automatizacin y control industrial.
El GPS permite obtener la posicin del vehculo en un sis-
Actualmente desarrolla sistemas de rastreo satelital con
tema de referencia global, la informacin que ste entrega
equipo skypatrol y hunter-pro en INTERTEL LTDA. y se
est encriptada en tramas segn la norma NMEA-0183.
especializa en el diseo e implementacin de tableros de
Esta informacin debe ser discriminada y transformada
control en TCONTROL S.A.
para obtener coordenadas de longitud y latitud, las cuales
pueden ser utilizadas para mostrar la ubicacin del ve-
ADRIN UTRERAS (Autor)
hculo en un mapa a tiempo real.
El INS entrega datos necesarios para el desarrollo de un Naci en Quito el 5 de Octubre de 1988, sus estudios secun-
autopiloto, tal como la actitud para mantener estable el darios los realiz en el Colegio Tcnico Superior Sucre
UAV en el aire, la orientacin para mantener fijo un rumbo donde obtuvo el ttulo de Tcnico Industrial Electrnico.
determinado y el desplazamiento del cuerpo. En el 2007 inicio sus estudios en la Escuela Politcnica
Nacional (EPN), en la carrera de Ingeniera en Electrnica
AGRADECIMIENTOS y Control. Fue colaborador en el proyecto UAV del con-
venio EPN-FAE y particip como expositor acerca del
Este trabajo ha sidodesarrollado y financiado en el
proyecto UAV en el Primer Seminario de Sistemas Areos
Proyecto de Investigacin Interno UAV, bajo el Conve-
No Tripulados en el ao 2012. Actualmente es Ingeniero
nio entre la Escuela Politcnica Nacional y el Centro de
Especialista en SIAPROCI CIA. LTDA., desarrollando
Investigacin y Desarrollo de la Fuerza Area Ecuatoriana
actividades de integracin, programacin, diseo y comer-
(EPN-CIDFAE).
cializacin de Sistemas de Seguridad Electrnicos (Control
REFERENCIAS de Accesos, CCTV e Incendios).
[1] FERRER, Gonzalo, Integracin Kalman de sen- reas de Intereses: Instrumentacin, Robtica Mvil y
sores inerciales INS con GPS en un UAV, 2009.[On- Control Electrnico Industrial.
line]. Disponible: http://upcommons.upc.edu/pfc/
bitstream/2099.1/6930/1/memoriadef.pdf PATRICIO BURBANO (Director)
[2] RODRGUEZ, Patricia, Aplicacin del Filtro de
Kalman al Seguimiento de Objetos en Secuen- El Prof.-MSc.-Ing. Patricio Burbano Romero naci el 9
cias de Imgenes, proyecto final de carrera, de octubre de 1951 en la ciudad de Riobamba, Ecuador.
2003.[Online]. Disponible: http://www.etsii.urjc. Realiz sus estudios superiores en la Escuela Politcnica
es/~asanz/documentos/MemoriaKalmanJun03.pdf Nacional (EPN), donde se gradu de Ingeniero en Elec-
[3] TEREJANU, Gabriel, Extended Kalman Filter Tu- trnica y Telecomunicaciones. Su Masterado, en Sistemas
torial. [Online]. Disponible: http://users.ices. y Control, lo obtuvo en 1984 en el UMIST, Universidad de
utexas.edu/~terejanu/files/tutorialEKF.pdf Manchester, Reino Unido. Ha sido docente en la Escuela
[4] Using LSM303DLH for a tilt compensated electronic Politcnica Nacional por ms de 38 aos, ocupando varios
compass [Online]. Disponible: http://spacegrant. cargos importantes y ha dirigido varios proyectos de in-
colorado.edu/COSGC_Files/Robotics/Workshop% vestigacin a lo largo de su vida como Profesor Principal
20Resources/LSM303%20AppNote.pdf del Departamento de Automatizacin y Control Indus-
[5] LSM303DLM Gua de usuario. trial en la Facultad de Elctrica y Electrnica de la Es-
[6] SIOURIS, George M., Aerospace Avionics Systems, cuela Politcnica Nacional. Actualmente es miembro de la
EEUU, 1993 Comisin de Evaluacin Interna de la Escuela Politcnica
118 Revista Politcnica - Julio 2013, Vol. 32, No. 2, Pginas: 118119
Diseo e Implementacin de un Sistema de Navegacin Inercial Tipo Strapdown para estimar la Posicin de un Robot Mvil.
Nacional y parte del Consejo del Departamento de Au- en el Instituto de Sistemas en Tiempo Real (RTS) de
tomatizacin y Control Industrial. Sus reas de interes son: la Universidad de Hannover, Alemania, como becario del
Identificacin de Sistemas, Control Moderno, Dinmica de DAAD (Instituto Alemn de Intercambio Acadmico). Ha
Sistemas, Control Automtico, Sistemas Inteligentes, entre trabajado en el rea de ID del CENACE, de CELEC
otros. EP y el Ministerio de Electricidad y Energa Renovable.
Actualmente, es Coordinador de la Red Ecuatoriana de
ANDRS ROSALES (Co-director) Universidades y Escuelas Politcnicas para Investigacin y
Posgrados REDU y ocupa el cargo de Profesor Principal
El Dr.-Ing. Andrs Rosales Acosta naci el 28 de abril de del Departamento de Automatizacin y Control Industrial
1977 en la ciudad de Quito, Ecuador. Realiz sus estu- en la Facultad de Elctrica y Electrnica de la Escuela
dios superiores en la Escuela Politcnica Nacional (EPN), Politcnica Nacional, desarrollando actividades de inves-
donde se gradu de Ingeniero en Electrnica y Control. Su tigacin, consultora y docencia. Sus reas de interes son:
Doctorado, en Ingeniera en Sistemas de Control, lo obtuvo Robtica Aplicada, Control Predictivo, Sistemas Multia-
en el 2009 luego de cursar sus estudios en el Instituto de gente, Modelado y Control de Procesos Industriales, Ener-
Automtica (INAUT) de la Universidad Nacional de San ga, entre otros.
Juan, Argentina; adems realiz una estada investigativa
Revista Politcnica - Julio 2013, Vol. 32, No. 2, Pginas: 119119 119