Main Content

offroadControllerMPPI

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

Since R2024b

expand all in page

Description

The offroadControllerMPPI System object™ enables you to plan a local path for offroad vehicles and heavy machinery following a reference path, typically generated by a global path planner such as RRT* or Hybrid A*, using the model predictive path integral (MPPI) algorithm. At each state of the vehicle along the reference path, the MPPI controller samples potential local trajectories for a short segment along the reference path, based on the current pose and velocity of the vehicle. 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,Name=Value)

Description

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

mppi = offroadControllerMPPI(refPath,Name=Value) configures the offroad MPPI controller using one or more optional name-value arguments.

example

Properties

expand all

Vehicle Parameters

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

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 an 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 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 unknown obstacles, but enables the controller to run at a faster rate.

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

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.

Larger number of trajectories can lead to a more thorough search of the trajectory space and potentially a better solution, at the cost of increased computations.

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

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

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

Standard deviation for vehicle inputs, specified as a 2-element numeric vector with nonnegative values. The default is [2 0.05] if the vehicle model is an ackermannKinematics object, and [2 0.5] for other vehicle models.

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

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

This property is read-only.

Number of states in each sampled trajectory, returned 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. Higher number of states offer a more detailed trajectory by capturing more precise changes in the vehicle's state, at the cost of increased computations.

Data Types: double

Path Following

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.

You must set this property using the refPath input argument at object creation.

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 x and y directions, respectively. Units are in meters. θ is the heading angle of the robot in radians. This goal tolerance value specifies the limit for determining whether the robot has reached the goal pose.

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

Occupancy map 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 occupancyMap or signedDistanceMap objects.

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

Optimization Parameters

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

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

    FieldDescription
    CostWeightWeights 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.

    • PathAlignment — 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.

    • PathFollowing — Weight for aligning with the lookahead poses of the reference path, specified as a nonnegative scalar. Default value is 1.

    VehicleCollisionInformationInformation about the dimension and shape of the vehicle for evaluating collision status, specified as a structure with these fields.

    • Dimension — Dimension of the vehicle specified as a 2-element numeric vector. If the shape of the vehicle is a point, the controller ignores the dimension and the value of this field is set 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 with the default value as [1 1].

    • Shape — Shape of the vehicle, specified as "Point" or "Rectangle". If the shape of the vehicle is a rectangle, the controller the vehicle's origin as the center of its rear edge.

    ObstacleSafetyMarginSafety margin that the vehicle must maintain from obstacles in meters, specified as a nonnegative scalar. The default value is 0.5.

Data Types: struct

Selectiveness of optimal trajectory, specified as a positive scalar.

Specify a value close to zero 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 in the MPPI algorithm adjusts sensitivity to cost differences, acting as a scaling factor that influences the weighting of candidate trajectories.

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 the vehicle.

Input Arguments

expand all

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:

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 vehicle steering angle of the vehicle in radians.

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

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

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

For other motion models, the VehicleInputs property of VehicleModel property 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

expand all

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 each 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 VehicleInputs property of VehicleModel property 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

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:

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 vehicle steering angle of the vehicle in radians. Each row of the matrix corresponds to the optimal state of the vehicle for each state along the trajectory.

Data Types: double

Extra information, returned as a structure with these fields.

FieldDescription
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. 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.

ControlSequences

Control commands corresponding to each sampled trajectory, returned as an N-by-2-by-T matrix depending on the vehicle model. 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.

LookaheadPosesReference 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 the each of the L lookahead poses in the form [x y θ ].
HasReachedGoalIndicates whether the vehicle has successfully reached the last pose of the reference path within the goal tolerance, and returns as true if successful. Otherwise, the value returns false.
ExitFlagScalar value indicating the exit condition.

  • 0 — If the control sequences and trajectories of the output are feasible.

  • 1 — If the control sequences and trajectories of the output are not feasible due to constraint violations.

  • 2 — If the next reference path pose is farther than what the vehicle can cover in the specified lookahead time, while travelling 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)

expand all

stepRun System object algorithm
releaseRelease resources and allow changes to System object property values and input characteristics
resetReset internal states of System object

Examples

collapse all

Open Live Script

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

Figure contains an axes object. The axes object with title Global Reference Path, xlabel X [meters], ylabel Y [meters] contains 4 objects of type image, line. One or more of the lines displays its values using only markers These objects represent Start, Goal, Path.

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 re-plan 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

Figure contains an axes object. The axes object with title MPPI Planning Local Paths on Global Ref. Path, xlabel X [meters], ylabel Y [meters] contains 304 objects of type image, line, quiver. One or more of the lines displays its values using only markers These objects represent Start, Goal, Path, Current Robot Pose.

Extended Capabilities

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

Version History

Introduced in R2024b

See Also

Topics