Introducción a la Robótica

L. Enrique Sucar
INAOE

Robótica Probabilista, L. E. Sucar 1

10.2 9.2 8.2 6.8 5.8 4.8 3.8 3.4 3

9.8 8.8 7.8 6.8 5.8 4.8 4.4 2.4 2

6.4 5.4 4.4 3.4 1.4 1

8.8 7.8 6.8 4 3 2 1 0
9.2 8.2 7.8 4.4 3.4 2.4 1.4 1

Sesión 7: Planeación de
trayectorias
Introducción a la Robótica
L. Enrique Sucar

Robótica Probabilista, L. E. Sucar 2

 Contendio
• Introducción
• Técnicas clásicas
– Búsqueda en grafos
– Programación dinámica
– Diagramas de Voronoi
– Grafos de visibilidad
– Campos potenciales

Robótica Probabilista, L. E. Sucar 3

 Planeación de Trayectorias
• Determinar una trayectoria en el espacio
de configuraciones, entre una
configuración inicial (inicio) y una
configuración final (meta), de forma que el
robot no colisione con los obstáculos y
cumpla con las restricciones cinemáticas
del robot

Robótica Probabilista, L. E. Sucar 4

 Planeación de Trayectorias
• Consideraciones adicionales:
– Trayectoria de distancia (tiempo o costo)
mínimo
– Ambientes semidesconocidos
– Ambientes dinámicos
– Restricciones adicionales
– Eficiencia (algoritmos anytime)

Robótica Probabilista, L. E. Sucar 5

 Plan
• Un plan es un conjunto de acciones
(operadores) que permiten a un agente
(robot) ir de un estado inicial a un
estado final o meta
• Los elementos básicos para hacer un
plan son:
– Estados (p. ej. Posición del robot),
incluyendo el estado incial y el estado
meta
– Operadores: acciones que llevan de un
estado a otro, Si Æ Sj
Robótica Probabilista, L. E. Sucar 6
 Ejemplo de Plan
• Considerando el mapa de rejilla:
• Estados:
– posición X,Y en el mapa
– Estado inicial: 0,0
– Estado meta (luz): Xm, Ym
• Acciones:
– Movimiento a alguna de las celdas vecinas
– X+1, Y+1, X-1, Y-1

Robótica Probabilista, L. E. Sucar 7

 Ejemplo de Plan
• Plan: buscar una serie de acciones básicas
que lleven al robot de la posición inicial a la
meta

Robótica Probabilista, L. E. Sucar 8

 Ejemplo: plan en el mapa
topológico
• Plan = búsqueda de una trayectoria en el
grafo, del nodo inicial al meta

Robótica Probabilista, L. E. Sucar 9

 Consideraciones iniciales
• Un robot rígido usualmente modelado
como un “punto”
• Un ambiente estático con obstáculos
conocidos
• Un robot capaz de navegar en segmentos
de línea recta y de mantener su
localización

Robótica Probabilista, L. E. Sucar 10

 Técnicas Clásicas
• Espacios discretos
– Búsqueda en grafos
– Programación dinámica
– Diagramas de Voronoi
– Grafos de visibilidad
• Espacios continuos
– Campos potenciales

Robótica Probabilista, L. E. Sucar 11

Ejemplo – técnicas de
búsqueda

A B C
M
I

D E F

Robótica Probabilista, L. E. Sucar 12

 Ejemplo: ESTADOS Y DISTANCIAS

A 4 B 4 C
3
5 5
I
M
4 3
D E F
2 4
Robótica Probabilista, L. E. Sucar 13
 ÁRBOL DE BÚSQUEDA

I
A D

B D A E

C E E B B F
11
D F B F C E A C M
14 17 15 15 13
M C M F
19 19 17 M
Robótica Probabilista, L. E. Sucar 14
25
Depth first - backtracking (LIFO)

Crea una agenda de un elemento (el nodo raíz)

hasta que la agenda este vacía o se alcance la meta
si el primer elemento es la meta
entonces acaba
si no elimina el primer elemento y
añade sus sucesores al frente de la agenda

