ES2476565A1

ES2476565A1 - Método de localización y mapeo simultáneo para dispositivos robóticos

Info

Publication number: ES2476565A1
Application number: ES201201234A
Authority: ES
Inventors: Tomás MARTINEZ MARIN; Eduardo LOPEZ REDONDO
Original assignee: Universidad de Alicante
Current assignee: Universidad de Alicante
Priority date: 2012-12-14
Filing date: 2012-12-14
Publication date: 2014-07-14
Anticipated expiration: 2032-12-14
Also published as: ES2476565B2; WO2014091043A1

Abstract

Método de localización y mapeo simultáneo para dispositivos robóticos que comprende al menos las etapas de asociación, localización y actualización del mapa o mapeado y que se caracteriza porque la modelización del mapa en la etapa de mapeado está basada en entidades denominadas objetos, definidas como una secuencia de puntos móviles ajustables dinámicamente en tamaño con el fin de representar la forma de los contornos de los obstáculos reales detectados por al menos un sensor del dispositivo robótico; y donde cada punto de la secuencia que representa los objetos tiene asociado una posición y un peso que indica el grado de movilidad del mismo. La idea básica es crear un modelo del entorno por el que se desplaza el vehículo al mismo tiempo que éste se encuentra localizado.

Description

MÉTODO DE LOCALIZACIÓN Y MAPEO SIMULTÁNEO PARA DISPOSITIVOS ROBÓTICOS

DESCRIPCIÓN Campo de la invención

El objeto de la invención es un sistema de localización y mapeo simultáneo para dispositivos robóticos, conocido en el campo de la robótica por su acrónimo SLAM. La idea básica es crear un modelo del entorno por el que se desplaza el vehículo al mismo tiempo que éste se encuentra localizado. La forma de localizarse a partir de las observaciones obtenidas por sensores, así como el modo en que se modela la información del entorno, generan las diversas técnicas del SLAM, campo técnico donde queda integrada la presente invención.

Estado de la técnica

El objeto de los métodos y sistemas SLAM es emplear los datos de un sensor de un vehículo robotizado (que en la presente memoria quedará referido indistintamente como robot o vehículo) para actualizar la posición del mismo, así como la información del entorno almacenada en una estructura denominada “mapa”. Normalmente, suele usarse como sensor un dispositivo láser, que es el que proporciona la información del entorno. Con ella, el robot debe ser capaz de corregir su posición y la de los elementos contenidos en el mapa. Esto se consigue extrayendo distintas características de los datos obtenidos por el sensor.

En el caso de un sensor láser, se entiende por “característica” cualquier región consecutiva de “puntos” correspondientes a coordenadas del mapa en la secuencia de datos proporcionada por el sensor, que pertenecen a un mismo objeto de una superficie real. Cada una de estas características tiene una incertidumbre de posición, al igual que el vehículo, que tiene dicha incertidumbre a través de la información que le proporciona su sistema de odometría (consistente en el ángulo de giro de las ruedas y la distancia recorrida). La actualización de la posición del vehículo y la de los elementos del mapa se realiza teniendo en cuenta la incertidumbre de las observaciones procedentes del sensor y la de la odometría.

En el proceso de localización, inicialmente, el vehículo detecta los obstáculos mediante su sensor. Posteriormente, el vehículo se mueve, mientras que su sistema de odometría le proporciona la distancia recorrida y el ángulo de giro de las ruedas. Con esta información se establece una primera hipótesis de localización. Tras ello, el robot vuelve a medir la localización de los objetos del entorno y, en base a esa localización, establece una segunda hipótesis de localización que no tiene por qué coincidir con la calculada mediante odometría. Finalmente, el robot fusiona la hipótesis de localización proporcionada por la odometría y por los sensores para, en función de la incertidumbre de ambas, establecer una hipótesis de localización definitiva.

Nuestra propuesta de SLAM consta de tres entradas y dos salidas. Las entradas consisten en el punto correspondiente a la última posición conocida del vehículo (Xt), los datos adquiridos por sensores externos, tales como un láser y un sensor de imagen, y finalmente, la pluralidad de puntos correspondiente al estado del mapa en el instante actual (Mt).

En SLAM, uno de los sensores externos más utilizados es el láser. Éste recoge la información métrica del entorno realizando un barrido angular de su haz láser de modo que, para cada ángulo de barrido de dicho haz, se conoce la distancia o rango al obstáculo más cercano. Toda la información de rangos adquirida, para cada posición del haz en un mismo barrido, queda definida en esta memoria como “scan”. Por otro lado, la información de la odometría tiene que ver con la distancia recorrida por el vehículo en un lapso de tiempo dado, así como con la posición de giro de sus ruedas. Normalmente, esta información suele recabarse empleando dispositivos conocidos como “encoders”, asociados a los motores de tracción y giro de las ruedas del vehículo.

El mapa de cualquier proceso de SLAM tiene el cometido de guardar la información proporcionada por el sensor externo. La forma en que se almacena esta información es una de las claves que confiere a cualquier estrategia de SLAM su ventaja resolutiva frente a las restantes. Por tanto, uno de los problemas técnicos a resolver por la presente invención es la optimización de la gestión de la información proporcionada por el sensor, es decir, el establecimiento de un mapa del entorno.

