Simultaneous Trajectory and Speed Planning for Autonomous Vehicles Considering Maneuver Variants

Abstract

The paper presents a technique of motion planning for autonomous vehicles (AV) based on simultaneous trajectory and speed optimization. The method includes representing the trajectory by a finite element (FE), determining trajectory parameters in Frenet coordinates, composing a model of vehicle kinematics, defining optimization criteria and a cost function, forming a set of constraints, and adapting the Gaussian N-point scheme for quadrature numerical integration. The study also defines a set of minimum optimization parameters sufficient for making motion predictions with smooth functions of the trajectory and speed. For this, piecewise functions with three degrees of freedom (DOF) in FE’s nodes are implemented. Therefore, the high differentiability of the trajectory and speed functions is ensured to obtain motion criteria such as linear and angular speeds, acceleration, and jerks used in the cost function and constraints. To form the AV roadway position, the Frenet coordinate system and two variable parameters are used: the reference path length and the lateral displacement perpendicular to reference line’s tangent. The trajectory shape, then, depends only on the final position of the AV’s mass center and the final reference’s curvature. The method uses geometric, kinematic, dynamic, and physical constraints, some of which are related to hard restrictions and some to soft restrictions. The planning technique involves parallel forecasting for several variants of the AV maneuver followed by selecting the one corresponding to a specified criterion. The sequential quadratic programming (SQP) technique is used to find the optimal solution. Graphs of trajectories, speeds, accelerations, jerks, and other parameters are presented based on the simulation results. Finally, the efficiency, rapidity, and prognosis quality are evaluated.

1. Introduction

Modern developments in the domain of autonomous vehicle (AV) motion planning are distinguished by a wide range of proposed techniques and methods having advantages and drawbacks depending on the forecasting goals under current conditions. The main planning challenge concerns the need for simultaneously satisfying multiple requirements that guarantee both the motion feasibility and its safety, including the controllability and stability within the subsequent tracking process.

The basic planning task consists of generating a forecast of trajectory and speed mode. On the one hand, it must conform to the principles of simplicity, smoothness, and unambiguity. For these purposes, analytical functions of polynomial and trigonometric types are often used [1], which change shape depending on parameters. The advantage of this approach implies that, with known parameters, any trajectory’s point may be directly computed. However, there are also problems associated with the fact that trajectories represented by polynomials of small degrees do not provide high differentiability. For large degrees, the polynomials may be unstable, and the sensitivity of DOFs corresponding to higher derivatives increases. This complicates the optimization procedure since the determined parameters, in this case, may differ by an order of magnitude.

On the other hand, the trajectory curve must meet the requirements of continuity and differentiability. Since the forecast optimality criteria themselves depend on the trajectory curvature’s derivatives, it is also necessary to ensure the equality of these derivatives at the point of changing planning cycles. In this case, most often it is impossible to ensure the continuity of higher speed derivatives, as well as kinematic parameters based on them such as yaw rate, angular acceleration, and jerks. In most of the existing techniques, these aspects are neglected, which affects the irregular and intermittent nature [2] of the listed parameters at transient points. However, these parameters are associated with vehicle control systems and actuators. Therefore, it is desirable to ensure the continuity of their functions and derivatives for transient processes.

To guide the AV on a roadway, two approaches may be used. The first one involves the formation of roadway boundaries and motion zones in the Cartesian coordinate system [3]. In this case, a space available for predicting the AV movement may be obtained by limiting motion zones for other traffic participants in such a way that a minimum safe distance is guaranteed. However, most often in this case, there is a need for separating the planning stage into sequential phases of determining the trajectory and distributing the speed mode. Another approach allows for the consideration of the formation of AV motion in Frenet coordinates [4,5]. In this case, usually, the midline of a road lane is in some way expressed as a reference path function. Thus, instead of Cartesian coordinates, two Frenet coordinates arise, such as the lane reference curve’s length and the lateral displacement perpendicular to the reference curve’s tangent. In this case, it is much easier to work with spatial constraints by imposing them on the lateral displacement within the boundaries of a road lane. However, we still need to make the transition to the AV’s local Cartesian system to decompose the components of kinematic parameters into longitudinal and transversal ones. Due to curvature, the full representation of the motion model in curvilinear coordinates becomes more complicated. Therefore, in this work, we will use a mixed approach in which there is a reference path curve in the Frenet coordinates, and the trajectory is predicted in the Cartesian coordinates.

Forecasting the speed mode [6] involves obtaining several kinematic parameters (linear and angular speeds, linear and angular accelerations, linear jerks) characterizing the AV dynamics and providing direct ties with vehicle sensors’ data (accelerometers, gyroscopes, radars), which allows for the monitoring and correcting of the AV control at the tracking stage. These same parameters may be used as criteria for optimizing the AV motion. The main task of speed planning aims at compromising the vehicle’s maximum performance, safety, and motion smoothness.

To implement the above, two different approaches may be used. The first one is represented by discrete modeling [7], where a system of equations based on vehicle dynamics (or kinematics) allows simultaneous optimization [8] of the trajectory and speed related differentially to each other. However, to ensure good convergence of the numerical solution, it is necessary to reduce the integration step, which increases iteration time. Another approach proceeds from representing the trajectory and speed by piecewise analytical functions [9], forms of which are known in advance and only the parameters that determine the nature of these functions are optimized. In this case, there are no problems with step control, but the motion can only be represented by a vehicle kinematic model.

Constraints are a separate issue. There are quite a number of restrictions stipulated by both the parameter nature and the possibility of its assessment. Thus, geometric restrictions [10] imply a non-violation of the roadway boundaries (permissible zone) and keep safe positions relative to other traffic participants. Kinematic constraints concern parameters based on speed [11] and its derivatives. Dynamic restrictions are determined by limits imposed on the vehicle’s powertrain system to realize its traction potential. The vehicle acceleration naturally decreases with speed, which must be reflected in the driving mode being planned. Physical limitations are based on the tire’s maximum adhesion property and the potential for motion stability. All these restrictions may be represented as hard and/or soft constraints. In the first case, satisfying limiting conditions is mandatory and formed by additional linear or nonlinear equalities and inequalities. In the second case, the restrictions themselves are included in the objective function [12], which determines the influence of the criteria but allows for violations of the limits for exceptional reasons. For example, this approach is appropriate in cases when a safe distance between vehicles in the same lane is not ensured.

Obstacles: In real conditions, the AV must maneuver among moving and stationary obstacles [13]. One of the main tasks on AV’s motion-planning strategy is to prevent collisions and maintain safe distances [14]. The issue is complicated by the fact that it is difficult to accurately estimate the motion directions and speeds of other participants for a relatively long time. In this regard, it may be assumed that during AV’s maneuvering, other vehicles barely change the nature of their movement. Thus, the task is to maximize the distances between vehicles while maneuvering, considering vehicles’ safety contours.

This study continues a series of our research on developing algorithms for AV motion planning. Initially, in paper [15], we outlined the main approaches to modeling trajectory and speed modes based on the vehicle kinematics model. At the same time, the sequential approach to optimizing the trajectory and speed was used. In the next research [16], we stipulated an advantage of using integral forms of the equality constraints instead of point ones. In articles [17], we tested various numerical integration schemes in optimization problems of motion planning.

Based on the above, we shall consider a motion-planning technique that involves the simultaneous prediction of trajectory and speed mode within a single finite element (FE) of the variable reference length. At the same time, depending on the number of lanes and presence of obstacles, trajectory variants are to be analyzed and estimated followed by possible selection of an optimal strategy in specific initial conditions. The general strategy of the technique is shown in Figure 1.

2. Motion-Planning Concept

Let us assume that AV forms a motion trajectory relative to the midline of a lane on which it is currently located [18,19] (Figure 2). Such a line may be obtained, for example, by the cubic spline interpolation providing sufficient smoothness and representing the curvature of a road section. If functional dependencies x = x(s_r), y = y(s_r) are built for a section, then, varying the parameter s_r relative to the initial path length s_r0, it is possible to determine the values x_sr, y_sr corresponding to the zero point of the lateral displacement L along the road width. Thus, the coordinates x_f, y_f of the AV mass center’s final position can be calculated as

$$x_f=x_{sr}-L_{f}sin\left({\alpha }_f\right),y_f=y_{sr}+L_{f}cos\left({\alpha }_f\right)$$

(1)

where α_f = the midline curve’s tangent slope at the point x_sr, and L_f = AV mass center’s lateral displacement at the final point.

Since the transverse coordinate L is always perpendicular to the curve s_r, the AV’s final angular position on any lane has ideally the same value α_f. However, the angle α actual value may for various reasons deviate from the ideal value, which is associated with the desired angle α_d. In the transverse direction, AV may be located in any lane by introducing such a parameter as the desired lateral displacement (position) L_d. Thus, the trajectory can be represented as a piecewise polynomial function with the basis D = x_f − x₀.

