# trajectoryOptimalFrenet

Find optimal trajectory along reference path

## Description

The `trajectoryOptimalFrenet`

object is a path planner which
samples and evaluates local trajectories based on a reference path. The planner generates a
set of terminal states based on the reference path and other parameters in the object. The
planner then connects the state to each terminal state using fourth or fifth-order
polynomials. To choose an optimal path, sampled trajectories are evaluated for kinematic
feasibility, collision, and cost.

## Creation

### Description

`trajectoryOptimalFrenet(`

creates a `refPath`

,`validator`

)`trajectoryOptimalFrenet`

object with reference path,
`refPath`

, in the form of an *n*-by-2 array of
`[`

waypoints and a state validator,
*x*
*y*]`validator`

, specified as a `validatorOccupancyMap`

object.

sets additional properties using one or more name-value pairs in any order.`planner`

= trajectoryOptimalFrenet(___,`Name,Value`

)

### Input Arguments

`refPath`

— Reference path

*n*-by-2 matrix

Reference path, specified as an *n*-by-2 matrix of
`[`

pairs, where *x*
*y*]*n* is the number of
waypoints.

**Example: **[100,100;400,400]

**Data Types: **`double`

`validator`

— State validator object

`validatorOccupancyMap`

object

State validator object, specified as a `validatorOccupancyMap`

object.

## Properties

**Note**

For the `'Weights'`

and `'FeasibilityParameters'`

properties, you cannot specify the entire structures at once. Instead, set their fields
individually as name-value pairs. For example,
`trajectoryOptimalFrenet(refPath,validator,'Deviation',0)`

sets the
`'Deviation'`

field of the structure `'Weights'`

.

`Weights`

— Weights for all trajectory costs

structure

The weights for all trajectory costs, specified as a structure containing scalars for the cost multipliers of the corresponding trajectory attributes. The total trajectory cost is a sum of all attributes multiplied by their weights. The structure has these fields.

`Time`

— Weight for time cost

`0`

(default) | positive scalar

The cost function multiplies the weight by the total time taken to reach the
terminal state. Specify this value as the comma-separated pair of
`'Time'`

and a positive scalar in seconds.

**Data Types: **`double`

`ArcLength`

— Weight for arc length cost

`0`

(default) | positive scalar

The cost function multiplies the weight by the total length of the generated
trajectories. Specify this value as the comma-separated pair of
`'ArcLength'`

and a positive scalar in meters.

**Data Types: **`double`

`LateralSmoothness`

— Weight for lateral jerk cost

`0`

(default) | positive scalar

The cost function multiplies the weight by the integral of lateral jerk
squared. This value determines the aggressiveness of the trajectory in the lateral
direction (perpendicular to the reference path). Specify this value as the
comma-separated pair of `'LateralSmoothness'`

and a positive
scalar. To penalize lateral jerk in the planned trajectory increase this cost
value.

**Data Types: **`double`

`LongitudinalSmoothness`

— Weight for longitudinal jerk cost

`0`

(default) | positive scalar

The cost function multiplies the weight by the integral of longitudinal jerk
squared. This value determines the aggressiveness of the trajectories in the
longitudinal direction (direction of the reference path). Specify this value as
the comma-separated pair of `'LongitudinalSmoothness'`

and a
positive scalar. To penalize large change in forward and backward acceleration
increase this cost value.

**Data Types: **`double`

`Deviation`

— Weight for deviation from reference path

`1`

(default) | positive scalar

The cost function multiplies the weight by the perpendicular distance from the
reference path at the end of the trajectory in meters. Specify this value as the
comma-separated pair of `'Deviation'`

and a positive scalar in
meters.

**Data Types: **`double`

**Data Types: **`struct`

`FeasibilityParameters`

— Structure containing feasibility parameters

structure

Feasibility parameters, specified as a structure containing scalar values to check the validity of a trajectory. The structure has these fields.

`MaxCurvature`

— Maximum curvature that vehicle can execute

`0.1`

(default) | positive real scalar

Maximum curvature that the vehicle can execute. Specify this value as the
comma-separated pair of `'MaxCurvature'`

and a positive real
scalar in m^{-1}. This value determines the kinematic
feasibility of the trajectory.

**Data Types: **`double`

`MaxAcceleration`

— Maximum acceleration in direction of motion of vehicle

`2.5`

(default) | positive real scalar

Maximum acceleration in the direction of motion of the vehicle. Specify this
value as the comma-separated pair of `'MaxAcceleration'`

and a
positive real scalar in m/s^{2}. To lower the limit on the
acceleration of the vehicle in the forward or reverse direction decrease this
value.

**Data Types: **`double`

**Data Types: **`struct`

`TimeResolution`

— Trajectory discretization interval

`0.1`

(default) | positive real scalar

Time interval between discretized states of the trajectory. Specify this value as
the comma-separated pair of `'TimeResolution'`

and a positive real
scalar in seconds. These discretized states determine state validity and cost
function.

**Data Types: **`double`

`CostFunction`

— User-defined cost function

nullCost (default) | function handle

The user-defined cost function, specified as a function handle. The function must
accept a matrix of *n*-by-7 states, `TRAJSTATES`

, for
each trajectory and return a cost value as a scalar. The `plan`

function
returns the path with the lowest cost.

For example, ```
leftLaneChangeCost = @(states)((states(end,2) <
refPath(end,2))*10)
```

creates a cost function handle to prioritize left lane
changes.

**Data Types: **`function handle`

`TrajectoryList`

— List of all possible trajectories

