cart2frenet

Convert Cartesian states to Frenet states

Syntax

cart2frenet(planner,cartesianStates)

Description

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