trajectoryGeneratorFrenet

Find optimal trajectory along reference path

Since R2020b

Description

The trajectoryGeneratorFrenet object generates alternate trajectories using fourth or fifth-order polynomials relative to a given reference path. Each trajectory defines a motion between Frenet states over a specified time span.

Frenet states describe their position, velocity and acceleration relative to a static reference path, specified as a referencePathFrenet object.

The object expresses Frenet states as a vector of form [S dS ddS L dL ddL], where S is the arc length and L is the perpendicular deviation from the direction of the reference path. Derivatives of S are relative to time. Derivatives of L are relative to the arc length, S.

To generate alternative trajectories, specify the initial and terminal Frenet states with a given time span to the connect object function.

Creation

Syntax

connectorFrenet = trajectoryGeneratorFrenet(refPath)

connectorFrenet = trajectoryGeneratorFrenet(refPath,'TimeResolution',timeValue)

Description

connectorFrenet = trajectoryGeneratorFrenet(refPath) generates trajectories between initial and terminal states relative to a reference path refPath specified as a referencePathFrenet object. The refPath input argument sets the ReferencePath property.

connectorFrenet = trajectoryGeneratorFrenet(refPath,'TimeResolution',timeValue) specifies the time interval for discretization. The timeValue argument sets the TimeResolution property.

Properties

TimeResolution — Discretization time interval between sampled Frenet states

0.1 (default) | positive scalar in seconds

Discretization time interval between sampled Frenet states, specified as a positive scalar in seconds. When using the connect object function, this property determines the resolution of the Times field of the generated trajectory structures, frenetTrajectory and globalTrajectory.

Data Types: double

ReferencePath — Reference path in Frenet coordinates

referencePathFrenet

Reference path in Frenet coordinates, specified as a referencePathFrenet object.

Object Functions

connect	Connect initial and terminal Frenet states
createParallelState	Create states using Frenet and global parameters

Examples

Generate Alternative Trajectories for Reference Path

Generate alternative trajectories for a reference path using Frenet coordinates. Specify different initial and terminal states for your trajectories. Tune your states based on the generated trajectories.

Generate a reference path from a set of waypoints. Create a trajectoryGeneratorFrenet object from the reference path.

waypoints = [0 0; ...
	50 20; ...
	100 0; ...
	150 10];
refPath = referencePathFrenet(waypoints);
connector = trajectoryGeneratorFrenet(refPath);

Generate a five-second trajectory between the path origin and a point 30 m down the path as Frenet states.

initState = [0 0 0 0 0 0];  % [S ds ddS L dL ddL]
termState = [30 0 0 0 0 0]; % [S ds ddS L dL ddL]
[~,trajGlobal] = connect(connector,initState,termState,5);

Display the trajectory in global coordinates.

show(refPath);
hold on
axis equal
plot(trajGlobal.Trajectory(:,1),trajGlobal.Trajectory(:,2),'b')
legend(["Waypoints","Reference Path","Trajectory to 30m"])

Create a matrix of terminal states with lateral deviations between –3 m and 3 m. Generate trajectories that cover the same arc length in 10 seconds, but deviate laterally from the reference path. Display the new alternative paths.

termStateDeviated = termState + ([-3:3]' * [0 0 0 1 0 0]);
[~,trajGlobal] = connect(connector,initState,termStateDeviated,10);

clf
show(refPath);
hold on
axis equal
for i = 1:length(trajGlobal)
    plot(trajGlobal(i).Trajectory(:,1),trajGlobal(i).Trajectory(:,2),'g')
end
legend(["Waypoints","Reference Path","Alternative Trajectories"])
hold off

Specify a new terminal state to generate a new trajectory. This trajectory is not desirable because it requires reverse motion to achieve a longitudinal velocity of 10 m/s.

newTermState = [5 10 0 5 0 0];
[~,newTrajGlobal] = connect(connector,initState,newTermState,3);

clf
show(refPath);
hold on
axis equal
plot(newTrajGlobal.Trajectory(:,1),newTrajGlobal.Trajectory(:,2),'b');
legend(["Waypoint","Reference Path","New Trajectory"])
hold off

Relax the restriction on the longitudinal state by specifying an arc length of NaN. Generate and display the trajectory again. The new position shows a good alternative trajectory that deviates off the reference path.

relaxedTermState = [NaN 10 0 5 0 0];
[~,trajGlobalRelaxed] = connect(connector,initState,relaxedTermState,3);

clf
show(refPath);
hold on
axis equal
plot(trajGlobalRelaxed.Trajectory(:,1),trajGlobalRelaxed.Trajectory(:,2),'g');
hold off

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

trajectoryGeneratorFrenet now supports code generation with DynamicMemoryAllocation='off'.

Version History

Introduced in R2020b