Trajectory. Let us apply the Hermitian interpretation of Lagrange polynomials [20] using basis functions and nodal degrees of freedom (DOFs). To model the trajectory as a function y(x), the fifth-degree polynomial (p = 5) may be considered, which requires three DOFs in a node. Then, y(x) may be expressed in the vector form

$$\mathit{A}={\left(\begin{array}{ccc}{c}_0& \cdots & c_p\end{array}\right)}^T,\mathit{X}={\left(\begin{array}{ccc}{x}^0& \cdots & x^p\end{array}\right)}^T,y\left(x\right)={\mathit{A}}^T\mathit{X},$$

(2)

where c_j = polynomial coefficient, j ∈ [0, p].

If define k = ((p + 1)/2 − 1) first derivatives of the vector X (where p is odd) and substitute the x-coordinates of the initial (0) and final (D) nodes, the matrix B is composed 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(D\right)\\ ⋮\\ d^k{\mathit{X}}^T/d{x}^k\left(D\right)\end{array}\right),\mathit{B}\mathit{A}={\mathit{Q}}_{tr},\mathit{A}={\mathit{B}}^{-1}{\mathit{Q}}_{tr},\mathit{F}={\left({\mathit{B}}^T\right)}^{-1}\mathit{X},$$

(3)

where Q_tr = set of trajectory nodal parameters.

As a result, expressions of Equations (2) and (3) are linked in the form

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

(4)

Considering one final element (FE) of the length D, the function y(x) may be represented by the sets of shape functions and DOF values. Then

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

(5)

where f_j = shape function and q_j = weight coefficient or DOF, j ∈ [1, p+1].

If we assume x = ξD, where parameter ξ ∈ [0, 1], then the shape functions may be derived through basis functions for an element of unit length. Thus, the basis functions F_ξ of the argument ξ ∈ [0, 1] are universal for the variable length D of a roadway segment. Defining the vector D and matrix D_d

$$\mathit{D}=\left(\begin{array}{ccc}{D}^0& \cdots & D^k\end{array}\right),{\mathit{D}}_d=diag\left(\begin{array}{cc}\mathit{D}& \mathit{D}\end{array}\right)$$

(6)

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

Thus, the functions F of Equation (3) may be replaced by the basis functions of the parameter ξ

$$\mathit{F}={\mathit{D}}_d{\mathit{F}}_{\xi },y\left(x\right)=y\left({\mathit{Q}}_{tr},D,\xi \right)={\mathit{Q}}_{tr}^T{\mathit{D}}_d{\mathit{F}}_{\xi }$$

(7)

Then, for the k-th derivative,

$$\frac{d^{k}y}{d{x}^k}=\frac{d}{Dd\xi }\left(\frac{1}{D^{k-1}}{\mathit{Q}}_{tr}^T{\mathit{D}}_d\frac{d^{k-1}{\mathit{F}}_{\xi }}{d{\xi }^{k-1}}\right)=\frac{d}{Dd\xi }\left(\frac{1}{D^{k-1}}{\mathit{Q}}_{tr}^T{\mathit{D}}_d\frac{d^{k-1}{\mathit{F}}_{\xi }}{d{\xi }^{k-1}}\right)={\mathit{Q}}_{tr}^T\left(\frac{{\mathit{D}}_d}{D^k}\right)\frac{d^k{\mathit{F}}_{\xi }}{d{\xi }^k}$$

(8)

Speed Model can be organized in the same way as the trajectory. We assume that the AV longitudinal speed V_ζ is distributed along the trajectory projection on the x-coordinate. According to Equations (3)–(8), the longitudinal component and its k-th derivative may be expressed as follows

$$V_{\zeta }\left(x\right)={\mathit{Q}}_{sp}^T{\mathit{D}}_d{\mathit{F}}_{\xi },\frac{d^k{V}_{\zeta }}{d{x}^k}={\mathit{Q}}_{sp}^T\left(\frac{{\mathit{D}}_d}{D^k}\right)\frac{d^k{\mathit{F}}_{\xi }}{d{\xi }^k}$$

(9)

where Q_sp = set of speed nodal parameters.

Full set of parameters. Since we consider one segment with initial (0) and final (f) points, each of them containing three DOFs, we need six variable parameters for each Q_tr and Q_sp to simultaneously search the trajectory y(x) and speed distribution V_ζ(x). Then, introducing nodal vectors

$$\begin{array}{c}{\mathit{q}}_{tr\mathbf{0}}=\left(\begin{array}{c}{q}_{tr\left(1\right)}\\ q_{tr\left(2\right)}\\ q_{tr\left(3\right)}\end{array}\right)=\left(\begin{array}{c}{y}_0\\ {\left.\frac{dy}{dx}\right|}_0\\ {\left.\frac{d^{2}y}{d{x}^2}\right|}_0\end{array}\right),{\mathit{q}}_{trf}=\left(\begin{array}{c}{q}_{tr\left(4\right)}\\ q_{tr\left(5\right)}\\ q_{tr\left(6\right)}\end{array}\right)=\left(\begin{array}{c}{y}_f\\ {\left.\frac{dy}{dx}\right|}_f\\ {\left.\frac{d^{2}y}{d{x}^2}\right|}_f\end{array}\right),{\mathit{Q}}_{tr}=\left(\begin{array}{c}{\mathit{q}}_{tr\mathbf{0}}\\ {\mathit{q}}_{trf}\end{array}\right)\\ {\mathit{q}}_{sp0}=\left(\begin{array}{c}{q}_{sp\left(1\right)}\\ q_{sp\left(2\right)}\\ q_{sp\left(3\right)}\end{array}\right)=\left(\begin{array}{c}{V}_{\zeta 0}\\ {\left.\frac{d{V}_{\zeta }}{dx}\right|}_0\\ {\left.\frac{d^2{V}_{\zeta }}{d{x}^2}\right|}_0\end{array}\right),{\mathit{q}}_{spf}=\left(\begin{array}{c}{q}_{sp\left(4\right)}\\ q_{sp\left(5\right)}\\ q_{sp\left(6\right)}\end{array}\right)=\left(\begin{array}{c}{V}_{\zeta f}\\ {\left.\frac{d{V}_{\zeta }}{dx}\right|}_f\\ {\left.\frac{d^2{V}_{\zeta }}{d{x}^2}\right|}_f\end{array}\right),{\mathit{Q}}_{sp}=\left(\begin{array}{c}{\mathit{q}}_{sp0}\\ {\mathit{q}}_{spf}\end{array}\right),\end{array}$$

(10)

The vectors Q_tr and Q_sp completely determine the configuration of the trajectory and speed distributions within the segment D. Nevertheless, not all of their members are variable parameters. Note that at the initial node the vectors q_tr0, q_sp0 are usually known from the previous solution. In turn, strict constraints may be imposed on the values of vectors q_tr0, q_sp0, which reduces the number of parameters to be optimized.

Note that the nodal parameter y_f is not independent in its pure form but is calculated from Equation (1), where the parameters s_r and L are involved. Their variation determines a combination of y_f and D. Thus, it is possible to form a set of variable parameters that best reflect both the trajectory shape and the speed distribution.

$$\mathit{q}={\left(\begin{array}{ccc}{s}_r& L_f& \begin{array}{ccc}{\left.\frac{dy}{dx}\right|}_f& {\left.\frac{d^{2}y}{d{x}^2}\right|}_f& \begin{array}{ccc}{V}_{\zeta f}& {\left.\frac{d{V}_{\zeta }}{dx}\right|}_f& {\left.\frac{d^2{V}_{\zeta }}{d{x}^2}\right|}_f\end{array}\end{array}\end{array}\right)}^T$$

(11)

In the case of the strict finite transverse displacement L_f and the corresponding angle α_f, these parameters depend only on the variable s_r, and the set of Equation (11) is shortened

$$\mathit{q}={\left(\begin{array}{ccc}{s}_r& {\left.\frac{d^{2}y}{d{x}^2}\right|}_f& \begin{array}{ccc}{V}_{\zeta f}& {\left.\frac{d{V}_{\zeta }}{dx}\right|}_f& {\left.\frac{d^2{V}_{\zeta }}{d{x}^2}\right|}_f\end{array}\end{array}\right)}^T$$

(12)

Numerical Integration Technique. To simultaneously satisfy the rapidity and quality of calculations, we shall use the numerical integration based on the N-point Gaussian quadrature scheme. Assume that some integrand z(x) is considered within a segment [x_i−1, x_i], then, expressing x = ξD

$${\int }_0^{D}z\left(x\right)dx=D{\int }_0^{1}z\left(\mathit{Q},D,\xi \right)d\xi \approx D{\sum }_{k=1}^N{w}_{k}z\left(\xi \left({\vartheta }_k\right)\right)det\left(J\left({\vartheta }_k\right)\right)$$

(13)

where w_k = integration weight in the k-th point, ϑ_k—k-th point in the master–element coordinate system, J = Jacobian, k ∈ [1, N], and N = number of integration points.