Uno de los factores determinantes en la localización de cualquier proceso de SLAM es la asociación de la información recibida en un instante por un sensor con la ya existente en el mapa. Identificando esta información capturada por el sensor en un instante con otra ya almacenada y asumiendo la invariabilidad del mapa, puede resolverse la localización del vehículo. Esta forma de localización está basada en la observación, puesto que requiere de la información procedente de algún sensor externo para su cálculo. Para que la asociación entre la información del sensor y la contenida en el mapa sea posible, todos los procesos de SLAM realizan una segmentación del scan. Esta segmentación consiste en la extracción de unas unidades de información significativas (las “características” anteriormente referidas) a partir del conjunto de rangos aportados por el sensor. Las características son una agregación de datos consecutivos del sensor que facilitan la asociación con la información almacenada en el mapa a un nivel de abstracción superior a la mínima posible basada en puntos simples.

Por otro lado, la odometría proporciona la información necesaria para calcular una localización del vehículo basada en su dinámica. Esta localización será denominada en adelante localización por predicción.

Una vez calculada la localización empleando toda la información sensorial del vehículo: (a) láser (localización por observación) y (b) encoders (predicción), cualquier estrategia SLAM fusiona ambas informaciones de localización para obtener una predicción de la localización única de un punto (Xt+1). A partir de ésta, se actualiza el mapa utilizando la información del sensor láser, obteniendo un nuevo estado del mapa (Mt+1).

Las técnicas de SLAM se pueden agrupar en dos clases: SLAM topológico y SLAM métrico.

El primero de ellos (SLAM topológico) trata de mantener un mapa del entorno basado en posiciones relativas de los lugares más significativos del entorno en el que se mueve el vehículo (habitaciones, pasillos, cruces de corredores, etc.). Los mapas de esta categoría [AdrienAngeli, StéphaneDoncieux, Jean A Arcady Meyer, David Filliat. Visual topological SLAM and global localization. In ICRA'09 Proceedings of the 2009 IEEE internationalconferenceonRobotics and Automation, páginas 2029 a 2034] se modelan empleando grafos de nodos conectados, de modo que cada uno de estos representan un escenario concreto del entorno. Cuando el robot llega a un lugar del mapa y desea dirigirse a otro, consulta su mapa topológico y averigua la distancia y dirección del nuevo destino.

En el segundo caso (SLAM métrico), el vehículo mantiene un mapa de las características más relevantes del entorno, que le sirven para localizarse. Las características son almacenadas en el mapa guardando su posición más probable dentro de un sistema de coordenadas métrico absoluto; la probabilidad de la posición se va modificando según el vehículo se desplaza por el entorno, utilizando las nuevas observaciones de las características para reforzar su certidumbre de posición.

5 A continuación, se establecen las diferentes técnicas de localización y mapeado empleadas en las técnicas de SLAM métrico en el estado de la técnica, por ser aquí donde se encuentran los problemas técnicos resueltos por la presente invención y, por tanto, donde se proponen las aproximaciones más novedosas de la presente invención.

10 Las técnicas de localización SLAM métrico se agrupan principalmente en tres categorías básicas: EKF (filtro extendido de Kalman), RBPF (filtro de partículas) y Scan – Matching.

Las técnicas de mapeo se clasifican en otras tres categorías: marcas, grid (rejilla), patrones y alineamiento de scans. Atendiendo a esta clasificación, se pueden considerar las

15 siguientes técnicas de SLAM teniendo en cuenta todas las posibles relaciones entre métodos de localización y mapeado:

TÉCNICAS: EKF RBPF SCAN-MATCHING

MARCAS: [1] - -

GRID: [3] [2] [4]

PATRONES: [5] - -

ALINEAMIENTO SCANS: - - [6]

Para cada una de las técnicas alternativas tenemos las siguientes referencias:

[1] R. Smith, M.Self, and P. Cheeseman.Estimating uncertain spatial relationships in robotics.In Autonomous Robot Vehicles, I. Cox and G. Wilfong, Eds. Springer Verlag, New York, 1988, pp. 167-193.

[2] G. Grisetti, C. Stachniss, and W.Burgard. Improving grid A based slam with rao

25 blackwellized particle filters by adaptive proposals and selective resampling. In IEEE International Conference on Robotics and Automation (ICRA,2005).

[3] Juan I. Nieto and Jose E. Guivant and Eduardo M.Nebot, The Hybrid Metric Maps(HYMMS): A novel map representation for denseSLAM. In IEEE International Conference on Robotics and Automation(ICRA 2004, pp.391-396).

[4] Chieh-Chih Wang and Charles Thorpe and Sebastian Thrun. Online Simultaneous Localization And Mapping with Detection And Tracking of Moving Objects: Theory and Results from a Ground Vehicle in Crowded Urban Areas. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA 2003).

[5] Tim Bailey and Juan Nieto. Scan-SLAM: Recursive Mapping and Localisation with Arbitrary – Shaped Landmarks. Workshop on Quantitative Performance Evaluation of Navigation Solutions for Mobile Robots, Robotics: Science and Systems Conference (RSS), 2008].

[6] P. Newman, D.Cole and K. Ho. Outdoor SLAM using visual appearance and laser ranging. In IEEE International (Conference on Robotics and Automation, 2006)

Explicación de la invención

El objeto de la presente invención es un sistema de localización y mapeo simultáneo para dispositivos robóticos que aporta mejoras en los procesos SLAM respecto de la asociación, localización y proceso de actualización del mapa. Por ser las dos primeras (asociación y localización) consecuencia del proceso de actualización, a continuación se muestran las distintas ventajas y aportaciones de la presente invención.

La aportación en la etapa de mapeado que se propone está relacionada con la modelización del mapa. Tal y como se ha comentado en el estado del arte, todos los modelos de mapa empleados en las técnicas de SLAM existente son de tipo basado en: (a) marcas; (b) rejilla de celdas probabilísticas; o bien (c) basados en patrones, esto es un conjunto de rangos consecutivos de todos los recibidos por el sensor (scan) y que representan un fragmento de un objeto real.

