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 de rutas en terreno irregular según los requisitos del vehículo

Este ejemplo muestra cómo utilizar navGraph y plannerAStar para encontrar una ruta a través de terreno accidentado teniendo en cuenta los requisitos y limitaciones del vehículo.

Este ejemplo ilustra las actividades de construcción que tienen lugar en una zona montañosa. Esta área contiene dos categorías distintas de vehículos. En primer lugar, el vehículo de transporte, que tiene dificultades para subir colinas, requiere un ascenso o descenso gradual y consume más combustible al atravesar alturas. En segundo lugar, los equipos de construcción como las excavadoras, que pueden circular sobre colinas con facilidad. Los transportistas pueden viajar más rápido en terrenos lisos y pueden beneficiarse de rutas más largas. Por el contrario, los equipos de construcción pueden beneficiarse de rutas más cortas porque se ven menos afectados por el terreno accidentado.

Crear mapa de terreno base

Este ejemplo utiliza el modelo de elevación digital del Servicio Geológico de los Estados Unidos (USGS) del Monte Washington. Si tiene una licencia de Mapping Toolbox™ , puede utilizar este código para cargar los datos GeoTIFF relevantes.

% Using the Mount Washington USGS digital elevation model.

[Z,~] = readgeoraster("USGS_one_meter_x32y491_NH_Umbagog_2016.tif",OutputType="double");

De lo contrario, este ejemplo incluye los datos relevantes en un archivo MAT. Cargue el archivo MAT en el espacio de trabajo.

load("USGS_four_meter_x32y491_NH_Umbagog_2016.mat")

Para crear un mapa de terreno base a partir de los datos de elevación, normalice los valores de altura. Luego, cree un mapa de ocupación utilizando los datos de elevación, especificando un umbral de altura por encima del cual el mapa indica una ubicación ocupada.

% Scaling map between 0 and 1, based on maximum height.
Z = double(Z);
minHeight = min(Z,[],"all");
maxHeight = max(Z,[],"all");
terrainMap = rescale(Z);

% Initialize occupancy map.
map = occupancyMap(terrainMap);

% Set obstacle threshold, beyond which no vehicle is allowed.
obsThres =0.87;
map.OccupiedThreshold = obsThres;
map.FreeThreshold = obsThres;

Extraer estados y vínculos mediante descomposición de áreas basada en cuadrículas

Extraiga el gráfico del mapa de ocupación utilizando el método de descomposición de cuadrícula. Primero, divida el mapa en cuadrículas según la resolución seleccionada, luego marque los estados (nodos) en cada intersección del gráfico.

% Downsample Grid: Reduce grid by specified factor.
downSampleFactor = 100;  
newSize = ceil(size(terrainMap)/downSampleFactor) + 1;
[row,col] = ind2sub(newSize,1:(prod(newSize)));

% Extract states for new grid.
states = grid2world(map,([row' col']-1)*downSampleFactor);

% Format states to be inside map.
states = helperFitStatesInMap(states,map);

% Extract occupancy matrix for states.
occMat = getOccupancy(map,states,"world");

% Generate links from occupancy matrix, based on obstacle threshold.
[~,links] = exampleHelperOccupancyMatrixToGraphData(reshape(occMat,newSize),obsThres);

Agregar parámetros

Defina parámetros que proporcionen estructura adicional a la escena. Por ejemplo, los vínculos de gráficos que se encuentran en un terreno plano se marcan como autopistas. Estos enlaces definen los enlaces que abarcan regiones sobre una altura determinada y conectan estados en diferentes niveles como puentes.

Las funciones de costes definidas más adelante en el ejemplo utilizarán esta información para generar diferentes rutas para diferentes vehículos.

% Extract maps containing highway and bridge areas.
[highwayMap,bridgeMap] = exampleHelperExtractTrailBridge(Z);