structure array

This property is read-only.

The `'TrajectoryList'`

property, returned as a structure array of
all the candidate trajectories and their corresponding parameters. Each structure has
these fields:

`Trajectory`

— An*n*-by-7 matrix of`[`

, where*x*,*y*,*theta*,*kappa*,*speed*,*acceleration*,*time*]*n*is the number of trajectory waypoints.`Cost`

— Cost of the trajectory.`MaxAcceleration`

— Maximum acceleration of the trajectory.`MaxCurvature`

— Maximum curvature of the trajectory.`Feasible`

— A four-element vector`[`

indicating the validity of the trajectory.*velocity*,*acceleration*,*curvature*,*collision*]The value of the elements can be either,

`1`

— The trajectory is valid.`0`

— The trajectory is invalid.`-1`

— The trajectory is not checked.

**Data Types: **`struct`

`TerminalStates`

— Structure of all goal states

structure

A structure that contains a list of goal states relative to the reference path. These parameters define the sampling behavior for generating alternative trajectory segments between start and each goal state. The structure has these fields.

`Longitudinal`

— Lengths of the trajectory segment

`30:15:90`

(default) | vector

Lengths of the trajectory segment, specified as a vector in meters.

**Data Types: **`double`

`Lateral`

— Array of deviations from reference path in perpendicular direction at goal state

`-2:1:2`

(default) | vector

Array of deviations from reference path in perpendicular direction at goal state, specified as a vector in meters.

**Data Types: **`double`

`Speed`

— Velocity at goal state in direction of motion

`10`

(default) | positive scalar

Velocity at the goal state in the direction of motion, specified as a positive scalar in m/s.

**Data Types: **`double`

`Acceleration`

— Acceleration at goal state in direction of motion

`0`

(default) | positive scalar

Acceleration at the goal state in the direction of motion, specified as a
positive scalar in m/s^{2}.

**Data Types: **`double`

`Time`

— Array of end-times for executing trajectory segment

`7`

(default) | positive vector

Array of end-times for executing the trajectory segment, specified as a positive vector in seconds.

**Data Types: **`double`

**Data Types: **`struct`

`Waypoints`

— Waypoints of reference path

[ ] (default) | *n*-by-2 matrix

Waypoints of reference path, specified as an *n*-by-2 matrix of
`[`

pairs, where *x*
*y*]*n* is the number of
waypoints. Waypoints act as a reference for planning alternative trajectories optimized
by this planner.

**Data Types: **`double`

`NumSegments`

— Number of longitudinal segments for each trajectory

`1`

(default) | positive scalar

Number of longitudinal segments for each trajectory. Specify this value as the
comma-separated pair of `'NumSegments'`

and a positive scalar. This
property generates intermediate longitudinal terminal states to which all lateral
terminal states are combined with for generating more motion primitives to each terminal
state.

For example, `'NumSegments',2`

creates two partitions between each
longitudinal terminal state. Trajectories are generated to reach the intermediate
longitudinal states with all the available lateral terminal states.

**Data Types: **`double`

`DeviationOffset`

— Deviation offset from reference path in lateral direction

`0`

(default) | scalar

Deviation offset from the reference path in the lateral direction. Specify this
value as the comma-separated pair of `'DeviationOffset'`

and a scalar.
A negative value offset the deviation to the right, and a positive value offset the
deviation to the left of the reference path in the lateral direction. Set this property
to bias your solution to a certain turn direction when avoiding obstacles in the
reference path.

**Data Types: **`double`

## Object Functions

`cart2frenet` | Convert Cartesian states to Frenet states |

`copy` | Create deep copy of object |

`frenet2cart` | Convert Frenet states to Cartesian states |

`plan` | Plan optimal trajectory |

`show` | Visualize trajectories |

## 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','--')

## Limitations

Self-intersections in the reference path can lead to unexpected behavior.

The planner does not support reverse driving.

Initial orientation for planning should be within

`-pi/2`

and`pi/2`

to the reference path.Limit the number of TerminalStates for real-time applications since computational complexity grows with it.

## More About

### Reference Path Generation

The `trajectoryOptimalFrenet`

and
`referencePathFrenet`

objects generate a reference path using the specified waypoints by fitting a
piecewise-continuous clothoid spline to the waypoints [2].

A clothoid connects each pair of adjacent waypoints (change in curvature is constant with respect to arc length).

These boundary conditions define the clothoid parameters:

If specified as an

*n*-by-2 array of [*x**y*] waypoints, then the solver will produce a C2 continuous spline (clothoid endpoints are tangent and have matching curvature with their neighbors).If specified as an

*n*-by-3 array of [*x**y**theta*] waypoints, then a degree of freedom is removed from the solver, so the spline is only C1 continuous (clothoid endpoints are tangent to their neighbors, but the curvature can change abruptly).

## References

[1] Werling, Moritz, Julius Ziegler,
Sören Kammel, and Sebastian Thrun. "Optimal Trajectory Generation for Dynamic Street Scenarios
in a Frenet Frame." *2010 IEEE International Conference on Robotics and
Automation*. 2010, pp. 987–993.

[2] Bertolazzi, Enrico, and Marco Frego. “Fast and Accurate Clothoid Fitting.” Mathematical Methods in the Applied Sciences 38, no. 5 (March 30, 2015): 881–97. https://doi.org/10.1002/mma.3114.

## Extended Capabilities

### C/C++ Code Generation

Generate C and C++ code using MATLAB® Coder™.

## Version History

**Introduced in R2019b**

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