Planificador de trayectorias para un robot movilorientado a la reconstruccion en 3D de entornos

desconocidos

Mauricio Fernando Jaramillo Morales

Universidad Nacional de Colombia

Sede Manizales

Facultad de Ingenierıa y Arquitectura

Departamento de Electrica, Electronica y Computacion

Manizales, Colombia

2010

Planificador de trayectorias para un robot movilorientado a la reconstruccion en 3D de entornos

desconocidos

por

Mauricio Fernando Jaramillo Morales

Tesis

Presentada a la Facultad de Ingenierıa y Arquitectura de la

Universidad Nacional de Colombia

Sede Manizales

En cumplimiento

de los Requerimientos

para el Grado de

Magister en Ingenierıa

Universidad Nacional de Colombia

Sede Manizales

Mayo 2010

Indice general

1. Introduccion 5

2. Marco teorico 82.1. Trabajos previos . . . . . . . . . . . . . . . . . . . . . . . . . 8

2.1.1. Etapas en la planificacion de trayectorias . . . . . . . . 12

3. Planificador de trayectorias 163.1. Clasificacion del espacio: Nodos vistos, no vistos y ocupados . 17

3.1.1. Clasificacion de nodos sin obstaculo presente en el cam-po visual de la camara . . . . . . . . . . . . . . . . . . 18

3.1.2. Clasificacion de nodos con obstaculo presente en elcampo visual de la camara. . . . . . . . . . . . . . . . 19

3.1.3. Clasificacion de los nodos ocupados . . . . . . . . . . . 223.1.4. Resultados . . . . . . . . . . . . . . . . . . . . . . . . . 24

3.2. Trayectoria deseada . . . . . . . . . . . . . . . . . . . . . . . . 263.2.1. Adaptacion de la funcion gaussiana en la obtencion de

la trayectoria deseada . . . . . . . . . . . . . . . . . . . 283.2.2. Tratamiento del angulo inicial del movil . . . . . . . . 303.2.3. Replanteamiento de la trayectoria adecuada cuando el

obstaculo esta muy cerca del robot movil. . . . . . . . 343.3. Mejor vista siguiente . . . . . . . . . . . . . . . . . . . . . . . 373.4. Validacion de resultados . . . . . . . . . . . . . . . . . . . . . 42

4. Controlador 494.1. Controlador modelo cinematico . . . . . . . . . . . . . . . . . 49

4.1.1. Linealizacion por retroalimentacion dinamica . . . . . . 504.1.2. Linealizacion por retroalimentacion estatica . . . . . . 52

4.2. Controlador modelo dinamico . . . . . . . . . . . . . . . . . . 53

3

4.3. Filtracion de las velocidades y voltajes obtenidos de las ruedasdel robot movil . . . . . . . . . . . . . . . . . . . . . . . . . . 54

4.4. Resultados . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 594.5. Validacion de resultados . . . . . . . . . . . . . . . . . . . . . 61

5. Voxelizacion 705.1. Voxelizacion basada en el algoritmo octree . . . . . . . . . . . 705.2. Puntos vistos por las camaras del robot movil y voxelizacion . 745.3. Resultados . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 775.4. Validacion de resultados. . . . . . . . . . . . . . . . . . . . . . 78

6. Resultados 826.1. Validacion de resultados . . . . . . . . . . . . . . . . . . . . . 86

7. Conclusiones y trabajos futuros 93

4

Capıtulo 1

Introduccion

La planificacion de trayectorias de robots moviles orientada a la reconstru-ccion de ambientes desconocidos, ha sido un reto importante en la robotica.

En la actualidad, se tiene una gran cantidad de investigadores centradosen desarrollar tecnicas de planificacion de trayectorias, reconstruccion en 3Ddel entorno desconocido y sistemas de control que le permitan al movil seguirlas trayectorias obtenidas [5],[6],[8],[9],[15],[23]. Pero aun quedan campos porexplorar, entre ellos la eficiencia de los desplazamientos del movil, ya queen muchas ocasiones son discontinuos e intermitentes, lo que repercute en unmayor gasto de energıa. Los trabajos realizados en la Universidad de Florida,por el grupo de investigacion, CRASAR (Center of Robotic Assisted Searchand Rescue) son una muestra clara del interes en el tema, y del alto numerode aplicaciones que se pueden obtener.

Un ejemplo de las aplicaciones con robots en busqueda y rescate urbano,se presento en el desastre del World Trade Center en 9/11/2001 y fue rea-lizado por el CRASAR. Los robots fueron usados para: busqueda de vıctimas,busqueda de caminos (de los cuales se pueda excavar rapidamente, a traves delos escombros), inspeccion estructural y deteccion de materiales peligrosos.En cada caso, los robots fueron usados porque ellos pueden ir con mayorprofundidad, rutinariamente viajan de 5 a 20 metros en el interior de losescombros, en cambio una camara montada en una polea solo llega a 2 metrosde profundidad. Ademas pueden entrar a espacios pequenos para un humanoo para un canino de rescate, o pueden entrar en un sitio en fuego o en granriesgo de un colapso estructural [7]. La reconstruccion del entorno puede

