Planificador de trayectorias para un robot movil

orientado a la reconstruccion en 3D de entornos

desconocidos

Mauricio Fernando Jaramillo Morales

Universidad Nacional de Colombia

Sede Manizales

Facultad de Ingeniera y Arquitectura

Departamento de Electrica, Electronica y Computacion

Manizales, Colombia

2010
Planificador de trayectorias para un robot movil
orientado a la reconstruccion en 3D de entornos
desconocidos

por

Mauricio Fernando Jaramillo Morales

Tesis

Presentada a la Facultad de Ingeniera y Arquitectura de la

Universidad Nacional de Colombia

Sede Manizales

En cumplimiento

de los Requerimientos

para el Grado de

Magister en Ingeniera

Universidad Nacional de Colombia

Sede Manizales
Mayo 2010
Indice general

1. Introduccion 5

2. Marco teorico 8
2.1. Trabajos previos . . . . . . . . . . . . . . . . . . . . . . . . . 8
2.1.1. Etapas en la planificacion de trayectorias . . . . . . . . 12

3. Planificador de trayectorias 16
3.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 el
campo visual de la camara. . . . . . . . . . . . . . . . 19
3.1.3. Clasificacion de los nodos ocupados . . . . . . . . . . . 22
3.1.4. Resultados . . . . . . . . . . . . . . . . . . . . . . . . . 24
3.2. Trayectoria deseada . . . . . . . . . . . . . . . . . . . . . . . . 26
3.2.1. Adaptacion de la funcion gaussiana en la obtencion de
la trayectoria deseada . . . . . . . . . . . . . . . . . . . 28
3.2.2. Tratamiento del angulo inicial del movil . . . . . . . . 30
3.2.3. Replanteamiento de la trayectoria adecuada cuando el
obstaculo esta muy cerca del robot movil. . . . . . . . 34
3.3. Mejor vista siguiente . . . . . . . . . . . . . . . . . . . . . . . 37
3.4. Validacion de resultados . . . . . . . . . . . . . . . . . . . . . 42

4. Controlador 49
4.1. Controlador modelo cinematico . . . . . . . . . . . . . . . . . 49
4.1.1. Linealizacion por retroalimentacion dinamica . . . . . . 50
4.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 ruedas
del robot movil . . . . . . . . . . . . . . . . . . . . . . . . . . 54
4.4. Resultados . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59
4.5. Validacion de resultados . . . . . . . . . . . . . . . . . . . . . 61

5. Voxelizacion 70
5.1. Voxelizacion basada en el algoritmo octree . . . . . . . . . . . 70
5.2. Puntos vistos por las camaras del robot movil y voxelizacion . 74
5.3. Resultados . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 77
5.4. Validacion de resultados. . . . . . . . . . . . . . . . . . . . . . 78

6. Resultados 82
6.1. Validacion de resultados . . . . . . . . . . . . . . . . . . . . . 86

7. Conclusiones y trabajos futuros 93

4
Captulo 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 centrados
en desarrollar tecnicas de planificacion de trayectorias, reconstruccion en 3D
del entorno desconocido y sistemas de control que le permitan al movil seguir
las trayectorias obtenidas [5],[6],[8],[9],[15],[23]. Pero aun quedan campos por
explorar, entre ellos la eficiencia de los desplazamientos del movil, ya que
en muchas ocasiones son discontinuos e intermitentes, lo que repercute en un
mayor gasto de energa. Los trabajos realizados en la Universidad de Florida,
por el grupo de investigacion, CRASAR (Center of Robotic Assisted Search
and Rescue) son una muestra clara del interes en el tema, y del alto numero
de 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 vctimas,
busqueda de caminos (de los cuales se pueda excavar rapidamente, a traves de
los escombros), inspeccion estructural y deteccion de materiales peligrosos.
En cada caso, los robots fueron usados porque ellos pueden ir con mayor
profundidad, rutinariamente viajan de 5 a 20 metros en el interior de los
escombros, en cambio una camara montada en una polea solo llega a 2 metros
de profundidad. Ademas pueden entrar a espacios pequenos para un humano
o para un canino de rescate, o pueden entrar en un sitio en fuego o en gran
riesgo de un colapso estructural [7]. La reconstruccion del entorno puede

