Localización y Mapeado de forma Simultánea (SLAM)

Transcripción

1 Capítulo 4 Localización y Mapeado de forma Simultánea (SLAM) 4.1. Introducción SLAM son las siglas de Simultaneous Localization and Mapping, y puede traducirse como Localización y Mapeado Simultáneos. 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, además, es uno de los problemas más importantes en la robótica. Como ya se comentó en la introducción, el problema del SLAM se manifiesta cuando el robot desconoce tanto la posición en que se encuentra, como el mapa de su entorno, teniendo acceso únicamente a las medidas que obtiene de los sensores y a las señales 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 probabilístico, hay dos tipos de SLAM: 42

2 4.1. Introducción 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(x t, m z 1:t, u 1:t ) (4.1) En cada iteración sólo se estimará la última posición del robot. Los subíndices que aparecen en z y u quieren decir que se conocen las señales de control y las observaciones desde el inicio hasta el instante actual (t). La postura tan sólo lleva el subíndice t porque sólo se estimará la postura actual que tiene el ROMEO, no las anteriores. La figura 4.1 muestra gráficamente en qué consiste el online SLAM. La representación de dicha figura se ha hecho empleando redes Bayesianas de correspondencia. Una red Bayesiana (o de creencia) es un grafo dirigido acíclico, que permite representar un conjunto de variables y su independencia probabilística de forma práctica y compacta y que está compuesta por nodos (variables) y arcos dirigidos (significan influencia de un nodo sobre otro). Más información acerca de redes Bayesianas puede encontrarse en [16]. Full SLAM: Esta otra variante de SLAM busca determinar la postura en toda la trayectoria a partir de las acciones de control y las observaciones de los sensores. p(x 1:t, m z 1:t, u 1:t ) (4.2) La figura 4.2 muestra gráficamente (empleando nuevamente una red Bayesiana) en qué consiste el full SLAM. En este caso, y a diferencia del anterior, se puede ver cómo la postura tiene el subíndice desde 1 hasta t, representando que se va a calcular toda la trayectoria a partir de las medidas de los sensores y de las señales de control que han tenido lugar desde que el robot comenzó su movimiento. En la práctica, el cálculo del full SLAM es normalmente inviable, debido a la elevada dimensionalidad del espacio continuo de parámetros y al gran número de características 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 técnica de Graph SLAM ([21], [17]), 4. Localización y Mapeado de forma Simultánea (SLAM) 43

3 4.1. Introducción Figura 4.1: Problema del online SLAM Figura 4.2: Problema del full SLAM que se basa en que la solución se puede ver como un gráfico poco denso, que se puede resolver eficientemente. Esto se traduce en una serie de restricciones cuadráticas que al resolverse dan el mapa y las posturas de máxima probabilidad. El algoritmo de SLAM puede implementarse de numerosas maneras, ya que es más 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 serán explicados en 4. Localización y Mapeado de forma Simultánea (SLAM) 44

4 4.1. Introducción detalle más adelante, son la extracción de características, asociación de datos, estimación del estado y actualizaciones del estado y de las características del entorno 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 complejidad del mismo será directamente proporcional a la complejidad de las ecuaciones que habrá que calcular durante la ejecución del algoritmo. Por ejemplo, no es lo mismo tratar con un vehículo de cuatro ruedas que con un robot humanoide, o con vehículos autónomos submarinos, vehículos autónomos voladores, etc. En este caso se tratará con un vehículo de cuatro ruedas, cuyo modelo se describió en un capítulo anterior. Hay varias opciones para obtener las medidas del entorno: Láser: Los láseres son muy precisos, eficientes, y la salida no requiere demasiado cómputo para procesarla. Sin embargo, si en el entorno del láser 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. Sónar: El sónar era muy empleado hace algunos años, y su precio es mucho más reducido que el de los láseres. Sus medidas no son tan buenas comparadas con las que proporciona el láser y, a menudo, proporcionan lecturas erróneas. Aunque bajo el agua, son la mejor elección y se asemejan al modo de navegar de los delfines. Visión por computador: Esta opción a veces proporciona resultados erróneos debido a los cambios en la luz y a entornos en ausencia de la misma, aunque recientemente se están produciendo muchos avances que palían en cierto modo este problema. A menudo, se emplea un sistema estéreo para tener medidas de profundidad. Las ventajas de emplear 4. Localización y Mapeado de forma Simultánea (SLAM) 45

5 la visión frente al láser o al sónar, es que se asemeja a la visión humana y a que una imagen contiene mucha más información que las lecturas de los otros dos sensores. Además, las cámaras son relativamente baratas, en comparación con los láseres o sónares. 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 láser, ya que, sin ser tan complejo como la visión por computador, proporciona unas medidas lo suficientemente fiables 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 asociación de datos basada en la máxima probabilidad. En el apéndice B se introducirán tanto el Filtro de Kalman como el Filtro Extendido de Kalman. Otra opción al implementar el SLAM es hacerlo con mapas en 2D ó 3D. El láser que se empleará en el algoritmo trabaja en dos dimensiones, por lo que no hay otra opción que trabajar en 2D. Sin embargo, hay técnicas que emplean la visión por computador o láseres 3D, que permiten construir mapas en tres dimensiones [11]. Los mapas en el EKF SLAM están basados en características. Una característica puede ser un punto o una línea por ejemplo. El número de características no debe ser muy grande, ya que el coste computacional sería 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 percepción como para el movimiento del robot. 4. Localización y Mapeado de forma Simultánea (SLAM) 46

