Main Content

# plan

Plan optimal trajectory

## Syntax

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

## Description

example

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

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(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')```

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

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

• d2s/dt2 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.

• d2l/ds2 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, 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 of feasible trajectory with minimum cost, returned as a positive integer scalar.

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

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.

## Version History

Introduced in R2019b