Improved Technique for Autonomous Vehicle Motion Planning Based on Integral Constraints and Sequential Optimization

Abstract

The study is dedicated to elaborating and analyzing a technique for autonomous vehicle (AV) motion planning based on sequential trajectory and kinematics optimization. The proposed approach combines the finite element method (FEM) basics and nonlinear optimization with nonlinear constraints. There were five main innovative aspects introduced in the study. First, a 7-degree polynomial was used to improve the continuity of piecewise functions representing the motion curves, providing 4 degrees of freedom (DOF) in a node. This approach allows using the irregular grid for roadway segments, increasing spans where the curvature changes slightly, and reducing steps in the vicinity of the significant inflections of motion boundaries. Therefore, the segment length depends on such factors as static and moving obstacles, average road section curvature, camera sight distance, and road conditions (adhesion). Second, since the method implies splitting the optimization stages, a strategy for bypassing the moving obstacles out of direct time dependency was developed. Thus, the permissible area for maneuvering was determined using criteria of safety distance between vehicles and physical limitation of tire–road adhesion. Third, the nodal inequality constraints were replaced by the nonlinear integral equality constraints. In contrast to the generally distributed approach of restricting the planning parameters in nodes, the technique of integral equality constraints ensures the disposition of motion parameters’ curves strictly within the preset boundaries, which is especially important for quite long segments. In this way, the reliability and stability of predicted parameters are improved. Fourth, the seamless continuity of both the sought parameters and their derivatives is ensured in transitional nodes between the planning phases and adjacent global coordinate systems. Finally, the problem of optimization rapidity to match real-time operation requirements was addressed. For this, the quadrature integration approach was implemented to represent and keep all the parameters in numerical form. The study considered cost functions, limitations stipulated by the vehicle kinematics and dynamics, as well as initial and transient conditions between the planning stages. Simulation examples of the predicted trajectories and curves of kinematic parameters are demonstrated. The advantages and limitations of the proposed approach are highlighted.

1. Introduction

The intensive research and developments in the field of AVs have provided a variety of methods and approaches to motion planning. Although the generalized techniques and their main ideas are sufficiently well represented in the literature, many details regarding improving the forecast rapidity and reliability as well as questions of the modeling quality remain open. The characteristics of selected studies on planning AV motion are presented in

Table 1. Characteristics of selected studies on planning AV motion.

Ref.	Path Model	Speed Model	Optimization Model	Constraints
[1]	The track circuit represents the allowable driving area; the center line is the reference; the forward vector is defined by the gradient of the center-line	No yaw is considered; the vehicle is facing in the same direction as the longitudinal velocity; the speed is determined based on the control of accelerations	MPC as the control structure; General Trajectory Optimization Problem: the goal is to maximize the track progression; sequential convex programming methods; sequential linearization	The left and right boundary points along the normal direction with the track width; linearized non-convex constraints of the optimization problem
[2]	Free collision trajectory; local path planner based on Vehicle Attractor Dynamic Approach (VADA)	Lateral vehicle dynamic model; single-track model with seven DOF	Control vector is formed by the steering angle rate and by the longitudinal acceleration; PID controller for the vehicle speed determines the throttle and brake pedal positions	Ranges of the maximum lateral and longitudinal accelerations; maximum speed
[3]	Cubic-curvature curves; modified bidirectional rapidly exploring random tree (bi-RRT) approach	Discrete speed model based on the final differences of the path curve	Feasible online SQP to minimize the time and acceleration costs	Limited curvature, time, speed, acceleration, kinematic constraints
[4]	Cubic splines connecting three path nodes situated at equal distances between the initial and the target point	Trapezoidal velocity profile; interpolating cubic polynomials for parameterization of the velocity profile	Band Matrix Method to find the path model variables. The trapezoidal velocity profile is smoothened to guarantee the acceleration continuity. Minimum travel time through the specified path	Lane constraints, speed limit 50 MPH, acceleration limit 10 m/s2, jerk limit 10 m/s3, initial and final velocity
[5]	Trajectory is combined of finite elements represented by the nodal DOF and basis functions based on the 5th order polynomial	FE model with 3 DOF in a node to ensure the jerk continuity; kinematic vehicle model with ideal turn	Sequential nonlinear optimization of trajectory and speed with nonlinear constraints; SQP method; curvature rate and slip angle as basic criteria for trajectory cost function; speed deviation, longitudinal acceleration and jerk are basic criteria for the speed profile cost function	Allowable motion zone with boundaries; critical slip speed, maximum acceleration by the powertrain properties, maximum adhesion, initial and final conditions
[6]	Trajectory is formed by the longitudinal and lateral displacements to avoid a set of obstacles between the initial and target locations	3 DOF vehicle model; system of 8 state-space differential equations including controls;	Nonlinear model predictive control framework for non-negligible optimal control problem (OCP); cost function criteria include: vehicle’s global position coordinates, steering angle, steering rate, longitudinal jerk, and parameters to prevent the minimum vertical tire load	State bounds: longitudinal and lateral displacements, yaw angle, steering angle, longitudinal speed and acceleration; control restrictions: steering rate, longitudinal jerk
[7]	Road structure is represented by the center lines of the adjacent lanes relevant for overtaking	Constant velocity model within the Kalman filter	SQP-method to optimize the cost function based on deviation from the reference trajectory, acceleration and jerk	Probabilistic forbidden zones; maximum lateral acceleration, limited steering angle, spatial constraints to make
[8]	Fifth degree polynomial trajectory generation method; single trajectory instead of piecewise	Fifth degree polynomial trajectory generation method	Two stages of constrained quadratic programming (QP); gradient based method that considers trajectory smoothness and curvature; cost function to evaluate efficiency, comfort, and safety consists of steady state relative distance, time to collision, total time, and acceleration	The sampling time range is between 4.5 s and 7.0 s; maximum trajectory curvature, maximum absolute value of offset between original trajectory and smoothed trajectory
[9]	Reference trajectory refers to a coarse trajectory; a reference trajectory is derived by searching an abstracted state space via dynamic programming (DP)	Speed is integrated as a state element in the Cartesian frame based on the jerk as a control parameter	Optimal control problem (OCP) is solved numerically via a gradient-based optimizer; the cost function is represented by deviations from reference trajectory (linear and angular coordinates), and control profiles towards zero (jerk and yaw rate)	Collision avoidance constraints; allowable bounds of the state/control profiles: maximum acceleration, speed, jerk, yaw rate, yaw angle
[10]	The combined trajectory planning and tracking method; vehicle moves forward along the lane with the longitudinal and lateral potentials	Speed is calculated from the vehicle kinematics model	MPC and Artificial Potential Field (APF); APF for environment, local vehicle, and driving style as parts of the objective function	The desired speed, maximum acceleration, limits of the controls and their increments; current lane boundary restrictions
[11]	Behavioral trajectory planning in three steps: path candidate generation, optimal speed profile generation, and trajectory selection; converting behavioral trajectory into a denser trajectory	Path-velocity decomposition method; spatiotemporal nodes; two steps: the speed limit profile generation and the optimal speed search determined by the maximum lateral acceleration	Numerical optimization for motion trajectory planning; objective function: reference cost, acceleration, jerk calculated by the final differences; basic SQP performs optimization by approximating the objective function using second-order differentiation	Speed limits, maximum vehicle acceleration, environment constraints, avoidance of collisions with dynamic objects constraints
[12]	Cubic polynomial for smooth curve of lateral offset depending on the path length	Cubic polynomial for the planned speed as function of the path length	Three criteria in terms of efficiency, comfort and safety are adopted to compose the cost function	Road speed limit, yaw rate, maximum and minimum accelerations
[13]	Bi-level optimization the trajectory planning; the prediction is based on current traffic conditions and vehicle driving behaviors determined by car-following and lane-changing models; lane-changing strategy tree	Second-order vehicle kinematics; speed distribution based on the acceleration profile optimization	The upper-level optimization minimizes the overall cost including travel time, fuel consumption, and lane-changing cost; the lower-level optimization determines the optimal acceleration for minimum travel time and fuel consumption by the given trajectory from the upper-level model	Distance between preceding and following vehicles; the minimum time interval between two consecutive lane-changing maneuvers; absolute values of the maximum deceleration (4 m/s2) and acceleration (2 m/s2)
[14]	For path planning any method can be used	The speed planner updates the path information by evaluating conditions and vehicle status, the planner calculates a speed profile based on longitudinal acceleration and jerk.	A minimum time optimization control problem; the optimal speed profile ensures a safe and comfortable vehicle speed for the is obtained.	Rollover criterion, maximum speed corresponding to the maximum lateral acceleration, skidding friction, steering rate, steer angle, maximum acceleration and jerk
[15]	Two high-level options (lane follow/wait and lane change); waypoint on the target-lane is selected using the ego-vehicle state information through the epsilon greedy strategy; sub-trajectories form a complete trajectory	Target speed is calculated using the maximum acceleration or deceleration to ensure a smooth sub-trajectory; then the target speed and final waypoint values are given, the PID controller generates longitudinal and lateral control	Double Q-Learning; deep Q-Learning algorithm is used to find an optimal action-selection policy to maximize the action-value function through minimizing the loss function between predicted action-value and the target action-value	Regular time step penalty, regular time step reward for progressing towards final destination, collision penalty, unsafe penalty, goal not required penalty, non-smoothness penalty, Success Reward
[16]	Trajectory design is based on a constrained optimization; clothoid trajectory for the lateral displacement and yaw angle	Kinematic equations for the longitudinal velocity and displacement for planning; vehicle is described by the dynamical bicycle model for tracking	Minimizing the tracking error and the ratios of the clothoid sections by a quadratic programming method; LPV-based control design method to guarantee the tracking of the generated trajectory	Finite horizon length; constraints are incorporated into the trajectory optimization problem; minimum and maximum lateral offsets
[17]	Quadratic time-based function for longitudinal displacement; 4th extent time depending function for lateral displacement	Linear time depending function for the vehicle longitudinal velocity; Gaussian distribution for describing the vehicle lateral velocity	MPC is converted to a standard quadratic programming problem; nonlinear vehicle dynamic model is linearized as a linear time invariant (LTI) state space form	Vehicle state and control constraints; maximum acceleration, road adhesion limit
[18]	Cubic spline interpolation to find the driving centerline points in the Cartesian coordinate system from sampling points of the Frenet frame; quintic time depending polynomial for lateral and longitudinal displacement	Fourth extent polynomials for the lateral and longitudinal velocities; 3th extent polynomials for the lateral and longitudinal accelerations	The cost function considers the longitudinal and lateral jerks, offset degree from the reference line, deviation between the planning and desired speeds, minimum distance between the AV and the obstacles, cost of a collision between the AV and the static or dynamic obstacles	Lateral position sampling range; predicted time sampling range; target speed sampling range; predicted time sampling interval; target speed sampling interval
[19]	Reference map; candidate paths in the curvilinear coordinate system; 4th order polynomial for the lateral offset	Desired velocity profile based on a set of speed limitations	Objective function includes the factors such as smoothness, consistency, reference tracking and safety	Prediction horizon; speed is restricted by imposing the lateral acceleration limitation
[20]	Path-velocity decomposition; the path is treated as an input	Double integrator model for the movement along the path; the model yields the agent’s position from its velocity and acceleration	Velocity is optimized to follow a reference by solving the optimal control problem (OCP)	Constraints: input, states, terminal position, prediction horizon
[21]	Independent movements in lateral and longitudinal directions; the sine function curve is applied to determine the trajectory; lane change in original lane (LC-O) stage and lane change in target lane (LC-T)	Velocity and acceleration in the lateral direction of lane change are obtained by the first and second derivatives of the trajectory curve model	Cost function is designed to optimize processes of LC-O stage and LC-T stage; objective functions represent desired accelerations at the LC-O stage and LC-T stages, the gap errors between vehicles, and the terminal cost	Maximum longitudinal and lateral accelerations, safe gap constraint at critical moment, gap of host vehicle and surrounding vehicles, the minimum time
[22]	A set of possible trajectories based on B-splines; quintic polynomials to connect initial state with a grid of terminal constraints in space and time dimensions	The first-order dynamics for the vehicle driveline based on the longitudinal acceleration, commanded acceleration, and time constant; virtual triple integrator system	The cost function selects the optimal (feasible and collision free) trajectory from the generated set by minimizing the cumulative error in the B-spline over the entire prediction horizon	Testing possible violations of constraints and occurrence of collisions; Schoenberg-Whitney’s Condition; constant zero acceleration for the extended trajectory
[23]	Third order polynomials to independently represent x and y displacement	Kinematic bicycle model to calculate state vector including velocity	Chance-constrained model predictive control (cc-MPC); model predictive contouring control cost function penalizing contouring deviation, lag error, control effort, and deviation from a reference speed	Chance-constraint enforced with deterministic nonlinear constraints; upper bounds on the probability of violating constraints using Cantelli’s inequality
[24]	Quintic polynomial for the lateral offset as function of longitudinal displacement; longitudinal position, lateral position, derivative, and the curvature of the lane curve to find the polynomial coefficients	Speed, acceleration, and jerk profiles are based on the quintic polynomial of the driving distance profile over time	Objective function of the constrained nonlinear optimization model containing acceleration and jerk of the ego vehicle, and the total driving time	Collision-avoidance constraints; maximum total time, speed, acceleration, and jerk; traffic state constraint
[25]	Sixth order time depending polynomial functions for the longitudinal and lateral positions	The speed kinematic model is represented by the time quadratic equation with acceleration and jerk; kinematic variables: longitudinal and lateral positions, accelerations, jerks	SQP algorithm to solve the nonlinear programming by minimizing the longitudinal and lateral accelerations and jerks, and time, the safety risk and discomfort for vehicle	Restricted distances between vehicles, constraints for the longitudinal and lateral speeds, accelerations, jerks, and horizon time
[26]	Three-dimensional spatio-temporal driving map; reference trajectory generation with a search-based method, followed by local refinement and smoothing	Vehicle states are: the lateral and longitudinal positions, velocity, and yaw angle; the action space includes possible acceleration and yaw angle	Time-invariant MPC extended by the reconstruction of convex feasible sets; objective function includes parameters: prediction horizon time, initial position, target destination, reference state of the spatio-temporal trajectory, control input	State constraints for each state; limits of control input; deviation of longitudinal velocity from the desired value with the limit of 20 m/s
[27]	Path is searched by ant colony algorithm according to the path enlightened by artificial potential field algorithm	Vehicle accelerates to reach speed of 80 km/h, deceleration to 40 km/h; vehicle travels at speed of 30, 50, and 100 km/h	Transfer probability function of ant colony algorithm, improved potential field algorithm; gravity model of safety lane change	Speed limits of front section 80 and 40 km/h;lateral acceleration changes from −0.15 g to 0.15 g

