ES2855119T3 - Método y dispositivo para el mapeo y la localización en tiempo real - Google Patents

Abstract

Un método para el mapeo en tiempo real, la localización y el análisis de cambios de un entorno, en particular en un entorno sin GPS, que comprende las siguientes etapas: (A) si no existe un mapa de referencia 3D del entorno, construir un mapa de referencia 3D de dicho entorno mediante (a) obtener el entorno con un escáner de rango láser (1) móvil en tiempo real a una velocidad de al menos 5 tramas por segundo, para proporcionar datos de escáner 3D, (b) construir una presentación de mapa utilizando los datos del escáner 3D para cada una de la pluralidad de poses del escáner de rango láser (1), teniendo cada pose una nube de puntos asociada definida por los datos del escáner 3D, teniendo la presentación del mapa una estructura de datos configurada para manejar de manera nativa puntos 3D, que se basa en una estructura híbrida compuesta por una estructura de vóxeles dispersa utilizada para indexar una lista compacta y densa de características en la presentación del mapa, lo que permite un acceso aleatorio en tiempo constante en coordenadas de vóxeles independientemente del tamaño del mapa, y un almacenamiento eficiente de los datos con escalabilidad sobre el entorno, y (c) construir, utilizando la presentación del mapa, el mapa de referencia 3D para el entorno utilizando un marco de localización y mapeo simultáneo 3D (3D SLAM), comprendiendo dicho edificio (i) utilizar un módulo de odómetro, estimando una pose actual del escáner de rango láser (1) en base al registro de una última nube de puntos en una presentación de mapa local, (ii) utilizar un módulo de optimización de trayectoria local, refinando la trayectoria de un conjunto de nubes de puntos para minimizar la deriva en la presentación del mapa local, y (iii) realizar fuera de línea una optimización de trayectoria global mediante la reconstrucción de un mapa completo del entorno teniendo en cuenta los cierres de bucles de trayectorias, formando de este modo dicho mapa de referencia 3D; y (B) en base a un mapa de referencia 3D existente del entorno, realizar un mapeo en tiempo real, localización y análisis de cambios de dicho entorno (d) obteniendo el entorno con un escáner de rango láser (1) en tiempo real a una velocidad de al menos 5 tramas por segundo, para proporcionar datos de escáner 3D, (e) durante el reconocimiento de lugar, identificando una ubicación actual del escáner de rango láser (1) dentro del entorno sin conocimiento previo de la pose del escáner de rango láser durante el reconocimiento de lugar, y calculando previamente descriptores simples y compactos de una escena obtenida por el escáner de rango láser (1) utilizando un espacio de búsqueda reducido dentro de la escena para autolocalizar el escáner en tiempo real, comprendiendo cada descriptor de la escena una imagen de rango de bins regulares, donde cada bin tiene un valor de rango medio estimado, (f) después de determinar la ubicación del escáner en el entorno, realizando un seguimiento de la pose del escáner registrando los datos del escáner actual dentro de dicho mapa de referencia 3D existente del entorno utilizando el método de punto más cercano iterativo estándar que emplea datos que comprenden información del vecino más cercano almacenada en el mapa de referencia 3D, (g) calculando la distancia entre cada punto de escaneo en los datos de escáner actuales y el punto más cercano en el mapa de referencia 3D, en donde el análisis de cambio se realiza aplicando un umbral a esta distancia, por lo que cada punto en los datos de escáner actuales que no tiene un vecino correspondiente en el modelo de referencia se considera un cambio, (h) mostrando información sobre el mapa de referencia 3D y los datos actuales del escáner 3D en una interfaz de usuario en tiempo real, estando dicha información preferiblemente en codificada en color de acuerdo con una clasificación de cambio / no cambio de dicha información.

Description

DESCRIPCIÓN

Método y dispositivo para el mapeo y la localización en tiempo real

Campo técnico

La presente invención se refiere, en general, a la localización y al mapeo, especialmente en entornos sin GPS, tales como interiores.

Antecedentes de la técnica

Se han descrito o están disponibles comercialmente diferentes soluciones para permitir la obtención de entornos con fines tales como la localización o el mapeo. Diferentes enfoques han dado lugar a diferentes soluciones.

Entre estos, una serie de sistemas de navegación en interiores, comercializados y en prototipo, se basan en sensores inerciales (por ejemplo, FootSLAM de DLR, Chirange Geospatial Indoor Tracking). Son pequeños y económicos, no obstante, la precisión de la posición es baja y se desvía significativamente con el tiempo. Además, los sistemas inerciales no generan información cartográfica. Por lo tanto, solo son adecuados para fines de navegación y posicionamiento, no para la generación de mapas.

Otros sistemas de posicionamiento en interiores se basan en la transmisión de señales de radio, de manera similar a las señales de GPS en entornos exteriores. Algunos sistemas utilizan la infraestructura existente (por ejemplo, redes WiFi en aeropuertos, Navizon), otros requieren la instalación de una infraestructura exclusiva (por ejemplo, NextNav, SenionLab). Los sistemas prácticamente no tienen costes de sensores (la aplicación cliente utiliza un teléfono inteligente con una aplicación de software exclusiva), pero requieren una infraestructura de red que emita la señal de radio. Además, no generan información cartográfica. Por lo tanto, solo son adecuados para fines de navegación y posicionamiento, no para la generación de mapas.

Otro producto interesante utiliza el escaneo 3D. ZEB1 es un producto comercial que utiliza escaneo láser 3D para mapeo rápido (en interior). El láser está montado sobre un resorte y es necesario crear un movimiento oscilante a mano. Genera un modelo 3D preciso del entorno interior. No obstante, el sistema no proporciona información inmediata al usuario, ya que el procesamiento de datos se realiza fuera de línea. Por lo tanto, el sistema es adecuado para cartografía, pero no para la localización en tiempo real.

Otra solución más es una mochila láser desarrollada en la Universidad de California, Berkeley, publicada como “Indoor Localization and visualization using a human-operated backpack system”, por Timothy Liu et al., en las actas de la Conferencia Internacional de 2010 sobre Posicionamiento y Navegación en Interiores (IPIN - Indoor Positioning and Indoor Navigation). Se trata de un proyecto de I D que propone una mochila equipada con varios escáneres de línea 2D utilizados para generar un modelo 3D de entornos interiores. De nuevo, no proporciona visualización en línea.

Una última solución se llama LOAM (Odometría y mapeo Lidar - Lidar Odometry and Mapping, en inglés) y consiste en un sensor portátil con algoritmos asociados que combinan escaneo láser e imágenes de video para localización y mapeo en tiempo real.

Casi todas estas soluciones carecen de visualización en tiempo real / en línea y, lo que es más importante, no permiten ninguna interacción directa del usuario en las etapas de obtención y procesamiento.

Problema técnico

Un objetivo de la presente invención es proporcionar un sistema, dispositivo y método que no solo permitan la obtención, el mapeo y la localización en tiempo real, particularmente en entornos sin GPS, sino que también proporcionen visualización en tiempo real y la posibilidad de interacción del usuario. Además, la presente invención debería permitir también proporcionar una comparación en tiempo real de los datos obtenidos actualmente con mapas obtenidos previamente. Esto permitiría identificar diferencias o cambios ocurridos desde el último mapeo. Dicha identificación en línea de cambios o diferencias puede ser de gran ventaja en aplicaciones tales como inspecciones de seguridad, construcción civil, así como gestión de emergencias o desastres.

Descripción general de la invención

Para conseguir este objetivo, la presente invención propone, en un primer aspecto, que se proporcione un método de acuerdo con la reivindicación 1.

La invención se refiere, además, a dicho método, en donde el módulo de optimización de la trayectoria local comprende un mecanismo de ventana local que optimiza un fragmento de trayectoria compuesto por un conjunto de poses y sus nubes de puntos asociadas con respecto a un mapa construido hasta el último conjunto registrado, en donde los puntos son convertidos, preferiblemente, en coordenadas mundiales utilizando la interpolación de poses en el grupo SE3, y en donde se utiliza, preferiblemente, una generalización del método del punto más cercano iterativo para encontrar la trayectoria que alinea mejor todos los puntos con el mapa; en donde el mecanismo de ventana local funciona de tal manera que, cuando la distancia entre la primera y la última pose de la lista es mayor que un umbral, las poses de las nubes se optimizan, y se produce una nueva lista con la pose refinada y las nubes de entrada.

La estructura de datos está configurada para manejar puntos 3D de manera nativa, y se basa en una estructura híbrida compuesta por una estructura de vóxeles dispersa que se utiliza para indexar una lista (compacta y densa) de características en la presentación del mapa, lo que permite un acceso aleatorio en tiempo constante en coordenadas vóxel independientemente del tamaño de mapa, y un almacenamiento eficiente de los datos con escalabilidad sobre el espacio explorado.

En una realización aún más preferida, la estructura de datos puede mantener cinco representaciones diferentes de los datos almacenados, otorgando de este modo coherencia entre las representaciones de datos internos después de cada actualización del mapa, siendo las cinco representaciones

(i) una lista (compacta y densa) de características, L y un índice para el último elemento, Lúitimo donde cada elemento, li e L, contiene toda la información asociada a una característica en el mapa, tal como la posición y el vector unitario normal en coordenadas mundiales, y, preferiblemente, información adicional,

(ii) una máscara de validez (compacta y densa), M, donde cada elemento, m¡ e M, es un valor booleano que indica si su muestra correspondiente li e L es válida o no, garantizando que mi = 0, i > Lúttmo,

(iii) una lista de huecos, H, donde cada elemento, hi e H < Lúttimo indica que ihi no es válido, así que, mhi = 0,

(iv) una representación de vóxeles V dispersa, construida con un tamaño de celda parametrizable, que almacena en cada celda, vi e V, el índice de su característica correspondiente en L, donde las características en L y las celdas en V están relacionadas de una manera de uno a uno, de acuerdo con la posición de /, y el tamaño de celda de V, y

(v) un árbol kd, K, que se utiliza para realizar búsquedas del vecino más cercano en el mapa, y que solo almacena referencias a la lista densa L para mantener baja su tamaño de memoria.

El presente método puede comprender, además, la etapa en la que, dada una zona de interés expresada por una posición central y un radio, las características internas se seleccionan haciendo un bucle sobre los elementos almacenados en L y el árbol kd K se reconstruye como un mecanismo rápido para búsquedas del vecino más cercano.

Preferiblemente, la etapa (e) comprende la identificación de un conjunto de posibles ubicaciones del escáner en base a los datos del escáner de la etapa (d), teniendo dicha etapa (e), además, las siguientes subetapas:

(e l) en base a los últimos datos del escáner,

calcular un descriptor q asociado y

recuperar un conjunto de ubicaciones candidatas r. Las ubicaciones candidatas tienen un descriptor similar a q, es decir, la distancia en el espacio de descriptores es menor que un radio umbral r. El conjunto de ubicaciones candidatas r se puede recuperar realizando una búsqueda radial en K dado un radio de umbral r en el espacio de descriptores, preferiblemente para datos de escáner de visión horizontal de 360°, aumentando las ubicaciones candidatas calculando los descriptores de entrada adicionales, cambiando horizontalmente los valores de rango, correspondiendo cada descriptor a las lecturas que produciría el escáner si se girara sobre su eje local y a continuación girara de acuerdo con i cada conjunto resultante de ubicaciones candidatas,

(e2) asociar un peso a cada ubicación potencial

donde d ^p es el descriptor asociado a la ubicación r ^p recuperada de K

vvr

r es 1 para descriptores que coinciden perfectamente y 0 para descriptores en el límite de la esfera de búsqueda, y

(e3) recolectar pesos en w y normalizar estos pesos para tener un max w = 1.

Ventajosamente, la etapa (e) comprende, además, las subetapas

(e4) actualizar el conjunto de ubicaciones candidatas mientras el sensor se desplaza estimando el movimiento (utilizando el módulo del odómetro tal como se describe en la etapa (c) (i) del método del primer aspecto anterior) y reevaluar el peso para cada pose de candidato inicial en base a los resultados de la consulta en la nueva pose, y

(e5) iterar la subetapa de actualización hasta que las poses candidatas converjan en una única ubicación (es decir hasta que el método sea capaz de eliminar la ambigüedad de la pose actual).

Particularmente para el movimiento en el suelo, en el que el escáner de rango láser está montado en una persona (por ejemplo, con una mochila) o en un vehículo que atraviesa un piso, el método puede comprender las siguientes etapas

(i) identificar en el mapa de referencia 3D las extensiones de un piso, en donde la extracción de piso se realiza sobre una representación de vóxeles dispersa del entorno (mapa de referencia 3D), V, donde cada celda completa, v(l), de la representación de vóxeles dispersa contiene un vector normal a la superficie definida localmente por los puntos alrededor de su centroide, n extrayendo un subconjunto de vóxeles que representan celdas de piso candidatas, ^ ^ comprobando que la componente vertical en sus normales asociadas sea dominante, es decir, n '(0,0,1) donde s es habitualmente un valor comprendido entre 0,5 y 1;

(ii) determinar la capacidad de ser alcanzadas de las celdas, en donde dada una celda alcanzable ^ r ^ , todas ( „d) „a>

las celdas circundantes, ’ se consideran alcanzables si se cumplen las siguientes condiciones:

donde - Vtamano de celda (6) representa la distancia máxima de etapa (por ejemplo 0,5 metros para un movimiento caminando, o Vtamano de celda para un movimiento en coche), ^1 en (7) representa el tamaño C , )

máximo de etapa vertical y * en (8) representa el volumen simplificado del observador, centrado sobre la celda del piso gi.

La estructura de mapa utilizable en el contexto de la presente invención comprende, preferiblemente, dos listas diferentes de elementos que están almacenados y sincronizados: una lista (compacta) de planos, L, y una cuadrícula (densa) de vóxeles, V, construida con un tamaño de vóxel específico, almacenando cada plano h e ¿ una posición en coordenadas mundiales, p¡, y una unidad normal, n i ; en donde cada vóxel, v¡ e ^ almacena un estado actual que puede ser completo, vacío o cercano, almacenando los vóxeles llenos un índice en el plano ^ e i , cuya posición asociada cae dentro, almacenando las celdas vacías una referencia nula y almacenando las celdas cercanas un índice en el plano ^ cuya distancia de posición dv asociada al centro del vóxel es la más pequeña; preferiblemente, se considera un vóxel cercano solo si la distancia d^v está por debajo de un umbral determinado; de lo contrario, el vóxel se considera vacío.

Para mejorar la robustez general del sistema, se considera combinar el seguimiento del escáner con un odómetro (por ejemplo, utilizando el módulo del odómetro tal como se describe en la etapa (c) (i) del método del primer aspecto anterior), de tal manera que, después de que una pose haya sido estimada, sus puntos asociados en coordenadas mundiales son almacenados en un árbol kd (creando de este modo un mapa del odómetro), dada una nueva obtención (nube de puntos obtenida) por el escáner de rango láser, (es decir, cuando un algoritmo de registro crea los conjuntos r .u ^{- m}

de puntos) (Pí ), busca los vecinos más cercanos tanto en el mapa de referencia 3D (Q n ) como en la nube de

—O

puntos previamente fijados (mapa del odómetro) (^ ,nn.í ), en donde las correspondencias se definen como

donde s corresponde al tamaño de la celda de vóxeles y compensa la resolución diferente entre el mapa de referencia real voxelizado y el árbol kd no discretizado de la nube previamente fijada.