For one-dimensional FE

$$\xi \left({\vartheta }_k\right)=\frac{\left({\xi }_1-{\xi }_0\right)}{2}\left({\vartheta }_k+1\right)+{\xi }_0=\frac{1}{2}\left({\vartheta }_k+1\right),det\left(J\right)=\frac{{\xi }_1-{\xi }_0}{2}=\frac{1}{2}$$

(14)

Denoting the vectors,

$$\mathit{\vartheta }=\left(\begin{array}{c}{\vartheta }_1\\ ⋮\\ {\vartheta }_N\end{array}\right),\mathit{w}=\left(\begin{array}{c}{w}_1\\ ⋮\\ w_N\end{array}\right),\mathit{\xi }={\left(\begin{array}{c}{\xi }_1\\ ⋮\\ {\xi }_N\end{array}\right)}^T,\mathit{z}={\left(\begin{array}{c}{z}_1\\ ⋮\\ z_N\end{array}\right)}^T$$

(15)

The short expression for calculating the integral becomes

$$D{\int }_0^{1}z\left(\mathit{Q},D,\xi \right)d\xi \approx \frac{D}{2}\mathit{z}\mathit{w}$$

(16)

where D = section length and z = vector of integrands of 1 × N size.

In most cases, we will need not only final integral values but also intermediate ones corresponding to the points x_i (or ξ_i). That is, the integration must also be conducted on the sections [x_i−1, x_i].

$$\begin{gathered} \begin{array}{c}{\int }_{x_{i-1}}^{x_i}z\left(x\right)dx=D{\int }_{{\xi }_{i-1}}^{{\xi }_i}z\left(D\xi \right)d\xi = \\ D{\Delta \xi }_i{\int }_0^{1}z\left(D\xi \left(\vartheta \right)\right)d\vartheta =\\ \frac{D}{2}{\Delta \xi }_i{\sum }_{k=1}^{N}z\left(D{\xi }_i\left({\vartheta }_k\right)\right)w_k= \\ \frac{D}{2}{\Delta \xi }_i{\sum }_{k=1}^N{w}_{k}z\left(D{,\xi }_{i,k}\right)=\frac{D}{2}{\Delta \xi }_i{\mathit{z}}_i^T\mathit{w}\end{array} \end{gathered}$$

(17)

If we combine all increments Δξ_i in a row vector Δξ, then the sum of Equation (13) can be obtained as follows

$${\int }_0^{D}z\left(x\right)dx=\frac{D}{2}\mathbf{\Delta }\xi diag\left({\mathit{z}}^T\right)\mathit{w}$$

(18)

Planning Strategy in the Presence of Obstacles. Let us consider the AV motion in the presence of other vehicles (Figure 3). We assume, for instance, that AV is moving along the middle (2) lane and may perform three maneuver variants [21]: lane changes to the first (1) and third (3) lanes and following the second lane. In terms of geometric sense, such a rearrangement may be considered the best, which ensures keeping the greatest distance from an obstacle. At the moment of starting the motion planning (1)–(4), AV fixes the midline points of the current lane in such a way to form smooth conjugations (splines) reflecting the road curvature. Then, considering the number of lanes, the longitudinal s_r and transverse L coordinates, it is possible to build a curvilinear grid of the road segment space relative to which the motion is planned.

By using radars, AV can obtain distances to objects and angles of measuring beams. Then, the coordinates of vehicles’ initial positions may be calculated relative to the AV’s xy coordinate system. To do this, we use the vehicle safety zones (contours) represented by conditional circles. Thus, for a passenger vehicle, three round areas of radius r relative to the front, middle, and rear vehicle parts are enough to form the safety contour. The positions of the front and back parts’ centers are calculated as follows

$$x_{cp}=x_c+h_{p}cos\left(\varphi \right),y_{cp}=y_c+h_{p}sin\left(\varphi \right),p\in [f,r]$$

(19)

where x_c, y_c = coordinates of the vehicle’s mass center, h_f, h_r = distances to the conditional centers of the vehicle’s front and rear parts, respectively, ϕ = yaw angle.

Thus, considering the radar measurements, the positions of other vehicles’ centers can be estimated, and based on them and the predetermined road segment curvilinear grid, the lateral displacements L_i,0 can be found. This helps to determine the corresponding traffic lane for each moving obstacle. If the lane’s midline as the vehicle’s current trajectory is considered, then, similarly to AV, we can compose interpolation dependencies x_i = x_i(s_i), y_i = y_i(s_i), i ∈ [1, N_v], where N_v is the number of fixed vehicles.

Therefore, we may estimate the vehicle motion parameters in discrete points over time intervals Δt and the total AV’s motion time t_f. If vehicles’ speeds V_i are accepted to be constants, then, the paths for the intervals Δt are Δs_i = V_iΔt and for the total time s_i = V_it_f. Knowing the paths, it is possible to determine coordinates x_i, y_i by the predefined interpolation. Then, considering the radius of safety zones r_i, the distance between the AV (0) and an i-th obstacle vehicle can be evaluated as

$$d_i=\sqrt{{\left(x_{cp0}-x_{cpi}\right)}^2+{\left(y_{cp0}-y_{cpi}\right)}^2}-r_0-r_i,p\in [f,r]$$

(20)

Note that, strictly speaking, to determine the vehicle mass center’s position, it is necessary to identify a vehicle and estimate its overall dimensions and wheelbase, which somewhat complicates the calculations. In this regard, we assume that moving obstacles behind AV may be conditionally defined by circles centered at the vehicles’ fronts, and obstacles ahead of AV may be represented by circles centered at the vehicles’ rears. The corresponding centers can be easily found by estimating the overall width of a moving obstacle. Then, the parameters L_i,0 can be replaced by L_i,f and L_i,r, respectively. Thus, the trajectories of moving obstacles can be considered as the trajectories of the safety contour circles’ centers.

Time. The time interval Δt_i between AV’s positions i − 1 and i can be calculated by the integration, considering the trajectory s and speed V

$${\Delta t}_i={\int }_{{\tau }_{i-1}}^{{\tau }_i}d\tau ={\int }_{s_{\left(i-1\right)}}^{s_{\left(i\right)}}\frac{ds}{V\left(s\right)}={\int }_{x_{i-1}}^{x_i}\frac{dx}{V_x\left(x\right)}={\int }_{x_{i-1}}^{x_i}{z}_{ti}\left(x\right)dx,z_{ti}\left(x\right)=\frac{1}{V_x\left(x\right)}$$

(21)

Each segment [x_i−1, x_i], in turn, may also be represented by a finite element corresponding to a range [ξ_i−1, ξ_i] and again considered in the segment ϑ ∈ [0, 1]. Then

$${\int }_{x_{i-1}}^{x_i}{z}_{ti}\left(x\right)dx=D{\int }_{{\xi }_{i-1}}^{{\xi }_i}{z}_{ti}\left(\xi \right)d\xi =D{\Delta \xi }_i{\int }_0^1{z}_{ti}\left(D\xi \left(\vartheta \right)\right)d\vartheta$$

(22)

Passing to numerical integration according to Equation (17), we obtain

$${\Delta t}_i=D\frac{{\Delta \xi }_i}{2}{\sum }_{k=1}^N{z}_{ti}\left(D{\xi }_i\left({\vartheta }_k\right)\right)w_k=D\frac{{\Delta \xi }_i}{2}{\mathit{z}}_{ti}^T\mathit{w}$$

(23)

Thus, it is possible to form a vector of time increments

$$\Delta t={\left(\begin{array}{ccc}{\Delta t}_1& \cdots & {\Delta t}_n\end{array}\right)}^T$$

(24)

where n = number of intervals.

The vector of time points can be obtained by the accumulative addition:

$$t={\left(\begin{array}{ccc}0& {\sum }_{i=1}^1{\Delta t}_i& \begin{array}{cc}\cdots & {\sum }_{i=1}^n{\Delta t}_i\end{array}\end{array}\right)}^T,t_f={\sum }_{i=1}^n{\Delta t}_i$$

(25)

Provided that the segment ξ ∈ [0, 1] is represented by the vector of increments Δξ, the final time can also be obtained as follows

$$t_f=\frac{D}{2}\mathbf{\Delta }\xi diag\left({\mathit{z}}_t^T\right)\mathit{w}$$

(26)

Spatial Constraints. The requirement of non-violating the roadway boundaries is the main limiting factor. It can be used as a hard or soft constraint. In the first case, it is assumed that there must be no safety contour exceeding the roadway boundaries. In the second case, it is possible to allow the crossing of the solid marking line subject to a critical approach to an obstacle. In this case, we shall use strict conditions. If the maximum lateral coordinate relative to an AV’s initial position as L_U (upper limit) is denoted for moving leftward and as L_L (lower limit) for the right bias, then the conditions for each point p ∈ [0, f, r] of the central, front, and rear vehicle’s parts have the form

$$L_{0,p}+r_0\le L_U,L_{0,p}-r_0\ge L_L$$

