# Optimal Speed Plan for the Overtaking of Autonomous Vehicles on Two-Lane Highways

^{*}

## Abstract

**:**

## 1. Introduction

^{2}-splines. The polynomial fitting involved iterations equivalent to the optimization of curvature parameters. To simulate a sufficiently long path, a fifth-degree polynomial was used. However, such polynomials are often unstable between nodes. For the speed distribution, a third-degree polynomial was proposed, where the transition time was determined based on the values of parameters (speeds and accelerations) at the end points. Acceleration was limited to the maximum value and was not related to the parameters of the power plant. Not enough information about the overtaking maneuver parameters was provided. González et al. [10] reviewed the methods used to plan autonomous vehicle movement. Graph algorithms were mainly used to determine the minimum maneuver path on the surrounding space grid. The State Lattice algorithm executed path searching using state-space mesh generation. Sampling-Based Planners generated random state-spaces and looked for their ties. A rapidly exploring random tree (RRT) made it possible to use structured spaces. Lines and circles, closed curves, polynomial curves, Bézier curves, and spline curves were used to represent the path forecast. A numerical method for optimizing a function subject to different constraints was used.

## 2. System Description

#### 2.1. Overtaking Phases

_{pb}, and during this time, the impeding vehicle travels the distance X

_{ib}. In Phase b (obstacle front reach), the passing vehicle travels from the bypass point (State 2) to the critical point (State 3), where its front aligns with that of the impeding vehicle. During this time, the passing and impeding vehicles travel distances X

_{po}and X

_{io}, respectively.

_{pf}to ensure an adequate safe distance d

_{f}between itself and the impeding vehicle, traveling the distance X

_{if}, as shown in Figure 1c. The passing and opposing vehicles must provide a safety margin distance X

_{m}between their fronts which is equivalent to the minimum safety margin time t

_{mm}. Note that in this phase, the lateral movement back to the original lane will not start until the passing vehicle has already overtaken the impeding vehicle.

#### 2.2. Assumptions

- (a)
- The roadway is assumed to be straight, with ideal surface friction and no external forces, such as gusts of wind. In addition, the road is assumed to have very gentle vertical curvature that would not obstruct sensor measurements;
- (b)
- Before the overtaking maneuver starts, it is assumed that the estimation of the vehicles’ positions has already been carried out, the forecast has been made, and the passing vehicle is ready to start the passing maneuver. In this state, the distance D
_{0}between the passing and opposing vehicles and the distance d_{0}between the impeding and passing vehicles are estimated using long- and short-range radars, respectively; - (c)
- The length of the impeding vehicle is estimated using machine vision technology. The sensor measurements (distance and angle) are assumed to be independent;
- (d)
- The AV is assumed to possess the input data required for the proposed model: roadway data (e.g., lane width and speed limit) and vehicle characteristics (e.g., acceleration–speed relation);
- (e)
- Only the passing vehicle is autonomous, and the impeding and opposing vehicles are assumed to be human-driven vehicles. The overtaking maneuver involves only one impeding vehicle;
- (f)
- In the heuristic algorithm, it is assumed that the actual speed fluctuations detected by the radars remain within the uncertainty thresholds, for which the forecast is considered reliable. Therefore, the changes of vehicle locations will occur within the boundaries determined by the specified measurement thresholds. If the threshold is exceeded, the forecast should be recalculated;
- (g)
- Uncertainty is considered in the estimation of the speeds of the passing and impeding vehicles. Based on the uncertainty thresholds, possible violations of the thresholds are analyzed. No uncertainty propagation throughout the prediction process is considered in this study;
- (h)
- The algorithm continuously works with the vehicle sensory system within the maneuver and only estimates new overtaking parameters when thresholds are violated, thus helping to reduce the load on the on-board computer system.

#### 2.3. Logic of the Speed Control Model

_{p}and distance X

_{p}required for safe maneuver completion are established using a heuristic algorithm, as described later. These variables are used for determining the optimal distribution of speeds and trajectory planning of the maneuver. Third, quadratic optimization is used to develop a smooth curve for the path of the passing vehicle that can serve as a reference for the control law implementation during maneuver realization.

_{p}, the system continues with the last prediction and is updated if the speed thresholds are violated. Otherwise, the maneuver is successfully completed.

#### 2.4. Establishing Operational Speed Thresholds

_{i}and the corresponding azimuth angles θ

_{i}are recorded, where the polar coordinates used with the origin point lie with the sensor location. The deterministic distance crossed by the vehicle in consecutive time intervals was presented by Hassein et al. [22] (2018):

_{oppi}denotes the distance traveled by the opposing vehicle during Δt and dp

_{i}denotes the distance traveled by the passing vehicle during Δt. The speed of the opposing vehicle, V

_{opp}, can then be derived as

_{i}and θ

_{i}measurements of the radar be denoted by e

_{d}and e

_{a}, respectively. Then, these errors will propagate and produce an error in V

