InsSo3D: Inertial Navigation System and 3D Sonar SLAM for turbid environment inspection

El artículo presenta InsSo3D, un sistema robusto de localización y mapeo simultáneo (SLAM) que combina un sonar 3D y una navegación inercial para generar mapas tridimensionales precisos y corregir la deriva de odometría en entornos submarinos turbios y de gran escala.

Lo esencial

Imagina que eres un buzo explorando un lago muy turbio, donde el agua está tan llena de barro y algas que no puedes ver ni tu propia mano frente a tu cara. En esas condiciones, una cámara normal es inútil; es como intentar leer un libro con los ojos cerrados. Pero, ¿qué pasaría si tu "ojo" pudiera ver a través de la oscuridad usando el sonido, como un murciélago, pero en 3D?

Ese es el problema que resuelve este paper, presentado por un equipo de investigadores llamado InsSo3D. Aquí te explico cómo funciona, usando analogías sencillas:

1. El Problema: El "Efecto Espejo" del Sonar Antiguo

Antes, los robots submarinos usaban sonares tradicionales. Imagina que lanzas una piedra a un lago y escuchas el "splash". Sabes a qué distancia está la roca, pero no sabes si está arriba, abajo o al lado. Es como tener una foto en 2D de un mundo 3D; pierdes la profundidad vertical. Esto hace que sea muy difícil construir un mapa real, porque el robot no sabe si está chocando contra el techo o contra el suelo.

2. La Solución: Un "Escáner Láser" que usa Sonido

Los autores usaron un nuevo tipo de sonar llamado Sonar 3D.

• La analogía: Piensa en el sonar antiguo como una linterna que solo ilumina una línea plana. El nuevo sonar 3D es como una máquina de rayos láser (Lidar) que funciona bajo el agua. En lugar de solo decir "hay algo a 5 metros", te dice: "hay algo a 5 metros, a la derecha y un poco hacia arriba".
• Esto elimina la confusión de la altura. Ahora el robot tiene una "foto" tridimensional completa de su entorno, incluso en agua sucia.

3. El Viajero: El Robot y su "Sentido de la Orientación"

El robot no solo tiene este sonar mágico, sino que también lleva un Sistema de Navegación Inercial (INS).

• La analogía: Imagina que el sonar es tus ojos, pero el INS es tu sentido del equilibrio y la orientación (como cuando cierras los ojos y aún sabes si te estás moviendo o girando).
• El sonar ve el mundo, y el INS le dice al robot: "Oye, me moví 2 metros hacia adelante y giré un poco". Juntos, trabajan para no perderse.

4. El Cerebro: Construyendo el Mapa (SLAM)

El sistema usa una técnica llamada SLAM (Localización y Mapeo Simultáneo). Es como si el robot estuviera dibujando un mapa de un castillo mientras camina por él, sin saber dónde empieza ni dónde termina.

• El Frontend (El Dibujante Rápido): El robot toma fotos de sonido (puntos 3D) y las une rápidamente, como si estuviera pegando fotos en un álbum. Usa un algoritmo inteligente (llamado CFEAR) para encontrar coincidencias entre una foto y la siguiente, ajustando la posición del robot en tiempo real.
• El Backend (El Editor de Mapas): A veces, el robot da vueltas y vuelve a un lugar que ya visitó. Aquí es donde el sistema detecta un "cierre de bucle" (Loop Closure).
  • La analogía: Es como cuando caminas por un bosque y de repente reconoces un árbol que viste hace una hora. Tu cerebro dice: "¡Espera! Si volví a este árbol, entonces todo el mapa que dibujé antes tenía un pequeño error". El sistema corrige todo el mapa para que encaje perfectamente, eliminando los errores acumulados.

5. Los Resultados: ¿Funciona de verdad?

Los investigadores probaron esto en dos lugares:

1. Una cantera inundada: Un lugar grande y real.
2. Un tanque de pruebas: Un lugar pequeño pero con paredes de hormigón que confunden a las brújulas y crean ecos falsos (ruido).

El resultado fue impresionante:

• Mientras que la navegación normal del robot (sin el sistema inteligente) se desviaba y perdía el rumbo (como un borracho caminando), InsSo3D mantuvo el error por debajo de 21 centímetros en una misión de 50 minutos.
• Crearon un mapa de 20 metros por 10 metros con una precisión de 9 centímetros.
• Lograron ver la pared del fondo del tanque, algo que las cámaras normales no pudieron hacer porque el agua estaba muy turbia.

En Resumen

InsSo3D es como darle a un robot submarino ojos de rayos X y un sentido de la orientación infalible. Le permite navegar y dibujar mapas detallados de lugares oscuros y sucios donde las cámaras humanas fallan, corrigiendo sus propios errores mientras avanza. Es un paso gigante para que los robots puedan inspeccionar tuberías, barcos hundidos o estructuras submarinas de forma segura, sin importar cuán turbia sea el agua.

Resumen técnico

Aquí tienes un resumen técnico detallado del artículo "InsSo3D: Inertial Navigation System and 3D Sonar SLAM for turbid environment inspection" en español:

1. Problema y Contexto

La navegación y el mapeo de entornos submarinos presentan desafíos únicos, especialmente en aguas turbias, oscuras o con corrientes dinámicas.

