Planificaci´on de Vistas para
Reconstrucci´on Tridimensional
de Objetos con Robots M´oviles
por
Juan Irving V´asquez G´omez
Tesis sometida como requisito parcial para obtener el grado de Doctor
en Ciencias en el Instituto Nacional de Astrof´ısica, ´Optica y Electr´onica
Supervisada por:
Dr. Luis Enrique Sucar Succar, INAOE
Dr. Rafael Murrieta Cid, CIMAT
c
INAOE 2014
El autor otorga al INAOE el permiso de reproducir y distribuir copias en su totalidad o en partes de esta tesis
Resumen
En rob´otica, los modelos tridimensionales (3D) de objetos f´ısicos se utilizan en tareas como manipulaci´on, reconocimiento o estimaci´on de posici´on. Por lo tanto, es importante que los robots puedan reconstruir los modelos 3D de los objetos de su ambiente. Para reconstruir un objeto es necesario colocar un sensor de rango en diferentes posiciones y orientaciones (vistas). Desde cada vista se observa y se reconstruye la superficie del objeto. Debido a que el objeto es desconocido cada vista se determina de forma iterativa. Determinar cada vista involucra resolver el problema de la “siguiente mejor vista”. Se ha demostrado que el problema es de complejidad NP-Duro.
Suponemos que el sensor est´a montado en el efector final de un robot manipu-lador m´ovil. Por lo tanto, determinar la siguiente mejor vista involucra determinar el estado del robot tal que el sensor satisface las restricciones de la “siguiente mejor vista”. A dicho estado le llamamos siguiente mejor estado/vista. Adem´as, el robot tiene un posicionamiento imperfecto que afecta la superficie observada o puede causar una colisi´on del robot. El problema abordado en esta tesis es determinar el siguiente mejor estado/vista robusto a errores de posicionamiento del robot. La mayor parte del trabajo relacionado se ha centrado en determinar la vista del sensor sin considerar las capacidades del robot. Los trabajos que determinan el estado del robot lo hacen mediante cinem´atica inversa, sin embargo, dichos tra-bajos pueden generar estados inalcanzables o caer en singularidades durante el c´alculo de la cinem´atica inversa.
En esta tesis se presenta un m´etodo de planificaci´on de vistas robusto a errores de posicionamiento para reconstrucci´on 3D de objetos con robots manipuladores m´oviles. El m´etodo determina el siguiente estado del robot, la vista del sensor y la serie de controles que se debe ejecutar desde el estado actual del robot. El m´etodo desarrollado planifica directamente en el espacio de estados evitando problemas con el c´alculo de la cinem´atica inversa o estados inalcanzables.
El m´etodo propuesto se basa en generar estados candidatos y evaluarlos con una funci´on de utilidad. La evaluaci´on verifica si un candidato aumenta la su-perficie reconstruida y satisface las restricciones de sensado (dependientes de la capacidad del sensor), de registro y posicionamiento (donde se verifica que el es-tado del robot y la trayectoria seguida sean libre de colisi´on).
Las contribuciones son: i) un m´etodo de trazado de rayos jer´arquico que en menor tiempo que un trazado de rayos uniforme aproxima la visibilidad de un candidato, ii) una estrategia de evaluaci´on que aplica la funci´on de utilidad como una serie de filtros que se deben satisfacer y iii) dos m´etodos para robustecer la planificaci´on ante incertidumbre en el posicionamiento. El primer m´etodo calcula la convoluci´on de la utilidad y la distribuci´on del error. El segundo m´etodo calcula la utilidad esperada de una estado/vista dependiendo de la serie de controles que se aplica desde el estado actual del robot.
El m´etodo se implement´o como un sistema integral de reconstrucci´on de ob-jetos. Se realizaron experimentos en simulaci´on y en un robot manipulador m´ovil de 8 grados de libertad. Como resultado, el sistema fue capaz de generar de forma aut´onoma modelos 3D de diversos objetos. El tiempo de planificaci´on de cada es-tado/vista est´a en el orden de segundos cuando no hay error de posicionamiento y en el orden de minutos cuando existe error. Los porcentajes de cobertura son en promedio mayores al 90 % incluso para objetos con huecos y auto oclusiones. El algoritmo redujo casi a cero la tasa de colisi´on del robot y aument´o el porcentaje de superficie cubierta. Este es uno de los primeros algoritmos que reconstruye objetos reales con un robot manipulador m´ovil.
Abstract
Three-dimensional (3D) models from real objects have several applications in robotics. For example, manipulation, object recognition, pose estimation, etc. Therefore, a robot must have the ability of building 3D models from the objects in its environment for interacting with them further. To build a 3D model a range sensor has to be placed at different poses (views). From each view the surface is observed and reconstructed. To determine the next view is known as the next best view problem. It has been demonstrated that the complexity of the problem is NP-Hard.
We assume that the sensor is mounted on the end effector of a mobile manipu-lator robot. Therefore, it is necessary to determine the robot state that matches the next best view. We call to such state the next best view/state. In addition the mobile manipulator has imperfect positioning which alters the coverage and cause robot collisions. The problem addressed in this thesis is to plan the next best view/state robust to positioning errors of the robot. The related work has focused on calculate the view state without considering the robot capabilities. The few works that calculate the robot state use inverse kinematics. However inverse kinematics could generate unreachable states or fall into singularities during the calculation.
We propose a view planning method for object reconstruction robust to posi-tioning errors. The algorithm calculates the sensor view, the robot state and the set of controls that the robot has to apply. The method calculates the next best
view directly in the state space avoiding inverse kinematics calculation. The met-hod is based on generating candidates and evaluated them with a utility function. The evaluation verifies if a candidate could increase the reconstructed surface and satisfies the constraints of sensing (according to the range sensor), registration and positioning (where the state and trajectory are tested for collision).
The contributions are: i) a hierarchical ray tracing method that approximates the visibility in a shorter time than a uniform ray tracing, ii) an evaluation strategy by filters that efficiently evaluates the restrictions of the problem and iii) two strategies to deal with positioning uncertainty. The fist strategy is a convolution of the utility function and the error distribution. The second strategy is the expected utility calculation depending on the controls that are going to be applied.
The method was implemented as an integral system for object reconstruction. Several experiments in simulation and with a real robot of 8 degrees of freedom were performed. The results show that the system is capable of reconstructing an object despite of their shape. The processing time for each view was several seconds for the case of perfect positioning and several minutes for the case of imperfect positioning. The average percentages of covered surface are bigger than 90 %, even for objects with difficult auto occlusions. The method decreases the collision rate almost to zero and increases the covered surface. This is one of the first methods that reconstructs real objects with a mobile manipulator robot.
Agradecimientos
Primero quiero agradecer a mis asesores, Dr. Luis Enrique Sucar y Dr. Rafael Murrieta, por su gu´ıa, paciente y certera, durante toda la investigaci´on. Adem´as, por el aporte de ideas y discusiones ya sea de forma presencial o remota. En especial al Dr. Sucar que desde mi residencia profesional y maestr´ıa ha confiado en mi y me ha motivado para seguir adelante.
Quiero agradecer tambi´en a mis revisores: Dr. Miguel Arias, Dr. Jes´us Gonza-lez, Dr. Eduardo Morales, Dr. Enrique Mu˜noz y Dr. Abraham S´anchez, quienes con sus cr´ıticas constructivas han mejorado sustancialmente el contenido de esta tesis.
Adem´as quiero dar las gracias a Luis Valent´ın y Rigoberto L´opez, quienes me ayudaron a entender los RRTs y a graficar el movimiento del robot. Gracias tambi´en al Dr. Felipe Orihuela, quien me ayud´o a entender algunos conceptos de estad´ıstica y en la implementaci´on de la convoluci´on.
Gracias a todas las personas miembros del laboratorio de rob´otica y del equi-po Markovito, quienes han colaborado en mi investigaci´on. Entre ellos a David Carrillo, quien que me ayud´o con la configuraci´on f´ısica del robot.
No pod´ıa faltar Kika Varela, a quien le agradezco su disponibilidad y apoyo en los experimentos en simulaci´on y con el robot real. Finalmente quiero reconocer el apoyo de mis padres y familia que siempre estuvieron pendientes de mi avance.
Dedicatorias
A Dios por brindarme la salud para llegar hasta este punto de mi vida.
A mis padres, Roberto Enrique V´asquez Mart´ınez y Batilde Valeria G´omez Carrera, quienes me han inculcado las ganas de luchar por mis sue˜nos.
´Indice general
1. Introducci´on 1
1.1. Motivaci´on . . . 1
1.2. Reconstrucci´on Tridimensional de Objetos . . . 2
1.3. Planificaci´on de Vistas . . . 4
1.4. Planificaci´on del Siguiente Mejor Estado/Vista . . . 6
1.4.1. Retos del Problema . . . 6
1.5. Pregunta de Investigaci´on . . . 7
1.6. Objetivos . . . 8
1.7. Hip´otesis y Resultados . . . 8
1.8. Contribuciones . . . 9
1.9. Algoritmo General de Planificaci´on . . . 11
1.10. Organizaci´on de la Tesis . . . 11
2. Fundamento Te´orico y Establecimiento del Problema 15 2.1. Escena de Reconstrucci´on . . . 16
2.1.1. El Objeto y el Espacio de Trabajo . . . 16
2.1.2. El Sensor y el Espacio de Vistas . . . 17
2.1.3. El Robot, Espacio de Configuraciones y Espacio de Estados 19 2.1.4. Definici´on de Estado/Vista . . . 21
2.2. Posicionamiento . . . 21
2.2.1. Control y Transici´on de Estados . . . 22
2.2.2. Caminos y Trayectorias . . . 22
2.3. Percepci´on . . . 23
2.3.1. Oclusiones . . . 23
2.4. Actualizaci´on del Modelo . . . 24
2.4.1. Registro . . . 24
2.4.2. Algoritmo Iterative Closest Point . . . 25
2.5. Problema del Siguiente Mejor Estado/Vista . . . 26
3. Trabajo Relacionado 29 3.1. Clasificaci´on de la Planificaci´on de Vistas . . . 30
3.2. Reconstrucci´on de Objetos . . . 31
3.2.1. Sistemas de Posicionamiento Fijo . . . 31
3.2.2. Sistemas de Posicionamiento M´ovil . . . 36
3.3. Exploraci´on de Ambientes . . . 37
3.3.1. Exploraci´on 2D de ambientes . . . 39
3.3.2. Reconstrucci´on 3D de Ambientes . . . 40
3.4. Planificaci´on de Movimientos en Condiciones de Incertidumbre . . 41
3.5. Resumen . . . 44
4. Modelo de la Escena y Modelo de Percepci´on 45 4.1. Modelo de la Escena de Reconstrucci´on . . . 45
4.1.1. Nube de Puntos del Objeto . . . 47
4.1.2. Octree Probabilista . . . 47
4.1.3. Malla Triangular de la Escena . . . 49
4.2. Modelo del Sensor y Actualizaci´on del Octree . . . 49
4.2.1. Actualizaci´on con una Imagen de Rango . . . 52
4.3. C´alculo de la Visibilidad . . . 53
4.3.1. Trazado de Rayos Jer´arquico . . . 53
4.3.2. Profundidad Inicial . . . 54
´INDICE GENERAL xi
4.3.4. Algoritmo de Trazado de Rayos Jer´arquico . . . 57
4.4. Detecci´on de Colisiones . . . 57
4.5. Resumen . . . 59
5. Planificaci´on del Sig. Mejor Estado/Vista 61 5.1. Utilidad de un Estado/Vista Candidato . . . 62
5.1.1. Funci´on de Utilidad . . . 63
5.2. Algoritmo de Siguiente Mejor Estado/Vista . . . 65
5.3. Planificaci´on de Movimientos . . . 68
5.4. Criterio de Paro . . . 70
5.5. An´alisis . . . 70
5.5.1. Tiempo de Procesamiento . . . 71
5.5.2. Convergencia . . . 73
5.5.3. Eficiencia contra Optimalidad . . . 74
5.6. Resumen . . . 74
6. Planif. bajo Incertidumbre en el Pos. 77 6.1. Error de Posicionamiento . . . 78
6.2. Error de Posicionamiento en el Espacio de Trabajo . . . 79
6.2.1. Modelo del Error . . . 79
6.2.2. Convoluci´on de la Utilidad . . . 79
6.3. Error en el Espacio de Estados . . . 81
6.3.1. Modelo del Error . . . 82
6.3.2. Modelo de Movimiento . . . 85
6.3.3. Utilidad Esperada . . . 87
6.3.4. An´alisis . . . 89
6.4. Resumen . . . 92
7. Experimentos y Resultados 95 7.1. Simulaci´on sin Error de Posicionamiento . . . 95
7.1.1. Notaci´on y Escena de Reconstrucci´on Simulada . . . 95
7.1.2. Validaci´on de la Funci´on de Utilidad . . . 97
7.1.3. Tiempo de Procesamiento . . . 100
7.1.4. Reconstrucci´on de Objetos Complejos . . . 101
7.1.5. Comparaci´on de Variantes . . . 102
7.2. Simulaci´on con Error de Pos. en el Espacio de Trabajo . . . 104
7.3. Simulaci´on con Error de Pos. en el Espacio de Estados . . . 109
7.3.1. Tasa de Colisi´on . . . 110
7.3.2. Reconstrucci´on de Objetos . . . 110
7.4. Reconstrucci´on en una Escena Real . . . 112
7.4.1. Objetivo . . . 112
7.4.2. Escena de Reconstrucci´on . . . 113
7.4.3. Resultados . . . 114
7.4.4. Discusi´on . . . 119
7.5. Reconstrucci´on en una Escena Real con Obst´aculos . . . 121
7.6. Resumen . . . 124
8. Conclusiones y Trabajo Futuro 127 8.1. Conclusiones . . . 127
8.2. Trabajo Futuro . . . 129
A. Par´ametros de Configuraci´on. 131
B. Datos Adicionales del Experimento con el Robot Real 137
C. Publicaciones 141
´Indice de figuras
1.1. Imagen de rango parcial y modelo tridimensional completo. . . 3
1.2. Ejemplo de diferentes vistas que observan un objeto. . . 5
1.3. Proceso de reconstrucci´on de objetos con planificaci´on de la siguien-te mejor vista. . . 5
1.4. Diagrama de flujo del proceso de reconstrucci´on tridimensional de objetos y planificaci´on del siguiente mejor estado/vista. . . 12
2.1. Proceso de reconstrucci´on de objetos. . . 15
2.2. Vista, geometr´ıa y cono de visibilidad de un sensor . . . 18
2.3. Configuraci´on del robot. . . 19
2.4. Oclusiones . . . 24
3.1. Clasificaci´on de los algoritmos de planificaci´on de vistas. . . 30
4.1. Representaci´on de la escena de reconstrucci´on . . . 46
4.2. Estructura de un octree . . . 48
4.3. Ejemplos de la representaci´on del ambiente. . . 50
4.4. trazado de Rayos . . . 51
4.5. Trazado de rayos uniforme y trazado de rayos jer´arquico . . . 54
4.6. Trazado de rayos jer´arquico . . . 55
4.7. Malla triangular del robot m´ovil. . . 59
5.1. Paradigma basado en b´usqueda para planificar vistas. . . 62
5.2. Algoritmo para determinar el siguiente mejor estado/vista (SMEV). 66 5.3. Extensi´on delRapidly-Exploring Random Tree (RRT). . . 69 6.1. Gr´afica de la utilidad de las vistas candidatas antes y despu´es de
la convoluci´on. . . 80 6.2. Transformaci´on de las muestras del espacio de controles al espacio
de utilidades. . . 83 6.3. Ilustraci´on de muestras generadas para calcular la utilidad esperada. 88 6.4. Volumen de muestreo . . . 91 7.1. Escena simulada de reconstrucci´on. . . 97 7.2. Objetos sint´eticos utilizados para simular diversas reconstrucciones. 97 7.3. Comparaci´on del la superficie cubierta usando los enfoques de
Ga-nancia de Informaci´on (IG) y Utilidad Combinada (UC). . . 99 7.4. Comparaci´on de los vol´umenes desconocidos en el octree utilizando
los enfoques de Ganancia de Informaci´on (IG) y Utilidad Combi-nada (UC). . . 99 7.5. Reconstrucci´on del objeto Conejo . . . 102 7.6. Ilustraci´on deloctree probabilista durante la reconstrucci´on del
ob-jeto Conejo. . . 103 7.7. Nube de puntos de los objetos reconstruidos. . . 103 7.8. Objetos tridimensionales del experimento con el m´etodo
convolu-ci´on despu´es de evaluaconvolu-ci´on (CAE). . . 106 7.9. Reconstrucci´on de los objetos esfera y taza bajo error de
posicio-namiento. . . 107 7.10. Reconstrucci´on del objeto SGI-Logo bajo error de posicionamiento 108 7.11. Tasa de colisi´on para diferentes k . . . 111 7.12. Porcentajes de cobertura de los objetos Conejo y Tetera. . . 111 7.13. Escena de reconstrucci´on. . . 113 7.14. Comparaci´on del modelo obtenido con respecto del objeto real. . . 116
´INDICE DE FIGURAS xv
7.15. Ejemplos de las diferentes configuraciones que el robot toma para
observar el objeto. . . 117
7.16. Octree final de la reconstrucci´on. . . 117
7.17. Ejemplos del desplazamiento del robot. . . 118
7.18. Ejemplo del registro de dos nubes de puntos del objeto. . . 120
7.19. Escena de reconstrucci´on con obst´aculos. . . 121
7.20. Nube de puntos acumulada del objeto. . . 122
7.21. Proyecci´on de los estados del robot desde donde se hizo una per-cepci´on. . . 122
7.22. Ejemplo de evasi´on de obst´aculos. . . 123
B.1. Primera parte de las percepciones realizadas en una reconstrucci´on del objeto real. . . 139
B.2. Segunda parte de las percepciones realizadas en una reconstrucci´on del objeto real. . . 140
´Indice de tablas
3.1. Algoritmos de planificaci´on para sistemas de posicionamiento fijo. 35
3.2. Planificaci´on de vistas para sistemas de posicionamiento m´ovil. . . 38
3.3. Planificaci´on para exploraci´on de ambientes. . . 42
7.1. Tiempo promedio de evaluaci´on de una vista utilizando trazado de rayos jer´arquico. . . 101
7.2. Resultados de la reconstrucci´on de cada uno de los objetos sint´eticos.101 7.3. Resultados de la reconstrucci´on para cada variante. . . 104
7.4. Cobertura para cada enfoque. UE-PE incrementa el porcentaje de cobertura y decrementa la tasa de colisi´on. . . 112
7.5. Resultados de la reconstrucci´on de la silla de oficina. . . 115
7.6. Reconstrucci´on de objetos en un escenario con obst´aculos. . . 123
A.1. Configuraci´on del sensor en los experimentos en simulaci´on. . . 131
A.2. Configuraci´on del experimento de la secci´on 7.1. Validaci´on de la funci´on de utilidad. . . 132
A.3. Configuraci´on del experimento sobre el tiempo de procesamiento y el c´alculo de la visibilidad. Secci´on 7.1.3 . . . 133
A.4. Configuraci´on del experimento de la reconstrucci´on de objetos di-ferentes. Secci´on 7.1.4 . . . 134
A.5. Configuraci´on del experimento para probar las variantes del algo-ritmo. Secci´on 7.1.5. . . 134
A.6. Par´ametros del experimento de planificaci´on del siguiente mejor estado/vista con error de posicionamiento. . . 135 A.7. Par´ametros de la distribuci´on del error usados en los experimentos
en simulaci´on. . . 135 B.1. Configuraci´on de la planificaci´on de movimientos para la
recons-trucci´on de la silla de oficina. . . 137 B.2. Par´ametros del sensor utilizado en el experimento en un ambiente
real. . . 138 B.3. Configuraci´on de la reconstrucci´on en una escena con obst´aculos. . 138
Nomenclatura
ι Nube de puntos percibida por el sensor.
A Robot
C Espacio de configuraciones
Cf ree Espacio de configuraciones libre
Cobs Espacio de configuraciones en colisi´on
F(v) Frustum del sensor.
O Regi´on de obst´aculos
V Espacio de vistas
W Espacio de trabajo. W =R3
Wf ree Espacio de trabajo libre
Wobj Objeto.Wobj ⊂ W
Wunk Regi´on desconocida del espacio de trabajo
X Espacio de estados
Xf ree Espacio de estados libre de colisi´on
τ Trayectoria. τ : [0, T]→ Xf ree xix
oc(v) N´umero de v´oxeles ocupados dentro del cubo encapsulador visibles desde
v
unk(v) N´umero de v´oxeles desconocidos dentro del cubo encapsulador visibles desde v
D Sensor de rango
M Octree probabilista
Mbbox Conjunto de v´oxeles dentro del cubo encapsulador
Mf ree Conjunto de v´oxeles libres
Munk Conjunto de v´oxeles desconocidos
Pobj Nube de puntos acumulada del objeto.
q Configuraci´on del robot.q = [x, y, z, θ0, . . . , β5]T U Conjunto de controles posibles.
u Control. Conjunto de velocidades o aceleraciones aplicadas al robot. Tam-bi´en se puede referir a la funci´on de entrada, u: [0, T]→U
v Vista. v = [x, y, z, α, β, γ]T
x Estado del robot.x={q,q˙}
PPV Problema de planificaci´on de vistas. RTO Reconstrucci´on tridimensional de objetos
Cap´ıtulo 1
Introducci´
on
En rob´otica, los modelos tridimensionales (3D) de objetos f´ısicos se utilizan en tareas como manipulaci´on, reconocimiento o estimaci´on de posici´on. Por lo tanto, es importante que los robots puedan reconstruir los modelos 3D de los objetos de su ambiente. Para reconstruir un objeto es necesario colocar un sensor de rango en diferentes posiciones y orientaciones (vistas). Desde cada vista se observa y se reconstruye la superficie del objeto. Debido a que el objeto es desconocido cada vista se determina de forma iterativa. Determinar cada vista involucra resolver el problema de la “siguiente mejor vista”. Se ha demostrado que el problema es de complejidad NP-Duro [Scott et al, 2003]. En esta tesis se propone un algoritmo de planificaci´on de vistas para robots manipuladores m´oviles, con un enfoque particu-lar en el tiempo de procesamiento, debido a que el tiempo disponible est´a limitado por la autonom´ıa del robot, y considerando la incertidumbre de la percepci´on y la incertidumbre en los movimientos del robot.
1.1.
Motivaci´
on
Los modelos tridimensionales de objetos reales tienen hoy en d´ıa aplicaciones en ´areas tan diversas como la industria, la medicina, la arqueolog´ıa, la rob´otica, etc. En rob´otica, los modelos tridimensionales les permiten a los robots interactuar
con su ambiente y realizar tareas como:
Manipulaci´on.Si se conoce el modelo tridimensional del objeto, es posible determinar los puntos de contacto donde el robot debe colocar los dedos del efector final para garantizar sujeci´on. El software GraspIt [Miller et Allen, 2004], desarrollado en la Universidad de Columbia, tiene implementados varios al-goritmos para planificar sujeci´on; todos ellos requieren un modelo tridimen-sional (en su caso una malla triangular) del objeto para calcular los puntos de contacto y detectar posibles colisiones.
Estimaci´on de pose. Para estimar la pose (posici´on y orientaci´on) de un objeto es necesario saber la transformaci´on traslacional y rotacional entre el objeto observado y uno de referencia. El objeto de referencia es usualmente un modelo tridimensional, ya sea una nube de puntos o una malla triangular. En [Kehoe et al, 2013] se estima la pose de un objeto mediante un ajuste de m´ınimos cuadrados entre nubes de puntos.
Planificaci´on de movimientos. Cuando el robot consiste de dos o m´as cuerpos r´ıgidos o el ambiente tiene obst´aculos, es necesario verificar que el robot no tenga auto colisiones o colisiones con el ambiente. La detecci´on de colisiones se hace con algoritmos de proximidad. Por ejemplo, el propuesto en [Eric Larsen et Manocha, 2000] utiliza mallas triangulares del robot y los objetos del ambiente.
As´ı que para que un robot manipulador m´ovil pueda realizar diversas tareas de forma aut´onoma, es necesario que sea capaz de reconstruir los objetos de su ambiente.
1.2.
Reconstrucci´
on Tridimensional de Objetos
Hay al menos dos formas de generar un modelo tridimensional. La primera es mediante modelado asistido por computadora (CAD); esta alternativa requiere
1.2. RECONSTRUCCI ´ON TRIDIMENSIONAL DE OBJETOS 3 largas horas de trabajo de un especialista en el ´area, con la limitante de que los modelos generados muchas veces no tienen detalles muy precisos. La segunda forma es mediante la reconstrucci´on tridimensional de objetos (RTO), que consiste en generar el modelo 3D haciendo mediciones de la superficie del objeto. La RTO es m´as adecuada cuando el proceso requiere ser autom´atico, como es el caso de los robots aut´onomos que interact´uan con el ambiente.
(a) (b)
Figura 1.1: A la izquierda se muestra una imagen de rango del objeto Conejo de Stanford. Una imagen de rango solo contiene una parte del objeto. En consecuen-cia es necesario tomar varias im´agenes desde diferentes posiciones y orientaciones, llamadas vistas. A la derecha se muestra el modelo 3D completo. Im´agenes toma-das del repositorio 3D de la Universidad de Stanford [Stanford University, 2009].
En la reconstrucci´on tridimensional de objetos intervienen tres elementos: el objeto, el sensor de rango y el sistema de posicionamiento. El objeto es la enti-dad a ser modelada. En esta tesis, los objetos a reconstruir son aquellos que se encuentran en ambientes de oficina, tanto objetos de tipo manipulable como mo-biliario. El sensor de rango adquiere la forma del objeto y la representa con una imagen de rango (consideramos una imagen de rango como un conjunto de puntos que pertenecen a la superficie del objeto). La figura 1.1(a) muestra el ejemplo de una imagen de rango. El sistema de posicionamiento cambia la perspectiva del sensor con respecto del objeto. Hay dos clases de sistemas de posicionamiento, los que cambian la posici´on del objeto, como un tornamesas, o los que cambian la perspectiva del sensor como es el caso de un brazo manipulador. En esta tesis suponemos que el sistema de posicionamiento es un robot manipulador m´ovil que
tiene montado el sensor de rango en el efector final, a esta configuraci´on se le llama “sensor en mano” del ingl´es eye-in-hand sensor.
El proceso de RTO consiste en percibir con el sensor varias im´agenes de rango desde diferentes poses, de tal forma que la mayor´ıa de la superficie del objeto sea observada. Una vez que se han percibido m´as de dos im´agenes de rango, ´estas se registran y se fusionan. El registro consiste en trasladar y rotar las im´age-nes a un mismo marco de referencia [Besl et McKay, 1992], de tal forma que los traslapes entre im´agenes coincidan. La etapa de fusi´on consiste en eliminar las partes redundantes del modelo. Al finalizar la reconstrucci´on es com´un aplicar un proceso de rellenado de superficies faltantes y refinado del modelo construido [Weyrich et al, 2004].
1.3.
Planificaci´
on de Vistas
Una vista de un sensor describe la posici´on y orientaci´on del sensor. Para reconstruir un objeto es necesario un conjunto de vistas desde donde el sensor hace observaciones. La figura 1.2 a) muestra un conjunto de vistas que observa un objeto. Determinar el conjunto de vistas necesario para reconstruir un objeto, cumpliendo las restricciones de sensado, posicionamiento y registro, es conocido como el problema de planificaci´on de vistas (PPV). A menos que se trate de un objeto muy sencillo, como una esfera, el PPV no es un problema f´acil, dado que el espacio de b´usqueda de la siguiente vista (o estado del robot como se ver´a m´as adelante) es muy grande (usualmente infinito). De acuerdo con Scott [Scott et al, 2003] el problema de planificaci´on de vistas es un problema NP-Duro. Cuando se conoce a priori la superficie completa del objeto, el problema se modela como el problema de la galer´ıa de arte, donde se busca hallar el m´ınimo n´umero de guardias que observen (vigilen) la galer´ıa. Para dicho problema existen diversas soluciones basadas en geometr´ıa computacional [O’Rourke, 1987], de tal forma que es posible determinar en una iteraci´on el conjunto de vistas que observa
1.3. PLANIFICACI ´ON DE VISTAS 5
(a) (b)
Figura 1.2: Ejemplo de diferentes vistas que observan un objeto. La sub-figura a) muestra varias vistas (posiciones y orientaciones del sensor) que observan un objeto. La sub-figura b) muestra un conjunto de estados del robot desde donde el sensor montado en el efector final observa el objeto. Cuando el sensor est´a montado en el efector final es necesario determinar adem´as de la vista del sensor el estado del robot.
toda la superficie. Sin embargo, en este trabajo suponemos que no se conoce la superficie del objeto que se quiere modelar. As´ı que la soluci´on al problema consiste de un proceso iterativo de observar el objeto y decir d´onde observar a continuaci´on. En espec´ıfico, el proceso consiste en cuatro pasos: posicionamiento del sensor, percepci´on de la imagen de rango, actualizaci´on del modelo y determinaci´on de la siguiente vista o estado del robot. La figura 1.3 ilustra el proceso. A la siguiente vista se le llama por definici´on la siguiente mejor vista (SMV) [Connolly, 1985].
Figura 1.3: Proceso de reconstrucci´on de objetos con planificaci´on de la siguiente mejor vista. Proceso iterativo de cuatro pasos que se repite hasta que se alcanza un criterio de paro.
1.4.
Planificaci´
on del Siguiente Mejor Estado/Vista
Tradicionalmente lasiguiente mejor vista (SMV) es aquella posici´on y orien-taci´on del sensor que maximiza el incremento de superficie reconstruida. En esta tesis nos interesa determinar la vista del sensor, el estado del robot y la serie de controles que al aplicarse generan una trayectoria libre de colisi´on desde el estado actual del robot. A este problema le llamamos planificaci´on del siguiente mejor estado/vista (SMEV). La definici´on formal del problema se hace m´as adelante en la secci´on 2.5.
1.4.1.
Retos del Problema
El problema del siguiente mejor estado/vista cambia el espacio de b´usqueda, del espacio de vistas al espacio de estados, y aumenta las restricciones del pro-blema. El nuevo espacio de b´usqueda tiene dimensionalidad igual a la dimensi´on del estado del robot, la cual es usualmente mayor al espacio de vistas. Las res-tricciones tradicionales del problema de SMV son sensado (asegurar visibilidad), posicionamiento del sensor y registro. A ellas se aumenta la restricci´on de posi-cionamiento del robot. La incorporaci´on de ´esta nueva restricci´on hace necesario resolver problemas de rob´otica m´ovil, como son: planificaci´on de movimientos, manejo de la incertidumbre en posici´on, reducci´on de la distancia de navegaci´on y r´apido procesamiento debido a los recursos de c´omputo limitados.
Manejo de incertidumbre en el posicionamiento
En particular, suponemos que el robot ejecuta un lazo de control abierto, es decir, el robot no se localiza hasta que percibe una imagen de rango. La consecuen-cia natural es que, mientras el robot se desplaza, su incertidumbre en la posici´on aumenta, provocando disminuci´on en la superficie cubierta, fallo en el registro o colisiones del robot con el objeto o el ambiente. Por lo tanto, es necesario que los estados/vista determinados sean robustos a un posicionamiento incierto. Con
1.5. PREGUNTA DE INVESTIGACI ´ON 7 robusto queremos decir que existe una muy baja probabilidad (menor que la que existe con la planificaci´on tradicional) de que el robot colisione y que el porcentaje de cobertura sea similar al que se tuviera si no existiera error de posicionamiento.
Reducci´on de la distancia de navegaci´on
A diferencia del problema de la siguiente mejor vista, donde el costo de re-construcci´on es ´unicamente el n´umero de percepciones, en el problema del SMEV es necesario contemplar el esfuerzo que realiza el robot para llegar a cada esta-do/vista. Una forma de estimar el esfuerzo que realiza el robot es mediante la distancia de navegaci´on. Por lo tanto es importante que el siguiente mejor esta-do/vista reduzca la distancia viajada por el robot.
Tiempo de procesamiento adecuado
El algoritmo de planificaci´on del siguiente mejor estado/vista debe funcionar en un tiempo adecuado para poder ser implementado en robots m´oviles. El robot que utilizamos en los experimentos tiene una autonom´ıa aproximada de una hora y media. Esto limita la reconstrucci´on a ese tiempo. En promedio se hacen quince iteraciones para reconstruir un objeto, quedando un tiempo de seis minutos por iteraci´on. Estos seis minutos se deben repartir entre el posicionamiento, registro, actualizaci´on y planificaci´on. Por lo tanto, un tiempo de procesamiento adecuado para planificar el siguiente mejor estado/vista debe ser menor a seis minutos.
1.5.
Pregunta de Investigaci´
on
La pregunta de investigaci´on de esta tesis es:
¿De qu´e forma es posible determinar, en un tiempo de procesamiento adecuado para la tarea, el siguiente mejor estado/vista para reconstrucci´on tridimensional de objetos robusto a incertidumbre en el posicionamiento?
1.6.
Objetivos
Dada la problem´atica de la planificaci´on de vistas para reconstrucci´on tridi-mensional de objetos con robots m´oviles se plante´o el objetivo de la investigaci´on como:
Desarrollar un algoritmo de planificaci´on de vistas para
reconstruc-ci´on tridimensional de objetos para robots manipuladores m´oviles que
genere, en un tiempo adecuado para la tarea, estados/vista robustos a
errores de posicionamiento y eficientes en la distancia de navegaci´on.
El objetivo general se divide en los siguientes objetivos parciales:
1. Establecer un modelo que caracterice el error de posicionamiento del robot y el costo de la nevegaci´on en la reconstrucci´on.
2. Establecer una funci´on de utilidad que eval´ue un estado del robot a partir de las restricciones de percepci´on y la restricciones de posicionamiento. 3. Dise˜nar un algoritmo de planificaci´on del siguiente mejor estado/vista (SMEV)
basado en el paradigma de b´usqueda (generaci´on y evaluaci´on de candida-tos).
4. Reducir el tiempo de procesamiento del algoritmo de planificaci´on del si-guiente mejor estado/vista.
5. Extender el algoritmo de planificaci´on del siguiente mejor estado/vista para que sea robusto a errores de posicionamiento.
1.7.
Hip´
otesis y Resultados
Partiendo de que la tarea es la de reconstrucci´on tridimensional de objetos con robots m´oviles, que el tiempo de procesamiento adecuado est´a en el orden de minutos y que sea robusto a incertidumbre de posicionamiento significa que
1.8. CONTRIBUCIONES 9 existe una muy baja probabilidad de que el robot colisione y que el porcentaje de cobertura de la superficie es similar al que se tuviera si no existiera error de posicionamiento; la hip´otesis de este trabajo es la siguiente:
Es posible determinar, en un tiempo de procesamiento aceptable para la tarea, el siguiente mejor estado/vista, robusto a incertidumbre en el posicionamiento, mediante un enfoque de generaci´on de candidatos y evaluaci´on de la utilidad es-perada.
A fin de mostrar la veracidad de la hip´otesis, se ha investigado, desarrollado e implementado, un algoritmo de planificaci´on que cumple con los requisitos estable-cidos. El algoritmo eval´ua los candidatos con la utilidad esperada, que incorpora la incertidumbre en posici´on, de tal forma que el estado/vista seleccionado como el mejor es robusto a la incertidumbre en el posicionamiento. Adem´as, el algorit-mo reduce el tiempo de procesamiento mediante un esquema de evaluaci´on por filtrado y una aproximaci´on de la visibilidad mediante trazado de rayos jer´arquico. El algoritmo se implement´o y se realizaron diversos experimentos en simulaci´on y con un robot manipulador m´ovil de ocho grados de libertad que lleva montado un sensor Kinect. En los experimentos en simulaci´on se compar´o la funci´on de utilidad propuesta basada en superficie de oclusi´on contra Ganancia de Informaci´on, se midi´o el tiempo de procesamiento, se reconstruyeron diversos objetos y se midi´o el desempe˜no del algoritmo cuando existe error de posicionamiento.
Los resultados muestran que el algoritmo es capaz de reconstruir objetos reales diversos a pesar de la incertidumbre en el sensor y en el posicionamiento robot. El porcentaje de reconstrucci´on alcanzado es cercano al cien por ciento.
1.8.
Contribuciones
Un algoritmo de planificaci´on que determina en un tiempo aceptable para la tarea el siguiente mejor estado/vista, robusto a errores de posicionamiento, para reconstrucci´on 3D de objetos con robots m´oviles.
De la cual se derivan las siguientes contribuciones:
Una funci´on de utilidad y una estrategia de evaluaci´on. La funci´on de uti-lidad incorpora las restricciones de posicionamiento, percepci´on y registro del problema del siguiente mejor estado/vista. La estrategia de evaluaci´on disminuye el tiempo de procesamiento requerido al aplicar cada restricci´on como un filtro. El orden de los filtros se coloc´o de menor a mayor con res-pecto del tiempo de procesamiento que requieren para calcularse.
Un algoritmo de trazado de rayos jer´arquico que aproxima la visibilidad de una vista en menor tiempo de procesamiento. El algoritmo es jer´arquico por que aprovecha la estructura jer´arquica de la representaci´on del objeto (la representaci´on del objeto por octree se describe en el cap´ıtulo 4) para calcular la visibilidad en una resoluci´on burda y solo aplicar una resoluci´on fina en las regiones importantes.
El m´etodo de evaluaci´on por convoluci´on despu´es de la evaluaci´on (CAE) que aumenta el porcentaje de cobertura de la reconstrucci´on cuando existe un error de posicionamiento en el espacio de trabajo (´unicamente en la posici´on del sensor).
Un algoritmo para calcular la utilidad esperada de una vista. La utilidad esperada determina la utilidad mas probable que se alcanzar´a en una percep-ci´on. Al utilizar la utilidad esperada como utilidad se aumenta el porcentaje de cobertura y se disminuye la tasa de colisi´on del robot cuando existe in-certidumbre en el posicionamiento.
1.9. ALGORITMO GENERAL DE PLANIFICACI ´ON 11
1.9.
Algoritmo General de Planificaci´
on
El algoritmo propuesto para determinar el siguiente mejor estado/vista (SMEV) se basa en generaci´on de candidatos y evaluaci´on de los mismos. La figura 1.4 muestra el diagrama de flujo de la reconstrucci´on junto con la planificaci´on del SMEV. La evaluaci´on, que es el proceso m´as costoso computacionalmente, se rea-liza a trav´es de una serie de filtros. Los primeros filtros son posicionamiento y registro. Despu´es se restringe el n´umero de candidatos a un conjunto peque˜no. A continuaci´on se planifican trayectorias a cada candidato desde el estado actual del robot. En seguida se eval´ua la utilidad o la utilidad esperada, dependiendo de si el posicionamiento es perfecto o existe error en el posicionamiento. Al final, se ordenan los candidatos restantes y se determina el SMEV como el candidato con mejor evaluaci´on.
1.10.
Organizaci´
on de la Tesis
El resto de la tesis se organiza de la siguiente forma. El cap´ıtulo 2, Marco Te´orico, describe los fundamentos te´oricos y la definici´on del problema que se aborda en la tesis. El cap´ıtulo 3, Trabajo Relacionado, hace una revisi´on de los trabajos cercanos; tres ´areas son tratadas, reconstrucci´on de objetos, exploraci´on de ambientes e incertidumbre en la planificaci´on de movimientos. El cap´ıtulo 4, Modelo del Ambiente y Modelo de Percepci´on, describe la forma en que se repre-senta la informaci´on y describe el algoritmo de trazado de rayos para aproximar la visibilidad de una vista. El cap´ıtulo 5, Planificaci´on del Siguiente Mejor Es-tado/Vista describe el algoritmo general de planificaci´on, y tambi´en describe la funci´on de utilidad con la que se eval´uan las vistas. El cap´ıtulo 6, Planificaci´on bajo Incertidumbre, describe la forma de evaluar una vista cuando existe error de posicionamiento en el robot; para ello se utiliza la utilidad esperada. La uti-lidad esperada se inserta en el algoritmo general, descrito en el cap´ıtulo 5, para
Figura 1.4: Diagrama de flujo del proceso de reconstrucci´on tridimensional de objetos y planificaci´on del siguiente mejor estado/vista (SMEV). El algoritmo de planificaci´on se basa en generar estados candidatos y evaluarlos a trav´es de una serie de filtros. Los primeros filtros aplicados son los que tienen un menos costo computacional. Para finalizar la evaluaci´on, si existe posicionamiento perfecto se calcula la utilidad simple, de lo contrario, se calcula la utilidad esperada.
1.10. ORGANIZACI ´ON DE LA TESIS 13 poder planificar vistas robustas a errores de posicionamiento. El capitulo 7, Expe-rimentos, describe los experimentos realizados en simulaci´on y con un robot real. Finalmente, el cap´ıtulo 8, Conclusiones y Trabajo Futuro, cierra la tesis mencio-nando las conclusiones de la investigaci´on y los caminos a seguir en el trabajo futuro.
Cap´ıtulo 2
Fundamento Te´
orico y
Establecimiento del Problema
En este cap´ıtulo se definen formalmente los conceptos de la reconstrucci´on tridimensional de objetos autom´atica y se establece el problema de la planificaci´on del siguiente mejor estado/vista (SMEV). La notaci´on utilizada en su mayor´ıa est´a de acuerdo al libro de La Valle, “Planning Algorithms” [LaValle, 2006], y se complementa con la notaci´on utilizada en [Torabi et Gupta, 2012].
La primer secci´on, “Escena de reconstrucci´on”, define los elementos que in-tervienen en la reconstrucci´on. Por ejemplo el objeto, el espacio de trabajo, el robot, etc. Las siguientes secciones describen en detalle los primeros tres pasos del proceso de reconstrucci´on tridimensional de objetos. Ver figura 2.1. El cap´ıtulo finaliza con el establecimiento del problema de planificaci´on del siguiente mejor estado vista (SMEV).
Figura 2.1: El proceso de reconstrucci´on de objetos se descompone en cuatro pasos: posicionamiento del robot, percepci´on, actualizaci´on del modelo y planificaci´on de la siguiente vista. El proceso se repite hasta que se alcanza un criterio de paro. A la ejecuci´on de los cuatro pasos se denomina como una iteraci´on de la reconstrucci´on.
2.1.
Escena de Reconstrucci´
on
La escena de reconstrucci´on consiste del espacio de trabajo, el objeto, el sensor de rango y el robot. En los siguientes p´arrafos se define formalmente cada uno de los elementos de la escena.
2.1.1.
El Objeto y el Espacio de Trabajo
El espacio de trabajo, o espacio f´ısico, es un espacio euclidiano de tres dimen-siones (3D) que se denota conW [LaValle, 2006],W =R3. El objeto es la entidad f´ısica de la cual se desea generar una representaci´on tridimensional. Formalmente, el objeto es un conjunto cerrado de puntos en el espacio de trabajo y se denota como Wobj.
Suponemos que es posible generar un cubo delimitando el espacio donde se encuentra el objeto. A dicho cubo se le llama cubo encapsulador y se denota como
Wbox ⊂ W. Suponemos que las dimensiones del cubo son conocidas. Siempre se
cumple que Wobj ⊆ Wbox
La regi´on desconocida es la parte del espacio de trabajo que puede esconder superficies del objeto de inter´es y se denota comoWunk. Al iniciar la reconstrucci´on
se cumple que Wunk=Wbox.
Suponemos que el espacio de trabajo es conocido, a excepci´on de la regi´on desconocida. El espacio de trabajo puede contener otros objetos, que no son de inter´es para la reconstrucci´on, pero el robot puede colisionar con ellos. Definimos ´estos otros objetos como obst´aculos del ambiente y se representan comoOenv. La
uni´on de los obst´aculos del ambiente, la regi´on desconocida y el objeto se llama regi´on de obst´aculos y se denota como O, es decir, O = Oenv∪Wobj ∪ Wunk. El
2.1. ESCENA DE RECONSTRUCCI ´ON 17
2.1.2.
El Sensor y el Espacio de Vistas
El sensor de rango es el dispositivo que permite adquirir la informaci´on geom´etri-ca en tres dimensiones de la superficie del objeto. En esta tesis, representamos un sensor de rango, D, con una tupla de tres elementos:
D={D, v, R} (2.1)
donde cada elemento se define a continuaci´on.
El espacio que ocupa el sensor es un conjunto cerrado de puntos que se denota
porD ⊂ W. La configuraci´on extr´ınseca del sensor se llama vista y se denota con
v, dicha vista est´a compuesta por seis par´ametros, v = [x, y, z, φ, θ, γ]T, donde x,
yyz indican la posici´on yφ,θ yγindican la orientaci´on del rayo director. φindica la rotaci´on sobre el eje x, θ indica la rotaci´on sobre el ejey y γ indica la rotaci´on sobre el eje z. Todos los elementos de una vista se definen con respecto del marco de referencia global. Al conjunto de todas las posibles vistas de un sensor se le llama espacio de vistas y se denota como V. La figura 2.2(a) ilustra la vista de un sensor.
Suponemos que el sensor tiene una geometr´ıa en perspectiva, donde hay un solo origen para todos los rayos que pasan a trav´es del plano de la imagen. La figura 2.2 (b) ilustra la geometr´ıa del sensor. El campo de visi´on del sensor (FOV por sus siglas en ingl´es de field of view) est´a determinado por un una pir´amide descrita por un conjunto de rayos,
R={ri|0≤ i < n;ri = [xi, yi, zi]T} (2.2)
donde xi,yi y zi son coordenadas en el espacio de trabajo.
Al intervalo entre la distancia m´ınima y m´axima que el sensor puede percibir se le denomina campo de profundidad. El cono de visibilidad (frustum) del sensor es la intersecci´on del campo de visi´on y el campo de profundidad y se denota como
X
z
y
α β
γ
(x,y,z) c
(a) Vista del sensor (b) Geometr´ıa del sensor
F(x)
(c) Cono de visibilidad (Frustum) del sensor
Figura 2.2: La figura a) muestra los par´ametros extr´ınsecos del sensor que de-terminan su posici´on y orientaci´on. Una vista es una combinaci´on particular de dichos par´ametros extr´ınsecos. La figura b) muestra la geometr´ıa en perspecti-va del sensor donde hay un solo origen para todos los rayos que pasan a trav´es del plano de la imagen. La figura c) ilustra en dos dimensiones el Frustum del sensor, F, determinado por la combinaci´on del campo de vista y del campo de profundidad. Las superficies del objeto ´unicamente son visibles si est´an dentro de
2.1. ESCENA DE RECONSTRUCCI ´ON 19
(a) Configuraci´on de un robot m´ovil (b) Configuraci´on del brazo rob´otico
Kata-na
Figura 2.3: La figura a) muestra los par´ametros de la configuraci´on de un robot m´ovil, q = (x, y, θ). La figura b) muestra los par´ametros de la configuraci´on del brazo manipulador Katana, q= (θ1, θ2, θ3, θ4, θ5).
F(v). La figura 2.2 (c) ilustra en dos dimensiones el campo de profundidad.
2.1.3.
El Robot, Espacio de Configuraciones y Espacio de
Estados
El robot es el dispositivo encargado de colocar el sensor de rango en la posici´on deseada. El robot est´a determinado por un conjunto cerrado de puntos en el espacio de trabajo y se denota como A, de tal forma que A ⊂ R3. Si el robot
no es articulado entonces A es un ´unico cuerpo r´ıgido. Si el robot es articulado, entonces el robot es una colecci´on de m cuerpos r´ıgidos, A={A1, A2...Am}.
Una configuraci´on,q, del robot es una especificaci´on del estado f´ısico del robot con respecto de un marco de referenciaFw fijo en el ambiente [Dudek et Jenkin, 2000].
En esta tesis se escribe q= [a1, a2. . . an]T, donde n es el n´umero de grados de
li-bertad. La figura 2.3 ilustra las configuraciones de un robot m´ovil y del brazo rob´otico Katana. De acuerdo con [LaValle, 2006], para un robot de n grados de
libertad, el conjunto de configuraciones posibles describe una variedad (traduci-do del ingl´es manifold) de dimensi´on n. A dicha variedad se le llama espacio de configuraciones y se denota como C.
El espacio de configuraciones en colisi´on, Cobs, se define como Cobs = {q ∈
C|A(q)∩ O 6= ∅}. El conjunto de configuraciones que no est´a en colisi´on se le
llama espacio de configuraciones libre y se define como Cf ree =C \ Cobs.
Un estado, x, del robot se define como x = (q,q˙), donde ˙q es la primera derivada de q. El estado puede incluir mayores derivadas, sin embargo esos casos no son tratados en la tesis. El espacio de estados, X, es el conjunto de todos los posibles estados del robot.
Cinem´atica Directa
El conjunto de puntos en el espacio de trabajo pertenecientes al robot para una determinada configuraci´on, q, se denota comoA(q).A(q) se obtiene transforman-do (traslaci´on y rotaci´on) cada cuerpo r´ıgitransforman-do del robot de acuertransforman-do a la cinem´atica directa del robot. Suponiendo que el robot est´a compuesto por una cadena ci-nem´atica de m cuerpos r´ıgidos, A = {A1,A2...Am}. La posici´on en W de cada
punto adeli-avo cuerpo r´ıgido est´a dada por la multiplicaci´on de las coordenadas del punto por la matriz de transformaci´on homog´enea que lo transforma al marco de referencia global [Craig, 2005].
Ai =T0T1. . . Ti−1
ai,x ai,y ai,z 1 (2.3)
donde Ti es una matriz de transformaci´on homog´enea que describe la relaci´on
entre el cuerpo r´ıgido Ai con respecto de Ai−1. En caso de A1, T0 se define con respecto del marco de referencia global. Una forma de obtener la matriz T es
2.2. POSICIONAMIENTO 21 mediante la definici´on de Denavit-Hartenberg que incorpora como par´ametro la configuraci´on del robot. Ver [Craig, 2005] para m´as detalle.
Sensor Montado en el Robot
Una de las suposiciones de esta tesis, es que el sensor est´a montado sobre el efector final del robot manipulador. As´ı que, suponemos que el ´ultimo de los cuerpos r´ıgidos es el sensor, es decir Am =D.
Proyecci´on de C a V
DefinimosCD como una funci´on de cinem´atica directa que proyecta una con-figuraci´on del robot en una vista del sensor, es decir CD :C → V. CD se obtiene directamente al multiplicar la posici´on y orientaci´on del sensor por la matriz de transformaci´on homog´enea T =T0T1. . . Ti−1 [Craig, 2005].
2.1.4.
Definici´
on de Estado/Vista
Consideramos un estado/vista, como un estado tal que el sensor montado en el efector final observa al objeto. Adem´as, suponemos que el sensor de rango es capaz de tomar una imagen de forma instant´anea, sin que haya efectos de difuminado causados por la velocidad. Esto nos permite evaluar el estado en el instante en que se tomar´a la imagen de rango.
2.2.
Posicionamiento
El posicionamiento consiste en colocar el robot en un estado/vista deseado. Para ello, el robot se debe desplazar desde su estado actual hasta el estado deseado. A continuaci´on se definen conceptos relacionados al posicionamiento.
2.2.1.
Control y Transici´
on de Estados
Suponemos que el robot es controlado por un conjunto de velocidades o ace-leraciones que se aplica a cada grado de libertad (DOF por sus siglas en ingl´es de degree of freedom). Al conjunto de velocidades o aceleraciones aplicadas se les llama control y se denota como u ∈ U. En caso de que el conjunto de controles sea finito, los controles se denominan primitivas de control.
Una funci´on de entrada (control) del robot se define como u: [0, T]→U. La variaci´on con respecto del tiempo del estado del robot est´a gobernada por la ecuaci´on de transici´on de estados [LaValle, 2006]:
˙
x=f(x, u) (2.4)
2.2.2.
Caminos y Trayectorias
Una trayectoria es un camino continuo parametrizado en el tiempoτ : [0, T]→ X que satisface las restricciones no holon´omicas [LaValle et Kuffner, 2001]. Par-tiendo de que el estado del robot cambia dependiendo del control de entrada,
u, ecuaci´on 2.4, la trayectoria, x(t), para t ∈ [0, T] se determina a trav´es de la integraci´on de 2.4.
τ(t) =x0+
Z t
0
f(x(t), u(t))dt (2.5)
Un camino es una funci´on continua, ρ : [0,1] → C. Cada punto del camino est´a dado por ρ(s) para alg´uns∈[0,1]. Si existe una serie de estados particulares que se son visitados en el camino, ´estos se definen como q1, q2, . . .. Note que un
camino se define ´unicamente en C sin necesariamente satisfacer las restricciones no holon´omicas.
2.3. PERCEPCI ´ON 23
2.3.
Percepci´
on
La percepci´on consiste en observar, con un sensor de rango, el objeto y adqui-rir una nube de puntos que representa la superficie del objeto. El resultado de la percepci´on es una imagen de 212 dimensiones, es decir, un conjunto de puntos 3D con respecto del marco de referencia del sensor. El conjunto de puntos transfor-mado al marco de referencia global se le llama nube de puntos y se denota con
ι.
ι={pi|pi = [xi, yi, zi]T} (2.6)
donde x,y y z son coordenadas en R3
La nube de puntos adquirida en la i-´esima iteraci´on se denota como ιi. En
relaci´on con el estado ,x , en que se hace una percepci´on, la nube de puntos se denota como ιx.
La nube de puntos acumulada despu´es dek percepciones se denota comoP =
S
i=1:kιi.
Suponemos que los puntos que pertenecen al cubo encapsulador pertenecen al objeto. La nube de puntos acumulada que pertenece al objeto es un subconjunto de P se denota como Pobj.
2.3.1.
Oclusiones
Las oclusiones, o ´areas de oclusi´on, son generadas cuando una superficie no es vista debido a las limitaciones de la t´ecnica de percepci´on utilizada o por la posici´on y orientaci´on del sensor. Por ejemplo, un sensor de rango activo genera oclusiones en las ´areas en las cuales el rayo emitido no pudo hacer contacto con la superficie. En la figura 2.4 se muestra las oclusiones generadas por un sensor de rango. Los planos de oclusi´on se forman en la uni´on de la superficie de oclusi´on con el espacio vac´ıo.
Figura 2.4: Las oclusiones o ´areas de oclusi´on est´an determinadas por las ´areas que que no son observadas por un sensor de rango.
2.4.
Actualizaci´
on del Modelo
Una vez que se ha realizado una percepci´on es necesario actualizar la represen-taci´on del objeto con la nueva informaci´on. La actualizaci´on consiste de los pasos de registro, fusi´on y actualizaci´on de otras posibles representaciones del objeto. El registro transforma (rotaci´on y traslaci´on) la reciente adquirida imagen de rango para que coincida con la nube de puntos acumulada del objeto. El registro es necesario por que el sistema de posicionamiento es imperfecto. La fusi´on consiste en eliminar las partes redundantes del objeto y crear un solo modelo unificado. La actualizaci´on de otras representaciones del objeto pude existir o no. Si solo se utiliza una nube de puntos como representaci´on del objeto, entonces la fusi´on es suficiente. Sin embargo, si existen otras representaciones tales como mapas de v´oxeles o mallas triangulares, ´estas se actualizan con la imagen de rango adquirida y registrada. Debido a que el registro es un proceso cr´ıtico en la reconstrucci´on, ´este se detalla en los siguientes p´arrafos.
2.4.1.
Registro
El registro es un proceso importante dentro de la reconstrucci´on. En este paso la nube de puntos m´as reciente del objeto se transforma al marco de referencia
2.4. ACTUALIZACI ´ON DEL MODELO 25 global, de tal forma que los puntos comunes con el modelo acumulado coincidan. Utilizando el od´ometro del robot es posible aproximar la pose real del sensor, y por tanto alinear las nubes de puntos de forma burda, sin embargo esta ali-neaci´on no es suficiente. Por lo tanto, se desea obtener la alineaci´on m´as precisa posible a partir de una alineaci´on burda, a este problema se le llama registro fino [Salvi et al, 2007].
La finalidad del registro no es ´unicamente acomodar las superficies de forma correcta, tambi´en se puede usar para re-localizar al robot. Un registro exitoso provee una posici´on m´as certera del sensor, y por tanto del robot. Al utilizar el registro como re-localizaci´on del robot, se reduce el error acumulado por el deslizamiento de las llantas. A continuaci´on se describe un m´etodo de registro fino ampliamente usado, el algoritmo Iterative Closest Point.
2.4.2.
Algoritmo Iterative Closest Point
El Algoritmo Iterative Closest Point (ICP) fue propuesto por Besl y Mckay [Besl et McKay, 1992], las entradas de ICP son: i) los puntos de la superficie de origen (la imagen de rango adquirida), ii) los puntos de la superficie objetivo (la nube de puntos acumulada del objeto) y iii) una transformaci´on 3D inicial que alinea la superficie origen de forma burda con la superficie objetivo (provista por el od´ometro del robot). ICP iterativamente refina la transformaci´on 3D a trav´es de empatar puntos de una superficie con la otra, y minimizar una m´etrica de error. Esto se repite hasta que una condici´on se cumple, por ejemplo cuando el cambio en la transformaci´on es muy peque˜no.
Usualmente se usan dos m´etricas. Una se conoce como la m´etrica “punto a punto” y la otra se conoce como m´etrica “punto a plano”. Seas= [si,x, si,y, si,z,1]T
la forma homog´enea de un punto en la superficie fuente, t = [ti,x, ti,y, ti,z,1]T la
forma homog´enea de un punto en la superficie objetivo, n = [ni,x, ni,y, ni,z,0]T
entonces la m´etrica punto a punto es:
Epunto−a−punto = X
i
(M ·si−ti)2 (2.7)
y la m´etrica punto a plano es:
Epunto−a−plano= X
i
((M ·si−ti)·ni)2 (2.8)
No siempre se puede lograr un registro exitoso de las superficies. De acuer-do con [Low, 2006], algunas de las causas que pueden impedir el registro de las superficies son las siguientes:
Insuficiente traslape entre superficies.
Insuficientes caracter´ısticas de forma en las transformaciones de cuerpos r´ıgidos tridimensionales. Por ejemplo, un plano, que no tiene puntos carac-ter´ısticos (curvas, picos, etc), puede alinearse con otro plano de m´ultiples formas, ya que uno rota sobre el otro sin que se aumente o disminuya la m´etrica de error.
Errores de medici´on demasiado grandes.
Error entre las poses iniciales demasiado grande.
2.5.
Problema del Siguiente Mejor Estado/Vista
El problema abordado en esta tesis es determinar un estado del robot, x∗ , llamado siguiente mejor estado/vista, que maximice la cantidad de superficie re-construida, minimice la distancia de navegaci´on con respecto del estado actual del robot, xc, y satisfaga las siguientes restricciones:
1. Restricciones de Posicionamiento.x∗
2.5. PROBLEMA DEL SIGUIENTE MEJOR ESTADO/VISTA 27
x∗
∈ Xf ree. Al menos una trayectoria libre de colisi´on, τ, que conecta xc y
x∗
es conocida.
2. Restricci´on de Sensado. Las superficies a observar, contenidas en el espa-cio desconocido, Wunk, ´unicamente ser´an visibles si est´an dentro del campo
de vista y campo de profundidad del sensor definido por F(x∗ ). 3. Restricci´on de Registro. La nube de puntos adquirida en x∗
debe ser registrada con la nube acumulada. Por lo tanto, al menos un m´ınimo traslape entre vistas es necesario. Es decir, ιx∗ ∩Pobj 6=∅, note que al momento que
x∗
Cap´ıtulo 3
Trabajo Relacionado
La planificaci´on de vistas abarca diversos problemas, todos ellos requieren determinar una serie de configuraciones del sensor a fin de cubrir una espacio determinado. Cada problema se forma a partir de la combinaci´on de tres variables: el objetivo, la informaci´on acerca del objetivo y el sistema de posicionamiento. Los objetivos pueden ser objetos o ambientes; la informaci´on del objetivo puede ser nula, parcial o completa; y el sistema de posicionamiento puede ser fijo o m´ovil. En particular, esta tesis aborda el problema de encontrar una serie de vistas de un robot manipulador m´ovil que cubran la superficie de un objeto desconocido. En este cap´ıtulo se analizan los trabajos m´as cercanos del ´area de planificaci´on de vistas. El resto del cap´ıtulo se organiza de la siguiente forma: Primero se describe una posible clasificaci´on de los problemas de planificaci´on de vistas. Despu´es, se analizan los trabajos de reconstrucci´on de objetos. En seguida se analizan algunos trabajos acerca de la exploraci´on de ambientes. Despu´es se analizan los trabajos de planificaci´on de movimientos con incertidumbre. Finalmente se hace un resumen y an´alisis del trabajo relacionado.
3.1.
Clasificaci´
on de la Planificaci´
on de Vistas
La figura 3.1 muestra una posible clasificaci´on de la planificaci´on de vistas. La primera divisi´on se basa en la informaci´on que se conoce del objetivo. Cuando se conoce de forma parcial o completa el modelo se realiza una planificaci´on basada en el modelo. El problema en esta planificaci´on es similar al problema de la galer´ıa de arte, el cual se define como: dado un conjunto de paredes encontrar la m´ıni-ma cantidad de guardias que puedan vigilar todas las paredes de la galer´ıa. Este tipo de planificaci´on determina todas las vistas necesarias en una sola iteraci´on. El lector puede consultar m´as informaci´on acerca de los trabajos existentes en [O’Rourke, 1987, Scott et al, 2003].
Cuando no se cuenta con informaci´on del objetivo se realiza una planificaci´on no basada en el modelo. En el caso de reconstrucci´on de objetos, la informaci´on disponible no va m´as all´a de la localizaci´on y posibles dimensiones del objeto. En este caso no se puede hacer una planificaci´on de todas las vistas necesarias en una sola iteraci´on, y utilizar un conjunto de vistas predeterminado puede ser insuficiente debido a la variaci´on de tama˜no y forma de los objetivos. Por lo tanto, la planificaci´on consiste en: determinar de forma iterativa las vistas que observen la superficie del objetivo, donde en cada iteraci´on se determina una configuraci´on del sensor llamada siguiente mejor vista (SMV).
Figura 3.1: Clasificaci´on de los algoritmos de planificaci´on de vistas.
3.2. RECONSTRUCCI ´ON DE OBJETOS 31 que utilizan sistemas de posicionamiento fijos (SPF) y los que usan sistemas de posicionamiento m´ovil. Los trabajos con SPF, que son la mayor´ıa, hacen uso de escenarios especializados con m´aquinas de posicionamiento muy precisas, por ejemplo coordinate measuring machines (CMMs) o brazos rob´oticos empotrados en una base fija. Debido a las suposiciones hechas en los SPF, los algoritmos de ´ese ´ambito no so directamente aplicables a los sistemas m´oviles, donde se hace uso de robots de tipo diferencial o tipo carro.
3.2.
Reconstrucci´
on de Objetos
Durante m´as de dos d´ecadas se ha abordado el problema de planificaci´on de vistas para reconstrucci´on 3D de objetos. La mayor´ıa de las investigaciones reali-zadas se han enfocado en resolver el problema utilizando ambientes de reconstruc-ci´on controlados con sistemas de posicionamiento fijo. Recientemente han surgido investigaciones enfocadas a sistemas de posicionamiento m´ovil donde se tienen que satisfacer un mayor n´umero de restricciones. A continuaci´on se analizan los principales trabajos de cada tipo.
3.2.1.
Sistemas de Posicionamiento Fijo
En estos trabajos la siguiente mejor vista (SMV) solo busca incrementar la superficie reconstruida. En la mayor´ıa de los casos no se necesita verificar las restricciones de posicionamiento, dado que el ambiente es controlado, ni las res-tricciones de registro, dado que el error de posicionamiento casi es nulo.
Entre los primeros trabajos est´a el de Conolly [Connolly, 1985], un algoritmo de b´usqueda, donde se representa el objeto con una rejilla de ocupaci´onoctree y se determina la siguiente vista con uno de dos algoritmos. El primero de ellos, llamado
planetarium, genera un conjunto de vistas candidatas alrededor del objeto. A este conjunto tambi´en se le conoce como esfera de vistas. Las vistas candidatas son
calculadas con una divisi´on uniforme de los paralelos y meridianos de una esfera alrededor del objeto. Se toman im´agenes de rango simuladas del modelo parcial y se determina la siguiente mejor vista como aquella desde donde se “ve” la mayor cantidad de ´area desconocida. Desafortunadamente este algoritmoplanetarium es demasiado lento por lo que el autor propuso el segundo algoritmo llamadonormal. El algoritmo normal toma en cuenta la cara de cada v´oxel que tiene adyacencia con ´areas no vistas y con el espacio vac´ıo. Se hace una suma vectorial pesada de las direcciones de cada cara y se obtiene la direcci´on de la vista. El algoritmo es r´apido, sin embargo no contempla las auto-oclusiones del objeto.
A partir del trabajo de [Connolly, 1985] siguieron varios trabajos que proponen variaciones y mejoras, pero conservando el paradigma de generar vistas alrededor del objeto para luego evaluarlas mediante una funci´on de utilidad [Banta et al, 1995,
Banta et al, 2000, Garc´ıa et Vel´azquez, 1998, Massios et Fisher, 1998, Morooka et al, 1999, Wong et al, 1999, Vasquez et al, 2009].
Una mejora notable fue la de Massios [Massios et Fisher, 1998], que propone agregar un criterio de calidad. Este criterio busca que las superficies sean esca-neadas de forma perpendicular. Sin embargo, el criterio entra en conflicto con el criterio de observar la mayor ´area desconocida, debido a que cuando busca ob-servar superficies de forma perpendicular se deja de obob-servar nuevas superficies. En recientes trabajos se ha complementado la calidad agregando la densidad de puntos por unidad de superficie [Low et Lastra, 2007].
En general los algoritmos hasta ahora mencionados utilizan el paradigma de b´usqueda, es decir, generan un conjunto de vistas candidatas y las eval´uan. La mayor´ıa de dichos algoritmos resuelvan las auto oclusiones del objeto. Sin embar-go, tienen la desventaja de ser lentos. Por lo que, a la par surgieron algoritmos basados en la superficie o tambi´en llamados de s´ıntesis.
Un algoritmo de s´ıntesis pionero en el ´area es el propuesto en [Maver et Bajcsy, 1993], donde se utiliz´o un sensor de triangulaci´on y el posicionamiento se redujo a un grado de libertad (giro del objeto). Se consideraron por separado las oclusiones
3.2. RECONSTRUCCI ´ON DE OBJETOS 33 ocasionadas por el sensor y las ocasionadas por objeto. La SMV se determina en dos etapas. En la primer etapa se determinan, mediante un an´alisis de visibili-dad, las partes ocluidas y los “arcos libres de oclusi´on”. En la segunda etapa se forma un histograma con los arcos de visibilidad, para finalmente determinar la SMV como el m´aximo valor del histograma. El m´etodo se limita al an´alisis de cada imagen de rango (212 dimensiones) y no del modelo 3D completo. Un m´eto-do similar que agrega una etapa de rellenam´eto-do de las superficies fue propuesto en [Garc´ıa et Vel´azquez, 1998].
Han surgido tambi´en trabajos que combinan la s´ıntesis y la b´usqueda. Por ejemplo, Banta y otros en [Banta et al, 1995, Banta et Abidi, 1996, Banta et al, 2000] proponen un m´etodo que selecciona entre determinar una vista con el m´eto-do de Maver [Maver et Bajcsy, 1993] o determinarla con el m´etom´eto-do de Connoly [Connolly, 1985]. Cuando ninguna de las formas anteriores funciona se utiliza una vista aleatoria hasta que se complete el modelo.
Trabajos recientes se han enfocado en reconstrucci´on de objetos utilizando c´amaras monoculares, dado su bajo costo y f´acil implementaci´on en los robots m´oviles. Estas c´amaras estiman la posici´on y forma de los objetos mediante se-guimiento de caracter´ısticas y optimizaci´on de par´ametros. En la serie de traba-jos del grupo de visi´on de la Universidad de Jena y la Universidad de Erlangen, Alemania [Wenhardt et al, 2006, Wenhardt et al, 2007, Trummer et al, 2010], se propone el uso de modelos lineales estad´ısticos, en especial el E-Criterion. Con este modelado calculan la SMV como aquella vista que reduce la incertidumbre en la posici´on de los puntos de la superficie observada. Sin embargo, esta t´ecni-ca de planifit´ecni-caci´on t´ecni-carece de soluci´on para las auto oclusiones de los objetos, las restricciones de posici´on o las restricciones de sensado.
Pocos trabajos determinan la configuraci´on del robot y el camino para al-canzarla. Torabi y Gupta en [Torabi et Gupta, 2012] proponen generar puntos cercanos al borde de la nube de puntos del objeto, llamados target points. A partir de esos target points y las restricciones del campo de visi´on del sensor
se generan vistas candidatas, las cuales son evaluadas calculando su visibilidad. Una vez determinada la vista, se obtiene la configuraci´on que empata con la vista del sensor mediante cinem´atica inversa. El camino que sigue el robot pa-ra alcanzar la tpa-rayectoria es determinado por un Probabilistic Road Map (PRM) [Kavraki et al, 1996]. Kriegel y otros en [Kriegel et al, 2012] combinan dos enfo-ques para determinar la SMV, un m´etodo basado en superficie y otro volum´etrico. Primero, se genera un conjunto de caminos candidatos sobre los bordes de la ma-lla triangular del objeto. Despu´es, se utiliza un Rapidly-Exploring Random Tree
(RRT) [LaValle et Kuffner, 2001] para generar los caminos que observan los bor-des de la malla triangular. En una segunda etapa eval´ua la ganancia de informaci´on de cada camino en un octree probabilista. La SMV es la vista que maximiza la ganancia de informaci´on.
La tabla 3.1 muestra un resumen de las caracter´ısticas de los algoritmos re-visados en esta secci´on. Las caracter´ısticas son: i) planificaci´on en el espacio de vistas (seis grados de libertad), ii) planificaci´on en el espacio de estados del robot, iii) generaci´on de la trayectoria para alcanzar el siguiente estado o vista, iv) cum-plimiento del criterio de calidad v) satisfacci´on de la restricci´on de registro, vi) sensado, vii) posicionamiento ix) planificaci´on robusta a la incertidumbre en el po-sicionamiento x) reducci´on la distancia de navegaci´on y xi) planificaci´on robusta a la incertidumbre en la percepci´on.
En resumen, ya sea utilizando m´etodos de s´ıntesis, b´usqueda o mixtos se puede llegar, en la mayor´ıa de los casos, a una reconstrucci´on casi total del objeto, siem-pre y cuando, la escena est´e controlada y se utilice un sistema de posicionamiento fijo. Sin embargo, no es posible implementar estos algoritmos de forma directa en sistemas de posicionamiento m´ovil, debido a las fuertes suposiciones que se hacen.
