Localización y construcción de un mapa en forma simultanea utilizando el filtro extendido de Kalman

Texto completo

(1)Localización y construcción de un mapa en forma simultánea utilizando el Filtro extendido de Kalman. Carlos Alberto Restrepo Patiño. Maestrı́a en Ingenierı́a Eléctrica Grupo de Investigación en robótica y percepción sensorial Universidad Tecnológica de Pereira Pereira, Colombia 2007.

(2) Localización y construcción de un mapa en forma simultánea utilizando el Filtro extendido de Kalman. Carlos Alberto Restrepo Patiño. Trabajo de grado para optar al tı́tulo de Magı́ster en Ingenierı́a Eléctrica. Director M. Sc. Luis Hernando Rı́os. Maestrı́a en Ingenierı́a Eléctrica Grupo de Investigación en robótica y percepción sensorial Universidad Tecnológica de Pereira Pereira, Colombia 2007.

(3) Nota de aceptación:. Firma del presidente del jurado. Firma del jurado. Firma del jurado. Pereira, agosto de 2007.

(4) Aquı́ estoy navegando sobre este rı́o de alegrı́as, de aciertos y desaciertos, buscando no estar a la orilla porque la vida pierde sentido cuando miramos pasar sus aguas sin dejarnos moldear por ellas, cuando no vemos en los torbellinos una oportunidad para crecer y aprender. Buscando que mis sueños crezcan al igual que crecen los riachuelos y luego se convierten en un rı́o que lleva en él piedras y al mismo tiempo vida persiguiendo el mar donde desembocan esos sueños para fortalecerse y seguir logrando mi proyecto de vida y ası́ contribuir a esta sociedad..

(5) A mi mamá y a mi abuelita porque me han dado la mano en los momentos más difı́ciles de mi vida, porque me han acompañado en los momentos más felices, porque todo lo que soy se lo debo a ustedes, porque no sólo me han dado los medios para alcanzar mis sueños si no que me han dado un gran ejemplo, por todo el amor que me han dado siempre quedaré en deuda con ustedes. A mi hermanito por ser mi mejor amigo, porque quiero que sepa que siempre podrá contar conmigo. Porque muchas de las cosas que hago son para ofrecerle un mejor futuro. A mi linda por aparecer en mi vida, por todo el amor que me da, por espantar mi soledad, por confiar en mı́ sin importar la adversidad. A mi tı́a por darme la oportunidad de conocerla y entenderla. Porque se que ahora puedo contar con su apoyo y amistad. A mi hermana por ser mi aliada, por la paciencia que me tiene y el cariño que me da..

(6) AGRADECIMIENTOS A la Universidad Tecnológica de Pereira por permitirme continuar con mis estudios, a los profesores Alfonso Alzate, Pompilio Tabarez y Luis Hernando Rı́os por su apoyo y confianza. A los profesores Alejandro Garcés, Luis Enrique Avendaño y Jason Molina por todos los valiosos aportes para la realización de este trabajo. A mis amigos porque sin importar las circunstancias siempre estuvieron conmigo. A todas las personas que participaron directa o indirectamente en la elaboración de este documento.. vi.

(7) RESUMEN En esta tesis se presenta una metodologı́a que permite a un robot en un ambiente desconocido ir levantando un mapa en forma incremental a medida que explora su entorno y simultáneamente localizarse en el mapa. Esto es conocido como el problema SLAM (Simultaneous Localization and Mapping) el cual es solucionado utilizando como herramienta de estimación el filtro extendido de Kalman (EKF Extended Kalman Filter). La solución al problema SLAM es considerada una pieza clave en la obtención de un sistema robótico verdaderamente autónomo. La idea central de este trabajo es que el robot, desde un punto de partida especı́fico llegue hasta un objetivo deseado. El principal inconveniente en esta tarea es que durante el recorrido el robot presentará errores odométricos que conllevan a una incertidumbre en su posición. Dado que, el mapa que levanta el robot en cada uno de los puntos del recorrido es relativo a su posición, este mapa tendrá también incertidumbres debido a la suma de los efectos de los errores tanto odométricos como de los sensores del robot. El resultado de la suma de estos errores es que la posición en la que el robot asume que se encuentra, no corresponde a la verdadera y además el mapa global que el robot tiene almacenado es diferente al real. Como consecuencia de esto el robot no llegará al objetivo deseado. Se propone en este trabajo una estimación del estado del robot utilizando EKF y simultáneamente realizar una navegación autónoma mediante campos potenciales para llegar hasta el objetivo dado por el mapa estimado. Una vez el robot llegue hasta el objetivo especifico, se emplea la técnica de programación dinámica para que el robot planee la ruta que lo retorne hasta el inicio utilizando la mı́nima trayectoria. Los resultados obtenidos son ciertamente destacables, comprobando la construcción exitosa de mapas en distintos entornos y con distintos robots, bajo distintas condiciones de incertidumbre, tanto para el robot como para los sensores. Estos mapas permitieron una buena navegación reactiva utilizando campos potenciales y además una exitosa planeación de trayectorias óptimas empleando programación dinámica.. vii.

(8) ABSTRACT This work presents a methodology that allows a robot in an unknown environment to build a map incremental form while it is exploring the environment, and locate on the map simultaneously. This task is known as SLAM problem (Simultaneous Localization and Mapping), which is solved using the Extended Kalman Filter (EKF) as the estimation tool. The SLAM problem’s solution is considered as a key piece in the task of getting a robotic system truly autonomous. The main idea of this work is that the robot commutes from a specific starting point to a required objective. The main inconvenient in this task is that during the route, the robot will present odometric errors that produce position uncertainty. Since the map made by the robot at each of the route points, is relative to its position, this will have also uncertainties, due to the sum of the effects of the odometric errors and the robot’s sensors. The result of the sum of these errors is that the position in which the robot assumes it is, does not correspond to the real one and also the global map that the robot has stored is different from the real. As a consequence of this, the robot does not commute to the required objective. In this work it is proposed a state estimation of the robot using EKF and simultaneously performing an autonomous navigation using potential fields to reach the objective given by the estimated map. Once the robot commutes to the specific objective, the dynamic programming technique is used so that the robot plans the route that returns it to initial point using the minimal trajectory. The results obtained are truly remarkable, verifying the successful building of maps in distinct environments and with distinct robots, under distinct uncertainty conditions, for the robot and the sensors. These maps allowed a good reactive navigation using potential fields and also a successful planning of optimal trajectories using dynamic programming.. viii.

(9) INTRODUCCIÓN La construcción de un mapa del entorno mediante un robot móvil es uno de los temas de investigación mas importantes dentro de la robótica móvil y la inteligencia artificial desde hace dos décadas. La capacidad de un robot móvil para construir un mapa de su entorno se considera que es fundamental para conseguir una total autonomı́a. Es por eso que existen numerosos trabajos dentro de esta área con este objetivo. La construcción de un mapa mediante un robot móvil es un problema complejo debido a su naturaleza. Para adquirir el mapa de un entorno, el robot móvil debe disponer de sensores que le permitan percibir los objetos del mundo que los rodea. Todos estos sensores presentan como caracterı́stica fundamental la existencia de ruido en sus medidas, medidas que son referenciadas relativamente a la posición del robot. Es importante resaltar esta caracterı́stica, ya que el hecho de que el entorno no sea observable de manera global es el origen de la dificultad del problema. Otra caracterı́stica importante es el limitado rango de los mismos, con alcances efectivos de pocos metros. El principal problema de la construcción de un mapa es el hecho de que las observaciones hechas por el robot son relativas al propio robot, y como el sistema odométrico es inherentemente inexacto, la posición del robot esta caracterizada por una incertidumbre intrı́nseca. Por tanto, el mapa no es globalmente observable. Cuando el robot realiza una medida, también con incertidumbre, está medida esta referida a su posición, ya incierta. Como consecuencia de este problema, la localización del robot y la construcción del mapa están acopladas y no se pueden resolver de manera rigurosa por ningún método que desacople ambas estimaciones. En el caso de que el robot no tuviera ruido odométrico, la construcción del mapa serı́a muy sencilla ya que éste serı́a globalmente observable, y bastarı́a realizar constantemente nuevas medidas para disminuir la incertidumbre del mapa y obtener cada vez un mapa más preciso. El problema SLAM investiga si es posible para un robot móvil inmerso en un ambiente inexplorado con una posición inicial desconocida, empezar a construı́r un mapa consistente con su ambiente mientras que al mismo tiempo determina su localización dentro de este mapa. La idea subyacente en cualquier algoritmo que aborde el problema SLAM es la minimización conjunta de las incertidumbres del robot y el mapa. El EKF es una herramienta de fusión sensorial y de estimación estocástica muy extendida en el mundo de la robótica. El desarrollo de este proyecto se aborda desde el punto de vista teórico de la formulación matemática del SLAM utilizando EKF desde la aplicabilidad práctica de la misma mediante algoritmos que permitan la construcción de un mapa adecuado para la posterior navegación y planeación de trayectorias.. ix.

