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
[x
y]
waypoints and a state validator,
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
[x
y]
pairs, where 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/s2. 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[x, y, theta, kappa, speed, acceleration, time]
, where 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[velocity, acceleration, curvature, collision]
indicating the validity of the trajectory.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/s2.
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
[x
y]
pairs, where 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
andpi/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
Comando MATLAB
Hai fatto clic su un collegamento che corrisponde a questo comando MATLAB:
Esegui il comando inserendolo nella finestra di comando MATLAB. I browser web non supportano i comandi MATLAB.
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)