% Add highway roads to links.
highwayMapVal = getOccupancy(highwayMap,states,"world");
isStateOnHighway = highwayMapVal(links);
isHighway = isStateOnHighway(:,1) & isStateOnHighway(:,2);

% Set maximum speed on each road for transporter vehicles.
transporterSpeedRange = [20 50];
MaxSpeedTransporter = randi(transporterSpeedRange,height(links),1);

% Add bridge roads to links.
bridgeVal = getOccupancy(bridgeMap,states,"world");
isStateOnBridge = bridgeVal(links);
isBridge= isStateOnBridge(:,1) & isStateOnBridge(:,2);

% Set maximum speed on each road for excavator vehicles.
excavatorSpeedRange = [10 30];
MaxSpeedExcavator = randi(excavatorSpeedRange,height(links),1);

Crear tabla de estados y enlaces

Combine todos sus datos para crear tablas de estado y enlaces.

statesTable = table(states,occMat*maxHeight, ...
                    VariableNames={'StateVector','Height'});
linksTable = table(links,isHighway,MaxSpeedTransporter,isBridge,MaxSpeedExcavator, ...
                    VariableNames={'EndStates','Highway', ...
                    'MaxSpeedTra','Bridge','MaxSpeedExc'});

head(statesTable)

    StateVector    Height
    ___________    ______

    0      2200    286.75
    0    2100.5    214.72
    0    2000.5    163.47
    0    1900.5    173.18
    0    1800.5    246.58
    0    1700.5    307.55
    0    1600.5    387.91
    0    1500.5    421.12

head(linksTable)

    EndStates    Highway    MaxSpeedTra    Bridge    MaxSpeedExc
    _________    _______    ___________    ______    ___________

     1    24      true          45         false         23     
     1     2      true          48         false         22     
     1    25      true          23         false         29     
     2    25      true          48         false         21     
     2     1      true          39         false         17     
     2     3      true          23         false         30     
     2    26      true          28         false         19     
     2    24      true          36         false         22     

Crear objeto navGraph y visualizar datos gráficos

Cree un objeto navGraph a partir del estado y vincule las tablas que comprenden todos los datos. Puede utilizar el objeto navGraph en funciones de costes para realizar cálculos de costes.

graphObj = navGraph(statesTable,linksTable);
% Visualize the map and graph.
t = tiledlayout(1,2);
t.TileSpacing = "compact";
t.Padding = "tight";

% Visualize map.
axh2 = nexttile;
ms = show(map,Parent=axh2);
title("Mt. Washington")
colormap(flipud(colormap("gray")));
ci = colorbar(axh2);
ci.TickLabels = strsplit(num2str(round(ci.Ticks*(maxHeight-minHeight),-1)+minHeight));
ci.Label.String = "Elevation in meters";

% Visualize graph.
axh = nexttile;
show(map,Parent=axh);
exampleHelperPlotGraph(axh,graphObj);
title("navGraph")
legend(axh,Location="northeast")

Configurar inicio y objetivo

Especifique su posición inicial y su posición objetivo, y luego busque el índice de estado más cercano para cada posición.

% Specify start and goal.
start = [100 700];
goal = [1900 1500];

% Finding closest state index for start and goal.
startID = graphObj.closestStateID(start);
goalID = graphObj.closestStateID(goal);

Ruta del vehículo transportador

Los vehículos de transporte funcionan mejor cuando realizan ascensos y descensos graduales y evitan alturas.

La función de coste de transición del vehículo transportador transporterVehicleTransitionCostFcn utiliza estas funciones de coste:

• Euclidean Distance

• Gradient Change Cost

• Highway Road Cost (Recompensa)

• Bridge Road Cost (Penalización)

La función de coste heurística del vehículo transportador transporterVehicleHeuristicCostFcn utiliza estas funciones de coste:

• Euclidean Distance

• Height Penalty

% Set the cost functions for the transporter vehicle.
graphObj.LinkWeightFcn = @(id1,id2,graphObj)transporterVehicleTransitionCostFcn(id1,id2,graphObj)

