articulo

Actas de las IX Jomadas Argentinas de Robótica 15-17 de noviembre, Córdoba, Argentina

Estimación de la covarianza de ICP para la localización de un robot diferencial mediante odometría y escaneo láser

ICP covariance estimation for the localization of a differential robot using odometry and laser scan

Instituto de Ciencias de la Computación, Universidad de Buenos Aires, Consejo Nacional de Investigaciones Científicas y Técnicas – Buenos Aires, Argentina

Resumen

En este trabajo se presenta un método probabilístico para resolver el problema de la localización de un robot diferencial. Se usa el Filtro Extendido de Kalman (EKF) para fusionar la información obtenida por registraciones de mediciones láser mediante ICP (IterativeClosest Point) con la información de odometría provista por encoders. Para utilizar EKF es necesario estimar la covarianza de cada fuente de información, sin embargo el algoritmo ICP no devuelve la covarianza asociada. En este artículo se describe una forma de calcular esta covarianza. Los resultados obtenidos muestran que el método de fusión de sensores resulta en una estimación más precisa de la pose del robot en comparación con las estimaciones que se podrían obtener mediante odometría e ICP individualmente.

Abstract

In this work we present a probabilistic method to solve the localization problem of a differential robot. The Extended Kalman Filter (EKF) is used to merge the information obtained from ICP (Iterative Closest Point) laser measurement records with the odometry information provided by encoders. To use EKF it is necessary to estimate the covariance of each information source, however the ICP algorithm does not return the associated covariance. This paper describes a way to calculate this covariance. The results obtained show that the sensor fusion method results in a more accurate estimation of the robot's pose compared to the estimations obtained through odometry and ICP individually.

Introducción

En aplicaciones de robótica móvil resulta indispensable abordar el problema de la localización, es decir, conocer la pose (posición y orientación) del robot en el ambiente. Este problema puede ser abordado en forma absoluta o relativa. En los métodos absolutos, se localiza al robot respecto de un sistema de referencia o mapa, utilizando sensores exteroceptivos que permiten medir u observar directamente el entorno en cada instante de tiempo. No obstante, estos métodos dependen de información previa y/o infraestructura específica (como es el caso del Sistema de Posicionamiento Global o GPS), y no suelen ser muy precisos en la navegación a corto plazo.

Por otro lado, los métodos de localización relativa (también conocidos como dead-reckoning) se basan en la idea de estimar la posición actual en forma incremental, a partir de la integración del desplazamiento respecto de la posición anterior. Un ejemplo clásico de dead-reckoning ampliamente utilizada en robots terrestres con ruedas es la odometría. Esta técnica consiste en usar sensores propioceptivos como los encoders que permiten traducir el giro de las ruedas durante un intervalo de tiempo en un desplazamiento lineal y angular del robot. Debido a que esta traducción de movimiento no suele ser precisa cuando se realizan rotaciones, se suelen incorporar otro tipo de sensores para mejorar la localización (Borenstein et al, 1997).

Los telémetros láser (o sensores láser) son capaces de medir distancias a los objetos del entorno con una resolución lineal de pocos milímetros. Ante cada sensado, se obtiene una nube de puntos que representa el contorno del ambiente dentro del rango de observación del sensor. A partir de obtener una transformación rígida que relacione una nube con la otra, es posible estimar el desplazamiento del robot durante ese intervalo de tiempo. El problema de hallar una transformación rígida que describa cómo se alinean correctamente dos escaneos consecutivos es conocido como método de registración o scanmatching(Gutmann y Schlegel, 1996, Lu y Milios, 1997, Se et al, 2002). Para realizar esta registración, existen diversos algoritmos. Uno de los más utilizados es IterativeClosest Point o ICP (Chen y Medioni, 1992;Besl y McKay, 1992}.

