plan

Plan optimal trajectory

Syntax

[traj,index,cost,flag] = plan(planner,start)

Description

[traj,index,cost,flag] = plan(planner,start) computes a feasible trajectory, traj, from a list of candidate trajectories generated from the trajectoryOptimalFrenet object, planner. start is specified as a six-element vector [s, ds/dt, d2s/dt2, l, dl/ds, d2l/ds2], where s is the arc length from the first point in the reference path, and l is normal distance from the closest point at s on the reference path.

The output trajectory, traj, also has an associated cost and index for the TrajectoryList property of the planner. flag is a numeric exit flag indicating status of the solution.

To improve the results of the planning output, modify the parameters on the planner object.

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.

start — Initial Frenet state

six-element vector

Initial Frenet state, specified as a 1-by-6 vector [s, ds/dt, d2s/dt2, l, dl/ds, d2l/ds2].

• s specifies the arc length from the first point in reference path in meters.

• ds/dt specifies the first derivative of arc length.

• d²s/dt² specifies the second derivative of arc length.

• l specifies the normal distance from the closest point in the reference path in meters.

• dl/ds specifies the first derivative of normal distance.

• d²l/ds² specifies the second derivative of normal distance.

Output Arguments

traj — Feasible trajectory with minimum cost

n-by-7 matrix

Feasible trajectory with minimum cost, returned as an n-by-7 matrix of [x, y, theta, kappa, speed, acceleration, time], where n is the number of trajectory waypoints.

• x and y specify the position in meters.

• theta specifies the orientation angle in radians.

• kappa specifies the curvature in m-1.

• speed specifies the velocity in m/s.

• acceleration specifies the acceleration in m/s2.

• time specifies the time in s.

index — Index of feasible trajectory with minimum cost

positive integer scalar

Index of feasible trajectory with minimum cost, returned as a positive integer scalar.

cost — Least cost of feasible trajectory

positive scalar

Least cost of feasible trajectory, returned as a positive scalar.

flag — Exit flag indicating solution status

0 | 1

Exit flag indicating the solution status, returned either as 0 or 1.

• 0 — Optimal trajectory was found.

• 1 — No feasible trajectory exists.

When no feasible trajectory exists, the planner returns an empty trajectory.

Extended Capabilities

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

Version History

Introduced in R2019b