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

Cálculo de un BallRadiusConstant apropiado para PlannerRRStar

Este ejemplo explica BallRadiusConstant de plannerRRTStar con mayor detalle y proporciona una intuición sobre cómo calcular un valor razonable para un problema de planificación determinado.

¿Qué es el BallRadiusConstant?

TRR frente a TRR*

La principal diferencia entre RRT y RRT* es el comportamiento de "recableado", que le da a RRT* su garantía de optimización asintótica. Cuando se genera un nuevo nodo, ${\mathit{x}}_{\mathrm{new}}$, en los planificadores basados en RRT, el planificador encuentra el nodo más cercano en el árbol, ${\mathit{x}}_{\mathrm{nearest}}$. Si la ruta entre nodos es válida (por ejemplo, libre de colisiones), RRT simplemente conecta los nodos, pero RRT* realiza pasos adicionales para optimizar el árbol.

En primer lugar, RRT* encuentra todos los nodos del árbol dentro de cierta distancia de ${\mathit{x}}_{\mathrm{new}}$, ${\mathit{X}}_{\mathrm{near}}=\left\{{\mathit{x}}_{\mathit{a}}{\mathit{x}}_{\mathit{b}}...\right\}$. Luego, RRT* encuentra el nodo, ${\mathit{x}}^{*}\in {\mathit{X}}_{\mathrm{near}}$, que proporciona a ${\mathit{x}}_{\mathrm{new}}$ la ruta válida más corta de regreso a ${\mathit{x}}_{\mathrm{start}}$ y crea un borde entre este par. Por último, el planificador realiza una operación de "recableado", que verifica si ${\mathit{x}}_{\mathrm{new}}$ puede proporcionar a cada $\mathit{x}\in {\mathit{X}}_{\mathrm{near}}$ una ruta más corta para regresar a ${\mathit{x}}_{\mathrm{start}}$, en cuyo caso $\mathit{x}$ se desconecta de su padre actual y se reubica en ${\mathit{x}}_{\mathrm{new}}$.

Radio de recableado

El objetivo de RRT* es proporcionar esta garantía de optimización asintótica limitando al mismo tiempo los gastos generales adicionales. Por lo tanto, es importante elegir un radio de bola de tamaño adecuado durante el paso de recableado: si es demasiado grande, el rendimiento en tiempo de ejecución del algoritmo se verá afectado; si es demasiado pequeño, es posible que el algoritmo no converja. La distancia utilizada por plannerRRTStar para encontrar los vecinos más cercanos se calcula utilizando la siguiente fórmula adaptada de [1]:

(1) $\mathit{r}=\min \left({\left(\gamma *\frac{\log \left(\mathit{N}\right)}{\mathit{N}}\right)}^{\frac{1}{\mathit{d}}},\eta \right),\mathrm{where}$