Robótica Probabilista, L. E. Sucar 15

 DEPTH-FIRST SEARCH

(I)
I
(A D) A D

(B D D) B D A E

(C E D D) C E E B B F
11
(E D D) D F B F C E A C M
14 M 17 F 15 15 13
C M
(D F D D)
19 19 17
M
(F D D) 25

(M D D)
Robótica Probabilista, L. E. Sucar 16
 Breadth first

Crea una agenda de un elemento (el nodo raíz)

hasta que la agenda este vacía o se alcance la meta
si el primer elemento es la meta
entonces acaba
si no elimina el primer elemento y
añade sus sucesores al final de la agenda

Problemas: árboles con arborescencia muy grande

Robótica Probabilista, L. E. Sucar 17

 ÁRBOL DE BÚSQUEDA

I
A D

B D A E

C E E B B F
11
D F B F C E A C M
14 17 15 15 13
M C M F
19 19 17 M
Robótica Probabilista, L. E. Sucar 18
25
 Algoritmos con Información

Hill-Climbing
Crea una agenda de un elemento (el nodo raíz)
hasta que la agenda este vacía o se alcance la meta
si el primer elemento es la meta
entonces acaba
si no elimina el primer elemento y añade sus
sucesores a la agenda
ordena todos los elementos de la agenda
selecciona el mejor y elimina el resto

Robótica Probabilista, L. E. Sucar 19

 BÚSQUEDA HILL-CLIMBING

Hill climbing
Heurística: ve a la ciudad más cercana

(I) I
A D

(A D) B D A E

C E E B B F
(D B) 11
D F B F C E A C M
14 M 17 F 15 15 13
(E C) C M
19 19 17
M
25
Robótica Probabilista, L. E. Sucar 20
Beam search

Crea una agenda de un elemento (el nodo raíz)

hasta que la agenda este vacía o se alcance la meta
si el primer elemento es la meta
entonces acaba
si no elimina el primer elemento y añade sus
sucesores a la agenda
ordena todos los elementos de la agenda y
selecciona los N mejores (los demás eliminalos)

Robótica Probabilista, L. E. Sucar 21

A*: utiliza dos medidas

Idea: usar estimaciones de los costos/distancias

que faltan junto con los costos/distancias
acumuladas

estim(total) = costo(camino recorrido) +

estim(camino que falta)

Robótica Probabilista, L. E. Sucar 22

Las estimaciones no son perfectas, por lo que se
usan sub-estimaciones

subestim(total) = costo(camino recorrido) +

subestim(camino que falta)

De nuevo expande hasta que los demás tengan sub-

estimaciones más grandes (v.g., subestimaciones
de distancias entre ciudades pueden ser líneas
rectas)

Robótica Probabilista, L. E. Sucar 23

Crea una agenda de un elemento (el nodo raíz)
hasta que la agenda este vacía o se alcance la meta y
los demás nodos sean de costos mayores o iguales
a la meta
si el primer elemento es la meta y los demás nodos
son de menor o igual costo a la meta
entonces acaba
si no elimina el primer elemento y añade sus
sucesores a la agenda
ordena todos los elementos de la agenda de
acuerdo al costo acumulado más las
subestimaciones de los que falta

Robótica Probabilista, L. E. Sucar 24

EJEMPLO DE BÚSQUEDA A*

Robótica Probabilista, L. E. Sucar 25

A*
1 I

13.4 A D 12.9

I
2

13.4 A D

19.4 A E 12.9

Robótica Probabilista, L. E. Sucar 26

 3 I

13.4 A D

19.4 A E

17.7 B F 13.0

Robótica Probabilista, L. E. Sucar 27

 I
4

D
13.4 A

19.4 A E

17.7 B F

13.0 M

Robótica Probabilista, L. E. Sucar 28

 Programación dinámica
• Procedimiento iterativo (recursivo) para
evaluar el costo de la trayectoria mínima
de cualquier punto a la meta
• Se considera un ambiente discreto y un
costo de moverse de un sitio (celda) a otro
• El algoritmo básico se conoce como
“iteración de valor”