5
ser llevada a cabo mediante el uso de camaras de video y reconstruccion
estereo, las camaras de video pueden ser camaras web, lo que influye de
una manera importante en una implementacion asequible a un presupuesto
relativamente bajo y a una facil consecucion, con lo cual se pueda obtener
resultados esperados segun la finalidad del robot. Ademas, existen trabajos
realizados por el grupo de investigacion PCI, en uno de ellos, se describe un
sistema estereo activo para la adquisicion de imagenes de rango de bajo costo
implementado sobre MATLAB . R El sistema combina metodos de estimacion
de parametros para dos vistas (tanto calibradas como no calibradas) [1], lo
que constituye una buena base en el diseno del sistema estereo para la fase
de adquisicion del entorno desconocido.
El proceso de planificacion de trayectorias de robots moviles orientados
a la reconstruccion eficiente del entorno a partir de tecnicas de vision en
estereo, consta fundamentalmente de las siguientes etapas:

Adquisicion de los puntos en 3D de la primera escena a reconstruir por

medio de vision estereo.

Reconstruccion de la escena usando los puntos en 3D hallados en el

paso anterior.

Calcular la posicion y orientacion que debe seguir el robot, para ga-

nar la mayor informacion del entorno, a partir de la primera escena
reconstruida.

Generar una trayectoria entre la posicion actual y la posicion deseada

para el robot.

Calcular el modelo cinematico del robot movil, con el cual se pueda

implementar un controlador, de tal manera que el robot se desplace
por 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 teora definida acerca de este tema, la metodologa

presentada anteriormente son los pasos que en comun siguen los trabajos
previos, 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 la
reconstruccion de la escena se utiliza en su mayora voxeles (cubos) para
clasificar 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 las
imagenes y de esta forma encontrar la posicion que entrega la mayor cantidad
de informacion desconocida.[44].
El problema en resumen es disenar un planificador de trayectorias para
un robot movil, el cual utilice como fuente de informacion la reconstruccion
incremental de un entorno desconocido de manera autonoma.
El contenido del documento es como sigue: en el Captulo 2, se presen-
tan trabajos previos y definiciones basicas, propias de la tematica de este
trabajo. En el Captulo 3, se aborda la metodologa propuesta para la obten-
cion del planificador de trayectorias, resaltando los algoritmos de clasificacion
del espacio, mejor vista siguiente y trayectorias deseadas. En el Captulo 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 el
Captulo 5, se muestran los resultados obtenidos en la reconstruccion de am-
bientes virtuales desconocidos. finalmente en el Captulo 6 se presentan las
conclusiones y los trabajos futuros.

7
Captulo 2

Marco teorico

A continuacion se presentaran trabajos previos del tema y se definiran al-

gunos terminos necesarios para una mejor comprension del metodo propuesto
de planificacion de trayectorias.
El captulo se encuentra organizado de la siguiente manera: en la Seccion
2.1, se muestran los metodos mas utilizados en el estado del arte, enfocados
en la obtencion del planificador de trayectorias para un robot movil, y en la
Seccion 2.2 se profundizara en las etapas que componen el planificador de
trayectorias 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 caminos
que obtengan la mayor cantidad de informacion posible en cada desplaza-
miento. En los siguientes artculos se muestran diferentes metodologas tanto
en la reconstruccion de escenas u objetos en 3D, como en la determinacion de
la posicion siguiente (mejor vista siguiente), las cuales son etapas importantes
en el diseno del planificador deseado.
Nikolaos et al.[2], al igual que en la tesis de maestra 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 una
funcion de optimizacion para calcular la mejor vista siguiente, cuyos parame-
tros, entre otros, estan medidos por la cantidad de celdas ocultas en cada
vista candidata. En Sanchiz [4], se sigue la misma metodologa, cambiando
el procesamiento de la imagen adquirida de voxel map, a triangulacion de la
nube de puntos en 3D, metodo usado en la vision estereo, pero en este caso
utlizando 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 se
escoge aquella que brinda mayor informacion. Para el proceso de adquisi-
cion utiliza un sensor de rango. Esta metodologa es usada por Banta en
[6], pero en ella se especifican unos pasos claros de como llegar a la mejor
vista 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 Maestra de Mark Micire, en el
cual se da una amplia descripcion de la operacion de busqueda y rescate de
cada 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 vctimas. lamentablemente por la
cantidad de polvo en los escombros, les fue difcil diferenciar los actores de
la escena.
En publicaciones realizadas por CRASAR, no se tiene referencia del uso
de vision estereo, pero si ven la necesidad de profundizar en el tema debido a
que en las situaciones de busqueda y rescate es importante poder determinar
la 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 de
utilizar tecnicas de mapeo en 3D, ya que la escena de los desastres presentan
estructuras complejas.
En los ultimos anos los algoritmos relacionados con el planificador de
trayectorias orientado a la reconstruccion de ambientes en 3D, no varan de-
masiado en los parametros que miden la mejor vista siguiente, mientras que
la metodologa usada para llegar a esa posicion optima requerida y la recons-

