El objetivo de esta práctica es estimar la posición y orientación (pose) de un robot móvil en un entorno 2D utilizando el método de balizas fijas, concretamente AprilTags, cuya posición y orientación en el mapa son conocidas.

El robot dispone de una cámara que detecta dichas balizas visuales y, a partir de su observación, se calcula la pose del robot respecto al sistema de referencia global (mundo). Para ello, se utilizan técnicas de visión por computador y transformaciones geométricas entre distintos sistemas de coordenadas.

En la visualización del ejercicio:

• El robot azul representa la posición obtenida por odometría, la cual incluye ruido acumulado.
• El robot rojo representa la posición estimada por el usuario mediante visión.

Para el funcionamiento del ejercicio, el robot sigue una estrategia de movimiento sencilla que le permite recorrer el mapa y orientarse correctamente:

• El robot avanza recto siempre que no haya obstáculos frente a él.
• Si encuentra un obstáculo, gira a la derecha si esta dirección está libre.
• Si tampoco puede girar a la derecha, gira a la izquierda.

Este comportamiento permite que el robot explore el entorno de forma continua y aumente la probabilidad de detectar balizas desde diferentes posiciones y orientaciones.

Para calcular la posición del robot se utiliza el método PnP (Perspective-n-Point), un problema clásico de visión por computador que permite estimar la posición y orientación de una cámara con respecto a un objeto conocido a partir de su proyección en la imagen.

En este ejercicio, el objeto conocido es un AprilTag, cuyo tamaño y geometría son conocidos. A partir de las esquinas detectadas del tag en la imagen y de los parámetros intrínsecos de la cámara, el algoritmo PnP calcula:

• Un vector de traslación (pose_t), que indica la posición del tag respecto a la cámara.
• Una matriz de rotación (pose_R), que indica la orientación del tag respecto a la cámara.

La librería pyapriltags utiliza internamente PnP para proporcionar directamente estas matrices cuando se detecta un tag.

A partir de esta información se construyen distintas matrices de transformación:

• Matriz cámara → tag: describe la pose del tag respecto a la cámara.
• Matriz tag → cámara: se obtiene invirtiendo la matriz anterior. Además, se realiza un cambio de ejes, ya que el sistema de coordenadas de la cámara no coincide con el del robot.
• Matriz mundo → tag: se construye a partir de la posición y orientación conocidas del tag en el mapa.
• Matriz cámara → robot: representa la posición fija de la cámara respecto al robot.

Finalmente, todas las transformaciones se encadenan para obtener la matriz mundo → robot:

M_mundo-robot = M_mundo-tag · M_tag-cámara · M_{cámara-robot}

Explicación del programa

Funciones del programa

orientar(target_yaw)

Gira el robot hasta alcanzar la orientación deseada target_yaw.
Calcula la diferencia angular entre la orientación actual y la deseada y ajusta la velocidad angular para girar. El bucle continúa hasta que el robot se aproxima a la orientación objetivo, momento en el que se detiene.

chech_move(i)

Decide la acción de movimiento del robot según los datos del sensor láser:

• Avanza recto si no hay obstáculos delante.
• Gira a la derecha si no puede avanzar recto.
• Gira a la izquierda si tampoco puede girar a la derecha.

calibration()

Calcula los parámetros intrínsecos de la cámara, necesarios para el algoritmo PnP.
Se determina la matriz de calibración a partir del tamaño de la imagen y la distancia focal, y se generan los coeficientes de distorsión, que en este caso se inicializan a cero.

Detección de AprilTags y PnP

El programa captura la imagen de la cámara, la convierte a escala de grises y detecta todos los AprilTags visibles.
Para cada tag, se obtiene su posición y orientación relativa a la cámara usando PnP. Se selecciona el tag más cercano al robot para minimizar errores y se construyen las matrices de transformación necesarias:

• Matriz cámara → tag: Pose del tag en la cámara.
• Matriz tag → cámara: Inversa de la anterior, adaptando los ejes a la referencia del robot.
• Matriz mundo → tag: Pose conocida del tag en el mundo.
• Matriz cámara → robot: Posición de la cámara respecto al robot.

Actualización de posición con odometría

Si no se detectan tags dentro de la distancia óptima, se utiliza la odometría para estimar el movimiento del robot desde la última posición visual confiable. Se calcula el desplazamiento en x, y y yaw y se suma a la última posición conocida para mantener la estimación de la pose.

Ciclo principal

El ciclo principal del programa realiza continuamente las siguientes acciones:

• Captura la imagen de la cámara y detecta AprilTags.
• Selecciona el tag más cercano y calcula la pose del robot usando PnP y matrices de transformación.
• Actualiza la pose estimada y la muestra en la interfaz gráfica.
• Si no se detectan tags, utiliza la odometría para estimar la posición.
• Decide el movimiento del robot mediante la función chech_move().
• Muestra la imagen procesada con los tags detectados y la pose estimada.