(27)

In the case of riding on the same lane, the safety conditions can be enhanced by the values L_U and L_L equal to half the lane width with the corresponding sign. As a result, by setting the upper (left) and lower (right) boundaries, it is possible to limit the lateral displacement for each trajectory variant. Note that the values of L_0,p can be obtained directly by interpolating the known coordinates from Equation (13).

3. Motion-Modeling Technique

3.1. Basics of Trajectory Geometry

Let us briefly describe the main components of the vehicle trajectory geometry with the ideal controllability based on the kinematic model.

The elementary arc length of the trajectory represented by function y(x) is

$$ds=\sqrt{d{x}^2+d{y}^2}=\sqrt{1+y_x^{\prime 2}}dx$$

(28)

Then, the arc’s first derivative along the x-coordinate is

$$s_x^{\prime }=\frac{ds}{dx}=\sqrt{1+y_x^{\prime 2}},ds=s_x^{\prime }dx$$

(29)

The second derivative concerning the x-coordinate is

$$s_x^{″}=\frac{d{s}_x^{\prime }}{dx}=\frac{1}{2}\frac{1}{\sqrt{1+y_x^{\prime 2}}}\frac{d}{dx}{y}_x^{\prime 2}=\frac{y_x^{\prime }{y}_x^{″}}{s_x^{\prime }}$$

(30)

The third derivative concerning the x-coordinate is

$$s_x^{‴}=\frac{1}{s_x^{\prime }}\left(y_x^{″2}-s_x^{″2}+{y_x^{\prime }y}_x^{‴}\right)=\frac{1}{s_x^{\prime }}\left(\left(y_x^{″}+s_x^{″}\right)\left(y_x^{″}-s_x^{″}\right)+{y_x^{\prime }y}_x^{‴}\right)$$

(31)

The derivative at any trajectory point is tied to the slope angle’s tangent

$$y_x^{\prime }=\mathit{tan}\left(\alpha \left(x\right)\right),\alpha \left(x\right)=\mathit{arctan}\left(y_x^{\prime }\right)$$

(32)

The expressions of Equation (29), considering Equation (32), may be rewritten as follows:

$$s_x^{\prime }=\sqrt{1+{\mathit{tan}}^2\left(\alpha \left(x\right)\right)}=\mathit{sec}\left(\alpha \left(x\right)\right)=1/\mathit{cos}\left(\alpha \left(x\right)\right)$$

(33)

The first derivative of the slope angle concerning the x-coordinate is

$${\alpha }_x^{\prime }=\frac{d\alpha }{dx}=\frac{d}{dx}\mathit{arctan}\left(y_x^{\prime }\right)=\frac{1}{1+{\left(y_x^{\prime }\right)}^2}{y}_x^{″}=\frac{y_x^{″}}{s_x^{\prime 2}}$$

(34)

The change of the tangent slope angle along the arc s length is the curvature K

$$K={\alpha }_s^{\prime }=\frac{d\alpha }{ds}=\frac{d\alpha }{dx}\frac{dx}{ds}=\frac{d\alpha }{dx}/\frac{ds}{dx}=\frac{{\alpha }_x^{\prime }}{s_x^{\prime }}$$

(35)

The second derivative concerning x is

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

(36)

The curvature’s change along the arc s is

$$\frac{dK}{ds}=\frac{d{\alpha }_s^{\prime }}{ds}=\frac{d}{dx}\left(\frac{{\alpha }_x^{\prime }}{s_x^{\prime }}\right)/\frac{ds}{dx}=\frac{1}{s_x^{\prime }}\frac{dK}{dx}=\frac{K_x^{\prime }}{s_x^{\prime }}$$

(37)

In turn, the curvature’s change of along the x-coordinate is

$$K_x^{\prime }=\frac{dK}{dx}=\frac{1}{s_x^{\prime }}\left(\frac{y_x^{‴}}{s_x^{\prime 2}}-2K{s}_x^{″}-K{s}_x^{″}\right)=\frac{1}{s_x^{\prime }}\left({\alpha }_x^{″}-K{s}_x^{″}\right)$$

(38)

The second derivative concerning the x-coordinate is

$$K_x^{″}=\frac{d^{2}K}{d{x}^2}=\frac{1}{s_x^{\prime }}\left(\frac{y_x^{\left(4\right)}}{s_x^{\prime 2}}-2s_x^{″}\left(\frac{y_x^{‴}}{s_x^{\prime 3}}+2K_x^{\prime }\right)-3K{s}_x^{‴}\right)$$

(39)

The instantaneous radius is defined as the reciprocal of the curvature

$$R=1/K$$

(40)

The central slip angle β characterizes the ratio between the lateral and longitudinal components of the mass center speed and is evaluated as follows

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

(41)

The first derivative of β along the x-coordinate is

$${\beta }_x^{\prime }=\frac{d}{dx}\left(\mathit{arcsin}\left(bK\right)\right)=\frac{b}{\sqrt{1-{\left(bK\right)}^2}}\frac{dK}{dx}=k_{\beta }{K}_x^{\prime }$$

(42)

where the coefficient

$$k_{\beta }=\frac{b}{\sqrt{1-{\left(bK\right)}^2}}$$

(43)

Its derivative is given by

$$k_{\beta x}^{\prime }=\frac{d{k}_{\beta }}{dx}=k_{\beta }^{3}K{K}_x^{\prime }$$

(44)

Then, the second derivative of β concerning x is

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

(45)

The vehicle’s yaw angle ϕ is obtained through the tangent slope angle and the central slip angle as follows

$$\varphi =\alpha -\beta$$

(46)

3.2. Basics of Vehicle Kinematics Model

The vehicle’s mass center trajectory is geometrically related to the longitudinal V_ζ and transverse V_μ speeds (Figure 2) based on the assumption that there is no tire’s sideslip. The mass center’s absolute speed may be decomposed as follows

$$\overrightarrow{V}=V\overrightarrow{\tau }+0\overrightarrow{\nu }=V_{\zeta }{\overrightarrow{u}}_{\zeta }+V_{\mu }{\overrightarrow{u}}_{\mu }=V_x{\overrightarrow{u}}_x+V_y{\overrightarrow{u}}_y$$

(47)

where $\overrightarrow{\tau }$, $\overrightarrow{\nu }$ = natural coordinate system’s unit vectors, ${\overrightarrow{u}}_{\zeta }$, ${\overrightarrow{u}}_{\mu }$ = unit vectors of the vehicle local coordinate system ζμ, and ${\overrightarrow{u}}_x$, ${\overrightarrow{u}}_y$, V_x, V_y = unit vectors and speed components of the fixed (global) coordinate system xy.

The longitudinal speed V_ζ and its k-th derivatives, according to Equation (9), is expressed in the forms

$$V_{\zeta }\left(x\right)={\mathit{Q}}_{sp}^T{\mathit{D}}_d{\mathit{F}}_{\xi },\frac{d^k{V}_{\zeta }}{d{x}^k}={\mathit{Q}}_{sp}^T\left(\frac{{\mathit{D}}_d}{D^k}\right)\frac{d^k{\mathit{F}}_{\xi }}{d{\xi }^k}$$

(48)

To obtain the kinematic parameters related to the AV dynamics, the following derivatives concerning the time t are needed

$$\frac{d{V}_{\zeta }}{dt}=\frac{d{V}_{\zeta }}{dx}\frac{dx}{dt}=\frac{d{V}_{\zeta }}{dx}{V}_x,\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$$

(49)

The projection V_x and longitudinal speed V_ζ are tied through the tangent angle α and central slip angle β. Then,

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

(50)

The first derivative concerning the coordinate x is

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

(51)

The lateral speed V_μ is defined by the longitudinal component V_ζ and the central slip angle β

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

(52)

The derivatives concerning time and x-coordinate obtain

$$\begin{array}{c}\frac{d{V}_{\mu }}{dt}=\frac{d{V}_{\mu }}{dx}\frac{dx}{dt}=\frac{d{V}_{\mu }}{dx}{V}_x,\frac{d{V}_{\mu }}{dx}=\frac{d{V}_{\zeta }}{dx}\mathit{tan}\left(\beta \right)+V_{\zeta }\frac{\mathit{d\beta }}{\mathit{dx}}{\mathit{sec}}^2\left(\beta \right),\\ \frac{d^2{V}_{\mu }}{d{t}^2}=\left(\frac{d^2{V}_{\mu }}{d{x}^2}{V}_x+\frac{d{V}_{\mu }}{dx}\frac{d{V}_x}{dx}\right)V_x,\\ \frac{d^2{V}_{\mu }}{d{x}^2}=\frac{d^2{V}_{\zeta }}{d{x}^2}\mathit{tan}\left(\beta \right)+\left(V_{\zeta }\frac{d^2\beta }{d{x}^2}+2\frac{d\beta }{dx}\left(\frac{d{V}_{\zeta }}{dx}+V_{\mu }\frac{d\beta }{dx}\right)\right){\mathit{sec}}^2\left(\beta \right)\end{array}$$