5

ser llevada a cabo mediante el uso de camaras de video y reconstruccionestereo, las camaras de video pueden ser camaras web, lo que influye deuna manera importante en una implementacion asequible a un presupuestorelativamente bajo y a una facil consecucion, con lo cual se pueda obtenerresultados esperados segun la finalidad del robot. Ademas, existen trabajosrealizados por el grupo de investigacion PCI, en uno de ellos, se describe unsistema estereo activo para la adquisicion de imagenes de rango de bajo costoimplementado sobre MATLAB R©. El sistema combina metodos de estimacionde parametros para dos vistas (tanto calibradas como no calibradas) [1], loque constituye una buena base en el diseno del sistema estereo para la fasede adquisicion del entorno desconocido.

El proceso de planificacion de trayectorias de robots moviles orientadosa la reconstruccion eficiente del entorno a partir de tecnicas de vision enestereo, consta fundamentalmente de las siguientes etapas:

Adquisicion de los puntos en 3D de la primera escena a reconstruir pormedio de vision estereo.

Reconstruccion de la escena usando los puntos en 3D hallados en elpaso anterior.

Calcular la posicion y orientacion que debe seguir el robot, para ga-nar la mayor informacion del entorno, a partir de la primera escenareconstruida.

Generar una trayectoria entre la posicion actual y la posicion deseadapara el robot.

Calcular el modelo cinematico del robot movil, con el cual se puedaimplementar un controlador, de tal manera que el robot se desplacepor la trayectoria deseada.

Realizar iterativamente estos pasos, y de esta forma lograr una re-construccion incremental del entorno que se desea explorar.

En realidad no existe una teorıa definida acerca de este tema, la metodologıapresentada anteriormente son los pasos que en comun siguen los trabajosprevios, pero con procedimientos diferentes. Por ejemplo en cuanto a la

6

adquisicion de las imagenes de la escena se utilizan sensores de rango [2],[6],imagenes de rango [5],[34],[35],[36], laser de rango [11],[15],[33],[40],[43], sen-sores sonares [7],[23], vision monocular [24], vision estereo [31], etc. En lareconstruccion de la escena se utiliza en su mayorıa voxeles (cubos) paraclasificar el espacio [2],[5],[6],[15],[16],[25],[32]. En cuanto a la posicion y orien-tacion deseada, se distingue una tendencia de determinar las oclusiones de lasimagenes y de esta forma encontrar la posicion que entrega la mayor cantidadde informacion desconocida.[44].

El problema en resumen es disenar un planificador de trayectorias paraun robot movil, el cual utilice como fuente de informacion la reconstruccionincremental de un entorno desconocido de manera autonoma.

El contenido del documento es como sigue: en el Capıtulo 2, se presen-tan trabajos previos y definiciones basicas, propias de la tematica de estetrabajo. En el Capıtulo 3, se aborda la metodologıa propuesta para la obten-cion del planificador de trayectorias, resaltando los algoritmos de clasificaciondel espacio, mejor vista siguiente y trayectorias deseadas. En el Capıtulo 4,se describe el controlador utilizado en el movil, y ademas se detalla la re-construccion en 3D del entorno virtual por medio de voxelizacion. En elCapıtulo 5, se muestran los resultados obtenidos en la reconstruccion de am-bientes virtuales desconocidos. finalmente en el Capıtulo 6 se presentan lasconclusiones y los trabajos futuros.

7

Capıtulo 2

Marco teorico

A continuacion se presentaran trabajos previos del tema y se definiran al-gunos terminos necesarios para una mejor comprension del metodo propuestode planificacion de trayectorias.

El capıtulo se encuentra organizado de la siguiente manera: en la Seccion2.1, se muestran los metodos mas utilizados en el estado del arte, enfocadosen la obtencion del planificador de trayectorias para un robot movil, y en laSeccion 2.2 se profundizara en las etapas que componen el planificador detrayectorias orientado a la reconstruccion de entornos desconocidos.

2.1. Trabajos previos

En el planificador de trayectorias de un robot movil orientado a la re-construccion de ambientes desconocidos, el objetivo principal es generar caminosque obtengan la mayor cantidad de informacion posible en cada desplaza-miento. En los siguientes artıculos se muestran diferentes metodologıas tantoen la reconstruccion de escenas u objetos en 3D, como en la determinacion dela posicion siguiente (mejor vista siguiente), las cuales son etapas importantesen el diseno del planificador deseado.

