robótica probabilista

Robótica Probabilista, L. E. Sucar 1

Introducción a la Robótica

L. Enrique SucarINAOE

Robótica Probabilista, L. E. Sucar 2

0

11.4

1

1.4 1

2.4

2.4

2

23

3.4

3

3.4

3.43.84.8

4.44.8

4.4

4.4

5.8

4

6.8

5.8

5.46.4

6.8

6.8

7.88.2

7.8

7.8

8.29.2

8.8

8.8

9.2

10.2

9.8

Sesión 7: Planeación de trayectorias

Introducción a la RobóticaL. Enrique Sucar

Robótica Probabilista, L. E. Sucar 3

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 4

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 5

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 6

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 7

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 8

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 9

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 10

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 11

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 12

Ejemplo – técnicas de búsqueda

I

D E F

MCBA

Robótica Probabilista, L. E. Sucar 13

A B C

I

D E F

M

3

34

4 4

42

5 5

Ejemplo: ESTADOS Y DISTANCIAS

Robótica Probabilista, L. E. Sucar 14

I

A D

B D A E

EC E B B F

D F B F C E A C M

M MC FM

11

14

19 19 17

17

25

15 15 13

ÁRBOL DE BÚSQUEDA

Robótica Probabilista, L. E. Sucar 15

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 acabasi no elimina el primer elemento y

añade sus sucesores al frente de la agenda

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 acabasi no elimina el primer elemento y

añade sus sucesores al frente de la agenda

Robótica Probabilista, L. E. Sucar 16

DEPTH-FIRST SEARCH

(I)

(A D)

(B D D)

(C E D D)

(E D D)

(D F D D)

(F D D)

(M D D)

IA D

B D A E

EC E B B F

D F B F C A C M

M MC F

M

11

14

19 19 17

17

25

15 15 13E

Robótica Probabilista, L. E. Sucar 17

Breadth first 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 acabasi no elimina el primer elemento y

añade sus sucesores al final de la agenda

Problemas: árboles con arborescencia muy grande

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 acabasi 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 18

I

A D

B D A E

EC E B B F

D F B F C E A C M

M MC FM

11

14

19 19 17

17

25

15 15 13

ÁRBOL DE BÚSQUEDA

Robótica Probabilista, L. E. Sucar 19

Algoritmos con InformaciónAlgoritmos con Información

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 acabasi no elimina el primer elemento y añade sus

sucesores a la agendaordena todos los elementos de la agenda selecciona el mejor y elimina el resto

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 acabasi no elimina el primer elemento y añade sus

sucesores a la agendaordena todos los elementos de la agenda selecciona el mejor y elimina el resto

Hill-ClimbingHill-Climbing

Robótica Probabilista, L. E. Sucar 20

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

(I)

(A D)

(D B)

(E C)

IA D

B D A E

EC E B B F

D F B F C A C M

M MC F

M

11

14

19 19 17

17

25

15 15 13E

BÚSQUEDA HILL-CLIMBING

Robótica Probabilista, L. E. Sucar 21

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 acabasi no elimina el primer elemento y añade sus

sucesores a la agendaordena todos los elementos de la agenda y selecciona los N mejores (los demás eliminalos)

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 acabasi no elimina el primer elemento y añade sus

sucesores a la agendaordena todos los elementos de la agenda y selecciona los N mejores (los demás eliminalos)

Robótica Probabilista, L. E. Sucar 22

A*: utiliza dos medidas

Idea: usar estimaciones de los costos/distanciasque faltan junto con los costos/distanciasacumuladas

A*: utiliza dos medidas

Idea: usar estimaciones de los costos/distanciasque faltan junto con los costos/distanciasacumuladas

estim(camino que falta) estim(camino que falta) ++costo(camino recorrido)costo(camino recorrido)estim(total) =estim(total) =

Robótica Probabilista, L. E. Sucar 23

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

subestim(total) =subestim(total) =subestim(camino que falta)subestim(camino que falta)

++costo(camino recorrido)costo(camino recorrido)

De nuevo expande hasta que los demás tengan sub-estimaciones más grandes (v.g., subestimacionesde distancias entre ciudades pueden ser líneasrectas)

De nuevo expande hasta que los demás tengan sub-estimaciones más grandes (v.g., subestimacionesde distancias entre ciudades pueden ser líneasrectas)

Robótica Probabilista, L. E. Sucar 24

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 igualesa la meta

si el primer elemento es la meta y los demás nodosson de menor o igual costo a la meta

entonces acabasi no elimina el primer elemento y añade sus

sucesores a la agendaordena todos los elementos de la agenda de

acuerdo al costo acumulado más lassubestimaciones de los que falta

Robótica Probabilista, L. E. Sucar 25

EJEMPLO DE BÚSQUEDA A* EJEMPLO DE BÚSQUEDA A*

Robótica Probabilista, L. E. Sucar 26

A*1 I

A D13.4 12.9

I

A D

A E

2

13.4

19.4 12.9

Robótica Probabilista, L. E. Sucar 27

I

A D

EE

F

3

B

13.4

A19.4

17.7 13.0

Robótica Probabilista, L. E. Sucar 28

I

A D

EE

F

13.0

B

13.4

A19.4

17.7

M

4

Robótica Probabilista, L. E. Sucar 29

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 30

Algoritmo de Programación DinámicaV(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 31

Ejemplo – programación dinámica

0

11.4

1

1.4 1

2.4

2.4

2

2

Robótica Probabilista, L. E. Sucar 32

Ejemplo – programación dinámica

0

11.4

1

1.4 1

2.4

2.4

2

23

3.4

3

3.4

3.43.84.8

4.44.8

4.4

4.4

5.8

4

6.8

5.8

5.46.4

6.8

6.8

7.88.2

7.8

7.8

8.29.2

8.8

8.8

9.2

10.2

9.8

Robótica Probabilista, L. E. Sucar 33

Ejemplo – programación dinámica

0

11.4

1

1.4 1

2.4

2.4

2

23

3.4

3

3.4

3.43.84.8

4.44.8

4.4

4.4

5.8

4

6.8

5.8

5.46.4

6.8

6.8

7.88.2

7.8

7.8

8.29.2

8.8

8.8

9.2

10.2

9.8

Robótica Probabilista, L. E. Sucar 34

Ejemplo de programación dinámica

Se considera un conjunto de celdas (road-map) libresde obstáculos obtenidas previamente

V(x,y) (pixel oscurosdenotan valores altos)GI

Robótica Probabilista, L. E. Sucar 35

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 36

El costo por avanzar incluye:Distancia entre celdasCosto de la celda destino de acuerdo a sutipo

Celdas ocupadas

Celdas de advertencia

Celdas de viaje

Celdas lejanas

Espacio de viaje

Robótica Probabilista, L. E. Sucar 37

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 38

Ejemplos de diagramas de Voronoi

Robótica Probabilista, L. E. Sucar 39

Ejemplo de Diagrama de Voronoi

Izquierda – mapa del espacio libreDerecha – diagrama de Voronoi (aproximado)

Roadmap

Robótica Probabilista, L. E. Sucar 40

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 41

Gráficas de visibilidad

Robótica Probabilista, L. E. Sucar 42

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 43

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 44

Campos de Potencial

Robótica Probabilista, L. E. Sucar 45

Campos de Potencial

Robótica Probabilista, L. E. Sucar 46

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 47

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 48

Campos de Fuerzas

Robótica Probabilista, L. E. Sucar 49

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 50

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 51

Trampas (mínimos locales)

Robótica Probabilista, L. E. Sucar 52

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 53

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 54

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