Sistema de posicionamiento interior mediante marcadores ArUco para una flota de robots móviles

Proyecto Fin de Carrera Ingeniería de Telecomunicación Formato de Publicación de la Escuela Técnica Superior de Ingeniería Autor: F. Javier Payán Somet Tutor: Juan José Murillo Fuentes Dep. Teoría de la Señal y Comunicaciones Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2013 Proyecto Fin de Carrera Grado en Ingeniería Electrónica, Robótica y Mecatrónica Sistema de posicionamiento interior mediante marcadores ArUco para una flota de robots móviles Autor: Benito Martínez Carral Tutores: Sergio Luis Toral Marín Samuel Yanes Luis Dpto. Ingeniería Electrónica Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2020 Proyecto Fin de Carrera Grado en Ingeniería Electrónica, Robótica y Mecatrónica Sistema de posicionamiento interior mediante marcadores ArUco para una flota de robots móviles Autor: Benito Martínez Carral Tutores: Sergio Luis Toral Marín Catedrático Samuel Yanes Luis Investigador Dpto. Ingeniería Electrónica Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2020 Proyecto Fin de Carrera: Sistema de posicionamiento interior mediante marcadores ArUco para una flota de robots móviles Autor: Benito Martínez Carral Tutores: Sergio Luis Toral Marín Samuel Yanes Luis El tribunal nombrado para juzgar el trabajo arriba indicado, compuesto por los siguientes profesores: Presidente: Vocal/es: Secretario: acuerdan otorgarle la calificación de: El Secretario del Tribunal Fecha: Agradecimientos En primer lugar me gustaría agradecerle a mi familia y amigos el apoyo incondicional que me han dado todo momento, han sido una parte fundamental de este camino. También me gustaría hacer una agradecimiento especial a mi amigo Alfonso Bermejo quien en un paseo por la montaña Palentina me ayudo a decantarme por esta carrera. Por último agradecer a todos los docentes que he tenido en mi etapa académica, todos me han aportado conocimientos , valores y consejos que recordaré siempre. Benito Martínez Carral Escuela Técnica Superior de Ingeniería de Sevilla. Sevilla, 2020 I Resumen En este proyecto se pretende diseñar un sistema de posicionamiento interior para una flota de robot móviles. Para ello se utilizará una cámara y una serie de códigos ArUco colocados en cada robot, y en una baliza fija a partir de la cual se estimará la posición de cada uno de ellos. Este proyecto se ha llevado acabao utilizando el lenguaje de programación python y su librería Open CV mayormente. III Capítulo 1: Introducción 1.1 Justificación El avance de la robótica ha llevado a que la presencia de robots en todos los campos y realizando múltiples tareas sea cada vez más habitual. Por ejemplo e pueden ver robots en cadenas montaje y paletizado , laboratorios o inspección de entornos peligrosos. Además una de las consecuencias de este hecho es el avance de la robótica colaborativa la cual ha permitido que que la automatización y la robótica sea más accesible para medianas y pequeñas empresas. En la figura 1.1 podemos ver una brazo robótico colaborativo de la empresa Universal Robots. Figura 1.1 Brazo robótico colaborativo UR5. En el caso de los robots móviles se debe a su capacidad para trabajar en cualquier entorno con gran eficiencia y precisión . El posicionamiento de dichos robots es un requerimiento fundamental a la hora de abordar cualquiera tarea en la que participen, la gran variedad de entornos ha llevado al desarrollo de multitud de técnicas y sistemas de posicionamiento. Concretamente en este proyecto se va a realizar un sistema de posicionamiento de robots móviles utilizando un sistema basado en cámaras y códigos ArUco orientado a entornos de interior. Este sistema de posicionamiento puede tener aplicaciones en entornos como talleres, almacenes o refinerías entre otros muchos. En la figura 1.2 se puede ver un sistema de posicionamiento en interiores utilizando códigos ArUco. 3 4Capítulo 1. Introducción Figura 1.2 Sistema de posicionamiento en interior basado en códigos ArUco. 1.2 Motivación del trabajo La motivación principal de este trabajo es controlar una flota de robots móviles en interior ,compuesta por robots móviles JetBot basados en la tarjeta Jetson Nano, una foto de dicho robots puede verse en la figura 1.3. Figura 1.3 Robot JetBot basado en Jetson Nano. Los sistemas de posicionamiento en interior tienen la desventaja de no contar con señal GPS, por tanto se ha optado por utilizar un sistema basado en cámaras y códigos ArUco, a su vez los sistemas de posicionamiento basado en cámaras cuentan con la desventaja de que la posición de dicha cámara con respecto al mundo es crítica, ya que si se mueve la cámara pero no el objeto a posicionar, las coordenadas de dicho objeto cambiarían cuando el objeto no se ha movido. El objetivo de proyecto es realizar un sistema de posicionamiento en interior donde la posición de la cámara no es crítica, ya que las coordenadas de los objetos a posicionar se transforman a coordenadas en el sistema de referencia de una baliza fija. 1.3 Objetivo 5 1.3 Objetivo El objetivo del proyecto es la obtención de la posición de diversos robots móviles pertenecientes a una flota con respecto a una baliza fija, con el objetivo de ser utilizada posteriormente para el guiado de dichos robots. Para realizar el sistema de posicionamiento se va a hacer uso de una cámara y de la biblioteca ArUco, la cual cuenta con una gran variedad de marcadores, de los cuales uno se colocará en un baliza fija, y otros se colocarán de forma visible en los diferentes robots de la flota. A continuación de desglosan los objetivos del proyecto. • Obtener los parámetros intrínsecos de la cámara a a utilizar mediante la calibración, con el objetivo de corregir distorsiones en las tareas posteriores. • Obtener la posición en 3D y orientación de los diferentes marcadores con respecto a una baliza fija. • Filtrar estos resultados mediante un Filtro de Kalman. Este filtro de uno de los más comunes en robótica móvil, fácilmente implementable y soluciona en parte el problema de pérdida de medidas. En el apartado 3.5 se explica con detalle el funcionamiento e implementación de este filtro. 1.4 Estructura del documento El desarrollo del proyecto se ha llevado a cabo dividiendo este en cuatro partes diferenciadas y una quinta parte como funcionalidad completa. •1- Captura En esta parte se lleva a cabo un programa que se encargará de tomar las fotos necesarias para la posterior calibración de la cámara. •2- Calibración En esta parte se lleva a cabo la calibración de la cámara, esto consiste en la obtención de sus los parámetros intrínsecos , los cuales serán necesarios para la posterior detección. •3- Detección En esta parte se obtiene la posición y orientación en 3D de cada robot perteneciente a la flota con respecto a la baliza. •4- Filtrado En esta parte se realiza el filtrado de los resultados obtenidos en a detección con el objetivo de suavizarlos y solucionar el problema de perdida de medidas. Capítulo 2: Estado del arte En primer lugar se muestran varios trabajos en los cuales se ha utilizado la librería ArUco para tareas de detección posicionamiento y guiado. •Robot Localization using ArUco En este primer proyecto se realiza un sistema de posicionamiento para robots en interior ,en este caso la cámara esta embarcada en el robot móvil, y los marcadores están colocados por el entorno.(Figura 2.1) Figura 2.1 Sistema de posicionamiento en interior basado en códigos ArUco. •Navegación en interiores usando visión basada localización y realidad aumentada Realiza un estudio sobre la detección de los marcadores ArUco y se utilizan como una herramienta de navegación.(Figura 2.2) 7 8Capítulo 2. Estado del arte Figura 2.2 Marcadores ArUco como herramientas para la navegación. •Posicionamiento Visual basado en ArUco . En este proyecto se colocan multitud de marcadores en un laboratorio, la distribución de marcadores y la posterior detección de los mismos permite realizar un mapeo del entorno que puede ser utilizado en tareas de guiado.2.3 Figura 2.3 Posicionamiento Visual basado en ArUco. 2.1 Sistemas de posicionamiento 9 2.1 Sistemas de posicionamiento Para que cualquier tarea basada en robots móviles funcione de forma segura y efectiva es necesario un sistema de localización preciso y robusto, pudiéndose diferenciar dos tipos de localización. • Posicionamiento Interior: es la tecnología que permite localizar elementos en interiores como almacenes , laboratorios o refinerías. • Posicionamiento Exterior : los sistemas de posicionamiento exterior tratan de determinar la posición de un elemento fuera de los edificios. A continuación se comentan diferentes técnicas de posicionamiento de robots, en interior y exterior. •Odometría La odometría consiste en estimar la posición y orientación del robot basándose en el movimiento de sus ruedas motrices o mediante mediciones del sistema motriz propio, es una técnica de posicionamiento relativo que utiliza sensores de tipo encoder en cada rueda motriz, válida tanto en posicionamiento interior como en exterior. Es un sistema barato y sencillo de implantar,pero tiene como desventaja la acumulación del error integral debido inexactitudes o errores como por ejemplo los errores sistemáticos como pueden ser el radio de las ruedas no constante durante el movimiento o la resolución del encoder, o errores no sistemáticos como el patinaje y rozamiento de las ruedas. Los enconders son sensores que transforman el movimiento en pulsos eléctricos, existen dos tipos principales, incrementales los cuales devuelven un número de pulsos por revolución y absolutos los cuales proporcionan una posición absoluta del encoder sin necesidad de puesta a cero. En la figura 2.4 se puede observar un encoder rotativo incremental . Figura 2.4 Encoder rotativo incremental E40S series. Como ejemplo se muestra la configuración de un robot de tipo diferencial. 10 Capítulo 2. Estado del arte vi vd=Rwi wd=⇒Variables de actuación p=wi wd v ˙ ϕ=vd+vi 2 vd−vi b=R 2 R 2 −R b R bwi wd Figura 2.5 Configuración Robot Diferencial. •Sistemas de navegación Inercial Estos sistemas basan su funcionamiento en la medición continua de la aceleración en los tres ejes mediante un acelerómetro y la orientación mediante un giróscopo,este sistema tiene como desventaja la acumulación de error en el tiempo debido a la falta de referencias externas. Es válido tanto en posicionamiento interior como exterior. •Sistema basado en sensores de proximidad Los sensores de proximidad miden la distancia relativa del sensor a los diferentes objetos de su entorno, la colocación de este tipo de sensores en los robots permite conocer su posición en dicho entorno,utilizando normalmente balizas o tags como puntos de referencia. Existen tres tipos básicos de sensores para este tipo de tareas , sensores de tiempo de vuelo, medidas de desplazamiento de fase y modulación en frecuencia. El más común es el de tiempo de vuelo, concretamente sensores ultrasónicos (figura 2.6). Figura 2.6 Módulo Ultrasonidos HC-SR04. Este tipo de técnicas de posicionamiento son técnicas SLAM en el cual el robot construye un mapa del entorno a la vez que estima su posición basándose en observaciones visuales. 2.2 Códigos ArUco 11 •Sistema de Posicionamiento Global (GPS) Este sistema permite conocer la posición de un objeto en exterior realizando la triangulación de las señales recibidas por diferentes satélites que orbitan la tierra. Son necesarios al menos 4 satélites para conocer la posición global del objeto, con una precisión media que puede llegar hasta 20m, por lo que en la mayoría de aplicaciones es necesario integrarlo con otros sensores o mejorar su precisión mediante el uso del GPS diferencial o GPS asistido. •Sistema basado en cámaras Estos sistemas de posicionamiento combinan las imágenes tomadas por varias cámaras fijas, para determinar la posición del robot. Este tipo de sistema es sensible a los cambios de iluminación, condiciones o ángulo de observación que pueden causar que el sistema falle. 2.2 Códigos ArUco La biblioteca AruCo fué desarrollada por Rafael Muñoz y Sergio Garrido, dos investigadores de la Universidad de Córdoba. Esta librería se basa en unos marcadores fiduciales con features cuadrados binarios (los marcadores fiduciales son objetos colocados en el campo de visión de un sistema de imágen con la finalidad de utilizarse como puntos de referencia), denominados códigos ArUco, en este proyecto se va a tratar con este tipo de marcadores utilizando la librería OpenCV, la cual cuenta con un módulo dedicado a este tipo de códigos ArUco. Su principal característica es que su detección es robusta rápida y simple. A continuación podemos ver un ejemplo de código AruCo. Figura 2.7 Ejemplo de código ArUco. Como se puede observar en la imagen 2.7 el código ArUco es un marcador cuadrado, rodeado por un filo ancho negro que rodea una matriz binaria interna. El borde negro permite su rápida detección, y la codificación binaria permite determinar su identificador. El tamaño del marcador determina el tamaño de la matriz interna. Por ejemplo un marcador de tamaño 4x4 esta compuesto por 16 bits. Estos códigos pueden ser detectados aunque estén rotados, sin embargo el proceso necesita identificar cada esquina para obtener la orientación. Los diccionarios de estos códigos tienen dos características principales . 18 Capítulo 3. Sistema de posicionamiento basado en códigos ArUco El sensor tiene MxN píxeles de tamaño hxw como se muestra en la siguiente imágen. Figura 3.5 Tamaño en píxeles del sensor. Se define la dimensión efectiva del pixel horizontal y vertical de la siguiente forma. ρx=w/N(m/pix) ρy=h/M(m/pix) También hay que tener en cuenta el desplazamiento del orgien de coordenadas del sensor con respecto al sistema de coordenadas de la cámara. Se define por lo tanto lo siguiente. Punto principal (pix) −→ (uo,vo) u=x ρx+uo v=y ρy+vo   u0 v0 w0 =  1/ρx0uo 0 1/ρxvo 0 0 1  ·K·CTW·W˜ P   u0 v0 w0 =A·CRW|CtW·W˜ P 3.2 Captura 19 u=u0 w0 v=v0 w0 Finalmente se definen : •Parámetros intrínsecos. Son parámetros propios de la cámara y de la óptica. Matriz de parámetros intrínsecos−→   f/ρx0uo 0f/ρxvo 0 0 1  =  fx0uo 0fyvo 0 0 1   –fx=f/ρx. Longitud efectiva horizontal [pix] –fy=f/ρy. Longitud efectiva vertical [pix] –(uo,vo). Coordenadas del punto principal [pix] –Parámetros de distorsión. No incluidos en la matriz A. •Parámetros extrínsecos . Son parámetros propios de la disposición relativa entre el objeto y la cámara. Matriz de parámetros extrínsecos −→ CRW|CtW –CRW. Orientación relativa. –CtW. Desplazamiento relativo. 3.2 Captura Como primer paso en el desarrollo del proyecto era necesario la realización de un programa que capturara un número de fotos. El objetivo es fotografiar el patrón mostrado en la Figura 3.6, necesario para el posterior calibrado de la cámara. 20 Capítulo 3. Sistema de posicionamiento basado en códigos ArUco Figura 3.6 Patrón necesario para la calibración de la cámara. Este primer programa recibe como argumentos los siguientes parámetros. •–imageNumber Número de imágenes a capturar. •–savePath Dirección en la cual se almacenarán las imágenes. •–imagePrefix Prefijo con el que se guardarán las imágenes. •–timeBetweenCaptures Tiempo entre captura. Cuando se lleva a cabo la ejecución de este programa se abrirá nuestra cámara en una nueva ventana y aparecerá una cuenta atrás y el número de imágenes tomadas hata el momento. Un ejemplo de ejecución de este programa se puede ver en la siguiente Figura 3.7. 3.3 Calibración 21 Figura 3.7 Programa de captura. 3.3 Calibración El objetivo del programa de calibración es la obtención de los parámetros intrínsecos de la cámara los cuales permiten posteriormente corregir la distorsión en la detección de las posiciones de los diferentes códigos. Para llevar a cabo la calibración se necesitan algunas fotos de un patrón bien definido como por ejemplo un tablero de ajedrez como el que se mostró anteriormente. Se buscan puntos específicos cuyas posiciones relativas son conocidas, como por ejemplo las esquinas del tablero de ajedrez. Conocidas las coordenadas de estos puntos en el espacio del mundo real y las coordenadas de la imágen, se pueden resolver los coeficientes de distorsión. Para una correcta calibración serán necesarias al menos 10 fotos del patrón, con diferentes orientaciones y posiciones en cada una de ellas. Figura 3.8 Ejemplo de imagenes para la calibración. El siguiente programa recibe como argumento lo siguiente: •–imageDir Ruta de las imagénes de calibración. •–imageFormat Formato de las imágenes (png/jpg). 22 Capítulo 3. Sistema de posicionamiento basado en códigos ArUco •–prefix Prefijo de las imágenes de calibración. •–squareSize Tamaño de los cuadrados del tablero de ajedrez (cm). •–width Número de esquinas negras en contacto en el ancho del tablero. ( Figura 3.9.) •–height Número de esquinas negras en contacto en el alto del tablero. ( Figura 3.9 .) •–saveFile Nombre del archivo .yml donde se almacenarán los parámetros de calibración. Figura 3.9 Ejemplo de los parámetros Width yHeight. El código en primer lugar a partir de los argumentos de entrada width,height ,squareSize, genera las coordenadas en 3D de los diferentes puntos del patrón, suponiendo Z = 0 como se puede ver en la Figura 3.10. objp = np.zeros((height∗width, 3), np.float32) objp[:, :2] = np.mgrid[0:width, 0:height].T.reshape(−1, 2) objp = objp ∗square_size 3.3 Calibración 23 −5 0 5 10 15 20 25 30 35 −5 0 5 10 15 20 25 30 35 Figura 3.10 Coordenadas 3D de los puntos del patrón. Posteriormente recorre las diferentes imágenes de calibración mediante la librería glob y detecta los puntos del patrón con la función cv2.findChessboardCorners() . Esta función recibe los siguientes argumentos. •gray Imágen del patrón pasada a escala de grises. •(width,height) Número de esquinas negras en contacto en el ancho y alto del tablero. El resultado de ejecutar esta función puede apreciarse en la Figura 3.12 ret, corners = cv2.findChessboardCorners(gray, (width, height), None) Figura 3.11 Resultado de la función cv2.findChessboardCorners() . Mediante la función cornerSubPix() afina la posición de las ubicaciones de las esquinas utilizando el gradiente. corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (−1, −1), criteria) Esta función recibe los siguientes argumentos. 24 Capítulo 3. Sistema de posicionamiento basado en códigos ArUco •gray Imágen del patrón pasada a escala de grises. •Corners Las coordenadas de las esquinas a afinar. El resto de argumentos que recibe esta función son parámetros para ajustar el algoritmo. •(11,11) La mitad de la longitud lateral de la ventana de busqueda. •(-1,-1) Indica que no hay zona cero en la búsqueda. •Criteria Criterios para la terminación del proceso iterativo de refinamiento de esquinas. En este caso los criterios del proceso iterativo son los siguientes. criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) Esto quiere decir que el proceso iterativo acabará cuando llegue a un número máximo de iteraciones (cv2.TERM_CRITERIA_MAX_ITER), o cuando la posición de la esquina se mueve menos que cv2.TERM_CRITERIA_EPS en alguna iteración. Figura 3.12 Gradiente en las diferentes direcciones. Obtiene los parámetros intrínsecos mediante la función cv2.calibrateCamera(). ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray. shape[::−1], None, None) Esta función recibe los siguientes argumentos. •objpoints Coordenadas en 3D de los puntos del tablero de ajedrez. 3.4 Detección 25 •imgpoints Coordenadas en 2D de los puntos del tablero en la imágen. •gray.shape[::-1] Tamaño de la imágen. Por último se almacenan los parámetros en un archivo .yml mediante la función sa- ve_coefficients(). def save_coefficients(mtx, dist, path): cv_file = cv2.FileStorage(path, cv2.FILE_STORAGE_WRITE) cv_file.write("K", mtx) cv_file.write("D", dist) cv_file.release() A continuación en la Figura 3.13 podemos ver el archivo generado por la función sa- ve_coefficients(). Figura 3.13 Archivo .yml generado por la función save_coefficients(). 3.4 Detección En esta parte del proyecto se pretende obtener la posición en 3D de los diferentes marcadores ArUco con respecto a la baliza. En primer lugar se cargan los parámetros intrínsecos de la cámara obtenidos anteriormente mediante la función loadCoefficients(). def loadCoefficients(path): 26 Capítulo 3. Sistema de posicionamiento basado en códigos ArUco cv_file = cv2.FileStorage(path, cv2.FILE_STORAGE_READ) cameraMatrix = cv_file.getNode("K").mat() distMatrix = cv_file.getNode("D").mat() cv_file.release() return [cameraMatrix, distMatrix] cameraMatrix,distMatrix = f.loadCoefficients("calibrationParameters.yaml"); Se carga uno de los diccionarios precargados en la libraría Opencv correspondiente con los marcadores que se van a utilizar, y los parámetros para el proceso de detección de los marcadores. arucoDict = aruco.Dictionary_get(f.aruco.DICT_5X5_50) arucoParameters = aruco.DetectorParameters_create() El código a continuación sigue los siguientes pasos de corma cíclica. •Detección de marcadores •Si se ha detectado al baliza y algún marcador perteneciente a la flota. –Obtención de la posición 3D de los marcadores. –Paso de coordenadas al sistema de referencia de la baliza. La detección de los marcadores se realiza mediante la función aruco.detectMarkers() , la cual devuelve como resultado las coordenadas en píxeles de las esquinas del marcador en la imágen, y los identificadores de los marcadores detectados, recibiendo como parámetros los siguientes. •gray Frame capturado pasado a escala de grises. •arucoDict Diccionario de marcadores ArUco previamente cargado. •arucoParameters Parámetros para el proceso de detección previamente cargados. •cameraMatrix Matriz de parámetros intrínsecos. •distMatrix Parámetros de distorsión. detectedCorners, detectedIDS, rejectedImgPoints = f.aruco.detectMarkers(gray, arucoDict, parameters=arucoParameters,cameraMatrix=cameraMatrix,distCoeff= distMatrix) A continuación en la Figura 3.14 se puede ver el resultado de ejecutar esta función junto con drawDetectedMarkers() para dibujar estos resultados en la imagen. 3.4 Detección 27 Figura 3.14 Resultados de la función aruco.detectMarkers(). En el caso de que entre los frames detectados se encuentre la baliza y además algún marcador perteneciente a la flota, se obtiene la posición 3D de los diferentes marcadores mediante la función aruco.estimatePoseSingleMarkers() . Esta función devolverá el vector de traslación y la matriz de rotación del marcador indicado con respecto a la cámara (C_rvec_M, C_tvec_M), recibiendo como parámetro los siguientes. •detectedCorner[idPos] Esquinas del marcador. •markerLength Longitud del marcador. •cameraMatrix Matriz de parámetros intrínsecos. •distMatrix Parámetros de distorsión. C_rvec_M, C_tvec_M, markerPoints = f.aruco.estimatePoseSingleMarkers( detectedCorners[idPos], markerLength, cameraMatrix,distMatrix) Para comprobar estos resultados se pintan mediante la función aruco.drawAxis() los ejes en 3D de los macadores, como podemos ver a continuación en la Figura 3.15. Figura 3.15 Resultados de la función aruco.estimatePoseSingleMarkers(). 34 Capítulo 3. Sistema de posicionamiento basado en códigos ArUco Siendo Tla tasa del filtro. Para la implementación del filtro en Python se ha creado la siguiente función. def KalmanFilter(Am,Qm,Cm,Rm,X_k_1,P_k_1,Y_k): #Prediction X_k_p = Am∗X_k_1 P_k_p = Am ∗P_k_1 ∗Am.T + Qm # if(np.isnan(Y_k).any()): if(np.isnan(np.array(Y_k, dtype=np.float64)).any()): X_k=X_k_p P_k=P_k_p else: #Correction S=(Cm ∗P_k_p ∗Cm.T) + Rm L_k = (P_k_p ∗Cm.T) ∗np.linalg.inv(S) X_k=X_k_p + (L_k ∗(Y_k −(Cm ∗X_k_p))) P_k=P_k_p −L_k ∗Cm ∗P_k_p return X_k,P_k Se muestra a continuación un experimento realizado con el fin de obtener la covarianza del error para cada parámetro y el error máximo aproximado en la detección. Para esto se han realizado cuatro experimentos en los que se han dejado dos marcadores en una posición fija diferente con respecto a la baliza en cada uno de ellos, a continuación se muestra como ejemplo las gráficas obtenidas para el marcador con Identificador 1 en uno de los experimentos. Figura 3.18 Posición marcador 1. 3.5 Filtrado 35 Figura 3.19 Ángulos marcador 1 (Grados). Los errores máximos con los códigos estáticos obtenidos son los siguientes. error máximo en x=1cm error máximo en y=1cm error máximo en z=2cm error máximo en alpha=5º error máximo en beta=5º error máximo en gamma=1º Las covarianzas del error obtenidas son las siguientes. rx =0.0001 ry =0.0001 rz =0.02 ralpha =1.5 rbeta =1.5 rgamma =0.5 Capítulo 4: Resultados Experimentales A continuación se van a realizar una serie de experimentos con el objetivo de sacar resultados y conclusiones del sistema de posicionamiento. Todos estos experimentos se han realizado utilizando una webcam de 1.3 megapixeles y una resolución del sensor de 1280x1024. Como primer resultado experimental se tomarán tres fotos en las que apareceran los marcadores 1 y 2 orientados de diferente forma con respecto a la baliza , y se mostrarán los resultados obtenidos con el objetivo de comprobar la precisión de este sistema en el cálculo de orientaciones. En la primera imagen 4.1 se puede observar que ambos códigos mantienen la misma orientación que la baliza, por tanto los ángulos de giro que se muestran como resultados son aproximadamente cero, en cambio en la imagen 4.2 en marcador 1 ( situado en la parte inferior derecha de la imagen) esta girado 90 º en el eje z ( gamma), y en 4.3 es el marcador 2 el que exsta girado 90ºen este mismo eje. Figura 4.1 Resultados rotación 1. 37 38 Capítulo 4. Resultados Experimentales Figura 4.2 Resultados rotación 2. Figura 4.3 Resultados rotación 3. Para comprobar la precisión en el cálculo de las posiciones se repetirá el experimento anterior pero mostrando además como resultados las posiciones del marcador 1 con respecto a la baliza. en este caso se colocarán un metro y una regla para fiabilizar el experimento. En la imagen 4.4 el marcador 1 esta desplazado aproximadamente 20 centímetros en el eje x, en la siguiente imagen (4.5) además esta trasladado aproximadamente 15 centímetros en el eje y. En la última imagen el marcador esta subido en una caja de unos 18cm de altura en el eje z. Figura 4.4 Resultados traslación 1. Figura 4.5 Resultados traslación 2. 39 Figura 4.6 Resultados traslación 3. A continuación se van a realizar experimentos para comprobar la eficacia en la detección de los códigos, para ello tapar diferentes partes de uno de los marcadores con monedas, se van a detectar con diferentes condiciones de iluminación, o con diferentes orientaciones. •Marcador destapado. En primer lugar se muestra una imagen del marcador destapado pudiendose comprobar que lo detecta perfectamente. Figura 4.7 Marcador Destapado. •Marcador tapado con una monenda. En este segundo caso se va a tapar el marcador con una moneda en diferentes posiciones para ver su comportamiento. Figura 4.8 Marcador tapado con una moneda. Como se puede observar se ha tapado el código en el centro y en una esquina y lo sigue detectando. 40 Capítulo 4. Resultados Experimentales •Marcador tapado con dos monedas. Figura 4.9 Marcador tapado con dos moneda. En este caso se ha podido observar que al tapar el centro con dos monedas el código lo sigue detectando perfectamente. En cambio, al tapar las esquinas exteriores se ha observado que dependiendo del porcentaje del filo blanco exterior que se tape, se detecta o no, como se puede observar en la figura 4.9. •Marcador tapado con tres monedas. Se ha probado a tapar diferentes partes del código con las tres monedas como se muestra en la imagen 4.10. Se ha podido observar que no depende tanto del porcentaje de marcador que se tape, como del número de esquinas del marcador que se tapen. También se ha observado que ahora le cuesta más detectarlo y aparecen parpadeos debido a las condiciones de iluminación. En este caso al tapar tres esquinas se sigue detectando en función del porcentaje del filo blanco exterior que sea tapado como en el caso anterior, y se ve más afectado por las condiciones de iluminación. Figura 4.10 Marcador tapado con tres monedas. 41 •Marcador tapado con cuatro monedas. En este caso se observa lo mismo que en el anterior: el marcador se detecta peor si las partes tapadas son esquinas, y no le afecta tanto que se tape una parte lisa como se puede observar en la imagen 4.11. Figura 4.11 Marcador tapado con cuatro moneda. •Marcador con diferentes condiciones de iluminación. En el caso de la detección de marcadores con diferentes condiciones de iluminación se ha observado que cuando el marcador esta totalmente destapado, la detección no se ve tan afectada por dichas condiciones , en cambio si tiene partes tapadas es mucho más sensible a diferentes condiciones de iluminación, sombras y reflejos. En la imagen 4.12 se puede observar el marcador totalmente destapado con 4 condiciones de iluminación diferentes y lo detecta sin problemas. En la imagen 4.13 se puede observar como el mismo código tapado de igual forma, es detectado o no en función de las condiciones de iluminación. Figura 4.12 Marcador destapado con diferentes condiciones de iluminación. 42 Capítulo 4. Resultados Experimentales Figura 4.13 Marcador tapado con monedas con diferentes condiciones de iluminación. •Marcador con diferentes orientaciones. A continuación se muestra la detección del código con diferentes orientaciones, nuevamente se observa que en condiciones de orientación desfavorables el factor clave siguen siendo la luz , los reflejos y las sombras. 43 Figura 4.14 Marcador con diferentes orientaciones. En el siguiente experimento se van a tomar varias fotos de dos códigos con respecto a la baliza , variando la posición de la cámara, con esto se comprobará el correcto funcionamiento del sistema de posicionamiento con respecto a la baliza, ya que si todo funciona correctamente la posición y orientación de los códigos no debe variar en las diferentes imágenes salvando el error de detección. Figura 4.15 Resultados movimiento de cámara 1. 50 Capítulo 4. Resultados Experimentales Figura 4.29 Posiciones del marcador 2 filtrados en la trayectoria 4.24 con un valor de q bajo. Figura 4.30 Ángulos del marcador 2 filtrados en la trayectoria 4.24 con un valor de qbajo. A continuación se muestran los resultados de otra trayectoria en las que se mueven los marcadores con identificadores 1 y 2 cruzandose entre sí. 51 Figura 4.31 Posiciones del marcador 1 filtrados en la trayectoria anterior. Figura 4.32 Ángulos del marcador 1 filtrados en la nueva trayectoria. 52 Capítulo 4. Resultados Experimentales Figura 4.33 Posiciones del marcador 2 filtrados en la nueva trayectoria. Figura 4.34 Ángulos del marcador 2 filtrados en la trayectoria anterior. Para solucionar el problema de la pérdida de muestras se ha programado el filtro de forma que si no se tiene medida en un instante de tiempo se lleve a cabo solo la fase de predicción. El modelo es un movimiento rectilíneo uniforme, por lo tanto si en el tramo en el que se pierden medidas el marcador no cambia de dirección el resultado será bastante bueno, en cambio si el marcador cambia de dirección el filtro tendrá que recuperarse y aparecerá un escalón. Se van realizar dos experimentos con la trayectoria 4.35 en los cuales se ha tapado el código en un tramo rectilíneo y en un tramo donde el marcador cambia de dirección. 53 Figura 4.35 Trayectoria utilizada para los experimento de pérdidas de medidas. Para estos experimentos se van a utilizar solo las medidas de posición en el eje X del marcador con identificador 1 con el objetivo de que se vea más claro. En primer lugar se muestra la trayectoria filtrada donde se pierden medidas durante un tramo rectilíneo. Figura 4.36 Pérdida de medidas en un tramo rectilíneo. Se puede observar que el filtro reacciona bien ya que el marcador no cambia de dirección. En la figura 4.37 se muestra la trayectoria filtrada donde se pierden medidas en un tramo donde el marcador cambia de dirección. 54 Capítulo 4. Resultados Experimentales Figura 4.37 Pérdida de medidas en un tramo donde se produce un cambio de dirección. Se puede observar como el filtro tiene que recuperarse cuando vuelve a tener medidas.En los dos primeros casos de 1 y 1.5 metros, situados en la parte superior izquierda y derecha de la imagen respectivamente, los códigos han sido detectados perfectamente, en cambio en los dos casos siguientes de 2 y 2.5 metros, situados en la parte inferior izquierda y derecha de la imagen, han aparecido parpadeos en algunos de los códigos siendo estos mayores en el último caso. Como experimento final se ha planteado un escenario lo más cercano a un caso real posible, en este la cámara está colocada a unos dos metros de altura, con una cierta inclinación , y se ha montado un marcador en un coche teledirigido, simulando el movimiento de un robot móvil de la flota como se muestra en la figura 4.38. Se ha utilizado una cámara de mayor resolución, ya que es un factor crítico a la hora de detectar los códigos cuando la cámara está lejos, tiene cierta orientación y las condiciones de iluminación no son las más apropiadas. Concretamente se ha utilizado la cámara Logitech C930e Webcam Full HD , la cual tiene una resolución de 1280 x 720. 55 Figura 4.38 Escenario experimento final. En el experimento el robot el cual lleva embarcado el marcador 1, da una vuelta a la baliza, a continuación se muestran una serie de frames del vídeo numerados para posteriormente relacionarlos con las gráficas resultantes.(Figura 4.39). Figura 4.39 Frames capturados del experimento final. En la figura 4.40 se muestran los resultados del experimento. 56 Capítulo 4. Resultados Experimentales Figura 4.40 Resultados de posición del experimento final. • El marcador parte de una posición en la que tiene coordenadas xeynegativas girado 90 grados en gamma tal como se puede comprobar en el frame 1. • Manteniendo la misma coordenada xaproximadamente aumenta la yhasta el frame 3, pasando por el frame 2 en el cual tiene una coordenada ynula, ya que pasa por el origen. En este trayecto el robot empieza el giro y su ángulo gamma tiende a cero . • A continuación, aumenta la coordenada en xmanteniendo la yhasta el frame 4. En este paso el robot mantiene el ángulo gamma a cero, hasta llegar al frame 4 donde empieza de nuevo el giro y el ángulo empieza a disminuir. • A partir del frame 4 hace lo opuesto a lo hecho hasta ahora para volver a una posición cercana a la de partida.Mantiene la coordenada x,y baja la yhasta el frame 5,y para finalizar mantiene la ymientras disminuye la xhasta el 6 que es el punto final. En este paso el ángulo sigue disminuyendo. Puede observarse la suma acumulativa del ángulo entre el segundo 25 y 25 en el ángulo gamma . Capítulo 5: Conclusiones y futuros trabajos En el desarrollo del proyecto se han sacado las siguientes conclusiones. • La detección de los códigos se ve afectada por el número de esquinas que se tapen, más que por la proporción de código tapado. • La detección de los códigos esta muy condicionada por la iluminación y sombras, siendo esto clave para el código sea detectado con ciertos ángulos y/o con partes del código tapas. • Los detección de los códigos en movimiento se ve muy afectada por las aceleraciones, en cambio si los códigos se mueven a velocidad constante se detectan bien a una velocidad considerablemente alta. • En el caso de que se pierdan medidas el filtro reaccionará bien si el marcador no cambia de dirección, en cambio si esto sucede el filtro se recuperará cuando vuelva a obtenerlas. •La resolución de la cámara es un factor crítico en la detección de los códigos . Como futuros trabajos o ampliaciones se propone lo siguiente. • Añadir más balizas y sacar las pociones de los marcadores por triangulación, con esto se reduciría el error en la posición. • Añadir más sensores como por ejemplo una IMU. Integrando este sensor se obtendría una segunda medida de la orientación del marcador lo que llevaría a reducir errores en orientación. • Añadir a cada robot móvil de la flota varios marcadores colocados en diferentes partes. Con esto se ganaría más probabilidad de que el sistema detecte uno de los códigos y obtenga la posición del robot móvil. 57 58 Capítulo 5. Conclusiones y futuros trabajos • Los colores blancos y negros se ven muy afectados por los brillos y sombras, como futuros trabajos se propone probar el sistema de posicionamiento utilizando otros códigos fiduciales de diferentes colores. • Se propone probar el sistema de posicionamiento con otro tipo de filtro o con otra variación del Filtro de Kalman. Índice de Figuras 1.1 Brazo robótico colaborativo UR5 3 1.2 Sistema de posicionamiento en interior basado en códigos ArUco 4 1.3 Robot JetBot basado en Jetson Nano 4 2.1 Sistema de posicionamiento en interior basado en códigos ArUco 7 2.2 Marcadores ArUco como herramientas para la navegación 8 2.3 Posicionamiento Visual basado en ArUco 8 2.4 Encoder rotativo incremental E40S series 9 2.5 Configuración Robot Diferencial 10 2.6 Módulo Ultrasonidos HC-SR04 10 2.7 Ejemplo de código ArUco 11 2.8 Marcador "0" para los diccionarios 4x4, 5x5, 6x6, y 7x7 12 3.1 Modelo proyeccón perspectiva 14 3.2 Tipos de distorsión más comunes 15 3.3 Componentes tangencial y radial de la distorsión 16 3.4 Modelo con relación de coordenadas 17 3.5 Tamaño en píxeles del sensor 18 3.6 Patrón necesario para la calibración de la cámara 20 3.7 Programa de captura 21 3.8 Ejemplo de imagenes para la calibración 21 3.9 Ejemplo de los parámetros Width y Height 22 3.10 Coordenadas 3D de los puntos del patrón 23 3.11 Resultado de la función cv2.findChessboardCorners() 23 3.12 Gradiente en las diferentes direcciones 24 3.13 Archivo .yml generado por la función save_coefficients() 25 3.14 Resultados de la función aruco.detectMarkers() 27 3.15 Resultados de la función aruco.estimatePoseSingleMarkers() 27 3.16 Suma vectorial de comprobación 29 3.17 Ciclo Filtro de Kalman 32 3.18 Posición marcador 1 34 3.19 Ángulos marcador 1 (Grados) 35 59