Nikolaos et al.[2], al igual que en la tesis de maestrıa de Laurana May-Ping [3], muestran los objetos de la escena contenidos en esferas discretizadas

8

(voxel map), en las cuales se definen las celdas como ocupadas o no, depen-diendo de la presencia del objeto en ellas. Adquirida la escena utilizan unafuncion de optimizacion para calcular la mejor vista siguiente, cuyos parame-tros, entre otros, estan medidos por la cantidad de celdas ocultas en cadavista candidata. En Sanchiz [4], se sigue la misma metodologıa, cambiandoel procesamiento de la imagen adquirida de voxel map, a triangulacion de lanube de puntos en 3D, metodo usado en la vision estereo, pero en este casoutlizando sensores de rango.

Sappa en [5] usa nuevamente el voxel map, pero se ayuda de la nor-mal de las celdas para hallar los bordes de los objetos, y de estas celdas seescoge aquella que brinda mayor informacion. Para el proceso de adquisi-cion utiliza un sensor de rango. Esta metodologıa es usada por Banta en[6], pero en ella se especifican unos pasos claros de como llegar a la mejorvista siguiente, mostrando solo resultados experimentales. Utiliza informa-cion monocular para formar la imagen de rango.

Por otro lado cabe resaltar los trabajos innovadores de busqueda y rescate[25], [26],[27],[28],[29],[30], entre los mas innovvadores los realizados por CRASAR,en el 2002 [7], bajo el marco de la tesis de Maestrıa de Mark Micire, en elcual se da una amplia descripcion de la operacion de busqueda y rescate decada uno de los robots moviles usados en el desastre del World Trade Center(9/11/2001). Son robots teleoperados, que en cuanto a vision, utilizan tecni-cas de segmentacion de color para hallar vıctimas. lamentablemente por lacantidad de polvo en los escombros, les fue difıcil diferenciar los actores dela escena.

En publicaciones realizadas por CRASAR, no se tiene referencia del usode vision estereo, pero si ven la necesidad de profundizar en el tema debido aque en las situaciones de busqueda y rescate es importante poder determinarla distancia en que se hallan los objetos y el tamano de los mismos, la cali-bracion de la posicion del movil en la escena de desastre, y la necesidad deutilizar tecnicas de mapeo en 3D, ya que la escena de los desastres presentanestructuras complejas.

En los ultimos anos los algoritmos relacionados con el planificador detrayectorias orientado a la reconstruccion de ambientes en 3D, no varıan de-masiado en los parametros que miden la mejor vista siguiente, mientras quela metodologıa usada para llegar a esa posicion optima requerida y la recons-

9

truccion del entorno en 3D si varıan, ante todo en el hardware. En cuanto a lamejor vista siguiente, muchos trabajos se han enfocado a la obtencion de losbordes de los objetos, por medio de diferentes metodologıas [17],[41]. AndreasNuchter presenta en los artıculos [8], [9],[33], [42] por medio de la transfor-macion Hough, las lıneas horizontales de la escena (bordes), dichas lıneasson candidatas a mejor vista siguiente y luego aplica en ellas una funcion de-pendiente de la ganancia de nueva informacion, la candidata que maximicedicha funcion es la mejor vista siguiente. La digitalizacion del entorno en 3D,la realiza un algoritmo de emparejamiento basado en iterative closest points(ICP) [37],[38],[39] el cual registra los escaneres en un sistema coordenadocomun. Al igual que en el trabajo anterior, existen multiples artıculos [22],en donde existen varias trayectorias o posiciones candidadatas, escogiendola que tenga la caracterıstica de mayor prioridad, como Rosenblatt en [21],el cual determina varias trayectorias como candidatas,y le da mayor prior-idad a aquella que proteja el movil de cualquier posible colision. Siguiendoesta misma metodologıa, Landa, en [10] hace referencia a robots moviles co-laboradores, cuyo algoritmo de mejor vista siguiente de manera individual,tiene como tarea encontrar discontinuidades en la escena (bordes), por mediode un sensor de rango, para luego aplicarle a los candidatos ubicados en lavecindad de dichas discontinuidades una funcion de visibilidad. La candida-ta que maximice dicha funcion sera la ganadora. El mapeo que realiza delentorno es en 2D. Similar metodologıa utiliza Burgard en [19], para robotscolaboradores, cuyo algoritmo de mejor vista siguiente de manera individual,lo enfoca en la visibilidad y cercanıa de los voxeles que adquiere durante laexploracion.

