RRT* e Informed RRT*
Como vimos en el post pasado sobre RRT, es un algoritmo útil para explorar un entorno, aunque no tan eficiente encontrando una ruta óptima entre dos puntos. RRT garantiza encontrar un camino cuando el número de iteraciones tiene a infinito, pero no garantiza que este camino sea óptimo, es decir, que se recorra la mínima distancia necesaria.
RRT* mejora este punto, garantizando la convergencia a un camino óptimo gracias al rewiring. Cada vez que añadimos un nuevo nodo, en vez de unirlo directamente como hijo del que tiene más cerca, se une al nodo que minimiza el coste (longitud) del camino al origen y luego se calcula si este nuevo nodo añadido podría mejorar algún coste de sus vecinos (nodos dentro de un radio especificado) si fuera su padre.
Informed RRT* mejora la velocidad de convergencia de RRT* restringiendo la zona de la que se sacan los puntos aleatorios con los que colocamos el nuevo nodo a una elipse que contiene el camino que hemos encontrado previamente.
Implementando RRT*
Calculando el coste al origen
Para empezar implementándolo tenemos que modificar la clase Vertex que he creado para los nodos en RRT y añadir una variable cost que inicializamos a 0 y la cuál modificamos cuando ese nodo se vaya a incluir en nuestro árbol. El nuevo coste será la distancia de este nuevo nodo al nodo del árbol al que lo unimos más el coste de ese nodo.
De esta manera, la distancia de un nodo cualquiera al origen siguiendo el camino dado por las conexiones del árbol está contenida en la variable coste de ese nodo.
Unión como hijo al árbol
Una vez hemos calculado la posición del nuevo nodo como hacíamos en RRT (avanzando una distancia step en la dirección marcada por el punto aleatorio y el nodo más cercano a él del árbol), en vez de unirlo directamente al nodo más cercano, seleccionamos nodos del árbol cercanos a la posición del nuevo nodo que llamamos vecinos. Para ello tenemos que recorrer todos los elementos del árbol, calcular su distancia a la posición y comprobar si es mayor o menor a un umbral previamente establecido.
Entre estos vecinos, calculamos cuál daría un menor coste al nuevo nodo a añadir si lo uniéramos a él, es decir, el que hace que la distancia del nuevo nodo al origen sea la menor.
Recableando: unión como padre
Y ahora es cuando se ejecuta la gran ventaja de RRT*: al añadir un nodo no sólo se mejora su coste al elegir entre sus vecinos, sino que su adición puede mejorar el coste de otros nodos si modificamos sus uniones con él.
Al igual que antes recogemos una lista de vecinos (obviando el vecino al que nos hemos unido antes) y comparamos sus costes actuales con el posible coste que tendrían si se unieran como hijos al nodo que acabamos de añadir. Si el posible coste es menor que el actual, los unimos y eliminamos la conexión del nodo con su antiguo padre. Podemos limitar el número de nuevas uniones o hacerlo con todos aquellos que cumplan la condición (aunque hay que tener cuidado al eliminar las referencias a los nodos padre ya que varios vecinos pueden presentar relaciones padre-hijo).
Una vez se realizan estas nuevas conexiones hay que propagar el nuevo coste a todos los hijos del nodo que hemos modificado. Podemos solucionarlo con una función recursiva:
void updateCosts(){ // el objeto que llama a la función ya ha cambiado su coste for (Vertex c : this.children){ c.cost = this.cost + dist(this, c); //el nuevo coste es el coste ya actualizado del padre más la distancia que lo separa con el hijo
c.updateCosts();
}}
Sin embargo, al tener muchísimos nodos tantas llamadas a funciones recursivas hacen que la pila se desborde (stack overflow). Una solución más eficiente es ir guardando en una pila los nodos e irlos actualizando (solución iterativa).
Añadimos en la animación que compruebe si el camino ha cambiado para pintarlo correctamente y este es el resultado:
Implementando Informed RRT*
Informed RRT* mejora la convergencia
En RRT* según añadamos más y más puntos la ruta mejora, pero realmente los nodos que mejoran el camino son aquellos que caen en una zona cercana a él y por tanto, pueden recablear sus nodos.
Así, en vez de elegir puntos aleatorios de todo nuestro espacio muestral, vamos a elegirlos dentro de una zona cercana al camino que ya hemos encontrado, en concreto, una elipse que abarque este camino, para encontrar la solución óptima antes.
Elección de los parámetros de la elipse
Se puede definir elipse como:
el lugar geométrico de los puntos cuya suma de las distancias a otros dos puntos llamados focos es constante
Esta definición de la elipse permite comprobar rápidamente si un punto está o no dentro de ella, solo hay que comprobar si la suma de las distancias al origen (foco 1) y al nodo en la zona de meta (foco 2) es mayor que la constante de la elipse 2a (entonces está fuera) o menor (está dentro).
¿Cómo definimos esta constante? Para asegurar que la elipse abarca el camino definimos 2a como el coste del nodo en la zona de meta, es decir, la longitud del camino. De esta manera, además, la elipse se adapta de forma inteligente a las necesidades de búsqueda. Si tenemos un camino largo porque hay mcuhas curvas entre obstáculos, la elipse será mayor para poder encontrar caminos alternativos. Por otra parte, si el camino es relativamente recto entre los focos, la elipse tendrá una forma más “estrecha” y se enfocará en añadir nodos que eliminen las curvas innecesarias en el camino.
Para la animación, hay que tener en cuenta que el camino ahora no solo cambia por un nuevo nodo que cae en la zona de meta, sino también por nodos añadidos en las inmediaciones que cambian el coste de un camino.
El resultado es:
GitHub
El código está en https://github.com/ivrolan/rrt_star
En la rama master se encuentra la implementación en Processing para RRT* y en la rama informed_rrt la implementación para Informed RRT*.