_{opp}of Equation (2). To calculate this error, let the four variables of Equation (1) (d

_{i}, θ

_{i}, d

_{i+1}, and θ

_{i+1}) be denoted by x

_{i}, where i denotes 1 to 3. Using the Taylor series, the standard deviation of Y, σ

_{y}, was given by Benjamin and Cornell [23]:

_{xi}denotes the standard deviation (SD) of random variable x

_{i}. By applying Equation (3) to Equation (1), the standard deviation of dV

_{oppi}, σ

_{dVoppi}, can be derived as

_{Vopp}, is obtained as

_{i}− imp

_{i}, σ

_{dVimpi}, of Equation (6) is given by

_{imp}and its standard deviation σ

_{Vimp}are calculated using Equations (2) and (5), after replacing dV

_{oppi}with dV

_{impi}(i denotes 1, 2, and 3).

_{opp}± 2·σ

_{Vopp}and V

_{imp}± 2·σ

_{Vimp}, respectively.

## 3. Heuristic Algorithm

#### 3.1. General

_{(−m)}and D

_{(−m)}, respectively, relative to the passing vehicle, where the index (−m) means the backward number of radar measurement cycles required before the prediction is made. Considering the time required for prediction, the values at the maneuver’s beginning time t

_{0}will become d

_{0}and D

_{0}, respectively. If the opposing and impeding vehicles continue their motions at speeds close to the measured ones, then the change in their positions will be approximately linear, which determines the slope (dX/dt) of the corresponding curves (blue and green curves in Figure 3). In turn, the speed measurements also have uncertainty.

**a**(t

_{min}, X

_{pmin}) corresponds to the minimum time and distance, while Point

**b**(t

_{max}, X

_{pmax}) corresponds to the maximum time and distance. Consider the vehicle’s maximum performance with a full fuel supply along the

**oa**curve. Considering the mean performance at Point

**f**(t

_{p}, X

_{p}), a distance (d

_{f}+ L

_{p}) would be needed to complete the maneuver, where the linear segment

**ab**represents a set of solutions that correspond to the desired time and distance of overtaking. To predict the minimum distance required for overtaking completion, the passing vehicle speed at the maneuver’s end, and the minimum safe distance between the passing and impeding vehicles (Figure 1c), the space d

_{fmin}(Figure 3) should be predetermined.

_{mm}guarantees a distance between the passing and opposing vehicles after maneuver completion. This corresponds to Point

**c**on the segment

**ab**. The

**oc**curve will represent the lower boundary of the field

**oac**of the valid time–distance realizations. If the speed changes of the opposing and impeding vehicles remain within the threshold values, the lower boundary of the opposing vehicle’s distance dependency with the basis (instant mean) line

**yp**will not reach Point

**c**, maintaining the safety margin until time t

_{s}. This may be the key point for determining the threshold conditions.

**yp**, and the impeding vehicle moves along the nominal straight line

**rp**. Then, the nominal distance (d

_{f}+ L

_{p}) for completing the maneuver (segment

**bk**) will correspond to t

_{a}. Considering t

_{mm}, the new boundary will imply the safety limit at time t

_{s}. Now, it is necessary to choose a point on segment

**ac**that would meet the required criteria. There are many approaches that can be employed to achieve this. One of the possible approaches is to use the trapezoid

**hlnj**to proportionally split segment

**ac**. That is, the vertical lines of the intersection Point

**z**of the trapezoid

**hlnj**diagonals correspond to intersect line

**ac**at Point

**f.**Segments

**fm**and

**fi**characterize the distances to the opposing and impeding vehicles, respectively. In this case, Point

**f**determines the t

_{p}and X

_{p}required for the passing vehicle. At this moment, the distance traveled by the opposing vehicle is X

_{o}, which corresponds to its final position X

_{of}(Point

**x**). This approach ensures a stable and gradual redistribution of Point

**f**by increasing the minimum safety margin.

**oa**and a lower limit that achieves the safety margin

**oc**. The search for rational values and the law of speed change in the third phase (Figure 1c) depends on the difference between the speeds of the passing and impeding vehicles, which may exhibit values from the admissible minimum to the maximum being stipulated by the full performance mode (i.e., the upper limit

**oa**at the beginning and completion of the lane change, Figure 3). Obviously, there is a need to optimize the movement trajectory in such a way as to ensure that both criteria (safety and power margins) account for possible changes in maneuver conditions (e.g., vehicle speeds and/or unpredictable forces).

#### 3.2. Vehicle Performance Thresholds

**oa**) represents the vehicle potential provided with a full fuel supply under the ideal conditions of motion. The definition of the upper limit can be based on the characteristic of the vehicle dynamic factor (specific free traction force) restricted by the conditions of road surface adhesion and the reduced total movement resistance, including the road macro-profile. This upper limit may be estimated using mapping and GPS. The excess of the dynamic factor can be employed to accelerate the vehicle. Therefore, it is possible to build the speed–time and time–distance dependences for the acceleration mode, by which the necessary overtaking time and distance can be determined using the iterative method considered by Diachuk at al. [24]. The obtained values will represent the vehicle performance on a straight road section. To account for the curvilinear trajectory of the maneuver, the values of the time–distance curve can be adjusted.