Joho en [11], ademas del parametro de visibilidad, le agrega a la busque-da de la mejor vista siguiente el parametro de costo de traslado, penalizandotraslados muy grandes, buscando ası que el planificador sea mas local. Parala reconstruccion en 3D del entorno desconocido usaron el mapa de super-ficies multi-nivel (MLS maps), en el cual las superficies de los elementos devolumen (voxel) de la escena discretizada, son representados independiente-mente por una funcion gaussiana, la cual esta regida por parametros comorugosidad, pendiente y deteccion de obstaculos. La reconstruccion en 3D conmapa multi-nivel, tambien es utilizado por Kummerle en [20]. Tovey en [18],determina un mapa de grises, etiquetando vertices como visitados, no visi-tados y no escaneados, y finalmente determinando el camino mas corto paradirigir el movil hacia zonas inexploradas.

10

Newman en [12], define propiedades importantes para delinear global-mente todas las consideraciones que debe tener un planificador para ser opti-mo.

Garantizar que el planificador explore de forma local.

Tratar de estar lejos de las areas ya visitadas.

Pasar a otra area local inexplorada, solo cuando el area local este to-talmente explorada.

Finalmente, el grupo CRASAR [13], muestra aplicaciones realizadas conrobots moviles teleoperados, en los cuales se corrigio deficiencias sufridas enel mapeo del entorno al tratar de sensar objetos transparentes, como el vidrio,de tal manera que se adaptan sensores sonares. La reconstruccion del entornofue realizado con voxel map.

En los planificadores anteriormente analizados, se pudo observar en al-gunos casos, la obtencion de caminos discretos y que no conservan en generallas condiciones de frontera en velocidad y aceleracion. En otras ocasiones porcaracterısticas propias del planificador, los desplazamientos realizados por elrobot movil eran intermitentes, al parar constantemente para calcular la posi-cion a la cual debıa continuar. Todas estas situaciones representan un altocosto energetico, y tienden a desgastar mas rapidamente las componentesmecanicas de los moviles.

Para revertir dicha situacion es necesario que el planificador genere trayec-torias continuas en velocidad y aceleracion de manera rapida. Por dicho moti-vo, la reconstruccion del entorno que necesita el planificador se puede realizarpor medio de baja resolucion. Lo anterior debido a que no es necesario unmodelo de la superficie de la escena, que generalmente se realiza por trian-gulacion de los puntos 3D. Ahora bien, para evitar caminos discretizados sedebe asegurar que los caminos generados por el planificador sean trayectoriassuaves. Consecuentemente las velocidades angulares de las ruedas maestrasdadas por el controlador deben describir la misma suavidad, haciendo de losdesplazamientos del robot mas fluidos.

11

2.1.1. Etapas en la planificacion de trayectorias

El planeamiento de trayectorias es un proceso que le permite al robotmovil tener autonomıa en sus desplazamientos, dependiendo de su finalidad.A continuacion se profundizara en algunas partes que componen un planifi-cador.

Adquisicion de los puntos en 3D de la primera escena a reconstruirpor medio de vision estereo.

En la geometrıa de dos vistas, es posible calcular correspondencias y -triangular los puntos 3D mediante la matriz fundamental F que relaciona doscamaras. El algoritmo se basa en la propiedad donde dos pares de camarasP1, P2 y P ′1, P

′2 poseen la misma matriz F y estan relacionadas por la matriz

de transformacion H como: P1 = P ′1H, P2 = P ′2H. Ası, es posible calcularuna realizacion de un par de camaras que si bien no son las matrices reales P1

y P2 estan relacionadas por la misma matriz fundamental, y de ellas podemosobtener puntos tridimensionales Xd que son producto de la transformacionproyectiva de las posiciones reales X, por lo que Xd = HX. Toda la teorıay los fundamentos matematicos son explicados en [1].

Reconstruccion de la escena con voxelizacion usando los puntos en3D hallados en el paso anterior, para realizar el mapa de grises.

La voxelizacion es una division del entorno virtual en cubos de dimen-siones fijas, que dependiendo de la ubicacion de los elementos en la escena,estaran ocupados o no. Los voxeles pueden presentar las siguientes etiquetas:

Voxel sin marcar: Voxel que aun no ha sido visto por el sensor.

Voxel vacıo: Voxel en el cual no se encuentra elemento de la escena.

Voxel ocupado: Voxel en el cual se encuentra un elemento de la escena.

12

Los voxeles ocupados seran aquellos en los cuales se encuentren puntos en3D obtenidos por medio de vision estereo. Los voxeles se iran actualizandoa medida que se obtengan mas nubes de puntos de la escena durante elrecorrido realizado por el movil en el entorno virtual.

En la Figura 2.1 se muestra la voxelizacion de una escena en diferentesresoluciones. Para lograr una mejor resolucion es necesario que los voxelesutilizados tengan menores dimensiones, lo que implica un gasto computa-cional mayor. Esta representacion es importante para el planificador, ya que

Figura 2.1: Ejemplo de voxelizacon

