4. LocalizaciÃ³n y Mapeado de forma SimultÃ¡nea (SLAM)

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.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.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.


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.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



Figura 4.3: Diagrama de flujo del algoritmo de SLAM



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



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



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



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



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



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.



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



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).



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.



La ecuacin que permite proyectar el estado hacia adelante es la siguiente:

xt = xt1 + FTx


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



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


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)



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.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,



(, ) 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)



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 .



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



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.



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.



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.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



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



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)



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



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)



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



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.



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



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)



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.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)



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,



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



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.



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,



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)



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


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



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,



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. LocalizaciÃ³n y Mapeado de forma SimultÃ¡nea (SLAM)

Documents

Transcript of 4. LocalizaciÃ³n y Mapeado de forma SimultÃ¡nea (SLAM)