**a**(Figure 5), the safety margin t

_{mm}to Point

**b**. The ratio of the inclination angles of opposing and impeding vehicles’ curves, providing an intersection in Point

**p**and adequate space d

_{f}+ L

_{p}to complete the maneuver, will ensure the time limit and the possible path of the maneuver. It is obvious that with a larger inclination angle of linear prediction for the opposing vehicle, the sensitivity’s influence on the remaining time safety margin t’

_{mm}is decreased. This does not mean that the vehicle will not be able to use the power corresponding to the segment

**ab**; however, the maneuver execution in this mode will be associated with a decrease in the guaranteed level of safety. Therefore, the condition of the maneuver possibility is t

_{a}> t

_{min}+ t’

_{mm}, where t’

_{mm}≤ t

_{mm}due to the lower sensitivity.

_{min}− t

_{l}according to the measured and evaluated data will be quite wide, which may lead to the calculation of large values of t

_{p}having no real meaning because of predicting a protracted maneuver. In this regard, the maximum maneuver time should also be limited based on the ratio of the times t

_{min}and t

_{a}, t

_{l}.

#### 3.3. Mathematical Formulation

**of**, suppose that the measurements are evaluated at intervals of Δt

_{m}based on the preset frequency of the radar system. Then, the current discrete time will be n·Δt

_{m}, where n takes both positive and negative values relative to the starting point of the maneuver (sample n = 0). The results of the relative measurements at time t

_{n}are as follows:

_{n}denotes the current measured distance between the passing and opposing vehicles; v

_{on}denotes the current measured opposing vehicle speed; d

_{n}denotes the current measured distance between the passing vehicle and the rear of the impeding vehicle; v

_{in}denotes the current measured impeding vehicle relative speed; and X

_{pn}and V

_{pn}denote the estimated current self-position and speed of the passing vehicle, respectively.

_{pr}and computational resources. In this regard, the algorithm should be organized to avoid frequent recalculations that do not significantly affect the quality of the forecast. Therefore, the time Δt

_{pr}must be a multiple of the time Δt

_{m}(Δt

_{pr}= m·Δt

_{m}), where m is the factor of cycle multiplicity. This is provided that to process the forecast, the time Δt

_{pr}= t

_{0}− T

_{−1}is needed (where T

_{−1}is the time before the forecast is made until t

_{0}) and that during this period, the vehicle speed does not change significantly, i.e.,

_{0}) = k (x − x

_{0}). The potential global positions of the opposing and impeding vehicles are determined relatively through the predicted movement of the passing vehicle. Therefore, while performing the maneuver, the current state vector assessment (X

_{pn}, V

_{pn})

^{T}is periodically recalculated based on the sensor fusion technology. Then,

_{l}within the overtaking pocket, the condition for the intersection of linear predictions at Point

**p**corresponding to t

_{l}is X

_{o}= X

_{i}. Note that V

_{o}and the time at the moment before the forecast are negative, thus T

_{−1}= − Δt

_{pr}= − m·Δt

_{m}. Consequently, the initial measurement is carried out in m cycles before the maneuver starts. That is,

_{−1}relative to t

_{0}is estimated as X

_{p}

_{(−m)}≈ −V

_{p}

_{(−m)}·Δt

_{pr}.

_{a}, the minimum distance X

_{o}− X

_{i}= d

_{fmin}+L

_{p}is the distance between the impeding and passing vehicles. Therefore, t

_{a}can be defined by the difference (d

_{fmin}+ L

_{p}) between the functions

**yp**and

**rp**, similar to Equation (12). That is,

**z**of the diagonals’ intersection, the distances at Points

**l**,

**h**,

**n**, and

**j**corresponding to t

_{min}, t

_{s}, using Equation (12), are

_{p}is the equality of the ordinates of segments X

_{lj}and X

_{hn}:

**ab**can be determined based on the coordinates of two points:

_{p}corresponding to time t

_{p}is

_{p}and X

_{p}(Figure 3), which can be used for the optimal distribution of speeds and trajectory planning of the maneuver.

## 4. Quadratic Optimization Model

#### 4.1. General

_{p}and X

_{p}are determined using the heuristic algorithm previously described, the desired trajectory of motion is determined using a kinematic model. Such a model makes path planning simpler and faster and provides a smooth curve that can be adjusted, depending on the priorities of the kinematic parameters. This curve can serve as a reference for the control implementation of laws of the autonomous vehicle during maneuver realization.

_{p}, X

_{p}) are determined before the maneuver starts at time t

_{0}. Obviously, there are many realizations of the distribution of the speed’s longitudinal component, such that their integral over the time interval (t

_{p}− t

_{0}) equals the distance X