Los primeros (las marcas) son los más sencillos, pero también los que presentan más limitaciones a la hora de capturar entornos con formas arbitrarias. Están pensados para representar entornos estructurados en base a puntos y líneas. Cada marca se representa por una posición y una probabilidad de incertidumbre asociada. Los segundos (la rejilla) modelan el entorno a base de celdas con una probabilidad de ocupación asociada. Los terceros (patrones) pueden considerarse una extensión de los primeros, en el sentido en que intentan ampliar el conjunto restringido de puntos y líneas con los que trabajan con el propósito de aprovechar la información de la forma de cualquier obstáculo para establecer localizaciones más robustas en entornos no demasiado estructurados.

Para evitar las limitaciones de las marcas y aprovechar las características de los otros métodos de una forma simplificada, la presente invención propone reemplazar la rejilla bidimensional y los patrones por entidades denominadas “objetos”, que consisten en una secuencia de puntos móviles ajustable dinámicamente en tamaño, con el fin de representar la forma de los contornos de los obstáculos reales que se van detectando en un mapa. Cada punto de la secuencia que representa los objetos tiene asociado una posición y un peso que indica el grado de movilidad del mismo. En el caso de los mapas de marcas/patrones y rejilla, existen probabilidades de localización asociadas a la posición de cada marca/patrón y celda, lo que permite al mapa actualizarse para lograr una consistencia global. En el caso de la invención, cada peso asociado a un punto del objeto representa la probabilidad de que su posición se encuentre más o menos cerca de la que realmente representa.

Las ventajas de esta aproximación de modelo de mapa son varias. Así pues, en relación con la estrategia basada en marcas, permite modelar cualquier contorno de forma precisa y detallada sin merma sustancial de la memoria empleada.

En relación con los modelos basados en rejilla, si bien ambas estrategias permiten capturar información de contornos de cualquier forma, en nuestro caso la memoria requerida para almacenar la información de los objetos crece linealmente con su número y su longitud y no cuadráticamente según la superficie de mapa explorado hasta el momento.

En relación con los modelos basados en patrones, permite actualizar incrementalmente la forma y tamaño de los mismos y, además, permite establecer una probabilidad de consistencia local para cada punto del objeto, en lugar de una probabilidad única asociada al patrón completo. Además, los objetos del mapa pueden adaptarse a las deformaciones locales de los contornos de los obstáculos que pudieran aparecer en tiempo real. En cambio, los patrones no se promedian para reducir o eliminar la incertidumbre.

Otra ventaja a destacar es que el método basado en rejilla no proporciona información sobre la conectividad de las celdas que están ocupadas (FIG. 17). Sin embargo, la presente invención almacena la información de los objetos como entidades independientes unas de otras, lo que permite establecer estrategias de asociación basadas en las posiciones relativas de los objetos. Dichos objetos, por lo tanto, quedan definidos como una secuencia ordenada de puntos con información de su posición (X,Y) y un peso (P) proporcional al grado de incertidumbre de esa posición. La ordenación de los puntos facilita introducir conceptos de geometría computacional, como la oclusión (es decir, la ocultación de determinados puntos de unos objetos respecto a otros, cuando éstos se superponen durante su detección) y el análisis de forma de los objetos.

Así pues, de acuerdo con lo enunciado en el estado de la técnica, los mapas basados en marcas sólo modelan puntos, rectas o patrones (definidos como el conjunto de medidas del sensor). La consistencia global del mapa se logra manteniendo una matriz de covarianza que refleja las incertidumbres en las posiciones del vector de estado utilizado, esto es, estado del vehículo (posición (X,Y) y orientación) y posición del resto de marcas.

El mapa basado en rejilla logra la consistencia del mapa manteniendo un mapa de probabilidades para celda. El mapa puede modificarse según varía la probabilidad asociada a sus celdas.

Sin embargo, los mapas basados en objetos utilizan los pesos asociados a cada punto de ellos como medida de su grado de consistencia global. Cuando un punto se añade a un objeto, su peso asociado es el menor posible (por ejemplo, 1). Conforme su posición se va actualizando en el tiempo en base a las medidas recibidas del sensor, su peso aumenta, de forma que éste es proporcional al número de actualizaciones de su posición empleando la información del sensor (FIG. 17). Una de las diferencias fundamentales que se observan entre el mapa de objetos y el de rejilla es que el primero almacena la información de contornos clasificada de forma natural por objetos físicos que detecta el sensor. Además, la ponderación de los puntos de los objetos permite, al igual que en los casos de mapas basados en marcas/patrones y rejilla, asegurar la consistencia global del mapa conforme el sistema evoluciona en el tiempo.

En los casos de rejilla y objetos, en el primero no existe información sobre la conectividad de las celdas, mientras que en el segundo, sencillamente, aparece de forma natural. Según ilustramos en la FIG. 17, el mapa de ocupación no aprovecha la potencial ventaja de conocer que todas las celdas con probabilidad de ocupación igual a 0,6 forman una única agrupación conexa, del mismo modo que las celdas que tienen una probabilidad de 0,7 también pueden estar representando un mismo contorno físico.

Esta forma de representar internamente la realidad física proporcionada por el sensor nos facilita introducir conceptos de geometría computacional del tipo oclusión entre objetos y análisis de forma de objetos para saber si están cerrados sobre sí mismos o para asociar, en base a forma, características del sensor con objetos ya existentes.

La forma de almacenar la información del sensor en los objetos nos permite asociar la segmentación del sensor (agrupaciones conexas de puntos del sensor que hacen referencia a un mismo objeto físico y que denominaremos “características”) con la información del mapa. En la presente invención, el proceso de asociación consiste en la utilización del concepto de objeto para asociar de forma robusta entidades del mapa utilizando los criterios geométricos de forma y oclusión.

