introducción a la robótica - ciencias computacionalesesucar/clases-irob/ir8-plan.pdfhasta que la...
TRANSCRIPT
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