proporciona informacion con la cual se puede calcular la posicion hacia dondedebe dirigirse el robot movil; generalmente se buscan los bordes de los objetosde la escena en donde pueden esconder mayor informacion. En este trabajo serealiza el proceso de voxelizacion basado en el algoritmo octree, el cual sub-divide el entorno en 8 cubos, y lo sigue realizando iterativamente hasta lograrla reconstruccion [15],[16]. La teorıa de dicho algoritmo se profundizara en elCapıtulo cinco.

13

Calcular la posicion y orientacion que debe seguir el robot, paraganar la mayor informacion del entorno, a partir de la primeraescena reconstruida.

Dicha informacion es calculada por el algoritmo de la ”mejor vista si-guiente”, el cual consta del calculo de la posicion y orientacion siguiente, ala cual el robot debe seguir para obtener la mayor informacion del entorno.Este problema tiene como restriccion que, a pesar de que se busca obte-ner la mayor cantidad de informacion nueva posible, es necesario tener unacantidad mınima de informacion coincidente que permita realizar el procesode registro. En [8] Nuchter presenta un robot autonomo el cual debe planeary dirigirse a multiples posiciones para garantizar la reconstruccion de unmodelo en 3D de su entorno. Para ello se debe elegir secuencialmente, desdela posicion actual del robot, cual es la mejor vista siguiente. Esta escogenciase rige por dos factores: Cual vista proporciona mayor informacion y puedeser alcanzado por el robot y cual vista proporciona el camino mas corto.

En Banta et al.[6], se presenta un algoritmo para encontrar la mejor vistasiguiente, mostrando unos pasos detallados, los cuales se pueden adaptarfacilmente a la reconstruccion en 3D por medio de voxelizacion.

Calcular el modelo cinematico del robot movil, con el cual se puedaimplementar un controlador, de tal manera que el robot se desplacepor la trayectoria deseada.

El robot movil que se utilizara en la simulacion es un vehıculo que tienedos llantas traseras, izquierda y derecha, identicas y paralelas entre sı, nodeformables y unidas por un eje. Ademas, usa una rueda frontal omnidire-ccional que asegura que la plataforma del robot se encuentre sobre un plano.

En la Figura 2.2 se presenta el modelo del robot. En ella x,y denota laposicion del punto medio del eje que une las dos llantas traseras, ϕ es el anguloque forma el eje de simetrıa del movil respecto al eje x positivo, wi y wd sonlas velocidades angulares de las llantas izquierda y derecha respectivamente,mientras que r es el radio de las llantas y 2l es la separacion entre ellas.

14

Figura 2.2: Robot movil en el plano

Las ecuaciones que describen la cinematica del movil son:

x =(wd + wi)r cosϕ

2(2.1)

y =(wd + wi)r sinϕ

2(2.2)

ϕ =(wd − wi)r

2l(2.3)

El control propuesto permite llevar a las variables de estado, (x, y, ϕ) elseguimiento de una trayectoria nominal, (x∗, y∗, ϕ∗), bajo la condicion queel movil se encuentre inicialmente sobre un punto de esta trayectoria. Estose lleva a cabo mediante un controlador basado en linealizacion de entrada-salida, el cual impone que las variables de estado tiendan asintoticamente ala trayectoria deseada, generando una dinamica remanente asociada a unade las variables de estado, la cual debe resultar estable. Mas aun, si lascomponentes de la trayectoria deseada, son tales que y∗ = f(x∗), dondef(x∗) es una funcion suave y que cumple f(0) = 0, entonces, un calculodirecto muestra que y(t) = f(x ∗ (t)) = y∗. El controlador que se usara sepuede estudiar con mayor profundidad en [14].

15

Capıtulo 3

Planificador de trayectorias

El objetivo principal de un planificador de trayectorias orientado a lareconstruccion de entornos en 3D, es guiar al robot para la obtencion de lamayor informacion posible del entorno a explorar.

Para lograr dicho objetivo se sigue la siguiente metodologıa: primero seclasifica el entorno adquirido como visto, no visto y ocupado, segundo sedetermina la mejor vista siguiente, la cual dirige el movil hacia el entornoinexplorado, y por ultimo se genera una trayectoria desde la posicion delmovil hacia la mejor vista siguiente, dicha trayectoria debe cumplir condi-ciones como fluidez, no trasladarse por zonas inexploradas y evitar colisioncon objetos en la escena.

El capıtulo se encuentra organizado de la siguiente manera: en la Seccion3.1, se muestra la clasificacion del espacio, como visto, no visto y ocupado, yen la Seccion 3.2, se profundiza en la trayectoria deseada, en la Seccion 3.3se presenta la metodologıa para determinar la mejor vista siguiente, y en laSeccion 3.4, se observa la validacion de resultados.

16

