global2frenet

Convert global states to Frenet states

Syntax

frenetState = global2frenet(refPath,globalState)

[_,latTimeDerivatives] = global2frenet(refPath,globalState)

[_] = global2frenet(refPath,globalState,sFrame)

Description

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

[_,latTimeDerivatives] = global2frenet(refPath,globalState) returns latTimeDerivatives containing 1st and 2nd order derivatives of lateral deviation with respect to time and a flag indicating if the heading should flip when converting to global coordinates.

[_] = global2frenet(refPath,globalState,sFrame) accepts a vector of arclengths, sFrame at which the Frenet frame should be centered at.

Input Arguments

refPath — Reference path

referencePathFrenet object

Reference path, specified as a referencePathFrenet object.

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:

• 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/s²

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

sFrame — Arclength frame locations

N-element vector

Arclength center locations for the Frenet frames, specified as an N-element vector in meters. For correct results, the vector between the frame and the xy location of the globalState must be orthogonal to the tangent angle.

Output Arguments

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

N-by-3 matrix

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 (false).

Note

If defining latTimeDerivatives without the use of global2frenet, the following rules should be followed:

1. The invertHeading flag should be true when:

   1. The vehicle is moving in reverse (speed is less than 0)

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

2. If 1b is true, then dL/dS must be negated.

Extended Capabilities

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

Version History

Introduced in R2020b