El algoritmo ICP es muy utilizado para resolver el problema de la registración por láser. Sin embargo, asume que siempre habrá algún cambio perceptible del entorno, lo cual no es cierto en determinados contextos, por ejemplo un robot moviéndose en línea recta por un pasillo recto y uniforme. Debido a este problema, conocido como perceptual aliasing, no alcanza con usar un láser con ICP para obtener un sistema fiable de localización relativa. Para abordar este problema, es posible fusionar la estimación obtenida mediante el algoritmo ICP, con la obtenida con la odometría basada en encoders. Esto se pueda hacer utilizando algún tipo de filtro probabilístico, como el Filtro Extendido de Kalman o EKF permite integrar sucesivas observaciones a través del tiempo y provee una predicción del estado del robot en todo momento.

Para utilizar correctamente EKF es necesario contar con un modelo de las incertidumbres de cada fuente de información. Éstas se ven reflejadas en la incertidumbre de cada predicción del estado, representada con una matriz de covarianza. En este artículo es presentar un método de localización probabilístico que fusiona la información obtenida por registraciones de mediciones láser mediante ICP con la información provista por odometría. Para modelar la incertidumbre asociada a las estimaciones obtenidas por el algoritmo ICP, se describe un método para el cálculo de la matriz de covarianza correspondiente.

Conceptos preliminares

En esta sección introduciremos algunos conceptos preliminares como son la odometría basada en encoders, la registración de nubes de puntos obtenidas por un sensor y el algoritmo ICP.

La odometría basada en encoders es una de las técnicas más utilizadas para estimar el movimiento de robots con sistemas de locomoción basados en ruedas. En este trabajo nos focalizamos en el sistema de tracción diferencial. Un vehículo diferencial posee dos ruedas con motores independientes, situadas a ambos lados del chasis. Aún cuando la mayoría de los robots no respetan estrictamente este modelo, por ejemplo en el caso de los que utilizan cuatro ruedas u orugas, el modelo diferencial resulta en una simplificación aceptable.

Las ecuaciones de movimiento utilizadas en este modelo suponen entonces que los movimientos se realizan a partir del control independiente de dos motores, y que el desplazamiento de las ruedas es directamente traducible al desplazamiento real del robot, aunque esto no siempre es cierto. Por esta razón, se genera un error acumulativo y no acotado.

Usando las ecuaciones clásicas de odometría y asumiendo el modelo diferencial, podemos escribir las velocidades lineales (en cada eje de coordenadas respecto del robot) y angular del robot en un instante de tiempo como el vector

, que se calcula como:

Para un movimiento incremental en un intervalo de tiempo, podemos proponer la siguiente matriz de covarianza

asociada los desplazamientos de cada rueda:

donde

son constantes de error que representan parámetros no determinísticos de la transmisión del motor y la interacción de las ruedas con el suelo.

La ecuación anterior se basa en que asumimos que los errores de las ruedas son independientes y que la varianza de los errores de las ruedas izquierda y derecha son proporcionales al valor absoluto de las distancias recorridas. Estas suposiciones, si bien no son perfectas, suelen ser suficientemente buenas. Los valores de las constantes

dependen del robot y del entorno, y deben ser establecidas individualmente en cada caso de forma experimental.

Utilizando la aproximación de Taylor de primer orden para la función

(Ec. 1) se puede aproximar la covarianza

de la siguiente forma:

Una nube de puntos en dos dimensiones puede ser modelada en coordenadas homogénea mediante una matriz

, compuesta de

puntos de la forma:

en coordenadas homogéneas, con lo cual

queda definida de la siguiente forma:

Dos nubes de puntos

se encuentran en una registración cuando para cualquier par de puntos

los mismos representan el mismo objeto en dos escaneos sucesivos. Es decir que existe una transformación rígida

tal que puede alinear todos los puntos de ambas nubes:

En general, se trabaja con transformaciones que directamente relacionan las dos nubes de la forma

. Si ambas nubes de puntos provienen de mediciones obtenidas en momentos sucesivos por un sensor montado en un robot móvil, podemos estimar el movimiento del robot de la siguiente forma: partiendo de una pose inicial

(matriz identidad), la pose del robot en tiempo

se calcula:

donde

es el movimiento estimado mediante la registración de un par de nubes de puntos entre los instantes

