Implementación en multiples´ plataformas de técnica esf ......Implementación en multiples´...

Implementacion enmultiples plataformas detecnica esferica de trazadode trayectorias homotopicas

libres de colision

Por:

Ing. Gerardo Ulises Dıaz Arango

Tesis sometida como requisito parcial para

obtener el grado de

MAESTRO EN CIENCIAS DE LA

ELECTRONICA

en el

Instituto Nacional de Astrofısica,

Optica y Electronica

Julio 2014

Tonantzintla, Puebla.

Supervisada por:

Dr. Luis Hernandez Martınez

Dr. Hector Vazquez Leal

c©INAOE 2014El autor otorga al INAOE el permiso de

reproducir y distribuir copias en su totalidad o

en partes de esta tesis

ii

RESUMEN

En este trabajo se presenta el desarrollo e implementacion de una metodo-

logıa para la planeacion de trayectorias libres de colisiones. Estas trayectorias

comunmente se utilizan para describir la ruta que debe seguir un robot movil

que realiza una tarea asignada. Nuestra metodologıa toma como bases el meto-

do de planeacion de trayectorias homotopicas (HPPM) y el algoritmo de segui-

miento esferico de trayectoria (SA). Se proponen dos complementos: un esquema

predictor-corrector y una tecnica para corregir efectos de reversion en la trayec-

toria. Combinando los metodos HPPM, SA, el esquema predictor-corrector y la

tecnica para corregir efecto de reversion se plantea una tecnica eficaz de planea-

cion de trayectorias libres de colisiones. Finalmente, resultados de simulacion en

multiples plataformas de PC e implementacion en un microcontrolador de 32 bits

validan la efectividad de la metodologıa.

i

ii

Abstract

In this work the development and implementation of a methodology for plan-

ning collision-free paths is presented. These paths are commonly used to describe

the route to be followed by a mobile robot that performs an assigned task. Our

methodology takes as its basis the homotopy path planning method (HPPM)

and the spherical tracking algorithm (SA). Two complements are proposed, a

predictor-corrector scheme and a technique for correcting reversal effects in the

path. Combining the methods HPPM, SA, the predictor-corrector scheme and

technique to correct reversal effect, is proposes a effective technical for planning

collision-free trajectories. Finally, simulation results of multiple PC platforms and

implementing a 32-bit microcontroller validate the effectiveness of the methodo-

logy.

iii

iv

Agradecimientos

Al Instituto Nacional de Astrofısica, Optica y Electronica todo el apoyo que

me brindo en mi formacion academica.

A los Doctores Luis Hernandez Martınez y Hector Vazquez Leal por haberme

aceptado como estudiante de maestrıa, ası mismo por todas las horas dedicadas

a mi formacion academica y en la direccion de la presente tesis.

A la doctora Marıa Teresa Sanz Pascual y los doctores Librado Arturo Sar-

miento Reyes, y Monico Linares Aranda por sus correcciones y sugerencias para

esta tesis.

Muchas gracias al CONACYT por la beca con la que me apoyaron economi-

camente para la realizacion de la tesis.

Y por supuesto a toda mi familia y amigos quienes siempre creyeron en mi.

v

vi

INDICE GENERAL

Resumen I

Indice general VIII

Indice de figuras X

Indice de tablas 1

1. Trazado de trayectorias 3

1.1. Introduccion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

1.2. Justificacion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

1.3. Objetivos y Metas . . . . . . . . . . . . . . . . . . . . . . . . . . 6

1.4. Conclusiones . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

2. Algoritmos de planeacion 9

2.1. Metodos de continuacion . . . . . . . . . . . . . . . . . . . . . . . 12

2.1.1. Homotopıa de Newton . . . . . . . . . . . . . . . . . . . . 14

2.2. HPPM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14

2.3. Seguimiento esferico de trayectoria . . . . . . . . . . . . . . . . . 19

2.4. Algoritmo predictor-corrector . . . . . . . . . . . . . . . . . . . . 21

2.4.1. Predictor de Euler . . . . . . . . . . . . . . . . . . . . . . 21

2.4.2. Metodo de Newton-Raphson . . . . . . . . . . . . . . . . . 24

2.5. Conclusiones

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25

vii

viii Indice general

3. Trazado de trayectorias libres de colisiones: Metodologıa 27

3.1. Trazado de trayectorias libres de colisiones . . . . . . . . . . . . . 27

3.1.1. Deteccion de efecto de reversion . . . . . . . . . . . . . . . 28

3.1.2. Tecnica para corregir efecto de reversion . . . . . . . . . . 30

3.1.3. Parametros de repulsion y pendientes de rectas auxiliares . 33

3.2. Tecnica de radio variable . . . . . . . . . . . . . . . . . . . . . . . 42

3.3. Implementacion en lenguaje C++ . . . . . . . . . . . . . . . . . . 45

3.4. Implementacion en microcontrolador . . . . . . . . . . . . . . . . 46

3.5. Conclusiones . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48

4. Casos de estudio 49

4.1. Caso de estudio 1 . . . . . . . . . . . . . . . . . . . . . . . . . . . 49

4.2. Caso de estudio 2 . . . . . . . . . . . . . . . . . . . . . . . . . . . 57

4.3. Caso de estudio 3 . . . . . . . . . . . . . . . . . . . . . . . . . . . 60

4.4. Discusion de resultados . . . . . . . . . . . . . . . . . . . . . . . . 63

4.5. Comparacion cualitativa con metodos existentes . . . . . . . . . . 66

4.6. Conclusiones . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67

5. Conclusiones 69

A. Apendice A 71

B. Apendice B 79

Referencias 81

INDICE DE FIGURAS

1.1. Principal tarea de un robot movil. . . . . . . . . . . . . . . . . . 4

2.1. Tarea principal del algoritmo de planeacion de trayectorias. . . . . 10

2.2. Representacion grafica de algunos de los principales algoritmos de

planeacion de trayectoria. . . . . . . . . . . . . . . . . . . . . . . 11

2.3. Representacion del algoritmo RRT. . . . . . . . . . . . . . . . . . 12

2.4. Familia de curvas homotopicas γ. . . . . . . . . . . . . . . . . . . 13

2.5. Representacion de obstaculos considerando el radio de un robot de

traccion diferencial. . . . . . . . . . . . . . . . . . . . . . . . . . . 16

2.6. Mapa de obstaculos en espacio normalizado bidimensional. . . . . 16

2.7. Curvas homotopicas resultantes en espacio bidimensional. . . . . 18

2.8. Representacion bidimensional de la interseccion entre la hiperesfera

y la curva homotopica. . . . . . . . . . . . . . . . . . . . . . . . . 19

2.9. Seguimiento esferico de una curva homotopica γ. . . . . . . . . . . 20

2.10. Esquema predictor-corrector. . . . . . . . . . . . . . . . . . . . . . 21

2.11. Corrector de N-R en el seguimiento esferico de trayectoria. . . . . 25

3.1. Efecto de reversion en el seguimiento: N-R converge a punto de

retroceso. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28

3.2. Tecnica de correccion de efecto de reversion en el seguimiento: N-R

converge a punto de avance. . . . . . . . . . . . . . . . . . . . . . 28

3.3. Estrategia para detectar reversiones en el seguimiento. . . . . . . 30

3.4. Estrategia para corregir reversiones en el seguimiento. . . . . . . 32

ix

x Indice de figuras

3.5. Representacion del mapa de entorno sin presencia de obstaculos. . 34

3.6. Trayectorias resultantes para valores de m1 y m2 cercanos a cero. 35

3.7. Trayectorias resultantes para valores de m1 y m2 lejanos a cero. . 36

3.8. Trayectorias resultantes de invertir las rectas auxiliares. . . . . . . 37

3.9. Curva homotopica para diferentes valores de parametro de repul-

sion p0 correspondiente al obstaculo circular C0. . . . . . . . . . . 39

3.10. Curva homotopica para diferentes valores de parametro de repul-

sion p0 correspondiente al obstaculo rectangular R0. . . . . . . . . 39

3.11. Trayectorias producidas por un cambio de signo en p0. . . . . . . 40

3.12. Trayectorias resultantes de variaciones en parametros de repulsion

y rectas auxiliares. . . . . . . . . . . . . . . . . . . . . . . . . . . 41

3.13. Trayectoria resultante de variaciones en parametros de repulsion. . 42

3.14. Deteccion de efecto de reversion durante el seguimiento de trayec-

toria. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43

3.15. Metodologıa propuesta para el trazado de trayectorias. . . . . . . 44

3.16. Representacion grafica de la implementacion en microcontrolador

del metodo de trazado de trayectorias. . . . . . . . . . . . . . . . 47

4.1. Representacion en el plano H1 de los primeros pasos del seguimiento

de trayectoria. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52

4.2. Representacion en el plano H1 del seguimiento de trayectoria. . . 54

4.3. Planeacion de trayectoria para tres obstaculos circulares. . . . . . 55

4.4. Interseccion entre el plano H1 y la superficie H2. . . . . . . . . . . 56

4.5. Planeacion de trayectorias en ambientes cerrados. . . . . . . . . . 57

4.6. Configuraciones de trayectorias libres de colisiones para un mismo

punto inicial y punto meta en ambientes cerrados. . . . . . . . . . 58

4.7. Trayectoria libre de colisiones resultante para un mapa de entorno

con 40 obstaculos circulares. . . . . . . . . . . . . . . . . . . . . . 61

4.8. Trayectoria libre de colisiones resultante para un mapa de entorno

con 200 obstaculos circulares probada en multiples plataformas. . 62

4.9. Tiempo total de calculo para cada mapa de obstaculos en las dife-

rentes plataformas. . . . . . . . . . . . . . . . . . . . . . . . . . . 63

4.10. Tiempo de calculo por iteracion de N-R para cada mapa de obstacu-

los. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65

INDICE DE TABLAS

3.1. Parametros utilizados en las simulaciones de las Figuras 3.7 y 3.6. 38

4.1. Parametros de simulacion caso de estudio 1. . . . . . . . . . . . . 50

4.2. Parametros de simulacion de mapas de obstaculos en ambientes

cerrados. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59

4.3. Parametros usados para obtener las trayectorias libres de colisiones

de los 20 mapas de entorno con obstaculos circulares. . . . . . . . 60

4.4. Comparacion cualitativa de las principales caracterısticas de los

metodos de planeacion de trayectorias libres de colision. . . . . . . 66

A.1. Metodo de Euler para localizar el vector predictor. . . . . . . . . . 72

A.2. Metodo de Newton-Raphson. . . . . . . . . . . . . . . . . . . . . 73

A.3. Tecnica utilizada para detectar efectos de reversion en la trayecto-

ria. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74

A.4. Tecnica utilizada para corregir efecto de reversion en el seguimiento

y mejorar la convergencia del metodo N-R. . . . . . . . . . . . . . 75

A.5. Metodo de radio variable. . . . . . . . . . . . . . . . . . . . . . . 76

A.6. Metodologıa para el trazado de trayectorias utilizando EHPPM y

algoritmo de seguimiento esferico. . . . . . . . . . . . . . . . . . 77

B.1. Principales caracterısticas de tarjeta de desarrollo ARM y PC uti-

lizada para realizar las simulaciones en las distintas plataformas. . 79

1

2 Indice de Tablas

CAPITULO 1

Trazado de trayectorias

1.1. Introduccion

En los ultimos anos, el campo de la robotica ha tenido importante actividad y

desarrollo en multiples aplicaciones. Siendo la robotica movil una de las vertien-

tes mas beneficiadas debido sus multiples campos de aplicacion. Una gran parte

de los avances realizados en esta rama de la robotica han sido motivados por el

uso de robots moviles en aplicaciones industriales, de exploracion, sistemas de

seguridad y uso domestico. Entre las mas importantes se encuentran: la explo-

racion espacial, el monitoreo de ductos petroleros y la distribucion de paquetes

en instalaciones industriales. Por lo general, los robots moviles realizan tareas en

ambientes hostiles o de difıcil acceso para el ser humano. Acorde a la tarea un

robot movil puede ser autonomo o semi autonomo dependiendo de la necesidad de

poca o ninguna intervencion humana. Todo robot con cierto grado de autonomıa

debe ser capaz de tomar decisiones y navegar en su entorno. Para que un robot

sea capaz de posicionarse y desplazarse de un punto A a un punto B sin colisio-

nar con obstaculos en su recorrido, requiere en su arquitectura de un modulo de

navegacion. El modulo de navegacion esta integrado principalmente por: a) Un

mapa de su entorno y b) Un algoritmo de planeacion de trayectoria [1]. La Figura

1.1 muestra el desplazamiento de un robot movil de un punto inicial a un punto

meta sin considerar objetos que obstruyan su trayecto.

El mapa de entorno, es la abstraccion de la vista superior del area de trabajo, esta

3

4 Capıtulo 1. Trazado de trayectorias

es proporcionada generalmente por camaras, sensores de radiacion y en algunos

casos sensores laser o sistemas sonar. Posteriormente, esta imagen es procesada pa-

ra crear una representacion bidimensional del entorno resaltando forma y tamano

de los objetos. Se debe aclarar que para esta tesis la parte del mapa no sera te-

ma de estudio, tomandolo como un entorno conocido a priori y a los obstaculos

como objetos estaticos o temporalmente estaticos [2, 3]. Teniendo a la planeacion

de ruta como el problema principal del modulo de navegacion. Un algoritmo de

planeacion es el encargado de trazar la ruta que un robot debe seguir para evitar

colisionar con algun obstaculo en su trayecto de un punto a otro.

En la literatura existe un gran numero de algoritmos utilizados para la planeacion

de trayectorias libres de colisiones. Entre ellos los mas utilizados son el algoritmo

Bug [4], el de campos potenciales artificiales [5, 6, 7], los basados en grafos [11] y

los de enfoque estocastico [8, 9, 10].

Figura 1.1: Principal tarea de un robot movil.

El algoritmo Bug, llamado ası por la similitud con el comportamiento de un

insecto, rodea todo obstaculo que se detecte en su recorrido al mismo tiempo que

trata de aproximarse a la posicion final [4]. El algoritmo de campos potenciales

artificiales modela el robot y los obstaculos como cargas electricas del mismo sig-

no y el punto meta como una carga de signo opuesto, generando de esta forma

un efecto de repulsion por parte de los obstaculos y una fuerte atraccion hacia el

punto meta [5, 6, 7]. Los algoritmos de grafos de visibilidad, modelado del espacio

libre y descomposicion en celdas, crean una representacion grafica del entorno y

trazan una ruta en el espacio libre de obstaculos. Desafortunadamente, el tiempo

de computo y complejidad de estos algoritmos se relaciona exponencialmente con

el numero de obstaculos. Por otro lado, los algoritmos con enfoque estocastico se

distinguen por rapidez de ejecucion y sencillez en implementacion, entre los mas

1.1. Introduccion 5

importantes se encuentran: los algoritmos de arboles aleatorios de exploracion

rapida (RRT: Rapidly Exploring Random Tree), mapas probabilısticos (PRM:,

Probabilistic Road Map Method), y el de planeacion aleatoria de trayectorias

(RPP: Randomized Path Planner) [8, 9]. El RRT en su version RRT-Connect, ha

probado ser muy rapido en obtener una serie de configuraciones con trayectorias

libres de colisiones [10]. Sin embargo, requiere de dos herramientas adicionales:

el algoritmo de Dijkstra y un algoritmo de pos procesado, los cuales incrementan

el tiempo de computo total. Los algoritmos basados en grafos, los de enfoque es-