6 El Algoritmo General del EKF SLAM El algoritmo general del EKF SLAM se muestra a grandes rasgos en la tabla 4.1. También se puede ver un diagrama de flujo del algoritmo en la figura 4.3, en el que aparecen las funciones más importantes que lo componen. A continuación, se presenta un breve resumen de los pasos que se irán dando en este capítulo, y que complementan a la tabla y al gráfico: 1. Cuando cambia la odometría debido al movimiento del robot, la incertidumbre asociada a la nueva posición del robot se actualiza en el EKF en la etapa de Predicción. 2. Se extraen las características del entorno observables desde la posición actual del robot. 3. El robot intenta asociar las nuevas características a las que ya había detectado anteriormente. 4. Las características re-observadas se emplean para actualizar la posición del robot en el EKF. 5. Las características nuevas, se añaden al EKF como nuevas observaciones para poder reobservarlas en siguientes iteraciones del algoritmo. En las figuras que van desde la 4.4 hasta la 4.8, se muestran gráficamente los pasos ya comentados. En la figura 4.4, el triángulo representa al robot, mientras que las estrellas representan a las características que el robot ha detectado en su entorno. El robot inicialmente mide la posición de las características 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 características, hay dibujadas unas elipses, que representan la incertidumbre existente en la posición de los mismos. Estas incertidumbres tienen su origen en los errores presentes en los sensores, tanto en 4. Localización y Mapeado de forma Simultánea (SLAM) 47

7 Figura 4.3: Diagrama de flujo del algoritmo de SLAM 4. Localización y Mapeado de forma Simultánea (SLAM) 48

8 Figura 4.4: Situación inicial los que permiten obtener las características (láser), como en los que permiten estimar la posición del robot (codificadores ópticos y giróscopo). La figura 4.5, muestra el movimiento del robot, y cómo éste estima su posición actual empleando la odometría. Al desplazarse el robot, y debido a errores en la odometría, la incertidumbre de la posición del mismo aumenta con respecto a la que tenía en la figura anterior. En la figura 4.6, el robot nuevamente mide la localización de las características empleando sus sensores, pero averigua que no se encuentran en la posición en la que deberían estar (dada la localización actual estimada del robot por la odometría). Por tanto, la posición estimada del robot no es correcta. Como las lecturas del láser son más fiables que las que proporciona la odometría, el robot emplea la posición actual de las características para situarse. La posición corregida viene dada por el triángulo en línea continua, mientras que la posición anterior y que era errónea, viene dada por el triángulo en línea discontinua, como puede verse en la figura 4.7. Si se corrige la posición del robot, es por que se piensa que la posición en la que estaba 4. Localización y Mapeado de forma Simultánea (SLAM) 49

9 Figura 4.5: El robot se mueve a otra posición Figura 4.6: El robot adquiere nuevamente las medidas de su entorno era errónea, y se quiere pasar a una que se acerque más 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 características del mapa, como se puede observar en las 4. Localización y Mapeado de forma Simultánea (SLAM) 50

10 elipses de la figura, que son menores que las que había en figuras anteriores. Figura 4.7: El robot actualiza su posición en función de las nuevas medidas obtenidas con el láser Figura 4.8: Comparativa de la posición real frente a las dos estimadas anteriormente Finalmente, en la figura 4.8 se pueden observar la posición real del robot (representada por 4. Localización y Mapeado de forma Simultánea (SLAM) 51

11 el triángulo con línea continua más sólida), la estimada con odometría (representada por el triángulo con línea discontinua) y la corregida gracias a las medidas del láser (representada por el triángulo con línea continua más suave). Ya que los sensores no son perfectos, la posición real no coincide con la posición corregida al emplear el láser pero se asemeja bastante más que a la estimada empleando la odometría únicamente. Es importante señalar en cualquier punto del algoritmo de SLAM, el robot siempre conocerá la estimación actual de su posición Inicio En primer lugar, y antes de comenzar con el algoritmo de SLAM, es necesario iniciar una serie de variables que se emplearán más adelante. Tales variables pueden ser las matrices de ruido, la longitud mínima de un segmento, el rango del láser, el número de puntos de que consta un barrido del láser, etc. Además, en el instante inicial, el vector de estados y la matriz de covarianzas tendrán el valor nulo. 0 x 0 = 0 (4.3) P 0 = (4.4) Tanto la dimensión del vector de estados como la de la matriz de covarianzas aumentarán en el tiempo, ya que, cada vez que se observe una nueva característica del mapa, ésta se añadirá 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 posición en la que se encuentra el robot con respecto al sistema de coordenadas global. El tercer elemento es 4. Localización y Mapeado de forma Simultánea (SLAM) 52

