Marker Based Visual Localization – Unibotics
Marker Based Visual Localization – Unibotics
Introducción
En esta práctica he desarrollado un sistema de localización visual para un robot móvil en un entorno simulado de Unibotics. El objetivo principal ha sido estimar la pose del robot en el mapa, es decir, su posición y orientación, a partir de la detección de marcadores visuales tipo AprilTag distribuidos en el entorno.
La práctica combina visión artificial, geometría proyectiva y navegación básica. Para ello, el robot utiliza su cámara para detectar los marcadores visibles y, a partir de su identificación y su posición conocida en el mapa, calcula una estimación de su propia pose. Esta estimación visual se complementa con la odometría del robot, que aunque es ruidosa, permite mantener una pose aproximada cuando en un instante dado no hay marcadores visibles.
Además, para evitar bloqueos durante la navegación, se ha incorporado una lógica sencilla basada en el láser, permitiendo detectar obstáculos cercanos y modificar el movimiento del robot cuando el entorno dificulta la visión directa de los tags.
Objetivo de la práctica
El objetivo de esta práctica ha sido construir un sistema capaz de:
- detectar AprilTags en la imagen de la cámara,
- identificar qué marcador se está observando,
- estimar la pose relativa entre la cámara y el marcador,
- transformar esa información a coordenadas globales del mapa,
- representar en pantalla la pose estimada del robot,
- y mantener un comportamiento razonable cuando no se detectan tags o existen obstáculos cercanos.
En el simulador, esta estimación se visualiza mediante un robot rojo, que representa la posición calculada por mi algoritmo, mientras que el robot verde indica la posición real y el azul corresponde a la odometría con ruido.
Fundamento teórico
Marcadores visuales AprilTag
Los AprilTags son marcadores fiduciales diseñados para ser detectados de forma robusta en imágenes. Cada uno contiene un identificador único, lo que permite no solo saber que hay un marcador visible, sino también reconocer cuál es.
En esta práctica, cada AprilTag tiene asociada una pose conocida en el entorno, definida mediante sus coordenadas globales y su orientación. Gracias a ello, una vez detectado un tag en la imagen, es posible calcular la posición del robot respecto al mapa.
Estimación de pose con PnP
A partir de las esquinas detectadas del AprilTag en la imagen y de la geometría conocida del marcador en el espacio 3D, se utiliza un problema de tipo Perspective-n-Point para estimar la rotación y traslación entre la cámara y el tag.
Este paso permite obtener la pose relativa del marcador con respecto a la cámara. Sin embargo, como lo que interesa realmente es la pose del robot en el mundo, ha sido necesario encadenar varias transformaciones geométricas entre sistemas de referencia.
Transformaciones de coordenadas
Para pasar de la observación local del tag a la pose global del robot, se han utilizado las siguientes relaciones:
- pose del tag en el mundo, conocida a partir del fichero de configuración,
- transformación entre el sistema óptico de la cámara y el sistema del marcador,
- transformación fija entre la cámara y la base del robot.
A partir de todo ello se obtiene finalmente una matriz homogénea que permite calcular la posición y orientación del robot en coordenadas del mapa.
Odometría y suavizado
La pose visual obtenida a partir de los tags puede fluctuar ligeramente entre fotogramas debido a ruido en la detección, pequeños errores de estimación y variaciones en la perspectiva. Para evitar saltos bruscos, se ha aplicado un suavizado tanto en la posición como en el ángulo de orientación.
Por otra parte, cuando el robot deja de ver marcadores durante unos instantes, la odometría permite propagar la pose estimada de manera continua, aunque con menor precisión.
Metodología seguida
1. Carga de información del entorno
El primer paso ha consistido en cargar la configuración de los AprilTags presentes en el escenario. En dicha configuración aparece, para cada marcador, su posición en el mapa y su orientación. Esta información es imprescindible para relacionar las detecciones visuales con una referencia global.
También se ha definido el modelo geométrico del tag, utilizando sus dimensiones reales, y los parámetros intrínsecos de la cámara del robot, necesarios para la resolución del problema de pose.
2. Detección de AprilTags en cada imagen
Durante la ejecución, el robot obtiene continuamente imágenes de la cámara frontal. Cada imagen se procesa para detectar marcadores AprilTag visibles.
Para cada detección se extraen:
- el identificador del tag,
- sus cuatro esquinas en la imagen,
- y el centro del marcador.
Esta información se utiliza posteriormente tanto para la estimación geométrica como para el control del robot.
Además, sobre la imagen mostrada en pantalla se representan los tags detectados, de forma que sea más fácil comprobar visualmente si la detección es correcta y qué marcador se está utilizando en cada momento.
3. Selección del mejor marcador
En muchas situaciones puede haber varios tags visibles al mismo tiempo. En ese caso no basta con utilizar cualquiera, ya que algunos pueden estar demasiado alejados, ser más pequeños en la imagen o producir una estimación menos estable.
Por ello, para cada marcador detectado se calcula una puntuación basada en dos criterios principales:
- el área del tag proyectado en la imagen, que da una idea de su cercanía y calidad visual,
- y la coherencia respecto a la pose anterior estimada, para penalizar cambios demasiado bruscos.
De esta forma, el sistema selecciona el marcador más fiable en cada iteración.
4. Estimación de la pose del robot
Una vez seleccionado el mejor tag visible, se calcula la pose relativa entre la cámara y dicho marcador usando las correspondencias entre las esquinas 2D detectadas y los puntos 3D conocidos del tag.
A continuación, esta información se transforma al sistema global del mapa teniendo en cuenta:
- la pose absoluta del AprilTag en el entorno,
- la relación entre el sistema del marcador y el sistema óptico de la cámara,
- y la transformación fija entre la cámara y la base del robot.
Con ello se obtiene directamente la pose estimada del robot en el mundo, expresada mediante:
- coordenada (x),
- coordenada (y),
- y orientación (yaw).
Esta pose se representa en el mapa mediante el robot rojo.
5. Suavizado de la estimación
La pose visual puede presentar pequeñas oscilaciones incluso cuando el robot está quieto o moviéndose suavemente. Para reducir este efecto, se ha aplicado un filtrado simple a la posición y a la orientación estimadas.
En el caso del ángulo, además, ha sido necesario tener en cuenta el salto natural entre (-\pi) y (\pi), para evitar errores en la interpolación de orientaciones.
Gracias a este suavizado, la trayectoria estimada resulta más estable y la visualización en el mapa es más coherente.
6. Propagación mediante odometría
Cuando no hay tags visibles, el sistema no puede realizar una corrección visual directa. En lugar de perder completamente la pose estimada, se utiliza la odometría del robot para propagar la última posición conocida.
No se toma la odometría bruta como una medida absoluta, ya que contiene ruido, sino que se emplea de forma incremental, actualizando la pose anterior según el desplazamiento observado entre iteraciones.
Esta estrategia permite mantener una estimación continua mientras el robot busca de nuevo un marcador visible.
7. Control visual del movimiento
Cuando el robot detecta un tag válido, no solo calcula su pose, sino que también ajusta su movimiento para orientarse hacia él. Para ello se utiliza el error horizontal entre el centro del marcador en la imagen y el centro de la cámara.
Si el tag aparece desplazado a un lado de la imagen, el robot genera una velocidad angular proporcional para recentrarlo. Si el frente está libre, además avanza hacia delante con velocidad lineal constante.
Este comportamiento permite que el robot mantenga el marcador dentro del campo de visión y continúe navegando mientras actualiza su pose.
8. Gestión de obstáculos con el láser
Uno de los problemas prácticos de esta tarea es que el robot puede quedarse demasiado cerca de paredes, muebles o patas de sillas, perdiendo visibilidad o quedando mal orientado respecto a los tags.
Para reducir este problema, se ha utilizado la información del láser en tres zonas:
- frontal,
- frontal izquierda,
- frontal derecha.
A partir de estas medidas se detecta si existe un obstáculo cercano. Cuando esto ocurre, el robot deja de avanzar y gira hacia el lado más despejado.
Con esta lógica se consigue que el robot no siga empujando hacia una zona bloqueada y tenga más probabilidades de recuperar la visibilidad de un marcador.
9. Estrategia de búsqueda
Si no se detecta ningún tag y tampoco hay un obstáculo que obligue a maniobrar, el robot entra en modo de búsqueda. En este estado gira sobre sí mismo para intentar recuperar un marcador visual.
Para que esta búsqueda sea más razonable, se recuerda el último lado por el que se observó un tag. Si no aparece ninguno durante un tiempo, la dirección de giro se invierte, evitando quedarse indefinidamente rotando hacia el mismo lado.
Este comportamiento sencillo mejora la exploración visual del entorno sin introducir una navegación demasiado compleja.
Problemas encontrados
Durante el desarrollo de la práctica han aparecido varios problemas importantes.
Inestabilidad de la pose
Una de las primeras dificultades fue que la pose estimada cambiaba bruscamente entre iteraciones, incluso con movimientos pequeños. Esto se debía principalmente al ruido de la detección y a la sensibilidad del cálculo geométrico. Se solucionó aplicando suavizado tanto en la posición como en la orientación.
Selección inadecuada de tags
Cuando había varios tags visibles, utilizar cualquiera de ellos producía estimaciones poco estables. La introducción de un criterio de selección basado en el tamaño aparente del tag y la continuidad respecto a la pose anterior permitió mejorar este aspecto.
Pérdida temporal de marcadores
En zonas con muebles, sillas o cambios de orientación, el robot dejaba de ver tags durante algunos instantes. Si en esos momentos no se utilizaba odometría, la estimación se volvía discontinua. El uso de la propagación incremental con odometría permitió mantener una pose razonable entre detecciones visuales.
Bloqueos cerca de obstáculos
Otro problema habitual fue que el robot podía quedarse orientado hacia una pared o una pata de silla, entrando en una situación en la que no avanzaba ni recuperaba tags. Para solucionarlo se incorporó la información del láser y una maniobra de giro orientada al lado más libre.
Resultados obtenidos
El sistema desarrollado permite localizar al robot de forma visual dentro del entorno utilizando AprilTags como referencia global. En los momentos en los que un marcador es visible, la pose estimada se aproxima de forma razonable a la posición real del robot, mejorando claramente la información proporcionada solo por la odometría.
El robot es capaz de:
- detectar tags visibles,
- estimar su pose en el mapa,
- corregir su orientación hacia el marcador,
- mantener una pose aproximada usando odometría cuando pierde visión,
- y reaccionar ante obstáculos cercanos para evitar bloqueos básicos.
La trayectoria del robot estimado en el mapa resulta coherente en la mayor parte de la ejecución, especialmente cuando la visibilidad de tags es buena y el entorno no está excesivamente obstruido.
Limitaciones
Aunque el comportamiento general es funcional, la práctica presenta algunas limitaciones.
- La localización depende de la visibilidad de los tags. Si el entorno oculta los marcadores durante demasiado tiempo, la precisión se degrada.
- La evitación de obstáculos es simple y reactiva, por lo que no siempre resuelve de manera óptima situaciones complejas con muebles o zonas estrechas.
- La odometría solo sirve como apoyo temporal, ya que el error acumulado crece con el tiempo.
- El control visual es básico y no incorpora una estrategia de navegación global ni una planificación de trayectoria.
Estas limitaciones son razonables dado el objetivo de la práctica, centrado en la localización visual y no en la navegación autónoma completa.
Conclusiones
En esta práctica he implementado un sistema de localización visual basado en AprilTags capaz de estimar la pose global de un robot en un entorno simulado. Para ello he combinado técnicas de detección de marcadores, estimación geométrica de pose, transformaciones entre sistemas de referencia, suavizado temporal y apoyo mediante odometría.
Además, he incorporado una lógica de movimiento que permite al robot orientarse hacia los tags visibles y reaccionar de forma básica ante obstáculos cercanos, mejorando la robustez del sistema en escenarios con muebles y zonas parcialmente ocluidas.
En conjunto, la práctica me ha permitido trabajar de forma integrada conceptos de visión artificial, robótica móvil y representación espacial, mostrando cómo una referencia visual conocida en el entorno puede utilizarse para corregir y mejorar la localización del robot.
Posibles mejoras
Como trabajo futuro, algunas mejoras interesantes serían:
- combinar la estimación visual y la odometría mediante un filtro más avanzado,
- mejorar la estrategia de evasión de obstáculos,
- utilizar varios tags simultáneamente para una estimación más robusta,
- e incorporar una navegación más inteligente que permita explorar el entorno buscando marcadores de forma más eficiente.