(53)

The yaw rate is the derivation of yaw angle ϕ = α – β. Thus, its view and derivative component concerning the x-coordinate are

$$\omega =\frac{d\varphi }{dt}=\frac{d\varphi }{dx}\frac{dx}{dt}=\frac{d\varphi }{dx}{V}_x,\frac{d\omega }{dx}=\frac{d}{dx}\left(\frac{d\varphi }{dx}{V}_x\right)=\frac{d^2\varphi }{d{x}^2}{V}_x+\frac{d{V}_x}{dx}\frac{d\varphi }{dx},$$

(54)

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

$$\epsilon =\frac{d\omega }{dt}=\frac{d\omega }{dx}\frac{dx}{dt}=\frac{d\omega }{dx}{V}_x=\frac{d^2\varphi }{d{x}^2}{V}_x^2+\frac{d{V}_x}{dx}\omega$$

(55)

The longitudinal and lateral accelerations in the vehicle’s local coordinates ζμ

$$\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),\left(\begin{array}{c}{a}_{\zeta }\\ a_{\mu }\end{array}\right)=\left(\begin{array}{c}\frac{d{V}_{\zeta }}{dt}-\omega V_{\mu }\\ \frac{d{V}_{\mu }}{dt}+\omega V_{\zeta }\end{array}\right)$$

(56)

The longitudinal and lateral jerks in the vehicle’s local coordinates ζμ

$$\overrightarrow{j}=\frac{d\overrightarrow{a}}{dt}={\left(\begin{array}{c}{j}_{\zeta }\\ j_{\mu }\end{array}\right)}^T\left(\begin{array}{c}{\overrightarrow{u}}_{\zeta }\\ {\overrightarrow{u}}_{\mu }\end{array}\right)={\left(\begin{array}{c}\frac{d^2{V}_{\zeta }}{d{t}^2}-\left(2\frac{d{V}_{\mu }}{dt}+V_{\zeta }\omega \right)\omega -\epsilon V_{\mu }\\ \frac{d^2{V}_{\mu }}{d{t}^2}+\left(2\frac{d{V}_{\zeta }}{dt}-V_{\mu }\omega \right)\omega +\epsilon V_{\zeta }\end{array}\right)}^T\left(\begin{array}{c}{\overrightarrow{u}}_{\zeta }\\ {\overrightarrow{u}}_{\mu }\end{array}\right)$$

(57)

4. Optimization and Constraints

4.1. Optimization Criteria

Note that the criteria for finding an optimal solution can be conditionally divided into dynamic and static. The first ones are generally time (speed) functions. Their common impact is aimed at ensuring trajectory smoothness and maneuver rapidity. The static criteria, in turn, are tied to parameters of the vehicle’s final position, being responsible for the AV’s disposition on the lane and the course direction. Let us consider the criteria in more detail.

The longitudinal speed deviation relative to a preset upper-level V_ζU may be the main “moving” factor of the AV model. The integral of the squared velocity deviation has the form

$$\begin{array}{c}{I}_V={\int }_0^{t_f}{\left(V_{\zeta U}-V_{\zeta }\right)}^{2}dt={\int }_0^s{\left(V_{\zeta U}-V_{\zeta }\right)}^2\frac{ds}{V}=\\ {\int }_0^D{\left(V_{\zeta U}-V_{\zeta }\right)}^2\frac{dx}{V_x\left(x\right)}=D{\int }_0^1{\left(V_{\zeta U}-V_{\zeta }\right)}^2\frac{d\xi }{V_x\left(x\right)}\end{array}$$

(58)

The integrand z_V(ξ) is expressed through the speed nodal parameters Q_sp

$$z_V\left(\xi \right)={\left(V_{\zeta U}-V_{\zeta }\left({\mathit{Q}}_{sp},D,\xi \right)\right)}^2/V_x\left({\mathit{Q}}_{sp},D,\xi \right),{{\mathit{z}}_V=z}_V\left({\mathit{Q}}_{sp},D,\xi \left({\mathit{\vartheta }}^T\right)\right)$$

(59)

Then,

$$I_V\approx \frac{D}{2}\mathbf{\Delta }\xi {\mathit{z}}_V\mathit{w}$$

(60)

The longitudinal and lateral jerk criteria [22] include all the kinematic parameters containing curvature and its derivatives. Thus, these criteria reflect the transient dynamics in two directions. The longitudinal direction is to a greater extent associated with the vehicle traction process, and the lateral direction is generally related to the steering control. The common approach is as follows

$$I_{jp}={\int }_0^{t_f}{j}_p^{2}dt={\int }_0^s{j}_p^2\frac{ds}{V}={\int }_0^D{j}_p^2\frac{dx}{V_x\left(x\right)}=D{\int }_0^1{j}_p^2\frac{d\xi }{V_x\left(D\xi \right)}=D{\int }_0^1{z}_{jp}\left(\xi \right)d\xi ,p\in [\zeta ,\mu ]$$

(61)

The integrand z_jp(ξ) is determined according to Equations (14)–(16) and depends on both trajectory Q_tr and speed Q_sp parameters, then, obtain

$$z_{jp}\left(\xi \right)=j_p^2\left({\mathit{Q}}_{tr},{\mathit{Q}}_{sp},D,\xi \right)/V_x\left({\mathit{Q}}_{sp},D,\xi \right),{\mathit{z}}_{jp}=z_{jp}\left({\mathit{Q}}_{tr},{\mathit{Q}}_{sp},D,\xi \left({\mathit{\vartheta }}^T\right)\right)$$

(62)

Then,

$$I_{jp}\approx \frac{D}{2}\mathbf{\Delta }\xi {\mathit{z}}_{jp}\mathit{w}$$

(63)

Motion time T reflects final time costs and may be calculated as follows

$$I_t={\int }_0^{t_f}dt={\int }_0^s\frac{ds}{V\left(s\right)}={\int }_0^D\frac{dx}{V_x\left(x\right)}=D{\int }_0^1\frac{d\xi }{V_x\left(D\xi \right)}=D{\int }_0^1{z}_T\left(\xi \right)d\xi$$

(64)

According to Equations (10) and (12), the integrand z_T(ξ) and T value

$$z_T\left(\xi \right)=\frac{1}{V_x\left({\mathit{Q}}_{sp},D,\xi \right)},{\mathit{z}}_T=z_T\left({\mathit{Q}}_{tr},{\mathit{Q}}_{sp},D,\xi \left({\mathit{\vartheta }}^T\right)\right),T=I_T\approx \frac{D}{2}\mathbf{\Delta }\xi {\mathit{z}}_T\mathit{w},$$

(65)

The final lateral position L_f should best match the desired lateral displacement L_d to satisfy the safe completion of maneuver and to control distances to the lane boundaries. Then,

$$I_L={\left(L_f-L_d\right)}^2$$

(66)

The final tangent angle α_f characterizes the vehicle control stability at the end of the maneuver, directly affecting the safety of subsequent motion. The desired value α_d must be defined by the road curve’s tangent at the vehicle’s final position. Then

$$I_{\alpha }={\left({\alpha }_f-{\alpha }_d\right)}^2$$

(67)

The position L_d and angle α_f depend on the lane to which a maneuver is planned.

Distances to moving obstacles. Let us assume that the distance between AV and a moving obstacle changes continuously. Then, the integral of the distance square between AV and i-th vehicle has the form

$$S_{di}={\int }_0^{t_f}{d}_i^{2}dt={\int }_0^{s_f}{d}_i^2\frac{ds}{V\left(s\right)}={\int }_0^D{d}_i^2\frac{dx}{V_x\left(x\right)}=D{\int }_0^1\frac{d_i^2}{V_x\left(D\xi \right)}d\xi =D{\int }_0^1{z}_{di}\left(\xi \right)d\xi$$

(68)

Using the schemes described above, it can be written

$$z_{di}\left(\xi \right)=\frac{d_i^2\left({\mathit{Q}}_{tr},{\mathit{Q}}_{sp},D,\xi \right)}{V_x\left({\mathit{Q}}_{sp},D,\xi \right)},{\mathit{z}}_{di}=z_{di}\left({\mathit{Q}}_{tr},{\mathit{Q}}_{sp},D,\xi \left({\mathit{\vartheta }}^T\right)\right),S_{di}\approx \frac{D}{2}\mathbf{\Delta }\xi {\mathit{z}}_{di}\mathit{w},$$

(69)

Since each new motion variant is planned separately, only those n vehicles detected by the AV sensory system on the destination lane are considered. Thus, the influence of criterion is inversely proportional to the sum of integrals

$$I_d={\sum }_{i=1}^n\frac{1}{S_{di}}$$

(70)

Cost Function C_f is formed as the sum of the weighted criteria indicated above. The condition of minimizing the objective function has the view

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

(71)

