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

Calcular un BallRadiusConstant apropiado para PlannerRRStar

Este ejemplo explica el BallRadiusConstant de plannerRRTStar con mayor detalle y proporciona 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 un nuevo nodo,${\mathit{x}}_{\mathrm{new}}$se genera en 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.

Primero, RRT* encuentra todos los nodos en el árbol dentro de cierta distancia para${\mathit{x}}_{\mathrm{new}}$, ${\mathit{X}}_{\mathrm{near}}=\left\{{\mathit{x}}_{\mathit{a}}{\mathit{x}}_{\mathit{b}}...\right\}$. RRT* luego encuentra el nodo,${\mathit{x}}^{*}\in {\mathit{X}}_{\mathrm{near}}$, que proporciona${\mathit{x}}_{\mathrm{new}}$con la ruta válido más corto de regreso a${\mathit{x}}_{\mathrm{start}}$y crea una ventaja entre este par. Por último, el planificador realiza una operación de "recableado", que comprueba si${\mathit{x}}_{\mathrm{new}}$puede proporcionar cada$\mathit{x}\in {\mathit{X}}_{\mathrm{near}}$con una ruta más corta para regresar a${\mathit{x}}_{\mathrm{start}}$, en ese caso$\mathit{x}$está desconectado de su principal actual y reparenteado a${\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 vecinos más cercanos se calcula usando 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, es decir, a medida que el número de nodos que llenan el espacio crece "linealmente", el radio debe reducirse para que el número de vecinos dentro de la bola que se encoge sólo 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 se han muestreado 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 debe 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$, colocar${\mathit{n}}_{\mathit{r},\mathit{d}}=\log \left(\mathit{N}\right)$y resolver 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}$remoto. Observe que BallRadiusConstant es solo la relación entre el espacio libre en la región que se puede muestrear y la medida de la bola unitaria multiplicada por una constante específica de la dimensión.

Visualizando 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 destaca es que todos los radios decaen gradualmente a medida que$\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, como$\mathit{N}$Los puntos producen una densidad menor en un espacio con mayor oscuridad que 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, trace el número de vecinos esperados como$\mathit{N}\to \infty$, por diferentes$\gamma$contra sus correspondientes$\mathit{r}$y demuestre 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 llega a ser muy bajo. Más importante aún, el número de puntos que puede esperar sólo aumenta logarítmicamente por diseño, por lo que si bien es posible que la densidad eventualmente alcance el volumen cada vez menor, necesitará haber procesado ya un gran número de puntos, es decir el efecto de una optimización del recableado se limitará a una porción mucho más pequeña del árbol.

Efectos de un BallRadiusConstant

Las fórmulas en What is 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];

Planifique 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 se irradian hacia afuera desde el estado start , pero en cambio verá resultados que son casi idénticos a los de RRT. Esto es una indicación de que el BallRadiusConstant no está bien adaptado al problema, por lo que a continuación calcula un valor más apropiado.

Calcular BallRadiusConstant apropiado

Calcular el volumen unitario-bola

Para dimensionar correctamente la constante, debe estimar la medida de Lebesgue del espacio de búsqueda ($\mathrm{vFree})$, y el volumen de una unidad de bola ($\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;

Calculador$\mathrm{vBall}$es sencillo, manejado por 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.

El 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 los BallRadiusConstant

Planifica usando 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 se construyó el árbol a partir de un BallRadiusConstant ( $\gamma =1193200$) 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 fuera de línea, 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 solnInfo a medida que se agregan nodos al árbol. Observe cómo debajo de cierto$\gamma$valor, 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 mayor$\gamma$coincide con un desempeño 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$sigue creciendo. Esto probablemente indica que el planificador está utilizando$\mathit{r}=\eta$(el MaxConnectionDistance del planificador) para períodos más largos durante la planificación, limitando así el número de vecinos considerados hasta $\mathit{N}$crece 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 proporciona algunos antecedentes sobre las diferencias entre RRT y RRT* e introduce 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 términos específicos ($\mathrm{vBall},\mathrm{vFree}$) podría derivarse 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* usando este BallRadiusConstant de tamaño más apropiado con los generados usando 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 discutió las tendencias y compensaciones entre la optimización del planificador y la eficiencia computacional.

Referencias

[1] Karaman, Sertac y Emilio Frazzoli. "Algoritmos basados ​​en muestreo para una planificación óptima del movimiento". La Revista Internacional de Investigación en Robótica, vol. 30, núm. 7, junio de 2011, págs. 846–94. DOI.org (referencia cruzada), https://doi.org/10.1177/0278364911406761.

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

Consulte también

plannerRRTStar | plannerRRT