El algoritmo ICP introducido por Besl& McKay (1992) y Chen &Medion (1992) permite calcular una registración entre un par de nubes de puntos. La entrada del algoritmo son ambas nubes de puntos, un criterio de parada y, opcionalmente, una estimación inicial de la transformación. La salida del algoritmo es una transformación con un error por debajo de un cierto umbral definido.

1. Para cada punto de la nube de puntos fuente, buscar el punto más cercano en la nube de puntos de referencia y establecer una correspondencia entre ellos. En la primera iteración del algoritmo se aplica la estimación inicial (si se contase con ella) de la transformación a la nube fuente.

2. Calcular la transformación que mejor alinee cada punto de la nube fuente con su correspondiente punto en la nube referencia hallado en el paso anterior.

3. Transformar todos los puntos de la nube fuente mediante la transformación obtenida en el paso anterior.

4. Determinar si la transformación obtenida en el paso anterior es lo suficientemente buena usando como función de costo basada en el error.

5. Si la transformación obtenida es suficientemente buena, terminar, sino volver al paso 1.

Para establecer correspondencias entre la nube fuente y la nube referencia (paso 1) se suele realizar la búsqueda del k-ésimo vecino más cercano utilizando un árbol k-dimensional (Greenspan y Yurick, 2003) . Con los pares de puntos ya apareados se eliminan outliers que podrían ser resultado de errores de medición y se construye una transformación

que describe cómo se alinean aproximadamente. Es decir, para

, un punto de la nube fuente y un punto de la nube referencia respectivamente, buscamos una transformación lineal

tal que

(paso 2). Para determinar cuán buena es la transformación obtenida se transforman todos los puntos de la nube fuente (paso 3), y se emplea una métrica basada en la sumatoria del error cuadrático medio considerando las distancias entre los puntos

y los puntos

transformados con

(paso 4).

Respecto del de error cuadrático medio, se pueden distinguir dos métricas: point-to-point(Besl& McKay, 1992) y point-to-plane(Chen &Medion, 1992). La primera mide el error a partir del cálculo de la distancia de un punto a su vecino más cercano:

donde

corresponden a la rotación y traslación asociados a la posición estimada. La segunda métrica consiste en medir la distancia de un punto al plano definido por los vecinos del punto correspondiente en la otra nube:

donde

corresponden a la rotación y traslación asociados a la posición estimada respectivamente, y

es la proyección de

en el plano correspondiente a

En cualquiera de los dos casos, la transformación

es considerada como la solución si

, donde

es un umbral definido de acuerdo a la fidelidad con la que se requiera la estimación. Lo anterior asegura la convergencia a un mínimo local, partiendo de una estimación inicial, y por lo tanto el algoritmo siempre termina. Si bien la métrica point-to-point resulta generalmente más estable, requiere un pre-procesamiento de los datos, perjudicando la performance de la registración, por lo que en la práctica se recomienda utilizar la métrica point-to-plane.

Para poder fusionar la información provista por la registración del láser con la de odometría mediante el Filtro Extendido de Kalman (EKF) tenemos que estimar el error producido por cada fuente de información, es decir, calcular una matriz de covarianza correspondiente para cada caso. La covarianza asociada a la odometría se obtiene a partir de la Ecuación (3), con lo cual resta calcular la covarianza correspondiente a ICP.

Para esto existen dos enfoques: la estimación de la covarianza por muestreo del espacio de resultados o por una fórmula cerrada (Censi, 2007; Bengtsson &Baerveldt, 2003). El primer enfoque consiste en el muestreo de todos los posibles resultados del algoritmo, para así obtener una caracterización de la distribución de probabilidad de los resultados. Este proceso es computacionalmente muy costoso, por lo que resulta impracticable para aplicaciones de robótica de tiempo real.

En contraposición al enfoque basado en muestreo, Bengtsson ;Baerveldt (2003) proponen el llamado Método del Hessiano. Este método es rápido de computar, pero no es suficientemente preciso, por lo que no sirve para resolver el problema de la localización. Por otro lado, Censi(2007) propone un método más preciso para estimar la covarianza del algoritmo ICP. Este método consiste en considerar a ICP como un algoritmo de minimización de una función de error

donde

es la registración de la nube de puntos

