Route Planning in Uneven Terrain Based on Vehicle Requirements

This example shows how to use navGraph and plannerAStar to find a path through rough terrain while accounting for vehicle-based requirements and constraints.

This example illustrates construction activities taking place in a mountainous area. This area contains two distinct categories of vehicles. First, the transporter vehicle, which has difficulty climbing hills, requires gradual ascent or descent, and consumes more fuel while traversing heights. Second, construction equipment like excavators, which can drive over hills with ease. Transporters can travel faster on smooth terrain, and can benefit from longer routes. In contrast, construction equipment can benefit from shorter routes because it is less affected by rough terrain.

Create Base Terrain Map

This example uses the Mount Washington U.S. Geological Survey (USGS) digital elevation model. If you have a Mapping Toolbox™ license, you can use this code to load the relevant GeoTIFF data.

% Using the Mount Washington USGS digital elevation model.

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

Otherwise, this example includes the relevant data in a MAT file. Load the MAT file into the workspace.

load("USGS_four_meter_x32y491_NH_Umbagog_2016.mat")

To create a base terrain map from the elevation data normalize the height values. Then, create an occupancy map using the elevation data, by specifying a height threshold above which the map denotes a location as occupied.

% 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;

Extract States and Links Using Grid-Based Area Decomposition

Extract the graph from the occupancy map using the grid decomposition method. First, divide the map into grids based on a selected resolution, then mark the states (nodes) at each intersection for the graph.

% 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);

Add Parameters

Define parameters that provide additional structure to the scene. For example, graph links that lie on flat terrain are marked as highways. These links define the links that span regions over a given height and connect states at different levels as bridges.

Cost functions defined later in the example will use this information to generate different routes for different vehicles.

% 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);

Create States and Links Table

Combine all of your data to create state and link tables.

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     

Create navGraph Object and Visualize Graph Data

Create a navGraph object from the state and link tables that comprises all data. You can use the navGraph object in cost functions to perform cost calculations.

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")

Set Up Start and Goal

Specify your start position and goal position, and then find the closest state index for each position.

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

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

Transporter Vehicle Path

Transporter vehicles work best when making gradual ascents and descents and avoiding heights.

The transporter vehicle transition cost function transporterVehicleTransitionCostFcn uses these cost functions:

• Euclidean Distance

• Gradient Change Cost

• Highway Road Cost (Reward)

• Bridge Road Cost (Penalty)

The transporter vehicle heuristic cost function transporterVehicleHeuristicCostFcn uses these cost functions:

• 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]

Plan the transporter vehicle path using a graph-based A* algorithm.

% 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

Excavator Vehicle Path

Because hilly terrain does not constrain excavators much, gradient is not a concern for calculating their path costs.

The excavator vehicle transition cost function excavatorVehicleTransitionCostFcn uses these cost functions:

• Euclidean Distance

• Bridge Road Cost (Reward)

The excavator vehicle heuristic cost function excavatorVehicleHeuristicCostFcn uses this cost functions:

• Euclidean Distance

Update the navGraph and planner objects with the excavator cost function.

% 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]

Plan the excavator vehicle path using a graph-based A* algorithm.

% 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

Path Comparison and Visualization

Plot the planned paths of the transporter and excavator against the occupancy map, both with and without overlaid state and link information. In each plot, the yellow path represents a transporter vehicle, which avoids hilly terrain by taking a longer route. As mentioned in the cost function, taking bridge is penalized, hence it avoids the taking bridge as much as possible. And heuristic penalize going over heights, so it tries to stay on lower terrain as much as possible.

The orange path represents an excavator taking a shorter route to the goal, which doesn't have any constraint while travelling in terrain.

The vehicles obtained different paths based on their cost functions.

figure

% Prepare elevation map for plotting.
axHandle = newplot;
imHandle = show(map,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);

Conclusion

As you can see from the plots, the cost function plays a significant role in the search for a path. This is because every vehicle has its own set of constraints formulated in cost functions and thus directs the results.

Additionally, you can adjust the cost function to relax or restrict each behavior with weights and observe how the path changes accordingly.

Cost Functions

Euclidean Distance — Approximate the distance traveled by a vehicle.

$$\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}.$$

Gradient Change Cost — Penalizes the slope of the path based on slope. For steeper slopes, the costs are higher than for more gradual slopes. Use this function to choose a path that stays on a similar plane.

$$\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}.$$

Height Penalty Cost — Penalizes paths that go over certain heights.

$$\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}.$$

Highway Road Cost — Cost of driving over a highway, based on speed and weight. This function rewards or penalizes the path, based on the vehicle.

$$\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 — Cost of crossing the bridge, based on speed and weight. This function rewards or penalizes the path, based on the vehicle.

$$\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}.$$

Custom Cost Functions

The transporterVehicleTransitionCostFcn uses Euclidean distance as the base cost. The function also penalizes high-gradient paths based on certain weight. Lastly, the function penalizes using bridges and rewards using highways based on maximum speed permitted on each link.

function cost = transporterVehicleTransitionCostFcn(id1,id2,navObj)
% transporterVehicleTransitionCostFcn computes transition cost based on
% constraints.
    
    % Extract states from indices.
    state1 = index2state(navObj,id1);
    state2 = index2state(navObj,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 = findlink(navObj,[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

The transporterVehicleHeuristicCostFcn uses Euclidean distance in 2-D as the base cost, and penalizes states beyond a certain height to avoid those areas as much as possible.

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 = state2index(navObj,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

The excavatorVehicleTransitionCostFcn uses Euclidean distance as the base cost, and rewards the paths connected through bridges.

function cost = excavatorVehicleTransitionCostFcn(state1,state2,navObj)
% excavatorVehicleTransitionCostFcn computes transition cost for excavator
% based on constraints.
    stateID1 = state2index(navObj,state1);
    stateID2 = state2index(navObj,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 = findlink(navObj,[stateID1 stateID2]);
    speedCost = double(navObj.Links.Bridge(linkids,:)).*navObj.Links.MaxSpeedExc(linkids,:);
    
    % Bring the costs to the same scale.
    cost = distCost - speedCost;
end

The excavatorVehicleHeuristicCostFcn uses Euclidean distance as the base cost.

function distCost = excavatorVehicleHeuristicCostFcn(state1,state2,navObj)
% excavatorVehicleHeuristicCostFcn computes heuristic cost for excavator
% based on constraints.
    stateID1 = state2index(navObj,state1);
    stateID2 = state2index(navObj,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

Helper Functions

helperFitStatesInMap moves points on the boundary, moved outside because of decomposition, inside the map.

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