Main Content

frenet2cart

Convert Frenet states to Cartesian states

Description

example

frenet2cart(planner,frenetStates) converts a six-element vector of 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

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

Figure contains an axes object. The axes object with title Binary Occupancy Grid contains 5 objects of type image, line, patch. These objects represent Waypoints, Reference Path, Optimal Trajectory.

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

Figure contains an axes object. The axes object with title Binary Occupancy Grid contains 6 objects of type image, line, patch. These objects represent Waypoints, Reference Path, Optimal Trajectory, Lane Boundaries.

Input Arguments

collapse all

Optimal trajectory planner in Frenet space, specified as a trajectoryOptimalFrenet object.

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

Introduced in R2019b