. The table shows that existing methods have several issues related to the following: (1) strictly satisfying the boundary conditions, (2) irregularity of trajectory segments, (3) ensuring the continuity of all parameters, (4) bypassing moving obstacles, and (5) focusing on fast numerical simulation. These issues are addressed in this study to strengthen prediction stability and will be discussed in more detail next.

The analysis of approaches to AV motion modeling is summarized in Table 1. The main aspects are focused on trajectory modeling, speed mode, the use of optimization methods, and imposed restrictions.

Conventionally, the work can be divided into using model predictive control (MPC) and state-space models [1], polynomial representation of trajectories [3,4,5,8,12], and methods using graph techniques to find the best trajectory [15,22]. To a lesser extent, attention is paid to restrictions, especially geometric ones in relation to the possible motion zone. Thus, Table 1 reflects the full range of approaches used today in the problems of autonomous vehicles.

The following important aspects need to be addressed and can be considered innovative.

Strictly Satisfying Boundary Conditions. When using a piecewise polynomial [17,19,25] representation of functions describing the planned geometric, kinematic, and dynamic AV characteristics, the function behavior within a roadway segment depends on the parameters specified at the nodal points. Different methods [2,11], including optimization, can determine these parameters. At the same time, the restrictions imposed on the nodal DOF ensure the permissible values of the functions at the nodes themselves but do not guarantee the stable behavior of the functions strictly within the desirable limits along a road section. This point is especially critical for the vehicle trajectory geometry, which means non-violation of the allowable external motion boundaries by a vehicle’s safety contour. Note that the positions of control points determining the safety contour are influenced not only by the position of the vehicle’s mass center but also by the yaw angle (angular displacement). Thus, the guarantee of non-violating the boundaries by the control points within a road section interval also contributes to ensuring the stability of predicting the vehicle admissible motion and the strict unambiguity of the steering control impact. Regarding the kinematic and dynamic parameters of the AV movement, the compliance of equality and inequality constraints in nodes is replaced by the conditions of integral equality constraints along the motion path. Thus, the boundary functions of sought parameters within segments can be represented as piecewise constant/linear or decomposed by the same FE-basis as the optimized parameters.

Irregularity of Trajectory Segments. When an AV is camera guided, the roadway curvature defines the natural limit of visibility/perceptivity of the three-dimensional space through the perspective projection. Typically, this is conditioned by a significant curvature change at the end of the sight distance. Additionally, the road section curvature does not remain constant within the visible distance. In connection with the above, the splitting of linear segments of the vehicle displacement should include the grid concentration in areas requiring an accurate description as well as in sites where static obstacles are located. In addition, the last segment must be allocated to compensate for the time needed to process the next forecast.

Ensuring the Continuity of all Parameters. With the piecewise polynomial representation, all the motion planning parameters are presented by the same basis of shape functions. This contributes to considering the continuity of higher derivatives of the motion parameters’ functions simultaneously with inflicting the integral restrictions. The basis functions derived based on a 7-degree Lagrange polynomial are used in this study framework. Such a solution ensures the continuity up to the third derivative at nodes and helps mitigate the discontinuity of parameters requiring higher than third derivatives (jerk, angular acceleration). This also makes it possible to coordinate the nature of the vehicle’s acceleration change with the specific properties of the propulsion system response.

Bypassing Moving Obstacles. A complete solution to avoiding moving obstacles [9,24] is possible only with the simultaneous optimization of AV motion in space and time, that is, when the trajectory and speed are distributed in parallel. However, it is evident that with such an approach, the number of nodal unknowns is doubled, leading to increasing variations and search time. Furthermore, when sequentially searching for the trajectory and speed to bypass a moving obstacle, complications arise related to splitting the maneuver into parts, allocating boundaries for them according to certain algorithms, and coordinating spatial constraints with the vehicle speed properties. This primarily occurs because the camera visibility zone limits the guaranteed maneuver.

Focusing on Fast Numerical Simulation. Since the study emphasizes the integral approach in representing constraints to improve the optimization quality, this may decrease the computational rapidity. However, the higher complexity of the nonlinear restrictions should not affect the optimization performance. In this regard, numerical methods are used to replace symbolic integration with a discrete summation. At the same time, a scheme of the numeric integration [3,11] must ensure the calculation accuracy and the smoothness of output functions.

This study’s purpose consists of developing a mathematical basis for representing nonlinear constraints in the form of integral equalities and a numerical integration technique for accelerating the optimization procedure and implementing the interpolation polynomials to ensure the smoothness of all optimized parameters.

2. Basics of Mathematical Tools

2.1. Representing Planning Parameters by Basis-Functions

Let us generalize the technique for obtaining the form functions based on Lagrange polynomials. Suppose a polynomial of extent p represents a function y(x). Then, it can be reflected in the matrix form

$$\mathit{A}=\left(\begin{array}{c}{c}_0\\ ⋮\\ c_p\end{array}\right),\hspace{1em}\hspace{1em}\mathit{X}=\left(\begin{array}{c}{x}^0\\ ⋮\\ x^p\end{array}\right),\hspace{1em}\hspace{1em}y\left(x\right)={\mathit{A}}^T\mathit{X}$$

(1)

where c_j = polynomial coefficient, j $\in$ [0, p].

Considering one FE of the length L, a function y(x) may be expressed by sets of shape functions and DOF values. Then

$$\mathit{Q}=\left(\begin{array}{c}{q}_1\\ ⋮\\ q_{p+1}\end{array}\right),\hspace{1em}\hspace{1em}\mathit{F}=\left(\begin{array}{c}{f}_1\\ ⋮\\ f_{p+1}\end{array}\right),\hspace{1em}\hspace{1em}y\left(x\right)={\mathit{Q}}^T\mathit{F}$$

(2)

where f_j = shape functions and q_j = weight coefficient or degree of freedom (DOF), j $\in$ [1, p + 1].

Let us define k = (p + 1)/2 − 1 first derivatives of the vector X provided that p is odd. Substituting the coordinates of the initial (0) and final (L) nodes, the matrix B is formed as

$$\mathit{B}=\left(\begin{array}{c}{\mathit{X}}^T\left(0\right)\\ ⋮\\ d^k{\mathit{X}}^T/d{x}^k\left(0\right)\\ {\mathit{X}}^T\left(L\right)\\ ⋮\\ d^k{\mathit{X}}^T/d{x}^k\left(L\right)\end{array}\right),\hspace{1em}\mathit{B}\mathit{A}=\mathit{Q},\hspace{1em}\mathit{A}={\mathit{B}}^{-1}\mathit{Q},\hspace{1em}\mathit{F}={\left({\mathit{B}}^T\right)}^{-1}\mathit{X}$$

(3)

As a result, using Equations (1)–(3), expressions are linked in the form

$$y\left(x\right)={\left({\mathit{B}}^{-1}\mathit{Q}\right)}^T\mathit{X}={\mathit{Q}}^T{\left({\mathit{B}}^T\right)}^{-1}\mathit{X}={\mathit{Q}}^T\mathit{F}$$

(4)

If we assume x = ξL, where parameter ξ $\in$ [0, 1], then after substituting it into B and excluding the length elements as multipliers, the shape functions take the form of basis functions for an element of unit length [28]. These functions for FE with four DOFs in a node (it is assumed that p = 7 and k = 3 in Equation (3)) corresponding to the derivative order and DOF are shown in Figure 1. As seen, the superposition of these functions can provide high accuracy of forecasts and boundary models.

Thus, the objective is to obtain the basis functions F_ξ of the argument ξ $\in$ [0, 1] that are universal for a variable length L of roadway segments. In this regard, the main integration can be reduced to the range [0, 1] of a function y(ξ). Denoting by the low index for i-th FE, let us define the vector l_i and matrix L_i

$${\mathit{l}}_i=\left(\begin{array}{ccc}{L}_i^0& \cdots & L_i^k\end{array}\right),\hspace{1em}{\mathit{L}}_i=diag\left(\begin{array}{cc}{\mathit{l}}_i& {\mathit{l}}_i\end{array}\right)$$

(5)

where k = (p + 1)/2 − 1.

Thus, for i-th FE, functions F of Equation (2) can be rewritten with basis functions of the parameter ξ

$${\mathit{F}}_i={\mathit{L}}_i{\mathit{F}}_{\xi },\hspace{1em}{y}_i\left(x\right)=y_i\left({\mathit{Q}}_i,L_i,\xi \right)={\mathit{Q}}_i^T{\mathit{L}}_i{\mathit{F}}_{\xi }$$

(6)

Additionally, for k-th derivative,

$$\frac{d^k{y}_i}{d{x}^k}=\frac{d}{L_{i}d\xi }\left(\frac{1}{L_i^{k-1}}{\mathit{Q}}_i^T{\mathit{L}}_i\frac{d^{k-1}{\mathit{F}}_{\xi }}{d{\xi }^{k-1}}\right)={\mathit{Q}}_i^T\left(\frac{{\mathit{L}}_i}{L_i^k}\right)\frac{d^k{\mathit{F}}_{\xi }}{d{\xi }^k}$$

(7)

Thus, the integral of an arbitrary function z_i(x) comprising y_i(x) or its derivatives within the interval [x_i−1, x_i], considering Equation (6), can be calculated as

$${{\displaystyle \int }}_{x_{i-1}}^{x_i}{z}_i\left(x\right)dx=L_i{{\displaystyle \int }}_0^1{z}_i\left({\mathit{Q}}_i,L_i,\xi \right)d\xi =L_i{{\displaystyle \int }}_0^1{z}_i\left(\xi \right)d\xi$$

(8)

2.2. One-Dimensional Quadrature Integral

Since the basis functions are built into the integration procedures, and the extent of derived interpolating polynomials is quite high, the problem of the computational speed of the iterative optimization scheme occurs. To solve it, numerical integration based on Gaussian schemes is used in this approach. Thus, the integral of Equation (8) for a function z_i(ξ) can be evaluated as

$${{\displaystyle \int }}_0^1{z}_i\left(\xi \right)d\xi ={{\displaystyle \sum }}_{k=1}^N{w}_k{z}_i\left(\xi \left({\lambda }_k\right)\right)det\left(J\left({\lambda }_k\right)\right)$$

(9)

where w_k = integration weight in the k-th point; λ_k—points in the master-element coordinate system; J = Jacobian, k $\in$ [1, N]; N = number of integration points.

Since for each FE the reduced length is the same and equals to 1, the Jacobian matrix for transiting to the master element and its determinant yield

$$J\left(\lambda \right)=\frac{\partial \left({\xi }_1,\dots ,{\xi }_N\right)}{\partial \left({\lambda }_1,\dots ,{\lambda }_N\right)},\hspace{1em}det\left(J\right)=\frac{{\xi }_1-{\xi }_0}{2}=\frac{1}{2}$$

(10)

The master element is defined on the segment [−1, 1], and the transition is conducted according to the rule

$$\xi \left({\lambda }_k\right)=\frac{\left({\xi }_1-{\xi }_0\right)}{2}\left({\lambda }_k+1\right)+{\xi }_0=\frac{1}{2}\left({\lambda }_k+1\right)$$

(11)

Let us represent the sets of integration points, weight coefficients, and function values as column vectors of length N

$$\mathit{\lambda }=\left(\begin{array}{c}{\lambda }_1\\ ⋮\\ {\lambda }_N\end{array}\right),\hspace{1em}\mathit{w}=\left(\begin{array}{c}{w}_1\\ ⋮\\ w_N\end{array}\right),\hspace{1em}\mathit{\xi }={\left(\begin{array}{c}{\xi }_1\\ ⋮\\ {\xi }_N\end{array}\right)}^T,\hspace{1em}{\mathit{z}}_i={\left(\begin{array}{c}{z}_{i1}\\ ⋮\\ z_{iN}\end{array}\right)}^T$$

(12)

Then, the integral of Equation (9) for i-th FE can be calculated as the scalar product of vectors

$${{\displaystyle \sum }}_{k=1}^N{w}_k{z}_i\left(\xi \left({\lambda }_k\right)\right)det\left(J\left({\lambda }_k\right)\right)=\frac{1}{2}{z}_i\left(\xi \left({\mathit{\lambda }}^T\right)\right)\mathit{w}=\frac{1}{2}{z}_i\left(\mathit{\xi }\right)\mathit{w}=\frac{1}{2}{\mathit{z}}_i\mathit{w}$$

(13)

Since, in the study, both the functional and constraints are to be integral over all n segments, their general scheme, owing to Equations (6) and (13), takes the vector-matrix form

$$I={{\displaystyle \sum }}_{i=1}^n{L}_i{{\displaystyle \int }}_0^1{z}_i\left(\xi \right)d\xi \approx \frac{1}{2}{{\displaystyle \sum }}_{i=1}^n{L}_i{\mathit{z}}_i\mathit{w}=\frac{1}{2}{\mathit{L}}_s\mathit{z}\mathit{w}$$

(14)

where L_s = vector of segment lengths; z = matrix of integrands with n × N size; and

$${\mathit{L}}_s={\left(\begin{array}{c}{L}_1\\ ⋮\\ L_n\end{array}\right)}^T,\hspace{1em}\mathit{z}=\left(\begin{array}{c}{\mathit{z}}_1\\ ⋮\\ {\mathit{z}}_n\end{array}\right)=\left(\begin{array}{ccc}{z}_{1,1}& \dots & z_{1,N}\\ ⋮& \ddots & ⋮\\ z_{n,1}& \dots & z_{n,N}\end{array}\right)$$

(15)

In this study, we use N = [3, …, 8] point schemes for comparing the quality and performance.

2.3. SQP Nonlinear Optimization

The sequential quadratic programming (SQP) optimization [3,5,7,11] algorithm is based on a quadratic approximation of the Lagrangian function. Its most valuable properties include the strict feasibility of founds, fast linear algebra routines, and good convergence.

Since any cost function S can be represented in the form of Equation (14) through a set q of nodal parameters, the generalized approach is written as

