Convert Cartesian states to Frenet states



cart2frenet(planner,cartesianStates) converts a 6-element vector of Cartesian states [x, y, theta, kappa, speed, acceleration] to a 6-element vector of Frenet states [s, ds, dds, l, dl, ddl], 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.


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(700,700);
grid(296:305,150:175) = 1;
grid(286:295,300:325) = 1;
grid(306:315,300:325) = 1;
grid(296:305,450:475) = 1;
grid(286:295,600:625) = 1;
grid(306:315,600:625) = 1;

Create a binaryOccupancyMap with the grid map.

map = binaryOccupancyMap(grid);

Assign the map to the state validator.

stateValidator.Map = map;

Plan and Visualize Trajectory

Create a reference path for the planner to follow.

refPath = [10,400;700,400];

Declare the cost function handle to prioritize left lane changes.

leftLaneChangeCost = @(states)((states(end,2) < refPath(end,2))*10);

Initialize the planner object with the reference path, the state validator, and the custom cost function.

plannerObj = trajectoryOptimalFrenet(refPath,stateValidator,'CostFunction',leftLaneChangeCost);

Assign lateral deviation values.

plannerObj.TerminalStates.Lateral = -10:10:10;

Trajectory Generation

Initialize the variables for the replanning loop.

previousDeviationValue = 0;
deviationValue = 0;
initState = zeros(1,6);

Replan until final waypoint is closest to the terminal state.

for j = 1:150
    % Deviation from the current reference path
    if initState(4) > 5
        deviationValue = 10;
    elseif initState(4) < -5
        deviationValue = -10;
    % Move reference path to the current lane
    if previousDeviationValue ~= deviationValue
        % Shift the waypoints by the deviation value
        plannerObj.Waypoints = [plannerObj.Waypoints(:,1), plannerObj.Waypoints(:,2) + deviationValue];
        % Shift the terminal states such that they remain fixed in world frame
        plannerObj.TerminalStates.Lateral = plannerObj.TerminalStates.Lateral - deviationValue;
        % Store the deviation value
        previousDeviationValue = deviationValue;
        % Update initState variable with the new planner object
        initState = cart2frenet(plannerObj,trajectory(10,1:6));
    % Generate a trajectory from initState
    trajectory = plan(plannerObj,initState);
    % Use 10th state of the current trajectory as a starting point for
    % replanning
    initState = cart2frenet(plannerObj,trajectory(10,1:6));
    % Visualize every 5th iteration
    if mod(j,5)==0
        hold on;

Input Arguments

collapse all

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

Vector of Cartesian states, specified as a 1-by-6 vector, [x, y, theta, kappa, speed, acceleration]. x and y specifies 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, and acceleration specify 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™.

Introduced in R2019b