Robótica Probabilista, L. E. Sucar 29

 Algoritmo de Programación Dinámica
V(x,y) : Costo acumulado de viaje a la celda más
cercana, si el robot está en la celda (x,y)

Inicio:
Vi(x,y) = 0 si la celda (x,y) es la meta
= infinito demás celdas

Regla de actualización:

Vi(x,y) =min m(dx,dy) en Mov{Vj(x+dx,y+dy) (Sig. celda)

+ Cmov((x,y),(dx,dy))} (avanzar)
Robótica Probabilista, L. E. Sucar 30
Ejemplo – programación
dinámica

2.4 2

1.4 1

2 1 0
2.4 1.4 1
Robótica Probabilista, L. E. Sucar 31
Ejemplo – programación
dinámica

10.2 9.2 8.2 6.8 5.8 4.8 3.8 3.4 3

9.8 8.8 7.8 6.8 5.8 4.8 4.4 2.4 2

6.4 5.4 4.4 3.4 1.4 1

8.8 7.8 6.8 4 3 2 1 0
9.2 8.2 7.8 4.4 3.4 2.4 1.4 1
Robótica Probabilista, L. E. Sucar 32
Ejemplo – programación
dinámica

10.2 9.2 8.2 6.8 5.8 4.8 3.8 3.4 3

9.8 8.8 7.8 6.8 5.8 4.8 4.4 2.4 2

6.4 5.4 4.4 3.4 1.4 1

8.8 7.8 6.8 4 3 2 1 0
9.2 8.2 7.8 4.4 3.4 2.4 1.4 1
Robótica Probabilista, L. E. Sucar 33
Ejemplo de programación dinámica

Se considera un conjunto de celdas (road-map) libres

de obstáculos obtenidas previamente

V(x,y) (pixel oscuros

I G denotan valores altos)

Robótica Probabilista, L. E. Sucar 34

 Programación dinámica – otros costos
Vi(x,y) : Costo acumulado de viaje a la celda más cercana,
si el robot está en la celda (x,y) con orientación θi

Inicio:
Vi(x,y) = 0 si la celda (x,y) es la meta
= infinito otras celdas (x,y)

Regla de actualización:

Vi(x,y) = min m_j=(dx,dy) en Mov{Vj(x+dx,y+dy)

(Sig. celda)
+ Cmov((x,y),(dx,dy)) (avanzar)
+ Cg(i,j)} (girar)
Robótica Probabilista, L. E. Sucar 35
El costo por avanzar incluye:
 Distancia entre celdas

 Costo de la celda destino de acuerdo a su

tipo

Celdas ocupadas

Celdas de advertencia

Celdas de viaje

Celdas lejanas

Espacio de viaje
Robótica Probabilista, L. E. Sucar 36
 Diagrama de Voronoi
• El diagrama de Voronoi es el conjunto de
puntos equidistantes a los límites de 2 o más
obstáculos
• Siguiendo el diagrama de Voronoi el robot
maximiza la distancia a los obstáculos
• Se puede utilizar un diagrama aproximado
considerando una distancia máxima a los
obstáculos de forma que se consideren las
limitaciones de los sensores

Robótica Probabilista, L. E. Sucar 37

Ejemplos de diagramas de
Voronoi

Robótica Probabilista, L. E. Sucar 38

 Ejemplo de Diagrama de Voronoi
 Izquierda – mapa del espacio libre
 Derecha – diagrama de Voronoi (aproximado)

Roadmap

Robótica Probabilista, L. E. Sucar 39

 Gráficas de visibilidad
• Técnica que produce una trayectoria de
longitud mínima mediante la solución de
un grafo (búsqueda)
• Se basa en la construcción de un grafo
de visibilidad, G = (V, E)
– Los vértices, V, corresponden a las esquinas
de todos los obstáculos (poligonales), el
inicio y la meta
– Los arcos, E, conectan todos los vértices
que son visibles entre sí
Robótica Probabilista, L. E. Sucar 40
Gráficas de visibilidad

