Create Terrain-Aware Global Planners for Offroad Navigation

Since R2024a

Step 2 of 4 in Offroad Navigation for Autonomous Haul Trucks in Open Pit Mine

In the previous example, a graph-based planner was developed to plan routes through a road network derived from the offroad elevation data of a copper mine. In this section, you will develop a simple onramp planner to help connect the vehicle to the shortest path through the road-network. Later, you will provide plannerHybridAStar with a custom cost function that allows for terrain-aware planning. This planner can be used in regions of the map that lack structure, or as a fallback planner in the event that the path-following controller fails to find a local solution.

Load Path Information from Image Procession Pipeline

Begin with the image and path-list results produced towards the end of the terrain processing example. For this example, you can load data generated during a previous run of the first example, or if you decided to save your own results, you can use those here instead.

load("OpenPitMinePart1Data.mat","imSlope","pathList","dem");

In the previous example, you identified the "roads" through the terrain using image processing techniques and a few example helpers. These paths were stored as a struct-array containing Nx2 sequences of IJ indices. Reality might be a simulation, but it still tends to be mapped in cartesian or geodesic coordinates, so let's convert these roads to XY using an occupancy-map populated with the slope-mask of the terrain, imSlope.

% Create map with resolution of 1 cell per meter
res = 1;
binMap = binaryOccupancyMap(~imSlope,res);

% Convert the grid-based paths to local XY coordinates
localPathList = pathList;
for i = 1:numel(pathList)
    localPathList(i).Path = grid2local(binMap,pathList(i).Path);
end

Plan Path Using A*

Similar to the previous example, convert the list of XY paths to a navGraph and place this inside plannerAStarGrid. This will serve as a high-level planner that generates pre-computed routes through the mine. For this example, the maxEdgeSize is set to 50. This should improve the likelihood that any start and goal SE2 poses will have a collision-free path to the nearest node's edges, while keeping the graph relatively sparse.

% Convert raw path list to graph information
maxElementPerEdge = 50;
[nodes,edges,edge2pathIdx,cachedPaths] = exampleHelperPath2GraphData(localPathList,maxElementPerEdge);

% Compute edge weights
edgeCosts = exampleHelperDefaultEdgeCost(cachedPaths,edge2pathIdx);

% Create A* planner using the navGraph
stateTable = table(nodes,VariableNames={'StateVector'});
linkTable = table(edges,edgeCosts,edge2pathIdx(:),VariableNames={'EndStates','Weight','Edge2PathIdx'});
roadNetwork = navGraph(stateTable,linkTable);
routePlanner = plannerAStar(roadNetwork);

Next plan a path through the graph, with start and goal poses lying away from the graph. Note that these poses contain orientation, which will become relevant in the later sections, but the graph only considers XY.

% Make start location slightly off the graph
start = [286.5 423.5 -pi/2];
goal  = [795.5 430.5  pi];

% Find closest node to start and goal location
[dStart,nearStartIdx] = min(vecnorm(nodes(:,1:2)-start(1:2),2,2));
[dGoal,nearGoalIdx] = min(vecnorm(nodes(:,1:2)-goal(1:2),2,2));

% Plan a route using just the graph
[waypoints, solnInfo] = plan(routePlanner,nearStartIdx,nearGoalIdx);

Once a path is found, use the Edge2PathIdx metadata stored on the edges of your navGraph to reconstruct the full path from cachedPaths:

% Convert path to link-sequence
edgePairs = [solnInfo.PathStateIDs(1:end-1)' solnInfo.PathStateIDs(2:end)'];
linkID = findlink(routePlanner.Graph,edgePairs);
networkPath = vertcat(cachedPaths(routePlanner.Graph.Links.Edge2PathIdx(linkID)).Path);

Downstream controllers often benefit from smooth reference paths if the vehicle has non-holonomic constraints. There are many ways to smooth a path via curve fitting or interpolation, here the exampleHelperSmoothReferencePath helper subsamples the incoming path, fits a piece-wise continuous cubic polynomial to the remaining points and then resamples the path and orientation. A larger subsampleInterval should result in a smoother path but can also allow for a greater deviation between original and resampled paths.

subsampleInterval = 5;
smoothedNetworkPath = exampleHelperSmoothReferencePath(networkPath,subsampleInterval);

Visualize Road Network and Path found Using plannerAStar

hIm = show(binMap);
hold on

gHandle = show(routePlanner.Graph); 
gHandle.XData = routePlanner.Graph.States.StateVector(:,1); 
gHandle.YData = routePlanner.Graph.States.StateVector(:,2);
exampleHelperPlotLines(networkPath,{"LineWidth",4});
exampleHelperPose2Quiver(smoothedNetworkPath,{"AutoScale","off","LineWidth",2});
startSpec = plannerLineSpec.start(MarkerEdgeColor="red",MarkerFaceColor="red");
plot(start(1),start(2),startSpec{:})
goalSpec = plannerLineSpec.goal(MarkerEdgeColor="green",MarkerFaceColor="green");
plot(goal(1),goal(2),goalSpec{:})
legendStrings = ["navGraph","networkPath","smoothedPath","Start","Goal"];
legend(legendStrings);