tocastico, el algoritmo insecto y el de campos potenciales artificiales han probado

ser de utilidad en la planeacion de trayectorias, sin embargo no siempre son exi-

tosos.

Recientemente se han reportado trabajos en los cuales se utilizan metodos de conti-

nuacion aplicados a planeacion de trayectorias (HPPM: Homotopy Path Planning

Method). El funcionamiento del HPPM es similar al del algoritmo de campos po-

tenciales artificiales, ya que presenta un tipo de efecto de repulsion por parte de

los obstaculos y una fuerte atraccion por el punto meta. Sin embargo, este no tiene

las desventajas del algoritmo de campos potenciales. En este trabajo de tesis uti-

lizaremos la version mejorada del metodo de continuacion aplicado a planeacion

de trayectorias (EHPPM: Enhanced version of HPPM) [1].


1.2. Justificacion

Todo robot movil autonomo o semi autonomo, requiere en su arquitectura un

modulo de navegacion con un apropiado metodo de planeacion de trayectoria. La

complejidad de los metodos y algoritmos existentes crece exponencialmente cuan-

do el numero de obstaculos se incrementa [4, 6, 7, 8, 9, 11]. Por otro lado, estos

algoritmos no garantizan encontrar siempre una ruta libre de colisiones. Desarro-

llar una adecuada tecnica de planeacion de trayectorias es de vital importancia

para la comunidad cientıfica, desarrolladores y usuarios de robots moviles. Se com-

prueba la eficacia del EHPPM para resolver una de las problematicas de mayor

interes en el area de la robotica movil.

1.3. Objetivos y Metas

Objetivo general

Implementar una metodologıa para el trazado de trayectorias libres de colisio-

nes con base en un metodo de continuacion homotopica y seguimiento esferico,

para ser aplicada dentro del modulo de navegacion de un robot movil.

Objetivos especıficos

Utilizar herramientas de software matematico para probar la eficacia del

metodo de planeacion de trayectoria.

Desarrollar una tecnica apropiada anti reversion en el seguimiento de tra-

yectoria homotopica.

Desarrollar una herramienta de software en lenguaje C++ capaz de localizar

una trayectoria libre de colisiones, para ser usada en diferentes plataformas

roboticas.

Implementar la metodologıa en un microcontrolador para probar su versa-

tilidad y sencillez en aplicaciones de robots pequenos.

1.4. Conclusiones 7

Metas

En este trabajo de investigacion, se desarrollara una metodologıa eficiente

de trazado de trayectorias libres de colisiones capaz de corregir efectos de

reversion en el seguimiento y encontrar una ruta exitosa.

Se validara la eficacia de la tecnica reportando tiempos de computo y numero

de iteraciones obtenidos de una serie de simulaciones en Maple 13 y lenguaje

C++.

Se probara el alcance de esta metodologıa en aplicaciones de robots moviles

pequenos mediante pruebas realizadas en una tarjeta de desarrollo con un

microcontrolador de 32 bits.

En el capıtulo 2 se realiza una breve revision de los metodos existentes mas rele-

vantes de planeacion de trayectorias. Ası como, una descripcion a detalle del meto-

do EHPPM, el algoritmo predictor-corrector y la tecnica de seguimiento esferico.

En el capıtulo 3 se muestran los principales aportes de esta investigacion; una

tecnica capaz de corregir el efecto de reversion en el seguimiento, implementacion

de la metodologıa en Maple, C++ y en un microcontrolador ARM de 32 bits.

Posteriormente, en el capıtulo 4 se presentan y discuten los resultados de una se-

rie de casos de estudio. Finalmente, en el capıtulo 5 se presentan las conclusiones

y el trabajo futuro.

1.4. Conclusiones

Los algoritmos de planeacion de trayectoria son indispensables para la co-

munidad de desarrolladores e investigadores en robotica movil. Existen multiples

algoritmos con diversos enfoques capaces de realizar esta tarea. Sin embargo, es-

tos suelen ser de alto costo computacional y no garantizan localizar trayectorias

exitosas. Por lo tanto una herramienta apropiada de trazado de trayectorias es de

vital importancia. En esta tesis se propone el uso de un metodo de continuacion

trabajando en conjunto con el algoritmo de seguimiento esferico y un esquema

predictor-corrector para formular una metodologıa eficaz de trazado de trayecto-

rias.


CAPITULO 2

Algoritmos de planeacion de trayectorias

En el presente capıtulo se describen brevemente los algoritmos de planeacion

de trayectoria existentes mas importantes, de entre ellos los mas sobresalientes

por su desempeno; el RRT en su version RRT-Connect y el algoritmo de campos

potenciales artificiales. Posteriormente, se da una breve introduccion a los metodos

de continuacion homotopica, su aplicacion en planeacion de trayectorias HPPM

y su version mejorada EHPPM. Finalmente, se presenta el algoritmo esferico y el

esquema predictor-corrector utilizados en el seguimiento de trayectorias.

El algoritmo de planeacion es el encargado de trazar la ruta que el robot movil

autonomo debe seguir para llegar de un punto a otro evitando colisiones con

obstaculos en su recorrido. Esta ruta es una curva descrita por una coleccion

de puntos en un mapa bidimensional. En la Figura 2.1a se muestra el problema

principal de los robots moviles al toparse con un obstaculo en su recorrido, y en la

Figura 2.1b se muestra la ruta solucion entregada por un algoritmo de planeacion

de trayectoria.

Existen un gran numero de algoritmos de planeacion, sin embargo, en esta sec-

cion solo se describiran los que han destacado por: eficacia, velocidad en ejecucion

o sencillez al programarlo. En primer lugar, tenemos el algoritmo insecto (Bug),

un algoritmo sencillo que requiere unas pocas lıneas de codigo para su implemen-

tacion. Este algoritmo es llamado ası por su semejanza con el comportamiento de

un insecto. Este algoritmo le permite al robot rodear el contorno del obstaculo

considerando una distancia de guarda, al mismo tiempo que trata de aproximarse

a la posicion final [4]. Sin embargo, se tiene la desventaja de requerir un sistema

9

10 Capıtulo 2. Algoritmos de planeacion

de sensores que le permitan conocer su posicion en todo momento, ası como tener

una percepcion de su entorno. Ademas, las rutas trazadas son ineficientes y no

siempre exitosas. En la Figura 2.2a se representa el funcionamiento principal de

este algoritmo al detectar un obstaculo en su camino.

(a) (b)

Figura 2.1: Tarea principal del algoritmo de planeacion de trayectorias.

El algoritmo de campos potenciales artificiales modela el robot y los obstaculos

como cargas electricas del mismo signo y el punto meta como una carga de signo

opuesto. Entonces, el comportamiento del robot presenta un efecto de repulsion

por parte de los obstaculos y una fuerte atraccion hacia el punto meta [5, 12, 7, 6].

A pesar de la rapidez en ejecucion este algoritmo puede fallar, ya que existe la

posibilidad de quedar atrapado en un mınimo local producido por obstaculos

vecinos muy cercanos. En la Figura 2.2b se muestra la representacion de este

algoritmo, ilustrando con flechas el efecto de repulsion o atraccion por parte de

los objetos y el punto meta hacia el robot.

En la categorıa de algoritmos basados en grafos los mas relevantes son: los de

visibilidad y descomposicion en celdas. El metodo de grafos de visibilidad consiste

en representar cada uno de los obstaculos del mapa de entorno como polıgonos. Y

se trazan todas las posibles combinaciones de conexiones entre el punto inicial y el

final con los vertices de los obstaculos y selecciona la ruta mas corta. El algoritmo

de descomposicion en celdas discretiza el espacio de trabajo en pequenas celdas

de ocupacion y traza la ruta a traves de celdas libres. Sin embargo, para ambos

la complejidad incrementa exponencialmente con el numero de obstaculos en el

mapa de entorno y en consecuencia el tiempo de computo. En las Figuras 2.2c y

11

2.2d se ilustra el funcionamiento de estos algoritmos.

(a) Algoritmo insecto. (b) Campos potenciales artificiales.

(c) Graficos de visibilidad. (d) Descomposicion en celdas.

Figura 2.2: Representacion grafica de algunos de los principales algoritmos deplaneacion de trayectoria.

En la categorıa de algoritmos con enfoque estocastico se tiene: el de arboles

de exploracion rapida (RRT), el basado en mapas probabilısticos (PRM) y el de

planeacion aleatoria de trayectorias (RPP). El algoritmos RRT en su version RRT-

Connect ha probado ser muy rapido en proporcionar una trayectoria de un punto

inicial qinit a un punto meta qgoal [8]. El funcionamiento del algoritmo RRT basico,

consiste en crear una distribucion aleatoria de puntos en el espacio sin ocupacion

de obstaculos. El algoritmo comienza con un punto inicial qinit que es agregado al

arbol vacio T . Para despues seleccionar un punto aleatorio del espacio libre qrand y

localizar el nodo mas cercano a este llamado qnear. Enseguida se calcula una nueva

configuracion qnew con un tamano de paso ε en direccion a qrand desde qnear. Si

no existe colision, se agrega el nodo qnew al arbol T , de esta manera el proceso se


repite k veces haciendo crecer el arbol de configuraciones T [9]. La representacion

del RRT basico se muestra en la Figura 2.3a. El algoritmo RRT-Connect es la

variante mas confiable del algoritmo base, consiste en crear un arbol Ta desde el

punto de inicio qinit y un arbol Tb desde el punto meta qgoal [10]. Ambos arboles

crecen simultaneamente, uno hacia el otro mejorando la convergencia. En la figura

2.3b se presenta el funcionamiento del algoritmo RRT-Connect. Una vez obtenido

el arbol T se debe aplicar el algoritmo de Dijkstra para construir un grafo con la

ruta mas corta.

(a) RRT forma basica. (b) RRT-Connect.

Figura 2.3: Representacion del algoritmo RRT.

Desafortunadamente, al igual que los metodos anteriores el algoritmo RRT no

tiene la certeza de encontrar una trayectoria exitosa. Al igual que los algoritmos de

grafos de visibilidad y descomposicion en celdas este produce trayectorias lineales

a trozos y por lo tanto, requiere de un algoritmo de pos procesado encargado de

suavizar la ruta considerando las caracterısticas holonomicas del robot.

2.1. Metodos de continuacion homotopica.

Los metodos de continuacion homotopica (HCM) son comunmente utilizados

para encontrar multiples soluciones de sistemas algebraicos no lineales (NAES,

por sus siglas en ingles, Nonlinear Algebraic Equations System) en los cuales los

metodos convencionales como el de Newton-Raphson suelen fallar. Estos sistemas

NAES son expresiones de la forma:

f(x) = 0 : Rn −→ Rn, (2.1)

2.1. Metodos de continuacion 13

Los metodos de homotopıa crean una continua perturbacion en el sistema f(x),

anadiendo una funcion G(x) y un parametro λ, llamado parametro homotopico,

convirtiendo al sistema original en un conjunto de ecuaciones de la forma:

H(x, λ) = λf(x) + (1− λ)G(x) = 0, (2.2)

donde H (f(x)) = H(x, λ) : Rn+1 −→ Rn, x ∈ Rn , λ ∈ [0, 1].

La funcion G(x) tiene soluciones conocidas o triviales de tal forma que:

Para λ = 0, H(x, λ) = 0 se tienen soluciones conocidas o triviales.

Para λ = 1, H(x, λ) = f(x) entonces la solucion del sistema original es

encontrada.

Los HCM generan un conjunto de soluciones de la forma H−1(0) para el

sistema f(x) = 0.

Entonces el conjunto H−1(0), generado por la continua perturbacion desde

λ = 0 hasta λ = 1 se representa con una familia de curvas γ [1, 13, 14, 23,

24, 25, 26, 27].

Figura 2.4: Familia de curvas homotopicas γ.

La Figura 2.4 muestra una familia de curvas γ producidas por la continua

perturbacion de la homotopıa en los sistema de ecuaciones no lineales. La curva

γ1 representa una secuencia de puntos solucion desde λ = 0 hasta λ = 1 [1,


15, 16, 17, 18, 19]. Por otra parte, γ2 representa una curva cerrada la cual no

contiene ninguna solucion del sistema original ya que no intersecta con λ = 1,

y γ3 es una curva que diverge al infinito sin intersectar con λ = 1, por lo tanto

tampoco contiene una solucion del sistema de NAES. En la Figura 2.4 tambien se

puede observar la presencia de puntos en los cuales las curvas sufren un cambio

abrupto de direccion, llamados puntos de retorno. Comunmente los metodos de

continuacion son utilizados para encontrar los puntos de interseccion con λ = 1,

es decir los puntos solucion del sistema original. Para las aplicaciones en las que

solo se desea conocer todas las soluciones de un NAES no interesa si estos puntos

pertenecen a una o varias curvas γ. Sin embargo, para este trabajo la atencion se

centra en las curvas trazadas por el HCM.

2.1.1. Metodo de homotopıa de Newton

La funcion G(x) determina el tipo de homotopıa que se utiliza. En este trabajo

se utiliza la homotopıa de Newton , para la cual se define la funcion G(x) =

f(x)− f(x0) [1, 18, 19, 25, 27]. Substituyendo G(x) en (2.2)

H(f(x), λ) = f(x)− (1− λ)f(x0) = 0; x ∈ Rn, (2.3)

donde x0 is el punto inicial. Entonces, si λ = 0 el sistema homotopico es reducido

a un sistema con solucion trivial H(f(x), 0) = f(x)− f(x0) = 0, y para λ = 1 se

tienen las soluciones del sistema original H(f(x), 1) = f(x) = 0.

2.2. Metodo de continuacion aplicado a planea-

cion de trayectoria

El metodo homotopico de planeacion de trayectoria (HPPM) se basa en un

tipo de efecto de repulsion por parte de los obstaculos y atraccion hacia el punto

meta muy semejante al algoritmo de campos potenciales artificiales. Sin embargo,

se tiene la gran diferencia que el algoritmo modela un efecto fısico, mientras que el

HPPM considera el contorno de los obstaculos como singularidades en el sistema

de ecuaciones homotopicas H(f(x), λ) causando un tipo de efecto de repulsion.

El HPPM permite obtener un conjunto de soluciones H−1(0) y generar curvas

continuas desde un punto inicial hasta un punto meta evadiendo singularidades

[1]. En este trabajo se utiliza una variante del metodo de continuacion aplicado

2.2. HPPM 15

a planeacion de trayectorias el EHPPM. Este permite tener un mayor control del

efecto de repulsion generado por las singularidades en el robot.

Considerando que los robots solo se pueden desplazar en un entorno bidimensional

x− y, el EHPPM representa al conjunto de obstaculos de la siguiente manera:

W (x, y) =i=k∑

i=1

pi

Obi(x, y), (2.4)

donde pi es el parametro que regula el efecto de repulsion entre el robot y el i-esimo

obstaculo, Obi(x, y) es la ecuacion que define cada obstaculo [1]. Este metodo solo

esta definido para objetos representados por circunferencias y aproximaciones de

rectangulos. Los objetos circulares son expresados de la forma:

Ci(x, y) = (x− xi)2 + (y − yi)

2 − rc2i = 0, (2.5)

donde rci es el radio de la circunferencia y las coordenadas (xi, yi) son el centro

de esta. Los obstaculos rectangulares son presentados como aproximaciones de

elipses de la siguiente manera:

Ri(x, y) =

(x− xiα

)2η+

(y − yiβ

)2η− 1 = 0, (2.6)

donde η es el parametro que define los que tan rectos son los vertices del rectangu-