_{p}. These curves will at least differ in the value of V

_{X}and its derivative dV

_{X}/dt (acceleration) at the nodal points.

#### 4.2. Objective Function

_{V}, W

_{A}, and W

_{S}denote the weighting factors of speed, acceleration, and sharpness, respectively; and J

_{V}, J

_{A}, and J

_{S}denote corresponding integral functions.

**q**(see Equation (A4), Appendix A) is omitted.

_{i}_{p}, considering Equations (A1), (A3), (A8), and (A10) (Appendix A). For the integral function J

_{V’’},

**g**= (

**g**

_{b1},

**g**

_{b2}, …,

**g**

_{bn})

^{T}, E = (E

_{4}, E

_{4}, …, E

_{4})

^{T}, E

_{4}denotes the identity matrix of the dimension 4 × 4, and M

_{q}denotes the transition matrix from the vector

**q**of degrees of freedom to the vector

**q**

_{f}of repeating degrees of freedom of all finite elements (FE). Note that in adjacent FE, the values of the nodes on the right and left are repeated in the vector

**q**

_{f}(e.g., q

_{3i}and q

_{4i}are equal to q

_{1}

_{(i+1)}and q

_{2}

_{(i+1),}respectively). Therefore, the excess degrees of freedom must be reduced by grouping node values instead of FE. That is, q

_{f}= M

_{q}·q.

^{2}(t), considering Equation (A11) (Appendix A):

_{4}= zero matrix (4 × 4).

#### 4.3. Constraints

#### 4.3.1. Lane Change-Related Constraint

_{t}(Figure 3). Then, the longitudinal component of the path is

_{t}can be determined iteratively (Figure 3) after the distribution of the longitudinal speed, based on Equations (27) and (28), considering X

_{pb}= X

_{p}(t

_{t}). According to a possible slight decrease in the impeding vehicle speed, X

_{pb}may correspond to a small longitudinal gap between the front of the passing vehicle and the rear of the impeding vehicle. Thus,

_{Y}(t) is represented similar to Equation (21).

#### 4.3.2. Location in Opposite Lane Constraint

_{po}may correspond to its position in the middle of the opposite lane. Possible deflections of this position are restricted by safe clearance to the road edge (Figure 1b). A lower limit of this clearance is the minimum safe distance between passing and impeding vehicles. Therefore, for X

_{po}and Y

_{po},

_{c}can be found iteratively according to the condition when the passing and impeding vehicles are abreast at the critical point (State 3, Figure 1b), considering X

_{p}(t

_{c}) = X

_{pb}+ X

_{po}(Point

**e**, Figure 3).

#### 4.3.3. Maneuver Completion

#### 4.4. Preparing the Reference Trajectories

_{X}, V

_{Y})

^{T}in global coordinates are determined. Therefore, it is necessary to transfer the speeds to the local coordinates of the passing vehicle (V

_{x}, V

_{y})

^{T}, in order to allow it to consider its maneuvering. Since the yaw angle φ is small,

_{p}, Y

_{p}; speeds in local coordinates V

_{x}, V

_{y}; and the yaw angle φ. As additional parameters, which can be directly measured on a vehicle, the accelerations that are components of the optimized speed plans reduced to the vehicle local coordinates may be used, as well as the yaw rate, which can be estimated indirectly as dφ/dt considering Equation (36).

## 5. Updating the Speed Plan

**f**are shown, where the threshold values of changes in speeds of the opposing and impeding vehicles are reached. In Figure 7a1, the speed of the impeding vehicle increases in such a way that the linear curve exceeds the upper boundary prior to the moment t

_{a}, and the segment

**b’k’**slightly goes up (green), along the path curve of the impeding vehicle. Basically, the value of the minimum required distance d

_{fmin}depends on the difference between the speeds of the passing and impeding vehicles, and, thus, will vary with the fluctuations in movement modes of the overtaking participants. However, the changes will not have a significant affect, and therefore, d

_{fmin}can be considered constant in the vicinity of Point t

_{a}. The determination of d

_{fmin}is described elsewhere [24,25]. The bias of the intersection point of the trapezoid diagonals in

**z’**leads to shifting of the optimal Point

**f’**up to the left. The required time t’

_{p}becomes shorter and the needed space X’

_{p}becomes larger. This may be explained by the significant sensitivity of the forecast to the impeding vehicle’s speed changes.

**ab**and

**ab’**are practically the same. However, even though the required time has decreased t’

_{p}< t

_{p}, unlike the previous case, the required space X’

_{p}decreases due to the larger space needed for the opposing vehicle. The most critical case is when the speed fluctuations of both the opposing and impeding vehicles reach the threshold boundaries simultaneously (Figure 7a3). The displacement of the minimum space segment d

_{fmin}+ L

_{p}for completing the maneuver can be so significant that the time t’

_{s}approaches the preset time t