(10) OBJETIVOS Objetivo general Analizar y desarrollar una metodologı́a para la construcción de mapas de entornos exteriores con un robot móvil, modelando tanto la incertidumbre de la posición del robot y la lectura de los sensores. Esta metodologı́a debe ser lo suficientemente robusta y confiable para permitir una navegación autónoma por parte del robot.. Objetivos especı́ficos – Modelar la geometrı́a del entorno para ser usado en el trabajo estocástico del EKF. Este modelo tendrá en cuenta desde su principio el coste computacional de su implementación, intentando minimizar el mismo. Esta formulación será implementada y experimentada en distintos entornos virtuales en los que deberá presentar resultados adecuados para su uso en tareas de localización y navegación. – Implementar el sistema de navegación reactivo para el robot utilizando la teorı́a de los campos potenciales y de esta forma alcanzar un objetivo especı́fico en el mapa estimado por el robot. Esta navegación se basa en el mapa generado por el modelo del sensores inmersos en un entorno virtual y estimado con el EKF. – Desarrollar las técnicas de programación dinámica para planear la ruta óptima desde el punto actual del robot hasta el punto de partida evadiendo los obstáculos detectados en el recorrido por el robot. – Implementar los métodos necesarios para la localización, el control reactivo, la planeación y ejecución de trayectorias por parte del robot para realizar una navegación autónoma. – Realizar un programa de simulación en el que se pueda estudiar diferentes entornos, configuraciones y modelos del robot y del sensor del mismo. Este programa será una herramienta importante para la prueba de los algoritmos que se desean implementar y deberá ser lo suficientemente funcional para futuras implementaciones de nuevas técnicas de navegación y de levantamiento de mapas.. x.

(11) ÍNDICE GENERAL. Introducción. ix. Objetivos. x. Índice general. xi. Lista de figuras. xiii. Lista de tablas. xvi. 1 Marco conceptual 1.1. 1. Formulación y estructura del problema SLAM . . . . . . . . . . . . . . . . . . . . . . . . .. 1. 1.1.1. SLAM probabilı́stico . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 2. 1.1.2. Estructura del SLAM probabilistico . . . . . . . . . . . . . . . . . . . . . . . . . .. 4. Solución al problema SLAM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 5. 1.2.1. EKF-SLAM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 6. 1.3. Problema de la construcción de un mapa . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 7. 1.4. Tipos de mapas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 9. 1.2. 2 Filtro extendido de Kalman para solucionar el problema SLAM 2.1. 10. Formulación Bayesiana del problema SLAM . . . . . . . . . . . . . . . . . . . . . . . . .. 10. 2.1.1. Filtro extendido de Kalman . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 12. 2.2. Formulación del algoritmo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 13. 2.3. Movimiento del robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 15. 2.4. Asociación de datos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 16. 2.4.1. Corrección del EKF . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 17. 2.4.2. Adición de nuevos objetos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 19. xi.

(12) ÍNDICE GENERAL. xii. 2.4.3. Clasificación de datos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 20. Propiedades del algoritmo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 21. 2.5. 3 Navegación reactiva utilizando campos potenciales 3.1. 23. Método de los potenciales artificiales . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 25. 3.1.1. 26. Potencial clásico . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 4 Planeación de una ruta óptima mediante programación dinámica. 30. 4.1. Formulación del problema . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 30. 4.2. Elementos del modelo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 31. 4.3. Representación por la ecuación recursiva . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 34. 4.4. Formulación del algoritmo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 34. 4.4.1. Problema de decisión de una etapa . . . . . . . . . . . . . . . . . . . . . . . . . . .. 37. 4.4.2. Problema de decisión de n etapas . . . . . . . . . . . . . . . . . . . . . . . . . . .. 38. 4.4.3. Función recursiva . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 39. 5 Resultados experimentales. 41. 5.1. Filtro extendido de Kalman . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 41. 5.2. Navegación reactiva utilizando campos potenciales . . . . . . . . . . . . . . . . . . . . . .. 63. 5.3. Planeación de una ruta óptima mediante programación dinámica . . . . . . . . . . . . . . .. 66. 6 Discusión y Conclusiones. 71. 6.1. Conclusiones . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 74. 6.2. Trabajos futuros . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 75. Bibliografı́a. 77.

(13) LISTA DE FIGURAS. 1.1. La esencia del problema SLAM. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 2. 1.2. Correlación de marcas. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 6. 2.1. Posición del robot con respecto al sistema global de referencia. . . . . . . . . . . . . . . . .. 13. 2.2. Construcción incremental del mapa. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 14. 2.3. Movimiento del robot. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 15. 2.4. Detección de un obstáculo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 17. 2.5. Pasos de la estimación del EKF. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 21. 3.1. Campo de potencial artificial. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 26. 3.2. Campo de potencial total. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 27. 3.3. Superficie generada. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 28. 4.1. Mapa obtenido al llegar al objetivo. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 31. 4.2. Discretización del entorno con zonas muertas para los obstáculos. . . . . . . . . . . . . . .. 31. 4.3. Posibles rutas para pasar a la etapa siguiente. . . . . . . . . . . . . . . . . . . . . . . . . .. 32. 4.4. Alternativas para la transición entre dos etapas. . . . . . . . . . . . . . . . . . . . . . . . .. 33. 4.5. Esquema del sistema desconocido. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 34. 4.6. Esquema del sistema separado en dos etapas. . . . . . . . . . . . . . . . . . . . . . . . . .. 36. 4.7. Esquema del sistema separado en n etapas. . . . . . . . . . . . . . . . . . . . . . . . . . . .. 36. 4.8. Representación de una sola etapa del problema. . . . . . . . . . . . . . . . . . . . . . . . .. 37. 4.9. Representación de n etapas del problema. . . . . . . . . . . . . . . . . . . . . . . . . . . .. 38. 5.1. Error en xr con ±2σ [cm]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 44. 5.2. Error en yr con ±2σ [cm]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 45. 5.3. Error en φr con ±2σ [rad]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 45. xiii.

(14) LISTA DE FIGURAS. xiv. 5.4. Error en xr con ±2σ [cm]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 46. 5.5. Error en yr con ±2σ [cm]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 47. 5.6. Error en φr con ±2σ [rad]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 47. 5.7. Error en xr con ±2σ [cm]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 48. 5.8. Error en yr con ±2σ [cm]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 49. 5.9. Error en φr con ±2σ [rad]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 49. 5.10 Error en xr con ±2σ [cm]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 50. 5.11 Error en yr con ±2σ [cm]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 51. 5.12 Error en φr con ±2σ [rad]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 51. 5.13 Error en xr con ±2σ [cm]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 52. 5.14 Error en yr con ±2σ [cm]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 53. 5.15 Error en φr con ±2σ [rad]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 53. 5.16 Error en xr con ±2σ [cm]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 54. 5.17 Error en yr con ±2σ [cm]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 55. 5.18 Error en φr con ±2σ [rad]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 55. 5.19 Error en xr con ±2σ [cm]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 56. 5.20 Error en yr con ±2σ [cm]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 57. 5.21 Error en φr con ±2σ [rad]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 57. 5.22 Error en xr con ±2σ [cm]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 58. 5.23 Error en yr con ±2σ [cm]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 59. 5.24 Error en φr con ±2σ [rad]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 59. 5.25 Error en xr con ±2σ [cm]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 60. 5.26 Error en yr con ±2σ [cm]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 61. 5.27 Error en φr con ±2σ [rad]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 61. 5.28 Error en xr con ±2σ [cm]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 62. 5.29 Error en yr con ±2σ [cm]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 63. 5.30 Error en φr con ±2σ [rad]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 63. 5.31 Navegación con un campo potencial con η = 10 y ξ = 2. . . . . . . . . . . . . . . . . . . .. 64. 5.32 Navegación con un campo potencial con η = 20 y ξ = 10. . . . . . . . . . . . . . . . . . . .. 64.

