PRÁCTICA 3: AUTOLOCALIZACIÓN VISUAL BASADA EN MARCADORES

Introducción al problema

El algoritmo implementado combina visión artificial (con balizas AprilTag) y odometría para estimar la posición y orientación del robot en un entorno conocido. El siguiente esquema muestra de forma clara el problema al que se busca dar solución, para el que se tienen que tener en cuenta los sistemas de referencia del mundo, de la cámara y de las balizas. Además se ha de considerar el offset entre la cámara y el centro del robot, así como la diferencia de orientación entre las balizas y el sistema de referencia del mundo según el robot se desplaza.

En definitiva queremos llegar a tener la matriz de transformación RT_mc como el producto de RT_mb @ RT_bc, siendo RT_bc la inversa de RT_cb.

Pasos seguidos

En cada iteración (1) el robot captura una imagen, (2) la convierte a escala de grises, (3) detecta las balizas visibles usando el detector de AprilTags y (4) toma solo la primera detectada.
Para cada baliza detectada (1) se recogen las coordenadas 2D de las esquinas en la imagen, (2) se prepara la lista de coordenadas 3D conocidas de esas esquinas en el sistema solidario con la baliza y (3) se prepara también su ubicación en el marco del mundo, usando el archivo yaml.

Usando solvePnP se calcula la matriz de transformación RT_cb que da la pose de la cámara respecto a la baliza. Esto se convierte en una matriz homogénea 4x4 y se calcula su inversa RT_bc para poder transformar en la dirección opuesta.

Por otra parte se calcula la matriz RT_mb, que transforma de la baliza al marco del mundo, considerando la orientación (yaw) de la baliza en el mapa.
Además se monta una matriz fija RT_robot_cam que refleja el desplazamiento físico entre la cámara montada en el robot y el marco del robot. Esto se hace por que el sistema de referencia de la cámara tiene x apuntando a la derecha, y abajo y z adelante, mientras que en el marco del robot y apunta a la izquierda, z arriba y x delante. Además se tiene en cuenta que la cámara no está perfectamente ubicada en el centro del robot.   

Finalmente todas estas matrices dan lugar a una matriz final resultante que será la que nos lleve a estimar la posición del robot, haciendo:

RT_final = RT_mb @ RT_bc @ R_optcam @ RT_robot_cam

Estimación de la posición y orientación 

Combinación alterna de visión y odometría

Para actualizar la estimación de la posición y orientación del robot se toman caminos alternativos en función de si el robot ve o no la baliza y si la ve de cerca o no.

Si la baliza está suficientemente cerca, el algoritmo confía en la localización visual, extrayendo la posición (x, y) y orientación (yaw) del robot a partir de RT_final. Con esto actualiza la posición estimada global, que es la que se muestra en el visor con el robot que deja el rastro rojo tras él.

Si no hay balizas cercanas (o si directamente el robot no ve balizas en un determinado momento) se usa la odometría del robot (HAL.getOdom) para estimar el incremento de movimiento desde la última iteración. En este caso se actualiza la posición estimada sumando ese desplazamiento a la posición de la iteración anterior.

Cabe decir además que para determinar si el robot está o no suficientemente cerca primero he probado a poner un umbral de 6 metros de distancia, y después un umbral de 4 metros, para poder comprobar si está distancia afecta significativamente a la estimación con SolvePnP. A continuación se muestran dos videos demostrativos, uno para la distancia de 6 metros y otro para la distancia de 4.

Umbral de distancia = 6 :

Umbral de distancia = 4 : 

Se observa como las estimaciones por encima de los cuatro metros son poco precisas y varían mucho de una iteración a otra, lo que se refleja como saltos bruscos en la linea roja. Sin embargo, una vez la distancia del robot a las balizas es inferior a 4 metros la estimación es mucho más estable y continua (como se muestra en el video más cortito para este caso).