Model and Control Autonomous Vehicle in Offroad Scenario

Since R2024a

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

This example shows how to integrate planners and controllers designed to enable a haul truck to navigate between points of interest in an open pit mine into Simulink® and Stateflow®. This example builds on the processes introduced in these examples:

Create Autonomous Planning Stack

Set up the maps and vehicle parameters using the exampleHelperCreateBehaviorModelInputs and exampleHelperPrepMPCInputs helper functions.

exampleHelperCreateBehaviorModelInputs
exampleHelperPrepMPCInputs

Define a start location, goal location, and initial translation and rotation for the haul truck. The start location and goal locations are given in 2-D space as [x, y, theta]. The initial translation of the truck is a 3-D xyz-coordinate, adjusting for the map center and terrain elevation. The initial rotation of the truck is a 3-D rotation as [yaw, pitch, roll].

startPose = [267.5 441.5 -pi/2]; % [x,y,theta]
goalPose  = [479.5  210.5 pi/2]; % [x,y,theta]
initialTranslationTruck = [(startPose(1:2)-mapSize/2) mapHeight.getMapData(startPose(1:2))-zOffset+8];
initialRotationTruck = [startPose(3) 0 0];

Open the top-level Simulink model.

model = 'IntegratedBehaviorModule';
open_system(model)

Warning: The System object 'exampleHelperPathVisualizerSys' specified in the MATLAB System block 'IntegratedBehaviorModule/Visualization and Server Functions/Global Path Viz' has a property 'VisualizationParameters' defined as tunable. It was set as a non-tunable parameter in the block to work in Simulink. Simulink does not allow parameters whose values are booleans, literal character vectors, string objects or non-numeric structures to be tunable.

The model has four primary components:

Operating Mode Logic — This component is our scheduler. It controls which planning algorithms are active, based on user input and the current location of the vehicle.
Global Planning — This component includes the algorithms where the vehicle determines an overall path to get from the startPose to the goalPose. It includes the route planner developed in the Create Route Planner for Offroad Navigation Using Digital Elevation Data example, and the terrain-aware planners developed in the Create Onramp and Terrain-Aware Global Planners for Offroad Navigation example. The route planner plans along the road network and the terrain-aware planners plan in free space outside the road network.
Local Planning and Following — This component includes the algorithms that help the vehicle navigate along the global paths planned in the previous component. It includes the local planner developed in the Navigate Global Path Through Offroad Terrain Using Local Planner example, and the MPC-based path follower developed in the Create Path Following Model Predictive Controller example.
Platform and Environment Model — This component includes the physics model for the truck, as well as information about the environment. For fast low-fidelity simulations, this component uses the bicycle kinematics model. For higher fidelity simulations, this component uses Unreal Engine®. You can specify the simulation type in the Specify Simulation Options section.

Create a figure for visualizing the map, planned paths, and the vehicle progress throughout the simulation. Note that the simulation updates the figure as the simulation runs.

f = figure; 
f.Visible = "on";
binMap = binaryOccupancyMap(~imSlope);
title("Haul Truck Simulation")

Specify Simulation Options

Before running the simulation, we can configure additional options specific to the upcoming run. The exampleHelperCreateBehaviorModelInputs helper function sets default values for these parameters, but you can set them using the UI controls in this section.

Use the vssPhysModel drop-down to specify the platform and environment model to use for the truck.

Simple — Use the Bicycle Kinematic Model (Robotics System Toolbox) block to represent the mining truck. Visualize the motion of the truck using the figure created in the previous section.
Unreal — Use the Simulation 3D Physics Dump Truck (Robotics System Toolbox) block to represent the mining truck. Visualize the motion of the truck in Unreal Engine® in addition to the figure created in the previous section.

vssPhysModel = "Simple";

% If using Unreal, ensure access to the open pit mine scene. 
if vssPhysModel == "Unreal"
    sceneName = "Offroad pit mining scene";
    mapFile = fullfile(userpath,'sim3d_project',matlabRelease.Release,'WindowsNoEditor\AutoVrtlEnv\Content\Paks\Maps.xlsx');
    if ~exist(mapFile,"file") || all(~contains(string(readtable(mapFile).MapName),sceneName))
        sim3d.maps.Map.download(sceneName);
    end
    plantVariant = '/PlatformAndEnv/Platform Model/Unreal (3D)/Simulation 3D Scene Configuration';
    set_param(model,"SimulationCommand","stop");
    set_param([model plantVariant],'ProjectFormat','Default Scenes');
    set_param([model plantVariant],"SceneDesc","Offroad pit mining scene");