lo. Para valores grandes de η se tienen vertices formados por curvas pronunciadas

y para valores pequenos se tienen vertices formados por curvas mas suaves. De

forma practica para para este trabajo de tesis, se usa η = 2. El punto (xi, yi)

representa su centro, 2α y 2β son la altura y la base respectivos del rectangulo.

Ademas, para impedir que el robot entre en contacto con la superficie de los

obstaculos se necesita considerar las dimensiones y restricciones de este[1]. Por

ejemplo, considerando las restricciones holonomicas de un robot con traccion di-

ferencial, Para los cuales, se cumple que el eje de rotacion es el punto intermedio

entre las ruedas de traccion. Con esto en cuenta, se debe modificar el mapa de

entorno realizando un incremento en el contorno de cada obstaculo de acuerdo al

radio del robot. Esto se puede comprender mejor observando la Figura 2.5.


Figura 2.5: Representacion de obstaculos considerando el radio de un robot detraccion diferencial.

La Figura 2.5 muestra el nuevo contorno de los obstaculos para un robot cuyo

radio es rbot, donde la lınea punteada representa el tamano original de los obstacu-

los y la lınea solida el tamano incrementado. Adicionalmente, se estandariza un

espacio normalizado de tal forma que colocamos el punto meta B en las coorde-

nadas (1, 1) y el punto inicial en (0, 0), en consecuencia los obstaculos deberan

localizarse en el espacio x ∈ [0, 1], y ∈ [0, 1] como se ejemplifica en la Figura 2.6a.

B [1]. Por otro lado, el EHPPM necesita un sistema de dos ecuaciones auxiliares

con una solucion en el punto meta B. Estas ecuaciones son de la forma

f1(x, y) = 0, x, y ∈ R, (2.7)

f2(x, y) = 0, x, y ∈ R, (2.8)

donde f1(1, 1) = 0 y f2(1, 1) = 0.

(a) (b)

Figura 2.6: Mapa de obstaculos en espacio normalizado bidimensional.@

2.2. HPPM 17

En esta tesis se proponen ecuaciones de lıneas rectas de la forma:

L1(x, y) = −y −m1x+ (b+m1a) = 0, (2.9)

L2(x, y) = −y −m2x+ (b+m2a) = 0, (2.10)

donde m1 y m2 son los parametros que determinan las pendientes respectivas a

cada lınea, (a, b) las coordenadas del punto de interseccion entre las lıneas auxi-

liares, es decir, el punto meta B. Por lo tanto, el punto solucion de este sistema

de ecuaciones se localiza en el punto meta B como se puede observar en la Figure

2.6b.

El EHPPM agrega las expresiones de los obstaculos a la ecuacion L2 convirtiendo

el sistema de ecuaciones en:

f1(x, y) = L1(x, y) = 0, (2.11)

f2(x, y) = L2(x, y) +W (x, y)−Q = 0, (2.12)

donde Q = W (a, b). El termino W (x, y) en (2.12), provoca que la expresion sea

singular en puntos del contorno de cada obstaculo. Teniendo en cuenta que ningun

punto del conjunto H−1(0) puede ser solucion de las expresiones Obi. Mientras

que, el termino Q desaparece el efecto de los parametros de repulsion pi en el

punto B. Aplicando (2.3) en (2.11) and (2.12), se tiene el sistema de ecuaciones

H =

H1(f1(x, y), λ) = f1(x, y)− (1− λ)f1(x0, y0) = 0,

H2(f2(x, y), λ) = f2(x, y)− (1− λ)f2(x0, y0) = 0,(2.13)

donde (x0, y0) es el punto inicial A.


(a) No existe presencia de obstaculos. (b) Obstaculos presentes.

(c) Obstaculos presentes.

Figura 2.7: Curvas homotopicas resultantes en espacio bidimensional.

La Figura 2.7 muestra en lınea solida el contorno de los obstaculos y en puntea-

da la familia de curvas γ. La curva γ1 en la Figura 2.7b representa una trayectoria

directa desde A a B sin efecto a causa de presencia de obstaculos, las curvas γ2,

γ3, γ4 y γ5 son curvas cerradas que no intersectan con el punto meta. En general,

para EHPPM solo esta definida una curva continua desde el punto inicial a un

punto objetivo. La curva exitosa libre de colisiones en un entorno con presencia

2.3. Seguimiento esferico de trayectoria 19

de obstaculos puede ser formada por la combinacion de dos o mas curvas γ, esto

acorde con los valores de los parametros de repulsion, ası como la proximidad

entre los obstaculos y la trayectoria directa. En la Figura 2.7c se muestra una tra-

yectoria directa formada por la combinacion de la trayectoria directa y las curvas

externas de los obstaculos en el mapa.

Debido a la existencia de mas de una curva γ es necesaria una tecnica apropiada

de seguimiento, para evitar perder la trayectoria solucion o caer en curvas cerradas

a casusa de saltos [1, 17, 21, 22, 29].

2.3. Seguimiento esferico de trayectoria

El HPPM requiere de una apropiada tecnica de seguimiento que evite perder

la trayectoria exitosa o caer en curvas cerradas. Aquı utilizaremos el algoritmo de

seguimiento hiperesferico (SA: Sperical Algorithm). El SA es propuesta como una

herramienta con una clara interpretacion y facil de programar en [21] . Este utiliza

esferas de n dimensiones para evitar perder la curva γ. El principio fundamental

del SA es colocar una esfera Si de radio r con centro localizado sobre la curva

homotopica en Oi, entonces el contorno de esta intersectara a la curva γ cuando

menos en dos puntos (Oi−1 y Oi+1) como se muestra en la Figura 2.8.

La formulacion de esta tecnica agrega una ecuacion mas a H, obteniendo ası un

sistema con n+ 1 ecuaciones y variables [21, 22, 23, 27].

Figura 2.8: Representacion bidimensional de la interseccion entre la hiperesferay la curva homotopica.

En el caso de estudio n = 2, debido a que el desplazamiento solo ocurre en

dimensiones de x − y. Tomando n = 2 y agregando la dimension del parametro


λ, se obtiene una version esferica del seguimiento. Cada esfera Si del seguimiento

es representada por:

Si(x, y, λ) = (x− cx)2 + (y − cy)

2 + (λ− cλ)2 − r2 = 0, (2.14)

donde (cx, cy, cλ) es el centro de la esfera y r el radio.

Usando (2.13) y (2.14) podemos obtener el sistema aumentado

HS =

H1(x, y, λ) = 0,

H2(x, y, λ) = 0,

Si(x, y, λ) = 0.

(2.15)

Este metodo requiere que la expresion Si de la esfera sea actualizado en cada

iteracion del seguimiento como se muestra en la Figura 2.9.

Figura 2.9: Seguimiento esferico de una curva homotopica γ.

El seguimiento de trayectoria no es una tarea facil ya que se puede perder la

curva debido a puntos de retorno o saltos hacia curvas cerradas. Por lo tanto, el

SA requiere como complemento un esquema predictor-corrector para formar una

eficaz herramienta de seguimiento.

2.4. Algoritmo predictor-corrector 21

2.4. Algoritmo predictor-corrector

El algoritmo predictor-corrector es un importante complemento del metodo

de seguimiento esferico de trayectoria. La Figura 2.10 describe el funcionamiento

del esquema predictor corrector aplicado en el seguimiento esferico. Para este

trabajo, se elige el esquema predictor de Euler el cual traza un vector tangente

~vp al punto (xi, yi, λi) de la curva homotopica y posteriormente, localiza el punto

predictor (xp, yp, λp) en la interseccion entre la esfera Si y el vector tangente

[20, 23]. Subsecuentemente, el metodo de Newton- Raphson (N-R) es utilizado

como corrector para encontrar el punto (xi+1, yi+1, λi+1) de interseccion entre la

esfera y la curva γ.

Figura 2.10: Esquema predictor-corrector.

2.4.1. Predictor de Euler

El algoritmo predictor Euler se utiliza como complemento de una apropiada

tecnica de seguimiento de trayectoria homotopica ya que nos proporciona una

solucion cercana a la solucion del sistema de ecuaciones homotopicas definidas

en cada paso del seguimiento. Los trabajos reportados [20, 23, 25] verifican la

efectividad de este algoritmo en combinacion con el seguimiento esferico para

trazar curvas homotopicas. la idea fundamental para encontrar el punto predictor

utilizando SA es expresada como:

(xp, yp, λp) = (cx, cy, cλ) + r‖~vp‖, (2.16)

donde (xp, yp, λp) es el punto predictor, (cx, cy, cλ) es el centro de la esfera, r

es el radio y ~vp es el vector tangente del i-esimo punto (xi, yi, λi) de la curva


homotopica γ. El proceso para obtener el vector tangente es formulado con base

en los trabajos [31, 28]. En este proceso, se propone la dependencia de cada una

de las variables respecto a un parametro arbitrario ρ , es decir, (x(ρ), y(ρ), λ(ρ)).

Entonces el sistema de ecuaciones homotopicas se reescribe como:

H(x(ρ), y(ρ), λ(ρ)) =

H1(x(ρ), y(ρ), λ(ρ)) = 0,

H2(x(ρ), y(ρ), λ(ρ)) = 0,(2.17)

se utiliza la regla de la cadena para derivar el sistema con respecto a ρ

∂H(x(ρ), y(ρ), λ(ρ))

∂ρ=∂H(x(ρ), y(ρ), λ(ρ))

∂(x(ρ), y(ρ), λ(ρ))

∂(x(ρ), y(ρ), λ(ρ))

∂ρ= 0, (2.18)

representando en forma vectorial se tiene:

∂H1∂x

∂H1∂y

∂H1∂λ

∂H2∂x

∂H2∂y

∂H2∂λ

∂x∂ρ

∂y∂ρ

∂λ∂ρ

=

(0

0

)

, (2.19)

donde el vector de derivadas parciales ∂(x(ρ), y(ρ), λ(ρ)))/∂ρ representa el vector

tangente ~vp. Reordenando los terminos se tiene:

∂H1∂x

∂H1∂y

∂H2∂x

∂H2∂y

∂x∂ρ

∂y∂ρ

=

∂λ

∂ρ

∂H1∂λ

∂H2∂λ

, (2.20)

con el fin de obtener la solucion de este sistema es necesario conocer el valor de

∂λ(ρ)/∂ρ. En [31] se propone:

∂λ

∂ρ= sg

(−1)n+1 det

∂H1∂x

∂H1∂y

∂H2∂x

∂H2∂y

, (2.21)

donde n = 2 y sg es un parametro que solo puede tomar el valor de +1 o −1. El

2.4. Algoritmo predictor-corrector 23

parametro Sg determina la direccion del punto predictor en direccion de avance

o retroceso del seguimiento. Este valor se encuentra correlacionado con las pen-

dientes de las rectas auxiliares. Para L1 y L2 tales que m1 6= m2 y se sabe de las

expresiones (2.11), (2.12) y (2.13) la relacion entre las expresiones del sistema H

con m1 y m2. A manera de explicar el efecto se propone m1 = 1 y m2 = 2. Por

propiedades de los determinantes se tiene:

det

∂H1∂x

∂H1∂y

∂H2∂x

∂H2∂y

= δ1 − δ2, (2.22)

δ1 =(∂H1∂x

) (∂H2∂y

),

δ2 =(∂H1∂y

) (∂H2∂x

).

Se hace la suposicion δ1 > δ2 en (2.22), entonces el signo del determinante

sera positivo. Por otro lado si se intercambian los valores de las pendientes, de tal

forma que m1 = 2 y m2 = 1 es similar a intercambiar los renglones en la matriz

del determinante de la siguiente manera:

det

∂H2∂x

∂H2∂y

∂H1∂x

∂H1∂y

= δ2 − δ1, (2.23)

recordando la suposicion anterior δ1 > δ2 en (2.23), en consecuencia el signo del

determinante es negativo. Se debe notar que δ1 > δ2 se utiliza solo para explicar

el cambio de signo en el determinante de la matriz relacionado con el valor de

m1 y m2. En este trabajo hemos realizado las suposiciones δ1 > δ2 y δ1 < δ2 , y

establecido la siguiente regla.

Para m1 > m2 entonces sg = −1.

para m2 > m1 entonces sg = +1.


La expresion (2.20) puede ser expresada como

∂x∂ρ

∂y∂ρ

=

∂H1∂x

∂H1∂y

∂H2∂x

∂H2∂y

−1

∂H1∂λ∂λ∂ρ

∂H2∂λ∂λ∂ρ

, (2.24)

reemplazando la expresion del vector tangente en la ecuacion (2.16) se obtiene

(xp, yp, λp) = (cx, cy, cλ) + r

∥∥∥∥∂(x(ρ), y(ρ), λ(ρ))

∂ρ

∥∥∥∥ , (2.25)

donde (xp, yp, λp) es el punto predictor, (cx, cy, cλ) es el centro de la i-esima esfera,

r es el radio de la esfera y ∂(x(ρ),y(ρ),λ(ρ))∂ρ

representa el vector tangente.

2.4.2. Metodo de Newton-Raphson

El metodo de Newton-Raphson N-R comunmente es utilizado para resolver

NAES de la forma (2.1) [29, 30], no obstante para la tesis se utiliza como metodo

de correccion para el sistema de ecuaciones HS al igual que en los trabajos [1, 22,

23, 25, 31]. Este es un proceso iterativo de convergencia local que utiliza un valor

inicial para acercarse a la solucion mediante aproximaciones de rectas tangentes

[30]. En el seguimiento esferico propuesto se utiliza el punto predictor de Euler

como punto inicial. El metodo N-R utilizado en seguimiento esferico de trayectoria

es descrito por la expresion:

(xj+1, yj+1, λj+1) = (xj, yj, λj)− [J (xj, yj, λj)]−1HS (xj, yj, λj) , (2.26)

donde j = 0, 1, 2, ..., (xj, yj, λj) son el conjunto de valores que se deben ac-

tualizar en cada iteracion hasta llegar a un valor final o solucion del sistema.

[J(xj, yj, λj)]−1 representa el Jacobiano inverso del sistema HS, HS(xj , yj, λj) es

el sistema evaluado en el punto (xj, yj, λj) para cada j-esima iteracion.

El metodo de N-R itera desde j = 0 donde (xj, yj, λj) = (xp, yp, λp), hasta llegar

a un maximo numero de iteraciones jmax o converger al punto (xi+1, yi+1, λi+1)

cumpliendo un mınimo margen de error como se muestra en la Figura 2.11. Es

decir, el punto final o solucion en este metodo iterativo es conseguido cuando se

cumple un apropiado criterio de convergencia o se realizan un maximo numero de

2.5. Conclusiones25

iteraciones. El criterio de convergencia es dado por la expresion:

‖ HS (xj+1, yj+1, λj+1) ‖ < err, (2.27)

en este trabajo se propone err = 1× 10−6 y jmax = 40.

Figura 2.11: Corrector de N-R en el seguimiento esferico de trayectoria.

La Figura 2.11 muestra la representacion grafica del metodo N-R aplicado al

seguimiento esferico de trayectoria. Este metodo toma como valor inicial el punto

(xp, yp, λp) para localizar el punto de interseccion entre la curva γ con la i-esima

esfera Si del seguimiento. En esta misma Figura 2.11 se muestra un ejemplo para el

cual el esquema corrector de N-R localiza el punto de interseccion (xi+1, yi+1, λi+1)

despues de 4 iteraciones.

2.5. Conclusiones

El metodo EHPPM es una variante de los metodos de continuacion que permite

planear trayectorias libres de colisiones, definido solo para obstaculos representa-

