41

41
I + D
REVISTA VNCULOS VOL. 10 NMERO 1 ENERO - JUNIO DE 2013
Localizacin de un robot mvil
tipo lego con el fltro de Kalman
Finding a mobile robot lego with the Kalman flter
Leidy Yolanda Lpez Osorio
Giovanni Rodrigo Bermdez Bohrquez

Fecha de recepcin: 12 de febrero de 2013
Fecha de aprobacin: 30 de abril de 2013
Resumen
En el proyecto de investigacin Diseo e implementacin de agentes robots, actuando
de manera cooperativa, en una zona colapsada simulada, como apoyo
a organismos de bsqueda y rescate urbano, se planearon diferentes
rutas para la bsqueda y rescate de las vctimas, las cuales eran
seguidas por los agentes, presentndose errores de localizacin
debidos a las condiciones fsicas de las plataformas. En este art-
culo se presenta la implementacin del fltro de Kalman en una
plataforma mvil Lego NXT con el software LabVIEW, para es-
timar la posicin y corregir los errores de localizacin.
Palabras clave:
Odometra, estimacin, fltro de Kalman, LabVIEW.

* Universidad Distrital Francisco Jos de Caldas Grupo de investigacin en Robtica Mvil Autnoma Bogot,
Colombia lylopezo@correo.udistrital.edu.co
** Universidad Distrital Francisco Jos de Caldas Grupo de investigacin en Robtica Mvil Autnoma Bogot,
Colombia, gbermudez@udistrital.edu.co
VINCULOS 010813.indd 41 26/08/2013 04:14:53 p.m.
42
42
Local i zaci n de un r obot mvi l t i po l ego con el f i l t r o de Kal man I + D
REVISTA VNCULOS VOL. 10 NMERO 1 ENERO - JUNIO DE 2013
Abstract
In the investigation project Design and implementation of robots agents ac-
ting cooperatively, in a simulated collapsed environment as organisms support
urban search and rescue, different routes were planned for the search and
rescue of victims, which were followed by agents, posing localization
errors due to the physical conditions of the platforms.
This article presents the implementation of the Kalman flter on a mobi-
le platform Lego NXT with LabVIEW software to estimate the position
and location correct errors.
Keywords
Odometry, estimation, Kalman flter, LabVIEW.
1. Introduccin
Conocer la posicin y orientacin de una
plataforma mvil en todo instante, es posible
haciendo el anlisis matemtico de sus ca-
ractersticas cinemticas y odomtricas; sin
embargo, localizar un robot haciendo uso de
dicho anlisis, es decir utilizando un mtodo
incremental, genera inconvenientes debido
a la acumulacin de errores a lo largo de la
trayectoria; hacindose necesario utilizar un
mtodo de correccin que permita ajustar el
recorrido de la plataforma de forma peri-
dica. En este caso se implementa la tcnica
recursiva del fltro de Kalman, que toma el
modelo cinemtico y las observaciones de
un sistema y las transforma en un modelo li-
neal a travs de la expansin de Taylor para
actualizar la covarianza de las mediciones
[1], con lo que se determinan los parmetros
correctos del sistema que evoluciona en el
tiempo. El fltro va prediciendo y auto ajus-
tndose con cada nueva medida [2], de esta
forma es posible corregir los errores y seguir
las rutas establecidas para la bsqueda y el
rescate de vctimas correctamente.
El desarrollo de esta tcnica recursiva se rea-
liza en LabVIEW, el cual brinda una herra-
mienta de estimacin y un mdulo de pro-
gramacin para el Lego NXT.
2. Determinacin de parmetros
2.1 Modelo cinemtico
Figura 1. Plataforma diferencial
Fuente: elaboracin propia.
VINCULOS 010813.indd 42 26/08/2013 04:14:53 p.m.
43
43
V N C U L O S LEI DY YOLANDA LPEZ OSORI O, GI OVANNI RODRI GO BERMDEZ BOHRQUEZ
REVISTA VNCULOS VOL. 10 NMERO 1 ENERO - JUNIO DE 2013
E N E R O - J U N I O D E 2 0 1 3
V OL U ME N 1 0 N ME R O 1
En la fgura 1 se muestra la plataforma con
traccin diferencial a la cual se le hace el mo-
delo cinemtico. El desplazamiento total y la
orientacin de la plataforma estn dados por
las ecuaciones (1) y (2) respectivamente, en
donde se presentan expresiones generales
aplicando todas las constantes correspon-
dientes para la plataforma donde y son el
desplazamiento y la orientacin de la rueda
izquierda, son el desplazamiento y la orien-
tacin de la rueda dererecha y b la distancia
entre las dos ruedas.
(1)
(2)
2.2 Modelo odmetrico
Un sistema que est permanentemente re-
gistrando la distancia recorrida describe la
evolucin temporal de la localizacin de una
plataforma mvil en funcin de sus propias
variables como se muestra en la ecuacin (3).
(3)
Donde (k + 1) es la posicin estimada, ( ) es
la posicin actual, ( ) es la entrada del siste-
ma y ( ) es el vector de errores sistemticos y
no sistemticos que pueden ser asociados a
la plataforma mvil. La entrada del sistema
( ) se encuentra determinada por la ecuacin
(4), donde ( ) es la distancia recorrida por
la plataforma en un intervalo ( , +1) y ( )
es la variacin de la orientacin en el mismo
intervalo.
(4)
La caracterizacin del vector de errores de
estado ( ) para la plataforma mvil, es asu-
mido como ( ) (0, ( )), donde ( ) es el error
caracterstico de estado de la plataforma.
Este error se encuentra dado por la ecuacin
(5); donde puede observarse que los errores
no son colineales.

