show
Visualize trajectories
Description
show(
visualizes the reference path
and trajectory from the candidates generated by the planner
)plan
function. The
trajectory is shown as a line plot. The plot also includes datatip mode, which can be used
to visualize the feasibility vector and index of the trajectory from the TrajectoryList
property.
show(
specifies additional options using one or more planner
,Name,Value
)Name,Value
pair
arguments.
Examples
Optimal Trajectory Planning in Frenet Space
This example shows how to plan an optimal trajectory using a trajectoryOptimalFrenet
object.
Create and Assign Map to State Validator
Create a state validator object for collision checking.
stateValidator = validatorOccupancyMap;
Create an obstacle grid map.
grid = zeros(50,100); grid(24:26,48:53) = 1;
Create a binaryOccupancyMap
with the grid map.
map = binaryOccupancyMap(grid);
Assign the map and the state bounds to the state validator.
stateValidator.Map = map; stateValidator.StateSpace.StateBounds(1:2,:) = [map.XWorldLimits; map.YWorldLimits];
Plan and Visualize Trajectory
Create a reference path for the planner to follow.
refPath = [0,25;100,25];
Initialize the planner object with the reference path, and the state validator.
planner = trajectoryOptimalFrenet(refPath,stateValidator);
Assign longitudinal terminal state, lateral deviation, and maximum acceleration values.
planner.TerminalStates.Longitudinal = 100; planner.TerminalStates.Lateral = -10:5:10; planner.FeasibilityParameters.MaxAcceleration = 10;
Specify the deviation offset value close to the left lateral terminal state to prioritize left lane changes.
planner.DeviationOffset = 5;
Trajectory Planning
Initial cartesian state of vehicle.
initCartState = [0 25 pi/9 0 0 0];
Convert cartesian state of vehicle to Frenet state.
initFrenetState = cart2frenet(planner,initCartState);
Plan a trajectory from initial Frenet state.
plan(planner,initFrenetState);
Trajectory Visualization
Visualize the map and the trajectories.
show(map) hold on show(planner,'Trajectory','all')
Partitioning Longitudinal Terminal States in Trajectory Generation
This example shows how to partition the longitudinal terminal states in optimal trajectory planning using a trajectoryOptimalFrenet
object.
Create and Assign Map to State Validator
Create a state validator object for collision checking.
stateValidator = validatorOccupancyMap;
Create an obstacle grid map.
grid = zeros(50,100); grid(25:27,28:33) = 1; grid(16:18,37:42) = 1; grid(29:31,72:77) = 1;
Create a binaryOccupancyMap
with the grid map.
map = binaryOccupancyMap(grid);
Assign the map and the state bounds to the state validator.
stateValidator.Map = map; stateValidator.StateSpace.StateBounds(1:2,:) = [map.XWorldLimits; map.YWorldLimits];
Plan and Visualize Trajectory
Create a reference path for the planner to follow.
refPath = [0,25;30,30;75,20;100,25];
Initialize the planner object with the reference path, and the state validator.
planner = trajectoryOptimalFrenet(refPath,stateValidator);
Assign longitudinal terminal state, lateral deviation, and maximum acceleration values.
planner.TerminalStates.Longitudinal = 100; planner.TerminalStates.Lateral = -5:5:5; planner.FeasibilityParameters.MaxAcceleration = 10;
Assign the number of partitions for the longitudinal terminal state.
planner.NumSegments = 3;
Trajectory Planning
Initial Frenet state of vehicle.
initFrenetState = zeros(1,6);
Plan a trajectory from initial Frenet state.
plan(planner,initFrenetState);
Trajectory Visualization
Visualize the map and the trajectories.
show(map) hold on show(planner,'Trajectory','all') hold on
Generate Lane Boundaries
Calculate end of reference path as Frenet state.
refPathEnd = cart2frenet(planner,[planner.Waypoints(end,:) 0 0 0 0]);
Calculate lane offsets on both sides of the lateral terminal states with half lane width value.
laneOffsets = unique([planner.TerminalStates.Lateral+2.5 planner.TerminalStates.Lateral-2.5]);
Calculate positions of lanes in Cartesian state.
numLaneOffsets = numel(laneOffsets); xRefPathEnd = ceil(refPathEnd(1)); laneXY = zeros((numLaneOffsets*xRefPathEnd)+numLaneOffsets,2); xIndex = 0; for laneID = 1:numLaneOffsets for x = 1:xRefPathEnd laneCart = frenet2cart(planner,[x 0 0 laneOffsets(laneID) 0 0]); xIndex = xIndex + 1; laneXY(xIndex,:) = laneCart(1:2); end xIndex = xIndex + 1; laneXY(xIndex,:) = NaN(1,2); end
Plot lane boundaries.
plot(laneXY(:,1),laneXY(:,2),'LineWidth',0.5,'Color',[0.5 0.5 0.5],'DisplayName','Lane Boundaries','LineStyle','--')
Input Arguments
planner
— Optimal trajectory planner in Frenet space
trajectoryOptimalFrenet
object
Optimal trajectory planner in Frenet space, specified as a trajectoryOptimalFrenet
object.
Name-Value Arguments
Specify optional pairs of arguments as
Name1=Value1,...,NameN=ValueN
, where Name
is
the argument name and Value
is the corresponding value.
Name-value arguments must appear after other arguments, but the order of the
pairs does not matter.
Before R2021a, use commas to separate each name and value, and enclose
Name
in quotes.
Example: 'Trajectory','all'
Trajectory
— Trajectory display option
'optimal'
(default) | 'all'
Trajectory display option, specified as the comma-separated pair consisting of
'Trajectory'
and either 'optimal'
or
'all'
.
ReferencePath
— Reference path display option
'on'
(default) | 'off'
Reference path display option, specified as the comma-separated pair consisting of
'ReferencePath'
and either 'on'
or
'off'
.
TrajectoryColor
— Trajectory color display option
'velocity'
(default) | 'acceleration'
| 'cost'
| 'none'
Trajectory color display option, specified as the comma-separated pair consisting
of 'TrajectoryColor'
and one of the following:
'acceleration'
'cost'
'velocity'
'none'
Set this property to display the specified trajectory as a color-gradient along the specified path.
Output Arguments
Version History
Introduced in R2019b
See Also
MATLAB Command
You clicked a link that corresponds to this MATLAB command:
Run the command by entering it in the MATLAB Command Window. Web browsers do not support MATLAB commands.
Select a Web Site
Choose a web site to get translated content where available and see local events and offers. Based on your location, we recommend that you select: .
You can also select a web site from the following list:
How to Get Best Site Performance
Select the China site (in Chinese or English) for best site performance. Other MathWorks country sites are not optimized for visits from your location.
Americas
- América Latina (Español)
- Canada (English)
- United States (English)
Europe
- Belgium (English)
- Denmark (English)
- Deutschland (Deutsch)
- España (Español)
- Finland (English)
- France (Français)
- Ireland (English)
- Italia (Italiano)
- Luxembourg (English)
- Netherlands (English)
- Norway (English)
- Österreich (Deutsch)
- Portugal (English)
- Sweden (English)
- Switzerland
- United Kingdom (English)