9
truccion del entorno en 3D si varan, ante todo en el hardware. En cuanto a la
mejor vista siguiente, muchos trabajos se han enfocado a la obtencion de los
bordes de los objetos, por medio de diferentes metodologas [17],[41]. Andreas
Nuchter presenta en los artculos [8], [9],[33], [42] por medio de la transfor-
macion Hough, las lneas horizontales de la escena (bordes), dichas lneas
son candidatas a mejor vista siguiente y luego aplica en ellas una funcion de-
pendiente de la ganancia de nueva informacion, la candidata que maximice
dicha 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 coordenado
comun. Al igual que en el trabajo anterior, existen multiples artculos [22],
en donde existen varias trayectorias o posiciones candidadatas, escogiendo
la que tenga la caracterstica 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. Siguiendo
esta misma metodologa, 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 medio
de un sensor de rango, para luego aplicarle a los candidatos ubicados en la
vecindad de dichas discontinuidades una funcion de visibilidad. La candida-
ta que maximice dicha funcion sera la ganadora. El mapeo que realiza del
entorno es en 2D. Similar metodologa utiliza Burgard en [19], para robots
colaboradores, cuyo algoritmo de mejor vista siguiente de manera individual,
lo enfoca en la visibilidad y cercana de los voxeles que adquiere durante la
exploracion.
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, penalizando
traslados muy grandes, buscando as que el planificador sea mas local. Para
la reconstruccion en 3D del entorno desconocido usaron el mapa de super-
ficies multi-nivel (MLS maps), en el cual las superficies de los elementos de
volumen (voxel) de la escena discretizada, son representados independiente-
mente por una funcion gaussiana, la cual esta regida por parametros como
rugosidad, pendiente y deteccion de obstaculos. La reconstruccion en 3D con
mapa 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 para
dirigir 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 con

robots moviles teleoperados, en los cuales se corrigio deficiencias sufridas en
el mapeo del entorno al tratar de sensar objetos transparentes, como el vidrio,
de tal manera que se adaptan sensores sonares. La reconstruccion del entorno
fue 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 general
las condiciones de frontera en velocidad y aceleracion. En otras ocasiones por
caractersticas propias del planificador, los desplazamientos realizados por el
robot movil eran intermitentes, al parar constantemente para calcular la posi-
cion a la cual deba continuar. Todas estas situaciones representan un alto
costo energetico, y tienden a desgastar mas rapidamente las componentes
mecanicas 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 realizar
por medio de baja resolucion. Lo anterior debido a que no es necesario un
modelo de la superficie de la escena, que generalmente se realiza por trian-
gulacion de los puntos 3D. Ahora bien, para evitar caminos discretizados se
debe asegurar que los caminos generados por el planificador sean trayectorias
suaves. Consecuentemente las velocidades angulares de las ruedas maestras
dadas por el controlador deben describir la misma suavidad, haciendo de los
desplazamientos 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 robot

movil tener autonoma 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 reconstruir

por medio de vision estereo.

En la geometra de dos vistas, es posible calcular correspondencias y -

triangular los puntos 3D mediante la matriz fundamental F que relaciona dos
camaras. El algoritmo se basa en la propiedad donde dos pares de camaras
P1 , P2 y P10 , P20 poseen la misma matriz F y estan relacionadas por la matriz
de transformacion H como: P1 = P10 H, P2 = P20 H. As, es posible calcular
una 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 podemos
obtener puntos tridimensionales Xd que son producto de la transformacion
proyectiva de las posiciones reales X, por lo que Xd = HX. Toda la teora
y los fundamentos matematicos son explicados en [1].

Reconstruccion de la escena con voxelizacion usando los puntos en