_{p}, and the margin t

_{mm}in relation to the minimum performance mode will not be provided. Point t’

_{p}is located the furthest from Point t

_{p}, even though the space required for the maneuver may remain almost unchanged X’

_{p}≈ X

_{p}. Similarly, a decrease in the speed of opposing and impeding vehicles will give a lower limit of time fluctuations

**f**(Figure 3). However, such decreases are not dangerous, and it makes sense to only recalculate the forecast to save energy and increase the movement stability. Based on the described scheme, it is possible to determine the allowable level of deviations, at which the margin of minimum safety is kept without the necessary recalculation.

_{L}_{p}< X

_{p}and t’

_{p}> t

_{p}. For the case in Figure 7b2, the diminished opposing vehicle speed demonstrates the need for a longer distance and time because of the reduced space X

_{o}for the opposing vehicle: X’

_{p}> X

_{p}and t’

_{p}> t

_{p}. For the case in Figure 7b3, the diminished speeds of both opposing and impeding vehicles move Point

**z’**quite far from

**z**, providing a longer time in almost the same space: X’

_{p}≈X

_{p}and t’

_{p}> t

_{p}. This may cause the double margin time t

_{mm}with unreasonable energy consumption. Therefore, the passing vehicle speed mode may be reduced.

_{n}are available prior to the moment of alignment with the rear of the impeding vehicle, after which the last d

_{n}value may be fixed, and measurements of D

_{n}can be carried out until the critical point. Recalculation after the critical point is possible if the next impeding vehicle appears in the lane, which does not provide a proper pocket or harshly reduces its speed.

**f**,

_{U}**f**(Figure 3), forecast recalculation is not required. Therefore, the autonomous control system must adjust the speed mode of the passing vehicle not only according to measurement changes, but also considering the matching with its own reference curve (

_{L}**of**in Figure 3).

_{a}denotes the instantaneous value of the hypothetical accident time compatible with predefined t

_{a}; t

_{pr}= Δt

_{pr}+ p·Δt

_{m}= (m+p)·Δt

_{m}, where p denotes the number of spare measurement cycles, by default, p = 2; and t

_{un}denotes the unaccounted time expenses (e.g., engine transition mode and control delay).

_{a}can be recalculated using Equation (13) for every

**n**-th measurement at t

_{n}as follows:

## 6. Overtaking Scenario modeling

#### 6.1. Vehicle Model Description

**x**denotes the state vector;

**y**denotes the output vector;

**u**denotes the control vector; and A, B, C, and D are matrices.

_{x}from Equation (34), and the state vector

**x**only contains parameters for the front wheel steering control

**u**= Θ

_{f}. Therefore,

**x**= (V

_{y}, Y, ω, φ)

^{T}, where Y denotes the lateral displacement in global coordinates, and ω = dφ/dt denotes the yaw rate. Other parameters are denoted above. Provided D = 0, the matrices A, B, and C can be derived as

_{f}and k

_{r}denote the front and rear tires’ side stiffness, respectively; and x

_{f}and x

_{r}denote the local longitudinal coordinates of the front and rear tire spots, correspondingly. Therefore, the output variables for reference tracking are

**y**= (Y, φ)

^{T}, which, in the real world, can be measured using a camera and sensors.

#### 6.2. Adaptive Model Predictive Control Tracking Optimization Problem

_{y}, Q

_{u}, and Q

_{∆u}denote positive semi-defined weight matrices; y

^{*}

_{k+i+1}

_{|k}denotes the Plant output reference signals at the ith prediction horizon step; y

_{k+i+1}

_{|k}denotes the Plant outputs at the ith prediction horizon step; u

^{*}

_{k+i}

_{|k}denotes the Plant target reference signals at the ith prediction horizon step; u

_{k+i}

_{|k}denotes the Plant inputs (manipulated variables) at the ith prediction horizon step; z

_{k}= (u

^{T}

_{k|k}, u

^{T}

_{k+1|k}, ··· u

^{T}

_{k+p−1|k}, ε

_{k}) denotes the solution; ε

_{k}denotes the scalar dimensionless slack variable used for constraint softening at control interval k; ρ

_{ε}denotes the constraint violation penalty weight; k denotes the current control interval; and p denotes the prediction horizon (number of intervals).

_{j}

_{,min(i)}and y

_{j}

_{,max(i)}denote the minimum and maximum values of the jth output at the ith prediction horizon step, respectively; u

_{j}

_{,min(i)}and u

_{j}

_{,max(i)}denote the minimum and maximum values of the jth input at the ith prediction horizon step, respectively; Δu

_{j}

_{,min(i)}and Δu

_{j}

_{,max(i)}denote the minimum and maximum values of the jth input rate at the ith prediction horizon step, respectively; h

^{(y)}

_{j,min(i)}and h

^{(y)}

_{j,max(i)}denote the minimum and maximum values of the jth output’s hard constraints at the ith prediction horizon step, respectively; h