where q = vector of the search-for parameters; I = vector of the integral criteria; W = vector of weight factors. Thus,

$$\mathit{W}=\left(\begin{array}{c}{W}_V\\ W_{j\zeta }\\ W_{j\mu }\\ W_t\\ W_L\\ W_{\alpha }\\ W_d\\ W_s\end{array}\right),\mathit{I}=\mathit{I}\left(\mathit{q}\right)=\left(\begin{array}{c}{I}_V\left(\mathit{q}\right)\\ I_{j\zeta }\left(\mathit{q}\right)\\ I_{j\mu }\left(\mathit{q}\right)\\ I_t\left(\mathit{q}\right)\\ I_L\left(\mathit{q}\right)\\ I_{\alpha }\left(\mathit{q}\right)\\ I_d\left(\mathit{q}\right)\\ I_s\left(\mathit{q}\right)\end{array}\right)$$

(72)

where W_V, W_jζ, W_jμ, W_t, W_L, W_α, W_d, W_s = weight coefficients for quadratic deviations of the speed, longitudinal and lateral jerks, final time, final lateral offset, final angular position, distances to obstacles, and final distance between vehicles at the end of maneuver, respectively.

4.2. Constraints

General approach. Kinematic, dynamic, and physical parameters of vehicle motion may be restricted. Usually, parameters’ values are constrained in the control points. Nevertheless, this does not guarantee the non-violation of limits between these points by a considered function. If it is supposed that all the parameters are represented by smooth functions, the integral approach to constraints may be implemented based on the same Gaussian scheme applied for the cost function calculation. Let us consider the general technique for some parameter ψ, which must not exceed the upper ψ_U and lower ψ_L bounds. Then, the sum of the areas between the upper limit and the ψ-function and between the lower limit and the ψ-function must be strictly equal to the area within limits. That is, if the ψ-function is determined relative to the time

$${\int }_0^{t_f}\left({\psi }_U-{\psi }_L\right)dt={\int }_0^{t_f}\left|{\psi }_U-\psi \right|dt+{\int }_0^{t_f}\left|\psi -{\psi }_L\right|dt$$

(73)

Each integral can be considered concerning the spatial coordinate by replacing the time differential. Integral between upper and lower bounds (ul):

$${\int }_0^{t_f}\left({\psi }_U-{\psi }_L\right)dt={\int }_0^{s_f}\left({\psi }_U-{\psi }_L\right)\frac{ds}{V\left(s\right)}=D{\int }_0^1\frac{\left({\psi }_U-{\psi }_L\right)}{V_x\left(D\xi \right)}d\xi =D{\int }_0^1{z}_{\psi }^{\left(ul\right)}\left(\xi \right)d\xi$$

(74)

Similar to Equations (16)–(18),

$$z_{\psi }^{\left(ul\right)}\left(\xi \right)=\frac{\left({\psi }_U-{\psi }_L\right)}{V_x\left(D\xi \right)},{\mathit{z}}_{\psi }^{\left(ul\right)}=z_{\psi }^{\left(ul\right)}\left(\xi \left({\mathit{\vartheta }}^T\right)\right),{\mathit{I}}_{\psi }^{\left(ul\right)}\approx \frac{D}{2}\mathbf{\Delta }\xi {\mathit{z}}_{\psi j}^{\left(ul\right)}\mathit{w},$$

(75)

Integral between the upper bound and ψ-function (uf):

$${\int }_0^{t_f}\left|{\psi }_U-\psi \right|dt={\int }_0^{s_f}\left|{\psi }_U-\psi \right|\frac{ds}{V\left(s\right)}=D{\int }_0^1\frac{\left|{\psi }_U-\psi \right|}{V_x\left(D\xi \right)}d\xi =D{\int }_0^1{z}_{\psi }^{\left(uf\right)}\left(\xi \right)d\xi$$

(76)

Thus,

$$z_{\psi }^{\left(uf\right)}\left(\xi \right)=\frac{\left|{\psi }_U-\psi \right|}{V_x\left(D\xi \right)},{\mathit{z}}_{\psi }^{\left(uf\right)}=z_{\psi }^{\left(uf\right)}\left(\xi \left({\mathit{\vartheta }}^T\right)\right),{\mathit{I}}_{\psi }^{\left(uf\right)}\approx \frac{D}{2}\mathbf{\Delta }\xi {\mathit{z}}_{\psi }^{\left(uf\right)}\mathit{w},$$

(77)

Integral between the ψ-function and lower bound (fl):

$${\int }_0^{t_f}\left|\psi -{\psi }_L\right|dt={\int }_0^{s_f}\left|\psi -{\psi }_L\right|\frac{ds}{V\left(s\right)}=D{\int }_0^1\frac{\left|\psi -{\psi }_L\right|}{V_x\left(D\xi \right)}d\xi =D{\int }_0^1{z}_{\psi }^{\left(fl\right)}\left(\xi \right)d\xi$$

(78)

Thus,

$$z_{\psi }^{\left(fl\right)}\left(\xi \right)=\frac{\left|\psi -{\psi }_L\right|}{V_x\left(D\xi \right)},{\mathit{z}}_{\psi }^{\left(fl\right)}=z_{\psi }^{\left(fl\right)}\left(\xi \left({\mathit{\vartheta }}^T\right)\right),{\mathit{I}}_{\psi }^{\left(fl\right)}\approx \frac{D}{2}\mathbf{\Delta }\xi {\mathit{z}}_{\psi }^{\left(fl\right)}\mathit{w},$$

(79)

The requirement of nonlinear equality constraint within the segment D is expressed as follows

$$c_{\psi }={\mathit{I}}_{\psi }^{\left(ul\right)}-{\mathit{I}}_{\psi }^{\left(uf\right)}-{\mathit{I}}_{\psi }^{\left(fl\right)}=0$$

(80)

Kinematic constraints. Using the scheme above, we may compose a vector c_k of nonlinear integral constraints for a set of kinematic parameters ψ_k, considering vectors of upper ψ_kU and lower ψ_kL limits.

$${\mathit{\psi }}_k=\left(\begin{array}{c}{V}_{\zeta }\\ \omega \\ \begin{array}{c}\epsilon \\ j_{\zeta }\end{array}\end{array}\right),{\mathit{\psi }}_{kU}=\left(\begin{array}{c}{V}_{\zeta U}\\ {\omega }_U\\ \begin{array}{c}{\epsilon }_U\\ j_{\zeta U}\end{array}\end{array}\right),{\mathit{\psi }}_{kL}=\left(\begin{array}{c}{V}_{\zeta L}\\ {\omega }_L\\ \begin{array}{c}{\epsilon }_L\\ j_{\zeta L}\end{array}\end{array}\right),{\mathit{c}}_k=\left(\begin{array}{c}{c}_{V\zeta }\\ c_{\omega }\\ \begin{array}{c}{c}_{\epsilon }\\ c_{j\zeta }\end{array}\end{array}\right)$$

(81)

Physical constraints. Subject to a relatively equal distribution of longitudinal and transverse forces between the AV wheels, the critical speed V_ζs criterion may be used stipulated by limiting the stable motion depending on the trajectory’s curvature [21] and tire-road adhesion. It can be derived as follows

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

(82)

where g = gravity acceleration constant, φ_μ = lateral adhesion coefficient.

If maximum tire-road adhesion is φ_max and the adhesion in the longitudinal direction requires a potential φ_ζ, the remaining lateral adhesion potential is

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

(83)

The longitudinal φ_ζ may be found through the equation of longitudinal dynamics in the dimensionless form

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

(84)

where f_r = coefficient of total rolling resistance, and f_d = specific drag force.

The longitudinal speed V_ζ must not exceed a threshold according to the condition V_ζ < V_ζs. V_ζs may change in a range [V_ζsmin, ∞). Thus, a rapidly saturating function may be applied, for instance, χ(·) = tanh(·). Then, the condition similar to Equation (73) is

$${\int }_0^{t_f}\chi \left(V_{\zeta s}\right)dt={\int }_0^{t_f}\left|\chi \left(V_{\zeta s}\right)-\chi \left(V_{\zeta }\right)\right|dt+{\int }_0^{t_f}\chi \left(V_{\zeta }\right)dt$$

(85)

Using Equations (74)–(80), it is possible to impose physical restrictions on the AV speed. Then

$${\mathit{\psi }}_p=V_{\zeta },{\mathit{\psi }}_{pU}=V_{\zeta s},{\mathit{\psi }}_{kL}=0,{\mathit{c}}_p=c_{V\zeta }$$

(86)

A special case concerns the vehicle’s traction potential. The vehicle’s maximum acceleration strictly depends on design features and decreases with growing speed. Thus, if the vehicle’s speed–acceleration characteristic is predefined, the following condition must be satisfied

