Documentation

# plan

Plan optimal trajectory

## Syntax

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

## Description

example

````[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.```

## Examples

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; end % 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)); end % 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 show(map) hold on; show(plannerObj,'TrajectoryColor','none'); drawnow end end```

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