3D 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 vaco: 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 en
3D obtenidos por medio de vision estereo. Los voxeles se iran actualizando
a medida que se obtengan mas nubes de puntos de la escena durante el
recorrido realizado por el movil en el entorno virtual.
En la Figura 2.1 se muestra la voxelizacion de una escena en diferentes
resoluciones. Para lograr una mejor resolucion es necesario que los voxeles
utilizados 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 donde

debe dirigirse el robot movil; generalmente se buscan los bordes de los objetos
de la escena en donde pueden esconder mayor informacion. En este trabajo se
realiza el proceso de voxelizacion basado en el algoritmo octree, el cual sub-
divide el entorno en 8 cubos, y lo sigue realizando iterativamente hasta lograr
la reconstruccion [15],[16]. La teora de dicho algoritmo se profundizara en el
Captulo cinco.

13
Calcular la posicion y orientacion que debe seguir el robot, para
ganar la mayor informacion del entorno, a partir de la primera
escena 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, a
la 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 una
cantidad mnima de informacion coincidente que permita realizar el proceso
de registro. En [8] Nuchter presenta un robot autonomo el cual debe planear
y dirigirse a multiples posiciones para garantizar la reconstruccion de un
modelo en 3D de su entorno. Para ello se debe elegir secuencialmente, desde
la posicion actual del robot, cual es la mejor vista siguiente. Esta escogencia
se rige por dos factores: Cual vista proporciona mayor informacion y puede
ser 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 vista
siguiente, mostrando unos pasos detallados, los cuales se pueden adaptar
facilmente a la reconstruccion en 3D por medio de voxelizacion.

Calcular el modelo cinematico del robot movil, con el cual se pueda

implementar un controlador, de tal manera que el robot se desplace
por la trayectoria deseada.

El robot movil que se utilizara en la simulacion es un vehculo que tiene

dos llantas traseras, izquierda y derecha, identicas y paralelas entre s, no
deformables 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 la
posicion del punto medio del eje que une las dos llantas traseras, es el angulo
que forma el eje de simetra del movil respecto al eje x positivo, wi y wd son
las 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:

(wd + wi )r cos
x = (2.1)
2
(wd + wi )r sin
y = (2.2)
2
(wd wi )r
= (2.3)
2l

El control propuesto permite llevar a las variables de estado, (x, y, ) el

seguimiento de una trayectoria nominal, (x, y, ), bajo la condicion que
el movil se encuentre inicialmente sobre un punto de esta trayectoria. Esto
se lleva a cabo mediante un controlador basado en linealizacion de entrada-
salida, el cual impone que las variables de estado tiendan asintoticamente a
la trayectoria deseada, generando una dinamica remanente asociada a una
de las variables de estado, la cual debe resultar estable. Mas aun, si las
componentes de la trayectoria deseada, son tales que y = f (x), donde
f (x) es una funcion suave y que cumple f (0) = 0, entonces, un calculo
directo muestra que y(t) = f (x (t)) = y. El controlador que se usara se
puede estudiar con mayor profundidad en [14].

15
Captulo 3

Planificador de trayectorias

El objetivo principal de un planificador de trayectorias orientado a la

reconstruccion de entornos en 3D, es guiar al robot para la obtencion de la
mayor informacion posible del entorno a explorar.
Para lograr dicho objetivo se sigue la siguiente metodologa: primero se
clasifica el entorno adquirido como visto, no visto y ocupado, segundo se
determina la mejor vista siguiente, la cual dirige el movil hacia el entorno
inexplorado, y por ultimo se genera una trayectoria desde la posicion del
movil hacia la mejor vista siguiente, dicha trayectoria debe cumplir condi-
ciones como fluidez, no trasladarse por zonas inexploradas y evitar colision
con objetos en la escena.
El captulo se encuentra organizado de la siguiente manera: en la Seccion
3.1, se muestra la clasificacion del espacio, como visto, no visto y ocupado, y
en la Seccion 3.2, se profundiza en la trayectoria deseada, en la Seccion 3.3
se presenta la metodologa para determinar la mejor vista siguiente, y en la
Seccion 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 diferentes