(5)
Siendo el coefciente de error de traslacin
de la plataforma relativo a D y expresado
en [
2
]; el coefciente de error de rotacin de
la plataforma relativo a D y expresado en
[
2
]; y el coefciente de error de rotacin de la
plataforma relativo a y expresado en [
2
].
Teniendo en cuenta los procesos de experi-
mentacin realizados para los movimientos
individuales y para un movimiento combi-
nado, a partir de una serie de aproximacio-
nes lineales, como mnimos cuadrados, se
pudieron determinar que los tres coefcien-
tes con mejores resultados fueron: = 0,5, =
0,08, = 0,3.
Con el modelo cinemtico ajustado para
las condiciones de pruebas requeridas por
el proyecto de investigacin Diseo e im-
plementacin de agentes robots, actuando de
manera cooperativa, en una zona colapsada si-
mulada, como apoyo a organismos de bsqueda
y rescate urbano y el modelo odomtrico
vectorial mostrado en la ecuacin (6), se rea-
lizaron pruebas a la plataforma. Como zona
de pruebas fue usado el coliseo de la Facul-
tad Tecnolgica de la Universidad Distrital
Francisco Jos de Caldas en donde se realiz
la simulacin de una zona colapsada [3].

(6)
Se implement un algoritmo de bsqueda
y evasin de obstculos, mediante la utili-
zacin de sensores ultrasnicos y asimismo,
mediante los modelos de cinemtica y odo-
metra se conocieron los desplazamientos de
la plataforma y se establecieron rutas de na-
vegacin (fgura 2).
VINCULOS 010813.indd 43 26/08/2013 04:14:53 p.m.
44
44
Local i zaci n de un r obot mvi l t i po l ego con el f i l t r o de Kal man I + D
REVISTA VNCULOS VOL. 10 NMERO 1 ENERO - JUNIO DE 2013
En la fgura 3 se muestra el resultado de la
implementacin de una ruta predetermina-
da a la plataforma.
Como se observa en la fgura 3, la platafor-
ma no sigue la trayectoria correctamente de-
bido a los errores de odometra que aumen-
tan constantemente y se van acumulando
con cada desplazamiento.
Es aqu donde se implementa el fltro de Kal-
man, que estimar los parmetros correctos
Figura 2. Modelo de exploracin de obstculos a partir de la generacin de trayectorias
implementado en LabVIEW
Fuente: [3].
Figura 3. Resultado obtenido para el seguimiento de una ruta predeterminada teniendo
en cuenta los errores odomtricos presentes en la plataforma mvil
Fuente: [3].
VINCULOS 010813.indd 44 26/08/2013 04:14:54 p.m.
45
45
V N C U L O S LEI DY YOLANDA LPEZ OSORI O, GI OVANNI RODRI GO BERMDEZ BOHRQUEZ
REVISTA VNCULOS VOL. 10 NMERO 1 ENERO - JUNIO DE 2013
E N E R O - J U N I O D E 2 0 1 3
V OL U ME N 1 0 N ME R O 1
del robot tomando la informacin prove-
niente de los sensores.
3. Fases e implementacin
del fltro de Kalman
El fltro de Kalman calcula el estado del pro-
ceso en un instante y obtiene informacin
(se realimenta) de la medida. Por tanto, las
ecuaciones del fltro se pueden clasifcar en
dos tipos: actualizacin del tiempo (predic-
cin) y actualizacin de las medidas (correc-
cin) [2].
3.1 Fase de prediccin (t2)
En esta fase se calcula la estimacin, es decir,
la posicin en que se encontrar la platafor-
ma segn el modelo cinemtico y odomtri-
co como se muestra en (7) [4]
(7)
Donde A y B son matrices dinmicas que se
calculan como jacobianos del sistema:
Para A:
Para B:
Las anteriores son las ecuaciones del fltro
de Kalman que se van a aplicar para calcu-
lar la posicin de la plataforma mvil que
incorpora los modelos de odometra y loca-
lizacin. El sistema de odometra nos da los
incrementos en la posicin del punto central
del mvil respecto de unos ejes de referencia
fjos, estos incrementos se tomarn como en-
tradas directas al sistema. [5]
En la fase de estimacin tambin se obtiene
la matriz de covarianza del sistema, la cual
almacena el error general de esta y est dada
por la ecuacin (8).
(8)
Donde =2, siendo es la varianza de las medi-
das tomadas y es el numero de medidas to-
madas y el error caracterstico de estado de
la plataforma expresado en la ecuacin (5)
3.2 Fase de correccin
En esta parte del fltro se corrige la posicin
de la plataforma con la informacin que se
recibe de los sensores; esta informacin est
representada por la ecuacin (9) [1].
(9)
Donde ( ) es el error asociado al sistema de
medicin y es el jacobiano en funcin de la
medida, y considerando que se mide direc-
tamente la posicin y la orientacin del ro-
bot se tiene:

Por tanto

En la correccin tambin se incorporan las
medidas tomadas por los sensores al estado
anteriormente estimado as:
VINCULOS 010813.indd 45 26/08/2013 04:14:54 p.m.
46
46
Local i zaci n de un r obot mvi l t i po l ego con el f i l t r o de Kal man I + D
REVISTA VNCULOS VOL. 10 NMERO 1 ENERO - JUNIO DE 2013

Posteriormente se actualiza la matriz de co-
varianzas, es decir se actualiza el error [5]:

Y fnalmente se obtiene la matriz de ganan-
cias del fltro de Kalman, que contiene la
nueva observacin infuenciada por el error
entre la estimacin y la medida. [6][7][8].
Donde
3.3 Implementacin
Para implementar el fltro de Kalman en el
software LabVIEW de National Instruments,
fue necesario adaptar las ecuaciones del siste-
ma a la herramienta; en la fgura 4 se muestra
el VI que calcula la estimacin del fltro.
Donde y(t) es la entrada del sistema, es de-
cir las mediciones de los sensores; u(t) es el
estado inicial del sistema (para el clculo de
la matriz de covarianza no importa el estado
inicial, puede ser cualquiera [5]); stochastic
State-Space Model es el modelo del sistema;
los jacobianos calculados, entradas y medi-
ciones; Second-Order Statistics Noise es el
modelo asociado al error del sistema y al
error en las medidas; yhat(t) entrega el va-
lor del estado estimado, xhat(t) entrega el
modelo estimado, estimation error retorna
el error de covarianza y L(t) es la matriz de
ganancias del fltro [9].
Una vez identifcadas las entradas y salidas
de fltro, se construyen las matrices que in-
gresan al sistema; A (fgura 5), B (fgura 6) y
matrices (fgura 7).
Figura 4. VI fltro de Kalman en LabVIEW
Fuente: elaboracin propia.
Figura 5. SubVI de la dinmica del sistema
(Jacobiano A)
Fuente: elaboracin propia.
Figura 6. SubVI de las entradas del sistema
(x, y, )
Fuente: elaboracin propia.
VINCULOS 010813.indd 46 26/08/2013 04:14:55 p.m.
47
47
V N C U L O S LEI DY YOLANDA LPEZ OSORI O, GI OVANNI RODRI GO BERMDEZ BOHRQUEZ
REVISTA VNCULOS VOL. 10 NMERO 1 ENERO - JUNIO DE 2013
E N E R O - J U N I O D E 2 0 1 3
V OL U ME N 1 0 N ME R O 1
Las entradas del sistema estn dadas por X,
Y y , los cuales dependen de la lectura de
los sensores (encoders y compas); para esto
se tienen dos SubVIs para la distancia (D) y
el ngulo (Theta) (fgura 8 y fgura 9).
Despus de construir el modelo del sistema
y el modelo del error, se implementa el fltro
de Kalman (fgura 10).
Para observar la respuesta del fltro se to-
maron pruebas con trayectorias aleatorias; a
continuacin se muestran dos de ellas. En la
primera (fgura 11) la plataforma se detiene
Figura 7. SubVI C, D, H, R y G
Fuente: elaboracin propia.
Figura 8. SubVI D
Fuente: elaboracin propia.
Figura 9. SubVI
Fuente: elaboracin propia.
Figura 10. Implementacin del fltro de Kalman
Fuente: elaboracin propia.
VINCULOS 010813.indd 47 26/08/2013 04:14:57 p.m.
48
48
Local i zaci n de un r obot mvi l t i po l ego con el f i l t r o de Kal man I + D
REVISTA VNCULOS VOL. 10 NMERO 1 ENERO - JUNIO DE 2013
en tres ocasiones; la trayectoria superior es
la real y la inferior es la respuesta del fltro
que resulta ser muy similar a la real, lo que
indica que los valores de ruido son muy si-
milares los reales.
En la fgura 12 se obtiene el error en la esti-
macin tomando los valores de las seales y
restndolos.
En la segunda prueba (fgura 13) la platafor-
ma no se detiene mientras navega por el en-
torno y de igual forma que en la prueba 1, la
respuesta del fltro (inferior) es muy similar
a la trayectoria real del robot (superior). En
la fgura 14 se muestra el error de la estima-
cin para esta prueba.
4. Conclusiones
En el desarrollo del proyecto Diseo e imple-
mentacin de agentes robots, actuando de mane-
ra cooperativa, en una zona colapsada simulada,
como apoyo a organismos de bsqueda y rescate
urbano fue necesario implementar el fltro de
Figura 11. Respuestas del fltro
de Kalman prueba 1
Fuente: elaboracin propia.
Figura 12. Error en la estimacin de la prueba 1
Fuente: elaboracin propia.
Figura 13. Respuestas del fltro
de Kalman prueba 2
Fuente: elaboracin propia.
Figura 14. Error en la estimacin de la prueba 2
Fuente: elaboracin propia.
VINCULOS 010813.indd 48 26/08/2013 04:14:58 p.m.
49
49
V N C U L O S LEI DY YOLANDA LPEZ OSORI O, GI OVANNI RODRI GO BERMDEZ BOHRQUEZ
REVISTA VNCULOS VOL. 10 NMERO 1 ENERO - JUNIO DE 2013
E N E R O - J U N I O D E 2 0 1 3
V OL U ME N 1 0 N ME R O 1
Kalman para mejorar el proceso de localiza-
cin del robot sobre la zona de exploracin,
ya que para algunas tcnicas de rescate se
siguieron rutas predefnas y los errores de
odometra podran ocasionar lecturas inco-
rrectas durante la bsqueda de la vctima; la
tcnica recursiva del fltro de Kalman permi-
ti estimar la prxima posicin de la plata-
forma correctamente con el ruido presente
en el entorno.
Una vez corregidos los problemas de locali-
zacin de cada plataforma; se realizar una
comunicacin ordenador robot con el obje-
tivo de monitorear la posicin del robot en
instantes de tiempo determinados, lo cual
permitir dirigir a las plataformas a puntos
especfcos en el entorno; adems se desa-
rrollarn algoritmos de fusin sensorial im-
plementados sobre LabVIEW y el mismo la-
drillo NXT, que redundarn la informacin
para hacer una estimacin an ms exacta.
5. Agradecimientos
La autora agradece al Centro de Investiga-
ciones y Desarrollo Cientfco de la Univer-
sidad distrital Francisco Jos de Caldas por
la fnanciacin del proyecto Diseo e imple-
mentacin de agentes robots, actuando de mane-
ra cooperativa, en una zona colapsada simulada,
como apoyo a organismos de bsqueda y rescate
urbano y a la Facultad Tecnolgica por el
prstamo del espacio de prueba usado para
el desarrollo del proyecto.
6. Referencias
[1] M. Pinto, A. P. Moreira and A. Matos,
Localization of Mobile Robots Using an
Extended Kalman Filter in a LEGO NXT.
IEEE Transactions on Education, 1-10.
2001.
[2] P. R. Muoz, T. Antonio and S. Monte-
mayor, DE IMGENES . Autor:, 57. [En
lnea], disponible en http://www.etsii.
urjc.es/~asanz/documentos/Memo-
riaK almanJun03.pdf. 2003.
[3] Filtro de Kalman Documentos Me-
moria de Kalman. Jun 2012. Retrie-
ved from: http://www.etsii.urjc.
es/~asanz/documentos/Memoria
Kalman.pdf
[4] R. G. Bermdez, Modelamiento cine-
mtico y odomtrico de robots mviles:
aspectos matemticos. Tecnura, vol.
20, no. 12. 2003.
[5] F. Mart, P. Barrera, J. Mar, G. D. Rob,
U. Rey y J. Carlos, Localizacin basada en
lgica difusa y fltros de Kalman para ro-
bots con patas, 12. [En lnea], disponible
en http://gsyc.es/jmplaza/papers/
cmpi2006-paco.pdf. 2006.
[6] P. Zarchan, undmentals of Kalman flte-
ring: practical approach volume 232 pro-
gress in astronautics and aeronautics,
tercera edicin 2009
[7] G. Welch and G. Bishop, An Introduc-
tion to the Kalman Filter. In Practice, 116.
2003.
[8] B. F. Aguado, J. A. Castao, C. Parque,
D. Boecillo, E. Z. Casanova, J. G. Garca-
Bermejo y E. Paseo, Diseo y simulacin
de un fltro kalman para un robot mvil, 6.
2004.
[9] Technology, I. System Identifcation
and Estimation in LabVIEW. . [En l-
nea], disponible en http://home.hit.
no/~hansha/documents/labview/
training /System Identifcation and Es-
timation in LabVIEW/System Identif-
cation and Estimation in LabVIEW.pdf.
2001.
VINCULOS 010813.indd 49 26/08/2013 04:14:58 p.m.