que obtuvimos en el último sensado. La función de error

dependerá de la implementación de ICP que estemos utilizando (point-to-point o point-to-plane).

Como en el caso considerado en el presente trabajo las nubes de puntos son obtenidas a partir de un telémetro láser, las mediciones vienen dadas en coordenadas polares

. Además, como los ángulos

están determinados por la resolución del sensor, la entrada al algoritmo ICP es simplemente el vector de distancias

. Podemos pensar al algoritmo ICP de la siguiente forma:

Luego, la aproximación de primer orden a la covarianza de

que representa la covarianza del algoritmo de ICP esta dada por:

Como el algoritmo ICP es iterativo y no conocemos su fórmula cerrada, no se puede calcular explicitamente

. Sin embargo,

están relacionadas por una función implícita. De hecho,

es punto crítico de la función de error evaluada en la nube de puntos obtenida del último sensado

ya que

. Utilizando el teorema de la función implícita obtenemos una expresión para

Además, por ser

una matriz simétrica, podemos reescribir la ecuación anterior de lasiguiente manera:

donde

es la matriz de covarianza de las mediciones láser, es decir, la incertidumbre sobre la estimación de la distancia

en cada dirección que mide el sensor. Es razonable suponerla con desvío constante

, por lo tanto

. Simplificando, podemos reescribir:

La forma cerrada de la covarianza de ICP se puede estimar entonces utilizando la función de error

del paso de minimización de ICP. Por razones de espacio el cálculo de las derivadas

no se trascriben aquí.

El método de localización propuesto se basa en la fusión mediante EKF de la información proveniente por odometría y por la registración láser utilizando el algoritmo ICP. Vamos a considerar el caso de un robot que se mueve sobre un plano en dos dimensiones, por lo tanto podemos definir el vector estado del robot para el instante

como:

donde

representan la posición del robot,

la orientación y

las velocidades lineales y angular, respectivamente. Todas las coordenadas aquí están dadas respecto del eje de referencia del mundo (notar la diferencia con

que están respecto del marco de referencia del robot).

El modelo de proceso a utilizar en el filtro de EKF se basa en un modelo cinemático clásico definido por la siguiente función:

Notar que este es un modelo general que refleja la cinemática de un robot holonómico sin restricciones y con velocidades constantes. Sin embargo, para este trabajo se consideró el caso de un robot con locomoción diferencial (no holonómico) como se observa en la Ecuación (1). El ruido del modelo de movimiento

debe ajustarse experimentalmente. En este trabajo, tanto la odometría basada en encoders como la registración por ICP son tomadas en el filtro EKF como mediciones de velocidad lineal y angular del robot (relativas a su marco de referencia), obtenidas a partir del desplazamiento relativo sensado en el último intervalo de tiempo:

A su vez, para utilizar EKF para la fusión de sensores, debemos definir el modelo de observación

que relaciona las mediciones con el vector de estado. Para ello, basta con llevar las velocidades expresadas en el marco del mundo (

) al marco de referencia del robot:

Para finalizar, en el caso en el que la observación corresponde a una medición obtenida mediante odometría,

se define según la Ecuación (3), mientras que si corresponde a una observación obtenida mediante ICP,

se define según la Ecuación (8). Estas incertezas modelan el ruido propio de los sensores (encoders y telémetro láser) y determinan el impacto de observar sus mediciones al estimar el desplazamiento relativo del robot en cada caso (odometría e ICP).

Resultados

Se utilizaron datasets disponibles públicamente que son parte del proyecto Rawseeds (Ceriani, 2009). Este repositorio de datos fue generado a partir de diversos recorridos de un robot móvil Robocom de locomoción diferencial tanto en entornos interiores como exteriores. Los datasets de Rawseeds cuenta con información de la pose real del robot en cada instante de tiempo (ground-truthen inglés), obtenida a partir del uso de un sistema de localización externo compuesto por cámaras que observan al robot en todo momento, en el caso del ambiente interior, o de un GPS de alta precisión, para exteriores. De esta forma es posible analizar la calidad de la localización obtenida por el método propuesto respecto de datos de mayor precisión.