end

Use the drop-down below to select the communication protocol for the simulation.

Simulink Signals — Individual components of the navigation stack communicate with each other through the top-level Simulink model.
An alternative to this option would be to have the individual components of the navigation stack communicate through middleware, such as ROS.

vssCommProtocol = "SLSignals";

Use the sliders below to change any of the vehicle's local planner parameters.

% Vehicle minimum turning radius (m)
tuneableTEBParamsCpp.MinTurningRadius = 17.2;

% Maximum-allowed linear velocity (m/s)
tuneableTEBParamsCpp.MaxVelocity(1) =16;

% Maximum-allowed angular velocity (rad/s)
tuneableTEBParamsCpp.MaxVelocity(2) =0.93;

% Maximum-allowed linear reverse velocity (m/s)
tuneableTEBParamsCpp.MaxReverseVelocity =8;

% Maximum-allowed linear acceleration (m/s^2)
tuneableTEBParamsCpp.MaxAcceleration(1) =0.6;

% Maximum-allowed angular acceleration (rad/s^2)
tuneableTEBParamsCpp.MaxAcceleration(2) =0.1;

% Safety distance between vehicle and obstacles (m)
tuneableTEBParamsCpp.ObstacleSafetyMargin = 1;

% Time steps that the controller generates velocity commands for (s)
tuneableTEBParamsCpp.LookaheadTime = 6;

Use these sliders to change the cost function optimization weights of the local planner.

% Time Weight — Higher values result in shorter travel time
tuneableTEBParamsCpp.CostWeights_T.Time = 100;

% Smoothness Weight — Higher values result in smoother paths
tuneableTEBParamsCpp.CostWeights_T.Smoothness = 500;

% Obstacle Weight — Higher values result in prioritizing obstacle avoidance
% over travel time and path smoothness
tuneableTEBParamsCpp.CostWeights_T.Obstacle =10;

Use the drop-down below to choose a map visualization for the simulation.

Signed Distance Field — Map shows distances to obstacles. Points on the map will return positive values if they lie outside an occupied region of space. Points on the map will return negative values if they lie inside an occupied region of space. For more information about singed distance maps, see the signedDistanceMap3D object.
Binary Occupancy Map — Map shows occupied status of regions. Black cells are occupied, and white cells are unoccupied. For more information about binary occupancy maps, see the binaryOccupancyMap object.
Occupancy Map — Map shows the probability that a region is occupied. Values close to 1 (black) represent a high probability that the cell contains an obstacle. Values close to 0 (white) represent a high probability that the cell is not occupied and obstacle free. For more information about binary occupancy maps, see the occupancyMap object.

maptype = 'Bin';
set_param([model '/Visualization and Server Functions/Map Visualization'],"MapType",maptype);

You can also change parameters like plotting colors, widths, and styles on the system object blocks in the Visualization and Server Functions subsystem. By default, the map uses a binary occupancy map. The road network is plotted in dark blue, the reference path is plotted in orange, the local planner output is plotted in light blue, and the MPC path follower output is plotted in pink.

Simulate the Autonomous Vehicle

Run the model by selecting Run on the Simulation tab of the Simulink toolstrip.

After starting the model, the haul truck stays in the WaitingForTask state. Select Begin Navigation in the model to begin.

You can also run the simulation programmatically by running the code below.

% Start the simulation. 
sm = simulation(model);
start(sm);

Warning: The System object 'exampleHelperPathVisualizerSys' specified in the MATLAB System block 'IntegratedBehaviorModule/Visualization and Server Functions/Global Path Viz' has a property 'VisualizationParameters' defined as tunable. It was set as a non-tunable parameter in the block to work in Simulink. Simulink does not allow parameters whose values are booleans, literal character vectors, string objects or non-numeric structures to be tunable.

% Update the goal location to start navigating.
goalBlockPath = [model '/TaskControllerIn/Simulink Signals/Confirm New Goal/newGoalBlock'];
sm = setBlockParameter(sm,goalBlockPath,"Value","goalPose");
pause(1);

% If the goal has been reached, end the simulation. 
pause(sm);
while get(sm.SimulationOutput.logsout,"TaskControllerMode").Values.Data(end) ~= "WaitingForTask"
    t = sm.Time;
    step(sm,"PauseTime",t+10); % Checks currently active state every 10 seconds of simulation time.