dos por circunferencias y aproximaciones rectangulares. Comunmente los metodos

homotopicos solo son de importancia los puntos que intersectan con λ = 1 sin im-

portar la trayectoria seguida, pero para esta variante de los HCM es de vital

importancia la curva γ. Por esta razon, es necesaria una apropiada tecnica de

seguimiento que evite extraviar el camino de soluciones, ası como, evitar saltos

hacia curvas cerradas o divergentes. La herramienta que es capaz de realizar esta

difıcil tarea, es el algoritmo de seguimiento esferico complementado con el esquema

predictor de Euler.


CAPITULO 3

Trazado de trayectorias libres de

colisiones: Metodologıa

En este capıtulo se presenta la metodologıa de trazado de trayectorias con

seguimiento esferico su propuesta y su implementacion en multiples plataformas

de software. Primero, se explica el efecto de reversion presente en el seguimiento

de trayectorias cuando se utiliza SA y los problemas que ocasiona en el trazado.

Se presentan las estrategias propuestas para detectar y corregir este efecto. En

seguida, se presenta el algoritmo disenado en Maple 13 de la metodologıa con el

cual se llevo a cabo el estudio de los parametros que intervienen en el trazado.

Por ultimo, se presenta la implementacion de la metodologıa en lenguaje C++ y

en un sistema embebido.

3.1. Trazado de trayectorias libres de colisiones

Como se menciono en el capıtulo anterior, el algoritmo esferico es una tecnica

apropiada de seguimiento de curvas homotopicas. La efectividad de este algoritmo

es debida a la interseccion de cada esfera con la curva en al menos dos puntos

durante el seguimiento. Para cada esfera existe un punto en direccion de avance

y el otro en direccion de retroceso. Sin embargo, la caracterıstica principal de la

tecnica tambien resulta ser el problema principal. Cuando el esquema corrector

utilizado converge al punto en direccion de retroceso se dice que la tecnica de

27

28 Capıtulo 3. Trazado de trayectorias libres de colisiones: Metodologıa

seguimiento presenta un efecto de reversion, tal efecto se puede observar en la

Figura 3.1.

Figura 3.1: Efecto de reversion en el seguimiento: N-R converge a punto de retro-ceso.

Por lo tanto, se propone una estrategia basada en una rotacion en 2-D capaz

de revertir el efecto de reversion durante el seguimiento, como se muestra en la

Figura 3.2. En esta tesis se diseno un algoritmo para el trazado de trayectorias

libres de colisiones en Maple 13. Este algoritmo es utilizado para probar la efec-

tividad de la estrategia para correccion de reversiones, ası como para estudiar el

efecto de los parametros involucrados en la metodologıa de trazado.

Figura 3.2: Tecnica de correccion de efecto de reversion en el seguimiento: N-Rconverge a punto de avance.

3.1.1. Deteccion de efecto de reversion

Durante el seguimiento de trayectoria cada esfera Si intersecta cuando menos

en dos puntos de la trayectoria homotopica, uno en direccion de avance (Oi+1 )

y el otro en direccion de retroceso (Oi−1). Sin embargo las no linealidades en el

3.1. Trazado de trayectorias libres de colisiones 29

sistema de ecuaciones H pueden provocar que la tecnica de correccion de N-R

converja al punto Oi−1 en vez de al punto Oi + 1. Cuando el metodo converge a

un punto en direccion de retroceso se dice que la tecnica de seguimiento presenta

un efecto de reversion [21, 22]. Estas reversiones en el seguimiento ocasionan un

trazado ineficiente o erroneo y de esta manera no se llega al punto meta. Por lo

tanto, es de vital importancia una estrategia para detectar y corregir reversiones

en el seguimiento. Durante el seguimiento el metodo mas sencillo para detectar

reversiones es realizar una comparacion de los puntos Oi−1 y Oi+1. Sin embargo,

cuando el radio de la esfera es demasiado pequeno estos puntos se encuentran

numericamente muy cerca haciendo difıcil diferenciarlos. En [22] se propone una

tecnica eficiente de deteccion de efectos de reversion en el seguimiento. Esta tecni-

ca compara los angulos de los vectores normales a la superficie de la esfera en los

puntos Oi−1 y Oi+1. La Figura 3.3 muestra una representacion de los vectores nor-

males en cada punto de interseccion sobre a superficie de la esfera. Los vectores

normales a cada punto son calculados utilizando el gradiente de la ecuacion (2.14)

que define a la esfera, expresado en la ecuacion:

~∇S = 2(x− cx)x+ 2(y − cy)y + 2(λ− cλ)λ, (3.1)

donde ~∇S representa el vector normal a un punto en la superficie de la esfera

Si. Los puntos Oi−1 y Oi+1 se evaluan en esta expresion para obtener los angulos

respecto a cada eje cartesiano. Los angulos respectivos a cada punto de intersec-

cion son (φi−1, ψi−1, ζi−1) y (φi+1, ψi+1, ζi+1). Una reversion en el seguimiento es


detectada cuando se cumple la condicion:

(φi−1, ψi−1, ζi−1) = (φi+1, ψi+1, ζi+1),

φi−1 = arc cos(2(x−cx)x‖∇S‖

)

Oi−1,

ψi−1 = arc cos(2(y−cy)y‖∇S‖

)

Oi−1,

ζi−1 = arc cos(2(λ−cλ)λ‖∇S‖

)

Oi−1.

φi+1 = arc cos(2(x−cx)x‖∇S‖

)

Oi+1,

ψi+1 = arc cos(2(y−cy)y‖∇S‖

)

Oi+1,

ζi+1 = arc cos(2(λ−cλ)λ‖∇S‖

)

Oi+1.

(3.2)

Sin importar la cercanıa de los puntos Oi−1 y Oi+1 los vectores normales tienen

direcciones muy diferentes, a menos que estos sean el mismo punto. Los angulos

de los vectores normales proveen mas informacion respecto a un punto que la

posicion del mismo [22].

Figura 3.3: Estrategia para detectar reversiones en el seguimiento.

3.1.2. Tecnica para corregir efecto de reversion

Cuando se detecta una reversion en el seguimiento es necesario contar con un

algoritmo capaz de localizar un nuevo punto en direccion de avance y corregir el

efecto de reversion. En [22, 21] se propone incrementar el radio de las hiperesferas

como tecnica de correccion. Sin embargo, debido a la existencia de curvas cerradas

en el mapa, como se explico en el capıtulo anterior, un incremento en el radio puede

provocar que el seguimiento salte entre curvas cuando estas son muy cercanas. En


este trabajo se propone colocar el punto predictor en una posicion cercana al punto

solucion en direccion de avance. Esta tecnica consiste en rotar el punto predictor

sobre la superficie de la esfera un angulo θ en direccion de avance. La Figura

3.4 presenta la tecnica propuesta para corregir reversiones en el seguimiento de

trayectoria.

De (2.13) y (2.11) se puede representar H1 como la ecuacion de un plano de

la forma:

H1 = −y −m1x+ λ (b+m1(a− x0)− y0) + (y0 +m1x0) (3.3)

donde (x0, y0) es el punto inicial, (a, b) es el punto meta y m1 es el parametro que

determina la pendiente de la recta L1 mostrada en la Figura 2.6. Los puntos de

la curva γ representan los puntos de interseccion entre el plano H1 y la superficie

H2, es decir, cada punto de la curva γ esta contenido en la superficie H2 y en el

plano H1. Por lo tanto, todos los puntos H−1(0) de la curva es un subconjunto de

los puntos H−11 (0). Utilizando la expresion (3.3) se puede encontrar una expresion

para λ con respecto a x, y de la forma:

λi =yi +m1xi − (y0 +m1x0)b+m1(a− x0)− y0

(3.4)

Con esta expresion se tiene que para todo punto (xi, yi) en el espacio bidimen-

sional se puede encontrar un punto (xi, yi, λi) en el espacio tridimensional que

este contenido en el plano H1. Entonces la rotacion del punto predictor sobre la

superficie de la esfera se puede realizar en dos dimensiones y despues mapearlo a

un entorno tridimensional. Debido a que las rotaciones de puntos en dos dimen-

siones se realizan respecto al origen, es necesario definir un sistema de referencia

auxiliar con origen en el centro de la esfera en el espacio bidimensional, como se

muestra en la Figura 3.4b.


(a) (b)

(c)

Figura 3.4: Estrategia para corregir reversiones en el seguimiento.

La Figura 3.4a muestra el punto predictor Pp en el plano original. En la Figura

3.4b se muestra el sistema de referencia auxiliar, donde P′

p es el punto predictor

y P′

rot es el punto predictor rotado un angulo θ. En la Figura 3.4c se presenta el

punto predictor rotado un angulo θ en el sistema de referencia original. Se tiene

la expresion de los puntos en su representacion bidimensional x− y de la forma:

O1 = (x1, y1),

O2 = (x2, y2),

O3 = (x3, y3),

O4 = (x4, y4),

Pp = (xp, yp),

(3.5)

donde los puntos O3 y Pp se proyectan respecto al origen O3 del sistema de referen-

cia auxiliar para obtener O′

3 , P′

p . Se obtiene el punto predictor rotado P′

rot en el


sistema de referencia auxiliar utilizando la matriz de rotacion en dos dimensiones.

O′

3 = (c′x, c′y) = (cx, cy)− (cx, cy) = (0, 0), (3.6)

P′

p = (x′p, y

′p) = (xp, yp)− (cx, cy), (3.7)

P′

rot = (x′rot, y

′rot); P

′

rot =

(cos(θ) − sin(θ)

sin(θ) cos(θ)

)(x′py′p

)

. (3.8)

El punto P ′rot se proyecta en el sistema de referencia original x−y para localizar

el valor correspondiente de λrot utilizando la expresion (3.4) se obtiene:

Prot = (xrot, yrot) = (cx, cy) + (x′rot, y

′rot), (3.9)

xrot

yrot

λrot

=

xrot

yrotyrot+m1xrot−(y0+m1x0)

b+m1(a−x0)−y0

; (xrot, yrot, λrot) ∈ H

−11 (0). (3.10)

La estrategia utilizada para corregir reversiones tambien pude ser utilizada

para obtener un punto predictor que evite errores en el metodo de correccion.

Comunmente, para puntos predictores cercanos a la solucion el metodo de Newton-

Raphson cumple el criterio de convergencia en unas pocas iteraciones. Sin embar-

go, en ocasiones debido a las no linealidades presentes en el sistema el metodo llega

al numero de iteraciones maximas jmax sin cumplir el criterio de convergencia. Es-

to ocasiona errores de convergencia a puntos de la curva homotopica provocando

perdidas de trayectoria en el seguimiento. En este trabajo se propone utilizar una

tecnica de rotacion de puntos predictores para corregir errores de convergencia.

De esta forma, cuando el metodo N-R llega a ese numero se ejecuta la tecnica de

reversion y se ejecuta nuevamente el corrector N-R. Por criterio, en esta tesis se

decidio utilizar jmax =40 iteraciones y θ =π4en la tecnica de correccion de efecto

de reversion.

3.1.3. Parametros de repulsion y pendientes de rectas auxi-

liares

En el capıtulo anterior es presentado el sistema de ecuaciones H y su depen-

dencia por un conjunto de parametros. En las secciones anteriores se expresa a


H1 como la ecuacion de un plano en el espacio tridimensional. De forma similar,

sin presencia de obstaculos, la ecuacion H2 representa a un plano y la curva ho-

motopica γ es puntos de interseccion entre estas superficies, como se muestra en

la Figura 3.5b.

Cuando existe presencia de obstaculos en el entorno, la ecuacion H2 es mo-

dificada lo cual implica una deformacion en la superficie que representa y en

consecuencia variaciones en la curva γ. De forma general, toda variacion en las

ecuaciones H1 y H2 modifica los puntos de interseccion entre las superficies des-

critas por estas. Por un lado, las variaciones en las ecuaciones H1 y H2 pueden ser

producidas por modificaciones en los parametros de repulsion. Por otro lado, estas

tambien pueden ser modificadas por cambios en los parametros que determinan

las pendientes de las recta auxiliares (m1 y m2).

(a) Representacion bidimensional. (b) Representacion tridimensional.

Figura 3.5: Representacion del mapa de entorno sin presencia de obstaculos.

La Figura 3.5a presenta la ruta mas corta entre un punto A y un punto B

representada como una lınea recta. La Figura 3.5b representa la trayectoria directa

sin presencia de obstaculos donde A es el punto inicial (0, 0, 0) y B el punto meta

(1, 1, 1).

En este apartado se muestran una serie de casos de estudio, primero para las

variaciones en la curva γ debidas a cambios en los valores de los parametros m1 y

m2. En seguida, para mostrar variaciones en la curva γ debidas a cambios en los

valores de los parametros de repulsion pi.


Variaciones en la curva γ debido a cambios en m1 y m2

Como se comento anteriormente la curva γ sufre cambios ante modificaciones

en los parametros m1 y m2, estos determinan la inclinacion de las rectas auxiliares

asociadas a cada ecuacion del sistema H. En las Figuras 3.6 se muestra una

serie simulaciones de casos de estudio para un mismo escenario de obstaculos y

diferentes valores de m1 y m2.

(a) Caso de estudio 1. (b) Caso de estudio 2.

Figura 3.6: Trayectorias resultantes para valores de m1 y m2 cercanos a cero.

En las Figuras 3.6a y 3.6b se muestran dos configuraciones de rectas auxiliares,

dondem1 ym2 tienen valores cercanos a cero. Los valores cercanos a cero producen

trayectorias libres de colisiones alejadas de la trayectoria directa y en consecuencia

trayectorias ineficientes. Por otro lado, en las Figuras 3.7 se presentan trayectorias

resultantes para valores de m1 y m2 mayores a cero. En la Figura 3.7a la curva

γ presenta un efecto de atraccion por parte de la trayectoria directa sin embargo

el efecto de los obstaculos en el plano impiden obtener una trayectoria eficiente.

Para m1 = 1, m2 = 2; la curva γ muestra un comportamiento aproximado al de la

trayectoria directa y el efecto de los obstaculos es muy pequeno, como se muestra

en la Figura 3.7b. En la Figura 3.7c la curva γ resultante es similar a la trayectoria

directa y el efecto de los obstaculos lejanos a ella es casi nulo. Como resultado de

estos casos de estudio se elige tomar a m1 = 1 y m2 = 4 como valores optimos y

se utilizan en posteriores simulaciones presentadas en el documento.


(a) Caso de estudio 3. (b) Caso de estudio 4.

(c) Caso de estudio 5.

Figura 3.7: Trayectorias resultantes para valores de m1 y m2 lejanos a cero.

Teniendo los valores 4 y 1 como valores optimos para m1 y m2 respectivamente

se estudian los casos para m1 = 4 y m2 = 1 y para m1 = 1 y m2 = 4. Al igual

que para cualesquiera valores de estos parametros la curva γ resultante es una

trayectoria directa. No obstante, en presencia de obstaculos el comportamiento de

γ es diferente para estos casos de estudio mostrados en las figuras 3.8.

La Figura 3.8a muestra la representacion de una curva que pasa bajo el con-

torno de los obstaculos, es decir, una curva inferior a la trayectoria directa. Se


tienen curvas inferiores cuando para cada punto (xi, yi) de la curva γ y (xl, yl)

de la trayectoria directa se cumple que xi = xl, yi < yl, en general se producen

cuando m1 < m2. En caso opuesto, una curva superior cumple xi = xl y yi > yl

y se producen cuando m1 > m2, como se muestra en la Figura 3.8b. En general,

