Convert Frenet states to global states
Generate Trajectory from Reference Path
Generate a reference path from a set of waypoints.
waypoints = [0 0; 50 20; 100 0; 150 10]; refPath = referencePathFrenet(waypoints);
Create a trajectoryGeneratorFrenet object from the reference path.
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] frenetTraj = connect(connector,initState,termState,5);
Convert the trajectory to the global states.
globalTraj = frenet2global(refPath,frenetTraj.Trajectory);
Display the reference path and the trajectory.
show(refPath); axis equal hold on plot(globalTraj(:,1),globalTraj(:,2),'b')
Specify global points and find the closest points on reference path.
globalPoints = waypoints(2:end,:) + [20 -50]; nearestPathPoint = closestPoint(refPath,globalPoints);
Display the global points and the closest points on reference path.
Interpolate between the arc lengths of the first two closest points along the reference path.
arclengths = linspace(nearestPathPoint(1,6),nearestPathPoint(2,6),10); pathStates = interpolate(refPath,arclengths);
Display the interpolated path points.
plot(pathStates(:,1),pathStates(:,2),'g') legend(["Waypoints","Reference Path","Trajectory to 30m",... "Global Points","Closest Points","Interpolated Path Points"])
refPath — Reference path
Reference path, specified as a
frenetState — Trajectory in Frenet coordinate frame
P-by-6 numeric matrix
Trajectory in the Frenet coordinate frame, returned as a P-by-6 numeric matrix with rows 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. P is the number of Frenet states specified.
latTimeDerivatives — Lateral time derivatives
Lateral time derivatives, specified as an N-by-3 matrix where each row is of the form [dL/dt ddL/dt^2 invertHeading] and N is the total number of points in
points. Each row contains the 1st and 2nd order time derivatives of lateral deviation and a flag, invertHeading, which indicates whether the heading should be flipped when converting to global coordinates (
true) or not (
latTimeDerivatives without the use of
global2frenet, the following rules should be followed:
The invertHeading flag should be true when:
The vehicle is moving in reverse (speed is less than 0)
The vehicle is stationary (speed is equal to 0), and the vehicle is facing away from the path's tangent vector. i.e. cos(|tangentAngle(obj,S)-thetaExpected|) < 0
If 1b is true, then dL/dS must be negated.
globalState — Trajectory in global coordinate frame
P-by-6 numeric matrix
Trajectories in the global coordinate frame, specified as a P-by-6 numeric matrix with rows of form
[x y theta kappa speed accel], where:
theta–– SE(2) state expressed in global coordinates, with
yin meters and
kappa–– Curvature, or inverse of the radius, in meters
speed–– Speed in the
thetadirection in m/s
accel–– Acceleration in the
thetadirection in m/s2
P is the number of Frenet trajectories converted to global trajectories.