En un tercer aspecto, la invención propone un dispositivo de escaneo láser móvil para mapeo, localización y análisis de cambios en tiempo real, en particular en entornos sin GPS, implementando uno o más de los métodos descritos en el presente documento. En particular, la invención se refiere a un dispositivo de escaneo láser móvil para mapeo en tiempo real, localización y análisis de cambios, en particular en entornos sin GPS, que comprende un escáner de alcance láser en tiempo real, una unidad de procesamiento, una unidad de fuente de alimentación y una unidad de control y visualización portátil, en donde el escáner de rango láser en tiempo real es capaz de obtener el entorno con una velocidad de al menos 5 tramas, preferiblemente, al menos 10 tramas, por segundo para proporcionar datos del escáner, la unidad de procesamiento está dispuesta para analizar dichos datos de escáner y para proporcionar resultados de procesamiento que comprenden un mapa / modelo 3D, localización e información de cambios a la unidad de control y visualización portátil, que está dispuesta para mostrar dichos resultados de procesamiento y permitir que un usuario controle el dispositivo de escaneo láser móvil.

Un dispositivo de acuerdo con la invención es, por lo tanto, capaz de procesamiento en línea, en tiempo real, proporcionando mapeo / modelizo 3D del entorno, localización precisa del usuario (con respecto al mapa generado o mapa / modelo existente), detección de cambios con respecto a un modelo obtenido previamente y se basa completamente en la señal láser, lo que lo hace independiente de la iluminación ambiental y de la señal GPS. Además, no requiere sensores adicionales tales como GPS o sensores inerciales. No obstante, la presente invención no excluye la agregación de sensores adicionales si se considera útil. Por lo tanto, se pueden agregar sensores opcionales para enriquecer el modelo generado (por ejemplo, cámaras a color). Además, aunque el dispositivo es capaz de proporcionar resultados en línea y en tiempo real al usuario, se prevé asimismo utilizar los datos obtenidos y seguir procesándolos fuera de línea, por ejemplo, para el refinado del modelo 3D obtenido para una localización futura y el análisis de cambios.

El dispositivo de acuerdo con la presente invención se puede utilizar y es útil en numerosas aplicaciones tales como, por ejemplo, mapeo / modelizado 3D (en interiores), gestión de instalaciones, localización y navegación en interiores precisa y en tiempo real, verificación de la información de diseño, análisis de cambios (por ejemplo, para inspecciones de salvaguardias), seguimiento del progreso (por ejemplo, para construcción civil), gestión de desastres y respuesta a los mismos, etc.

En el dispositivo de escaneo láser móvil, la unidad de visualización y control es preferiblemente un ordenador con pantalla táctil, más preferiblemente un ordenador de tableta.

El dispositivo de escaneo láser móvil es más preferiblemente una mochila o un dispositivo montado en un vehículo.

En un cuarto aspecto, la invención propone la utilización de métodos o dispositivos de escaneo láser móviles, tales como los descritos en el presente documento para mapeo / modelizado 3D en exteriores e interiores, preferiblemente en interiores; la gestión de instalaciones; la localización y navegación interior precisa y en tiempo real; la asistencia a personas discapacitadas o ancianas; la verificación de información de diseño; el análisis de cambios, tal como para inspecciones de salvaguardias; el seguimiento del progreso, tal como en la construcción civil; o la gestión de desastres y la respuesta a los mismos.

Un quinto aspecto se refiere a un producto de programa informático que tiene instrucciones ejecutables por ordenador para hacer que un dispositivo programable, preferiblemente un dispositivo de escaneo láser móvil o su unidad de procesamiento tal como se describe en el presente documento, ejecuten uno o varios de los métodos de la presente invención.

En un último aspecto, la invención también se refiere a un medio legible por ordenador, teniendo almacenado en el mismo datos que representan instrucciones de creación ejecutables por un procesador programado, comprendiendo el medio legible por ordenador instrucciones para hacer que un dispositivo programable, preferiblemente un dispositivo de escaneo láser móvil de la invención o su unidad de procesamiento, ejecute uno de los presentes métodos.

Los aspectos anteriores, y aún más detalles de variantes, alternativas y combinación de características, así como sus ventajas, se describirán con más detalle a continuación.

Breve descripción de los dibujos

A continuación, se describirán aspectos y realizaciones preferidos de la invención, a modo de ejemplo, con referencia a los dibujos adjuntos.

Figura 1: Componentes de hardware de una realización preferida de un dispositivo de escaneo láser móvil, de acuerdo con la presente invención, llamado plataforma de escaneo láser móvil (sistema MLSP (Mobile Laser Scanning Platform, en inglés), o simplemente MLSP) que comprende un escáner láser 3D 1, una mochila 2, una unidad de procesamiento 3 (contenida en el interior de la mochila, que se muestran por separado solo con fines ilustrativos) y una tableta 4.

Figura 2: Instantánea (en blanco y negro, de una instantánea originalmente coloreada) de la interfaz de usuario tal como es proporcionada al usuario en tiempo real. Muestra un entorno de túnel que se ha escaneado en dos momentos. En la pantalla de color real, el verde indica que no hay cambios entre las obtenciones y el rojo indica nuevas construcciones entre las dos obtenciones.

Figura 3: Efecto del cierre del bucle en una pista de muestra de los conjuntos de datos KITTI. La trayectoria se muestra como estimada en línea y como trayectoria globalmente optimizada. El mapa (real) está coloreado de acuerdo con los vectores normales de los puntos con un esquema diferente para los dos mapas (tal como que la zona violeta es el mapa local).

Figura 4: Tiempo de selección de puntos para el mapa local (que habitualmente contiene menos de 1 M de características) y el mapa global (que habitualmente contiene más de 1 M de características), figura 4(a) tiempo de selección del punto del mapa local para diferentes radios de búsqueda y figura 4(b) tiempo de selección del punto del mapa global para diferentes radios de búsqueda.

Figura 5: Realización preferida de las representaciones cartográficas propuestas. Las celdas completas se muestran como cuadros de color gris oscuro. Las celdas cercanas se representan como cuadros de color gris claro con una línea que conecta su centroide con el vecino más cercano asociado. Las celdas vacías se muestran como cuadros blancos.

Figura 6: Histogramas de rotación para un entorno simétrico y para uno no simétrico.

Figura 7: Selección de típicos. Los ejes representan las principales dimensiones dominantes de las transformaciones detectadas. Cada punto representa una transformación candidata en gris de acuerdo con la iteración en la que se ha marcado como valor atípico (se han omitido algunas transformaciones atípicas demasiado alejadas del centro). Los puntos de color gris oscuro en las elipses centrales representan transformaciones marcadas como típicas. Las elipses representan las estimaciones normales en iteraciones posteriores específicas.

Figura 8: Resultados del algoritmo de extracción del piso. Los puntos negros representan las posiciones del escáner durante la obtención. Estas ubicaciones se han utilizado para seleccionar automáticamente el conjunto de celdas activas iniciales.

Figura 9: Selección de parámetros empíricos para la reducción del espacio de búsqueda, desviación del sensor (izquierda) con respecto a la altura media con respecto al suelo observada durante varios recorridos, desviación (derecha) del sensor con respecto al eje vertical (Z) observada durante varios recorridos.

Figura 10: Análisis de la deriva utilizando solo el reconocimiento de lugar (seguimiento de clasificación) donde el clasificador contiene datos relacionados con múltiples entornos. La referencia real para dicho experimento se considera la trayectoria final generada por el módulo de seguimiento inicializado en el edificio correcto para una descripción de la métrica de error adoptada.

Figuras 11 (a) y (b): Dos rutas de muestra, utilizadas para generar el análisis de la deriva que se muestra en la figura 10. Línea de puntos, la ruta fundamental del terreno estimada utilizando el sistema completo. Línea continua, la ruta estimada utilizando el seguimiento por clasificación. El círculo negro muestra la trama después de la cual el usuario ha sido identificado de manera única en un edificio específico.

Figura 12: Resultados del algoritmo de selección de típicos propuesto.

Figura 13: Resultados de la integración del odómetro durante un recorrido de muestra dentro de un edificio donde el sensor se mueve a una habitación no mapeada (A, ilustrada en (la derecha)) sin perder de vista su posición y, a continuación, realiza dos bucles en el exterior del edificio (C y D).

Figura 14: Comparación de la precisión del seguimiento entre el ICP estándar (línea discontinua) y la implementación robusta propuesta (línea continua) en un entorno sin valores atípicos (superior) y en un entorno con valores atípicos (inferior).

Figura 15: Rendimiento general del sistema durante el seguimiento para una configuración montada en una mochila: las líneas grises continuas son el tiempo empleado en procesar cada trama (en segundos). La línea horizontal discontinua indica el tiempo máximo de ejecución para obtener resultados en tiempo real utilizando un sensor Velodyne HDL-32E (12 Hz).

Otros detalles y ventajas de la presente invención serán evidentes a partir de la siguiente descripción detallada de varios aspectos y realizaciones no limitativos haciendo referencia a dibujos adjuntos. De hecho, la descripción detallada a continuación no debe ser interpretada como una limitación del alcance de la invención, sino más bien para ilustrar aspectos particulares presentados en la descripción general, reivindicaciones y dibujos.

Descripción de las realizaciones preferidas

Tal como ya se mencionó anteriormente, una de las principales ventajas de las realizaciones preferidas de la presente invención, tal como se describe en el presente documento, residen en el concepto de proporcionar análisis y supervisión de cambios en tiempo real en entornos sin GPS (por ejemplo, interiores). El usuario puede inspeccionar una instalación y ver los cambios en un dispositivo de mano mientras camina por la instalación. Las metodologías y algoritmos subyacentes preferidos se resumen a continuación y se detallan más adelante.

Un flujo de trabajo básico para una ubicación previamente desconocida (no escaneada) requiere en principio dos etapas: (A) la construcción de un modelo de referencia 3D en T0 y (B) el análisis de la localización, el seguimiento y el cambio en base al modelo de referencia 3D en T1. Cuando vuelva a visitar dicha ubicación o en los casos en que ya exista un mapa apropiado, la etapa (B) será suficiente.

(A) Construcción del mapa de referencia 3D

El mapa de referencia 3D se construye utilizando una implementación 3D SLAM (localización y mapeo simultáneo) basada en un escáner de alcance láser móvil tal como se describe a continuación. Las características principales son, preferiblemente:

1) Una presentación de mapa eficiente que permite el acceso aleatorio a la muestra en tiempo constante, búsqueda rápida del vecino más cercano y escalabilidad en áreas extensas (véase la sección A.2. que se muestra a continuación)

2) El marco SLAM (véase la sección A.3. que se muestra a continuación) contiene:

a) un odómetro, para estimar la pose actual en base al registro de la última nube en la representación del mapa local;

b) una optimización de la trayectoria local que refina la trayectoria de un conjunto de nubes para minimizar la deriva en el mapa generado;

c) una optimización de la trayectoria global, que permite reconstruir un mapa completo del entorno aprovechando los cierres de bucles.

El odómetro se utiliza habitualmente en tiempo real. La optimización del mapa se puede realizar en una etapa de posprocesamiento.

(B) Análisis de la localización, el seguimiento y el cambio en base al modelo de referencia 3D

El análisis de la localización, el seguimiento y el cambio en tiempo real requiere, en general, un mapa de referencia 3D existente del entorno, que se ha generado previamente tal como se ha descrito anteriormente. Los componentes principales son preferiblemente

1) Durante el reconocimiento de lugar, el sistema identifica la ubicación actual dentro del entorno conocido sin conocimiento previo de la pose del sensor. Calcula previamente descriptores simples y compactos de la escena y utiliza una estrategia eficiente para reducir el espacio de búsqueda con el fin de autolocalizar el sensor en tiempo real (véase la sección B.2. que se muestra a continuación).

2) Una vez que el sensor está localizado en el entorno conocido, el sistema comienza a realizar un seguimiento de la pose del sensor registrando la observación actual (escaneo 3D) dentro del mapa de referencia 3D utilizando el método estándar de punto iterativo más cercano (ICP - Iterative Closest Point, en inglés). Con el fin de seguir con precisión la pose del sensor en tiempo real, el sistema implementa una serie de mejoras, por ejemplo, emplea una estructura de datos especialmente diseñada para búsquedas rápidas del vecino más cercano (véase la sección B.3. que se muestra a continuación).

3) Dada la información del vecino más cercano en la estructura de datos, MLSP puede calcular eficientemente la distancia entre cada punto de escaneo en la observación actual y el punto más cercano en el modelo de referencia 3D. El análisis de los cambios se realiza aplicando un umbral simple a esta distancia, por ejemplo, cada punto en el escaneo actual que no tiene un vecino correspondiente en el modelo de referencia (o que tiene un vecino correspondiente en el modelo de referencia, pero que está más lejos que el umbral) se considera un cambio. La interfaz de usuario en tiempo real muestra el modelo de referencia 3D y las observaciones actuales que están codificadas por colores de acuerdo con una clasificación de cambio / no cambio.

A. Construcción de un mapa de referencia 3D

El mapeo 3D preciso y la estimación de la trayectoria 6DOF utilizando sensores exteroceptivos son problemas clave en muchos campos. Los sensores láser en movimiento en tiempo real ganaron popularidad debido a sus precisas medidas de la profundidad, alta frecuencia de tramas y gran campo de visión.

En un aspecto preferido, la presente invención propone un método o marco de optimización para la localización y el mapeo simultáneo (SLAM - Simultaneous Localization And Mapping, en inglés) que modeliza adecuadamente el proceso de obtención en un escenario de escaneo mientras se mueve. Cada medición se proyecta de nuevo correctamente en la trama de referencia del mapa considerando una trayectoria de tiempo continuo que se define como la interpolación lineal de un conjunto discreto de poses de control en SE3. La invención propone, asimismo, una estructura de datos particularmente eficiente que hace uso de una representación de vóxeles dispersa híbrida, lo que permite una gestión de mapas grandes. Gracias a esto, los inventores también pudieron realizar una optimización global sobre las trayectorias, restableciendo la deriva acumulada cuando se realizan bucles.

Los inventores mostraron experimentalmente que dicha estructura mejora la localización y el mapeo con respecto a soluciones que compensan los efectos de distorsión sin incluirlos en la etapa de optimización. Además, los inventores muestran que la estructura de datos propuesta proporciona operaciones lineales o constantes en el tiempo con respecto al tamaño del mapa, para realizar SLAM en tiempo real, y maneja mapas muy grandes.

A.1. Introducción

La generación de mapas 3D y la estimación de trayectorias son bloques de construcción fundamentales para una amplia variedad de aplicaciones en robótica, guiado autónomo y vigilancia. Las técnicas de localización y mapeo simultáneos (SLAM) construyen conjuntamente el mapa de un entorno desconocido y localizan el sensor en el mismo entorno. Se han propuesto formulaciones SLAM para cámaras estándar, cámaras de profundidad y escáneres láser. La mayoría de los sistemas SLAM basados en escáneres láser utilizan variaciones del algoritmo Iterative Closest Point (ICP) para realizar alineaciones de escaneos. Se puede encontrar una revisión de los algoritmos ICP centrados en aplicaciones en tiempo real en el documento de S. Rusinkiewicz y M. Levoy, “Efficient variants of the ICP algorithm”, en 3DIM, 2001. Los sensores LIDAR 3D en movimiento en tiempo real, tales como los escáneres Velodyne, han ganado popularidad recientemente: estos dispositivos tienen una alta velocidad de datos, a menudo proporcionan un campo horizontal completo de 360° y tienen una buena precisión en las mediciones de distancia.