las trayectorias inferiores pasan por debajo del contorno de los obstaculos con res-

pecto al eje de simetrıa representado por la trayectoria directa y las trayectorias

superiores pasan por encima de los obstaculos. Los parametros y posiciones de los

obstaculos del mapa de entorno utilizados para las simulaciones de las figuras 3.7

y 3.6 se muestran en la Tabla 3.1.

(a) Trayectoria inferior. (b) Trayectoria superior.

Figura 3.8: Trayectorias resultantes de invertir las rectas auxiliares.


Ob xi yi rci α β piC0 0.7 0.3 0.1 - - 0.001C1 0.7 0.3 0.1 - - 0.001C2 0.3 0.7 0.1 - - 0.001C4 0.7 0.9 0.05 - - 0.001

C5 0.7 0.1 0.05 - - 0.001C6 0.3 0.9 0.05 - - 0.001C7 0.3 0.1 0.05 - - 0.001C8 0.9 0.7 0.05 - - 0.001C9 0.9 0.3 0.05 - - 0.001C10 0.1 0.7 0.05 - - 0.001C11 0.1 0.3 0.05 - - 0.001C12 0.1 0.3 0.05 - - 0.001R0 0.5 0.15 - 0.15 0.05 0.1R1 0.55 0.85 - 0.15 0.05 0.1R2 0.15 0.5 - 0.05 0.15 0.1R3 0.85 0.5 - 0.05 0.15 0.1

Figure 3.6a m1 = 0.001, m2 = 0.01 A = (0, 0), B = (1, 1)Figure 3.6b m1 = 0.01, m2 = 0.1 A = (0, 0), B = (1, 1)Figures 3.7a m1 = 1, m2 = 2 A = (0, 0), B = (1, 1)Figures 3.7b m1 = 1, m2 = 1.1 A = (0, 0), B = (1, 1)Figures 3.7c m1 = 1, m2 = 4 A = (0, 0), B = (1, 1)Figures 3.8a m1 = 1, m2 = 4 A = (0, 0), B = (1, 1)Figures 3.8b m1 = 4, m2 = 1 A = (0, 0), B = (1, 1)

Tabla 3.1: Parametros utilizados en las simulaciones de las Figuras 3.7 y 3.6.

En la tabla 3.1 se observa la posicion de los obstaculos (xi, yi), parametros

de repulsion pi y tamano de los mismos. Los sımbolos Ci y Ri representan los i-

esimos obstaculos circulares y rectangulares respectivamente. Los obstaculos C0,

C3 y C12 solo estan presentes en los mapas de obstaculos de la Figura 3.6

Variaciones en la curva γ debido a cambios en los parametros de repul-

sion pi

El parametro pi correspondiente a cada obstaculo en el termino W de la ex-

presion H2 determina la distancia entre la curva γ y el contorno del obstaculo

Obi. Esta distancia podrıa interpretarse como un efecto de repulsion por parte de

los obstaculos en la trayectoria, sin embargo, en realidad se tiene que para puntos


del contorno de cada obstaculo la superficie H2 esta indefinida. En las Figuras

3.9 se muestra el efecto de repulsion producido por diferentes valores del parame-

tro p0 correspondiente al obstaculo circular C0. En la ecuacion (2.12) el termino

W muestra, para valores de pi grandes, un mayor efecto en la expresion f2 y en

consecuencia en la funcion homotopica H2.

(a) (b) (c)

Figura 3.9: Curva homotopica para diferentes valores de parametro de repulsionp0 correspondiente al obstaculo circular C0.

(a) (b) (c)

Figura 3.10: Curva homotopica para diferentes valores de parametro de repulsionp0 correspondiente al obstaculo rectangular R0.

Para obstaculos rectangulares el exponente 2η en la ecuacion (2.6) ocasiona

que para puntos diferentes a los contenidos en los contornos de R0 y C0 ocurra que

R0 � C0. Por lo tanto, se el parametro de repulsion de los obstaculos rectangulares


requiere ser mayor que en los circulares para obtener un efecto similar, como se

muestra en las Figuras 3.10. Si se tienen parametros de repulsion negativos la

curva presenta un comportamiento similar al producido cuando se intercambian

los valores de las pendientes de las rectas auxiliares [33], es decir la curva actua

como una curva inferior o superior como se muestra en la Figura 3.8b.

(a) Trayectorias superior. (b) Trayectorias inferior.

Figura 3.11: Trayectorias producidas por un cambio de signo en p0.

El valor negativo en p0 provoca un cambio en la direccion de la trayectoria,

es decir, para m1 > m2 la curva γ recorrera el contorno del obstaculo C0 por la

parte inferior como se muestra en la Figura 3.8b. En las Figuras 3.8a y 3.8b se

observa la diferencia en el comportamiento de la curva homotopica debido a un

cambio de signo en el parametro de repulsion. En contraste sucede para m1 > m2

y p0 con signo negativo produce una curva γ que recorre el obstaculo C0 por la

parte superior del contorno.

Como resultado del estudio se obtienen las siguientes propiedades:

Si m1 > m2 y p1, . . . , pk positivos en un mapa de entorno con k obstaculos,

la curva γ recorrera la parte superior del contorno de los obstaculos que

intersecten con la trayectoria directa o sean lo suficiente cercanos a esta.


Si m1 < m2 y p1, . . . , pk positivos en un mapa de entorno con k obstaculos,

la curva γ recorrera la parte inferior del contorno de los obstaculos que in-

tersecten con la trayectoria directa o sean lo suficiente cercanos a esta.

Si m1 > m2 y pi con signo negativo, donde Obi es un obstaculo cercano a

la trayectoria directa. Entonces la curva γ recorrera la parte inferior de este

obstaculo.

Si m1 < m2 y pi con signo negativo, donde Obi es un obstaculo cercano a la

trayectoria directa. Entonces la curva γ recorrera la parte superior de este

obstaculo.

(a) Trayectoria inferior. (b) Trayectoria superior.

Figura 3.12: Trayectorias resultantes de variaciones en parametros de repulsion yrectas auxiliares.

En las Figuras 3.12a se presenta una trayectoria inferior para un mapa de

entorno en el cual la trayectoria debe recorrer el contorno de los obstaculos sin

alejarse de la trayectoria directa. En la Figura 3.12b se presenta una trayectoria

superior que rodea solo a los obstaculos cercanos a la trayectoria directa. En ambas

figuras se producen trayectorias que requieren un numero grande de pasos en el

seguimiento. El signo del parametro de repulsion correspondiente a cada obstaculo

permite crear trayectorias optimas que requieran de un menor numero pasos en el


metodo como se muestra en la Figura 3.13. En la Figura 3.13 se muestra el ejemplo

de una trayectoria directa producida al modificar el signo en los parametros de

repulsion de los obstaculos R3, R5 y C0.

Figura 3.13: Trayectoria resultante de variaciones en parametros de repulsion.

3.2. Tecnica de radio variable

El tamano de radio afecta el numero de iteraciones en el seguimiento, es de-

cir un radio grande puede ocasionar pocas iteraciones pero tambien podrıa causar

saltos a curvas aisladas [21, 23]. Una forma de solucionar este problema es agregar

una tecnica para controlar el tamano de radio. En [23] se propone un metodo que

relaciona el valor de las pendientes de las curva γ respecto al parametro λ de la

siguiente forma:

ri+1 = r0

(

1− (1− Γ) exp−(|μi||μi+1|

))

(3.11)

donde ri+1 es el radio de la esfera Si+1, r0 es el radio inicial de las esferas, Γ

es el maximo factor por el cual se multiplica r0 y μi es la derivada parcial del

parametro homotopico con respecto a ρ en el i-esima iteracion del seguimiento,

es decir, μi = (∂λ/∂ρ)i y μi+1 = (∂λ/∂ρ)i+1. Un metodo de radio variable en la

metodologıa involucra modificaciones en la tecnica de deteccion de reversiones. En

3.2. Tecnica de radio variable 43

la Figura 3.14a se muestra la metodologıa con radio fijo y la correcta deteccion

de reversion para i = 2.

(a) Correcta deteccion con radio fi-jo.

(b) Deteccion fallida con radio va-riable.

(c) Correcta deteccion con radiovariable.

Figura 3.14: Deteccion de efecto de reversion durante el seguimiento de trayectoria.

La variacion en el tamano de radio causa que el punto de interseccion entre la

esfera S3 y la curva γ no coincidan con el centro de S2, en este caso la tecnica para

detectar efecto de reversion falla como se muestra en la Figura3.14b. En [21] se

propone mover el centro de la esfera S3 sobre la lınea formada por la interpolacion

de los puntos O2 y O3. La Figura 3.14c muestra claramente la interpretacion

grafica de esta tecnica, donde el punto de interseccion entre la esfera S3 y la curva

γ coinciden en el punto O2. El nuevo centro de la esfera S2 es representado con el

punto O3 movido un Δr = ri− ri+1 desde el punto O3 como esta representado en

la figura 3.14c.

El centro de las esferas cuando se utiliza radio variable es calculado con la

expresion:

Oi+1 = (cx, cy, cλ) = (xi+1, yi+1, λi+1)+Δr ‖(xi+1, yi+1, λi+1)− (xi, yi, λi)‖ (3.12)

donde para el ejemplo mostrado en la Figura 3.14c i = 2 , Oi+1 es el nuevo centro

de la esfera S3, (xi+1, yi+1, λi+1) es el punto O3 y (xi, yi, λi) es el punto O2.


Utilizando las tecnicas y metodos descritos anteriormente se puede formular un

algoritmo que contemple los efectos de reversion en el seguimiento y la tecnica de

radio variable. La Figura 3.15 muestra el diagrama de flujo de la metodologıa en

el cual se incluyen las tecnicas de deteccion y correccion de reversiones, ası como

un metodo para variar el radio.

Figura 3.15: Metodologıa propuesta para el trazado de trayectorias.

3.3. Implementacion en lenguaje C++ 45

3.3. Implementacion en lenguaje C++

Por lo general, las plataformas utilizadas para desarrollar robots moviles se

basan en leguajes de programacion C o C++. Por lo tanto, es importante des-

arrollar el algoritmo de planeacion de trayectorias en un lenguaje mas general,

en este caso C++. Esto permite una mayor compatibilidad con diversas platafor-

mas de desarrollo y permite una mayor flexibilidad en desarrollo de aplicaciones

modulares. Con esto como objetivo, se desarrolla una herramienta de software

modular en C++ del algoritmo presentado en la Figura 3.15. El usar Jacobiano,

determinantes de matrices y matrices inversas en forma numerica implica la nece-

sidad de utilizar una biblioteca de calculo matematico, este problema se soluciona

usando expresiones simbolicas. Las expresiones en (3.13) representa el sistema de

ecuaciones homotopicas en su forma simbolica.

H1 = L1(x, y)− (1− λ)L1(x0, y0),

H2 = (L2(x, y) +W (x, y)−Q)− (1− λ) (L2(x0, y0) +W (x0, y0)−Q) ,

Si = (x− cx)2 + (y − cy)2 + (λ− cλ)2 − r2i .

(3.13)

Agrupando terminos y derivando respecto a cada variable se obtiene la expre-

sion simbolica del Jacobiano, donde cada termino de la matriz se representa como

J1,1 = −m1,

J1,2 = −1,

J1,3 = −y0 −m1x0 +m1a+ b,

J2,1 = −m2 +Wpx(x, y);

J2,2 = −1 +Wpy(x, y),

J2,3 = −y0 −m2x0 + b+m2a+W (x0, y0)−Q,

J3,1 = 2x− 2cx,

J3,2 = 2y − 2cy,

J3,3 = 2λ− 2cλ,

(3.14)

los terminos Wpx(x, y) y Wpy(x, y) representan las derivadas parciales de W (x, y)

con respecto a las variables x , y. Utilizando uniones e intersecciones de circun-

ferencias se pueden representar todo tipo de obstaculos en el entorno. Con lo

antes mencionado, se formula una metodologıa que use unicamente obstaculos

circulares, por lo tanto se obtienen las expresiones para Wpx(x, y) y Wpy(x, y)


representadas por:

Wpx(x, y) =∂W (x,y)∂x

=∑i=ki=1

−2pi(x−xi)((x−xi)2+(y−yi)2−rc2)

2 ,

Wpy(x, y) =∂W (x,y)∂y

=∑i=ki=1

−2pi(y−yi)((x−xi)2+(y−yi)2−rc2)

2 .

(3.15)

El metodo de Newton-Raphson incluye la matriz inversa del Jacobiano, sin

embargo, obtener la matriz inversa es una tarea de alto costo computacional, en

nuestro caso se eligio colocar su expresion simbolica con respecto a cada termino

de la matriz del Jacobiano. De manera practica se utiliza la simbologıa J [HS] pa-

ra referirse al Jacobiano del sistema HS(x, y, λ) en la ecuacion (3.17). La matriz

inversa debe ser calculada en cada iteracion de N-R. Ası, utilizando la represen-

tacion simbolica de la matriz se ahorra tiempo de computo ya que el termino J∗

se calcula una vez en cada iteracion.

[J(HS)]−1 =

(J2,2J3,3−J2,3J3,2)(J∗)

−(J1,2J3,3−J1,3J3,2)(J∗)

(J1,2J2,3−J1,3J2,2)(J∗)

−(J2,1J3,3−J2,3J3,1)(J∗)

(J1,1J3,3−J1,3J3,1)(J∗)

−(J1,1J2,3−J1,3J2,1)(J∗)

(J2,1J3,2−J2,2J3,1)(J∗)

−(J1,1J3,2−J1,2J3,1)(J∗)

(J1,1J2,2−J1,2J2,1)(J∗)

, (3.16)

donde

J∗ = J1,1J2,2J3,3−J1,1J2,3J3,2−J1,2J2,1J3,3+J1,2J2,3J3,1+J1,3J2,1J3,2−J1,3J2,2J3,1.

(3.17)

Con estos cambios realizados en el algoritmo se programa una herramienta de

planeacion de trayectoria en un entorno flexible de desarrollo. En el capıtulo cuatro

se muestran los resultados obtenidos al utilizar esta herramienta para obtener

trayectorias libres de colisiones en diversos mapas de entorno.

3.4. Implementacion en microcontrolador

Los robots moviles son de diversos tamanos y formas acorde a la aplicacion

para la cual es disenado, el tamano del mismo generalmente suele ser una restric-

cion del hardware. Con el fin de probar el alcance y eficacia de la metodologıa

en aplicaciones para robots moviles pequenos se implementa el algoritmo en una

3.4. Implementacion en microcontrolador 47

tarjeta de desarrollo de Texas Instruments. Esta tarjeta de desarrollo cuenta con

un microcontrolador de 32 bits con multiples modulos de interfaz, adquisicion de

datos y calculo numerico de datos en formato de punto flotante [32]. Sin embargo,

para nuestro caso solo se utiliza para comprobar la efectividad y velocidad del

algoritmo para trazar rutas libres de colisiones de robots moviles en ambientes

controlados. En la Figura 3.16 se muestra un diagrama del sistema utilizado para

medir los tiempos de computo y efectividad de la metodologıa implementada.

Figura 3.16: Representacion grafica de la implementacion en microcontrolador delmetodo de trazado de trayectorias.

Se utiliza una tarjeta de desarrollo TivaC Series (K-TM4C1294XL), la cual

tiene un microcontrolador ARM de 32 bits que permite realizar calculo de datos

en un formato de punto flotante. El algoritmo es implementado en la tarjeta me-

diante un compilador de C++, ademas se establece comunicacion serial utilizando

