Daniel Ivan Zamorano Acosta

 cenidet
Centro Nacional de Investigación y Desarrollo Tecnológico
Departamento de Ingeniería Mecatrónica
TESIS DE MAESTRÍA EN CIENCIAS
Visión Estereoscópica y Estimación de Pose para el
Posicionamiento de un Brazo Robótico
Presentada por:
Ing. en Electrónica Ángel Ernesto Gómez Sánchez
Ing. en Electrónica Daniel Iván Zamorano Acosta
Como requisito para la obtención del grado de:
Maestría en Ciencias en Ingeniería Mecatrónica
Directores de tesis:
Dr. José Ruiz Ascencio
Dr. Juan Manuel Rendón Mancha
Cuernavaca, Morelos
30 de abril de 2008
cenidet
Centro Nacional de Investigación y Desarrollo Tecnológico
Departamento de Ingeniería Mecatrónica
TESIS DE MAESTRÍA EN CIENCIAS
Visión Estereoscópica y Estimación de Pose para el
Posicionamiento de un Brazo Robótico
Presentada por:
Ing. en Electrónica Ángel Ernesto Gómez Sánchez
Ing. en Electrónica Daniel Iván Zamorano Acosta
Como requisito para la obtención del grado de:
Maestría en Ciencias en Ingeniería Mecatrónica
Directores de tesis:
Dr. José Ruiz Ascencio
Dr. Juan Manuel Rendón Mancha
Jurado:
Dr. Alejandro Rodríguez Palacios – Presidente
Dr. Moisés González García – Secretario
Dr. José Ruiz Ascencio – Vocal
Dr. Juan Manuel Rendón Mancha – Segundo Vocal
Cuernavaca, Morelos
30 de abril de 2008
CONTENIDO
Lista de figuras ........................................................................................................................................v Lista de tablas......................................................................................................................................viii Simbología............................................................................................................................................... ix Abreviaturas y acrónimos ................................................................................................................ xv Prefacio................................................................................................................................................... xvi 1. Visión Robótica ............................................................................................................................. 1 1.1 Antecedentes ...................................................................................................................................................... 1 1.1.1 Visión Robótica en el CENIDET ..................................................................................................... 1 1.2 Planteamiento del problema....................................................................................................................... 2 1.2.1 Objetivos ...................................................................................................................................................... 3 1.2.2 Alcances y limitaciones........................................................................................................................ 3 1.3 Estado del Arte................................................................................................................................................... 4 1.3.1 Primeros trabajos en visión ............................................................................................................... 4 1.3.2 Trabajos en visión actuales............................................................................................................... 6 1.4 Conclusiones del capítulo 1 .................................................................................................................. 8 2. Calibración de cámaras ............................................................................................................. 9 2.1 Geometría proyectiva en 2D ................................................................................................................... 10 2.1.1 Representación homogénea de líneas y puntos ................................................................ 10 2.1.2 Grados de libertad ............................................................................................................................... 11 2.1.3 Geometría proyectiva en 1D y 2D .............................................................................................. 12 2.1.4 Intersección de líneas Y puntos unidos por rectas ........................................................... 13 2.1.6 Líneas al infinito .................................................................................................................................... 14 i 2.1.7 Principio de dualidad.......................................................................................................................... 14 2.1.8 Cónicas ...................................................................................................................................................... 14 2.2 Modelo de una Cámara.............................................................................................................................. 16 2.2.1 Modelo de cámara Pinhole ............................................................................................................. 16 2.2.2 Traslación de un punto principal.................................................................................................. 20 2.2.4 Matriz de calibración .......................................................................................................................... 21 2.2.5 Rotación y traslación en 3-D.......................................................................................................... 22 2.2.6 Matriz de cámara.................................................................................................................................. 23 2.2.7 Cámara proyectiva finita................................................................................................................... 23 2.2.8 Cámara proyectiva .............................................................................................................................. 24 2.3 Transformaciones proyectivas en 2D................................................................................................. 24 2.3.1 Transformaciones de puntos ......................................................................................................... 24 2.3.2 Transformación de líneas ................................................................................................................ 25 2.3.3 Transformación de cónicas ............................................................................................................ 26 2.3.4 Jerarquía de transformaciones..................................................................................................... 26 2.4 Estimación de una homografía y cálculo de la matriz de cámara P ................................. 30 2.4.1 Solución homogénea ......................................................................................................................... 31 2.4.2 Algoritmo de transformación lineal directa............................................................................. 33 2.4.3 Configuraciones degeneradas...................................................................................................... 34 2.4.4 Solución a partir de líneas y otras entidades........................................................................ 34 2.4.6 Solución no homogénea .................................................................................................................. 35 2.4.7 Solución mínima ................................................................................................................................... 36 2.4.8 Solución sobre-determinada .......................................................................................................... 36 ii 2.5 Resultados de la calibración de cámaras ........................................................................................ 37 2.26 Patrón de calibración........................................................................................................................... 37 2.27 Fijación de las cámaras ..................................................................................................................... 38 2.28 Proceso de calibración ....................................................................................................................... 40 2.29 Obtención de la matriz de cámara............................................................................................... 45 2.6 Conclusiones del capítulo 2 ............................................................................................................... 47 3. Recuperación de coordenadas en tres dimensiones .......................................................48 3.1 Pruebas de algoritmos de procesamiento de imágenes ......................................................... 48 3.1.1 Procesamiento digital de imágenes usando Matlab......................................................... 48 3.2 Recuperación de coordenadas en 3D usando cámaras fijas ............................................... 53 3.2.1 Resultados para cámaras FIJAS................................................................................................. 55 3.3 Recuperación de coordenadas en 3D usando estimación de pose.................................. 56 3.3.1 Resultados para estimación de pose........................................................................................ 61 3.4 Conclusiones del capítulo 3 ............................................................................................................... 64 4. El Robot PUMA y su control .......................................................................................................65 4.1 Estructura mecánica del robot puma.................................................................................................. 65 4.1.1 Espacio de trabajo............................................................................................................................... 67 4.2 Construcción de la 4ta articulación ....................................................................................................... 67 4.2.1 Funcionamiento del servomotor................................................................................................... 69 4.3 Rediseño de las tarjetas de control para el robot PUMA ........................................................ 70 4.3.1 Funcionamiento de la tarjeta de control .................................................................................. 72 4.4 Adaptación de los programas de la PC y del microcontrolador para su uso con las
nuevas tarjetas ........................................................................................................................................................ 74 4.5 Aplicación de un controlador PID sobre el Robot PUMA ........................................................ 76 iii 4.5.1 Ajustes del controlador PID............................................................................................................ 78 4.6 Conclusiones del capítulo 4 ............................................................................................................... 79 5. Resultados.........................................................................................................................................80 5.1 Interfaz gráfica en matlab.......................................................................................................................... 80 5.1.1 Descripción del programa ............................................................................................................... 81 5.1.2 Modo de operación de la interfaz................................................................................................ 83 5.2 Prueba del sistema de control del robot y el sistema de visión. ......................................... 86 5.2.1 Pruebas de la cinemática inversa del robot con su efector final.......... 87 5.3 Resultados del modo cámaras fijas .................................................................................................... 89 5.4 Resultados del modo estimación de pose....................................................................................... 94 6. Conclusiones....................................................................................................................................99 6.1 Objetivos cubiertos ....................................................................................................................................... 99 6.2 Aportaciones...................................................................................................................................................100 6.3 Trabajos futuros............................................................................................................................................100 Bibliografía .......................................................................................................................................... 102 ANEXO .................................................................................................................................................. 104 A. Imágenes del sistema completo............................................................................................................104 B. Circuito final de la tarjeta de control....................................................................................................105 iv LISTA DE FIGURAS
Capítulo 1
Figura 1-1: Shakey de Artificial Intelligence Center y SRI International. ............................. 5
Capítulo 2
Figura 2-1: Representación de puntos en el espacio 1D................................................... 11
Figura 2-2: Geometría proyectiva en 1D ........................................................................... 12
Figura 2-3: Geometría proyectiva en 2D ........................................................................... 13
Figura 2-4: Esquema de cámara oscura ........................................................................... 16
Figura 2-5: Esquema de una cámara CCD ....................................................................... 17
Figura 2-6: Esquema simplificado de una cámara ............................................................ 18
Figura 2-7: Modelo Pinhole simplificado............................................................................ 18
Figura 2-8: Esquema simplificado de cámara ................................................................... 19
Figura 2-9: Triángulos semejantes .................................................................................... 20
Figura 2-10: Sistema de coordenadas de la imagen ( x, y ) y de la cámara ( xcam, ycam )...... 20
Figura 2-11: Transformación euclidiana entre el marco coordenado del mundo y la
cámara............................................................................................................................... 23
Figura 2-12: Distorsión originada por una trasformación de afinidad plana. ..................... 30
Figura 2-13: Trasformación proyectiva entre líneas .......................................................... 30
Figura 2-14: Patrón de calibración..................................................................................... 38
Figura 2-15: Soportes ........................................................................................................ 39
Figura 2-16: Fijación de los soportes................................................................................. 39
Figura 2-17: Fijación de las cámaras a los soportes ......................................................... 40
Figura 2-18: Imágenes para la calibración de cámaras fijas. ............................................ 41
Figura 2-19: Resultados de la calibración, puntos reales y calculados. ............................ 45
v Capítulo 3
Figura 3-1: Diagrama con los casos fundamentales en el procesamiento de imágenes. . 49
Figura 3-2: Valores de RGB en un LED. ........................................................................... 50
Figura 3-3: Tres LEDs en una escena, su extracción y sus centroides encontrados........ 51
Figura 3-4: Estructura del programa detector de tres LEDs y recuperación de sus
coordenadas en 3D. .......................................................................................................... 52
Figura 3-5: Rayos de proyección en cada imagen. ........................................................... 54
Figura 3-6: Tres LEDs frente a dos cámaras fijas calibradas............................................ 55
Figura 3-7: Ilustración geométrica de tres puntos en el espacio. ...................................... 57
Figura 3-8: Imagen del objetivo para la estimación de pose. ............................................ 63
Figura 3-9: Gráfica de la solución obtenida para la estimación de pose. .......................... 64
Capítulo 4
Figura 4-1: Eslabones, articulaciones y motores del brazo. .............................................. 66
Figura 4-2: Espacio de trabajo (vista superior y lateral). ................................................... 67
Figura 4-3: Diseño de la 4ta articulación. ........................................................................... 68
Figura 4-4: Diseño final de la 4ta articulación.................................................................... 68
Figura 4-5: Servomotor y cámara usados en la 4ta articulación. ...................................... 69
Figura 4-6: Funcionamiento de un servomotor.................................................................. 70
Figura 4-7: Interfaz construida para el control del robot. ................................................... 72
Figura 4-8: Etapa de adquisición de datos. ....................................................................... 73
Figura 4-9: Diagrama de flujo para el programa de la PC2. .............................................. 74
Figura 4-10: Diagrama de flujo del programa del PIC. ...................................................... 75
Figura 4-11: Diagrama de flujo de la interrupción programada en el PIC. ........................ 76
Figura 4-12: Movimiento cicloidal (S) y su velocidad (S’) y aceleración (S’’). ................... 78
vi Capítulo 5
Figura 5-1: Interfaz gráfica mostrando el modo de operación de dos cámaras fijas. ........ 81
Figura 5-2: Diagrama de flujo del programa de la interfaz de usuario............................... 82
Figura 5-3: Modo de operación.......................................................................................... 83
Figura 5-4: Interfaz gráfica mostrando el modo de operación de cámara en mano. ......... 84
Figura 5-11: Configuración del manipulador plano XY. ..................................................... 88
Figura 5-12: Proyección en el plano formado por el 2 do y 3er eslabón. .......................... 89
Figura 5-13: Lazo de control usado en el sistema de visión y control del robot ................ 90
Figura 5-14: a) Cámaras fijas y objeto buscado, b) Vista superior del sistema de ejes
sobre el robot, c) Origen sobre del sistema coordenado................................................... 90
Figura 5-15: Posición inicial del robot................................................................................ 91
Figura 5-16: Interfaz gráfica final mostrando el modo de cámaras fijas. ........................... 93
Figura 5-17: Acercamiento al efector final sobre el LED. .................................................. 93
Figura 5-18: Triángulos azul y cian con los puntos de sus vértices. ................................. 94
→
→
Figura 5-19: Plano de la cámara y vectores de dirección c1 , c 2 ..................................... 95
Figura 5-21: Acercamiento al efector final sobre el objetivo de prueba............................. 98
vii LISTA DE TABLAS
Capítulo 2
Tabla 2-1: Puntos para la calibración de la cámara 1 ....................................................... 42
Tabla 2-2: Puntos para la calibración de la cámara 2 ....................................................... 42
Tabla 2-3: Resultados de pruebas para P1 ....................................................................... 44
Tabla 2-4: Resultados de pruebas para P2 ....................................................................... 44
Capítulo 3
Tabla 3-1: Resultados de la localización de coordenadas espaciales de los tres LEDs ... 56
Tabla 3-2: Pruebas de la estimación de pose ................................................................... 62
Capítulo 4
Tabla 4-1: Especificaciones del robot................................................................................ 66
Tabla 4-2: Características principales del servomotor....................................................... 69
Tabla 4-3: Constantes utilizadas en los controladores. ..................................................... 79
Capítulo 5
Tabla 5-1: Valores de los parámetros del robot................................................................. 88
Tabla 5-2: Resultados de las pruebas con el sistema de dos cámaras fijas ..................... 92
Tabla 5-3: Resultados de las pruebas de estimación de pose .......................................... 97
viii SIMBOLOGÍA
1D
Una dimensión.
2D
Dos dimensiones.
3D
Tres dimensiones.
x
Coordenada cartesiana abscisa.
y
Coordenada cartesiana ordenada.
R1
Espacio euclidiano unidimensional.
R2
Espacio euclidiano bidimensional.
R3
Espacio euclidiano tridimensional.
x
Símbolo remarcado representa un vector columna.
T
Transpuesta de un vector o matriz.
x , x′
Puntos en la imagen.
xT
Transpuesta de vector columna.
a, b, c, d , e, f , k
Constantes generalizadas.
Ρ1
Espacio proyectivo unidimensional.
Ρ2
Espacio proyectivo bidimensional.
l
Vector columna de una recta.
x1 , x2 , x3
Coordenada arbitraria generalizada.
X
Punto en el mundo representado por un vector homogéneo
( x, y , z ,1)T .
w
Factor de escalamiento para coordenadas homogéneas.
×
Producto cruz o producto vectorial.
i
Vector unitario en la dirección del eje x.
j
Vector unitario en la dirección del eje y.
k
Vector unitario en la dirección del eje z.
C
Matriz de coeficientes.
c
Vector de coeficientes.
xi
i-ésimo punto de una serie de puntos.
l∞
Línea al infinito.
ix xi
i-ésima coordenada abscisa del i-ésimo punto.
yi
i-ésima coordenada ordenada del i-ésimo punto.
C
Centro de proyección.
P
Punto en el espacio.
f
Distancia focal o distancia del centro de proyección al plano
de la cámara.
X ,Y , Z
Coordenada “x”, “y”, “z” de un punto en el espacio P .
I
Matriz identidad de dimensión 3 × 3.
P
Matriz homogénea de 3 × 4 llamada matriz de cámara.
pij
Elemento de la matriz de cámara.
xcam
Coordenada de un punto en la cámara (abscisa).
ycam
Coordenada de un punto en la cámara (ordenada).
K
Matriz de calibración.
px
Coordenada del punto principal de la cámara (abscisa).
py
Coordenada del punto principal de la cámara (ordenada).
X cam
Puntos del mundo representado por un vector homogéneo y
localizado en el origen del sistema coordenado de la
cámara.
mx
El número de píxeles por unidad de distancia en
coordenadas de imagen en la dirección “x”.
my
El número de píxeles por unidad de distancia en
coordenadas de imagen en la dirección “y”.
αx
Longitud focal en términos de las dimensiones de los pixeles
en la dirección “x”.
αy
Longitud focal en términos de las dimensiones de los pixeles
en la dirección “y”.
x0
Punto principal en términos de las dimensiones de los
pixeles en la dirección “x”.
y0
Punto principal en términos de las dimensiones de los
pixeles en la dirección “y”.
x s
Parámetro de oblicuidad entre los ejes “x”, “y” de los pixeles.
%
X
Vector no homogéneo representando las coordenadas de
un punto en el marco coordenado del mundo.
%
X
cam
Vector que representa un punto en el marco coordenado de
la cámara.
R
C~
Matriz de rotación de 3 × 3 .
Coordenadas del centro de la cámara en el marco
coordenado del mundo.
M
Matriz no singular de 3 × 3 producto de K y R .
H
Matriz de transformación.
h
Función invertible en Ρ 2 .
h( xi )
Función invertible en Ρ 2 del i-ésimo punto.
ε
Variable de grado de isometría.
HE
Matriz de transformación isométrica.
s
Factor de escalada isotrópico utilizada en las similaridades
(transformaciones proyectivas).
t
Vector de traslación.
tx
Traslación en la dirección “x”.
ty
Traslación en la dirección “y”.
Hs
Matriz de transformación de similaridad.
A
Matriz no singular que representa la afinidad.
HA
Transformación de afinidad.
R(θ )
Matriz de rotación en el ángulo θ .
R(φ )
Matriz de rotación en el ángulo φ .
D
Matriz Diagonal.
λ1
Factor de escalamiento en la rotación en “x”.
λ2
Factor de escalamiento en la rotación en “y”.
HP
Matriz de transformación proyectiva.
vT
Vector de escalamiento de transformaciones proyectivas.
v
Factor de escalamiento de transformaciones proyectivas.
xi h
Vector de 9 elementos hechos a partir de los elementos de
la matriz de transformación H .
h jT
j-ésimo renglón de la matriz de transformación H .
hi
i-ésimo elemento de h .
Ai
Matriz cuadrática de coordenadas de puntos para la
solución del sistema de transformación lineal directa.
wi′
Factor de escalamiento de coordenadas homogéneas.
n
Coeficiente generalizado.
W
Factor de escala arbitrario.
p
Vector que contiene los elementos de la matriz P .
ρ
Factor de escala arbitrario de puntos en la imagen.
pi
i-ésimo punto en el sistema coordenado de la cámara.
a, b, c
Constante generalizada, distancia de un cateto del triángulo
objetivo.
p1 , p2 , p3
Puntos conocidos del espacio del triángulo de pruebas.
s1
Distancia del centro de perspectiva al punto p1 del triángulo.
s2
Distancia del centro de perspectiva al punto p2 del triángulo.
s3
Distancia del centro de perspectiva al punto p3 del triángulo.
q1
Punto p1 del triángulo proyectado en la imagen.
q2
Punto p2 del triángulo proyectado en la imagen.
q3
Punto p3 del triángulo proyectado en la imagen.
ui
i-ésima coordenada del i-ésimo punto qi (abscisa).
vi
i-ésima coordenada del i-ésimo punto qi (ordenada).
ji
i-ésimo vector unitario del centro de perspectiva al i-ésimo
punto pi observado del triángulo.
α
Ángulo opuesto al cateto a .
β
Ángulo opuesto al cateto b .
γ
Ángulo opuesto al cateto c .
u
Variable generalizada proporciona la relación entre s2 y s1 .
xii v
Variable generalizada proporciona la relación entre s3 y s1 .
T
Vector de traslación que es la distancia del centro de
proyección hasta el punto p1 .
tx
Coordenada componente del vector T en la dirección “x”.
ty
Coordenada componente del vector T en la dirección “y”.
tz
Coordenada componente del vector T en la dirección “z”.
p 'i
i-ésimo punto en el sistema coordenado del mundo.
rij
ij-esimo elemento de la matriz de rotación.
S
Función de posición.
S’
Función de velocidad.
S’’
Función de aceleración.
τ
Constante de tiempo.
T
Tiempo total de movimiento.
Kp
Constante proporcional.
Ki
Constante integral.
Kd
Constante derivativa.
θ
Vector de los ángulos de las articulaciones del robot.
θn
n-ésimo ángulo del n-ésimo eslabón del robot.
p
Vector de la coordenada espacial deseada.
px
Coordenada componente del vector p en la dirección “x”.
py
Coordenada componente del vector p en la dirección “y”.
pz
Coordenada componente del vector p en la dirección “z”.
ai
i-ésimo desplazamiento sobre el eje coordenado “x”.
di
i-ésimo desplazamiento sobre el eje coordenado “z”.
αi
i-ésimo ángulo de giro sobre el eje coordenado “x” del iésimo eslabón.
θi
i-ésimo ángulo de giro sobre el eje coordenado “z” del iésimo eslabón.
P1i
i-ésimo punto del triángulo de la primera solución (método
xiii de Grunert).
P2i
i-ésimo punto del triángulo de la segunda solución (método
de Grunert).
→
ci
i-ésimo vector perpendicular al planos de la solución
obtenida (triángulo de la primera y segunda solución).
a'
Distancia en pixeles del cateto a .
b'
Distancia en pixeles del cateto b .
c'
Distancia en pixeles del cateto c .
ra ' −b '
Relación entre los catetos a ' y b ' .
rb ' −c '
Relación entre los catetos b ' y c ' .
rc '− a '
Relación entre los catetos c ' y a ' .
xiv ABREVIATURAS Y ACRÓNIMOS
CCD
Charge-Coupled-Device.
CENIDET
Centro Nacional de Investigación y Desarrollo Tecnológico.
CIL
Computational Imaging Lab.
CNAD
Centro Nacional de Actualización Docente.
CVC
Centro de Visión por Computador.
EMVA
Association of European Machine Vision Companies.
GDL
Grados de libertad.
LED
Light-Emitting Diode.
NHTE
Núcleo Híbrido de Transición de Estados.
PC
Personal Computer.
PC1
Computadora que controla al sistema de visión.
PC2
Computadora que controla al robot.
PIC
Controlador de Interfaz Periférico.
PID
Proporcional Integral Derivativo.
PUMA
Programmable Universal Manipulator for Assembly.
PWM
Pulse-Width Modulation.
RGB
Red, Green, Blue.
UAB
Universidad Autónoma de Barcelona.
UPF
Universitat Pompeu Fabra.
Vcd
Voltaje de corriente directa.
xv PREFACIO
La visión robótica es un campo joven multidisciplinario, ya que engloba
conocimientos de diversos campos, como son: la robótica, la visión artificial y el
control. Este campo avanza en forma rápida y frecuentemente aparecen nuevos
resultados, dando a conocer nuevas y mejores técnicas de visión, para
aplicaciones cada vez más complicadas y exigentes.
En los primeros años de la robótica móvil, el enfoque utilizado para calcular y
controlar los movimientos del robot, se basaba en el conocimiento completo del
ambiente donde se movía. Entre las características principales podemos destacar
que no se tenía contemplada la aparición de objetos no modelados (Cazorla, Otto
y Sáez 2002).
Hoy en día el control inteligente en robots tiene como objetivo el poder realizar
tareas en un ambiente cada vez menos estructurado y variante, respondiendo
apropiadamente a obstáculos y objetivos cuya posición es variable y desconocida.
Para ello es necesario incorporar sensores más adecuados para percibir el
ambiente, conocerlo y poder interactuar en él.
La visión es un sentido muy poderoso pero a la vez complicado, otorga gran
cantidad de información acerca del entorno y da pauta a una interacción
inteligente con éste, proporcionando además la identificación de la posición y de
la relación entre los objetos.
Entre las formas más comunes de visión robótica está la cámara fija que puede
incluir al robot dentro de la escena y la cámara montada sobre o cerca del efector
terminal (“ojo en mano”). El primer paso a realizar al implementar un sistema de
visión robótica es la calibración de la cámara que será su sensor de percepción
del entorno.
Este trabajo de investigación contribuye a la asimilación de técnicas de visión
artificial y de robótica en el CENIDET, haciendo la aportación de un sistema físico
compuesto de un brazo robótico tipo PUMA (Programmable Universal Manipulator
for Assembly), con un sistema de visión y la implementación de un algoritmo para
calibración de cámaras. Esto constituirá una base para futuros trabajos en estas
áreas.
Para el desarrollo de esta investigación se aprovechó el trabajo realizado en el
2005, “Construcción de un brazo robótico de tres grados de libertad (GLB) y su
control mediante el Núcleo Híbrido de Transición de Estados”, como plataforma
experimental, desarrollando algunas modificaciones para la adaptación al nuevo
sistema de visión.
xvi La mayor parte de los programas usados fueron elaborados en Matlab R2006b de
Mathworks ®, con la ayuda de las herramientas de adquisición y procesamiento
de imágenes. También fue necesaria la utilización de otros softwares como son:
SolidWorks de SolidWorks Corporation ®, para la elaboración en 3D de piezas a
construir; Protel DXP 2004, de Altium ®, necesario para la elaboración de circuitos
electrónicos impresos y esquemáticos; MPLAB de Microchip Technology Inc,
usado en la programación de los microcontroladores del robot; Turbo C++ de
Borland ®, donde se reprogramó al brazo robótico un control PID (Proporcional
Integral Derivativo).
Para presentar este trabajo de tesis, el documento se divide en las siguientes
partes:
El capítulo 1 menciona antecedentes en el campo de robótica y visión artificial,
en el CENIDET y en otros centros de investigación en el mundo. Se presenta el
planteamiento del problema, los objetivos, el alcance del proyecto y las
limitaciones de éste. También se aborda el estado del arte en visión robótica.
En el capítulo 2 se muestra la teoría básica respecto a la calibración de cámaras,
se abordan los temas de geometría proyectiva y modelos de cámaras, se
muestran además los resultados obtenidos en la calibración de cámaras, como
requisito mínimo para desarrollar este sistema de visión.
El capítulo 3 trata el tema de recuperación de coordenadas en tres dimensiones,
aprovechando la calibración de las cámaras y aplicando los métodos de cámaras
fijas y estimación de pose. Además, se muestra la técnica de procesamiento de
imágenes usada para la localización de un objeto, que funcionará como “blanco”
para posicionar el efector final del robot.
En el capítulo 4 se muestran las características mecánicas y eléctricas del robot,
las adaptaciones necesarias que se le hicieron en la parte mecánica, eléctrica, de
programación y de control para que funcione con el sistema de visión.
En el capítulo 5 se presenta la integración de los sistemas de visión y robótico,
las pruebas finales hechas y los resultados obtenidos usando los métodos de
cámaras fijas y estimación de pose.
El capítulo 6 presenta los objetivos cubiertos, las aportaciones, las limitantes y
los trabajos futuros derivados de esta investigación.
xvii CAPÍTULO 1
1. VISIÓN ROBÓTICA
La aplicación de la visión artificial en sistemas robóticos, llamado “visión robótica”, es
una disciplina que va adquiriendo desde hace unas décadas un desarrollo muy
significativo. Computadoras más veloces y el crecimiento en el desarrollo de sistemas
robóticos, hacen posible la implementación de métodos de visión desarrollados
teóricamente décadas atrás, sobre sistemas robóticos actuales.
En este trabajo se usaron dos métodos de visión: “Visión estereoscópica” y “Estimación
de pose” con el fin de localizar un objeto en el espacio y posicionar el efector final de un
brazo robótico sobre dicho objeto.
En las siguientes secciones se presentan trabajos de visión robótica, desarrollados en
el CENIDET (Centro Nacional de Investigación y Desarrollo Tecnológico); se describen
el problema, objetivos y alcances de este trabajo de tesis y el estado del arte de la
visión robótica.
1.1 ANTECEDENTES
1.1.1 VISIÓN ROBÓTICA EN EL CENIDET
La visión robótica en el CENIDET es un campo en el cual se ha incursionado en
departamentos como Ciencias Computacionales, en el área de Inteligencia Artificial y
en el departamento de Mecatrónica.
En el caso del departamento de mecatrónica se desarrollaron trabajos como:
“Construcción de un brazo robótico de tres GDL y su control mediante el núcleo híbrido
de transición de estados” (Villalobos y Ramírez 2005), donde se realizó la construcción
de un brazo robótico de tres grados de libertad tipo PUMA como plataforma para probar
la eficiencia de la metodología del Núcleo Híbrido de Transición de Estados (NHTE)
como controlador de una planta no lineal. Este brazo se utilizó para anexarle una
cámara en su efector final e implementar algoritmos de calibración de cámaras,
detección de objetos en imágenes y cálculo de sus coordenadas en 3D, para llevar a
cabo el control de sus movimientos, obteniendo con esto un prototipo de visión robótica
real en el CENIDET.
1 Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
Por parte del grupo de inteligencia artificial se realizaron tesis como: “Diseño de un
sistema de calibración automática para un sistema de visión robótica modular” (Pérez
Ramírez 2002), donde se aborda el tema de calibrar un sistema modular de visión
robótica, estando el sistema de visión, el robot y el sistema de calibración en módulos
que no están directamente ligados, esto se desarrolló en un sistema computacional
donde al seleccionar un punto en una imagen éste era enviado al sistema de referencia
del robot para que el brazo se moviera hacia ese punto. En la simulación, la cámara no
estaba montada en el robot, además, la matriz de calibración de la cámara y los
parámetros del robot se supusieron conocidos.
En trabajos como: “Detallador de tareas para procesos de Visión Robótica” (López
Solórzano 2003) y “Esquema para la integración de Sistemas de Visión Robótica”
(Peralta Abundez 2003), fueron abordados temas como la integración entre sistemas
de visión artificial por computadora y sistemas de control de robots articulados.
1.2 PLANTEAMIENTO DEL PROBLEMA
La problemática a resolver es la calibración de cámaras, necesaria para extraer
información métrica a partir de imágenes bidimensionales del mundo en tres
dimensiones. Aunque existen técnicas para inferir esta información desde imágenes
captadas por cámaras no calibradas, estas técnicas limitan la posibilidad de realizar
aplicaciones efectivas en visión, tal como interacción en ambientes, seguimiento y
reconstrucción en tres dimensiones, los cuales se basan en el conocimiento de la
traslación y rotación de la cámara (Aracena Pizarro, Campos y Clésio 2005).
Los procesos de calibración proporcionan una matriz que contiene la información de
rotación, traslación y escala de la cámara, ésta es llamada matriz de cámara.
La matriz de cámara contiene 11 parámetros que representan una transformación lineal
del mundo real a la cámara, es decir, traslada cada punto del mundo real a un punto en
una imagen, sin embargo el proceso inverso no es posible, puesto que se pierde una
dimensión (profundidad) (Zisserman y Hartley 2003), de tal manera, que es necesario,
a partir del conocimiento de la matriz de cámara, aplicar métodos como visión
estereoscópica y estimación de pose para recuperar dicha dimensión.
El problema se resume al cálculo de los 11 parámetros de la matriz de cámara y la
recuperación de coordenadas en 3D a partir de los métodos de visión estereoscópica y
estimación de pose.
2 Capítulo 1 Visión Robótica
1.2.1 OBJETIVOS
OBJETIVO GENERAL
Usar la visión estereoscópica provista por dos cámaras calibradas y la estimación de
pose de una cámara calibrada montada sobre el robot, para localizar un punto en el
espacio y usar esta información dentro de un lazo de control que lleve el efector final de
un brazo robótico tipo PUMA al punto final deseado.
OBJETIVOS PARTICULARES
i)
ii)
iii)
iv)
v)
vi)
vii)
Diseño y construcción de una articulación de al menos un grado de libertad
adicional para el eslabón terminal del brazo PUMA.
Construcción de un eslabón terminal con base para cámara e índice
posicional.
Implementar el método de calibración directa.
Implementar dos métodos de recuperación de coordenadas en 3D: visión
estereoscópica (un par de cámaras fijas) y estimación de pose (cámara en
mano).
Reconocer y localizar un blanco (objetivo) en una imagen; un LED en caso
de visión estereoscópica y 3 LEDs en caso de estimación de pose,
controlando todas las variables posibles (iluminación, control de la escena,
color, etc.) para facilitar la tarea al sistema de visión.
Resolver la localización espacial del blanco aplicando el método de visión
estereoscópica y realizar la aproximación del eslabón final al blanco.
Realizar desplazamientos para localizar el blanco, resolver la localización
espacial del blanco aplicando el método de estimación de pose y realizar la
aproximación del eslabón final al blanco.
1.2.2 ALCANCES Y LIMITACIONES
Los alcances de este trabajo de tesis de maestría son los siguientes:
•
•
•
Asimilación de las técnicas básicas de visión artificial en un ambiente
estructurado.
Estudio de algunos modelos de cámara y asimilación de las técnicas básicas de
calibración.
Conocer superficialmente la cinemática y dinámica del robot tipo PUMA.
3 Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
•
•
•
•
Instrumentación del brazo con una articulación y un eslabón terminal que porte
una cámara y un índice de posición.
Desarrollo de
la secuencia de control de posición y algoritmo de
calibración con ayuda de Matlab.
Programación de dicha secuencia y procedimiento de calibración.
Evaluación experimental del sistema.
Entre las limitaciones que se presentaron durante el desarrollo de la investigación se
encuentran:
• Sólo se aplicó un método de calibración de cámaras, llamado calibración directa,
se usó sólo este método ya que presenta poca complejidad en la resolución de
sus ecuaciones.
• La falta de herramienta y equipo para la construcción de la cuarta articulación
ocasionó deficiencias simétricas, que se reflejaron en el cálculo de la cinemática
inversa del robot.
• No se desarrolló el control del robot en la plataforma de MatLab, debido a la
limitante del tiempo para el desarrollo de la investigación.
• El estado mecánico del robot fue la limitante más significativa para la evaluación
del sistema, ya que el modelo dinámico que lo representaba no fue el correcto y
ocasionó problemas en su control y por consiguiente errores de posición.
1.3 ESTADO DEL ARTE
1.3.1 PRIMEROS TRABAJOS EN VISIÓN
Muchas han sido las investigaciones hechas décadas atrás en trabajos relacionados
con visión, calibración de cámaras estereoscópicas, estimación de pose para el caso
de una sola cámara y reconstrucción en tres dimensiones, dichas investigaciones
aportan múltiples métodos y formalismos matemáticos útiles para la resolución de los
problemas tratados en este trabajo de tesis.
A finales de los 60s el Artificial Intelligence Center y SRI International reportaron el
desarrollo de un robot móvil llamado Shakey, dotado con una limitada habilidad para
percibir y modelar ambientes, podía realizar tareas planeadas, buscar rutas y
reacomodar objetos simples (Fu, González y Lee 1987).
4 Capítulo 1 Visión Robótica
Figura 1-1: Shakey de Artificial Intelligence Center y SRI International.
Los primeros trabajos en procesamiento automático de imágenes se hicieron en la
década de los 70s por gente como: Hans Moravec, Gerald J. Sussman. Aunque aun
no existía la robótica o visión computacional tal cual, ya aparecían las primeras
aplicaciones industriales y se hacía investigación en robótica, imágenes médicas y
aéreas (Hayet 2007).
En 1973 Bolles y Paul de Stanford mostraron un sistema retroalimentado con visión,
con un brazo robótico y una computadora PDP-10, para el ensamblado de bombas de
agua de automóviles (Fu, González y Lee 1987).
Los problemas de recursos de computación limitados, hacían difíciles de realizar los
sueños de los pioneros, la noción de “dificultad” de los problemas a resolver entre
máquinas y humanos era diferente, lo que es fácil para humanos (reconocer caras,
moverse acoplando sensores y motorización) no lo es para las máquinas.
A finales de los 70s David Marr propone una dirección en cuanto a la visión artificial:
abandonar toda búsqueda de modelización
simbólica de los procesos de
reconocimiento de patrones y reconstrucción y propone una teoría inspirada
5 Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
completamente en estudios sobre la visión humana, algoritmos reactivos basados en
comportamientos elementales (Hayet 2007).
En los años 80s y 90s, abordaban el tema de calibración de cámaras, análisis y
reconstrucción 3D, investigaciones como: “Camera Self-Calibration: Theory and
Experiments” (Faugera, Luong y Maybank 1992), “A Guided Tour Through Multiview
Relations” (Moons 1998), “Camera Calibration Using Line Correspondences” (R.
Hartley 1993), se enfocaron en la búsqueda de mejores métodos para el cálculo de los
coeficientes de la “matriz de cámara”, que contiene la información de rotación,
traslación y escala de la cámara, y que establece la correspondencia entre el sistema
de coordenadas en tres dimensiones del mundo físico y del sistema visual
(coordenadas de una imagen en dos dimensiones), estos estudios se basaron en el
modelo de cámara “Pinhole”.
Otro tema de investigación importante fue el estudio de reconstrucciones en 3D
abordando el problema de encontrar un punto en el espacio a partir de imágenes
provenientes de cámaras con pose o calibración conocida, para el caso de dos
cámaras calibradas o visión estereoscópica el proceso requiere la intersección en el
espacio de dos rayos, éste método es comúnmente llamado “Triangulación” (Hartley y
Sturm 1997).
En el 2003, Richard Hartley y Andrew Zisserman, presentan el libro “Multiple View
Geometry in Computer Vision”, el cual se tomó como base teórica de esta tesis,
mostrado en el Capitulo 2, para el estudio de geometría proyectiva, transformaciones
en 2D y 3D, estudio de modelos de cámaras, calibración de cámaras donde se “obtuvo
la matriz de cámara” y las bases de la reconstrucción en 3D.
1.3.2 TRABAJOS EN VISIÓN ACTUALES
Actualmente, un sistema de visión robótica es una tecnología que integra aplicaciones
informáticas, ópticas y electrónicas capaces de dotar del sentido de la vista a máquinas
y robots, que así pueden percibir su entorno y reaccionar según unas normas exigidas.
La adquisición de imágenes y su análisis automático, sin necesidad de establecer
contacto físico alguno, en cualquier momento y lugar, permite controlar procesos
industriales con gran eficiencia y total seguridad.
El cometido de un sistema de visión artificial es dotar a las máquinas del “sentido de la
vista” para que puedan captar el aspecto y las cualidades de un objeto o escena (color,
luminosidad, forma y tamaño).
Un sistema de visión artificial captura imágenes individuales o secuencias de vídeo,
que luego son procesadas informáticamente con el fin de analizar y medir una o varias
características del objeto en cuestión. Toda esta información debe facilitar la toma de la
decisión más adecuada en un proceso productivo.
6 Capítulo 1 Visión Robótica
Los componentes básicos de un sistema de visión artificial se describen a continuación.
La cámara capta la imagen, la computadora, provista de una tarjeta de vídeo y del
software adecuado, digitaliza y procesa la señal visual. Se requiere también una
iluminación apropiada para la escena y el trabajo encomendado. Actualmente existe
una gran variedad de los componentes básicos que integran un sistema de visión
artificial. Muchos de estos elementos esenciales pueden combinarse y configurar el
sistema más adecuado a cada necesidad (Banespyme 2006).
El sistema robótico puede ser definido desde diversos puntos de vista:
Con independencia respecto a la definición de "robot":
"La Robótica es la conexión inteligente de la percepción a la acción"...
(Banespyme 2006)
En base a su objetivo:
"La Robótica consiste en el diseño de sistemas. Actuadores de locomoción,
manipuladores, sistemas de control, sensores, fuentes de energía, software de calidad,
todos estos subsistemas tienen que ser diseñados para trabajar conjuntamente en la
consecución de la tarea del robot"... (Banespyme 2006)
Tomando esta última definición, el sistema robótico consta de:
•
•
•
•
•
•
•
Estructura mecánica
Transmisiones
Sistema de accionamiento (actuadores)
Sistema sensorial (sensores)
Elementos terminales
Sistema de control (controlador)
Alimentación
La función del controlador es gobernar el trabajo de los actuadores (los dispositivos que
originan el movimiento) y las transmisiones (modificadores del movimiento). La
alimentación proporciona la energía necesaria para todo el sistema. Además de estos
tres subsistemas, los robots de segunda generación incorporan sensores que reciben
la señal de realimentación procedente de los actuadores pasando la información al
controlador, que debe calcular la corrección del error. El entorno proporciona también
información que reciben los sensores y se envía de nuevo al controlador para hacer los
ajustes necesarios para la realización de la tarea (González Fernández 2003).
Existen diversos portales en internet que contienen información relevante respecto a
visión y robótica actual como son: Machine Vision Portal, con noticias y una guía de
productos y también Machine Vision Online, que presenta una gran biblioteca de
artículos técnicos y descripciones de aplicaciones industriales. Las revistas
7 Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
especializadas aportan muchos datos de interés: The Journal of Electronic Imaging y
Vision Systems Design e incluso Machine Vision Tech Group, de la Society of
Manufacturing Engineers (SME), que cuenta con un activo foro de discusión e
intercambio de información.
Algunos departamentos universitarios, centrados en la investigación básica y aplicada,
apuestan fuerte por el tema, como el Centro de Visión por Computador (CVC, de la
UAB), el Computational Imaging Lab (CILab, de la UPF) y el Grupo de Automática,
Robótica y Visión Artificial (Escuela Politécnica Superior de la UA).
Muchas empresas ofrecen su gama de productos y aplicaciones, como National
Instruments, DALSA Coreco, The Imaging Source Europe y The Computer Vision
Industry. Entre las empresas, DataPixel, expertos en visión 3D, óptica, electrónica e
ingeniería de software y DCM Sistemes, especializados en iluminadores para visión
artificial. Para cerrar esta lista parcial, Association of European Machine Vision
Companies (EMVA), que reúne las empresas y las asociaciones del sector.
1.4 CONCLUSIONES DEL CAPÍTULO 1
En éste capítulo se presentó y detalló el problema de nuestro tema de investigación,
así como los objetivos, alcances y las limitaciones del mismo. En el estado del arte
presentado en la sección anterior se mostraron algunos trabajos relacionados con la
solución del problema, en particular el libro “Multiple View Geometry in Computer
Vision”, de donde se obtuvo la base teórica del tema de calibración y el estudio de la
geometría proyectiva.
El primer paso para encontrar la solución al problema de calibración, fue el estudio del
modelo de la cámara y su representación matemática, el cual se muestra en el
siguiente capítulo.
Los métodos de visión estereoscópica y estimación de pose se usaron para la
recuperación de la información en 3D de un objeto en el espacio aprovechando el
conocimiento de la calibración de las cámaras, explicándose más a detalle en el
capítulo 3.
8 CAPÍTULO 2
2. CALIBRACIÓN DE CÁMARAS
El primer paso para desarrollar un sistema de visión robótica real es el de calibración
de cámaras, con la calibración de cámaras se obtienen los parámetros internos
(intrínsecos) y externos (extrínsecos) de la cámara. El proceso de calibración de
cámara, es necesario para poder extraer información métrica a partir de imágenes en
2D del mundo 3D. El proceso de calibración de cámara abre la posibilidad de realizar
aplicaciones efectivas en visión, tal como el reconocimiento, seguimiento y
reconstrucción 3D, los cuales se basan en el conocimiento de la calibración y pose de
la cámara (Aracena Pizarro, Campos y Clésio 2005).
Para comprender la técnica de calibración y los métodos de recuperación de
coordenadas utilizados, es necesario tener conocimiento de la geometría proyectiva,
pues conceptos como coordenadas homogéneas y modelos de cámaras tienen su
fundamento en ella.
Las secciones de este capítulo proporcionan conceptos, fundamentos,
representaciones, terminologías y notaciones que serán usadas a lo largo de este
documento, como parte central para el análisis, comprensión e implementación de las
técnicas de calibración y recuperación de coordenadas en 3D.
En la primera sección de este capítulo se abordan conceptos para la representación de
las diferentes entidades geométricas (punto, línea y cónica), así como la transformación
proyectiva en 1D y 2D (una dimensión y dos dimensiones respectivamente). Esta
sección es una introducción a la geometría proyectiva.
En la sección dos, se presentan de forma general el esquema de cámara Pinhole, su
funcionamiento básico y el modelo matemático. También se presentan algunas
cuestiones matemáticas relevantes para comprenderlo y entenderlo mejor e ir
presentando de forma gradual un modelo más completo de la cámara en la que se
desarrollará un análisis matemático de una transformación proyectiva, hasta llegar a
obtener un modelo de la “matriz de calibración” y sus diferentes casos típicos.
En la tercera sección de este capítulo, se presentan con mayor detalle conceptos y
desarrollos para las transformaciones proyectivas en dos dimensiones. También se
presentan las transformaciones de líneas, puntos, cónicas, así como las jerarquías de
transformaciones. En esta sección se estudian las transformaciones proyectivas y su
descomposición.
9 Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
En la cuarta sección se presentan los conceptos aplicados para comprender, realizar y
obtener la calibración de una cámara. Se indica el cálculo y estimación de una
homografía, así como algunas de sus configuraciones posibles para el cálculo de una
posible solución a partir de líneas y otras entidades geométricas (puntos, cónicas). En
ésta sección se obtienen los fundamentos básicos para poder realizar la calibración de
cámara, obteniendo la solución en diferentes enfoques, la homogénea, la mínima y la
sobre determinada.
En la última sección se presentan la metodología empleada y los resultados obtenidos
en la calibración de las cámaras con las que se trabajaron para el desarrollo del
proyecto.
2.1 GEOMETRÍA PROYECTIVA EN 2D
Esta sección introduce los principales conceptos geométricos y la notación que se
requiere para entender el material presentado en éste capítulo.
Algunos de estos conceptos son familiares, como la formación de puntos o la
representación de líneas, mientras que otras son más complejas. Estos conceptos
serán planteados en el caso planar o 2D porque así son más fácilmente visualizados.
Comenzaremos por describir la representación de puntos, líneas y cónicas en
coordenadas homogéneas y cómo estas entidades se dibujan bajo transformaciones
proyectivas.
2.1.1 REPRESENTACIÓN HOMOGÉNEA DE LÍNEAS Y PUNTOS
Un punto en un plano puede ser representado por un par coordenado ( x, y ) en R 2 . Se
considerará la proyección entre espacios vectoriales y se representarán tales
proyecciones como matrices. El producto de una matriz y un vector es otro vector, esto
lleva a la distinción entre vectores columnas y vectores renglones, ya que una matriz
puede ser multiplicada por un vector columna por la derecha y por la izquierda por un
vector renglón. Las entidades geométricas por defecto serán representadas por
vectores columna. Un símbolo remarcado x siempre representa un vector columna y
su transpuesta es el vector x T . De acuerdo con esta convención, un punto en el plano
será representado por el vector columna ( x, y ) . Por lo que se escribirá x = ( x, y ) .
T
T
Una recta puede ser expresada por una ecuación de la forma ax + by + c = 0 , donde los
coeficientes a, b y c pueden tener diferentes valores, permitiendo representar así
diferentes líneas. De este modo, una línea puede ser representada por un vector
(a, b, c ) T. La correspondencia de las líneas no es uno a uno, ya que las ecuaciones
10 Capítulo 2 Calibración de cámaras
ax + by + c = 0 y (ka )x + (kb ) y + kc = 0 representa a la misma línea, esto es para cualquier
valor de k diferente de cero. Si la relación entre dos vectores es un factor, que al ser
multiplicado por uno de ellos obtenemos el otro vector, se consideran a estos vectores
equivalentes. Un vector equivalente bajo este tipo de relación es conocido como vector
T
homogéneo. Cualquier vector particular (a, b, c ) es un representativo de la clase
equivalente. Una serie de vectores de clases equivalentes en R 3 , a excepción del
T
vector (0,0,0) , forman el espacio proyectivo Ρ 2 .
Un punto x = ( x,y ) está situado sobre una recta l = (a, b, c)T si y solo si ax + by + c = 0 .
T
Esto puede ser escrito en términos de un producto interno de vectores representantes
T
T
de los puntos, como: ( x, y,1)( a, b, c ) = ( x, y,1) l = 0 , donde el punto (x, y ) en R 2 es
representado como un vector de tres elementos (3-vector) al agregar al final la
coordenada 1. Nótese que para cualquier valor diferente de cero para la constante k y
la línea l , la ecuación ( kx, ky, k ) l = 0 si y solo si ( x, y,1) l = 0 . Por consiguiente es
normal considerar la serie de vectores (kx, ky, k ) para cualquier valor de k ≠ 0 como
T
una representación del punto ( x,y ) en R 2 . Así como las líneas, los puntos pueden ser
T
representados por vectores homogéneos. Un vector homogéneo arbitrario de la forma
T
T
en R 2 .
x = ( x1, x 2, x3) representa al punto x1 , x 2
x3
x3
(
)
2.1.2 GRADOS DE LIBERTAD
Para representar un punto en el espacio R 2 se requiere proporcionar dos valores,
llamados coordenadas x y y , así también para representar una recta de la forma
(ax + by + c = 0) se define un par de parámetros independientes dados por las relaciones
entre los coeficientes {a : b : c} . Entonces los grados de libertad son el número mínimo
de parámetros que se requiere para especificar un punto, una recta o cualquier otra
entidad geométrica sin considerar los valores de sus coeficientes.
Figura 2-1: Representación de puntos en el espacio 1D.
11 Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
2.1.3 GEOMETRÍA PROYECTIVA EN 1D Y 2D
Para continuar, se estudiará el caso particular de 1D dimensión y así entender el
concepto fundamental de la geometría proyectiva. Se puede observar que para
representar un punto X en R1 se requiere sólo de un coeficiente. En la figura 2-2 se
muestra de manera gráfica la representación de este punto. Una forma de representar
un punto en coordenadas homogéneas es agregando al final una coordenada más,
T
teniendo como resultado un punto X = ( x, w ) en R 2 para un plano proyectivo Ρ1 . Con
la geometría proyectiva se logra la representación de un punto en un plano proyectivo.
Una forma de visualizar esto, es suponiendo un punto como origen del que parten
líneas en todas direcciones. En la figura 2-2 se muestra gráficamente esta idea.
Figura 2-2: Geometría proyectiva en 1D.
La geometría proyectiva en 2D es un caso generalizado de la geometría proyectiva en
1D. Con la diferencia de que en éste, los puntos en R 2 son representados por tres
T
coeficientes x = ( x, y, w ) para un plano proyectivo P 2 .
En la figura 2-3 se observa la idea de profundidad en los puntos, se puede ver que los
vectores cuya relación es un coeficiente k ≠ 0 que los multiplica, son equivalentes y
representan al mismo punto en coordenadas homogéneas.
12 Capítulo 2 Calibración de cámaras
Figura 2-3: Geometría proyectiva en 2D.
2.1.4 INTERSECCIÓN DE LÍNEAS Y PUNTOS UNIDOS POR RECTAS
Dadas dos líneas l = ( a, b, c ) y l ′ = ( a′, b′, c′ ) en R 2 ,
queremos encontrar su
intersección. Se define el vector x = l × l ′ , donde × es el producto vectorial o producto
cruz. Por la identidad del triple producto escalar, l ⋅ ( l × l ′ ) = l ′ ⋅ ( l × l ′ ) = 0 tenemos que
T
T
l T x = l ′T x = 0 . De esta forma, si x es la representación de un punto, entonces x se
encuentra en la línea l y en la línea l ′ .
Por lo tanto, la intersección de dos líneas l y l ′ es un punto x = l × l ′ .
Por ejemplo si se desea determinar la intersección de las líneas x = 1 y y = 1. Se tiene
que la línea x = 1 es equivalente a − x + 1 = 0 , su representación homogénea es
T
l = ( −1, 0,1) , la línea y = 1 es equivalente a − y + 1 = 0 , su representación homogénea
es l ′ = ( 0, −1,1) , si aplicamos la relación x = l × l ′ tenemos que:
T
j k ⎤ ⎡1⎤
⎡i
⎢
x = l × l ′ = ⎢ −1 0 1 ⎥⎥ = ⎢⎢1⎥⎥
⎢⎣ 0 −1 1 ⎥⎦ ⎢⎣1⎥⎦
13 Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
Una expresión para una línea que pasa a través de dos puntos x y x ′ puede ser
derivada de un análisis similar al anterior, definiendo a la línea como l = x × x ′ .
2.1.6 LÍNEAS AL INFINITO
Los puntos x = ( x1, x 2, x3) tales que x3 ≠ 0 corresponden a puntos finitos en R 2 . Los
T
puntos con la última coordenada x3 = 0 son conocidos como puntos ideales, o puntos al
T
infinito. Estos puntos pueden ser escritos como ( x1, x 2,0) y están sobre una línea
singular llamada línea al infinito, se representa con el vector l∞ = (0, 0,1)T .
2.1.7 PRINCIPIO DE DUALIDAD
El principio de dualidad dice:
A cada teorema de la geómetra proyectiva bidimensional le corresponde un teorema
dual, el cual es derivado intercambiando los roles de puntos y líneas en el teorema
original.
Por ejemplo la ecuación l T x = 0 , implica que x T l = 0 . El principio de dualidad puede ser
trasladado a conceptos como el de intersección de dos líneas y observar que es dual al
de líneas que unen a dos puntos.
2.1.8 CÓNICAS
La cónica es una de las entidades geométricas importantes cuya representación en
coordenadas no homogéneas es:
ax 2 + bxy + cy 2 + dx + ey + f = 0
Homogenizando al hacer las siguientes sustituciones: x α x1
x3
ax 21 + bx1 x2 + cx 2 2 + dx1 x3 + ex2 x3 + fx 2 3 = 0
y expresada en forma matricial:
14 y y α x2
x3
tenemos:
(2.1)
Capítulo 2 Calibración de cámaras
x T Cx = 0
(2.2)
donde la matriz de coeficientes C está dada por:
b / 2 d / 2⎤
⎡ a
⎢
c
e / 2 ⎥⎥
C = ⎢b / 2
⎢⎣ d / 2 e / 2
f ⎥⎦
(2.3)
Note que la matriz de coeficientes de la cónica C es simétrica. Como en el caso de la
representación homogénea de líneas y puntos, sólo las proporciones de los elementos
de la matriz son importantes, puesto que si a C se le multiplica por un escalar diferente
de cero éste no afecta a la ecuación superior. Así C es la representación homogénea
de una cónica.
Suponga que desea construir una cónica que pase por una serie de puntos xi . ¿Para
definir a una cónica y que esta sea única, cuántos puntos son necesarios? De la
ecuación 2.1 cada punto xi aporta una restricción a los coeficientes de la cónica,
puesto que la cónica pasa a través de ( xi , yi ) entonces:
T
ax 2 i + bxi yi + cy 2 i + dxi + eyi + f = 0
(2.4)
Esta restricción se puede escribir como:
(x
Donde c = ( a b c d
e
2
i
xi yi
y 2i
xi
yi 1) c = 0
f ) , si agrupamos las restricciones de cinco puntos
T
tenemos:
⎡ x 21
⎢ 2
⎢x 2
⎢ x 23
⎢ 2
⎢x 4
⎢ x2
⎣ 5
x1 y1
x2 y2
y 21
y 22
x1
x2
x3 y3
x4 y4
y 23
y 24
x3
x4
x5 y5
y 25
x5
15 y1 1⎤
⎥
y2 1⎥
y3 1⎥ c = 0
⎥
y4 1⎥
y5 1⎥⎦
(2.5)
Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
La ecuación anterior muestra una cónica determinada completamente por cinco puntos
en cualquier posición en general.
2.2 MODELO DE UNA CÁMARA
Es importante conocer el concepto básico de la cámara PINHOLE y de la cámara CCD
que en la actualidad es la tecnología comúnmente utilizada para las cámaras digitales
ya sea para cámaras de video, fotográficas o WebCam's. La cámara es el sensor que
permite realizar una transformación entre el mundo tridimensional (3D) y una imagen
bidimensional (2D). En esta sección se comenzará con un modelo simple de cámara
que es la cámara Pinhole básica. Los modelos desarrollados están principalmente
diseñados para cámaras con sensor tipo CCD (charge-coupled-device), que se
considera ideal y que no existe distorsión radial. A continuación se irán desarrollando
los conceptos básicos, así como su modelo matemático.
2.2.1 MODELO DE CÁMARA PINHOLE
En la figura 2-4 se presenta un esquema de la cámara oscura. Este esquema
representa de forma básica la cámara Pinhole, la cual mediante un orificio muy
pequeño C en una pared deja entrar la luz externa que es proyectada en una pared
interior de la cámara oscura. El resultado en esta cámara es una imagen invertida del
mundo exterior sobre un plano que se encuentra en el interior de ésta.
Figura 2-4: Esquema de cámara oscura.
El modelo de cámara oscura puede ser considerado para ser utilizado como modelo de
una cámara normal CCD, como el esquema que se muestra en la figura 2-5, en la que
16 Capítulo 2 Calibración de cámaras
un lente es el encargado de dejar pasar la luz como el pequeño orificio del esquema
anterior y tenemos como la parte posterior de la cámara un sensor tipo CCD que es el
encargado de recibir la luz del exterior y convertirla a valores de voltajes.
La cámara CCD es un sensor de imagen que utiliza elementos semiconductores
fotosensibles en forma de arreglos matriciales. Los sensores activos de este arreglo
son distribuidos en píxeles individuales. Las cámaras CCD son dispositivos que poseen
una muy baja deformación geométrica de la imagen, una muy buena sensibilidad a la
luz y una muy buena resolución llegando típicamente a los 400,000 pixeles. El tiempo
de captura de la imagen esta típicamente en el rango de 1/60s y 1/10000s.
Si nos fijamos en la figura 2-5, podemos ver que la imagen en el CCD está invertida,
pero mediante procesamiento electrónico las cámaras digitales nos entregan una
imagen derecha, es decir el plano de la imagen que nosotros vemos está en forma
normal y sin inversión. Con esto podemos representar de una forma más simplificada el
modelo anterior suponiendo un plano entre la lente y mundo real lo que nos dará una
proyección del mundo real sobre una imagen bidimensional en forma no invertida tal
como se obtiene en una cámara normal. Esta suposición nos simplificará el modelo de
la cámara. Esta se muestra de manera gráfica en el esquema de la figura 2-6.
Figura 2-5: Esquema de una cámara CCD.
17 Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
Figura 2-6: Esquema simplificado de una cámara.
Figura 2-7: Modelo Pinhole simplificado.
A la proyección de puntos que pasan por un centro de proyección C se le conoce como
proyección central y es mostrada en la figura 2-7, donde la línea de proyección del
punto P pasa por un centro de proyección.
Considérese una proyección central de puntos en el espacio sobre un plano. Dejando
que el centro de proyección sea el origen de un sistema de coordenadas cartesianas
considérese el plano z = f , que es llamado el plano de la imagen o plano focal. Bajo el
modelo de la cámara Pinhole, un punto en el espacio con coordenadas P = ( X , Y , Z )
T
es proyectado a un punto en el plano de la imagen donde una línea une el punto P con
el centro de proyección a través del plano de la imagen. Esto se puede ver en la figura
18 Capítulo 2 Calibración de cámaras
2-8. Se tomará el modelo simplificado haciendo la consideración ya antes mencionada
quedando un esquema que se muestra en la figura 2-9.
Por triángulos semejantes, se puede observar que el punto ( X , Y , Z ) es proyectado al
T
( fX Z , fY Z )
T
punto
sobre el plano de la imagen.
( X ,Y , Z ) a
T
(
fX
Z
, fY
)
T
Z
(2.6)
Si el mundo y puntos de la imagen son representados por vectores homogéneos,
entonces la proyección es simplemente expresada como una función lineal entre estas,
los vectores en coordenadas homogéneas y pueden ser escritos en términos de una
multiplicación de matrices como:
⎛X⎞
⎜ ⎟
⎜Y ⎟
⎜Z ⎟α
⎜ ⎟
⎜1⎟
⎝ ⎠
⎛ fX ⎞ ⎡ f
⎜ ⎟ ⎢
⎜ fY ⎟ = ⎢
⎜ Z ⎟ ⎢
⎝ ⎠ ⎣
f
⎛X⎞
0⎤ ⎜ ⎟
⎜Y ⎟
0⎥⎥⎜ ⎟
Z
1 0⎥⎦⎜⎜ ⎟⎟
⎝1⎠
(2.7)
también puede ser escrita como diag ( f , f ,1)[I | 0] , donde diag( f , f ,1) es una matriz
diagonal y [ I | 0] representa a una matriz compuesta por un bloque de 3 × 3 (la matriz
identidad) más un vector columna de ceros.
Si se introduce la notación X para los puntos en el mundo representados por un vector
T
homogéneo ( X , Y , Z ,1) , x para los puntos de la imagen representados por un vector
homogéneo vector-3, y P para una matriz homogénea de 3 × 4 definida como
P = diag ( f , f ,1)[I | 0] llamada matriz de cámara, podemos escribir la ecuación 2.7 de
forma compacta como x = PX .
Figura 2-8: Esquema simplificado de cámara.
19 Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
Figura 2-9: Triángulos semejantes.
Figura 2-10: Sistema de coordenadas de la imagen (
x, y )
y de la cámara (
xcam, ycam ).
2.2.2 TRASLACIÓN DE UN PUNTO PRINCIPAL
En la ecuación 2.6 se asume que el origen de las coordenadas en la imagen está en el
punto principal. En la práctica esto no es así, en general la proyección es:
(x, y, z )T α
⎛⎜ fx + p , fy + p ⎞⎟
x
y
z
⎝ z
⎠
T
Donde ( px , p y )T son las coordenadas del punto principal, figura 2.10, por lo que
podemos reescribir las ecuaciones en coordenadas homogéneas como:
20 Capítulo 2 Calibración de cámaras
⎛X ⎞
⎜ ⎟
⎜Y ⎟
⎜Z ⎟α
⎜ ⎟
⎜1⎟
⎝ ⎠
⎛ fX + Zp x ⎞ ⎡ f
⎜
⎟ ⎢
⎜ fY + Zp y ⎟ = ⎢
⎜
⎟ ⎢
Z
⎝
⎠ ⎣
px
f
py
1
⎛X⎞
0⎤⎜ ⎟
⎜Y ⎟
0⎥⎥⎜ ⎟
Z
0⎥⎦⎜⎜ ⎟⎟
⎝1⎠
(2.8)
2.2.4 MATRIZ DE CALIBRACIÓN
Ahora si se define,
⎡f
K = ⎢⎢
⎢⎣
f
px ⎤
p y ⎥⎥
1 ⎥⎦
(2.9)
Entonces la ecuación 2.8 puede ser expresada de forma compacta como:
x = K [ I | 0] Xcam
(2.10)
La matriz K es llamada la matriz de calibración de cámara. Xcam es el punto en el
mundo representado por un vector homogéneo ( X , Y , Z ,1) y asume que estos puntos
T
se encuentran localizados en el origen del sistema de coordenadas Euclidianas con el
eje principal de la cámara, orientado en línea recta hacia el eje Z.
En la matriz K se supone que las coordenadas de la imagen están en coordenadas no
homogéneas y tienen una escala igual para ambos ejes de direcciones x y y . En el
caso de las cámaras CCD, existe la posibilidad de que los pixeles no sean cuadrados,
además si las coordenadas de la imagen son medidas en pixeles, esto tendrá el efecto
de introducir factores de escala diferentes en cada dirección. Si el número de píxeles
por unidad de distancia en coordenadas de imagen son mx y m y en las direcciones x y
y respectivamente, entonces la transformación desde las coordenadas del mundo a
las coordenadas de píxeles se obtiene al multiplicar la matriz de la ecuación 2.9 por un
factor diag (mx , m y ,1) , así la forma general de la matriz de calibración de cámara es:
⎡α x
K = ⎢⎢
αy
⎣⎢
x0 ⎤
y0 ⎥⎥
1 ⎦⎥
(2.11)
Donde α x = fmx y α y = fm y representan la longitud focal en términos de las
dimensiones de los pixeles en las direcciones x y y respectivamente. De manera
21 Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
similar x0 = mx p x y y0 = m y p y es el punto principal en términos de las dimensiones de
los pixeles.
De forma general se considera a la matriz de calibración de cámara de la siguiente
forma:
⎡α x s
K = ⎢⎢
αy
⎢⎣
x0 ⎤
y0 ⎥⎥
1 ⎥⎦
(2.3.4.4)
El parámetro agregado s es conocido como parámetro de oblicuidad y se refiere al
ángulo que existe entre los ejes en las direcciones x y y de los pixeles.
2.2.5 ROTACIÓN Y TRASLACIÓN EN 3-D
En general, los puntos en el espacio serán expresados en términos de un marco
diferente de coordenadas Euclidianas, conocido como el marco coordenado del mundo.
Dos marcos coordenados son relacionados por una rotación y una traslación. Ver figura
% es un 3-vector no homogéneo que representa las coordenadas de un punto
2-11. Si X
% representa el mismo punto en el marco
en el marco coordenado del mundo, X
cam
% = R( X
%− C%) , donde C~ representa las
coordenado de la cámara, entonces X
cam
coordenadas del centro de la cámara en el marco coordenado del mundo y R es la
matriz de rotación de 3 × 3 representando a la orientación del marco coordenado de la
cámara. Esta ecuación puede ser escrita en coordenadas homogéneas como:
X cam
⎛X⎞
⎜ ⎟
⎡ R − RC ⎤ ⎜ Y ⎟ ⎡ R − RC ⎤
X
=⎢
=
1 ⎥⎦ ⎜ Z ⎟ ⎢⎣ 0
1 ⎥⎦
⎣0
⎜ ⎟
⎝1⎠
(2.12)
Escribiendo de forma compacta queda expresada como:
x = KR[ I | −C%]X
(2.13)
Donde x está ahora en el marco coordenado de la imagen. Ésta es la proyección
general dada por una cámara Pinhole. Se puede observar que el modelo de la cámara
Pinhole está representado por P = KR[ I | −C~ ] . A menudo es conveniente hacer
explicito el centro de la cámara y en lugar de eso representar el mundo como una
% = R( X
%− C%) , en este caso la matriz P es la matriz de
transformación de imagen X
cam
cámara.
22 Capítulo 2 Calibración de cámaras
Figura 2-11: Transformación euclidiana entre el marco coordenado del mundo y la cámara.
2.2.6 MATRIZ DE CÁMARA
En general la matriz de cámara pinhole P representa la transformación de puntos de
un marco coordenado del mundo real a un marco coordenado en la imagen como
x = PX y está compuesta por:
⎡ p11
P = ⎢⎢ p21
⎢⎣ p31
p12
p22
p13
p23
p32
p33
p14 ⎤
p24 ⎥⎥
p34 ⎥⎦
Dentro de esta matriz de cámara están contenidos los parámetros:
Internos (Intrínsecos) K α x ,α y , s , p x , p y y externos (extrínsecos) R, C .
2.2.7 CÁMARA PROYECTIVA FINITA
Una matriz de cámara P = KR[ I | −C~ ] donde la matriz de calibración de cámara es de la
forma:
⎡α x s
K = ⎢⎢
αy
⎢⎣
x0 ⎤
y0 ⎥⎥
1 ⎥⎦
en el que fue agregado el factor s que es el factor de inclinación, este parámetro
normalmente es cero, es llamada cámara proyectiva finita. Una cámara proyectiva finita
23 Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
tiene 11 grados de libertad. Este es el mismo número de grados de libertad que tiene
una matriz 3 × 4, definiendo un factor de escala arbitrario ( p34 − 1) , es decir está
definida salvo un escalar. La submatriz KR de 3 × 3 del lado izquierdo es una matriz no
M . Por lo que
P
puede ser redefinida como
singular llamada
~
−1
P = M [ I | M p4 ] = KR[ I | −C ] . Donde M = KR puede ser descompuesta en los valores
de K y R utilizando la descomposición RQ . La descomposición RQ separa una matriz
en el producto de una matriz triangular superior y una matriz ortogonal.
2.2.8 CÁMARA PROYECTIVA
Una cámara proyectiva general está representada por una matriz de 3 × 4 arbitraria
homogénea de rango 3 y tiene 11 grados de libertad. El rango tres es un requisito
necesario para esta matriz, si el rango es menor a 3 entonces proyectará a una línea o
punto y no un plano; en otras palabras no proyectará a una imagen 2D.
2.3 TRANSFORMACIONES PROYECTIVAS EN 2D
En esta sección se estudian las transformaciones proyectivas en 2D, así como los
modelos de transformación para las diferentes entidades geométricas (puntos, líneas,
cónicas). Dichas transformaciones son estudiadas y modeladas con una matriz de
transformación H . Estos modelos proporcionan una representación matemática
apropiada para el cálculo computacional.
2.3.1 TRANSFORMACIONES DE PUNTOS
La geometría es el estudio de las propiedades invariantes de las entidades geométricas
bajo un grupo de transformaciones. Desde este punto de vista, la geometría proyectiva
en 2D es el estudio de las propiedades del plano proyectivo Ρ 2 que es invariante bajo
un grupo de transformaciones conocidas como proyectividades.
Definición: Una proyectividad es una función invertible h de Ρ 2 en sí mismo tal que
tres puntos cualesquiera x1 , x2 y x3 se encuentran en una línea si y solo si h( x1 ) ,
h( x2 ) , h( x3 ) también se encuentran en un línea.
Las proyectividades forman un grupo, puesto que la inversa de una proyectividad forma
también una proyectividad. Una proyectividad es también llamada una transformación
proyectiva o una homógrafia, estos términos son sinónimos.
24 Capítulo 2 Calibración de cámaras
En la definición anterior, proyectividad se definió en términos de un concepto
geométrico libre de coordenadas de un punto que se encuentran sobre una línea. Es
posible obtener un equivalente de la definición de proyectividad, que resulta lo siguiente
resulta:
Teorema: Una transformación h : Ρ 2 a Ρ 2 es una proyectividad si y solo si existe una
matriz no singular H tal que para cualquier punto en Ρ 2 representado por un vector x
se cumple que h( x ) = Hx .
Para interpretar este teorema, cualquier punto en Ρ 2 es representado como x , un 3vector homogéneo, y Hx como una transformación o función lineal de coordenadas
homogéneas. El teorema no será demostrado completamente, solamente se mostrará
que cualquier transformación lineal de coordenadas homogéneas es una proyectividad.
Demostración: Sean x1 , x2 y x3 puntos situados en la línea l . Así l T xi = 0 para
i=1,...,3. Sea H una matriz no singular de 3 × 3. Podemos verificar que lH −1 Hxi = 0 . De
esta manera, todos los puntos Hxi están sobre la línea H −1 xi , y la colinealidad es
preservada por la transformación.
Una transformación proyectiva plana es una transformación lineal en coordenadas
homogéneas representada por una matriz de 3 × 3 :
⎛ x1′ ⎞ ⎡ h11
⎜ ⎟ ⎢
⎜ x2′ ⎟ = ⎢h21
⎜ x′ ⎟ ⎢ h
⎝ 3 ⎠ ⎣ 31
h12
h22
h32
h13 ⎤⎛ x1 ⎞
⎜ ⎟
h23 ⎥⎥⎜ x2 ⎟
h33 ⎥⎦⎜⎝ x3 ⎟⎠
(2.14)
o más brevemente x ′ = Hx .
La matriz H puede ser cambiada por una multiplicación por un factor arbitrario de
escala diferente de cero sin alterar la transformación proyectiva, por lo que se dice que
H es una matriz homogénea.
2.3.2 TRANSFORMACIÓN DE LÍNEAS
Si los puntos xi están situados sobre la línea l , entonces la transformación de los
puntos xi′ = Hxi bajo una transformación proyectiva, se encuentran sobre la línea
l ′ = Hl . De esta manera los puntos que se encuentran sobre la línea se mantienen,
dado que l ′T xi′ = l T H −1 Hxi = 0 . Éste da la regla de transformación para líneas.
Dada una trasformación de puntos x ′ = Hx una línea es trasformada como:
25 Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
l ′ = H −1l
(2.15)
Una forma alternativa es l ′T = l T H −1 .
2.3.3 TRANSFORMACIÓN DE CÓNICAS
Dada la transformación de puntos x ′ = Hx la ecuación de la cónica se transforma en:
T
x T Cx = x ′T ⎡⎣ H −1 ⎤⎦ CH −1 x ′
x T Cx = x ′T ⎡⎣ H −T ⎤⎦ CH −1 x′
(2.16)
en una forma cuadrática x ′T C ′x ′ , con C ′ = H −T CH −1 , es la regla de las transformaciones
de cónicas.
2.3.4 JERARQUÍA DE TRANSFORMACIONES
En esta sección se describen características importantes de una transformación
proyectiva y algunas de sus propiedades geométricas. Se presentan las
transformaciones en orden de complejidad comenzando por la más particular, la
isometría, y generalizando sucesivamente hasta llegar a la transformación proyectiva.
La jerarquía de las transformaciones es:
• Isometrías
• Similaridades
• Transformaciones afines o afinidades
• Transformaciones proyectivas
• Descomposición de una transformación proyectiva
• Invariantes
A continuación se explica más a detalle cada una de las transformaciones.
26 Capítulo 2 Calibración de cámaras
Isometrías
Las isometrías son transformaciones del plano R 2 que preservan la distancia
Euclidiana (iso = similar, metric = medida). Una isometría es representada como:
⎛ x′ ⎞ ⎡ε cos θ
⎜ ⎟ ⎢
⎜ y′ ⎟ = ⎢ ε sin θ
⎜1⎟ ⎢ 0
⎝ ⎠ ⎣
− sin θ
cos θ
t x ⎤⎛ x ⎞
⎜ ⎟
t y ⎥⎥⎜ y ⎟
1 ⎥⎦⎜⎝ 1 ⎟⎠
0
(2.17)
donde ε = ±1. Si ε = 1 la isometría preserva la orientación y es una transformación
euclidiana (una composición de traslación y rotación). Si ε = −1 entonces la isometría
tiene una orientación invertida.
La transformación euclidiana modela el movimiento de un objeto rígido.
Una transformación euclidiana planar puede ser escrita en una forma compacta como:
⎡R
x′ = H E x = ⎢ T
⎣0
t⎤
1⎥⎦
(2.18)
donde R es una matriz de rotación de 2× 2 y t es un 2-vector de traslación.
En esta transformación las características preservadas o invariantes son: la distancia
(entre dos puntos), el ángulo (entre dos líneas) y el área.
Similaridades
Una similaridad es una isometría compuesta con un factor de escala isotrópico. En el
caso de una transformación Euclidiana compuesta con un escalamiento la similaridad
tiene la siguiente representación matricial:
⎛ x′ ⎞ ⎡ s cos θ
⎜ ⎟ ⎢
⎜ y′ ⎟ = ⎢ s sin θ
⎜1⎟ ⎢ 0
⎝ ⎠ ⎣
− s sin θ
s cos θ
0
t x ⎤⎛ x ⎞
⎜ ⎟
t y ⎥⎥⎜ y ⎟
1 ⎥⎦⎜⎝ 1 ⎟⎠
(2.19)
que puede ser escrita de forma compacta como:
⎡ sR t ⎤
x′ = H s x = ⎢ T
⎥
⎣ 0 1⎦
27 (2.20)
Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
Donde el escalar s representa el escalamiento isotrópico. Las características
invariantes son los ángulos entre líneas, paralelismo, relación entre longitudes,
relación entre áreas.
Transformaciones afines o afinidades
Una afinidad es una transformación lineal no singular seguida por una traslación. Ésta
tiene la representación matricial:
⎛ x′ ⎞ ⎡ a11
⎜ ⎟ ⎢
⎜ y′ ⎟ = ⎢a21
⎜1⎟ ⎢ 0
⎝ ⎠ ⎣
a12
a22
0
t x ⎤⎛ x ⎞
⎜ ⎟
t y ⎥⎥⎜ y ⎟
1 ⎥⎦⎜⎝ 1 ⎟⎠
(2.21)
o en bloques queda de la forma:
⎡ A t⎤
x′ = H A x = ⎢ T
⎥
⎣0 1⎦
(2.22)
donde A es una matriz no singular de 2 × 2.
Las características invariantes de esta transformación son el paralelismo, la relación de
longitudes de segmentos de líneas, la relación entre áreas.
Una forma de entender los efectos geométricos de la componente lineal A de una
afinidad, es la composición de dos transformaciones fundamentales, llamadas rotación
y escalamiento no isotrópico. La matriz de afinidad A puede siempre ser
descompuesta como:
A = R(θ ) R(−φ ) DR(φ )
(2.23)
donde R(θ ) y R(φ ) son las rotaciones para θ y φ respectivamente, y D es una matriz
diagonal:
⎡λ 0 ⎤
D=⎢ 1
⎥
⎣ 0 λ2 ⎦
Por lo tanto, la matriz de afinidad A es el producto de una rotación ( φ ); un
escalamiento por λ1 y λ2 en la dirección x y y respectivamente; una rotación de
regreso (por − φ ); y finalmente otra rotación (por θ ). La única diferencia entre la
afinidad y una similaridad, es el escalamiento no isotrópico. La esencia de una afinidad
28 Capítulo 2 Calibración de cámaras
es este escalamiento en direcciones ortogonales, orientada por un ángulo particular. En
la figura 2-13, se ve un ejemplo de esto.
Transformaciones proyectivas
Una transformación proyectiva plana es una transformación lineal en coordenadas
homogéneas representada por una matriz 3× 3 :
⎛ x1′ ⎞ ⎡ h11
⎜ ⎟ ⎢
⎜ x2′ ⎟ = ⎢h21
⎜ x′ ⎟ ⎢ h
⎝ 3 ⎠ ⎣ 31
h12
h22
h32
h13 ⎤⎛ x1 ⎞
⎜ ⎟
h23 ⎥⎥⎜ x2 ⎟
h33 ⎥⎦⎜⎝ x3 ⎟⎠
o de forma compacta x ′ = Hx .
Ésta en general, es una transformación lineal no singular de coordenadas
homogéneas. Ésta generaliza una transformación de afinidad, que es la composición
de una transformación lineal no singular de coordenadas no homogéneas y una
traslación. Se puede representar como:
⎡A
x′ = H P x = ⎢ T
⎣v
t⎤
v ⎥⎦
(2.24)
T
donde el vector v = (v1 , v2 ) , es un factor de escalamiento para x y y respectivamente.
No siempre es posible tener el factor de escala v de la matriz con valor igual a uno
porque puede tener también valor de cero.
La característica invariante más importante es la relación cruz de cuatro puntos
colineales: una relación de longitudes sobre una línea es invariante bajo las afinidades,
pero no bajo las proyectividades. Sin embargo, una relación de relaciones o relación
cruz de longitudes sobre una línea es una descripción invariante.
Descomposición de una transformación proyectiva
Una transformación proyectiva puede ser descompuesta en una cadena de
trasformaciones, donde cada matriz en la cadena representa una transformación de
más alta jerarquía que la matriz anterior.
⎡ sR t ⎤ ⎡ K
H = HS H AHP = ⎢ T ⎥ ⎢ T
⎣ 0 1⎦ ⎣0
29 t⎤ ⎡ I
1⎥⎦ ⎢⎣v T
t⎤ ⎡ A
=
v ⎥⎦ ⎢⎣v T
t⎤
v ⎥⎦
(2.25)
Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
donde A es una matriz no singular dada por A A = sRK + tv T , y K es una matriz
triangular superior normalizada con det K = 1 . Esta descomposición es válida solo si
v ≠ 0 , y es única si s es positivo.
Figura 2-12: Distorsión originada por una trasformación de afinidad plana.
Figura 2-13: Trasformación proyectiva entre líneas.
2.4 ESTIMACIÓN DE UNA HOMOGRAFÍA Y CÁLCULO DE LA MATRIZ DE
CÁMARA P
En esta sección se abordará el problema de la estimación de la homografía, el presente
contexto será tomado para el cálculo de algunas transformaciones o de otras entidades
matemáticas basadas en mediciones de alguna naturaleza. Esta definición es un tanto
vaga, tendremos que hacerla más concreta, por lo que a continuación que se irán
presentando diferentes casos de estudio.
30 Capítulo 2 Calibración de cámaras
Homografía en 2D: Dados una serie de puntos xi en el plano proyectivo Ρ 2 y una
correspondiente serie de puntos x ′ también en Ρ 2 , se determina la transformación
proyectiva que toman cada uno de los puntos xi en x′ . En la práctica, los puntos xi y
x ′ , son puntos situados en dos imágenes o en la misma imagen, cada imagen es
considerada como un plano proyectivo Ρ 2 .
Consideraremos una serie de correspondencias xi ↔ xi′ entre dos imágenes. El
problema es el cálculo de una matriz H de 3 × 3 tal que Hxi = xi′ para cada valor de i .
La primera cuestión es, cuántos puntos de correspondencia xi ↔ xi′ son necesarios
para calcular la matriz de transformación H . La matriz de transformación H contiene 9
constantes de entrada, pero si definimos el límite de escala, el total de grados de
libertad en una transformación proyectiva son 8. Por otra parte, cada correspondencia
punto a punto cuenta para un par de restricciones, puesto que para cada punto xi , en
la primera imagen los dos grados de libertad del punto de la segunda imagen
corresponden a la transformación del punto Hxi . Un punto en 2D tiene dos grados de
libertad correspondientes a las componentes x y y , cada una de las cuales puede ser
especificada separadamente. En otro caso, el punto es especificado como un vector-3
homogéneo, que también tienen dos grados de libertad. Como consecuencia
solamente son necesarios cuatro puntos para especificar completamente las
restricciones de H .
Dicho lo anterior, podemos continuar con la solución de la matriz de transformación H .
2.4.1 SOLUCIÓN HOMOGÉNEA
Para determinar H se da un conjunto de correspondencias de cuatro puntos en 2D,
xi ↔ xi′ . La transformación está dada por la ecuación xi = Hxi′ . Ésta es una ecuación
que tiene vectores homogéneos; estos vectores-3 xi y Hxi′ no son iguales, pero
tienen la misma dirección con diferente magnitud. Este vector puede ser expresado en
términos de un producto cruz como:
xi′ × Hxi = 0
(2.26)
ésta forma permitirá obtener y deducir una simple solución lineal para H .
Si el i-ésimo renglón de H está representado por hiT , entonces podremos escribir:
31 Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
⎡ h1T xi ⎤
⎢
⎥
Hxi = ⎢h 2T xi ⎥
⎢ h 3T xi ⎥
⎣
⎦
(2.27)
escribiendo xi′ = ( xi′, yi′, wi′) , el producto cruz puede ser expresado explícitamente como:
⎡ yi′h3T xi − wi′h 2T xi ⎤
⎢
⎥
xi′ × Hxi = ⎢ wi′h1T xi − xi′h3T xi ⎥
⎢ xi′h 2T xi − yi′h1T xi ⎥
⎣
⎦
(2.28)
puesto que h jT xi = x T i x j para j =1,…,3 , tenemos:
⎡ 0T
⎢
T
⎢ wi′x i
⎢ − yi′xT i
⎣
− wi′xT i
0T
xi′xT i
yi′xT i ⎤ ⎛ h1 ⎞
⎥⎜ ⎟
− xii′ xT i ⎥ ⎜ h 2 ⎟ = 0
0T ⎥⎦ ⎜⎝ h3 ⎟⎠
(2.29)
la ecuación tiene la forma Ai h = 0 . Donde Ai es una matriz de 3 × 9, y h es un vector-9
hecho de los valores internos de la matriz H .
⎡ h1 ⎤
⎡ h1
⎢ 2⎥
h = ⎢ h ⎥ , H = ⎢⎢ h4
⎢ h3 ⎥
⎢⎣ h7
⎣ ⎦
h2
h5
h8
h3 ⎤
h6 ⎥⎥
h9 ⎦⎥
(2.30)
con hi como el i-ésimo termino de h . Hay que notar tres cosas importantes relativas a
estas ecuaciones, que son:
1. La ecuación Ai h = 0 es una ecuación lineal con h de incógnita. La matriz de
elementos de Ai es cuadrática, en ella se conocen las coordenadas de los
puntos.
2. Puesto que las ecuaciones de tres por tres, ecuación 2.29, solamente dos de
ellas son linealmente independientes, usualmente se omite la tercera ecuación.
32 Capítulo 2 Calibración de cámaras
Entonces el conjunto de ecuaciones queda como:
⎡ 0
⎢ T
⎣ wi′x i
T
− wi′x
0T
T
⎛ h1 ⎞
yi′x i ⎤ ⎜ 2 ⎟
⎥ h =0
− χ i′xT i ⎦ ⎜⎜ 3 ⎟⎟
⎝h ⎠
T
i
(2.31)
Ésta será escrita como Ai h = 0 que es una matriz de 2× 9 .
3. En la ecuación se mantiene, para cada punto xi , la representación en
T
coordenadas homogéneas como ( xi′, yi′, wi′ ) . Un caso es cuando wi′ = 1 , que
significa que las coordenadas ( xi′, yi′ ) están medidas en la imagen.
Cada correspondencia dada, genera dos ecuaciones
correspondencias tenemos Ai h = 0 , donde A es:
⎡ 0
⎢ w′x
⎢ 1 1
⎢ 0
A=⎢
⎢ w2′ x2
⎢ 0
⎢
⎢⎣ w3′ x3
0
w1′ y1
0
w2′ y2
0
w3′ y3
0
w1′w1
0
w2′ w2
0
w3′ w3
− w1′x1
0
− w2′ x2
0
− w3′ x3
0
− w1′ y1
0
− w2′ y2
0
− w3′ y3
0
− w1′w1
0
− w2′ w2
0
− w3′ w3
0
independientes,
y1′x1
− x1′x1
y2′ x2
− x2′ x2
y3′ x3
− x3′ x3
y1′ y1
− x1′ y1
y2′ y2
− x2′ y2
y3′ y3
− x3′ y3
con
cuatro
y1′w1 ⎤
− x1′w1 ⎥⎥
y2′ w2 ⎥
⎥ (2.32)
− x2′ w2 ⎥
y3′ w3 ⎥
⎥
− x3′ w3 ⎥⎦
Con la cual se busca un valor diferente de cero para h .
2.4.2 ALGORITMO DE TRANSFORMACIÓN LINEAL DIRECTA
{
}
Dados n ≥ 4 correspondencias de puntos de 2D a 2D xi ↔ xi′ , se determina la matriz
{
}
de la homografía en 2D tal que xi ↔ Hxi′ .
Algoritmo
{
}
1. Para cada correspondencia xi ↔ xi′ se determina la matriz A . En general
solamente las primeras dos columnas necesitan ser usadas.
2. Se ensamblan las n matrices Ai de 2 × 9 dentro de una matriz A de 2n × 9 .
33 Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
3. Se obtiene la SVD (Singular value decomposition) de A . El vector singular
corresponde al valor más pequeño es la solución de h . Específicamente, si
A = UDV T con la matriz D con los elementos positivos, arreglados en orden
descendente. Entonces h es la última columna de V .
4. La matriz H es determinada a partir de h .
2.4.3 CONFIGURACIONES DEGENERADAS
Si se considera la solución mínima de una homografía, que es calculada usando cuatro
correspondencia de puntos, y suponiendo que tres de los puntos x1 , x2 , x3 son
colineales y si la correspondencia los puntos x1′ , x2′ , x3′ también son colineales
entonces la homografía no está suficientemente restringida y existirá una familia de
homografías que transformen de x1 a x1′ , por otra parte si los correspondientes puntos
x1′ , x2′ , x3′ no son colineales, entonces H no puede ser transformación de xi a xi′ ,
puesto que en transformación proyectiva conserva la colinealidad. No obstante el
conjunto de las ocho ecuaciones homogéneas obtenidas de 2.31. Debe tener una
solución diferente de cero, dando origen a una matriz H . Cuando se tiene una familia
de soluciones para la matriz H , es decir, que no tiene una solución única se le conoce
como Configuración degenerada.
Por lo tanto, si una transformación H tiene rango 1, quiere decir que:
1. Hay una dependencia lineal en sus renglones
2. Esta transformación realizará proyecciones en 2D a 1D, esto es, de un plano a
una línea.
2.4.4 SOLUCIÓN A PARTIR DE LÍNEAS Y OTRAS ENTIDADES
Los desarrollos antes presentados son exclusivamente de homografías calculadas a
partir de correspondencias de puntos. Sin embargo un desarrollo similar puede ser
empleado para el cálculo de homografías a partir de correspondencias de líneas.
Comenzando desde la transformación de línea li = H T li′ , podemos deducir una
ecuación matricial de la forma Ah = 0 . De modo semejante, una homografía puede ser
determinada a partir de correspondencias entre cónicas.
Por lo tanto una homografía puede ser determinada por correspondencias de:
1. Puntos
34 Capítulo 2 Calibración de cámaras
2. Líneas
3. Cónicas
4. Combinación de líneas y puntos, etc.
Suponiendo que se tienen correspondencias de puntos Xi ↔ xi entre puntos en 3D y
puntos en 2D de una imagen y se desea encontrar una matriz de cámara P de 3 × 4 tal
que xi = PXi para toda i. Los puntos en 3D son homogéneos por lo que son vectores
de 4 elementos ( X , Y , Z ,W )
T
y será W = 1 .
Tal como se hizo para el cálculo de una homografía se puede utilizar la relación:
⎡ 0T
⎢
T
⎣ wi X i
− wi XT i
0T
⎛ P1 ⎞
yi XT i ⎤ ⎜ 2 ⎟
⎥ P
− xi XT i ⎦ ⎜⎜ 3 ⎟⎟
⎝P ⎠
(2.33)
quedando expresada de la forma AP = 0 , donde A es una matriz de 2n × 12 . La matriz
de proyección P es calculada por la solución del conjunto de ecuaciones Ap = 0 ,
donde p es el vector que contiene los elementos de entrada de la matriz P .
2.4.6 SOLUCIÓN NO HOMOGÉNEA
Suponiendo que los vectores xi y su correspondiente xi′ son vectores homogéneos se
plantean las matrices de transformación. Para el caso de la matriz de cámara las
correspondencias serán representadas como xi ↔ Xi , donde xi son los puntos en la
imagen 2D y Xi son los puntos del mundo real 3D. En el caso de la matriz de cámara,
generalmente, el plano proyectivo no se encuentra a una distancia unitaria del origen.
Es decir una transformación dada por xi = PXi donde xi representa un punto en la
imagen representado por el vector (ρxi , ρyi , ρ ) y está dada como:
T
⎡ ρxi ⎤ ⎡ p11
⎢ ρy ⎥ = ⎢ p
⎢ i ⎥ ⎢ 21
⎢⎣ ρ ⎥⎦ ⎢⎣ p31
p12
p13
p22
p32
p23
p33
35 ⎡X ⎤
p14 ⎤ ⎢ i ⎥
Y
p24 ⎥⎥ ⎢ i ⎥
⎢Z ⎥
p34 ⎥⎦ ⎢ i ⎥
⎣1⎦
(2.32)
Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
⎡ ρxi ⎤ ⎡ p11 X i + p12Yi + p13 Z i + p14 ⎤
⎢ ρy ⎥ = ⎢ p X + p Y + p Z + p ⎥
22 i
23 i
24 ⎥
⎢ i ⎥ ⎢ 21 i
⎢⎣ ρ ⎥⎦ ⎢⎣ p31 X i + p32Yi + p33 Z i + p34 ⎥⎦
(2.33)
Para tener un vector homogéneo, dividimos ambos lados de la ecuación lineal entre el
último elemento, quedando como:
⎡ p11 X i + p12Yi +
⎢
⎡ xi ⎤ ⎢ p31 X i + p32Yi +
⎢ y ⎥ = ⎢ p21 X i + p22Yi +
⎢ i⎥ ⎢ p X + p Y +
32 i
⎢⎣ 1 ⎥⎦ ⎢ 31 i
1
⎢
⎣
p13 Z i + p14 ⎤
p33 Z i + p34 ⎥
⎥
p23 Z i + p24 ⎥
p33 Z i + p34 ⎥
⎥
⎥
⎦
(2.34)
Al despejar las ecuaciones e igualar a cero, quedan expresadas como:
p11 X i + p12Yi + p13 Z i + p14 + xi p31 X i + xi p32Yi + xi p33 Z i + xi p34 = 0
p21 X i + p22Yi + p23 Z i + p24 + yi p31 X i + yi p32Yi + yi p33 Z i + yi p34 = 0
(2.35)
éste par de ecuaciones se obtiene por casa i-ésimo punto de correspondencia entre
xi ↔ Xi .
2.4.7 SOLUCIÓN MÍNIMA
La matriz P tiene 12 términos, e ignorando el factor de escala, tiene 11 grados de
libertad, entonces es necesario tener 11 ecuaciones para resolver el sistema para P .
Dado que de cada punto de correspondencia se obtienen dos ecuaciones, se requiere
un mínimo de 5½ correspondencias para resolver P . El ½ indica que del sexto punto
se usa solamente una de las ecuaciones, se necesita únicamente conocer la
coordenada x (o alternativamente la y ) del sexto punto de la imagen.
Dado el número mínimo de correspondencias, la solución es exacta al resolver Ap = 0
dónde A en este caso es una matriz de 11×12 . En general A será de rango 11.
2.4.8 SOLUCIÓN SOBRE-DETERMINADA
Si por ruido en las coordenadas del punto no son exactas y se tiene n ≥ 6
correspondencias de puntos, entonces no obtendrá una solución exacta para la
36 Capítulo 2 Calibración de cámaras
ecuación Ap = 0 . Como en la estimación de la homografía una solución para P podría
ser obtenida mediante la minimización del error algebraico o geométrico.
La aproximación del error se da, minimizando
Ap
sujeto a normalización de
restricciones. Las posibles restricciones son:
1.
p = 1;
2.
pˆ 3 = 1 , donde p̂3 es el vector ( p31 , p32 , p33 ) .
T
Así que el error en las posiciones de los puntos genera error en P , y al utilizar más de
6 puntos y resolver por mínimos cuadrados mejoramos la precisión de P .
2.5 RESULTADOS DE LA CALIBRACIÓN DE CÁMARAS
En esta sección se mencionan los resultados obtenidos en la calibración de las
cámaras Logitech 1 y 2. Se enumeraron debido a que cada cámara a pesar de ser el
mismo modelo, tienen parámetros intrínsecos distintos. Se muestra el patrón utilizado,
los programas elaborados y las imágenes obtenidas y utilizadas.
2.26 PATRÓN DE CALIBRACIÓN
El patrón de calibración, fue construido mediante dos placas de cristal de 25x30 cm,
colocadas a 90˚ entre ellas, para formar así los ejes X , Y y Z. Se usó una cuadrícula
en blanco y negro, cada cuadro mide 2cm de ancho y alto, de esta manera conocemos
la posición de cada punto (intersección de un cuadro negro con uno blanco) y su
distancia en centímetros con respecto a un punto de referencia seleccionado como se
muestra en la figura 2-14.
37 Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
Figura 2-14: Patrón de calibración.
2.27 FIJACIÓN DE LAS CÁMARAS
Para llevar a cabo la calibración de estas cámaras, fue necesario fijarlas en la escena
donde se encuentra el brazo robótico, de esta manera los parámetros que se obtengan
no cambiarán, a menos que se muevan las cámaras de lugar. Se construyó un soporte
tubular para cada una de las cámaras de forma que el robot quedara aproximadamente
equidistante y centrado entre cada uno de los soportes. Estos soportes fueron
realizados con tubos galvanizados de ½”, un par de rieles que permiten una movilidad
en sentido horizontal. Estos soportes son ajustables en altura y de manera horizontal
para permitir una mejor colocación de las cámaras y tener así una mejor visión del área
de trabajo. El la figura 2-15 se muestra la estructura final de los soportes de las
cámaras fijas para la calibración en el área de trabajo y la distribución que tienen con
respecto al robot. En la figura 2-16 se muestra cómo se colocaron los tubos sobre los
rieles y la forma en que se aseguraron para garantizar su firmeza y rigidez. En la figura
2-17 se muestra la forma de fijar las cámaras a los soportes (tubos).
38 Capítulo 2 Calibración de cámaras
Figura 2-15: Soportes
Figura 2-16: Fijación de los soportes
39 Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
Figura 2-17: Fijación de las cámaras a los soportes
2.28 PROCESO DE CALIBRACIÓN
Una vez fijadas las cámaras se procedió a tomar las imágenes correspondientes a
cada cámara, donde se incluyó en la escena el patrón de calibración y la parte final del
brazo robótico. Las imágenes obtenidas se muestran en la figura anexa 1.
Enseguida se procedió a obtener los parámetros de calibración como se indica en
siguiente procedimiento.
Primeramente, se obtuvieron las imágenes desde las cámaras del patrón de calibración
usando el software Matlab a una resolución de 1280x960 pixeles, después se procedió
a convertir la imagen a escala de grises, para luego convertirla a blanco y negro.
La imagen en blanco y negro define de manera más clara las intersecciones de los
cuadros negros y blancos, facilitando la búsqueda de las coordenadas en pixeles de los
puntos requeridos para la calibración, posteriormente se localizaron dichos puntos, esto
se hizo seleccionando 6 puntos en el patrón, de los cuales se conoce la posición en el
plano cartesiano en el marco de referencia seleccionado, las coordenadas en pixeles
de estos puntos se buscaron manualmente en la imagen del patrón.
Una vez obtenidas las correspondencias entre los puntos de la imagen y los puntos
conocidos en el mundo real, es decir que xi ↔ Xi , las coordenadas de cada punto son
sustituidas en el sistema de ecuaciones
p11 X i + p12Yi + p13 Z i + p14 + xi p31 X i + xi p32Yi + xi p33 Z i + xi p34 = 0
p21 X i + p22Yi + p23 Z i + p24 + yi p31 X i + yi p32Yi + yi p33 Z i + yi p34 = 0
40 Capítulo 2 Calibración de cámaras
Éste sistema es resuelto en Matlab, de esta manera se obtiene la matriz P con los 11
parámetros que contienen la información de rotación y traslación de las cámaras, así
como los parámetros intrínsecos de éstas.
Para la calibración de las cámaras Logitech 1 y 2 se trabajó con las imágenes que
fueron pasadas por el proceso antes descrito para obtener al final las imágenes en
blanco y negro que se muestran en la figura 2-18.
Figura 2-18: Imágenes para la calibración de cámaras fijas.
Utilizando la expresión 2.32:
⎡ ρxi ⎤ ⎡ p11
⎢ ρy ⎥ = ⎢ p
⎢ i ⎥ ⎢ 21
⎢⎣ ρ ⎥⎦ ⎢⎣ p31
p12
p13
p22
p32
p23
p33
⎡X ⎤
p14 ⎤ ⎢ i ⎥
Y
p24 ⎥⎥ ⎢ i ⎥
⎢Z ⎥
p34 ⎥⎦ ⎢ i ⎥
⎣1⎦
Se prosigue a plantear el sistema de ecuaciones para P de la cámara Logitech 1 con
las correspondencias que se muestran en la tabla 2-1, para la cámara 2 se presentan
los puntos de correspondencias en la tabla 2-2.
41 Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
Tabla 2-1: Puntos para la calibración de la cámara 1.
PUNTOS
1
2
3
4
5
6
Coordenadas en el mundo real
(cm) X Y Z
[8,0,20]’
[12,0,18]’
[14,0,4]’
[0,14,4]’
[0,14,16]’
[0,8,20]’
Coordenadas en la imagen
(pixeles) x y
[177,20]’
[94,58]’
[48,454]’
[603,442]’
[602,109]’
[461,24]’
Tabla 2-2: Puntos para la calibración de la cámara 2.
PUNTOS
1
2
3
4
5
6
Coordenadas en el mundo real
(cm) X Y Z
[14,0,6]’
[8,0,14]’
[10,0,22]’
[0,12,18]’
[0,14,14]’
[0,14,2]’
Coordenadas en la imagen
(pixeles) x y
[406,579]’
[446,468]’
[424,383]’
[613,422]’
[632,471]’
[633,596]’
Resolviendo para la parte derecha:
⎡ ρxi ⎤ ⎡ p11 X i + p12Yi + p13 Z i + p14 ⎤
⎢ ρy ⎥ = ⎢ p X + p Y + p Z + p ⎥
22 i
23 i
24 ⎥
⎢ i ⎥ ⎢ 21 i
⎢⎣ ρ ⎥⎦ ⎢⎣ p31 X i + p32Yi + p33 Z i + p34 ⎥⎦
y dividiendo entre la tercera coordenada se obtiene:
⎡ p11 X i + p12Yi +
⎢
⎡ xi ⎤ ⎢ p31 X i + p32Yi +
⎢ y ⎥ = ⎢ p21 X i + p22Yi +
⎢ i⎥ ⎢ p X + p Y +
32 i
⎢⎣ 1 ⎥⎦ ⎢ 31 i
1
⎢
⎣
p13 Z i + p14 ⎤
p33 Z i + p34 ⎥
⎥
p23 Z i + p24 ⎥
p33 Z i + p34 ⎥
⎥
⎥
⎦
y finalmente se obtienen un par de ecuaciones con 11 incógnitas por cada
correspondencia de puntos {xi ↔ xi′} de la forma:
p11 X i + p12Yi + p13 Z i + p14 + xi p31 X i + xi p32Yi + xi p33 Z i + xi p34 = 0
p21 X i + p22Yi + p23 Z i + p24 + yi p31 X i + yi p32Yi + yi p33 Z i + yi p34 = 0
42 Capítulo 2 Calibración de cámaras
y que al sustituir para las 6 correspondencias se tiene:
p11 X 1 + p12Y1 + p13 Z1 + p14 + x1 p31 X 1 + x1 p32Y1 + x1 p33 Z1 + x1 p34 = 0
p21 X 1 + p22Y1 + p23 Z1 + p24 + yi p31 X 1 + y1 p32Y1 + y1 p33 Z1 + y1 p34 = 0
p11 X 2 + p12Y2 + p13 Z 2 + p14 + x2 p31 X 2 + x2 p32Y2 + x2 p33 Z 2 + x2 p34 = 0
p21 X 2 + p22Y2 + p23 Z 2 + p24 + y2 p31 X 2 + y2 p32Y2 + y2 p33 Z 2 + y2 p34 = 0
p11 X 3 + p12Y3 + p13 Z 3 + p14 + x3 p31 X 3 + x3 p32Y3 + x3 p33 Z 3 + x3 p34 = 0
p21 X 3 + p22Y3 + p23 Z 3 + p24 + y3 p31 X 3 + y3 p32Y3 + y3 p33 Z 3 + y3 p34 = 0
p11 X 4 + p12Y4 + p13 Z 4 + p14 + x4 p31 X 4 + x4 p32Y4 + x4 p33 Z 4 + x4 p34 = 0
p21 X 4 + p22Y4 + p23 Z 4 + p24 + y4 p31 X 4 + y4 p32Y4 + y4 p33 Z 4 + y4 p34 = 0
p11 X 5 + p12Y5 + p13 Z 5 + p14 + x5 p31 X 5 + x5 p32Y5 + x5 p33 Z 5 + x5 p34 = 0
p21 X 5 + p22Y5 + p23 Z 5 + p24 + y5 p31 X 5 + y5 p32Y5 + y5 p33 Z 5 + y5 p34 = 0
p11 X 6 + p12Y6 + p13 Z 6 + p14 + x6 p31 X 5 + x6 p32Y5 + x6 p33 Z 5 + x6 p34 = 0
p21 X 6 + p22Y6 + p23 Z 6 + p24 + y6 p31 X 5 + y6 p32Y5 + y6 p33 Z 5 + y6 p34 = 0
El sistema de ecuaciones obtenida son resueltas con el software Matlab, arrojando los
siguientes resultados:
⎡− 11.4232 3.0281 − 1.7370 845.0457⎤
P1 = ⎢⎢ − 0.2809 0.0108 − 12.2788 682.7313⎥⎥
⎢⎣ − 0.0041 − 0.0061 − 0.0033
1.0000 ⎥⎦
⎡− 8.9560 8.0475 − 2.1397 511.9978⎤
1.6843 − 11.7688 584.1860⎥⎥
P2 = ⎢⎢ 1.3889
⎢⎣− 0.0043 − 0.0010 − 0.0033
1.0000 ⎥⎦
Donde P1 y P2 corresponden a la cámara 1 y 2 respectivamente.
Una vez obtenidas las P' s fue necesario comprobar si la matriz obtenida podía hacer la
transformación de puntos en el mundo real a puntos en la imagen, para ello se elaboró
un programa en Matlab donde dado un punto [X , Y , Z ]' , se obtuviera una coordenada
en pixeles [x, y ]' , de acuerdo a xi = PXi , arrojando los siguientes resultados, los que
se muestran en las tablas 2-3 y 2-4 para la cámara 1 y 2 respectivamente. En la figura
2-19. Se presentan los resultados en la imagen, los puntos de color verde representan
las coordenadas reales en la imagen y los puntos de color rojo son los puntos
calculados.
43 Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
Tabla 2-3: Resultados de pruebas para
Coordenadas X Y Z
cm
Coordenadas
calculadas (pixeles)
P1 .
Coordenadas
reales (pixeles)
Error
ex
ey
[4,0,6]’
[819,631]’
[821,629]’
[8,0,10]’
[788,597]’
[790,596]’
[12,0,16]’
[2,0,18]’
[4,0,2]’
[0,2,2]’
[0,4,6]’
[0,8,12]’
[0,4,20]’
[0,12,22]’
[757,538]’
[848,495]’
[815,673]’
[864,671]’
[886,637]’
[931,587]’
[904,481]’
[987,483]’
[758,537]’
[850,494]’
[817,670]’
[866,668]’
[888,637]’
[931,587]’
[905,481]’
[988,484]’
Tabla 2-4: Resultados de pruebas para
Coordenadas X Y Z
cm
Coordenadas
calculadas (pixeles)
2
2
2
1
1
2
2
2
2
0
1
1
1
1
3
3
0
0
0
1
P2 .
Coordenadas
reales (pixeles)
Error
ex
ey
[4,0,6]’
[481,539]’
[480,538]’
[8,0,10]’
[449,512]’
[450,511]’
[12,0,16]’
[2,0,18]’
[4,0,2]’
[0,2,2]’
[0,4,6]’
[0,8,12]’
[0,4,20]’
[0,12,22]’
[413,461]’
[489,402]’
[483,580]’
[528,569]’
[544,533]’
[578,479]’
[539,382]’
[613,377]’
[413,460]’
[488,405]’
[482,577]’
[526,567]’
[542,532]’
[576,479]’
[538,385]’
[614,377]’
44 1
1
1
1
0
1
1
2
2
2
1
1
1
3
3
2
1
0
3
0
Capítulo 2 Calibración de cámaras
Figura 2-19: Resultados de la calibración, puntos reales y calculados.
2.29 OBTENCIÓN DE LA MATRIZ DE CÁMARA
Para el método de estimación de pose se requiere tener el conocimiento de la matriz de
cámara K , esta matriz contiene la información de los parámetros internos de la
cámara. La estimación de pose consiste básicamente en determinar la matriz P a partir
de tres puntos conocidos. Los parámetros intrínsecos de la cámara siempre se
45 Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
mantienen, y cuando la cámara se mueve lo que cambia son sus parámetros
extrínsecos, es decir, la rotación y traslación.
Los parámetros internos de la cámara son los únicos que permanecen constantes sin
importar que la cámara cambie de posición una vez que la cámara es calibrada. Para
poder recupera la matriz de parámetros intrínsecos (matriz de calibración K ) hay que
observar que la matriz de cámara P representa la trasformación de puntos del marco
de referencia del mundo al marco coordenado de la imagen como xi = PXi .
Escribiendo más a detalle, se encuentra expresada como:
x = KR[ I | −C%]X
Donde R es la matriz de rotación. Vemos que en general P = KR[ I | −C%] . A menudo es
conveniente no hacer explícito el centro de la cámara, y en lugar de eso representar la
matriz de cámara como P = K [ R | −t ] . Donde t = RC~ es la traslación.
La submatriz KR de 3 × 3 del lado izquierdo es una matriz no singular llamada M . Por
lo que P puede ser definida como P = M [ I | M −1 p34 ] = KR[ I | −C~ ] . Donde M = KR puede
ser descompuesta en los valores de K y R utilizando la factorización QR . La
factorización QR separa una matriz en el producto de una matriz triangular superior y
una matriz ortogonal.
Utilizando la matriz de cámara de la cámara 1:
⎡− 11.4232 3.0281 − 1.7370 845.0457⎤
P1 = ⎢⎢ − 0.2809 0.0108 − 12.2788 682.7313⎥⎥
⎢⎣ − 0.0041 − 0.0061 − 0.0033
1.0000 ⎥⎦
se obtiene la matriz M ,que está compuesta por :
⎡− 11.4232 3.0281 − 1.7370
M = ⎢⎢ − 0.2809 0.0108 − 12.2788
⎢⎣ − 0.0041 − 0.0061 − 0.0033
Aplicando la factorización QR con Matlab se tiene:
0.0196 ⎤
⎡− 0.8347 0.5504
⎢
R = ⎢ 0.2106
0.3518 − 0.9121⎥⎥
⎢⎣ − 0.5089 − 0.7571 − 0.4096⎥⎦
46 ⎤
⎥
⎥
⎥⎦
Capítulo 2 Calibración de cámaras
Y finalmente la matriz de calibración K :
⎡1386.1 30.2 525.3⎤
K = ⎢⎢ 0
1383.1 641.0⎥⎥
⎢⎣ 0
0
1 ⎥⎦
2.6 CONCLUSIONES DEL CAPÍTULO 2
En este capítulo se estudió los conceptos básicos de la geometría proyectiva, que se
uso en la calibración de cámaras, gracias a este estudio se obtuvieron los11
parámetros de la matriz P que contienen la información de rotación y traslación de las
cámaras, así como los parámetros intrínsecos de éstas. Al final del capítulo se
mostraron los resultados obtenidos y la metodología usada para encontrar la matriz de
cámara y la matriz de calibración.
Una vez que se tienen las cámaras calibradas, es necesario recuperar la información
tridimensional del objeto buscado, esto se muestra en el capítulo 3, aplicando el
método de visión estereoscópica y estimación de pose.
47 CAPÍTULO 3
3. RECUPERACIÓN DE COORDENADAS EN TRES
DIMENSIONES
Una vez que se tienen cámaras calibradas, es necesario recuperar, de manera
automática, la información métrica de un objeto, es decir, sus coordenadas (x, y, z ) , a
partir de imágenes bidimensionales.
La información métrica se usó dentro del lazo de control para posicionar el efector final
del robot PUMA sobre el objeto.
Se usó el método de triangulación para el caso de dos cámaras fijas y estimación de
pose para una sola cámara, los objetos fueron elegidos para su fácil detección por las
cámaras, el objeto elegido fue un LED de color rojo para dos cámaras fijas y 3 LEDs
formando un triángulo para el método de estimación de pose, el cual se explica en la
sección 3.1 y 3.2.
En la sección 3.3 se expone la solución al problema de triangulación, el procedimiento
que se usó para encontrarla y los resultados obtenidos en pruebas realizadas.
La sección 3.4 muestra el algoritmo usado con el que se abordó el problema de
estimación de pose, las pruebas hechas al sistema con este algoritmo y los resultados
obtenidos de dichas pruebas.
3.1 PRUEBAS DE ALGORITMOS DE PROCESAMIENTO DE IMÁGENES
En esta actividad se realizó el procesado de imágenes tomadas por las cámaras, con el
fin de localizar las coordenadas x y y en pixeles del objeto, se muestra también las
características de las imágenes, los métodos de procesamiento usados en los
resultados obtenidos.
3.1.1 ANÁLISIS DE IMÁGENES USANDO MATLAB
El análisis de imágenes, generalmente es dividido en dos amplias categorías: métodos
donde la entrada es una imagen y la salida es también una imagen y métodos donde la
entrada es una imagen y la salida son atributos extraídos de esa imagen. La figura 3-1
muestra el diagrama con los pasos fundamentales en el procesamiento de imágenes,
no quiere decir que todos los pasos se apliquen en una imagen, más bien muestra los
48 Capítulo 3 Recuperación de coordenadas en 3D
procesos que podemos aplicar a las imágenes para diferentes propósitos y
posibilidades con distintos objetivos, por ejemplo, mejoramiento de la información
gráfica para la interpretación humana; o procesamiento de sus datos para
almacenamiento, transmisión y representación de la percepción de maquinas
autónomas (Gonzalez y Woods 2002).
El primer paso para el procesado de las imágenes es su adquisición, existen diferentes
dispositivos que producen imágenes digitales, en nuestro caso se usaron cámaras
digitales con sensores CCD, a una resolución de 640x480 pixeles. Con la herramienta
de adquisición de imágenes de Matlab ® se obtuvieron imágenes a color,
representadas por componentes de tres intensidades, de acuerdo al modelo RGB24
(rojo, verde y azul con niveles de 0 a 255), también conocido como color verdadero a
24-bits (Nixon y Aguado 2002).
El uso de imágenes a color, es motivado por el hecho de que el color es un poderoso
descriptor, que simplifica la identificación de objetos y extracción de la escena de los
mismos (Gonzalez y Woods 2002). Aprovechando esta característica de las imágenes
a color, se decidió usar LEDs de color rojo, como objeto a ser identificado y ubicado y
usar esa información en la recuperación de sus coordenadas en el espacio en tres
dimensiones.
El siguiente paso es el procesado de los colores, donde se separó la componente en
rojo, en la que se buscaron niveles mayores a 180 en cada pixel y con diferencias en
niveles mayores a 110 con las otras componentes, figura 3-2, de acuerdo a esta regla
se formó una nueva imagen.
Figura 3-1: Diagrama con los casos fundamentales en el procesamiento de imágenes.
49 Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
La nueva imagen es procesada de nuevo, aplicando un proceso de segmentación, en
el cual se buscan las áreas marcadas por los tres LEDs (de color blanco en la nueva
imagen), figura 3-3, identificando su posición en la imagen y obteniendo su centroide en
coordenadas (x,y) para cada LED, de esta manera se obtienen los atributos de interés
de la imagen original que son las coordenadas de cada LED en el plano de la imagen.
Matlab ® ofrece herramientas para procesamiento digital de imágenes, Image
Processing Toolbox y adquisición de imágenes, Image Acquisition Toolbox, los cuales,
son una colección de funciones en código Matlab para la solución a problemas de estas
áreas, la figura 3-4 muestra el diagrama con las funciones y las secuencias usadas
para llevar a cabo el procesado de las imágenes.
Figura 3-2: Valores de RGB en un LED.
50 Capítulo 3 Recuperación de coordenadas en 3D
Figura 3-3: Tres LEDs en una escena, su extracción y sus centroides encontrados.
51 Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
Figura 3-4: Estructura del programa detector de tres LEDs y recuperación de sus coordenadas en 3D.
52 Capítulo 3 Recuperación de coordenadas en 3D
3.2 RECUPERACIÓN DE COORDENADAS EN 3D USANDO CÁMARAS
FIJAS
Es posible recuperar la información geométrica de un punto en el espacio
determinando su posición en dos imágenes tomadas por cámaras calibradas, este
proceso requiere la intersección de dos rayos en el espacio, este método se conoce
como triangulación (Hartley y Sturm 1997).
Cada imagen tiene asociada una matriz de cámara P1 y P2 , y X es un punto
[ X R , YR , Z R ]T en centímetros, en el espacio en 3D que es visible en las dos imágenes y
proyectado en cada imagen como (Hartley y Sturm 1997):
x1 = P1 X
x 2 = P2 X
El problema de triangulación consiste en encontrar la intersección de las dos líneas en
el espacio. En la figura 3-5 se muestran los rayos de proyección en el espacio 3D
definido por x1 y C1 para la imagen 1 y x 2 y C2 para la imagen 2, donde x1 , x 2 son la
proyección del punto X sobre cada imagen y C1 , C2 son los centros de las cámaras.
Uno de los métodos más comunes para la solución al problema de triangulación es la
triangulación lineal. Conociendo que x = PX , se escriben las coordenadas
T
homogéneas x = ρ (x, y,1) , donde ( x, y ) son las coordenadas en pixeles en la imagen
y ρ es un factor de escala desconocido. Denotando Pi , donde i = 1,2,3; son los
renglones de la matriz P , entonces estas matrices pueden ser escritas de la siguiente
manera:
T
ρx = P1T X , ρy = P2T X , ρ = P3T X
Eliminando ρ usando la tercera ecuación, se llega a:
xP3 X = P1 X
T
T
yP3 X = P2 X
T
T
De las dos imágenes se obtienen un total de 4 ecuaciones con X como incógnita, las
cuales se pueden escribir en la forma AX = B (Hartley y Sturm 1997).
53 Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
⎡ x1 − p11, 4 ⎤ ⎡( p11,1 − x1 p13,1 )
⎢ y − p1 ⎥ ⎢( p1 − y p1 )
2, 4 ⎥
2 ,1
1
3,1
⎢ 1
=⎢
⎢ x2 − p 21, 4 ⎥ ⎢( p 21,1 − x2 p 2 3,1 )
⎢
⎥ ⎢
⎣ y 2 − p 2 2, 4 ⎦ ⎣( p 2 2,1 − y 2 p 2 3,1 )
( p1
( p1
( p2
( p2
− x1 p13, 2 )
2 , 2 − y1 p13, 2 )
1, 2
( p1
( p1
) ( p2
) ( p2
1, 2
− x 2 p 2 3, 2
2, 2
− y 2 p 2 3, 2
− x1 p13,3 ) ⎤
⎥⎡ X R ⎤
2 , 3 − y1 p13, 3 ) ⎥ ⎢
YR ⎥⎥
⎢
⎥
)
−
2
x
p
1, 3
1
3, 3
⎥ ⎢⎣ Z R ⎥⎦
−
2
y
p
2,3
1
3, 3 ) ⎦
1, 3
Donde:
⎡( p11,1 − x1 p13,1 )
⎢( p1 − y p1 )
2 ,1
1
3,1
A=⎢
⎢( p 21,1 − x 2 p 2 3,1 )
⎢
⎣( p 2 2,1 − y 2 p 2 3,1 )
( p1
( p1
( p2
( p2
− x1 p13, 2 )
2 , 2 − y1 p13, 2 )
1, 2
1, 2
− x 2 p 2 3, 2
2, 2
− y 2 p 2 3, 2
( p1
( p1
) ( p2
) ( p2
− x1 p13,3 ) ⎤
⎥
2 , 3 − y1 p13, 3 ) ⎥
⎥
1, 3 − x1 p 2 3, 3 )
⎥
2 , 3 − y1 p 2 3, 3 ) ⎦
1, 3
⎡ x1 − p11, 4 ⎤
⎢ y − p1 ⎥
1
2, 4 ⎥
B=⎢
⎢ x2 − p 21, 4 ⎥
⎢
⎥
⎣ y2 − p 2 2, 4 ⎦
⎡X ⎤
X = ⎢⎢ Y ⎥⎥
⎢⎣ Z ⎥⎦
Resolviendo el sistema de ecuaciones en Matlab, se obtiene el valor de [ X R , YR , Z R ]T del
punto en el espacio.
Figura 3-5: Rayos de proyección en cada imagen.
54 Capítulo 3 Recuperación de coordenadas en 3D
3.2.1 RESULTADOS PARA CÁMARAS FIJAS
Aplicando los pasos mostrados en la figura 3-4, para la recuperación de las
coordenadas en la imagen y en el espacio en 3D, se construyó una base con un patrón
de medidas conocidas, donde se realizaron desplazamientos de los tres LEDs, de esta
manera fue posible medir los desplazamientos de los LEDs, respecto al centro de
calibración y comparar los resultados arrojados por el programa en Matlab ®.
Figura 3-6: Tres LEDs frente a dos cámaras fijas calibradas.
La figura 3-6, muestra la colocación de las cámaras, la convención de los ejes y la base
sobre la que se realizaron los desplazamientos.
Fue necesario calibrar cada cámara en esa posición de acuerdo a la teoría mostrada
en el capítulo 2, para la obtención de su matriz de cámara P1 y P2 , obteniendo los
siguientes resultados:
0.2280
641.3354⎤
⎡− 25.9219 − 2.3479
⎢
P1 = ⎢ − 4.3614 − 9.1373 − 21.7797 743.9219⎥⎥
⎢⎣ − 0.0091 − 0.0182
0.0005
1.0000 ⎥⎦
55 Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
⎡− 21.2801 12.9573 − 0.8376 578.7588⎤
P2 = ⎢⎢ − 7.7119 − 4.1176 − 21.5954 649.5631⎥⎥
⎢⎣− 0.02101 − 0.0103 − 0.0010
1.0000 ⎥⎦
Una vez calibradas las cámaras se realizaron desplazamientos de los LEDs en rangos
de 2cm en el plano xy arrojando los resultados mostrados en el cuadro 3-1.
Tabla 3-1: Resultados de la localización de coordenadas espaciales de los tres LEDS.
Centro de los LEDs
en imagen 1
[x, y] pixeles
Centro de los LEDs
en imagen 2
[x, y] pixeles
1
[641.5023,695.8605]
2
Coordenada calculada
Coordenada real
[X, Y, Z] cm
[X, Y, Z] cm
[578.9136,613.5342]
[ 0.0461, 0.0611, 1.9717]
[0,0,2]
[705.7438,720.4070]
[701.4657,623.2864]
[-0.0211, 6.0862, 2.0407]
[0,6,2]
3
[566.4408,736.9562]
[644.1043,653.4559]
[ 5.8401, 5.8398, 2.0577]
[6,6,2]
4
[269.5185,721.9400]
[306.3822,692.4791]
[15.6049,-2.1847, 2.2067]
[16,-2,2]
5
[575.7365,672.7642]
[433.5523,603.9024]
[-0.0495,-8.3218, 1.7064]
[0,-8,2]
6
[696.6018,669.9262]
[530.7636,588.1802]
[-5.9159,-5.9716, 1.6564]
[-6,-6,2]
7
[820.1474,767.5843]
[888.4126,641.4783]
[-0.1883,13.9730, 1.9770]
[0,14,2]
8
[734.6464,680.5429]
[599.1905,592.9000]
[-5.7688,-1.6852, 1.7811]
[-6,-2,2]
9
[722.8273,652.1399]
[504.3430,571.9845]
[-9.7295,-10.0151,1.6859]
[-10,-10,2]
10
[629.5826,660.0781]
[446.6214,588.3132]
[-3.9754,-10.2555,1.7006]
[-4,-10,2]
Prueba
3.3 RECUPERACIÓN DE COORDENADAS EN 3D USANDO ESTIMACIÓN
DE POSE
El problema de la estimación de pose es básicamente el poder determinar la pose
(Rotación y Traslación) de la cámara a partir de tres puntos conocidos en el espacio
3D, para esto se realizó la construcción de un triángulo equilátero de 5cm en donde a
cada vértice se le colocó un led rojo. En este momento los parámetros internos de la
cámara ya son conocidos y serán utilizados para la solución de este problema. El
método empleado es conocido como la solución de Grunert (1841) (Haralick 1994).
En el proceso de calibración se determinaron los parámetros internos (α x , s, x0,α y , y0 ) y
los externos (R, T). Los parámetros internos no cambian con la posición de la cámara,
56 Capítulo 3 Recuperación de coordenadas en 3D
pero no sucede lo mismo con los parámetros externos, ya que estos parámetros fueron
calculados con respecto a un marco de referencia conocido (patrón de calibración) y si
la cámara cambia de posición estos parámetros también cambian, por lo que hay que
volver a calcularlos para un nuevo marco de referencia.
Este problema se presenta cuando la cámara se encuentra colocada sobre robots
móviles, o como es nuestro caso al ser colocada sobre el efector final del brazo
robótico tipo PUMA, a este tipo de configuración se le llama cámara en mano.
A continuación se muestra el planteamiento usado por Grunert. En la figura 3-7 se
ilustra el problema.
Figura 3-7: Ilustración geométrica de tres puntos en el espacio.
sean:
p1 , p2 y p3
⎛ xi ⎞
⎜ ⎟
; pi = ⎜ yi ⎟; i = 1,2,3.
⎜z ⎟
⎝ i⎠
tres puntos de un triángulo conocido, cuya posición en el espacio se desconoce y sus
dimensiones están dadas por:
a = p2 − p3
b = p1 − p3
c = p1 − p2
Ahora, hacemos que el origen del sistema coordenado de la cámara sea el centro de
perspectiva y que el plano de proyección de la imagen esté a una distancia f . Y si los
puntos proyectados en la imagen de p1 , p2 y p3 son q1 , q2 y q3 respectivamente.
57 Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
⎛ ui ⎞
⎟;
⎝ vi ⎠
( qi ) = ⎜
i = 1, 2,3
Los vectores unitarios desde el centro de perspectiva a los puntos observados
p1 , p2 y p3 están dados por:
ji
⎛ xi ' ⎞
1
⎜ '⎟
y ;
'2
'2
2 ⎜ i ⎟
xi + yi + f ⎜ ⎟
⎝ f ⎠
i = 1, 2,3
El centro de perspectiva con los tres puntos del triángulo forma un tetraedro. Los
ángulos que se forman del centro de perspectiva y los lados opuestos a, b y c son
α , β y γ respectivamente y están dados por:
cos α = j2 ⋅ j3
cos β = j1 ⋅ j3
cos γ = j1 ⋅ j2
Si las distancias de los puntos p1 , p2 y p3 al centro de perspectiva son s1 , s2 y s3
respectivamente, para determinar la posición de los puntos p1 , p2 y p3 con respecto
al marco de referencia de la cámara, es suficiente determinar las distancias s1 , s2 y s3
respectivamente dado que:
pi = si ji ; i = 1, 2,3
nos darán las coordenadas de los puntos con respecto al marco de referencia de la
cámara.
La solución propuesta es una aplicación de la ley de cosenos para los ángulos
α , β y γ . Aplicando esta ley tenemos:
s2 + s3 − 2s2 s3 cos α = a 2
(3.1)
s1 + s3 − 2s1s3 cos β = b 2
(3.2)
s1 + s2 − 2s1s1 cos γ = c 2
(3.3)
2
2
2
2
2
2
58 Capítulo 3 Recuperación de coordenadas en 3D
si
s2 = us1 ;
s3 = vs1
(3.4)
y las sustituimos en las ecuaciones 3.1, 3.2, 3.3, entonces:
(
)
s1 u 2 + v 2 − 2uv cos α = a 2
2
(
)
s1 1 + v 2 − 2u cos β = b 2
2
(
)
s1 1 + u 2 − 2u cos γ = c 2
2
(3.5)
(3.6)
(3.7)
Igualando la ecuación 3.5 con 3.6 y la ecuación 3.6 con 3.7, se obtiene el siguiente
sistema de ecuaciones:
u2 +
b2 + a 2 2
2a 2
a2
v
−
2
uv
cos
α
+
u
cos
β
−
=0
b2
b2
b2
(3.8)
u2 −
c2 2
c2
b2 − c2
v
+
2
v
cos
β
−
2
u
cos
γ
+
=0
b2
b2
b2
(3.9)
De la ecuación 3.8 se puede obtener una expresión para u 2 que queda de la siguiente
forma:
u2 = −
b2 − a2 2
a2
2a 2
+
−
+
v
uv
v
2
cos
α
cos
β
b2
b2
b2
Esta expresión para u 2 puede ser sustituida en la ecuación 3.9 para obtener una
expresión para u :
⎛ a2 − c2 ⎞ ⎛ a2 − c2 ⎞
a2 − c2
⎜
⎟
⎟
1
2
cos
β
+
+1
v 2 ⎜⎜
−
−
v
⎟ ⎜
b2
b 2 ⎟⎠
b2
⎝
⎝
⎠
u=
2(cos γ − v cos α )
(3.10)
y esta ecuación es sustituida nuevamente en la ecuación 3.8 para obtener un polinomio
de cuarto orden en v , de la forma:
A4 v 4 + A3v 3 + A2 v 2 + A1v + A0 = 0
(3.11)
Al encontrar las raíces de este polinomio se obtiene una solución para v que puede ser
sustituida en 3.10 y resolver para u . Teniendo los valores de u y v pueden ser
sustituidas en 3.5, 3.6 o 3.7 y obtener la solución para s1 , la cual será la solución para
59 Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
la posición de nuestro origen al centro de perspectiva de la cámara. Con este
procedimiento se tienen 4 soluciones de las cuales dos de ellas son complejas y las
otras dos son reales, existe la incertidumbre de cuál de las soluciones reales es la que
corresponde a nuestro problema ya que ambas son totalmente válidas.
Para las ecuaciones anteriores los valores que se propusieron para a , b y c son de
5cm, que son las medidas del triángulo que se utilizó en estas pruebas.
La solución para s1 es la distancia del centro de proyección hasta el punto p1 del
triángulo el cual es considerado como el origen de nuestro nuevo marco de referencia,
T
lo que nos da como resultado el vector de traslación T = ⎡⎣t x , t y , t z ⎤⎦ .
Por el momento sólo falta determinar la orientación de este nuevo marco de referencia,
este problema puede ser planteado como sigue. Dadas las coordenadas de tres puntos
en el sistema coordenado del espacio 3D se requiere determinar una matriz de rotación
(R ) y un vector de traslación (T ) que satisfaga la siguiente ecuación:
pi = Rpi + T ;
'
⎛ xi ⎞
⎜ ⎟
donde pi = ⎜ yi ⎟;
⎜z ⎟
⎝ i⎠
i = 1,2,3
(3.12)
i = 1,2,3 , son los puntos en 3D en el sistema coordenado de la
⎛ x 'i ⎞
⎜ ⎟
cámara y p 'i = ⎜ y 'i ⎟;
⎜ z' ⎟
⎝ i⎠
i = 1,2,3 , son los puntos en 3D en el sistema coordenado del
mundo.
Con objeto de poder resolver este problema linealmente se expresa la matriz de
rotación como:
⎡ r11
R = ⎢⎢r21
⎢⎣r31
r12
r22
r32
r13 ⎤
r23 ⎥⎥
r33 ⎥⎦
Las incógnitas en la matriz de rotación no son independientes y cumplen con:
r112 + r122 + r132 = r212 + r222 + r232 = r312 + r322 + r332 = 1
60 Capítulo 3 Recuperación de coordenadas en 3D
r13 = r21r32 − r22 r13
r23 = r12 r31 − r11r32
(3.13)
r33 = r11r22 − r12 r21
Y si los tres vértices del triángulo son coplanares, podemos asumir que zi' = 0 para
i = 1,2,3 .
La ecuación 3.12 puede ser escrita como:
xi = r11 xi' − r12 yi' + t x
yi = r21 xi' − r22 yi' + t y
i = 1,2,3
(3.14)
zi = r31 xi' − r32 yi' + t z
que escrito en termino de matrices queda expresada como:
AX = B
donde:
⎡ x1'
⎢
⎢0
⎢0
⎢ '
⎢ x2
A=⎢0
⎢
⎢0
⎢ x'
⎢ 3
⎢0
⎢
⎣0
0
0
0
x
y
'
2
0
0
0
0
1
0
0
0
'
2
x
0
'
2
y
0
0
x 2'
0
x 2'
0
0
1
0
y 3'
0
0
0
0
1
0
0
x
'
3
'
3
0
0
0
1
0
0
0
x 3'
y 3'
0
0
0⎤
⎥
0⎥
1⎥
⎥
0⎥
0⎥
⎥
1⎥
0⎥
⎥
0⎥
⎥
1⎦
tx
ty
tz
y i'
0
0
0
0
1
0
0
'
1
'
1
0
0
0
1
'
1
'
1
0
0
y
[
x
y
y
X = r11 r12
r21 r22
B = [x1
z1
y1
x2
r31 r32
y2
z2
x3
y3
]
T
z3 ]
T
3.3.1 RESULTADOS PARA ESTIMACIÓN DE POSE
Este procedimiento se implementó en MATLAB y se realizaron pruebas del algoritmo.
Estas consistieron en tomar una imagen a nuestro triángulo de pruebas (objetivo) con
la cámara montada sobre el servomotor, para luego introducir la información de las
coordenadas en pixeles al algoritmo descrito y determinar la distancia a la que se
encuentra el objetivo. La imagen del objetivo se muestra en la figura 3-8.
61 Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
En la figura 3-9 se pueden observar las dos soluciones que proporciona el algoritmo, la
solución que está dada por el triángulo azul es las que se acerca más al valor real y en
este caso la proporcionó el valor más pequeño de las soluciones de v .
Algunos resultados obtenidos se muestran en la Tabla 3-2. En esta tabla se muestran
los puntos de los vértices del triángulo de pruebas en pixeles ( qi ) que se utilizaron. Se
utilizó al final el valor más pequeño de v obtenido de las raíces de la ecuación 3.11
para calcular la distancia de los puntos en 3D en el sistema coordenado del mundo
( pi ) en centímetros. Para estas pruebas se supuso que el plano de la cámara es
paralelo al plano del objetivo lo cual hace que la distancia a la que se encuentra la
cámara del objetivo es constante en su coordenada Z en el marco de referencia de la
cámara, por lo que se hace solamente una comparación en la coordenada Z de las
soluciones obtenidas. En la última columna se muestra la distancia que hay entre el
objetivo y la cámara.
En los puntos en 3D en el sistema coordenado del mundo ( pi ) de cada corrida puede
observarse que hay diferencias en los valores de la coordenada z de cada punto. Esto
es debido a que los planos de la cámara y objetivo no se encuentran completamente
paralelos, esto hace notar que la inclinación entre estos planos afecta a las soluciones
obtenidas del método Grunert (1841).
Tabla 3-2: Pruebas de la estimación de pose.
Coordenadas de los Vértices en pixeles
( qi )
Puntos en 3D en el sistema coordenado del
mundo
( pi )
Prueba
q1
q2
q3
[u, v ] pixel [u, v ] pixele [u, v ] pixele
es
s
s
p1
[x, y, z ] cm
[-2.76, 2.25
,20.32]
1
[179,134]
[422,129]
[305,342]
[-2.93, 2.39,
21.51]
[-2.59, 2.64,
30.00]
2
[230,158]
[296,154]
[317,303]
[-2.68, 3.74,
31.12]
3
[105,14]
[189,35]
[233,272]
[-3.42, 3.73,
16.42]
[-3.78, 4.12,
18.18]
62 p2
[x, y, z ] cm
[2.17
,2.50,
21.58]
[2.17
,2.50,
21.58]
[2.25,
2.74,
29.84]
[2.25,
2.74,
29.84]
[1.27,
3.81,
18.44]
[1.18,
3.55,
Raíz
utilizada
de la
ecuació
n 3.11
p3
[x, y, z ] cm
Distanci
a del
objetivo
Real
v
Z cm
[-0.29, -1.96,
21.77]
1.06
20
[-0.27, -1.85,
20.51]
0.95
20
[-0.05, -1.63,
31.16]
1.03
30
[-0.05, -1.63,
31.12]
0.99
30
[-1.56, -0.42,
18.74]
1.09
18
[-1.56, -0.42,
18.74]
0.99
18
Capítulo 3 Recuperación de coordenadas en 3D
[-3.13, 3.57,
27.80]
4
[203,116]
[382,128]
[284,280]
[-3.24, 3.69,
28.73]
[3.12, 0.77,
19.57]
5
[482,208]
[497,59]
[680,203]
[4.10, 1.02,
25.72]
17.15]
[1.80,
3.40,
29.10]
[1.70,
3.22,
27.54]
[4.03,
4.27,
23.18]
[3.75,
3.96,
21.57]
[-0.98, -0.88,
29.06]
1.03
27
[-0.98, -0.87,
28.99]
0.99
27
[7.69, 0.97,
21.82]
1.17
25
[7.84, 0.99,
22.25]
0.91
25
Figura 3-8: Imagen del objetivo para la estimación de pose.
63 Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
Figura 3-9: Gráfica de la solución obtenida para la estimación de pose.
3.4 CONCLUSIONES DEL CAPÍTULO 3
En éste capítulo, se mostró la solución al problema de recuperación de coordenadas en
3D, con el método de visión estereoscópica y con el de estimación de pose, las
pruebas de los métodos consistieron en colocar un objeto sobre una cuadrícula con
medidas conocidas, haciendo movimientos frente a las cámaras calibradas, las
coordenadas del objeto sobre la imagen se encontraron de manera automática
mediante un procesado de la imagen, una vez conocidas dichas coordenadas y
aplicando las ecuaciones correspondientes a cada método se logró obtener la
coordenada x, y y z del objeto.
Fue complicado obtener un error exacto, en lo que refiere a la posición real y a la
calculada, debido a que se midió la posición real a mano y el trazado de la cuadrícula
se hizo de la misma forma.
La información métrica del objeto obtenida en este capítulo, se usó para indicarle al
robot la posición final a la que debe llegar, sólo fue necesario transformar las
coordenadas cartesianas x, y y z a coordenadas angulares, que son las que usa el
robot para posicionarse, a manera de ir ensamblando el sistema de visión robótica, sólo
queda conocer el control del robot, esto se muestra en el siguiente capítulo.
64 CAPÍTULO 4
4. EL ROBOT PUMA Y SU CONTROL
En este capítulo se presenta una breve reseña del trabajo hecho en (Villalobos y
Ramirez 2005), este robot se utilizó para anexarle una cámara en su efector final e
implementando algoritmos de calibración de cámaras, detección de objetos en
imágenes y cálculo de sus coordenadas en 3D, para llevar a cabo el control de sus
movimientos, obteniendo con esto un prototipo de visión robótica real en el CENIDET.
En la sección 4.1 se muestran las características mecánicas y espacio de trabajo del
robot PUMA, mismas que se estudiaron para poder construir y montar una cuarta
articulación con base para una cámara.
La sección 4.2 presenta la construcción de la 4ta articulación y el tipo de control que se
realizará sobre ella.
La sección 4.3 presenta el rediseño de las tarjetas de adquisición de datos y potencia
para el control del robot.
Con los cambios en las tarjetas de control, fue necesaria la reprogramación de la PC
que controla al robot y de los microcontroladores, esto se muestra en la sección 4.4.
Debido a modificaciones en la estructura mecánica del robot y en el sistema electrónico
del control, se adapto el control PID (Proporcional-Integral-Derivativo) que se hizo en
(Villalobos y Ramirez 2005), esto se presenta en la sección 4.5.
4.1 ESTRUCTURA MECÁNICA DEL ROBOT PUMA
La mayor parte del brazo está hecho de aluminio debido a su baja densidad y buenas
propiedades mecánicas. El cuerpo y el eje del hombro, figura 4-1, son piezas
sometidas a mayores esfuerzos y están hechas de acero buscando una mayor rigidez
en la estructura. La tabla 4-1 define los movimientos de las articulaciones.
65 Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
Figura 4-1: Eslabones, articulaciones y motores del brazo.
Tabla 4-1: Especificaciones del robot.
Estructura mecánica
Articulado Vertical
Número de ejes
Rango de Movimiento
3
Eje 1, Cintura
330°
Eje 2, Hombro
310°
Eje 3, Codo
Máximo radio de operación
Sensores de realimentación
310°
331mm
Encoders ópticos incrementales en cada eje, 500
cuentas/revolución, señales cuadradas desfasadas 90°, con
alimentación de 5Vcd a 40mA, modelo HP HEDS-9100
Motores Pittman ® de 1090 rpm a 24Vcd con reducción
5:9:1, modelo GM9234C212-R3
Actuadores
Reducción por poleas:
Cintura
12.5:1
Hombro
12.5:1
Codo
Transmisión
Carga máxima
Peso del brazo
5:1
Caja de engranes, poleas y bandas dentadas
0.250 Kg
17 Kg.
66 Capítulo 4 El Robot PUMA y su control
4.1.1 ESPACIO DE TRABAJO
La longitud de los eslabones y los grados de rotación de las articulaciones son las
dimensiones que determina el espacio de trabajo del brazo. La figura 4-2 muestra
algunas dimensiones y el alcance del robot antes de anexarle una 4ta articulación.
Figura 4-2: Espacio de trabajo (vista superior y lateral).
El estudio de estas dimensiones ayudo a resolver la cinemática inversa del robot,
mostrada en el capítulo 5.
4.2 CONSTRUCCIÓN DE LA 4 TA ARTICULACIÓN
En esta sección se presenta el prototipo construido para la cuarta articulación, en la
que se montó la cámara para su uso con el método de estimación de pose; Se
enumeran las consideraciones hechas para ello y se muestra el prototipo construido.
Tanto la cámara como el motor que se usaron se eligieron de acuerdo a las
restricciones de espacio y fuerza que tiene el brazo robótico.
Durante el acopio de componentes, se eligió usar un servo motor que además de tener
poco peso, ofreciera la fuerza necesaria para mover a la cámara junto con su montura
e índice posicional.
La cámara se eligió en este caso, considerando su peso y que su foco estuviera
simétricamente localizado sobre su eje vertical, de esta manera, se evitaba el hecho de
calcular dicho centro y es más fácil localizar el centro de la cámara con respecto al
punto de referencia a usar.
67 Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
El material de construcción de la montura de la cámara y de la estructura que soportara
a esta y al motor fue aluminio, ya que es un material ligero, económico, de fácil
adquisición y fácil de trabajar.
El diseño elegido para construir esta articulación se muestra a continuación en la figura
4-3, donde se puede observar que el movimiento de la articulación es de manera
horizontal, debido a que ya se cuenta con dos movimientos verticales, en el brazo y
antebrazo y solo uno horizontal en la cintura del robot y se piensa que este movimiento
ayudará a localizar de manera más exacta el objeto buscado, ya que el servomotor
permite movimientos hasta de un grado de resolución, así una vez localizado en la
línea vertical y aproximado en la horizontal mediante los movimientos de la cintura, el
índice posicional es girado de manera que se efectúe un contacto no brusco de éste
sobre el objeto.
Figura 4-3: Diseño de la 4 ta articulación.
Figura 4-4: Diseño final de la 4ta articulación.
68 Capítulo 4 El Robot PUMA y su control
4.2.1 FUNCIONAMIENTO DEL SERVOMOTOR
El movimiento de la cuarta articulación se llevo a cabo con un servomotor de la marca
Hitec ®, modelo HS-311. Se le realizaron pruebas de funcionamiento y par de fuerza
para garantizar su funcionamiento.
El servomotor puede ser llevado a posiciones angulares específicas al enviar una señal
codificada. Siempre que la señal exista en la línea de entrada, el servomotor mantendrá
la posición angular del engranaje. Cuando la señal cambia, la posición angular cambia.
En la práctica, se usan servomotores para posicionar superficies de control como el
movimiento de palancas, pequeños ascensores y timones. Éstos también se usan en
juguetes de radio control, títeres y por supuesto en robots.
Los servomotores son sumamente útiles en robótica. Los motores son pequeños, como
se observa la figura 4-5, tienen internamente una circuitería de control y es sumamente
poderoso para su tamaño. El servomotor HS-311 de Hitec ® tiene una fuerza promedio
de 3kg/cm a una alimentación de 4.8 Vcd. Su potencia es proporcional a la carga
mecánica. Un servomotor, por consiguiente, no consume mucha energía. También
puede ver los 3 alambres de conexión externa, uno es para alimentación 5Vcd (rojo),
conexión a tierra GND (negro) y el alambre amarillo es el de la señal de control. Se
presentan las características principales en la tabla 4-2.
Tabla 4-2: Características principales del servomotor.
Torque 4.8-6v
Velocidad 4.8-6v
Tamaño 4.8-6v
Peso 4.8-6v
3.0 - 3.7 kg.
0.19 - 0.15 segundos
40 x 20 x 37mm
43 g.
Figura 4-5: Servomotor y cámara usados en la 4ta articulación.
69 Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
La fuerza motriz del servomotor tiene algunos circuitos de control y un potenciómetro
(una resistencia variable) esta es conectada al eje central del servomotor. Este
potenciómetro permite a la circuitería de control, supervisar el ángulo actual del
servomotor. Si el eje está en el ángulo correcto, entonces el motor está apagado. Si el
circuito sensa que el ángulo no es el correcto, el motor girará en la dirección adecuada
hasta llegar al ángulo correcto. El eje del servomotor es capaz de llegar alrededor de
los 180 grados, en algunos llega a los 210 grados, pero varía según el fabricante. Un
servomotor se usa normalmente para controlar un movimiento angular de entre 0 y 180
grados.
El cable de control se usa para comunicar el ángulo. El ángulo está determinado por la
duración de un pulso que se aplica al alambre de control. El servomotor requiere ser
alimentado por un pulso cada 20 milisegundos. La longitud del pulso determinará los
giros de motor. Por ejemplo, un pulso de 1.5 ms., hará que el motor se torne a la
posición de 90 grados (llamado la posición neutra). Si el pulso es menor de 1.5 ms.,
entonces el eje se acercará a los 0 grados. Si el pulso es mayor de 1.5ms, el eje se
acercará a los 180 grados (figura 4-6). La duración del pulso tiene un rango de 0.6 mS
a 2.4 mS, dado que el servomotor toma una duración de 400 uS para un recorrido de
40 grados. El servomotor es lineal a lo largo de todo su recorrido. Tomando en cuenta
lo anterior ser realizó una tarjeta de control para éste dispositivo la cual se muestra en
la siguiente sección.
Figura 4-6: Funcionamiento de un servomotor.
4.3 REDISEÑO DE LAS TARJETAS DE CONTROL PARA EL ROBOT PUMA
Las tarjetas construidas en “Construcción de un brazo robótico de tres GDL y su control
mediante el núcleo híbrido de transición de estados” (Villalobos y Ramirez 2005),
presentan deficiencias al momento de probar los controladores del robot, esta cuestión
70 Capítulo 4 El Robot PUMA y su control
nos impidió hacer pruebas con la cuarta articulación montada en el robot y pruebas de
posición del efector final. Es por ello, que fue necesaria la construcción de nuevas
tarjetas de control y ofrecer una solución a las siguientes deficiencias.
•
•
•
•
•
•
•
•
El valor de encoder que envía el PIC a la PC que controla al robot, que
llamaremos PC2 lo hace en dos partes de cuatro bits, debido a esto, la PC2
recibe la parte alta en un tiempo y la parte baja en otro tiempo y
correspondiente a otro conteo. Esto se hace debido a que el puerto paralelo se
maneja con sólo 5 bits de entrada.
La solución que se presenta, es usar el puerto en forma bidireccional a 8 bits de
datos, configurándolo vía software, de esta manera se recibe el valor del conteo
del encoder en forma completa y correspondiente al tiempo de lectura.
Este mismo puerto de datos es usado por la PC2 para enviarle el valor de PWM
al PIC a 8 bits.
La etapa de potencia de los motores basada en el puente H LMD18200, tiene
limitaciones en el manejo de corriente y es propensa a quemarse en caso de
que falle el movimiento de alguna articulación, por el exceso de corriente
requerida por los motores.
La solución fue, construir una interfaz de potencia más robusta a partir de
transistores TIP101 y TIP106 que proporciona hasta 15 amperes de corriente.
El robot solo tiene controladas dos articulaciones, cintura y hombro, faltando por
controlar el codo.
La solución es sintonizar un controlador para la ésta última articulación.
Y por último fue necesario agregar la interfaz controladora del servomotor, de la
cuarta articulación.
Otro cambio necesario para el mejoramiento de la interfaz fue el del microcontrolador,
anteriormente se usó el PIC16F84 de Microchip Technology Inc, la nueva tarjeta usa el
PIC16F877A de Microchip Technology Inc, se decidió este cambio para aumentar la
cantidad de puertos de entrada y salida digitales y hacer más compacto al sistema, ya
que este último dispositivo cuenta con un generador de señal PWM que alimenta al
controlador del motor, este cambio sustituyó al generador de PWM SG3524N y a un
arreglo R-2R usado como convertidor digital analógico para alimentar al PWM, a un
buffer 74HC541 y una compuerta inversora 74HC14, además, este cambio mejoró la
calidad de la señal de PWM e hizo posible la modificación de su frecuencia vía
software, para futuras aplicaciones.
La interfaz construida se muestra en la figura 4-7 y en el anexo B se muestra el
diagrama del circuito empleado.
71 Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
Figura 4-7: Interfaz construida para el control del robot.
4.3.1 FUNCIONAMIENTO DE LA TARJETA DE CONTROL
En la etapa de adquisición de datos, constituida principalmente por un PIC16F877, se
reciben mediante una interrupción, los pulsos generados por los encoders de los
motores, de aquí mismo son enviados a la de control cada 4ms para calcular la señal
de control, que a su vez regresa cada 20ms el valor de PWM que el PIC tendrá que
generar y enviar a la etapa de potencia, como se muestra en la figura 4-8.
72 Capítulo 4 El Robot PUMA y su control
Figura 4-8: Etapa de adquisición de datos.
Las pruebas hechas a esta parte de la tarjeta fueron:
1. Verificación de la entrada de los pulsos a los PIC´s en los pines 33 y 34,
midiendo con un osciloscopio en dichos pines.
2. Verificación del envío del dato del PIC a la PC2, imprimiendo en pantalla dicho
valor.
3. Verificación de que el valor generado de PWM es el que la PC2 envió al PIC,
midiendo con un osciloscopio en el pin 17.
En la etapa de potencia, constituida principalmente por transistores TIP101 y TIP106,
se recibe la señal de PWM y de dirección del motor, con niveles de voltaje de 0 a 5V.
La señal de PWM, es amplificada a 12V y enviada a los motores. La señal de dirección
la envía la PC2 y solo nos indica el sentido de giro del motor.
Las pruebas hechas a esta parte de la tarjeta fueron:
1. Verificación de la entrada de la señal de PWM proveniente del PIC midiendo con
un osciloscopio en los puentes de aislamiento.
2. Verificación de la entrada de la señal de dirección proveniente de la PC2
midiendo con un osciloscopio en los puentes de aislamiento.
3. Se retiraron los puentes de aislamiento, ver figura 4-7, y se colocó un motor de
CD en la salida de cada etapa de potencia, se colocaron niveles de voltaje de 0
73 Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
a 5V en cada pin para verificar que el motor se accionara e hiciera el cambio de
dirección, con 0V entrega la máxima salida y con 5V la salida mínima.
Para controlar la 4ta articulación se utilizó el PIC16F873, el cual cuenta con generador
de PWM y comunicación serial, se envió mediante una interfaz serial RS232 el valor
de ángulo requerido, el PIC generó la señal de PWM y se colocó el servomotor a la
salida del PIC, se comprobó que el ángulo enviado fue el ángulo de salida, en el anexo
B se muestra el diagrama del circuito.
4.4 ADAPTACIÓN DE LOS PROGRAMAS DE LA PC Y DEL
MICROCONTROLADOR PARA SU USO CON LAS NUEVAS TARJETAS
Debido al cambio en las tarjetas de control del robot, fue necesaria la adaptación de los
programas de la PC2 y del microcontrolador, para el manejo de un bus bidireccional de
8 bits, en la figura 4-9 se muestra el diagrama de flujo usado para el cambio del
programa en la PC2.
Figura 4-9: Diagrama de flujo para el programa de la PC2.
74 Capítulo 4 El Robot PUMA y su control
Al inicio, el programa espera a recibir las coordenadas puerto serie desde la PC donde
están conectadas las cámaras, a la que se llamará PC1, una vez recibidas, lee el valor
de los encoders para conocer la posición de las articulaciones, luego ejerce la ley de
control para establecer el valor de la señal PWM y el sentido de giro de los motores,
esto se envía al PIC que genera la señal de PWM que alimentará a los motores. El
valor de los encoders es leído constantemente hasta que la articulación llegue a su
posición final en ese momento la PC2 al motor, a la que se llamará PC2, enviará una
señal via puerto serie a la PC1 para avisarle que el robot se posicionó en la
coordenada deseada.
El programa del PIC se muestra en la figura 4-10 y en la figura 4-11 el diagrama de
flujo de la interrupción que genera el conteo del encoder de los motores.
Figura 4-10: Diagrama de flujo del programa del PIC.
75 Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
Figura 4-11: Diagrama de flujo de la interrupción programada en el PIC.
El programa constantemente verifica los puertos del PIC RB7 y RB6 por los cuales la
PC2 le indica si va a escribir valor de PWM o leer datos del encoder. Estos puertos se
activan con un 1 lógico, si esto sucede con RB7 el programa entra a una rutina de
envío de valor actual del encoder, si fue el puerto RB6 el activo, se recibe el valor para
generar la señal de PWM que será enviada al motor.
La interrupción programada se activa en el momento que se mueve una articulación y
empieza a incrementar o decrementar una variable que contiene la cuenta del encoder,
después, regresa de la interrupción y es enviada esta cuenta a la PC2.
4.5 APLICACIÓN DE UN CONTROLADOR PID SOBRE EL ROBOT PUMA
Después de terminada la tarjeta de adquisición de datos y potencia fue necesario
probar un control para posición del robot; la base de dicho control fue un PID
programado en (Villalobos y Ramirez 2005), al cual se le hicieron los siguientes
ajustes:
1. Se programó un generador de trayectoria cicloidal variable (Angeles 2003), e
independiente en cada articulación, ya que el generador de trayectoria anterior
solo realiza un movimiento de 0˚ a 90˚ y viceversa.
2. La nueva rutina de generación de trayectoria, considera la posición inicial del
robot como ángulo 0˚ y desde este ángulo parte a cualquier ángulo pedido,
permitido en el movimiento del robot y de esa nueva posición pasa a cualquier
otro ángulo requerido y así sucesivamente.
3. Se reprogramó el modo de adquisición de valores de los encoders,
anteriormente se adquirían datos en dos paquetes de 4 bits (nible alto y bajo)
que posteriormente son reagrupados para recuperar el valor original;
76 Capítulo 4 El Robot PUMA y su control
actualmente esto se realiza en un solo paso, de forma que se adquieren los
datos desde el puerto paralelo en un solo paquete de 8 bits.
4. Se programaron un par de puertos seriales, uno para recibir ángulos de la PC1,
este tiene una velocidad de comunicación de 115200 baudios y otro para enviar
el ángulo de posición de la cuarta articulación a una velocidad de 19200
baudios.
5. Se ajustaron las constantes del controlador PID, para un mejor funcionamiento
del robot con la nueva dinámica.
Para la generación de las trayectorias se utilizó un movimiento cicloidal (Angeles
2003), esta función produce un movimiento suave (S), una velocidad (S’) y aceleración
(S’’) cero al inicio y final del intervalo, esto ayuda a evitar esfuerzos en los motores de
las articulaciones del robot.
La función cicloidal está definida por:
s (τ ) = τ −
1
sin( 2πτ )
2π
La respuesta de esta señal se puede ver en la figura 4-12, en la cual se puede observar
la posición (S), la velocidad (S’) y la aceleración (S’’).
Para utilizar esta función como generador de trayectoria variable, en la que se pudiera
cambiar el valor del ángulo final y el tiempo que transcurrirá para llegar a este punto, se
tomó en cuenta la velocidad de operación típica del robot que es de 30°/seg.
Tomando en cuenta esta velocidad se propuso una velocidad de 18°/seg (90° en 5
seg).
La constante de tiempo
τ
para la generación de la trayectoria se calculo como:
T = abs (angulo ) *
τ=
t
T
angulo = angulo _ nuevo − angulo _ old
77 5 .0
90.0
Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
Figura 4-12: Movimiento cicloidal (S) y su velocidad (S’) y aceleración (S’’).
donde
T = tiempo total del movimiento
ángulo = ángulo la trayectoria a generar
ángulo_n = ángulo que se desea
ángulo_old = ángulo anterior
la posición en función del tiempo se generó con la siguiente ecuación:
position(t ) = angulo *
PI
PI * t ⎞
⎛
* ⎜1.0 − cos
⎟
360.0º ⎝
T ⎠
Las ecuaciones anteriores se programaron para cada articulación de manera
independiente, para dar la posición de un ángulo variable a cada articulación.
4.5.1 AJUSTES DEL CONTROLADOR PID
Las tarjetas servoamplificadoras desarrolladas en (Villalobos y Ramirez 2005) son
alimentadas por una fuente de 24 Vcd lo que para ellos es equivalente a 255 cuentas
digitales, para compensar cargas constantes tales como la fricción y la inercia debido a
la masa de la articulación, es aplicada a la salida del controlador una constante de 8 V78 Capítulo 4 El Robot PUMA y su control
(85 cuentas digitales) para compensar la sona muerta del actuador. Ademas la
máxima salida del controlador esta limitada por software a 18.8 Vcd (200 cuentas
digitales).
cd
Al rediseñar y contruir nuevas tarjetas servoamplificadoras las características del
controlador fueron afectadas, ya que la alimentación de la nueva etapa de potencia es
con 12 Vcd, de tal forma que el nivel de voltaje para compezar la zona muerta también
cambio siendo de aproximadamente 2 Vcd (40 cuentas digitales) y la máxima salida del
controlador esta limitada a 11.7 Vcd (250 cuentas digitales).
Todas éstas modificaciones al programa y a la tarjeta de potencia hacen que tengan
que ser reajustadas las constantes de los controladores de las articulaciones, dicho
ajuste se realizó de manera manual hasta tener una buena respuesta en velocidad y
posición en el movimiento del robot. Teniendo un valor final de las constantes del
controlador para cada articulación de:
Tabla 4-3: Constantes utilizadas en los controladores.
Kp
5.00
Cintura
Ki
0.0001
Kd
0.01
Kp
4.0
Hombro
Ki
0.001
Kd
0.0
Kp
2.0
Codo
Ki
0.01
Kd
0.2
4.6 CONCLUSIONES DEL CAPÍTULO 4
Fue necesario el conocimiento del robot para poder probar los métodos de visión
estereoscópica y estimación de pose en el robot, debido a problemas en las tarjetas de
control del robot y a la adición de una nueva articulación se rediseñaron y construyeron
nuevas tarjetas de control, esta actividad no fue programada en los alcances de la
tesis, ya que el robot se suponía funcional, además fue necesario darle un
mantenimiento mecánico, ya que cada articulación presentaban problemas de
movimiento, estos ajustes causaron cambios en la dinámica del robot y fue necesario
además un reajuste en el control y su reprogramación.
Una vez resueltos los problemas adicionales a esta investigación, se procedió a
realizar pruebas que validaran al sistema completo, esto se muestra en el capítulo 5.
79 CAPÍTULO 5
5. RESULTADOS
Una vez completo el sistema de visión robótica, es decir, se tienen probados los
métodos de calibración, detección de objetos y el control del robot, fue necesario
realizar pruebas de integración, en este capítulo se presentan los resultados a dichas
pruebas.
Como parte de la automatización del proceso de: detección, cálculo de coordenadas,
conversión de coordenadas y control del robot, fue necesario elaborar una interfaz en la
plataforma de Matlab R2006b ® que uniera todas estas partes, en la sección 5.1 se
presenta el programa desarrollado y sus funciones.
Una vez terminada la interfaz gráfica fue posible probar el funcionamiento del sistema,
en la sección 5.2 se muestran la manera en que se hicieron las pruebas y los
resultados obtenidos.
En la sección 5.3 y 5.4 se exponen los resultados para los métodos de cámaras fijas y
estimación de pose, respectivamente.
5.1 INTERFAZ GRÁFICA EN MATLAB
El programa desarrollado se elaboró en Matlab R2006b ® usando las herramientas de
procesamiento de imágenes, adquisición de imágenes y desarrollo de interfaces
gráficas GUIDE y tiene como finalidad la selección del modo de operación del sistema,
su visualización y solución de los algoritmos de procesamiento de imágenes, la
localización 3D del objeto y el cálculo de los ángulos de las articulaciones, necesarios
para que el efector final del robot se posicione sobre el objeto buscado.
La interfaz gráfica, figura 5-1, tiene como función principal hacer que el usuario final
visualice y seleccione cualquiera de los modos de operación del sistema. Dependiendo
del modo seleccionado se activarán los botones y controles necesarios para su uso y
se visualizará el resultado para cada uno de ellos, en la sección 5.1.2 se muestra la
descripción de cada cuadro y botón de la interfaz.
80 Capítulo 5 Resultados
Figura 5-1: Interfaz gráfica mostrando el modo de operación de dos cámaras fijas.
5.1.1 DESCRIPCIÓN DEL PROGRAMA
El flujo de control del programa desarrollado se muestra en el siguiente diagrama de
flujo, figura 5-2.
81 Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
Figura 5-2: Diagrama de flujo del programa de la interfaz de usuario.
82 Capítulo 5 Resultados
5.1.2 MODO DE OPERACIÓN DE LA INTERFAZ
En el menú desplegable de modo de operación figura 5-3, se elige cualquiera de los
tres modos de operación: Dos cámaras fijas, cámara en mano y movimiento manual.
Figura 5-3: Modo de operación.
Modo 1. Dos cámaras fijas
En este modo se activan las ventanas de visualización cámara 1 y 2, mostrado en la
figura 5-1, y se desactivan los controles deslizantes para el movimiento manual y el
botón inicio y se activa el botón parar.
Se toman dos imágenes, una con cada cámara y se buscan las coordenadas x y y del
objeto en cada una de ellas, se resuelve el método de triangulación y se obtienen las
coordenadas x,y y z del objeto, se convierten estas coordenadas en ángulos mediante
la cinemática inversa y se envían vía puerto serie a la PC2. Constantemente se repite
esta operación, haciendo posible un seguimiento del objeto siempre y cuando se
encuentre dentro del área de trabajo del robot, hasta que se presione el botón parar.
Modo 2. Cámara en mano
Este modo activa la ventana de visualización cámara 3, mostrado en la figura 5-4, y
desactiva los botones de inicio y los controles deslizantes y activa el botón de paro.
Se toma una imagen con la cámara a 45° y se buscan al objeto, si no se encuentra el
objeto a la vista, se hace otra toma a 90° y si de igual forma no se encuentra al objeto
se hace otra toma a 135°, cuando el objeto es encontrado se toman sus coordenadas x
y y de cada LED del triángulo, se resuelve el método de estimación de pose, se
obtienen las coordenadas x, y y z del objeto y se envían al la PC2 que controla al robot
vía puerto serie.
83 Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
Figura 5-4: Interfaz gráfica mostrando el modo de operación de cámara en mano.
Modo3. Movimiento manual
Este modo activa las ventanas cámara 1 y 2, ver figura 5-1, para visualizar el
movimiento del robot y se activan los controles deslizantes. Envía a la PC2 vía puerto
serie constantemente, cada coordenada en ángulos correspondiente a cada control
deslizante, estos están delimitados en un rango de 0 a 180 grados.
Ventanas de visualización.
La interfaz cuenta con tres ventanas figura 5-5, para mostrar las imágenes tomadas por
las cámaras, nombradas cámara 1, 2 y 3, estas se activarán dependiendo del modo de
trabajo seleccionado.
84 Capítulo 5 Resultados
Figura 5-5: Ventana de visualización de las imágenes de las cámaras
Botón inicio
Inicializa el modo seleccionado y se mantiene desactivado hasta que se presione el
botón parar.
Figura 5-6: Botón inicio
Botón parar
Detiene el modo elegido y regresa al inicio del programa y se mantiene desactivado
hasta que el botón inicio es presionado.
Figura 5-7: Botón parar
Controles deslizantes
Consta de 4 controles deslizantes figura 5-8, uno para cada articulación, cada uno tiene
un rango de operación de 0 a 180, que son los grados en los que se moverá cada
articulación, este valor será enviado a la PC2.
85 Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
Figura 5-8: Controles deslizantes
Cuadro de texto de posición calculada
Estos cuadros, figura 5-9, muestran el resultado de las coordenadas calculadas en los
modos de dos cámaras fijas y cámara en mano.
Figura 5-9: Cuadros de texto
Las coordenadas X, Y y Z para la posición calculada del objeto y Cintura, Hombro,
Codo y Efector final, se usan para el cálculo del ángulo de cada articulación.
5.2 PRUEBA DEL SISTEMA DE CONTROL DEL ROBOT Y EL SISTEMA DE
VISIÓN.
Antes de realizar pruebas sobre el sistema de visión completo, es necesario resolver la
cinemática inversa del robot. Como se mostró en el capítulo 3, el método de cámaras
fijas y el de estimación de pose nos entregan coordenadas X, Y y Z en el plano
cartesiano, del objeto buscado y en el capítulo 4, se mostró que el control del robot
hace referencia a coordenadas polares, en ángulos, para cada articulación, entonces, a
partir de la cinemática inversa convertiremos las coordenadas cartesianas a polares.
86 Capítulo 5 Resultados
5.2.1 Pruebas de la cinemática inversa del robot con su efector final.
El objetivo del problema cinemático inverso consiste en encontrar los valores que
T
deben adoptar las coordenadas (ángulos) del robot θ = [θ1 ,θ 2 ...,θ n ] para que su
extremo se posicione y oriente según una determinada localización espacial.
El procedimiento se basa en encontrar suficiente número de relaciones geométricas en
las que intervendrán las coordenadas del extremo del robot, sus coordenadas
articulares y las dimensiones físicas de sus elementos (Barrientos 1997).
En la figura 5-10, se muestra la configuración del robot y en la tabla 5-1 las los valores
de sus parámetros, el dato de partida es la coordenada espacial p = ( p x , p y , p z ), dada
por el objeto localizado.
El robot posee una estructura planar, quedando este plano definido por el ángulo de la
primera variable articular θ1 , ver figura 5-11, correspondiente a la cintura del robot.
El valor de θ1 se obtiene como (Barrientos 1997):
⎛ py
⎝ px
θ1 = tan −1 ⎜⎜
⎛ p 2 + p 2 −d 2
⎞
3
x
y
⎟⎟ − tan −1 ⎜
⎜⎜
d3
⎠
⎝
⎞
⎟
⎟⎟
⎠
Figura 5-10: Configuración del robot.
87 Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
Tabla 5-1: Valores de los parámetros del robot.
Eslabón
ai
αi
di
θi
1
0cm
90°
35cm
θ1*
2
15cm
0°
0cm
θ 2*
3
22cm
0°
16cm
θ 3*
*Variable
Figura 5-5: Configuración del manipulador plano XY.
Para encontrar los ángulos θ 2 ,θ 3 , se considera el plano formado por él 2 do y 3er eslabón
del brazo, ver figura 5-12. Bajo esta consideración, se obtiene la siguiente expresión:
p x + p y − d 3 + p z − a 2 − a3
2
cos θ 3 =
2
2
2
2
2
2 a 2 a3
y por lo tanto, θ 3 esta dado por:
⎛ 1 − cos θ 2
3
⎜ cos θ 3
⎝
θ 3 = tan −1 ⎜
⎞
⎟
⎟
⎠
del mismo modo, se obtiene θ 2 :
⎛
θ 2 = tan −1 ⎜⎜
pz
⎜ px 2 + p y 2 − d32
⎝
88 ⎞
⎟ − tan −1 ⎛⎜ a3 sen(θ 3 ) ⎞⎟
⎜ a + a cos θ ⎟
⎟⎟
3
3 ⎠
⎝ 22
⎠
Capítulo 5 Resultados
Las ecuaciones obtenidas se programaron en Matlab y las coordenadas p = ( p x , p y , p z )
son la posición del objeto encontrado por el sistema de visión, de esta manera se
obtiene el ángulo de cada articulación para que el efector final se posicione sobre él.
Figura 5-62: Proyección en el plano formado por el 2
do
y
3er eslabón.
5.3 RESULTADOS DEL MODO CÁMARAS FIJAS
Para realizar las pruebas del control del robot y el sistema de visión fue necesario
elaborar el lazo de control que se muestra en la figura 5-13.
En el sistema de visión, en los modos de cámaras fijas y cámara en mano, se busca el
objeto, se calcula su coordenada p = ( p x , p y , p z ), se resuelve la cinemática inversa del
robot y envían los ángulos obtenidos vía puerto serie. La PC2 recibe dichos ángulos y
genera una trayectoria para cada articulación, dependiendo de la posición inicial del
robot, colocando así el efector final sobre el objeto y manteniéndolo en esa posición.
Para la ejecución de este modo de operación del sistema, se fijaron las cámaras como
se muestra en la figura 5-14 a); en esta posición se calibraron, tomando como
coordenada (0,0,0 ) el origen del hombro del robot, ver figura 5-14 c); de esta forma, el
objeto p = ( p x , p y , p z ) encontrado por el sistema de visión hace referencia a ese origen,
ver figura 5-14 b).
89 Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
Figura 5-7: Lazo de control usado en el sistema de visión y control del robot.
a)
b)
c)
Figura 5-8: a) Cámaras fijas y objeto buscado, b) Vista superior del sistema de ejes sobre el robot, c)
Origen sobre del sistema coordenado.
90 Capítulo 5 Resultados
Para realizar las pruebas de validación del sistema de visión con dos cámaras fijas se
colocó al robot en la posición mostrada en la figura 5-14, partiendo de esta posición el
robot se mueve hacia el objeto localizado, de acuerdo a las coordenadas recibidas.
Figura 5-9: Posición inicial del robot.
En la tabla 5-2 se muestran 20 posiciones diferentes del objeto, sus coordenadas
p = ( p x , p y , p z ), calculadas, el ángulo calculado y el ángulo al que llegó el robot para
cada articulación:
91 Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
Tabla 5-2: Resultados de las pruebas con el sistema de dos cámaras fijas
Corrida
POSICIÓN CALCULADA
ÁNGULO CALCULADO
POSICIÓN EN ÁNGULOS DEL ROBOT
X
Y
Z
CINTURA
HOMBRO
CODO
Efector final
CINTURA
HOMBRO
CODO
Efector final
1
22
33
-3
35
-95
-10
90
34.99
-94.99
-9.98
90
2
30
13
-6
8
-145
63
90
7.98
-144.8
63.22
90
3
29
-2
-14
-16
-167
69
90
-16.05
-166.89
69.24
90
4
29
15
-8
12
-148
61
90
11.99
-147.98
61.04
90
5
12
30
-8
53
-149
63
90
53
-148.99
62.88
90
6
18
24
-21
40
-158
38
90
39.99
-156.56
38.33
90
7
23
20
5
28
-132
77
90
27.99
-131.77
76.85
90
8
26
22
4
23
-122
56
90
22.99
-121.91
55.35
90
9
11
27
-9
56
-164
79
90
55.99
-164.2
79.23
90
10
1
29
-22
76
-162
40
90
76
-161.94
40.32
90
11
31
-6
-20
-26
-150
31
90
-25.99
-150.14
30.09
90
12
24
23
-16
28
-146
36
90
27.93
-145.69
36.14
90
13
27
-8
-16
-27
-172
68
90
-26.97
-171.76
69
90
14
32
15
0
7
-125
49
90
6.97
-125.33
48.59
90
15
13
13
-26
60
-210
72
90
59.99
-209.95
72.43
90
16
6
29
-17
66
-164
57
90
66
-164.5
57.6
90
17
22
21
-26
30
-135
39
90
30.01
-134.2
-9.71
90
18
19
31
-11
40
-129
24
90
40
-129.2
23.28
90
19
20
17
-15
33
-180
79
90
33
-180.02
79.52
90
20
32
9
-15
-1
-146
40
90
-0.89
-145.97
39.39
90
En la figura 5-16, se muestra la interfaz gráfica final funcionando en el modo de dos
cámaras fijas, mostrando la posición final del robot y en la figura 5-17 se muestra un
acercamiento del efector final al objeto.
92 Capítulo 5 Resultados
Figura 5-10: Interfaz gráfica final mostrando el modo de cámaras fijas.
Figura 5-11: Acercamiento al efector final sobre el LED.
93 Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
5.4 RESULTADOS DEL MODO ESTIMACIÓN DE POSE
La implementación del método de Grunert (Haralick 1994), que se presentó en el
capítulo 3 cuenta con cuatro posibles soluciones de las cuales dos son soluciones
complejas y dos son soluciones reales. De las soluciones reales solamente una de
ellas corresponde a la orientación del objetivo, aunque ambas cumplen
matemáticamente con las ecuaciones de Grunert.
Las soluciones reales pueden ser vistas en la figura 5-17, donde se ven dos triángulos
uno “azul” y otro “cian” y cada uno representa una de las posibles orientaciones del
objetivo.
De las dos soluciones anteriores se conocen los puntos de los vértices de cada
triángulo, denominados P11, P12 y P13 para el primer triángulo (azul) y P21, P22 y P23
para el segundo triangulo (cian), donde estos puntos están en el marco de referencia
de la cámara, es decir tomando a la cámara como el origen de ese sistema
coordenado; esto se puede observar en la figura 5-18.
Figura 5-12: Triángulos azul y cian con los puntos de sus vértices.
Para determinar la solución correcta de entre los dos planos, se calculó la distancia
entre los puntos q1 , q2 y q3 que son las proyecciones perspectivas en el plano de la
cámara de los puntos Pi1, Pi2 y Pi3, donde i = 1,2. Se denomina “ c’ ” a la distancia
entre q1 y q2, “ a’ ” a la distancia entre q2 y q3 y “ b’ ” a la distancia entre q3 y q1;
estas distancias están en píxeles y son un indicativo del paralelismo entre el plano de la
94 Capítulo 5 Resultados
cámara y el plano del objetivo (triangulo de pruebas), si los planos se encuentran
paralelos las distancias a’, b’, c’ serán iguales y máximas.
Teniendo en cuenta a lo anterior, para saber cuál de las dos soluciones es la correcta
basta con colocar el plano de la cámara paralelo al plano del objetivo (triángulo azul o
cian), además de orientar a la cámara en la dirección del vector perpendicular a ellos
→
→
( c1 y c 2 respectivamente); con esto se garantiza el paralelismo y que además la
cámara pueda ver al objetivo. Estos puntos no son alcanzables, en general, por el
manipulador, así que sólo se realiza una aproximación a ellos.
El punto de intersección que va del centro de perspectiva (C) al vector de dirección del
plano (ci , i= 1,2 ) tendrá una distancia proporcional al ángulo de inclinación del plano
del objetivo, entre mayor sea este ángulo, mayor será la distancia a la que se
encontrarán los puntos. Si la distancia es muy grande llegará a una posición a la que el
manipulador no podrá alcanzar.
Para resolver esta situación se plantea posicionar el robot a un 20% del centro de
perspectiva C y el vector normal del plano (ci), estos puntos se denominan PUNTO1 y
PUNTO2 para el plano 1 y 2 respectivamente, esto se observa en la figura 5-19.
→
Figura 5-13: Plano de la cámara y vectores de dirección
95 →
c1 , c 2 .
Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
Con este movimiento se pretende mejorar la relación de las distancias de los puntos
proyectados en el plano de la imagen, es decir, que las distancias entre los puntos se
hicieran más similares entre ellas. Si en lugar de mejorar, la relación empeora,
sabemos que la solución correcta es la otra, es decir, la relación entre los catetos del
triángulo está dada por:
a'
b'
b'
=
c'
c'
=
a'
ra '− b ' =
rb '− c '
rc ' − a '
Considerando que nuestro triángulo de pruebas es equilátero, la relación que hay entre
cada uno de sus catetos tiende a 1. Lo que se busca al colocar la cámara en la
posición del punto PUNTO1 o PUNTO2 es hacer que las relaciones entre los catetos
del triangulo sean lo más cercano a 1, sí el triángulo tiene una inclinación, esta afectará
a la posición de sus puntos proyectados sobre el plano de la cámara y por consiguiente
a la relación de las distancias de estos puntos (q1, q2 y q3). Se propone hacer un
movimiento hacia cualquiera de las opciones P1 o P2 y si se descubre que fue
equivocado, se entrega como resultado la otra solución.
El procedimiento anterior fue programado con el fin de comprobar su efectividad al
determinar la solución correcta de las dos que el método de Grunert nos entrega. Los
resultados no fueron favorables pues se encontraron las siguientes limitaciones que
impidieron demostrar su efectividad:
1. El movimiento propuesto tiene la finalidad de colocar la cámara paralela al plano
del objetivo y colineal al vector perpendicular al plano del objetivo, los puntos
localizados para dicha prueba son PUNTO1, PUNTO2. Si estas posiciones se
encuentran fuera del área de trabajo del robot, no serán alcanzadas, por lo que
no podemos realizar el procedimiento.
2. En caso de que la posición de prueba (PUNTO1, PUNTO2) se encuentre en el
área de trabajo del robot, existe otra limitante. Los movimientos realizados por
las articulaciones para alcanzar una posición deseada cambian el ángulo de
orientación de la cámara y para poder orientar la cámara en la dirección al vector
normal al plano (ci , i= 1,2) se requiere que el efector final realice movimientos
en dos grados de libertad y sólo se cuenta con uno.
En conclusión, para ubicar al objetivo de pruebas se utilizó siempre la solución más
cercana al centro de la cámara por seguridad del robot, de la cámara y de la cuarta
articulación.
Para realizar las pruebas de sistema de estimación de pose se colocó al robot en la
posición mostrada en la figura 5-15, partiendo de esta posición el robot se mueve hacia
el objeto localizado, de acuerdo a las coordenadas recibidas. En la tabla 5-3 se
96 Capítulo 5 Resultados
muestran 20 posiciones diferentes del objeto, sus coordenadas
p = ( p x , p y , p z ),
calculadas, el ángulo calculado y el ángulo al que llegó el robot para cada articulación:
Tabla 5-3: Resultados de las pruebas de estimación de pose.
Coordenadas
calculadas
Ángulos calculados
Posición alcanzada por el robot
PRUEBA
X
Y
Z
Cintura
Hombro
Codo
Servomotor
Cintura
Hombro
Codo
Servomotor
1
19
30
15
39
-131
16
90
38.673
-130.02
16.93
90
2
39
19
-18
3
-114
-10
90
3.105
-116
-10.3
90
3
40
4
-18
-16
-116
-10
90
-16.069
-115.84
-10.64
90
4
36
-5
-15
-27
-110
-18
90
-27.658
-110.06
-18.18
90
5
38
6
-14
-11
-112
-10
90
-11.081
-111.5
-10.53
90
6
26
19
-13
21
-152
53
90
20.795
-152.78
52.62
90
7
31
20
-8
14
-125
27
90
13.923
-125.77
26.4
90
8
37
9
-15
6
-113
-10
90
-7.115
-113.13
-285
90
90
9
37
6
-15
-11
-114
-10
90
-11.413
-113.36
-10.08
10
37
6
-17
-11
-170
-10
90
-10.837
-117.29
-10.37
90
11
29
18
-16
15
-140
28
90
14.446
-139.23
27.604
90
90
12
24
24
-18
28
138
18
90
27.736
-139.43
-18.81
13
22
22
-12
31
-157
62
90
31.407
-156.95
62.96
90
14
12
28
-9
53
-158
-72
90
51.963
-151.93
8.68
90
15
11
27
-18
56
-166
56
90
55.979
-166
55.95
90
16
11
28
-13
56
-162
66
90
56.448
-355
68.23
90
17
12
35
-12
52
-122
10
90
50.952
-121.95
10.37
90
18
18
31
-12
41
-132
26
90
41.365
-131.55
25.33
90
19
17
27
-21
43
-146
22
90
45-153
-146.37
21.11
90
20
36
19
-7
6
-101
-10
90
6.151
-102.26
-10.44
90
En la figura 5-20, se muestra la interfaz gráfica funcionando en el modo de estimación
de pose y en la figura 5-21 se muestra la imagen vista por la cámara en un
acercamiento al objeto, la posición final del robot.
97 Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
Figura 5-20: Interfaz gráfica mostrando el método de estimación de pose.
Figura 5-14: Acercamiento al efector final sobre el objetivo de prueba.
98 CAPÍTULO 6
6. CONCLUSIONES
En este capítulo se exponen las conclusiones y comentarios finales de la investigación.
En este trabajo de tesis se implementó un sistema de visión robótica con dos
algoritmos distintos de calibración de cámara, probados mediante la aplicación a un
robot tipo PUMA (Programmable Universal Manipulator for Assembly), el cual fue
acondicionado para este fin, como parte del trabajo.
La calibración de las cámaras arrojó el resultado deseado, que fue la matriz de cámara
P, se logró la implementación de un algoritmo con el que a partir de la imagen de un
patrón de calibración se obtuvo dicha matriz.
Para verificar que la matriz de cámara fue la correcta, se hizo una transformación de
puntos del mundo real en 3D al plano de la imagen, se eligieron puntos conocidos del
patrón de calibración y aplicando la relación x=PX, donde x, es la coordenada en
pixeles P, la matriz de cámara y X es el punto conocido en el mundo real; el resultado
de esta transformación fueron coordenadas en que diferían en aproximadamente 3
pixeles respecto a las elegidas en las imágenes, tomando en cuenta que la búsqueda
del pixel se hizo en forma manual, esto quiere decir que la elección de las coordenadas
x y y del punto en la imagen se hizo tomando un promedio de los pixeles que rodeaban
a ese punto.
Es importante señalar en este punto que la calibración se hace fuera de línea, es decir,
los datos se ingresan al algoritmo de manera manual y sólo se hace una vez, siempre y
cuando no se muevan las cámaras en el caso de cámaras fijas.
Los parámetros internos y externos contenidos en la matriz de cámara, fueron
utilizados directamente en el método de visión estereoscópica, no así para el método
de estimación de pose, donde fue necesaria una descomposición RQ, con el fin de
obtener estrictamente los parámetros internos de la cámara, necesarios para resolver
el algoritmo de este método.
6.1 OBJETIVOS CUBIERTOS
Al término del trabajo de tesis se logró cumplir con los siguientes objetivos:
1. Se resolvió el problema principal de la tesis, que fue la calibración de cámaras
para extraer información métrica a partir de imágenes bidimensionales del
99 Visión estereoscópica y estimación de pose para el posicionamiento de un brazo robótico
mundo en tres dimensiones, determinando los parámetros internos y externos de
las cámaras.
2. Se usaron los métodos de visión estereoscópica provista por dos cámaras
calibradas y estimación de pose de una cámara calibrada, para localizar un
punto en el espacio y se usó esta información dentro de un lazo de control que
llevó el efector terminal de un brazo robótico tipo PUMA al punto final deseado.
3. Se construyó una articulación con base para cámara de un grado de libertad
adicional para el eslabón terminal del brazo PUMA.
4. Se reconoció y localizó un blanco (objetivo) mediante el procesamiento de una
imagen.
5. Se realizó la aproximación del eslabón terminal al blanco.
6.2 APORTACIONES
Las aportaciones hechas con la conclusión de esta investigación fueron las siguientes:
• Un sistema de visión robótica físico compuesto de un brazo robótico tipo PUMA,
con un sistema de visión y la implementación de un algoritmo para calibración
de cámaras.
• La construcción de una 4ta articulación al robot.
• El diseño y construcción de tarjetas electrónicas de control más robustas y de
más fácil mantenimiento respecto a las anteriores cuyas deficiencias se
muestran en el capítulo 4.
• Este proyecto sentará las bases para el trabajo conjunto de dos líneas de
investigación del CENIDET: Robótica y visión artificial, asimilando las técnicas
necesarias para realizar la implementación real (no simulación) de sistemas de
robótica inteligente.
6.3 TRABAJOS FUTUROS
Los trabajos futuros relacionados con esta investigación se listan a continuación:
1. La optimización de los códigos hechos en Matlab®, para minimizar el tiempo en
procesado de las imágenes y poder así hacer control del robot en tiempo real y
poder hacer seguimiento de trayectorias del objeto.
2. El rediseño de la 4ta articulación, anexándole un segundo grado de libertad, con
el fin de mejorar la precisión del método de estimación de pose.
3. Utilizar otros tipos de controladores para el robot, con el fin de mejorar el
resultado obtenido en la posición final, ya que el actual presenta deficiencias a
100 Capítulo 6 Conclusiones
perturbaciones mecánicas y no compensa el efecto de la gravedad, metiendo
oscilaciones en las articulaciones que dependen de dicha compensación, codo y
hombro.
4. Mejorar la forma de medición para el establecimiento del sistema de referencia y
evitar agrandar errores.
101 BIBLIOGRAFÍA
Angeles, Jorge. 2003. Fundamentals of Robotic Mechanical Systes, Theory, Methods, and
Algorithms. s.l. : Edit. Springer-Verlag New Cork, 2003.
Aracena Pizarro, Diego, Campos, Pedro and Clésio, Luís T. 2005. Comparación de técnicas
de calibración de cámaras digitales. http://www.scielo.cl/pdf/rfacing/v13n1/art07.pdf. [Online]
2005.
Banespyme. 2006. Visión artificial.
ww.banespyme.org/imagesWeb/ArchivoMultimedia/Documentacion/24/visionartificial.pdf.
[Online] 2006.
Barrientos, Antonio. 1997. Fundamentos de Robótica. s.l. : Universidad Politécnica de Madrid,
1997.
Berthold, Klaus and Horn, Paul. 1986. Robot Vision. 1986.
Cazorla, Miguel Ángel, Otto, Colomina and Sáez, Juan Manuel. 2002. Robótica para las
ingenierías en informática en la universidad de Alicante.
http://www.dccia.ua.es/~jmsaez/Publicaciones/PublicacionesDocentes/Rob%F3tica%20JENUI
%202002.pdf. [Online] 2002.
FAIRCHILD SEMICONDUCTOR. Hoja de especificaciones de TIP101 y TIP106.
http://www.ortodoxism.ro/datasheets/fairchild/TIP101.pdf. [Online].
Faugera, Olivier D, Luong, Quang-Tuan and Maybank, Stephen J. 1992. Camera SelfCalibration: Theory and Experiments. European Conference on Computer Vision, pp. 321-334.
Fu, K.S., González, R. C. and Lee, C. S. G. 1987. Robotics: Control, sensing, vision and
intelligence. s.l. : MacGraw-Hill Book Company, 1987.
González Fernández, Víctor R. 2003. Introducción a la Robótica.
http://cfievalladolid2.net/vieja/tecno/cyr_01/robotica/. [Online] 2003.
Gonzalez, Rafael C. and Woods, Richard E. 2002. Digital Image Processing. s.l. : Prentice
Hall, 2002.
Hartley, R. 1993. Camera Calibration Using Line Correspondences. DARPA93, pp. 361-366.
Hayet, J.B. 2007. Visíon robótica : introducción.
www.cimat.mx:88/~jbhayet/CLASES/VISIONROB/intro.pdf. [Online] 2007.
López Solórzano, Juan Gabriel. 2003. Detallador de tareas para procesos de Visión Robótica.
Cuernavaca, Morelos : CENIDET, 2003.
Microchip Technology Inc. Hoja de especificaciones de PIC16F877A. [Online]
http://ww1.microchip.com/downloads/en/DeviceDoc/39582b.pdf.
102 Moons, Theo. 1998. A Guided Tour Through Multiview Relations. Lecture Notes in Computer
Science, p. 304.
National Semiconductor. Hoja de especificaciones del puente H LMD18200. [Online]
http://www.national.com/pf/LM/LMD18200.html.
Nixon, Mark S. and Aguado, Alberto S. 2002. Feature Extraction in Computer Vision and
Image Processing. Great Britain : Planta Tree, 2002.
Peralta Abundez, Edson Ignacio. 2003. Esquema para la integración de Sistemas de Visión
Robótica. Cuernavaca, Morelos : CENIDET, 2003.
Pérez Ramírez, Agustín 2002. Diseño de un sistema de calibración automática para un
sistema de visión robótica modular. CENIDET. Cuernavaca, Morelos : s.n., 2002.
Haralick, Robert M. 1994. Review and Analysis of solutions of the Three Point Perspective
Pose Estimation Proble. Internacional Journal of Computer Vision.
The MathWorks. Tutorial de interfaces graficas con Matlab. MATLAB and Simulink for
Technical Computing. [Online] http://www.mathworks.com/matlabcentral/.
Hartley, Richard and Sturm, Peter. 1997. Triangulation. Computer Vision and Image
Understanding: CVIU, pp. 146--157.
Villalobos, David Jiménez y Ramirez, Juan Carlos. 2005. Construcción de un brazo robótico
de tres gdl y su control mediante el núcleo híbrido de transición de estados. Cuernavaca,
Morelos : CENIDET, 2005.
Zisserman, Andrew y Hartley, R. 2003. Multiple View Geometry in Computer Vision. s.l. :
Cambridge University Press, 2003.
103 ANEXO
A. IMÁGENES DEL SISTEMA COMPLETO
104 B. CIRCUITO FINAL DE LA TARJETA DE CONTROL
26
27
Circuito de conexión entre PIC16F877A y puerto paralelo de la PC
LPT1_CINTURA
D Connector 25
VSS
VSS
VDD
VDD
2
1
D 7 _ C IN
12
31
1K
RE0/RD/AN5
RE1/WR/AN6
RE2/CS/AN7
MCLR/VPP
VCC R1_CIN
D 6 _ C IN
1
15pF
D 5 _ C IN
OSC1/CLKI
OSC2/CLKO
19
20
21
22
27
28
29
30
D 4 _ C IN
13
14
X1_CIN
C2_CIN
RD0/PSP0
RD1/PSP1
RD2/PSP2
RD3/PSP3
RD4/PSP4
RD5/PSP5
RD6/PSP6
RD7/PSP7
GIRO_CIN
D 3 _ C IN
C1_CIN
15pF
RA0/AN0
RA1/AN1
RA2/AN2/VREF-/CVREF
RA3/AN3/VREF+
RA4/T0CKI/C1OUT
RA5/AN4/SS/C2OUT
15
16
17
VEL_CIN
18
23
24
25
26
D 2 _ C IN
2
3
4
5
6
7
RC0/T1OSO/T1CKI
RC1/T1OSI/CCP2
RC2/CCP1
RC3/SCK/SCL
RC4/SDI/SDA
RC5/SDO
RC6/TX/CK
RC7/RX/DT
D 1 _ C IN
RB0/INT
RB1
RB2
RB3/PGM
RB4
RB5
RB6/PGC
RB7/PGD
D 0 _ C IN
33
E1_CIN
34
E2_CIN
35
SW11
36
SW12
37
38
39
40
1
14
2
15
3
16
4
17
5
18
6
19
7
20
8
21
9
22
10
23
11
24
12
25
13
PIC_CINTURA
D0_CIN
D1_CIN
D2_CIN
D3_CIN
D4_CIN
D5_CIN
D6_CIN
D7_CIN
8
9
10
VCC
11
32
C3_CIN
100pF
PIC16LF877A-I/P
Circuito de la etapa de potencia
VCC
RP7
180
VEL_CIN
8
IN1
IN2
IN3
IN4
IN5
IN6
IN7
OUT1
OUT2
OUT3
OUT4
OUT5
OUT6
OUT7
GND
COM
16
15
14
13
12
11
10
3
3
D1
SALIDA
TIP106-1
1 PNP
TIP101-1
NPN
1
2
ULN-CIN
1
2
3
4
5
6
7
RP6
2 .2 K
2.2K 2.2K
2
RP1
RP3
180
RP4
180
RP5
2 .2 K
100pF
CP1
100pF
GIRO_CIN
CP2
VCC
RP2
D2
D3
RP8
4.7K
CP3
47pF
9
ULN2003A
12V
3
3
2
2
RP9
2 .2 K
TIP106-2
1 PNP
TIP101-2
NPN
1
D5
D6
12V
105 RP11
4.7K
17
16
Conexion a motores
RP10
180
D4
1
9
2
10
3
11
4
12
5
13
6
14
7
15
8
CP4
47pF