$$\underset{\mathit{q}}{\min }S\left(\mathit{q}\right)\hspace{1em}\mathrm{subject} \mathrm{to}\hspace{1em}\{\begin{array}{c}{\mathit{C}}_{eq}\left(\mathit{q}\right)=0\\ {\mathit{A}}_{eq}\mathit{q}={\mathit{b}}_{eq}\\ {\mathit{q}}_L\le \mathit{q}\le {\mathit{q}}_U\end{array},\hspace{1em}\mathit{q}=\left(\begin{array}{c}{\mathit{q}}_1\\ ⋮\\ {\mathit{q}}_{n+1}\end{array}\right),\hspace{1em}{\mathit{q}}_i=\left(\begin{array}{c}{q}_{d\left(i-1\right)+1}\\ ⋮\\ q_{d\left(i-1\right)+d}\end{array}\right)$$

(16)

where q = vector of nodal parameters to be optimized; C_eq(q) = vector function of nonlinear equality constraints; A_eq, b_eq = matrix and vector of equality linear constraints, respectively; q_L, q_U = lower and upper limits; i $\in$ [1, n] = segment number; d = (p + 1)/2 = DOF in a node.

Thus, the nonlinear inequality constraints are omitted since the main task is to replace them with integral functions and include them in the nonlinear equality constraints.

2.4. Integral Representation of Constraints

As noted in Section 1, most of the constraints are represented linearly and concern only nodal values of parameters. When a roadway section is divided into a pretty large number of segments with relatively small lengths, the high solution quality is ensured. However, by reducing the number of segments and increasing their lengths, which is especially important for roads with small curvature, the behavior of the sought functions within the intervals depends only on the constraints of the nodal parameters and the optimization function. Thus, increasing the guarantees on restricting the desirable solutions can be achieved by replacing the linear nodal constraints with nonlinear integral ones.

Consider the idea in more detail based on the example of ensuring the trace y(x) of the AV mass center within the upper y_U and y_L lower boundaries. As seen in Figure 2a, the nodes and the local extremum are located inside the bounds. The area enclosed between the functions of the upper and lower boundaries is precisely equal to the sum of the areas above (yellow) and below (blue) the function y(x). It is evident that if an FE grid step is proportional to the overall length of a single vehicle, the probability of violating the boundaries by a local extremum is relatively small, which makes it sufficient sometimes for using the linear inequality constraints. However, suppose an FE is much longer. In that case, a situation depicted in Figure 2b may occur when the nodal values formally satisfy the boundary conditions, and the local extremum surpasses the limits (red zone). In this case, the area between boundaries is less than the sum of integrals taken for modules of differences above and below the function y(x). This point can be used for enhancing the optimization.

Thus, the cumulative boundary condition on the section x $\in$ [0, D] divided into n finite elements can be written as

$${{\displaystyle \int }}_0^{\mathit{D}}\left(y_U\left(x\right)-y_L\left(x\right)\right)dx={{\displaystyle \int }}_0^{\mathit{D}}\left|\left(y_U\left(x\right)-y\left(x\right)\right)\right|dx+{{\displaystyle \int }}_0^{\mathit{D}}\left|\left(y\left(x\right)-y_L\left(x\right)\right)\right|dx$$

(17)

Now, let us track the formation of conditions for equality constraints from the point of view of the optimization procedure. Consider the problem statement for an arbitrary criterion h along the trajectory s bounded by the functions h_min and h_max in the same basis functions of Equation (6). If a function h is strictly within the limits of the functions h_min and h_max, then

$${{\displaystyle \int }}_0^{\mathit{s}}\left(h_{max}-h_{min}\right)ds={{\displaystyle \int }}_0^{\mathit{s}}\left|\left(h_{max}-h\right)\right|ds+{{\displaystyle \int }}_0^{\mathit{s}}\left|\left(h-h_{min}\right)\right|ds$$

(18)

The integral of Equation (18) can be split into sections. Then, for i-th FE, considering ds = $s_x^{\prime }$ dx

$${{\displaystyle \int }}_{x_{i-1}}^{x_i}\left(h_{max}-h_{min}\right)s_x^{\prime }dx={{\displaystyle \int }}_{x_{i-1}}^{x_i}\left|\left(h_{max}-h\right)\right|s_x^{\prime }dx+{{\displaystyle \int }}_{x_{i-1}}^{x_i}\left|\left(h-h_{min}\right)\right|s_x^{\prime }dx$$

(19)

Passing to the variable ξ, let us express the integrands in terms of certain functions z_i(ξ) of an optimized parameter, which in the general case depends on both the trajectory’s Q_yi and speed’s Q_vi DOF vectors of each i-th segment. Integral between upper and lower bounds:

$$\begin{gathered} z_{uli}\left(\xi \right)=\left(h_{max}\left({\mathit{Q}}_{yi},{\mathit{Q}}_{vi},L_i,\xi \right)-h_{min}\left({\mathit{Q}}_{yi},{\mathit{Q}}_{vi},L_i,\xi \right)\right)s_{xi}^{\prime }\left({\mathit{Q}}_{yi},L_i,\xi \right), \\ {\mathit{z}}_{uli}=z_{uli}\left({\mathit{Q}}_{yi},{\mathit{Q}}_{vi},L_i,\xi \left({\mathit{\lambda }}^T\right)\right),\hspace{1em}{\mathit{I}}_{ul}\approx \frac{1}{2}{\mathit{L}}_s{\mathit{z}}_{ul}\mathit{w} \end{gathered}$$

(20)

Integral between upper bound and criterion function:

$$\begin{gathered} z_{uhi}\left(\xi \right)=\left|h_{max}\left({\mathit{Q}}_{yi},{\mathit{Q}}_{vi},L_i,\xi \right)-h\left({\mathit{Q}}_{yi},{\mathit{Q}}_{vi},L_i,\xi \right)\right|s_{xi}^{\prime }\left({\mathit{Q}}_{yi},L_i,\xi \right), \\ {\mathit{z}}_{uhi}=z_{uhi}\left({\mathit{Q}}_{yi},{\mathit{Q}}_{vi},L_i,\xi \left({\mathit{\lambda }}^T\right)\right),\hspace{1em}{\mathit{I}}_{uh}\approx \frac{1}{2}{\mathit{L}}_s{\mathit{z}}_{uh}\mathit{w} \end{gathered}$$

(21)

Integral between the criterion function and lower bound:

$$\begin{gathered} z_{hli}\left(\xi \right)=\left|h\left({\mathit{Q}}_{yi},{\mathit{Q}}_{vi},L_i,\xi \right)-h_{min}\left({\mathit{Q}}_{yi},{\mathit{Q}}_{vi},L_i,\xi \right)\right|s_{xi}^{\prime }\left({\mathit{Q}}_{yi},L_i,\xi \right), \\ {\mathit{z}}_{hli}=z_{hli}\left({\mathit{Q}}_{yi},{\mathit{Q}}_{vi},L_i,\xi \left({\mathit{\lambda }}^T\right)\right),\hspace{1em}{\mathit{I}}_{hl}\approx \frac{1}{2}{\mathit{L}}_s{\mathit{z}}_{hl}\mathit{w} \end{gathered}$$

(22)

Thus, the requirement of nonlinear equality constraints along all the segments, according to Equation (16) is expressed as follows

$$c_{eq}={\mathit{I}}_{ul}-{\mathit{I}}_{uh}-{\mathit{I}}_{hl}=\frac{1}{2}{\mathit{L}}_s\left({\mathit{z}}_{ul}-{\mathit{z}}_{uh}-{\mathit{z}}_{hl}\right)\mathit{w}=0$$

(23)

Consequently, by combining (mainly summing) values of nonlinear equality constraints for a set of criteria, it is possible to form conditions for improving the optimization quality.

3. Technique of Planning Boundaries and Avoiding Obstacles

General Moments. Figure 3a shows a generalized motion planning scheme in conditions of external boundaries and moving obstacles. Let us assume that the AV is equipped with radar systems for scanning space and determining the speeds of moving objects and distances to them. Suppose also that the AV is guided by a computer vision technology allowing the recognition of road marking lines and identification of moving objects in online mode. However, the camera vision is limited by the road section’s distance and curvature. In the perception of an oncoming roadway through the perspective view, there is a moment when the road marking lines practically merge, and the accuracy of determining a destination point is abruptly reduced. In this regard, let us consider phased planning. Thus, in Figure 3a, the AV’s trajectories (red) are independently planned for phases between positions ①–② and ②–③. Let us also outline such an important aspect as the time needed for processing the visual camera information and for computing optimizations. Therefore, the forecast should last within a motion segment. Given this, the trajectories of the planned parts for subsequent phases must be seamlessly joined to ensure the smoothness between preceding and following forecasts. That is, the prediction must be built inside the points 0 and 1 (or what is the same—between points n and n + 1). Thus, if the motion tracking stage continues up to (and including) the last point (n + 1), a new prediction is based on the data set obtained in the previous position (n).

Number of Segments. As noted, by using basis functions composed of the 7-degree polynomial, there is an option for making an irregular FE mesh with relatively large segments in areas with relatively small curvature. However, some segments are mandatory to form the motion zone’s boundaries. Thus, segment 0–1 (L₀) corresponds to a forecast computation delay for the next road section; segment 1–2 (L₁) is usually associated with forming internal boundaries in the initial position 1; subsequent segments may also be assigned to include internal constraints; segment (L_b) is responsible for a sufficient space required to bypass obstacles with a minimum change in steered wheels’ angles; segment n provides a transition stage and serves to compensate the time spent for the next prediction procedure. Thus, the number of segments within one roadway section mainly depends on the obstacles.

External and Internal Boundaries. Road markings delineate the outer boundaries of space available for organizing the AV motion. Since the camera fixes these boundaries at the forecast beginning moment, they can be considered static. In particular, this method describes the boundaries by the basis functions through the nodal parameters. This is convenient because if the nodal parameters of the roadway’s upper and lower boundaries are calculated, it is easy to obtain the lane’s boundary marking lines (even if they are absent) using an arithmetic proportion and a road width. Note that some obstacles may cause a blind spot for identifying outer boundaries. Then, they can be virtually completed based on the available information about the visible parts and their analysis. Internal boundaries exclude the road space parts forbidden for motion, such as zone A in Figure 3a.

On the other hand, a part of space should be allocated for moving obstacles in such a way as to make the least influence on both their motion mode and safety. These zones (B and C) should be tied to the motion speeds relative to the bypassing AV, to road conditions (adhesion), and cannot be defined unambiguously, i.e., are dynamic.

Stages of Forming Obstacle Avoidance Boundaries. Ideally, the problem of avoiding obstacles is solved by simultaneously optimizing the trajectory and speed [26]. However, this significantly increases the number of nodal parameters’ combinations and the computation time, respectively. In this regard, with sequential optimization at the trajectory search step, it is impossible to explicitly include time to determine the boundaries of areas B and C. Therefore, it is necessary to proceed from some heuristic approaches considering the dynamic nature of safety zones for moving objects and allowing to avoid the explicit time factor. The restriction of zone B in the first planning section, when the distance d between the AV and the impeding (blue) vehicles in position ① is set, can be carried out by estimating the critical gap between the vehicles during emergency braking. At the same time, it is evident that the space reduction between the vehicles comes faster than during the AV acceleration. Suppose such a gap is accepted as a criterion depending on the difference of the vehicles’ initial speeds and tire adhesion value. In that case, it is possible to determine when zone B must be closed. Thus, in the first stage, the movement D under the angle ϕ to the AV longitudinal axis ζ is planned. By evaluating the segment’s length L_b, a decision concerning the safeness of performing the speed maneuver can be made. Suppose the boundaries critically constrain a passage within L_b. In that case, it is necessary to cancel the maneuver planning and wait for a convenient situation, continuing to move along the lane with reduced speed.

Initially, it is difficult to predict the vehicles’ mutual position in the position ② since the time costs can only be calculated after redistributing the AV speed. If the current section’s curvature changes slightly, then a forecast estimation of the impeding vehicle position may be built as well. Additionally, at point n, the position and speed of the impeding (blue) vehicle can be measured by using the AV sensory system. At this stage, it is necessary to determine a corridor for the possible lane change in the presence of two moving obstacles (blue and green vehicles). In the case of the green vehicle, the space restriction technique is equivalent to the blue vehicle restriction in the first phase ①–②. The issue of closing zone B for the blue vehicle is much more complicated. Since time is not explicitly present in trajectory planning, let us use the partially heuristic approach shown in Figure 3b. Suppose that when the vehicle is located at point n, the distance to the impeding (blue) vehicle is Δd (can be positive or negative). If we assume that the AV keeps its initial speed V_a, then for a specific time period t it will cover the distance P_a = V_a·t. During the same time, the blue vehicle, presumably also maintaining the speed V_i, will pass the distance P_i = V_i·t. At the same time, a certain distance Δs will be set between the vehicles, at which it is safe to start changing the lane. That is, neglecting the curvature in the area of the maneuver beginning can be written

$$\frac{P_a}{V_a}=\frac{P_i}{V_i},\hspace{1em}{P}_i=P_a\frac{V_i}{V_a},\hspace{1em}{P}_a=\Delta d+P_i+\Delta s+l_a$$

(24)

where l_a = length of AV safe contour.

Substituting for P_i, then

$$P_a=\Delta d+P_a\frac{V_i}{V_a}+\Delta s+l_a,\hspace{1em}{P}_a\left(1-\frac{V_i}{V_a}\right)=\Delta d+\Delta s+l_a,\hspace{1em}{P}_a=\frac{\Delta d+\Delta s+l_a}{1-V_i/V_a}$$

(25)

It follows from Equation (25) that if Δd < 0 and |Δd| = Δs + l_a, P_a = 0 and the lane change may be started. Note that if V_a = V_i, then P_a = ∞, meaning outrunning is impossible. Further, the distance P_a is also deviated by a certain angle relative to the x-axis; the larger the angle, the greater the curvature in this area. However, this angle is slightly less than ϕ. If we introduce the coefficient k_ϕ (≈0.8), then the point x_f can be estimated as

$$x_f\approx P_a\mathit{cos}\left(k_{\varphi }{\varphi }_{\left(0\right)}^{\left(2\right)}\right)$$

(26)

where ${\varphi }_{\left(0\right)}^{\left(2\right)}$ = angle between the new x-axis and ζ-axis in the point 0 of the second phase.

Then, the point x_b, considering that l_i is the length of the impeding vehicle save contour, can be approximately estimated as

$$x_b\approx x_f-\left(\Delta s+l_i/2\right)\mathit{cos}\left(k_{\varphi }{\varphi }_{\left(0\right)}^{\left(2\right)}\right)$$

(27)

The maneuver can be considered as possible if such a corridor L_b (Figure 3a) between the constrained areas B and C is provided, ensuring its high-speed passage without a significant impact of the AV steering system. The critical length of such a corridor is commensurate with several lengths of the AV itself. If such a corridor does not correspond to critical capabilities, the maneuver should be postponed, or another option accepted.

The next important issue concerns the transition between the stages’ coordinate systems at the points n → 0 and n + 1 → 1. Thus, the first phase trajectory on segment L_n should ideally coincide with the second phase trajectory on segment L₀. Obviously, the first phase parameters at nodes n, n + 1 will differ from the second phase’s parameters in the corresponding nodes 0, 1. Particular attention should be given to the first phase’s node n + 1, in which the continuity of the curvature and its derivatives must be ensured for a seamless transition to the new coordinate system of node 1. This issue will be considered in detail in the paragraph on forming initial conditions.

4. Modeling the AV Motion

Figure 4 shows the scheme of the vehicle kinematic model [10,13,23] for generating a trajectory of the mass center C. Three coordinate systems are placed at this point. First, the motion trajectory is considered relative to the absolute coordinate system xy. Longitudinal V_ζ and lateral V_μ velocities are considered in the local moving coordinate system ζμ turned on the angle ϕ relative to the absolute one. The tangential a_τ and normal a_n accelerations are decomposed in the natural coordinate system located at the angle β to the local or at the angle α to the global coordinate systems. The natural coordinate system rotates with an angular velocity Ω relative to the instantaneous center O with a radius R. Let us assume that the traction forces are realized by all the wheels.

Let us consider the primary theoretical basis that allows establishing strict mathematical relations between the geometric, kinematic, and dynamic parameters for AV motion planning. All the formulas are represented in the final view, omitting the deriving stages.

4.1. Basic Parameters of Trajectory Planning

If a trajectory function y(x) is explicitly determined by a set of nodal parameters q_y, it and its k-th derivative are evaluated according to Equations (6) and (7).

$$y_i\left(x\right)={\mathit{Q}}_{yi}^T{\mathit{L}}_i{\mathit{F}}_{\xi },\hspace{1em}\frac{d^k{y}_i}{d{x}^k}={\mathit{Q}}_{yi}^T\left(\frac{{\mathit{L}}_i}{L_i^k}\right)\frac{d^k{\mathit{F}}_{\xi }}{d{\xi }^k}$$

(28)

The elementary arc length gives

$$ds=\sqrt{1+{\left(dy/dx\right)}^2}dx=s_x^{\prime }dx$$

(29)

Considering $s_x^{\prime }$ as a variable, its first derivative concerning the coordinate x is

$$\frac{d{s}_x^{\prime }}{dx}=s_x^{″}=\frac{1}{s_x^{\prime }}\frac{dy}{dx}\frac{d^{2}y}{d{x}^2}$$

(30)

Second derivative of $s_x^{\prime }$ concerning x

$$\frac{d^2{s}_x^{\prime }}{d{x}^2}=\frac{d}{dx}\left(\frac{1}{s_x^{\prime }}\frac{dy}{dx}\frac{d^{2}y}{d{x}^2}\right)=\frac{1}{s_x^{\prime }}\left({\left(\frac{d^{2}y}{d{x}^2}\right)}^2+\frac{dy}{dx}\frac{d^{3}y}{d{x}^3}-{\left(\frac{d{s}_x^{\prime }}{dx}\right)}^2\right)$$

(31)

For the tangent angle, the trajectory derivative defines the tangent of the slope angle

$$dy/dx=tan\left(\alpha \left(x\right)\right)$$

(32)

Consequently, the slope angle is

$$\alpha \left(x\right)=\mathit{arctan}\left(dy/dx\right)$$

(33)

Let us consider the derivative of $\alpha \left(s\right)$ concerning the arc s

$$\frac{d\alpha }{ds}=\frac{d\alpha }{dx}\frac{dx}{ds}=\frac{d\alpha }{dx}/\frac{ds}{dx}=\frac{1}{s_x^{\prime }}\frac{d\alpha }{dx}$$

(34)

The first derivative by x

$$\frac{d\alpha }{dx}=\frac{d}{dx}\mathit{arctan}\left(\frac{dy}{dx}\right)=\frac{1}{{\left(s_x^{\prime }\right)}^2}\frac{d^{2}y}{d{x}^2}$$

(35)

The second derivative by x, expressing K to be the curvature

$$\frac{d^2\alpha }{d{x}^2}=\frac{d}{dx}\left(\frac{1}{{\left(s_x^{\prime }\right)}^2}\frac{d^{2}y}{d{x}^2}\right)=\frac{1}{{\left(s_x^{\prime }\right)}^2}\frac{d^{3}y}{d{x}^3}-2K\frac{d{s}_x^{\prime }}{dx}$$

(36)

The curvature is the rate change in the arc angle along the arc itself, i.e.,

$$K\left(s\right)=\frac{d\alpha }{ds},\hspace{1em}K\left(x\right)=\frac{1}{s_x^{\prime }}\frac{d\alpha }{dx}=\frac{1}{{\left(s_x^{\prime }\right)}^3}\frac{d^{2}y}{d{x}^2}$$

(37)

In turn, the curvature changes along the coordinate x

$$\frac{dK}{dx}=\frac{d}{dx}\left(\frac{1}{{\left(s_x^{\prime }\right)}^3}\frac{d^{2}y}{d{x}^2}\right)=\frac{1}{s_x^{\prime }}\left(\frac{1}{{\left(s_x^{\prime }\right)}^2}\frac{d^{3}y}{d{x}^3}-3K\frac{d{s}_x^{\prime }}{dx}\right)=\frac{1}{s_x^{\prime }}\left(\frac{d^2\alpha }{d{x}^2}-K\frac{d{s}_x^{\prime }}{dx}\right)$$

(38)

Second derivative concerning x

$$\frac{d^{2}K}{d{x}^2}=\frac{d}{dx}\left(\frac{dK}{dx}\right)=\frac{1}{s_x^{\prime }}\left(\frac{1}{{\left(s_x^{\prime }\right)}^2}\frac{d^{4}y}{d{x}^4}-\left(\frac{2}{{\left(s_x^{\prime }\right)}^3}\frac{d^{3}y}{d{x}^3}+4\frac{dK}{dx}\right)\frac{d{s}_x^{\prime }}{dx}-3K\frac{d^2{s}_x^{\prime }}{d{x}^2}\right)$$

(39)

The instantaneous radius is derived as the curvature’s inverse. Its sign, consequently, shows the rotational direction around the instant center O (Figure 4).

$$R=1/K$$

(40)

The central slip angle β shows the relation between the longitudinal and lateral components of the mass center velocity and can be estimated based on the ideal turn geometry

$$\beta =\mathit{arcsin}\left(b/R\right)=\mathit{arcsin}\left(bK\right)$$

(41)

The derivative of angle β concerning x, introducing the coefficient k_β

$$\frac{d\beta }{dx}=\frac{d}{dx}\left(\mathit{arcsin}\left(bK\right)\right)=k_{\beta }\frac{dK}{dx},\hspace{1em}{k}_{\beta }=\frac{b}{\sqrt{1-{\left(bK\right)}^2}}$$

(42)

The derivative of k_β relative to x is given by

$$\frac{d{k}_{\beta }}{dx}=\frac{d}{dx}\left(\frac{b}{\sqrt{1-{\left(bK\right)}^2}}\right)=k_{\beta }^{3}K\frac{dK}{dx}$$

(43)

The second derivative of β concerning x, using Equations (42) and (43)

$$\frac{d^2\beta }{d{x}^2}=k_{\beta }\left(K{\left(\frac{d\beta }{dx}\right)}^2+\frac{d^{2}K}{d{x}^2}\right)$$

(44)

The yaw angle φ, in contrast to the vehicle models with elastic tires, can be expressed purely geometrically as the difference between the angles of tangent slope and central slip (Figure 4)

$$\varphi =\alpha -\beta \hspace{1em}\mathrm{and}\hspace{1em}\frac{d\varphi }{dx}=\frac{d\alpha }{dx}-\frac{d\beta }{dx}$$

(45)

Thus, the first and second trajectory’s derivatives affect the yaw angle ϕ.

4.2. Basic Parameters of Kinematics Planning

The longitudinal speed V_ζ according to Equations (6) and (7) includes the nodal parameters q_v inherited by the V_ζ derivatives for using in accelerations and jerks. Deriving the first and second derivatives concerning time t, obtain

$$\frac{d{V}_{\zeta }}{dt}=\frac{d{V}_{\zeta }}{dx}\frac{dx}{dt}=\frac{d{V}_{\zeta }}{dx}{V}_x,\hspace{1em}\frac{d^2{V}_{\zeta }}{d{t}^2}=\frac{d^2{V}_{\zeta }}{d{x}^2}{V}_x^2+\frac{d{V}_{\zeta }}{dx}\frac{d{V}_x}{dx}{V}_x$$

(46)

where d^kV_ζ/dx^k is defined by Equation (7), V_x = projection of absolute speed V on the global x-axis.

If the function V_ζ(x) is explicitly determined by a set of nodal parameters q_v, it and its k-th derivative are evaluated according to Equations (6) and (7).

$$V_{\zeta i}\left(x\right)={\mathit{Q}}_{vi}^T{\mathit{L}}_i{\mathit{F}}_{\xi },\hspace{1em}\frac{d^k{V}_{\zeta i}}{d{x}^k}={\mathit{Q}}_{vi}^T\left(\frac{{\mathit{L}}_i}{L_i^k}\right)\frac{d^k{\mathit{F}}_{\xi }}{d{\xi }^k}$$

(47)

The mass-center velocity can be represented through decompositions in the natural, local and global, coordinate systems (Figure 4).

$$\overrightarrow{V}={\left(\begin{array}{c}V\\ 0\end{array}\right)}^T\left(\begin{array}{c}\overrightarrow{\tau }\\ \overrightarrow{\nu }\end{array}\right),\hspace{1em}\overrightarrow{V}={\left(\begin{array}{c}{V}_{\zeta }\\ V_{\mu }\end{array}\right)}^T\left(\begin{array}{c}{\overrightarrow{u}}_{\zeta }\\ {\overrightarrow{u}}_{\mu }\end{array}\right),\hspace{1em}\overrightarrow{V}={\left(\begin{array}{c}{V}_x\\ V_y\end{array}\right)}^T\left(\begin{array}{c}{\overrightarrow{u}}_x\\ {\overrightarrow{u}}_y\end{array}\right)$$

(48)

where $\overrightarrow{\tau }$, $\overrightarrow{\nu }$ = basis vectors of the natural coordinate system; ${\overrightarrow{u}}_{\zeta }$, ${\overrightarrow{u}}_{\mu }$ = unit vectors of the vehicle local coordinate system; ${\overrightarrow{u}}_x$, ${\overrightarrow{u}}_y$ = unit vectors of the fixed (global) coordinate system xy.

If M is the matrix of plane rotation, the transitions between the coordinate systems can be expressed as follows

$$M(·)=\left(\begin{array}{cc}\mathit{cos}(·)& \mathit{sin}(·)\\ -\mathit{sin}(·)& \mathit{cos}(·)\end{array}\right),\hspace{1em}\hspace{1em}\left(\begin{array}{c}\overrightarrow{\tau }\\ \overrightarrow{\nu }\end{array}\right)=M\left(\beta \right)\left(\begin{array}{c}{\overrightarrow{u}}_{\zeta }\\ {\overrightarrow{u}}_{\mu }\end{array}\right),\hspace{1em}\hspace{1em}\left(\begin{array}{c}{\overrightarrow{u}}_{\zeta }\\ {\overrightarrow{u}}_{\mu }\end{array}\right)=M\left(\varphi \right)\left(\begin{array}{c}{\overrightarrow{u}}_x\\ {\overrightarrow{u}}_y\end{array}\right)$$

(49)

Correspondingly, for velocities

$$\left(\begin{array}{c}{V}_x\\ V_y\end{array}\right)=M^T\left(\varphi \right)\left(\begin{array}{c}{V}_{\zeta }\\ V_{\mu }\end{array}\right)$$

(50)

The absolute speed is determined as the relation of elementary arc length to elementary time increment

$$V=\frac{ds}{dt}=\frac{ds}{dx}\frac{dx}{dt}=s_x^{\prime }{V}_x=\frac{V_x}{cos\left(\alpha \right)}=\frac{V_{\zeta }}{\mathit{cos}\left(\beta \right)}$$

(51)

where V_x = projection of absolute speed on the x-axis.

The projection of absolute speed unavoidably occurs in all kinematic parameters to replace the x-coordinate derivative concerning time t. The longitudinal velocity V_ζ in the local coordinates and the projection V_x are tied by the dependency

$$V_x=V_{\zeta }\frac{\mathit{cos}\left(\alpha \right)}{\mathit{cos}\left(\beta \right)}$$

(52)

The first derivative concerning the coordinate x is

$$\frac{d{V}_x}{dx}=\left(\frac{d{V}_{\zeta }}{dx}+V_{\zeta }\left(\frac{d\beta }{dx}\mathit{tan}\left(\beta \right)-\frac{d\alpha }{dx}\mathit{tan}\left(\alpha \right)\right)\right)\frac{\mathit{cos}\left(\alpha \right)}{\mathit{cos}\left(\beta \right)}$$

(53)

The lateral speed, considering the ideal turn of the vehicle kinematic model (Figure 4), is expressed by the longitudinal component V_ζ

$$V_{\mu }=V_{\zeta }\mathit{tan}\left(\beta \right)$$

(54)

The derivative concerning time gives

$$\frac{d{V}_{\mu }}{dt}=\frac{d{V}_{\mu }}{dx}\frac{dx}{dt}=\frac{d{V}_{\mu }}{dx}{V}_x$$

(55)

where

$$\frac{d{V}_{\mu }}{dx}=\frac{d{V}_{\zeta }}{dx}\mathit{tan}\left(\beta \right)+\frac{V_{\zeta }}{{\mathit{cos}}^2\left(\beta \right)}\frac{d\beta }{dx}$$

(56)

The yaw rate can be evaluated as the derivation of yaw angle φ, Equation (45), in the current global coordinates. Thus,

$$\omega =\frac{d\varphi }{dt}=\frac{d}{dt}\left(\alpha -\beta \right)=\left(\frac{d\alpha }{dx}-\frac{d\beta }{dx}\right)V_x$$

(57)

Since the global coordinate systems differ only by a constant angle of mutual disposition, this does not affect the yaw rate value ω (Figure 4).

The derivative component with respect to the coordinate x is

$$\frac{d\omega }{dx}=\left(\frac{d^2\alpha }{d{x}^2}-\frac{d^2\beta }{d{x}^2}\right)V_x+\frac{d{V}_x}{dx}\left(\frac{d\alpha }{dx}-\frac{d\beta }{dx}\right)$$

(58)

The angular acceleration ε is the derivative of the yaw rate ω concerning time

$$\epsilon =\frac{d\omega }{dt}=\frac{d\omega }{ds}\frac{ds}{dt}=\frac{d\omega }{dx}/\frac{ds}{dx}V=\left(\frac{d^2\alpha }{d{x}^2}-\frac{d^2\beta }{d{x}^2}\right)V_x^2+\frac{d{V}_x}{dx}\omega$$

(59)

The longitudinal and lateral accelerations in the local vehicle coordinate system ζμ can be derived from Equation (48) via decomposition components

$$\overrightarrow{a}=\frac{d\overrightarrow{V}}{dt}={\left(\begin{array}{c}{a}_{\zeta }\\ a_{\mu }\end{array}\right)}^T\left(\begin{array}{c}{\overrightarrow{u}}_{\zeta }\\ {\overrightarrow{u}}_{\mu }\end{array}\right), \hspace{1em}(\begin{array}{c}{a}_{\zeta }\\ a_{\mu }\end{array})=\left(\begin{array}{c}\frac{d{V}_{\zeta }}{dx}{V}_x-\omega V_{\mu }\\ \frac{d{V}_{\mu }}{dx}{V}_x+\omega V_{\zeta }\end{array}\right)$$

(60)

The longitudinal jerk [14,25] characterizes the acceleration change, impacting the dynamics transient. This parameter comprises all other kinematic criteria and can be used for insuring the smoothness of non-stationary motion. Differentiating Equation (60) and taking the longitudinal component, obtain

$$j_{\zeta }=\frac{d^2{V}_{\zeta }}{d{t}^2}-\left(2\frac{d{V}_{\mu }}{dt}+V_{\zeta }\omega \right)\omega -V_{\mu }\epsilon$$

(61)

4.3. Optimization Criteria

Let us consider the formation of objective functions that provide the search for the best approximations of the desirable trajectory (steering control) and velocity distribution functions.

4.3.1. Trajectory Cost Function

This stage aims to determine the vector of nodal parameters q_y corresponding to Equation (28). Since the nodes of internal FEs are adjacent, the values in them are the same. This leads to the need to form a complete set of DOFs for each i-th FE in which the vehicle motion section consists. Then

$${\mathit{Q}}_{yi}=\left(\begin{array}{c}{\mathit{q}}_{y\left(i\right)}\\ {\mathit{q}}_{y\left(i+1\right)}\end{array}\right)$$

(62)

Path Length. This characterizes the travelling distance that influences the total energy consumption. It can be calculated as the integral of elementary arcs.

$$I_s={{\displaystyle \int }}_0^{s}ds={{\displaystyle \int }}_0^D{s}_x^{\prime }dx={{\displaystyle \sum }}_{i=1}^n{L}_i{{\displaystyle \int }}_0^1{z}_{si}\left(\xi \right)d\xi$$

(63)

According to the scheme of Equation (8), the integral is transformed to the view of Equations (14) and (15), where the function z_si($\xi$) and the final form of the integral are expressed as

$$z_{si}\left(\xi \right)=s_{xi}^{\prime }\left({\mathit{Q}}_{yi},L_i,\xi \right),\hspace{1em}{\mathit{z}}_{si}=z_{si}\left({\mathit{Q}}_{yi},L_i,\xi \left({\mathit{\lambda }}^T\right)\right),\hspace{1em}{I}_s\approx \frac{1}{2}{\mathit{L}}_s{\mathit{z}}_s\mathit{w}$$

(64)

In practical calculations, it is better to use a specific parameter that reflects the deviation of the trajectory length from the section one, then

$$I_D=I_s/\mathit{D}$$

(65)

Slip Angle Deviation. The angle β is associated with the mass center’s lateral velocity component as well as with the current curvature and, consequently, the position of vehicle’s steered wheels. Minimizing this angle in the quadratic sense along the section D provides the least activation of the steering control, which positively affects the motion stability and the possibility of increasing the course speed. Then,

$$I_{\beta }={{\displaystyle \int }}_0^{\mathit{D}}{\left(\beta \left(x\right)\right)}^{2}dx={{\displaystyle \sum }}_{i=1}^n{L}_i{{\displaystyle \int }}_0^1{z}_{\beta i}\left(\xi \right)d\xi$$

(66)

Expressing the integrand z_βi($\xi$) according to the scheme Equations (14)–(16), get

$$z_{\beta i}\left(\xi \right)={\beta }^2\left({\mathit{Q}}_{yi},L_i,\xi \right),\hspace{1em}{\mathit{z}}_{\beta i}=z_{\beta i}\left({\mathit{Q}}_{yi},L_i,\xi \left({\mathit{\lambda }}^T\right)\right), I_{\beta }\approx \frac{1}{2}{\mathit{L}}_s{\mathit{z}}_{\beta }\mathit{w}$$

(67)

Curvature Rate. This reflects the intensity of changing the curvature component and also affects the yaw speed and other kinematic parameters (acceleration, jerk) containing it. Thus, the maximum smoothness of the trajectory nature should be ensured. Then,

$$I_{dK}={{\displaystyle \int }}_0^{\mathit{D}}{\left(\frac{dK}{dx}\right)}^{2}dx={{\displaystyle \sum }}_{i=1}^n{L}_i{{\displaystyle \int }}_0^1{z}_{dKi}\left(\xi \right)d\xi$$

(68)

Using the integrand z_dKi($\xi$) according to the scheme Equations (14)–(16), obtain

$$\begin{gathered} z_{dKi}\left(\xi \right)={\left(\frac{dK}{dx}\left({\mathit{Q}}_{yi},L_i,\xi \right)\right)}^2, {\mathit{z}}_{dKi}=z_{dKi}\left({\mathit{Q}}_{yi},L_i,\xi \left({\mathit{\lambda }}^T\right)\right), \\ {I}_{dK}\approx \frac{1}{2}{\mathit{L}}_s{\mathit{z}}_{dK}\mathit{w} \end{gathered}$$

(69)

Deviation of Curvature’s Second Order Derivative. This reflects the intensity of curvature rate that affect the vehicle dynamics. From this point of view, the DOFs to be optimized are selected in such a way as to reduce the transient processes when controlling the AV, which also simplifies the task of complex control. Then

$$I_{d2K}={{\displaystyle \int }}_0^{\mathit{D}}{\left(\frac{d^{2}K}{d{x}^2}\right)}^{2}dx={{\displaystyle \sum }}_{i=1}^n{L}_i{{\displaystyle \int }}_0^1{z}_{d2Ki}\left(\xi \right)d\xi$$

(70)

Denoting the integrand z_d2Ki($\xi$) and following the scheme of Equations (14)–(16), get

$$\begin{gathered} z_{d2Ki}\left(\xi \right)={\left(\frac{d^{2}K}{d{x}^2}\left({\mathit{Q}}_{yi},L_i,\xi \right)\right)}^2,\hspace{1em}{\mathit{z}}_{d2Ki}=z_{d2Ki}\left({\mathit{Q}}_{yi},L_i,\xi \left({\mathit{\lambda }}^T\right)\right), \\ {I}_{d2K}\approx \frac{1}{2}{\mathit{L}}_s{\mathit{z}}_{d2K}\mathit{w} \end{gathered}$$

(71)

Deviation of Fourth Order Derivative. This derivative is an element of the second curvature derivative, however, in Equation (3) it is not included in the nodal parameter. As a result, this leads to discontinuity of this parameter at the bounds of adjacent FEs. To mitigate this deficiency as much as possible, it is necessary to add this parameter in the objective function. Then

$$I_{d4y}={{\displaystyle \int }}_0^{\mathit{D}}{\left(\frac{d^{4}y}{d{x}^4}\right)}^{2}dx={{\displaystyle \sum }}_{i=1}^n{L}_i{{\displaystyle \int }}_0^1{z}_{d4yi}\left(\xi \right)d\xi$$

(72)

Denoting the integrand z_d4yi($\xi$) and following the scheme of Equations (14)–(16), obtain

$$\begin{gathered} z_{d4yi}\left(\xi \right)={\left(\frac{d^{4}y}{d{x}^4}\left({\mathit{Q}}_{yi},L_i,\xi \right)\right)}^2,\hspace{1em}{\mathit{z}}_{d4yi}=z_{d4yi}\left({\mathit{Q}}_{yi},L_i,\xi \left({\mathit{\lambda }}^T\right)\right), \\ {I}_{d4y}\approx \frac{1}{2}{\mathit{L}}_s{\mathit{z}}_{d4y}\mathit{w} \end{gathered}$$

(73)

Minimization. The cost function S_y can be represented as a sum of criteria weighed by proportional coefficients W_y and written in the matrix form.

$$S_y\left({\mathit{q}}_y\right)={\mathit{W}}_y^T{\mathit{I}}_y\left({\mathit{q}}_y\right)\to min$$

(74)

where q_y = vector of variable nodal parameters corresponding to Equation (16); I_y = vector of criteria integrals; W_y = vector of weighting coefficients.

$${\mathit{W}}_y=\left(\begin{array}{c}{W}_D\\ W_{\beta }\\ W_{dK}\\ W_{d2K}\\ W_{d4y}\end{array}\right),\hspace{1em}{\mathit{I}}_y\left({\mathit{q}}_y\right)=\left(\begin{array}{c}{I}_D\\ I_{\beta }\\ I_{dK}\\ I_{d2K}\\ I_{d4y}\end{array}\right)$$

(75)

where W_D, W_β, W_dK, W_d2K, W_d4y = weighting factors of the path, slip angle, curvature first and second derivatives, and fourth order y derivative, respectively.

4.3.2. Kinematics Cost Function

Considering that the vector of nodal parameters q_y is already found, the next step implies determining the vector q_v. By analogy with Equation (62)

$${\mathit{Q}}_{vi}=\left(\begin{array}{c}{\mathit{q}}_{v\left(i\right)}\\ {\mathit{q}}_{v\left(i+1\right)}\end{array}\right)$$

(76)

Travel time is needed to form the discrete time series for motion tracking task. In lesser degree this criterion is suitable for optimization since it does not correspond to the quadratic form of the objective function and asymptotically decreases to t_s with increasing speed.

$$I_t={{\displaystyle \int }}_0^{t_s}dt={{\displaystyle \int }}_0^{\mathit{s}}\frac{ds}{V\left(s\right)}={{\displaystyle \int }}_0^{\mathit{D}}\frac{s_x^{\prime }dx}{s_x^{\prime }{V}_x}={{\displaystyle \int }}_0^{\mathit{D}}\frac{dx}{V_x\left(x\right)}={{\displaystyle \sum }}_{i=1}^n{L}_i{{\displaystyle \int }}_0^1{z}_{ti}\left(\xi \right)d\xi$$

(77)

Let us use the integrand z_ti($\xi$) according to the scheme Equations (14)–(16), then

$$z_{ti}\left(\xi \right)=\frac{1}{V_x\left({\mathit{Q}}_{yi},{\mathit{Q}}_{vi},L_i,\xi \right)},\hspace{1em}{\mathit{z}}_{ti}=z_{ti}\left({\mathit{Q}}_{yi},{\mathit{Q}}_{vi},L_i,\xi \left({\mathit{\lambda }}^T\right)\right),\hspace{1em}{I}_t\approx \frac{1}{2}{\mathit{L}}_s{\mathit{z}}_t\mathit{w}$$

(78)

Longitudinal speed can be redistributed relative to a preset level V_ζmax to provide the minimum deflection in the quadratic sense. Then

$$I_v={{\displaystyle \int }}_0^{\mathit{s}}{\left(V_{\zeta max}-V_{\zeta }\right)}^{2}ds={{\displaystyle \int }}_0^{\mathit{D}}{\left(V_{\zeta max}-V_{\zeta }\right)}^2{s}_x^{\prime }dx={{\displaystyle \sum }}_{i=1}^n{L}_i{{\displaystyle \int }}_0^1{z}_{vi}\left(\xi \right)d\xi$$

(79)

Denoting the integrand z_vi($\xi$) and following the scheme of Equations (14)–(16), obtain

$$\begin{gathered} z_{vi}\left(\xi \right)={\left(V_{\zeta max}-V_{\zeta }\left({\mathit{Q}}_{vi},L_i,\xi \right)\right)}^2{s}_x^{\prime }\left({\mathit{Q}}_{yi},L_i,\xi \right), \\ {\mathit{z}}_{vi}=z_{vi}\left({\mathit{Q}}_{yi},{\mathit{Q}}_{vi},L_i,\xi \left({\mathit{\lambda }}^T\right)\right),\hspace{1em}{I}_v\approx \frac{1}{2}{\mathit{L}}_s{\mathit{z}}_v\mathit{w} \end{gathered}$$

(80)

The longitudinal acceleration criterion helps to even its deflection and decrease power consumption during changing AV speed. Thus,

$$I_{a\zeta }={{\displaystyle \int }}_0^{\mathit{s}}{a}_{\zeta }^{2}ds={{\displaystyle \int }}_0^{\mathit{D}}{a}_{\zeta }^2{s}_x^{\prime }dx={{\displaystyle \sum }}_{i=1}^n{L}_i{{\displaystyle \int }}_0^1{z}_{a\zeta i}\left(\xi \right)d\xi$$

(81)

Determine the integrand z_aζi($\xi$), then, according to the scheme of Equations (14)–(16), obtain

$$\begin{gathered} z_{a\zeta i}\left(\xi \right)={\left(a_{\zeta }\left({\mathit{Q}}_{yi},{\mathit{Q}}_{vi},L_i,\xi \right)\right)}^2{s}_x^{\prime }\left({\mathit{Q}}_{yi},L_i,\xi \right), \\ {\mathit{z}}_{a\zeta i}=z_{a\zeta i}\left({\mathit{Q}}_{yi},{\mathit{Q}}_{vi},L_i,\xi \left({\mathit{\lambda }}^T\right)\right),\hspace{1em}{I}_{a\zeta }\approx \frac{1}{2}{\mathit{L}}_s{\mathit{z}}_{a_{\zeta }}\mathit{w} \end{gathered}$$

(82)

The lateral acceleration criterion is responsible for motion stability and safety. It characterizes the lateral tire reactions. Thus,

$$I_{a\mu }={{\displaystyle \int }}_0^{\mathit{s}}{a}_{\mu }^{2}ds={{\displaystyle \int }}_0^{\mathit{D}}{a}_{\mu }^2{s}_x^{\prime }dx={{\displaystyle \sum }}_{i=1}^n{L}_i{{\displaystyle \int }}_0^1{z}_{a\mu i}\left(\xi \right)d\xi$$

(83)

Using the integrand z_aμi($\xi$), and expressions of Equations (14)–(16), obtain

$$\begin{gathered} z_{a\mu i}\left(\xi \right)={\left(a_{\mu }\left({\mathit{Q}}_{yi},{\mathit{Q}}_{vi},L_i,\xi \right)\right)}^2{s}_x^{\prime }\left({\mathit{Q}}_{yi},L_i,\xi \right), \\ {\mathit{z}}_{a\mu i}=z_{a\mu i}\left({\mathit{Q}}_{yi},{\mathit{Q}}_{vi},L_i,\xi \left({\mathit{\lambda }}^T\right)\right),\hspace{1em}{I}_{a\mu }\approx \frac{1}{2}{\mathit{L}}_s{\mathit{z}}_{a\mu }\mathit{w} \end{gathered}$$

(84)

The longitudinal jerk [9] criterion provides a smooth acceleration during transient dynamics, ensuring the propulsion system’s stable and predictable operation. Then,

$$I_{j\zeta }={{\displaystyle \int }}_0^{\mathit{s}}{j}_{\zeta }^{2}ds={{\displaystyle \int }}_0^{\mathit{D}}{j}_{\zeta }^2{s}_x^{\prime }dx={{\displaystyle \sum }}_{i=1}^n{L}_i{{\displaystyle \int }}_0^1{z}_{j\zeta i}\left(\xi \right)d\xi$$

(85)

Determine the integrand z_jζi($\xi$), then, according to the scheme of Equations (14)–(16), obtain

$$\begin{gathered} z_{j\zeta i}\left(\xi \right)={\left(j_{\zeta }\left({\mathit{Q}}_{yi},{\mathit{Q}}_{vi},L_i,\xi \right)\right)}^2{s}_x^{\prime }\left({\mathit{Q}}_{yi},L_i,\xi \right), \\ {\mathit{z}}_{j\zeta i}=z_{j\zeta i}\left({\mathit{Q}}_{yi},{\mathit{Q}}_{vi},L_i,\xi \left({\mathit{\lambda }}^T\right)\right),\hspace{1em}{I}_{j\zeta }\approx \frac{1}{2}{\mathit{L}}_s{\mathit{z}}_{j\zeta }\mathit{w} \end{gathered}$$

(86)

Minimization. The function S_v to be optimized is represented as a linear combination in the matrix form

$$S_v\left({\mathit{q}}_v\right)={\mathit{W}}_v^T{\mathit{I}}_v\left({\mathit{q}}_v\right)\to min$$

(87)

where q_v = vector of unknown node parameters, Equation (60), I_v = vector of integrals, W_v = vector of weighting coefficients.

$${\mathit{W}}_v=\left(\begin{array}{c}{W}_v\\ W_{a\zeta }\\ \begin{array}{c}{W}_{a\mu }\\ W_{j\zeta }\end{array}\end{array}\right),\hspace{1em}\hspace{1em}{\mathit{I}}_v\left({\mathit{q}}_v\right)=\left(\begin{array}{c}{I}_v\left({\mathit{q}}_y,{\mathit{q}}_v\right)\\ I_{a\zeta }\left({\mathit{q}}_y,{\mathit{q}}_v\right)\\ \begin{array}{c}{I}_{a\mu }\left({\mathit{q}}_y,{\mathit{q}}_v\right)\\ I_{j\zeta }\left({\mathit{q}}_y,{\mathit{q}}_v\right)\end{array}\end{array}\right)$$

(88)

where W_v, W_aζ, W_aμ, W_jζ = weighting factors for the speed, longitudinal and lateral accelerations, and longitudinal jerk, respectively.

4.4. Integral Equality Constraints

Using the technique of Equations (17)–(22), let us note the parameters for which it is expedient to introduce integral restrictions. All separately generated vectors of equality constraints will be represented by a single vector to match the shape in Equation (16).

4.4.1. Trajectory Restrictions

Curvature. The trajectory curvature should not exceed the limits K_max and K_min conditioned by the maximum turn angles of the steered wheels. That is, for i-th FE

$${{\displaystyle \int }}_{x_{i-1}}^{x_i}\left(K_{max}-K_{min}\right)dx={{\displaystyle \int }}_{x_{i-1}}^{x_i}\left|\left(K_{max}-K\right)\right|dx+{{\displaystyle \int }}_{x_{i-1}}^{x_i}\left|\left(K-K_{min}\right)\right|dx$$

(89)

Considering K_max to be a constant and K_min = −K_max, according to Equations (17)–(22)

$$\begin{gathered} z_{uli}\left(\xi \right)=\left(K_{max}-K_{min}\right), \\ {z}_{uhi}\left(\xi \right)=\left|K_{max}-K\left({\mathit{Q}}_{yi},L_i,\xi \right)\right|, \\ {z}_{hli}\left(\xi \right)=\left|K\left({\mathit{Q}}_{yi},L_i,\xi \right)-K_{min}\right| \end{gathered}$$

(90)

Other Options. According to a similar scheme, the parameters defined for trajectory optimization may also be used (curvature’s higher derivatives). However, the number of restrictions, in this case, may become redundant in relation to the quality of optimization. Some of these parameters are better to be considered in the kinematic constraints by adjusting the smoothness of the speed change.

4.4.2. Physical Restrictions

First of all, despite using the vehicle kinematic model, the speed plan should be linked to the possibilities of realizing the tire–road adhesion. Thus, if the lateral adhesion coefficient ϕ_ζ is already set by the traction mode, the maximum lateral forces are limited by the potential ϕ_μ [29].

$${\phi }_{\mu }={\phi }_{max}\sqrt{1-{\left({\phi }_{\zeta }/{\phi }_{max}\right)}^2}$$

(91)

where ϕ_max = maximum adhesion value.

Thus, even with minimum traction use, the curvature and motion speed must be such that ϕ_μ < ϕ_max. Since the maximum vehicle traction is realized on the wheels and associated with the acceleration nature, the ϕ_ζ will be limited simultaneously by the adhesion limit and the powertrain system potential.

The equation of longitudinal dynamics in dimensionless form, where ϕ_ζ reflects the degree of using the traction potential on the wheels necessary to ensure a_ζ, can be written as

$${\phi }_{\zeta }=a_{\zeta }/g+f_d+f_r$$

(92)

where f_r = coefficient of total rolling resistance; f_d = specific drag force; g = gravitational acceleration constant.

$$f_d=F_d/\left(mg\right)={\rho }_a{C}_x{A}_f/\left(2mg\right)$$

(93)

where F_d = drag force; m = vehicle gross mass; ρ_a = air density under the normal conditions; C_x = aerodynamic drag coefficient, and A_f = frontal (projective) vehicle square.

Assuming that the curvature for high-speed motion is relatively small and the steered wheels’ turn angles differ little from β, the longitudinal component f_r of rolling resistance in the vehicle’s local coordinates can be estimated as follows

$$f_r\approx \left(\left(F_{r1}+F_{r3}\right)\mathit{cos}\left(\beta \right)+F_{r2}+F_{r4}\right)/\left(mg\right)$$

(94)

where F_r1, F_r2, F_r3, F_r4 = forces of rolling resistance on wheels.

For each wheel, considering their local coordinates, the rolling resistance is proportional to the vertical reaction, then

$$F_{rk}=R_{zk}{f}_{rk}$$

(95)

where R_zk = vertical wheel load (may be accepted as mg/4 for simplicity); f_rk = k-th wheel rolling resistance coefficient.

Each wheel moves with its speed (Figure 4), then

$$f_{rk}=q_{sy1}+q_{sy3}\left|V_{\zeta k}\right|+q_{sy4}{\left(V_{\zeta k}/V_m\right)}^4$$

(96)

V_ζk = longitudinal speed in k-th wheel coordinate system, V_m = speed at which the empirical measurements were made; q_sy1, q_sy3, q_sy4 = coefficients [30].

Consider the component of the centrifugal force acting in the vehicle transverse plane and balanced by the sum of the tires’ side reactions

$$m{V}^{2}K\mathit{cos}\left(\beta \right)=mg{\phi }_{\mu }$$

(97)

Then, the critical value of the mass center absolute velocity, according to the road adhesion condition, gives

$$V_{cr}=\sqrt{\frac{g{\phi }_{\mu }}{K\mathit{cos}\left(\beta \right)}}$$

(98)

Since the longitudinal speed is related to the absolute one by expression Equation (51), the critical value V_ζs by the sliding condition yields

$$V_{\zeta s}=V_s=\sqrt{g{\phi }_{\mu }\mathit{cos}\left(\beta \right)/K}$$

(99)

4.4.3. Kinematic Restrictions

Speed. The predicted speed can be determined within certain limits [12], which in the general case can also be functions of nodal parameters. However, within the nearest prediction horizon, the easiest way is to set the upper V_ζmax and lower V_ζmin limits to constant values. Here, for brevity, we write only the main line, followed by applying the scheme of Equations (17)–(22). For i-th FE

$${{\displaystyle \int }}_{x_{i-1}}^{x_i}\left(V_{max}-V_{min}\right)s_{xi}^{\prime }dx={{\displaystyle \int }}_{x_{i-1}}^{x_i}\left|V_{\zeta max}-V_{\zeta }\right|s_{xi}^{\prime }dx+{{\displaystyle \int }}_{x_{i-1}}^{x_i}\left|V_{\zeta }-V_{\zeta min}\right|s_{xi}^{\prime }dx$$

(100)

However, with a substantially curvilinear motion, it may turn out that V_ζs < V_ζmax. Therefore, one more condition is necessary to ensure the requirement V_ζ < V_ζs. Obviously, for zero curvature, V_ζs from Equation (51) becomes infinite, and it’s impossible to use it directly in integral equalities. In this regard, let us use a function ψ(ν) = arctan(ν) that rapidly saturates with increasing speed. Thus

$${{\displaystyle \int }}_{x_{i-1}}^{x_i}\psi \left(V_{\zeta s}\right)s_{xi}^{\prime }dx={{\displaystyle \int }}_{x_{i-1}}^{x_i}\left|\psi \left(V_{\zeta s}\right)-\psi \left(V_{\zeta }\right)\right|s_{xi}^{\prime }dx+{{\displaystyle \int }}_{x_{i-1}}^{x_i}\psi \left(V_{\zeta }\right)s_{xi}^{\prime }dx$$

(101)

According to Equations (17)–(22)

$$\begin{gathered} z_{uli}\left(\xi \right)=arctan\left(V_{\zeta s}\left({\mathit{Q}}_{vi},L_i,\xi \right)\right)s_{xi}^{\prime }\left({\mathit{Q}}_{yi},L_i,\xi \right), \\ {z}_{uhi}\left(\xi \right)=\left|arctan\left(V_{\zeta s}\left({\mathit{Q}}_{vi},L_i,\xi \right)\right)-arctan\left(V_{\zeta }\left({\mathit{Q}}_{vi},L_i,\xi \right)\right)\right|s_{xi}^{\prime }\left({\mathit{Q}}_{yi},L_i,\xi \right), \\ {z}_{hli}\left(\xi \right)=arctan\left(V_{\zeta }\left({\mathit{Q}}_{vi},L_i,\xi \right)\right)s_{xi}^{\prime }\left({\mathit{Q}}_{yi},L_i,\xi \right) \end{gathered}$$

(102)

Yaw Rate. As known, the vehicle is equipped with a gyroscope for estimating the yaw rate [12], and therefore, it should also be restricted when planning. This is due to the provision of directional stability of the vehicle. Thus, with symmetrical limits ω_min = −ω_max obtain

$${{\displaystyle \int }}_{x_{i-1}}^{x_i}\left({\omega }_{max}-{\omega }_{min}\right)s_{xi}^{\prime }dx={{\displaystyle \int }}_{x_{i-1}}^{x_i}\left|{\omega }_{max}-\omega \right|s_{xi}^{\prime }dx+{{\displaystyle \int }}_{x_{i-1}}^{x_i}\left|\omega -{\omega }_{min}\right|s_{xi}^{\prime }dx$$

(103)

Angular Acceleration. This characterizes the ratio of the yaw rate intensity and the rotational velocity of the steered wheels. Similar to ω, for ε the integral criterion has a view

$${{\displaystyle \int }}_{x_{i-1}}^{x_i}\left({\epsilon }_{max}-{\epsilon }_{min}\right)s_x^{\prime }dx={{\displaystyle \int }}_{x_{i-1}}^{x_i}\left|{\epsilon }_{max}-\epsilon \right|s_x^{\prime }dx+{{\displaystyle \int }}_{x_{i-1}}^{x_i}\left|\epsilon -{\epsilon }_{min}\right|s_x^{\prime }dx$$

(104)

Longitudinal Jerk. This may be restricted to mitigate and provide the reliable transients of powertrain modes. Supposing the upper j_ζmax and lower j_ζmin limits to be constant, obtain

$${{\displaystyle \int }}_{x_{i-1}}^{x_i}\left(j_{\zeta max}-j_{\zeta min}\right)s_x^{\prime }dx={{\displaystyle \int }}_{x_{i-1}}^{x_i}\left|j_{\zeta max}-j_{\zeta }\right|s_x^{\prime }dx+{{\displaystyle \int }}_{x_{i-1}}^{x_i}\left|j_{\zeta }-j_{\zeta min}\right|s_x^{\prime }dx$$

(105)

4.4.4. Dynamic Restrictions

When planning the dynamic mode of AV motion, it is necessary to consider both the vehicle powertrain potential and the ability to realize traction on wheels according to the road adhesion condition. In this case, the acceleration limit determined by adhesion and distributed along the path s can be estimated as a_ζmax = ϕ_ζmaxg and a_ζmin = −a_ζmax (Figure 5a). At the same time, the acceleration provided by the propulsion system under normal road conditions is a function of the longitudinal speed and is limited by the throttle response characteristic [5] for the case of maximum fuel supply (Figure 5b). In turn, the upper limit a_ζU, which determines the maximum vehicle performance, will change stepwise following the number of automatic transmission gears, and the lower limit a_ζL can be set based on certain expediency for a particular maneuver. Thus, the distribution of accelerations must simultaneously correspond to the two approaches in Figure 5. For example, (a) if the road adhesion is low, then a_ζmax < a_ζU, (b) if road condition is good, then a_ζmax > a_ζU, (c) if the use of the braking system is not planned, then it can be assumed that a_ζL ≥ −0.5 m/s², and (d) if intensive braking is required, then a threshold value within a_ζmin ≤ a_ζL ≤ −0.5 m/s² may be set.

However, there is a problem with composing the integral constraints in Figure 5b. The nodal parameters of acceleration distribution are determined relative to the path nodes. Therefore, it is impossible to obtain an explicit form of dependency where the speed grows strictly in one direction. In this regard, it is proposed to use an interpolation approach. The values of speed and acceleration for each segment of a path section can be determined at several points, followed by sorting in ascending order of speeds. Using the new polynomial representation of both the current accelerations and the boundaries, it is possible to use again the numerical integration schemes corresponding to N points and equivalent to Equation (9). Of course, this will lead to errors in representing the curves’ shapes but ensures that the vehicle powertrain system’s capabilities will not be exceeded for the corresponding gear.

4.4.5. Boundary Restrictions

The most critical and complex problem of the vehicle motion geometry is providing such traces of control points [1, …, 6] (Figure 6) that retain their trajectories strictly within the motion zone boundaries [1,4]. Thus, the approach in Figure 2 can also be applied to the trajectories of the safe contour control points. These points are chosen in such a way as to cover the curvature of the borders within the half of vehicle’s length, which is sufficient for maneuvers on the roadway.

Points 5 and 6 are sliding. Their abscissas coincide with the mass center abscissa, that is, x_(p) = x, where p = 5, 6, and the ordinates, respectively,

$$y_{\left(p\right)}=y+r_{\mu \left(p\right)}/\mathit{cos}\left(\varphi \right)$$

(106)

where r_μ(p) = ±Y/2 = transversal coordinate of point p in the vehicle coordinate system.

For external points, p = [1, 2, 3, 4], the effect of the AV yaw angle is significant. Then

$$y_{\left(p\right)}=y+r_{\left(p\right)}\mathit{sin}\left({\varphi }_{\left(p\right)}+\varphi \right)\hspace{1em}\mathrm{and}\hspace{1em}{x}_{\left(p\right)}=x+r_{\left(p\right)}\mathit{cos}\left({\varphi }_{\left(p\right)}+\varphi \right)$$

(107)

where x, y, φ = current values of the mass center position and the vehicle yaw angle in the grid nodes; r_(p), φ_(p) = radius module and angle of the critical point p location in the vehicle coordinate system.

Unlike the trajectories of points 5 and 6 that can be directly included in the integration scheme, control points [1, 2, 3, 4] are moving within segment lengths L_i(p) influenced by the yaw angle. In this case, the integration can be organized within the FE’s internal coordinate if considering the functions of control points’ trajectories and boundaries as shifted and stretched under the yaw angle influence. This will provide dependencies on only one variable x and, therefore, the possibility of applying the same general integration scheme. However, if the values of y_(p)-ordinates of the control points can be easily found according to Equation (107), determining the boundaries’ values at the points x_(p) is complicated by the fact that x_(p) exceeds a segment limit in which the integration is performed. Thus, the problem is reduced to determining the corresponding parameters ξ in the local and adjacent segments to use their nodal parameters in calculating the ordinates of the upper y_Ui(p) and lower y_Li(p) boundaries.

$${\xi }_{i\left(p\right)}=\left(x_{i\left(p\right)}-x_i\right)/L_i$$

(108)

The obtained values can be within the following limits stipulating the point position in i-th segment. Note that ξ_i(p) is a vector with a length corresponding to many integration points. Thus, it is assumed that the procedure is carried out for each its member.

$$\{\begin{array}{c}0>{\xi }_{i\left(p\right)}\ge -1 \Rightarrow i-1 \Rightarrow {\xi }_{i-1\left(p\right)}=\left(x_{i-1\left(p\right)}-x_{i-1}\right)/L_{i-1} \\ 0<{\xi }_{i\left(p\right)}\le 1 \Rightarrow i \Rightarrow {\xi }_{i\left(p\right)}=\left(x_{i\left(p\right)}-x_i\right)/L_i\\ 1<{\xi }_{i\left(p\right)}\le 2 \Rightarrow i+1 \Rightarrow {\xi }_{i\left(p\right)}=\left(x_{i+1\left(p\right)}-x_{i+1}\right)/L_{i+1}\end{array}$$

(109)

Thus, calculating the boundaries’ ordinates is performed by considering which segment the integration point belongs to. Then, for each point p = [1, 2, 3, 4]

$$\begin{gathered} z_{ULi\left(p\right)}\left(\xi \right)=\left(y_{Ui\left(p\right)}\left({\mathit{Q}}_{yi},L_i,\xi \right)-y_{Li\left(p\right)}\left({\mathit{Q}}_{yi},L_i,\xi \right)\right), \\ {z}_{Uhi\left(p\right)}\left(\xi \right)=\left|y_{Ui\left(p\right)}\left({\mathit{Q}}_{yi},L_i,\xi \right)-y_{i\left(p\right)}\left({\mathit{Q}}_{yi},L_i,\xi \right)\right|, \\ {z}_{hLi\left(p\right)}\left(\xi \right)=\left|y_{i\left(p\right)}\left({\mathit{Q}}_{yi},L_i,\xi \right)-y_{Li\left(p\right)}\left({\mathit{Q}}_{yi},L_i,\xi \right)\right| \end{gathered}$$

(110)

4.5. Coordination of Transients between Phases

As noted, when changing global coordinates for different movement phases, it is necessary to ensure the coordination of geometric and speed parameters in the transition node. Therefore, let us reflect in parentheses a parameter’s phase number by the upper index and several transition points by the lower index.

4.5.1. Geometric Transition Conditions

The task is to determine the vector of nodal parameters for node 1 in the new coordinate system based on the vector of parameters of the same node (n + 1) in the previous coordinate system (Figure 2). That is

$${\mathit{q}}_{y\left(1\right)}^{\left(2\right)}={\left(\begin{array}{ccc}{y}_{\left(1\right)}^{\left(2\right)}& {\frac{dy}{dx}|}_1^{\left(2\right)}& \begin{array}{cc}{\frac{d^{2}y}{d{x}^2}|}_1^{\left(2\right)}& {\frac{d^{3}y}{d{x}^3}|}_1^{\left(2\right)}\end{array}\end{array}\right)}^T$$

(111)

If AV in the node n of the first phase fixes by a camera the road boundaries for the second phase, then the new global coordinate system is estimated at the angle $-{\varphi }_{\left(0\right)}^{\left(2\right)}$ for the longitudinal axis ζ of the local AV coordinate system, which corresponds to the AV location under the angle ${\varphi }_{\left(0\right)}^{\left(2\right)}$ relative to the longitudinal x-axis of the new global coordinate system. The global coordinate systems are located at the angle Δφ for each other

$$\mathsf{\Delta }\varphi ={\varphi }_{\left(n\right)}^{\left(1\right)}-{\varphi }_{\left(0\right)}^{\left(2\right)}$$

(112)

The coordinates of point 1 in the new coordinate system, considering Equation (49)

$$\left(\begin{array}{c}{x}_{\left(1\right)}^{\left(2\right)}\\ y_{\left(1\right)}^{\left(2\right)}\end{array}\right)=M^T\left(\mathsf{\Delta }\varphi \right)\left(\begin{array}{c}\mathsf{\Delta }{x}_{\left(n+1\right)}^{\left(1\right)}\\ \Delta y_{\left(n+1\right)}^{\left(1\right)}\end{array}\right),\hspace{1em}\left(\begin{array}{c}\mathsf{\Delta }{x}_{\left(n+1\right)}^{\left(1\right)}\\ \Delta y_{\left(n+1\right)}^{\left(1\right)}\end{array}\right)=\left(\begin{array}{c}{x}_{\left(n+1\right)}^{\left(1\right)}\\ y_{\left(n+1\right)}^{\left(1\right)}\end{array}\right)-\left(\begin{array}{c}{x}_{\left(n\right)}^{\left(1\right)}\\ y_{\left(n\right)}^{\left(1\right)}\end{array}\right)$$

(113)

The first derivative in the node 1 can be determined according to Equation (32)

$${\frac{dy}{dx}|}_{\left(1\right)}^{\left(2\right)}=\mathit{tan}\left({\alpha }_{\left(1\right)}^{\left(2\right)}\right)$$

(114)

Note that the angle β does not depend on the coordinate system, and therefore

$${\beta }_{\left(n+1\right)}^{\left(1\right)}={\beta }_{\left(1\right)}^{\left(2\right)}$$

(115)

Then, considering Equation (45)

$${\beta }_{\left(n+1\right)}^{\left(1\right)}={\alpha }_{\left(n+1\right)}^{\left(1\right)}-{\varphi }_{\left(n+1\right)}^{\left(1\right)},\hspace{1em}{\beta }_{\left(1\right)}^{\left(2\right)}={\alpha }_{\left(1\right)}^{\left(2\right)}-{\varphi }_{\left(1\right)}^{\left(2\right)}$$

(116)

In its turn

$${\alpha }_{\left(n+1\right)}^{\left(1\right)}-{\varphi }_{\left(n+1\right)}^{\left(1\right)}={\alpha }_{\left(1\right)}^{\left(2\right)}-{\varphi }_{\left(1\right)}^{\left(2\right)},\hspace{1em}{\alpha }_{\left(1\right)}^{\left(2\right)}={\alpha }_{\left(n+1\right)}^{\left(1\right)}-{\varphi }_{\left(n+1\right)}^{\left(1\right)}+{\varphi }_{\left(1\right)}^{\left(2\right)}$$

(117)

Considering Equation (112) the yaw angle at point 1 becomes

$${\varphi }_{\left(1\right)}^{\left(2\right)}={\varphi }_{\left(n+1\right)}^{\left(1\right)}-\mathsf{\Delta }\varphi ={\varphi }_{\left(n+1\right)}^{\left(1\right)}-{\varphi }_{\left(n\right)}^{\left(1\right)}+{\varphi }_{\left(0\right)}^{\left(2\right)}$$

(118)

Then, substituting Equation (118) in Equation (117), obtain

$${\alpha }_{\left(1\right)}^{\left(2\right)}={\alpha }_{\left(n+1\right)}^{\left(1\right)}-{\varphi }_{\left(n\right)}^{\left(1\right)}+{\varphi }_{\left(0\right)}^{\left(2\right)}={\alpha }_{\left(n+1\right)}^{\left(1\right)}-\mathsf{\Delta }\varphi$$

(119)

Finally, Equation (114) becomes

$${\frac{dy}{dx}|}_{\left(1\right)}^{\left(2\right)}=\mathit{tan}\left({\alpha }_{\left(n+1\right)}^{\left(1\right)}-\mathsf{\Delta }\varphi \right)$$

(120)

The second derivative at node 1 can be determined under the condition that the curvature at the transition point is equal.

$$K_{\left(n+1\right)}^{\left(1\right)}=K_{\left(1\right)}^{\left(2\right)}$$

(121)

The curvature value at the point (n + 1) is already calculated, then, calculating the value s’_x from Equation (29), in accordance with Equation (37), obtain

$${\frac{d^{2}y}{d{x}^2}|}_{\left(1\right)}^{\left(2\right)}=K_{\left(n+1\right)}^{\left(1\right)}{\left({s_x^{\prime }|}_{\left(1\right)}^{\left(2\right)}\right)}^3$$

(122)

The third derivative in node 1 can be determined from the condition of equality of curvature derivatives along the path s at the transition point. Considering Equation (38)

$$\frac{dK}{ds}=\frac{dK}{dx}\frac{dx}{ds}=\frac{1}{s_x^{\prime }}\frac{dK}{dx}=\frac{1}{{\left(s_x^{\prime }\right)}^2}\left(\frac{1}{{\left(s_x^{\prime }\right)}^2}\frac{d^{3}y}{d{x}^3}-3K\frac{d{s}_x^{\prime }}{dx}\right),\hspace{1em}{\frac{dK}{ds}|}_{\left(n+1\right)}^{\left(1\right)}={\frac{dK}{ds}|}_{\left(1\right)}^{\left(2\right)}$$

(123)

Then, considering Equation (30) and already calculated components

$${\frac{d^{3}y}{d{x}^3}|}_{\left(1\right)}^{\left(2\right)}=\left({\frac{dK}{ds}|}_{\left(n+1\right)}^{\left(1\right)}{\left({s_x^{\prime }|}_{\left(1\right)}^{\left(2\right)}\right)}^2+3K_{\left(1\right)}^{\left(2\right)}{\frac{d{s}_x^{\prime }}{dx}|}_{\left(1\right)}^{\left(2\right)}\right){\left({s_x^{\prime }|}_{\left(1\right)}^{\left(2\right)}\right)}^2$$

(124)

4.5.2. Kinematic Transition Conditions

As in the previous case, the task is to determine the vector of nodal speed parameters in the node 1 of the new coordinate system based on the parameter vector in the same node (n + 1) of the previous coordinate system (Figure 2). That is

$${\mathit{q}}_{v\left(1\right)}^{\left(2\right)}={\left(\begin{array}{ccc}{V}_{\zeta \left(1\right)}^{\left(2\right)}& {\frac{d{V}_{\zeta }}{dx}|}_{\left(1\right)}^{\left(2\right)}& \begin{array}{cc}{\frac{d^2{V}_{\zeta }}{d{x}^2}|}_{\left(1\right)}^{\left(2\right)}& {\frac{d^3{V}_{\zeta }}{d{x}^3}|}_{\left(1\right)}^{\left(2\right)}\end{array}\end{array}\right)}^T$$

(125)

When coordinating the nodal parameters in node 1 (n + 1) of the transition between planning phases, it is necessary to ensure that the following conditions are met (Figure 2b). Equality of longitudinal speeds:

$$V_{\zeta \left(1\right)}^{\left(2\right)}=V_{\zeta \left(n+1\right)}^{\left(1\right)}$$

(126)

The first derivative in different coordinate systems can be estimated from the continuity of the relative longitudinal acceleration in the local AV coordinate system, following from Equation (46). Then, one can equate

$${\frac{d{V}_{\zeta }}{dt}|}_{\left(1\right)}^{\left(2\right)}={\frac{d{V}_{\zeta }}{dt}|}_{\left(n+1\right)}^{\left(1\right)}$$

(127)

Since the value for the previous phase at node (n + 1) is already known, using the formula of Equation (52) for the new V_x, obtain

$${\frac{d{V}_{\zeta }}{dx}|}_{\left(1\right)}^{\left(2\right)}={\frac{d{V}_{\zeta }}{dt}|}_{\left(n+1\right)}^{\left(1\right)}/V_{x\left(1\right)}^{\left(2\right)},\hspace{1em}{V}_{x\left(1\right)}^{\left(2\right)}=V_{\zeta \left(1\right)}^{\left(2\right)}\frac{\mathit{cos}\left({\alpha }_{\left(1\right)}^{\left(2\right)}\right)}{\mathit{cos}\left({\beta }_{\left(1\right)}^{\left(2\right)}\right)}$$

(128)

The second derivative can be found in the same way based on the equalities of the second derivatives concerning the time of the longitudinal velocity V_ζ at the point of phase change according to Equation (46).

$${\frac{d^2{V}_{\zeta }}{d{t}^2}|}_{\left(1\right)}^{\left(2\right)}={\frac{d^2{V}_{\zeta }}{d{t}^2}|}_{\left(n+1\right)}^{\left(1\right)}$$

(129)

Then, considering Equation (53), obtain

$${\frac{d^2{V}_{\zeta }}{d{x}^2}|}_{\left(1\right)}^{\left(2\right)}=\left({\frac{d^2{V}_{\zeta }}{d{t}^2}|}_{\left(n+1\right)}^{\left(1\right)}-{\frac{d{V}_{\zeta }}{dx}|}_{\left(1\right)}^{\left(2\right)}{\frac{d{V}_x}{dx}|}_{\left(1\right)}^{\left(2\right)}{V}_{x\left(1\right)}^{\left(2\right)}\right)/{\left(V_{x\left(1\right)}^{\left(2\right)}\right)}^2$$

(130)

The third derivative in the transition node can be found by considering the equalities of the corresponding derivative concerning the time

$${\frac{d^3{V}_{\zeta }}{d{t}^3}|}_{\left(1\right)}^{\left(2\right)}={\frac{d^3{V}_{\zeta }}{d{t}^3}|}_{\left(n+1\right)}^{\left(1\right)}$$

(131)

Differentiating Equation (46), obtain

$$\frac{d^3{V}_{\zeta }}{d{t}^3}=\frac{d^3{V}_{\zeta }}{d{x}^3}{V}_x^3+3\frac{d^2{V}_{\zeta }}{d{x}^2}\frac{d{V}_x}{dx}{V}_x^2+\frac{d{V}_{\zeta }}{dx}\left(\frac{d^2{V}_x}{d{x}^2}{V}_x^2+{\left(\frac{d{V}_x}{dx}\right)}^2{V}_x\right)$$

(132)

Wherefrom

$$\frac{d^3{V}_{\zeta }}{d{x}^3}=\left(\frac{d}{dt}\left(\frac{d^2{V}_{\zeta }}{d{t}^2}\right)-3\frac{d^2{V}_{\zeta }}{d{x}^2}\frac{d{V}_x}{dx}{V}_x^2-\frac{d{V}_{\zeta }}{dx}\left(\frac{d^2{V}_x}{d{x}^2}{V}_x^2+{\left(\frac{d{V}_x}{dx}\right)}^2{V}_x\right)\right)/V_x^3$$

(133)

Differentiating Equation (53), obtain

$$\begin{gathered} \frac{d^2{V}_x}{d{x}^2}=\left(\frac{d{V}_{\zeta }}{dx}\frac{\mathit{cos}\left(\alpha \right)}{\mathit{cos}\left(\beta \right)}+\frac{d{V}_x}{dx}\right)\left(\frac{d\beta }{dx}\mathit{tan}\left(\beta \right)-\frac{d\alpha }{dx}\mathit{tan}\left(\alpha \right)\right) \\ +\left(\frac{d^2{V}_{\zeta }}{d{x}^2}+V_{\zeta }\left(\frac{d^2\beta }{d{x}^2}\mathit{tan}\left(\beta \right)-\frac{d^2\alpha }{d{x}^2}\mathit{tan}\left(\alpha \right)+{\left(\frac{d\beta }{dx}\right)}^2\frac{1}{{\mathit{cos}}^2\left(\beta \right)}-{\left(\frac{d\alpha }{dx}\right)}^2\frac{1}{{\mathit{cos}}^2\left(\alpha \right)}\right)\right)\frac{\mathit{cos}\left(\alpha \right)}{\mathit{cos}\left(\beta \right)} \end{gathered}$$

(134)

Carrying out manipulations similar to the previous paragraph with the known parameters from the previous phase, the last component for phase matching in speed can be determined

$${\frac{d^3{V}_{\zeta }}{d{x}^3}|}_{\left(1\right)}^{\left(2\right)}$$

4.5.3. Linear Equality Constraints

The task is to form vectors for predicting the trajectory and speed, determining the preset parameters at the initial and final nodes. According to Equation (16), these parameters are associated with the solution vector by matrices A_eq. Note that parameters can be assigned arbitrarily for the first node of the first phase, and transition values are calculated for subsequent phases as described in the previous paragraph.

Boundary conditions of the trajectory. Usually, at the initial node of the first phase, its position, derivative, and curvature with its derivatives are considered to be known. In this regard, let us preset all DOFs in node 1. At the same time, at the last node (n + 1), let us leave the DOFs self-determining except for the curve tangent slope, due to which the curve stability is preserved, but the rigidity of conditions is reduced. In this way

$${\mathit{b}}_{eq}={\left(\begin{array}{ccc}{y}_{\left(1\right)}^{\left(1\right)}& {\frac{dy}{dx}|}_{\left(1\right)}^{\left(1\right)}& \begin{array}{cc}{\frac{d^{2}y}{d{x}^2}|}_{\left(1\right)}^{\left(1\right)}& \begin{array}{cc}{\frac{d^{3}y}{d{x}^3}|}_{\left(1\right)}^{\left(1\right)}& {\frac{dy}{dx}|}_{\left(n+1\right)}^{\left(1\right)}\end{array}\end{array}\end{array}\right)}^T$$

(135)

The first four elements for the starting node can be obtained from Equations (32), (37) and (38), and subsequent values correspond to the vector of Equation (111). The fifth element is obtained by averaging the slope angles at the (n + 1) nodes of the upper and lower boundaries.

The matrix A_eq, in this case, is a rectangular matrix of dimension 5 × 4(n + 1), where all elements are equal to 0, except for

$${\mathit{A}}_{eq\left(1,1\right)}={\mathit{A}}_{eq\left(2,2\right)}={\mathit{A}}_{eq\left(3,3\right)}={\mathit{A}}_{eq\left(4,4\right)}={\mathit{A}}_{eq\left(5,4n+2\right)}=1$$

(136)

Boundary conditions for speed. A vector of five elements can also represent the conditions for external speed nodes. For the phases following the first, the initial four elements are defined as transitional based on the requirements of Equation (125). The fifth element characterizes the change in speed along the x-coordinate in the last node and can be determined from the relation of Equation (46), setting the change in velocity over time in the local AV coordinates. The remaining DOFs at the last node are determined by the optimization,

$${\mathit{b}}_{eq}={\left(\begin{array}{ccc}{V}_{\zeta \left(1\right)}^{\left(1\right)}& {\frac{d{V}_{\zeta }}{dx}|}_{\left(1\right)}^{\left(1\right)}& \begin{array}{cc}{\frac{d^2{V}_{\zeta }}{d{x}^2}|}_{\left(1\right)}^{\left(1\right)}& \begin{array}{cc}{\frac{d^3{V}_{\zeta }}{d{x}^3}|}_{\left(1\right)}^{\left(1\right)}& {\frac{d{V}_{\zeta }}{dx}|}_{\left(n+1\right)}^{\left(1\right)}\end{array}\end{array}\end{array}\right)}^T$$

(137)

In the case of the first node of the initial phase, the definition of velocity derivatives can be determined from the conditions established by Equations (46), (60), (61) and (133), setting the initial speed, acceleration, and jerk. The matrix A_eq remains the same as for the trajectory case.

5. Simulation Example

Consider an example of a two-stage prediction of the AV curvilinear motion with bypassing moving obstacles. Let us use the Audi A4 Quattro’s data as vehicle model parameters. The set of required initial data is summarized in

Table 2. Data for simulating the AV motion prediction.

Parameter	Value	Parameter	Value	Parameter	Value
c, [m]	1.43	m, [kg]	1960	ρa, [kg/m3]	1.225
b, [m]	1.37	ϕmax	0.8	Cx	0.24
Y, [m]	1.551	Vζmax, [km/h]	85	Af, [m2]	2.04
|rζk|, [m]	2.5	Vζ (0), [km/h]	60	D, [m]	67.98/76.8
|rμk|, [m]	1.2	n	6	d, [m]	17.5/ 21.5

. All calculations are performed on the authors’ program composed in the MATLAB [31] environment. The “fmincon” [31] function of nonlinear optimization with nonlinear constraints is used for the minimization procedure.

5.1. Trajectory Searching

As specified in Equation (75), let us form a vector of weight coefficients that determine the same nature of optimization for all stages. Then

$${\mathit{W}}_y={\left(\begin{array}{ccc}0.01& 1& \begin{array}{cc}0.01& \begin{array}{cc}0.01& 3\end{array}\end{array}\end{array}\right)}^T$$

(138)

Here, the used values are explained as follows. The first coefficient is highly sensitive, but at the same time the minimum path factor reliably sets the initial trajectory skeleton for any configuration of conditions. The second coefficient focuses on the minimum possible curvature. The third and fourth coefficients are responsible for the smoothness and flatness of the trajectory. Small values are explained by the fact that, firstly, their influence is already indirectly taken into account by the integral equality constraints, and, secondly, an increase in these coefficients can lead to ambiguity in the form of the trajectory in various phases and conditions. The last coefficient strengthens the requirement of minimum discontinuities of the fourth derivative in the nodes of adjacent FEs. Figure 7 shows the results of trajectory optimization for both phases.

5.2. Kinematics Searching

As shown in Equation (88), four weight coefficients determine the nature of the velocity distribution along the motion trajectory. The following set of coefficients can be recommended as providing a stable forecast for both phases:

$${\mathit{W}}_v={\left(\begin{array}{ccc}1& 0& \begin{array}{cc}0.03& 0.03\end{array}\end{array}\right)}^T$$

(139)

The choice of coefficients was made by multiple testing for mutual influence, and the values are explained as follows. The first coefficient is responsible for the rapid speed approach to the preset upper limit; therefore, its influence is set to the greatest. The second coefficient should average and minimize the use of longitudinal acceleration. However, it is in conflict with the fourth impact coefficient. On the other hand, the increase in longitudinal acceleration is not a problem, since the integral equality constraints already limit it. The third coefficient reduces the use of lateral acceleration and is also in conflict with the first coefficient, therefore, its value is small, purely to take into account safety requirements. Additionally, the fourth coefficient determines the intensity of the influence of longitudinal jerk, which is also limited by the integral equality constraints.

Thus, a significant value of the coefficient is not required. Note that using the settings of Equation (137) it is possible to influence the longitudinal acceleration nature at the phase’s final node. Thus, considering the transient nature of the vehicle dynamics in the bypassing zone of the impeding vehicle at the end of the first phase, it is recommended to set the fifth element in Equation (137) equal to zero. Firstly, when the camera visually assesses the road situation at the beginning of the first phase, the curvature of the following road section is unknown. Therefore, to ensure increased safety, the longitudinal acceleration at the end of the first phase must be close to zero. However, this requirement is no longer strict when returning to the initial lane at the end of the second phase. The results of determining the kinematic parameters by phases are shown in Figure 8.

5.3. Analysis of Results

Trajectories (Figure 7a–c) with a minimum number of nodes are smooth, compact, of moderate curvature, and do not have excessive inflections. Although the calculations are carried out in different coordinate systems, the junction in the phase change node is accurate in value and derivatives. The curvature and its derivative along the trajectory (Figure 7d,e) are within the established limits and do not exceed them. As seen, the trajectories maintain sufficient clearance within the boundaries of the motion area, which confirms the effectiveness of the proposed solution with the safety contour’s control points. Note that the grid irregularity affects the concentration of curvature in these places due to a significant difference in the FE lengths between the corresponding adjacent segments. However, the trajectory’s overall stability, smoothness and monotonicity are preserved.

The distribution of speed and accelerations (Figure 8a–d) is represented by smoothed curves, matching the phase conjugation node in values and derivatives. The speed values do not exceed both the preset upper limit and the variable level of values according to the side slip condition (Figure 8b). Accelerations (Figure 8c–g) demonstrate continuity and do not exceed the vehicle technical capabilities as well as the physical limits in terms of tire–road adhesion. Longitudinal jerk and angular acceleration (Figure 8h,j) give some discontinuities at the nodes, which, as explained above, is due to the absence of the fourth derivative as a nodal parameter in the curve model. However, due to the inclusion in the cost function, it is possible to minimize gaps between the same nodes of adjacent FEs. At the same time, a strict distribution of these parameters’ values within the established limits is practically ensured. Note that the longitudinal acceleration and jerk boundaries are not symmetrical and permanent and can be set depending on the nature of the required action (e.g., acceleration and deceleration).

6. Conclusions

This paper has developed a mathematical basis for representing nonlinear constraints in the form of integral equalities and a numerical integration technique for accelerating the optimization procedure and implemented the interpolation polynomials to ensure the smoothness of all optimized parameters. Based on this study, the following comments are offered:

The most important advantage of the proposed approach is the significant calculation speed while ensuring prediction quality due to the numerical representation of all current parameters and the use of quadrature integration schemes. This predetermines the possibility of implementing optimization procedures in conjunction with real-time machines for HIL (hardware-in-loop) modeling and online implementation on natural objects. Note that a reliable and stable result is provided even with the five-point Gaussian integration scheme.

The proposed phase change method ensures seamless transitions for the main parameters of the trajectory and speed and their derivatives between adjacent fixed coordinate systems. Furthermore, having a road model generator would make it possible to realize the prediction continuity for any configuration of road conditions with arbitrary curvature.

Despite the essential irregularity of the FE grid, the forecasts are highly stable to changing initial optimization conditions. This is owing to the increased role of the integral equality constraints, which explains that, for example, regardless of the emphasis on any weight coefficients when searching for a trajectory, its general character remains unambiguous and approximately the same.

The idea of the integral equality constraints fully justified itself, providing good convergence of the cost function and determining the parameters within the given limits. Moreover, the computations are based on the same quadrature scheme as the calculations of the objective function components, which unifies the approach.

The distribution of speed parameters is represented by a fully kinematic model, although considering some physical limitations. Such an ideal curve has a slightly higher limiting potential than an actual vehicle can realize. However, this remark applies only to extreme riding modes, which are often prevented by preset safety criteria.

The longitudinal acceleration can be considered as the main numerical criterion for predicting the AV behavior that does not exceed both the propulsion system potential’s threshold of 3.5 m/s² and the reached peak value of 2.5 m/s². At the same time, the nature of changing the acceleration is stipulated by introducing the jerk restrictions: 5 m/s³ for the upper limit due to the propulsion system’s properties, and −2.5 m/s³ is the lower limit. Here, the asymmetry implies a lower intensity of deceleration than acceleration since during bypassing obstacles along the planning section the AV should not activate the braking system. From a safety point of view, the lateral acceleration peaks are about 6 m/s², which corresponds to the curvature peak points. However, the maximum acceleration at the same points is enough below the set limit of about 7.8 m/s², which guarantees a margin for tires’ adhesion conditions. It is possible to introduce a lateral acceleration limit regulated in the value of 0.4 g; however, the goal of this technique is to assess the maximum planning potential in terms of vehicle dynamics.

For future research, in the flat vehicle model used to form a plan of speed parameters, it is expedient to include moments that reflect dynamic phenomena such as roll, lateral elasticity of tires, dynamic distribution of vertical reactions, and traction and lateral forces. This approach will increase the forecast safety and feasibility of an actual vehicle.