end

stop(sm);

Results

This example, you learned how to integrate different planning subsystems together to create a navigation stack for an offroad autonomous vehicle. The individual algorithms of the navigation stack were developed and tested incrementally in MATLAB® code, then brought into Simulink for system-level simulation.

For further information on the states in the Stateflow chart and which parts of the model they trigger, see the following table. The table also shows which of the previous examples align with each operational state.

Operational State	Subsystem	Logic/Description	Modules/Functionality	Source Example
`WaitingForTask`	N/A	Waits for the user to provide a `goal` location. This is the initial state of the model when first run, and the agent returns to this state once the vehicle has reached the current `goal`. Setting a new `goal` will trigger the `RoutePlanner.`	N/A	N/A
`RoutePlanner`	`GlobalPlanner/ RoutePlanner`	Whenever a new `goal` has been assigned, the agent attempts to plan a route using the RoutePlanner. This subsystem produces a reference path that will be followed by the `LocalPlanner` and `Controller` subsystems.	Constructs the `navGraph`-based `plannerAStar` object. The planner then finds the nearest nodes to the current pose (`curPose`) and goal location (`goal`), and searches for the shortest path between the two. Once found, the `RoutePlanner` uses the simple `OnrampPlanner` to connect the `curPose` to the start node, and converts the sequence of edges to a dense SE2 path.	Create Route Planner for Offroad Navigation Using Digital Elevation Data
`GlobalPlanner`	`GlobalPlanner/` `TerrainAwarePlanner`	In this example, the `TerrainPlanner` serves two primary purposes: 1) As a backup planner in the event that the `LocalPlanner` fails to generate a time-optimal trajectory while following the reference-path. 2) As a planner used to transition the vehicle from the end of the `RoutePlanner` reference-path to the `goal` Similar to the `RoutePlanner`, the `TerrainPlanner` produces a reference-path, before handing control back to the `LocalPlanner`.	The `TerrainPlanner` subsystem contains the terrain-aware `plannerHybridAStar` module. Similar to the `LocalPlanner`, this module takes a set of tunable parameters as input, and exposes a set of fixed mask-parameters on the `TerrainAwarePlannerBlock`.	Create Terrain-Aware Global Planners for Offroad Navigation
`LocalPlanner`	`PathFollower/ LocalPlanner`	This subsystem takes in the reference path produced by either the `RoutePlanner` or `GlobalPlanner` subsystems and generates a sequence of control commands which follow the reference path while avoiding local obstacles. This command sequence is fowarded to the `Controller`, and iteratively switches between local path generation and path following until the goal has been reached or a global replan is needed.	This subsystem contains two modules: 1) A block responsible for extracting a local map from the global map 2) The local optimization-based `controllerTEB`. This subsystem takes `tuneableTEBParams` as input, and contains additional fixed mask-parameters on the `tebControllerBlock`. The agent forwards time-stamped control sequences and optimized path to the `FollowingPath` subsystem.	Navigate Global Path through Offroad Terrain Using Local Planner
`FollowingPath`	`PathFollower/ Controller`	The `Controller` subsystem is tasked with producing a velocity command given the local path and current timestep. The controller is also responsible for determining whether a new local plan should be requested, and checks whether the vehicle has either reached the end of the reference-path, or reached the goal. If the agent has reached the end of the reference path, which corresponds to the `goal`, then the agent returns to the `WaitingForTask` mode, otherwise the controller requests a global plan, which should produce a new reference path between the current location and the goal state from the `GlobalPlanner`.	This subsystem serves as the second stage in a cascade controller. The first layer feeds the global reference path to controllerTEB, which generates a locally optimal path along and control sequence for the vehicle. Those outputs are then fed to a variant subsystem, which offers two control modes: 1) `TEBIndexer`: simply indexes into the optimal control sequence based on the elapsed simulation time 2) `TEBMPCCascade`: uses an MPC-based controller to generate vehicle commands for the current timestep. This controller uses an incoming SE2 path to generate a set of reference states for the MPC optimizer. For performance reasons, this controller only takes vehicle kinematics/constraints into account, and assumes that the provided reference path is collision free.	1) Navigate Global Path through Offroad Terrain Using Local Planner 2) Create Path Following MPC Controller

Model and Control Autonomous Vehicle in Offroad Scenario

Create Autonomous Planning Stack

Specify Simulation Options

Simulate the Autonomous Vehicle

Results

Related Topics