utilidades, 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 de
donde 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 Captulo
se propone la realizacion de la clasificacion del espacio antes de la voxelizacion
utilizando nodos a los cuales se les asigna un espacio determinado, el cual
recibira la etiquetas de ocupado, visto o no visto. De esta forma la seleccion
del espacio se vuelve mas rapida, al manejar un punto y no un cubo en el
analisis general.
Partiendo del hecho que la reconstruccion realizada por el robot es un
espacio limitado, ubicamos los nodos dependiendo de un delta de distancia
entre ellos y que sea igual en los ejes x, y, z. La division entre la distancia
total de cada eje y la distancia delta entre los nodos determina la cantidad
de 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 30

cm.

17
 En la Figura 3.2, se muestra un espacio limitado entre -150 y 150 cm
para los ejes (x, y) y un espacio entre 0 y 40 cm para el eje z. Y un delta de
distancia 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: 20

cm.

Para el proceso de adquisicion, el robot movil cuenta con un par de

camaras web fijas ubicadas en la parte superior. Estas camaras poseen un
alcance visual, que vara dependiendo de las especificaciones propias de cada
camara. Existe una gran cantidad de casos que se pueden analizar en cuan-
to a la etiqueta de los nodos, referente a condiciones especficas dentro del
alcance visual de las camaras como son: ausencia de obstaculo, cantidad de
obstaculo, tamano del obstaculo, etc. En el algoritmo desarrollado que se
muestra a continuacion se analizaron dos casos generales:

3.1.1. Clasificacion de nodos sin obstaculo presente en

el 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 la
tarea se enfoca en ubicar los nodos vistos. Todo el algoritmo desarrollado
y descrito a continuacion se realizo primero en el plano (x, y). En este caso
especfico, la condicion que debe cumplir un nodo para que sea etiquetado
como visto es estar dentro del alcance visual de la camara. Para determinar
que 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 las
proyecciones del alcance visual de la camara, son nodos vistos. Un ejemplo se
puede observar en la Figura 3.3, en donde los nodos de color rojo son nodos
vistos y los azules, nodos no vistos, y las semirectas rojas son las proyecciones
de dos camaras web de un alcance visual de 150 grados en total, y con un
angulo 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 los
nodos no vistos. De tal manera cuando un nodo cumple la condicion de
visto, toma el ndice y por ende la posicion cartesiana del vector no visto, e
inmediatamente 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 su
etiqueta, sera el fin del algoritmo.

3.1.2. Clasificacion de nodos con obstaculo presente en

el 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 los
puntos adquiridos del objeto. Por ello de ahora en adelante el obstaculo
sera visto en 3D como una esfera y en 2D como una circunferencia. En la
Figura 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 como
la segunda proyeccion la de mayor elevacion angular. Tambien se muestran
2 proyecciones rojas de alcance visual, de las cuales la de menor elevacion
angular es etiquetada como primera proyeccion y la de mayor elevacion an-
gular como segunda proyeccion. En el caso de existir un obstaculo dentro
del campo visual de las camaras, los nodos califican como vistos, si cumple
alguna de las siguientes condiciones:

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

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

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

Un ejemplo de la aplicacion del algoritmo anterior, se muestra en la Figura

3.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 objeto
esta en el alcance visual de las camaras. Por esta razon solo una proyeccion
de las camaras hacia el objeto, esta contenida en el alcance visual de las
camaras, y a partir de dicha proyeccion se clasifican los nodos. Los nodos
vistos seran los nodos que cumplan las siguientes 2 condiciones:

El angulo de elevacion del nodo debe estar entre los angulos de elevacion
de la primera proyeccion del alcance visual de las camaras y el angulo
de elevacion de la proyeccion hacia el obstaculo que este presente en el
alcance 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 la
proyeccion hacia el objeto dentro del alcance visual de las camaras, y
ademas la distancia euclidiana entre el movil y el nodo debe ser menor
a la distancia euclidiana entre el movil y el centro de la circunferencia
que representa el obstaculo.

Un ejemplo de dicho algoritmo se puede observar en la Figura 3.5(a), en

donde las lneas azules representan las proyecciones del alcance visual de las
camaras, mientras que las lineas rojas, representan las proyecciones de las
camaras hacia el objeto. En la Figura 3.5(b), se puede observar el caso donde
la otra proyeccion hacia el objeto esta en el alcance visual. El algoritmo para
obtener los nodos vistos cumple con la misma metodologa anteriormente
mencionada.

(a) Proyeccion 1 del objeto en alcance (b) Proyeccion 2 del objeto en alcance
visual visual

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 delta