el protocolo RS232 y el software MATLAB. Este software nos permite adquirir los

datos de la trayectoria obtenidos en el microcontrolador y posteriormente repre-

sentarlos como una curva en el plano bidimensional x− y. De esta misma forma,

se mide el tiempo de computo total de la metodologıa en el microcontrolador,

numero de pasos del seguimiento e iteraciones totales de N-R en todo el

proceso. Considerando el efecto que se presenta por el retardo en la comunicacion;

se resta el tiempo de retardo acumulado del tiempo total de ejecucion. El algo-

ritmo implementado utiliza aproximadamente 25% de la capacidad de memoria

Flash del microcontrolador, con lo cual se comprueba la sencillez del algoritmo.


3.5. Conclusiones

Se mostro que con apropiados valores de cada parametro que interviene en el

sistema de ecuaciones H se pueden crear trayectorias optimas que involucran el

mınimo numero de pasos en el seguimiento. Se genero un criterio para determinar

los parametros de repulsion y las pendientes de las rectas auxiliares. Cuando los

mapas de entorno son complejos el metodo puede presentar efectos de reversion

en el seguimiento ocasionando que el metodo pierda la trayectoria exitosa. Se

presento la tecnica usada para reversiones basada en una tecnica que compara los

angulos de los vectores normales a cada punto. Se explica la tecnica propuesta

para corregir reversiones basada en una rotacion en 2-D sobre un plano auxiliar.

Con todo lo anterior, se crea un algoritmo eficiente de trazado de trayectoria

capaz de corregir efectos de reversion en el seguimiento. El algoritmo propuesto se

implemento en Maple 13 con la finalidad de analizar el efecto de cada parametro

involucrado en el sistema H. Se presento la herramienta modular desarrollada en

lenguaje C++, ası como su implementacion en un sistema embebido.

CAPITULO 4

Casos de estudio

En este capıtulo se presentan una serie de configuraciones de mapas de en-

torno para los cuales se obtiene exitosamente una trayectoria libre de colisiones.

En primer lugar, se ilustra la forma en que opera la metodologıa con un ejemplo

numerico. En segundo lugar, se muestran una serie de simulaciones en Maple 13

para mapas de entorno en ambientes cerrados. Se presentan graficas comparativas

de tiempos de calculo total de la metodologıa en C++, Maple 13 y en un sistema

embebido. Por ultimo, se discuten los resultados de las simulaciones.

4.1. Caso de estudio 1: Mapa de entorno con

tres obstaculos circulares.

En esta seccion se ilustra el proceso de trazado de trayectorias usando la

metodologıa antes descrita para un mapa de entorno con tres obstaculos circulares.

Se tiene los obstaculos C0, C1, C2 con posiciones (xi, yi) y tamano de radio rci

mostrado en la Tabla 4.1. A es el punto inicial y B el punto meta, m1 y m2 son los

parametros que determinan las pendientes de las lıneas rectas auxiliares y pi es

el parametro de repulsion respectivo a cada obstaculo. Para este caso de estudio

los tres obstaculos circulares se interponen en la trayectoria directa del punto A

al punto B.

49

50 Capıtulo 4. Casos de estudio

Obstaculo Tipo de obstaculo xi yi rci piC0 Circular 0.2 0.2 0.1 0.001C1 Circular 0.5 0.5 0.1 -0.001C2 Circular 0.8 0.8 0.1 0.001Fig.4.3 m1 = 4, m2 = 1 A = (0, 0), B = (1, 1)

Tabla 4.1: Parametros de simulacion caso de estudio 1.

Usando los valores de la tabla anterior se tienen las ecuaciones caracterısticas

de cada obstaculo de la siguiente manera:

C0(x, y) = (x− 0.2)2 + (y − 0.2)2 − (0.1)2 = 0,

C1(x, y) = (x− 0.5)2 + (y − 0.5)2 − (0.1)2 = 0,

C2(x, y) = (x− 0.8)2 + (y − 0.8)2 − (0.1)2 = 0,

(4.1)

Ci(x, y) es la expresion en dos dimensiones de cada obstaculo.

Teniendo el punto meta B y los parametros m1 y m2 se substituyen en las

ecuaciones (2.9) y (2.10) para formular las lıneas auxiliares L1 y L2 como:

L1(x, y) = −y − 4x+ 5 = 0,

L2(x, y) = −y − x+ 2 = 0,(4.2)

si m1 = 4 y m2 = 1, se tiene m1 > m2, por lo tanto, la curva resultante se com-

portara como una trayectoria superior y sg = −1.

Utilizando (2.4) se tiene la expresion para W (x, y) y Q de la siguiente forma:

W (x, y) =p1

C0(x, y)+

p2

C1(x, y)+

p3

C2(x, y), (4.3)

Q = W (1, 1) =p1

C0(1, 1)+

p2

C1(1, 1)+

p3

C2(1, 1), (4.4)

donde, para este ejemplo p1 = 0.001, p2 = −0.001, p3 = 0.001.

Con todo lo anterior se formulan las ecuaciones de f1 y f2 :

f1(x, y) = −y − 4x+ 5 = 0,

f2(x, y) = −y − x+ 2 +W (x, y)−Q = 0(4.5)

al evaluar se obtiene Q = 0.01303 y para el punto meta (a, b) = (1, 1) se cumple

4.1. Caso de estudio 1 51

que f1(1, 1) = 0 y f2(1, 1) = 0.

En este ejemplo el radio de la primera esfera es r1 = r0 = 0.01. De las expre-

siones (2.13), (2.14), (2.15) y (4.4) se tiene el sistema de ecuaciones homotopicas

HS como:

H1(x, y, λ) = f1(x, y)− (1− λ)f1(0, 0) = 0,

H2(x, y, λ) = f2(x, y)− (1− λ)f2(0, 0) = 0,

S1(x, y, λ) = (x− cx)2 + (y − cy)2 + (λ− cλ)2 − (0.01)2 = 0,

(4.6)

reordenando y eliminando terminos se obtiene el siguiente sistema de ecuaciones:

H1(x, y, λ) = −y − 4x+ 5λ = 0,

H2(x, y, λ) = −y − x+W (x, y) + 2λ+ (λ− 1)W (0, 0)− λQ = 0,

S1(x, y, λ) = (x− cx)2 + (y − cy)2 + (λ− cλ)2 − (0.01)2 = 0,

(4.7)

al evaluar los terminos W (0, 0) = 0.1.

Para el primer paso del seguimiento (cx, cy, cλ) = (x0, y0, 0). El punto predictor

(xp, yp, λp) se obtiene utilizando el vector predictor de Euler como se describe en el

capıtulo 2. Se tiene para el primer paso del seguimiento i = 1 y O1 = (x0, y0, 0) =

(0, 0, 0).

Usando (2.25) se obtiene el vector predictor como:

∂λ

∂ρ= −1(−1)3 det

(∂H1∂x

∂H1∂y

∂H2∂x

∂H2∂y

)

= 2.76462, (4.8)

continuando con el procedimiento del metodo de Euler se tiene:

(∂x∂ρ∂y∂ρ

)

=

(−4 −1

−0.92154 −0.92154

)−1(−13.8231

−5.52924

)

=

(2.60770

3.39229

)

, (4.9)

~vp =

∂x∂ρ∂y∂ρ∂λ∂ρ

=

2.60770

3.39229

2.76462

, (4.10)

donde μ1 =∂λ∂ρ= 2.76462.

Substituyendo ~vp y (cx, cy, cλ) en (2.25) se calcula el primer punto predictor


como:

‖~vp‖ = (0.511895, 0.665913, 0.542699), (4.11)

continuando con el proceso se utiliza el vector tangente y el radio r1 de la primera

esfera del seguimiento para obtener el punto predictor como:

(xp, yp, λp) = (0, 0, 0) + 0.01 (0.511895, 0.665913, 0.542699) ,

= (0.005118, 0.006659, 0.005426). (4.12)

En la Figura 4.1a se muestra la representacion de la primera esfera y el punto

predictor en el plano H1.

(a) Primer punto predictor. (b) Correccion de punto predictor.

(c) Segunda esfera del seguimien-to.

Figura 4.1: Representacion en el plano H1 de los primeros pasos del seguimientode trayectoria.

Una vez obtenido el punto predictor (xp, yp, λp) se calcula el punto de inter-

seccion entre la curva γ y el plano H1 usando el metodo de Newton-Raphson. Se

obtiene el punto O2 despues tres iteraciones del metodo N-R como se muestra en

la Figura 4.1b. O2 = (x2, y2, λ2) = (0.005079, 0.006707, 0.005404),


seguido de esto, se calcula un nuevo punto predictor y radio para la esfera S2

utilizando las ecuaciones (2.21), (2.25) de la misma forma que se calculo antes se

obtiene:

μ2 =∂λ

∂ρ= −(−1)3 det

(∂H1∂x

∂H1∂y

∂H2∂x

∂H2∂y

)

= 2.764620, (4.13)

teniendo ∂λ∂ρse calcula el vector predictor

(∂x∂ρ∂y∂ρ

)

=

(−4 −1

−0.912043 −0.912794

)−1(−13.695664

−5.478265

)

=

(2.563971

3.439780

)

,

(4.14)

por lo tanto, el vector predictor es:

~vp =

∂x∂ρ∂y∂ρ∂λ∂ρ

=

2.563971

3.439780

2.739132

. (4.15)

Reemplazando μ1, μ2 y Γ = 2 en la expresion (3.11) se obtiene el radio para la

esfera S2 de la forma:

r2 = 0.01(1− (1− Γ)e−(

|2.76462||2.739132|)

)= 0.013644, (4.16)

La Figura 4.1c se muestra la esfera S2 con radio r2 y centro en O2, en esta ima-

gen se observa el problema para detectar el efecto de reversion en el seguimiento.

Entonces se debe calcular el cambio en el tamano del radio, Δr = 0.01−0.013644 =

0.003644 y utilizando la expresion (3.12) y el nuevo centro de la esfera S2 como

se muestra en la Figura 4.2a.

O2 = (cx, cy, cλ) = O2 + 0.003644(O2 −O1),

= (0.00693, 0.009152, 0.007374). (4.17)


(a) Nuevo centro para segunda esferadel seguimiento.

(b) Obtencion de segundo punto pre-dictor.

Figura 4.2: Representacion en el plano H1 del seguimiento de trayectoria.

Se usa el nuevo centro de la esfera S2 para obtener el nuevo punto predictor el

cual se encuentra representado en la Figura 4.2b. Nuevamente se realiza el proceso

del vector tangente para localiza el punto predictor sobre la superficie de la esfera

S2 de la siguiente manera:

‖~vp‖ = (0.503719, 0.675781, 0.538131), (4.18)

(xp, yp, λp) = O2 + 0.016937 (0.503719, 0.675781, 0.538131) ,

= (0.013803, 0.018373, 0.014717). (4.19)

De esta forma se continua calculando los centros de las esferas y los puntos Oi

de la trayectoria. En la Figura 4.3 se presenta la trayectoria libre de colisiones del

punto A al punto B para el mapa de entorno con tres obstaculos. En esta figura se

observa el efecto del parametro de repulsion negativo p2, el cual causa que la curva

γ pase inferior a el obstaculo Ob2. En la Figura 4.4a se muestra la interpretacion

en tres dimensiones de las superficies representadas por las ecuaciones H1, H2,

ası como, la curva de interseccion γ.


Figura 4.3: Planeacion de trayectoria para tres obstaculos circulares.


(a) Vista superior de la interseccion en-tre H1 y H2.

(b) Vista lateral inferior de la intersec-cion entre H1 y H2.

(c) Vista lateral superior de la interseccion entre H1 y H2.

Figura 4.4: Interseccion entre el plano H1 y la superficie H2.

En la Figura 4.4a se presenta la vista superior de la interseccion entre las su-

perficiesH1 yH2 representada por la curva γ. En la Figura 4.4b se aprecia el efecto

de deformacion de la superficie H2 producida por los obstaculos, presentandose

para puntos cercanos del contorno de estos una mayor deformacion. En la Figura

4.4c se observa el efecto del signo negativo en el parametro de repulsion de C1,

provocando un cambio en la direccion de la trayectoria. Los obstaculos C0 y C2

provocan una deformacion en H2 tal que la superficie cercana a los obstaculos se


vuelve concava, por otro lado, debido al signo negativo de p1 la superficie H2 es

convexa cerca del contorno del obstaculo C1. Estas deformaciones ocasionan que

la interseccion de H2 con el plano H1 se presenten superior o inferior al contorno

del obstaculo con respecto a la trayectoria directa.

4.2. Caso de estudio 2: Operacion en ambientes

cerrados

Los escenarios presentados anteriormente muestran configuraciones de obstacu-

los contenidos en un espacio normalizado. Sin embargo, aun cuando los obstaculos

estan contenidos dentro de ese espacio normalizado, no implica que la trayectoria

tambien lo este. En aplicaciones industriales y de uso domestico los robots reali-

zan tareas en ambientes cerrados y por ello se deben considerar las restricciones

del area de trabajo. En los siguientes ejemplos se muestran una serie de mapas de

obstaculos en los cuales se obtienen trayectorias libres de colisiones en un entorno

confinado por muros. La Figura 4.5a muestra un mapa de entorno para un robot

confinado en una habitacion, donde el punto inicial y el punto meta se localizan

en el espacio libre.

(a) (b)

Figura 4.5: Planeacion de trayectorias en ambientes cerrados.

Las Figuras 4.5a y 4.5b presentan dos configuraciones diferentes de trayectorias


para punto de inicio y meta diferentes. En estas figuras se muestra la flexibilidad

de la metodologıa de poder elegir cualquier punto del espacio libre como punto

inicial o punto meta. Ademas, en la Figura 4.5b se muestra que la interseccion de

la curva γ con las rectas auxiliares no afecta el trazado. Por lo tanto, la metodo-

logıa propuesta es capaz de trazar una ruta libre de colisiones entre dos puntos

cualesquiera del espacio libre. La Tabla 4.2 contiene los parametros de simulacion

utilizados para obtener las trayectorias de las Figuras 4.5a y 4.5b.

(a) (b)

Figura 4.6: Configuraciones de trayectorias libres de colisiones para un mismopunto inicial y punto meta en ambientes cerrados.

En las Figuras 4.6a y 4.6b son descritas dos configuraciones de trayectorias

libres de colisiones obtenidas al variar los parametros de repulsion de algunos

obstaculos. La correcta eleccion de parametros de repulsion permite crear tra-

yectorias optimas entre un punto y otro como se muestra en la Figura 4.6b. La

Tabla 4.2 contiene los parametros de repulsion pi de cada obstaculo en los mapas

de entorno, Ci representa el i-esimo obstaculo circular, Ri representa el i-esimo

obstaculo rectangular. Los muros que confinan el robot dentro de una habitacion

provocan que la trayectoria permanezca en el interior del area normalizada. Para

este caso de estudio se proponen los parametros de repulsion de cada obstaculo

circular pi = 0.0001 y para los rectangulares pi = 1. La variacion en el signo pro-


porciona la flexibilidad de crear trayectorias mas cortas u optimas como se puede

observar al comparar las Figuras 4.6a y 4.6b.