graphObj = 
  navGraph with properties:

           States: [529x2 table]
            Links: [3802x5 table]
    LinkWeightFcn: @(id1,id2,graphObj)transporterVehicleTransitionCostFcn(id1,id2,graphObj)

% Create planner object and assign required heuristic cost function.
plannerTransporterObj = plannerAStar(graphObj);
plannerTransporterObj.HeuristicCostFcn = @(id1,id2)transporterVehicleHeuristicCostFcn(id1,id2,graphObj)

plannerTransporterObj = 
  plannerAStar with properties:

    HeuristicCostFcn: @(id1,id2)transporterVehicleHeuristicCostFcn(id1,id2,graphObj)
          TieBreaker: 0
               Graph: [1x1 navGraph]

Planifique la ruta del vehículo transportador utilizando un algoritmo A* basado en gráficos.

% Find the shortest path using graph-based A* algorithm.
[pathTransporter,solutionInfoTransporter] = plan(plannerTransporterObj,startID,goalID);

% Show path, along with all other state data.
pathTr = graphObj.States(solutionInfoTransporter.PathStateIDs,:)

pathTr=28×2 table
      StateVector       Height
    ________________    ______

      99.5     700.5    567.98
     199.5     800.5    554.13
     299.5     900.5    507.06
     399.5    1000.5    572.18
     499.5    1100.5    619.22
     599.5    1100.5    728.65
     699.5    1100.5    764.68
     799.5    1000.5    687.09
     899.5     900.5    792.39
     899.5     800.5    818.72
     899.5     700.5    785.51
     999.5     600.5    651.14
    1099.5     600.5    723.14
    1199.5     500.5    795.17
    1299.5     400.5    742.55
    1399.5     400.5    688.49
      ⋮

if(~isempty(pathTr))
    pathPoses = [pathTr.StateVector pathTr.Height];
    transporterPathLength = sum(nav.algs.distanceEuclidean(pathPoses(1:end-1,:),pathPoses(2:end,:)))
end

transporterPathLength = 3.8136e+03

Ruta del vehículo excavador

Debido a que el terreno montañoso no limita mucho a las excavadoras, la pendiente no es una preocupación para calcular los costes de recorrido.

La función de coste de transición del vehículo excavador excavatorVehicleTransitionCostFcn utiliza estas funciones de coste:

• Euclidean Distance

• Bridge Road Cost (Recompensa)

La función heurística de coste del vehículo excavador excavatorVehicleHeuristicCostFcn utiliza estas funciones de coste:

• Euclidean Distance

Actualice los navGraph y los objetos del planificador con la función de coste de la excavadora.

% Update the cost function of the navGraph object.
graphObj.LinkWeightFcn = @(state1,state2)excavatorVehicleTransitionCostFcn(state1,state2,graphObj)

graphObj = 
  navGraph with properties:

           States: [529x2 table]
            Links: [3802x5 table]
    LinkWeightFcn: @(state1,state2)excavatorVehicleTransitionCostFcn(state1,state2,graphObj)

% Create planner object
plannerExcavatorObj = plannerAStar(graphObj);

% Update the cost function of the planner object.
plannerExcavatorObj.HeuristicCostFcn = @(state1,state2)excavatorVehicleHeuristicCostFcn(state1,state2,graphObj)

plannerExcavatorObj = 
  plannerAStar with properties:

    HeuristicCostFcn: @(state1,state2)excavatorVehicleHeuristicCostFcn(state1,state2,graphObj)
          TieBreaker: 0
               Graph: [1x1 navGraph]

Planifique la trayectoria del vehículo excavador utilizando un algoritmo A* basado en gráficos.

% Find the shortest path using graph-based A* algorithm.
[pathExcavator,solutionInfoExcavator] = plan(plannerExcavatorObj,startID,goalID);

% Show path, along with all other state data.
pathEx = graphObj.States(solutionInfoExcavator.PathStateIDs,:)

