Main Content

frenet2global

Convert Frenet states to global states

Description

example

globalState = frenet2global(refPath,frenetState) converts Frenet trajectory states to global states.

Examples

collapse all

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.

plot(globalPoints(:,1),globalPoints(:,2),'r*','MarkerSize',10)
plot(nearestPathPoint(:,1),nearestPathPoint(:,2),'b*','MarkerSize',10)

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"])

Figure contains an axes object. The axes object contains 6 objects of type line. These objects represent Waypoints, Reference Path, Trajectory to 30m, Global Points, Closest Points, Interpolated Path Points.

Input Arguments

collapse all

Reference path, specified as a referencePathFrenet object.

Trajectory in the Frenet coordinate frame, specified 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.

Output Arguments

collapse all

Trajectories in the global coordinate frame, returned as a P-by-6 numeric matrix with rows of form [x y theta kappa speed accel], where:

  • x y and theta –– SE(2) state expressed in global coordinates, with x and y in meters and theta in radians

  • kappa –– Curvature, or inverse of the radius, in meters

  • speed –– Speed in the theta direction in m/s

  • accel –– Acceleration in the theta direction in m/s2

P is the number of Frenet trajectories converted to global trajectories.

The absolute difference between theta and the orientation of the closest point on the path from a global state must not exceed pi/2 when converting between global and Frenet coordinate systems.

Extended Capabilities

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

Introduced in R2020b