$${\int }_{V_{\zeta \left(i\right)min}}^{V_{\zeta \left(i\right)max}}\left(a_{\zeta U}-a_{\zeta L}\right)d{V}_{\zeta }={\int }_{V_{\zeta \left(i\right)min}}^{V_{\zeta \left(i\right)max}}\left|a_{\zeta U}-a_{\zeta }\right|d{V}_{\zeta }+{\int }_{V_{\zeta \left(i\right)min}}^{V_{\zeta \left(i\right)max}}\left|a_{\zeta }-a_{\zeta L}\right|d{V}_{\zeta }$$

(87)

where a_ζU, a_ζL = upper and lower limit values of acceleration achievable by the vehicle.

The general scheme for obtaining integrals is the same as Equations (74)–(80). Then, the dynamic constraint has the form

$${\mathit{\psi }}_d=a_{\zeta },{\mathit{\psi }}_{dU}=a_{\zeta U},{\mathit{\psi }}_{dL}=a_{\zeta L},{\mathit{c}}_d=c_{a\zeta }$$

(88)

Boundary constraints. Another type of constraint determines boundary conditions of kinematic parameters. Since the initial (0) values defined in Equation (10) are known or may be evaluated directly, one can require (but not necessarily), for example, that the predicted final (f) values of acceleration and jerk would correspond to desirable values A_ζf and J_ζf. That is,

$${\mathit{\psi }}_b=\left(\begin{array}{c}{a}_{\zeta f}\\ j_{\zeta f}\end{array}\right),{\mathit{\psi }}_{bf}=\left(\begin{array}{c}{A}_{\zeta f}\\ J_{\zeta f}\end{array}\right),{\mathit{c}}_b={\mathit{\psi }}_b-{\mathit{\psi }}_{bf}=\mathbf{0}$$

(89)

Complete Set of Constraints. Combining all the parameters defined in the kinematic, physical, dynamic, and boundary constraints, the vectors of parameters, limits, and nonlinear constraints are as follows

$$\mathit{\psi }=\left(\begin{array}{c}{\mathit{\psi }}_k\\ {\mathit{\psi }}_p\\ {\mathit{\psi }}_d\\ {\mathit{\psi }}_b\end{array}\right),{\mathit{\psi }}_U=\left(\begin{array}{c}{\mathit{\psi }}_{kU}\\ {\mathit{\psi }}_{pU}\\ {\mathit{\psi }}_{dU}\\ {\mathit{\psi }}_{b0}\end{array}\right),{\mathit{\psi }}_L=\left(\begin{array}{c}{\mathit{\psi }}_{kL}\\ {\mathit{\psi }}_{pL}\\ {\mathit{\psi }}_{dL}\\ {\mathit{\psi }}_{bf}\end{array}\right),{\mathit{c}}_{eq}=\left(\begin{array}{c}{\mathit{c}}_k\\ {\mathit{c}}_p\\ {\mathit{c}}_d\\ {\mathit{c}}_b\end{array}\right)$$

(90)

Initial conditions. If the sought-for parameters from the previous cycle are known, they are automatically accepted as the initial ones for the next planning. To determine the initial parameters q_tr0 and q_sp0, the expressions of Equations (32)–(40) and (49)–(57) are to be used.

In Equation (10), the initial parameters for the first cycle are not set. To form geometric nodal parameters, first, the nearest point [x_r0, y_r0] corresponding to the zero-path length s_r0 is determined on the reference curve. This allows for the estimation of the initial distance L₀ to the reference provided that the center of the Cartesian coordinate system is tied to the vehicle’s mass center. Then, x₀ = 0, y₀ = 0. Assuming that at the initial time moment, AV follows parallel to the reference,

$${\left.\frac{dy}{dx}\right|}_0={\left.\frac{d{y}_r}{d{x}_r}\right|}_{s_r=0},\mathrm{s}\mathrm{u}\mathrm{b}\mathrm{j}\mathrm{e}\mathrm{c}\mathrm{t}\mathrm{t}\mathrm{o}{\left.\frac{dL}{dx}\right|}_0=0$$

(91)

We determine the next initial parameter from the curvatures of the trajectory and reference. By analogy with Equations (34) and (35)

$$K_r=\frac{y_{rx}^{″}}{s_{rx}^{\prime 3}},K_t=\frac{y_{tx}^{″}}{s_{tx}^{\prime 3}},K_t=\frac{1}{R_r-L}=\frac{1}{\frac{1-L{K}_r}{K_r}}=\frac{K_r}{1-L{K}_r}$$

(92)

where r, t = reference and trajectory, correspondingly.

Then,

$${\left.\frac{d^{2}y}{d{x}^2}\right|}_0=y_{tx}^{″}=K_t{s}_{tx}^{\prime 3},y_{tx}^{″}=\frac{s_{tx}^{\prime 3}}{1-L{y}_{rx}^{″}/s_{rx}^{\prime 3}}\frac{y_{rx}^{″}}{s_{rx}^{\prime 3}}=\frac{y_{rx}^{″}{s}_{rx}^{\prime 3}}{s_{rx}^{\prime 3}+L{y}_{rx}^{″}}$$

(93)

Therefore, due to Equations (29) and (91), for the points [x_r0, y_r0] и [x₀, y₀]

$$s_{tx}^{\prime }=s_{rx}^{\prime },s_{tx}^{″}=s_{rx}^{″}$$

(94)

To work with speed derivatives, we need the trajectory’s third derivative, which can be obtained from Equation (93)

$$y_{tx}^{‴}=\frac{d}{dx}\left(\frac{y_x^{″}{s}_{tx}^{\prime 3}}{s_x^{\prime 3}+L{y}_x^{″}}\right)=\frac{\left(s_{tx}^{\prime 3}{y}_x^{‴}+3y_x^{″}{s}_{tx}^{\prime 2}{s}_{tx}^{″}\right)\left(s_x^{\prime 3}+L{y}_x^{″}\right)-\left(y_x^{″}{s}_{tx}^{\prime 3}\right)\left(3s_x^{\prime 2}{s}_{tx}^{″}+y_x^{″}\frac{dL}{dx}+L{y}_x^{‴}\right)}{{\left(s_x^{\prime 3}+L{y}_x^{″}\right)}^2}$$

(95)

Or after considering Equation (91)

$$y_{tx}^{‴}=\frac{s_{rx}^{\prime 2}\left(\left(s_{rx}^{\prime }{y}_{rx}^{‴}+3y_{rx}^{″}{s}_{rx}^{″}\right)\left(s_{rx}^{\prime 3}+L{y}_{rx}^{″}\right)-y_{rx}^{″}{s}_{rx}^{\prime }\left(3s_{rx}^{\prime 2}{s}_{rx}^{″}+L{y}_{rx}^{‴}\right)\right)}{{\left(s_{rx}^{\prime 3}+L{y}_{rx}^{″}\right)}^2}$$

(96)

If the reference is mathematically predefined, the first three derivatives are easy to obtain.

To define the initial speed parameters, the values V_ζ0, a_ζ0, j_ζ0, and the expressions Equations (47)–(57) are used.

5. Simulation

We use the data of the Audi A4 3.2 FSI [23] to represent the AV. All the calculations are accomplished by using MATLAB tools [24]. The basic function to realize the optimization procedure is fmincon. The five-point Gauss quadrature scheme is accepted for the numerical integration, and the relative path step is Δξ = 0.1.

Let us define constraints and initial values. Speed

$$V_{\zeta 0}=60\left[\frac{km}{h}\right],V_{\zeta U}=80\left[\frac{km}{h}\right],V_{\zeta L}=50\left[\frac{km}{h}\right]$$

(97)

Acceleration

$$a_{\zeta U}={\phi }_{max}g\left[\frac{m}{s^2}\right],a_{\zeta L}=-0.5\left[\frac{m}{s^2}\right],a_{\zeta 0}=a_{\zeta f}=0\left[\frac{m}{s^2}\right],a_{\zeta lim}=a_{\zeta lim}\left(V_{\zeta }\right)$$

(98)

where a_ζlim = function of the vehicle throttle response characteristic.

Jerk

$$j_{\zeta U}=13\left[\frac{m}{s^3}\right],j_{\zeta L}=-6.5\left[\frac{m}{s^3}\right],j_{\zeta f}=0\left[\frac{m}{s^3}\right],$$

(99)

Yaw rate

$${\omega }_U=0.5\left[\frac{rad}{s}\right],{\omega }_L=-0.5\left[\frac{rad}{s}\right]$$

(100)

Angular acceleration

$${\epsilon }_U=3\left[\frac{rad}{s^2}\right],{\epsilon }_L=-3\left[\frac{rad}{s^2}\right]$$

(101)

First, let us set the values of the weight coefficients W in Equation (72).

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

(102)

Considering the order of variable parameters, the coefficient values were selected based on the condition of the influence proportionality but with an emphasis on safety and maneuver precision.

Let us define constraints and initial values. Speed

$$V_{\zeta 0}=60\left[\frac{km}{h}\right],V_{\zeta U}=80\left[\frac{km}{h}\right],V_{\zeta L}=50\left[\frac{km}{h}\right],$$