(15) LISTA DE FIGURAS. xv. 5.33 Navegación con un campo potencial con η = 20 y ξ = 15. . . . . . . . . . . . . . . . . . . .. 65. 5.34 Navegación con un campo potencial con η = 20 y ξ = 2. . . . . . . . . . . . . . . . . . . .. 65. 5.35 Mapa aleatorio con 20 obstáculos. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 66. 5.36 Espacio de solución con zonas muertas para los obstáculos. . . . . . . . . . . . . . . . . . .. 66. 5.37 Mapa aleatorio con 30 obstáculos. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 67. 5.38 Espacio de solución con zonas muertas para los obstáculos. . . . . . . . . . . . . . . . . . .. 67. 5.39 Mapa con tres paredes. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 68. 5.40 Espacio de solución con zonas muertas para los obstáculos. . . . . . . . . . . . . . . . . . .. 68. 5.41 Mapa en U. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 69. 5.42 Espacio de solución con zonas muertas para los obstáculos. . . . . . . . . . . . . . . . . . .. 69. 5.43 Mapa en L. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 70. 5.44 Espacio de solución con zonas muertas para los obstáculos. . . . . . . . . . . . . . . . . . .. 70. 6.1. Levantamiento de un mapa utilizando EKF. . . . . . . . . . . . . . . . . . . . . . . . . . .. 71. 6.2. Levantamiento de un mapa utilizando EKF. . . . . . . . . . . . . . . . . . . . . . . . . . .. 72.

(16) LISTA DE TABLAS. 5.1. Caracterı́sticas del LMS 200. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 42. 5.2. Caracterı́sticas del B21r. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .. 43. 5.3. Construcción de un mapa con 20 marcas en forma incremental con un control constante. . .. 44. 5.4. Construcción de un mapa con 40 marcas en forma incremental con un control constante. . .. 46. 5.5. Construcción de un mapa con 80 marcas en forma incremental con un control constante. . .. 48. 5.6. Construcción de un mapa con 80 marcas en forma incremental con un control constante. . .. 50. 5.7. Construcción de un mapa con 80 marcas en forma incremental con un control constante. . .. 52. 5.8. Construcción de un mapa con 80 marcas en forma incremental con un control constante. . .. 54. 5.9. Construcción de un mapa con 80 marcas en forma incremental con un control constante. . .. 56. 5.10 Construcción de un mapa con 80 marcas en forma incremental con un control constante. . .. 58. 5.11 Construcción de un mapa con 80 marcas en forma incremental con un control constante. . .. 60. 5.12 Construcción de un mapa con 80 marcas en forma incremental con un control constante. . .. 62. xvi.

(17) 1. MARCO CONCEPTUAL El problema del mapeo y la localización simultanea, SLAM, investiga si es posible para un robot móvil, inmerso en un ambiente inexplorado con una posición inicial conocida, empezar a construir un mapa consistente con su ambiente mientras que al mismo tiempo determina su localización dentro de este mapa. Una solución al problema del SLAM ha sido visto como una utopia por los investigadores en robótica ya que esto es hacer que un robot sea verdaderamente autónomo. La “solución” del problema SLAM ha sido uno de los éxitos más notables de la comunidad cientı́fica que investiga temas relacionados con robótica móvil desde la década pasada. El SLAM ha sido formulado y solucionado como un problema teórico en un número de formas diferentes. El SLAM ha sido implementado en un gran número de dominios desde robots terrestres en ambientes internos y externos, acuáticos y aéreos. En un nivel conceptual y teórico, SLAM puede ser considerado ahora como un problema solucionable.. 1.1 Formulación y estructura del problema SLAM SLAM es un proceso a través del cual un robot móvil puede crear un mapa de su ambiente y al mismo tiempo utilizar este mapa para deducir su ubicación. En SLAM tanto la trayectoria del robot como la ubicación de las marcas son estimadas en cada uno de los instantes sin la necesidad de tener un conocimiento previo de ellas. Considérese un robot moviéndose a través de un ambiente tomando observaciones relativas de un número desconocido de marcas, usando un sensor localizado en el robot como aparece en la Figura 1.1. En un instante de tiempo k, las siguientes cantidades son definidas de acuerdo a la Figura 1.1: • xk es el vector de estado que describe la localización y orientación del robot. • uk es el vector de control, aplicado en el tiempo k − 1, para conducir al robot a el estado xk en el tiempo k. • mi es un vector que describe la posición de la i-esima marca cuya verdadera localización es invariante en el tiempo. 1.

(18) 2 • zik es una observación tomada desde el robot de la localización de la i-esima marca en el tiempo k. Cuando existen multiples observaciones de marcas en cualquier tiempo o cuando la marca no es relevante para la discusión, la observación puede escribirse simplemente como z. Adicionalmete, los siguientes conjuntos son definidos: • X0:k = {x0 , x1 , . . . , xk } = {X0:k−1 , xk }: la historia de la localización del robot. • U0:k = {u0 , u1 , . . . , uk } = {U0:k−1 , uk }: la historia de las salidas de control. • m = {m1 , m2 , . . . , mn }}: el conjunto de todas las marcas. • Z0:k = {z1 , z2 , . . . , zk } = {Z0:k−1 , zk }: el conjunto de todas las marcas observadas.. mj. x k+2 u k+2. z k,j. xk+1 uk+1. xk x k-1. uk Robot. z k-1,j. mi. Marcas. Verdadero Estimado. Figura 1.1: La esencia del problema SLAM.. 1.1.1. SLAM probabilı́stico. En la forma probabilı́stica, el problema SLAM requiere que la distribución de probabilidad. P(xk , m | Z0:k , U0:k , x0 ). (1.1).

(19) 3 sea calculada para todos los tiempos k. Esta distribución de probabilidad, describe la densidad posterior conjunta de la localización de la marca y el estado del robot en el tiempo k, dadas las observaciones almacenadas y las distribuciones de controles hasta incluir un tiempo k junto con el estado inicial del robot. En general, una solución recursiva al problema SLAM es deseada. Empezando con una estimación de la distribución P(xk−1 , m | Z0:k−1 , U0:k−1 ) en el tiempo k − 1, la unión posterior, siguiendo un control uk y una observación zk es computarizada usando el teorema de Bayes. Este cálculo requiere que un modelo de transición de estado y un modelo de observación sean definidas describiendo el efecto de las salidas de control y de la observación respectivamente. El modelo de observación describe la probabilidad de hacer una observación zk cuando la ubicación del robot y la marca son conocidas y es generalmente descrita en la forma: P(zk | xk , m). (1.2). Es razonable asumir que una vez que la localización del robot y el mapa son definidos, las observaciones son condicionalmente independientes dado el mapa y estado actual del robot [1, 2]. El modelo del movimiento del robot puede ser descrito en términos de la distribución de probabilidad sobre la transición de estado en la forma: P(xk | xk−1 , uk ). (1.3). De este modo, el estado de transición es asumido como un proceso markoviano en el cual el estado proximo xk depende únicamente del estado inmediatamente precedente xk−1 y el control aplicado uk y es independiente tanto de las observaciones como del mapa [3, 4, 5]. El algoritmo SLAM es implementado en un estándar recursivo de dos pasos, el primero de ellos es la propagación (actualización de tiempo) y corrección (actualización de medida) en la forma: Actualización de tiempo P(xk , m | Z0:k−1 , U0:k , x0 ) =. Z. P(xk | xk−1 , uk )P(xk−1 , m | Z0:k−1 , U0:k−1 , x0 )dxk−1. (1.4). Actualización de medidas P(xk , m | Z0:k , U0:k , x0 ) =. P(zk | xk , m)P(xk , m | Z0:k−1 , U0:k , x0 ) P(zk | Z0:k−1 , U0:k ). (1.5). Las ecuaciones (1.4) y (1.5) proveen un procedimiento recursivo para calcular la union posterior.

