Esta página se ha traducido mediante traducción automática. Haga clic aquí para ver la última versión en inglés.

plannerRRTStar

Cree un planificador de ruta RRT óptimo (RRT*)

Desde R2019b

Descripción

El objeto plannerRRTStar crea un planificador RRT asintóticamente óptimo, RRT*. El algoritmo RRT* converge a una solución óptima en términos de la distancia del espacio de estados. Además, su tiempo de ejecución es un factor constante del tiempo de ejecución del algoritmo RRT. RRT* se utiliza para resolver problemas de planificación geométrica. Un problema de planificación geométrica requiere que dos estados aleatorios cualesquiera extraídos del espacio de estados puedan conectarse.

Creación

Sintaxis

planner = plannerRRTStar(stateSpace,stateVal)

planner = plannerRRTStar(___,Name=Value)

Descripción

planner = plannerRRTStar(stateSpace,stateVal) crea un planificador RRT* a partir de un objeto de espacio de estado, stateSpace, y un objeto de validador de estado, stateVal. El espacio de estado de stateVal debe ser el mismo que stateSpace. stateSpace y stateVal también establece las propiedades StateSpace y StateValidator del planner objeto.

planner = plannerRRTStar(___,Name=Value) establece propiedades usando uno o más argumentos de nombre-valor además de los argumentos de entrada en la sintaxis anterior. Puede especificar StateSampler, BallRadiusConstant, ContinueAfterGoalReached, MaxNumTreeNodes, MaxIterations, MaxConnectionDistance, GoalReachedFcn y GoalBias propiedades como argumentos de nombre-valor.

Propiedades

StateSpace — Espacio de estado para planificador

objeto del espacio de estados

Espacio de estados para el planificador, especificado como un objeto de espacio de estados. Puede utilizar objetos de espacio de estados como stateSpaceSE2, stateSpaceDubins, stateSpaceReedsShepp y stateSpaceSE3. También puede personalizar un objeto de espacio de estado utilizando el objeto nav.StateSpace .

StateValidator — Validador de estado para planificador.

objeto validador de estado

Validador de estado para el planificador, especificado como objeto de validador de estado. Puede utilizar objetos de validación de estado como validatorOccupancyMap, validatorVehicleCostmap y validatorOccupancyMap3D.

StateSampler — Muestreador de espacio de estados para muestrear el espacio de entrada

stateSamplerUniform objeto (predeterminado) | stateSamplerGaussian objeto | stateSamplerMPNET objeto | nav.StateSampler objeto

Desde R2023b

Muestra de espacio de estados utilizada para encontrar muestras de estados en el espacio de entrada, especificada como un objeto stateSamplerUniform , objeto stateSamplerGaussian , objeto stateSamplerMPNET , o nav.StateSampler objeto. De forma predeterminada, el plannerRRTStar utiliza muestreo de estado uniforme.

BallRadiusConstant — Constante utilizada para estimar el radio de búsqueda de vecinos cercanos

100 (predeterminado) | escalar positivo

Constante utilizada para estimar el radio de búsqueda de vecinos cercanos, especificado como un escalar positivo. El radio se estima de la siguiente manera:

$$r=\min \left(\gamma {\left(\frac{\log (n)}{n}\right)}^{\frac{1}{d}},\eta \right)$$

donde:

• γ — El valor de la propiedad BallRadiusConstant

• n — Número actual de nodos en el árbol

• d — Dimensión del espacio de estados

• η — El valor de la propiedad MaxConnectionDistance

γ se define de la siguiente manera:

$$\gamma ={{\displaystyle 2}}^d\left(1+\frac{1}{d}\right)\left(\frac{{{\displaystyle V}}_{Free}}{{{\displaystyle V}}_{Ball}}\right)$$

donde:

• V_Free — Volumen libre aproximado en el espacio de búsqueda

• V_Ball — Volumen de la bola unitaria en dimensiones d

Las fórmulas anteriores definen un BallRadiusConstant de tamaño "apropiado" para un espacio determinado, lo que significa que a medida que crece el número de nodos que llenan el espacio y se reduce el radio, el número esperado de vecinos crece logarítmicamente. Los valores más altos darán como resultado un número promedio más alto de vecinos dentro de la bola d por iteración, lo que generará más candidatos para recableado. Sin embargo, valores por debajo de este mínimo sugerido podrían dar lugar a un único vecino cercano, lo que no produce resultados asintóticamente óptimos.