pathEx=19×2 table
      StateVector       Height
    ________________    ______

      99.5     700.5    567.98
     199.5     800.5    554.13
     299.5     900.5    507.06
     399.5    1000.5    572.18
     499.5    1100.5    619.22
     599.5    1200.5    694.06
     699.5    1200.5    623.38
     799.5    1200.5    545.85
     899.5    1200.5     670.5
     999.5    1200.5    761.96
    1099.5    1300.5    792.39
    1199.5    1300.5    846.44
    1299.5    1300.5    1052.9
    1399.5    1400.5    915.71
    1499.5    1500.5    795.17
    1599.5    1500.5    707.93
      ⋮

if(~isempty(pathEx))
    pathPoses = [pathEx.StateVector pathEx.Height];
    excavatorPathLength = sum(nav.algs.distanceEuclidean(pathPoses(1:end-1,:),pathPoses(2:end,:)))
else
    disp("Path not found.")
end

excavatorPathLength = 2.6276e+03

Comparación y visualización de rutas

Trace las rutas planificadas del transportador y la excavadora en comparación con el mapa de ocupación, con y sin información de enlace y estado superpuesta. En cada gráfico, la ruta amarilla representa un vehículo de transporte, que evita el terreno montañoso tomando una ruta más larga. Como se menciona en la función de coste, tomar el puente está penalizado, por lo que se evita tomar el puente tanto como sea posible. Y la heurística penaliza el paso por alturas, por lo que intenta permanecer en terrenos más bajos tanto como sea posible.

La ruta naranja representa una excavadora que toma una ruta más corta hacia la meta, que no tiene ninguna restricción mientras viaja por el terreno.

Los vehículos obtuvieron diferentes rutas según sus funciones de costes.

figure

% Prepare elevation map for plotting.
axHandle = newplot;
imHandle = map.show(Parent=axHandle);
title("Mt. Washington")

% Plot paths.
exampleHelperPlotPathStartGoal(axHandle,pathExcavator,pathTransporter,start,goal);

% Prepare the map to be overlaid with the graph.
axHandle2 = newplot;
imHandle2 = show(map,Parent=axHandle2);
title("navGraph")

% Plot graph.
exampleHelperPlotGraph(axHandle2,graphObj);

% Plot paths.
exampleHelperPlotPathStartGoal(axHandle2,pathExcavator,pathTransporter,start,goal);

Conclusión

Como puede verse en los gráficos, la función de costes juega un papel importante en la búsqueda de una ruta. Esto se debe a que cada vehículo tiene su propio conjunto de restricciones formuladas en funciones de costes y, por lo tanto, dirige los resultados.

Además, puede ajustar la función de coste para relajar o restringir cada comportamiento con pesos y observar cómo la ruta cambia en consecuencia.

Funciones de costes

Distancia euclidiana — Distancia aproximada recorrida por un vehículo.

$$\mathit{d}=\sqrt{\sum _{\mathit{i}=1}^{\mathit{n}}{\left({\mathit{q}}_{\mathit{i}}-{\mathit{p}}_{\mathit{i}}\right)}^2},\mathrm{where}\mathit{p}\mathrm{and}\mathit{q}\mathrm{are}\mathrm{two}\mathrm{states}\mathrm{in}\mathrm{space}.$$

$$\mathrm{Each}\mathrm{state}\mathrm{consists}\mathrm{of}a\mathrm{vector}\mathrm{of}\mathrm{the}\mathrm{form}[x,y,z],\mathrm{representing}\mathrm{the}\mathrm{Cartesian}\mathrm{coordinates}\mathrm{of}\mathrm{the}\mathrm{state}.$$

Coste de cambio de gradiente: penaliza la pendiente de la ruta en función de la pendiente. Para pendientes más pronunciadas, los costes son más altos que para pendientes más graduales. Utilice esta función para elegir una ruta que permanezca en un plano similar.