Ob xi yi rci α β pi (4.5a) pi (4.5b) pi (4.6a) pi (4.6b)C0 0.8 0.5 0.1 - - 0.001 0.001 0.001 0.001C1 0.5 0.3 0.1 - - -0.001 -0.001 0.001 -0.001C2 0.1 0.5 0.1 - - 0.001 0.001 0.001 0.001C3 0.25 0.75 0.1 - - 0.001 0.001 0.001 0.001C4 0.65 0.25 0.05 - - 0.001 0.001 0.001 -0.001C5 0.8 0.4 0.1 - - 0.001 0.001 0.001 0.001C6 0.65 0.45 0.05 - - 0.001 0.001 0.001 0.001C7 0.35 0.55 0.05 - - -0.001 -0.001 0.001 0.001C8 0.5 0.65 0.025 - - 0.001 -0.001 0.001 0.001R0 0.1 0.95 - 0.1 0.05 0.1 0.1 0.1 0.1R1 0.85 0.1 - 0.15 0.1 0.1 0.1 -0.1 -0.1R2 0.2 0.15 - 0.1 0.05 0.1 0.1 0.1 0.1R3 0.45 0.85 - 0.05 0.15 0.1 0.1 0.1 0.1R4 0.55 0.55 - 0.15 0.05 -0.1 -0.1 0.1 0.1R5 0.7 0.95 - 0.1 0.05 0.1 0.1 0.1 0.1R6 0.95 0.4 - 0.05 0.1 0.1 0.1 0.1 0.1R7 0.5 0.05 - 0.1 0.05 -0.1 -0.1 -0.1 -0.1R8 -0.05 0.5 - 0.05 0.6 0.1 0.1 0.1 0.1R9 1.05 0.5 - 0.05 0.6 -0.1 -0.1 -0.1 -0.1R10 0.5 1.05 - 0.6 0.05 0.1 0.1 0.1 0.1R11 0.5 -0.05 - 0.6 0.05 -0.1 -0.1 -0.1 -0.1

Figure 4.5a m1 = 1, m2 = 4 A = (0.3, 0.3), B = (0.8, 0.6)Figure 4.5b m1 = 0.1, m2 = 0.01 A = (0.1, 0.3), B = (0.83, 0.62)

Figures 4.6a y 4.6b m1 = 1, m2 = 4 A = (0.15, 0.3), B = (0.8, 0.24)

Tabla 4.2: Parametros de simulacion de mapas de obstaculos en ambientes cerra-dos.

Los muros que confinan el espacio de trabajo del robot en los mapas anteriores

son representados por los obstaculos rectangulares R8, R9, R10 y R11. El signo de

cada parametro de repulsion para estos obstaculos se elige de tal forma que el

efecto de repulsion obligue a la trayectoria a rodear de manera superior o inferior

acorde al obstaculo. De esta forma se mantiene la curva γ en el interior del espacio

confinado.


4.3. Caso de estudio 3: Pruebas en multiples pla-

taformas

Con la finalidad de probar la robustez y eficacia de la metodologıa, se crean

21 mapas de entorno con obstaculos circulares dispuestos aleatoriamente en el

espacio normalizado. Los obstaculos tienen un radio maximo de 0.05 y un mınimo

de 0.002. Los mapas de obstaculos contienen desde 0 a 200 obstaculos con un

incremento de 10 entre cada uno de ellos. Estos mapas se prueban en las dos

diferentes plataformas de software (Maple13 y C++), ası como C++ en un sistema

embebido usando los parametros de la Tabla 4.3. Las Figuras 4.7 y 4.8 muestran

la trayectoria libre de colisiones y el mapa de entorno para 40 y 200 obstaculos

dispuestos aleatoriamente dentro del espacio normalizado. Se observa en estas

figuras la eficacia de la metodologıa en las diferentes plataformas para obtener

con exito la trayectoria libre de colisiones. Si bien la trayectoria es exitosa no es

optima ya que el parametro de repulsion es el mismo para todos los obstaculos.

Principales parametros de simulacion

(x0, y0) = (0, 0)

(a, b) = (1, 1)

m1 = 4, m2 = 1

pi = 0.005

P1 = 1

P2 = 2

r0 = 0.01

err = 0.000001

imax = 1000

jmax = 40

Tabla 4.3: Parametros usados para obtener las trayectorias libres de colisiones delos 20 mapas de entorno con obstaculos circulares.


(a) Metodologıa en Maple 13. (b) Metodologıa en C++.

(c) Metodologıa en sistema embebido.

Figura 4.7: Trayectoria libre de colisiones resultante para un mapa de entorno con40 obstaculos circulares.


(a) Metodologıa en Maple 13. (b) Metodologıa en C++.

(c) Metodologıa en sistema embebido.

Figura 4.8: Trayectoria libre de colisiones resultante para un mapa de entorno con200 obstaculos circulares probada en multiples plataformas.

Para los dos mapas de entorno mostrados en las figuras anteriores la metodo-

logıa en las diferentes plataformas obtuvo exitosamente una trayectoria libre de

colisiones. Ademas, se puede observar que se obtuvo la misma trayectoria.

4.4. Discusion de resultados 63

4.4. Discusion de resultados

Como se explico anteriormente los tiempos de calculo o tiempo de ejecucion

para cada mapa de obstaculo dependen de la complejidad del mapa de entorno

o numero de pasos del seguimiento. En las Figuras 4.9a y 4.9b se muestran las

graficas de tiempo que toma trazar una trayectoria libre de colisiones contra el

numero de obstaculos en el mapa.

(a) Tiempo de calculo total en mili segundos vs. numero de obstaculos.

(b) Tiempo de calculo total en segundos vs. numero de obstaculos.

Figura 4.9: Tiempo total de calculo para cada mapa de obstaculos en las diferentesplataformas.

La grafica de la Figura 4.9a representa el tiempo de calculo total para cada

mapa de obstaculo. En esta imagen se muestra una comparacion de la metodologıa

implementada en C++ con radio fijo y radio variable. En ella se muestra una


mejora significativa de tiempo de calculo total para la metodologıa con radio

variable. La grafica de la Figura 4.9b presenta los tiempos de calculo totales contra

el numero de obstaculos, esto para la metodologıa con radio variable implementada

en Maple 13 y en el sistema embebido con microcontrolador ARM. En esta figura

se muestra una mejora significativa de tiempos de calculo para el sistema embebido

con respecto a la metodologıa en Maple 13. Como se menciono anteriormente el

tiempo de calculo total es afectado por la complejidad del mapa de entorno. Sin

embargo, para esta metodologıa tambien se tiene una dependencia con el numero

de obstaculos. Durante cada paso del recorrido el metodo de N-R requiere realizar

un numero indeterminado de iteraciones, este proceso es el de mas alto costo

computacional en la metodologıa. Dependiendo del numero de obstaculos en el

mapa, los calculos que se realizan en este proceso son mas grandes. Por lo tanto,

el tiempo de calculo en cada iteracion del metodo de N-R depende del numero de

obstaculos en el entorno.

4.4. Discusion de resultados 65

(a) Tiempo de calculo por iteracion de N-R vs. numero de obstaculos en C++.

(b) Tiempo de calculo por iteracion de N-R vs. numero de obstaculos en Maple 13 yARM.

Figura 4.10: Tiempo de calculo por iteracion de N-R para cada mapa de obstacu-los.

En las Figuras 4.10a y 4.10b se muestra la dependencia de cada iteracion de

N-R con respecto al numero de obstaculos. Para la metodologıa en lenguaje C++

implementada en PC y microcontrolador ARM la grafica muestra una dependen-

cia lineal. Sin embargo, para Maple 13 esta caracterıstica no se cumple debido a

manejo de datos de este software. No obstante, se puede decir que la metodologıa

propuesta en lenguaje C++ tiene una dependencia lineal con respecto al numero

de obstaculos.


4.5. Comparacion cualitativa con metodos exis-

tentes

Con todos los casos de estudio realizados se determinaron las caracterısticas de

la metodologıa con el fin de realizar una comparacion con los metodos existentes.

Debido a que la naturaleza del EHPPM es un metodo matematico no es posible

compararlo cuantitativamente con los diversos metodos y algoritmos reportados.

Sin embargo, es posible realizar una comparacion cualitativa de las principales ca-

racterısticas de la metodologıa propuesta en esta tesis y los algoritmos existentes,

como se muestra en la tabla 4.4.

Metodo Repetible Obstaculos Velocidad Post Trayectoriaprocesado exitosa

Bug Sı Cualquier Muy rapido No NoGrafos de Sı Polıgonos Muy rapido Sı SıvisibilidadDescom- Sı Cuadrados Lento Sı Depende deposicion en la resolucionceldasCampospotenciales No Cualquier Muy rapido No NoMapas No Cualquier Lento Sı Probabi-probabi- lısticalısticosRRT Sı Cualquier Rapido Sı Probabi-

lısticaMetodologıa Sı Rectangulos Muy rapido No Sıpropuesta y cırculos

Tabla 4.4: Comparacion cualitativa de las principales caracterısticas de los meto-dos de planeacion de trayectorias libres de colision.

La metodologıa que se propone en esta tesis es rapida en ejecucion, ademas

las trayectorias obtenidas para cada configuracion de parametros son unicas, por

lo tanto, el metodo es repetible. Los metodos RRT y los basados en grafos requie-

ren de una etapa de post-procesado ya que las trayectorias obtenidas con estos

metodos son representadas por segmentos de lınea [8, 9, 10, 11]. En contraste, la

metodologıa propuesta por su naturaleza traza curvas suaves.

Por un lado, el exito del algoritmo de descomposicion en celdas depende del ta-

4.6. Conclusiones 67

mano de la resolucion de la cuadricula [11]. Por otro lado, los algoritmos Bug

y el de campos potenciales artificiales no garantiza encontrar con exito una tra-

yectoria libre de colisiones y para los metodos de enfoque estocastico el exito

es probabilıstico [8, 9, 10]. La metodologıa aquı propuesta demostro ser eficaz al

obtener trayectorias libres de colisiones para todos los casos de estudio reportados.

4.6. Conclusiones

En esta seccion se explico proceso de la metodologıa de trazado de trayecto-

rias con un ejemplo numerico. Se probo la efectividad de esta manejando mapas

de obstaculos en entornos cerrados demostrando su versatilidad en aplicaciones

industriales y de uso domestico. Se valido la robustez de la misma manejando

mapas de entorno complejos con decenas de obstaculos. Se mostro la eficiencia de

la metodologıa con radio variable ya que presenta una disminucion significativa

de tiempo de calculo total al ser implementada en lenguaje C++. Lo anterior

convierte a la metodologıa en una herramienta muy importante para aplicaciones

en robotica movil. Ademas, se presento la comparacion cualitativa de las princi-

pales caracterısticas de la metodologıa propuesta con los metodos de planeacion

de trayectoria existentes. Se demostro la dependencia lineal entre el numero de

obstaculos en el mapa de entorno y el tiempo de calculo de la metodologıa en

lenguaje C++ implementada en PC y microcontrolador. El sistema embebido de-

mostro tener un tiempo de calculo apropiado para ser utilizado en el modulo de

navegacion de un robot real. Para todos los mapas de obstaculos presentados en

esta seccion la metodologıa obtuvo una trayectoria exitosa libre de colisiones del

punto de inicio al punto meta.


CAPITULO 5

Conclusiones

En esta tesis se presenta un metodo para la planeacion de trayectorias libres

de colisiones tomando como base las bondades presentes en los metodos de con-

tinuacion. La aportacion principal de este trabajo ha sido desarrollar una nueva

metodologıa para el trazado de trayectorias libres de colisiones y al mismo tiempo

una tecnica para corregir los efectos de reversion en el seguimiento.

Se ha presentado un estudio completo de la nueva metodologıa y se presentan

un conjunto de propiedades de cada uno de los parametros que intervienen en esta.

Al mismo tiempo, se estudio el comportamiento de las trayectorias homotopicas

ante todo tipo de variaciones de parametros de repulsion y rectas auxiliares.

La metodologıa de planeacion de trayectorias libres de colisiones usando meto-

dos de continuacion presenta nuevas e interesantes propiedades. Antes que nada,

logra obtener curvas suaves a diferencia de los metodos usados comunmente que

proveen trayectorias de lıneas a trozos. Asimismo, la complejidad del metodo tie-

ne una relacion lineal con el numero de obstaculos en el mapa de entorno. Con

una apropiada eleccion de parametros de repulsion, se pueden trazar trayectorias

optimas. Otra de las propiedades fundamentales de la nueva metodologıa es po-

seer una tecnica de correccion de efectos de reversion presente en el seguimiento

esferico de trayectorias.

La creacion de una herramienta de trazado de trayectorias libres de colisiones

es una aportacion muy importante en el campo de robotica movil. Otro resultado

de esta tesis es la implementacion de la metodologıa en un sistema embebido es-

pecialmente disenado para probar la versatilidad de esta en aplicaciones de robots

69

70 Capıtulo 5. Conclusiones

reales. Ademas, el metodo de trazado se mejoro incluyendo un metodo de radio

variable que reduce el numero de pasos del seguimiento.

Dentro de esta tesis se ha aplicado la metodologıa de planeacion de trayec-

torias a una serie de casos de estudio para mostrar su alcance, con el objeto de

probar que el metodo puede ser utilizado en multiples aplicaciones.

La metodologıa propuesta en esta tesis es solo el inicio de un nuevo metodo

para el trazado de trayectorias en el area de robotica movil, ya que solo se presenta

el estudio de mapas de obstaculos en entorno bidimensionales.

Aunque la investigacion principal en esta tesis ha sido la homotopıa aplicada

a trazado de trayectorias libres de colisiones, la tecnica de correccion de efecto de

reversion en seguimiento hace del algoritmo esferico una herramienta muy eficaz

que puede ser utilizada en una serie de areas de estudio. Esta metodologıa como la

tecnica de seguimiento esferico con correccion de reversiones presenta interesantes

caracterısticas que las vuelve tema de estudio en otras areas de investigacion.

Trabajo futuro

Realizar el estudio y desarrollo extendido de esta metodologıa en ambientes

tridimensionales es de vital importancia en el area de robotica movil.

Llevar a cabo un estudio del desempeno de esta metodologıa al ser integrada

en el modulo de navegacion de robots reales. Medir el tiempo de respuesta

en el sistema y analisis de su comportamiento en ambientes cuasi estaticos.

APENDICE A

Algoritmo de planeacion de trayectorias

libres de colisiones

En este apendice se describe el algoritmo de planeacion de trayectorias libres

de colisiones implementado en las diferentes plataformas de software. De forma

practica se define la siguiente notacion:

J(HS(x, y, λ)) =

J1,1(x, y, λ) J1,2(x, y, λ) J1,3(x, y, λ)

J2,1(x, y, λ) J2,2(x, y, λ) J2,3(x, y, λ)

J3,1(x, y, λ) J3,2(x, y, λ) J3,3(x, y, λ)

, (A.1)

donde cada termino de la matriz representa

J1,1 =∂H1(x,y,λ)∂x

, J1,2 =∂H1(x,y,λ)

∂y, J1,3 =

∂H1(x,y,λ)∂λ

,

J2,1 =∂H2(x,y,λ)∂x

, J2,2 =∂H2(x,y,λ)

∂y, J2,3 =

∂H2(x,y,λ)∂λ

,

J3,1 =∂Sn(x,y,λ)∂x

, J3,2 =∂Sn(x,y,λ)∂y

, J3,3 =∂Sn(x,y,λ)∂λ

.

(A.2)

En la siguiente tabla se describe el algoritmo utilizado para obtener el vector

71

72 Apendice A. Apendice A

predictor de Euler presentado en el capıtulo 2.

Predictor de EulerEntradas:

(cx, cy, cλ);m1;m2;

if (m1 > m2) thensg = −1;elsesg = +1;end if ;

A =

