Introducción a la navegación reactiva y a la planificación de misiones
MT3005 - Robótica 1
Ejecutando la planificación en un mundo dinámico
planificación de movimiento
+ control de trayectorias
NO fue parte de la planificación original
NO fue parte de la planificación original
¿Qué debe hacer el robot?
¿Qué debe hacer el robot?
Para no colisionar con el peatón, el robot debe cambiar su comportamiento de ir hacia la meta por uno de evadir obstáculos.
¿Qué debe hacer el robot?
Para no colisionar con el peatón, el robot debe cambiar su comportamiento de ir hacia la meta por uno de evadir obstáculos.
Esto, sin embargo, va en contra de la perspectiva que hemos empleado por el momento para sistemas de control.
¿Qué debe hacer el robot?
Para no colisionar con el peatón, el robot debe cambiar su comportamiento de ir hacia la meta por uno de evadir obstáculos.
Esto, sin embargo, va en contra de la perspectiva que hemos empleado por el momento para sistemas de control.
¿Por qué?
{^I}\dot{\boldsymbol{\xi}}={^I}\mathbf{\mathrm{Rot}}_B(\theta)\mathbf{A}^\dagger\mathbf{B}\mathbf{v}=\mathbf{f}\left({^I}\boldsymbol{\xi},\mathbf{v}\right)
{^I}\dot{\boldsymbol{\xi}}={^I}\mathbf{\mathrm{Rot}}_B(\theta)\mathbf{A}^\dagger\mathbf{B}\mathbf{v}=\mathbf{f}\left({^I}\boldsymbol{\xi},\mathbf{v}\right)
\mathbf{v}(\boldsymbol{\xi})
\lim_{t\to\infty} \boldsymbol{\xi}(t)=\boldsymbol{\xi}_\mathrm{ss}
{^I}\dot{\boldsymbol{\xi}}={^I}\mathbf{\mathrm{Rot}}_B(\theta)\mathbf{A}^\dagger\mathbf{B}\mathbf{v}=\mathbf{f}\left({^I}\boldsymbol{\xi},\mathbf{v}\right)
\mathbf{v}(\boldsymbol{\xi})
\lim_{t\to\infty} \boldsymbol{\xi}(t)=\boldsymbol{\xi}_\mathrm{ss}
Esto implica que el comportamiento se termina de ejecutar "eventualmente" (i.e. asintóticamente*), y asume que el agente siempre estará ejecutando el mismo comportamiento.
{^I}\dot{\boldsymbol{\xi}}={^I}\mathbf{\mathrm{Rot}}_B(\theta)\mathbf{A}^\dagger\mathbf{B}\mathbf{v}=\mathbf{f}\left({^I}\boldsymbol{\xi},\mathbf{v}\right)
\mathbf{v}(\boldsymbol{\xi})
\lim_{t\to\infty} \boldsymbol{\xi}(t)=\boldsymbol{\xi}_\mathrm{ss}
Esto implica que el comportamiento se termina de ejecutar "eventualmente" (i.e. asintóticamente*), y asume que el agente siempre estará ejecutando el mismo comportamiento.
*aunque en algunos casos llega a ejecutarse lo suficientemente cerca en tiempo finito
¿Qué necesitamos?
seguir el camino
esquivar obstáculos
obstáculo detectado
sin obstáculos
¿Qué necesitamos?
seguir el camino
esquivar obstáculos
obstáculo detectado
sin obstáculos
¿Qué necesitamos?
seguir el camino
esquivar obstáculos
obstáculo detectado
sin obstáculos
¿Qué necesitamos?
seguir el camino
esquivar obstáculos
obstáculo detectado
sin obstáculos
Es decir, no fue suficiente con la planificación, sino que tuvo que complementarse con un comportamiento reactivo para esquivar obstáculos y una lógica que permita el cambio entre comportamientos.
Esto es distinto a la perspectiva "tradicional" de sistemas dinámicos que hemos empleado por el momento.
\(\Rightarrow\) Necesitamos un nuevo modelo.
Preámbulo histórico: behavior-based robotics
El nuevo modelo
\dot{\mathbf{x}}=\mathbf{f}_\mathbf{q}(\mathbf{x},\mathbf{u})
El nuevo modelo
\dot{\mathbf{x}}=\mathbf{f}_\mathbf{q}(\mathbf{x},\mathbf{u})
estado continuo
El nuevo modelo
\dot{\mathbf{x}}=\mathbf{f}_\mathbf{q}(\mathbf{x},\mathbf{u})
estado continuo
estado discreto
dinámica continua que cambia acorde al estado discreto del sistema
El nuevo modelo
\dot{\mathbf{x}}=\mathbf{f}_\mathbf{q}(\mathbf{x},\mathbf{u})
estado continuo
estado discreto
Recibe el nombre de sistema dinámico híbrido.
dinámica continua que cambia acorde al estado discreto del sistema
El nuevo modelo
\dot{\mathbf{x}}=\mathbf{f}_\mathbf{q}(\mathbf{x},\mathbf{u})
estado continuo
estado discreto
dinámica continua que cambia acorde al estado discreto del sistema
En otras disciplinas también se conocen como Variable Structure Systems (VSS) o Switched-Mode Systems.
La perspectiva completa
\dot{\mathbf{x}}=\mathbf{f}_1(\mathbf{x},\mathbf{u})
\dot{\mathbf{x}}=\mathbf{f}_2(\mathbf{x},\mathbf{u})
\dot{\mathbf{x}}=\mathbf{f}_3(\mathbf{x},\mathbf{u})
q=1
q=2
q=3
\mathbf{x}\coloneqq \mathbf{x}_0
\mathbf{x} \in G_{1,2}
\mathbf{x} \in G_{2,1}
\mathbf{x} \in G_{2,3}
\mathbf{x} \in G_{3,1}
\mathbf{x} \coloneqq R_{1,2}(\mathbf{x})
\mathbf{x} \coloneqq R_{2,1}(\mathbf{x})
\mathbf{x} \coloneqq R_{2,3}(\mathbf{x})
\mathbf{x} \coloneqq R_{3,1}(\mathbf{x})
La perspectiva completa
\dot{\mathbf{x}}=\mathbf{f}_1(\mathbf{x},\mathbf{u})
\dot{\mathbf{x}}=\mathbf{f}_2(\mathbf{x},\mathbf{u})
\dot{\mathbf{x}}=\mathbf{f}_3(\mathbf{x},\mathbf{u})
q=1
q=2
q=3
\mathbf{x}\coloneqq \mathbf{x}_0
\mathbf{x} \in G_{1,2}
\mathbf{x} \in G_{2,1}
\mathbf{x} \in G_{2,3}
\mathbf{x} \in G_{3,1}
\mathbf{x} \coloneqq R_{1,2}(\mathbf{x})
\mathbf{x} \coloneqq R_{2,1}(\mathbf{x})
\mathbf{x} \coloneqq R_{2,3}(\mathbf{x})
\mathbf{x} \coloneqq R_{3,1}(\mathbf{x})
modos de operación
La perspectiva completa
\dot{\mathbf{x}}=\mathbf{f}_1(\mathbf{x},\mathbf{u})
\dot{\mathbf{x}}=\mathbf{f}_2(\mathbf{x},\mathbf{u})
\dot{\mathbf{x}}=\mathbf{f}_3(\mathbf{x},\mathbf{u})
q=1
q=2
q=3
\mathbf{x}\coloneqq \mathbf{x}_0
\mathbf{x} \in G_{1,2}
\mathbf{x} \in G_{2,1}
\mathbf{x} \in G_{2,3}
\mathbf{x} \in G_{3,1}
\mathbf{x} \coloneqq R_{1,2}(\mathbf{x})
\mathbf{x} \coloneqq R_{2,1}(\mathbf{x})
\mathbf{x} \coloneqq R_{2,3}(\mathbf{x})
\mathbf{x} \coloneqq R_{3,1}(\mathbf{x})
condiciones de transición o guardas
La perspectiva completa
\dot{\mathbf{x}}=\mathbf{f}_1(\mathbf{x},\mathbf{u})
\dot{\mathbf{x}}=\mathbf{f}_2(\mathbf{x},\mathbf{u})
\dot{\mathbf{x}}=\mathbf{f}_3(\mathbf{x},\mathbf{u})
q=1
q=2
q=3
\mathbf{x}\coloneqq \mathbf{x}_0
\mathbf{x} \in G_{1,2}
\mathbf{x} \in G_{2,1}
\mathbf{x} \in G_{2,3}
\mathbf{x} \in G_{3,1}
\mathbf{x} \coloneqq R_{1,2}(\mathbf{x})
\mathbf{x} \coloneqq R_{2,1}(\mathbf{x})
\mathbf{x} \coloneqq R_{2,3}(\mathbf{x})
\mathbf{x} \coloneqq R_{3,1}(\mathbf{x})
saltos en el estado o resets
La perspectiva completa
Transición entre distintos modos de operación [estados] mediante condiciones discretas [eventos]
\(\Rightarrow\) máquina de estados finitos
La perspectiva completa
Transición entre distintos modos de operación [estados] mediante condiciones discretas [eventos]
\(\Rightarrow\) máquina de estados finitos
+ distinto sistema dinámico para cada modo de operación
La perspectiva completa
Transición entre distintos modos de operación [estados] mediante condiciones discretas [eventos]
\(\Rightarrow\) máquina de estados finitos
+ distinto sistema dinámico para cada modo de operación
= Autómata Híbrido
Ejemplo
Objetivo:
Llegar a la meta esquivando obstáculos, sin planificación.
sensores de distancia
Propuesta v1
Conclusión
NO es tan simple como parece
¿Más problemas? Estabilidad
¿Más problemas? Zeno
La perspectiva completa
Dos problemas similares
¿Cuál es la diferencia?
Dos problemas similares
¿Cuál es la diferencia?
Dos problemas similares
\(\Rightarrow\) el espacio de configuración
Espacio de configuración para brazos
Espacio de configuración para robots móviles
Espacio de configuración para robots móviles
Espacio de configuración para robots móviles
Espacio de configuración para robots móviles
occupancy grid
- Métodos de búsqueda en grafos | cuadrícula:
- algoritmo de Dijkstra.
- variantes como A*, D*, Theta*, Hybrid A*, etc.
- Métodos basados en muestreo:
- rapidly exploring random trees (RRT) y variantes "óptimas" (RRT*).
- probabilistic roadmaps (PRM).
- Virtual potential fields.
- Optimización no lineal (Control 2).
Métodos de planificación de movimiento
- Métodos de búsqueda en grafos | cuadrícula:
- algoritmo de Dijkstra.
- variantes como A*, D*, Theta*, Hybrid A*, etc.
- Métodos basados en muestreo:
- rapidly exploring random trees (RRT) y variantes "óptimas" (RRT*).
- probabilistic roadmaps (PRM).
- Virtual potential fields.
- Optimización no lineal (Control 2).
Métodos de planificación de movimiento
Rapidly Exploring Random Trees (RRT)
Entrada:
punto inicial y meta en el espacio de configuración
Salida:
recorrido de inicio a meta
Rapidly Exploring Random Trees (RRT)
Algoritmo:
- Insertar el(los) punto(s) inicial(es) en el(los) árbol(es).
- Mientras que el árbol no pueda conectarse a la meta:
- Muestree un punto aleatorio \(r\) en el espacio de configuración.
- Encuentre el punto \(p\) en el árbol que esté más cercano a \(r\).
- Añada una rama de un largo predeterminado a \(p\), en la dirección de \(r\).
- Si la nueva rama interseca un obstáculo:
- Descarte la nueva rama (o acórtela).
- Calcule el recorrido desde el punto de inicio hasta la meta a través del árbol.
- Atajos en el recorrido: para cualquiera dos puntos en el recorrido, añada una línea directa entre ellos a menos que esta interseque un obstáculo.
Ejemplo RRT
Algunas variantes
RRT* (RRT + "optimalidad")
Kinodynamic RRT
(RRT + restricciones no holonómicas)
Probabilistic Roadmaps (PRM)
Probabilistic Roadmaps (PRM)
Entrada:
punto inicial y meta en el espacio de configuración
Salida:
recorrido de inicio a meta
Construcción del mapa:
- Mientras que el número de puntos en el mapa sea menor que cierto valor de umbral:
- Muestree un punto aleatorio en el espacio de configuración.
- Si el nuevo punto no se encuentra en colisión:
- Conecte el nuevo punto a todos los puntos en el mapa mediante líneas, siempre y cuando éstas no intersequen obstáculos.
Probabilistic Roadmaps (PRM)
Encontrar recorrido:
- Conecte el punto de inicio al punto más cercano en el mapa tal que la línea que los conecta no interseque obstáculos.
- Conecte la meta al punto más cercano en el mapa tal que la línea que los conecta no interseque obstáculos.
- Encuentre un recorrido que vaya desde el inicio hasta la meta exclusivamente dentro del mapa.
Probabilistic Roadmaps (PRM)
Ejemplo PRM
Métodos de búsqueda en grafos
Métodos de búsqueda en grafos
De mapas a grafos | grafo de visibilidad
Aumento de mapa
Aumento de mapa
Aumento de mapa
Procedimiento
Para cada nodo \(n\), matiene un registro del tamaño del camino más corto desde el inicio a \(n\), encontrado hasta el momento, llamado \(g(n)\).
El algoritmo de Dijkstra
Entradas:
grafo de visibilidad, inicio \(S\) y meta \(G\)
Salida:
recorrido de \(S\) a \(G\)
Algoritmo:
- Etiquete a todos los nodos como "no visitados".
- Marque \(S\) para que tenga \(g(S)=0\).
- Mientras que existan nodos sin visitar:
- Escoja el nodo sin visitar con el menor \(g(n)\).
- Etiquete \(n\) como visitado.
-
Para cada vecino \(r\) de \(n\):
- \(g(r)=\min\{g(r), g(n)+d(n,r)\}\).
- Asigne de qué vecino proviene \(g(r)\).
El algoritmo de Dijkstra
El algoritmo de Dijkstra
S
G
Ejemplo Dijkstra
S
N_1
G
N_2
N_3
N_4
N_5
N_8
Ejemplo Dijkstra
N_6
N_7
S
N_1
G
N_2
N_3
N_4
N_5
N_6
N_7
N_8
Ejemplo Dijkstra
S
N_1
11
10
18
11
11
15
32
22
37
20
31
18
11
11
20
20
11
10
G
N_2
N_3
N_4
N_5
N_6
N_7
N_8
Ejemplo Dijkstra
S
N_1
11
10
18
11
11
15
32
22
37
20
31
18
11
11
20
20
11
10
G
N_2
N_3
N_4
N_5
N_6
N_7
N_8
Ejemplo Dijkstra
- Para cada nodo \(n\), se utiliza adicionalmente una heurística que estima qué tan lejos está \(n\) de la meta, llamada \(h(n)\).
- Una heurística es admisible sólo si nunca sobreestima la distancia real. Una empleada usualmente (que cumple con el requerimiento) es la distancia entre dos puntos.
Búsqueda A* | Dijkstra + heurística
Búsqueda A* | Dijkstra + heurística
Entrada:
grafo de visibilidad, inicio \(S\) y meta \(G\)
Salida:
recorrido de \(S\) a \(G\)
Algoritmo:
- Etiquete a todos los nodos como "no visitados".
- Marque \(S\) para que tenga \(g(S)=0\), \(f(S)=g(S)+\)\(h(S)\).
- Mientras que existan nodos sin visitar:
- Escoja el nodo sin visitar con el menor \(f(n)\).
- Etiquete \(n\) como visitado.
- Para cada vecino \(r\) de \(n\):
- \(g(r)=\min\{g(r), g(n)+d(n,r)\}\).
- \(f(r)=g(r)+h(r)\).
Búsqueda A* | Dijkstra + heurística
Búsqueda A* | Dijkstra + heurística
Dijkstra vs A*
Variante kinodinámica: Hybrid A*
Variante kinodinámica: Hybrid A*
entre muchas otras variantes y algoritmos más...
Otras referencias a consultar
-
Steven M. LaValle's Planning Algorithms.
- Planificación de movimiento empleando la Navigation Toolbox de MATLAB.
- Sección 5.2 Map-Based Planning del libro de Peter Corke.
MT3005 - Lecture 14 (2026)
By Miguel Enrique Zea Arenales
