Revista Colombiana de Física, Vol. 43, No.1 de 2011. Ubicación De Coordenadas 3D Para Un Brazo Robótico De 3 Grados De Libertad Mediante Técnicas De Visión Por Computador Y Control Digital 3D Coordinate Location For A Robotic Arm With 3 Degrees Of Freedom Using Computer Vision And Digital Control Techniques D. Revelo a, D. Usama a, D. Bravo * b. a Grupo de Óptica y Láser, Universidad del Cauca. Popayán, Colombia. b Grupo de Diseño simulación y control, Universidad del Cauca. Popayán, Colombia. Recibido 10.04.10; Aceptado 19.12.10; Publicado en línea 24.04.11. Resumen En este trabajo se presenta el control automático de posición de un brazo mecánico de 3 grados de libertad. Para esto se utilizaron técnicas de visión por computador que determinan las coordenadas tridimensionales del brazo mecánico y un controlador digital RST que le permite al dispositivo ubicarse, en tiempo real, en las coordenadas enviadas a través de un PC de forma rápida y con error nulo en estado estable. El brazo está conformado por 3 servomotores cuyo comportamiento se registra por dos cámaras webcam y analiza con un software especializado en procesamiento de video; se advierte la ventaja de utilizar las cámaras como sensores: así se logra medir las variables de posición con precisión sin adicionar carga al sistema. De esta forma se unen técnicas de procesamiento digital de video y algoritmos de control digital para desarrollar un prototipo mecánico de posicionamiento tridimensional. Palabras clave: Visión de Máquina; Visión Estereoscópica; Controlador RST. Abstract In this work, it is presented a positional automatic control of a mechanical arm with 3 degrees of freedom. For this, we implemented several techniques of computer vision that determine the three-dimensional coordinates of the mechanical arm and a RST digital controller that allows the device to be located, in real time, in the coordinates sent across a PC quickly and without mistakes and in a stable state. The arm is made of 3 servomotors whose behavior is registered by two webcams and is also examined with the help of a specialized software in video processing. It is important to emphasize the advantage of using these cameras as sensors: they make it possible to measure the positional variables accurately without adding load to the system. This way, different digital video-processing techniques and digital control algorithms are combined in order to develop a mechanical prototype of three-dimensional positioning. Keywords: Machine Vision; Stereoscopic Vision; RST controller. PACS: 07.07.Df, 07.05.Dz. 2010 Revista Colombiana de Física. Todos los derechos reservados. * dbravo@unicauca.edu.co
Rev.Col.Fís., Vol. 43, No. 1 de 2011. 1. Introducción En este artículo se presentan técnicas para la estimación de la posición tridimensional de un objeto (en particular el extremo de un brazo robótico), partiendo de dos vistas tomadas por dos cámaras web. Esta tarea se engloba dentro de una tarea principal que es la de poder realizar el control de posición del elemento terminal o herramienta de un brazo que cuenta con tres grados de libertad, para la posterior aplicación en procesos automáticos. La técnica empleada para la determinación de las coordenadas tridimensionales del punto de interés es la llamada visión estereoscópica, Se han usado dos cámaras idénticas alineadas y calibradas, separadas una distancia conocida, similar a la disposición de los ojos en la visión humana. Sobre las webcam se utilizan filtros infrarrojos para obtener la representación de la imagen sólo en este rango de frecuencias para facilitar el tratamiento y seguimiento del punto de interés. Con las correspondientes coordenadas en pixeles del punto de interés para cada imagen se ha estimado la posición del punto tridimensional que representa el extremo del robot. Para ejecutar el control de posición sobre los servomotores se han implementado leyes de control digital RST en un ordenador; de las leyes de control se envían las señales de control por el puerto serial del PC hacia los servomotores, mientras que las cámaras web son los sensores de realimentación de la bucla de control. 2. Visión Estereoscópica 2.1. Imagen Estereoscópica La representación de una escena tridimensional sobre un plano imagen es una aplicación en la que muchos puntos del espacio se corresponden con uno solo. De otra forma, el punto de la imagen no determina unívocamente el correspondiente punto del espacio tridimensional. Sin embargo, la información perdida sobre la profundidad puede obtenerse empleando técnicas estereoscópicas. Como muestra la Fig. 1, la imagen estereoscópica implica la obtención de dos imágenes separadas del mismo objeto (un único punto w del espacio tridimensional; el punto w representa un LED infrarrojo ubicado en el extremo del brazo). La distancia entre los centros de las dos lentes se denomina línea base. El objetivo consiste en hallar las coordenadas (X, Y, Z) del punto w que tiene las coordenadas (x 1, y 1 ) y (x 2, y 2 ) en las dos imágenes. Se supone que ambas cámaras son idénticas y que sus respectivos sistemas de coordenadas están difiriendo exclusivamente en la posición de sus orígenes. Si se considera el origen de coordenadas global el mismo que el origen de coordenadas de una de las cámaras (centro de la imagen), entonces se puede determinar la coordenada Z del punto de interés. Las coordenadas reales sobre el eje X del punto de interés se proyectan en las dos imágenes según las ecuaciones (1) y (2). Fig. 1: Disposición geométrica de las cámaras para obtener la profundidad de un punto de interés w. Sin embargo, debido a la separación entre las cámaras y a que la coordenada Z de w es la misma en los sistemas de coordenadas de ambas cámaras, se tiene que: donde B es la distancia de la línea base. Fig. 2: Vista superior de la Fig. 1. Reemplazando (2), en (3) se obtiene: (1) (2) (3) (4) 90
D. Revelo, D. Usama, D. Bravo: Ubicación De Coordenadas 3D Para Un Brazo Robótico De 3 Grados De Libertad Mediante Técnicas De Visión Por Computador Y Control Digital Finalmente restando (1) de (5) se obtiene la coordenada de profundidad. 3. Descripción del Brazo Robótico 3.1. Características del Brazo Robótico El brazo robótico está constituido por tres servomotores; los servos representan tres articulaciones de las cuales 2 son rotoides y una es prismática. En la Fig. 3. se muestra el esquema articular del robot. En el extremo del brazo, en el lugar de la herramienta, se ubica un LED infrarrojo; el LED representa el punto de interés para las cámaras del cual se busca obtener sus coordenadas tridimensionales de un sistema de coordenadas global ubicado en el centro de la imagen de una de las cámaras. (5) (6) 3.2. Modelo Geométrico Este modelo expresa las coordenadas operacionales del robot, en función de sus coordenadas articulares. Tabla No 1: Parámetros geométricos. Donde: j σ j α j d j θ j r j 1 0 0 0 θ 1 0 2 0 90 0 θ 2 0 3 1 90 0 90 r 3 σ j es 0 si la articulación es rotoide y 1 si la articulación es prismática. α j : ángulo entre los ejes Z j-1 y Z j, correspondiente a una rotación alrededor de X j-1 d j : distancia entre Z j-1 y Z j a lo largo de X j-1 θ j : ángulo entre los ejes X j-1 y X j correspondiente a una rotación alrededor de Z j r j : distancia entre X j-1 y X j a lo largo de Z j j-1 T j (7) Utilizando la tabla de parámetros geométricos y aplicando la matriz j-1 T j a cada una de las articulaciones del robot se obtiene: 0 T 3 = 0 T 1 1 T 2 2 T 3 0 T 3 (8) Fig. 3: Esquema articular del brazo robótico. Donde la ecuación de posición es: (9) Como en realidad se necesitan las coordenadas articulares θ 1, θ 2 y r 3 en términos de las variables de posición, se utiliza el método de Paul para encontrar dichas variables, obteniendo: (10) Fig. 4: Vista en 3D del brazo robótico. (11) 91
Rev.Col.Fís., Vol. 43, No. 1 de 2011. (12) 4. Proceso de Calibración de las Cámaras El proceso de calibración de una cámara consiste en estimar los parámetros intrínsecos y extrínsecos de la misma. Este proceso se puede realizar en dos pasos: Primero se estima la matriz de proyección M y después se estiman los parámetros intrínsecos y extrínsecos de la cámara a partir de la matriz de proyección M. La estimación de la matriz de proyección M puede ser realizada partiendo de diferentes escenarios conocidos o desconocidos. En el caso de tener una escena conocida, se sitúan una serie de puntos en el mundo 3D cuyas posiciones respecto al sistema de coordenadas del mundo 3D son conocidas. Se recoge una imagen de la escena y se obtienen la posición en píxeles de los correspondientes puntos en la imagen. El conjunto de posiciones de los puntos 3D p = [X, Y, Z] junto con sus correspondientes posiciones en la imagen q = [U, V] se utiliza para definir una ecuación lineal del sistema, la cual se puede solucionar para obtener los elementos de la matriz de proyección M. En el proceso de calibración geométrica de la cámara se deben determinar los parámetros extrínsecos e intrínsecos, los cuales se identifican como: Parámetros Intrínsecos Kx, Ky : Factor de escala. F : Distancia focal. Cx,Cy: Punto principal. Dx,Dy: Distorsión geométrica. Fig. 6: A la izquierda imagen original, a la derecha imagen corregida. 5. Montaje Experimental y Algoritmo Se toman dos cámaras web a las cuales se les extrae el filtro visible y se les implanta filtros infrarrojo, las cámaras están separadas 5 cm entre sí y dispuestas paralelamente como se muestra en la Fig. 7. Posteriormente, se sitúa en el órgano terminal del robot un LED infrarrojo y este sistema se ubica perpendicularmente a los ejes ópticos de las cámaras, a una distancia tal que el lugar de operación del robot encaje completamente en las imágenes tomadas por las cámaras (ver Fig. 8. y Fig. 9); esto garantiza mayor resolución en la medida. Parámetros Extrínsecos T: Vector de rotación. R: Matriz de rotación. Fig. 7: Vista en 3D del sistema de visión y el brazo robótico. Fig. 5: Imagen de la grilla utilizada para el proceso de calibración de las cámaras web. Fig. 8: Máxima posición en las imágenes del campo de trabajo del brazo robótico en la izquierda. 92
D. Revelo, D. Usama, D. Bravo: Ubicación De Coordenadas 3D Para Un Brazo Robótico De 3 Grados De Libertad Mediante Técnicas De Visión Por Computador Y Control Digital Fig. 11: Curva de respuesta en tiempo real para el controlador RST. Fig. 9: Máxima posición, en las imágenes, del campo de trabajo del brazo robótico en la derecha. El algoritmo se trabajó en la plataforma de programación Dev-C++ versión 4.9.9.2 y las librerías desarrolladas por Intel para procesamiento de imágenes OpenCV versión 1.1. Las imágenes procesadas tenían un tamaño de 640 X 480. 6. Controlador RST 7. Resultados Tabla No. 2: Planta en lazo abierto. θ (º) Real θ (º) Medido 15 3.3 20 12.2 25 17.4 30 22.0 35 30.3 40 36.1 45 42.5 50 48.8 55 54.3 60 60.4 65 68.4 70 72.1 75 80.1 80 85.7 85 89.7 90 97.6 Tabla No. 3: Planta en lazo cerrado. Fig. 10: Controlador RST diseñado en Matlab 7.0. T muestreo = 0.2s El tiempo de muestreo se calculó teniendo en cuenta el tiempo de un ciclo de máquina. El controlador se diseñó de tal manera que su error en estado estable fuera cero, como se muestra en la Fig. 11. θ (º) Real θ (º) Medido 15 15 20 20 25 25.1 30 30 35 35 40 40 45 45 50 50 55 55.1 60 60 65 65 70 70 75 75 80 80 85 85.1 90 90.1 93
Rev.Col.Fís., Vol. 43, No. 1 de 2011. Eje Z 2 0-2 -4-6 -8-10 15 10 5 Eje Y 0-5 -10-5 Eje X Medido Real Fig. 12: Medidas de la posición utilizando la visión estereoscópica y el controlador RST. En la Tabla No. 3. se observa que el controlador RST permite mantener nulo el error en estado estable, esto permite que la resolución del sistema dependa únicamente de la resolución de la visión estereoscópica. La resolución del sistema en lazo abierto es de. Para determinar la resolución del sistema en lazo cerrado, se tomaron medidas en el plano (X, Z) con variación en el eje Y; en la Fig. 12. se muestran los resultados para Y = 0. En cada una de las variaciones de Y se observó un comportamiento similar, con resolución de. 8. Conclusiones Utilizar la visión estereoscópica como sensor de realimentación permite realizar una significante reducción en el error de posición. El uso de cámaras con filtros infrarrojos minimiza el ruido en las imágenes tomadas y por ende se reduce el tiempo de procesamiento. 0 5 El sistema realimentado tiene mayor resolución, respecto al sistema en lazo abierto, con un factor de 10. Trabajar con imágenes de mayor tamaño mejora la resolución del sistema. Esta técnica puede ser usada en robots con mayores grados de libertad. 9. Agradecimientos Los autores desean agradecer a la Vice-rectoría de Investigaciones de la Universidad del Cauca por soporte para el desarrollo de este trabajo al interior del Grupo de Óptica y Láser. Esta investigación fue realizada bajo el proyecto BPIN 0020-05492-0000. Referencias [1] Ollero, Anibal. Robótica, manipuladores y robots móviles. Marcombo, 2001. [2] Bradski, Gary. Kaehler, Adrian. Learning OpenCV: Computer Vision whit the OpenCV Library. 1ra ed. E.U.A. Edicones O Reilly 2008. [3] Ogata, Katsuhiko. Sistemas de control en tiempo discreto, 2a ed. Mexico 1996. Prentice Hall hispanoamericana S.A. [4] Sciavicco, Lorenzo. Siciliano, Bruno. Modeling and Control of Robot Manipulators, McGraw-Hill, 1996. [5] Martinez, Rafael K. Control de Movimiento de Robots Manipuladores, UNAM, México, 1990. [6] Khalil, Wisama. Dombre, Etienne. Modeling, Identification and Control of robots, Hermes Penton Science, 2002. 94