$$\mathit{gradientCost}=\mathit{w}*\frac{\left|{\mathit{q}}_3-{\mathit{p}}_3\right|}{\mathit{d}},\mathrm{where}\mathit{w}\mathrm{is}\mathrm{the}\mathrm{weight},\mathrm{and}{\mathit{p}}_3\mathrm{and}{\mathit{q}}_3\mathrm{are}\mathrm{the}\mathrm{heights}\mathrm{of}\mathrm{the}\mathrm{respective}\mathrm{states}.$$

Coste de penalización de altura: penaliza las rutas que superan ciertas alturas.

$$\mathit{htPenalty}=\{\begin{array}{ll}\mathit{w}& ,\mathrm{for}{\mathit{s}}_3>\mathit{r}*\mathit{maxHeight}\\ 0& ,\mathrm{for}{\mathit{s}}_3\le \mathit{r}*\mathit{maxHeight}\end{array},\mathrm{where}{\mathit{s}}_3\mathrm{is}\mathrm{the}\mathrm{height}\mathrm{of}\mathit{a}\mathrm{given}\mathrm{state},\mathrm{and}\mathit{r}\mathrm{is}\mathrm{the}\mathrm{percentage}\mathrm{of}\mathrm{maximum}\mathrm{height}.$$

Coste de la carretera: coste de conducir por una autopista, según la velocidad y el peso. Esta función premia o penaliza la trayectoria, según el vehículo.

$$\mathit{highwayCost}=\{\begin{array}{ll}\mathit{w}*\mathit{maxSpeed}& ,\mathrm{for}\mathit{link}.\mathit{Highway}==1\\ 0& ,\mathrm{for}\mathit{link}.\mathit{Highway}==0\end{array},\mathrm{where}\mathit{maxSpeed}\mathrm{is}\mathrm{the}\mathrm{maximum}\mathrm{speed}\mathrm{permitted}\mathrm{on}\mathrm{the}\mathrm{road},\mathrm{and}\mathit{w}\mathrm{the}\mathrm{weight}.$$

Bridge Road Cost — Coste de cruzar el puente, basado en la velocidad y el peso. Esta función premia o penaliza la trayectoria, según el vehículo.

$$\mathit{bridgeCost}=\{\begin{array}{ll}\mathit{w}*\mathit{maxSpeed}& ,\mathrm{for}\mathit{link}.\mathit{Bridge}==1\\ 0& ,\mathrm{for}\mathit{link}.\mathit{Bridge}==0\end{array},\mathrm{where}\mathit{maxSpeed}\mathrm{is}\mathrm{the}\mathrm{maximum}\mathrm{speed}\mathrm{permitted}\mathrm{on}\mathrm{the}\mathrm{road},\mathrm{and}\mathit{w}\mathrm{the}\mathrm{weight}.$$

Funciones de costes personalizadas

El transporterVehicleTransitionCostFcn utiliza la distancia euclidiana como coste base. La función también penaliza las rutas con pendiente alta en función de cierto peso. Por último, la función penaliza el uso de puentes y premia el uso de autopistas en función de la velocidad máxima permitida en cada enlace.

function cost = transporterVehicleTransitionCostFcn(id1,id2,navObj)
% transporterVehicleTransitionCostFcn computes transition cost based on
% constraints.
    
    % Extract states from indices.
    state1 = navObj.index2state(id1);
    state2 = navObj.index2state(id2);
    
    % Add height parameter to define pose.
    pose1 = [state1 navObj.States.Height(id1)];
    pose2 = [state2 navObj.States.Height(id2)];

    % Compute Euclidean distance. 
    distCost = nav.algs.distanceEuclidean(pose1,pose2);

    bridgePenaltyWt = 100;
    gradientPenaltyWt = 20;
    highwayRewardWt = 1;

    % Compute gradientCost to reduce change in ascent or descent.
    h1 = navObj.States.Height(id1);
    h2 = navObj.States.Height(id2);
    gradient = abs(h2-h1)./distCost;
    gradientCost = gradient*gradientPenaltyWt;

    % Reward taking highway roads with speed cost.
    linkids = navObj.findlink([id1 id2]);
    highwayReward = highwayRewardWt*double(navObj.Links.Highway(linkids,:)).*navObj.Links.MaxSpeedTra(linkids,:);

    % Penalize taking bridges.
    bridgePenalty = bridgePenaltyWt*double(navObj.Links.Bridge(linkids,:)).*navObj.Links.MaxSpeedTra(linkids,:);
    
    % Bring the costs to the same scale.
    cost = distCost + gradientCost - highwayReward + bridgePenalty;
end

El transporterVehicleHeuristicCostFcn utiliza la distancia euclidiana en 2-D como coste base y penaliza a los estados más allá de cierta altura para evitar esas áreas tanto como sea posible.

function cost = transporterVehicleHeuristicCostFcn(state1,state2,navObj)
% transporterVehicleHeuristicCostFcn computes heuristic cost for transporter based on constraints.
    distCost = nav.algs.distanceEuclidean(state1,state2);

    maxHtRatio = 0.75;
    heightPenaltyWt = 1000;

    % Avoid high peaks.
    stateIDs = navObj.state2index(state1);
    h1 = navObj.States.Height(stateIDs);
    htPenalty = double(h1 > (maxHtRatio*max(navObj.States.Height)));
    heightPenalty = heightPenaltyWt*htPenalty;

    % Bring the costs to the same scale.
    cost = distCost + heightPenalty;
end

El excavatorVehicleTransitionCostFcn utiliza la distancia euclidiana como coste base y recompensa las rutas conectadas a través de puentes.

function cost = excavatorVehicleTransitionCostFcn(state1,state2,navObj)
% excavatorVehicleTransitionCostFcn computes transition cost for excavator
% based on constraints.
    stateID1 = navObj.state2index(state1);
    stateID2 = navObj.state2index(state2);
    
    wt = 1;
    % Add scaled height parameter for Euclidean distance.
    pose1 = [state1 navObj.States.Height(stateID1)*wt];
    pose2 = [state2 navObj.States.Height(stateID2)*wt];

    distCost = nav.algs.distanceEuclidean(pose1,pose2);

    % Reward taking bridge roads with speed cost.
    linkids = navObj.findlink([stateID1 stateID2]);
    speedCost = double(navObj.Links.Bridge(linkids,:)).*navObj.Links.MaxSpeedExc(linkids,:);
    
    % Bring the costs to the same scale.
    cost = distCost - speedCost;
end

El excavatorVehicleHeuristicCostFcn utiliza la distancia euclidiana como coste base.

function distCost = excavatorVehicleHeuristicCostFcn(state1,state2,navObj)
% excavatorVehicleHeuristicCostFcn computes heuristic cost for excavator
% based on constraints.
    stateID1 = navObj.state2index(state1);
    stateID2 = navObj.state2index(state2);
    
    wt = 1;
    % Add scaled height parameter for Euclidean distance.
    pose1 = [state1 navObj.States.Height(stateID1)*wt];
    pose2 = [state2 navObj.States.Height(stateID2)*wt];
 
    distCost = nav.algs.distanceEuclidean(pose1,pose2);
end

Funciones auxiliares

helperFitStatesInMap mueve puntos en el límite, movidos afuera debido a la descomposición, dentro del mapa.

function states = helperFitStatesInMap(states,map)
    states(states(:,1) < map.XWorldLimits(1),1) = map.XWorldLimits(1);
    states(states(:,1) > map.XWorldLimits(2),1) = map.XWorldLimits(2);
    states(states(:,2) < map.YWorldLimits(1),2) = map.YWorldLimits(1);
    states(states(:,2) > map.YWorldLimits(2),2) = map.YWorldLimits(2);
end