(20) 4 P(xk , m | Z0:k , U0:k , x0 ) para el estado del robot xk y el mapa m en el tiempo k basado en todas las observaciones Z0:k y todas las salidas de control U0:k hasta incluir el tiempo k. La función recursiva depende del modelo del robot P(xk | xk−1 , uk ) y el modelo de la observación P(zk | xk , m). Vale la pena notar que el problema de la construcción del mapa puede ser formulada calculando la densidad condicional p(m | X0:k , Z0:k , Uk ). Esto asume que la ubicación del robot xk es conocida, o por lo menos determinada en cada instante, sujeto al conocimiento previo de la posición inicial. Un mapa m es entonces construido a través de combinar observaciones de diferentes posiciones. Prácticamente, el problema de la localización puede ser formulado calculando la distribución de probabilidades P(x | Z0:k , U0:k , m). Esto asume que la localización de las marcas es conocida con certeza y que el objetivo es calcular y estimar la localización del robot con respecto a estas marcas.. 1.1.2. Estructura del SLAM probabilistico. Para simplificar la discusión en esta sección, se dejara el condicionamiento en variables históricas en (1.1) y se escribirá la unión posterior requerida en el mapa y la localización del robot como P(xk , m | zk ) e inclusive P(xk , m) como el contexto lo permita. El modelo de observación P(zk | xk , m) hace explı́cita la dependencia de las observaciones tanto en el robot como en la localización de las marcas. Con lo que se concluye que la probabilidad conjunta no puede ser dividida en una manera obvia P(xk , m | zk ) , P(xk | zk )P(m | zk ), y por supuesto es bien conocido a través del mapeo consistente que esta partición produce estimaciones inconsistentes [6, 7]. Sin embargo, el problema del SLAM tiene más estructura que la que se alcanza a ver a través de estas ecuaciones. Observando de nuevo a la Figura 1.1, se puede apreciar que la mayorı́a del error entre la localización estimada y la verdadera de la marca es común entre marcas y es debido a una fuente singular; errores del desconocimiento de la posición del robot cuando la observación de la marca es hecha. A su vez, esto implica que los errores estimados en la localización de la marca están altamente correlacionados. Esto significa que una ubicación relativa entre cualquiera de las dos marcas mi − m j , puede ser conocidas con una alta certeza, inclusive cuando la localización absoluta de una marca mi es incierta. De una forma probabilı́stica esto quiere decir que la densidad conjunta de probabilidades para el par de marcas p(mi , m j ) esta altamente limitada sin importar que las densidades marginales p(mi ) pueden ser bastante dispersas..

(21) 5 La caracterı́stica interna más importante en SLAM es que las correlaciones de los estimados de las marcas se incrementan monotónicamente tanto como observaciones son realizadas. Esto significa que el conocimiento de la localización relativa de las marcas siempre mejora y nunca diverge, independientemente del movimiento del robot. En términos probabilı́sticos esto quiere decir que la union de la densidad probabilı́stica en todas las marcas p(m) crece monotónicamente cuando aumenta el número de observaciones. Esta convergencia ocurre porque las observaciones hechas por el robot se pueden considerar como cercanamente independientes medidas con respecto al sistema global de coordenadas. Considere que el robot de la Figura 1.1 en la localización xk observa las marcas mi y m j . La localización relativa de las marcas observadas es claramente independiente del sistema coordenado relativo al robot y las siguientes observaciones realizadas desde el robot dara paso independiente a la medida relativa de la relación entre las marcas. Como el robot se mueve a la posición xk+1 desde donde nuevamente observa la marca m j esto permite estimar la localización tanto del robot como de la marca para que actualizada a la previa localización relativa xk . A su vez esto propaga de nuevo la actualización de la marca mi aunque esta marca no es observada desde la localización xk+1 . Esto ocurre porque las dos marcas están altamente correlacionadas, su relación relativa es bien conocida, desde medidas previas. El hecho de que la misma información medida es usada para actualizar estas dos marcas las hace más correlacionadas. El termino medidas cercanamente independientes es apropiado porque los errores de observación pueden ser correlacionados a través de movimientos sucesivos del robot. También note que, en la Figura 1.1 en la posición xk+1 el robot observa dos nuevas marcas relativas a m j . Estas nuevas marcas son inmediatamente unidas o correlacionadas a el resto del mapa. En nuevas posiciones actualizaciones de estas nuevas marcas también actualizara la marca m j y a través de esta la marca mi . Estos es, todas las marcas terminan formando una red unida por la localización relativa o las correlaciones cuya precisión o valor se incrementa cuando una observación es realizada como se aprecia en la Figura 1.2.. 1.2 Solución al problema SLAM Una solución al problema probabilı́stico del SLAM implica encontrar una representación adecuada para el modelo de observación (1.2) y el modelo de movimiento (1.3), esto permite una estimación eficiente y consistente de las distribuciones de probabilidad (1.4) y (1.5). La representación más común es de la forma del modelo estado-espacio con ruido Gaussiano aditivo, liderado por el EKF para la solución del SLAM..

(22) 6. Figura 1.2: Correlación de marcas.. 1.2.1. EKF-SLAM. Las bases para el método EKF-SLAM es describir el robot en la forma P(xk | xk−1 , uk ) ⇐⇒ xk = f(xk−1 , uk ) + wk donde f(·) es el modelo cinemático del robot y wk es ruido Gaussiano con media cero y con covarianza Qk y corresponde a errores en el modelo del sistema. El modelo de observación es descrito en la forma P(zk | xk , m) ⇐⇒ zk = h(xk , m) + vk donde h(·) describe la geometrı́a de la observación y donde vk es ruido Gaussiano con media cero y covarianza Rk , el cual es debido a errores en las lecturas de los sensores. Con estas definiciones el método EKF puede ser aplicado para calcular la media   x̂k|k  m̂k. y covarianza Pk|k.   P xx P xm =  T P xx Pmm.   .    x   = E  k m =E k|k. ". Z0:k.   . x −x̂ x −x̂ T k. k. m−m̂k. k. k. m−m̂k. de la distribución posterior conjunta P(xk , m | Z0:k , U0:k , x0 ) de:. Z0:k. #.

(23) 7 Actualización de tiempo. x̂k|k−1 = f(x̂k−1|k−1 , uk ) P xx, k|k−1 = ∇f P xx, k|k−1 ∇f T + Qk donde ∇f es el Jacobiano de f evaluado en el estado estimado x̂k−1|k−1 .. Actualización de medida   x̂k|k  m̂k.  i  h  = x̂k|k−1 m̂k−1 + Wk zk − h(x̂k|k−1 , m̂k−1 ) Pk|k = Pk|k−1 − Wk Sk WTk. donde Sk = ∇h Pk|k−1 ∇hT + Rk Wk = Pk|k−1 ∇hT S−1 k siendo ∇h el Jacobiano de h evaluado en el estado x̂k|k−1 .. 1.3 Problema de la construcción de un mapa Desde los años 90, la construcción de mapas con robots ha sido dominada por las técnicas probabilı́sticas, y sin duda alguna son estas técnicas las que han conseguido resultados más notables. Estas técnicas probabilı́sticas se fundamentan en el teorema de Bayes, del que se deriva la formulación Bayesiana al problema SLAM previamente descrita. La construcción de un mapa mediante un robot móvil es un problema complejo ya que los sensores con los que el robot percibe el entorno tiene ruido en sus medidas las cuales son referenciadas a la posición del robot. Entre estos sensores se pueden citar los sonares o ultrasonidos, infrarrojos, escáneres láser, cámaras mono o estéreo y sensores de contacto. Todos estos sensores presentan como caracterı́stica fundamental la existencia de ruido en sus medidas las cuales son referenciadas relativamente a la posición del robot. Es importante resaltar esta caracterı́stica, ya que el hecho de que el entorno no sea observable de manera global, es el origen de la dificultad del problema. Otra caracterı́stica importante es el alcance limitado de los sensores con rangos de pocos metros..

(24) 8 Existe otro tipo de sensores que permite al robot medir su estado referenciado a un sistema de coordenadas global o absoluto, de forma total o parcial, como es el global positioning system (GPS) y las brújulas. Sin embargo, estos sensores externos no suelen funcionar bien en interiores y en presencia de ruidos magnéticos, como los provocados por corrientes eléctricas, por lo que no son muy utilizados en aplicaciones de interiores. Además, se considera especialmente interesante el caso de que el robot no tenga ninguna información absoluta de su estado, por lo que aun siendo factible su utilización, muchos investigadores abordan el problema de la construcción del mapa sin su ayuda. Aunque no son absolutamente necesarios, los robots móviles también suelen incorporar sensores propioceptivos que le permiten obtener medidas relacionadas con su estado, como son velocidad, incremento de posición y aceleraciones. Algunos de estos sensores son los encoders ópticos incrementales, tacogeneratrices, acelerómetros, giroscopios y sensores de velocidad relativos al exterior. Estos sensores permiten obtener una estimación incremental del movimiento del robot, pero debido a su ruido inherente no son capaces de mantener la posición del robot correctamente a lo largo del tiempo, ya que los errores son acumulativos. En el caso de no existir sensores, los desplazamientos del robot pueden ser estimados en base a los comandos motrices y al modelo cinemático y dinámico del robot. Al sistema capaz de estimar el movimiento relativo incremental del robot se le denomina odometrı́a. En un proceso tı́pico de construcción del mapa de un entorno, se parte de un mapa vacı́o o desconocido en el que el robot parte de una posición conocida, tı́picamente el origen de coordenadas. Comienza el proceso, y el robot es capaz de construir la parte del mapa cercana a su posición, pero debido al limitado rango de los sensores, el robot debe moverse hacia las zonas que quieran ser incluidas en el mapa. Según se va desplazando hacia esas nuevas áreas, observa los objetos presentes en ellas y va actualizando el mapa del entorno. El ruido de las medidas hace que el mapa no sea exacto, pero a su vez el robot se mueve con desplazamientos de los que sólo se tiene una estimación, con lo que su posición tampoco es exacta. Cuando se realiza observaciones de objetos nuevos, la estimación de la posición de esos objetos depende tanto de la incertidumbre de la posición del robot como de la incertidumbre de las medidas realizadas. El procesamiento del mapa del entorno se puede realizar después de la adquisición y almacenamiento de los datos sensoriales, con lo que el tiempo de procesamiento no es critico, o se puede realizar incrementalmente, es decir a medida que llegan los datos de los sensores. Esta segunda forma de procesamiento requiere que el tiempo de procesamiento empleado en la actualización del mapa con la llegada de cada información sensorial, sea inferior al periodo de muestreo de dichos sensores evitando la perdida de información..