Dichos sensores (escáneres) obtienen mediciones mientras se mueven y, por lo tanto, representan sistemas de proyección no centrales que deforman las tramas obtenidas a lo largo de la trayectoria. La alineación de dichas nubes de puntos producidas requiere un tratamiento adecuado del efecto de deformación en los puntos 3D. El marco SLAM propuesto en el documento de F. Moosmann y C. Stiller, “Velodyne SLAM”, en IVS, 2011, deshace la deformación de cada nube dada la velocidad actual del sensor, realiza ICP y vuelve a deshacer la deformación de los puntos con la nueva velocidad estimada. El algoritmo LOAM (J. Zhang y S. Singh, “LOAM: Lidar odometry and mapping in real-time”, en RSS, 2014) realiza una estimación continua del movimiento al centrarse en los bordes y las características planas para eliminar el efecto de deformación en cada nube. Cuando se genera una trama completa, se deshace la deformación de la nube de puntos final utilizando la pose final prevista. El trabajo de C. H. Tong, S. Anderson, H. Dong y T. D. Barfoot, “Pose interpolation for laser-based visual odometry”, Journal of Field Robotics, vol. 31, págs. 731 a 757, 2014, realiza la interpolación empleando un modelo de proceso gaussiano (GPGN - Gaussian Process Gauss-Newton, en inglés) de tiempo continuo que se basa en características coincidentes en las imágenes de reflectancia de la obtención.

En un aspecto preferido de la presente invención se propone utilizar un mecanismo de ventana local que optimiza un fragmento de trayectoria compuesto por un conjunto de poses y sus nubes de puntos asociadas con respecto al mapa construido hasta el último conjunto registrado. Los puntos se convierten en coordenadas mundiales utilizando la interpolación de pose en el grupo SE3 y se utiliza una generalización de ICP para encontrar la trayectoria que alinea mejor todos los puntos con el mapa. En esta formulación, la operación de deshacer la deformación forma parte de la estrategia de optimización.

Un aspecto importante para los sistemas SLAM es su escalabilidad a grandes entornos y una gestión en tiempo real del mapa para soportar la rutina de optimización. En general, la escalabilidad se consigue utilizando estructuras dispersas, tales como árboles octales generales, mapas de vóxeles densos que utilizan indexación cíclica de volumen o representaciones dispersas basadas en nieblas de vóxeles. En un aspecto, la invención se centra en una estructura de datos que maneja de forma nativa puntos 3D y que se basa en una estructura híbrida compuesta por una estructura de vóxeles dispersa, que se utiliza para indexar una lista compacta densa de características. Esto permite un acceso aleatorio en tiempo constante en coordenadas de vóxeles independientemente del tamaño del mapa, y un almacenamiento eficiente de los datos con escalabilidad sobre el espacio explorado. La estructura propuesta actualmente es capaz de mantener en la memoria todo el mapa global y actualizar las secciones locales en caso de que se emplee la optimización del gráfico (por ejemplo, para realizar cierres de bucle).

Las principales contribuciones de algunas realizaciones de la invención son (i) la utilización de un algoritmo ICP generalizado que incorpora deshacer la deformación en el proceso de estimación, (ii) la utilización de una estructura eficiente para la gestión de mapas que permite tanto consultas espaciales rápidas como gestión de grandes entornos. Los inventores han validado su enfoque utilizando conjuntos de datos disponibles públicamente y entornos interiores / exteriores obtenidos adicionales.

La sección A.2. que se muestra a continuación presenta la estructura de datos para la gestión de mapas y sus operaciones disponibles; la sección A.3. presenta el marco de optimización; la sección A.4. muestra los resultados experimentales obtenidos con este método y, en la sección A.5. saca algunas conclusiones.

A.2. Representación de mapas

Una estructura de datos adecuada para aplicaciones SLAM en tiempo real debería proporcionar (i) acceso a muestra aleatorio en tiempo constante (en promedio) a características almacenadas, (ii) iteración de características exhaustiva en tiempo lineal con respecto al número de elementos almacenados e (iii) búsquedas rápidas del vecindario más cercano dada una función de consulta. Además, debería proporcionar (iv) escalabilidad sobre el espacio explorado y (v) debería soportar de manera eficiente la agregación y eliminación de características.

La propiedad (i) se asocia, en general, a representaciones densas de vóxeles, donde las necesidades de memoria para la escalabilidad (iv) son el principal inconveniente y las exploraciones exhaustivas (ii) son lentas. La propiedad (ii), a la inversa, está asociada a estructuras dispersas, donde las necesidades de memoria (iv) son muy bajas, pero los tiempos de acceso aleatorio (i) son lentos (logarítmicos en el caso de los árboles kd). Para aprovechar los beneficios intrínsecos de las estructuras densas y dispersas al tiempo que conserva todas las propiedades necesarias, la estructura preferida del mapa propuesta mantiene cinco representaciones diferentes de los datos almacenados. Se debe garantizar la coherencia entre las representaciones de datos internos después de cada actualización del mapa.

(i) Una lista compacta y densa de características, L y un índice para el último elemento, ^{U ltim o,} donde cada elemento, e L , contiene toda la información asociada a una característica en el mapa (información de posición, normal y adicional).

(ii) Una máscara de validez compacta y densa, M, donde cada elemento, m¡ e M , es un valor booleano que indica si su muestra correspondiente, 1 F 1 , es válida o no, garantizando que m¡ = > ^{U ltim o.}

(iii) Una lista de huecos, H, donde cada elemento, ^

^{U ltim o,} indica que ^ no es válido por lo que, ^>u*¡ ^ .

(iv) Una representación de vóxeles V dispersa, construida con un tamaño de celda parametñzable, que almacena en cada celda, vr e * , el índice de su característica correspondiente en L. Las características en L y las celdas en V están relacionadas de una manera de uno a uno, de acuerdo con la posición de /, y con el tamaño de celda de V. La presente representación de vóxeles dispersos se basa en una estructura OpenVDB (K. Museth, “Vdb: High-resolution sparse volumes wirh Dynamic topology.” ACM Transaction on Graphics, vol.

32, no. 3, 2013).

(v) Un árbol kd, K, que se utiliza para realizar búsquedas de vecindarios más cercanos en el mapa. K solo almacena referencias a la lista L densa para mantener bajo su tamaño de memoria. El árbol kd se puede construir en una región local del mapa si es necesario (por ejemplo, siguiendo una zona alrededor de la última ubicación de observación).

Al tener una lista densa de características, el tiempo para explorar exhaustivamente todo el mapa es lineal en la cantidad de elementos contenidos. Por otro lado, las consultas arbitrarias se resuelven en un tiempo de acceso aleatorio constante (en promedio) aprovechando la estructura de vóxeles dispersos de OpenVDB y el sistema de almacenamiento en memoria oculta.

Dada una nueva característica p, que se agregará al mapa, la estructura de datos propuesta se modifica de la siguiente manera: considérese la posición mundial de la característica, pw y calcúlese su celda de vóxeles correspondiente, v¡.

Si la celda ya está llena (’V - 0 ), su información asociada se recupera de ^ y el valor se actualiza si es necesario. En caso contrario (’ ’ < ®), se agrega una nueva característica a la estructura. Para hacerlo, la posición de inserción, /, en L se calcula de la siguiente manera:

entonces, los valores internos se actualizan de la siguiente manera:

V,- = j , I j = P, m . = 1

y

Aúltimo— Alímo^~ 1 SÍ H 0