Finalmente, en el proceso de localización, las técnicas conocidas como Scan-Matching se fundamentan, en general, en el algoritmo de ajuste de puntos denominados ICP [Lu and E.

E. Milios. Robot pose estimation in unknown environments by matching 2D range scans. In Proc. IEEE Comp. Soc. Conf. on Computer Vision and Pattern Recognition, Seattle, WA, pp.935-938,June 1994]. Básicamente, este procedimiento trata de hallar el desplazamiento relativo entre dos colecciones de puntos: una procedente de una característica extraída del sensor y, la otra, de alguna entidad almacenada en el mapa. La idea básica de ICP consiste en hacer coincidir ambas colecciones de puntos empleando una transformación basada en un desplazamiento y una rotación. Para ello, se establecen asociaciones a priori entre los puntos de ambas secuencias y se resuelve la transformación resultante. El proceso se repite hasta que una colección de puntos encaja con la otra. Por desgracia, una de las desventajas principales de este algoritmo es su grado de convergencia. La técnica de ICP no converge al resultado deseado cuando las colecciones de puntos se encuentran muy distantes (esto ocurre cuando el error acumulado de posición del vehículo es grande y se revisita una zona: cerrado de bucle) o bien cuando la rotación relativa de ambas es notable.

En este punto, la presente invención resuelve el problema utilizando una transformación para lograr un desplazamiento y rotación relativos entre los puntos de la característica y del objeto, tal y como se describe a continuación. Para ello, se hace uso de la información sobre la forma de las colecciones de puntos a encajar. Esta información se resume en una secuencia de datos denominada “signatura” que se forma calculando el ángulo relativo entre dos puntos consecutivos de la colección de puntos originales. La ventaja de trabajar con la signatura es que ésta es invariante a traslaciones y rotaciones de la secuencia original. A diferencia del algoritmo ICP, donde hay que establecer asociaciones entre los puntos de las colecciones originales, la propuesta que se presenta es la de establecer dichas relaciones empleando exclusivamente la información de forma contenida en la signatura, donde la información relevante como esquinas y en general, puntos angulosos, queda reflejada claramente. De este modo, pueden establecerse hipótesis de asociación entre puntos de ambas signaturas que se corresponden con transformaciones de traslación y rotación.

A lo largo de la descripción y las reivindicaciones la palabra "comprende" y sus variantes no pretenden excluir otras características técnicas, aditivos, componentes o pasos. Para los expertos en la materia, otros objetos, ventajas y características de la invención se desprenderán en parte de la descripción y en parte de la práctica de la invención. Los siguientes ejemplos y dibujos se proporcionan a modo de ilustración, y no se pretende que restrinjan la presente invención. Además, la presente invención cubre todas las posibles combinaciones de realizaciones particulares y preferidas aquí indicadas.

Breve descripción de las figuras

A continuación se pasa a describir de manera muy breve una serie de dibujos que ayudan a comprender mejor la invención y que se relacionan expresamente con una realización de dicha invención que se presenta como un ejemplo no limitativo de ésta.

-: La FIG.1 muestra una gráfica con las signaturas de objeto y de observación, incluyendo el desplazamiento relativo entre ambas.

-: La FIG.2 muestra un esquema con la asociación de objetos para un caso particular en el que se parte de tres signaturas de observaciones (k, k+1, k+2) y N signaturas de objetos.

-: La FIG.3 muestra una gráfica con el desplazamiento relativo entre objetos y observaciones, donde el máximo se produce para un desplazamiento de once puntos.

-: La FIG.4 muestra una gráfica con la construcción de la signatura en el caso práctico de la FIG.3, calculándose los puntos homólogos de referencia. -La FIG.5 muestra el esquema del proceso de localización que forma parte del objeto

de la invención. -La FIG.6 muestra un mapa basado en objetos. -La FIG.7 muestra el mapa basado en objetos de la figura 6 durante el proceso de

actualización.

-: La FIG.8 muestra la actualización del mapa de objetos para el caso donde exista solape entre los puntos de observación y los del objeto asociado. -La FIG.9 muestra la actualización del mapa de objetos para el caso donde existan puntos de una observación que no solapan con puntos del objeto asociado.

-: La FIG.10 muestra la actualización del mapa de objetos para el caso donde existen puntos de un objeto que no solapan con puntos de alguna observación extraída del sensor.

-: La FIG.11 muestra un esquema de la solución hardware para el método de localización y mapeo simultáneo para dispositivos robóticos objeto de la presente invención.

-: La FIG.12 muestra esquemáticamente la realización de la segmentación en la solución hardware de la invención. -La FIG.13 muestra esquemáticamente la implementación hardware de la correlación cruzada entre la signatura de un objeto y la de una característica. -La FIG.14 muestra el esquema general de asociación implementado en una FPGA según el método objeto de la presente invención. -La FIG.15 se muestran las salidas de los correladores cruzados en función del ángulo de visión del sensor (α). -La FIG.16 muestra el esquema de localización final que se obtiene fusionando las hipótesis de localización basadas en la observación y la única de predicción. -La FIG.17 muestra una comparativa de las técnicas de mapeado basadas en marcas y en rejilla junto con la propuesta de mapeado empleando pesos en los objetos.

Exposición detallada de modos de realización y ejemplo

