offroadControllerMPPI

Local path planner for offroad vehicles and heavy machinery using MPPI algorithm

Since R2024b

Description

The offroadControllerMPPI System object™ enables you to plan a local path for offroad vehicles and heavy machinery following a reference path using the model predictive path integral (MPPI) algorithm. The step function samples multiple sets of control inputs over a short time horizon and propagates the initial state of the vehicle to generate potential trajectories. The controller evaluates these trajectories using a cost function to avoid obstacles and smooth the path, and chooses an optimal local trajectory and optimal velocity for the vehicle.

To plan a local path using the MPPI algorithm:

1. Create the offroadControllerMPPI object and set its properties.

2. Call the object with arguments, as if it were a function.

To learn more about how System objects work, see What Are System Objects?

Note

This System object requires the Robotics System Toolbox™ Offroad Autonomy Library support package. For more information about how to download this support package, see Install Robotics System Toolbox Offroad Autonomy Library Support Package.

Creation

Syntax

mppi = offroadControllerMPPI(refPath)

mppi = offroadControllerMPPI(refPath,PropertyName=Value)

Description

mppi = offroadControllerMPPI(refPath) creates an offroad MPPI controller System object for local path planning of a vehicle along the global reference path refPath.

mppi = offroadControllerMPPI(refPath,PropertyName=Value) specifies properties of the offroad MPPI controller using one or more optional name-value arguments.

Input Arguments

refPath — Reference path to follow

N-by-2 matrix | N-by-3 matrix | navPath object with SE(2) state space

Reference path to follow, specified as an N-by-2 matrix, N-by-3 matrix, or navPath (Navigation Toolbox) object with an SE(2) state space. When specified as a matrix, each row represents a pose on the path. Use the LookAheadTime property to select a part of the ReferencePath for which to optimize the trajectory and generate velocity commands.

This argument sets the ReferencePath property.

Note

If you specify the reference path as an N-by-2 matrix, then the controller computes the heading angle from the x and y coordinates.

Properties

Vehicle Parameters

VehicleModel — Kinematic model of vehicle for state propagation

Read-only: bicycleKinematics object (default) | ackermannKinematics object | differentialDriveKinematics object | unicycleKinematics object

This property is read-only.

Kinematic model of the vehicle for state propagation, specified as a bicycleKinematics object, ackermannKinematics object, differentialDriveKinematics object, or unicycleKinematics object. The default vehicle model is a bicycleKinematics object with default properties. The vehicle model determines the trajectory states, control limits, and the size and type of control inputs the vehicle can accept.

To set this property, you must specify the VehicleModel name-value argument at object creation.

Trajectory Generation Parameters

LookaheadTime — Lookahead time for trajectory planning in seconds

4 (default) | positive scalar

Lookahead time for trajectory planning in seconds, specified as a positive scalar.

The vehicle model determines the maximum forward velocity of the vehicle. The lookahead time of the controller multiplied by the maximum forward velocity of the vehicle determines the lookahead distance of the controller. If you do not specify parameters to set the maximum velocity of the vehicle model, the controller uses a maximum forward velocity of 5 m/s and a maximum reverse velocity of 0.1 m/s to determine the lookahead distance.

The lookahead distance of the controller is the distance from the current pose of the vehicle for which the controller must optimize the local path. The lookahead distance determines the segment of the global reference path from the current pose that the controller considers for each iteration of local path planning. The reference path pose at a lookahead distance from the current pose is the lookahead point, and all reference path poses between the current pose and the lookahead point are lookahead poses.

Increasing the value of the lookahead time causes the controller to generate controls further into the future, enabling the vehicle to react earlier to unseen obstacles at the cost of increased computation time. Conversely, a smaller lookahead time reduces the available time to react to new obstacles, but enables the controller to run at a faster rate.

Data Types: single | double | int8 | int16 | int32 | int64 | uint8 | uint16 | uint32 | uint64

NumTrajectory — Number of sampled trajectories

500 (default) | positive integer

Number of sampled trajectories, specified as a positive integer.

The controller calculates the optimal trajectory by averaging the controls of candidate trajectories, with weights determined by their associated costs.