(103)

Acceleration

$$a_{\zeta U}={\phi }_{max}g\left[\frac{m}{s^2}\right],a_{\zeta L}=-0.5\left[\frac{m}{s^2}\right],a_{\zeta 0}=a_{\zeta f}=0\left[\frac{m}{s^2}\right],a_{\zeta lim}=a_{\zeta lim}\left(V_{\zeta }\right)$$

(104)

where a_ζlim = function of the vehicle throttle response characteristic.

Jerk

$$j_{\zeta U}=13\left[\frac{m}{s^3}\right],j_{\zeta L}=-6.5\left[\frac{m}{s^3}\right],j_{\zeta f}=0\left[\frac{m}{s^3}\right],$$

(105)

Yaw rate

$${\omega }_U=0.5\left[\frac{rad}{s}\right],{\omega }_L=-0.5\left[\frac{rad}{s}\right]$$

(106)

Angular acceleration

$${\epsilon }_U=3\left[\frac{rad}{s^2}\right],{\epsilon }_L=-3\left[\frac{rad}{s^2}\right]$$

(107)

Experiment 1. Let us consider variants for the AV motion planning along a curved road section with three lanes (Figure 4) under the condition of perfect adhesion with φ_max = 0.85. Note that many variants are possible regarding the lateral vehicle displacement L. However, for the sake of compactness, we built three prediction variants related to the lane change along the lanes’ midlines. The AV is surrounded by four moving obstacles denoted as v_i, i ∈ [1, 2, …, 4]. Further, Index 0 denotes the initial position, and f—is the final position. The initial vehicles’ speeds were, respectively, V_i0 = (45, 50, 50, 60) km/h.

As can be seen, moving to Lane 1 requires the greatest change in curvature and, accordingly, a larger basis than for passing to Lane 3. At the same time, the smallest change in curvature on Lane 2 and the distant position of the impeding vehicle allows for the maximum acceleration and length of the prediction basis. Note that while driving along Lane 2, the trajectory forecast does not tend to coincide with the centerline but is formed in such a way as to ensure the least curvature.

Figure 5 depicts the results of forecasts for speeds and accelerations along with restrictions. The speed limits under the sideslip condition are shifted to the maneuver’s beginning movement to Lane 1 and are shifted to the final phase for passing to Lane 3. The restrictions for following Lane 2 are significantly higher than the speed limit V_ζmax and are not reflected in the figure’s boundaries. However, the speed in Lane 2 does not increase as the acceleration reaches the limit of the powertrain’s potential, and the final state of AV requires zero longitudinal acceleration. Note that the curvatures in the maneuvers’ final phases are approximately the same, which is reflected in the final values of lateral accelerations that are close in modules (Figure 5c). As seen in Figure 5d, not a single total acceleration curve exceeds the limit a_max by the adhesion properties.

Figure 6 presents the kinematic and geometric parameters used to impose restrictions. As seen in Figure 6a, all the longitudinal jerk curves are well below the upper limit and do not exceed the lower limit. An important indicator is the smoothness and unambiguity of the yaw rate and acceleration curves (Figure 6b,c). This characterizes the strict stability of the yaw angle and the absence of uncontrolled dynamic phenomena. Figure 6d shows that AV increases the distance from vehicles located behind and reduces gaps with the vehicles ahead while keeping safe spaces.

Figure 7 reflects the geometric parameters characterizing the maneuvers’ trajectories. As seen in Figure 7a, all curvature curves are extremely smooth and concordant with the polynomial used for the trajectory model. The steering angle (Figure 7d) scales the curvature due to the ideal kinematic model. The curvature derivatives (Figure 7b,c) are also smooth without intense oscillations and sharp spikes, characterizing a consistent speed distribution along the trajectory’s curvature.

Experiment 2. Now, we consider the motion-planning variants (Figure 8) under the same conditions as for Experiment 1 but with worse adhesion φ_max = 0.5. In this case, more time and distance are needed to complete the maneuver since the traction potential is limited by an external factor. Therefore, the length of the required path s is closer to the upper limit of the reference s_r length.

As seen, the nature of the maneuvers differs in phase. Thus, on Lane 2, there is no significant change in curvature, and AV may be quickly accelerated. For the maneuver to Lane 1, the intensity is shifted to the initial phase, and for the maneuver to Lane 2, it is shifted to the final phase.

Figure 9 depicts the results of forecasting speeds and accelerations under the condition of reduced tire-road adhesion. Accordingly, the critical speed values, caused by the locally increased curvature, also decrease and limit the increase in AV speed for maneuvers to Lanes 1 and 3 (Figure 9a). At the same time, the curvature of the trajectory for driving on Lane 2 changes little, which allows the speed to rise since the critical one, in this case, is greater than the preset maximum. The same is noted for longitudinal accelerations with a difference that the vehicle, naturally, cannot realize its full traction potential. As seen in Figure 9d, the maximum acceleration of the maneuver to Lane 1 is formed at the beginning of the lane change, and for Maneuver 3 at the end. With that, accelerations do not exceed the limit achievable under the conditions of full adhesion.

Figure 10 presents the kinematic parameters used as constraint criteria. As seen, all curves meet the requirements of smoothness and uniqueness, and their extremum values are significantly less than the preset limit ones. The yaw rate and angular acceleration for maneuvering along Lane 2 are noticeably lower than others due to the stability of the trajectory curvature. This also allows using a wider range of the jerk than for passing to Lanes 1 and 3.

Figure 11 shows the trajectory geometric parameters relative to the time. The greatest work on vehicle control occurs during the maneuver to Lane 1. However, the adhesion is reduced, the curves are stable, situated within the established limits, and the vehicle control is completely consistent with the kinematic characteristics in Figure 10.

6. Conclusions

The study has developed a technique for planning the AV motion ensuring simultaneous optimization of the trajectory and speed mode. The proposed technique is fully workable and provides results that are adequate to the initial conditions and preset restrictions. Note that the obtained results are characterized for the AV with a drive type in which traction is redistributed by all wheels, since the critical speed criterion is related to the entire vehicle and not to a single drive axle. Also, in the calculations, it was assumed that the minimum length of the maneuver trajectory should be greater than the AV’s stopping distance from the initial speed.

Regarding the results of testing the technique, the following conclusions can be drawn.

• A clear advantage is the use of a 5th-degree polynomial as a trajectory basis possessing sufficient flexibility and good stability (i.e., predetermined trajectory shape), which reduces the time required to optimize the trajectory model. All curves formed with the participation of the trajectory model are characterized by smoothness, unambiguity, and simplicity of form, providing a positive effect for the control stability at the tracking stage.
• The proposed technique is based on the minimum number of variable parameters (5–7) necessary for high-quality prediction of the maneuver and its speed mode. Note that despite the multiple optimization criteria, it is recommended to shift priorities to control the maneuver’s accuracy (lateral displacement L) and safety (distance between objects during lane changes and the final distance between vehicles within the same lane).
• The variation of the lateral displacement L within the lane boundaries allows for the obtainment of several trajectory variants. However, in this study, we relied on minor deviations from the lane’s centerline. In this regard, for simulation examples, only three forecasting options were used. The priorities for decision-making on choosing the best trajectory [25] can be the minimum travel time, maximum speed at the end of the maneuver, the longest trajectory, minimum control costs, or a combination of these. In this study, we do not focus on the problem of selecting a criterion since the goal was to verify the technique itself in general.
• For simulations, a device equipped with an Intel(R) Core(TM) i7-7500U CPU @ 2.7 GHz, two cores, and 8 GB RAM on the 64-bit Windows 10 was used. The software environment was MATLAB R2022b with the basic optimization function fmincon. The average time to calculate one forecast is about 0.35–1.4 s depending on the settings, declared accuracy, and initial conditions. The performance can be significantly enhanced by optimizing the MATLAB code, using parallel computing, compiling to the C/C++ language, and applying more efficient multi-core processors.
• The accuracy of the constraints plays a key role in the solution convergence and needed number of iterations. Due to the integral approach to equality constraints, the sensitivity to the residual order increases. Thus, for the guaranteed convergence of equality constraints on the criteria of critical speed, acceleration, and jerks, the recommended accuracy should correspond to the order of 10⁻¹².
• The choice of a number of integration points for the Gaussian quadrature scheme and some sections into which the finite element interval is divided significantly affects the forecast performance. Multiple virtual simulations have revealed that the optimal interval step is Δξ = 0.2... 0.25, and the number of integration points is N = 3... 5.
• The following may be noted as disadvantages. When forecasts are subsequent, the conjunction smoothness at transition nodes is ensured only for parameters that require no more than the second derivative of the trajectory and speed. At the same time, for some parameters, such as curvature derivative, angular acceleration, and jerk, discontinuities of functions at nodes may appear. To eliminate these shortcomings, it is necessary to shift the modeling emphasis from trajectory and velocity to the level of the curvature’s and speed’s derivatives. This approach will be studied further.