Ejemplo: BallRadiusConstant=80

Tipos de datos: single | double

ContinueAfterGoalReached — Continuar optimizando después de alcanzar el objetivo

false (predeterminado) | true

Decida si el planificador continúa optimizando después de alcanzar el objetivo, especificado como false o true. El planificador también finaliza independientemente del valor de esta propiedad si se alcanza el número máximo de iteraciones o el número máximo de nodos del árbol.

Ejemplo: ContinueAfterGoalReached=true

Tipos de datos: logical

MaxNumTreeNodes — Número máximo de nodos en el árbol de búsqueda

1e4 (predeterminado) | número entero positivo

Número máximo de nodos en el árbol de búsqueda (excluido el nodo raíz), especificado como un entero positivo.

Ejemplo: MaxNumTreeNodes=2500

Tipos de datos: single | double

MaxIterations — Número máximo de iteraciones

1e4 (predeterminado) | número entero positivo

Número máximo de iteraciones, especificado como un entero positivo.

Ejemplo: MaxIterations=2500

Tipos de datos: single | double

MaxConnectionDistance — Longitud máxima de movimiento

0.1 (predeterminado) | escalar positivo

Longitud máxima de un movimiento permitida en el árbol, especificada como escalar.

Ejemplo: MaxConnectionDistance=0.3

Tipos de datos: single | double

GoalReachedFcn — Función de callback para determinar si se alcanzó el objetivo

@nav.algs.checkIfGoalIsReached | identificador de función

Función de callback para determinar si se alcanza el objetivo, especificada como un identificador de función. Puede crear su propia función de objetivo alcanzado. La función debe seguir esta sintaxis:

 function isReached = myGoalReachedFcn(planner,currentState,goalState)

donde:

• planner : el objeto planificador creado, especificado como objeto plannerRRTStar .

• currentState — El estado actual, especificado como un vector real de tres elementos.

• goalState — El estado objetivo, especificado como un vector real de tres elementos.

• isReached : una variable booleana para indicar si el estado actual ha alcanzado el estado objetivo, devuelta como true o false.

Para utilizar GoalReachedFcn personalizado en el flujo de trabajo de generación de código, esta propiedad debe establecerse en un identificador de función personalizado antes de llamar a la función del plan y no se puede cambiar después de la inicialización.

Tipos de datos: function handle

GoalBias — Probabilidad de elegir el estado objetivo durante el muestreo de estados

0.05 (predeterminado) | escalar real en el rango [0,1]

Probabilidad de elegir el estado objetivo durante el muestreo de estados, especificado como un escalar real en el rango [0,1]. La propiedad define la probabilidad de elegir el estado objetivo real durante el proceso de selección aleatoria de estados del espacio de estados. Puede comenzar estableciendo la probabilidad en un valor pequeño como 0.05.

Ejemplo: GoalBias=0.1

Tipos de datos: single | double

Funciones del objeto

plan	Planificar ruta entre dos estados
copy	Crear copia del objeto del planificador

Ejemplos

Planifique la ruta óptima entre dos estados

Crea un espacio de estados.

ss = stateSpaceSE2;

Cree un validador de estado basado en occupancyMap utilizando el espacio de estado creado.

sv = validatorOccupancyMap(ss);

Cree un mapa de ocupación a partir de un mapa de ejemplo y establezca la resolución del mapa en 10 celdas/metro.

load exampleMaps.mat
map = occupancyMap(simpleMap,10);
sv.Map = map;

Establezca la distancia de validación para el validador.

sv.ValidationDistance = 0.01;

Actualice los límites del espacio de estados para que sean los mismos que los límites del mapa.

ss.StateBounds = [map.XWorldLimits; map.YWorldLimits; [-pi pi]];

Cree un planificador de rutas RRT* y permita una mayor optimización una vez alcanzado el objetivo. Reduzca las iteraciones máximas y aumente la distancia máxima de conexión.

planner = plannerRRTStar(ss,sv, ...
          ContinueAfterGoalReached=true, ...
          MaxIterations=2500, ...
          MaxConnectionDistance=0.3);

Establezca los estados de inicio y objetivo.

start = [0.5 0.5 0];
goal = [2.5 0.2 0];

Planifique una ruta con la configuración predeterminada.

rng(100,'twister') % repeatable result
[pthObj,solnInfo] = plan(planner,start,goal);

