cart2frenet
Convert Cartesian states to Frenet states
Description
cart2frenet(
converts a six-element vector of planner
,cartesianStates
)cartesianStates
[x, y, theta,
kappa, speed,
acceleration]
to a six-element vector of Frenet states
[s, ds/dt,
d2s/dt2,
l, dl/ds,
d2l/ds2]
,
where s is arc length from the first point in reference path, and
l is normal distance from the closest point at s on
the reference path.
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.
cartesianStates
— Vector of Cartesian states
six-element vector
Vector of Cartesian states, specified as a 1-by-6 vector
[x, y, theta,
kappa, speed,
acceleration]
.
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
.
Example: [110 110 pi/4 0 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)