A larger number of trajectories can lead to a more thorough search of the trajectory space and a potentially better solution, at the cost of increased computation time.

Data Types: single | double | int8 | int16 | int32 | int64 | uint8 | uint16 | uint32 | uint64

SampleTime — Time between consecutive states of trajectory in seconds

0.1 (default) | positive scalar

Time between consecutive states of the trajectory in seconds, specified as a positive scalar. Smaller values lead to more precise trajectory predictions, but increase computation time.

Data Types: single | double | int8 | int16 | int32 | int64 | uint8 | uint16 | uint32 | uint64

StandardDeviation — Standard deviation for vehicle inputs

two-element numeric vector

Standard deviation for vehicle inputs, specified as a two-element numeric vector with nonnegative values. If the vehicle model is an ackermannKinematics object, the default value of this property is [2 0.05]. For all other vehicle models, the default value is [2 0.5].

Each element of the vector specifies the standard deviation for a control input of the vehicle model, such as steering or velocity. Each model has two control inputs. By introducing such variability into the control inputs during trajectory sampling, the controller can better explore different possible trajectories. Larger standard deviations enable the controller to account for uncertainties in the control input and achieve more realistic trajectory predictions.

Data Types: single | double | int8 | int16 | int32 | int64 | uint8 | uint16 | uint32 | uint64

NumState — Number of states in each sampled trajectory

Read-only: positive integer

This property is read-only.

Number of states in each sampled trajectory, stored as a positive integer.

This property sets the temporal resolution of the trajectory. The value of this property depends on the lookahead time and sample time of the controller. A higher number of states creates a more detailed trajectory by capturing more precise changes in the state of the vehicle, at the cost of increased computation time.

Data Types: double

Path Following

ReferencePath — Reference path to follow

N-by-2 matrix | N-by-3 matrix | navPath object with SE(2) state space

This property is read-only after object creation.

Reference path to follow, stored as an N-by-2 matrix, N-by-3 matrix, or navPath (Navigation Toolbox) object with an SE(2) state space. Use the LookAheadTime property to select a part of the ReferencePath for which to optimize the trajectory and generate velocity commands.

You can use global planners like plannerRRTStar (Navigation Toolbox) or plannerHybridAStar (Navigation Toolbox) to generate reference paths.

Use the refPath input argument to set the property at object creation.

GoalTolerance — Tolerance around goal pose

[1 1 0.1] (default) | three-element vector

Tolerance around the goal pose, specified as a three-element vector of the form [x y θ]. x and y denote the position of the robot in the x- and y-directions, respectively, units are in meters. θ is the heading angle of the robot in radians. This value specifies the maximum distance from the goal pose at which the planner can consider the robot to have reached the goal pose.

Data Types: single | double | int8 | int16 | int32 | int64 | uint8 | uint16 | uint32 | uint64

Map — Occupancy map representing environment

binaryOccupancyMap object (default) | occupancyMap object | signedDistanceMap object

Occupancy map representing the environment, specified as a binaryOccupancyMap object, occupancyMap (Navigation Toolbox) object, or signedDistanceMap (Navigation Toolbox) object. The default map is a binaryOccupancyMap object with default properties.

The controller considers the space outside the boundary of the map as free while optimizing the trajectory. The controller evaluates the collision status of each sampled trajectory with reference to the occupancy map. It discards any trajectory for which the minimum distance to the closest obstacle is less than the obstacle safety margin defined in the optimization parameters.

Note

• For faster performance, use an occupancyMap or signedDistanceMap object.

• Larger maps can lead to slower performance. Consider using a local map to represent the environment around the vehicle.

Optimization Parameters

Parameter — Options for cost function and optimization

structure

Options for cost function and optimization, specified as a structure with this field.