3.1. Clasificacion del espacio: Nodos vistos,

no vistos y ocupados

La clasificacion del espacio en la reconstruccion en 3D, tiene diferentesutilidades, una de ellas es la informacion topografica del espacio visto, oculto,y el espacio ocupado por los obstaculos, de esta forma se lleva un registro dedonde se ha estado, hacia donde se debe ir y en que lugar estan los obstaculos.

A diferencia de los trabajos previos [2], [3] en los cuales se realiza la re-construccion en 3D primero y despues se clasifica el espacio, en este Capıtulose propone la realizacion de la clasificacion del espacio antes de la voxelizacionutilizando nodos a los cuales se les asigna un espacio determinado, el cualrecibira la etiquetas de ocupado, visto o no visto. De esta forma la selecciondel espacio se vuelve mas rapida, al manejar un punto y no un cubo en elanalisis general.

Partiendo del hecho que la reconstruccion realizada por el robot es unespacio limitado, ubicamos los nodos dependiendo de un delta de distanciaentre ellos y que sea igual en los ejes x, y, z. La division entre la distanciatotal de cada eje y la distancia delta entre los nodos determina la cantidadde nodos en el espacio. En la Figura 3.1 se muestra un espacio limitado entre-120 y 120 cm para los ejes (x, y) y un espacio entre 0 y 30 cm para el eje z,y un delta de distancia de 30 cm entre nodos para cada eje.

(a) Ejes x,y,z (b) Ejes x,y

Figura 3.1: Distribucion de los nodos en un espacio determinado. Delta 30cm.

17

En la Figura 3.2, se muestra un espacio limitado entre -150 y 150 cmpara los ejes (x, y) y un espacio entre 0 y 40 cm para el eje z. Y un delta dedistancia de 20 cm entre nodos para cada eje.

(a) Ejes x,y (b) Ejes x,y,z

Figura 3.2: Distribucion de los nodos en un espacio determinado. Delta: 20cm.

Para el proceso de adquisicion, el robot movil cuenta con un par decamaras web fijas ubicadas en la parte superior. Estas camaras poseen unalcance visual, que varıa dependiendo de las especificaciones propias de cadacamara. Existe una gran cantidad de casos que se pueden analizar en cuan-to a la etiqueta de los nodos, referente a condiciones especıficas dentro delalcance visual de las camaras como son: ausencia de obstaculo, cantidad deobstaculo, tamano del obstaculo, etc. En el algoritmo desarrollado que semuestra a continuacion se analizaron dos casos generales:

3.1.1. Clasificacion de nodos sin obstaculo presente enel campo visual de la camara

Los nodos se inicializan como no vistos Xn (NV), Yn (NV), Zn (NV),en el vector NV se guardan los ındices de los nodos no vistos. Ahora latarea se enfoca en ubicar los nodos vistos. Todo el algoritmo desarrolladoy descrito a continuacion se realizo primero en el plano (x, y). En este casoespecıfico, la condicion que debe cumplir un nodo para que sea etiquetadocomo visto es estar dentro del alcance visual de la camara. Para determinarque nodos cumplen esta condicion, obtenemos el angulo de elevacion a partir

18

de la ubicacion cartesiana del nodo, si dicho angulo esta entre el angulo de lasproyecciones del alcance visual de la camara, son nodos vistos. Un ejemplo sepuede observar en la Figura 3.3, en donde los nodos de color rojo son nodosvistos y los azules, nodos no vistos, y las semirectas rojas son las proyeccionesde dos camaras web de un alcance visual de 150 grados en total, y con unangulo inicial del movil de 180 grados.

(a) Ejes x,y,z (b) Ejes x,y

Figura 3.3: Nodos vistos y no vistos. Caso 1.

Para lograr la etiqueta de los nodos vistos en el programa de Matlab,se recurre a los ındices del vector que guarda la posicion cartesiana de losnodos no vistos. De tal manera cuando un nodo cumple la condicion devisto, toma el ındice y por ende la posicion cartesiana del vector no visto, einmediatamente dicho ındice es eliminado del vector de los nodos no vistos.De esta forma en el momento en que todos los nodos no vistos cambien suetiqueta, sera el fin del algoritmo.

3.1.2. Clasificacion de nodos con obstaculo presente enel campo visual de la camara.

Para lograr un mejor desempeno y facilidad de analisis en todos los aspec-tos, cualquier objeto del ambiente virtual sin importar su forma o tamano,sera enmarcado en una esfera, la cual garantice la presencia de todos lospuntos adquiridos del objeto. Por ello de ahora en adelante el obstaculosera visto en 3D como una esfera y en 2D como una circunferencia. En laFigura 3.4(a), se muestran 2 proyecciones rojas hacia el objeto, de las cuales

19

