SLAM con mediciones angulares: método por triangulación estocástica

Ingeniería Investigación y Tecnología, volumen XIV (número 2), abril-junio 2013: 257-274 ISSN 1405-7743 FI-UNAM (artículo arbitrado) SLAM con mediciones angulares: método por triangulación estocástica Bearing-Only SLAM: Stochastic Triangulation Method Información del artículo: recibido: abril de 2012, aceptado: agosto de 2012 Munguía-Alcalá Rodrigo Francisco Departamento de Ciencias Computacionales Centro Universitario de Ciencias Exactas e Ingenierías Universidad de Guadalajara, Jalisco Correo: [email protected] Grau-Saldes Antoni Departamento de Ingeniería de Sistemas, Automática e Informática Industrial Universidad Politécnica de Cataluña, Barcelona, España Correo: [email protected] Descriptores: • SLAM • vehículos autónomos • sensores angulares • localización • mapeo • navegación de robots Resumen El SLAM (simultaneous localization and mapping) es una técnica en la cual un robot o vehículo autónomo opera en un entorno a priori desconocido, utilizando únicamente sus sensores de abordo, mientras construye un mapa de su entorno, el cual utiliza al mismo tiempo para localizarse. Los sensores tienen un gran impacto en los algoritmos usados en SLAM. Enfoques recientes se están centrando en el uso de cámaras como sensor principal, ya que generan mucha información y están bien adaptadas para su aplicación en sistemas embebidos: son ligeras, baratas y ahorran energía. Sin embargo, a diferencia de los sensores de rango, los cuales proveen información angular y de rango, una cámara es un sensor proyectivo que mide el ángulo (bearing) respecto a los elementos de la imagen, por lo que la profundidad o rango no puede ser obtenida mediante una sola medición. Lo anterior ha motivado la aparición de una nueva familia de métodos en SLAM: los métodos de SLAM basados en sensores angulares, los cuales están principalmente basados en técnicas especiales para la inicialización de características en el sistema, permitiendo el uso de sensores angulares (como cámaras) en SLAM. Este artículo presenta un método práctico para la inicialización de nuevas características en sistemas de SLAM basados en sensores angulares. El método propuesto implementa mediante un retardo una técnica de triangulación estocástica para definir una hipótesis para la profundidad inicial de las características. Para mostrar el desempeño del método propuesto se presentan resultados experimentales con simulaciones y también se presentan dos casos de aplicación para escenarios con datos reales procedentes de distintos sensores angulares. SLAM con mediciones angulares: método por triangulación estocástica Ingeniería Investigación y Tecnología, volumen XIV (número 2), abril-junio 2013: 257-274 ISSN 1405-7743 FI-UNAM 258 Introducción La localización y mapeo simultáneos, SLAM por sus siglas (simultaneous localization and mapping), es sin duda uno de los problemas más importantes a resolver en robótica, en el camino de construir robots móviles verdaderamente autónomos. Las técnicas de SLAM se refieren a cómo un robot móvil opera en un entorno a priori desconocido utilizando únicamente sus sensores de abordo, mientras construye un mapa de dicho entorno, el cual a su vez utiliza para localizarse. Los sensores del robot tienen un gran impacto en los algoritmos usados en SLAM. Los primeros enfoques se centraron en el uso de sensores de rango como sónares o láseres (Auat et al., 2011), (Vázquez et al., 2009). Sin embargo, hay algunas dificultades con el uso de este tipo de sensores en SLAM: la asociación de datos es difícil, son costosos y a menudo están limitados a mapas 2D. Puede consultarse (Durrant y Bailey, 2006), para una revisión del problema general del SLAM. Las cuestiones anteriores han motivado que mucho del trabajo reciente se esté centrando en el uso de cámaras como sensor principal. Las cámaras se han vuelto muy interesantes para la comunidad investigadora en robótica porque proveen cuantiosa información, son ligeras, baratas y de bajo consumo de energía. Usando visión artificial, un robot puede localizarse a sí mismo utilizando objetos comunes del entorno como puntos de referencia (landmarks). Por otro lado, mientras que los sensores de rango proveen información angular y de rango (profundidad), una cámara es un sensor proyectivo, el cual mide el ángulo (bearing) respecto a características en la imagen. Por tanto, la información de profundidad (rango) no puede ser obtenida en una sola toma. Este hecho ha propiciado la aparición de una nueva familia de métodos de SLAM, que está basada en técnicas especiales para la inicialización de características con la finalidad de permitir el uso de sensores angulares (como cámaras) en los sistemas de SLAM. En años recientes varias mejoras y variantes a este tipo de métodos han aparecido en la literatura (Williams et al., 2007), (Chekhlov et al., 2006), así como diferentes esquemas para incrementar el número de características soportadas en el mapa (Munguía y Grau, 2009). Sin embargo, el proceso de la inicialización de nuevas características continúa siendo quizás el aspecto más importante a tener en cuenta en los sistemas de SLAM basados en mediciones angulares para incrementar la robustez. En este artículo, se presentan diferentes resultados de la investigación de los autores de acuerdo con la problemática del SLAM basado en sensores angulares. Particularmente, se presenta un algoritmo llamado método por triangulación estocástica, el cual aborda el problema de la inicialización de nuevas características en el mapa. Descripción del problema El SLAM con mediciones angulares es un problema de SLAM parcialmente observable, en el cual el sensor utilizado para percibir el entorno del robot provee única- Abstract The SLAM or Simultaneous Localization and Mapping, is a technique in which a robot or autonomous vehicle operate in an a priori unknown environment, using only its onboard sensors to simultaneously build a map of its surroundings and use it to track its position. The sensors have a large impact on the algorithm used for SLAM. Recent approaches are focusing on the use of cameras as the main sensor, because they yield a lot of information and are well adapted for embedded systems: they are light, cheap and power saving. However, unlike range sensors which provide range and angular information, a camera is a projective sensor which measures the bearing of images features. Therefore depth information (range) cannot be obtained in a single step. This fact has propitiated the emergence of a new family of SLAM algorithms: the Bearing-Only SLAM methods, which mainly rely in especial techniques for features system-initialization in order to enable the use of bearing sensors (as cameras) in SLAM systems. In this work a practical method is presented, for initializing new features in bearing-only SLAM systems. The proposed method, defines a single hypothesis for the initial depth of features, by the use of an stochastic technique of triangulation. Several simulations as well two scenarios of applications with real data are presented, in order to show the performance of the proposed method. Keywords: • SLAM • autonomous-vehicles • bearing-sensors • localization • mapping • robot-navigation 259 Munguía-Alcalá Rodrigo Francisco y Grau-Saldes Antoni Ingeniería Investigación y Tecnología, volumen XIV (número 2), abril-junio 2013: 257-274 ISSN 1405-7743 FI-UNAM mente información angular en cuanto a puntos de referencia (landmark) del entorno. Dicha información no es suficiente para calcular el estado completo de la característica (la cual modela el punto de referencia) mediante una sola medición. Para inferir la profundidad de una característica el sensor debe observarla repetidamente mientras el robot se mueve libremente a través de su entorno, estimando el ángulo entre la característica y el centro del robot. La diferencia entre las mediciones angulares es el paralaje (parallax). De hecho, el paralaje es la clave que permite la estimación de la profundidad de las características. En el caso de trayectorias en entornos de interiores, un movimiento de centímetros es suficiente para producir paralaje, por otro lado, cuanto mayor sea la distancia donde se encuentre la característica, más tendrá que viajar el sensor para producir paralaje. a) Modelo de movimiento del robot Para un entorno 2D, se considera un robot y/o vehículo de configuración diferencial, cuya posición y orientación están definidas por x  v , y el cual está equipado con un sensor capaz de realizar mediciones z(k) angulares y de distancia, θ y r, respectivamente. (1) Con el modelo predictivo discreto, para el movimiento del robot (2) siendo δ la distancia recorrida por el centro del robot y θ’ el giro realizado por el robot a cada instante k (3) donde δr y δl representan la distancia recorrida por cada rueda del robot y a la separación entre las mismas. Habitualmente δr y δl se obtienen mediante los codificadores de posición (encoders) de las ruedas del robot. Mediante el uso de un Filtro de Kalman Extendido se propagan las estimaciones de la posición del robot, así como también las estimaciones de las características. b) Medición e inicialización de las características Para el contexto mencionado, se considera el tipo de característica más simple: un punto tal que para una característica dada i (4) El estado completo x , el cual incluye todas las características i , está conformado por En este caso la ecuación de medición hi(x v ,i) viene dada por (6) Donde atan2 es una función que toma dos argumentos (x e y), para calcular el arco tangente de y/x, dentro de un rango [-π, π]. Para inicializar una nueva característica i en el mapa, es necesario invertir hi(x v ,i) a i(z(k),x v). En este caso invirtiendo (6) tenemos que (7) c) Caso de SLAM con mediciones angulares En el caso particular de SLAM con mediciones angulares, el sensor principal proveería únicamente lecturas z(k) tal que z(k) = [θ] (8) En este caso, considerando la ecuación de medición hi(x v ,i) (9) Por inspección se observa que no sería posible invertir la ecuación (9) para obtener una función i(z(k),x v) con la cual inicializar el estado completo de la característica i = [xi,yi]. En este caso, una posibilidad podría ser el definir una característica parcialmente inicializada con una forma geométrica diferente (por ejemplo, una línea sobre la cual se asuma que el punto debería encontrarse). ˆ x() v vv v x ky θ   =   () r zk θ  =  ( 1) ( ) ( 1) ( ) ( 1) ( ) cos( ) sin( ) vk vk vk vk vk vk xx yy δθ δθ θ θθ + + +    ′ +    ′ = +       ′ +    ( ) /2 rl δ δδ = + ( ) / rl a θ δδ ′= − ( ) 22 ( )( ) ˆˆ (x ,y ) atan2 , iv iv ivi i vi v v rxx yy hy yx x θθ   − +− = =   − −−    ( ) ( ) cos ˆ ysin i vv i i vv xr x yr y θθ θθ  ++  = =   ++   [ ] ( ) ˆˆ (x , y ) atan2 , i v i i vi v v h y yx x θθ  == − −−  ˆ y i i i x y  =  v12 n ˆ ˆˆˆ ˆ x x , y , y ,....y T TT T T  = SLAM con mediciones angulares: método por triangulación estocástica Ingeniería Investigación y Tecnología, volumen XIV (número 2), abril-junio 2013: 257-274 ISSN 1405-7743 FI-UNAM 260 Tenemos entonces que en el proceso de inicialización de características, en el SLAM con mediciones angulares, una sola medición es insuficiente para determinar la posición completa de la característica. Por tan- to, se requieren al menos dos mediciones angulares θi y θj, realizadas en dos posiciones diferentes del vehículo x vi y x vj (figura 1). donde: (10) Si las mediciones angulares y la posición del vehículo fueran perfectamente conocidas, el cálculo de la posición de la característica seria trivial mediante la función g(x vi, x vj, θi, θj) (10). Sin embargo, la estimación de la posición de la característica puede estar mal condicionada dependiendo de diferentes factores: i) la incertidumbre en la estimación de las posiciones del vehículo, ii) la incertidumbre en las mediciones angulares, y iii) las particularidades del desplazamiento entre ambas posiciones del vehículo, las cuales producen el paralaje. Trabajo relacionado El SLAM basado en mediciones angulares (Bearing- Only SLAM) ha recibido mucha atención por parte de la comunidad investigadora en robótica en la década presente y en la pasada, y particularmente el SLAM monocular, el cual constituye un caso particular del SLAM en el que una cámara de video representa la única entrada sensorial del sistema. En Deans y Martial (2000), se propone una combinación entre optimización global (ajustamiento de burbu- ja) para la inicialización de características y un filtro de Kalman para la estimación del estado. En este método, debido a la limitación impuesta por la línea-base, en la cual las características pueden ser inicializadas, y dependiendo del movimiento de la cámara y la locación de los objetos en el entorno, algunas características no pueden ser inicializadas. En Strelow y Sanjiv (2003) se propone un método similar, pero combinando una cámara con sensores inerciales mediante un Filtro de Kalman Iterado (IEKF). En el trabajo de Bailey (2003) se presenta una variante de inicialización restringida (constrained initialization), donde estimaciones pasadas de la posición del vehículo son retenidas en el estado del SLAM conjuntamente con las mediciones respectivas, de manera que la inicialización de las características puede llevarse a cabo hasta que el paralaje sea suficiente para permitir una inicialización Gaussiana de la característica. El criterio utilizado para determinar si la estimación está bien condicionada es la denominada distancia de Kullback. La complejidad del método de muestreo propuesto para evaluar esta distancia es considerablemente alta. s sin( ) s sin( ) c cos( ) c cos( ) i vi i j vj j i vi i j vj j θθ θθ θθ θθ =−=− =−=− () () vi i j vj j i vj vi i j i j ji vj i j vi j i vi vj i j i j ji x sc x s c y y cc sc sc y sc y s c x x cc sc sc − +−   −   − +−  −   ˆ ˆˆ y (x , x , , ) i vi vj i j g θθ = = Figura 1. Inicialización de una característica mediante la intersección de dos mediciones angulares 261 Munguía-Alcalá Rodrigo Francisco y Grau-Saldes Antoni Ingeniería Investigación y Tecnología, volumen XIV (número 2), abril-junio 2013: 257-274 ISSN 1405-7743 FI-UNAM Aparte del Filtro del Kalman, se utilizan habitualmente otras técnicas de estimación como los filtros de partículas (PF) en el SLAM basado en mediciones angulares. En Kwok y Dissanayake (2004) se propone una variación al PF estándar para remediar el problema del empobrecimiento del muestreo en este tipo de sistemas. El estado inicial de las características se aproxima utilizando una suma de funciones de distribución Gaussianas, la cual define un conjunto de hipótesis para la posición de los objetos, incluyendo, desde un principio, a las características dentro del mapa. En observaciones subsiguientes, se pasa un test estadístico para eliminar hipótesis débiles. En Kwok y Dissanayake (2005) se extiende el algoritmo anterior mediante un filtro de suma de Gaussianas. Este método es quizás el primer método de inicialización de características sin retardo propuesto en la literatura. El principal inconveniente de este método reside en que el número de filtros requeridos crece exponencialmente con el número de características, aumentando exponencialmente la carga computacional. Algunos de los avances más notables en SLAM basado en mediciones angulares fueron presentados en Davison (2003), quien mostró la factibilidad del SLAM en tiempo real utilizando una única cámara (SLAM Monocular) como fuente sensorial del sistema. Su propuesta está basada en el Filtro de Kalman Extendido como técnica de estimación del estado y un filtro de partículas para incorporar nuevas características al mapa mediante un esquema de inicialización parcial. En este caso, el PF no está correlacionado con el resto del mapa, manteniendo un conjunto de hipótesis de profundidad uniformemente distribuidas a lo largo de la línea de vista de cada nueva característica. Después cada nueva observación se utiliza para actualizar la distribución de las posibles profundidades, hasta que la variancia sea suficientemente pequeña para considerarse una estimación Gaussiana. En este punto la característica se agrega al mapa como una entidad tridimensional. Una desventaja de este enfoque reside en que la distribución inicial de partículas tiene que cubrir todos los posibles valores de profundidad, haciéndolo difícil de aplicar cuando se detecta un gran número de partículas, o bien, cuando se consideran características distantes en el entorno. Jensfelt et al. (2006) presentan un método donde la idea consiste en retrasar N pasos la estimación del SLAM, utilizando dichos N pasos para determinar buenos candidatos a ser considerados como características del mapa, así como también estimar su posición 3D. El principal objetivo de este trabajo se centra en el manejo del mapa para lograr un desempeño en tiempo real. Sola et al. (2005) presentan un método basado en el Filtro de Kalman Iterado. Para la inicialización de características utiliza una aproximación al método de Filtro de Suma de Gaussianas, el cual permite inicializar sin retraso las características en el mapa. En Eade y Drummond (2006) se propone un método basado en el FastSLAM (Montemerlo et al., 2002). En este tipo de enfoque, la posición del robot es representada por partículas, mientras que un conjunto de filtros de Kalman refina la estimación de las características. En Montiel et al. (2006) se presenta un método en donde, no existe una transición explícita de un estado “parcial” a uno “totalmente” inicializado para cada característica. En este sentido, las características son inicializadas en el primer instante en el cual son observadas mediante una profundidad inversa e incertidumbres predeterminadas heurísticamente a cubrir un amplio rango de profundidad. Por lo que características lejanas pueden ser representadas e incluidas en el mapa. La tabla 1 presenta un resumen de los métodos anteriormente mencionados. En los métodos con retardo, Método Con o sin retardo Representación inicial Estimación (Deans y Martial, 2000) Ret. Ajuste burbuja EKF (Strelow y Sanjiv, 2003) Ret. Triangulación IEKF (Bailey, 2003) Ret. Triangulación EKF (Davison, 2003) Ret. Hipótesis múltiple EKF (Kwok y Dissanayake, 2004) Ret. Hipótesis múltiple PF (Kwok y Dissanayake, 2005) Sin Ret. Hipótesis múltiple PF (Sola et al., 2005) Sin Ret. Hipótesis múltiple EKF (Jensfelt et al., 2006) Ret. Triangulación EKF (Eade y Drummond, 2006) Ret. Hipótesis sencilla FastSLAM (Montiel et al., 2006) Sin Ret. Hipótesis sencilla EKF (Este trabajo) Ret. Triang.-hipótesis sencilla EKF Tabla 1. Resumen de métodos SLAM con mediciones angulares: método por triangulación estocástica Ingeniería Investigación y Tecnología, volumen XIV (número 2), abril-junio 2013: 257-274 ISSN 1405-7743 FI-UNAM 262 una característica observada en el instante t se agrega al mapa en un tiempo subsiguiente t + k. Por lo general este retardo es utilizado en los diferentes métodos para recabar información que permita inicializar robustamente las características en el mapa. Por otro lado, los métodos sin retardo se aprovechan de la observación de la característica desde el instante t para localizar el vehículo, pero la actualización del mapa estocástico debe calcularse cuidadosamente. Respecto a la representación inicial, en la tabla 1, el ajuste de burbuja se refiere a métodos que están basados en procesamiento por lotes, y buscan una solución óptima utilizando todas las mediciones disponibles. Los métodos de triangulación calculan explícitamente la intersección de las líneas definidas por las observaciones, éstos requieren definir una condición para considerar la estimación bien condicionada. Los métodos de hipótesis sencilla definen la representación inicial de las características con la única función de distribución de probabilidad. A menudo estos métodos son fáciles de calcular, pero requieren atención con respecto a la linealidad en la ecuación de medición. Los métodos de hipótesis múltiple definen la representación inicial de las características con una función de probabilidad múltiple. Usualmente son métodos robustos en relación con la linealidad de la ecuación de medición, pero su complejidad es alta. La columna estimación se refiere a la técnica empleada para el proceso de estimación principal del SLAM. La mayoría de los algoritmos están basados en el Filtro de Kalman Extendido, pero otros emplean filtros de partículas u otras variaciones, como FastSLAM. Parametrización de la profundidad inversa Como se mencionó en la sección anterior, los métodos basados en hipótesis múltiples son robustos para su aplicación con problemas altamente no lineales, a costa de una complejidad de implementación usualmente alta. En contraste, los métodos basados en una sola hipótesis (de implementación menos compleja), como el EKF, el cual recurre a la linealización de primer orden, sufren cuando la incertidumbre no puede ser representada de manera Gaussiana. a) Mejora de la linealidad En Eade y Drummond (2006) y Montiel et al. (2006) se ha mostrado que el uso de una parametrización inversa para la profundidad de las características mejora notablemente la linealidad de la ecuación de medición, incluso para pequeños cambios en la posición del sensor, los cuales producen pequeños cambios en el ángulo de paralaje. Este hecho permite representar la incertidumbre en la profundidad de un rango (desde lo cercano hasta lo infinito) de manera Gaussiana, permitiendo a su vez, utilizar un esquema estándar EKF para la implementación robusta del SLAM con mediciones angulares. Tomando como referencia la parametrización propuesta en Montiel et al. (2006), una característica en un entorno 2D estaría definida por el vector de 4 dimensiones (11) el cual modela un punto localizado en (12) Siendo xi e yi las coordenadas del centro del vehículo cuando la característica es observada por primera vez, y θi representa el acimut (respecto al marco de referencia global W) para el vector direccional unitario m(θi). La profundidad del punto di se codifica por su inversa ρi = 1/di (figura 2). La figura 3 muestra la reconstrucción de un punto mediante mediciones angulares afectadas de ruido a diferentes paralajes, usando tanto la parametrización euclidiana (figura 3a), como la profundidad inversa (figura 3b). b) Inicialización sin retardo En el método presentado por Montiel et al. (2006) no existe una transición explícita de un estado “parcial” a uno “totalmente” inicializado, esto significa que las características son agregadas al mapa y expresadas mediante su representación final desde que son observadas por primera vez. En lo subsiguiente se referirá a esta propuesta como método de la profundidad inversa sin retardo (o PI-SR). Para el método PI-SR, el valor inicial para ρi y su variancia inicial asociada σρ 2, se determinan heurísticamente para cubrir una región de aceptación de 95%, es decir, un enorme rango de profundidad [dmin ,∞] codificado en su inversa ρi = 1/di, tal que ρini= ρmin / 2, σρ(ini) = ρmin / 4, ρmin = 1/dmin. Entonces una nueva característica new , detectada en un instante k, se compone de manera que (13) [ ] ˆ y T i iii i xy θρ = ( ) 1 i i ii xm y θ ρ  +   new ˆ y ini v i v i vk i i x x y y z θ θ ρ ρ     = =   +    263 Munguía-Alcalá Rodrigo Francisco y Grau-Saldes Antoni Ingeniería Investigación y Tecnología, volumen XIV (número 2), abril-junio 2013: 257-274 ISSN 1405-7743 FI-UNAM donde xv , yv , θv se toman directamente del estado actual x v , ρini es la profundidad inversa inicial y zk es medición angular inicial. El nuevo estado del sistema x  se compone simplemente agregando la nueva característica new al final del vector de estados, tal como se muestra en la ecuación (5). Por ejemplo, para agregar una nueva característica en un mapa con 2 características, el nuevo vector resultaría (14) La matriz de covariancias del estado del sistema P, es expandida en el proceso de inicialización por 1 2 ˆ x ˆˆ xy ˆ y v   =   1 new 2 new ˆ x ˆ y ˆ xˆ y ˆ y v    =   Figura 3. Simulación de la reconstrucción de un punto mediante observaciones con diferentes paralajes. La posición del vehículo es conocida. Se introduce un error de σθ=1° (grados) en las mediciones angulares. Las graficas (a) y (b) muestran la evolución de la función de probabilidad para la profundidad d y la profundidad inversa ρ, mientras el paralaje de las observaciones crece: en (a), la función de probabilidad converge a una forma Gaussiana, pero las estimaciones iniciales son altamente no Gaussianas. En contraste, las funciones de probabilidad para la profundidad inversa (b) (la abscisa está en metros inversos) son notablemente Gaussianas incluso para pequeños paralajes Figura 2. Parametrización de una característica 2D mediante la profundidad inversa SLAM con mediciones angulares: método por triangulación estocástica Ingeniería Investigación y Tecnología, volumen XIV (número 2), abril-junio 2013: 257-274 ISSN 1405-7743 FI-UNAM 264 (15) Donde J es el Jacobiano de la función de inicialización, σz 2 es la variancia del sensor angular y σρ(ini) es la variancia que representa la incertidumbre de la profundidad inversa inicial. c) Casos de prueba El método de la parametrización inversa sin retardo es, sin duda, una buena opción para su implementación en sistemas de SLAM basados en sensores angulares, debido a su claridad y escalabilidad. Por otro lado, en experimentos basados en simulación, se encontró que bajo ciertas circunstancias los parámetros iniciales fijos ρini y σρ(ini) tienen que ser sintonizados manualmente para asegurar la convergencia del algoritmo. Considérese, por ejemplo, las pruebas experimentales ilustradas en la figura 4, en las cuales un vehículo equipado con odometría y un sensor angular (ambos con ruido), se desplaza por una línea recta (realizando SLAM) a través de un entorno, que contiene 5 características (4 cercanas a la trayectoria del vehículo y una más distante). El gráfico en la figura 4a ilustra tres casos encontrados en la estimación final del algoritmo, de izquierda a derecha: i) estimación final con una gran deriva (en rotación), ii) divergencia y iii) estimación correcta del algoritmo. Las pruebas consistieron en variar los parámetros iniciales (ρini, σρ(ini)) y observar el efecto en la estimación final, ejecutando 20 veces el algoritmo para cada caso. En el primer caso, (figura 4b) los parámetros iniciales fueron establecidos de manera que las características inicializaran a una distancia media entre el vehículo y la característica más distante (ρini=0.01, σρ(ini)=0.005). En este caso se encontró una gran deriva en las estimaciones finales para casi todas las ocasiones que se ejecutó el algoritmo. En el siguiente caso, los valores iniciales fueron establecidos para inicializar las características de una posición cercana al vehículo (figura 4c) (ρini= 0.5, σρ(ini)= 0.25), aquí se encontró un deficiente porcentaje de convergencia (aproximadamente 50%), por lo que fue notoria la influencia de la característica más lejana, ya que al retirarla de la simulación (manteniendo los mismos valores iniciales) el porcentaje de convergencia se incrementó 80%. Finalmente en el último caso (figura 4d) se utilizaron diferentes valores iniciales, dependiendo de la cercanía o lejanía de las características de acuerdo con el vehículo. Para las cuatro características cercanas se utilizaron valores iniciales ρini= 0.5, σρ(ini)= 0.25 y para la distante ρini= 0.01, σρ(ini)= 0.005. En este último caso se obtuvo una efectividad de 90%. Con base en los resultados de las diferentes simulaciones realizadas, se puede inferir que la profundidad inversa inicial y su variancia asociada pueden tratarse previamente a su inclusión en el mapa (en lugar de utilizar valores preestablecidos), con tal de incrementar la efectividad del método. Método por triangulación estocástica En la sección anterior se observó que la sintonización manual de los parámetros iniciales mejora la efectividad del método PI-SR. Teniendo en cuenta este contex- to, es razonable considerar la posibilidad de recolectar información acerca de la profundidad, previamente a la inclusión de las características en el mapa. En este sen- 2 2 () 00 00 00 k T new z ini P PJ J ρ σ σ   =   Figura 4. Simulación del método PI-SR utilizando distintos parámetros iniciales de la profundidad inversa y su incertidumbre asociada (ρini, σρ(ini)) para un vehículo desplazándose en línea recta a través de un entorno consistente en 4 características cercanas y una distante 265 Munguía-Alcalá Rodrigo Francisco y Grau-Saldes Antoni Ingeniería Investigación y Tecnología, volumen XIV (número 2), abril-junio 2013: 257-274 ISSN 1405-7743 FI-UNAM tido, la figura 5 muestra que unos cuantos grados de paralaje son suficientes para reducir significativamente la incertidumbre de la profundidad. La idea clave del método propuesto por los autores consiste en: i) estimar dinámicamente la profundidad inicial inversa ρini de manera que ésta sea más consistente a su valor real y ii) incorporar directamente la incertidumbre relacionada con el proceso de estimación de ρini en la matriz de covariancias del sistema cuando la característica es inicializada, en lugar de utilizar un valor inicial σρ(ini). En lo subsiguiente nos referiremos al enfoque propuesto como método por triangulación estocástica o (MTE). a) Puntos candidatos Cuando una característica es detectada por primera vez, se almacena una parte del estado actual x , la matriz de covariancias P y la medición del sensor. Estos datos λi se llaman puntos candidatos y están compuestos por (16) Los valores [x1,y1,θ1] representan la posición actual del vehículo, [σ1(x), σ1(y), σ1(θ)] representan sus variancias asociadas tomadas de la matriz de covariancias del estado Pk, y z1 es la primera medición angular de la característica. En los instantes subsiguientes, la característica se rastrea hasta que se alcanza el umbral de paralaje mínimo αmin. El paralaje α se estima utilizando (figura 6): i) la línea base de desplazamiento b. ii) λi y sus datos asociados. iii) el estado actual del vehículo x v y la medición actual zi. 1 1 1 1 1( ) 1( ) 1( ) ,,,, , , T i xy xy z θ λ θ σσσ  = Figura 6. Parametrización del método MTE Figura 5. Simulación que muestra el decremento de la incertidumbre en la profundidad de una característica con relación al incremento del ángulo de paralaje. Nótese que sólo unos grados de paralaje son suficientes para reducir considerablemente la incertidumbre en la estimación SLAM con mediciones angulares: método por triangulación estocástica Ingeniería Investigación y Tecnología, volumen XIV (número 2), abril-junio 2013: 257-274 ISSN 1405-7743 FI-UNAM 272 20 Figura 12. Estimación de la trayectoria de la cámara y la estructura de la escena a partir de una secuencia de video de 790 fotogramas, capturada en un entorno de escritorio. Se muestran los fotogramas 1, 111, 203, 533 y 688 (columna izquierda) , así como la estimación del algoritmo Figura 12. Estimación de la trayectoria de la cámara y la estructura de la escena a partir de una secuencia de video de 790 fotogramas, capturada en un entorno de escritorio. Se muestran los fotogramas 1, 111, 203, 533 y 688 (columna izquierda) , así como la estimación del algoritmo para dichos fotogramas en una vista X-Y (columna central) y una vista X-Z (columna derecha) 273 Munguía-Alcalá Rodrigo Francisco y Grau-Saldes Antoni Ingeniería Investigación y Tecnología, volumen XIV (número 2), abril-junio 2013: 257-274 ISSN 1405-7743 FI-UNAM las características permite utilizar de manera directa un esquema estándar EKF en el proceso de estimación, simplificando de esta manera su implementación. Por otro lado, en experimentos mediante simulaciones se observó que en ocasiones es necesario sintonizar manualmente los parámetros iniciales de la profundidad inversa para asegurar la efectividad de algoritmo. Este hecho motivó a los autores al desarrollo del método propuesto en el cual, se estiman de manera dinámica los parámetros iniciales de la profundidad inversa. Se presentaron diversas simulaciones para demostrar la validez de la presente propuesta. De igual manera, se presentaron resultados experimentales obtenidos con el método propuesto para distintos escenarios de SLAM utilizando datos reales de sensores. Dichos escenarios experimentales consistieron en: i) un pequeño robot móvil capaz de rastrear la dirección de arribo de una fuente de ultrasonidos y ii) un contexto de SLAM monocular en 6 DOF, en el que una cámara web, capaz de moverse libremente por su entorno, representa la única entrada sensorial del sistema. Es importante mencionar que un beneficio de los métodos sin retardo, reside en el hecho de que las características proveen información acerca de la orientación del sensor desde el principio. Por otro lado, en los métodos con retardo (como el propuesto en este trabajo) puede ser útil esperar hasta que el movimiento del sensor produzca algunos grados de paralaje (por tanto, recabando información de profundidad) con el objetivo de mejorar la robustez del sistema. Más aún, cuando se usan cámaras en entornos reales densos y dinámicos, el retardo puede utilizarse para rechazar características débiles antes de su inclusión en el mapa. Al tenor de los resultados experimentales, se considera que el método propuesto es una buena opción para su implementación en sistemas de SLAM basados en sensores angulares. Referencias Auat F., De la Cruz C., Carelli R., Bastos T. Navegación autónoma asistida basada en SLAM para una silla de ruedas robotizada en entornos restringidos. Revista Iberoamericana de Automática e Informática Industrial, volumen 8 (número 2), 2011: 81-92. Bailey T. Constrained Initialisation for Bearing-Only SLAM, IEEE International Conference on Robotics and Automation ICRA, 2003. Chekhlov D., Pupilli M., Mayol-Cuevas W., Calway A. Real-Time and Robust Monocular SLAM Using Predictive Multi-Resolu- tion Descriptors, en: Proc. Int. Symposium Visual Computing, 2006. Davison A. Real-Time Simultaneous Localization and Mapping with a Single Camera, en: Proceedings of the ICCV, 2003. Deans H., Martial M. Experimental Comparison of Techniques for Localization and Mapping Using a Bearing-Only Sensor, en: International Symposium on Experimental Robotic ISER, 2000. Figura 13. Imagen (compuesta) de la vista superior del entorno experimental (ilustración izquierda). Estimación, mediante el método MTE de la trayectoria de la cámara y la estructura de la escena a partir de 790 fotogramas (ilustración derecha). La escala de la gráfica se encuentra en centímetros. En esta figura se puede observar que tanto la trayectoria de la cámara como la estructura de la escena, fueron recobrados de manera satisfactoria. En ambas ilustraciones se agregan etiquetas para indicar la relación entre algunos objetos del entorno experimental y su correspondientes características del mapa SLAM con mediciones angulares: método por triangulación estocástica Ingeniería Investigación y Tecnología, volumen XIV (número 2), abril-junio 2013: 257-274 ISSN 1405-7743 FI-UNAM 274 Durrant-Whyte H., Bailey T. Simultaneous Localization and Mapping: part I. IEEE Robotics & Automation Magazine, volumen 13 (número 2), 2006: 99-16. Eade E., Drummond T. Scalable Monocular SLAM, en: IEEE Computer Vision and Pattern Recognition CVPR, 2006. Jensfelt P., Folkesson J., Kragic D., Christensen H. Exploiting Distinguishable Image Features in Robotics Mapping and Localization, en: European Robotics Symposium EUROS, 2006. Kwok N.M., Dissanayake G. An Efficient Multiple Hypotheses Filter for Bearing-Only SLAM. International Conference on Intelligent Robots and Systems IROS, 2004. Kwok N.M., Dissanayake G. Bearing-Only SLAM Using a SPRT Based Gaussian Sum Filter. IEEE International Conference on Robotics and Automation ICRA, 2005. Montiel J.M.M., Civera J., Davison A. Unified Inverse Depth Parametrization for Monocular SLAM. Robotics: Science and Systems Conference, 2006. Montemerlo M., Thrun S., Koller D., Wegbreit B. FastSLAM: Factored Solution to the Simultaneous Localization and Mapping Problem. National Conference on Artificial Intelligence AAAI, 2002. Munguía R., Grau A. Monocular SLAM for Visual Odometry. Proceedings of the 4rd IEEE International Conference on Intelligent Signal Processing WISP, 2007. Munguía R., Grau A. Single Sound Source SLAM. Progress in Pattern Recognition, Image Analysis and Applications LNCS, número 5197, 2008:70-77. Munguía R., Grau A. Closing Loops with a Virtual Sensor Based on Monocular SLAM. IEEE Transactions on Instrumentation and Measurement, volumen 58 (número 8), 2009: 2377-2385. Sola J., Devy M., Monin A., Lemaire T. Undelayed Initialization in Bearing only SLAM. IEEE International Conference on Intelligent Robots and Systems IROS, 2005. Strelow S., Sanjiv D. Online Motion Estimation from Image and Inertial Measurements. Workshop on Integration of Vision and Inertial Sensors INERVIS, 2003. Vázquez-Martín R., Núñez P., Bandera A., Sandoval F. Curvature- Based Environment Description for Robot Navigation Using Laser Range Sensors. Sensors, volumen 9 (número 8), 2009: 5894-5918. Williams G., Klein G., Reid D. Real-time SLAM Relocalisation, en: Proceedings of the ICCV, 2007. Semblanza de los autores Rodrigo Francisco Munguía-Alcalá. Obtuvo el grado de doctor por la Universidad Politécnica de Cataluña en control, visión y robótica, en 2009. El grado de maestro en ciencias por la misma universidad en arquitectura de ordenadores, en 2006. Y el título de ingeniero en comunicaciones y electrónica por la Universidad de Guadalajara, en 2002. Actualmente se desempeña como profesor investigador adscrito al Departamento de Ciencias Computacionales del Centro Universitario de Ciencias Exactas e Ingenierías de la Universidad de Guadalajara. Desde 2012 es miembro del Sistema Nacional de Investigadores Nivel C. Antoni Grau-Saldes. Recibió el grado de maestro en ciencias y de doctor en ciencias de la computación por la Universidad Politécnica de Cataluña en 1990 y 1997. Es profesor de la Escuela de Informática de Barcelona. Sus áreas de investigación son la visión por computador, reconocimiento de patrones, robótica, automatización industrial y educación en desarrollo sustentable. Ha publicado más de 100 artículos de investigación y es coautor de tres libros. Ha presidido varias conferencias internacionales y es miembro del IEEE, IAPR y el IFAC. Este artículo se cita: Citación estilo Chicago Munguía-Alcalá, Rodrigo Francisco, Antoni Grau-Saldes. SLAM con mediciones angulares: método por triangulación estocástica. Ingeniería Investigación y Tecnología, XIV, 02 (2013): 257-274. Citación estilo ISO 690 Munguía-Alcalá R.F., Grau-Saldes A. SLAM con mediciones angulares: método por triangulación estocástica. Ingeniería Investigación y Tecnología, volumen XIV (número 2), abril-junio 2013: 257-274.