12 Algoritmo General del EKF SLAM(x t 1, P t 1, u t, z t, N t 1 ): 1.- Actualización del número de características. N t = N t 1 Etapa de Predicción: 2.- Obtención de la matriz F x F x = Proyección del estado hacia adelante. v t sin(θ t 1 ) x t = x t 1 + F T x v t cos(θ t 1 ) ω t 4.- Obtención de la matriz G t. 0 0 v t cos(θ t 1 ) G t = I + F T x 0 0 v t sin(θ t 1 ) Proyección de la covarianza del error hacia adelante. P t = G t P t 1 G T t + F T x R t F x Etapa de Actualización: 6.- Obtención de la matriz de ruido Q t. 7.- Para cada característica observada, z i t = (ρ i t, αt) i T. 8.- Para cada característica del mapa, z k t = ( ρ k t, α t k ) T. 9.- Obtención de la matriz F x,k F x,k = F x 4. Localización y Mapeado de forma Simultánea (SLAM) 53

13 10.- Obtención del Jacobiano de la estimación, H k. H k = h k F x,k 11.- Obtención de la matriz de covarianzas de la estimación, Ψ k. Ψ k = H k P t H T k + Q t 12.- Obtención de la distancia de Mahalanobis al cuadrado, D 2 M. D 2 M = (z i z k ) T Ψ 1 k (z i z k ) 13.- Se busca la mínima distancia de Mahalanobis por debajo de un umbral Fin Para Para esa recta con mínima distancia de Mahalanobis inferior al umbral. K = P t H T k Ψ 1 t Actualización del vector de estados. x t = x t + K(z i z k ) 17.- Actualización de la matriz de covarianzas Fin Para. P t = P t KΨ k K T Si hay disponibles medidas de GPS 20.- Obtención de la matriz de ruido Q GP St Obtención del Jacobiano de la medida, H GP St. H GP St = Nueva etapa de actualización Fin Si. Ψ t = H GP St P t H T GP S t + Q GP St K = P t H T GP S t Ψ 1 t. x t = x t + K X GP S x t (1) Y GP S x t (2) P t = P t KΨ t K T. 4. Localización y Mapeado de forma Simultánea (SLAM) 54

14 24.- Si alguna recta no coincide con ninguna otra, se añade al mapa para compararla posteriormente Aumento del vector de estado, x t 26.- Aumento de la matriz de covarianza, P t 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 orientación hacia cada característica ya detectada Predicción Tal y como se observa en la figura 4.3, la etapa de Predicción es la primera que tiene lugar en el algoritmo de SLAM. En dicha etapa, se estimarán el vector de estados y la matriz de covarianzas, usando tan sólo la odometría. Se conoce como odometría a las técnicas de posicionamiento que emplean la información de sensores que adquieren datos del propio sistema (en este caso, del ROMEO), para obtener una aproximación de la posición real en la que se encuentra el robot en un determinado instante, respecto a un sistema de referencia inicial. Como ya se vio en el capítulo 2, el ROMEO lleva incorporado varios sensores, como unos codificadores ópticos o un giróscopo, que permiten obtener la velocidad lineal, la curvatura, y la velocidad angular. Un buen sistema de odometría sería 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 estimación tanto de el vector de estados como de la matriz de covarianzas. Sin embargo, esta estimación tan sólo afectará a la parte del estado correspondiente a la posición del robot, es decir, que del vector de estados se estimarán los tres primeros elementos, y de la matriz de covarianzas se estimará únicamente 4. Localización y Mapeado de forma Simultánea (SLAM) 55

15 la submatriz 3x3 superior izquierda. A continuación se recuerda el modelo en tiempo continuo empleando la velocidad lineal y la curvatura. ẋ = v sin(θ) (4.5) ẏ = v cos(θ) (4.6) θ = v γ (4.7) Sin embargo, para poder trabajar con un computador con dicho modelo, es necesario discretizarlo. Una aproximación del modelo discreto que rige el desplazamiento del ROMEO, es la siguiente: x t x t 1 sin(θ t 1 ) y t = y t 1 + t cos(θ t 1 ) v + N (0, R t ) (4.8) γ θ t θ t 1 v. donde R t es la matriz de covarianza de los errores inducidos debido a desviaciones en γ y El modelo anterior únicamente tiene en cuenta la velocidad (v) y la curvatura (γ), proporcionadas 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 giróscopo. La relación entre la curvatura y la velocidad angular es la que sigue: ω = v γ (4.9) Dicho modelo se muestra a continuación, y no es más que el anterior, en el que se ha sustituido la curvatura por la velocidad angular. x t x t 1 v sin(θ t 1 ) y t = y t 1 + t v cos(θ t 1 ) + N (0, R t ) (4.10) ω θ t θ t 1 En este caso, R t es la matriz de covarianza de los errores inducidos debido a desviaciones en la velocidad angular (ω) y en la velocidad (v). 4. Localización y Mapeado de forma Simultánea (SLAM) 56