$$\mathit{N}:\mathit{\#}\mathrm{nodes}\mathrm{in}\mathrm{tree}$$

$$\mathit{d}:\mathit{\#}\mathrm{dimensions}\mathrm{of}\mathrm{the}\mathrm{state}-\mathrm{space}$$

$$\eta :\mathrm{MaxConnectionDistance}$$

$$\gamma :\mathrm{BallRadiusConstant}$$

(2) $\gamma =2^{\mathit{d}}*\left(1+\frac{1}{\mathit{d}}\right)*\frac{\mathrm{vFree}}{\mathrm{vUnitBall}}$

$$\mathrm{vFree}:\mathrm{Lebesgue}\mathrm{Measure}$$

$$\mathrm{vUnitBall}:\mathrm{volume}\mathrm{of}\mathrm{unit}-\mathrm{ball}\mathrm{in}\mathit{d}\mathrm{dimensions}$$

Significado e intuición detrás del BallRadiusConstant

Las fórmulas anteriores definen un radio de tamaño "apropiado" para un espacio dado, lo que significa que a medida que el número de nodos que llenan el espacio crece linealmente, el radio debería contraerse de modo que el número de vecinos dentro de la bola que se contrae solo crezca logarítmicamente.

La intuición aproximada aquí surge de la expectativa de que todos los puntos ${\mathit{x}}_{\mathrm{new}}$$\in \mathit{X}$ en el árbol han sido muestreados de manera uniforme e independiente de la porción libre del espacio de configuración. Se puede decir que los puntos muestreados de esta manera han sido generados mediante un proceso homogéneo de puntos de Poisson.

Entonces, para una iteración dada del algoritmo, N puntos han sido muestreados uniformemente en el espacio libre, lo que significa que debería haber una densidad promedio de puntos por unidad de volumen (o "intensidad" de puntos por unidad de "medida", para espacios de dimensiones arbitrarias):

(3) $\lambda =\frac{\mathit{N}}{\mathrm{vFree}}$ = densidad

Por lo tanto, se deduce que la cantidad de puntos que puede esperar ver en cualquier porción del espacio de planificación es simplemente el volumen de esa porción multiplicado por la densidad. Para este ejemplo, el número de puntos dentro de una bola d de radio $\mathit{r}$ es más importante:

$$\left(4\right){\mathit{n}}_{1,\mathit{d}}=\mathrm{vUnitBall}*\lambda =\frac{\mathrm{vUnitBall}}{\mathrm{vFree}}*\mathit{N}$$

$$\left(5\right){\mathit{n}}_{\mathit{r},\mathit{d}}={\mathit{n}}_{1,\mathit{d}}*{\mathit{r}}^{\mathit{d}}$$

$${\mathit{n}}_{1,\mathit{d}}:\mathrm{expected}\mathrm{\#points}\mathrm{inside}\mathrm{unit}-\mathrm{ball}$$

$${\mathit{n}}_{\mathit{r},\mathit{d}}:\mathrm{expected}\mathrm{\#points}\mathrm{inside}\mathrm{ball}\mathrm{of}\mathrm{radius}\mathit{r}$$

Recordando que el objetivo es que el número de vecinos crezca logarítmicamente como $\mathit{N}\to \infty$, establezca ${\mathit{n}}_{\mathit{r},\mathit{d}}=\log \left(\mathit{N}\right)$ y resuelva para $\mathit{r}$:

$$\left(6\right)\mathit{r}={\left(\frac{\mathrm{vFree}}{\mathrm{vUnitBall}}*\frac{\log \left(\mathit{N}\right)}{\mathit{N}}\right)}^{\frac{1}{\mathit{d}}}$$

Los coeficientes restantes de la ecuación dos se derivan de la prueba de convergencia en el Apéndice G de [1], pero con $\mathit{N}$ eliminado. Tenga en cuenta que BallRadiusConstant es solo la relación entre el espacio libre en la región muestreable y la medida de la bola unitaria multiplicada por una constante específica de la dimensión.

Visualización del radio vs N y BallRadiusConstant

Ahora que hemos mostrado la relación entre BallRadiusConstant y el radio, veamos su comportamiento y veamos si coincide con la intuición:

% Plot unsaturated radius as N increases for different dimensions
figure;
d = [2 3 6 10];
N = logspace(0,6,1000)';
r = (log(N)./N).^(1./d);
legendEntries = "d = " + split(num2str(d));
exampleHelperPlotBallRadius(gca,N,r,"\gamma = 1",legendEntries);

Lo primero que llama la atención es que todos los radios decaen gradualmente como $\mathit{N}\to \infty$. Esto se alinea con la ecuación (3)/(6), que muestra la relación recíproca entre una densidad que aumenta linealmente y el objetivo de un número de vecinos que aumenta logarítmicamente. El gráfico también muestra cómo los problemas en dimensiones superiores requieren un radio mayor durante un período de tiempo más largo, ya que los puntos $\mathit{N}$ producen una densidad menor en un espacio con mayor oscuridad que la que produciría el mismo número en un espacio con menor oscuridad.

% Plot unsaturated radius for 3-dim space with varying BallRadiusConstant
figure;
dIdx = 2;
gammaEstimate = (50:50:250).^(d(dIdx));
rGamma = r(:,d(dIdx)).*(gammaEstimate.^(1/d(dIdx)));
legendEntries = "\gamma= " + split(num2str(gammaEstimate));
exampleHelperPlotBallRadius(gca,N,rGamma,"d = " + num2str(d(dIdx)),legendEntries);

Por último, grafica el número de vecinos esperados como $\mathit{N}\to \infty$, para diferentes $\gamma$ contra sus $\mathit{r}$ correspondientes y demuestra que en todos los casos, el número de vecinos esperados en iteraciones equivalentes es idéntico y aumenta logarítmicamente.

% Plot expected number of neighbors as N increases
figure;
nNeighbor = rGamma.^d(dIdx).*(N./log(N))./gammaEstimate./(2^d(dIdx)*(1+1/d(dIdx)));
plot(N,nNeighbor,LineWidth=5);
title(["# Expected Nodes in r(\gamma) vs N","(d=3)"]);
xlabel("N");
ylabel("N\_(r,d)");
legend(legendEntries);

Entonces, ¿qué pasa si la gamma es incorrecta? Observe que en el segundo gráfico, proporcionar una gamma más pequeña produce un radio más pequeño y, dado que la bola d ya es pequeña cuando N es pequeño (es decir, la densidad es baja), la probabilidad de que la bola contenga más de un punto se vuelve muy baja. Más importante aún, la cantidad de puntos que puede esperar solo aumenta logarítmicamente por diseño, por lo que, si bien es posible que la densidad eventualmente alcance el volumen menguante, deberá haber procesado ya una gran cantidad de puntos, lo que significa que el efecto de una optimización de recableado se limitará a una porción mucho más pequeña del árbol.

Efectos de un BallRadiusConstant insuficiente

Las fórmulas en ¿Qué es BallRadiusConstant? muestran el límite inferior aproximado requerido para lograr la garantía asintótica de RRT*'.

A continuación, configure un problema de planificación donde BallRadiusConstant no sea lo suficientemente grande para mostrar el resultado del planificador.

Plantear el problema de planificación

% Load an occupancyMap
load exampleMaps.mat; 
smallMap = occupancyMap(ternaryMap);
res = smallMap.Resolution;

% Create a state-space whose bounds span the map limits
limits = [smallMap.XWorldLimits; smallMap.YWorldLimits; -pi pi];
ss = stateSpaceSE2(limits);

% Create a state-validator
sv = validatorOccupancyMap(ss,Map=smallMap);
sv.ValidationDistance = (1/res)/10;

% Define start and goal locations
start = [100 100 0]; goal = [400 350 0];

Plan usando RRT* con BallRadiusConstant predeterminado

% Define RRT* planner with default BallRadiusConstant
maxDist = 20;
rrtStar = plannerRRTStar(ss,sv,MaxConnectionDistance=maxDist,ContinueAfterGoalReached=true);

% Plan a path with RRT*
rng(0);
tic;
[rrtStarPath, rrtStarSolnInfo] = plan(rrtStar,start,goal);
tRRTStarDefault = toc;

Comparar resultados con RRT estándar

% Plan path for the same problem using RRT
rrt = plannerRRT(ss,sv,MaxConnectionDistance=maxDist);
rng(0);
[rrtPath, rrtSolnInfo] = plan(rrt,start,goal);

% Display results, note how the RRT* tree fails to optimally explore the
% space.
subplot(1,2,1);
show(smallMap); 
title(["RRT*", "\gamma = 100 (default)"]);
hold on;
plot(rrtStarSolnInfo.TreeData(:,1),rrtStarSolnInfo.TreeData(:,2));
plot(rrtStarPath.States(:,1),rrtStarPath.States(:,2),LineWidth=5);

% Compare these results to the generic RRT planner
subplot(1,2,2);
show(smallMap);
title("RRT");
hold on;
plot(rrtSolnInfo.TreeData(:,1),rrtSolnInfo.TreeData(:,2));
plot(rrtPath.States(:,1),rrtPath.States(:,2),LineWidth=5);
hold off;

Al usar RRT*, puede esperar ver rutas que irradian hacia afuera desde el estado start, pero en cambio ve resultados que son casi idénticos a los de RRT. Esto es una indicación de que BallRadiusConstant está mal adaptado al problema, por lo que a continuación debes calcular un valor más apropiado.

Calcular BallRadiusConstant apropiado

Calcular el volumen unitario-bola

Para dimensionar correctamente la constante, debes estimar la medida de Lebesgue del espacio de búsqueda ($\mathrm{vFree})$, y el volumen de una bola unitaria ($\mathrm{vBall})$ definido en el espacio SE2, donde los estados se representan como $\left[\mathit{x}\mathit{y}\theta \right]$:

% Get the number of dimensions in the state-space (SE2 is a 3-dimensional
% state-space)
d = ss.NumStateVariables;

El cálculo de $\mathrm{vBall}$ es sencillo y se realiza mediante exampleHelperNBallVolume:

% Calculate unit-ball volume in d dimensions
vUnitBall = exampleHelperNBallVolume(d);

Volumen libre aproximado del espacio

La medida de Lebesgue es una medida del volumen en subconjuntos de un espacio euclidiano de n dimensiones. En términos sencillos, esto simplemente significa el volumen acumulado contenido dentro de una colección de subregiones del espacio y, en el contexto de este ejemplo, debe encontrar el volumen de espacio libre contenido en el dominio de búsqueda.

occupancyMap proporciona una representación discreta del espacio en XY, donde todos los theta son válidos si la celda correspondiente es válida. Por lo tanto, puedes definir el volumen de una celda libre como el área de la celda, multiplicada por la distancia máxima entre dos orientaciones:

L = 1/res; % Cell length/width
A = L^2;
vCell = A*pi; % Max orientation dist for stateSpaceSE2 is pi

El espacio libre total es el volumen de una celda libre multiplicado por el número de celdas libres en la región de búsqueda:

numFreeCells = nnz(checkOccupancy(smallMap) == 0);
vFree = vCell*numFreeCells;

Calcular BallRadiusConstant

Calcular el BallRadiusConstant:

gammaEstimate = (2^d)*(1+1/d)*(vFree/vUnitBall);

Compare los resultados de RRT* utilizando BallRadiusConstant apropiados

Planifique utilizando el nuevo BallRadiusConstant

% Update the BallRadiusConstant
rrtStar.BallRadiusConstant = gammaEstimate;
rng(0);
tic;
[rrtStarCorrectedPath,rrtStarCorrectedSolnInfo] = plan(rrtStar,start,goal);
tRRTStarCorrected = toc;

Analizar resultados

% Display original results
figure
subplot(1,2,1);
show(smallMap);
title(["RRT*","\gamma = 100 (default)"])
hold on;
plot(rrtStarSolnInfo.TreeData(:,1),rrtStarSolnInfo.TreeData(:,2));
plot(rrtStarPath.States(:,1),rrtStarPath.States(:,2),LineWidth=5);

% Display corrected results
subplot(1,2,2);
show(smallMap);
title(["RRT*", "\gamma = " + num2str(gammaEstimate)]);
hold on;
plot(rrtStarCorrectedSolnInfo.TreeData(:,1),rrtStarCorrectedSolnInfo.TreeData(:,2));
plot(rrtStarCorrectedPath.States(:,1),rrtStarCorrectedPath.States(:,2),LineWidth=5);
hold off

Observe cómo el árbol construido a partir de un BallRadiusConstant ($\gamma =1193200$) correctamente ajustado se irradia desde el estado inicial a medida que se expande a través del espacio. Compare esto con el planificador predeterminado cuyo árbol continúa subdividiendo el espacio pero no mejora las ramas.

Verifique la mejora en la longitud de la ruta observando algunos resultados cuantitativos. Primero cargue algunos resultados obtenidos sin conexión, donde el planificador RRT* buscó una ruta en el mismo problema usando diferentes BallRadiusConstant:

% Generate new results with the following
exampleHelperGenerateOfflineComparison(rrtStar,start,goal,gammaCompared);

% Load offline planning results
load("plannerComparison.mat","t","gammaCompared","pathCosts");

Primero, compare las longitudes de ruta devueltas en la estructura solutionInfo a medida que se agregan nodos al árbol. Observe cómo por debajo de un determinado valor $\gamma$, todos los planificadores producen los mismos resultados y la longitud de la ruta no mejora después de que se encuentra la solución inicial.

figure;
plot(pathCosts);
title("RRT* Path Cost vs # Nodes (N)");
xlabel("N");
ylabel("Path Cost");
legendEntries = "\gamma = " + num2str(gammaCompared(:),"%3.1e");
legend(legendEntries,Location="north");

A continuación, compare el tiempo dedicado a la planificación. Tenga en cuenta que un $\gamma$ mayor coincide con un rendimiento de planificación más lento. Esto se alinea con la expectativa de que se consideren más nodos por iteración durante el paso de recableado. Tenga en cuenta que el aumento en el cálculo parece disminuir a medida que $\gamma$ continúa creciendo. Esto probablemente indica que el planificador está usando $\mathit{r}=\eta$(el MaxConnectionDistance del planificador) durante períodos más largos durante la planificación, lo que limita el número de vecinos considerados hasta que $\mathit{N}$ crezca lo suficiente para $\gamma <\eta$.

% Convert gamma values to categories
x = categorical(string(num2str(gammaCompared(:),"%3.1e")));
x = reordercats(x,string(x));
cOrder = colororder;

% Compare RRT* runtime performance for different gamma
bar(x,t,FaceColor="flat",CData=cOrder(mod(0:numel(x)-1,size(cOrder,1))+1,:));
title(["RRT* Planning Time","N = " + num2str(size(pathCosts,1)) + ", dim = 3"]);
xlabel("\gamma");
ylabel("time (s)");

Resumen

Este ejemplo proporcionó algunos antecedentes sobre las diferencias entre RRT y RRT* e introdujo el concepto de BallRadiusConstant. Primero, el ejemplo mostró la relación entre BallRadiusConstant y el radio de recableado de RRT*. Además, el ejemplo mostró cómo esta constante afecta el comportamiento de planificación al cambiar el número promedio de vecinos considerados durante el paso de recableado.

A continuación, el ejemplo mostró cómo se podrían derivar términos específicos ($\mathrm{vBall},\mathrm{vFree}$) de un problema de planificación dado utilizando la dimensionalidad del espacio de estados y la medida de Lebesgue (derivada de occupancyMap). Luego, el ejemplo comparó los resultados producidos por RRT* utilizando este BallRadiusConstant de tamaño más apropiado contra aquellos generados utilizando el valor predeterminado.

Por último, el ejemplo analizó un conjunto de resultados fuera de línea para el mismo problema en un amplio rango de valores BallRadiusConstant y analizó las tendencias y compensaciones entre la optimalidad del planificador y la eficiencia computacional.

Referencias

[1] Karaman, Sertac y Emilio Frazzoli. “Algoritmos basados en muestreo para la planificación óptima del movimiento”. The International Journal of Robotics Research, vol. 30, núm. 7, junio de 2011, págs. 846–94. DOI.org (Crossref), https://doi.org/10.1177/0278364911406761.

[2] S. M. LaValle. Algoritmos de planificación. Cambridge University Press, 2006.

Consulte también

plannerRRTStar | plannerRRT