(25) 9. 1.4 Tipos de mapas En la década de los ochenta y principio de los noventa, la construcción de mapas fue dividida en dos vertientes: la métrica y la topológica. Los mapas métricos recogen las caracterı́sticas geométricas del entorno, mientras que los mapas topológicos describen más que todo la conectividad entre diferentes lugares del mismo. Una primera contribución importante a los mapas métricos la constituye la realizada por Elfes y Moravec con su algoritmo de mapeo por ocupación de celdas (Occupancy Grid Mapping Algorithm) [8], el cual representa los mapas mediante una rejilla dividida en pequeñas celdas que modelan el espacio libre y el espacio ocupado del entorno del robot, mediante una distribución de probabilidad que va desde -1 a 1. De esta manera surgieron los ası́ llamados mapas de celdas (Grids Map). Otro algoritmo métrico que fue presentado paralelamente fue el de Chatila [9], el cual usa conjuntos de poliedros para describir la geometrı́a del entorno. Con este trabajo se reporta un nuevo nivel taxonómico conocido como Mapas de objetos (Features Map). Los mapas topológicos representan el entorno como un conjunto de lugares significativos que están conectados mediante trayectos o enlaces. Los primeros reportes sobre mapas topológicos incluyen los trabajos de Kuipers [10, 11] y el trabajo de Mataric [12]. Kuipers usaron el concepto de lugares caracterı́sticos (distinctives places), en el cual se definen propiedades especificas, basadas en las entradas de los sensores, a fin de que sean medidas distintivas de cada hito o marca. Por otro lado, un enlace en un mapa topológico significa que el robot puede viajar con éxito entre los dos hitos involucrados. Entonces, se puede adicionar un enlace en un mapa topológico sólo si el robot ha hecho el correspondiente viaje. Por su parte, Mataric [12] reporta la posibilidad que los enlaces sean establecidos al mismo tiempo que los hitos, lugares caracterı́sticos o marcas son identificados. Desde los noventa a la actualidad, la construcción de mapas en robótica móvil ha estado dominada por técnicas probabilı́sticas. En este sentido, los trabajos presentados por Smith [13], introdujeron el marco estadı́stico para resolver el problema del modelado del entorno y la localización simultanea que ya habı́a sido planteado por investigadores como Crowley [14], Chatila y Durrant-Whyte [15]. A partir de este momento la construcción de mapas probabilı́sticos se conoce comúnmente como SLAM Simultaneous Localization And Mapping [16, 17, 18, 19]. Dentro de la taxonomı́a de los mapas probabilı́sticos, los más comúnmente usados son los que emplean los filtros de Kalman para estimar el mapa y la localización del robot [16, 20, 21, 22, 23, 24], arrojando como resultados mapas que usualmente representan la localización de hitos, marcas o objetos significativos del entorno. Otra de las familias de los mapas probabilı́sticos se basa en el algoritmo de máxima expectación de A.P. Dempster o simplemente EM (Expectation Maximization) [18, 25, 26, 27]. Esta metodologı́a enfoca sobre todo el problema de correspondencia de datos, el cual consiste en determinar si los datos que se reciben por lo sensores corresponden a una misma entidad fı́sica del mundo real que ya fue observada en instantes de tiempo anteriores..

(26) 2. FILTRO EXTENDIDO DE KALMAN PARA SOLUCIONAR EL PROBLEMA SLAM Los algoritmos que alcanzan los mejores resultados al intentar solucionar el problema del SLAM, son los que basan su solución en técnicas probabilı́sticas. Todo algoritmo que se basa en técnicas probabilı́sticas está fundamentado en el teorema de Bayes. Debido a que el filtro de Kalman fundamenta su base conceptual y matemática en este teorema, se hace necesario hacer una breve descripción del teorema de Bayes aplicado al problema SLAM.. 2.1 Formulación Bayesiana del problema SLAM Los algoritmos más importantes en el levantamiento de mapas modelan la posición del robot y su entorno de una manera probabilı́stica y utilizan la inferencia para realizar una transformación de las medidas, obtenidas por el robot, a un mapa probabilı́stico. Esta formulación es la más extendida debido a la naturaleza misma del problema, ya que la incertidumbre presente en los sensores como en el movimiento del robot hace que este planteamiento sea el más adecuado. Un robot móvil explorando un ambiente desconocido deberá moverse a través del entorno de acuerdo a la lectura que tome los sensores del ambiente en cada punto. Es por esto que es fácil reconocer dos tipos de datos, las medidas de la odometrı́a del robot y de los sensores de percepción. Se puede asumir que estas medidas llegan de forma secuencial y alternativamente en el tiempo [28, 29], como: u 1 , z 1 , u 2 , z 2 , . . . , u t , zt Siendo u el desplazamiento relativo a la posición anterior del robot y z una medida sensorial tomada respecto al robot de un objeto del entorno y los subı́ndices representan el instante del tiempo en que fueron tomadas.. 10.

(27) 11 El estado del sistema xt en el instante t está dado por la posición del robot st y la posición de cada uno de los objetos del entorno mt . Esto es [28, 30]:    st   xt ≡  mt. La densidad de probabilidad del estado xt condicionado al conjunto de todos los datos almacenados hasta el instante t, se representa como p(xt | zt , ut ). Donde el superı́ndice t representa la agrupación de datos desde el inicio hasta el instante t, por lo que zt = z1 , z2 , . . . , zt y ut = u1 , u2 , . . . , ut . El teorema de Bayes permite escribir la probabilidad p(xt | zt , ut ) como [31, 32, 33]: p(xt | zt , ut ) = p(st , mt | zt , ut ) = ηp(zt | st , mt , zt−1 , ut )p(st , mt | zt−1 , ut ). (2.1). En donde η es un factor de normalización de la función de probabilidad. Esta función de probabilidad, representa cualquier solución probabilı́stica al problema SLAM. Realizando la hipótesis de Markov [34, 35, 36, 34], es decir, que el único estado que existe es el representado por st y mt , la ecuación (2.1) se puede escribir como [30, 37, 28]: p(st , mt | zt , ut ) = ηp(zt | st , mt )p(st , mt | zt−1 , ut ) Utilizando la ley de probabilidad total se puede expresar el segundo factor de la derecha de la siguiente manera [38, 33, 39]: t−1. t. p(st , mt | z , u ) =. Z Z. p(st , mt | zt−1 , ut , st−1 , mt−1 )p(st−1 , mt−1 | zt−1 , ut )dst−1 dmt−1. Haciendo uso nuevamente de la hipótesis de Markov y asumiendo que el movimiento del robot es independiente del mapa que lleva construido, se tiene: t−1. t. p(st , mt | z , u ) =. Z. p(st | ut−1 , st−1 )p(st−1 , m | zt−1 , ut−1 )dst−1. Con lo que se puede expresar (2.1) como [40, 41, 37, 42]: t. t. p(st , mt | z , u ) = ηp(zt | st , mt ). Z. p(st | ut , st−1 )p(st−1 , m | zt−1 , ut−1 )dst−1. (2.2). Por lo que la solución al problema SLAM se puede hacer de forma recursiva, necesitando en cada instante de tiempo los datos sensoriales obtenidos para ese instante de tiempo con su probabilidad y la función de probabilidad del estado en el instante anterior. La ecuación (2.1) se conoce como.