• Parameter — Optimization parameters, specified as a structure with these fields.

  Field	Description
  CostWeight	Weights for different components of the default cost function, specified as a structure with these fields. ObstacleRepulsion — Weight for obstacle avoidance, promoting a safe distance from obstacles, specified as a nonnegative scalar. Default value is 200. PathFollowing — Weight for closely following the planned path, specified as a nonnegative scalar. Default value is 1. ControlSmoothing — Weight for smooth control actions, avoiding abrupt changes, specified as a nonnegative scalar. Default value is 1. PathAlignment — Weight for aligning with the lookahead poses of the reference path, specified as a nonnegative scalar. Default value is 1.
  VehicleCollisionInformation	Information about the dimension and shape of the vehicle for evaluating collision status, specified as a structure with these fields. Dimension — Dimensions of the vehicle, specified as a two-element numeric vector. If the shape of the vehicle is a point, the controller ignores the dimensions, and sets the value of this field to [0 0]. If the shape of the vehicle is a rectangle, the elements of this vector must be positive, indicating the length and width of the rectangle. The default value for a rectangular vehicle is [1 1]. Shape — Shape of the vehicle, specified as "Point" or "Rectangle". If the shape of the vehicle is a rectangle, the controller sets the vehicle origin as the center of its rear edge.
  ObstacleSafetyMargin	Safety margin that the vehicle must maintain from obstacles, in meters, specified as a nonnegative scalar. The default value is 0.5.

Data Types: struct

TrajectorySelectionBias — Selectiveness of optimal trajectory

1000 (default) | positive scalar

Selectiveness of the optimal trajectory, specified as a positive scalar. This property is equivalent to the parameter λ in the MPPI algorithm and can be in the range (0, inf].

Specify a value close to 0 to favor the trajectory with the lowest cost as the optimal trajectory. Specify a larger value to favor the average of all sampled trajectories as the optimal trajectory.

The TrajectorySelectionBias property acts as a scaling factor that lets you adjust the sensitivity of the MPPI algorithm to cost differences among trajectories. By choosing a lower value, you can favor the most cost-effective trajectory. Conversely, higher values will allow the algorithm to consider a wider range of trajectory costs, giving you control over how much weight is placed on cost optimization versus averaging.

Data Types: single | double | int8 | int16 | int32 | int64 | uint8 | uint16 | uint32 | uint64

Usage

Syntax

[optControl,optTraj,info] = mppi(curState,curControl)

Description

[optControl,optTraj,info] = mppi(curState,curControl) computes the optimal velocity and trajectory for the current pose and velocity of a vehicle.

Input Arguments

curState — Current state of vehicle

three-element vector | four-element vector

Current state of the vehicle, specified as a three-element numeric vector or four-element vector depending on the vehicle model set in the VehicleModel property of mppi:

• unicycleKinematics — [x y θ]

• bicycleKinematics — [x y θ]

• differentialDriveKinematics — [x y θ]

• ackermannKinematics — [x y θ ψ]

x is the current x-coordinate in meters, y is the current y-coordinate in meters, θ is the current heading angle of the vehicle in radians, and ψ is the steering angle of the vehicle in radians.

Data Types: single | double | int8 | int16 | int32 | int64 | uint8 | uint16 | uint32 | uint64

curControl — Current control commands of vehicle

two-element vector

Current control commands of the vehicle, specified as a two-element numeric vector, where the form depends on the motion model of the VehicleModel property of mppi.

For ackermannKinematics objects, the control commands are [v psiDot].

For other motion models, the value of the VehicleInputs property of the object specified to the VehicleModel property of mppi determines the format of the control command:

• "VehicleSpeedSteeringAngle" — [v psiDot]

• "VehicleSpeedHeadingRate" — [v omegaDot]

• "WheelSpeedHeadingRate" (unicycleKinematics only) — [wheelSpeed omegaDot]

• "WheelSpeeds" (differentialDriveKinematics only) — [wheelL wheelR]

v is the vehicle velocity in the direction of motion in meters per second. psiDot is the steering angle rate in radians per second. omegaDot is the angular velocity at the rear axle in radians per second. wheelL and wheelR are the left and right wheel speeds in radians per second, respectively.

Data Types: single | double | int8 | int16 | int32 | int64 | uint8 | uint16 | uint32 | uint64

Output Arguments

optControl — Optimal control commands of vehicle

N-by-2 matrix

Optimal control commands of the vehicle, returned as an N-by-2 matrix, where N is the number of states along the trajectory. Each row of the matrix corresponds to the optimal velocity of the vehicle for a state along the trajectory. Each column depends on the motion model of the VehicleModel property:

For ackermannKinematics objects, the control commands are [v psiDot].

For other motion models, the value of the VehicleInputs property of the object specified to the VehicleModel property of mppi determines the format of the control command:

• "VehicleSpeedSteeringAngle" — [v psiDot]

• "VehicleSpeedHeadingRate" — [v omegaDot]

• "WheelSpeedHeadingRate" (unicycleKinematics only) — [wheelSpeed omegaDot]

• "WheelSpeeds" (differentialDriveKinematics only) — [wheelL wheelR]

v is the vehicle velocity in the direction of motion in meters per second. psiDot is the steering angle rate in radians per second. omegaDot is the angular velocity at the rear axle in radians per second. wheelL and wheelR are the left and right wheel speeds in radians per second, respectively.

Data Types: double

optTraj — Optimal trajectory of vehicle

N-by-3 matrix | N-by-4 matrix

Optimal trajectory of the vehicle, returned as an N-by-3 matrix or N-by-4 matrix, depending on the vehicle model set in the VehicleModel property:

• unicycleKinematics — N-by-3 matrix where each row is of the form [x y θ]

• bicycleKinematics — N-by-3 matrix where each row is of the form [x y θ]

• differentialDriveKinematics — N-by-3 matrix where each row is of the form [x y θ]

• ackermannKinematics — N-by-4 matrix where each row is of the form [x y θ ψ]

N is the number of states along the trajectory. x is the current x-coordinate in meters, y is the current y-coordinate in meters, θ is the current heading angle of the vehicle in radians, and ψ is the steering angle of the vehicle in radians. Each row of the matrix corresponds to the optimal state of the vehicle for a state along the trajectory.

Data Types: double

info — Extra information

structure

Extra information, returned as a structure with these fields.

Field	Description
Trajectories	All trajectories sampled at the current state, returned as an N-by-4-by-T matrix for the Ackermann vehicle model and N-by-3-by-T matrix for the unicycle, bicycle, and differential drive vehicle models. N is the number of states along the trajectory, and T is the number of sampled trajectories. The matrix returns the state of the vehicle at each of the N states for each of the T sampled trajectories in a form determined by the vehicle model. See the curState argument for more details about the form for each vehicle model.
ControlSequences	Control commands corresponding to each sampled trajectory, returned as an N-by-2-by-T matrix. N is the number of states along the trajectory and T is the number of sampled trajectories. The matrix returns the control commands of the vehicle at each of the N states for each of the T sampled trajectories in a form determined by the vehicle model. See the curControl argument for more details about the form for each vehicle model.
LookaheadPoses	Reference path poses considered for the specified lookahead time, returned as an L-by-3 matrix where L is the number of lookahead poses. The matrix returns each of the L lookahead poses in the form [x y θ].
HasReachedGoal	Indicates whether the vehicle has successfully reached the last pose of the reference path within the goal tolerance, returned as true if successful. Otherwise, this value is false.
ExitFlag	Scalar value indicating the exit condition. 0 — The control sequences and trajectories of the output are feasible. 1 — The control sequences and trajectories of the output are not feasible due to constraint violations. 2 — The next reference path pose is farther than what the vehicle can cover in the specified lookahead time while traveling at the maximum forward velocity of the vehicle model.

Data Types: struct

Object Functions

To use an object function, specify the System object as the first input argument. For example, to release system resources of a System object named obj, use this syntax:

release(obj)

Common to All System Objects

step	Run System object algorithm
release	Release resources and allow changes to System object property values and input characteristics
reset	Reset internal states of System object

Examples

Plan Local Path for Offroad Vehicle Using MPPI Algorithm

Load an occupancy map.

load("smallOffroadMap.mat")

Visualize the map.

map = occupancyMap(smallOffroadMap);
h1 = show(map);
ax1 = h1.Parent;
title("Offroad Occupancy Map")
hold on

Create a state validator for the occupancy map in the SE(2) state space with a validation distance.

ss = stateSpaceSE2([map.XWorldLimits; map.YLocalLimits; -pi pi]);
validator = validatorOccupancyMap(ss,Map=map);
validator.ValidationDistance = 1;

Set the start and goal poses.