^{(u)}

_{j,min(i)}and h

^{(u)}

_{j,max(i)}denote the minimum and maximum values of the jth input’s hard constraints at the ith prediction horizon step, respectively; h

^{(∆u)}

_{j,min(i)}and h

^{(∆u)}

_{j,max(i)}denote the minimum and maximum values of the jth input rates’ hard constraints at the ith prediction horizon step, respectively; n

_{y}denotes the number of output parameters; n

_{u}denotes the number of input parameters; and n

_{Δu}denotes the number of input rate parameters.6.3. Simulink Model

_{x}and the desired reference values Ref = (Y, Phi)

^{T}at the current time. Block 6 (Result) accumulates the calculated outputs. The model does not comprise external disturbances and measurement noise.

## 7. Application

#### 7.1. Initial Conditions Data

_{−1}= −0.1 s, the initial data vector is formed as follows:

_{(−1)}, V

_{o(−1)}, d

_{(−1)}, V

_{i(−1)}, L

_{i(−1)}, X

_{p(−1)}, V

_{p(−1)}) = (480, 70, 35, 65, 22.5, 0, 70),

_{min}= 7.79 s and S

_{fmin}= 25 m. The minimum time margin was set as t

_{mm}= 1 s. Substituting these values into Equations (12)–(18) and (27)–(33) gives the following rational values (Figure 9a,b): overtaking global longitudinal projection X

_{p}= 250 m, overtaking time t

_{p}= 8.9 ≈ 9 s, bypass time during lane change t

_{t}= 5.1 s, and time to the critical Point t

_{c}= 6.9 s.

#### 7.2. Parameters of the AMPC Controller

#### 7.3. Reference Speeds, Accelerations, and Displacements

_{Xa}(Figure 9d) may lead to the appearance of such a peak near the critical point, when the longitudinal accelerations in the phase of maneuver completion are negative and larger than the absolute value of 0.5 m/s

^{2}. That would mean the use of service braking and activation of the vehicle’s working brake system. From the point of view of ensuring the maximum vehicle stability during the lane change, it is undesirable to use the tire longitudinal force values close to those which may considerably reduce the tire’s lateral adhesion. In connection with the foregoing, it may be recommended to focus on the value of the speed V

_{pf}at which the distribution of the speed plan requires decelerations, provided only by limiting the engine power consumption. In this case, the selected value V

_{pf}ensures maneuver completion with acceleration close to zero.

_{V}coefficient very much reduces the speed consumption, but significantly increases the need for acceleration at the beginning of the maneuver. The increase of the W

_{A}coefficient reduces the cumulative consumption of acceleration, but does not provide smoothness in the boundary zones of the speed plan, and the peak speed value rises. Increasing the W

_{S}coefficient distributes speeds evenly over time.

_{V}= 0.2, W

_{A}= 0.2, and W

_{S}= 0.6. In the distribution of transverse speeds of the bypass phase, the priority is divided between the control smoothness and the cumulative acceleration intake: W

_{V}= 0.2, W

_{A}= 0.4, and W

_{S}= 0.4. In the final phase, due to the lane change at high speeds, the main priority is focused on reducing the lateral accelerations, respectively: W

_{V}= 0.1, W

_{A}= 0.6, and W

_{S}= 0.3. It is obvious, however, that priorities may vary, depending on the situation.

_{p}= 250 ≈ 249.4 m, and the transverse component Y

_{p}is strictly within 3.6 m, but has a residual of 0.23 m at the time t

_{p}= 9 s. At this moment, the passing vehicle is almost in the middle of its lane and continues stable movement, i.e., the situation is uncritical. The AMPC controller calculates the discrete control signal based on the information on the previous value and reference tracks. However, it is almost impossible to avoid tracking delay completely. The same effect can be observed in relation to the lateral speed V

_{Y}(Figure 10b), which coincides in terms of shape and values with the initial one in Figure 9d, but lags a bit in time.

## 8. Concluding Remarks

_{p}and X

_{p}) characterize the average speed V

_{xa}and are obviously not enough, because many curves integrable in the interval (t

_{0}, t

_{p}) can give the same X

_{p}. In addition, the vehicle’s ability to increase speed is a function of the speed and depends on the vehicle’s characteristics (i.e., acceleration is a 3D surface as a function of vehicle speed and throttle activation level). Therefore, in this study, it was assumed that the speed nodes are interconnected by curves over time, differentiable at least twice (i.e., providing smoothness). Then, the derivatives at the nodes which reflect the slope (acceleration) and the curvature (sharpness) were included as members of the objective function, along with their weights. Since this is an FE model of the curve, no other parameters except for the nodal V

_{x}, dV

_{x}/dt, and d

^{2}V

_{x}/dt

^{2}can be included, because only the nodes need to be distributed, consistent with the vehicle technical features.

## Author Contributions

## Funding

## Acknowledgments

## Conflicts of Interest