J1,1(cx, cy, cλ) J1,2(cx, cy, cλ)

J2,1(cx, cy, cλ) J2,2(cx, cy, cλ)

;

∂λ(ρ)∂ρ= −sg (detA) ;

∂x∂ρ

∂y∂ρ

= ∂λ(ρ)∂ρ(A)−1

J1,3(cx, cy, cλ)

J2,3(cx, cy, cλ)

;

~vp =(∂x(ρ)∂ρ

, ∂y(ρ)∂ρ

, ∂λ(ρ)∂ρ

);

μ = ∂λ(ρ)∂ρ;

Salidas:μ;~vp;

Tabla A.1: Metodo de Euler para localizar el vector predictor.

El metodo de Newton-Raphson es una de las partes principales del algoritmo

de planeacion. La Tabla A.2 muestra la descripcion de este metodo contemplando

las condiciones de operacion utilizadas en esta tesis.

73

Newton-Raphson

Entrada:(xp, yp, λp);

Valores iniciales:j = 0;err = 1× 10−6;err1 = 1;

while (j < 40) && (err1 > err)if j = 0 then

(x1, y1, λ1) = (xp, yp, λp)− [J (xp, yp, λp)]−1HS (xp, yp, λp) ;

else

(xj+1, yj+1, λj+1) = (xj, yj, λj)− [J(xj, yj, λj)]−1HS (xj, yj , λj) ;

end if ;err1 = ‖HS (xj+1, yj+1, λj+1) ‖;j = j + 1;

end while;(xi+1, yi+1, λi+1) = (xj+1, yj+1, λj+1) ;

Salidas:j;(xi+1, yi+1, λi+1);

Tabla A.2: Metodo de Newton-Raphson.

En las Tablas A.3 y A.4 se presentan los algoritmos de las tecnicas para detec-

tar y corregir reversiones. En la Tabla A.4 se presenta el algoritmo de la tecnica

utilizada para corregir el efecto de reversion. Cuando se detecta una reversion en el

seguimiento este algoritmo rota el vector en direccion positiva y se evalua Newton-

Raphson. Si se detecta nuevamente reversion, entonces se ejecuta nuevamente una

rotacion en direccion negativa.


Detectar reversion

Entradas:Oi−1;Oi+1;

φi−1 = arc cos(2(x−cx)x‖∇Sn‖

)

Oi−1;

ψi−1 = arc cos(2(y−cy)y‖∇Sn‖

)

Oi−1;

ζi−1 = arc cos(2(λ−cλ)λ‖∇Sn‖

)

Oi−1;

φi+1 = arc cos(2(x−cx)x‖∇Sn‖

)

Oi+1;

ψi+1 = arc cos(2(y−cy)y‖∇Sn‖

)

Oi+1;

ζi+1 = arc cos(2(λ−cλ)λ‖∇Sn‖

)

Oi+1;

if ((φi+1, ψi+1, ζi+1) == (φi−1, ψi−1, ζi−1)) thenrever = 1;

elserever = 0;

end if ;Salida:

rever;

Tabla A.3: Tecnica utilizada para detectar efectos de reversion en la trayectoria.

75

Corregir reversion

Entradas:(xp, yp, λp);(cx, cy, cλ);m1;(a, b);(x0, y0);

Valores iniciales:θ = π

4;

(x′

p, y′

p) = (xp, yp)− (cx, cy);

(x′

rot

y′

rot

)

=

(cos(θ) − sin(θ)sin(θ) cos(θ)

)(x′

p

y′

p

)

; //rotacion” + ”

(xrot, yrot) = (cx, cy) + (x′

rot, y′

rot);

λrot =yrot+m1xrot−(y0+m1x0)

b+m1(a−x0)−y0;

(xp, yp, λp) = (xrot, yrot, λrot);

Newton-Raphson;Oi+1 = (xi+1, yi+1, λi+1)i ;

Detectar reversion;

if (rever == 1)‖(j == 40) then

(x′

rot

y′

rot

)

=

(cos(θ) sin(θ)− sin(θ) cos(θ)

)(x′

p

y′

p

)

; //rotacion”− ”

(xrot, yrot) = (cx, cy) + (x′

rot, y′

rot);

λrot =yrot+m1xrot−(y0+m1x0)

b+m1(a−x0)−y0;

(xp, yp, λp) = (xrot, yrot, λrot);

Newton-Raphson;end if ;

Salida:(xi+1, yi+1, λi+1) ;

Tabla A.4: Tecnica utilizada para corregir efecto de reversion en el seguimiento ymejorar la convergencia del metodo N-R.


La tabla A.5 muestra la descripcion del bloque utilizado para variar el radio

de las esferas durante el seguimiento de la trayectoria.

Radio variable

Entradas:r0;Γ;μi;μi+1;

ri+1 = r0

(

1− (1− Γ) exp−(|μi||μi+1|

))

;

Salidas:ri+1;

Tabla A.5: Metodo de radio variable.

Utilizando todos los bloques descritos en las tablas anteriores se presenta el

algoritmo de la metodologıa de planeacion de trayectorias propuesta en esta tesis.

La tabla A.6 muestra el algoritmo que describe cada el algoritmo del diagrama de

flujo presentado en el capıtulo 3.

77

Metodologıa de trazado de trayectorias libres de colisionEntradas:

m1, m2;(a, b);(x0, y0);Ob1, Ob2, Ob3, . . . , Obk;p1, p2, p3, . . . , pk;imax;r0;Γ;

Valores iniciales:λ = 0;i = 1;μi = 1;

while (i < imax && λ < 1)if (i == 1) then

O1 = (x0, y0, 0);(cx, cy, cλ) = O1;

Vector predictor de Euler;(xp, yp, λp) = (cx, cy, cλ) + r0 ‖~vp‖ ;

end if ;Newton-Raphson;Oi+1 = (xi+1, yi+1, λi+1)i ;if (i > 2) then;Detectar reversion;if (rever == 1)||(j == 40) then;Corregir reversion;

end if ;end if ;(cx, cy, cλ) = (xi+1, yi+1, λi+1)i ;Vector predictor de Euler;Radio variable;Δr = ri − ri+1;(cx, cy, cλ) = (xi, yi, λi) + Δr ‖(xi+1, yi+1, λi+1)− (xi, yi, λi)‖ ;(xp, yp, λp) = (cx, cy, cλ) + ri+1 ‖~vp‖ ;i = i+ 1;

end while;Salidas:

O1, O2, O3, . . . , Oi;plot(O1, O2, O3, . . . , Oi);

Tabla A.6: Metodologıa para el trazado de trayectorias utilizando EHPPM y al-goritmo de seguimiento esferico.


APENDICE B

Hardware

En este apendice se muestra la tabla con las principales caracterısticas de la

tarjeta de desarrollo usada para probar la metodologıa en un sistema embebido

ası como, las caracterısticas principales de la PC utilizada para implementar la

metodologıa en Maple 13 y C++.

ARM R© (Cortex−M4, EK-TM4C129GXL) PC

120 MHz SO: Windows 7

1 MB Memoria Flash Core i5 2.5 GHz

256 KB SRAM 6 GB RAM

2-KB EEPROM

32-bit (Unidad de punto flotante)

4.75 VDC a 5.25VD(Voltaje de alimentacion)

2.25 in x 5 in x 0.75 in (L x W x H)

Tabla B.1: Principales caracterısticas de tarjeta de desarrollo ARM y PC utilizadapara realizar las simulaciones en las distintas plataformas.

79

80 Apendice B. Apendice B

REFERENCIAS

[1] H. Vazquez-Leal, A. Marin-Hernandez, Y. Khan, A. Yildirim, U. Filobello-

Nino, R. Castaneda-Sheissa, V.M. Jimenez-Fernandez. Exploring collision-free

path planning by using homotopy continuation methods. Applied Mathematics

and Computation, vol. 219, 2013, pp. 7514-7532.

[2] Q. Zhao, S. Yan. Collision-free path planning for mobile robots using chaotic

particle swarm optimization. Lecture Notes in Computer Science, vol. 3612,

2005, pp. 632-635.

[3] R. Kala, A. Shukla, R. Tiwari. Robotic path planning in static environment

using hierarchical multi-neuron heuristic search and probability based fitness.

Neurocomputing, vol. 74, 2011, pp. 2314-2335.

[4] Buniyamin N., Wan Ngah W.A.J., Sariff N., Mohamad Z. A Simple Local Path

Planning Algorithm for Autonomous Mobile Robots, International journal of

systems applications, engineering development, vol. 5, 2011, pp. 151-158.

[5] O. Khatib. Real-time obstacle avoidance for manipulators and mobile robots.

The International Journal of Robotics Research, vol. 5, 1986, pp. 90–98.

[6] Min Cheol Lee, Min Gyu Park. Artificial potential field based path planning

for mobile robots using a virtual obstacle concept , Advanced Intelligent

Mechatronics, IEEE/ASME International Conference, vol.2, 2003, pp. 735 -

740.

81

82 Referencias

[7] T. Laue and T. Rofer. A behavior architecture for autonomous mobile robots

based on potential fields. RoboCup 2004: Robot Soccer World Cup VIII, 2005,

pp. 122-133.

[8] Steven M. LaValle.Planning Algorithms. Cambridge University Press, 2006,

pp.153-203.

[9] LaValle and J. J. Kuffnere. Rapidly-exploring random trees: Progress and pros-

pects. S. M. In Proceedings Workshop on the Algorithmic Foundations of

Robotics, 2000.

[10] James J. Kuffner. RRT-Connect: An Effcient Approach to Single-Query Path

Planning. IEEE International Conference on Robotics and Automation, vol.

2, 2000, pp.995-101.

[11] R. Siegwart, I. Nourbakhsh. Introduction to Autonomous Mobile Robots.

The MIT Press, Cambridge, 2004.

[12] Helbert Eduardo Espitia Cuchango. Algoritmo para planear trayectorias de

robots moviles, empleando campos potenciales y enjambres de partıculas activas

brownianas. Ciencia e ingenierıa neogranadina, Bogota, vol.22, 2012, pp. 75 -

96.

[13] Melville RC, Trajkovic L, Fang S-C, Watson LT. Artificial parameter ho-

motopy methods for the DC operating point problem. IEEE Transactions on

Computer-Aided Design of Integrated Circuits and Systems, vol. 12, 1993, pp.

861-877.

[14] Watson LT, Sosonkina M, Melville RC, Morgan AP, Walker HF. Algorithm

777: HOMPACK90: a suite of Fortran 90 codes for globally convergent homo-

topy algorithms. ACM Trans Math Softw, vol.23, 1997, pp. 514-549.

[15] Dyes A, Chan E, Hofmann H, Horia W, Trajkovic L. Simple implementations

of homotopy algorithms for finding DC solutions of nonlinear circuits. ISCAS,

vol.6, 1999, pp. 290-293.

[16] O. Brock, O. Khatib. Real-time replanning in high-dimensional configura-

tion spaces using sets of homotopic paths. IEEE International Conference on

Robotics and Automation, vol. 1, 2000, pp. 550–555.

Referencias 83

[17] H. Vazquez-Leal, R. Castaneda-Sheissa, A. Yildirim, Y. Khan, A. Sarmiento-

Reyes, V. Jimenez-Fernandez, A.L. Herrera-May, U.Filobello-Nino, F. Rabago-

Bernal, C. Hoyos-Reyes. Biparameter Homotopy-based Direct Current Simu-

lation of Multistable Circuits. British Journal of Mathematics Computer

Science, vol.2, 2012, pp. 137-150.

[18] K. Ahuja, L. Watson, S. Billups. Probability-one homotopy maps for mixed

complementarity problems. Computational Optimization and Applications,

vol. 41, 2008, pp. 363–375.

[19] K.S. Gritton, J. Seader, W.-J. Lin. Global homotopy continuation procedures

for seeking all roots of a nonlinear equation. Computers ChemicalEnginee-

ring, vol. 25, 2001, pp. 1003-1019.

[20] T. L. Wayburn, J. D. Seader. Homotopy continuation methots for computer-

aided process design. Computers Chemical Engineering, vol. 11, 1987, pp.

7-25.

[21] Yamamura K. Simple algorithms for tracing solution curves. IEEE Interna-

tional Symposium on Circuits and Systems, vol. 6, 1992, pp. 2801 - 2804.

[22] D. Torres-Munoz, H. Vazquez-Leal, L. Hernandez-Martinez, A. Sarmiento-

Reyes. Improved spherical continuation algorithm with application to the

double-bounded homotopy (DBH), Computational and Applied Mathema-

tics, vol.33, 2014, pp. 147–161.

[23] J. Manuel Oliveros-Munoz, Hugo Jimenez-Islas. Hyperspherical path tracking

methodology as correction step in homotopic continuation methods. Chemical

Engineering Science, vol. 97, 2013, pp. 413-429.

[24] H. Vazquez-Leal, A. Sarmiento-Reyes, Y. Khan, A. Yildirim, U. Filobello-

Nino, R. Castaneda-Sheissa, A.L. Herrera-May, V.M.Jimenez-Fernandez, S.F.

Hernandez-Machuca, L. Cuellar-Hernandez. New aspects of double bounded

polynomial homotopy. British Journal of Mathematics Computer Science,

vol. 3, 2013, pp. 549-566.

[25] H. Jimenez-Islas. SEHPE: Programa para la solucion de sistemas de ecua-

ciones no lineales mediante metodo homotopico con seguimiento hiperesferico.

Avances en Ingenierıa Quımica, vol. 6, 1996, pp.174-179.

84 Referencias

[26] R. Castaneda-Sheissa, H. Vazquez-Leal, A. Yildirim, U. Filobello-Nino, A.

Sarmiento-Reyes, L. Hernandez Martınez. Metodo modificado de hiperesferas

aplicado a homotopıas biparametricas: simulacion de circuitos con transstores

bipolares. XVIII International IBERCHIP Workshop, 2012. pp.160-163.

[27] Hector Vazquez-Leal, Roberto Castaneda-Sheissa, Felipe Rabago-Bernal,

Luis Hernandez-Martınez, Arturo Sarmiento-Reyes, and Uriel Filobello-Nino.

Powering Multiparameter Homotopy-Based Simulation with a Fast Path-

Following Technique. International Scholarly Research Network ISRN Applied

Mathematics, vol. 2011, 2011, Article ID 6106372.

[28] Layne T. Watson. Probability-one homotopies in computational science.

Journal of Computational and Applied Mathematics, vol. 140, 2002, pp. 785-

807.

[29] Eugene L. Allgower and Kurt Georg. Introduction to Numerical Continuation

Methods, Classics in Applied Mathematics SIAM, vol.45, 2003.

[30] Arturo Sarmiento Reyes. A partition method for the deternination of multiple

DC operating points. Delf University press, 1994, pp. 40-45.

[31] Hugo Jimenez-Islas. Paquete computacional para la solucion de sistemas de

ecuaciones no lineales. Tesis de Maestrıa, Instituto Tecnologico de Celaya,

Celaya, Gto, 1988.

[32] Texas Instruments. Tiva r C Series TM4C1294 Connected LaunchPad Eva-

luation Kit EK-TM4C1294XL user’s guide. Technical Documents, Literature

Number: SPMU365A, Marzo 2014.

[33] Damian Darıo Lopez Hernandez. Implementacion en un robot PIONEER 3P-

DX una tecnica de homotopıa libre de colision. Tesis de Maestrıa, Universidad

Veracruzana, Xalapa, Veracruz, Mexico, 2013.

Date post:	04-Sep-2020
Category:	Documents
Upload:	others
View:	1 times
Download:	0 times