de distancia que existe entre cada nodo en los ejes x, y y z. De tal manera
que si la ubicacion cartesiana del nodo es Xn, Y n, Zn, el espacio que le
pertenece sera Xn + Delta, Y n + Delta, Zn + Delta.
Si dentro del espacio de un nodo, se determina la existencia de un punto
perteneciente al objeto adquirido, el nodo queda etiquetado como ocupado.
El algoritmo para identificar los nodos ocupados, empieza por encontrar el
nodo mas cercano al primer elemento del vector que guarda la posicion de los
puntos de la esfera. Para ello se resta las coordenadas de la posicion de todos
los nodos (x, y, z), con las coordenadas del punto de la esfera, las restas se
guardan 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 es

el vector en donde se guardan las iteraciones de todos los nodos, y xx2 , yy2 y
z representan la ubicacion del punto de la esfera. Luego se ordena el vector
de menor a mayor, siendo el primer elemento del vector la distancia mnima.
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 identificar
si el nodo encontrado en la iteracion esta ocupado, cualquier punto de la
esfera debe estar contenida en el espacio que representa el nodo. Para ello
se deben cumplir tres condiciones suponiendo que el espacio perteneciente al
nodo esta comprendido entre Xn y Xn + Delta, Y n y Y n + Delta, Zn y
Zn + 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 el
mismo nodo gane la competencia de la distancia mnima y poder analizar los
demas nodos y puntos de la esfera. El ciclo de busqueda de nodos ocupado
acaba cuando el vector que guarda los puntos de la esfera se encuentra vaco.
En la Figura 3.6, se muestra en 2D el proceso de eliminacion paso a paso
de los puntos de la esfera y la etiqueta de los nodos ocupados y los nodos
analizados 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. Nodos
analizados 2.

Figura 3.6: Nodos ocupados, Nodos analizados y eliminacion de puntos de la

esfera.

3.1.4. Resultados

A continuacion se muestran resultados, en donde se vara la ubicacion del

obstaculo (esfera), orientacion del robot movil, alcance visual de las camaras,
tamano de la esfera, cantidad de nodos y distancia maxima de reconstruccion
del entorno. En la Figura 3.7(a), se modifica la ubicacion del obstaculo, de
tal forma que no se encuentra en el alcance visual del robot. Sin embargo, los
nodos estan bien etiquetados. En la Figura 3.7(b), se modifica la orientacion
del robot, y se mantiene la ubicacion del obstaculo.

(a) Modificacion de la ubicacion del obs- (b) Cambio de orientacion del robot
taculo. movil.

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 del
diametro de la esfera de 15 a 25 cm, de tal manera que la cantidad de nodos
ocupados aumenta, como se puede observar en la figura aumento de 6 a 8
nodos ocupados.

(a) Reduccion del campo visual de las (b) Aumento del diametro de la circun-
camaras web. ferencia.

Figura 3.8: Prueba 3 y 4.

En la Figura 3.9(a), se aumenta la cantidad de nodos, al disminuir el delta

de 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 la

obtencion de caminos suaves en el momento de rodear un obstaculo en la
necesidad de estar en una mejor posicion para adquirir informacion de zonas
no vistas. A continuacion se propone una metodologa en 2D (ejes x, y), para
la obtencion del planificador de trayectorias. En la Figura 3.10(a), se muestra
la simulacion de un ambiente en el cual el movil se encuentre en la posicion
(0,0). La circunferencia representa el obstaculo, en donde estan circunscritos
los puntos adquiridos del objeto, y por ultimo la posicion hacia donde nos
dirigimos. Como se puede observar no es posible llegar al objetivo por medio
de una lnea recta, de tal forma que se debe encontrar la manera de rodear la
circunferencia. 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 la
Ecuacion (3.2), que representa la ecuacion de la semirrecta, y despejar y de la
Ecuacion (3.3), que representa la ecuacion de la circunferencia. En donde m
es la pendiente, y los valores de x1 , y1 , determinan la posicion del movil, en
la Ecuacion (3.2). Y las variables a y b, son las coordenadas cartesianas que
representan la ubicacion del centro de la circunferencia, en la Ecuacion (3.3).
Y por ultimo se igualan las ecuaciones, el sistema resultante y los despejes
previos se resuelven en Matlab. Se elimina posteriormente la trayectoria que
se 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