Develop Onramp Planner

The networkPath generated by the route planner in the previous section only represents the shortest path inside the network, so the next step is to connect the start location to the route.

First look up all nodes that are connected to the start node using the graph's successors function:

successorNodeIDs = successors(routePlanner.Graph,solnInfo.PathStateIDs(1));

next use the graph's findlink function to retrieve the IDs of the links connecting the initial node to its successors:

initLinkNodePair = [repmat(solnInfo.PathStateIDs(1),numel(successorNodeIDs),1) successorNodeIDs(:)];
initLinkID = findlink(routePlanner.Graph,initLinkNodePair);

and lastly retrieve the full paths for each edge by indexing into the cachedPaths using the Edge2PathIdx meta-data stored in links.

initEdgePaths = cachedPaths(routePlanner.Graph.Links.Edge2PathIdx(initLinkID));

% Display local neighbor paths to initial node in the solution
for i = 1:numel(initEdgePaths)
    exampleHelperPlotLines(initEdgePaths(i).Path,{":","LineWidth",3});
end
exampleHelperPaddedZoom(initEdgePaths,2,DataGetter=@(x)x.Path,PaddingFactor=1.5);
legend([legendStrings(:); "localEdge_" + (1:numel(initEdgePaths))']);

Next, use the helper exampleHelperConnectRSToPath to find a collision-free connection between the start pose and networkPath. The helper works by attempting to connect a Reeds-Shepp segment between the initial pose and the edges connected to the start-node. These connections are attempted from the nearest to farthest points and exits when a collision-free segment is found, or all points have been exhausted. During this process, the dimensions of the vehicle are taken into account to ensure the path is truly collision-free. Vehicle dimensions used in this example are defined inside helper exampleHelperMiningTruckSpecs.

% Define the parameters for a large mine vehicle 
[vehicleLength, vehicleWidth, minTurnRadius] = exampleHelperMiningTruckSpecs;
vehDims = exampleHelperVehicleGeometry(vehicleLength,vehicleWidth,"collisionChecker");

% Find closest collision-free path between start state and planned route
onrampPath = exampleHelperConnectRSToPath(smoothedNetworkPath, initEdgePaths, start, binMap, vehDims,minTurnRadius);

The final step is to combine the two path segments.

origStates = [onrampPath; smoothedNetworkPath];

% Display the nearest path and closest neighbor
exampleHelperPlotLines(onrampPath,{"LineWidth",3});
exampleHelperPaddedZoom(onrampPath,2,PaddingFactor=2);
legend([legendStrings(:); "localEdge_" + (1:numel(initEdgePaths))';"onrampRoute"],'Location','southwest')

Develop Terrain-Aware Planner

In the previous sections you developed tools for creating and planning on road networks where none originally existed, but what should be done if there is no clear way to reach this network, or if the vehicle is in a region where a road network cannot be inferred? In this section you will learn how to make plannerHybridAStar "terrain-aware" by supplying a custom cost function.

Define Parameters of Vehicle and Global Planner

Begin by specifying some parameters needed to represent the geometry and kinematic constraints of the vehicle. These have been roughly separated into a set of parameters that might change throughout the vehicle's operation, like costs, and those that will likely remain fixed, like the length and width of the vehicle:

% Get params for free-space planner
[tuneableTerrainAwareParams, fixedTerrainAwareParams] = exampleHelperTerrainPlannerParams

tuneableTerrainAwareParams = struct with fields:
          MinTurningRadius: 17.2000
     MotionPrimitiveLength: 22.5147
               ForwardCost: 1
               ReverseCost: 1
    DirectionSwitchingCost: 1
                  MaxAngle: 15

fixedTerrainAwareParams = struct with fields:
                       Length: 6.5000
                        Width: 5.7600
          NumMotionPrimitives: 5
    AnalyticExpansionInterval: 3
        InterpolationDistance: 1
                   NumCircles: 3
                   Resolution: 1

Next, convert the digital elevation model generated in the previous example to a cost map containing gradient and cost information:

% Create costMaps from dem
[costMap,maxSlope] = exampleHelperDem2mapLayers(dem,tuneableTerrainAwareParams.MaxAngle,fixedTerrainAwareParams.Resolution);
obstacles = getLayer(costMap,"terrainObstacles");

For the sake of this example, let's ensure there is enough room around the previously-computed road network to allow the global planners to operate. To do this, convert the road-network to an occupancy map, inflate it by a desired radius, and then remove those inflated cells from the slope mask. Note that in the real world, you may instead want to remove roads that are too narrow to traverse.

% Create an empty map of equal size to current obstacle map
tmpMap = binaryOccupancyMap(zeros(obstacles.GridSize),Resolution=obstacles.Resolution);

figure;
subplot(1,3,1);
show(obstacles);
title('Original Slope Mask');

% Inflate the road network
vehDims = exampleHelperVehicleGeometry(fixedTerrainAwareParams.Length,fixedTerrainAwareParams.Width,"collisionChecker");
collisionChecker = inflationCollisionChecker(vehDims,fixedTerrainAwareParams.NumCircles);
setOccupancy(tmpMap,vertcat(pathList.Path),1,'grid');
inflate(tmpMap,collisionChecker.InflationRadius*1.5);

% Display inflated road network
subplot(1,3,2);
show(tmpMap);
title('Inflated Road Network');

% Remove inflated skeleton from the original map
occupancyMatrixOriginal = obstacles.occupancyMatrix;
occupancyMatrixWidened = occupancyMatrixOriginal & ~tmpMap.occupancyMatrix;
setOccupancy(obstacles,occupancyMatrixWidened);
widenedRegions = occupancyMatrixWidened ~= occupancyMatrixOriginal;

% Display road network (blue) & widened regions (red) 
subplot(1,3,3);
show(obstacles);
exampleHelperOverlayImage(flipud(tmpMap.occupancyMatrix),gca,Color=[0 0.4470 0.7410]);
exampleHelperOverlayImage(flipud(widenedRegions),gca,Color=[1,0,0]);
title('Widened Slope Mask');

Create plannerHybridStar Object with Custom Cost Function

The example helper exampleHelperCreateTerrainPlanner initializes plannerHybridAStar with the previously created costMap and parameters.

terrainPlanner = exampleHelperCreateTerrainPlanner(costMap,tuneableTerrainAwareParams,fixedTerrainAwareParams);

By default the helper populates the planner's TransitionCostFcn with exampleHelperZHeuristic:

transitionFcn = @(motionSegment)exampleHelperZHeuristic(motionSegment, costMap, gWeight)

This helper adds the change in terrain height to the distance along the motion-primitive, but any function which supports the TransitionCostFcn interface can be supplied as an optional fourth input. See the following for more details:

help nav.algs.hybridAStar.transitionCost

Plan Terrain-Aware Path

Next you will set the end of the path generated from the routePlanner as the start of the terrain-aware problem and plan a path to the goal location that lies off the road network.

start = origStates(end,:); 

% Plan route
[terrainAwarePath,motionDir,solnInfo] = plan(terrainPlanner,start,goal);

% Save the robot states by concatenating the terrain aware results from the end of the path.
networkPoses = [networkPath headingFromXY(networkPath)];
originalReferencePath = [onrampPath; networkPoses; terrainAwarePath.States];
smoothedReferencePath = [origStates; terrainAwarePath.States];

Visualize Path from Terrain-Aware Planner

% Display map, start, and goal
obstacles = getLayer(costMap,"terrainObstacles");
figure;
subplot(1,2,1)
axis square;
hndl1 = show(obstacles);
hold on;
plot(start(1),start(2),startSpec{:})
plot(goal(1),goal(2),goalSpec{:})
legend
subplot(1,2,2)
axis square;
hndl2 = copyobj(hndl1, gca);
hold on;
plot(start(1),start(2),startSpec{:})
plot(goal(1),goal(2),goalSpec{:})
set(hndl2.Parent,"Ylim",[400 450],"Xlim",[780 820])
title("Zoomed-In View")
legend

% Display planner results
figure
show(terrainPlanner);
hold on;
exampleHelperVisualizeTerrainPlanner(terrainAwarePath.States,costMap,fixedTerrainAwareParams,tuneableTerrainAwareParams);
plot(start(1),start(2),startSpec{:})
plot(goal(1),goal(2),goalSpec{:})
title("Terrain Aware Motion Planning")

Conclusion

In this example you developed two different ways of generating collision-free paths while satisfying the vehicle's kinematic and geometric constraints.

The first half of the example focused on generating collision-free paths between a vehicle's SE2 location and a route found in the road network. You used the reedsSheppConnection supplied with the vehicle's MinTurningRadius to generate potential connections to the network, and used a validatorVehicleCostmap supplied with the vehicle's dimensions to check whether those paths were collision-free.

The second half of the example focused on supplying plannerHybridAStar with a custom cost function that added elevation to the transition cost, biasing the planner towards solutions that minimize vertical motion. This planner also received the same MinTurningRadius and validatorVehicleCostmap, and can be used in scenarios where the simple onramp planner fails to find a connection or in regions of the map where the road-network does not exist.

In Navigate Global Path through Offroad Terrain Using Local Planner, you will use controllerTEB as a local-planner capable of following the global paths generated here while avoiding obstacles and adhering to the same kinematic and geometric constraints.