## Abbreviations

NMPC | Model Predictive Control |

RRT | Rapid Random Tree |

GPS | Global Positioning System |

LQP | Linear Quadratic Programming |

AMPC | Adaptive Model Predictive Control |

SUV | Single Unit Vehicle |

MO | Measured Outputs |

FE | Finite Element |

## Appendix A: Representing the Speed Function by Finite Elements

_{0}, t

_{p}) varies along the X-coordinate of the road segment 0−X

_{p}, according to the law V

_{X}(t). Then, for a grid of

**n**time intervals (t

_{0}, t

_{1}, t

_{2}, …, t

_{n}),

_{i}is the time interval (t

_{i}− t

_{i−1}), which is generally variable.

**i**-th time segment (t

_{i−1}, t

_{i}),

_{i}] is the FE local time; q

_{1i}, q

_{2i}, q

_{3i}, and q

_{4i}represent impact coefficients, where q

_{1i}and q

_{3i}are speeds at FE nodes and q

_{2i}and q

_{4i}are accelerations (derivatives) at the corresponding nodes; and f

_{τ1}, f

_{τ2}, f

_{τ3}, and f

_{τ4}are basis functions.

**f**for an FE of a unitary length (ΔT = 1) is based on the cubic polynomial with two degrees of freedom at a node, providing smoothness and continuous differentiability, as follows:

_{ξ}**q**does not depend on τ, only the basis functions (Equation (A5)) are integrated.

_{i}## References

- Easa, S.M. Automated highways. In Encyclopaedias of Electrical and Electronics Engineering; Webster, J.G., Ed.; John Wiley & Sons: New York, NY, USA, 2017; pp. 79–88. [Google Scholar]
- Al-Qaysi, Q.; Easa, S.M.; Ali, N. Proposed Canadian automated highway system architecture: Object-oriented approach. Can J. Civ. Eng.
**2003**, 30, 955–969. [Google Scholar] [CrossRef] - Narla, S.; Stowell, H. Connected and Autonomous Vehicles; Institute of Transportation Engineers: Washington, DC, USA, 2019. [Google Scholar]
- Ozbay, K. Developments in connected and automated vehicles. Intell. Transp. Syst. J.
**2018**, 22, 187–189. [Google Scholar] [CrossRef] - Shladover, S. Connected and automated vehicle systems: Review. Intell. Transp. Syst. J.
**2018**, 22, 190–200. [Google Scholar] [CrossRef] - National Cooperative Highway Research Program. Connected and Autonomous Vehicles and Transportation Infrastructure Readiness; Project 20–24(111); Transportation Research Board: Washington, DC, USA, 2017. [Google Scholar]
- Goldin, P. 10 Advantages of Autonomous Vehicles. ITSdigest. 2018. Available online: https://www.itsdigest.com/10-advantages-autonomous-vehicles (accessed on 13 May 2020).
- Schwarting, W.; Alonso-Mora, J.; Paull, L.; Karaman, S.; Rus, D. Safe nonlinear trajectory generation for parallel autonomy with a dynamic vehicle model. IEEE Trans. Intell. Transp. Syst.
**2018**, 19, 2994–3008. [Google Scholar] [CrossRef] [Green Version] - Talamino, J.P.; Sanfeliu, A. Anticipatory kinodynamic motion planner for computing the best path and velocity trajectory in autonomous driving. Robot. Auton. Syst.
**2019**, 114, 93–105. [Google Scholar] [CrossRef] - González, D.; Pérez, J.; Milanés, V.; Nashashibi, F. A review of motion planning techniques for automated vehicles. IEEE Trans. Intell. Transp. Syst.
**2016**, 17, 1135–1145. [Google Scholar] [CrossRef] - Gu, T.; Dolan, J.M.; Lee, J. Automated tactical maneuver discovery, reasoning and trajectory planning for autonomous driving. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, Daejeon, Korea, 9–14 October 2016; pp. 5474–5480. [Google Scholar]
- Wang, F.; Yang, M.; Yang, R. Conflict-probability-estimation-based overtaking for intelligent vehicles. IEEE Trans. Intell. Transp. Syst.
**2009**, 10, 366–370. [Google Scholar] [CrossRef] [Green Version] - Kala, R.; Warwick, K. Motion planning of autonomous vehicles on a dual carriageway without speed lanes. Electronics
**2015**, 4, 59–81. [Google Scholar] [CrossRef] [Green Version] - Babu, M.; Oza, Y.; Singh, A.K.; Krishna, K.M.; Medasani, S. Model predictive control for autonomous driving based on time scaled collision cone. arXiv
**2018**, arXiv:1712.04965. [Google Scholar] - Tomas-Gabarron, J.-B.; Egea-Lopez, E.; Garcia-Haro, J. Optimization of vehicular trajectories under Gaussian noise disturbances. Future Internet
**2013**, 5, 1–20. [Google Scholar] [CrossRef] [Green Version] - Liu, C.; Zhan, W.; Tomizuka, M. Speed profile planning in dynamic environments via temporal optimization. In Proceedings of the IEEE Intelligent Vehicles Symposium IV, Los Angeles, CA, USA, 11–14 June 2017; pp. 154–159. [Google Scholar]
- Berntorp, K.; Weiss, A.; Danielson, C.; Kolmanovsky, I.V.; Cairano, S.D. Automated driving: Safe motion planning using positively invariant sets. In Proceedings of the IEEE 20th International Conference on Intelligent Transportation Systems, Yokohama, Japan, 16–l9 October 2017; pp. 1–6. [Google Scholar]
- Hu, X.; Chen, L.; Tang, B.; Cao, D.; He, H. Dynamic path planning for autonomous driving on various roads with avoidance of static and moving obstacles. Mech. Syst. Signal Process.
**2018**, 100, 482–500. [Google Scholar] [CrossRef] - Molinari, F.; Anh, N.N.; Re, L.D. Efficient mixed integer programming for autonomous overtaking. In Proceedings of the American Control Conference, Seattle, WA, USA, 1 May 2017; pp. 2303–2308. [Google Scholar]
- Murgovski, N.; Sjöberg, J. Predictive cruise control with autonomous overtaking. In Proceedings of the 54th IEEE Conference on Decision and Control, Osaka, Japan, 15–18 December 2015; pp. 644–649. [Google Scholar]
- Németh, B.; Gáspár, P.; Hegedűs, T. Optimal control of overtaking maneuver for intelligent vehicles. J. Adv. Transp.
**2018**. [Google Scholar] [CrossRef] - Hassein, U.; Diachuk, M.; Easa, S.M. In-vehicle passing collision warning system for two-lane highways considering driver characteristics. Transp. Res. Rec.
**2018**, 2672, 101–112. [Google Scholar] [CrossRef] - Benjamin, J.R.; Cornell, C.A. Probability, Statistics, and Decision for Civil Engineers; Dover Publications: Mineola, NY, USA; McGraw-Hill: New York, NY, USA, 2014; pp. 1–704. [Google Scholar]
- Diachuk, M.; Easa, S.M.; Hassein, U.; Shihundu, D. Modeling passing maneuver based on vehicle characteristics for in-vehicle collision warning systems on two-lane highways. Transp. Res. Rec.
**2019**, 2673, 165–178. [Google Scholar] [CrossRef] - Diachuk, M.; Easa, S.M. Guidelines for roundabout circulatory and entry widths based on vehicle dynamics. J. Traffic Transp. Eng.
**2018**, 5, 361–371. [Google Scholar] [CrossRef] - MathWorks. Available online: www.mathworks.com/help/mpc/ug/adaptive-mpc.html (accessed on 1 October 2019).
- American Association of State Highway and Transportation Officials (AASHTO). A Policy on Geometric Design of Highway and Streets; AASHTO: Washington, DC, USA, 2018. [Google Scholar]
- Easa, S.M. Geometric design. In Civil Engineering Handbook; Chapter 63; Chen, W.F., Liew, J.Y., Eds.; CRC Press: Boca Raton, FL, USA, 2002. [Google Scholar]