Como se ha indicado, las principales aportaciones de la invención se encuentran detalladas en el proceso de mapeado, el proceso de asociación y el proceso de localización. Para explicar detenidamente la invención, en primer lugar, se describe el proceso de asociación basado en la signatura de las características (unidimensional) y de los objetos (secuencias basadas en la forma). Posteriormente se explica, el proceso de localización basado en hipótesis asociativas entre puntos notables de las signaturas de las características y de su objeto asociado para lograr una transformación en desplazamiento y rotación que logre un buen alineamiento de los puntos de las características y los de los objetos. Por último se describirá cómo los puntos se desplazan, borran o añaden a los objetos del mapa.

Los objetos quedan definidos como una secuencia ordenada de puntos. Cada uno de ellos contiene información de posición en un sistema de referencia absoluto así como una medida de probabilidad asociada a que la información de posición del punto se encuentre en un entorno próximo de la posición real que representa. Así pues, dado cualquier conjunto ordenado CO de N pares ordenados,

donde (xi, yi) representan sus coordenadas en el mapa, definimos su signatura SCO como,

donde (xi+1, yi+1) representan las coordenadas de los puntos consecutivos en el orden del 10 objeto.

La asociación entre las observaciones (características) procedentes del sensor y los objetos del mapa se realiza intentando encajar la signatura de la observación (SOBV) en alguna región de la signatura del objeto (SOBJ). Para ello, calculamos el error cuadrático medio de

15 ambas signaturas, de modo que para cada desplazamiento de la signatura de observación sobre la del objeto se obtiene una medida de similitud entre ambas:

ECM( , (∝ ( (− (+

El mínimo de la función de error cuadrático medio nos indica el desplazamiento relativo 20 entre las signaturas para el que existe un mejor encaje entre ambas. La figura 1 muestra ambas signaturas y el desplazamiento relativo para el que se produce el mejor encaje.

La asociación entre las regiones del scan segmentadas (características) y las de los objetos del mapa se realiza de forma paralela, calculando el error cuadrático medio de cada una de

25 las signaturas de las observaciones (SOBV) con todas aquellas signaturas de los objetos del mapa (SOBJ) que son compatibles con su posición. El punto de la función error cuadrático medio donde se produzca un mínimo generará una hipótesis de asociación entre una característica y un objeto del mapa.

30 En la figura 2 se ilustra el proceso descrito para un caso particular en el que partimos de tres signaturas de observaciones (k, k+1, k+2) y N signaturas de objetos.

El esquema general de asociación anteriormente visto genera una hipótesis de asociación, esto es, parejas característica – objeto del mapa, donde cada una de éstas, a su vez, establece una hipótesis de localización que se resuelve como se indica a continuación.

El desplazamiento relativo para el que se produce un mínimo de la función de error cuadrático medio (ECM (SOBJ, SOBV)(i)) permite establecer una asociación entre los puntos de la signatura de la observación con los de su asociada del objeto; en la figura 3, idéntica a la figura 1, se muestra cómo el desplazamiento relativo para el que se produce el mínimo es de once puntos.

Está representado en la figura 3 un punto de la signatura de una característica de la observación (3) y más concretamente, el de valor máximo. El punto homólogo (3’) en la signatura del objeto está representado teniendo en cuenta el desplazamiento relativo.

Por construcción de la signatura, a partir de las coordenadas de puntos (por ejemplo, en las secuencias mostradas en la figura 4) se pueden calcular puntos homólogos de referencia en las secuencias originales de observación (OBV) y de objeto (OBJ) (31, 31’, 32, 32’, 33, 33’, 34, 34’, 35, 35’).

Para realizar una transformación de desplazamiento y rotación en dos dimensiones, ésta queda definida por, al menos, dos parejas de puntos. Una de las parejas viene determinada por los puntos homólogos calculados anteriormente, y la otra puede establecerse considerando como asociados aquellos puntos de las secuencias originales que equidisten de sus respectivos puntos de referencia en las secuencias. Una vez establecidas las asociaciones de puntos en un entorno de los puntos de referencia en ambas secuencias, la transformación de desplazamiento y rotación queda determinada. En la figura 5 se muestra un ejemplo del esquema de localización propuesto, basado en la forma.

Tal y como se observa en dicha figura 5, la hipótesis de localización (51) queda establecida en las siguientes etapas:

i. Definición del objeto asociado OBJ (52) y de la característica OBV (53).

ii. Definición de la signatura del objeto SOBJ (54) y de la característica SOBV (55).

iii. Establecimiento de la correlación entre las signaturas de objeto y de la característica (54,55) de acuerdo con:

ECM( ∝ ( (− (+

iv. Una etapa de desplazamiento relativo de signaturas y establecimiento de puntos homólogos en las signaturas (56).

5 v. Una etapa de establecimiento de puntos homólogos en la característica y en su objeto asociado (57,57’).

vi. Una etapa de establecimiento de asociaciones entre los puntos de la secuencia de la característica y la del objeto en un entorno centrado en los puntos homólogos de referencia (58).

10 vii. Una etapa final de resolución de la localización (59).

Una vez realizada la localización del vehículo, el mapa se actualiza a partir de su estado actual y de la información procedente del sensor. La actualización del mapa se realiza de forma local, teniendo en cuenta que los objetos del mapa quedan definidos por una secuencia de puntos unidimensional. Los puntos que definen los objetos se caracterizan por

15 sus coordenadas de posición absoluta en el mapa (X,Y) y por su peso (P), tal y como se observa en el ejemplo de la figura 6.

La actualización de un objeto del mapa requiere la localización del vehículo calculada en el instante actual (Xt) así como el estado del mapa en el instante anterior (Mt-1). De esta forma,

20 la información local de las observaciones extraídas del sensor se convierten a coordenadas absolutas (X,Y) del mapa empleando la localización del vehículo ya estimada (Xt) lo que permite integrar en un mismo sistema de referencia absoluto los datos del sensor y los puntos de los objetos del mapa.

25 En la figura 7 se ilustra un ejemplo de un objeto del mapa (71) junto con las observaciones del sensor (72) convertidas al sistema de referencia absoluto, de acuerdo con la posición del sensor (73), e incluyendo los puntos del sensor fuera de rango (74).

A la hora de actualizar una región de un objeto empleando la información sensorial, podemos distinguir varios casos dependiendo del grado de solapamiento de los puntos del sensor con los puntos del objeto desde la posición del sensor:

CASO 1) Existe solape entre los puntos de la observación y los del objeto asociado.

En este caso, la actualización de una región de un objeto se realiza imbricando los puntos actualizados de la observación con los del objeto. La actualización de un punto de la observación se realiza considerando la información local de los pesos de los dos puntos más cercanos del objeto, tal y como se observa en la figura 8.

La actualización del punto de la observación se realiza de la siguiente forma:

Xobv t =Xobvi (Pobvi/(Pobvi + 1)) + Xobv t-1 (1/Pobvi+1))

Donde Xobv t son las coordenadas absolutas actualizadas del punto de observación; Xobv t-1 son las coordenadas absolutas sin actualizar del punto de la observación; Xobvi son las coordenadas absolutas del punto de intersección entre el rayo de observación que une el sensor con el punto de observación sin actualizar (Xobv t-1) y el segmento definido por los dos puntos del mapa más cercanos a éste último. Finalmente Pobvi es el peso del punto Xobvi definido como Pobvi = Pi(α – αi)/ α) + Pj(αi/α) donde αi es el ángulo entre el punto Xobvi y el punto i-ésimo del objeto visto desde el sensor. Finalmente, α es el ángulo entre el punto iésimo del objeto y el siguiente visto desde el sensor.

CASO 2) Existen puntos de observación que no solapan con puntos del objeto asociado.

En este caso la actualización de la región del objeto se realiza añadiendo los puntos de la observación que no solapan al objeto. Los nuevos puntos que se introducen en el objeto son de peso 1. Todo ello, tal y como se observa en la figura 9.

CASO 3) Existen puntos de un objeto que no solapan con puntos de alguna observación extraída del sensor.

Como se puede observar en la figura 10, la actualización de la zona del objeto que no solapa con alguna observación se realiza restando una unidad los pesos de los puntos de esa región. Cuando el peso de los puntos es negativo, se elimina el objeto al que pertenece.

Ejemplo de realización práctica de la invención

Para aprovechar todas las ventajas de las innovaciones descritas se presenta una posible realización práctica (hardware) de las mismas empleando una FPGA. Ésta se caracteriza porque es un dispositivo lógico programable de propósito general. Las FPGA están compuestas por bloques lógicos configurables (BLC) que se comunican entre sí mediante conexiones programables. De este modo, cualquier FPGA consta de una matriz bidimensional de estos bloques, rodeados de conexiones modificables entre ellos. Además, con el propósito de comunicar la FPGA con el exterior, ésta dispone de un conjunto de puertos de entrada y de salida configurables por el usuario.

A continuación, y siguiendo lo preconizado por la presente invención, se describe una realización hardware implementada en una FPGA. Puesto que la característica más destacable de una FPGA es la de permitir ejecutar en paralelo distintas tareas, en la figura 11 se muestran distintos bloques para ejecutar correctamente la invención. Así cada uno de los bloques, representa la reserva de la parte de los recursos lógicos configurables de la FPGA global para lograr un propósito concreto.

Así, por ejemplo, tenemos como la señal de adquisición de datos de los sensores en un instante St es la entrada de datos al conjunto del sistema (100) configurado en la FPGA y que obtiene como resultados el mapa de objetos Mt+1 y posición Xt+1 actualizados. El sistema comprende un primer módulo de segmentación (101) que representa todos aquellos recursos de la FPGA configurados para ejecutar la segmentación del scan del sensor de entrada (seña St) con el propósito de generar características. Del mismo modo, en el sistema de la invención (100) se implementan el módulo de predicción (102), el módulo de asociación y localización (103), el módulo de fusión (104) y el módulo generador del mapa de objetos (105).

Cada uno de los módulos anteriores está implementado por bloques lógicos, que en conjunto permiten definir la funcionalidad de cada módulo dentro del sistema programado en la FPGA. Así tenemos los siguientes bloques lógicos configurables básicos:

a) Bloque lógico configurable tipo punto scan real (10) configurado para representar la información asociada a un punto del scan del sensor (rango y ángulo).

b) Bloque lógico configurable tipo punto scan simulado, configurado para representar la información asociada a un punto de un scan simulado por el proceso de SLAM (rango y ángulo). Así pues, desde una localización del vehículo se simula la propagación de los rayos sobre los objetos del mapa, del mismo modo que sucede con los rayos del sensor sobre los objetos reales del entorno.

c) Bloque lógico configurable tipo partícula, que está configurado para representar la información de una partícula o punto asociado a un objeto. Por lo tanto, este bloque tratará información relacionada con la posición (X,Y) de la partícula, así como su peso.

d) Bloque lógico configurable tipo localización de vehículo, configurado para representar la posición del vehículo en un momento concreto del proceso de SLAM. Por lo tanto, la información que maneja es la de una posición (X,Y) y una orientación (ϕ).

El proceso de segmentación, implementado en el módulo de segmentación (101) está configurado para extraer las características del scan del sensor. El criterio por el que se decide que una región de puntos del scan se eleva al rango de característica suele ser la proximidad local que existe entre dos puntos consecutivos de esa región. Si dos de ellos están relativamente cerca, se asume que pertenecen a la misma característica. En caso contrario, termina una característica y comienza otra.