Visualiza los resultados.

map.show
hold on
% Tree expansion
plot(solnInfo.TreeData(:,1),solnInfo.TreeData(:,2),'.-')
% Draw path
plot(pthObj.States(:,1),pthObj.States(:,2),'r-','LineWidth',2)

Planifique la ruta a través del mapa de ocupación en 3D utilizando RRT Star Planner

Cargue un mapa de ocupación tridimensional de una manzana de la ciudad en el espacio de trabajo. Especifique el umbral para considerar las celdas libres de obstáculos.

mapData = load("dMapCityBlock.mat");
omap = mapData.omap;
omap.FreeThreshold = 0.5;

Infle el mapa de ocupación para agregar una zona de amortiguamiento para una operación segura alrededor de los obstáculos.

inflate(omap,1)

Cree un objeto de espacio de estados SE(3) con límites para las variables de estado.

ss = stateSpaceSE3([0 220;0 220;0 100;inf inf;inf inf;inf inf;inf inf]);

Cree un validador de estado de mapa de ocupación 3D utilizando el espacio de estado creado. Asigne el mapa de ocupación al objeto validador de estado. Especifique el intervalo de distancia de muestreo.

sv = validatorOccupancyMap3D(ss, ...
     Map = omap, ...
     ValidationDistance = 0.1);

Cree un planificador de ruta en estrella RRT con una distancia máxima de conexión aumentada y un número máximo reducido de iteraciones. Especifique una función de objetivo personalizada que determine que una ruta alcanza el objetivo si la distancia euclidiana al objetivo está por debajo de un umbral de 1 metro.

planner = plannerRRTStar(ss,sv, ...
          MaxConnectionDistance = 50, ...
          MaxIterations = 1000, ...
          GoalReachedFcn = @(~,s,g)(norm(s(1:3)-g(1:3))<1), ...
          GoalBias = 0.1);

Especifique las poses de inicio y objetivo.

start = [40 180 25 0.7 0.2 0 0.1];
goal = [150 33 35 0.3 0 0.1 0.6];

Configure el generador de números aleatorios para obtener resultados repetibles.

rng(1,"twister");

Planifique la ruta.

[pthObj,solnInfo] = plan(planner,start,goal);

Visualice la ruta planificada.

show(omap)
axis equal
view([-10 55])
hold on
% Start state
scatter3(start(1,1),start(1,2),start(1,3),"g","filled")
% Goal state
scatter3(goal(1,1),goal(1,2),goal(1,3),"r","filled")
% Path
plot3(pthObj.States(:,1),pthObj.States(:,2),pthObj.States(:,3), ...
      "r-",LineWidth=2)

Más acerca de

Constante del radio de la bola

La principal diferencia entre RRT y RRT* es el comportamiento de recableado, que garantiza la optimización asintótica para el algoritmo RRT*. Cuando los planificadores basados ​​en RRT generan un nuevo nodo, el planificador encuentra el nodo más cercano en el árbol. Si la ruta entre nodos está libre de colisiones y es válida, el algoritmo RRT conecta los nodos, pero el algoritmo RRT* realiza pasos adicionales para optimizar el árbol después de conectar los nodos. Primero, RRT* encuentra todos los nodos en el árbol dentro de cierta distancia al nuevo nodo, luego RRT* encuentra el nodo que proporciona al nuevo nodo la ruta válida más corta de regreso al nodo inicial y agrega un borde entre el nodo y el nuevo. nodo. Por último, el planificador realiza una operación de recableado, que comprueba si el nuevo nodo puede proporcionar a cada uno de los nodos más cercanos una ruta más corta de regreso al nodo inicial. En el caso de que haya una ruta más corta, el nodo se desconecta del principal actual y se vuelve a vincular al nodo más cercano.

El radio en el que se produce el recableado se denomina constante del radio de la bola. Seleccionar una constante de radio de bola adecuada es importante ya que el objetivo de RRT* es garantizar la optimización asintótica y al mismo tiempo limitar cualquier cálculo adicional adicional. Si la constante del radio de la bola es demasiado grande, el tiempo de ejecución de RRT* aumenta. Si la constante del radio de la bola es demasiado pequeña, es posible que el algoritmo no converja hacia un resultado óptimo.

plannerRRTStar usa esta fórmula de distancia, adaptada de [1], para encontrar los vecinos más cercanos:

$$r=\min \left(\gamma {\left(\frac{\log (n)}{n}\right)}^{\frac{1}{d}},\eta \right)$$

donde:

• n — Número de nodos en el árbol

• d — Número de dimensiones del espacio de estados

• η — Distancia máxima de conexión

• γ — Constante del radio de la bola, definida como:

  $$\gamma ={{\displaystyle 2}}^d\left(1+\frac{1}{d}\right)\left(\frac{{{\displaystyle V}}_{Free}}{{{\displaystyle V}}_{Ball}}\right)$$

  donde:

  • V_Free — Medida de Lebesgue, el volumen libre aproximado en el espacio de búsqueda

  • V_Ball — Volumen de la bola unitaria en dimensiones d

Significado e intuición de la constante del radio de la bola

Las fórmulas para r y γ definen un radio de tamaño apropiado para un espacio y una densidad de muestreo determinados. Y a medida que el número de nodos que llenan el espacio crece linealmente, el radio debe reducirse y el número de vecinos dentro de la bola que se encoge crece logarítmicamente.

Esta intuición surge de la expectativa de que todos los puntos recién muestreados en el árbol hayan sido muestreados de manera uniforme e independiente de una porción libre del espacio de configuración. Al muestrear puntos de esta manera, se puede decir que se generaron mediante un proceso de puntos de Poisson homogéneo. Esto significa que en cada iteración de RRT*, n puntos se han muestreado uniformemente en el espacio libre, por lo que debería haber una densidad promedio de puntos por unidad de volumen, λ Para espacios de dimensiones arbitrarias, existe una intensidad de puntos por unidad de medida.

$$\lambda =\frac{N}{V_{Free}}$$

Por lo tanto, la cantidad de puntos que puede esperar ver en cualquier parte del espacio de planificación es el volumen de esa parte, multiplicado por la densidad. Para RRT*, el foco está en el número de puntos dentro de una bola de d dimensiones del radio r:

$$n_{1,d}=V_{Ball}·\lambda =\frac{V_{Ball}}{V_{Free}}·n$$

$${\text{n}}_{1,d}={\text{n}}_{1,d}·r^d$$

donde,

• n_1,d — Número esperado de puntos dentro de la bola unitaria de d dimensiones

• n_r,d — Número esperado de puntos dentro de la bola de d dimensiones con un radio r

Y recordando que el objetivo para el número de vecinos es crecer logarítmicamente cuando n se acerca al infinito, puede establecer n_r,d=log(n) y resolver para r:

$$\text{r=}{\left(\frac{V_{Free}}{V_{Ball}}·\frac{\log (n)}{n}\right)}^{\frac{1}{d}}$$

Los coeficientes restantes de la fórmula 2 se derivan de la prueba de convergencia en [1]. Sin embargo, al eliminar n, puede ver que la constante del radio de la bola es una relación entre el espacio libre en la región de muestra y la medida de la bola unitaria multiplicada por una constante específica de la dimensión.

Referencias

[1] Karaman, S. and E. Frazzoli. "Sampling-Based Algorithms for Optimal Motion Planning." The International Journal of Robotics Research. Vol. 30, Number 7, 2011, pp 846 – 894.

Capacidades ampliadas

Generación de código C/C++

Genere código C y C++ mediante MATLAB® Coder™.

Notas y limitaciones de uso:

• Para utilizar GoalReachedFcn personalizado en el flujo de trabajo de generación de código, esta propiedad debe establecerse en un identificador de función personalizado antes de llamar a la función del plan y no se puede cambiar después de la inicialización.

Historial de versiones

Introducido en R2019b

R2023b: Especificar el enfoque de muestreo para la planificación de rutas

Ahora puede especificar muestreo uniforme, muestreo gaussiano, muestreo MPNet o un enfoque de muestreo personalizado para generar muestras para la planificación de rutas. Utilice el argumento de nombre y valor StateSampler para especificar el enfoque de muestreo.

Consulte también

Objetos

• plannerRRT | plannerBiRRT | stateSpaceReedsShepp | stateSpaceDubins | stateSpaceSE2 | stateSpaceSE3 | validatorOccupancyMap | validatorVehicleCostmap | validatorOccupancyMap3D | stateSamplerUniform

Funciones

• plan | copy

Temas

• Calcular un BallRadiusConstant apropiado para PlannerRRStar