startPose = [100 55 pi/2];
goalPose = [60 185 pi/2];

Create an RRT* path planner. Increase the maximum connection distance.

rng(7)
rrtstar = plannerRRTStar(validator.StateSpace,validator);
rrtstar.MaxConnectionDistance = 5;

Plan a global path from the start pose to the goal pose.

route = plan(rrtstar,startPose,goalPose);
refPath = route.States;

RRT* uses a random orientation, which can cause unnecessary turns. Align the orientation to the path, except for at the start and goal states.

headingToNextPose = headingFromXY(refPath(:,1:2));
refPath(2:end-1,3) = headingToNextPose(2:end-1);

Visualize the path.

plot(ax1,startPose(1),startPose(2),plannerLineSpec.start{:})
plot(ax1,goalPose(1),goalPose(2),plannerLineSpec.goal{:})
plot(ax1,refPath(:,1),refPath(:,2),plannerLineSpec.path{:})
title("Global Reference Path")
legend("Start","Goal")
hold off

Set Up and Run Local Planner

Specify the properties of the vehicle.

vehicleObj = bicycleKinematics;
vehicleObj.VehicleSpeedRange = [-5 15];
vehicleObj.MaxSteeringAngle = pi/4;

Initialize the local map.

maxForwardVelocity = vehicleObj.VehicleSpeedRange(2);
lookaheadTime = 4;
maxDistance = maxForwardVelocity*lookaheadTime;
localmap = occupancyMap(2*maxDistance,2*maxDistance,map.Resolution);

Initialize the MPPI controller system as the local planner.

mppi = offroadControllerMPPI(refPath,Map=localmap,VehicleModel=vehicleObj);
mppi.LookaheadTime = lookaheadTime;
mppi.Parameter.VehicleCollisionInformation = struct("Dimension",[2 2],"Shape","Rectangle");
mppi.Parameter.CostWeight.PathFollowing = 1.5;

Initialize the current pose and velocity of the vehicle.

curPose = refPath(1,:);
curVel = [0 0];
timestep = 0.1;
recoveryCount = 0; % For recovery behavior
iterationCount = 0;

Set up the visualization of the simulation.

figure
h2 = show(localmap);
ax2 = h2.Parent;
hold on
plot(ax2,startPose(1),startPose(2),plannerLineSpec.start{:})
plot(ax2,goalPose(1),goalPose(2),plannerLineSpec.goal{:})
plot(ax2,refPath(:,1),refPath(:,2),plannerLineSpec.path{:})
title("MPPI Planning Local Paths on Global Ref. Path")

Plan the local path along the reference path.

isGoalReached = false;
while ~isGoalReached && iterationCount<300
   % Sync local map with the global map
   moveMapBy = curPose(1:2) - localmap.XLocalLimits(end)/2;
   move(localmap,moveMapBy,FillValue=0.5);
   syncWith(localmap,map);

   % Local Planning: Generate new controls with MPPI
   [optVel,optTraj,info] = mppi(curPose,curVel);
   trajectories = info.Trajectories;
   lookaheadPoint = info.LookaheadPoses(end,:);
   isGoalReached = info.HasReachedGoal;

   if info.ExitFlag == 1
       % No valid solution found. Implement recovery behavior here.
       recoveryCount = recoveryCount + 1;
       if recoveryCount > 3
           disp("No valid path")
           break
       end
   elseif info.ExitFlag == 2
       % Vehicle far from reference path. May need to replan using global planner
       disp("Far from reference path")
       break
   end

   velCmd = optVel(1,:);
   curPose = curPose + derivative(vehicleObj,curPose,velCmd)'*timestep;
   curVel = velCmd;

   % Visualization
   show(localmap,Parent=ax2,FastUpdate=1);
   quiver(ax2,curPose(:,1),curPose(:,2),cos(curPose(:,3)),sin(curPose(:,3)),0.5, ...
       "o",MarkerSize=2,ShowArrowHead="off", ...
       Color="#104280",DisplayName="Current Robot Pose")
   drawnow limitrate

   iterationCount = iterationCount + 1;
end
hold off

Extended Capabilities

C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.

Version History

Introduced in R2024b