trajectoryOptimalFrenet

Find optimal trajectory for reference path

Description

The trajectoryOptimalFrenet object generates an optimal, feasible, and collision-free trajectory for the reference path.

The plan function computes an optimal trajectory between start and terminal states. The function samples multiple trajectories for each pair of states and chooses a feasible trajectory with the least cost.

Creation

Description

planner = trajectoryOptimalFrenet(refPath,validator) creates a trajectoryOptimalFrenet object with reference path, refPath, in the form of a n-by-2 array of [x y] waypoints and a state validator, validator, specified as a validatorOccupancyMap object.

example

planner = trajectoryOptimalFrenet(refPath,validator,Name,Value) sets additional properties using one or more name-value pairs in any order.

Input Arguments

expand all

Reference path, specified as an n-by-2 matrix of [x y] pairs, where n is the number of waypoints.

Example: [100,100;400,400]

Data Types: double

stateValidator object, specified as an validatorOccupancyMap object.

Properties

expand all

Note

For the 'Weights' and 'FeasibilityParameters' properties, you cannot specify the entire structures at once. Instead, set their fields individually as name-value pairs. For example, trajectoryOptimalFrenet(refPath,validator,'Deviation',0) sets the 'Deviation' field of the structure 'Weights'.

The weights for all trajectory costs, specified as a structure containing scalars for the cost multipliers of the corresponding trajectory attributes. The total trajectory cost is a sum of all attributes multiplied by their weights. The structure has the following fields.

The cost function multiplies the weight by the total time taken to reach the terminal state. Specify this value as the comma-separated pair of 'Time' and a positive scalar in seconds.

Data Types: double

The cost function multiplies the weight by the total length of the generated trajectories. Specify this value as the comma-separated pair of 'ArcLength' and a positive scalar in meters.

Data Types: double

The cost function multiplies the weight by the integral of lateral jerk squared. This value determines the aggressiveness of the trajectory in the lateral direction (perpendicular to the reference path). Specify this value as the comma-separated pair of 'LateralSmoothness' and a positive scalar. Increase the cost value to penalize lateral jerk in the planned trajectory.

Data Types: double

The cost function multiplies the weight by the integral of longitudinal jerk squared. This value determines the aggressiveness of the trajectories in the longitudinal direction (direction of the reference path). Specify this value as the comma-separated pair of 'LongitudinalSmoothness' and a positive scalar. Increase this cost value to penalize large change in forward and backward acceleration.

Data Types: double

The cost function multiplies the weight by the perpendicular distance from the reference path at the end of the trajectory. Specify this value as the comma-separated pair of 'Deviation' and a positive scalar in meters.

Data Types: double

Data Types: struct

Feasibility parameters, specified as a structure containing scalar values to check the validity of a trajectory. The structure has the following fields.

Maximum curvature that the vehicle can execute. Specify this value as the comma-separated pair of 'MaxCurvature' and a positive real scalar in m-1. This value determines the kinematic feasibility of the trajectory.

Data Types: double

Maximum acceleration in the direction of motion of the vehicle. Specify this value as the comma-separated pair of 'MaxAcceleration' and a positive real scalar in m/s2. Decrease this value to lower the limit on the acceleration of the vehicle in the forward or reverse direction.

Data Types: double

Data Types: struct

Time interval between discretized states of the trajectory. Specify this value as the comma-separated pair of 'TimeResolution' and a positive real scalar in seconds. The state validity and cost function are based on these discretized states.

Data Types: double

User-defined cost function, specified as a function handle. The function must accept a matrix of n-by-7 states, TRAJSTATES, for each trajectory and return a cost value as a scalar. The cost function signature is:

cost = CostFunction(TRAJSTATES)

The plan function returns the path with the lowest cost.

Data Types: function handle

This property is read-only.

The 'TrajectoryList' property, returned as a structure containing an array of all trajectories and their corresponding parameters. The structure has the following fields:

  • Trajectory

  • Cost

  • MaxAcceleration

  • MaxCurvature

  • Feasible — A four-element vector [velocity acceleration curvature collision] indicating the validity of the path.

    A value of 1 means that the trajectory is valid, 0 means invalid, and –1 means not checked.

Data Types: struct

A structure that contains a list of goal states relative to the reference path. These parameters define the sampling behavior for generating alternative trajectory segments between start and each goal state. The structure has the following fields.

Lengths of the trajectory segment, specified as a vector in meters.

Data Types: double

Array of deviations from reference path in perpendicular direction at goal state, specified as a vector in meters.

Data Types: double

Velocity at the goal state in the direction of motion, specified as a scalar in m/s.

Data Types: double

Acceleration at the goal state in the direction of motion, specified as a scalar in m/s2.

Data Types: double

Array of end-times for executing the trajectory segment, specified as a positive vector in seconds.

Data Types: double

Data Types: struct

Waypoints of reference path, specified as an n-by-2 matrix of [x y] pairs, where n is the number of waypoints, which act as a reference for planning alternative trajectories optimized by this planner.

Data Types: double

Object Functions

cart2frenetConvert Cartesian states to Frenet states
frenet2cartConvert Frenet states to Cartesian states
planPlan optimal trajectory
showVisualize trajectories

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(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;
    end
    
    % 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));
    end
    
    % 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
        show(map)
        hold on;
        show(plannerObj,'TrajectoryColor','none');
        drawnow
    end
end

Limitations

  • Self-intersections in the reference path can lead to unexpected behavior.

  • The planner does not support reverse driving.

  • Initial orientation for planning should be within -pi/2 and pi/2 to the reference path.

  • Keep the number of TerminalStates in check for real-time applications since computational complexity grows with it.

References

[1] Werling, Moritz, Julius Ziegler, Sören Kammel, and Sebastian Thrun. "Optimal trajectory generation for dynamic street scenarios in a frenet frame." 2010 IEEE International Conference on Robotics and Automation. 2010, pp. 987–993.

Extended Capabilities

C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.

Introduced in R2019b