**Figure 1.**Phases of overtaking: (

**a**) obstacle-rear reach, (

**b**) obstacle-front reach, and (

**c**) maneuver completion.

**Figure 7.**Influence of threshold values of speed fluctuations on the prediction reliability: (

**a**) Increase in speeds of opposing and impeding vehicles (Cases 1-3); (

**b**) decrease in speeds of opposing and impeding vehicles (Cases 1-3).

**Figure 9.**Planning reference tracks for state parameters: (

**a**) Vehicle path’s prognosis; (

**b**) definition of point t

_{p}; (

**c**) predicted passing vehicle’s global displacements; (

**d**) plan of global velocities; (

**e**) plan of global accelerations.

**Figure 10.**Simulation results of vehicle steering control prognosis during overtaking: (

**a**) Global displacements; (

**b**) steering control and lateral speed projection.

© 2020 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/).

## Share and Cite

**MDPI and ACS Style**

Easa, S.M.; Diachuk, M.
Optimal Speed Plan for the Overtaking of Autonomous Vehicles on Two-Lane Highways. *Infrastructures* **2020**, *5*, 44.
https://doi.org/10.3390/infrastructures5050044

**AMA Style**

Easa SM, Diachuk M.
Optimal Speed Plan for the Overtaking of Autonomous Vehicles on Two-Lane Highways. *Infrastructures*. 2020; 5(5):44.
https://doi.org/10.3390/infrastructures5050044

**Chicago/Turabian Style**

Easa, Said M., and Maksym Diachuk.
2020. "Optimal Speed Plan for the Overtaking of Autonomous Vehicles on Two-Lane Highways" *Infrastructures* 5, no. 5: 44.
https://doi.org/10.3390/infrastructures5050044