De los sensores láser disponibles en los datasets se optó por utilizar el de mayor alcance (LMS291) con el objetivo de contar con la mayor información posible ante cada muestreo, es decir, se buscó evaluar el desempeño del método de localización en condiciones favorables de sensado. Entre las características principales se puede mencionar: frecuencia de muestreo: 75Hz, distancia máxima de sensado: 80m, campo de visión: 180 , resolución angular: 0,25^o , error aproximado a máxima distancia: 10mm.

Se realizaron experimentos para analizar la covarianza estimada para ICP con el método propuesto, calcular el error de posición y orientación de un robot móvil en una trayectoria y comparar el resultado de la generación de mapas de nubes de puntos. Sobre los conjuntos de datos analizados, por cuestiones de espacio se muestran a continuación sólo los resultados obtenidos para el recorrido 27b de Rawseeds generado dentro de un edificio de la Università di Milano-Bicocca, en Milán.

A continuación se puede observar la covarianza estimada para ICP en dos áreas de interés dentro recorrido 27b de Rawseeds. Para analizar los resultados, mostramos las elipses correspondientes a las covarianzas de dos casos típicos de registración láser mediante ICP: una curva (A), un pasillo con algunos objetos (B) (ver Figura 1). Para poder visualizar los datos de covarianza de ICP, se utilizaron elipses que representan un intervalo de confianza del 95% para una distribución Gausianabivariada.

Figura 1: Recorrido completo, marcando dos áreas de interés: A y B.

En la Figura 2(a) se puede observar cómo aumenta el tamaño y cambia la orientación de la elipse a medida que el robot efectúa un giro en la curva y luego vuelve a reducirse el tamaño cuando el láser percibe las pareces del pasillo nuevamente. Estas variaciones indican cambios en la certeza de la información de pose obtenida mediante ICP. En la Figura 2(b) puede evaluarse el caso del pasillo con objetos. Se puede observar como la elipse disminuye su tamaño indicando menor covarianza, es decir, mayor certeza sobre la información proporcionada por ICP cuando nos aproximamos a los objetos y se agranda en el sentido del pasillo cuando no tenemos otras referencias más que las paredes laterales del pasillo.

(a) Área de intrés A. (b) Área de interés B.

Figura 2: Análisis de la covarianza estimada para ICP mediante elipses que representan un intervalo dovare confianza del 95% para una distribución Gausianabivariada. Dos áreas de interés en el recorrido: (a) en una curva y (b) en un pasillo recto con objetos.

Análisis del error de posición y orientación

En la Figura 3 podemos observar la comparación del método de localización propuesto (fusión de sensores mediante EKF con covarianza estamada para ICP) respecto de utilizar sólo odometría o sólo ICP y medir el error en el cálculo de la posición en cada momento de la trayectoria respecto de la trayectoria real (ground-truth). Se puede apreciar que el recorrido del robot calculado por el método de localización propuesto se aproxima más a la trayectoria real que el recorrido calculado sólo por odometría o sólo por el algorimo de ICP.

Figura 3: Comparación de la trayectoria estimadas para el recorrido 27b del datasetRawseeds.

En la Figura 4, se muestra el error absoluto de posición y orientación respecto del ground-truth para cada uno de los tres métodos analizados: fusión mediante EKF entre odometría e ICP con covarianza estimada, sólo ICP, y sólo odometría. Se puede apreciar cómo el error producido al fusionar la información provista por ICP utilizando la estimación de la covarianza es menor que en los otros dos métodos evaluados.

(a) Diagrama de caja del error absoluto de posición. (b) Diagrama de caja del error absoluto de orientación.

Figura 4: Análisis de Error absoluto de posición y orientacion de la trayectoria.

Análisis del mapa generado por las nubes de puntos

Si bien no es el foco del trabajo, el método propuesto permite generar un mapa del entorno a partir de acumular periódicamente las nubes de puntos correspondientes a cada sensado del telémetro láser. A continuación se presentan los mapas construidos en el mismo recorrido 27b de Rawseedsutilizando como origen la posición y orientación del láser según las diferentes variantes del método de localización. La comparación de los mapas construídos respecto el plano del entorno permite una evaluación cualitativa de los resultados.