(28) 12 filtro de Bayes para solucionar el problema SLAM. Para evaluar la expresión (2.1) se requieren dos funciones de probabilidad denominadas generativas. Estas funciones corresponden al sistema odométrico y al de percepción externo. Por lo general estas funciones son invariantes en el tiempo, con lo que el modelo de medida se puede expresar como p(zt | st , mt ) y el modelo del movimiento por p(st | ut , st−1 ). El problema principal es que la ecuación (2.2) no puede implementar, ni resolverse en su forma general, por lo que es necesaria la realización de simplificaciones, suposiciones o hipótesis añadidas para su solución.. 2.1.1. Filtro extendido de Kalman. Una de las soluciones más difundidas a la ecuación (2.2) del filtro de Bayes es la basada en el filtro de Kalman. El filtro de Kalman es un estimador recursivo lineal de mı́nimos cuadrados [43, 44, 45, 46]. Esta herramienta es muy utilizada en el área de la robótica, la estimación con incertidumbre y la fusión sensorial. La aplicación para sistemas no lineales se hace mediante la extensión del filtro de Kalman al linealizar las ecuaciones, el cual recibe el nombre de Filtro Extendido de Kalman (EKF). El EKF se puede derivar de la suposición de que la probabilidad del estado p(st , mt | zt , ut ) se puede modelar por una distribución de probabilidad unimodal Gaussiana multidimensional [28, 47, 48], caracterizada por su valor esperado y su matriz de varianzas y covarianzas [32, 39, 49]: xt = st mt ⇔ x(k) x(k) ∼ N(x̂(k | k), P(k | k)) Donde: x̂(k | k) = E [x(k)] P(k | k) = E [(x(k) − x̂(k | k))(x(k) − x̂(k | k))T ] Por la propia naturaleza del EKF, esta solución requiere el uso de un mapa geométrico [30, 50, 51], basado en elementos discretos con determinadas caracterı́sticas geométricas para poder ser parametrizados en el vector de estado a estimar por el algoritmo [52, 53, 54, 55]. La solución al problema SLAM con EKF tiene como principal ventaja que es capaz de mantener la estimación completa a posteriori explı́citamente de forma incremental, a costa de suponer que tanto el ruido del sistema odométrico como del sistema sensorial pueden ser aproximados por una.

(29) 13 distribución normal [56, 30, 57, 58]. El EKF requiere para el cálculo de su matriz de covarianza que tanto el modelo de movimiento como el de medida, los cuales por lo general son no lineales, sean linealizados. Generalmente, se supone que esta linealización es aceptable y que los errores cometidos son pequeños, lo que es asumido por la gran mayorı́a de las implementaciones existentes.. 2.2 Formulación del algoritmo Para el desarrollo del EKF se parte de la simplificación de que el entorno en el cual se desplaza el robot es bidimensional y plano, y de que la posición del robot se puede representar por un sistema de coordenadas cartesianas [59, 60, 61], como se ilustra en la Figura 2.1, con media y covarianza definidas como: x̂r = [ x̂r ŷr φ̂r ]T  2   σ xr xr σ2xr yr σ2xr φr    Pr =  σ2xr yr σ2yr yr σ2yr φr   2  σ xr φr σ2yr φr σ2φr φr. W φr yr. xr Figura 2.1: Posición del robot con respecto al sistema global de referencia..

(30) 14 La observación de un objeto en el plano por parte del robot permite la creación iterativa de un mapa en el mismo sistema coordenado [21, 62, 63] como se muestra en la Figura 2.2. La matriz de covarianza Pm de este mapa incluye una correlación cruzada entre los diferentes objetos del mapa. Dado que la posición de los objetos son invariantes con el tiempo, estas correlaciones podrı́an incrementarse con cada observación del mismo objeto a medida que el mapa es más rı́gido. El vector de estado de la ubicación de los objetos en el mapa es también modelado por una distribución Gaussiana [64, 65, 66, 51] con media y covarianza definidas por: x̂m = [ x̂1 ŷ1 . . . x̂n ŷn ]T  2  σ x1 x1   σ2x y  1 1 . Pm =  ..   σ2x x  1 n σ2x1 yn. σ2x1 y1 . . . σ2y1 y1 . . . .. . ... 2 σy1 xn . . . σ2y1 yn . . .. σ2x1 xn σ2x1 yn σ2y1 xn σ2y1 yn .. .. . . 2 2 σ xn xn σ xn yn σ2xn yn σ2yn yn.          . W ( x i , yi ). Figura 2.2: Construcción incremental del mapa.. En un instante cualquiera de tiempo el vector de estado que representa el modelo del entorno está compuesto por la unión de los vectores de estado de todos y cada uno de los objetos del entorno y el estado del robot [67, 68, 30]. Esto es:    x̂r   x̂a =  x̂m.

(31) 15   Pr Prm Pa =  T Prm Pm.   . 2.3 Movimiento del robot Entre el instante k y el instante k + 1 el robot realiza un movimiento expresado por el vector x̂δ , el cual define el incremento en la posición y orientación del robot expresado en coordenadas relativas al robot [16, 69], como se puede observar en la Figura 2.3.. W xδ. xr(k) xr(k+1). Figura 2.3: Movimiento del robot.. En la práctica, este vector es estimado por la odometrı́a del robot, de forma no perfecta debido a los errores propios del sistema odométrico de un robot móvil. Por tanto se debe modelar por una distribución Gaussiana, en la que la media es el valor de la medida odometrı́a y su covarianza suele ser obtenida mediante experimentación previa del sistema odométrico [66, 51, 70]. Realizando la hipótesis de que el único objeto que se mueve en el mapa es el robot, la función de predicción está dada por:. x̂−a.   = f(x̂a , x̂δ ) = .   x̂ + x̂δ · cos(φ̂r ) − ŷδ sin(φ̂r )   r g(x̂r , x̂δ )   ŷr + x̂δ · sin(φ̂r ) + ŷδ cos(φ̂r )  =   φ̂r + φ̂δ x̂m  x̂m.        .

(32) 16 y su covarianza: P−a = ∇fxa Pa ∇fxTa + ∇fxδ Pδ ∇fxTδ. (2.3). donde las matrices Jacobianas ∇fxa y ∇fxδ necesarias para el cálculo de la matriz de covarianzas tiene la forma:    ∇gxr 0rm  ∂f  ∇fxa = =  ∂xa (x̂a ,x̂δ )  0Trm Im  (x̂a ,x̂δ )    ∇gxδ  ∂f  ∇fx∂ = =  ∂xδ (x̂a ,x̂δ )  0Trm  (x̂a ,x̂δ ) donde las matrices Jacobianas ∇gxr y ∇gxδ son expresadas como:. ∇gxr =. ∂g ∂xr. ∇gxδ =. (x̂r ,x̂δ ). ∂g ∂xδ.   1 0 − x̂δ · sin(φ̂r ) − ŷδ cos(φ̂r )  =  0 1 x̂δ · cos(φ̂r ) − ŷδ sin(φ̂r )  0 0 1. (x̂r ,x̂δ ).     .    cos(φ̂r ) − sin(φ̂r ) 0    =  sin(φ̂r ) cos(φ̂r ) 0    0 0 1. La dispersidad de la matrices Jacobiana provoca que la matriz de covarianzas P−a modifique sus valores sólo para la varianza en la posición del robot y el resto de objetos del entorno, permaneciendo constantes las varianzas y covarianzas entre los objetos Pm [66, 30]. Es por esto que la ecuación (2.3) se puede implementar de manera más eficiente como: P−a.   ∇gxr Pr ∇gTx + ∇gxδ Pδ ∇gTx ∇gxr Prm r δ =  T (∇gxr Prm ) Pm.   . 2.4 Asociación de datos En el instante k + 1 el robot observa un objeto Oi del entorno como se ilustra en la Figura 2.4. Se considera por simplicidad que una observación está compuesta por la medida de un único objeto del entorno, siendo la formulación extendible fácilmente al caso de que el robot realice varias medidas de distintos objetos al mismo tiempo. La medida que realiza el robot se expresa como el vector relativo al mismo zk+1 y su varianza Rk+1 , las cuales son dadas como [16, 23, 30]: zk+1.    r  =   θ.

(33) 17 Rk+1.   σ2 σ2rθ =  2r σrθ σ2θ.   . La varianza, o los parámetros necesarios para calcularla, se suelen obtener experimentalmente mediante ensayos a priori del sistema sensorial del robot.. W yi yj. r. θ. xj xi Figura 2.4: Detección de un obstáculo. El primer paso es realizar la comparación de esta observación con todos y cada uno de los objetos previamente almacenados en el mapa, para decidir si corresponde o no a alguno de dichos objetos, y a cuál en caso positivo. Si la observación se asocia correctamente a un objeto del mapa, entonces se proceda a la etapa de corrección del EKF con la información de este emparejamiento. Cuando la observación no es asociada a ningún objeto del mapa previo, eso quiere decir que dicha observación corresponde a un objeto que ha sido visto por primera vez y deberá ser añadido al mapa.. 2.4.1. Corrección del EKF. Si una observación ha sido emparejada con alguno de los objetos ya existentes en el mapa, entonces se puede hacer con este emparejamiento una mejora en la estimación del mapa. Dado que ya se tiene una estimación de la observación (xi , yi ) previamente almacenada en el mapa, se calcula una.

