Upload
daniel-garcia
View
15
Download
1
Embed Size (px)
Citation preview
Captulo 4
Localizacin y Mapeado de forma
Simultnea (SLAM)
4.1. Introduccin
SLAM son las siglas de Simultaneous Localization and Mapping, y puede traducirse como
Localizacin y Mapeado Simultneos. Fue desarrollado originariamente por Hugh Durrant-
White y John J. Leonard [5], basado en en un trabajo anterior de Smith, Self y Cheeseman
[18].
El SLAM constituye el bloque central de este proyecto y, adems, es uno de los problemas
ms importantes en la robtica.
Como ya se coment en la introduccin, el problema del SLAM se manifiesta cuando el
robot desconoce tanto la posicin en que se encuentra, como el mapa de su entorno, teniendo
acceso nicamente a las medidas que obtiene de los sensores y a las seales de control. En el
SLAM, el robot tiene que construir un mapa del entorno desconocido al mismo tiempo que se
localiza con respecto a dicho mapa.
Desde el punto de vista probabilstico, hay dos tipos de SLAM:
42
4.1. Introduccin
Online SLAM: Conlleva estimar la postura (x) y el mapa (m) dadas las acciones de
control (u) y las observaciones de los sensores (z).
p(xt,m|z1:t,u1:t) (4.1)
En cada iteracin slo se estimar la ltima posicin del robot. Los subndices que apare-
cen en z y u quieren decir que se conocen las seales de control y las observaciones desde
el inicio hasta el instante actual (t). La postura tan slo lleva el subndice t porque slo se
estimar la postura actual que tiene el ROMEO, no las anteriores. La figura 4.1 muestra
grficamente en qu consiste el online SLAM. La representacin de dicha figura se ha
hecho empleando redes Bayesianas de correspondencia.
Una red Bayesiana (o de creencia) es un grafo dirigido acclico, que permite representar
un conjunto de variables y su independencia probabilstica de forma prctica y compacta
y que est compuesta por nodos (variables) y arcos dirigidos (significan influencia de un
nodo sobre otro). Ms informacin acerca de redes Bayesianas puede encontrarse en [16].
Full SLAM: Esta otra variante de SLAM busca determinar la postura en toda la trayec-
toria a partir de las acciones de control y las observaciones de los sensores.
p(x1:t,m|z1:t,u1:t) (4.2)
La figura 4.2 muestra grficamente (empleando nuevamente una red Bayesiana) en qu
consiste el full SLAM. En este caso, y a diferencia del anterior, se puede ver cmo la
postura tiene el subndice desde 1 hasta t, representando que se va a calcular toda la
trayectoria a partir de las medidas de los sensores y de las seales de control que han
tenido lugar desde que el robot comenz su movimiento.
En la prctica, el clculo del full SLAM es normalmente inviable, debido a la elevada di-
mensionalidad del espacio continuo de parmetros y al gran nmero de caractersticas que hay
en los mapas que se van construyendo. Por este motivo, en los siguientes apartados, se ver c-
mo resolver el problema del online SLAM, dejando de lado el caso del full SLAM. Una forma
de abordar el problema del full SLAM es empleando la tcnica de Graph SLAM ([21], [17]),
4. Localizacin y Mapeado de forma Simultnea (SLAM) 43
4.1. Introduccin
Figura 4.1: Problema del online SLAM
Figura 4.2: Problema del full SLAM
que se basa en que la solucin se puede ver como un grfico poco denso, que se puede resolver
eficientemente. Esto se traduce en una serie de restricciones cuadrticas que al resolverse dan
el mapa y las posturas de mxima probabilidad.
El algoritmo de SLAM puede implementarse de numerosas maneras, ya que es ms un
concepto que un algoritmo. Consta de muchos pasos, cada uno de los cuales puede ser resuelto
con varios algoritmos distintos. Algunos de los principales pasos, y que sern explicados en
4. Localizacin y Mapeado de forma Simultnea (SLAM) 44
4.1. Introduccin
detalle ms adelante, son la extraccin de caractersticas, asociacin de datos, estimacin del
estado y actualizaciones del estado y de las caractersticas del entorno.
4.1.1. Consideraciones hardware
A la hora de afrontar el problema de SLAM, es fundamental tener en cuenta el hardware del
robot que se va a emplear.
El modelo que describe el movimiento del robot es de gran importancia, ya que la comple-
jidad del mismo ser directamente proporcional a la complejidad de las ecuaciones que habr
que calcular durante la ejecucin del algoritmo. Por ejemplo, no es lo mismo tratar con un ve-
hculo de cuatro ruedas que con un robot humanoide, o con vehculos autnomos submarinos,
vehculos autnomos voladores, etc. En este caso se tratar con un vehculo de cuatro ruedas,
cuyo modelo se describi en un captulo anterior.
Hay varias opciones para obtener las medidas del entorno:
Lser: Los lseres son muy precisos, eficientes, y la salida no requiere demasiado cm-
puto para procesarla. Sin embargo, si en el entorno del lser hay superficies de cierto
material, como el cristal, los resultados que proporciona pueden diferir en gran medida
de la realidad. Otro inconveniente es que no se pueden usar bajo el agua, ya que sta
perturba la luz y su rango se ve considerablemente reducido.
Snar: El snar era muy empleado hace algunos aos, y su precio es mucho ms reducido
que el de los lseres. Sus medidas no son tan buenas comparadas con las que proporciona
el lser y, a menudo, proporcionan lecturas errneas. Aunque bajo el agua, son la mejor
eleccin y se asemejan al modo de navegar de los delfines.
Visin por computador: Esta opcin a veces proporciona resultados errneos debido a
los cambios en la luz y a entornos en ausencia de la misma, aunque recientemente se
estn produciendo muchos avances que palan en cierto modo este problema. A menudo,
se emplea un sistema estreo para tener medidas de profundidad. Las ventajas de emplear
4. Localizacin y Mapeado de forma Simultnea (SLAM) 45
4.2. SLAM con el Filtro Extendido de Kalman
la visin frente al lser o al snar, es que se asemeja a la visin humana y a que una imagen
contiene mucha ms informacin que las lecturas de los otros dos sensores. Adems, las
cmaras son relativamente baratas, en comparacin con los lseres o snares. En cambio,
el coste computacional se ve incrementado con respecto a los sensores anteriores.
Aunque el ROMEO est equipado con los tres dispositivos anteriormente descritos, en el
algoritmo de SLAM se har uso del lser, ya que, sin ser tan complejo como la visin por
computador, proporciona unas medidas lo suficientemente fiables.
4.2. SLAM con el Filtro Extendido de Kalman
Existen varias formas de abordar el problema del SLAM. En este proyecto, el algoritmo de
SLAM se basar en el Filtro Extendido de Kalman (EKF), que se aplica al online SLAM usando
una asociacin de datos basada en la mxima probabilidad. En el apndice B se introducirn
tanto el Filtro de Kalman como el Filtro Extendido de Kalman.
Otra opcin al implementar el SLAM es hacerlo con mapas en 2D 3D. El lser que se
emplear en el algoritmo trabaja en dos dimensiones, por lo que no hay otra opcin que trabajar
en 2D. Sin embargo, hay tcnicas que emplean la visin por computador o lseres 3D, que
permiten construir mapas en tres dimensiones [11].
Los mapas en el EKF SLAM estn basados en caractersticas. Una caracterstica puede ser
un punto o una lnea por ejemplo. El nmero de caractersticas no debe ser muy grande, ya que
el coste computacional sera muy elevado, haciendo al algoritmo muy ineficiente.
Como en cualquier algoritmo basado en el EKF, el EKF SLAM asume un ruido Gaussiano
tanto para la percepcin como para el movimiento del robot.
4. Localizacin y Mapeado de forma Simultnea (SLAM) 46
4.2. SLAM con el Filtro Extendido de Kalman
4.2.1. El Algoritmo General del EKF SLAM
El algoritmo general del EKF SLAM se muestra a grandes rasgos en la tabla 4.1. Tambin se
puede ver un diagrama de flujo del algoritmo en la figura 4.3, en el que aparecen las funciones
ms importantes que lo componen.
A continuacin, se presenta un breve resumen de los pasos que se irn dando en este cap-
tulo, y que complementan a la tabla y al grfico:
1. Cuando cambia la odometra debido al movimiento del robot, la incertidumbre asociada
a la nueva posicin del robot se actualiza en el EKF en la etapa de Prediccin.
2. Se extraen las caractersticas del entorno observables desde la posicin actual del robot.
3. El robot intenta asociar las nuevas caractersticas a las que ya haba detectado anterior-
mente.
4. Las caractersticas re-observadas se emplean para actualizar la posicin del robot en el
EKF.
5. Las caractersticas nuevas, se aaden al EKF como nuevas observaciones para poder re-
observarlas en siguientes iteraciones del algoritmo.
En las figuras que van desde la 4.4 hasta la 4.8, se muestran grficamente los pasos ya
comentados.
En la figura 4.4, el tringulo representa al robot, mientras que las estrellas representan a las
caractersticas que el robot ha detectado en su entorno. El robot inicialmente mide la posicin
de las caractersticas con respecto a donde se encuentra l mismo, empleando sus sensores (las
medidas de los sensores se ilustran con los rayos). Alrededor del robot y de las caractersticas,
hay dibujadas unas elipses, que representan la incertidumbre existente en la posicin de los
mismos. Estas incertidumbres tienen su origen en los errores presentes en los sensores, tanto en
4. Localizacin y Mapeado de forma Simultnea (SLAM) 47
4.2. SLAM con el Filtro Extendido de Kalman
Figura 4.3: Diagrama de flujo del algoritmo de SLAM
4. Localizacin y Mapeado de forma Simultnea (SLAM) 48
4.2. SLAM con el Filtro Extendido de Kalman
Figura 4.4: Situacin inicial
los que permiten obtener las caractersticas (lser), como en los que permiten estimar la posicin
del robot (codificadores pticos y girscopo).
La figura 4.5, muestra el movimiento del robot, y cmo ste estima su posicin actual emple-
ando la odometra. Al desplazarse el robot, y debido a errores en la odometra, la incertidumbre
de la posicin del mismo aumenta con respecto a la que tena en la figura anterior.
En la figura 4.6, el robot nuevamente mide la localizacin de las caractersticas empleando
sus sensores, pero averigua que no se encuentran en la posicin en la que deberan estar (dada
la localizacin actual estimada del robot por la odometra). Por tanto, la posicin estimada del
robot no es correcta.
Como las lecturas del lser son ms fiables que las que proporciona la odometra, el robot
emplea la posicin actual de las caractersticas para situarse. La posicin corregida viene dada
por el tringulo en lnea continua, mientras que la posicin anterior y que era errnea, viene
dada por el tringulo en lnea discontinua, como puede verse en la figura 4.7.
Si se corrige la posicin del robot, es por que se piensa que la posicin en la que estaba
4. Localizacin y Mapeado de forma Simultnea (SLAM) 49
4.2. SLAM con el Filtro Extendido de Kalman
Figura 4.5: El robot se mueve a otra posicin
Figura 4.6: El robot adquiere nuevamente las medidas de su entorno
era errnea, y se quiere pasar a una que se acerque ms a la que ocupa realmente. Por ello, el
robot estar mejor localizado que antes. Del mismo modo, si el robot est mejor localizado, se
reducir su incertidumbre y la de las caractersticas del mapa, como se puede observar en las
4. Localizacin y Mapeado de forma Simultnea (SLAM) 50
4.2. SLAM con el Filtro Extendido de Kalman
elipses de la figura, que son menores que las que haba en figuras anteriores.
Figura 4.7: El robot actualiza su posicin en funcin de las nuevas medidas obtenidas con el
lser
Figura 4.8: Comparativa de la posicin real frente a las dos estimadas anteriormente
Finalmente, en la figura 4.8 se pueden observar la posicin real del robot (representada por
4. Localizacin y Mapeado de forma Simultnea (SLAM) 51
4.2. SLAM con el Filtro Extendido de Kalman
el tringulo con lnea continua ms slida), la estimada con odometra (representada por el
tringulo con lnea discontinua) y la corregida gracias a las medidas del lser (representada por
el tringulo con lnea continua ms suave). Ya que los sensores no son perfectos, la posicin
real no coincide con la posicin corregida al emplear el lser pero se asemeja bastante ms que
a la estimada empleando la odometra nicamente.
Es importante sealar en cualquier punto del algoritmo de SLAM, el robot siempre conocer
la estimacin actual de su posicin.
4.2.2. Inicio
En primer lugar, y antes de comenzar con el algoritmo de SLAM, es necesario iniciar una
serie de variables que se emplearn ms adelante. Tales variables pueden ser las matrices de
ruido, la longitud mnima de un segmento, el rango del lser, el nmero de puntos de que consta
un barrido del lser, etc.
Adems, en el instante inicial, el vector de estados y la matriz de covarianzas tendrn el
valor nulo.
x0 =
0
0
0
(4.3)
P0 =
0 0 0
0 0 0
0 0 0
(4.4)
Tanto la dimensin del vector de estados como la de la matriz de covarianzas aumentarn
en el tiempo, ya que, cada vez que se observe una nueva caracterstica del mapa, sta se aadir
al vector de estados, al mismo tiempo que provocar el aumento de la matriz de covarianzas.
Los dos primeros elementos del vector de estados son las coordenadas x e y de la posicin en
la que se encuentra el robot con respecto al sistema de coordenadas global. El tercer elemento es
4. Localizacin y Mapeado de forma Simultnea (SLAM) 52
4.2. SLAM con el Filtro Extendido de Kalman
Algoritmo General del EKF SLAM(xt1,Pt1,ut, zt, Nt1):1.- Actualizacin del nmero de caractersticas.
Nt = Nt1
Etapa de Prediccin:
2.- Obtencin de la matriz Fx.
Fx =
1 0 0 0 00 1 0 0 00 0 1 0 0
3.- Proyeccin del estado hacia adelante.
xt = xt1 + FTx
v t sin(t1)v t cos(t1)
t
4.- Obtencin de la matriz Gt.
Gt = I + FTx
0 0 v t cos(t1)0 0 v t sin(t1)0 0 0
Fx5.- Proyeccin de la covarianza del error hacia adelante.
Pt = GtPt1GTt + FTxRtFx
Etapa de Actualizacin:
6.- Obtencin de la matriz de ruido Qt.
7.- Para cada caracterstica observada, zit = (it,
it)T .
8.- Para cada caracterstica del mapa, zkt = (kt ,
kt )T .
9.- Obtencin de la matriz Fx,k.
Fx,k =
1 0 0 0 0 0 0 0 00 1 0 0 0 0 0 0 00 0 1 0 0 0 0 0 00 0 0 0 0 1 0 0 00 0 0 0 0 0 1 0 0
4. Localizacin y Mapeado de forma Simultnea (SLAM) 53
4.2. SLAM con el Filtro Extendido de Kalman
10.- Obtencin del Jacobiano de la estimacin, Hk.
Hk = hkFx,k
11.- Obtencin de la matriz de covarianzas de la estimacin, k.
k = HkPtHTk + Qt
12.- Obtencin de la distancia de Mahalanobis al cuadrado, D2M .
D2M = (zi zk)T1k (zi zk)13.- Se busca la mnima distancia de Mahalanobis por debajo de un umbral.
14.- Fin Para.
15.- Para esa recta con mnima distancia de Mahalanobis inferior al umbral.
K = PtHTk
1t .
16.- Actualizacin del vector de estados.
xt = xt + K(zi zk)17.- Actualizacin de la matriz de covarianzas.
Pt = Pt KkKT .18.- Fin Para.
19.- Si hay disponibles medidas de GPS
20.- Obtencin de la matriz de ruido QGPSt .
21.- Obtencin del Jacobiano de la medida, HGPSt .
HGPSt =
1 0 0 0 0 00 1 0 0 0 0
22.- Nueva etapa de actualizacin.
t = HGPStPtHTGPSt + QGPSt
K = PtHTGPSt
1t .
xt = xt + K
XGPS xt(1)YGPS xt(2)
Pt = Pt KtKT .
23.- Fin Si.
4. Localizacin y Mapeado de forma Simultnea (SLAM) 54
4.2. SLAM con el Filtro Extendido de Kalman
24.- Si alguna recta no coincide con ninguna otra, se aade al mapa
para compararla posteriormente.
25.- Aumento del vector de estado, xt
26.- Aumento de la matriz de covarianza, Pt
27.- Fin Si.
Tabla 4.1: Algoritmo General del EKF SLAM
el ngulo que forma el eje X del sistema solidario al robot con el eje X del sistema coordenado
global. A partir de ah, los elementos que constituyen el vector de estado, van agrupados por
parejas, y son la distancia y la orientacin hacia cada caracterstica ya detectada.
4.2.3. Prediccin
Tal y como se observa en la figura 4.3, la etapa de Prediccin es la primera que tiene lugar
en el algoritmo de SLAM.
En dicha etapa, se estimarn el vector de estados y la matriz de covarianzas, usando tan
slo la odometra. Se conoce como odometra a las tcnicas de posicionamiento que emplean
la informacin de sensores que adquieren datos del propio sistema (en este caso, del ROMEO),
para obtener una aproximacin de la posicin real en la que se encuentra el robot en un deter-
minado instante, respecto a un sistema de referencia inicial. Como ya se vio en el captulo 2,
el ROMEO lleva incorporado varios sensores, como unos codificadores pticos o un girscopo,
que permiten obtener la velocidad lineal, la curvatura, y la velocidad angular. Un buen sistema
de odometra sera aquel que tuviera un error de menos de 2cm por cada metro recorrido, y uno
menor a 2 por cada 45 girados.
En esta etapa, como ya se ha comentado, se va a realizar una estimacin tanto de el vector
de estados como de la matriz de covarianzas. Sin embargo, esta estimacin tan slo afectar a
la parte del estado correspondiente a la posicin del robot, es decir, que del vector de estados
se estimarn los tres primeros elementos, y de la matriz de covarianzas se estimar nicamente
4. Localizacin y Mapeado de forma Simultnea (SLAM) 55
4.2. SLAM con el Filtro Extendido de Kalman
la submatriz 3x3 superior izquierda. A continuacin se recuerda el modelo en tiempo continuo
empleando la velocidad lineal y la curvatura.
x = v sin() (4.5)y = v cos() (4.6)
= v (4.7)
Sin embargo, para poder trabajar con un computador con dicho modelo, es necesario dis-
cretizarlo. Una aproximacin del modelo discreto que rige el desplazamiento del ROMEO, es
la siguiente: xt
yt
t
=xt1
yt1
t1
+ t sin(t1)cos(t1)
v +N (0,Rt) (4.8)
donde Rt es la matriz de covarianza de los errores inducidos debido a desviaciones en y
v.
El modelo anterior nicamente tiene en cuenta la velocidad (v) y la curvatura (), propor-
cionadas por los codificadores pticos. Sin embargo, hay otro modelo equivalente, en el que
en vez de usarse la curvatura, se usa la velocidad angular proporcionada por el girscopo. La
relacin entre la curvatura y la velocidad angular es la que sigue:
= v (4.9)
Dicho modelo se muestra a continuacin, y no es ms que el anterior, en el que se ha susti-
tuido la curvatura por la velocidad angular.xt
yt
t
=xt1
yt1
t1
+ tv sin(t1)v cos(t1)
+N (0,Rt) (4.10)
En este caso, Rt es la matriz de covarianza de los errores inducidos debido a desviaciones
en la velocidad angular () y en la velocidad (v).
4. Localizacin y Mapeado de forma Simultnea (SLAM) 56
4.2. SLAM con el Filtro Extendido de Kalman
El girscopo que lleva incorporado el ROMEO es mucho ms estable que los codificadores
pticos, por lo que, siempre que ambos sensores estn disponibles, se emplear el segundo de
los modelos vistos. Sin embargo, si las lecturas del girscopo no estuvieran accesibles, habra
que emplear el primer modelo, en el que se tena en cuenta a la curvatura en vez de a la velocidad
angular.
Las ecuaciones que permiten estimar el vector de estados y la matriz de covarianzas son las
que se vieron desde el paso 2 al 5 de la tabla 4.1.
En primer lugar hay que obtener la matriz Fx. La funcin de esta matriz es mapear un vector
de dimensin 3, en uno de dimensin 3 + 2Nt. Esto es necesario porque la dimensin del vector
de estados se incrementa en 2 cada vez que se incrementa Nt en 1 unidad (se ha detectado una
nueva caracterstica en el mapa, que en este caso sern rectas).
Por tanto, Fx puede definirse como,
Fx =
1 0 0 0 00 1 0 0 00 0 1 0 0
2Nt
(4.11)
El resto de los pasos que quedan hasta llegar al 5 dependern del modelo que se emplee. En
los siguientes subapartados se derivarn las ecuaciones de la etapa de Prediccin considerando
ambos casos.
4.2.3.1. Ecuaciones predictivas considerando la curvatura
En esta subseccin se vern las ecuaciones de la etapa de Prediccin teniendo en cuenta que
se emplea el modelo del ROMEO en el que intervienen la velocidad y la curvatura.
4. Localizacin y Mapeado de forma Simultnea (SLAM) 57
4.2. SLAM con el Filtro Extendido de Kalman
La ecuacin que permite proyectar el estado hacia adelante es la siguiente:
xt = xt1 + FTx
v t sin(t1)v t cos(t1)
v t
(4.12)
Cabe destacar que en la tabla 4.1 slo aparecen las ecuaciones para el caso del girscopo ya
que es lo que mejores resultados da.
Para obtener la matriz Gt (Paso 4) es necesario calcular antes el Jacobiano de la expresin
anterior con respecto al estado. Dicho Jacobiano es,1 0 v t cos(t1)0 1 v t sin(t1)0 0 1
(4.13)
Para que Gt tenga las dimensiones apropiadas, habr que introducir la matriz Fx y manipu-
lar un poco la ecuacin anterior, obteniendo la ecuacin del Paso 4:
Gt = I + FTx
0 0 v t cos(t1)0 0 v t sin(t1)0 0 0
Fx (4.14)
Ya slo queda el Paso 5 para completar la etapa de Prediccin, que viene dado por la si-
guiente ecuacin:
Pt = GtPt1GTt + FTxRtFx (4.15)
De esta ecuacin, lo nico que se desconoce es la matriz Rt, que se calcular a continuacin:
Rt = M
v 00
MT (4.16)
La matriz M es el Jacobiano de la expresin 4.12 con respecto a la velocidad y la curvatura.
Los valores de v y se fijan al inicio del algoritmo y representan las desviaciones de la
4. Localizacin y Mapeado de forma Simultnea (SLAM) 58
4.2. SLAM con el Filtro Extendido de Kalman
velocidad y de la curvatura respectivamente.
M =
t sin(t1) 0t cos(t1) 0
t t v
(4.17)
4.2.3.2. Ecuaciones predictivas considerando la velocidad angular
En la subseccin anterior, se vieron las ecuaciones de la etapa de Prediccin considerando
la velocidad lineal y la curvatura. En cambio, en esta subseccin se har lo mismo pero teniendo
en cuenta la velocidad angular en lugar de la curvatura. Las ecuaciones son prcticamente las
mismas salvo pequeos cambios, que se vern a continuacin.
La ecuacin que permite proyectar el estado hacia adelante es la siguiente:
xt = xt1 + FTx
v t sin(t1)v t cos(t1)
t
(4.18)
Para obtener la matriz Gt (Paso 4) es necesario calcular antes el Jacobiano de la expresin
anterior con respecto al estado. Como ni la curvatura ni la velocidad angular dependen del
estado, este Jacobiano ser el mismo en ambos casos. Sin embargo, para tener el desarrollo
completo de la etapa de Prediccin en cada caso, se mostrar de nuevo:1 0 v t cos(t1)0 1 v t sin(t1)0 0 1
(4.19)
Al igual que antes, se introduce Fx en la expresin del clculo de Gt:
Gt = I + FTx
0 0 v t cos(t1)0 0 v t sin(t1)0 0 0
Fx (4.20)
4. Localizacin y Mapeado de forma Simultnea (SLAM) 59
4.2. SLAM con el Filtro Extendido de Kalman
Ya slo queda el Paso 5 para completar la etapa de Prediccin, que viene dado por la si-
guiente ecuacin:
Pt = GtPt1GTt + FTxRtFx (4.21)
Como ocurra en el caso de la curvatura, De esta ecuacin, lo nico que se desconoce es la
matriz Rt, que se calcular a continuacin:
Rt = M
v 00
MT (4.22)
La matriz M es el Jacobiano de la expresin 4.18 con respecto a la velocidad lineal y la
velocidad angular. Los valores de v y se fijan al inicio del algoritmo y representan las
desviaciones de la velocidad lineal y de la velocidad angular respectivamente.
M =
t sin(t1) 0t cos(t1) 0
0 t
(4.23)
4.2.4. Actualizacin
Una vez concluida la etapa de Prediccin, se pasa a la etapa de Actualizacin. En la etapa de
Actualizacin se emplean las medidas del lser para poder determinar si el ROMEO ha pasado
ya por un sitio determinado o no, y as poder corregir la posicin y el mapa en consecuencia.
Sin embargo, para poder acometer esta etapa, es necesario tener medidas del lser.
Como el tiempo que tarda el lser en suministrar un mapa del entorno frontal cercano es
mayor que el tiempo que tarda en ejecutarse la etapa de Prediccin, lo normal es que la etapa
de Prediccin finalice mucho antes de que haya un mapa de medidas disponibles por parte del
lser. Lo que se hace para no estar esperando, es continuar repitiendo la etapa de Prediccin
tantas veces como sea necesario hasta que el lser pueda proporcionar unas medidas. Una vez
aclarado este punto, se puede proceder a explicar la etapa de Actualizacin.
4. Localizacin y Mapeado de forma Simultnea (SLAM) 60
4.2. SLAM con el Filtro Extendido de Kalman
4.2.4.1. Extraccin de caractersticas
El lser que hay instalado en el ROMEO, lo que hace es generar una serie de puntos en
un espacio bidimensional, que representan la distancia a un obstculo para un valor de ngulo
determinado. Ya se ha comentado que para el algoritmo de SLAM es necesario obtener unas
caractersticas del entorno. Dichas caractersticas pueden ser puntos, paredes, esquinas, etc.
Las cualidades que debe tener una buena caracterstica son las siguientes:
Las caractersticas deben ser fcilmente reobservables.
Las caractersticas deberan ser distinguibles individualmente unas de otras.
Las caractersticas deben encontrarse en un gran nmero en el entorno por el que se va a
desplazar el robot.
Las caractersticas deben ser estticas, ya que si una caracterstica fuera mvil, no se
encontrara siempre en el mismo lugar, por lo que el robot, dada dicha caracterstica, no
sabra situarse.
En el presente proyecto, las caractersticas que se emplearn sern rectas. Por esto mismo,
y antes de empezar con la etapa de Actualizacin propiamente dicha, hay que extraer lneas
(paredes) del conjunto de puntos que ha proporcionado el lser. En el captulo anterior se vio
el algoritmo empleado para extraer las rectas mencionadas, por lo que se obviar en el presente
captulo.
Las rectas que se obtienen con el algoritmo de extraccin de rectas, vienen definidas por
su pendiente (m) y el valor de la ordenada en el origen (a). Sin embargo, para el algoritmo de
SLAM se necesita que dichas rectas vengan expresadas en funcin de la distancia que hay hasta
el origen local del vehculo () y del ngulo que forma el eje X del sistema de coordenadas
local, con la perpendicular a la recta que pasa por el origen (). Estos parmetros se pueden ver
grficamente en la figura 4.9. La ecuacin de la recta empleando estos dos ltimos parmetros,
4. Localizacin y Mapeado de forma Simultnea (SLAM) 61
4.2. SLAM con el Filtro Extendido de Kalman
(, ) quedara de la siguiente forma:
= x cos() + y sin() (4.24)
Figura 4.9: Parmetros de una recta
A continuacin se ver como obtener dichos parmetros a partir de los parmetros originales
que proporciona el algoritmo de extraccin de rectas, (m, a).
4.2.4.1.1. Obtencin de la distancia a una recta
Sea un punto P del plano expresado en coordenadas cartesianas, P (xp, yp), y sea una recta
expresada por su ecuacin general, R A x + B y + C = 0. Entonces, la expresin queproporciona la distancia desde dicho punto P a la recta R es la siguiente:
=|A xp +B yp + C|
A2 +B2(4.25)
Ahora hay que transformar la ecuacin de la recta que proporciona el algoritmo de extrac-
cin a la forma general que utiliza la ecuacin anterior.
y = m x+ a (4.26)
4. Localizacin y Mapeado de forma Simultnea (SLAM) 62
4.2. SLAM con el Filtro Extendido de Kalman
m x y + a = 0 (4.27)
A = m (4.28)
B = 1 (4.29)
C = a (4.30)
Como ya se conocen los parmetros necesarios de la ecuacin general de la recta, tan slo
falta conocer las coordenadas (xp, yp) del punto desde el que se calcular la distancia. Como se
quiere obtener la distancia al origen de coordenadas, se tendr que xp = 0 y que yp = 0. Por
tanto, la ecuacin que da la distancia de una recta al origen de coordenadas es la siguiente:
=|a|m2 + 1
(4.31)
4.2.4.1.2. Obtencin del ngulo entre el eje de abscisas y la normal a la recta
La obtencin del ngulo que forman la perpendicular a la recta que pasa por el origen
con el eje de abscisas es inmediata sin ms que aplicar un poco de geometra. La expresin que
lo proporciona es:
= arctan(1m
) (4.32)
Sin embargo, en algunos casos la expresin anterior no proporciona el ngulo exacto, sino
que puede haber un desfase de pi radianes. Por eso mismo, se detallarn a continuacin todas
las clases posibles de rectas y cules seran sus ngulos.
Recta con m > 0 y a > 0: Esta recta se muestra en la figura 4.10 y, como se puede ob-
servar, el ngulo que forma la normal a la recta con el eje X se encontrara en el segundo
cuadrante. Pero se puede comprobar como la expresin 4.32 proporciona el ngulo entre
la prolongacin de la normal y el eje X (al que se ha llamado en la figura), situado en
el cuarto cuadrante.
Para pasar del ngulo en el cuarto cuadrante al del segundo cuadrante, tan slo hay que
aadir pi radianes al ngulo .
4. Localizacin y Mapeado de forma Simultnea (SLAM) 63
4.2. SLAM con el Filtro Extendido de Kalman
Figura 4.10: ngulo para una recta con m > 0 y a > 0
Recta con m < 0 y a > 0: Esta recta se muestra en la figura 4.11 y, como se puede ve
en la misma, el ngulo que forma la normal a la recta con el eje X se encontrara en el
primer cuadrante. En este caso, el ngulo que proporcionara la expresin 4.32 (), s que
coincide con el ngulo deseado entre la normal a la recta y el eje de abscisas, por lo que
no habr que aadirle nada ms.
Recta con m > 0 y a < 0: Esta recta se muestra en la figura 4.12 y, como observa en
ella, el ngulo que forma la normal a la recta con el eje X se encontrara en el cuarto
cuadrante. Al igual que en el caso anterior, el ngulo coincide con el deseado, as que
no ser necesaria ninguna modificacin del mismo.
Recta con m < 0 y a < 0: Esta recta se muestra en la figura 4.13 y, como se puede ver, el
ngulo que forma la normal a la recta con el eje X se encontrara en el tercer cuadrante.
Sin embargo, se puede comprobar como la expresin 4.32 proporciona el ngulo entre la
prolongacin de la normal y el eje de abscisas (), situado en el primer cuadrante.
Para pasar del ngulo en el primer cuadrante al del tercer cuadrante, tan slo hay que
4. Localizacin y Mapeado de forma Simultnea (SLAM) 64
4.2. SLAM con el Filtro Extendido de Kalman
Figura 4.11: ngulo para una recta con m < 0 y a > 0
sustraer pi radianes al ngulo .
Este detalle es de suma importancia, ya que si no se hubiera tenido en cuenta, hubiera
ocasionado una mala interpretacin de las rectas extradas, provocando una mala asociacin de
las mismas, y por consiguiente, un mal funcionamiento del algoritmo.
4.2.4.1.3. Consideraciones adicionales de la extraccin de caractersticas
Puede ocurrir que el lser vaya detectando una pared y, de repente, se encuentre una puerta
abierta, y a continuacin siga con la misma pared. Las rectas detectadas a ambos lados de la
puerta, como una es prolongacin de la otra, tendrn los mismos parmetros. Sin embargo, el
algoritmo tiene que saber que se trata de dos rectas diferentes, y no de la misma. Lo mismo
ocurrira cuando el lser va detectando una pared poco a poco, y cada recta que detecte sera
una prolongacin de la anterior.
4. Localizacin y Mapeado de forma Simultnea (SLAM) 65
4.2. SLAM con el Filtro Extendido de Kalman
Figura 4.12: ngulo para una recta con m > 0 y a < 0
Por las razones ya comentadas, se hace necesario que, adems de almacenar los parmetros
(, ), se almacenen tambin los puntos extremos de las rectas. Adems se obtendr tambin su
longitud, ya que tambin puede ser necesaria ms adelante.
4.2.4.2. Asociacin de datos
Una vez se han extrado las caractersticas que ha detectado el lser, es necesario saber si
dichas caractersticas son nuevas o si ya se haban detectado anteriormente.
Los problemas que pueden surgir en el proceso de asociacin de datos son los siguientes:
En cada paso no tienen por qu reobservarse caractersticas.
Se puede observar algo como una caracterstica y no volver a observarlo nunca ms.
Se puede asociar errneamente una caracterstica a una detectada previamente.
4. Localizacin y Mapeado de forma Simultnea (SLAM) 66
4.2. SLAM con el Filtro Extendido de Kalman
Figura 4.13: ngulo para una recta con m < 0 y a < 0
Para ello, en primer lugar habr que expresar ambas rectas en el mismo sistema de co-
ordenadas. Esto es as porque el lser siempre proporcionar medidas respecto al sistema de
coordenadas local del ROMEO. Sin embargo, debe haber un sistema de coordenadas global en
el que se site al ROMEO y a todas las rectas detectadas, para ir construyendo un mapa del te-
rreno. Este sistema de coordenadas global tendr su centro en el lugar desde donde el ROMEO
comenz su trayectoria.
Una vez expresadas todas las rectas (las detectadas anteriormente y las actuales) en el mismo
marco de referencia, lo que se hace es aplicar la distancia de Mahalanobis para ver la similitud
de las mismas.
Finalmente, se realiza la asociacin propiamente dicha, donde se establece una correspon-
dencia entre las rectas nuevas y las antiguas y donde se indican tambin las rectas nuevas que no
se corresponden con las antiguas, por lo que seran nuevas caractersticas a aadir en el mapa.
En los siguientes apartados se ver paso a paso todo el proceso de asociacin de datos.
4. Localizacin y Mapeado de forma Simultnea (SLAM) 67
4.2. SLAM con el Filtro Extendido de Kalman
4.2.4.2.1. Paso de parmetros expresados en un marco de referencia global a uno local
Lo que se har en este apartado ser obtener una estimacin de los parmetros de las rectas
que ya se haban detectado anteriormente (expresadas en un marco de referencia global) en el
sistema de referencia centrado en el robot.
Figura 4.14: Parmetros de una recta en un marco de referencia local y otro global
En la figura 4.14 se muestran todos los parmetros que intervendrn en dicho cambio de
sistemas de referencia, que se explicarn a continuacin:
Ejes coordenados X e Y : Son los ejes coordenados del sistema de referencia global. Su
origen se encuentra en la posicin desde la que parti el ROMEO.
Ejes coordenados dentro de los ejes X , Y : Son los ejes coordenados del sistema de
4. Localizacin y Mapeado de forma Simultnea (SLAM) 68
4.2. SLAM con el Filtro Extendido de Kalman
referencia local. Su origen de coordenadas respecto al sistema de coordenadas global est
localizado en la posicin actual del vehculo, (x(k), y(k)). El ngulo que forman ambos
ejes de abscisas viene dado por la orientacin actual del vehculo ((k)).
rk: Distancia que hay entre los orgenes de coordenadas de los dos sistemas de referencia,
el local y el global. Su obtencin es inmediata aplicando la siguiente expresin:
rk =x2(k) + y2(k) (4.33)
k: ngulo que forma la recta que une los dos orgenes de coordenadas de los sistemas
de referencia con el eje de abscisas del sistema de coordenadas global. Se puede obtener
de la siguiente forma:
k = arctan(y(k)
x(k)) (4.34)
i: Longitud del segmento normal a la recta i que pasa por el origen de coordenadas local
y que comienza en la recta y termina en dicho origen de coordenadas.
i: ngulo entre el eje de abscisas del sistema de coordenadas local y el segmento normal
a la recta i que pasa por el origen de coordenadas.
ri: Longitud del segmento normal a la recta i que pasa por el origen de coordenadas global
y que comienza en la recta y termina en dicho origen de coordenadas.
i: ngulo entre el eje de abscisas del sistema de coordenadas global y el segmento
normal a la recta i que pasa por el origen de coordenadas.
Sin ms que aplicar un poco de geometra, se puede obtener la posicin predicha de las
rectas en un marco de referencia local al vehculo.
zi =
ii
=ri rk cos(i k)
i (k)
(4.35)
Sin embargo, esta ltima frmula, aunque es vlida para la situacin que aparece en la figura
4.14, no siempre ser as, pudiendo dar resultados de i menores que cero, y valores incorrectos
4. Localizacin y Mapeado de forma Simultnea (SLAM) 69
4.2. SLAM con el Filtro Extendido de Kalman
de i. Esto ocurre cuando se produce una interseccin entre la recta detectada y el segmento
que une los dos orgenes de coordenadas de los dos sistemas de referencia [7]. En este ltimo
caso, la frmula que habra que emplear para obtener una estimacin de los parmetros de la
recta sera la siguiente:
zi =
ii
=ri + rk cos(i k)
i (k) + pi
(4.36)
ste es un detalle importante y habr que tenerlo en cuenta en otras estimaciones posteriores
y en los Jacobianos que se calculen ms adelante.
4.2.4.2.2. Obtencin de la distancia de Mahalanobis
Para ver cunto de parecido tienen dos rectas (la estimada y la obtenida por el lser), se
puede emplear la distancia de Mahalanobis. La distancia de Mahalanobis es una distancia basa-
da en la correlacin de variables y es muy til para determinar la similitud de una muestra
desconocida con una conocida. Su valor al cuadrado se puede determinar haciendo uso de la
siguiente expresin:
D2M = (z z)T1k (z z) (4.37)
donde,
z =
(4.38)
z =
(4.39)y k es la matriz de covarianzas para z.
Dado que tanto z como z son conocidos, es necesario obtener k, que viene dada por la
siguiente expresin:
k = HkPtHTk + Qt (4.40)
4. Localizacin y Mapeado de forma Simultnea (SLAM) 70
4.2. SLAM con el Filtro Extendido de Kalman
En la ecuacin anterior, Qt es la matriz de ruido de la observacin, Hk es el Jacobiano
de la estimacin con respecto al estado del vehculo y a la observacin, y Pt es la matriz de
covarianzas del estado del ROMEO. Como los valores de Q y P son conocidos, se proceder al
clculo de Hk, con lo que ya se podra obtener la distancia de Mahalanobis.
Antes de proceder al clculo del Jacobiano, hay que aclarar que sus dimensiones seran 2x5,
mientras que las dimensiones de la matriz P son (3 + 2Nt)x(3 + 2Nt). Por tanto, para que
cuadren las dimensiones de ambas matrices, habr que mapear el Jacobiano (que en adelante
se llamar hk) en una matriz de dimensin mayor (Hk). Esta ltima matriz tendr los valores
del Jacobiano distribuidos de forma que afecten nicamente a la recta estimada que se est
considerando.
Para mapear hk en Hk, hay que introducir una nueva matriz, llamada Fx,k, y con la siguiente
forma:
Fx,k =
1 0 0 0 0 0 0 0 00 1 0 0 0 0 0 0 00 0 1 0 0 0 0 0 00 0 0 0 0 1 0 0 00 0 0 0 0
2k20 1 0 0
2Nt2k
(4.41)
La relacin entre las matrices hk y Hk es
Hk = hkFx,k (4.42)
Como ya se dijo anteriormente, la matriz hk tiene dimensin 2x5 y su forma es:
hk =
hk11 hk12 hk13 hk14 hk15hk21 hk22 hk23 hk24 hk25
(4.43)
Antes de comenzar con el clculo del Jacobiano, hay que sealar que, como se coment
en un apartado anterior, hay dos pares de expresiones que permiten estimar los parmetros de
las rectas en el marco de referencia local, en funcin de si hay interseccin entre la recta y el
4. Localizacin y Mapeado de forma Simultnea (SLAM) 71
4.2. SLAM con el Filtro Extendido de Kalman
segmento que une los orgenes o no. Del mismo modo, y ya que el Jacobiano depende de z,
habr dos Jacobianos posibles.
A continuacin se mostrarn los elementos del Jacobiano supuesto que no hay interseccin
entre la recta y el segmento:
hk11 =ix
=1
rk(x cos(i k) + y sin(i k)) (4.44)
hk12 =iy
= 1rk
(y cos(i k) + x sin(i k)) (4.45)
hk13 =i
= 0 (4.46)
hk14 =iri
= 1 (4.47)
hk15 =ii
= rk sin(i k) (4.48)
hk21 =ix
= 0 (4.49)
hk22 =iy
= 0 (4.50)
hk23 =i
= 1 (4.51)
hk24 =irj
= 0 (4.52)
hk25 =ij
= 1 (4.53)
Si, en cambio, hubiera interseccin entre la recta y el segmento, las expresiones seran las
que siguen:
hk11 =ix
=1
rk(x cos(i k) y sin(i k)) (4.54)
hk12 =iy
=1
rk(y cos(i k) + x sin(i k)) (4.55)
hk13 =i
= 0 (4.56)
hk14 =iri
= 1 (4.57)
4. Localizacin y Mapeado de forma Simultnea (SLAM) 72
4.2. SLAM con el Filtro Extendido de Kalman
hk15 =ii
= rk sin(i k) (4.58)
hk21 =ix
= 0 (4.59)
hk22 =iy
= 0 (4.60)
hk23 =i
= 1 (4.61)
hk24 =irj
= 0 (4.62)
hk25 =ij
= 1 (4.63)
Una vez calculado el Jacobiano, ya se puede conocer la distancia de Mahalanobis al cuadra-
do.
En el siguiente apartado se ver qu criterio se va a seguir para asociar las rectas.
4.2.4.2.3. Asociacin de rectas
Para encontrar cules de las rectas detectadas por el lser en el instante actual son rectas
que ya haban sido detectadas en instantes anteriores, hay que computar las distancias de Ma-
halanobis entre una recta detectada y todas las existentes.
Tambin se hace necesario definir un umbral para la distancia de Mahalanobis. ste viene
definido por la distribucin 2.
Lo que se hace es buscar a las dos rectas cuyas distancias de Mahalanobis a la actual sean
las menores y, adems, que sean menores que el umbral. Si ocurre esto, la recta actual se haba
detectado anteriormente, y se corresponder con una de las dos rectas cuyas distancias de Ma-
halanobis eran menores. Para discernir a qu recta corresponde la recta actual, se computarn
las distancias desde sus extremos a los extremos de las otras dos rectas. Una vez hecho esto, la
recta actual se asociar con aquella recta que est ms cercana a ella misma. Esto se hace porque
4. Localizacin y Mapeado de forma Simultnea (SLAM) 73
4.2. SLAM con el Filtro Extendido de Kalman
puede ocurrir que se asociara una recta con una paralela a la misma, desechando la verdadera
recta con la que se debera corresponder. De esta manera se consigue que la asociacin de rectas
sea ms robusta. En cambio, el coste computacional del algoritmo se ver incrementado, ya que
por cada recta detectada se tiene que realizar el bucle para obtener las distancias de Mahalanobis
dos veces, en vez de una sola.
Si no hubiera ninguna recta que proporcionase un valor de la distancia de Mahalanobis
menor que el primer umbral, se considerara que esa recta no se haba detectado anteriormente,
y dar lugar a una nueva caracterstica del mapa.
A este mtodo de asociacin se le conoce como la asociacin de datos en funcin del vecino
ms cercano y con umbral simple. Hay otros mtodos ms complejos pero que no se han em-
pleado en este proyecto, como el test de compatibilidad conjunta [19] o uno basado en grficos
[1].
Sin embargo, el que se haya asociado a priori con este mtodo una recta detectada con otra
ya existente no quiere decir que en realidad se traten de la misma recta. Lo nico que indica es
que su distancia al origen de coordenadas y el ngulo de su normal son muy parecidos o iguales,
pero tambin es posible que se trate de dos segmentos de recta que pertenezcan a una misma
pared.
Por ello, hay que estudiar todos los casos posibles cuando se asocian dos rectas.
Dos rectas alejadas en el espacio: Si hay dos rectas separas por una distancia apreciable,
debe haber una puerta o abertura entre ellas, por lo que no se deberan asociar. Por tanto,
la recta detectada pasar con las rectas no asociadas, cuyo procedimiento se detallar ms
adelante.
Dos rectas superpuestas parcialmente: Si parte de las rectas se superponen, claramente
es indicativo de que pertenecen a la misma recta. Lo nico que habra que hacer en este
caso, sera actualizar la longitud de la misma y sus extremos.
Dos rectas superpuestas totalmente: Aqu puede haber dos posibilidades.
4. Localizacin y Mapeado de forma Simultnea (SLAM) 74
4.2. SLAM con el Filtro Extendido de Kalman
Que la nueva recta detectada sea ms pequea que la antigua recta y que quede con-
tenida en ella. En este caso, ambas rectas seran la misma, y no habra que modificar
ni la longitud ni los extremos de la misma.
Que la nueva recta sea ms grande que la antigua y que la contenga. En este caso,
habra que actualizar la nueva longitud de la recta, as como la de sus extremos.
En cualquiera de los casos anteriores en los que se asocien dos rectas, hay que tener en cuen-
ta que ambas rectas pueden tener parmetros ligeramente diferentes, aunque sean muy pareci-
dos. Por ello, realmente habra que actualizar dichos parmetros. Sean (r1, 1) los parmetros
de la primera recta, y (r2, 2) los de la segunda, siendo l1 y l2 sus respectivas longitudes.
La forma en que se actualizarn los parmetros ser dndole mayor o menor importancia a
los parmetros de cada recta en funcin de su longitud. Por tanto, los parmetros (r, ) de la
recta resultante sern:
r =r1l1 + r2l2l1 + l2
(4.64)
=1l1 + 2l2l1 + l2
(4.65)
Ahora se estudiar lo que ocurre con las rectas que no se han podido asociar con otras, o
cuya asociacin ha sido descartada posteriormente tras el estudio de sus extremos, como ya se
ha visto.
Para que una recta considerada como nueva se pueda aadir al mapa, tiene que ser detectada
al menos un nmero determinado de veces. Esto es as para evitar que rectas producidas por
medidas errneas o por una mala extraccin de las mismas a partir de las medidas, sean aa-
didas al mapa. Por ello, cada recta no asociada con ninguna otra, se comparar con las rectas
candidatas a ser aadidas. Si se encuentran rectas que coincidan, se incrementar el contador
del nmero de veces que se ha detectado dicha recta. Sino, se aade la recta al conjunto de
rectas candidatas. Cuando el contador supere un cierto umbral, se considerar esa recta como
buena, y se agregar al mapa.
Bsicamente lo que se hace es lo mismo que en la asociacin de datos anterior. Se expresan
4. Localizacin y Mapeado de forma Simultnea (SLAM) 75
4.2. SLAM con el Filtro Extendido de Kalman
todas las rectas en el mismo sistema de coordenadas y se compararan sus parmetros. Aquellas
con parmetros muy similares, se considerarn como la misma recta, mientras que aquellos que
difieran en ellos, se considerarn como rectas diferentes.
Un detalle importante a tener en cuenta es que en grandes recorridos, el nmero de posibles
rectas candidatas a aadirse al mapa puede ser muy elevado, aumentando el coste computacional
del algoritmo en gran medida. Por ello, lo que se puede hacer es eliminar las rectas candidatas
una vez se hayan intentado asociar sin xito un determinado nmero de veces. De esta forma,
se irn eliminando las rectas candidatas ms antiguas. Esto no supone problema alguno en el
algoritmo, ya que si una recta se ha intentado asociar sin xito muchas veces, lo ms probable es
que no se asocie prximamente, porque el robot estar en otra zona donde no podr detectarla.
Si la recta fuera realmente un tramo de pared, cuando el robot revisitara esa zona, se aadira
de nuevo a la lista de candidatas y habra posibilidades de asociarla otra vez.
Una vez se ha establecido la asociacin (o no asociacin) de rectas detectadas por el lser,
se proceder a actualizar el vector de estados y la matriz de covarianzas.
4.2.4.3. Actualizacin de x y P
La actualizacin de x y P tendr lugar tantas veces en una iteracin como rectas detectadas
se hayan asociado a otras existentes.
Esta etapa tan slo consiste en aplicar las ecuaciones que se detallarn a continuacin.
k = HkPtHTk + Qt (4.66)
Esta matriz, tan slo es una matriz auxiliar para simplificar la notacin. A continuacin se
define la ganancia de Kalman como:
K = PtHTk
1k (4.67)
La actualizacin del vector de estados viene dada por
xt = xt + K(zi zi) (4.68)
4. Localizacin y Mapeado de forma Simultnea (SLAM) 76
4.2. SLAM con el Filtro Extendido de Kalman
Y por ltimo, la actualizacin de la matriz de covarianzas la proporciona la siguiente expre-
sin:
Pt = Pt KkKT (4.69)
Como se coment anteriormente, para un conjunto de medidas del lser, se realizar la
etapa de actualizacin tantas veces como rectas se hayan podido asociar a rectas anteriormente
detectadas.
Se puede observar de las ecuaciones anteriores que en la etapa de Actualizacin, adems
de actualizar la posicin y orientacin del robot, tambin se actualizan los parmetros carac-
tersticos de las rectas que se han detectado. Esto implica que los puntos extremos de las rectas
tambin debern variar.
Realmente la variacin de los parmetros no suele ser muy grande, por lo que tampoco es
necesario ser muy riguroso en la actualizacin de los puntos extremos de las rectas, ya que la
variacin no ser apreciable. Una opcin puede ser mantener las coordenadas X de los puntos
extremos (para rectas horizontales) y calcular las coordenadas Y con los nuevos parmetros.
Para las rectas verticales el razonamiento sera anlogo, pero intercambiando las coordenadas
X por las Y . Para otro tipo de rectas podra verse si se asemejan ms a una horizontal o a una
vertical y aplicar el razonamiento anterior.
Otra posibilidad, y que es la que se ha empleado en este proyecto, es la de proyectar los
puntos de la recta antigua sobre los de la nueva recta. Esto puede llevar a errores en funcin de
la medida en que varen los parmetros y de la posicin de las rectas en el plano. Por tanto, lo
que se har es construir varias rectas intermedias con parmetros comprendidos entre la original
y la final. Una vez hecho esto, se proyectarn los puntos extremos de la recta inicial en la primera
de las rectas auxiliares. A continuacin, se proyectarn los puntos extremos de la primera recta
auxiliar sobre la segunda recta. Y se repite el proceso sucesivamente hasta que se alcance la
recta objetivo.
4. Localizacin y Mapeado de forma Simultnea (SLAM) 77
4.2. SLAM con el Filtro Extendido de Kalman
4.2.4.4. Actualizacin por GPS
Tal y como se describi en un captulo anterior, el ROMEO lleva incorporado un dispositivo
GPS, con el que puede localizarse de forma bastante fiable si est operando en modo diferencial.
Por ello, y para reducir los errores, se pueden emplear las medidas que suministra el GPS
para actualizar la posicin del ROMEO.
El proceso de actualizar con medidas GPS es mucho ms simple que el que se hizo con las
medidas del lser. En este caso, la medida sera:
z =
XGPSYGPS
(4.70)
El Jacobiano a calcular esta vez, H, sera tambin ms sencillo que los que se han calculado
anteriormente, siendo el siguiente:
H =
1 0 0 00 1 0 0
(4.71)
Ya se pueden aplicar de nuevo las ecuaciones de actualizacin que, aunque son muy simi-
lares a las del apartado anterior, se mostrarn a continuacin para facilitar su entendimiento.
Esta etapa tan slo consiste en aplicar las ecuaciones siguientes.
t = HtPtHTt + QGPSt (4.72)
K = PtHTk
1t (4.73)
La actualizacin del vector de estados esta vez viene dada por
xt = xt + K
XGPS xt(1)YGPS xt(2)
(4.74)
Finalmente, la actualizacin de la matriz de covarianzas queda como antes
Pt = Pt KtKT (4.75)
4. Localizacin y Mapeado de forma Simultnea (SLAM) 78
4.2. SLAM con el Filtro Extendido de Kalman
La matriz QGPSt que aparece en la expresin 4.72 es la matriz de ruido del GPS, es decir,
indica la confiabilidad que se tiene de la certeza de las medidas que proporciona el GPS, para
darles mayor o menor peso en la actualizacin.
QGPSt =
XGPS 00 YGPS
(4.76)
Figura 4.15: Trayectoria del ROMEO segn odometra (en rojo) y segn el GPS (en verde)
Esta ltima matriz es importante, ya que puede haber zonas en la que las medidas GPS
no sean fiables, por lo que no habr que darles demasiada importancia, sino ms bien, todo
lo contrario. En la figura 4.15 se muestran dos trayectorias descritas por el ROMEO, siendo
la trayectoria en rojo una estimacin de la odometra, y la trayectoria azul una lectura de las
posiciones GPS a lo largo del tiempo. Como se conoce el recorrido que se hizo con el ROMEO,
se puede afirmar que al comienzo del mismo las medidas GPS son bastante fiables. Sin embargo,
al final del recorrido la calidad de las medidas es mucho peor, ya que el ROMEO describi un
cuadrado y ah no aparece como tal. La trayectoria estimada con la odometra nicamente,
4. Localizacin y Mapeado de forma Simultnea (SLAM) 79
4.2. SLAM con el Filtro Extendido de Kalman
aunque no sea la exacta, se asemeja bastante a la que describi el ROMEO realmente, ya que
el girscopo es bastante fiable. La elipse azul que aparece al final de la trayectoria en rojo,
nicamente representa a la incertidumbre que hay en la posicin que ocupa el ROMEO. Esto se
tratar en mayor profundidad en el siguiente captulo.
Para finalizar con la actualizacin del GPS queda destacar que para que la actualizacin se
pueda llevar a cabo correctamente, antes hay que hacer coincidir los sistemas de referencia del
GPS y el sistema global de referencia en el que se construir el mapa. O lo que es lo mismo,
en el instante inicial el origen del sistema de referencia del GPS tiene que coincidir con el del
ROMEO, y lo mismo con sus ejes de abscisas.
Por ello, para hacer coincidir los orgenes de los dos sistemas coordenados, bastar con
realizar una traslacin de uno sobre el otro. Por ejemplo, sea (XGPS0 , YGPS0) la posicin del
ROMEO en el instante inicial proporcionada por el GPS. Lo que habra que hacer, sera tomar
dicha posicin como la (0, 0). Al resto de medidas que suministrara el GPS, habra que sus-
traerles la cantidad inicial, para que los orgenes de ambos sistemas de referencia coincidieran.
Por ejemplo, la posicin GPS del ROMEO en el sistema de coordenadas global en el instante t
sera (XGPSt XGPS0 , YGPSt YGPS0).
Habiendo trasladado los ejes del GPS no es suficiente, ya que stos tendrn probablemente
una orientacin diferente a la que tienen los ejes coordenados del sistema de referencia global.
Esto se puede hacer empleando las dos o tres primeras medidas de la odometra y del GPS. Lo
que se hara sera unir el origen de coordenadas global con la posicin estimada por la odometra
en un instante t (siendo t un valor pequeo), y obtener el ngulo que formara ese segmento con
el eje X . El procedimiento para el GPS sera el mismo, salvo que con el segmento que uniera el
origen del sistema del GPS con la medida que proporcionara el mismo en el instante t. Tan slo
restara ver la diferencia entre el ngulo de ambas rectas con sus respectivos ejes de ordenadas
para conocer el giro que habra que hacer en uno de los sistemas de referencia (). Como t
debe ser pequeo, el ROMEO no habr recorrido mucha distancia, por lo que podra iniciarse
nuevamente el algoritmo con el siguiente vector de estados inicial (en este caso es ms sencillo
4. Localizacin y Mapeado de forma Simultnea (SLAM) 80
4.2. SLAM con el Filtro Extendido de Kalman
rotar los ejes del ROMEO que los del GPS).
x0 =
0
0
(4.77)
Una vez tenidas en cuenta estas consideraciones iniciales, se podra aplicar el algoritmo que
se ha visto hasta ahora sin efectuar ninguna modificacin ms.
Por ltimo, y para concluir el algoritmo de SLAM, tan slo queda la etapa de aumento del
vector de estado y de la matriz de covarianzas, que se ver en el siguiente apartado.
4.2.4.5. Aumento de x y P
Cada vez que se quiere aadir una nueva recta al mapa, hay que aumentar la dimensin tanto
del vector de estado, como de la matriz de covarianzas. Se entiende como nueva recta, aquella
que ya se ha detectado un nmero determinado de veces y que no coincide con ninguna anterior,
como ya se coment anteriormente. Las rectas que sin coincidir con ninguna anterior y que no
se han detectado un nmero mnimo de veces, no se tendrn en cuenta en la etapa de aumento.
Simplemente se almacenarn hasta que se hayan repetido el nmero suficiente de veces como
para que se consideren como vlidas y sean aadidas al mapa.
El proceso de aumentar el vector de estados es muy simple. Lo nico que hay que hacer es
aadir al final el valor de la distancia a la recta y el ngulo que forma el eje de abscisas con su
normal que pasa por el origen, todo ello en un marco de referencia global.
4. Localizacin y Mapeado de forma Simultnea (SLAM) 81
4.2. SLAM con el Filtro Extendido de Kalman
xt =
x(k)
y(k)
(k)
...
...
ri
i
(4.78)
Por tanto, lo nico que hay que hacer para poder aumentar el vector de estados es trans-
formar los parmetros de la recta expresados en un sistema de referencia local, en uno global.
Es el proceso inverso al que se emple en la seccin de asociacin de datos. Para recordar los
diferentes parmetros que aparecern en las ecuaciones siguientes, se muestra de nuevo la figura
4.16.
Empleando un poco de geometra se llega a que,rii
=i + rk cos((k) + i k)
(k) + i
(4.79)
Como antes, la ltima ecuacin slo es vlida si no hubiese interseccin entre la unin de los
orgenes de los sistemas coordenados y la propia recta. En caso contrario, la frmula a emplear
sera la siguiente: rii
=i rk cos((k) + i k)
(k) + i pi
(4.80)Tan slo queda ahora aumentar la matriz de covarianzas. Sin embargo, este proceso no es
tan simple como el anterior, ya que habr que aadir dos filas y dos columnas a la misma, siendo
necesario calcular algn Jacobiano, que se detallar a continuacin.
En primer lugar, se calcular el Jacobiano de (ri, i)T , Jx, respecto del vector de estados,
4. Localizacin y Mapeado de forma Simultnea (SLAM) 82
4.2. SLAM con el Filtro Extendido de Kalman
Figura 4.16: Parmetros de una recta en un marco de referencia local y otro global
[x(k), y(k), (k)], suponiendo que no hay interseccin:
Jx =
Jx(1, 1) Jx(1, 2) Jx(1, 3)Jx(2, 1) Jx(2, 2) Jx(2, 3)
(4.81)
Jx(1, 1) =rix(k)
=1
rk(x(k) cos((k) + i k) y(k) sin((k) + i k)) (4.82)
Jx(1, 2) =riy(k)
=1
rk(y(k) cos((k) + i k) + x(k) sin((k) + i k)) (4.83)
Jx(1, 3) =ri(k)
= rk sin((k) + i k) (4.84)
Jx(2, 1) =ix(k)
= 0 (4.85)
Jx(2, 2) =iy(k)
= 0 (4.86)
4. Localizacin y Mapeado de forma Simultnea (SLAM) 83
4.2. SLAM con el Filtro Extendido de Kalman
Jx(2, 3) =i(k)
= 1 (4.87)
A continuacin, es necesario calcular tambin el Jacobiano de (ri, i)T , Jz, respecto de la
observacin, [i, i], suponiendo nuevamente que no hay interseccin:
Jz =
Jz(1, 1) Jz(1, 2)Jz(2, 1) Jz(2, 2)
(4.88)
Jz(1, 1) =rii
= 1 (4.89)
Jz(1, 2) =rii
= rk sin((k) + i k) (4.90)
Jz(2, 1) =ii
= 0 (4.91)
Jz(2, 2) =ii
= 1 (4.92)
Si hubiera interseccin, los dos Jacobianos calculados quedaran de la siguiente forma:
Jx =
Jx(1, 1) Jx(1, 2) Jx(1, 3)Jx(2, 1) Jx(2, 2) Jx(2, 3)
(4.93)
Jx(1, 1) =rix(k)
= 1rk
(x(k) cos((k) + i k) y(k) sin((k) + i k)) (4.94)
Jx(1, 2) =riy(k)
= 1rk
(y(k) cos((k) + i k) + x(k) sin((k) + i k)) (4.95)
Jx(1, 3) =ri(k)
= rk sin((k) + i k) (4.96)
Jx(2, 1) =ix(k)
= 0 (4.97)
Jx(2, 2) =iy(k)
= 0 (4.98)
Jx(2, 3) =i(k)
= 1 (4.99)
Jz =
Jz(1, 1) Jz(1, 2)Jz(2, 1) Jz(2, 2)
(4.100)4. Localizacin y Mapeado de forma Simultnea (SLAM) 84
4.2. SLAM con el Filtro Extendido de Kalman
Jz(1, 1) =rii
= 1 (4.101)
Jz(1, 2) =rii
= rk sin((k) + i k) (4.102)
Jz(2, 1) =ii
= 0 (4.103)
Jz(2, 2) =ii
= 1 (4.104)
A continuacin se muestra la matriz de covarianzas, siendo las dos ltimas filas y columnas
las que se aaden en este proceso. El resto de la matriz se ha dividido en submatrices para
facilitar el entendimiento de las operaciones siguientes:
Pt =
p11 p12 p13 p14 p1(2i1) p1(2i) p1(2i+1)p21 p22 p23 p24 p2(2i1) p2(2i) p2(2i+1)p31 p32 p33 p34 p3(2i1) p3(2i) p3(2i+1)p41 p42 p43 p44 p4(2i1) p4(2i) p4(2i+1)...
......
.... . .
......
...
p(2i1)1 p(2i1)2 p(2i1)3 p(2i1)4 p(2i1)(2i1) p(2i1)(2i) p(2i1)(2i+1)p(2i)1 p(2i)2 p(2i)3 p(2i)4 p(2i)(2i1) p(2i)(2i) p(2i)(2i+1)p(2i+1)1 p(2i+1)2 p(2i+1)3 p(2i+1)4 p(2i+1)(2i1) p(2i+1)(2i) p(2i+1)(2i+1)
(4.105)
En primer lugar, se aadir la covarianza para la nueva recta que se quiere aadir al mapa
[15]. p(2i)(2i) p(2i)(2i+1)p(2i+1)(2i) p(2i+1)(2i+1)
= Jxp11 p12 p13
p21 p22 p23
p31 p32 p33
JTx + JzQtJTz (4.106)
Tras esto, se aadir la covarianza entre la recta y el ROMEO.
p(2i)1 p(2i)2 p(2i)3p(2i+1)1 p(2i+1)2 p(2i+1)3
= Jxp11 p12 p13
p21 p22 p23
p31 p32 p33
(4.107)
Tambin es necesario aadir la covarianza entre el ROMEO y la recta, que sera la matriz
4. Localizacin y Mapeado de forma Simultnea (SLAM) 85
4.2. SLAM con el Filtro Extendido de Kalman
transpuesta resultante.p1(2i) p1(2i+1)
p2(2i) p2(2i+1)
p3(2i) p3(2i+1)
=p11 p21 p31
p12 p22 p32
p13 p23 p33
JTx = p(2i)1 p(2i)2 p(2i)3p(2i+1)1 p(2i+1)2 p(2i+1)3
T
(4.108)
Finalmente, tan slo queda aadir la covarianza de la nueva recta consigo misma, que otra
vez originan dos matrices, siendo una la transpuesta de la otra.
p(2i)4 p(2i)(2i1)p(2i+1)4 p(2i+1)(2i1)
= Jxp14 p1(2i1)p24 p2(2i1)p34 p3(2i1)
(4.109)
p4(2i) p4(2i+1)...
...
p(2i1)(2i) p(2i1)(2i+1)
=
p14 p24 p34...
......
p1(2i1) p2(2i1) p3(2i1)
JTx =(
p(2i)4 p(2i)(2i1)p(2i+1)4 p(2i+1)(2i1)
)T(4.110)
Y tras esto, el proceso de aumento del vector de estados y de la matriz de covarianzas ha
finalizado.
Podra parecer que ya puede dar comienzo otra iteracin del algoritmo de SLAM. Sin em-
bargo, podra ocurrir lo siguiente. Se han detectado dos rectas paralelas y que pertenecen a la
misma pared, pero separadas por un espacio. A priori se podra pensar que hay una puerta abier-
ta y que en ese espacio no debera haber nada. Sin embargo, si en una iteracin del algoritmo
posterior se detectase que ah hubiera pared, y no una puerta, la recta resultante se asociara
con una de las dos rectas anteriores, prolongando la misma y pudiendo llegar a solaparse con la
otra de las dos. Tras esto, claramente se puede afirmar que las dos rectas anteriores eran parte
de la misma recta. Sin embargo, con el algoritmo que se ha descrito hasta ahora, se seguiran
considerando ambas rectas por separado. Por tanto, habr que ejecutar un nuevo algoritmo en
el que se comparen todas las rectas para fusionar las que se han solapado.
En muchos algoritmos de SLAM, adems de todo lo ya comentado se realiza cada cierto
tiempo una asociacin de datos ms robusta, para tratar de detectar bucles ms grandes. Lo que
ocurrira sera que probablemente se obtendran mejores resultados en algunos experimentos,
4. Localizacin y Mapeado de forma Simultnea (SLAM) 86
4.2. SLAM con el Filtro Extendido de Kalman
pero dicha asociacin de datos se sale del alcance de este proyecto, ya que su objetivo era
montar un algoritmo de SLAM y mostrar la eficacia del mismo.
Una vez aclarado este punto, puede dar comienzo la siguiente iteracin del algoritmo de
SLAM.
4. Localizacin y Mapeado de forma Simultnea (SLAM) 87