En la Figura 5(a) se ve como los errores en la estimación de la pose calculada por odometría basada encoders produce una deformación considerable en el mapa generado. Esto se debe principalmente al error producido por el cálculo de la rotación utilizando odometría. En la Figura 5(b) se puede observar un mejor resultado al construir el mapa utilizando la pose obtenida por ICP. En particular, las esquinas del mapa fueron mejor reproducidas, ya que la rotación fue mejor estimada. De todas formas, vemos un desfasaje en el mapa producto de los errores en el cálculo de la traslación del robot. Por último, si analizamos la Figura 5(c) se puede apreciar que el resultado de la reconstrucción utilizando la fusión de sensores mediante EKF y la covarianza calculada por el método propuesto es más fiel que la reconstrucción utilizando sólo ICP o sólo odometría.Esto se debe a que EKF es capaz de combinar mejor la información, si las incertidumbres modeladas se corresponden con la distribución de los errores en la realidad.

(a) Mapa generado mediante odometría basada en encoders.

(b) Mapa generado medianteICP utilizando el telémetro láser.

Figura 5: Comparación de mapas generados para diferentes variantes del método de localización.

Conclusiones

En el presente trabajo se presenta un método probabilístico de localización utilizando como fuentes de información la odometría por encoders y las mediciones provenientes de un telémetro láser registradas mediante el algoritmo ICP. Los datos de las distintas fuentes de localización son fusionados mediante un Filtro Extendido de Kalman. Esto es posible gracias a poder modelar la covarianza para el resultado de la registración de las nubes de puntos mediante ICP. Los resultados muestran que esta fusión mejoró la precisión de la localización del robot respecto de utilizar sólo ICP u odometría. Esta mejora se puede observar analizando las métricas de error absoluto de posición y orientación respecto de la información de ground-truth provista por los datasets utilizados, así como también en los mapas del ambiente construidos a partir de conocer la pose del robot durante su trayectoria.

Referencias

Borenstein, J., Everett, H. R., Feng, L., &Wehe, D. (1997). Mobile robot positioning: Sensors and techniques. Journal of robotic systems, 14(4), 231-249.

Gutmann, J. S., & Schlegel, C. (1996, October). Amos: Comparison of scan matching approaches for self-localization in indoor environments. In Proceedings of the First Euromicro Workshop on Advanced Mobile Robots (EUROBOT'96) (pp. 61-67). IEEE.

Lu, F., &Milios, E. (1997). Robot pose estimation in unknown environments by matching 2d range scans. Journal of Intelligent and Robotic systems, 18(3), 249-275.

Se, S., Lowe, D., & Little, J. (2002). Mobile robot localization and mapping with uncertainty using scale-invariant visual landmarks. The international Journal of robotics Research, 21(8), 735-758.

Chen, Y., &Medioni, G. (1992). Object modelling by registration of multiple range images. Image and vision computing, 10(3), 145-155.

Besl, P. J., & McKay, N. D. (1992, April). Method for registration of 3-D shapes. In Sensor fusion IV: control paradigms and data structures (Vol. 1611, pp. 586-606). International Society for Optics and Photonics.

Greenspan, M., &Yurick, M. (2003, October). Approximate kd tree search for efficient ICP. In Fourth International Conference on 3-D Digital Imaging and Modeling, 2003. 3DIM 2003. Proceedings. (pp. 442-448). IEEE.

Censi, A. (2007, April). An accurate closed-form estimate of ICP's covariance. In Proceedings 2007 IEEE international conference on robotics and automation (pp. 3167-3172). IEEE.

Bengtsson, O., &Baerveldt, A. J. (2003). Robot localization based on scan-matching — estimating the covariance matrix for the IDC algorithm. Robotics and Autonomous Systems, 44(1), 29-40.

Ceriani, S., Fontana, G., Giusti, A., Marzorati, D., Matteucci, M., Migliore, D., &Taddei, P. (2009). Rawseeds ground truth collection systems for indoor self-localization and mapping. Autonomous Robots, 27(4), 353.