(34) 18 estimación de la lectura relativa al robot para esa observación como [23, 66, 51]:  p  ( x̂i − x̂r )2 + (ŷi − ŷr )2 ẑi = hi (x̂a ) =  r ) − φ̂r arctan( ŷx̂ii −ŷ − x̂r.   . La observación zk+1 sirve para mejorar la estimación del estado del sistema. Esta observación se puede representar en función del estado del sistema mediante el modelo no lineal explı́cito de observacion. zk+1 = h j (xm ) + rk+1 En donde j representa la posición (x j , y j ) del objeto con respecto al sistema global de coordenadas 2.4 y rk+1 es ruido blanco Gaussiano que contamina la medida del sensor real y esta dado por: rk+1 ∼ N(0, Rk+1 ) La innovación está dada por la diferencia entre la lectura tomada por el robot z en el tiempo k + 1 y la lectura estimada ẑi con la información almacenada hasta ese instante, esto es [30, 56]: νi = zk+1 − ẑi La covarianza S de la innovación es calculada como: Si = ∇hxa P−a ∇hTxa + R La ganancia del EKF se obtiene según la expresión: Wi = P−a ∇hTxa S−i donde la matriz Jacobiana ∇hxa , es dada por: ∂hi ∇hxa = ∂xa con. x̂−a.   = . −△x d △y d2. −△y d −△x d2. 0 0 ... 0 −1 0 . . . 0. △x d −△y d2. △x = x̂i − x̂r △y = ŷi − ŷr p d = ( x̂i − x̂r )2 + (ŷi − ŷr )2. △y d △x d2.  0 . . . 0   0 ... 0.

(35) 19 La estimación del mapa en el instante k + 1, con toda la información recibida hasta ese instante se calcula como: xˆ+a = xˆ−a + Wi νi P+a = P−a − Wi Si WTi Esta corrección requiere la actualización completa de toda la matriz de covarianzas Pa , es esta etapa la fase más critica en cuestión de coste computacional en todo el proceso de construcción del mapa.. 2.4.2. Adición de nuevos objetos. A medida que el robot explora el entorno va encontrando nuevos objetos los cuales deben ser almacenados en el mapa que tiene guardado hasta ese instante. Primero, el vector del estado del mapa y la matriz de covarianzas son aumentados con la nueva observación z, y la covarianza R, con medidas relativas al robot [23, 51, 16]. x̂aum. Paum.    x̂a   =  z.    Pr Prm 0    =  PTrm Pm 0    0 0 R. La función gi convierte la lectura del objeto z dada en coordenadas polares a el sistema global dado en coordenadas cartesianas [71, 72].     xi   xr + r cos(θ + φr )  =  gi (xv , z) =  yr + r sin(θ + φr ) yi.   . Con lo que la estimación del nuevo estado, ya aumentado, está dado por: x̂+a.   = fi (x̂aum ) = . x̂a gi (x̂v , z).   . El estado de los objetos del entorno que ya existı́an en el mapa permanecen inalterados mientras que el estado del nuevo objeto se calcula como una función del estado del robot y la medida teórica. El resto de objetos del entorno distintos al robot no intervienen en esta ecuación..

(36) 20 La matriz de covarianzas, se calcula como: P+a = ∇fxaum Paum ∇fxTaum La matriz Jacobiana es dada por:. ∇fxaum =. ∂fi ∂xaum. (x̂aum ).   Ir 0 0  =  0 Im 0  ∇gxr 0 ∇gz.     . Donde las matrices Jacobianas ∇gxr y ∇gz se presentan a continuación. ∂gi ∇gxr = ∂xr ∂gi ∇gz = ∂z. (x̂r ,z). (x̂r ,z).   1 0 −r sin(θ + φ̂r ) =  0 1 r cos(θ + φ̂r ).   .   cos(θ + φ̂r ) −r sin(θ + φ̂r ) =  sin(θ + φ̂r ) r cos(θ + φ̂r ).   . Gracias a la dispersión de las anteriores matrices Jacobianas, el costo computacional de la adición de nuevos objetos crece sólo linealmente con el número de objetos del mapa, ya que no es necesario realizar la actualización completa de la matriz Pa. 2.4.3. Clasificación de datos. Cuando el robot toma una lectura de un objeto en su ambiente, es necesario determinar si esa lectura corresponde a un elemento ya almacenado en su mapa o si por el contrario es un nuevo objeto que se deberá adicionar al mapa. Para realizar una asociación correcta se hace necesario emplear una distancia estadı́stica para determinar la similitud entre las variables aleatorias multidimensionales. Para realizar la discriminación con un nivel de confianza α se realiza un test con la distancia de Mahalanobis entre el objeto nuevo y cada uno de los que se tiene alamcenado en el mapa. Esta distancia esta dada por [31, 73, 56, 30, 74]: νiT Si νi < χ2(dim(νi ),α) Este test emplea la métrica o distancia de Mahalanobis de la innovación para aceptar la medida o rechazarla en función de la confianza deseada como se ilustra en la Figura 2.5..

(37) 21. x a Pa xδ. x a Pa z k+1. υi = z k+1− z i. No. 2. υiT S i υi< χ(dim(. υi),α). Si. x a Pa. x a Pa. Figura 2.5: Pasos de la estimación del EKF.. 2.5 Propiedades del algoritmo El EKF para solucionar el problema del SLAM tiene unas interesantes propiedades de consistencia y convergencia, pero también tiene un inconveniente que es su elevado costo computacional. Estas propiedades claves para cualquier algoritmo de construcción de mapas probabilı́sticos, se suelen denominar como las tres C: consistencia, convergencia y costo computacional. La consistencia es una propiedad de la estimación del estado x̂a , la cual consiste en la coherencia entre dicha estimación del estado y el estado real del entorno. La consistencia se define como [16, 6, 75]: x̂a ,Pa es consitente si Pa − E[(xa − x̂a )(xa − x̂a )T ] es semidefinida positiva. Dicho de otra forma, el estado real del entorno debe estar dentro del margen establecido por la media y la varianza de la estimación del mismo. Por lo que, una varianza injustificadamente pequeña dejará fuera de su intervalo de confianza el estado real del entorno, lo cual conlleva a la divergencia.

(38) 22 del algoritmo. Esto puede suceder por errores en el cálculo de la matriz de covarianza, ya sean numéricos o lógicos, o por las linealizaciones realizadas. Las propiedades de convergencia del EKF aplicado al problema SLAM se encuentran condensadas en varios teoremas enunciados y demostrados en [16] y completados en [23] y [21]. • El determinante de la matriz de covarianzas de los objetos del mapa, excluido el robot y cualquier submatriz principal como la varianza de cada objeto, es una función monótona no creciente del tiempo, es decir, decrece a medida que se incorporan observaciones al filtro. • Cuando el número de observaciones tiende a infinito, la estimación de los objetos del mapa excepto el robot, tiende a estar totalmente correlacionados. Esto quiere decir que la distancia relativa entre dos objetos cualesquiera del mapa tienden a ser conocida perfectamente, con incertidumbre nula. • Cuando el número de observaciones tiende a infinito, la covarianza de cada objeto del mapa tiende a un lı́mite inferior definido por la covarianza inicial del robot. La propiedad de consistencia está intimamente relacionada con la convergencia. En la tesis [16,76] se demuestra que para obtener una estimación consistente mediante el EKF, es necesario mantener la matriz completa Pa , con todas las correlaciones entre los objetos del mapa, y por lo tanto, no se puede obtener una solución correcta al problema sin el costo computacional que ello implica. El ignorar estas correlaciones lleva automáticamente a la inconsistencia y divergencia del algoritmo. El origen de las correlaciones es el hecho de que las observaciones sean siempre relativas al robot. Los teoremas de convergencia unidos a las propiedades de consiste, antes descrita, concluye que el algoritmo EKF tiende a obtener el estado real del entorno, a medida que realiza más observaciones [7]. No obstante, hay que reiterar que este resultado se demuestra suponiendo que las linealizaciones efectuadas son despreciables. Es importante recordar que además el filtro de Kalman obtiene la estimación óptima, sin tener en cuenta las linealizaciones realizadas, en el sentido que minimiza el error cuadrático medio y la varianza. El costo computacional del algoritmo queda establecido por el costo computacional en la etapa de corrección la cual crece automáticamente con el número de objetos del mapa..

(39) 3. NAVEGACIÓN REACTIVA UTILIZANDO CAMPOS POTENCIALES La evasión de obstáculos en tiempo real es un problema que ha sido tratado ampliamente con el método de potenciales artificiales para robots móviles [77]. En este método se propone una función llamada potencial artificial, esta función esta compuesta por un potencial atractivo producido por el objetivo que se desea alcanzar y un potencial repulsivo inducido por los obstáculos. El concepto básico de este método es crear un campo potencial artificial en el que navega el robot, donde el objetivo representa el mı́nimo global y cerca de los obstáculos el gradiente negativo del potencial es repulsivo y crece indefinidamente. La estrategia de control consiste en dirigir el robot hacı́a el negativo del gradiente de una función de potencial, actuando ası́, el objetivo como un atractor y los obstáculos como repulsores. El método clásico fue desarrollado por Khatib [78] y permite de una manera sencilla y elegante la evasión de obstáculos para planificación global de trayectorias [79, 80], y para la navegación de robots [81]. Se debe notar que este método se adapta a la problemática del control de movimiento del robot, porque la fuerza que proviene del potencial artificial permite el cálculo de una dirección de desplazamiento proporcional a la intensidad de esta fuerza [82]. De esta manera, el gradiente se toma como el campo vectorial de desplazamientos del robot y la fuerza se emplea para orientar el vector de desplazamiento del robot colineal al gradiente. Algunas de las ventajas de este método radican en la continuidad de las fuerzas resultantes que se generan, en la posibilidad de trabajar tanto con obstáculos fijos como móviles, ası́ como en la poca demanda computacional que se traduce en una implementación en tiempo real. Sin embargo, este método tiene algunas limitaciones inherentes, entre las que se encuentran situaciones de bloqueo debido a la aparición de mı́nimos locales, imposibilidad del robot para pasar entre obstáculos muy cercanos, oscilación en presencia de obstáculos y objetivos no alcanzables por obstáculos cercanos. El método de potenciales artificiales desarrollado por Khatib [78], para aplicaciones en robots manipuladores y robots móviles ha sido ampliamente utilizado para la evasión de obstáculos de manera local. Krogh extendió este concepto tomando en cuenta la velocidad del robot en la vecindad de los obstáculos y junto con Thorpe [83] crean el campo potencial generalizado para planificación de trayectorias globales y locales. El método es muy utilizado debido a su fácil implementación en tiempo real, es por esto que se ha tratado de dar solución a sus limitaciones inherentes y de ahı́ se 23.

(40) 24 derivan la mayorı́a de las publicaciones que han aparecido en la última década. Para solucionar el problema de los mı́nimos locales se han desarrollado modificaciones a este método como la que proponen Borenstein y Koren [84], que básicamente es una combinación del método de potenciales para la planificación local y un método heurı́stico basado en el modelado del espacio de trabajo por medio de un enrejado bidimensional al que le llama campo de fuerzas virtuales (VFF Virtual Force Field). Volpe y Khosla [79], proponen una función potencial supercuadrática, que es un función matemática utilizada comúnmente para modelar los obstáculos, está función previene la creación de mı́nimos locales cuando se añade un pozo atractivo con simetrı́a esférica. Un campo potencial rotativo se presenta en [82], el cuál rodea al obstáculo por tres envolturas con la idea de definir una fuerza repulsiva ortogonal al obstáculo cuando el robot está cerca de él, paralela al obstáculo cuando el vehı́culo está a una cierta distancia más lejos y progresivamente aleja del obstáculo al robot móvil. En [81] se incluyen funciones de navegación para eliminar los mı́nimos locales. Ge y Cui en [85], presentan una nueva función de potencial que toma en cuenta la distancia relativa entre el robot y la meta, la cual asegura que la posición de la meta es un mı́nimo global de todo el potencial, solucionando ası́ el problema de las metas no alcanzables con obstáculos cercanos (GNRON - Goals Non-Reachable with Obstacles Nearby). En la literatura se han utilizado cuatro diferentes enfoques para los campos potenciales artificiales: • Acoplamiento del potencial artificial directamente al sistema vı́a el control: en otras palabras, el control, ya sea fuerza o torque, es aplicado colinealmente al gradiente del campo potencial, implantando una relación dinámica entre el robot y su ambiente. Esta aproximación no conlleva a un seguimiento del gradiente. Sin embargo, se garantiza la evasión por potenciales que tienden al infinito en los lı́mites del obstáculo. • Consideración de la dinámica del robot y las limitaciones de los actuadores en el diseño del potencial: un ejemplo es el campo potencial generalizado de Krogh, que considera el tiempo necesario para alcanzar la meta o un obstáculo, en lugar de la distancia [77]. Un doble integrador modela al robot móvil tomando en cuenta la saturación del actuador. • Uso de un control retroalimentado para seguimiento del gradiente del campo potencial: Khatib [78], propone una estrategia de linealización por retroalimentación que requiere el conocimiento exacto de la dinámica del robot. Koditschek [81], elimina de este requerimiento con su enfoque de movimiento natural (natural motion) para sistema mecánicos, extendiendo la filosofı́a del Control PD a sistemas mecánicos no-lineales. La idea es diseñar una función de navegación y seguir su gradiente dependiendo implı́citamente de la inercia.

(41) 25 del sistema. Este enfoque está limitado a sistemas mecánicos disipativos no sujetos a fuerzas externas como la gravedad. • Utilización del gradiente como un vector de velocidades deseadas para el robot: el control fuerzas-torques son empleadas para orientar el vector de velocidad del robot colineal al gradiente, el cual resulta en seguimiento exacto de las trayectorias deseadas delineadas por las lı́neas de gradiente [80]. La invarianza a lo largo de las lı́neas de gradiente se alcanza vı́a un control de modos deslizantes, requiriendo únicamente el conocimiento del gradiente y de los lı́mites del sistema.. 3.1 Método de los potenciales artificiales Para este trabajo se utiliza el método de potenciales artificiales desarrollado por Khatib [78], que permite resolver el problema de la evasión de obstáculos, por medio de un control sencillo y elegante, el cuál es fácilmente implementable para aplicaciones en tiempo real. Este método ha sido modificado por muchos autores para solucionar algunos de sus problemas inherentes, sin embargo, las modificaciones resuelven algunos de los problemas por separado y no necesariamente de una manera óptima, por lo que en este trabajo se utiliza el método sin modificaciones, es decir, el método clásico. Las principales ventajas de este método radican en que: • Es un algoritmo simple obtenido por medio de un método analı́tico. • Existe continuidad en la fuerza resultante. • Funciona para obstáculos tanto fijos como móviles. Su mayor desventaja es la posible existencia de mı́nimos locales para ciertas configuraciones del objetivo y los obstáculos. Otra de sus desventajas es que presenta oscilación en presencia de obstáculos y en pasajes angostos. El método clásico consiste en la creación de un campo potencial artificial en el cuál la meta constituye un polo atractivo y los obstáculos son superficies repulsivas como se ilustra en la Figura 3.1. El objetivo al cuál se quiere llegar es un mı́nimo global de la función de potencial y cerca de los obstáculos la función de potencial crece indefinidamente como se puede apreciar en al Figura 3.2. El robot navega siguiendo el negativo del gradiente de esta función hasta alcanzar su mı́nimo global como se muestra en la Figura 3.3. El gradiente indica la dirección del máximo ascenso y en este.

(42) 26. Obstáculo. 4. 2. Objetivo. 0. Obstáculo. −2. −4. −6 −6. −4. −2. 0. 2. 4. 6. Figura 3.1: Campo de potencial artificial.. caso se pretende avanzar hacı́a donde decrece la función para alcanzar un mı́nimo en la meta, por lo se toma el negativo del gradiente. La fuerza que se obtiene del potencial artificial induce una aceleración proporcional y colineal a ésta. La fuerza de atracción producida por la meta genera una trayectoria continua que apunta siempre hacı́a ésta. La fuerza de repulsión producida por los obstáculos se dirige de manera que se aleja de éstos. Al combinar estas dos fuerzas y al dirigir el vehı́culo en dirección de la fuerza resultante es posible obtener un control efectivo. Cuando las fuerzas de repulsión y atracción tienen la misma magnitud y dirección contraria, el vehı́culo queda atrapado en un mı́nimo local.. 3.1.1. Potencial clásico. El método que se presenta en este trabajo es el propuesto por Khatib [78]. Se empezará formalizando el problema para un obstáculo y la meta y se extenderá al caso general. Considérese un espacio de trabajo bidimensional y un sistema de referencia fijo (X1 , X2 ) en el que se mueve el vehı́culo articulado. Las coordenadas de un punto de referencia del robot están dadas por q = (y1 , y2 ), las coordenadas del obstáculo están dadas por qobs = (s1 , s2 ) y las del objetivo son.