Plan optimal trajectory



[traj,index,cost] = 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 6-element vector [s, ds, dds, l, dl, ddl], 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. To improve the results of the planning output, modify the parameters on the planner object.


collapse all

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(700,700);
grid(296:305,150:175) = 1;
grid(286:295,300:325) = 1;
grid(306:315,300:325) = 1;
grid(296:305,450:475) = 1;
grid(286:295,600:625) = 1;
grid(306:315,600:625) = 1;

Create a binaryOccupancyMap with the grid map.

map = binaryOccupancyMap(grid);

Assign the map to the state validator.

stateValidator.Map = map;

Plan and Visualize Trajectory

Create a reference path for the planner to follow.

refPath = [10,400;700,400];

Declare the cost function handle to prioritize left lane changes.

leftLaneChangeCost = @(states)((states(end,2) < refPath(end,2))*10);

Initialize the planner object with the reference path, the state validator, and the custom cost function.

plannerObj = trajectoryOptimalFrenet(refPath,stateValidator,'CostFunction',leftLaneChangeCost);

Assign lateral deviation values.

plannerObj.TerminalStates.Lateral = -10:10:10;

Trajectory Generation

Initialize the variables for the replanning loop.

previousDeviationValue = 0;
deviationValue = 0;
initState = zeros(1,6);

Replan until final waypoint is closest to the terminal state.

for j = 1:150
    % Deviation from the current reference path
    if initState(4) > 5
        deviationValue = 10;
    elseif initState(4) < -5
        deviationValue = -10;
    % Move reference path to the current lane
    if previousDeviationValue ~= deviationValue
        % Shift the waypoints by the deviation value
        plannerObj.Waypoints = [plannerObj.Waypoints(:,1), plannerObj.Waypoints(:,2) + deviationValue];
        % Shift the terminal states such that they remain fixed in world frame
        plannerObj.TerminalStates.Lateral = plannerObj.TerminalStates.Lateral - deviationValue;
        % Store the deviation value
        previousDeviationValue = deviationValue;
        % Update initState variable with the new planner object
        initState = cart2frenet(plannerObj,trajectory(10,1:6));
    % Generate a trajectory from initState
    trajectory = plan(plannerObj,initState);
    % Use 10th state of the current trajectory as a starting point for
    % replanning
    initState = cart2frenet(plannerObj,trajectory(10,1:6));
    % Visualize every 5th iteration
    if mod(j,5)==0
        hold on;

Input Arguments

collapse all

Optimal trajectory planner in frenet space, specified as a trajectoryOptimalFrenet object.

Initial Frenet state, specified as a 1-by-6 vector, [s, ds, dds, l, dl, ddl]. s specifies the arc length from the first point in reference path in meters, ds specifies the first derivative of arc length, dds specifies the second derivative of arc length, l specifies the normal distance from the closest point in the reference path, dl specifies the first derivative of normal distance, and ddl specifies the second derivative of normal distance.

Output Arguments

collapse all

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

Index of feasible trajectory with least cost, retuned as a positive integer scalar.

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

Extended Capabilities

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

Introduced in R2019b