H = H - { h 0\ si H * 0

De esta manera, mientras el conjunto de huecos contiene elementos, la agregación de características llena los huecos en la representación densa. Cuando no quedan huecos, las características se agregan al final de la lista. En caso de que se deba eliminar una característica del mapa, su celda de vóxeles correspondiente, vi, se calcula de la misma manera que antes. El valor almacenado en v¡ indica la posición de la característica en la lista densa, v< , y los valores se actualizan de la siguiente manera:

mr ¡ = 0 , H = H {vl ) , v, = - 1

De esta manera, eliminar características genera nuevos huecos en la lista densa, sin actualizar el valor de Lúltimo. Puesto que M y H se actualizan correctamente durante la operación, la representación de datos internos sigue siendo coherente, pero la presencia de demasiados huecos puede conducir a una disminución del rendimiento.

Para hacer frente a este problema, los inventores proponen, en una realización particularmente preferida, introducir una operación compacta que rellene los huecos con los últimos elementos de las listas realizando un intercambio en ambos vectores L y M. Los valores afectados en V son actualizados a continuación de acuerdo con las nuevas posiciones y Ultimo se mueve al nuevo último elemento de la lista compactada. El coste de esta operación es lineal con respecto al número de huecos por lo que, en el caso d e f t= 0 , no hace nada.

Finalmente, con el fin de proporcionar un mecanismo rápido para las búsquedas de vecinos más cercanos, dada una zona de interés expresada por una posición central y un radio, las características internas pueden ser seleccionadas haciendo un bucle sobre los elementos almacenados en L (coste lineal con el número de muestras en el mapa) y el árbol kd K se reconstruye. Los elementos en K solo almacenan una referencia a las características asociadas en L, por lo que el espacio de memoria de K sigue siendo pequeño (lineal con el número de características presentes en la zona de interés) y constante en promedio. La misma operación se puede realizar sin iterar sobre toda la lista visitando la estructura de vóxeles. Los inventores investigan en la sección experimental las diferencias entre estos dos mecanismos.

Una vez que se ha creado el árbol, seguirá siendo válido incluso si se agregan nuevas características (los elementos ya existentes en L no se modifican) o se eliminan las características existentes (los elementos en L se marcan como huecos, pero su valor no se reemplaza), pero no si se realizan ambas operaciones (los elementos eliminados en L pueden ser sobreescritos).

Para realizar las operaciones propuestas de manera eficiente, las adiciones de nubes se posponen preferiblemente hasta que se requiera un nuevo árbol kd. Cuando esto sucede, las entidades ya existentes en el mapa fuera de la zona de interés son eliminadas, creando nuevos huecos. A continuación, se agregan nubes pospuestas, agregando solo las características que están dentro de la zona de interés. De esta manera, los huecos creados previamente son rellenados con las nuevas muestras en un tiempo constante. Si después de todas las adiciones aún quedan huecos (se eliminaron más características de las que se agregaron), se puede realizar una operación compacta, con un coste lineal con respecto al número de huecos restantes. Finalmente, K se reconstruye utilizando los elementos de L y se puede utilizar hasta que se requiera uno nuevo.

A.3. Marco SLAM

Un marco de optimización preferido de la invención está compuesto por dos módulos consecutivos: un odómetro, que estima la posición de cada nube dado un mapa, y un optimizador de trayectoria local, que refina la trayectoria de un conjunto de nubes. Ambos módulos emplean la estructura de datos del mapa, tal como se describe en este documento, para manejar el mapa en crecimiento.

Cada característica almacenada en el mapa M está compuesta por una posición mundial de punto pw, su vector unitario normal nw e información adicional (por ejemplo, reflectancia). Estos últimos no se utilizan en las etapas de registro. Este marco también se puede ampliar para realizar una optimización de la trayectoria global que permita reconstruir un mapa completo del entorno aprovechando los cierres de bucles.

La entrada de dicho marco es un conjunto de nubes de puntos 3D producidas con los datos transmitidos por el sensor (en el caso de un escáner Velodyne, la nube de puntos se genera después de una revolución completa del sensor). Cada nube de puntos C¡ está compuesta por un conjunto de puntos P ~ ÍP

un conjunto de marcas de tiempo relativas T ~ 'M y un conjunto de vectores unitarios normales N ~ Las marcas de tiempo relativas se asignan de modo que el primer punto producido tenga la marca de tiempo 0 y el último tenga 1. Los vectores unitarios normales se pueden estimar con la formulación de mínimos cuadrados sin restricciones propuesta en el documento de H. Badino, D. Huber, Y. Park y T. Kanade, “Fast and accurate computation of surface normals from range images”, en ICRA, 2011, aprovechando el filtrado de cajas en la estructura de la cuadrícula de nubes de puntos.

Odómetro

Inicialmente, es necesario producir una primera estimación de la trayectoria del sensor, recuperando la pose de cada nube de puntos. Puesto que el sensor se está moviendo, se considera como pose representativa de la nube la pose del sensor cuando se recibe el último punto.

Se realiza un ICP de punto a plano entre un subconjunto de puntos de la última nube recibida y el mapa. Como en el documento de F. Moosmann y C. Stiller, “Velodyne SLAM”, en IVS, 2011, los puntos seleccionados de la nube no son deformados considerando el último movimiento estimado antes de realizar el registro.

Dada la nube que se va a registrar C¡, se considera el último movimiento relativo estimado utilizando la pose de las

_{dos nubes registradas anteriormente K} r _{/ - i ’ M} r _/-2 f Sifs y = iog(r _# . _{-2 i - \} ,) _j <= SE3

donde y se expresa en álgebra se3 con la función de mapeo inverso log (•) (H. Strasdat, “Local accuracy and global consistency for efficient slam”. Tesis doctoral, Imperial College London, 2012).

^ = Í P J J = 1:ATS

Por lo tanto, se considera el subconjunto de puntos seleccionados con normales asociadas ^N, ' V y marcas de tiempo relativas T. = Us El deshacer la deformación se realiza en los puntos seleccionados calculando:

r, ⁼ r,^{, -exp(y)}

Pv = r ,"1- r M-cxp(y*^)® pv

nV; = r (t , 1 •r,_,-cxp(/*/jj)®p.

donde es la pose predicha de la nube C\ y

y ^ s; son los puntos seleccionados en el marco de coordenadas locales de la pose de la nube predicha ^ V cxp0 grupo de mapas SE3 al álgebra se3.

Dados estos elementos, se realiza el registro estimando la pose

con un ICP de punto a plano entre los puntos f

no deformados y normales Ps¡ y y el mapa M, proporcionando como suposición inicial.

Cada nube registrada C¡ con su pose asociada

es agregada a una lista de nubes registradas RC^odo :

Optimizador de trayectoria local

Este módulo toma como entrada la lista de nubes con sus poses asociadas RC^odo y realiza un refinado de trayectoria empleando un enfoque de ventana local. Cuando la distancia entre la primera y la última pose de la lista es mayor que

un umbral, se optimizan las posiciones de las nubes y se produce una nueva lista RCgEF l c>, r * s í ] i í 1 • con |a pose refinada y las nubes de entrada. Cabe señalar que esta etapa integra correctamente el deshacer la deformación en la optimización.

La función objetivo e(0 minimizada en esta etapa es la suma de los errores de alineación individuales de cada nube ©(•):

^{que, a su vez, depende de la pose asociada al primer y último punto de la nube. La pose inicial de la primera nube de} la secuencia, r UDOo , se supone que es la pose final de la última nube del conjunto optimizado anterior. e/(*) se calcula como el error total de un ICP generalizado de punto a plano en una trayectoria definida por la interpolación lineal en SE3 entre dos poses:

r i2« = r,-exp(/,. íogíf-' - r , ) ) (4)

donde ^ l2> representa la pose mundial interpolada en el tiempo t¡ asociado con el punto Psr seleccionado para el registro. Dado p‘‘>, las coordenadas mundiales estimadas del punto actual, pnn y nNN son respectivamente su punto más cercano recuperado del mapa y su normal asociado.

La función objetivo completa se minimiza alternando una etapa de Gauss-Newton y la búsqueda de nuevas correspondencias en el mapa, hasta que se cumple un criterio de convergencia o se alcanza un número máximo de iteraciones.

Los inventores sugieren utilizar la formulación múltiple propuesta en el documento de R. Kuemmerle, G. Grisetti, H. Strasdat, K. Konolige y W. Burgard, “g2o: A general framework for graph optimization”, en ICRA, 2011: la optimización es realizada sobre un vector de perturbación AF compuesto por un elemento del álgebra se3 sobre una pose ^ F en SE3. La operación de composición se define como ^ exp(Ar)<g>r |_0s j acobianos de los términos en la función objetivo se evalúan aplicando la regla de composición como

y, de manera similar, para

. Cada término e/(*) en la Ecuación 1 implica un par de poses consecutivas, por lo que el Hessiano aproximado da como resultado una matriz tridiagonal de bloques fácilmente manejable mediante algoritmos estándar para la factorización Cholesky en matrices dispersas.

Una vez finalizada la optimización, la lista RC^ref se puede actualizar con las poses optimizadas. A continuación, todo el conjunto de puntos y normales de las nubes son convertidos en coordenadas mundiales de acuerdo con la Ecuación 3 y, a continuación, son agregados al mapa M. En esta etapa, se aprovecha la estrategia eficiente para actualizar el mapa local descrito en la sección A.2.: antes de agregar puntos, primero se borran del mapa todos los puntos que están más allá de un radio determinado desde la última posición de trayectoria y a continuación, se agregan las nubes transformadas de RC^ref. Una vez que se actualiza el mapa, se crea un nuevo árbol kd en los puntos resultantes para permitir búsquedas posteriores de los vecinos más cercanos. La lista RC^odo es borrada y la estimación del odómetro para el próximo registro en la nube se actualiza de acuerdo con las dos últimas poses de RC^ref. La formulación propuesta representa una descripción adherente del modelo de sensor real, que obtiene puntos mientras se está moviendo: las transformaciones de puntos en las coordenadas mundiales involucran poses iniciales y finales de cada nube. Además, la estimación de cada pose (aparte de la primera y la última) está directamente influenciada por dos nubes.

Optimizador de trayectoria global

El marco propuesto se puede ampliar para realizar una optimización global de la trayectoria fuera de línea. De hecho, un límite del optimizador de trayectoria local propuesto consiste en la incapacidad de refinar puntos (y, consecuentemente, poses) que ya han sido agregadas al mapa. Esta limitación es aceptable, en general, cuando se exploran entornos a escala local, pero, al moverse en entornos muy grandes, se puede acumular deriva. Para estos casos, se deben tener en cuenta las técnicas de optimización global que aprovechan los cierres de bucle o las mediciones absolutas externas.

Los inventores proponen una optimización de la trayectoria global que hace uso de una descripción de mapa enriquecida: para cada característica en el mapa se agrega a su posición y normal en coordenadas mundiales (pW y nW), las coordenadas originales del punto pL y el vector unidad normal nL en el marco de referencia del sensor local, la marca de tiempo t relativa y el ID de índice de la nube que lo origina. Se puede observar que, dado el índice de la nube y una trayectoria, las coordenadas locales de los puntos y la normal son información redundante, pero los inventores prefieren almacenarlas para evitar nuevos cálculos.

Los inventores también proponen emplear dos mapas Mi y Mg, respectivamente, un mapa local y uno global. Mi es utilizado por el odómetro y los módulos optimizadores de trayectoria local. Cuando se necesita eliminar puntos de Mi, por el contrario, los mueve al mapa global. Además, en cada etapa del optimizador local, las correspondencias seleccionadas utilizadas en el ICP generalizado se agregan a una lista

donde para cada punto de consulta p?í, tomado de la nube IDh con su "*i normal asociado y la marca de tiempo se recupera de Míos datos asociados al vecino más cercano utilizado en la última etapa de la optimización: su posición PvA’< , vector normal ’V IDvy' del índice de la nube y la marca de tiempo tyN¡ . Cabe señalar que toda la información está en coordenadas locales del sensor.

Tener información local en el mapa es fundamental en esta etapa, y las necesidades de memoria siguen siendo bajas, puesto que no es necesario almacenar nubes completas, sino solo los puntos que se agregan al mapa en cada etapa. Cabe señalar que la lista Lc debe ser completada después de cada etapa del optimizador local, ya que la agregación de nuevas nubes puede sobrescribir puntos antiguos en el mapa.

_{De manera similar, se crea una lista de todas las poses} i — I _t1 r _{/ ,} ^ _{( asociadas a las nubes apilando las poses refinadas} mediante la etapa de optimización local. Cabe señalar que, dadas Nc, nubes, la lista de poses contiene Nc + 1 elementos. La optimización de la trayectoria global se realiza minimizando

donde

La función objetivo en la Ecuación 5 todavía representa un ICP generalizado de punto a plano, donde tanto la consulta como el punto del modelo se expresan en coordenadas locales y se transforman en coordenadas mundiales con las poses asociadas a sus nubes y las marcas de tiempo de interpolación.

La optimización de la ecuación 5 con Gauss-Newton todavía da como resultado una matriz Hessiana aproximada dispersa, ya que cada término de la suma implica solo tres (cuando ,D ™¡ ~ 1) o cuatro poses de la trayectoria completa, pero la matriz no es un bloque tridiagonal, ya que dos puntos de la misma nube pueden estar asociados a puntos de nubes diferentes. Por esta razón, los inventores emplean un enfoque de optimización de gráficos, tal como se propone en el documento de R. Kuemmerle, G. Grisetti, H. Strasdat, K. Konolige y W. Burgard, “g2o: A general framework for graph optimization”, en ICRA, 2011.

Para reducir el tiempo de cálculo, se propone no volver nunca a calcular las asociaciones de características, suponiendo que las características se corresponden correctamente con el optimizador de trayectoria local. Una vez finalizada la optimización, tanto el mapa global como el local son actualizados calculando las coordenadas mundiales de todas las características.

Esta optimización se puede aplicar a una secuencia completa de nubes para refinar una trayectoria completa. Además, en presencia de detecciones de bucle, las correspondencias que representan el bucle permiten estimar una trayectoria que afina todas las poses, limitando el bucle a cerrarse correctamente.

No obstante, cabe señalar que dicha optimización global no es adecuada para el cálculo en tiempo real, ya que involucra todas las poses y todas las asociaciones realizadas a lo largo de toda la trayectoria.

No obstante, muestra que, reteniendo la información adecuada, la estructura de datos actual se puede emplear para la optimización global y los cierres de bucles. El refinado de la trayectoria global podría ser realizado de manera más eficiente con soluciones de optimización de gráficos de poses, tal como la presentada en el documento de M. NieRner, M. Zollhofer, S. Izadi y M. Stamminger, “Real time 3D reconstruction at scale using voxel hashing”, ACM Transactions on Graphics, 2013, pero la capacidad de mantener mapas grandes en la memoria es un factor clave para recrear los mapas después de que se cierran los bucles.

A.4. Resultados experimentales

Los inventores probaron el sistema en conjuntos de datos reales obtenidos utilizando un Velodyne HDL-32E. Un primer conjunto de datos fue obtenido por un operador que transportaba el sensor mientras exploraba un entorno de interior de aproximadamente 10 x 35 x 3 metros. De manera similar, se obtuvo un segundo conjunto de datos en un edificio industrial interior de aproximadamente 16 x 65 x 10 metros. Se obtuvo un tercer conjunto de datos con el sensor montado en el techo de un automóvil mientras se conducía en condiciones normales de tráfico realizando cuatro bucles en un distrito de la ciudad, cada uno de unos 500 metros de longitud. Además, los inventores evaluaron su marco frente a los conjuntos de datos de KITTI disponibles públicamente (H. Strasdat, “Local accuracy and global consistency for efficient slam”. Tesis doctoral, Imperial College London, 2012), que proporcionan obtenciones de Velodyne HDL-64E montadas en automóviles tomadas en varios entornos urbanos y a distintas velocidades. Los conjuntos de datos de aprendizaje de KITTI también ponen a disposición una referencia real medida por GPS de cada pista. No obstante, las nubes de puntos 3D proporcionadas ya se han deformado utilizando el movimiento estimado del sistema de odometría a bordo. Por esta razón, los inventores utilizaron únicamente aquellas pistas de aprendizaje para las que estaban disponibles los datos nativos sin procesar.

La optimización de la trayectoria local se puede emplear para generar modelos 3D locales de alta definición de los entornos obtenidos. Para verificar la calidad de los modelos generados, los inventores han procesado los dos conjuntos de datos de interior utilizando una resolución de vóxel de 1 cm con un umbral para activar la optimización local de 2 m. Esto da como resultado aproximadamente 8 millones de puntos para el primer conjunto de datos y aproximadamente 24 millones para el segundo. A continuación, se creó un modelo de referencia registrando por pares escaneos del entorno tomados con el escáner ZF 5010C de alta resolución utilizando el método del documento de J. Yao, MR Ruggeri, P. Taddei y V. Sequeira, “Automatic scan registration using 3D linear and planar features.”, 3D Research, vol. 1, no. 3, págs. 1 a 18, 2010. Los inventores han registrado con precisión los dos modelos y han calculado las distancias de punto a punto entre ellos. No hay distorsiones visibles en los modelos y los histogramas de las distancias entre las dos nubes tienen picos inferiores a 0,02 m, lo que está dentro de la precisión nominal del sensor Velodyne HDL-32E utilizado.

Para estimar la calidad del seguimiento y la deriva acumulada, los inventores han ejecutado el marco actual en todos los conjuntos de datos de aprendizaje de KITTI utilizando como datos de entrada las lecturas sin procesar del sensor (10 pistas en total). Además, para demostrar el beneficio de incorporar el movimiento del sensor en el marco de optimización, también han ejecutado el sistema actual en las mismas pistas, pero empleando las nubes oficiales preprocesadas de los conjuntos de datos (eliminando la deformación utilizando el movimiento estimado del sistema de odometría a bordo). En este caso, los inventores no realizaron ninguna deformación durante la optimización (es decir, utilizaron solo el módulo de odometría). Para estos experimentos utilizaron un tamaño de vóxel de 15 cm en los mapas y no realizaron cierres de bucle. La figura 2 muestra los resultados de ambos experimentos en términos de traslación relativa promedio y error de rotación generado utilizando segmentos de trayectoria de 100 m, 200 m, ..., 800 m de longitud (véase el documento de H. Strasdat, “Local accuracy and global consistency for efficient slam.”. Tesis doctoral, Imperial College London, 2012 para una descripción de la métrica de error adoptada). Está claro que al incorporar la eliminación de la deformación de la nube en el marco de la optimización se obtienen mejores resultados y se reduce la deriva tanto de traslación como de rotación (en particular, el error de traducción mejoró en un porcentaje de 0,3 puntos en promedio). Cabe señalar que el algoritmo de vanguardia actual para el banco de pruebas de KITTI que solo emplea datos LIDAR (LOAM) funciona mejor. Cabe señalar, no obstante, que se ha validado directamente en las nubes de puntos no deformados originales y que procesa las nubes solo a 1 Hz.

Para evaluar las mejoras introducidas por la estrategia de optimización global propuesta después de integrar cierres de bucle, los inventores habilitaron esta función en una pista de muestra del conjunto de datos de KITTI que contiene un solo bucle. Su mecanismo de detección de bucle es muy simple y no es adecuado para una aplicación real: se detecta un bucle cuando la distancia entre la pose actual y una pose anterior en el tiempo es menor que un umbral. A continuación, se registra la última nube en el mapa global y, si esto tiene éxito, se agregan las correspondencias encontradas a la optimización global. La figura 3 muestra el efecto del cierre del bucle en la pista considerada. Los resultados experimentales también mostraron la mejora de la optimización global tanto en el odómetro como en la trayectoria local optimizada.

Los inventores compararon su sistema con el Velodyne SLAM [documento de F. Moosmann y C. Stiller, “Velodyne SLAM”, en IVS, 2011] que también realiza una compensación del movimiento en las nubes de puntos obtenidas. Para comparar los dos sistemas, los inventores midieron la deriva acumulada utilizando el conjunto de datos de automóviles al aire libre. Puesto que la misma ubicación se vuelve a visitar varias veces, estimaron la deriva registrando el mapa local inicial generado con el generado en cada etapa posterior. Los componentes de traslación y orientación de la transformación de registro que alinean el mapa local actual con el inicial indican cuánta deriva se ha acumulado. Una de las características más destacadas del [documento de F. Moosmann y C. Stiller, “Velodyne SLAM”, en IVS, 2011] es la presencia de una estrategia de refinado del mapa (llamada adaptación) basada en un conjunto de pruebas heurísticas que influyen positivamente en la estimación de la trayectoria. Puesto que el presente sistema se centra en la estrategia de optimización modelizando adecuadamente el problema, los inventores desactivaron esta característica en el trabajo original para enfocar el análisis en la estimación de la trayectoria. Los resultados después de cada ciclo se muestran en la Tabla I. Cabe señalar que se acumula menos deriva que en el trabajo original. Además, el presente sistema es una formulación natural del problema que requiere menos parámetros de configuración que las estrategias heurísticas del Velodyne SLAM. El rendimiento del sistema actual es superior al sistema Velodyne SLAM tanto en términos de tiempo de ejecución como en la capacidad de mantener un mapa global del entorno, mientras que en el trabajo original solo se mantiene un mapa local. Se ha confirmado la capacidad de utilizar el mapa global, en caso de la utilización de cierre de bucle y la técnica de optimización global para corregir la deriva acumulada en el primer bucle y la utilización del mapa global para los siguientes bucles.

Con el fin de evaluar el rendimiento de la representación cartográfica propuesta, los inventores han medido el tiempo de ejecución de cada operación mientras se ejecuta el conjunto de datos del automóvil en exterior en un PC equipado con una CPU Intel Xeon E5-2650.

Tal como era de esperar, las operaciones de agregación se realizan en un tiempo lineal con respecto al número de entidades agregadas al mapa, siendo el tiempo medio de 36,4 ns por cada entidad, lo que da un tiempo medio de inserción de nubes de 1,81 ms para el sensor HDL-32E.

Las operaciones de borrado en el marco SLAM actual solo se realizan sobre el mapa local, justo antes de actualizar el árbol kd.

TABLA I: Informes de error de deriva,

Bucle Guiñada Cabeceo Inclinación dx dy dz

Optimizador de trayectoria local

1° -1,4° -0,7°

-0,62 m -0,26 m 0,39 m

2° -2,7°

-0,1° -1,16 m -0,86 m 0,89 m

3°

-0,7° -1,17 m -1,16 m 1,80 m

4° -5,5°

-1,0° -2,37 m -1,45 m 2,33 m

Velodyne SLAM

1° 3,33° 0,05° -0,9° 1,53 m 0,80 m 3,60 m

2° 6,54° 0,3° -1,7° 2,97 m 1,82 m 7,29 m

3° 9,96°

-2,5° 4,54 m 2,87 m 11,04 m

4° 13,2° 0,9°

5,93 m 4,16 m 14,54 m

Las características que se eliminarán se seleccionan realizando una búsqueda de radio alrededor el punto de interés (por ejemplo, la última pose estimada del sensor) y son agregadas al mapa global. Los resultados muestran un tiempo de eliminación constante por cada característica, que es de una media de 30,84 ns.

La selección de características que se eliminarán del mapa local se puede realizar de dos maneras: utilizando la estructura de vóxeles o iterando sobre la lista densa. La figura 4(a) muestra los tiempos de búsqueda promedio basados en el número de características almacenadas en el mapa y en el radio de búsqueda. Tal como se puede observar, la utilización de la lista densa siempre proporciona el mismo rendimiento (lineal con el número de entidades almacenadas, independientemente del radio de búsqueda). Por otro lado, los tiempos de búsqueda de vóxeles aumentan a medida que lo hace el radio y, en todos los casos, presentan peores resultados.

Puesto que no se borran puntos del mapa global, las operaciones compactas solo ocurren en el local. Gracias a la estrategia propuesta de posponer la incorporación de nuevas nubes hasta que se solicite un nuevo árbol kd, solo el 7,79 % de las veces el número de huecos creados es mayor que el número de características añadidas, siendo necesario realizar una operación compacta. En estos casos, los tiempos de ejecución muestran un comportamiento lineal con respecto al número de huecos restantes, siendo el tiempo medio de cada operación de 1,81 ms.

Finalmente, para las operaciones de cierre de bucle, el mapa global debe ser consultado alrededor de una zona de interés. Tal como sucedió con el mapa local, esta selección se puede realizar de dos maneras. La figura 4(b) muestra los resultados de utilizar la estructura de vóxeles y la lista densa. Tal como se puede observar, para un radio de búsqueda inferior a 90 metros, el vóxel supera la lista densa. No obstante, a medida que aumenta el radio, el almacenamiento en la memoria oculta falla en la estructura interna de vóxeles dispersa y conduce a una gran pérdida de rendimiento.

El sistema es capaz de procesar nubes a 12,93 Hz (es decir, en tiempo real, con respecto a la tasa de obtención de Velodyne) cuando la optimización de la trayectoria local no está activa, mientras que la frecuencia disminuye a 7,5 Hz utilizando la optimización de la trayectoria local, que es cercana al tiempo real. Cabe señalar que el registro y la optimización local no están codificados para ser ejecutados en multihilo, por lo que los inventores esperan que el rendimiento se pueda incrementar tanto en el odómetro como en la optimización local.

En el modo de odómetro el tiempo empleado en registrar las nubes es el 54 % del total, mientras que en el modo de optimización local el 30 % del tiempo se dedica al registro del odómetro y el 35 % a la optimización de la trayectoria local. El registro incluye el tiempo de búsqueda del vecino más cercano, mientras que el impacto de cada operación realizada sobre los mapas local y global se resume en la Tabla II, cuando se trabaja en modo odómetro (primera fila) y cuando se realiza la optimización de trayectoria local (segunda fila). Las operaciones de suma, eliminación y compactación en el mapa local se muestran en las columnas agregar, eliminar y compactar, respectivamente, donde los tiempos de eliminación también incluyen la selección de puntos sobre el mapa local y la agregación al mapa global. El impacto de construir el árbol kd sobre todo el mapa local se muestra en la columna árbol kd y, finalmente, el impacto de agregar los puntos eliminados del mapa local en el mapa global se muestra en la columna agregar g.

TABLA II: Rendimiento del sistema

Frecuencia Agregar Borrar Compacto Árbol kd Agregar g.

Odómetro 12,93 Hz 2,1 % 0,9 % 0,2 % 25,0 % 0,3 %

Local 7,54 Hz 1,2 % 0,5 % 0,1 % 13,7 % 0,2 %

A. 5. Conclusión

El presente documento presenta un marco para la optimización local de nubes de puntos obtenidas utilizando láseres en movimiento. En particular, los inventores incorporaron el movimiento de obtención en la optimización interpolando cada nube de puntos obtenida entre su posición inicial y final. Los inventores demostraron experimentalmente, utilizando conjuntos de datos disponibles públicamente que, modelizando correctamente el movimiento del sensor, es posible reducir los errores de estimación de la odometría.

Además, presentan una estructura de datos eficiente para gestionar grandes mapas 3D voxelizados constituidos por características dispersas. La estructura de datos del mapa es adecuada tanto para la optimización del mapa local como para la optimización global fuera de línea. Sus experimentos muestran que, para el primer problema, dicha estructura proporciona odometría en tiempo real y refinado local casi en tiempo real. Estos rendimientos pueden incluso ser mejorados aprovechando las operaciones de múltiples subprocesos cuando se realiza la optimización de la trayectoria local (por ejemplo, búsqueda del vecino más cercano, deformación de nubes).

B. Análisis de la localización, el seguimiento y el cambio en base a un modelo de referencia 3D

Los enfoques basados en árboles octales o árboles kd proporcionan tiempos de búsqueda razonables para los vecinos más cercanos (típicamente logarítmicos con respecto al tamaño del mapa) y buena escalabilidad. En su enfoque, los inventores introducen una representación de vóxeles alternativa que combina los accesos aleatorios rápidos proporcionados por representaciones de vóxeles densas y la escalabilidad proporcionada por estructuras de datos dispersas.

Con el fin de garantizar un seguimiento de poses correcto, un sistema preferido realiza una selección eficiente de puntos que se utilizarán en el proceso de registro que garantiza una buena estabilidad geométrica para el algoritmo ICP. Por lo tanto, una estrategia para descartar de manera eficiente los valores atípicos garantiza que el registro se realice solo utilizando correspondencias que sean globalmente coherentes (típicas).

El presente marco preferido fusiona en el proceso de registro con respecto al modelo de referencia real un odómetro robusto que es capaz de realizar un seguimiento en tiempo real incluso cuando el usuario abandona el mapa, o si el entorno observado difiere demasiado del modelo obtenido inicialmente (por ejemplo, se cambiaron los muebles). Volviendo a entrar en el mapa conocido, el sistema recupera automáticamente la posición correcta y, por lo tanto, evita la acumulación de deriva.

B.1. Los principales beneficios de las realizaciones preferidas se describen a continuación:

1) Una estrategia de reconocimiento de lugar escalable para localizar el sensor en entornos muy grandes utilizando un conjunto de descriptores calculados previamente y evitando accesos al mapa de referencia real.

2) Una estructura de datos eficiente para representar el mapa que proporciona búsquedas de los vecinos más cercanos en tiempo constante y un tamaño de memoria bajo.

3) Una estrategia de selección de puntos rápida que garantiza resultados geométricamente estables.

4) Una técnica de selección de valores atípicos que elimina de manera eficiente la interferencia de valores atípicos durante el proceso de registro.

5) Una fusión entre un odómetro local y el registro contra el mapa de referencia real que aprovecha valores atípicos estáticos y permite al usuario navegar a través de zonas no mapeadas.

6) Un sistema completo que proporciona resultados en tiempo real con alta precisión.

La descripción que se muestra a continuación está estructurada de la siguiente manera: la Sección B.2. presenta una estrategia preferida de una nueva localización y reconocimiento de lugares en línea, la Sección B.3. muestra cómo realizar un seguimiento en línea una vez que se ha identificado la pose del usuario en un entorno conocido. A continuación, la Sección B.4. presenta resultados experimentales y, finalmente, la Sección B.5. presenta las conclusiones.

B.2. Reconocimiento de lugar

El componente de reconocimiento de lugar se ocupa de recuperar una estimación inicial de la localización y la orientación del usuario sin información a priori. Puede ser ejecutado en línea a una cierta velocidad de tramas, para proporcionar ubicaciones candidatas dada la observación actual del sensor. Además, por motivos de escalabilidad, no debería hacer uso del modelo de mapa durante la ejecución, ya que podría proporcionar poses candidatas relacionadas con ubicaciones distantes (y, por lo tanto, no cargadas en la memoria), o incluso mapas diferentes. Con el fin de cumplir estos dos requisitos, preferiblemente se introduce una etapa de preprocesamiento para (1) reducir el espacio de búsqueda de poses disponibles y (2) entrenar un clasificador robusto y compacto que, dada una observación, estime eficientemente la posibilidad de estar en una ubicación específica.

Reducción del espacio de búsqueda

Inicialmente, de manera preferible, se detectan zonas navegables entre todo el mapa. Estas zonas se definen como el volumen en el que se puede colocar el sensor durante la exploración del entorno. Además, en general, se puede suponer sin pérdida de generalidad que el eje Z del modelo de mapa está aproximadamente alineado con el vector de gravedad.

Puesto que los inventores se han centrado en el movimiento sobre el suelo (sensor montado en una mochila o en un vehículo), se espera que las zonas navegables sean un espacio relativamente estrecho sobre el piso navegable. Por esta razón, en primer lugar, se identifican los alcances del piso. La extracción de piso se realiza sobre una

representación de vóxeles dispersa del entorno, V, donde cada celda completa, V , contiene un vector normal a la —(i)

superficie definida localmente mediante los puntos alrededor de su centroide, n . Se extrae un subconjunto de vóxeles que representan celdas de piso candidatas, ^ — * , comprobando que la componente vertical en sus normales —(/) T

asociadas sea dominante, es decir, n • (0 ,0 ,1 ) > £ donde e es habitualmente un valor comprendido entre 0,5 y 1. No obstante, esta restricción, por sí sola, puede conducir a clasificar demasiadas celdas como piso (por ejemplo, mesas o estantes vacíos).

Para resolver este problema, los inventores proponen introducir el concepto de capacidad de ser alcanzado. Dada una celda alcanzable ^ c ^ , todas las celdas circundantes ,g m )<=F son consideradas como alcanzables si se cumplen las siguientes condiciones:

C w n V = 0

8 (8 )

donde - Vtamanodeceidaen (6) representa la distancia máxima de la etapa (por ejemplo, 0,5 metros para un movimiento de caminar, o Vtamano de celda para un movimiento de automóvil), &\ en (7) representa el tamaño máximo de la etapa C -vertical y en (8) representa el volumen simplificado del observador, centrado en la celda del piso g¡ (un cilindro circundante en la presente implementación).

Las celdas alcanzables iniciales se pueden proporcionar manualmente, pero, puesto que la generación del mapa se realiza preferiblemente colocando el escáner sobre celdas alcanzables, esta inicialización se puede realizar automáticamente suponiendo que las celdas del piso debajo de las posiciones de obtención son alcanzables.

De acuerdo con estas condiciones, la detección de todas las celdas del piso ^ £ F se realiza en un estilo de algoritmo de inundación, tal como se ilustra en la Tabla III, que muestra el algoritmo donde, inicialmente, A almacena el primer conjunto de celdas alcanzables.

Tabla III: Extracción del piso de inundación

Require: A * 0 , F * 0 , F n A = 0

F' < - 0

While A * 0 do

£<-0

While A * 0 do

a < -A . pop()

F .rem ove(/)

B ,p u s h (/)

End if

End for

End while

A<— B

End while

Return F*

Una vez que se ha identificado el piso, el espacio navegable, N, se define como el conjunto de celdas, ,7< * G A , p0r encima de las celdas del piso, donde nV) <~^ V = 0 .

Con el fin de reducir aún más el espacio navegable sin pérdida de precisión, los inventores también proponen introducir restricciones físicas relacionadas con la operatividad particular del sistema (por ejemplo, límites verticales y angulares en la posible pose del sensor para un montaje de sensor específico) que proporciona un espacio navegable efectivo ^ ^ . Dichas restricciones se seleccionan empíricamente mediante la ejecución de un conjunto de experimentos en conjuntos de datos de muestra (véase la Sección B.4.).

Clasificador de poses

Con el fin de construir un clasificador de poses, inicialmente es necesario definir una representación compacta de cada observación individual. En particular, los inventores adoptan el descriptor compacto simple y rápido de calcular definido por el documento de P. Taddei, C. Sánchez, A. L. Rodríguez, S. Ceriani, V. Sequeira, de 2014, “Detecting ambiguity in localization problems using depth sensors.” En: 3DV: se divide la imagen de rango en Wb x Hb bins regulares y, para cada uno, se estima un valor de rango medio. Todos estos valores se apilan en un descriptor del marco d observado.

^t A continuación, se genera aleatoriamente un conjunto de poses de aprendizaje i_'r — ^t rT o ,...,r uT ....r v nr en el espacio navegable efectivo conocido ^ ^ . Para cada pose r , í-, se sintetiza una imagen de profundidad proyectando con rayos el mapa 3D a un plano de imagen del sensor alineado con la pose proporcionada y los

inventores extraen su descriptor ^ de la imagen de profundidad generada. Se construye un árbol kd

el r

que mapea todos los descriptores T¡ generados a su pose T¡ correspondiente. Dado un descriptor q, el conjunto de pares de ubicación / descriptor que están cerca en el espacio del descriptor se puede recuperar realizando búsquedas eficientes en 7, con complejidad logarítmica en el espacio del descriptor. Cabe señalar que, dado el conjunto de

muestras de aprendizaje ^ ^ 1, también es posible construir clasificadores más compactos, por ejemplo, tal como se describe en el documento de B. Glocker, S. Izadi, J. Shotton, A. Criminisi, 2013. “Real-time rgb-d camera relocalization.” En: ISMAR. Sin embargo, los inventores observaron experimentalmente que N* era lo suficientemente pequeño como para retener el conjunto de aprendizaje completo en la memoria y realizar la clasificación mediante búsquedas radiales del vecino más cercano en el espacio de descriptores del árbol kd.

Durante la ejecución, el clasificador de poses se utiliza para recuperar la mayor cantidad de ubicaciones posibles dada la observación actual. En particular, los inventores dividen el proceso en dos etapas diferentes: la inicialización, que se ocupa de la estimación de posibles ubicaciones cuando no se dispone de información a priori, y la actualización, que se ocupa de la evaluación de las ubicaciones candidatas y el nuevo muestreo de las mismas.

En la etapa de inicialización, es necesario dibujar un conjunto de posibles ubicaciones del sensor dada una única observación del sensor. Los inventores proponen proceder como sigue:

1. Dada la última observación del sensor, se calcula su descriptor asociado q y recupera un conjunto de ubicaciones candidatas I realizando una búsqueda radial en T dado un umbral r en el espacio del descriptor. En el caso de sensores que proporcionen un campo de visión horizontal de 360 grados, se pueden incrementar las ubicaciones candidatas calculando descriptores de entrada adicionales desplazando horizontalmente los valores de rango. Cada descriptor corresponde a las lecturas que produciría el sensor si se girase sobre su eje local. Cada conjunto resultante de ubicaciones candidatas se rota a continuación de acuerdo con i.

W|. A r cT '

2. Se asocia un peso p a cada ubicación potencial p :

, Ir p recuperada de T. wr p es 1 para descriptores de coincidencia perfecta y 0 para descriptores en el límite de la esfera de búsqueda.

3. Finalmente, los pesos se recogen en w y se normalizan para tener max w = 1.

_{La etapa de actualización se ocupa de la actualización de las posibles ubicaciones} r = r _{0” " ’} r _{\ > mientras el sensor} se mueve dados sus pesos asociados w “ w o>-->w a' . Cabe señalar que esta etapa hace uso de un odómetro que registra una nube hasta su predecesor de acuerdo con la técnica explicada en el siguiente apartado. En particular, los inventores proceden de la siguiente manera:

1. Se utiliza el odómetro y la observación actual para actualizar todas las ubicaciones en I .

2. Cuando se recorre una distancia dada desde que se crearon las últimas ubicaciones potenciales, se calcula un nuevo descriptor q a partir de la última observación. Esto se utiliza para recuperar de T un conjunto de posibles ubicaciones ^ , de manera similar a la etapa 1 en la etapa de inicialización.

_{3. El peso asociado a cada posible ubicación} r,.er _{se calcula como:}

n(q |f )n (f )

^{W- = -------- ' v} rJ n(q)

y una vez todos los pesos han sido calculados, se normalizan para tener un valor máximo de 1.

4. Se actualiza * * y w = w y se repite la iteración de la etapa de actualización.

La ecuación (9) calcula el peso asociado a cada posible ubicación utilizando el teorema de Bayes expresado en teoría de posibilidades como en el documento de D. Dubois, 2006. “Possibility theory and statistical reasoning.” Computational statistics and Data Analysis 51 (1), 47 a 69, the Fuzzy Approach to Statistical Analysis. Los términos individuales de (9) son:

La ecuación (10) estima la posibilidad del descriptor q, dada la pose T de la misma manera que en la etapa 2 de la etapa de inicialización (en caso de múltiples descriptores de entrada, cada uno debe ser tenido en cuenta

individualmente). La ecuación (11) evalúa la probabilidad de estar en la posición i encontrando la ubicación más compatible en el conjunto de ubicaciones potenciales ^ . Esta compatibilidad se define como la distancia relativa

_{ponderada (Ecuación (12)) entre la pose potencial anterior} r _{* y la pose} r _{J . La ecuación (13) estima el carácter} distinto de la observación actual comparando el número de vecinos recuperados con respecto al tamaño del conjunto de aprendizaje, por ejemplo, poses extremadamente ambiguas, tal como en pasillos, producirán muchos resultados, lo que resulta en una alta ambigüedad.

La etapa de actualización es iterada hasta que las poses potenciales convergen en una única ubicación, es decir, cuando la covarianza del centroide de I calculado de acuerdo con los pesos w es pequeña. En este punto, se considera el problema resuelto y se inicia el componente de seguimiento de pose.

El sistema preferido descrito de manera general anteriormente se basa en una nueva ponderación iterativa de posibles ubicaciones con aplicación de bootstrap rápida que utiliza una observación de un solo sensor. Un factor clave para la escalabilidad a mapas grandes es el cálculo previo de descriptores ligeros de los mapas de referencia y su organización en una estructura de árbol kd con poses asociadas. De esta manera, las consultas en el espacio del descriptor se utilizan para poblar de manera eficiente el sistema con ubicaciones candidatas dada la primera observación. A continuación, en las etapas de actualización posteriores, el movimiento estimado y las consultas en el espacio del descriptor se utilizan para dibujar un nuevo conjunto de posibles ubicaciones y sus pesos asociados.

Este enfoque es comparable con las técnicas generales de localización de Monte Carlo presentadas en [el documento de S. Thrun, D. Fox, W. Burgard, F. Dellaert, 2001. “Robust Monte Carlo localization for mobile robots.” Artificial intelligence 128 (1), 99 a 141] y [Thrun et al. (2005) Thrun, Burgard y Fox] que utilizan filtros de partículas. No obstante, sus técnicas se dirigen a estimar con precisión la distribución de la probabilidad del sensor aproximándola con un conjunto de partículas ponderadas para resolver todas las etapas del problema de localización [S. Thrun, W. Burgard, D. Fox, 2005. “Probabilistic Robotics” (Intelligent Robotics and Autonomous Agents.) The MIT Press].

Por el contrario, el presente componente de reconocimiento de lugar solo necesita una estimación de pose rápida y aproximada, ya que el componente de seguimiento de pose posterior realiza un seguimiento preciso de la pose (Sección B.3.) una vez que se ha identificado una ubicación única. Además, el presente sistema solo tiene que garantizar que las posibles ubicaciones no se descarten y, por lo tanto, no requiere una estimación de densidad de probabilidad de pose de sensor precisa. Por esta razón, no se requiere un muestreo denso del espacio navegable, tal como muestra la Sección B.4. No obstante, una densidad de muestreo baja puede provocar una pérdida de seguimiento en ciertos casos debido a una inicialización incorrecta de las partículas. Este problema se supera dibujando un nuevo conjunto de partículas cada vez que se realiza la etapa de actualización.

B.3. Seguimiento de pose

El componente de seguimiento de pose se ocupa de calcular el movimiento local del sensor a medida que se mueve por el entorno. Conociendo la pose estimada anterior, cuando se recibe una nueva obtención, los inventores realizan un registro local entre el mapa y los puntos observados. A partir de la transformación resultante, se infiere el movimiento implícito y se aplica a la pose estimada previamente.

Para realizar con precisión un seguimiento de la pose del sensor en tiempo real, es importante (1) emplear una estructura de datos diseñada específicamente para búsquedas de vecinos más cercanos y (2) seleccionar correctamente un subconjunto estable y representativo de los puntos de entrada para realizar el registro. No obstante, para garantizar estimaciones correctas, (3) los valores atípicos deben ser detectados adecuadamente. Esto es particularmente importante en entornos degenerados que contienen pocas direcciones dominantes grandes, por ejemplo, pasillos largos, túneles o ambientes simétricos donde existen pocos puntos adecuados para dificultar los registros erróneos.

Representación de mapa

En la estructura de mapa actualmente propuesta, se almacenan y sincronizan dos listas diferentes de elementos: una lista compacta de planos, L, y una cuadrícula densa de vóxeles, V, construida con un tamaño de vóxel específico.

Cada plano h € ¿ almacena una posición en coordenadas mundiales, p¡, y una unidad normal, n i . Cada vóxel, vt e V almacena un estado actual que puede ser completo, vacío o cercano. Los vóxeles llenos almacenan un índice en el plano e ¿ , cuya posición asociada cae dentro. En particular, los puntos del mapa de referencia que pertenecen al vóxel se utilizan para estimar los parámetros del plano. Las celdas vacías almacenan una referencia nula y las celdas cercanas almacenan un índice en el plano cuya distancia de posición dh asociada al centro del vóxel es la más pequeña. Cabe señalar que los inventores consideran preferiblemente un vóxel cercano solo si la distancia di, está por debajo de un umbral dmax determinado; en caso contrario el vóxel se considera vacío. La figura 5 ilustra la representación propuesta.

Con esta representación de mapa, todas las búsquedas de vecinos más cercanos se calculan previamente fuera de línea y se almacenan dentro de la cuadrícula densa. En tiempo de ejecución, dado un punto de consulta en coordenadas mundiales, los inventores aproximan el cálculo de su vecino más cercano en el mapa calculando el vóxel que lo contiene. A continuación, si el estado de la celda es completo o cercano, se devuelve el plano asociado. De lo contrario, se notifica que no hay ningún vecino.

Cabe señalar que, para el enfoque propuesto (1) todas las operaciones realizadas durante una sola búsqueda presentan un tiempo de ejecución constante, independientemente del tamaño del mapa. En comparación, las estructuras de árbol kd proporcionan, en promedio, tiempos logarítmicos con respecto al tamaño del mapa. Además, estableciendo correctamente dmax, (2) se realiza implícitamente una proyección externa inicial de correspondencias que están demasiado separadas cuando se buscan vecinos más cercanos en ICP.

El principal inconveniente de utilizar estructuras densas de vóxeles para representar entornos grandes consiste en su tamaño de memoria. Los inventores resuelven este problema utilizando una estructura jerárquica de tres niveles, donde los nodos intermedios son bloques de 32 x 32 x 32 nodos. De esta manera, cuando un nodo está completamente vacío, no es necesario almacenarlo y, dado el tamaño de hoja propuesto, 25 x 25 x 25, se puede direccionar cada celda interna individual utilizando solo dos bytes, más un bit adicional para marcar las vacías (15 1 bits). Adicionalmente, las implementaciones actuales permiten la transmisión de modo que solo la parte dentro del rango del sensor tiene que estar en la memoria. Puesto que la velocidad de movimiento del sensor está varios órdenes de magnitud por debajo de las operaciones de carga asociadas, las necesidades de memoria de ejecución en línea siempre están limitadas y el mapa siempre se actualiza alrededor de la posición del sensor.

Estrategia de selección de puntos

Se debe garantizar que el subconjunto seleccionado de puntos de la obtención actual sea suficientemente representativo para bloquear correctamente los grados de libertad menos definidos durante el registro. De manera similar al trabajo descrito en el documento [de Gelfand et al. (2003) Gelfand, Ikemoto, Rusinkiewicz y Levoy], los

inventores consideran la contribución de mover un punto, p¡, y su normal asociada, n , mediante un vector de transformación [A r'A t7 J en |a distancia entre el punto y el plano. Esto se puede expresar como:

linealizando las rotaciones utilizando la aproximación de ángulos pequeños.

Considerando solo las rotaciones en la ecuación (14), el error introducido en la distancia de punto a plano es proporcional a la distancia de punto con respecto a sensor, y al ángulo entre su rayo normal y el de visión. Esto lleva a seleccionar puntos lejanos y puntos cuya normal sea lo más perpendicular posible con respecto al rayo de visión. Desgraciadamente, el láser en movimiento produce puntos distribuidos de forma no uniforme y, en particular, las zonas distantes son obtenidas con una densidad de puntos más baja y, por lo tanto, proporcionan valores normales mal estimados. Asimismo, para entornos circulares, cuando el sensor se acerca al eje de simetría, los ángulos entre los rayos de visión y las normales desaparecen.

Los inventores resuelven preferiblemente estos problemas distinguiendo explícitamente entre traslaciones y rotaciones. Para limitar adecuadamente las traslaciones, solo consideran normales de punto. Calculan la matriz de covarianza para las traslaciones Ct, como:

y extraen sus vectores propios asociados, X l> X2’ X3 ’ y sus valores propios asociados

. Los puntos de obtención se clasifican por lo tanto en tres bins, A A A J como sigue:

Cuando los tres bins están equilibrados, los grados de libertad de traslación están igualmente restringidos. Por otro lado, en casos degenerados, por ejemplo, pasillos largos, un bin estará considerablemente menos poblado que los demás, por ejemplo, el que contiene los puntos cuyas normales asociadas son paralelas al eje longitudinal.

Con respecto a las orientaciones, se calculan los ejes de rotación principales utilizando productos cruzados entre posiciones y normales. La matriz de covarianza resultante se define de la siguiente manera:

De manera similar a las traslaciones, se calculan los vectores propios asociados X l> X2< X3 y los valores propios

Á > A , > Á Entonces, puntos de la nube de entrada se clasifican en tres intervalos, A A A } de la siguiente manera:

p, e bj <-» n. • x/ 1 < n,- • x*|, V i * j

Para cada intervalo, se aproxima el centro de rotación como la media ponderada de las posiciones contenidas, de acuerdo con su distancia al sensor (Esta aproximación es válida para sensores. Para otros campos de visión puede ser necesaria una aproximación alternativa):

Z IIp í II'p .

c = — -----------

¿¡Pili

í=i

y, a continuación, para cada punto en el bin, se estima cuánto contribuye al bloqueo de las rotaciones sobre su vector propio correspondiente, x, como:

El primer término en la ecuación (15) pondera la influencia de la normal de un punto dado de acuerdo con su perpendicularidad al eje de rotación (cuanto más perpendicular, mayor es el peso). El numerador del segundo término estima la calidad de las rotaciones de bloqueo sobre x calculando el ángulo entre el vector que conecta el centro de rotación al punto, y el vector perpendicular al plano definido por la normal y el eje de rotación. Finalmente, el denominador normaliza el resultado en el rango [0 ...1], por lo que la selección del punto es independiente de la distancia al sensor.

Cuando los bins asociados con valores pequeños de d contienen demasiados puntos, las rotaciones alrededor del eje considerado están poco limitadas: es necesario seleccionar solo los puntos con los valores más altos. La figura 6 ilustra este concepto mostrando histogramas de muestras de valores de d recuperados de un entorno con alta simetría y de otro donde las rotaciones están debidamente definidas.

Registro y selección de típicos

Para fines de registro, los inventores consideran como correspondencias incorrectas aquellas entre puntos de IV

sensores (en coordenadas mundiales), , y puntos del mapa, q¡, que son inconsistentes con el resto de las correspondencias. Esto ocurre cuando: (a) el punto visto por el sensor corresponde a un objeto que no está presente w

en el mapa (es decir, algo que fue agregado o eliminado después de la obtención original) o (b) el " estimado está lejos de su punto correspondiente en el mapa. En ambos casos, el vecino más cercano no tiene sentido geométrico con respecto a las otras correspondencias. Los modos clásicos de identificar estos valores atípicos emplean heurísticas basadas en posiciones relativas y normales entre puntos correspondientes: los vecinos cuya distancia es mayor que un umbral determinado o con orientaciones normales muy diferentes se consideran valores atípicos. Se pueden encontrar ejemplos en [Rusinkiewicz y Levoy (2001)]. Estas aproximaciones son útiles cuando se utilizan valores de tolerancia altos (por ejemplo, los puntos correspondientes más allá de 5 pueden ser incorrectos en la mayoría de los casos) pero, en estos casos, su poder discriminativo es bajo.

Los inventores consideran inicialmente los bins relacionados con las traslaciones descritas anteriormente. A continuación, evalúan si las rotaciones están correctamente definidas en todos los ejes. Si este no es el caso para un eje determinado, agregan un bin que contiene los puntos que limitan mejor dicha rotación, es decir, puntos con los valores di más grandes.

A continuación, consideran el último movimiento estimado (utilizando las dos poses registradas previamente) para realizar una estimación inicial de la nueva pose del sensor:

A partir de esta estimación, cada iteración del algoritmo ICP crea n conjuntos aleatorios de puntos, S, donde cada ..(/> p o

conjunto contiene /(puntos seleccionados al azar de cada bin (habitualmente k= 1). Para cada uno de estos I ^{r p}

P' , utilizando 1 ' , y se busca su correspondiente »Q |1 ^. I ^p (/) ^-- (|X^ ^{p| p} . I ^{^} ^(/)

plano más cercano en el mapa, s , creando la correspondencia ^ ^ . Una vez resueltas

todas las correspondencias en cada conjunto, la rígida transformación 1 trv 1 J qUe minimiza la expresión

se calcula para cada uno de ellos de manera independiente.

Considerando que las correspondencias de cada conjunto se definen sobre puntos observados que se bloquean correctamente en los seis grados de libertad, se espera que sus transformaciones rígidas asociadas sean similares. No obstante, en presencia de valores atípicos y considerando el número reducido de puntos para cada conjunto, las transformaciones resultantes serán aleatoriamente diferentes. Se puede aproximar el error de estimación con una distribución gaussiana e identificar correspondencias atípicas eliminando los conjuntos que divergen de dicha distribución. Se procede iterativamente considerando inicialmente todas las transformaciones y calculando la distribución normal asociada donde:

siendo - ^{y ' '} las transformaciones r ^,ígidas asociadas con cada conjunto expresadas como un vector, donde las rotaciones están en los ángulos de guiñada, cabeceo e inclinación. A continuación, de acuerdo con N(//,S)^ |as distancias de mahalanobis para cada conjunto se calculan como

y se descartan las transformaciones con una probabilidad asociada menor del 1 %. Este proceso se repite iterativamente (actualizando N (//,E )con las transformaciones restantes en cada etapa) hasta que no se descarta ninguna transformación o se alcanza un número mínimo de transformaciones típicas. El registro final se estima considerando solo las correspondencias presentes en los conjuntos asociados con las transformaciones restantes.

La figura 7 muestra los resultados después de la estrategia de selección de valores atípicos propuesta. Cabe señalar cómo todas las transformaciones calculadas independientemente se distribuyen alrededor de una posición central bien definida. Cabe señalar, asimismo, que, después de cada iteración de eliminación de valores atípicos, las distribuciones rápidamente convergen hacia la transformación final estimada, al considerar todas las correspondencias marcadas como típicas.

Integración de odómetro

Para mejorar la robustez general del sistema, los inventores combinan preferiblemente su componente de seguimiento del sensor propuesto con un odómetro.

Después de estimar una pose, sus puntos asociados en coordenadas mundiales se almacenan en un árbol kd. Dada una nueva obtención, cuando el algoritmo de registro crea los conjuntos de puntos ( P/ busca vecinos más cercanos

tanto en el mapa de referencia ^ ,n ' ^como en la nube previamente fijada ,n ' \ A continuación, las correspondencias se definen como:

f w u M, I lLpr w -q, M < 1 Olí

ÍPÍ q, n } lpr o -q,

,q, ,n,} K -q f \-S> lp?-q?|

donde s corresponde al tamaño de celda de vóxeles y compensa la resolución diferente entre el mapa de referencia real voxelizado y el árbol kd no discretizado de la nube previamente fijada.

Los principales beneficios son que (a) las superficies que faltan en el mapa de referencia se pueden aprovechar durante el proceso de registro y que (b) el sistema permite explorar zonas no mapeadas mediante el seguimiento continuo del usuario.

B.4. Resultados

Para evaluar el sistema de localización y seguimiento propuesto, los inventores realizaron varias pruebas utilizando cuatro conjuntos de datos diferentes obtenidos con un escáner LIDAR: (a) un edificio de dos pisos con un gran laboratorio en la planta baja y varias oficinas en el primer piso, con una superficie aproximada de 1400; (b) un edificio de conferencias de una sola planta y una superficie aproximada de 1800; (c) un taller industrial de techos muy altos y con una superficie aproximada de 3000; (d) un gran túnel subterráneo que puede ser explorado mediante un automóvil, y con una longitud total de 2.2. Todos los modelos se obtienen registrando las obtenciones en un marco de referencia común utilizando el método del documento de J. Yao, M. R. Ruggeri, P. Taddei, V. Sequeira, 2010. “Automatic scan registration using 3D linear and planar surfaces.” 3D Research1 (3), 1 a 18. El mapa final se genera almacenando puntos y normales asociados (y, si están presentes, colores) después de una etapa de submuestreo de vóxeles de tamaño 1 o 10.

Para estos conjuntos de datos, los inventores evaluaron el sistema utilizando un sensor Velodyne HDL-32E montado en tres configuraciones diferentes: en una mochila para recorridos, en un Segway y en la parte superior de un automóvil. Los resultados se generaron utilizando un ordenador con una CPU Intel Xeon a 2,80 GHz con 8 GB de RAM y un sistema operativo de 64 bits.

Reconocimiento de lugares

Con el fin de reducir el espacio de búsqueda para el reconocedor de lugares, se calcularon los pisos de todos los mapas utilizando el algoritmo de inundación propuesto. En esta etapa, los inventores utilizaron celdas de vóxeles (20) grandes para realizar los cálculos, ya que no hay necesidad de una representación muy detallada de los límites mínimos. El tiempo promedio de cálculo del piso para los tres edificios fue de solo 0,14 mientras que el conjunto de datos del túnel necesitó 3,91. La figura 8 muestra los resultados del edificio de oficinas.

Una vez que se calcularon los pisos, los inventores estimaron el espacio navegable efectivo, ,Av £ ^ . En particular, para la aplicación montada en la mochila, los inventores realizaron varias pruebas que incluían caminar normalmente sobre superficies planas, correr, caminar por escaleras y realizar rotaciones rápidas. Durante estas pruebas, la posición del observador fue seguida y registrada de manera continua. Algunos de los resultados obtenidos se presentan en la figura 8.

La figura 9 (izquierda) muestra el histograma de desviaciones con respecto a la altura media con respecto al suelo. Los resultados muestran una distribución con una desviación estándar de a = 2,97. De esta manera, considerando que el sensor montado en la mochila se encuentra 10 por encima de la cabeza del portador, y que la altura humana entre los percentiles de 5 % y 95 % está en el rango [150,7 ... 188,7], de acuerdo con [McDowell MA y CL (2008)], el rango de altura efectiva con respecto al suelo se estimó en [154,7 ... 204,6].

Con respecto a las orientaciones, se considera un movimiento libre sobre el eje Z Los otros dos grados de libertad están limitados, puesto que las personas habitualmente solo se inclinan algunos grados mientras caminan. La figura 9 (derecha) muestra el histograma de desviaciones con respecto al eje Z absoluto observado durante las pruebas de evaluación (los valores están centrados en, p = 3,66° con desviación estándar a = 2,37°). De acuerdo con esto, el presente proceso de aprendizaje solo considera desviaciones de ± 8,41° con respecto al eje vertical (p 2a).

Dados estos parámetros, la reducción de volumen total en el espacio de búsqueda (considerando solo posiciones) se muestra en la Tabla IV. Cabe señalar cómo, para edificios regulares (edificio de oficinas (a) y de conferencias (b)), el espacio de búsqueda resultante es de alrededor del 2 % al 3 % del volumen total del mapa, mientras que, en el taller (c) y el túnel (d) esta relación es considerablemente menor, debido a los techos altos del primero y a la baja densidad de zonas navegables en el segundo.

Tabla IV: Reducción del espacio navegable

Para medir solo los rendimientos del reconocimiento de lugares, los inventores utilizaron cinco secuencias de obtención y estimaron las pistas de referencia real empleando su componente de seguimiento con una posición de sensor inicializada manualmente. A continuación, entrenaron a un clasificador de reconocimiento de lugares utilizando conjuntamente los tres edificios diferentes. A continuación, cada pista fue procesada utilizando solo el componente de reconocimiento de lugar (seguimiento basado en la clasificación). Puesto que no proporcionaron información sobre el edificio específico en el que se movía el usuario, las primeras soluciones candidatas se distribuyeron uniformemente en los tres entornos. Durante los experimentos, cada vez que el sensor se movía más de 2, se consultaba el reconocedor de lugar. El número total de bins en el descriptor utilizado fue de 12 x 1, y las consultas se realizaron con un radio de 6 en el espacio del descriptor. Las posibilidades a priori para poses potenciales se calcularon considerando dmax = 1 y que las ubicaciones solo eran comparables si su orientación relativa era menor de 45. El tamaño total del conjunto de aprendizaje utilizado para los tres edificios fue de 1473 KB.

Observaron que después de dos o tres iteraciones, aproximadamente dentro de las 10 desde la posición inicial, las soluciones candidatas se agruparon en el entorno único correcto y, después, siguieron de cerca la posición correcta del sensor. La figura 10 muestra los resultados del análisis de la deriva de todas las secuencias empleadas. Cabe señalar que, para cada pista, los inventores eliminaron las estimaciones de pose inicial relacionadas con las tramas donde el sistema aún no tiene suficiente información para identificar de manera única el entorno. De acuerdo con lo descrito por A. Geiger, P. Lenz, C. Stiller, R. Urtasun, 2013. “Vision meets robotics: The kitti dataset.” International Journal of Robotics Research, los gráficos muestran los errores promedio de la deriva de traslación y rotación dados en cualquier punto de las pistas después de una longitud de pista específica.

Seguimiento de pose

El componente de seguimiento de pose ha sido evaluado aislando cada uno de sus componentes y generando resultados individuales (representación de mapa, selección de puntos, selección de interior e integración de odómetro) y midiendo la precisión y el rendimiento general del sistema completo.

Representación de mapa

Para evaluar la escalabilidad de la representación de mapa propuesta y comparar cómo funciona con respecto a los árboles kd estándar, los inventores midieron las necesidades de espacio para cargar la estructura de vóxeles completa de cada conjunto de datos en la memoria, y aislaron las búsquedas de vecinos más cercanos en el proceso de registro para estimar el tiempo de cálculo promedio por cada consulta.

La Tabla V muestra el tamaño de memoria de cada conjunto de datos (cuarta columna), considerando el tamaño del vóxel (segunda columna) y las dimensiones de la estructura completa del vóxel (tercera columna). Cabe señalar que, para el edificio industrial (c), se consideran dos casos: uno que extiende el mapa original agregando información sobre el exterior, (c)-o, y el mapa original donde solo se almacena el interior (c)-i.

También es importante señalar que, en todos los casos, el tamaño de la memoria de la estructura de datos de vóxeles es menor que la nube de puntos que los generó.

Tabla V: Tamaños de mapa para los diferentes conjuntos de datos

La Tabla VI compara los tiempos de búsqueda del vecino más cercano de la representación cartográfica propuesta con respecto a un árbol kd estándar. Para esta prueba, ambas estructuras contenían el mismo número de puntos y las consultas se realizaron utilizando los mismos datos. Los resultados de las columnas 2 y 3 se expresan en nanosegundos por punto, y representan el tiempo promedio considerando todas las consultas. La columna 4 muestra el error promedio del vecino más cercano de la representación cartográfica propuesta, debido a la discretización del espacio. La columna 5 muestra el número total de puntos en el mapa.

Tabla VI: Tiempos de cálculo y errores promedio del vecino más cercano del mapa para la representación del mapa propuesta en comparación con una implementación estándar de árbol kd.

Cabe señalar cómo, los tiempos de búsqueda promedio son siempre alrededor de 10 veces más rápidos que utilizando árboles kd. Cabe señalar, asimismo, cómo el error general en los casos (a), (b) y (d), donde se utilizó un tamaño de celda de vóxeles de 10, es de alrededor de 0,2. Si se reduce a 5, tal como se muestra en el caso (c), el error cae a 0,08.

Selección de puntos

En los experimentos, los inventores observaron que su técnica de selección de puntos para garantizar la estabilidad geométrica siempre proporcionaba resultados sólidos. También observaron que, si esta función no estaba habilitada, se perdía el seguimiento cuando se navegaba por pasillos. No obstante, no se detectaron diferencias significativas al comparar la estabilidad de los resultados con respecto a la técnica propuesta por [Gelfand et al. (2003) Gelfand, Ikemoto, Rusinkiewicz y Levoy]. Por otro lado, los tiempos de ejecución siempre fueron menores con la presente técnica, ya que la estrategia de organización en bins utilizada evita clasificar puntos de acuerdo con sus capacidades de bloqueo.

Se realizó una prueba adicional para evaluar la estrategia de selección de puntos para entornos simétricos. En este caso, la presente técnica bloqueó correctamente las orientaciones seleccionando los puntos correctos, pero la propuesta en [Gelfand et al. (2003) Gelfand, Ikemoto, Rusinkiewicz y Levoy] fracasó. En este caso, la presente estrategia de selección de puntos no se ve afectada por la distancia entre los puntos y el sensor. De esta manera se pueden seleccionar puntos críticos tales como los mostrados en los casos A y C. Este hecho es evidente cuando se comparan los resultados para el caso B. Puesto que la presente selección está normalizada de acuerdo con las distancias, el efecto de los puntos más lejanos no compromete la selección de los más cercanos.

Selección habitual

Con el fin de evaluar la estrategia de selección de típicos propuesta, los inventores procedieron de la siguiente manera: los inventores montaron un sensor Velodyne HDL-32E en un trípode sin moverlo. La primera trama se utilizó como modelo de referencia y, durante el resto del experimento, se agregaron progresivamente valores atípicos (por ejemplo, personas moviéndose al azar y objetos movidos). De esta manera, podrían clasificar las correspondencias de los típicos evaluando la distancia entre cada punto y su vecino más cercano en la trama de referencia.

La figura 4 muestra la precisión final de la presente estrategia de selección de típicos con respecto al número de valores atípicos en la nube de entrada. Cabe señalar cómo, cuando el número total de valores atípicos es inferior al 20 %, la precisión actual es casi siempre del 100 % (no se seleccionan correspondencias incorrectas para el registro). A medida que aumenta el número total de valores atípicos, la precisión disminuye. En promedio, el experimento propuesto tuvo un 27,83 % de correspondencias erróneas, que conducen a una precisión del 98,97 %.

Integración del odómetro

Para ilustrar los beneficios de la integración del odómetro propuesta en el componente de actualización de pose, los inventores registraron una pista en la que, comenzando desde el interior del edificio (a), se movieron a una habitación no escaneada y realizaron algunos bucles saliendo del edificio y entrando desde una puerta diferente. La figura 13 muestra los resultados conseguidos.

Cabe señalar cómo, cuando el sensor sale del entorno conocido (casos A, C y D), el seguimiento se basa únicamente en el odómetro. Además, durante las transiciones entre el mapa conocido y las zonas no mapeadas, la estrategia de selección de puntos propuesta toma gradualmente más información del mapa más conveniente sin ninguna lógica específica para tratar con estas situaciones (tómese, por ejemplo, la transición mostrada en el caso C, derecho). Tal como se puede observar, la precisión del algoritmo de registro propuesto garantiza que, cuando el usuario vuelve a entrar en el mapa después de explorar las zonas no mapeadas, la deriva del odómetro es lo suficientemente baja como para que el seguimiento utilizando el mapa de referencia pueda continuar. Finalmente, cuando el sensor se mueve dentro del espacio conocido, se puede observar cómo algunos de los puntos utilizados para el registro se toman del odómetro. Esto se debe, en general, a la presencia de puntos que no tienen correspondencias válidas en el mapa de referencia, pero sí en el mapa local del odómetro. Por ejemplo, el entorno en el caso B tiene grandes ventanales que permiten que el sensor obtenga puntos del exterior, que no están presentes en el mapa original.

Precisión y rendimiento generales

Para medir la precisión general de la técnica de seguimiento de pose propuesta, los inventores realizaron un análisis de los residuos de ICP después de cada registro de nubes. Esto se debe a la falta de una trayectoria de referencia real para el movimiento libre en un escenario de interior grande, ya que la zona a cubrir es demasiado grande para utilizar sistemas de referencia externos precisos.

La figura 14 (superior) muestra las distancias promedio de punto a plano cuando el movimiento se produce dentro de un escenario libre de valores atípicos, tanto para el algoritmo ICP clásico de punto a plano como para el ICP robusto actual. La ausencia de valores atípicos se garantizó realizando las obtenciones inmediatamente después de escanear el modelo de referencia real, representado con un tamaño de celda de vóxeles de 10. Los residuos de ambos enfoques son casi idénticos, alcanzando un máximo de 2, lo que está dentro de la precisión nominal de Sensor Velodyne HDL-32E.

Por otro lado, la figura 14 (inferior) muestra diferencias significativas cuando se introducen cambios en el entorno. En este caso, la pista se grabó tras rehabilitar el entorno. La implementación de ICP robusto actual proporciona resultados mucho mejores que la utilización del ICP de punto a plano clásico, debido a la selección eficiente de correspondencias típicas. En este caso, los residuos alcanzan un pico en 3, debido a la interferencia de los valores atípicos en la estimación de la distancia de punto a plano para calcular el histograma mostrado.

Puesto que el sistema debe proporcionar resultados en tiempo real, los inventores midieron el rendimiento general durante el seguimiento de pose para diferentes tipos de movimiento en todos los conjuntos de datos. La figura 15 muestra el tiempo de ejecución invertido en registrar cada nube y calcular la nueva pose del sensor para un escenario de configuración caminando. Este proceso tarda normalmente entre 20 y 30 ms pero, en algunas tramas, se observa un pico alrededor de los 100 ms. Estos picos están relacionados con la generación del árbol kd para el odómetro, que se activa cuando se recorre una distancia fija desde el momento de la última actualización del árbol. Cuanto más rápido se mueva el sensor, más afectará este evento al rendimiento general. Para evitar la caída de la trama, la generación del árbol kd y el odómetro se ejecutan en diferentes subprocesos y este último utiliza el último árbol kd disponible hasta que el nuevo está listo.

En la Tabla VII se muestra el rendimiento promedio del sistema para tres configuraciones diferentes (caminando, montado en segway y montado en automóvil). Cabe señalar cómo, cuanto más rápido se mueve el sensor, menor es el rendimiento debido a las actualizaciones del árbol kd del odómetro. Puesto que el sensor Velodyne HDL-32E proporciona lecturas a 12 Hz, todos los casos garantizan resultados en tiempo real, dejando tiempo del procesador para realizar operaciones adicionales. Por último, cabe señalar que, en la implementación actual, todos los cálculos de seguimiento se realizaron utilizando un solo núcleo de CPU.

Tabla VII: Rendimiento de ejecución del sistema para diferentes configuraciones

5 Conclusión

La presente invención presenta un sistema completo con realizaciones preferidas para ayudar en aplicaciones de localización en interiores, que proporciona resultados en tiempo real y escalas al tamaño del mapa, lo que permite la exploración de entornos muy grandes.

Al agregar una etapa de preprocesamiento, se ha propuesto un reconocedor de lugar eficiente que aprovecha el movimiento local del sensor, medido con un odómetro, y un descriptor compacto y rápido de calcular. Durante el aprendizaje del reconocedor de lugares, se ha propuesto una estrategia de reducción del espacio de búsqueda que considera las limitaciones físicas relacionadas con un modo de funcionamiento particular del sistema.

El seguimiento de pose se realiza utilizando una representación de mapa eficiente, que proporciona tiempos de búsqueda constantes del vecino más cercano y que mantiene bajas las necesidades de memoria. El presente algoritmo de registro proporciona resultados robustos (1) seleccionando puntos que garantizan la estabilidad geométrica, (2) descartando de manera eficiente los valores atípicos y (3) fusionándose con un odómetro local, lo que permite utilizar puntos que no están presentes en el mapa de referencia para el registro y navegar a través de zonas no mapeadas.

Los resultados experimentales han demostrado que el sistema es altamente escalable, realiza un seguimiento a la velocidad de tramas, lo que deja mucho tiempo de CPU para ejecutar operaciones adicionales y producir resultados muy precisos (dentro de la precisión nominal del sensor utilizado), incluso cuando se introducen muchos valores atípicos.

Claims (14)

REIVINDICACIONES

1. Un método para el mapeo en tiempo real, la localización y el análisis de cambios de un entorno, en particular en un entorno sin GPS, que comprende las siguientes etapas:

(A) si no existe un mapa de referencia 3D del entorno, construir un mapa de referencia 3D de dicho entorno mediante

(a) obtener el entorno con un escáner de rango láser (1) móvil en tiempo real a una velocidad de al menos 5 tramas por segundo, para proporcionar datos de escáner 3D,

(b) construir una presentación de mapa utilizando los datos del escáner 3D para cada una de la pluralidad de poses del escáner de rango láser (1), teniendo cada pose una nube de puntos asociada definida por los datos del escáner 3D, teniendo la presentación del mapa una estructura de datos configurada para manejar de manera nativa puntos 3D, que se basa en una estructura híbrida compuesta por una estructura de vóxeles dispersa utilizada para indexar una lista compacta y densa de características en la presentación del mapa, lo que permite un acceso aleatorio en tiempo constante en coordenadas de vóxeles independientemente del tamaño del mapa, y un almacenamiento eficiente de los datos con escalabilidad sobre el entorno, y

(c) construir, utilizando la presentación del mapa, el mapa de referencia 3D para el entorno utilizando un marco de localización y mapeo simultáneo 3D (3D SLAM), comprendiendo dicho edificio

(i) utilizar un módulo de odómetro, estimando una pose actual del escáner de rango láser (1) en base al registro de una última nube de puntos en una presentación de mapa local,

(ii) utilizar un módulo de optimización de trayectoria local, refinando la trayectoria de un conjunto de nubes de puntos para minimizar la deriva en la presentación del mapa local, y

(iii) realizar fuera de línea una optimización de trayectoria global mediante la reconstrucción de un mapa completo del entorno teniendo en cuenta los cierres de bucles de trayectorias, formando de este modo dicho mapa de referencia 3D; y

(B) en base a un mapa de referencia 3D existente del entorno, realizar un mapeo en tiempo real, localización y análisis de cambios de dicho entorno

(d) obteniendo el entorno con un escáner de rango láser (1) en tiempo real a una velocidad de al menos 5 tramas por segundo, para proporcionar datos de escáner 3D,

(e) durante el reconocimiento de lugar, identificando una ubicación actual del escáner de rango láser (1) dentro del entorno sin conocimiento previo de la pose del escáner de rango láser durante el reconocimiento de lugar, y calculando previamente descriptores simples y compactos de una escena obtenida por el escáner de rango láser (1) utilizando un espacio de búsqueda reducido dentro de la escena para autolocalizar el escáner en tiempo real, comprendiendo cada descriptor de la escena una imagen de rango de bins regulares, donde cada bin tiene un valor de rango medio estimado,

(f) después de determinar la ubicación del escáner en el entorno, realizando un seguimiento de la pose del escáner registrando los datos del escáner actual dentro de dicho mapa de referencia 3D existente del entorno utilizando el método de punto más cercano iterativo estándar que emplea datos que comprenden información del vecino más cercano almacenada en el mapa de referencia 3D,

(g) calculando la distancia entre cada punto de escaneo en los datos de escáner actuales y el punto más cercano en el mapa de referencia 3D, en donde el análisis de cambio se realiza aplicando un umbral a esta distancia, por lo que cada punto en los datos de escáner actuales que no tiene un vecino correspondiente en el modelo de referencia se considera un cambio,

(h) mostrando información sobre el mapa de referencia 3D y los datos actuales del escáner 3D en una interfaz de usuario en tiempo real, estando dicha información preferiblemente en codificada en color de acuerdo con una clasificación de cambio / no cambio de dicha información.

2. El método de acuerdo con la reivindicación 1, en el que el módulo de optimización de la trayectoria local comprende un mecanismo de ventana local que optimiza un fragmento de trayectoria compuesto por un conjunto de poses y sus nubes de puntos asociadas con respecto a un mapa construido hasta el último conjunto registrado, en el que los puntos son convertidos preferiblemente en coordenadas mundiales utilizando interpolación de pose en el grupo SE3 y en el que se utiliza preferiblemente una generalización del método del punto más cercano iterativo para encontrar la trayectoria que alinea mejor todos los puntos con el mapa; en donde el mecanismo de ventana local funciona de tal manera que, cuando la distancia entre la primera y la última pose en la lista es mayor que un umbral, las poses de la nube se optimizan y se produce una nueva lista con la pose refinada y las nubes de entrada.

3. El método de acuerdo con la reivindicación 1 o 2, en el que la estructura de datos mantiene cinco representaciones diferentes de los datos almacenados, garantizando de este modo la coherencia entre las representaciones de datos internos después de cada actualización de mapa, siendo las cinco representaciones

(i) una lista compacta y densa de características, L y un índice del último elemento, ^/.último, donde cada elemento, /, e ^ , contiene toda la información asociada a una característica en el mapa, tal como la posición y el vector unitario normal en coordenadas mundiales, y preferiblemente información adicional,

(ii) una máscara de validez compacta y densa, M, donde cada elemento, m> e ^ , es un valor booleano que indica si su muestra correspondiente, h e es válida o no, garantizando que w¡ = 0 ,i > Ultimo,

(iii) una lista de huecos, H, donde cada elemento,

< Lúltimo, indica que no es válido, así que,

(iv) una representación de vóxeles l/dispersa, construida con un tamaño de celda parametrizable, que almacena en cada celda, vt e ^ , el índice de su característica correspondiente en L, donde las características en L y las celdas en V están relacionadas de una manera de uno a uno, de acuerdo con la posición de ^ , y el tamaño de celda de V, y

(v) un árbol kd, K, que se utiliza para realizar búsquedas de vecino más cercano en el mapa y que solo almacena referencias a la lista densa L para mantener bajo su tamaño de memoria.

4. El método de acuerdo con la reivindicación 3, en el que, dada una zona de interés expresada por una posición central y un radio, las características interiores se seleccionan haciendo un bucle sobre los elementos almacenados en L, y el árbol kd K se reconstruye como un mecanismo rápido para las búsquedas del vecino más cercano.

5. El método de acuerdo con la reivindicación 3,

en el que la etapa (e) comprende la identificación de un conjunto de posibles ubicaciones del escáner en base a los datos del escáner de la etapa (d), teniendo, además, dicha etapa (e), las siguientes subetapas:

(e1) en base a los últimos datos del escáner 3D, calcular un descriptor q asociado y recuperar un conjunto de ubicaciones candidatas T realizando una búsqueda radial en K

dado un radio de umbral r en el espacio del descriptor, preferiblemente para datos de escáner de visión horizontal de 360°, aumentar las ubicaciones candidatas mediante el cálculo de descriptores de entrada adicionales mediante el desplazamiento horizontal de los valores de rango, correspondiendo cada descriptor a las lecturas que produciría el escáner si se girase sobre su eje local y a continuación girase de acuerdo con cada conjunto resultante de ubicaciones candidatas,

w .

(e2) asociar un peso p a cada ubicación potencial

donde d ^p es el descriptor asociado a la ubicación r ^p recuperada de K

vvr

p es 1 para descriptores que coinciden perfectamente y 0 para descriptores en el límite de la esfera de búsqueda, y

(e3) recolectar pesos en w y normalizar estos pesos para tener un max w = 1.

6. El método de acuerdo con la reivindicación 3, en el que la etapa (e) comprende la identificación de un conjunto de posibles ubicaciones del escáner en base a los datos del escáner de la etapa (d), teniendo dicha etapa (e), además, las siguientes subetapas:

(e1) en base a los últimos datos del escáner 3D, calcular un descriptor q asociado y recuperar un conjunto de ubicaciones candidatas T j teniendo las ubicaciones candidatas un descriptor similar a q,

vvr r c r_{(e2) asociar un peso p a cada ubicación potencial 1 p c 1 :}

donde d ^p es el descriptor asociado a la ubicación r ^p recuperada de K

w r

r es 1 para descriptores que coinciden perfectamente y 0 para descriptores en el límite de la esfera de búsqueda, y

(e3) recolectar pesos en w y normalizar estos pesos para tener un max w = 1.

(e4) actualizar el conjunto de ubicaciones candidatas mientras el sensor se mueve estimando el movimiento y reevaluando el peso para cada pose candidata inicial en base a los resultados de la consulta en la nueva pose, y

(e5) iterar la subetapa de actualización hasta que las poses candidatas converjan en una única ubicación.

7. El método de acuerdo con cualquiera de las reivindicaciones 1 a 6, particularmente para el movimiento en el suelo, en el que el escáner de rango láser (1) está montado en una persona o en un vehículo que atraviesa un piso, que comprende las siguientes etapas

(i) identificar en el mapa de referencia 3D las extensiones del piso, donde la extracción del piso se realiza sobre una representación de vóxeles dispersa del entorno, V, donde cada celda completa v(i), de la representación de vóxeles dispersa contiene un vector normal a la superficie definida localmente por los puntos alrededor de su -(<>

centroide, n extrayendo un subconjunto de vóxeles que representan celdas de piso candidatas, comprobando que la componente vertical en sus normales asociadas sea dominante, es decir, n • (0.0.1) - £ donde £ es habitualmente un valor comprendido entre 0,5 y 1;

(ii) determinar la capacidad de ser alcanzadas de las celdas, en donde dada una celda alcanzable ^ r ^ , todas /,,0) „<2) „(” )-),= f

las celdas circundantes, ’ * .....s 1 se consideran alcanzables si se cumplen las siguientes condiciones:

donde - ^{Vtamano de celda} (6) representa la distancia máxima de etapa (por ejemplo 0,5 metros para un movimiento caminando, o ^{Vtamano de celda} para un movimiento en coche), ^1 en (7) representa el tamaño máximo de etapa ^, vertical y ^C * ⁿ en

(8) representa el volumen simplificado del observador, centrado sobre la celda del piso g¡ 8. El método de acuerdo con cualquiera de las reivindicaciones 1 a 7, en el que la estructura del mapa comprende dos listas diferentes de elementos que están almacenados y sincronizados: una lista (compacta) de planos, L, y una cuadrícula (densa) de vóxeles, V, construida con un tamaño de vóxel específico, almacenando cada plano A e ¿ una posición en coordenadas mundiales, p¡, y una unidad normal, n i ; en donde cada vóxel, v< e ^ almacena un estado actual que puede ser completo, vacío o cercano, almacenando los vóxeles llenos un índice en el plano , cuya posición asociada cae dentro, almacenando las celdas vacías una referencia nula y almacenando las celdas cercanas un índice en el plano l ' ¡ eL cuya distancia de posición dh asociada al centro del vóxel es la más pequeña; preferiblemente, se considera un vóxel cercano solo si la distancia dv está por debajo de un umbral determinado; de lo contrario, el vóxel se considera vacío.

9. El método de acuerdo con cualquiera de las reivindicaciones 1 a 8, en el que, para mejorar la robustez general del sistema, el seguimiento del escáner se combina con un odómetro, en donde, después de que una pose ha sido estimada, sus puntos asociados en coordenadas mundiales son almacenados en un árbol kd,

dada una nueva obtención por el escáner de rango láser (1), cuando un algoritmo de registro crea los conjuntos de ÍT „ M

puntos (P/ ), busca los vecinos más cercanos tanto en el mapa de referencia ( i n ) como en la nube de puntos ( o ~ °

,n ' ) previamente fijados, en donde las correspondencias se definen como:

donde s corresponde al tamaño de la celda de vóxeles y compensa la resolución diferente entre el mapa de referencia real voxelizado y el árbol kd no discretizado de la nube previamente fijada.

10. Un dispositivo de escaneo láser móvil para mapeo, localización y análisis de cambios, en particular en entornos sin GPS, dispuesto para implementar el método tal como el reivindicado en una cualquiera de las reivindicaciones 1 a 9.

11. Dispositivo de escaneo láser móvil de acuerdo con la reivindicación 10, que comprende un escáner de rango láser (1) en tiempo real, una unidad de procesamiento (3), una unidad de fuente de alimentación y una unidad de visualización y control (4) de mano, en la que el escáner de rango láser (1) en tiempo real es capaz de obtener el entorno con una velocidad de al menos 5 tramas por segundo para proporcionar datos del escáner, la unidad de procesamiento (3) está dispuesta para analizar dichos datos de escáner y para proporcionar resultados de procesamiento que comprenden mapa / modelo 3D, localización e información de cambios a la unidad de control y visualización (4) de mano, que está dispuesta para mostrar dichos resultados de procesamiento y permitir a un usuario controlar el dispositivo de escaneo láser móvil.

12. Dispositivo de escaneo láser móvil de acuerdo con la reivindicación 10 u 11, en el que la unidad de visualización y control (4) es una tableta.

13. Dispositivo de escaneo láser móvil de acuerdo con cualquiera de las reivindicaciones 10 a 12, en el que dicho dispositivo de escaneo láser móvil es una mochila (2) o un dispositivo montado en un vehículo.

14. Utilización de un método de acuerdo con cualquiera de las reivindicaciones 1 a 9 o de un dispositivo de escaneo láser móvil de acuerdo con cualquiera de las reivindicaciones 10 a 13, para el mapeo / modelizado de interiores 3D; la gestión de instalaciones; la localización y navegación interior precisa y en tiempo real; la asistencia a personas discapacitadas o ancianas; la verificación de información de diseño; el análisis de cambios, tal como para inspecciones de salvaguardias; el seguimiento del progreso, tal como en la construcción civil; o la gestión de desastres y la respuesta a los mismos.