Plan and Follow Path with Offroad Vehicle

R2026b

Since R2026b

This example shows how to load the haul truck vehicle model from the Offroad Vehicle Library, plan a collision-free reference path using plannerHybridAStar (Navigation Toolbox), and follow the path using offroadControllerMPPI.

Load the haul truck model and retrieve its vehicle data. For more information about the dimensions of the haul truck, see the Haul Truck page.

[haulTruck,haulTruckData] = loadvehicle("haultruck");
truckWidth = haulTruckData.TrackWidth;
truckLength = 18.05;
rAxleOffset = [-3.2587 0 0]; % Offset of vehicle body center from the rear axle

Create an occupancy map and inflate the map by the half the width of the haul truck to ensure clearance. Then create a validator for ensuring the vehicle does not collide with any obstacles.

load("smallOffroadMap.mat")
map = occupancyMap(smallOffroadMap);
inflatedMap = copy(map);
inflate(inflatedMap,truckWidth/2);

ss = stateSpaceSE2;
ss.StateBounds = [map.XWorldLimits; map.YWorldLimits; [-pi pi]];

sv = validatorOccupancyMap(ss,Map=inflatedMap);
sv.ValidationDistance = 0.5;

Plan a collision-free path using plannerHybridAStar with the minimum turning radius of the haul truck.

planner = plannerHybridAStar(sv,MinTurningRadius=haulTruckData.MinTurningRadius);

startPose = [135 15 2*pi/3];
goalPose = [110 240 pi/3];
truckPath = startPose(1:2);
pathObj = plan(planner,startPose,goalPose);
refPath = pathObj.States(:,1:2);

Visualize the start, goal, reference path, and initialize the truck path plot.

show(map);
hold on
plot(startPose(1),startPose(2),plannerLineSpec.start{:});
plot(goalPose(1),goalPose(2),plannerLineSpec.goal{:});
plot(refPath(:,1),refPath(:,2),plannerLineSpec.path{:});
truckPathLine = plannerLineSpec.path(LineStyle="--",Color=gca().ColorOrder(6,:),DisplayName="Truck Path");
hTruckPath = plot(truckPath(:,1),truckPath(:,2),truckPathLine{:});
hTruckPath.Marker = "none";
legend(AutoUpdate="off")
title("Reference Path")

Visualize the haul truck at the start pose of the reference path.

show(haulTruck,Position=[startPose(1) startPose(2) 0 startPose(3)], ...
    Parent=gca,Frames="off",PreservePlot=false,FastUpdate=true,Visuals="on");
axis([85 165 0 80 0 10])
view(-45,30)
title("Haul Truck Model at Starting Pose")

Create an offroadControllerMPPI controller to follow the planned path. Specify the motion model of the haul truck and set the collision information using dimensions of the haul truck.

mppi = offroadControllerMPPI(refPath,Map=map, ...
    VehicleModel=haulTruckData.MotionModel);
mppi.Parameter.VehicleCollisionInformation.Dimension = [truckLength truckWidth];
mppi.Parameter.VehicleCollisionInformation.Offset = rAxleOffset;
mppi.Map = map;

Set up the path following animation with a top-down view. Initialize the truck path plotted line.

view(2);
axis([0 250 0 250 0 10])
title("Haul Truck Path Planning and Following");

Simulate the path following. At each time step, compute the optimal controls, propagate the vehicle state, and update the animated figure.

curState = startPose;
curControl = [0 0];
dt = mppi.SampleTime;

isGoalReached = false;
while ~isGoalReached
    % Update controls using MPPI to follow path
    [optControls,optTraj,info] = mppi(curState,curControl);
    curControl = optControls(1,:);
    stateDot = derivative(haulTruckData.MotionModel,curState',curControl');
    curState = curState + stateDot' * dt;
    isGoalReached = info.HasReachedGoal;

    % If controller cannot follow path then break
    if info.ExitFlag == 2
        break
    end

    % Update visualization
    truckPath = [truckPath; curState(1:2)];
    set(hTruckPath,XData=truckPath(:,1));
    set(hTruckPath,YData=truckPath(:,2));
    show(haulTruck,Position=[curState(1) curState(2) 0 curState(3)], ...
        Parent=gca,Frames="off",PreservePlot=false,FastUpdate=true,Visuals="on");
    drawnow;
end
hold off