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

Planificación todoterreno con modelos de elevación digitales

Este ejemplo muestra cómo procesar y almacenar información 2.5-D y presenta varias técnicas para usarla en un planificador de rutas todoterreno.

La planificación de robots móviles a menudo implica encontrar la ruta más corta para un robot con ruedas en presencia de obstáculos. Los planificadores suelen asumir que el espacio de planificación es un plano cartesiano 2-D, con ciertas regiones marcadas como fuera de los límites debido a la presencia de obstáculos. Cuando se trata de vehículos todoterreno, los entornos también pueden contener cambios de elevación, lo que los convierte en un problema tridimensional. La planificación en espacios de mayores dimensiones coincide con tiempos de planificación más largos, por lo que un compromiso eficaz puede ser planificar en un espacio 2.5-D utilizando modelos de elevación digitales (DEM). Este ejemplo muestra el proceso de creación y prueba de heurísticas de planificación en simulación y luego muestra cómo aplicar esas heurísticas para lograr una planificación de ruta 2.5-D para un planificador todoterreno con datos del mundo real. En este ejemplo, utilizará

• Planificador de rutas A-Star para robots móviles con ruedas

• Planificador de ruta híbrido A-Star para robots similares a automóviles

Definir el espacio de planificación

Crear superficie de planificación

La mayoría de los modelos de entorno se crean a partir de sensores montados en el robot o se recuperan de una base de datos basada en la pose estimada de la plataforma. Cree una superficie utilizando peaks para que actúe como modelo de entorno para probar el efecto de diferentes funciones de costo para el planificador.

% Add helper folders to path
addpath("Analysis","Data","Heuristics","MapProcessing");

% Create surface
[X,Y] = meshgrid(-100:100);
Z = peaks(max(size(X)))*1.5; % Units in meters
X = X + 100;                 % Move X origin from center to corner
Y = Y + 100;                 % Move X origin from center to corner
Z(Z<0) = 0;                  % Discard valleys

% Visualize surface
figure
ax = gca;
surf(ax,X,Y,Z,EdgeColor="none")
title("Test Terrain")
xlabel("X (m)")
ylabel("Y (m)")
zlabel("Z (m)")
colormap("turbo")
ax.PlotBoxAspectRatio = [1 1 0.15];
view([-29.59 21.30])

Definir heurística para el espacio 2.5-D

A continuación, defina las heurísticas. Estas heurísticas utilizan datos de elevación de diferentes maneras para crear un costo, $\mathit{C}$, que puede resultar en una mejora frente a la planificación de ruta en solo dos dimensiones:

exampleHelperZHeuristic

Esta función de coste aumenta el estado de planificación (XY) con la altura de la superficie (Z) antes de calcular la distancia. Esto debería dar como resultado la ruta más corta posible a través de la variedad 2.5-D, equilibrando los cambios en elevación con la distancia para encontrar la ruta más directo entre dos puntos:

${\mathit{C}}_{\mathit{Z}}=||{{\mathit{s}}^{*}}_2-{{\mathit{s}}^{\prime }}_1||$, donde ${{\mathit{s}}^{*}}_{\mathit{i}}=\left[{\mathit{x}}_{\mathit{i}}{\mathit{y}}_{\mathit{i}}{\mathit{z}}_{\mathit{i}}*\mathit{w}\right]$,

exampleHelperGradientHeuristic

Esta función de costo agrega un costo que escala con la inclinación de la pendiente en la misma dirección que el rumbo. A diferencia de la heurística Z, esta penalización no se escala según la distancia y, por lo tanto, debería inclinar al planificador hacia rutas que minimicen los cambios inmediatos de elevación, lo que podría ahorrar energía:

$${\mathit{C}}_{\mathrm{Slope}}=\{\begin{array}{ll}||{\mathit{s}}_2-{\mathit{s}}_1||+\mathit{w}*⟨\widehat{|{\mathit{s}}_2-{\mathit{s}}_1|},|{\mathit{g}}_{\mathit{x}}{\mathit{g}}_{\mathit{y}}|⟩& \mathrm{if}\mathrm{slope}<{\mathrm{slope}}_{\max }\\ {\mathit{e}}^{⟨\widehat{|{\mathit{s}}_2-{\mathit{s}}_1|},|{\mathit{g}}_{\mathit{x}}{\mathit{g}}_{\mathit{y}}|⟩}& \mathrm{else}\end{array}$$

exampleHelperRolloverHeuristic

Esta función de costo es similar a exampleHelperGradientHeuristic, pero al invertir los valores X e Y del gradiente espacial, penaliza los movimientos cuya dirección es perpendicular a la pendiente. En otras palabras, esta función debería minimizar el riesgo de vuelco buscando rutas con ángulos de vuelco bajos:

$${\mathit{C}}_{\mathrm{Roll}}=\{\begin{array}{ll}||{\mathit{s}}_2-{\mathit{s}}_1||+\mathit{w}*⟨\widehat{|{\mathit{s}}_2-{\mathit{s}}_1|},|{\mathit{g}}_{\mathit{y}}{\mathit{g}}_{\mathit{x}}|⟩& \mathrm{if}\mathrm{slope}<{\mathrm{slope}}_{\max }\\ {\mathit{e}}^{⟨\widehat{|{\mathit{s}}_2-{\mathit{s}}_1|},|{\mathit{g}}_{\mathit{y}}{\mathit{g}}_{\mathit{x}}|⟩}& \mathrm{else}\end{array}$$

Cada función de coste incluye una variable de ponderación que escala la heurística que la acompaña. Puede ajustar el peso para priorizar la ruta 2-D más rápida (peso = 0) o una ruta potencialmente más eficiente desde el punto de vista energético (peso = 4).

Discretizar y almacenar información del entorno en capas de mapas

Con las funciones de costes definidas, sabemos qué información recuperar del entorno. Muestrear discretamente la superficie 3D y almacenar los resultados en objetos mapLayer. Estos proporcionan transformaciones entre coordenadas cartesianas y funciones para consultar o modificar datos, similares a los objetos binaryOccupancyMap y occupancyMap.

Construir capa Z

Puede tomar la capa de altura directamente de la superficie y almacenarla en un objeto mapLayer, que servirá efectivamente como su Modelo de elevación digital. Cree una capa de mapa para contener la altura de la superficie sin el desorden del suelo ni los obstáculos.

% Query and store the Z-height
zLayer = mapLayer(flipud(Z),LayerName="Z");

Calcular gradientes y convertirlos a costos

Calcule el gradiente y la información de costes de la capa Z. Primero, calcule los gradientes X e Y de la superficie y utilice la función contour para visualizar los gradientes como un mapa de contorno.

[gx,gy] = gradient(Z);
figure
contour(X,Y,Z)
title("Terrain Slope")
hold on
quiver(X,Y,gx,gy)
axis equal
hold off

Almacene los gradientes X e Y en objetos mapLayer.

dzdx = mapLayer(flipud(gx),LayerName="dzdx");
dzdy = mapLayer(flipud(gy),LayerName="dzdy");

Define la función exampleHelperGradientToCost, que asigna valores de gradiente a un costo. Para que sea más intuitivo aplicar una variable de ponderación de costes, utilice pendientes para determinar la ponderación de costes. Los valores de pendiente de 0 a una pendiente máxima escalan linealmente en el rango [0 1], y los valores mayores que la pendiente máxima escalan exponencialmente:

$$\mathit{C}=\{\begin{array}{ll}\frac{|\mathit{m}|}{{\mathit{m}}_{\max }}& ,0\le |\mathit{m}|\le {\mathit{m}}_{\max }\\ {\mathit{e}}^{\left(\frac{|\mathit{m}|}{{\mathit{m}}_{\max }}-1\right)}& ,{\mathit{m}}_{\max }\le |\mathit{m}|\end{array},$$

donde $\mathit{m}$ es la pendiente. Define el ángulo de inclinación máximo como 15 grados y conviértelo en la pendiente máxima.

maxinclineangle = 15; % Degrees
maxSlope = tand(maxinclineangle); % Max preferred slope for vehicle

Visualice la función pendiente-coste no ponderada.

slope2cost = @(x)exampleHelperGradientToCost(maxSlope,flipud(x));
slope = linspace(-2*maxSlope,2*maxSlope,100);
plot(slope,slope2cost(slope))
hold on
xline(maxSlope*[-1 1],"r:")
yline(1,"b:")
title("Cost vs Slope")
xlabel("Slope")
ylabel("Cost")
axis square

Calcule los costes utilizando la función de pendiente y guárdelos en capas de mapas.

xCost = mapLayer(slope2cost(gx),LayerName="xCost");
yCost = mapLayer(slope2cost(gy),LayerName="yCost");
diagCost = mapLayer(slope2cost(sqrt(gx.^2+gy.^2)),LayerName="diagCost");

Obstáculos del terreno

Además de la información de elevación y pendiente, debes representar áreas que están fuera de los límites, ya sea por pendientes altas o por obstáculos. Utilice objetos binaryOccupancyMap o occupancyMap para representar dichas regiones para un planificador plannerAStarGrid .

Genere una matriz aleatoria, luego marque todos los elementos con valores por encima de un umbral específico como ubicaciones de obstáculos. Para representar las probabilidades de los obstáculos durante las pruebas, establezca las probabilidades de los obstáculos en valores aleatorios.

rng(4) % Set RNG seed for repeatable results
obstacleprob = rand(max(size(X)));

Crea un binaryOccupancyMap que contenga los obstáculos y el terreno inflado.

obstacleMap = binaryOccupancyMap(flipud(obstacleprob>0.998),LayerName="obstacles");
terrainObstacles = binaryOccupancyMap(obstacleMap,LayerName="terrainObstacles");
inflate(terrainObstacles,2);

Verifique cada celda con las tres restricciones de pendiente y bloquee las celdas que infringen las tres restricciones de pendiente.

obstaclesAndTerrain = double(getMapData(terrainObstacles));
mInvalidSlope = getMapData(xCost) > 1 & getMapData(yCost) > 1 & getMapData(diagCost) > 1;
obstaclesAndTerrain(mInvalidSlope) = true;
setMapData(terrainObstacles,obstaclesAndTerrain);

Muestre un mapa que contenga los obstáculos sin procesar y un mapa que contenga obstáculos inflados y terreno no válido.

figure
show(obstacleMap,Parent=subplot(1,2,1))
show(terrainObstacles,Parent=subplot(1,2,2))
title("Obstacles and Gradient-Restricted Regions")

Combinar capas de mapas individuales

Para organizar sus datos y mantener sus capas sincronizadas, agregue sus capas a un multiLayerMap. Esto garantiza que la actualización de las propiedades compartidas de una capa también actualice las demás, de modo que los datos siempre representen la misma región del espacio cartesiano.

costMap = multiLayerMap({zLayer dzdx dzdy terrainObstacles obstacleMap xCost yCost diagCost});

Haga que los mapas sean egocéntricos actualizando el valor GridOriginInLocal del mapa de costo X y luego restaure el mapa a su configuración original.

xCost.GridOriginInLocal = -xCost.GridSize/xCost.Resolution/2

xCost = 
  mapLayer with properties:

   mapLayer Properties
               DataSize: [201 201]
               DataType: 'double'
           DefaultValue: 0
               GridSize: [201 201]
              LayerName: 'xCost'
    GridLocationInWorld: [-100.5000 -100.5000]
      GridOriginInLocal: [-100.5000 -100.5000]
     LocalOriginInWorld: [0 0]
             Resolution: 1
           XLocalLimits: [-100.5000 100.5000]
           YLocalLimits: [-100.5000 100.5000]
           XWorldLimits: [-100.5000 100.5000]
           YWorldLimits: [-100.5000 100.5000]
        GetTransformFcn: []
        SetTransformFcn: []

% Restore map to original settings
costMap.GridOriginInLocal = [0 0];

Visualice los obstáculos además de las tres capas de costes basadas en pendientes. En estos gráficos, observe cómo el umbral de pendiente afecta el coste:

• Gris: espacio libre

• Tonos de azul: costes variables por debajo del umbral suave (cuanto más cerca del gris, menor es el coste)

• Tonos de rojo: costes variables por encima del umbral suave (rojo más oscuro, coste más alto)

• Negro: obstáculos y celdas que infringen el umbral de pendiente en todas las direcciones

figure(5)
obstaclesAndTerrain(obstaclesAndTerrain < 1) = nan; % Turn 0s to NaNs for plotting on costmaps

show(visualizationHelper,xCost,subplot(1,3,1),hold="on",title="X Slope",Threshold=1);
surface(flipud(obstaclesAndTerrain),FaceColor=[0 0 0])

show(visualizationHelper,yCost,subplot(1,3,2),hold="on",title="Y Slope",Threshold=1);
surface(flipud(obstaclesAndTerrain),FaceColor=[0 0 0])

show(visualizationHelper,diagCost,subplot(1,3,3),hold="on",title="Diagonal Slope",Threshold=1);
surface(flipud(obstaclesAndTerrain),FaceColor=[0 0 0])
sgtitle("Unweighted Costs with Obstacles")

Planificar rutas utilizando heurísticas 2.5-D

Crear planificadores

Cree un conjunto de planificadores, cada uno de los cuales utilice una de las heurísticas definidas, y compare sus resultados de planificación. Seleccione el tipo de planificador como

• plannerAStarGrid para robots móviles con ruedas.

• plannerHybridAStar para robots tipo automóviles.

plannerType = "plannerAStarGrid"

plannerType = 
"plannerAStarGrid"

Especifique las poses de inicio y objetivo en coordenadas mundiales.

start = [54 131 pi/3];
goal  = [143 104 pi/3];

Si el tipo de planificador es plannerAStarGrid, convierte las ubicaciones de inicio y destino en coordenadas de cuadrícula.

if strcmp(plannerType,"plannerAStarGrid")
    start = world2grid(costMap,start(:,1:2));
    goal  = world2grid(costMap,goal(:,1:2));
end

Utilice la función auxiliar exampleHelperCreatePlannerObject para crear variaciones del planificador de ruta especificado. Si especifica plannerAStarGrid como el planificador de rutas, la función exampleHelperCreatePlannerObject calcula heurísticas Z, heurísticas de gradiente y heurísticas de rollover utilizando los parámetros gWeight y hweight.

Si especifica plannerHybridAStar como el planificador de rutas, la función exampleHelperCreatePlannerObject calcula heurísticas Z, heurísticas de gradiente y heurísticas de rollover utilizando el parámetro gWeight.

gWeight y hWeight son parámetros de función de costo personalizados que determinan la prioridad de los nodos durante el proceso de búsqueda.

• gWeight representa el peso o costo de la ruta desde el nodo inicial hasta el nodo actual. Aumente este valor para minimizar la distancia real recorrida y priorizar la búsqueda de una ruta más corta entre la pose inicial y la pose objetivo.

• hWeight representa el costo estimado o el valor heurístico desde el nodo actual hasta el nodo objetivo. Aumente este valor para minimizar la distancia estimada hasta la pose objetivo y priorizar la búsqueda de rutas que estén más cerca del objetivo.

La combinación de estos pesos guía al planificador hacia la búsqueda de una ruta óptima basada en la prioridad.

gWeight = 1;
hWeight = 1;
[defaultPlanner,heightAwarePlanner,gradientAwarePlanner,rolloverAwarePlanner] = exampleHelperCreatePlannerObject(plannerType,costMap,gWeight,hWeight);

Planificar rutas

Planifique las rutas con cada heurística desde el inicio hasta el objetivo y guárdelos en un arreglo de celdas. Utilice tic y toc para medir el tiempo que tarda cada planificador heurístico en planificar su ruta.

% Planners return a sequence of IJ cells
paths = cell(4,1);
pathNames = ["Default","Elevation-Aware Path","Gradient-Aware Path","Rollover-Aware Path"];
planningTime = nan(4,1);
tic;
paths{1} = plan(defaultPlanner,start,goal);
planningTime(1) = toc;
paths{2} = plan(heightAwarePlanner,start,goal);
planningTime(2) = toc-planningTime(1);
paths{3} = plan(gradientAwarePlanner,start,goal);
planningTime(3) = toc-planningTime(2);
paths{4} = plan(rolloverAwarePlanner,start,goal);
planningTime(4) = toc-planningTime(3);

if strcmp(plannerType,"plannerHybridAStar")
    for i=1:4 %#ok<UNRCH>
        paths{i} = paths{i}.States;
    end
end

Comparar y analizar resultados

Compare el desempeño de cada planificador definiendo un conjunto de métricas y visualizando el movimiento a lo largo de cada ruta. Este ejemplo modela la plataforma como un vehículo terrestre simple y hace algunas suposiciones simplificadoras relacionadas con la energía requerida para operar el robot. Edite estos parámetros para ver cómo se ven afectados los resultados de la planificación.

Definir los parámetros del robot y del escenario

% Define simulation parameters
scenarioParams.Gravity = 9.81;    % m/s^2
scenarioParams.UpdateRate = 1/60; % Hz

% Define vehicle parameters for estimating power consumption
scenarioParams.Robot.Mass = 70;     % kg, no payload
scenarioParams.Robot.Velocity = 0.6; % m/s
scenarioParams.Robot.AmpHour = 24;   % A-hr
scenarioParams.Robot.Vnom = 24;                             % V, Voltage
scenarioParams.Robot.RegenerativeBrakeEfficiency = 0;

Definir métricas de rendimiento

Para el análisis, utilice la distancia de la ruta tridimensional, la duración del recorrido, el tiempo de planificación y el gasto de energía como una medida aproximada de la eficiencia de la ruta. También defina los ángulos de cabeceo y balanceo mínimo, máximo y promedio del vehículo, para comparar la calidad relativa de cada trayectoria. Para obtener más información, consulte exampleHelperCalculateMetrics.

% Display result
figure
animateResults = true;
statTable = exampleHelperCalculateMetrics(costMap,scenarioParams,paths,planningTime,pathNames,plannerType)

statTable=4×8 table
                            Distance (m)    Power (W)    Travel Time (s)    Planning Time (s)    Max Pitch (deg)    Average Pitch (deg)    Max Roll (deg)    Average Roll (deg)
                            ____________    _________    _______________    _________________    _______________    ___________________    ______________    __________________

    Default                     101.2        226.85          2.8112              0.19972             13.744               6.4108               18.797              7.4844      
    Elevation-Aware Path        100.7        218.13          2.7972               1.3234             12.274               4.1055               14.542              3.9993      
    Gradient-Aware Path        100.71        218.13          2.7975               7.7527             11.702               4.1577                 7.79              2.2989      
    Rollover-Aware Path        100.85        218.64          2.8013               4.7031             14.751               4.7195               4.4032             0.78052      

Visualice los resultados de la ruta usando exampleHelperVisualizeResults. Esta función auxiliar proyecta todas las rutas generadas previamente en la superficie 3D.

exampleHelperVisualizeResults(gca,statTable,scenarioParams,costMap,paths,pathNames,~animateResults,plannerType);

Visualizar los resultados de la ruta

Visualiza al robot siguiendo cada una de las rutas planificadas.

figure
exampleHelperVisualizeResults(gca,statTable,scenarioParams,costMap,paths,pathNames,animateResults,plannerType);

Al analizar los resultados, el planificador consciente de la elevación ofrece el mejor resultado para la distancia y el tiempo 3D más bajos, y mantiene el consumo de energía relativamente bajo, pero el planificador consciente de la elevación hace poco para alejar al robot de regiones con pendientes pronunciadas o altas. ángulos de balanceo. El planificador predeterminado también toma una ruta corta, incluso con la altura incluida en la métrica de distancia, pero gasta energía adicional en pendientes ascendentes y descendentes que el planificador consciente de la elevación evita, lo que resulta en un mayor consumo de energía.

Los planificadores basados ​​en gradientes intercambian distancia y tiempo por rutas más seguras que consumen menos energía en comparación con los otros planificadores. El planificador que tiene en cuenta el gradiente minimiza el tiempo dedicado a subir y bajar colinas, mientras que el planificador que tiene en cuenta los vuelcos busca una ruta que lo mantenga nivelado mientras atraviesa el terreno. Estos resultados se reflejan en las métricas de cabeceo y balanceo, donde el planificador con reconocimiento de gradiente muestra el cabeceo promedio más bajo, y el planificador con reconocimiento de balanceo muestra ángulos de balanceo máximos y promedio muy por debajo de los otros planificadores.

Plan basado en datos del modelo digital de elevación del mundo real

Ahora que ha probado las heurísticas en una superficie artificial, pruébelas utilizando datos de elevación del mundo real.

Importar datos del mundo real

Puede importar datos de elevación digitales desde una variedad de fuentes. Este ejemplo utiliza un archivo TIF descargado de la base de datos de cobertura terrestre nacional del USGS que contiene datos de elevación del Parque Nacional del Campo de Batalla de Manassas, Virginia, en los EE. UU. Si desea descargar este archivo, siga estos pasos:

1. En la barra de direcciones de la base de datos, busque "Manassas National Battlefield Park, VA".

2. Haga zoom a la región de interés.

3. En la pestaña Conjuntos de datos, en Datos, seleccione Productos de elevación (3-DEP) > DEM de 1 metro y haga clic en Buscar productos.

4. Descargue el archivo que cubre la región para la que desea datos de elevación.

Una vez descargado, puede extraer los datos de elevación del archivo TIF utilizando la función readgeoraster de Mapping Toolbox ™:

Nota: Este código requiere Mapping Toolbox

% Load the terrain data 
[elevation,info] = readgeoraster("USGS_1m_x28y430_VA_Fairfax_County_2018.tif",OutputType="double");
res = 2.5;

% Select a portion of the terrain data
Zinit = flipud(elevation(700:1100,800:1200));
[Xinit,Yinit] = meshgrid(1:length(Zinit));

% Interpolate for better matching with image resolution
[Xreal,Yreal] = meshgrid(1:(1/res):length(Zinit));
Zreal = interp2(Xinit,Yinit,Zinit,Xreal,Yreal);

En este ejemplo, los datos se procesaron para usted y se almacenaron en un archivo MAT. Cargue los datos posprocesados ​​para la subregión de interés.

% Load saved surface data and resolution
load("manassasData.mat");

% Visualize surface
surf(Xreal,Yreal,Zreal,EdgeColor="none")
formatAxes(visualizationHelper,title="Manassas Battlefield Terrain",colormap="jet")
pbaspect([1 1 0.2])

Cargar imágenes satelitales de la misma ubicación

img = imread("visitorcenter_satellitesq.jpg");
imgResized = imresize(img,[1001 1001]);
imshow(img)

Crear capas de datos

Repita el mismo proceso que en la sección Calcular gradientes y convertir a costo para crear las capas de gradiente y costo de gradiente.

figure
[gxReal,gyReal] = gradient(Zreal);
contour(Xreal,Yreal,Zreal)
formatAxes(visualizationHelper,title="Terrain Slope",hold="on",colormap="jet")
quiver(Xreal,Yreal,gxReal,gyReal)

Convierta la imagen de satélite en un mapa de obstáculos del terreno.

% Construct data layers
zLayerReal = mapLayer(flipud(Zreal),Resolution=res,LayerName="Z");
dzdxReal = mapLayer(flipud(gxReal),Resolution=res,LayerName="dzdx");
dzdyReal = mapLayer(flipud(gyReal),Resolution=res,LayerName="dzdy");
xCostReal = mapLayer(slope2cost(flipud(gxReal)),Resolution=res,LayerName="xCost");
yCostReal = mapLayer(slope2cost(flipud(gyReal)),Resolution=res,LayerName="yCost");
diagCostReal = mapLayer(slope2cost(flipud(sqrt(gxReal.^2+gyReal.^2))),Resolution=res,LayerName="diagCost");

Debido a que hay disponible una imagen satelital aérea, puede intentar aplicar costes o invalidar áreas del mapa en función de la ausencia o presencia de vegetación. Utilice tres filtros: uno para resaltar colores con baja intensidad espectral (carreteras), otro que se centre en los verdes (campos abiertos) y un tercero que resalte marrones y amarillos (hierba alta).

% Use thresholding to segment the image
[BWroads,roadRGB] = exampleHelperHighlightRoad(imgResized);
[BWfields,fieldRGB] = exampleHelperHighlightField(imgResized);
[BWbrush,brushRGB] = exampleHelperHighlightBrush(imgResized);
figure
imshow(roadRGB,Parent=subplot(1,3,1))
title("Tarmac")
imshow(fieldRGB,Parent=subplot(1,3,2))
title("Low Grass")
imshow(brushRGB,Parent=subplot(1,3,3))
title("High Brush")

Combine estos tres mapas para crear un coste de esfuerzo del terreno con los siguientes pesos para cada tipo de terreno:

• Carretera o asfalto — 0

• Hierba baja — 1

• Pincel alto — 3

Cuente el número de estas categorías para las que califica cada celda y calcule el coste de cada celda.

count = double(BWroads) + double(BWfields) + double(BWbrush);
terrainCost = (double(BWfields) + double(BWbrush)*3)./count;

Reemplace los valores NaN con valores interpolados de los vecinos.

[ii,jj] = meshgrid(1:size(terrainCost,1),1:size(terrainCost,2));
inan = isnan(terrainCost);
terrainCost(inan) = max(griddata(ii(~inan),jj(~inan),terrainCost(~inan),ii(inan),jj(inan),"linear"),0);

Para simplificar, este ejemplo marca cualquier terreno con una pendiente o un coste de superficie superior a un umbral determinado como ocupado, pero estos costes también podrían incorporarse a funciones de costes nuevas o existentes.

Inserte los costes del terreno en una capa y defina un coste máximo del terreno. Cambiar el coste máximo del terreno puede afectar qué terreno el mapa de ocupación binario ve como un obstáculo.

terrainLayer = mapLayer(terrainCost,Resolution=res,LayerName="surfaceCost");
maxTerrainCost = 2.5;

Convierte los costos del terreno en un objeto binaryOccupancyMap.

terrainObstaclesReal = binaryOccupancyMap(terrainLayer.getMapData >= maxTerrainCost,Resolution=res,LayerName="terrainObstacles");

Infla las celdas bloqueadas en pendientes y colócalas como obstáculos.

inflate(terrainObstaclesReal,.25);
terrainData = getMapData(terrainObstaclesReal);
mInvalidSlope = getMapData(xCostReal) > 1 & getMapData(yCostReal) > 1 & getMapData(diagCostReal) > 1;
terrainData(mInvalidSlope) = true;
setMapData(terrainObstaclesReal,terrainData);

Muestre los costes del terreno y el mapa de ocupación uno al lado del otro.

figure
show(visualizationHelper,terrainLayer,subplot(1,2,1),title="Terrain Surface Cost",Threshold=maxTerrainCost)
show(terrainObstaclesReal,Parent=subplot(1,2,2))

Combine las capas del mapa de costos en un objeto multiLayerMap.

costMapReal = multiLayerMap({zLayerReal dzdxReal dzdyReal terrainObstaclesReal xCostReal yCostReal diagCostReal});

Defina posiciones de inicio y objetivo en coordenadas mundiales.

start = [290 300];
goal = [95 165];

Cree una configuración de planificación configurando las funciones de costes personalizadas. Explore cómo las diferentes configuraciones impactan la ruta planificada.

Planificar ruta usando plannerHybridAstar

El híbrido A-Star con un costo de transición predeterminado tCostType y un costo de expansión analítica predeterminado aeCostType es útil para planificar una ruta óptima y factible para un robot similar a un automóvil en presencia de obstáculos en un entorno estructurado. También se deben tener en cuenta la altura y la calidad de la superficie de la carretera al planificar una ruta óptima en una carretera irregular o fuera de ella. Esto se puede lograr seleccionando tCostType o aeCostType personalizados.

Configurar la función de costo de transición

La función establecida en la propiedad TransitionCostFcn de plannerHybridAStar calcula la distancia recorrida (o el costo) entre dos poses en el gráfico de planificación.

    tCostType = "tDefault";
    transFcnChoices = exampleHelperHybridAStarFcnOptions("Function Type",tCostType);
    tWeightRange    = exampleHelperHybridAStarFcnOptions("Weight Range",tCostType);
    tWLow  = tWeightRange(1);
    tWHigh = tWeightRange(2);
    tStep  = tWeightRange(3);
    tWeight = 1;
    vArgs = exampleHelperHybridAStarFcnOptions("Cost Function Inputs",tCostType);
    tCostSetting = exampleHelperHybridAStarFcnOptions("Function Selection",transFcnChoices(1),vArgs{:});

Configurar la función de costo de expansión analítica

La función establecida en la propiedad AnalyticalExpansionCostFcn de plannerHybridAStar calcula la conexión Reeds-Shepp desde la pose actual hasta la pose objetivo. Este es el último segmento de la ruta real de plannerHybridAStar.

    aeCostType = "aeDefault";
    aeFcnChoices = exampleHelperHybridAStarFcnOptions("Function Type", aeCostType);
    aeWeightRange = exampleHelperHybridAStarFcnOptions("Weight Range",  aeCostType);
    aeWLow = aeWeightRange(1);
    aeWHigh = aeWeightRange(2);
    aeStep = aeWeightRange(3);
    aeWeight = 1;

    vArgs = exampleHelperHybridAStarFcnOptions("Cost Function Inputs",aeCostType);
    aeCostSetting = exampleHelperHybridAStarFcnOptions("Function Selection",aeFcnChoices(1), vArgs{:});

Cree un objeto plannerHybridAStar para las funciones de costo seleccionadas.

% Create a stateValidator object
map = getLayer(costMapReal,"terrainObstacles");
validator = validatorOccupancyMap;

% Assigning map to stateValidator object
validator.Map = map;

validator.StateSpace.StateBounds = [map.XLocalLimits; map.YWorldLimits; [-pi pi]];

pHybridAStarObj = plannerHybridAStar(validator,tCostSetting{:},aeCostSetting{:});

Agregue ángulos de rumbo a las posiciones inicial y final para planificar la ruta utilizando el planificador Hybid-A*.

startPose = [start pi/3];
goalPose  = [goal pi/3];

tic;
hybridAStarPathRealObj = plan(pHybridAStarObj,startPose,goalPose);
hybridAStarPlanningTimeReal = toc;

hybridAStarPathReal = hybridAStarPathRealObj.States;

Planificar ruta usando plannerAStarGrid

Configurar la función G-Cost

La función establecida en la propiedad GCostFcn de plannerAStarGrid calcula la distancia recorrida (o el costo) entre dos nodos en el gráfico de planificación. Luego, el planificador agrega este coste al coste acumulado entre el nodo raíz y el nodo actual para formar el coste g:

$${\mathit{g}}_{{\mathrm{cost}}_{\mathit{i}}}=\sum _{\mathit{n}=0}^{\mathit{i}-1}{\mathit{g}}_{{\mathrm{node}}_{\mathit{n}-1},{\mathrm{node}}_{\mathit{n}}}+\mathrm{gCostFcn}\left({\mathrm{node}}_{\mathit{i}-1},{\mathrm{node}}_{\mathit{i}}\right)$$

gCostType = "gDefault";
gFcnChoices = exampleHelperFcnOptions("Function Type",gCostType);
gWeightRange = exampleHelperFcnOptions("Weight Range",gCostType,"g");
gWLow = gWeightRange(1);
gWHigh = gWeightRange(2); 
gStep = gWeightRange(3);
gWeight = 1;
vArgs = exampleHelperFcnOptions("Cost Function Inputs",gCostType);
gCostSetting = exampleHelperFcnOptions("Function Selection",gFcnChoices(1),vArgs{:});

Configurar la función de costo H

El hCostFcn del planificador calcula el costo total, o costo estimado, entre el nodo actual y el nodo objetivo. El coste h sirve como heurística para guiar al planificador durante la exploración. Para satisfacer la garantía de optimización de A*, también se debe subestimar el coste total:

$${\mathit{h}}_{{\mathrm{cost}}_{\mathit{i}}}=\mathrm{hCostFcn}\left({\mathrm{node}}_{\mathit{i}},{\mathrm{node}}_{\mathrm{goal}}\right)$$

hCostType = "hDefault";
hFcnChoices = exampleHelperFcnOptions("Function Type",hCostType);
hWeightRange = exampleHelperFcnOptions("Weight Range",hCostType);
hWLow = hWeightRange(1);
hWHigh = hWeightRange(2);
hStep = hWeightRange(3);
hWeight = 1;
vArgs = exampleHelperFcnOptions("Cost Function Inputs",hCostType);
hCostSetting = exampleHelperFcnOptions("Function Selection",hFcnChoices(1),vArgs{:});

Sume el coste g y el coste h para calcular el coste f. El planificador A* utiliza el coste f para priorizar el siguiente nodo a explorar en el gráfico:

${\mathit{f}}_{{\mathrm{cost}}_{\mathit{i}}}={\mathit{g}}_{{\mathrm{cost}}_{\mathit{i}}}+{\mathit{h}}_{{\mathrm{cost}}_{\mathit{i}}}$

Construya el planificador A* con las funciones de costes seleccionadas.

pAStarGridObj = plannerAStarGrid(getLayer(costMapReal,"terrainObstacles"),hCostSetting{:},gCostSetting{:});

Convierta las posiciones inicial y final en coordenadas de cuadrícula. Luego, planifique un camino entre las posiciones inicial y final.

startPosition = world2grid(costMapReal,start);
goalPosition = world2grid(costMapReal,goal);

tic;
aStarGridPathReal = plan(pAStarGridObj,startPosition,goalPosition);
aStarGridPlanningTimeReal = toc;

Comparar y analizar resultados

Compare el rendimiento de los planificadores plannerHybridAStar y plannerAStarGrid calculando estas métricas: Distancia del recorrido tridimensional, duración del recorrido, tiempo de planificación y gasto de energía. Estas métricas son indicadores aproximados de la eficiencia de la ruta. Además, utilice los ángulos de inclinación máximo, inclinación promedio y balanceo del vehículo para evaluar y comparar la calidad relativa de cada trayectoria. Visualice el movimiento a lo largo de cada ruta.

Edite los parámetros del robot y del escenario para ver cómo se ven afectados los resultados de la planificación y las métricas.

Definir los parámetros del robot y del escenario

% Define simulation parameters
scenarioParams.Gravity = 9.81;    % m/s^2
scenarioParams.UpdateRate = 1/60; % Hz

% Define vehicle parameters for estimating power consumption
scenarioParams.Robot.Mass = 70;     % kg, no payload
scenarioParams.Robot.Velocity = 0.6; % m/s
scenarioParams.Robot.AmpHour = 24;   % A-hr
scenarioParams.Robot.Vnom = 24;                             % V, Voltage
scenarioParams.Robot.RegenerativeBrakeEfficiency = 0;

Definir métricas de rendimiento

hybridAStarStatTableReal = exampleHelperCalculateMetrics(costMapReal,scenarioParams,hybridAStarPathReal,hybridAStarPlanningTimeReal,"Hybrid A-Star Planner","plannerHybridAStar")

hybridAStarStatTableReal=1×8 table
                             Distance (m)    Power (W)    Travel Time (s)    Planning Time (s)    Max Pitch (deg)    Average Pitch (deg)    Max Roll (deg)    Average Roll (deg)
                             ____________    _________    _______________    _________________    _______________    ___________________    ______________    __________________

    Hybrid A-Star Planner       262.83        197.68          7.3009              3.2824              2.4935               0.65724              1.8676             0.74708      

astarGridStatTableReal = exampleHelperCalculateMetrics(costMapReal,scenarioParams,aStarGridPathReal,aStarGridPlanningTimeReal,"A-Star Grid Planner","plannerAStarGrid")

astarGridStatTableReal=1×8 table
                           Distance (m)    Power (W)    Travel Time (s)    Planning Time (s)    Max Pitch (deg)    Average Pitch (deg)    Max Roll (deg)    Average Roll (deg)
                           ____________    _________    _______________    _________________    _______________    ___________________    ______________    __________________

    A-Star Grid Planner       264.16        197.87          7.3377              0.16267             2.7563               0.7005               1.8954             0.77303      

De las métricas se puede inferir que el planificador plannerHybridAStar tiene valores bajos de distancia 3D, tiempo de recorrido y consumo de energía. Esto implica que el rendimiento de plannerHybridAStar es mejor que el de plannerAStarGrid.

Además, plannerHybridAStar tiene ángulos de giro bajos, lo que significa que el planificador puede dirigir eficazmente el robot lejos de áreas con pendientes pronunciadas.

Visualizar resultados

Mostrar la ruta planificada en una figura del área.

figure
subplot(2,1,1);
exampleHelperVisualizeResults(gca,hybridAStarStatTableReal,scenarioParams,costMapReal,{hybridAStarPathReal},"Hybrid A-Star Planner",~animateResults,"plannerHybridAStar")
subplot(2,1,2);
exampleHelperVisualizeResults(gca,astarGridStatTableReal,scenarioParams,costMapReal,{aStarGridPathReal},"A-Star Grid Planner",~animateResults,"plannerAStarGrid")

Este ejemplo introdujo el concepto de planificación todoterreno para robots móviles. Aprendió a aplicar planificadores 2D a búsquedas 3D proporcionando funciones de costes personalizadas que aprovechan la información 2.5-D. Esta información 2.5-D a menudo viene en forma de modelos digitales de elevación, que pueden obtenerse de bases de datos como la Base de datos nacional de cobertura terrestre del USGS, o construirse en tiempo real a partir de sensores a bordo. Por último, este ejemplo le mostró cómo se puede almacenar esta información en mapas, cómo construir diferentes funciones de costes y cómo analizar el impacto de esas funciones en la eficiencia y calidad de la ruta.