• Limitaciones de los sensores ópticos: Las soluciones basadas en cámaras o LiDAR dependen de la visibilidad y funcionan solo a cortas distancias (generalmente < 3 metros), fallando en condiciones de baja visibilidad.
• Limitaciones del Sonar 2D tradicional: Aunque el sonar acústico opera a largas distancias y es resistente a la turbidez, los sonares 2D tradicionales producen imágenes que carecen de información de elevación, generando ambigüedad en la reconstrucción 3D. Resolver esta ambigüedad requiere movimientos muy restringidos o configuraciones complejas.
• Desafío del SLAM 3D: Los sistemas de SLAM (Localización y Mapeo Simultáneo) existentes para sonar 3D suelen ser escasos, costosos, computacionalmente intensivos o carecen de detección de bucles de cierre (loop closure) y optimización global, lo que provoca deriva en la odometría a largo plazo.

2. Metodología: InsSo3D

El artículo presenta InsSo3D, un marco de trabajo robusto y eficiente para SLAM 3D a gran escala que combina un Sonar 3D (WaterLinked Sonar3D-15) y un Sistema de Navegación Inercial (INS) compuesto por un DVL (Log de Velocidad Doppler), un AHRS (Sistema de Referencia de Actitud y Rumbo) y un sensor de presión.

El algoritmo se divide en dos componentes principales (ver Fig. 3 del artículo):

A. Frontend: Creación de Sub-mapas

• Registro de Nubes de Puntos: Utiliza una variante 3D del algoritmo CFEAR para generar una representación esparcida de puntos de superficie orientados. Para la optimización del registro, emplea Generalized ICP (GICP) mediante la implementación fast gcip, que utiliza una métrica de distancia plano-a-plano, ideal para nubes de puntos orientadas.
• Estrategia de Registro: Realiza dos tipos de registro simultáneos para mayor robustez:
  1. Frame-to-Frame: Entre el cuadro actual y el anterior.
  2. Frame-to-Sub-map: Entre el cuadro actual y la representación TSDF (Truncated Signed Distance Function) del sub-map actual o el primer cuadro del sub-map.
• Optimización Local: Se utiliza un grafo de factores para estimar la pose del cuadro actual, fusionando las restricciones de registro secuencial y no secuencial.
• Criterio de Finalización: Un sub-map se cierra cuando la superposición entre el último y el primer cuadro cae por debajo de un umbral, o si el sub-map tiene suficientes puntos, evitando así la acumulación excesiva de deriva dentro del sub-map.

B. Backend: Generación del Mapa Global

• Optimización del Grafo de Poses: Cuando se cierra un sub-map, se registra contra sub-maps anteriores para detectar bucles de cierre (loop closures).
  • Se identifica un conjunto de sub-maps candidatos basándose en la superposición de nubes de puntos en una cuadrícula voxel.
  • Si el error de registro es bajo, se añade una restricción de bucle de cierre al grafo de factores global.
• Fusión Global (TSDF): Los sub-maps se integran en un mapa global TSDF. El sistema maneja dinámicamente la actualización del mapa: si la optimización del grafo modifica la pose de un sub-map, este se elimina y se reintegra con la nueva pose corregida.
• Representación: Se utiliza una variación de "space carving" en el TSDF para mantener un registro de las regiones observadas y libres, permitiendo una reconstrucción 3D coherente.

3. Contribuciones Clave

1. Uso de Sonar 3D: Habilita un SLAM robusto, a gran escala y preciso en entornos no estructurados, eliminando la ambigüedad de elevación inherente a los sonares 2D.
2. Integración de CFEAR y GICP: Adaptación de algoritmos de registro robustos y resistentes al ruido para la coincidencia de cuadros y sub-maps en datos de sonar 3D.
3. Validación Exhaustiva: Evaluación cuantitativa en entornos reales (cantera inundada y tanque de pruebas) comparando con datos de verdad terreno (SfM visual y sistema de seguimiento de movimiento Qualisys).

4. Resultados Experimentales

El sistema fue evaluado en dos escenarios: una cantera inundada (47 minutos, 230m recorridos) y un tanque de pruebas con condiciones de turbidez y interferencias magnéticas.

• Precisión de Trayectoria:
  • El error promedio de la trayectoria (APERMS) se mantuvo consistentemente por debajo de 21 cm (0.213 m en la cantera y 0.078 m en el tanque tras alineación).
  • Comparado con la odometría pura, InsSo3D redujo drásticamente la deriva, incluso en el tanque donde la brújula falló debido a interferencias magnéticas del hormigón, causando una deriva de casi 40° en el yaw en la odometría.
• Precisión del Mapa:
  • Se reconstruyó un entorno de 20m x 10m con un error medio de reconstrucción de 9 cm (0.086 m en cantera, 0.073 m en tanque).
  • El sistema logró mapear paredes traseras del tanque que las cámaras estereoscópicas no pudieron capturar debido a la distancia y la visibilidad.
• Rendimiento Computacional:
  • El frontend opera a un promedio de 70 ms por cuadro (~14 Hz), superando la frecuencia de adquisición del sonar (6 Hz), lo que permite operación en tiempo real.
  • El backend y la generación del mapa global son procesos asíncronos más lentos pero no afectan la capacidad de navegación en tiempo real.

5. Significado y Conclusión

InsSo3D demuestra que es posible realizar navegación autónoma segura y mapeo 3D de alta fidelidad en condiciones submarinas extremas (aguas turbias, oscuras) donde los sensores ópticos fallan.

• Impacto: Permite la inspección de activos offshore y estructuras naturales/artificiales sin depender de la visibilidad.
• Limitaciones Futuras: El sistema depende de características geométricas (puede fallar en simetrías espaciales) y es sensible a efectos de multipath acústico. Los autores proponen futuras investigaciones para fusionar datos de sonar y cámara (para texturizar mapas y mejorar la localización) y abordar la robustez en entornos con simetría.

En resumen, InsSo3D representa un avance significativo hacia la autonomía a largo plazo de vehículos subacuáticos (AUV) en entornos desafiantes, ofreciendo una solución escalable y precisa para la inspección submarina.