frenet2cart
Convert Frenet states to Cartesian states
Description
frenet2cart(
converts a six-element vector of planner
,frenetStates
)frenetStates
[s, ds/dt,
d2s/dt2,
l, dl/ds,
d2l/ds2]
to a six-element vector of Cartesian states [x,
y, theta, kappa,
speed, acceleration]
.
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','--')
Input Arguments
planner
— Optimal trajectory planner in Frenet space
trajectoryOptimalFrenet
object
Optimal trajectory planner in Frenet space, specified as a trajectoryOptimalFrenet
object.
frenetStates
— Vector of Frenet states
six-element vector
Vector of Frenet states, 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.
Example: [10 1 0 3 0 0]
Data Types: double
Extended Capabilities
C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.
Version History
Introduced in R2019b
See Also
MATLAB 명령
다음 MATLAB 명령에 해당하는 링크를 클릭했습니다.
명령을 실행하려면 MATLAB 명령 창에 입력하십시오. 웹 브라우저는 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)