Robótica Probabilista, L. E. Sucar 41

 Gráficas de visibilidad
• Existen algoritmos eficientes para construir el
grafo de visibilidad y hacer la búsqueda de la
trayectoria óptima
• La desventaja es que las trayectorias pasan
muy cerca de los obstáculos y consideran un
robot puntual – se puede aminorar este
problema dilatando a los obstáculos en función
del tamaño del robot (problema con robots no
cilíndricos)

Robótica Probabilista, L. E. Sucar 42

 Campos de Potencial
• Se basa en una analogía con campos
potenciales eléctricos:
– El robot se ve como una partícula con carga
eléctrica
– El espacio libre se considera como un campo
potencial
– Los obstáculos tienen un carga eléctrica del
mismo signo del robot (se repelen)
– La meta tiene una carga eléctrica de signo
opuesto al robot (se atraen)
Robótica Probabilista, L. E. Sucar 43
Campos de Potencial

Robótica Probabilista, L. E. Sucar 44

Campos de Potencial

Robótica Probabilista, L. E. Sucar 45

 Campos de Potencial
• El campo de potencial diferencial se
construye sumando el campo de la meta,
Ug, y el campo de los obstáculos, Uo:
U(q) = Ug(q) + Σ Uo(q)
• A partir de este campo se construye un
campo de fuerzas artificial:
F = - ∇ U(q) = ( dU/dx )
( dU/dy )
Robótica Probabilista, L. E. Sucar 46
 Campos de Potencial

• Una vez construido el campo de

fuerzas, el robot se mueve en función
de la fuerza local (similar al caso de los
valores en programación dinámica)
• Con esto se tiene un esquema robusto
que implícitamente tiene un plan de
cualquier punto del espacio a la meta

Robótica Probabilista, L. E. Sucar 47

Campos de Fuerzas

Robótica Probabilista, L. E. Sucar 48

 Funciones de Potencial
• Para obtener las fuerzas hay que modelar el
las funciones de potencial de la meta y
obstáculos – calculando el potencial para
cada punto del espacio libre:
• Meta – “atractor parabólico”
Ug(q) = k1 dist(q, meta)2
• Obstáculo – “barrera potencial exponencial”
Uo(q) = k2 dist(q, obstáculo)-1
Robótica Probabilista, L. E. Sucar 49
 Funciones de Potencial
• Ventajas:
– Se pueden generar trayectorias en tiempo
real a partir del campo de fuerzas
– Las trayectorias generadas son suaves
– Facilita el acoplar la parte de planeación
con control
• Desventajas:
– Mínimos locales!

Robótica Probabilista, L. E. Sucar 50

Trampas (mínimos locales)

Robótica Probabilista, L. E. Sucar 51

 Mínimos Locales
• Estrategias para evitar trampas:
– Backtracking
– Movimientos aleatorios
– Usar otra estrategia, como seguimiento de
paredes
– Aumentar el potencial repulsivo de
regiones visitadas

Robótica Probabilista, L. E. Sucar 52

 Referencias
• [Dudek, Jenkin] – Cap 5
• [Siegwart et al.] – Cap 6
• [Thrun et al.] – Cap 6
• Romero et al., Exploration and navigation
for mobile robots with perceptual
limitations, IJARS, 2006

Robótica Probabilista, L. E. Sucar 53

 Actividades
• Práctica 3: Mapas y Planeación
– Representa el ambiente mediante un mapa de celdas
de ocupación (dilatar obstáculos)
– En base a dicho mapa desarrolla un algoritmo para
encontrar una ruta de una celda inicial a una final en
base a programación dinámica
– Haz que el robot “ejecute” la ruta encontrada
– Prueba tu sistema en diferentes ambientes con
diferentes posiciones iniciales y finales
– Opcional: incorpora “costos” a las celdas de acuerdo
a su distancia a los obstáculos (u otros factores) y
repite el mismo procedimiento
Robótica Probabilista, L. E. Sucar 54