Aprovechando la capacidad de procesamiento paralelo de la FPGA, el módulo de segmentación (101) comprende tantos bloques lógicos configurables tipo punto scan real

(10) como medidas contenga el scan; todos los bloques lógicos configurables están conectados uno a continuación de otro, de modo que excepto los bloques lógicos extremos, cada uno tiene dos vecinos, tal y como se muestra en la figura 12.

A partir de la información inicial (rango y ángulo de un punto del scan) que se carga en cada bloque lógico scan real (10), cada uno de estos realiza en paralelo una comparación de su rango con el de su vecino de la izquierda (por ejemplo), almacenando la diferencia en el propio bloque. A continuación, se realiza un recorrido secuencial de los bloques lógicos de derecha a izquierda comparando la diferencia de rangos de un bloque con la almacenada en el bloque de su izquierda. Si la comparación no supera un determinado umbral (que dependerá de la aplicación), ambos puntos pertenecen a la misma característica; en caso contrario, se incrementa un contador de número de características detectadas y se continua con la comparación entre vecinos.

Por otro lado, el módulo generador del mapa de objetos (105) reserva los bloques lógicos necesarios de tipo partículas (bloque lógico configurable tipo partícula) para poder almacenar la información de cada uno de los objetos del mapa. Cada objeto del mapa está compuesto por un conjunto de estos bloques lógicos configurables tipo partícula conectados 10 entre sí. Cada vez que un punto se añade a un objeto, se reserva memoria para el bloque lógico correspondiente y se inicializa con los valores de posición (X,Y) y peso adecuados. Del mismo modo, la eliminación de un punto de un objeto se traduce en el correspondiente borrado del bloque asociado al punto. El módulo de asociación y localización (103) está configurado para establecer la asociación entre las características extraídas del scan y los

15 objetos del mapa. Tal y como ha sido establecido anteriormente, ésta se basa en la correlación cruzada de dos signaturas. El error cuadrático medio de dos secuencias unidimensionales viene dado por la expresión:

ECM(∝ ( (−C (+

20 Esta ecuación requiere de un gran número de multiplicaciones, restas y sumas. En un microprocesador de propósito general, el cálculo de esta función supone un coste computacional elevado, ya que no se puede ejecutar de forma paralela. Sin embargo, en una FPGA, se puede realizar vía hardware de una manera eficiente empleando hardware ad hoc que realiza sumas y multiplicaciones simultáneamente. La mayoría de las FPGA

25 contienen unos bloques lógicos que realizan estos tipos de operaciones de multiplicación y acumulación de forma atómica. Normalmente, a este tipo de operaciones se les denomina operaciones MAC (acumulación múltiple o multiplyaccumulation). Concretamente, el fabricante de FPGA Xilinx ® denomina a estas unidades hardware MAC dedicadas, DSP Slice. En la presente invención se denominan bloques MAC.

30 El hardware del error cuadrático medio entre la signatura de un objeto y la de una característica se muestra en la figura 13. Como se observa en esta figura, el hardware necesario comprende un registro de desplazamiento o línea de retardo (esto es un array de memoria que desplaza el contenido de cada posición un lugar a la derecha o a la izquierda en cada ciclo de reloj) así como tantos bloques MAC como elementos tenga la signatura del objeto con el que queremos establecer el error cuadrático medio. El funcionamiento se describe a continuación.

i. En primer lugar los datos de la signatura del objeto se cargan en un array de memoria. En el ejemplo mostrado en la figura, la línea de datos B.

ii. A continuación se cargan los datos de la signatura de la característica en una línea de retardo.

iii. En cada ciclo de reloj, la línea de retardo desplaza un lugar de memoria a la derecha el contenido de cada posición.

iv.: Los contenidos de las líneas A y B se restan registro a registro y los resultados obtenidos se introducen en los bloques MAC para realizar el cuadrado del error.

v.: Finalmente, los bloques MAC se ejecutan por orden de izquierda a derecha multiplicando los contenidos de sus entradas (A y B) y sumando posteriormente el resultado del bloque MAC de su izquierda.

vi. El resultado final del error cuadrático medio para un desplazamiento concreto de la línea de retardo se devuelve en la salida del bloque MAC más a la derecha.

El elemento descrito configura un bloque lógico ECM que realiza el error cuadrático medio de dos signaturas (60) y comprende tres entradas: las signaturas de la característica (SCAR) y del objeto (SOBJ) así como el desplazamiento relativo entre ellas. La salida del bloque es el error cuadrático medio de los datos de entrada para un desplazamiento concreto.

La ventaja de trabajar con una FPGA frente a un procesador de propósito general es que es posible reservar el hardware necesario para replicar los bloques lógicos que queramos y, de este modo, acelerar la ejecución de los procesos. Así, el número de bloques lógicos ECM

(60) que serán necesarios para acelerar al máximo la asociación será igual al número de segmentos de objetos con los que queremos establecer la asociación de las características extraídas en la etapa de segmentación (101).

El esquema de asociación general se muestra en la figura 14, donde en primer lugar se tienen en cuenta aquellas secciones de objetos que son compatibles con la última posición conocida del vehículo. En segundo lugar se establece una asociación inicial por ángulo de visión del sensor entre las características segmentadas con los segmentos objetos considerados. En tercer lugar, se calculan las signaturas de las características, así como la de los segmentos de objetos. En cuarto lugar, se emplea el bloque lógico ECM (60) para calcular la función de error cuadrático medio entre las signaturas asociadas. En quinto lugar, se seleccionan los máximos de las salidas de los bloques (ECM) (60) que sean compatibles, esto es, que mantengan una relación angular entre ellos que sea coherente con la que existe entre los ángulos de visión de las características del scan. En la figura 15 se muestran las salidas de los módulos ECM en función del ángulo de visión del sensor (α). En dicha figura se han representado los diferentes mínimos para las funciones de error cuadrático medio (ECM) calculadas. Así, los mínimos identificados mediante un punto son máximos compatibles entre sí porque mantienen la misma relación angular que existe entre las características extraídas del scan. Sin embargo, el mínimo identificado mediante una cruz no es un mínimo compatible puesto que no está situado en alguna de las ventanas de compatibilidad αi y por tanto no será tenido en cuenta a la hora de generar a partir de ella una hipótesis de localización.

Finalmente, se selecciona uno de los mínimos que se encuentren en alguna de las regiones αi para establecer una pareja de puntos de referencia en las signaturas correspondientes. Cada uno de los posibles máximos establece una hipótesis de localización. Continuando con el ejemplo de la figura 15, de todos los mínimos de las funciones de error cuadrático medio ECM, sólo las identificadas como ECM1 y ECM5 indican las mejores hipótesis de localización porque en ellos es donde la función ECM crece y decrece con mayor rapidez. Sin embargo, en los puntos identificados en ECM4 en las ventanas 2, 3 y 4, la función es casi constante en un entorno. Por lo tanto, se escogerán como hipótesis de localización más prometedoras o fiables aquellos mínimos de la función ECM donde su variación de pendiente sea lo mayor posible.

Resuelta la localización, se calcula una figura de mérito del encaje entre el scan y el mapa (por ejemplo, el error cuadrático medio). En caso de que esta no resulte todo lo buena que se esperaba, se intenta mejorar volviendo al principio del proceso, esto es, reasociando las características a los segmentos del mapa hasta conseguir un mejor encaje basada la figura de mérito establecida.

Finalmente, y tras resolver la localización a partir del encaje del scan con el mapa (localización basada en la observación) queda por determinar como se establece la fusión de esta con la procedente de la información proporcionada por la odometría del vehículo.

Cada hipótesis de localización basada en la observación contemplada en el proceso descrito en la sección anterior tiene asociada una función de densidad de probabilidad gaussiana cuya media (µ) y covarianza (Σ) quedan establecidas a partir de las múltiples realizaciones del algoritmo ICP desde diferentes puntos iniciales. A su vez, cada gaussiana queda ponderada en función de la figura del mérito (error cuadrático medio) obtenida por su hipótesis de localización según hemos descrito anteriormente.

Del mismo modo, la hipótesis de localización procedente de la predicción también se modela como una gaussiana de parámetros (µp,Σp) establecidos según la dinámica del vehículo. Así, la localización final se obtiene fusionando las hipótesis de localización basadas en la observación y la única de predicción, cada una de ellas con un peso asociado que indica la fiabilidad de la hipótesis, tal y como se muestra en la figura 16.

Los pesos de cada una de las hipótesis de localización obtenidas de la observación se normalizan, de modo que su suma es igual a 1. De esta manera, la función densidad de probabilidad de la localización final del vehículo resulta ser el producto de todas las funciones de densidad anteriormente comentadas: N funciones de densidad de probabilidad, que están asociadas a N hipótesis de observación consideradas y una función de densidad asociada a la localización por predicción.

Claims

REIVINDICACIONES

1.-Método de localización y mapeo simultáneo para dispositivos robóticos equipados con

sensores para detectar su posición en un mapa, caracterizado por que comprende:

a) obtener datos relativos a dicha posición en forma de:

-

características (53) definidas como secuencias ordenadas de puntos correspondientes a coordenadas del mapa proporcionadas por los sensores; y

-

objetos (52) definidos como secuencias ordenadas de puntos correspondientes a las coordenadas de posición (xi, yi) de dichos dispositivos en el mapa, junto con un peso (P) proporcional al grado de incertidumbre de dicha posición respecto de la posición real del dispositivo en el mapa;

b) calcular el entorno próximo de la posición del dispositivo mediante la definición de signaturas (SCO) (54, 55) donde

siendo (xi+1, yi+1) coordenadas de los puntos consecutivos a los puntos (xi, yi) en una secuencia ordenada de los objetos;

c) realizar una asociación entre las signaturas de observación (SOBV) (55) de las coordenadas obtenidas mediante los sensores, y las signaturas de los objetos del mapa (SOBJ) (54) que son compatibles con su posición, calculando el error cuadrático medio de ambas signaturas (ECM (SOBJ, SOBV)), obteniendo una medida de su similitud.
2.-Método de acuerdo con la reivindicación anterior que comprende, además, las etapas de:

d) calcular el desplazamiento relativo (56) para el que se produce un mínimo de la función de error cuadrático medio (ECM (SOBJ, SOBV)), estableciendo una asociación entre los puntos de la signatura de la observación con los de la signatura asociada del objeto;

e) establecer puntos homólogos entre la característica de observación (OBV) y su objeto asociado (OBJ) (57, 57’);

f) establecer asociaciones entre los puntos de la secuencia de la característica de observación (OBV) y la del objeto (OBJ) en un entorno del mapa centrado en puntos homólogos de referencia (58);

g) resolver la localización (59) del dispositivo robótico en el mapa.
3.-Método de acuerdo con la reivindicación anterior donde, una vez realizada la localización

(59) del dispositivo robótico, se actualiza el mapa a partir de su estado actual y de la información procedente de los sensores.
4.-Dispositivo de localización y mapeo simultáneo para dispositivos robóticos que comprende medios para ejecutar el método según cualquiera de las reivindicaciones 1 a 3.
5.-Robot móvil que comprende medios para ejecutar el método según cualquiera de las reivindicaciones 1 a 3.