se etiqueta como primera proyeccion la de menor elevacion angular y comola segunda proyeccion la de mayor elevacion angular. Tambien se muestran2 proyecciones rojas de alcance visual, de las cuales la de menor elevacionangular es etiquetada como primera proyeccion y la de mayor elevacion an-gular como segunda proyeccion. En el caso de existir un obstaculo dentrodel campo visual de las camaras, los nodos califican como vistos, si cumplealguna de las siguientes condiciones:

El angulo de elevacion del nodo debe estar entre los angulos de elevacionde la primera proyeccion del alcance visual de las camaras y el angulode elevacion de la primera proyeccion hacia el obstaculo.

El angulo de elevacion del nodo debe estar entre los angulos de elevacionde la segunda proyeccion del alcance visual de las camaras y el angulode elevacion de la segunda proyeccion hacia el obstaculo.

El angulo de elevacion del nodo debe estar entre los angulos de elevacionde la primera y la segunda proyeccion hacia el obstaculo y ademasla distancia euclidiana entre el movil y el nodo debe ser menor a ladistancia euclidiana entre el movil y el centro de la circunferencia querepresenta el obstaculo.

Un ejemplo de la aplicacion del algoritmo anterior, se muestra en la Figura3.4, en diferentes vistas.

(a) Ejes x,y (b) Ejes x,z

Figura 3.4: Nodos vistos y no vistos. Caso 2.

20

Otro caso que se puede presentar ocurre cuando solo una parte del objetoesta en el alcance visual de las camaras. Por esta razon solo una proyeccionde las camaras hacia el objeto, esta contenida en el alcance visual de lascamaras, y a partir de dicha proyeccion se clasifican los nodos. Los nodosvistos seran los nodos que cumplan las siguientes 2 condiciones:

El angulo de elevacion del nodo debe estar entre los angulos de elevacionde la primera proyeccion del alcance visual de las camaras y el angulode elevacion de la proyeccion hacia el obstaculo que este presente en elalcance visual de las camaras.

El angulo de elevacion del nodo debe estar entre los angulos de ele-vacion de la primera proyeccion del alcance visual de las camaras y laproyeccion hacia el objeto dentro del alcance visual de las camaras, yademas la distancia euclidiana entre el movil y el nodo debe ser menora la distancia euclidiana entre el movil y el centro de la circunferenciaque representa el obstaculo.

Un ejemplo de dicho algoritmo se puede observar en la Figura 3.5(a), endonde las lıneas azules representan las proyecciones del alcance visual de lascamaras, mientras que las lineas rojas, representan las proyecciones de lascamaras hacia el objeto. En la Figura 3.5(b), se puede observar el caso dondela otra proyeccion hacia el objeto esta en el alcance visual. El algoritmo paraobtener los nodos vistos cumple con la misma metodologıa anteriormentemencionada.

(a) Proyeccion 1 del objeto en alcancevisual

(b) Proyeccion 2 del objeto en alcancevisual

Figura 3.5: Nodos vistos y no vistos. Parte del objeto en alcance visual.

21

3.1.3. Clasificacion de los nodos ocupados

Cada nodo representa un espacio determinado, que corresponde al deltade distancia que existe entre cada nodo en los ejes x, y y z. De tal maneraque si la ubicacion cartesiana del nodo es Xn, Y n, Zn, el espacio que lepertenece serıa Xn+Delta, Y n+Delta, Zn+Delta.

Si dentro del espacio de un nodo, se determina la existencia de un puntoperteneciente al objeto adquirido, el nodo queda etiquetado como ocupado.El algoritmo para identificar los nodos ocupados, empieza por encontrar elnodo mas cercano al primer elemento del vector que guarda la posicion de lospuntos de la esfera. Para ello se resta las coordenadas de la posicion de todoslos nodos (x, y, z), con las coordenadas del punto de la esfera, las restas seguardan en el vector Bocx, como se muestra en la ecuacion (3.1):

Bocx = [|(Xn(R)− xx2(1))|+ |(Yn(R)− yy2(1))|+ |(Zn(R)− z(1))|] (3.1)

Donde Xn, Yn y Zn representan la ubicacion del nodo en el espacio, R esel vector en donde se guardan las iteraciones de todos los nodos, y xx2, yy2 yz representan la ubicacion del punto de la esfera. Luego se ordena el vectorde menor a mayor, siendo el primer elemento del vector la distancia mınima.Paso seguido se halla la iteracion en donde se encuentra la distancia menor,y de esta forma obtenemos la posicion en el plano del nodo. Para identificarsi el nodo encontrado en la iteracion esta ocupado, cualquier punto de laesfera debe estar contenida en el espacio que representa el nodo. Para ellose deben cumplir tres condiciones suponiendo que el espacio perteneciente alnodo esta comprendido entre Xn y Xn + Delta, Y n y Y n + Delta, Zn yZn+Delta:

La distancia xx2 debe estar entre Xn y Xn+Delta.

La distancia yy2 debe estar entre Y n y Y n+Delta.

La distancia z debe estar entre Zn y Zn+Delta.

22

Para utilizar el mismo proceso se debe eliminar el nodo ocupado y el pun-to o los puntos de la esfera contenidos en el punto ocupado, para evitar que elmismo nodo gane la competencia de la distancia mınima y poder analizar losdemas nodos y puntos de la esfera. El ciclo de busqueda de nodos ocupadoacaba cuando el vector que guarda los puntos de la esfera se encuentra vacıo.En la Figura 3.6, se muestra en 2D el proceso de eliminacion paso a pasode los puntos de la esfera y la etiqueta de los nodos ocupados y los nodosanalizados que no alcanzaron la etiqueta de ocupados.

(a) Paso1.Nodos ocupados 1. (b) Paso 2.Nodos ocupados 2.

(c) Paso 3.Nodos ocupados 3. (d) Paso 4.Nodos ocupados 3. No-dos analizados 1.

23

(e) Paso 5.Nodos ocupados 4. Nodosanalizados 2.

Figura 3.6: Nodos ocupados, Nodos analizados y eliminacion de puntos de laesfera.

3.1.4. Resultados

A continuacion se muestran resultados, en donde se varıa la ubicacion delobstaculo (esfera), orientacion del robot movil, alcance visual de las camaras,tamano de la esfera, cantidad de nodos y distancia maxima de reconstrucciondel entorno. En la Figura 3.7(a), se modifica la ubicacion del obstaculo, detal forma que no se encuentra en el alcance visual del robot. Sin embargo, losnodos estan bien etiquetados. En la Figura 3.7(b), se modifica la orientaciondel robot, y se mantiene la ubicacion del obstaculo.

(a) Modificacion de la ubicacion del obs-taculo.

(b) Cambio de orientacion del robotmovil.

Figura 3.7: Prueba 1 y 2.

En Figura 3.8(a), se muestra la reduccion del alcance visual de 150 gra-

24

dos en total a 100 grados, y en la Figura 3.8(b) se muestra el aumento deldiametro de la esfera de 15 a 25 cm, de tal manera que la cantidad de nodosocupados aumenta, como se puede observar en la figura aumento de 6 a 8nodos ocupados.

(a) Reduccion del campo visual de lascamaras web.

(b) Aumento del diametro de la circun-ferencia.

Figura 3.8: Prueba 3 y 4.

En la Figura 3.9(a), se aumenta la cantidad de nodos, al disminuir el deltade distancia entre los nodos de 30 a 20 cm, y por ultimo en la Figura 3.9(b),se aumenta el espacio de reconstruccion 3D, en el eje x de [-120, 120]cm a[-200, 200]cm, en el eje y de [120 120]cm a [-200 200]cm, y en el eje z de [0,30]cm a [0, 60]cm.

(a) Aumento de los nodos. (b) Aumento del espacio de trabajo.

Figura 3.9: Prueba 5 y 6.

25

3.2. Trayectoria deseada

Una de las situaciones mas comprometidas para el planificador, es laobtencion de caminos suaves en el momento de rodear un obstaculo en lanecesidad de estar en una mejor posicion para adquirir informacion de zonasno vistas. A continuacion se propone una metodologıa en 2D (ejes x, y), parala obtencion del planificador de trayectorias. En la Figura 3.10(a), se muestrala simulacion de un ambiente en el cual el movil se encuentre en la posicion(0,0). La circunferencia representa el obstaculo, en donde estan circunscritoslos puntos adquiridos del objeto, y por ultimo la posicion hacia donde nosdirigimos. Como se puede observar no es posible llegar al objetivo por mediode una lınea recta, de tal forma que se debe encontrar la manera de rodear lacircunferencia. En la Figura 3.10(b), se encuentran los dos puntos de inter-seccion de la trayectoria con la circunferencia, que resulta de despejar y de laEcuacion (3.2), que representa la ecuacion de la semirrecta, y despejar y de laEcuacion (3.3), que representa la ecuacion de la circunferencia. En donde mes la pendiente, y los valores de x1, y1, determinan la posicion del movil, enla Ecuacion (3.2). Y las variables a y b, son las coordenadas cartesianas querepresentan la ubicacion del centro de la circunferencia, en la Ecuacion (3.3).Y por ultimo se igualan las ecuaciones, el sistema resultante y los despejesprevios se resuelven en Matlab. Se elimina posteriormente la trayectoria quese encuentre dentro de la circunferencia, evadiendo de esta forma colisiones.

y − y1 = m(x− x1) (3.2)

(x− a)2 + (y − b)2 = r2 (3.3)

26