16 El giróscopo que lleva incorporado el ROMEO es mucho más estable que los codificadores ópticos, por lo que, siempre que ambos sensores estén disponibles, se empleará el segundo de los modelos vistos. Sin embargo, si las lecturas del giróscopo no estuvieran accesibles, habría que emplear el primer modelo, en el que se tenía 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 F x. La función de esta matriz es mapear un vector de dimensión 3, en uno de dimensión 3 + 2N t. Esto es necesario porque la dimensión del vector de estados se incrementa en 2 cada vez que se incrementa N t en 1 unidad (se ha detectado una nueva característica en el mapa, que en este caso serán rectas). Por tanto, F x puede definirse como, F x = } 0 {{ 0 } 2N t (4.11) El resto de los pasos que quedan hasta llegar al 5 dependerán del modelo que se emplee. En los siguientes subapartados se derivarán las ecuaciones de la etapa de Predicción considerando ambos casos Ecuaciones predictivas considerando la curvatura En esta subsección se verán las ecuaciones de la etapa de Predicción teniendo en cuenta que se emplea el modelo del ROMEO en el que intervienen la velocidad y la curvatura. 4. Localización y Mapeado de forma Simultánea (SLAM) 57

17 La ecuación que permite proyectar el estado hacia adelante es la siguiente: v t sin(θ t 1 ) x t = x t 1 + F T x v t cos(θ t 1 ) v t γ (4.12) Cabe destacar que en la tabla 4.1 sólo aparecen las ecuaciones para el caso del giróscopo ya que es lo que mejores resultados da. Para obtener la matriz G t (Paso 4) es necesario calcular antes el Jacobiano de la expresión anterior con respecto al estado. Dicho Jacobiano es, 1 0 v t cos(θ t 1 ) 0 1 v t sin(θ t 1 ) (4.13) Para que G t tenga las dimensiones apropiadas, habrá que introducir la matriz F x y manipular un poco la ecuación anterior, obteniendo la ecuación del Paso 4: 0 0 v t cos(θ t 1 ) G t = I + F T x 0 0 v t sin(θ t 1 ) F x (4.14) Ya sólo queda el Paso 5 para completar la etapa de Predicción, que viene dado por la siguiente ecuación: P t = G t P t 1 G T t + F T x R t F x (4.15) De esta ecuación, lo único que se desconoce es la matriz R t, que se calculará a continuación: R t = M σ v 0 M T (4.16) 0 σ γ La matriz M es el Jacobiano de la expresión 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. Localización y Mapeado de forma Simultánea (SLAM) 58

18 velocidad y de la curvatura respectivamente. t sin(θ t 1 ) 0 M = t cos(θ t 1 ) 0 t γ t v (4.17) Ecuaciones predictivas considerando la velocidad angular En la subsección anterior, se vieron las ecuaciones de la etapa de Predicción considerando la velocidad lineal y la curvatura. En cambio, en esta subsección se hará lo mismo pero teniendo en cuenta la velocidad angular en lugar de la curvatura. Las ecuaciones son prácticamente las mismas salvo pequeños cambios, que se verán a continuación. La ecuación que permite proyectar el estado hacia adelante es la siguiente: v t sin(θ t 1 ) x t = x t 1 + F T x v t cos(θ t 1 ) ω t (4.18) Para obtener la matriz G t (Paso 4) es necesario calcular antes el Jacobiano de la expresión 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 Predicción en cada caso, se mostrará de nuevo: 1 0 v t cos(θ t 1 ) 0 1 v t sin(θ t 1 ) (4.19) Al igual que antes, se introduce F x en la expresión del cálculo de G t : 0 0 v t cos(θ t 1 ) G t = I + F T x 0 0 v t sin(θ t 1 ) F x (4.20) Localización y Mapeado de forma Simultánea (SLAM) 59

19 Ya sólo queda el Paso 5 para completar la etapa de Predicción, que viene dado por la siguiente ecuación: P t = G t P t 1 G T t + F T x R t F x (4.21) Como ocurría en el caso de la curvatura, De esta ecuación, lo único que se desconoce es la matriz R t, que se calculará a continuación: R t = M σ v 0 M T (4.22) 0 σ ω La matriz M es el Jacobiano de la expresión 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. t sin(θ t 1 ) 0 M = t cos(θ t 1 ) 0 (4.23) 0 t Actualización Una vez concluida la etapa de Predicción, se pasa a la etapa de Actualización. En la etapa de Actualización se emplean las medidas del láser para poder determinar si el ROMEO ha pasado ya por un sitio determinado o no, y así poder corregir la posición y el mapa en consecuencia. Sin embargo, para poder acometer esta etapa, es necesario tener medidas del láser. Como el tiempo que tarda el láser en suministrar un mapa del entorno frontal cercano es mayor que el tiempo que tarda en ejecutarse la etapa de Predicción, lo normal es que la etapa de Predicción finalice mucho antes de que haya un mapa de medidas disponibles por parte del láser. Lo que se hace para no estar esperando, es continuar repitiendo la etapa de Predicción tantas veces como sea necesario hasta que el láser pueda proporcionar unas medidas. Una vez aclarado este punto, se puede proceder a explicar la etapa de Actualización. 4. Localización y Mapeado de forma Simultánea (SLAM) 60

20 Extracción de características El láser 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 obstáculo para un valor de ángulo determinado. Ya se ha comentado que para el algoritmo de SLAM es necesario obtener unas características del entorno. Dichas características pueden ser puntos, paredes, esquinas, etc. Las cualidades que debe tener una buena característica son las siguientes: Las características deben ser fácilmente reobservables. Las características deberían ser distinguibles individualmente unas de otras. Las características deben encontrarse en un gran número en el entorno por el que se va a desplazar el robot. Las características deben ser estáticas, ya que si una característica fuera móvil, no se encontraría siempre en el mismo lugar, por lo que el robot, dada dicha característica, no sabría situarse. En el presente proyecto, las características que se emplearán serán rectas. Por esto mismo, y antes de empezar con la etapa de Actualización propiamente dicha, hay que extraer líneas (paredes) del conjunto de puntos que ha proporcionado el láser. En el capítulo anterior se vio el algoritmo empleado para extraer las rectas mencionadas, por lo que se obviará en el presente capítulo. Las rectas que se obtienen con el algoritmo de extracción 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 función de la distancia que hay hasta el origen local del vehículo (ρ) 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 parámetros se pueden ver gráficamente en la figura 4.9. La ecuación de la recta empleando estos dos últimos parámetros, 4. Localización y Mapeado de forma Simultánea (SLAM) 61

21 (ρ, α) quedaría de la siguiente forma: ρ = x cos(α) + y sin(α) (4.24) Figura 4.9: Parámetros de una recta A continuación se verá como obtener dichos parámetros a partir de los parámetros originales que proporciona el algoritmo de extracción de rectas, (m, a) Obtención de la distancia a una recta Sea un punto P del plano expresado en coordenadas cartesianas, P (x p, y p ), y sea una recta expresada por su ecuación general, R A x + B y + C = 0. Entonces, la expresión que proporciona la distancia desde dicho punto P a la recta R es la siguiente: ρ = A x p + B y p + C A2 + B 2 (4.25) Ahora hay que transformar la ecuación de la recta que proporciona el algoritmo de extracción a la forma general que utiliza la ecuación anterior. y = m x + a (4.26) 4. Localización y Mapeado de forma Simultánea (SLAM) 62

22 m x y + a = 0 (4.27) A = m (4.28) B = 1 (4.29) C = a (4.30) Como ya se conocen los parámetros necesarios de la ecuación general de la recta, tan sólo falta conocer las coordenadas (x p, y p ) del punto desde el que se calculará la distancia. Como se quiere obtener la distancia al origen de coordenadas, se tendrá que x p = 0 y que y p = 0. Por tanto, la ecuación que da la distancia de una recta al origen de coordenadas es la siguiente: ρ = a m2 + 1 (4.31) Obtención del ángulo entre el eje de abscisas y la normal a la recta La obtención del ángulo que forman la perpendicular a la recta que pasa por el origen con el eje de abscisas es inmediata sin más que aplicar un poco de geometría. La expresión que lo proporciona es: α = arctan( 1 m ) (4.32) Sin embargo, en algunos casos la expresión anterior no proporciona el ángulo exacto, sino que puede haber un desfase de π radianes. Por eso mismo, se detallarán a continuación todas las clases posibles de rectas y cuáles serían sus ángulos. Recta con m > 0 y a > 0: Esta recta se muestra en la figura 4.10 y, como se puede observar, el ángulo que forma la normal a la recta con el eje X se encontraría en el segundo cuadrante. Pero se puede comprobar como la expresión 4.32 proporciona el ángulo entre la prolongación 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 sólo hay que añadir π radianes al ángulo α. 4. Localización y Mapeado de forma Simultánea (SLAM) 63

23 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 encontraría en el primer cuadrante. En este caso, el ángulo que proporcionaría la expresión 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 añadirle nada más. 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 encontraría en el cuarto cuadrante. Al igual que en el caso anterior, el ángulo α coincide con el deseado, así que no será necesaria ninguna modificación 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 encontraría en el tercer cuadrante. Sin embargo, se puede comprobar como la expresión 4.32 proporciona el ángulo entre la prolongación 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 sólo hay que 4. Localización y Mapeado de forma Simultánea (SLAM) 64

24 Figura 4.11: Ángulo para una recta con m < 0 y a > 0 sustraer π radianes al ángulo α. Este detalle es de suma importancia, ya que si no se hubiera tenido en cuenta, hubiera ocasionado una mala interpretación de las rectas extraídas, provocando una mala asociación de las mismas, y por consiguiente, un mal funcionamiento del algoritmo Consideraciones adicionales de la extracción de características Puede ocurrir que el láser vaya detectando una pared y, de repente, se encuentre una puerta abierta, y a continuación siga con la misma pared. Las rectas detectadas a ambos lados de la puerta, como una es prolongación de la otra, tendrán los mismos parámetros. Sin embargo, el algoritmo tiene que saber que se trata de dos rectas diferentes, y no de la misma. Lo mismo ocurriría cuando el láser va detectando una pared poco a poco, y cada recta que detecte sería una prolongación de la anterior. 4. Localización y Mapeado de forma Simultánea (SLAM) 65

25 Figura 4.12: Ángulo para una recta con m > 0 y a < 0 Por las razones ya comentadas, se hace necesario que, además de almacenar los parámetros (ρ, α), se almacenen también los puntos extremos de las rectas. Además se obtendrá también su longitud, ya que también puede ser necesaria más adelante Asociación de datos Una vez se han extraído las características que ha detectado el láser, es necesario saber si dichas características son nuevas o si ya se habían detectado anteriormente. Los problemas que pueden surgir en el proceso de asociación de datos son los siguientes: En cada paso no tienen por qué reobservarse características. Se puede observar algo como una característica y no volver a observarlo nunca más. Se puede asociar erróneamente una característica a una detectada previamente. 4. Localización y Mapeado de forma Simultánea (SLAM) 66

26 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 coordenadas. Esto es así porque el láser siempre proporcionará medidas respecto al sistema de coordenadas local del ROMEO. Sin embargo, debe haber un sistema de coordenadas global en el que se sitúe al ROMEO y a todas las rectas detectadas, para ir construyendo un mapa del terreno. 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 asociación propiamente dicha, donde se establece una correspondencia entre las rectas nuevas y las antiguas y donde se indican también las rectas nuevas que no se corresponden con las antiguas, por lo que serían nuevas características a añadir en el mapa. En los siguientes apartados se verá paso a paso todo el proceso de asociación de datos. 4. Localización y Mapeado de forma Simultánea (SLAM) 67

27 Paso de parámetros expresados en un marco de referencia global a uno local Lo que se hará en este apartado será obtener una estimación de los parámetros de las rectas que ya se habían detectado anteriormente (expresadas en un marco de referencia global) en el sistema de referencia centrado en el robot. Figura 4.14: Parámetros de una recta en un marco de referencia local y otro global En la figura 4.14 se muestran todos los parámetros que intervendrán en dicho cambio de sistemas de referencia, que se explicarán a continuación: Ejes coordenados X e Y : Son los ejes coordenados del sistema de referencia global. Su origen se encuentra en la posición desde la que partió el ROMEO. Ejes coordenados dentro de los ejes X, Y : Son los ejes coordenados del sistema de 4. Localización y Mapeado de forma Simultánea (SLAM) 68

28 referencia local. Su origen de coordenadas respecto al sistema de coordenadas global está localizado en la posición actual del vehículo, (x(k), y(k)). El ángulo que forman ambos ejes de abscisas viene dado por la orientación actual del vehículo (θ(k)). r k : Distancia que hay entre los orígenes de coordenadas de los dos sistemas de referencia, el local y el global. Su obtención es inmediata aplicando la siguiente expresión: r k = x 2 (k) + y 2 (k) (4.33) β k : Ángulo que forma la recta que une los dos orígenes 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. r i : 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 más que aplicar un poco de geometría, se puede obtener la posición predicha de las rectas en un marco de referencia local al vehículo. z i = ρ i = r i r k cos(ϕ i β k ) (4.35) α i ϕ i θ(k) Sin embargo, esta última fórmula, aunque es válida para la situación que aparece en la figura 4.14, no siempre será así, pudiendo dar resultados de ρ i menores que cero, y valores incorrectos 4. Localización y Mapeado de forma Simultánea (SLAM) 69

29 de α i. Esto ocurre cuando se produce una intersección entre la recta detectada y el segmento que une los dos orígenes de coordenadas de los dos sistemas de referencia [7]. En este último caso, la fórmula que habría que emplear para obtener una estimación de los parámetros de la recta sería la siguiente: z i = ρ i = r i + r k cos(ϕ i β k ) (4.36) α i ϕ i θ(k) + π Éste es un detalle importante y habrá que tenerlo en cuenta en otras estimaciones posteriores y en los Jacobianos que se calculen más adelante Obtención de la distancia de Mahalanobis Para ver cuánto de parecido tienen dos rectas (la estimada y la obtenida por el láser), se puede emplear la distancia de Mahalanobis. La distancia de Mahalanobis es una distancia basada en la correlación 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 expresión: DM 2 = (z z) T Ψ 1 k (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 expresión: Ψ k = H k P t H T k + Q t (4.40) 4. Localización y Mapeado de forma Simultánea (SLAM) 70

30 En la ecuación anterior, Q t es la matriz de ruido de la observación, H k es el Jacobiano de la estimación con respecto al estado del vehículo y a la observación, y P t es la matriz de covarianzas del estado del ROMEO. Como los valores de Q y P son conocidos, se procederá al cálculo de H k, con lo que ya se podría obtener la distancia de Mahalanobis. Antes de proceder al cálculo del Jacobiano, hay que aclarar que sus dimensiones serían 2x5, mientras que las dimensiones de la matriz P son (3 + 2N t )x(3 + 2N t ). Por tanto, para que cuadren las dimensiones de ambas matrices, habrá que mapear el Jacobiano (que en adelante se llamará h k ) en una matriz de dimensión mayor (H k ). Esta última matriz tendrá los valores del Jacobiano distribuidos de forma que afecten únicamente a la recta estimada que se está considerando. forma: Para mapear h k en H k, hay que introducir una nueva matriz, llamada F x,k, y con la siguiente La relación entre las matrices h k y H k es F x,k = } 0 {{ 0} 0 1 } 0 {{ 0 } 2k 2 2N t 2k (4.41) H k = h k F x,k (4.42) Como ya se dijo anteriormente, la matriz h k tiene dimensión 2x5 y su forma es: h k = h k 11 h k12 h k13 h k14 h k15 (4.43) h k21 h k22 h k23 h k24 h k25 Antes de comenzar con el cálculo del Jacobiano, hay que señalar que, como se comentó en un apartado anterior, hay dos pares de expresiones que permiten estimar los parámetros de las rectas en el marco de referencia local, en función de si hay intersección entre la recta y el 4. Localización y Mapeado de forma Simultánea (SLAM) 71

31 segmento que une los orígenes o no. Del mismo modo, y ya que el Jacobiano depende de z, habrá dos Jacobianos posibles. A continuación se mostrarán los elementos del Jacobiano supuesto que no hay intersección entre la recta y el segmento: h k11 = ρ i x = 1 r k ( x cos(ϕ i β k ) + y sin(ϕ i β k )) (4.44) h k12 = ρ i y = 1 r k (y cos(ϕ i β k ) + x sin(ϕ i β k )) (4.45) h k13 = ρ i θ = 0 (4.46) h k14 = ρ i r i = 1 (4.47) h k15 = ρ i ϕ i = r k sin(ϕ i β k ) (4.48) h k21 = α i x = 0 (4.49) h k22 = α i y = 0 (4.50) h k23 = α i θ = 1 (4.51) h k24 = α i r j = 0 (4.52) h k25 = α i ϕ j = 1 (4.53) Si, en cambio, hubiera intersección entre la recta y el segmento, las expresiones serían las que siguen: h k11 = ρ i x = 1 r k (x cos(ϕ i β k ) y sin(ϕ i β k )) (4.54) h k12 = ρ i y = 1 r k (y cos(ϕ i β k ) + x sin(ϕ i β k )) (4.55) h k13 = ρ i θ = 0 (4.56) h k14 = ρ i r i = 1 (4.57) 4. Localización y Mapeado de forma Simultánea (SLAM) 72

32 h k15 = ρ i ϕ i = r k sin(ϕ i β k ) (4.58) h k21 = α i x = 0 (4.59) h k22 = α i y = 0 (4.60) h k23 = α i θ = 1 (4.61) Una vez calculado el Jacobiano, ya se puede conocer la distancia de Mahalanobis al cuadrado. h k24 = α i r j = 0 (4.62) h k25 = α i ϕ j = 1 (4.63) En el siguiente apartado se verá qué criterio se va a seguir para asociar las rectas Asociación de rectas Para encontrar cuáles de las rectas detectadas por el láser en el instante actual son rectas que ya habían sido detectadas en instantes anteriores, hay que computar las distancias de Mahalanobis entre una recta detectada y todas las existentes. También se hace necesario definir un umbral para la distancia de Mahalanobis. Éste viene definido por la distribución χ 2. Lo que se hace es buscar a las dos rectas cuyas distancias de Mahalanobis a la actual sean las menores y, además, que sean menores que el umbral. Si ocurre esto, la recta actual se había detectado anteriormente, y se corresponderá con una de las dos rectas cuyas distancias de Mahalanobis eran menores. Para discernir a qué recta corresponde la recta actual, se computarán 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é más cercana a ella misma. Esto se hace porque 4. Localización y Mapeado de forma Simultánea (SLAM) 73

33 puede ocurrir que se asociara una recta con una paralela a la misma, desechando la verdadera recta con la que se debería corresponder. De esta manera se consigue que la asociación de rectas sea más 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 consideraría que esa recta no se había detectado anteriormente, y dará lugar a una nueva característica del mapa. A este método de asociación se le conoce como la asociación de datos en función del vecino más cercano y con umbral simple. Hay otros métodos más complejos pero que no se han empleado en este proyecto, como el test de compatibilidad conjunta [19] o uno basado en gráficos [1]. Sin embargo, el que se haya asociado a priori con este método 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 también 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 deberían asociar. Por tanto, la recta detectada pasará con las rectas no asociadas, cuyo procedimiento se detallará más 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 habría que hacer en este caso, sería actualizar la longitud de la misma y sus extremos. Dos rectas superpuestas totalmente: Aquí puede haber dos posibilidades. 4. Localización y Mapeado de forma Simultánea (SLAM) 74

34 Que la nueva recta detectada sea más pequeña que la antigua recta y que quede contenida en ella. En este caso, ambas rectas serían la misma, y no habría que modificar ni la longitud ni los extremos de la misma. Que la nueva recta sea más grande que la antigua y que la contenga. En este caso, habría 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 cuenta que ambas rectas pueden tener parámetros ligeramente diferentes, aunque sean muy parecidos. Por ello, realmente habría que actualizar dichos parámetros. Sean (r 1, ϕ 1 ) los parámetros de la primera recta, y (r 2, ϕ 2 ) los de la segunda, siendo l 1 y l 2 sus respectivas longitudes. La forma en que se actualizarán los parámetros será dándole mayor o menor importancia a los parámetros de cada recta en función de su longitud. Por tanto, los parámetros (r, ϕ) de la recta resultante serán: r = r 1l 1 + r 2 l 2 l 1 + l 2 (4.64) ϕ = ϕ 1l 1 + ϕ 2 l 2 l 1 + l 2 (4.65) Ahora se estudiará lo que ocurre con las rectas que no se han podido asociar con otras, o cuya asociación ha sido descartada posteriormente tras el estudio de sus extremos, como ya se ha visto. Para que una recta considerada como nueva se pueda añadir al mapa, tiene que ser detectada al menos un número determinado de veces. Esto es así para evitar que rectas producidas por medidas erróneas o por una mala extracción de las mismas a partir de las medidas, sean añadidas al mapa. Por ello, cada recta no asociada con ninguna otra, se comparará con las rectas candidatas a ser añadidas. Si se encuentran rectas que coincidan, se incrementará el contador del número de veces que se ha detectado dicha recta. Sino, se añade 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. Básicamente lo que se hace es lo mismo que en la asociación de datos anterior. Se expresan 4. Localización y Mapeado de forma Simultánea (SLAM) 75

35 todas las rectas en el mismo sistema de coordenadas y se compararan sus parámetros. Aquellas con parámetros muy similares, se considerarán como la misma recta, mientras que aquellos que difieran en ellos, se considerarán como rectas diferentes. Un detalle importante a tener en cuenta es que en grandes recorridos, el número de posibles rectas candidatas a añadirse 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 número de veces. De esta forma, se irán eliminando las rectas candidatas más antiguas. Esto no supone problema alguno en el algoritmo, ya que si una recta se ha intentado asociar sin éxito muchas veces, lo más probable es que no se asocie próximamente, 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 añadiría de nuevo a la lista de candidatas y habría posibilidades de asociarla otra vez. Una vez se ha establecido la asociación (o no asociación) de rectas detectadas por el láser, se procederá a actualizar el vector de estados y la matriz de covarianzas Actualización de x y P La actualización de x y P tendrá lugar tantas veces en una iteración como rectas detectadas se hayan asociado a otras existentes. Esta etapa tan sólo consiste en aplicar las ecuaciones que se detallarán a continuación. Ψ k = H k P t H T k + Q t (4.66) Esta matriz, tan sólo es una matriz auxiliar para simplificar la notación. A continuación se define la ganancia de Kalman como: K = P t H T k Ψ 1 k (4.67) La actualización del vector de estados viene dada por x t = x t + K(z i z i ) (4.68) 4. Localización y Mapeado de forma Simultánea (SLAM) 76

36 Y por último, la actualización de la matriz de covarianzas la proporciona la siguiente expresión: P t = P t KΨ k K T (4.69) Como se comentó anteriormente, para un conjunto de medidas del láser, se realizará la etapa de actualización tantas veces como rectas se hayan podido asociar a rectas anteriormente detectadas. Se puede observar de las ecuaciones anteriores que en la etapa de Actualización, además de actualizar la posición y orientación del robot, también se actualizan los parámetros característicos de las rectas que se han detectado. Esto implica que los puntos extremos de las rectas también deberán variar. Realmente la variación de los parámetros no suele ser muy grande, por lo que tampoco es necesario ser muy riguroso en la actualización de los puntos extremos de las rectas, ya que la variación no será apreciable. Una opción puede ser mantener las coordenadas X de los puntos extremos (para rectas horizontales) y calcular las coordenadas Y con los nuevos parámetros. Para las rectas verticales el razonamiento sería análogo, pero intercambiando las coordenadas X por las Y. Para otro tipo de rectas podría verse si se asemejan más 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 función de la medida en que varíen los parámetros y de la posición de las rectas en el plano. Por tanto, lo que se hará es construir varias rectas intermedias con parámetros comprendidos entre la original y la final. Una vez hecho esto, se proyectarán los puntos extremos de la recta inicial en la primera de las rectas auxiliares. A continuación, se proyectarán 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. Localización y Mapeado de forma Simultánea (SLAM) 77

37 Actualización por GPS Tal y como se describió en un capítulo 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 posición del ROMEO. El proceso de actualizar con medidas GPS es mucho más simple que el que se hizo con las medidas del láser. En este caso, la medida sería: z = X GP S (4.70) Y GP S El Jacobiano a calcular esta vez, H, sería también más sencillo que los que se han calculado anteriormente, siendo el siguiente: H = (4.71) Ya se pueden aplicar de nuevo las ecuaciones de actualización que, aunque son muy similares a las del apartado anterior, se mostrarán a continuación para facilitar su entendimiento. Esta etapa tan sólo consiste en aplicar las ecuaciones siguientes. Ψ t = H t P t H T t + Q GP St (4.72) K = P t H T k Ψ 1 t (4.73) La actualización del vector de estados esta vez viene dada por x t = x t + K X GP S x t (1) (4.74) Y GP S x t (2) Finalmente, la actualización de la matriz de covarianzas queda como antes P t = P t KΨ t K T (4.75) 4. Localización y Mapeado de forma Simultánea (SLAM) 78

38 La matriz Q GP St que aparece en la expresión 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 actualización. Q GP St = σ X GP S 0 (4.76) 0 σ YGP S Figura 4.15: Trayectoria del ROMEO según odometría (en rojo) y según 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 más bien, todo lo contrario. En la figura 4.15 se muestran dos trayectorias descritas por el ROMEO, siendo la trayectoria en rojo una estimación de la odometría, 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 odometría únicamente, 4. Localización y Mapeado de forma Simultánea (SLAM) 79