createParallelState

Create states using Frenet and global parameters

Syntax

[globalState,frenetState,latTimeDerivatives] = createParallelState(refPath,S,L,V,A)

[_] = createParallelState(refPath,S,L,V,A,invertHeading)

Description

[globalState,frenetState,latTimeDerivatives] = createParallelState(refPath,S,L,V,A) takes an arclength S, lateral deviation L, body velocity V, and body acceleration A, and returns the global states globalState, Frenet states frenetState which run parallel to the reference path, and the lateral time derivatives of the Frenet states latTimeDerivatives.

[_] = createParallelState(refPath,S,L,V,A,invertHeading) optionally accepts a vector invertHeading, indicating whether the state heading of each waypoint should be inverted or not.

Input Arguments

refPath — Reference path

referencePathFrenet object

Reference path, specified as a referencePathFrenet object.

S — Path arclengths

N-element column vector

Path arclengths, specified as an N-element column vector in meters, where N is the total number of desired arclengths to calculate.

L — Lateral deviations

N-element column vector

Lateral deviation, specified as an N-element vector in meters. The lateral deviation is measured as the distance along the normal vector of the current path to the original path.

Data Types: single | double

V — Body velocity

N-element column vector

Body velocity, specified as an N-element vector in meters per second. The velocity is derived by transforming the global velocity to the Frenet frame located at S along the path.

Data Types: single | double

A — Body acceleration

N-element column vector

Body velocity, specified as an N-element vector in meters per second squared. The acceleration is derived by transforming the global acceleration to the Frenet frame located at S along the path.

Data Types: single | double

invertHeading — Invert heading flag

N-element column vector

Invert heading flag, specified as an N-element column vector of binary values, indicating whether the state heading of each waypoint should be inverted or not. 0 indicates to not invert the heading, and 1 indicates to invert the heading.

Data Types: single | double

Output Arguments

globalState — States in global coordinate frame

P-by-6 numeric matrix

States 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 total number of Global states.

frenetState — States in Frenet coordinate frame

P-by-6 numeric matrix

States 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 total number of Frenet states.

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. createParallelState derives the first and second derivatives by transforming the global velocity and acceleration to the Frenet frame at the arclength along the path. 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.

Version History

Introduced in R2022a